#pragma config(Sensor, dgtl1, Quad,

sensorQuadEncoder)
#pragma config(Sensor, dgtl3, Bump0,
sensorTouch)
#pragma config(Sensor, dgtl4, Bump1,
sensorTouch)
#pragma config(Sensor, dgtl5, Sonar,
sensorSONAR_cm)
#pragma config(Sensor, dgtl7, Off,
sensorLEDtoVCC)
#pragma config(Sensor, dgtl8, Code1,
sensorLEDtoVCC)
#pragma config(Sensor, dgtl9, Code2,
sensorLEDtoVCC)
#pragma config(Motor, port1,
ReelMotor,
tmotorVex393_HBridge, ope
nLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard
!!*//
bool bitA(){
//Retrieves the first bit of the current input.
if(SensorValue[dgtl1] % 360 < 180){
return true;
}else{
return false;
}
}
bool bitB(){
//Retrieves the second bit of the current input.
if(SensorValue[dgtl1] % 360 >= 270 || SensorValue[dgtl1] % 360 < 90){
return true;
}else{
return false;
}
}
int distance = 0;

//length of cable

int code = 1;

//0: working, 1-3: waiting for input cycle n

void addDistance(){
//Adds input to distance.
if(code > 0 && SensorValue[dgtl3] == 1){
//Input if code is 1-3 an
d input switch is pressed.
float a = 2 * (3 - (float)code);
distance += bitA() * (int)pow(2,a) * 2;
distance += bitB() * (int)pow(2,a);
//Converts the quaternar
y digit to decimal and adds the result to distance.
code ++;
if(code == 4){
code = 0;
//Sets code
up for the next step.
}
delay(500);
}
}
void reelCable(){
//Starts or stops motor.
if(code == 0){
if(SensorValue[dgtl5] < distance){
motor[port1] = 42;
}else{
motor[port1] = 0;
code = 6;
}
}
}
bool on;

void displayCode(){
//Displays code on LEDs.
if(on == false){
SensorValue[dgtl7] = 1;
}else{
SensorValue[dgtl7] = 0;
}
if(code == 1 || code == 3){
SensorValue[dgtl8] = 1;
}else{
SensorValue[dgtl8] = 0;
}
if(code == 2 || code == 3){
SensorValue[dgtl9] = 1;
}else{
SensorValue[dgtl9] = 0;
}
}
task main(){
while(code < 6){
on = true;
while(SensorValue[dgtl4] == 0 && code < 6){
addDistance();
reelCable();
displayCode();
delay(1);
}
motor[port1] = 0;
delay(500);
on = false;
displayCode();
while(SensorValue[dgtl4] == 0 && code < 6){
delay(1);
}
delay(500);
}
SensorValue[dgtl7] = 0;
}