#include "utilities.nxc"
// this is the shell of an odometry program.
// the odometer should run in one thread, and the drive control in another
// The odometer should continuously display the current position and
// heading to the screen
//
// Rupert Brooks, Sept 2007

task odometer () {
   long wheelbval;
   long wheelcval;
   long degrees=0,xout=0,yout=0;
   while(true) {
       // read degrees of rotation of each wheel
       wheelbval=MotorTachoCount(OUT_B);
       wheelcval=MotorTachoCount(OUT_C);

      // this does nothing useful.  Put your odometry code here
      degrees++;
      xout++;
      yout++;
      Wait(100);

      ValueOut(0,LCD_LINE1,"Heading: ",degrees,0) ;
      ValueOut(0,LCD_LINE2,"XPos ",xout,0)  ;
      ValueOut(0,LCD_LINE3,"YPos ",yout,0)  ;
   }
}

task drive () {
   // drive forward, wait for a button, turn, wait for a button....
   while(true) {
      TextOut(0,LCD_LINE3,"Press a button to go",0) ;
      waitButton();
      OnFwdEx(OUT_C,25,RESET_NONE);
      OnFwdEx(OUT_B,25,RESET_NONE);
      Wait(4000);
      OffEx(OUT_C,RESET_NONE);
      OffEx(OUT_B,RESET_NONE);
      waitButton();
      OnRevEx(OUT_C,25,RESET_NONE) ;
      Wait(2000)            ;
      OffEx(OUT_B,RESET_NONE) ;
      OffEx(OUT_C,RESET_NONE);
   }
}

task main () {
    Precedes(odometer,drive);
}
