#pragma config(Sensor, in4,    rightLight,          sensorReflection)
#pragma config(Sensor, in5,    leftLight,           sensorReflection)
#pragma config(Sensor, in9,    Ultrasonic,          sensorSONAR, int1)
#pragma config(Motor,  port2,           rightMotor,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port3,           leftMotor,     tmotorNormal, openLoop)
#pragma config(Motor,  port8,           servo,         tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//Directions:
//    1: Forward
//    2: Left
//    1: Right
//    1: Backward

int move(int timeMoving,int direction);
void turn_off();

task main () {
   
    // Starting Values
    int darkValue = 180;
    int robotMaxDistance = 20; //Robot sensed with 60"
    //Wait Seven Seconds after turned on
    turn_off();
    wait1Msec(3000);
    //Continuously Loop
    while (true) {
//Detect for Robot
if (SensorValue(Ultrasonic) < robotMaxDistance) {
//turn servo motor left
motor[port8] = -127;
} else {
//turn servo motor right
motor[port8] = 127;
}
//Detect edges of Ring
if(SensorValue[rightLight] >= darkValue || SensorValue[leftLight] >= darkValue)  {
move(80, 90);
//if both sensors are black move away
if (SensorValue[rightLight] >= darkValue && SensorValue[leftLight] >= darkValue){
turn_off();
move(600, 4);
move(800, 3);
}
//if right sensor is black move away
if(SensorValue[rightLight]  >= darkValue) {
turn_off();
move(600, 4);
move(800, 2);
}
//if left sensor is black move away
if(SensorValue[leftLight] >= darkValue) {
turn_off();
move(600, 4);
move(800, 3);
}
//// if the sonar pointed toward the left, turn right,
} else if (motor[port8] >= 50) {
move(1, 3);
//if the sonar pointed toward the left, turn left,
} else if (motor[port8] <= -50) {
move(1, 2);
//else if it's not right or left, move forward
} else {
move(1, 1);
}
    }       
}

int move(int timeMoving,int direction) {
    //beginning variables set
    int left_speed;
    int right_speed;
    int leftMotorValue = 127;
    int rightMotorValue = 100;
//if forward
    if (direction == 1) {
left_speed = leftMotorValue;
right_speed = rightMotorValue;
//if left
} else if (direction == 2) {
left_speed = leftMotorValue;
right_speed = -rightMotorValue;
//if right
} else if (direction == 3) {
left_speed = -leftMotorValue;
right_speed = rightMotorValue;
//if backward
} else if (direction == 4) {
left_speed = -leftMotorValue;
right_speed = -rightMotorValue;
}
    //move
motor[port2] = left_speed;
motor[port3] = right_speed;
    //stay moving
wait1Msec(timeMoving);
return 0;
}

void turn_off() {
    //Shut down
    motor[port3]= 0;
    motor[port2]= 0;
    //wait
    wait1Msec(100);
}

#pragma config(Motor,  port8,           servo,         tmotorNormal, openLoop)