IT GOES NORTH MOST OF THE TIME
Dependencies: HMC6352 Motor mbed
main.cpp
- Committer:
- vsavkin3
- Date:
- 2012-10-04
- Revision:
- 1:6812f830e5a1
- Parent:
- 0:1a0fa585e36c
- Child:
- 2:25d2332ec855
- Child:
- 3:4ee5247f47ec
File content as of revision 1:6812f830e5a1:
#include "mbed.h" #include "Motor.h" 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){ 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); } }