void main()
{
//Assumption: Motor A is right motor. Motor B is left motor.
//Assumption: reflect 1 is right sensor, reflect 2 is left sensor.
initialize();
wait1Msec(200);
//int ticks = 4000;
while(getSensor(BUTTON) != 1){}
wait1Msec(200);
while(getSensor(BUTTON) != 0){}
resetTimer();
while (time1() < 4100)
lineFollow();
//Hypothetically, this loop should get us to the
//the point in course C where we are parallel with exit 2.
//Now we turn left.
turnRight(1150);
//We have now turned left and should be pointing at exit 2.
setMotor(MOTOR_A, -100);
setMotor(MOTOR_B, 100);
while (getSensor(BUMPER) != 1);
wait1Msec(200);
setMotor(MOTOR_A, 100);
setMotor(MOTOR_B, -100);
wait1Msec(880);
turnLeft(900);
setMotor(MOTOR_A, -100);
setMotor(MOTOR_B, 100);
//Drive forward to exit 2.
while(getSensor(BUTTON) != 1){}
wait1Msec(200);
while(getSensor(BUTTON) != 0){}
setMotor(MOTOR_A, 0);
setMotor(MOTOR_B, 0);
}
void doShortCourse(int state) {
int nextState = 0;
switch (state) {
case 0: { //move 2 metres to the wall
forwards(20);
while (!wallFound(1)) {
}
nextState = 1;
break;
}
case 1: { //found a wall follow it
setSensorSide(1);//follow left wall
while (wallFound(1)) {
correctForwardMotion();
}
nextState = 2;
break;
}
case 2: { //wall ended find a line
forwards(20);
while (!checkForLine()) {
}
nextState = 3;
break;
}
case 3: { //line found follow it
lineFollow();
nextState = 4;
break;
}
case 4: { // docked please stop.
while(wallFound(3)) {
}
stopLineFollow();
nextState = 7;
break;
}
}
if (nextState != 7){
doShortCourse(nextState);
}
}
void code(){
lastTime = time;
time = micros();
dt = (time-lastTime)/1000000;
followerL[0] = dt;
followerM[0] = dt;
followerR[0] = dt;
lineFollow(followerL);
lineFollow(followerM);
lineFollow(followerR);
//gyroPID();
if(followerM[1] > 700 && followerL[1] > 700 && followerR[1] <= 700){
lastRead = 2;
}
if(followerM[1] <= 700 && followerL[1] > 700 && followerR[1] > 700){
lastRead = 1;
}
if(followerM[1] > 700 && followerL[1] <= 700 && followerR[1] <= 700){
lastRead = 0;
}
if(followerM[1] <= 700 || followerL[1] <= 700 || followerR[1] <= 700){
//driveL.write(left);
driveL.write(90+followerL[5]-followerR[5]+25);
//driveR.write(right);
driveR.write(90-followerR[5]+followerL[5]-25);
}
else{
if(lastRead == 2){
driveL.write(70);
driveR.write(110);
}
else if(lastRead == 1){
driveL.write(110);
driveR.write(70);
}
else{
driveL.write(90);
driveR.write(90);
}
}
serialComms();
yaw = getYaw() - yawOffset;
lastError = error;
error = setPoint - encoderArm.getPosition();
if(dt != 0)
derivative = (error-lastError)/dt;
else
derivative = 0;
integral += error*dt;
output = error*kP + integral*kI + derivative*kD;
if(abs(output) > 90)
output = 90*(abs(output)/output);
if(output < -30)
armMotor.write((-30)+90);
else if(output > 50)
armMotor.write((50)+90);
else
armMotor.write(output+90);
}
void doLongCourse(int state) {
int nextState = 0;
switch (state) {
case 0: { //move 2 metres to the wall
forwards(20);
while (!wallFound(1)) {
}
nextState = 1;
break;
}
case 1: { //found a wall follow it
setSensorSide(1);//follow left wall
while (wallFound(1)) {
correctForwardMotion();
}
nextState = 2;
break;
}
case 2: { //turn away from the wall and run to the right hand side
right();
delay(150);
nextState = 3;
break;
}
case 3:{
forwards(20);
while(!wallFound(2)) {
}
nextState = 4;
break;
}
case 4 :{
setSensorSide(2);
while(wallFound(2)){
correctForwardMotion();
}
nextState = 5;
break;
}
case 5: { //wall ended find a line
forwards(20);
while (!checkForLine()) {
}
nextState = 6;
break;
}
case 6: { //line found follow it
lineFollow();
nextState = 7;
break;
}
case 7: { // docked please stop.
while(wallFound(3)) {
}
stopLineFollow();
nextState = 8;
break;
}
}
if (nextState != 8){
doLongCourse(nextState);
}
}
