#pragma config(Sensor, dgtl1,  red,            sensorLEDtoVCC)
#pragma config(Sensor, dgtl2,  yellow,         sensorLEDtoVCC)
#pragma config(Sensor, dgtl3,  green,          sensorLEDtoVCC)
#pragma config(Sensor, dgtl4,  butt1,          sensorTouch)
#pragma config(Sensor, dgtl5,  butt2,          sensorTouch)
#pragma config(Sensor, dgtl6,  butt3,          sensorTouch)
#pragma config(Sensor, dgtl11, sonarSensor,    sensorSONAR_cm)
#pragma config(Motor,  port2,           leftmotor,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,           rightmotor,    tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           elevatormotor, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port5,           light,         tmotorVexFlashlight, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

int sonar_value;

int floor1 = 240;
int floor2 = 140;
int floor3 = 30;
int floors[] = {24, 14, 4};
int targetFloor = 1;

task main()
{
    sonar_value = SensorValue(sonarSensor);
    while(true){
        sonar_value = SensorValue(sonarSensor);

        startMotor(rightmotor, vexRT[Ch2]);
        startMotor(leftmotor, vexRT[Ch3]);
        if(vexRT[Btn6U]){
            startMotor(elevatormotor, 127);
        }
        else if(vexRT[Btn6D]){
            startMotor(elevatormotor, -127);
        }
        if(!vexRT[Btn6U] && !vexRT[Btn6D] && !vexRT[Btn8D] && !vexRT[Btn8R] && !vexRT[Btn8U]){
            startMotor(elevatormotor, 0);
        }

        if(SensorValue(butt1)){
            targetFloor = 1;
        }
        if(SensorValue(butt2)){
            targetFloor = 2;
        }
        if(SensorValue(butt3)){
            targetFloor = 3;
        }
        if(sonar_value > floors[targetFloor - 1]){
            startMotor(elevatormotor, -65);
        }
        else if(sonar_value < floors[targetFloor - 1]){
            startMotor(elevatormotor, 40);
        }
        else{
            stopMotor(elevatormotor);
        }
        if(sonar_value <= floor3){
            turnLEDOn(green);
        }
        else{
            turnLEDOff(green);
        }
        if(sonar_value > floor3 && sonar_value <= floor2){
            turnLEDOn(yellow);
        }
        else{
            turnLEDOff(yellow);
        }
        if(sonar_value > floor2 && sonar_value <= floor1){
            turnLEDOn(red);
        }
        else{
            turnLEDOff(red);
        }
    }
}