# 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 – Maximilian Gerhardt Aug 15 at 18:15
• @MaximilianGerhardt, okay, sorry, i'll edit it in just a sec – PotatoMan Aug 15 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. – Delta_G Aug 15 at 19:13
• @Delta_G, okay, I'll do that – PotatoMan Aug 15 at 19:13
• What units do you have the angles in? Degrees or radians? – Delta_G Aug 16 at 4:03