#pragma config(Sensor, S1,     UltraSonicSensor, sensorEV3_Ultrasonic)
#pragma config(Sensor, S2,     GyroSensor,     sensorEV3_Gyro)
#pragma config(Sensor, S4,     LightSensor,    sensorEV3_Color)
#pragma config(Motor,  motorA,          LeftMotor,     tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor,  motorB,          LiftMotor,     tmotorEV3_Medium, PIDControl, encoder)
#pragma config(Motor,  motorD,          RightMotor,    tmotorEV3_Large, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void setMotor(int x, int y) {
    motor[LeftMotor] = x;
    motor[RightMotor] = y;

}

void detectObject(){
        const int distanceToMaintain = 4;
        int currentDistance = SensorValue[UltraSonicSensor];
        if(((distanceToMaintain - currentDistance) < 1) && ((distanceToMaintain - currentDistance) > -1)){
            setMotor(0, 0);
            motor[LiftMotor] = -10;
            sleep(500);
            setMotor(10, -10);
            sleep(1500);
            setMotor(10, 10);
            sleep(1000);
            motor[LiftMotor] = 10;
            sleep(500);
            setMotor(-10, -10);
            sleep(1000);
            setMotor(-10, 10);
            sleep(1500);

        }

}


task main()
{
    int color = 8;
    while(true){

        int light = SensorValue[LightSensor];
        displayCenteredBigTextLine(6, "LightSensor: %3d cm", light);
        while (SensorValue[LightSensor] < color) {
            setMotor(30, 0);
            sleep(300);
            detectObject();
        }

        while (SensorValue[LightSensor] >= color){
            setMotor(5, 20);
            detectObject();
        }
        sleep(50);
    }
}