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!
EDIT: Added code from PasteBin
#include &lt;Encoder.h&gt;

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) &gt;= 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(&quot;        &quot;);
  Serial.print(Xnew);
  Serial.print(&quot;        &quot;);
  Serial.println(Ynew);
}

//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 &gt;= 360) Correction += 360;
  else if (fi &lt;= -360) Correction -= 360;
}
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.