IT GOES NORTH MOST OF THE TIME
Dependencies: HMC6352 Motor mbed
main.cpp
- Committer:
- vsavkin3
- Date:
- 2012-10-04
- Revision:
- 4:9011798d1a20
- Parent:
- 3:4ee5247f47ec
- Child:
- 5:99545cf4dff4
File content as of revision 4:9011798d1a20:
#include "mbed.h" #include "Motor.h" #include "HMC6352.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); enum direction{ Right, Forward }; int main() { enum direction dir=Forward; printf("Front Right Left\n\r"); int i=0; float readLeftVals[10]; float readRightVals[10]; float 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=21/readLeft; readRight=irRight; readRight=21/readRight; readFront=irFront; readFront=21/readFront; readFrontVals[i] = readFront; readRightVals[i] = readRight; readLeftVals[i] = readLeft; } while (1){ readLeft=irLeft; readLeft=21/readLeft; readRight=irRight; readRight=21/readRight; readFront=irFront; readFront=21/readFront; if (i>=10) { i=0; } readFrontVals[i] = readFront; readRightVals[i] = readRight; readLeftVals[i] = readLeft; i++; float 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; } avgFront = readFront; avgRight = readRight; //printf("Front: %5f Right: %5f Left: %5f\n\r", avgFront, avgRight, avgLeft); switch(dir){ case Forward: if (avgFront <= 40) { right.speed(0); left.speed(0); // printf("stopped!\n\r"); //wait(0.1); dir=Right; // printf("less than 18\n\r"); } else { right.speed(1); left.speed(1); } break; case Right: if (avgLeft>=50 || avgFront<=40) { right.speed(-1); left.speed(0); // printf("turning!\n\r"); } else { right.speed(0); left.speed(0); // printf("stopped!\n\r"); //wait(0.1); dir=Forward; } break; } wait(0.01); } }