#pragma config(Sensor, dgtl1,  EncR,           sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  EncL,           sensorQuadEncoder)
#pragma config(Sensor, dgtl11, Sonar,          sensorSONAR_cm)
#pragma config(Motor,  port1,           RightMotor,    tmotorVex393_HBridge, openLoop, reversed, driveRight)
#pragma config(Motor,  port2,           ARM,           tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,           Claw,          tmotorServoStandard, openLoop)
#pragma config(Motor,  port10,          LeftMotor,     tmotorVex393_HBridge, openLoop, reversed, driveLeft)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void ResetEncoders()
{ SensorValue[EncL] = 0;
    SensorValue[EncR] = 0;
}


void DriveForwardUntilWall()
{
  while(SensorValue[Sonar] >= 30) {
    motor[LeftMotor] = 60;
    motor[RightMotor] = 60;}
}
void TurnRight(){

        while(SensorValue[EncR] < 270 || SensorValue[EncL] > -270)
{
        if(SensorValue[EncR] < 270) //PosEnc forward shaft rotation
{
        motor[RightMotor] = 63;
}
    else
{
        motor[RightMotor] = 0;
}

if(SensorValue[EncL] > -270) //NegEnc reversed shaft rotation
{
 motor[LeftMotor] = -63;
}
else
{
 motor[LeftMotor] = 0;
}
}

void TurnLeft(){

        while(SensorValue[EncL] < -270 || SensorValue[EncR] > 270)
{
        if(SensorValue[EncL] < 270) //PosEnc forward shaft rotation
{
        motor[LeftMotor] = 63;
}
    else
{
        motor[LeftMotor] = 0;
}

if(SensorValue[EncR] > -270) //NegEnc reversed shaft rotation
{
 motor[RightMotor] = -63;
}
else
{
 motor[RightMotor] = 0;
}
}


}

void Stop(){
  motor[LeftMotor] = 0;
  motor[RightMotor] = 0;
  }

task main(){
        if (vexRT[Btn5U] == 1){
            ResetEncoders();
        wait1Msec(5000);
        DriveForwardUntilWall();
        ResetEncoders();
        wait1Msec(5000);
        TurnLeft();
        Stop();
        ResetEncoders();
        DriveForwardUntilWall();
        TurnRight();
        Stop();
        DriveForwardUntilWall();
        TurnRight();
        Stop();
        ResetEncoders();
        DriveForwardUntilWall();
    return;
    }
        else{
            setMotor(port3, vexRT[Ch2]/2);
            if (vexRT[Btn8D] == 1){
            arcadeControl(Ch3, Ch4, 18);
            }
            }
    armControl(port2, Btn7U, Btn7D, 75);
    }