#define ODO_POLL_PERIOD    100
#define uM_PER_DEGREE      480   // calibrated: 480 micrometers per degree

#define TURN_PER_DIFF_DEG  240    // this was calibrated: 240 millidegrees per degree


long xPos=0, yPos=0;  // units: micrometers | range: +- 2 km

long xPosMM=0, yPosMM=0;

long angle=0;

task odometry();

void goTo(long x, long y) {
  long delX, delY;
  
  delX = x-xPosMM;
  delY = y-yPosMM;

  long dSquared = delX*delX+delY*delY;

  long d = Sqrt(dSquared);
  // we need 100*cos in order to use the Acos
  long theCos = delX*100/d;
  long trajAngle = Acos(theCos);

  NumOut(0,LCD_LINE6,d);
  NumOut(0,LCD_LINE7,trajAngle);
  
  if (delY < 0)
    trajAngle = 360 - trajAngle;

  trajAngle = trajAngle-90;
  if (trajAngle <0)
    trajAngle += 360;
    
  // now we need to find which direction to turn...
  
  long diff = trajAngle*1000 - angle;
  
  int turn = -1;
  if (diff > 0 && diff < 180000 || diff < -180000)
    turn = 1;

  OnFwdEx(OUT_B,turn*40,RESET_NONE);
  OnRevEx(OUT_C,turn*40,RESET_NONE);

  until (abs(angle-trajAngle*1000) < 5000);

  OnFwdRegEx(OUT_BC,100,OUT_REGMODE_SYNC,RESET_NONE);
  
  until (abs(x-xPosMM) < 30 && abs(y-yPosMM) < 30);
  
  OffEx(OUT_BC,RESET_NONE);


}

task main() {
  start odometry;
  Wait(800);
  goTo(300,300);
  Wait(800);
  goTo(-300,300);
  Wait(800);
  goTo(0,0);
}

task odometry() {

  int deltaLeft, deltaRight;  // units: micrometers | range: +- 3cm
  int deltaLeftDeg, deltaRightDeg;  // units: degrees
  
  long deltaX, deltaY;         // units: micrometers | range: +- 3cm

  long deltaAngle;       // units: millidegrees

  long distance;
  int sine, cosine, lastSine=0, lastCosine=100;
  
  long lastLeft=0,lastRight=0;
  
  ResetTachoCount(OUT_BC);
  
  while (true) {
    deltaLeftDeg  = MotorTachoCount(OUT_C)-lastLeft;
    deltaRightDeg = MotorTachoCount(OUT_B)-lastRight;
    //ResetTachoCount(OUT_BC);
    lastLeft = MotorTachoCount(OUT_C);
    lastRight = MotorTachoCount(OUT_B);
    
    deltaLeft = uM_PER_DEGREE*deltaLeftDeg;
    deltaRight = uM_PER_DEGREE*deltaRightDeg;
    
    distance =  (deltaLeft+deltaRight)/2;
    
    deltaAngle = (deltaRightDeg-deltaLeftDeg)*TURN_PER_DIFF_DEG;
    long tempAngle = angle + deltaAngle;

    if (tempAngle >= 360000) {
      angle = tempAngle - 360000;
    } else if (tempAngle < 0) {
      angle = tempAngle + 360000;
    } else {
      angle = tempAngle;
    }
    
    int angleDeg = angle/1000;
    sine = Sin(angleDeg);
    cosine = Cos(angleDeg);
    
    deltaX = (distance*(-lastSine))/100/2;
    deltaY = (distance*lastCosine)/100/2;

    deltaX += (distance*(-sine))/100/2;
    deltaY += (distance*cosine)/100/2;
    
    lastSine = sine;
    lastCosine = cosine;
    
    xPos += deltaX;
    yPos += deltaY;
    
    xPosMM = xPos/1000;
    yPosMM = yPos/1000;

    NumOut(0,LCD_LINE1,angle/1000,false);
    NumOut(0,LCD_LINE3,xPosMM);
    NumOut(0,LCD_LINE4,yPosMM);
    
    Wait(ODO_POLL_PERIOD);
  }
}
    

