IT GOES NORTH MOST OF THE TIME
Dependencies: HMC6352 Motor mbed
Revision 1:6812f830e5a1, committed 2012-10-04
- Comitter:
- vsavkin3
- Date:
- Thu Oct 04 19:11:02 2012 +0000
- Parent:
- 0:1a0fa585e36c
- Child:
- 2:25d2332ec855
- Child:
- 3:4ee5247f47ec
- Commit message:
- Robot sometimes sees front and turns right appropriately until there's nothing stopping it again.
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Oct 02 19:33:05 2012 +0000 +++ b/main.cpp Thu Oct 04 19:11:02 2012 +0000 @@ -3,18 +3,103 @@ Motor right(p21, p23, p22); // pwm, fwd, rev Motor left(p26, p24, p25); // pwm, fwd, rev +AnalogIn irLeft(p20); +AnalogIn irFront(p19); +AnalogIn irRight(p18); +AnalogIn sonarLeft(p17); + +enum direction{ Right, Forward }; int main() { + enum direction dir=Forward; + + printf("Front Right Left\n\r"); + int i=0; + int readLeftVals[10]; + int readRightVals[10]; + int readFrontVals[10]; + float readLeft; + float readRight; + float readFront; + + //Initialize the averager before the robot even moves + for (int j=0; j<10; j++); + { + readLeft=irLeft; + readLeft=10/readLeft; + + readRight=irRight; + readRight=10/readRight; + + readFront=irFront; + readFront=10/readFront; + + readFrontVals[i] = readFront; + readRightVals[i] = readRight; + readLeftVals[i] = readLeft; + } + while (1){ - for (float s= -1.0; s < 1.0 ; s += 0.01) { - right.speed(s); - left.speed(s); - wait(0.02); - } - for (float s= 1.0; s > -1.0 ; s -= 0.01) { - right.speed(s); - left.speed(s); - wait(0.02); + readLeft=irLeft; + readLeft=10/readLeft; + + readRight=irRight; + readRight=10/readRight; + + readFront=irFront; + readFront=10/readFront; + + + + + if (i>10) + { + i=0; + } + readFrontVals[i] = readFront; + readRightVals[i] = readRight; + readLeftVals[i] = readLeft; + i++; + + int avgFront = 0, avgRight = 0, avgLeft = 0; + + for (int j=0; j<10; j++) + { + avgFront += readFrontVals[j]/10; + avgLeft += readLeftVals[j]/10; + avgRight += readRightVals[j]/10; } + + printf("Front: %5f Right: %5f Left: %5f\n\r", avgFront, avgRight, avgLeft); + + switch(dir){ + case Forward: + if (avgFront <= 18) + { + wait(0.5); + dir=Right; + printf("less than 18\n\r"); + } + else + { + right.speed(1); + left.speed(1); + } + break; + case Right: + if (avgLeft>30 || avgFront<=18) + { + right.speed(-1); + left.speed(1); + printf("turning!\n\r"); + } + else + { + wait(0.5); + dir=Forward; + } + break; + } + wait(0.01); } }