#include "utilities.nxc"
// Example program for seesaw lab
// This program attempts to balance an object on the seesaw, using
// feedback from the light sensor.
//
// it contains a number of flaws
// Rupert Brooks, Sept 2007

#define CRITICALLIGHT 40
#define POWER 75

// This routine waits until the value returned from the light sensor
// exceeds the CRITICALLIGHT value
byte waitLight() {
    byte lightlevel;
    TextOut(0,LCD_LINE2,"Waiting light 1a",0);
    do {
       lightlevel=Sensor(S3);
    } while(lightlevel<CRITICALLIGHT);

    PlayTone(TONE_B3,10) ;
    return(lightlevel);
}

// This tilts the seesaw by rotating motor A for 180 degrees
// dir determines which way it tilts
// dir is expected to be +-1
void tilt(char dir) {
   long starttach,seektach,endtach,difftach;
   PlayTone(TONE_C5,10);
   starttach=MotorTachoCount(OUT_A);
   seektach=starttach+(dir*180);
   ValueOut(0,LCD_LINE2,"Tilt " ,dir,0);
   OnFwd(OUT_A,dir*POWER);
   do {
      endtach=MotorTachoCount(OUT_A);
      difftach=(endtach-seektach)*dir;
      ValueOut(0,LCD_LINE8,"Tach: ",endtach,0);
      } while (difftach<0);
      
   Off(OUT_A);
   return;
}



task main() {
   long counter=0; // ambitious... to make this a long
   byte lightlevel;
   char dir=1;
   titleScreen("Seesaw","Rupert Brooks");
   ClearScreen();
   SetSensorType(S3,SENSOR_TYPE_LIGHT_ACTIVE);
   SetSensorMode(S3,SENSOR_MODE_PERCENT);
   // simplistic balancing algorithm
   //   Wait for the light
   //   Increment the counter and update the readout
   //   tilt the beam
   //   flip the direction for next time
   //   wait 100 ms
   while(1) {
      lightlevel=waitLight();
      counter++;
      ValueOut(0,LCD_LINE7,"Light:  " , lightlevel,0);
      ValueOut(0,LCD_LINE6,"Count:  " , counter,0);
      tilt(dir);
      dir*=-1;
      Wait(100);
   }
}
