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); } }