#define ROT_L      1489  //Trachocount per 360 degree robot rotation
#define ROT_R      1480  //Trachocount per 360 degree robot rotation
#define DIST       5     //Calibrated miliseconds per meter


#define STRAIGHT1   50   //Straight distance 1
#define ROTATION1   30   //First rotation

#define STRAIGHT2   30   //Straight Distance 2
#define ROTATION2   170  //Second rotation (absolute value)


task main() {

        ResetAllTachoCounts(OUT_AC);
        ClearScreen();

        int fwd_time1, fwd_time2, rot1, rot2, target, total;
        bool left = false;
        
        
        //We figure out the time of motor rotation as per calibration
        
        fwd_time1 = STRAIGHT1 * DIST;
        fwd_time2 = STRAIGHT2 * DIST;

        //Execute out first forward motion
        OnFwd(OUT_AC,40);
        Wait(fwd_time1);
        Off(OUT_AC);
        Wait(500);

        //And Display the tracho on the screen
        TextOut(0,LCD_LINE1,"Motor A (R):");
        NumOut(80,LCD_LINE1,GetOutput(OUT_A, TachoCount));

        TextOut(0,LCD_LINE2,"Motor C (L):");
        NumOut(80,LCD_LINE2,GetOutput(OUT_C, TachoCount));
        

       //First Rotation
        if(ROTATION1 < 180)  //Left hand turn
        {
              rot1 = ROT_R * ROTATION1/360;  //Gives req trachocount
              target = MotorTachoCount(OUT_A) + rot1;
              while (MotorTachoCount(OUT_A) < target);
                    {OnFwd(OUT_A, 40);}
              Off(OUT_A);
              Wait(500);
         }

         if(ROTATION1 > 180){  //Right hand turn
          {
              rot1 = ROT_L * (ROTATION1 -180) /360;  //Gives req trachocount
              target = MotorTachoCount(OUT_C) + rot1;
              while (MotorTachoCount(OUT_C) < target);
                    {OnFwd(OUT_C, 40);}
              Off(OUT_C);
              Wait(500);
           }

           //Execute second forward motion
        OnFwd(OUT_AC,40);
        Wait(fwd_time2);
        Off(OUT_AC);
        Wait(500);


               //Second Rotation
               rot2 = ROTATION2 - "HEADING";  //
               //***************************************
               //Get Heading from Odometer
               //*************************************888
               
        if(rot2 < 180 && rot2 > 0)  //Left hand turn
        {
        
              rot2 = ROT_R * rot2/360;  //Gives req trachocount
              target = MotorTachoCount(OUT_A) + rot2;
              while (MotorTachoCount(OUT_A) < target);
                    {OnFwd(OUT_A, 40);}
              Off(OUT_A);
              Wait(500);
         }
         else{  //Right hand turn
              rot2 = ROT_L * (rot2 -180) /360;  //Gives req trachocount
              target = MotorTachoCount(OUT_C) + rot2;
              while (MotorTachoCount(OUT_C) < target);
                    {OnFwd(OUT_C, 40);}
              Off(OUT_C);
              Wait(500);
           }






}}


