# Follow up question to Robot path planning

So I was browsing through the localization section and found this question and the code which answered it. https://robotics.stackexchange.com/a/7932/21145 but I have a follow-up question to this. So the code is exactly what I need but my "beacon" isn't in the middle but in the front (image following). Do I have to change the code or is the solution implemented general enough, I am not quite sure? The beacon is marked as point A.

Edit: Code and clarification better showcase of beacon location

So I chose the mathematical convection and I took the code from the previous question and modified it. The heading is alligned with the x-Axis. Is it theoratically right? So new it would be:
A_x = M_x + cos(θ + (-25))*r
A_y = M_y + sin(θ + (-25))*r

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

//target points

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

//turning  angle
float turnAngle;

//***************
//***************

//***************
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 distance = 0;
//**************

void setup() {
// pin setup
Serial.begin(9600);
}

void loop() {
//*************
destinationHeading = atan2((yt-yc), (xt-xc)); //calculate turning angle
degrees
//*************

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 * <100 percent speed - varies by your
application>;
vLeftMotor = -speedModifier * <100 percent speed - varies by your
application>;
<send the vRightMotor/vLeftMotor speed commands to the motors>
vMotor = vRightMotor;
thetaDot = (tireRadius * vMotor) / (wheelBase/2);
}
<send zero speed to the motors>

distance = sqrt((xt - xc)^2 + (yt - yc)^2);
if (distance > 0) {
// go forward
}
xc = xt;
yc = yt;
//****************
}


I feel like I only had to change one line or am I wrong? I mean if I change the currentheading so that beacon is on a straight line with the next point it works. Maybe I didn't clarify it but I want to reach the next point so that the point A is the same as the point I'm heading to. I did make the angular offset a negative number because if I compare the angle to x-Axis it does go cw and not ccw, I tested it with geogebra and it is fine if I do it like this.

The robot I use is a Arlo Complete Robot System so I could let the robot turn with other commands.

• It looks like you've edited this question to ask a new question, but now it's hard to tell what the new question is and what the original question was. If there is a new question, please just start a new question. If you think the original question here gives good context, then just link to this post in the body of your new question. – Chuck Sep 25 '18 at 13:22
• Regarding what looks to me to be the new question in there, the only time the beacon heading should be used is when you attempt to get to the robot body $M$ after having measured the beacon location $A$. Again, if there is a new question, please start a new question. – Chuck Sep 25 '18 at 13:24
• @Chuck sry that was my bad I will start a new question regarding this topic. – Sentrinity Sep 26 '18 at 6:16

In the other question, the beacon was being tracked by a camera to establish where the robot was located. I'm assuming your beacon is being used for the same purpose here.

I'm assuming that the distance from your robot's turning center $M$ and the beacon center $A$ is $r$, i.e. $r = \overline{MA}$.

You'll need to be tracking your robot's heading in order to do any of the path planning as described in the other answer, but now you're stuck with additional task of relating the measured beacon location back to the turning center location to be able to orient correctly to reach the target location(s).

This is fortunately a relatively straightforward task, but you need to be careful with the conventions you choose. Is your heading along the +y-axis, or the +x-axis? Does your heading angle increase clockwise or counterclockwise (anti-clockwise)?

I'm assuming here the y-axis points "up," x-axis points "right," the heading zero angle is aligned with the +y-axis, and heading angle increases CW. Refer to the diagram below:

This is where you need to be careful with the convention that you choose. It might not be the same as what I chose, but the method should be the same. You need to create a drawing that shows your choice of axes and take the same steps.

The easiest way to go about this is to put your axes centered on the turning center $M$, as I did in the graphic above. Next, find the relationship between your beacon location $A$ and the heading angle $\theta$. Given an $<x, y>$ coordinate scheme, at a heading of $\theta = 0$, the location of $A$ would be $<0, +r>$. If $\theta = 90^{\circ}$, then $A = <+r, 0>$.

This is a trig problem, and you can see now that you need $\mbox{Trig Function 1}$, or $\mbox{TF1}$ such that $\mbox{TF1}(0^{\circ}) = 0$ and $\mbox{TF1}(90^{\circ}) = +1$, then you need a second trig function $\mbox{TF2}(0^{\circ}) = +1$ and $\mbox{TF2}(90^{\circ}) = 0$.

Then you get:

$$A_x = M_x + TF1(\theta)*r \\ A_y = M_y + TF2(\theta)*r \\$$

This problem was simplified by putting the coordinate axes on $M$ such that $M_x = 0$ and $M_y = 0$. The trig functions that satisfy the criteria above are $\mbox{TF1} = \mbox{sine}$ and $\mbox{TF2} = \mbox{cosine}$. I'm not sure that there's a good way to intuitively explain how to determine this other than practice, learning the special values for trig functions, and then more practice (do all the homeworks!).

So, this means that you can find the coordinate of $A$ as:

$$A_x = M_x + \sin{(\theta)}*r \\ A_y = M_y + \cos{(\theta)}*r \\$$

You're not trying to find $A$, though, you're given $A$ and you're trying to find $M$! This is now straightforward - solve for $M$!

$$M_x = A_x - \sin{(\theta)}*r \\ M_y = A_y - \cos{(\theta)}*r \\$$

Again, as discussed in the other question to the other OP, you haven't described your method for determining heading, so I'm assuming it's just dead reckoning as described under the "Last Update" heading in the other answer.

The scope of your question seemed to be more about how you relate beacon location back to turning center location, so hopefully this covers it all in enough detail for you. Again, remember to do the same steps with your own coordinate system and reference datums - they might not be the same as what I've used!

• ty very much for your fast answer, so do I have to replace xc and yc with Ax−sin(θ)∗r and Ay−cos(θ)∗r ? I mean for the code. To determine the heading I will just put the robot on the x-axis so that the heading is 0°. Yeah the beacon is being tracked by a PLT 300 it's a totalstation. – Sentrinity Sep 14 '18 at 6:24
• And if point was for example not perpendicular to the connection line between but in a 45° so I would just have to change θ then or do I have to do other stuff? – Sentrinity Sep 14 '18 at 12:23
• @Sentrinity - If you will just put the robot on the x-axis so that the heading is 0°`, then at $\theta = 0$ you have $A = <+r, 0>$. This is different than the example I gave where the example heading I used was zero degrees when aligned to the y-axis. Directionality matters, too - are you using a mathematical convention of positive counter-clockwise, or are you using a navigational convention of positive clockwise? – Chuck Sep 14 '18 at 12:45
• Regarding your second point, I'm assuming you mean that the direction from $M$ to the beacon $A$ is not along the same line that defines the forward vector. Is this correct? In that case you will again need to define your reference frames, then add the beacon angular offset to your heading, such that $\theta_{\mbox{effective}} = \theta_{\mbox{beacon angular offset}} + \theta_{\mbox{heading}}$. The angular offset would be the angle that $\overline{MA}$ makes with your forward vector, as defined using the same convention you're using for the heading! – Chuck Sep 14 '18 at 12:49
• Yeah I meant that the direction from M to the beacon A is not the same line that defines the forward vector. I don't get what you mean with mathematical convention or navigational convention could you rephrase that? – Sentrinity Sep 14 '18 at 13:08