#pragma config(Sensor, S1,     touchSensor,    sensorEV3_Touch)
#pragma config(Sensor, S2,     gyroSensor,     sensorEV3_Gyro, modeEV3Gyro_RateAndAngle)
#pragma config(Sensor, S3,     colorSensor,    sensorEV3_Color, modeEV3Color_Color)
#pragma config(Sensor, S4,     sonarSensor,    sensorEV3_Ultrasonic)
#pragma config(Motor,  motorA,          armMotor,      tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor,  motorB,          leftMotor,     tmotorEV3_Large, PIDControl, driveLeft, encoder)
#pragma config(Motor,  motorC,          rightMotor,    tmotorEV3_Large, PIDControl, driveRight, encoder)
#pragma config(StandardModel, "EV3_REMBOT")
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void turnleft();
void turnright();
void checkror();
float Roleft=-83;
float Roright=87;

task main()
{
    int s=25;

    wait1Msec(1000);
    playSound(soundLowBuzzShort);
    moveMotorTarget(leftMotor, 470, s-10); //firstwalk
    moveMotorTarget(rightMotor, 470, s-10);

    waitUntilMotorStop(leftMotor);
    waitUntilMotorStop(rightMotor);
    turnright(); //turn0
    moveMotorTarget(leftMotor, 560, s);
    moveMotorTarget(rightMotor, 560, s);
    waitUntilMotorStop(leftMotor);
    waitUntilMotorStop(rightMotor);
    turnleft(); //turn1
    moveMotorTarget(leftMotor, 640, s);
    moveMotorTarget(rightMotor, 640, s);
    waitUntilMotorStop(leftMotor);
    waitUntilMotorStop(rightMotor);
    turnleft(); //--

    moveMotorTarget(leftMotor, 550, s);
    moveMotorTarget(rightMotor, 550, s);
    waitUntilMotorStop(leftMotor);
    waitUntilMotorStop(rightMotor);
    moveMotorTarget(leftMotor, 600, s);
    moveMotorTarget(rightMotor, 600, s);
    waitUntilMotorStop(leftMotor);
    waitUntilMotorStop(rightMotor);
    turnright(); //turn2
    moveMotorTarget(leftMotor, 670, s-10);
    moveMotorTarget(rightMotor, 670, s-10);
    waitUntilMotorStop(leftMotor);
    waitUntilMotorStop(rightMotor);
    turnright(); //--
    wait1Msec(10);
    moveMotorTarget(leftMotor, 1220, s-10);
    moveMotorTarget(rightMotor, 1220, s-10);
    waitUntilMotorStop(leftMotor);
    waitUntilMotorStop(rightMotor);
    moveMotorTarget(leftMotor, 1200, s-10);
    moveMotorTarget(rightMotor, 1200, s-10);
    waitUntilMotorStop(leftMotor);
    waitUntilMotorStop(rightMotor);
    turnleft(); //turn3
    moveMotorTarget(leftMotor, 600, s);
    moveMotorTarget(rightMotor, 600, s);
    waitUntilMotorStop(leftMotor);
    waitUntilMotorStop(rightMotor);
    turnleft(); //--
    moveMotorTarget(leftMotor, 3120, s-10);
    moveMotorTarget(rightMotor, 3120, s-10);
    waitUntilMotorStop(leftMotor);
    waitUntilMotorStop(rightMotor);
    turnright(); //turn4
    moveMotorTarget(leftMotor, 660, s);
    moveMotorTarget(rightMotor, 660, s);
    waitUntilMotorStop(leftMotor);
    waitUntilMotorStop(rightMotor);
    turnright();     //--
    moveMotorTarget(leftMotor, 1190, s-10);
    moveMotorTarget(rightMotor, 1190, s-10);
    waitUntilMotorStop(leftMotor);
    waitUntilMotorStop(rightMotor);
    moveMotorTarget(leftMotor, 1200, s-10);
    moveMotorTarget(rightMotor, 1200, s-10);
    waitUntilMotorStop(leftMotor);
    waitUntilMotorStop(rightMotor);
    moveMotorTarget(leftMotor, 1400, s-10);
    moveMotorTarget(rightMotor, 1400, s-10);
    waitUntilMotorStop(leftMotor);
    waitUntilMotorStop(rightMotor);
    turnleft(); //turn5
    moveMotorTarget(leftMotor, 640, s);
    moveMotorTarget(rightMotor, 640, s);
    waitUntilMotorStop(leftMotor);
    waitUntilMotorStop(rightMotor);
    turnleft(); //--
    moveMotorTarget(leftMotor, 1280, s-10);
    moveMotorTarget(rightMotor, 1280, s-10);
    waitUntilMotorStop(leftMotor);
    waitUntilMotorStop(rightMotor);
    moveMotorTarget(leftMotor, 1550, s-10);
    moveMotorTarget(rightMotor, 1550, s-10);
    waitUntilMotorStop(leftMotor);
    waitUntilMotorStop(rightMotor);
    moveMotorTarget(leftMotor, 1600, s-10);
    moveMotorTarget(rightMotor, 1600, s-10);
    waitUntilMotorStop(leftMotor);
    waitUntilMotorStop(rightMotor);
    turnright(); //turn6
    moveMotorTarget(leftMotor, 620, s);
    moveMotorTarget(rightMotor, 620, s);
    waitUntilMotorStop(leftMotor);
    waitUntilMotorStop(rightMotor);
    turnright(); //--
    checkror();
    wait1Msec(10);
    checkror();
    wait1Msec(10);
    moveMotorTarget(leftMotor, 1790,s);
    moveMotorTarget(rightMotor, 1790, s);
    waitUntilMotorStop(leftMotor);
    waitUntilMotorStop(rightMotor);
    turnleft(); //lastturn
    moveMotorTarget(leftMotor, 560, s);
    moveMotorTarget(rightMotor, 560, s);
    waitUntilMotorStop(leftMotor);
    waitUntilMotorStop(rightMotor);
}

void turnright(){
    resetGyro(S2);
        playSound(soundBeepBeep);
    repeatUntil(getGyroDegrees(S2) > Roright)
    {
        setMotorSpeed(leftMotor, 10);
        setMotorSpeed(rightMotor, -10);
    }
    setMotorSpeed(leftMotor, 0);
    setMotorSpeed(rightMotor, 0);
}

void turnleft(){
    resetGyro(S2);
        playSound(soundBeepBeep);
    repeatUntil(getGyroDegrees(S2) < Roleft)
    {
        setMotorSpeed(leftMotor, -10);
        setMotorSpeed(rightMotor, 10);
    }
    setMotorSpeed(leftMotor, 0);
    setMotorSpeed(rightMotor, 0);
}

void checkror(){
    if(getGyroDegrees(S2)<Roright || getGyroDegrees(S2)>Roright ){
    repeatUntil((getGyroDegrees(S2)>Roright-1 )&& (getGyroDegrees(S2)< Roright+1) ){
            if(getGyroDegrees(S2)<Roright-1 ){
                setMotorSpeed(leftMotor,1);
                setMotorSpeed(rightMotor,-1);
            }
            else if(getGyroDegrees(S2)>=Roright+1 ){
                setMotorSpeed(leftMotor,-1);
                setMotorSpeed(rightMotor,1);
            }
        }
        setMotorSpeed(leftMotor,0);
        setMotorSpeed(rightMotor,0);
        //resetGyro(S2);
    }
}