IT GOES NORTH MOST OF THE TIME
Dependencies: HMC6352 Motor mbed
main.cpp
- Committer:
- vsavkin3
- Date:
- 2012-10-04
- Revision:
- 2:25d2332ec855
- Parent:
- 1:6812f830e5a1
File content as of revision 2:25d2332ec855:
#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; 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++); { readFront=irFront; readLeft=irLeft; readRight=irRight; readLeft=readLeft*3.3/.0032; readRight=readRight*3.3/.0032; readFront=readFront*3.3/.0032; readFrontVals[i] = readFront; readRightVals[i] = readRight; readLeftVals[i] = readLeft; } while (1){ readLeft=irLeft; readRight=irRight; readFront=irFront; readLeft=readLeft*3.3/.0032; readRight=readRight*3.3/.0032; readFront=readFront*3.3/.0032; 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; } printf("Front: %5f Right: %5f Left: %5f\n\r", avgFront, avgRight, avgLeft); avgFront = readFront; avgLeft = readLeft; switch(dir){ case Forward: if (avgFront >= 520) { left.speed(0); right.speed(0); dir=Right; wait(0.5); printf("less than 18\n\r"); } else { right.speed(1); left.speed(1); } break; case Right: if (avgFront >= 470 || avgLeft<=510) { //if(avgFront <=480){dir=Forward; wait(0.5);} right.speed(-1); left.speed(1); printf("turning!\n\r"); } else { left.speed(0); right.speed(0); wait(0.5); dir=Forward; } break; } wait(0.01); } }