#pragma config(Sensor, S4,     distancereadings, sensorEV3_Ultrasonic)
#pragma config(Motor,  motorB,          ultrasonicdrive, tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor,  motorC,          targetdrive,   tmotorEV3_Large, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()

{







   // zero encoders motor b abd c

   resetMotorEncoder(motorB);

   resetMotorEncoder(motorC);

   sleep(3000);





   // setting to turn on rsdar sensor

   setMultipleMotors(5, motorB, motorC);

   moveMotorTarget(motorB , 10, 5);

   moveMotorTarget(motorC, 10, 5);

   sleep(3000);







   // turn on rsdar sensor


   //******** NEED TO TURN ON GETTING MOTOR BE ENCODER GETTING ANGLE AND WRITING TO FILE**********
   //******** NEED TO TURN ON ALTRASONIC SENSOR GETTING DISTANCE AND WRITING TO FILE**************

   //  first half cylcle

   setMultipleMotors(5,motorB,motorC);

   moveMotorTarget(motorB, 170, 5);

   moveMotorTarget(motorC, 170, 5);

   sleep(3000);





   // first full cycle

   setMultipleMotors(-5,motorB,motorC);

   moveMotorTarget(motorB, 160, -5);

   moveMotorTarget(motorC, 160, -5);

   sleep(3000);



   // second half cycle

   setMultipleMotors(5,motorB,motorC);

   moveMotorTarget(motorB, 160, 5);

   moveMotorTarget(motorC, 160, 5);

   sleep(3000);



   // sensond full cycle

   setMultipleMotors(-5,motorB,motorC);

   moveMotorTarget(motorB, 160, -5);

   moveMotorTarget(motorC, 160, -5);

   sleep(3000);



   // turn off radar sensor

   //******** NEED TO TURN OFF GETTING MOTOR BE ENCODER GETTING ANGLE AND WRITING TO FILE**********
   //******** NEED TO TURN OFF ALTRASONIC SENSOR GETTING DISTANCE AND WRITING TO FILE**************
[/color]


   // stop all motors

   stopMultipleMotors(motorB, motorC);

   sleep(3000);





}