IT GOES NORTH MOST OF THE TIME
Dependencies: HMC6352 Motor mbed
Diff: main.cpp
- Revision:
- 6:2b89c05628ce
- Parent:
- 5:99545cf4dff4
- Child:
- 7:60e5e064698d
--- a/main.cpp Thu Oct 04 22:05:37 2012 +0000 +++ b/main.cpp Thu Oct 04 22:53:42 2012 +0000 @@ -9,7 +9,7 @@ AnalogIn irRight(p18); HMC6352 compass(p9, p10); -enum direction{ Right, Forward }; +enum direction{ Right, Forward, Left, TurnNorth }; int main() { enum direction dir=Forward; @@ -52,7 +52,6 @@ readFront=21/readFront; - printf("Heading: %f\n\r", compass.sample()/10.0); @@ -75,27 +74,70 @@ } avgFront = readFront; avgRight = readRight; - //printf("Front: %5f Right: %5f Left: %5f\n\r", avgFront, avgRight, avgLeft); + printf("Front: %5f Right: %5f Left: %5f", avgFront, avgRight, avgLeft); + float globalDir = compass.sample()/10.0; + printf(" Heading: %f\n\r", globalDir); switch(dir){ case Forward: + printf("CHARGE!n\r"); if (avgFront <= 40) { right.speed(0); left.speed(0); - // printf("stopped!\n\r"); + printf("stopped!\n\r"); //wait(0.1); dir=Right; - // printf("less than 18\n\r"); } else { - right.speed(1); - left.speed(1); + if (globalDir > 45 && globalDir <= 135) + { + //East + if (avgLeft >= 50) + { + right.speed(0); + left.speed(0); + dir = TurnNorth; + } + else + { + right.speed(1); + left.speed(1); + } + } + else if (globalDir > 135 && globalDir <= 225) + { + //It's going south O.o + right.speed(1); + left.speed(1); + } + else if (globalDir > 225 && globalDir <= 315) + { + //West + if (avgRight >= 50) + { + right.speed(0); + left.speed(0); + dir = TurnNorth; + } + else + { + right.speed(1); + left.speed(1); + } + } + else + { + //It's going north! + right.speed(1); + left.speed(1); + } } break; case Right: - if (avgLeft>=50 || avgFront<=40) + printf("TURN!\n\r"); + if (/*avgLeft>=50 ||*/ avgFront<=40) { right.speed(-1); left.speed(0); @@ -110,6 +152,21 @@ dir=Forward; } break; + case TurnNorth: + printf("TO THE NORTH!\n\r"); + if (globalDir>330 || globalDir < 30) + { + //It's going north!! + right.speed(1); + left.speed(1); + dir=Forward; + } + else + { + right.speed(.5); + left.speed(-.5); + } + break; } wait(0.01); }