# Path Planning code

I posted a question regarding robot path planning here now the question is mainly about the code and logic but still I shortly explain the initial situation.

So I tried to change the code so that the program is gonna work for my problem. My goal is to get my robot which is an Arlo robot to reach predefined points with the help of a beacon(A) and the plt 300 which is following the beacon with a laser. So that the beacon is right above the predefined point.

I decided to use the mathematical convention. So in the picture the coordinates of the point A would be:
A_x = M_x + cos(θ + 24)*r
A_y = M_y + sin(θ + 24)*r
Now I just want to know if the code would theoratically work or not.
Edit
I declared missing variables, fixed the float to binary operator error. Changed the code like Chuck suggested, changed destinationHeading not sure if it right.

//Here is the code
#include <ArloRobot.h>
#include <NewPing.h>
#include <Servo.h>

// Arlo and serial objects required
ArloRobot Arlo;                               // Arlo object
SoftwareSerial ArloSerial(12, 13);            // Serial in I/O 12, out I/O 13

int countsLeft, countsRight;                  // Encoder counting variables

//current points
float xc = -300;
float yc = 300;

//target points

float xt = -300;
float yt = -300;

//turning  angle
float turnAngle;

float r = 12;

float xm = 0;
float ym = 0;

float xn = 0;
float yn = 0;

float turnRight = 1;
float turnLeft = -1;
float wheelBase = 39.5;
float speedModifier = 0;
float vRightMotor = 0;
float vLeftMotor = 0;
float vMotor = 0;
float theta = 0;
float dT = 0.00104;
float distance = 0;

void setup() {
// pin setup
Serial.begin(9600);
ArloSerial.begin(19200);                    // Start DHB-10 serial com
Arlo.begin(ArloSerial);                     // Pass to Arlo object
Arlo.clearCounts();                         // Clear encoder counts
}

void loop() {

//calculate turning angle

if (turnAngle > 180)
{
turnAngle = turnAngle - 360;
}

if (turnAngle < -180)
{
turnAngle = turnAngle + 360;
}

if (turnAngle < 0)
{
speedModifier = turnRight;
}

if (turnAngle > 0)
{
speedModifier = turnLeft;
}

theta = 0;
while(abs(abs(theta) - abs(turnAngle)) > 0)
{

vRightMotor = speedModifier * 50;
vLeftMotor = -speedModifier * 50;
vMotor = vRightMotor;
theta = theta + thetaDot * dT;
}

distance = sqrt(((xn - xm)* (xn - xm)) + ((yn - ym) * ( yn-ym)));
if (distance > 0)
{
Arlo.writeSpeeds(72, 72);                 // Go forward 72 counts/sec
}
xm = xn;
ym = yn;

}


The main code is from the answer of this question here

I think your problem revolves around the line:

float currentHeading = startingHeading + beaconHeading;


float currentHeading = startingHeading;


The only time that the beacon heading comes into play is when you update your robot's position by taking a measurement of the beacon.

I don't see a beacon/position update section anywhere in your code, so right now it looks like the whole thing is running open loop. If that's the case, then you don't need the beacon angle offset anywhere.

If you do decide to incorporate a beacon position measurement somewhere in your code, then you would update your heading with the code I provided at the end of the other question:

$$M_x = A_x - \sin{(\theta_{\mbox{heading}} + \theta_{\mbox{beacon offset}})}*r \\ M_y = A_y - \cos{(\theta_{\mbox{heading}} + \theta_{\mbox{beacon offset}})}*r \\$$

But again, this requires you to measure $$$$. If you don't measure the beacon, you can omit the beacon offset angle altogether.

:EDIT:

If you want to put the beacon on the target, consider the following graphic:

The robot faces in some direction and there's a target. In order to get the beacon on the target, you would want to end up like this:

That is, you want the robot to drive not quite the direct heading to the target. You want to modify the "target" heading by some amount in order to be able to end with the beacon on the target.

So, how much do you need to adjust your robot center $$M$$ to target heading in order to achieve the beacon $$A$$ to target heading? That depends on how far away the robot center is right now ($$r$$, the red line) and how far away the beacon is from the robot center ($$dy$$, in blue, and $$dx$$, in light orange.)

