Прямая кинематика робота с дифференциальным приводом

Я ломал голову над этим последние несколько дней и не могу заставить его работать. Итак, у меня есть небольшой робот с поворотным энкодером на каждом колесе, и я хочу отслеживать его положение (в x и у) на плоской плоскости. Я знаю, что это называется прямой кинематикой, но я не могу заставить ее работать. Я нашел много отличных общедоступных таблиц, но я думаю, что из всех них это лучший Уравнения расчета X и Y на странице 74:

Vicc=L * (Din / (Dout–Din) + ½) (стр. 73) //Din — внутреннее колесо пройденного пути поворота, Dout — внешнее колесо

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

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

θtδ=θt+θΔ //измерение угла

Где Xtδ и Ytδ — положение робота (x, y); Xt/Yt/θt — положение робота в момент начала отсчета положения; θΔ — угол робота сейчас;

. Вот код, который я пытался написать с помощью уравнений. Я ожидал, что он запишет мои координаты в x и y, но когда я двигал колеса, он присылал мне большие случайные числа, которые колеблются вверх и вниз, что выглядит как синусоида. Что я делаю не так? Не могли бы вы направить меня на правильный путь? Мой конечный результат, которого я хочу достичь, - это управление двумя скоростями двигателя и знание положения роботов (хотя это не должно быть очень точным). Заранее спасибо!

EDIT: добавлен код из PasteBin

#include <Encoder.h>

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

float R = 385; //мм
float r = 12.5; //Диаметр колеса в мм
float L = 8; //Расстояние между колесами в см

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

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

int angle= 0; // Текущий угол робота
unsigned long t;

int Xold= 0; //позиция в начале расчета
int Xnew= 0; //новая рассчитанная позиция
int Ystart= 0; //позиция в начале расчета
int Ynew= 0;//новая, рассчитанная позиция
int OldAngle= 0; //угол в начале расчета
int NewAngle= 0;// новый угол в конце расчета
int Vicc;

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

void loop() {
  Encoders();
  if ((millis() - t) >= 100) {
    Xold= Ynew; //устанавливаем новую позицию на старую
    Yold= Ynew; // установка новой позиции на старую
    OldAngle= NewAngle; //Установка нового угла на старый
    
    Vicc = (L * (LeftDist / (RightDist / LeftDist))) + 0.5;
    Xnew= Xold + (Vicc * (sin(OldAngle + Angle) - sin (OldAngle))); //Вычисление X
    Ynew= Yold+ (Vicc * (cos(OldAngle) - cos(OldAngle+ Angle))); //Вычисление Y
    NewAngle = OldAngle+ Angle;
    
    t = millis();
  }

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

//0,9 мм/1 град
void Encoders() {
  LNewPos = Left.read();
  if (LNewPos != LOldPos) {
    LOldPos = LNewPos;
    LeftDist = ((LNewPos / 840) * (2 * r * 3.14)) / 10; //см/ об.
  }
  //------------------------------------------------ -----
  RNewPos = Right.read();
  if (RNewPos != ROldPos) {
    ROldPos = RNewPos;
    RightDist = ((RNewPos / 840) * (2 * r * 3.14)) / 10; //см/ об.
  }
  angle= (abs(RightDist - LeftDist) * 0.9) - Correction; //рассчитываем угол, на который повернулся робот
  if (fi >= 360) Correction += 360;
  else if (fi <= -360) Correction -= 360;
}