/**
  Source for motorized Cow
  Author : Erik Amzallag
  Organization : FreeLUG
  Email : brickerik at free dot fr
  Date : July 2004
  
  Light Sensor on Sensor_2
  MicroMotor(s) on Out_A
  
  You can use or modify this source freely.

  In France, trains are running on left. So the cow watchs the train from the right to the left.
  Before starting the programme, the head should be on the right, and you should place a white brick front of the light sensor, on the other side of the train tracks.
  A good height for the light sensor is one brick high.
  
  You can change the direction of head reversing the call to the two functions turn_right() and turn_left(), ensuring that the head of the cow is on the left at the beginning.


*/

//Tolerance for the difference between the initial value of the light and the mesured light
//Adjust it if necessary
#define TOLERANCE  5

//Speed for the micro-motor
#define POW_ROT 1

//Time during the cow turns the head  (Left and Right)
#define TIM_ROT_L 90
#define TIM_ROT_R 90

//Time between the moment when the train is detected and the moment when the cow turns the head
//You can modify it : big value if the sensor is far away from the cow, small or null value else.
#define DELAY 0



int init_light;   //Variable for the initial value of the light
int mutex = 1;    //Mutual Exclusion Semaphore for the action "turn the head"

task main()
{
     SetSensor(SENSOR_2,SENSOR_LIGHT);
     SetSensorMode (SENSOR_2, SENSOR_MODE_PERCENT);
     init_light = SENSOR_2;
     SetPower(OUT_A,POW_ROT);
     start watch;
}

task watch()
{
     while (true){
           if ((SENSOR_2 < init_light - TOLERANCE ) && (mutex==1)) {
              mutex = 0;              //Take the semaphore
              Wait(DELAY);
              turn_left();            //Turn the head to the left
              while (SENSOR_2 < init_light - TOLERANCE ){
                    //When the train is still front of the sensor, the head waits to come back
                    Wait(15);
              }
              Wait(200);
              turn_right();           //Turn the head to the right
              mutex = 1;              //Give the semaphore
           }
     }
}

void turn_left()
{
    On(OUT_A);
    OnFwd(OUT_A);
    Wait(TIM_ROT_L);
    Off(OUT_A);
}

void turn_right()
{
     On(OUT_A);
     OnRev(OUT_A);
     Wait(TIM_ROT_R);
     Off(OUT_A);

}