#define ODO_POLL_PERIOD    100
#define WHEELBASE_X_100    1200   // these are not correct values
#define DIST_PER_DEG_X_100 800


int xPos=0, yPos=0, heading=0;

// heading of 0 degrees means pointing in positive x-axis
// start the robot in that position

task odometry();

task main() {
  start odometry;
}

task odometry() {
  int motorBChange, motorCChange;
  
  int deltaX, deltaY;
  
  int theta,deltaTheta;
  int turningRadius;
  
  ResetTachoCount(OUT_BC);
  
  while (true) {
    motorBChange = MotorTachoCount(OUT_B);
    motorCChange = MotorTachoCount(OUT_C);
    ResetTachoCount(OUT_BC);
    
    if (motorBChange == motorCChange) {
      // special case: straight line
      
      // remember: Sin returns 100*sin !!!!  same for Cos
      deltaX = (DIST_PER_DEG_X_100*motorBChange*(-Sin(theta)))/10000;
      deltaY = (DIST_PER_DEG_X_100*motorBChange*Cos(theta))/10000;
      // heading did not change
    } else {
      deltaTheta = (motorBChange-motorCChange)*DIST_PER_DEG_X_100/WHEELBASE_X_100;
      turningRadius = (motorBChange+motorCChange)*DIST_PER_DEG_X_100/(2*deltaTheta);

      deltaX = 0;  //  ...

    }
    
    
    Wait(ODO_POLL_PERIOD);
  }
}
    