So now, if your heading is described as an angle that increases positive CCW from the +x-axis, you can calculate the naive heading (red) as $$atan2(\mbox{target}_y - M_y, \mbox{target}_x - M_x)$$, and then you can see that the heading offset to get the beacon at the target is $$asin(dy/r)$$.

Finally, the new distance you need to travel, $$dS$$, then length of the orange line, is $$cos(\theta_{\mbox{heading offset}})*|r| - dx$$. The length of $$r$$, $$|r|$$, is found with the Pythagorean Theorem $$\sqrt{\left(\mbox{target}_x - M_x \right)^2 + \left(\mbox{target}_y - M_y \right)^2}$$.

• I added how I defined point A and to calculate point M isn't hard and it is different to your suggestions because I decided to align the robot with the x-Axis but I currently don't know how to implement the beacon/position update into the code. I get the math behind it but don't know where to put it in the code. I get <Ax,Ay> from my tracking device. I know you wrote how right now but I just dont get it. – Sentrinity Sep 27 '18 at 6:26
• And I want to reach the goal point so that A = goal point and not that M = goal point. – Sentrinity Sep 27 '18 at 6:44
• Do I even need the coordinates of M? I just don't know if and how to implement the changes. – Sentrinity Sep 27 '18 at 7:11
• And how can I determine the right dt I didn't find like a default number for the arduino uno. I just put a number in, which I found in the arduino forum. – Sentrinity Sep 27 '18 at 9:14
• You need the coordinates of $M$ because it's the center of your robot; if you try to just use $A$ as if it were the center then (1) you won't go straight (from A's perspective) and (2) you won't turn about A, so all your turns are harder to evaluate. If you need a $dT$ value then you should have a static/persistent variable. Call millis or micros to get the current time, subtract the previous time, then set the previous time to the current time. – Chuck Sep 27 '18 at 13:00

What you have implemented in the sourcecode is called steering behavior and was introduced by Craig Reynolds in the year 1999 in the paper Steering behaviors for autonomous characters The idea is to use a locomotion model (which is given in your sourcecode too and then calculate the angle of the car like robot. Implementing steering behavior in software is a bit tricky. As far as i can see from your previous postings, you are working hard on the subject. Mostly, the prototype doesn't work in the first trial and then reprogramming is needed (I'm talking with experience, my own steering project was a mess).

For reasons of simplification I can recommend to use a object oriented programming language. Your code right now was probably written in C. If this could be extended into the directions of structs and Python classes it is easier to modify. If the project is to complicated, it is a good idea to simplify the task first. This is possible by not controlling the robot autonomously, but the software is painting only the line from the robot to the goal and a human operator has to steer to the goal manually. This kind of setup is called in the literature a “head up display”. It is easier to program than a full blown steering control.

• Ty very much for the answer. The programm is written on arduino so yeah it is on C. I chose arduino because I only was able to get the information of the location of the beacon through the serial terminal of arduino. Before I tried it with a Propeller Microcontroller but it didn't work. I already have an obstacle avoiding sketch for the robot but as metioned I am now looking for the path planning. I will try to simplify task first. Thanks again for the answer and the tips. Was your project on Python? – Sentrinity Sep 26 '18 at 9:00
• @Sentrinity Python is a useful language, because it allows to divide a complex problems into smaller chunks. One class is for the locomotion model, one of the hardware control and one class for the userinterface. Each class should have less than 100 lines of code. This makes it possible to copy&paste previously written code into new projects. – Manuel Rodriguez Sep 26 '18 at 11:12
• But as mentioned I have to do it on arduino or can you use a python code for the arduino uno microcontroller? – Sentrinity Sep 26 '18 at 11:19
• @Sentrinity Are you unsure, which programming language is right for the Arduino Microcontroller? I have an idea ... – Manuel Rodriguez Sep 26 '18 at 11:23
• I mean Arduino is in C right and I don't have the time right know to learn to program on python. That is also one of the reasons I chose the Arduino Uno because I have some experience with it. We used it in school. – Sentrinity Sep 26 '18 at 11:29