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 startingHeading = 0;
float beaconHeading = 24;
float destinationHeading = 0;
float effectiveHeading = startingHeading + beaconHeading;
float currentHeading = startingHeading;
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 tireRadius = 7;
float speedModifier = 0;
float vRightMotor = 0;
float vLeftMotor = 0;
float vMotor = 0;
float theta = 0;
float thetaDot = 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() {
xm = xc - (cos(startingHeading + beaconHeading) * r);
ym = yc - (sin(startingHeading + beaconHeading) * r);
xn = xt - (cos(startingHeading + beaconHeading) * r);
yn = yt - (sin(startingHeading + beaconHeading) * r);
destinationHeading = atan2((yn - ym), (xn - xm))- startingHeading;
//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 * 50;
vLeftMotor = -speedModifier * 50;
vMotor = vRightMotor;
thetaDot = (tireRadius * vMotor) / (wheelBase / 2);
theta = theta + thetaDot * dT;
}
currentHeading = destinationHeading;
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