#include <Encoder.h>

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

float R = 385; //mm
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 fi = 0;
unsigned long t;

int Xt = 0;
int Xtb = 0;
int Yt = 0;
int Ytb = 0;
int fit = 0;
int fitb = 0;
int Vicc;

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

void loop() {
  Encoders();
  if ((millis() - t) >= 100) {
    Xt = Xtb;
    Yt = Ytb;
    fit = fitb;

    Vicc = (L * (LeftDist / (RightDist / LeftDist))) + 0.5;
    Xtb = Xt + (Vicc * (sin(fit + fi) - sin (fit)));
    Ytb = Yt + (Vicc * (cos(fit) - cos(fit + fi)));
    fitb = fit + fi;

    t = millis();
  }
  Serial.print(Vicc);
  Serial.print("        ");
  Serial.print(Xtb);
  Serial.print("        ");
  Serial.println(Ytb);
}

//0.9mm/1 deg
void Encoders() {
  LNewPos = Left.read();
  if (LNewPos != LOldPos) {
    LOldPos = LNewPos;
    LeftDist = ((LNewPos / 840) * (2 * r * 3.14)) / 10; //MM / Rev
  }
  //-----------------------------------------------------
  RNewPos = Right.read();
  if (RNewPos != ROldPos) {
    ROldPos = RNewPos;
    RightDist = ((RNewPos / 840) * (2 * r * 3.14)) / 10; //MM / Rev
  }
  fi = (abs(RightDist - LeftDist) * 0.9) - Correction;
  if (fi >= 360) Correction += 360;
  else if (fi <= -360) Correction -= 360;
}