IT GOES NORTH MOST OF THE TIME
Dependencies: HMC6352 Motor mbed
Revision 11:0fd03f66af0d, committed 2012-10-10
- Comitter:
- vsavkin3
- Date:
- Wed Oct 10 08:27:10 2012 +0000
- Parent:
- 10:168a1b176c77
- Child:
- 12:204600e080c4
- Commit message:
- FUUUUU
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Oct 10 08:22:18 2012 +0000 +++ b/main.cpp Wed Oct 10 08:27:10 2012 +0000 @@ -17,7 +17,7 @@ enum direction dir=Forward; compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); - printf("Front Right Left\n\r"); + //printf("Front Right Left\n\r"); int j=0; float readLeft; float readRight; @@ -63,9 +63,9 @@ stable[4][j] = avgFront; j++; - printf("Front: %5f Right: %5f Left: %5f dLeft: %5f dRight: %5f", avgFront, avgRight, avgLeft,diagLeft,diagRight); + //printf("Front: %5f Right: %5f Left: %5f dLeft: %5f dRight: %5f", avgFront, avgRight, avgLeft,diagLeft,diagRight); float globalDir = compass.sample()/10.0; - printf(" Heading: %f\n\r", globalDir); + //printf(" Heading: %f\n\r", globalDir); switch(dir){ case Forward: @@ -75,7 +75,7 @@ { right.speed(0); left.speed(0); - printf("stopped!\n\r"); + //printf("stopped!\n\r"); //wait(0.1); NorthOn=0; dir=Right; @@ -85,7 +85,7 @@ { right.speed(0); left.speed(0); - printf("stopped2!\n\r"); + //printf("stopped2!\n\r"); //wait(0.1); NorthOn=0; dir=Left; @@ -97,14 +97,14 @@ left.speed(0); dir = TurnNorth; } - else {right.speed(0.95); left.speed(1); - printf("CHARGE!\n\r"); - if((avgLeft<35&&stable[0][0]<35&&stable[0][1]<35) - &&(avgRight<35&&stable[1][0]<35&&stable[1][1]<35)) NorthOn=1; + else {right.speed(1); left.speed(1); + //printf("CHARGE!\n\r"); + if((avgLeft>35&&stable[0][0]>35&&stable[0][1]>35) + &&(avgRight>35&&stable[1][0]>35&&stable[1][1]>35)) NorthOn=1; } break; case Right: - printf("TURN Right!\n\r"); + ////printf("TURN Right!\n\r"); if ( (avgFront<=35||stable[4][0]<35||stable[4][1]<35)/*||stable[4][2]<35||stable[4][3]<35)*/ || (diagLeft<=35||stable[2][0]<35||stable[2][1]<35)/*||stable[2][2]<35||stable[2][3]<35)*/ @@ -112,20 +112,20 @@ { right.speed(-1); left.speed(0); - // printf("turning!\n\r"); + // //printf("turning!\n\r"); } else { right.speed(0); left.speed(0); - // printf("stopped!\n\r"); + // //printf("stopped!\n\r"); //wait(0.1); dir=Forward; } break; case Left: - printf("TURN Left!\n\r"); + //printf("TURN Left!\n\r"); if ( /*(avgRight<=31.7 ||stable[1][0]<30||stable[1][1]<30||stable[1][2]<30||stable[1][3]<30)||*/ (avgFront<=35||(stable[4][0]<35||stable[4][1]<35))/*&&(stable[4][2]<35||stable[4][3]<35))*/ @@ -134,20 +134,20 @@ { right.speed(0); left.speed(-1); - // printf("turning!\n\r"); + // //printf("turning!\n\r"); } else { right.speed(0); left.speed(0); - // printf("stopped!\n\r"); + // //printf("stopped!\n\r"); //wait(0.1); dir=Forward; } break; case TurnNorth: - printf("TO THE NORTH!\n\r"); + //printf("TO THE NORTH!\n\r"); if( (avgLeft<35||stable[0][0]<35||stable[0][1]<35) || @@ -158,7 +158,10 @@ (diagRight<35||stable[3][0]<35||stable[3][1]<35) || (avgFront<35||stable[4][0]<35||stable[4][1]<35) - + ) + {right.speed(0); + left.speed(0); + dir=Forward;} if (globalDir>330 || globalDir < 30) { //It's going north!!