/*
This program is for my wheel balancing robot. It keeps a 40t gear rolling 
along a pair of parallel beams indefinitely by tipping the beam when the 
central light sensor detects its presence. For video, see 
http://www.youtube.com/user/SciGuy16#play/all/uploads-all/0/LvXVA7QVnwI
*/

#define time 50       // Time to wait once triggered before starting motor  \\
#define mot 200      // Time motor is powered for \\
int wheel,nowheel,thresh;
void beep()
{
     PlayTone(100,50);
     Wait(100);
     PlayTone(100,50);
}
task main()
{    ClearScreen();
     SetSensorType(S1,SENSOR_TYPE_LIGHT_INACTIVE);
     SetSensorMode(S1,SENSOR_MODE_RAW);
     beep();                                     // Start Calibrating the sensor  \\
     TextOut(30,LCD_LINE4,"nowheel",false);
     until(ButtonPressed(BTNCENTER,true)==1);
     nowheel=SENSOR_1;
     until(ButtonPressed(BTNCENTER,true)==0);
     ClearScreen();
     beep();
     TextOut(30,LCD_LINE4,"wheel",false);
     until(ButtonPressed(BTNCENTER,true)==1);
     wheel=SENSOR_1;
     ClearScreen();
     beep();                                     //  Finish Calibration  \\
     thresh=(wheel+nowheel)/2;                   //  Set Treshold  \\
     NumOut(30,LCD_LINE1,nowheel,false);
     NumOut(30,LCD_LINE2,wheel,false);           //  Display values for dubugging purposes \\
     NumOut(30,LCD_LINE3,thresh,false);
     OnRev(OUT_A,100);             // Tip the beam to start the cycle  \\
     Wait(mot);
     Off(OUT_A);
     while(true)
     {
           OnFwd(OUT_A,70);
           Wait(mot);
           Off(OUT_A);
           until(SENSOR_1<thresh);
           Wait(time);
           OnRev(OUT_A,70);
           Wait(mot);
           Off(OUT_A);
           until(SENSOR_1<thresh);
           Wait(time);
     }
}