task main() {
RobotState state;
initialize(&state);
short leftSpeed, rightSpeed;
short prevState = INITIALSTATE;
bool distLess50;
bool irDetected = false;
bool sawRedBlue = false;
bool goBackward = false;
float startAngle = 0;
//waitForStart();
wait1Msec(state.delayTime*1000);
INITIALDRIVE();
while(true){
//if state changes: stop motors, play tone, reset timers, gyro and lights
if (prevState != state.currentState){
if (prevState != PARK_DRIVE2 && prevState != CHECKEND){
stopMotors();
leftSpeed = 0;
rightSpeed = 0;
}
AUDIO_DEBUG(500, 10);
time1[T1] = 0;
resetGyroAccel(&state);
LEDController(0x00);
distLess50 = false;
startAngle = state.degrees;
}
getSensors(&state);
prevState = state.currentState;
if(state.currentState == FINDLINE_TURN){
drive(0, TURNSPEED);
if(state.color2 == RED || state.color2 == BLUE){
sawRedBlue = true;
}
if (sawRedBlue && state.color2 == BLACK){
state.currentState = LINEFOLLOW;
}
if(abs(state.degrees) > 10){
state.currentState = LINEFOLLOW;
}
/* state FINDLINE_DRIVE seems unnecessary
} else if (state.currentState == FINDLINE_DRIVE) {
drive(DRIVESPEED, DRIVESPEED*.95);
if (state.irDir == 5 && irDetected == false) {
state.currentState = SCOREBLOCK;
} else if(state.color2 == RED || state.color2 == BLUE){
state.currentState = LINEFOLLOW;
} */
} else if (state.currentState == LINEFOLLOW) {
if (state.dist < 50) {
distLess50 = true;
}
if (state.irDir == 5 && irDetected == false) {
state.currentState = SCOREBLOCK;
} else if (state.dist > 50 && distLess50 && irDetected == true) {
state.currentState = GOTOEND;
} else if (goBackward){
if (state.color == RED || state.color == BLUE) {
leftSpeed = -DRIVESPEED*LINEFOLLOWRATIO;
rightSpeed = -DRIVESPEED;
}else {
leftSpeed = -DRIVESPEED;
rightSpeed = -DRIVESPEED*LINEFOLLOWRATIO;
}
} else {
if (state.color2 == RED || state.color2 == BLUE) {
leftSpeed = DRIVESPEED*LINEFOLLOWRATIO;
rightSpeed = DRIVESPEED;
} else {
leftSpeed = DRIVESPEED;
rightSpeed = DRIVESPEED*LINEFOLLOWRATIO;
}
}
drive(leftSpeed, rightSpeed);
} else if (state.currentState == SCOREBLOCK) {
irDetected = true;
goBackward = true;
LEDController(B2);
blockScorer();
state.currentState = LINEFOLLOW;
} else if (state.currentState == GOTOEND) {
drive(-DRIVESPEED, -DRIVESPEED);
if(time1[T1] >= 300) {
state.currentState = PARK_TURN1;
}
} else if (state.currentState == PARK_TURN1) {
drive(-TURNSPEED, TURNSPEED);
if(abs(state.degrees) >= 15){
state.currentState = PARK_DRIVE1;
}
} else if (state.currentState == PARK_DRIVE1) {
drive(-DRIVESPEED, -DRIVESPEED);
if(state.color == RED || state.color == BLUE){
state.currentState = PARK_TURN2;
}
} else if (state.currentState == PARK_TURN2) {
DRIVESPECIAL(-TURNSPEED, TURNSPEED);
if(abs(state.degrees) >= 22.5){
state.currentState = PARK_DRIVE2; //skip state HARVEST
}
} else if (state.currentState == HARVEST) {
motor[harvester] = 100;
DRIVESPECIAL(2*DRIVESPEED, 2*DRIVESPEED);
if (time1[T1] >= 500){
leftSpeed = startAngle/abs(startAngle)*TURNSPEED;
rightSpeed = -startAngle/abs(startAngle)*TURNSPEED;
drive(leftSpeed, rightSpeed);
if (abs(startAngle-state.degrees) <= 0.5){
state.currentState = PARK_DRIVE2;
}
}
} else if (state.currentState == PARK_DRIVE2) {
DRIVESPECIAL(-2*DRIVESPEED, -2*DRIVESPEED);
if(abs(state.x_accel) > 35 && state.x_accel < 100){
wait1Msec(20);
state.currentState = CHECKEND;
}
} else if (state.currentState == CHECKEND) {
if(abs(state.x_accel) > 35 && abs(state.x_accel) < 100){
state.currentState = END; //if it's > 30 after 1 sec of pushing, you're done
} else {
state.currentState = PARK_DRIVE2; //else you need to do more pushing
}
} else if (state.currentState == END){
DRIVESPECIAL(-2*DRIVESPEED, -2*DRIVESPEED);
if(time1[T1] >= 1000){
break;
}
} else {
break;
}
}
}
task main() {
RobotState state;
initialize(&state);
short leftSpeed, rightSpeed;
short prevState = INITIALSTATE;
bool distLess50;
bool irDetected = false;
bool goBackward = false;
float startAngle = 0;
int numTimeAccelTriggered = 0;
waitForStart();
wait1Msec(state.delayTime*1000);
INITIALDRIVE();
while(true){
//if state changes: stop motors, play tone, reset timers, gyro and lights
if (prevState != state.currentState){
if (prevState != PARK_DRIVE2 && prevState != CHECKEND){
stopMotors();
leftSpeed = 0;
rightSpeed = 0;
}
AUDIO_DEBUG(500, 10);
time1[T1] = 0;
resetGyroAccel(&state);
LEDController(0x00);
distLess50 = false;
startAngle = state.degrees;
}
getSensors(&state);
prevState = state.currentState;
if(state.currentState == FINDLINE_TURN){
drive(0, TURNSPEED);
if(abs(state.degrees) > 10){
state.currentState = LINEFOLLOW;
}
} else if (state.currentState == LINEFOLLOW) {
if (state.dist < 50) {
distLess50 = true;
}
if (state.irDir == 5 && irDetected == false) {
state.currentState = SCOREBLOCK;
} else if (state.dist > 50 && distLess50 && irDetected == true){
state.currentState = GOTOEND;
} else if (goBackward){
leftSpeed = -DRIVESPEED;
rightSpeed = -DRIVESPEED;
} else {
leftSpeed = DRIVESPEED;
rightSpeed = DRIVESPEED;
}
drive(leftSpeed, rightSpeed);
} else if (state.currentState == SCOREBLOCK) {
irDetected = true;
goBackward = true;
LEDController(B2);
blockScorer();
state.currentState = LINEFOLLOW;
} else if (state.currentState == GOTOEND) {
drive(-DRIVESPEED, -DRIVESPEED);
if(time1[T1] >= 300) {
state.currentState = PARK_TURN1;
}
} else if (state.currentState == PARK_TURN1) {
drive(-TURNSPEED, TURNSPEED);
if(abs(state.degrees) >= 15){
state.currentState = PARK_DRIVE1;
}
} else if (state.currentState == PARK_DRIVE1) {
drive(-DRIVESPEED, -DRIVESPEED);
if((state.color == RED || state.color == BLUE) && time1[T1] > 500){
state.currentState = PARK_TURN2;
}
} else if (state.currentState == PARK_TURN2) {
DRIVESPECIAL(-TURNSPEED, TURNSPEED);
if(abs(state.degrees) >= 27){
state.currentState = PARK_DRIVE2; //skip state HARVEST
}
} else if (state.currentState == HARVEST) {
motor[harvester] = 100;
DRIVESPECIAL(2*DRIVESPEED, 2*DRIVESPEED);
if (time1[T1] >= 500){
leftSpeed = startAngle/abs(startAngle)*TURNSPEED;
rightSpeed = -startAngle/abs(startAngle)*TURNSPEED;
drive(leftSpeed, rightSpeed);
if (abs(startAngle-state.degrees) <= 0.5){
state.currentState = PARK_DRIVE2;
}
}
} else if (state.currentState == PARK_DRIVE2) {
DRIVESPECIAL(-2*DRIVESPEED, -2*DRIVESPEED);
if(abs(state.x_accel) > 35 && time1[T1] >= 300){
numTimeAccelTriggered++;
} else {
numTimeAccelTriggered = 0;
}
if (numTimeAccelTriggered >= 3) {
state.currentState = END;
}
wait1Msec(20);
} else if (state.currentState == END){
DRIVESPECIAL(-2*DRIVESPEED, -2*DRIVESPEED);
if(time1[T1] >= 300){
break;
}
} else {
break;
}
}
}