#pragma config(Sensor, S1,     TouchRight,          sensorTouch)
#pragma config(Sensor, S2,     SonarSensor,         sensorSONAR)
#pragma config(Sensor, S4,     TouchLeft,           sensorTouch)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main(){

  //wait10Msec(1000); (for video)

  nMotorPIDSpeedCtrl[motorA] = mtrSpeedReg;
  nMotorPIDSpeedCtrl[motorC] = mtrSpeedReg;

  while(true){
    if(SensorValue(TouchLeft) == 0 && SensorValue(TouchRight) == 0){
      motor[motorA] = -60;
      motor[motorC] = -60;
      if(nMotorEncoder[motorB] > 360){
        motor[motorB] = 0;
      }
    }

    if(SensorValue(TouchLeft) == 0 && SensorValue(TouchRight) == 1){
      motor[motorA] = 0;
      motor[motorC] = -25;
    }

    if(SensorValue(TouchLeft) == 1 && SensorValue(TouchRight) == 0){
      motor[motorA] = -25;
      motor[motorC] = 0;
    }

    if(SensorValue(TouchLeft) == 1 && SensorValue(TouchRight) == 1){
      motor[motorA] = 0;
      motor[motorC] = 0;
      if(SensorValue(SonarSensor) > 9){
         PlaySound(soundBeepBeep);
         nMotorEncoder[motorB] = 0;
         nMotorEncoderTarget[motorB] = 360;
         motor[motorB] = -35 ;
         while(nMotorRunState[motorB] != runStateIdle){}
         motor[motorB] = 0;
       }
       else{
         while(SensorValue(SonarSensor) < 25){
           motor[motorA] = 25;
           motor[motorC] = 25;
         }
        nMotorEncoder[motorA] = 0;
        nMotorEncoder[motorC] = 0;
        nMotorEncoderTarget[motorA] = -215;
        nMotorEncoderTarget[motorC] = 215;
        motor[motorA] = -25;
        motor[motorC] = 25;
        while(nMotorRunState[motorA] != runStateIdle && nMotorRunState[motorC] != runStateIdle){}
       }
    }
  }
}