#define MotorOnTime 50
#define StdTripTime 22
#define Up 1
#define Down 0
#define TimeLimit 60

int Seuil;
int TripTime;
int flag;
int WaitTimeDown=16;
int WaitTimeUp=16;
int UpDown;
int WatchDogTriggered=0;

task main()
{
  
  flag = 0;
  SetSensor(SENSOR_1,SENSOR_LIGHT);
  start timing;
  start watchdog;
  Seuil = SENSOR_1 + 2; // defines the light threshold
  UpDown=Down;
  OnFwd(OUT_A);
  Wait(MotorOnTime);
  Off(OUT_A);
  
  while (true)
  {
    until (flag == 1); // wait till the wheel cross sensor beam
    flag=0;
    Wait(WaitTimeDown); // wait a bit more
    OnRev(OUT_A);       // flip the see-saw
    Wait(MotorOnTime);
    Off(OUT_A);
    UpDown=Up;

    until (flag == 1); // wait till the wheel cross sensor beam
    flag=0;
    Wait(WaitTimeUp); // wait a bit more
    OnFwd(OUT_A);     // flip the see-saw
    Wait(MotorOnTime);
    Off(OUT_A);
    UpDown=Down;

  }
}

// Tries to adjust waiting time so that the amplitude of
// the wheel movement is constant

task timing()
{
    int count;

    while (true)
    {
      ClearTimer(1);
      until (SENSOR_1>Seuil);

      flag=1;
      TripTime = Timer(1);
      if (WatchDogTriggered==0)
      {
        if (UpDown==Up)
        {
          WaitTimeDown += (StdTripTime-TripTime)/2;
          if (WaitTimeDown<1) WaitTimeDown=1;
        }
        else
        {
          WaitTimeUp += (StdTripTime-TripTime)/2;
          if (WaitTimeUp<1) WaitTimeUp=1;
        }
      }
      else
      {
      WatchDogTriggered=0;
      }

      while (true)
      {
        count = 0;
        repeat (10)
        {
          if (SENSOR_1<Seuil) { count++; Wait(1); }
        }
        if (count == 10) break;
      }
    }
}

// If the wheel doesn't cross sensor beam for a long time
// it is probably trapped. Shake the see-saw to free it...

task watchdog()
{
  while (true)
  {
    if (Timer(1)>TimeLimit)
    {
      ClearTimer(1);
      PlaySound(1);
      WatchDogTriggered=1;
      Toggle(OUT_A);
      OnFor(OUT_A,10);
      Toggle(OUT_A);
      OnFor(OUT_A,10);
    }
  }  
}