#define READ_INTERVAL    10
#define CORRECTION_FACTOR 1000
#define ROTATION_ANGLE    60
#define ONFWD_TIME        200

task main() {
     SetSensorLight(S3);
     int wait_time, signed_rotation, light, light_tresh = 0, counter = 0, lr=0;
     bool correction = false;
     signed_rotation = ROTATION_ANGLE;
     ResetAllTachoCounts(OUT_A);
     
     Wait(500);   //for the sensor to initialise
     
     
     light_tresh = Sensor(S3)+5;     //set threshold to ambiant light

     OnFwdEx(OUT_A,100,RESET_NONE);
     while (MotorTachoCount(OUT_A) < signed_rotation);
     OffEx(OUT_A,RESET_NONE);

     signed_rotation = -signed_rotation;
     
     while(true){
        counter = 0;
        
        light = Sensor(S3);

        while( light > light_tresh ) {    //If sensor detects ball
              //we take 1 count each 10ms to guage speed of ball
              counter += 1;
              Wait(READ_INTERVAL);
              light = Sensor(S3);
              correction = true; //Establish that a correction is needed
        }


        //Correction loop
        if(correction && lr == 0){
            OnRevEx(OUT_A,100,RESET_NONE);
            while (MotorTachoCount(OUT_A) > signed_rotation);
            OffEx(OUT_A,RESET_NONE);
            signed_rotation = -signed_rotation;
            correction = false;
            lr = 1;
            }
        else{if(correction){
            OnFwdEx(OUT_A,100,RESET_NONE);
            while (MotorTachoCount(OUT_A) < signed_rotation);
            OffEx(OUT_A,RESET_NONE);
            signed_rotation = -signed_rotation;
            correction = false;
            lr = 0;
            }}

              //End of scanning/Adjusting thread
       
       
}//End True
}//End of Main



