Follow up question to Robot path planning

Question

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 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
    Serial.begin(9600);
     }

    void loop() {
    //*************
    destinationHeading = atan2((yt-yc), (xt-xc)); //calculate turning angle
    destinationHeading = destinationHeading * 180/3.1415; //convert to 
    degrees
    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 
    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);` 
    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.

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:

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!