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.

Robot from above

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 beaconHeading = -25;
    float startingHeading = 0;
    float currentHeading = startingHeading + beaconHeading;

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

    void setup() {
    // pin setup

    void loop() {
    destinationHeading = atan2((yt-yc), (xt-xc)); //calculate turning angle
    destinationHeading = destinationHeading * 180/3.1415; //convert to 
    turnAngle = destinationHeading - currentHeading;

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

    currentHeading = destinationHeading;
    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.

  • $\begingroup$ 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. $\endgroup$
    – Chuck
    Sep 25, 2018 at 13:22
  • $\begingroup$ 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. $\endgroup$
    – Chuck
    Sep 25, 2018 at 13:24
  • $\begingroup$ @Chuck sry that was my bad I will start a new question regarding this topic. $\endgroup$
    – Sentrinity
    Sep 26, 2018 at 6:16

1 Answer 1


Welcome to Robotics, Sentrinity! I'm glad the other answer helps!

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:

enter image description here

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!

  • $\begingroup$ 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. $\endgroup$
    – Sentrinity
    Sep 14, 2018 at 6:24
  • $\begingroup$ 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? $\endgroup$
    – Sentrinity
    Sep 14, 2018 at 12:23
  • $\begingroup$ @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? $\endgroup$
    – Chuck
    Sep 14, 2018 at 12:45
  • $\begingroup$ 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! $\endgroup$
    – Chuck
    Sep 14, 2018 at 12:49
  • $\begingroup$ 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? $\endgroup$
    – Sentrinity
    Sep 14, 2018 at 13:08

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.