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!

EDIT: Added code from PasteBin

#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() {
  t = millis();

void loop() {
  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("        ");
  Serial.print("        ");

//0.9mm/1 deg
void Encoders() {
  LNewPos = Left.read();
  if (LNewPos != LOldPos) {
    LOldPos = LNewPos;
    LeftDist = ((LNewPos / 840) * (2 * r * 3.14)) / 10; //cm/ Rev
  RNewPos = Right.read();
  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 Commented Aug 15, 2019 at 18:15
  • @MaximilianGerhardt, okay, sorry, i'll edit it in just a sec
    – PotatoMan
    Commented 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.
    – Delta_G
    Commented Aug 15, 2019 at 19:13
  • @Delta_G, okay, I'll do that
    – PotatoMan
    Commented Aug 15, 2019 at 19:13
  • What units do you have the angles in? Degrees or radians?
    – Delta_G
    Commented Aug 16, 2019 at 4:03

1 Answer 1


From your use of the number 360 at the end of the Encoders() function, I am thinking that you are using degrees for the angles. That won't work with sin and cos. The trig functions all expect angles in radians. To convert degrees to radians, multiply by PI/180.

  • I have just changed it to calculate it in radians, but it does the same thing, the x and y values go up, reaches an integer overflow and goes back down,
    – PotatoMan
    Commented Aug 16, 2019 at 7:37
  • Is angle still an int? I would think radians would probably need to be in floating point.
    – Delta_G
    Commented Aug 16, 2019 at 20:55
  • No, i converted it to a float and I'm trying to take a little different approach and calculating velocities of each wheel/angular velocities and will try out a different equational you are not
    – PotatoMan
    Commented Aug 16, 2019 at 21:16
  • OK. You'll have to update the code here then to get further help.
    – Delta_G
    Commented Aug 16, 2019 at 23:31

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.