# Forward kinematics of a differential drive robot

I've been braking my head over this for the past few days and can't make it work.. So, I have a small robot with a rotary encoder on each wheel and i want to track its position (in x and y) on a flat plane. I know it's called forward kinematics, but i can't seem to get it working. I found a lot of great public sheets, but i think, out of all of them, this is the best one It had the equations of how to calculate the X and Y on page 74:

Vicc=L * (Din / (Dout–Din) + ½) (page 73) //Din is the inner wheel of the turn traveled distance, Dout is the outer wheel

Xtδ=Xt+Vicc*(sin(θt+θΔ) – sinθt)

Ytδ=Yt+Vicc*(cosθt - cos(θt+θΔ))

θtδ=θt+θΔ //angle measurement

Where Xtδ and Ytδ are the position of the robot (x, y); Xt/Yt/θt is the position of the robot when the counting of the position has been started; θΔ is the angle of the robot now;

. Here's The code that I tried to write using the equations. I expected it to write my coordinates in x and y, but when i moved the wheels, it sent me big, random numbers, which are fluctuating up and down, which looks like a sine wave. What am I doing wrong? Could you guide me to the right path? My end result, that I want to reach is controlling 2 both motor speeds and knowing the robots position (doesn't need to be very precise though). Thanks in advance!

``````#include <Encoder.h>

Encoder Left(3, 2);
Encoder Right(0, 1);

float R = 385; //mm
float r = 12.5; //Wheel diameter in mm
float L = 8; //Distance between the wheels in cm

double LeftDist = 0;
long LOldPos  = -999;
float LNewPos;
int Correction;

double RightDist = 0;
long ROldPos  = -999;
float RNewPos;

int angle= 0; // Robots current angle
unsigned long t;

int Xold= 0; //position at the start of the calculation
int Xnew= 0; //new, calculated position
int Ystart= 0; //position at the start of the calculation
int Ynew= 0;//new, calculated position
int OldAngle= 0; //angle at the start of the calculation
int NewAngle= 0;// new angle at the end of the calculation
int Vicc;

void setup() {
Serial.begin(9600);
t = millis();
}

void loop() {
Encoders();
if ((millis() - t) >= 100) {
Xold= Ynew; //setting the new position to old
Yold= Ynew; // setting the new position to old
OldAngle= NewAngle; //Setting the new angle to old

Vicc = (L * (LeftDist / (RightDist / LeftDist))) + 0.5;
Xnew= Xold + (Vicc * (sin(OldAngle + Angle) - sin (OldAngle))); //Calculation of X
Ynew= Yold+ (Vicc * (cos(OldAngle) - cos(OldAngle+ Angle))); //Calculation of Y
NewAngle = OldAngle+ Angle;

t = millis();
}

Serial.print(angle);
Serial.print("        ");
Serial.print(Xnew);
Serial.print("        ");
Serial.println(Ynew);
}

//0.9mm/1 deg
void Encoders() {
if (LNewPos != LOldPos) {
LOldPos = LNewPos;
LeftDist = ((LNewPos / 840) * (2 * r * 3.14)) / 10; //cm/ Rev
}
//-----------------------------------------------------
if (RNewPos != ROldPos) {
ROldPos = RNewPos;
RightDist = ((RNewPos / 840) * (2 * r * 3.14)) / 10; //cm/ Rev
}
angle= (abs(RightDist - LeftDist) * 0.9) - Correction; //calculate the angle the robot has turned
if (fi >= 360) Correction += 360;
else if (fi <= -360) Correction -= 360;
}
``````
• Please, instead of " I thought will work, but it didn't" state exactly what you expected it to output / do and what you observed. Do you have intermediate values you can check? Isolated test cases? We don't have your hardware here and the code is practically uncommented Aug 15, 2019 at 18:15
• @MaximilianGerhardt, okay, sorry, i'll edit it in just a sec Aug 15, 2019 at 18:18
• While you’re at it, it might help to change the variable names from things that are easy to type to things that are easy to read and follow. Aug 15, 2019 at 19:13
• @Delta_G, okay, I'll do that Aug 15, 2019 at 19:13
• What units do you have the angles in? Degrees or radians? Aug 16, 2019 at 4:03