![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
IT GOES NORTH MOST OF THE TIME
Dependencies: HMC6352 Motor mbed
main.cpp
- Committer:
- vsavkin3
- Date:
- 2012-10-10
- Revision:
- 11:0fd03f66af0d
- Parent:
- 10:168a1b176c77
- Child:
- 12:204600e080c4
File content as of revision 11:0fd03f66af0d:
#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); AnalogIn dirLeft(p17); AnalogIn dirRight(p16); HMC6352 compass(p9, p10); enum direction{ Right, Forward, Left, TurnNorth }; int main() { enum direction dir=Forward; compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); //printf("Front Right Left\n\r"); int j=0; float readLeft; float readRight; float readFront; float diagRight; float diagLeft; float stable[5][4]; readLeft=irLeft; readRight=irRight; readFront=irFront; diagRight=dirRight; diagLeft=dirLeft; int NorthOn=0; int NorthCount=0; for(j=0;j<2;j++) stable[0][j] = readLeft; for(j=0;j<2;j++) stable[1][j] = readRight; for(j=0;j<2;j++) stable[2][j] = diagLeft; for(j=0;j<2;j++) stable[3][j] = diagRight; for(j=0;j<2;j++) stable[4][j] = readFront; j=1; while (1){ if(j>=2) j=0; readLeft=irLeft; readRight=irRight; readFront=irFront; diagRight=dirRight; diagLeft=dirLeft; readLeft=21/readLeft; readRight=21/readRight; readFront=21/readFront; diagRight=21/diagRight; diagLeft=21/diagLeft; float avgFront = 0, avgRight = 0, avgLeft = 0; avgFront = readFront; avgRight = readRight; avgLeft = readLeft; stable[0][j] = avgLeft; stable[1][j] = avgRight; stable[2][j] = diagLeft; stable[3][j] = diagRight; stable[4][j] = avgFront; j++; //printf("Front: %5f Right: %5f Left: %5f dLeft: %5f dRight: %5f", avgFront, avgRight, avgLeft,diagLeft,diagRight); float globalDir = compass.sample()/10.0; //printf(" Heading: %f\n\r", globalDir); switch(dir){ case Forward: if ((avgFront<35&&stable[4][0]<35&&stable[4][1]<35)/*&&stable[4][2]<35&&stable[4][3]<35)*/|| (diagLeft<35&&stable[2][0]<35&&stable[2][1]<35/*&&stable[2][2]<35&&stable[2][3]<35)*/ /*(avgLeft<40&&stable[0][0]<40&&stable[0][1]<40&&stable[0][2]<40&&stable[0][3]<40)*/)) { right.speed(0); left.speed(0); //printf("stopped!\n\r"); //wait(0.1); NorthOn=0; dir=Right; } else if((diagRight<35&&stable[3][0]<35&&stable[3][1]<35/*&&stable[3][2]<35&&stable[3][3]<35)*/ /*(avgRight<40&&stable[1][0]<40&&stable[1][1]<40&&stable[1][2]<40&&stable[1][3]<40))*/)) { right.speed(0); left.speed(0); //printf("stopped2!\n\r"); //wait(0.1); NorthOn=0; dir=Left; } else if(((globalDir > 30 && globalDir <= 180)||(globalDir > 180 && globalDir <= 330))&&NorthOn) { right.speed(0); left.speed(0); dir = TurnNorth; } else {right.speed(1); left.speed(1); //printf("CHARGE!\n\r"); if((avgLeft>35&&stable[0][0]>35&&stable[0][1]>35) &&(avgRight>35&&stable[1][0]>35&&stable[1][1]>35)) NorthOn=1; } break; case Right: ////printf("TURN Right!\n\r"); if ( (avgFront<=35||stable[4][0]<35||stable[4][1]<35)/*||stable[4][2]<35||stable[4][3]<35)*/ || (diagLeft<=35||stable[2][0]<35||stable[2][1]<35)/*||stable[2][2]<35||stable[2][3]<35)*/ ) { 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; case Left: //printf("TURN Left!\n\r"); if ( /*(avgRight<=31.7 ||stable[1][0]<30||stable[1][1]<30||stable[1][2]<30||stable[1][3]<30)||*/ (avgFront<=35||(stable[4][0]<35||stable[4][1]<35))/*&&(stable[4][2]<35||stable[4][3]<35))*/ ||(diagRight<=35||(stable[3][0]<35||stable[3][1]<35))/*&&(stable[3][2]<35||stable[3][3]<35))*/ ) { right.speed(0); left.speed(-1); // //printf("turning!\n\r"); } else { right.speed(0); left.speed(0); // //printf("stopped!\n\r"); //wait(0.1); dir=Forward; } break; case TurnNorth: //printf("TO THE NORTH!\n\r"); if( (avgLeft<35||stable[0][0]<35||stable[0][1]<35) || (avgRight<35||stable[1][0]<35||stable[1][1]<35) || (diagLeft<35||stable[2][0]<35||stable[2][1]<35) || (diagRight<35||stable[3][0]<35||stable[3][1]<35) || (avgFront<35||stable[4][0]<35||stable[4][1]<35) ) {right.speed(0); left.speed(0); dir=Forward;} if (globalDir>330 || globalDir < 30) { //It's going north!! right.speed(0); left.speed(0); dir=Forward; } else { if(globalDir<=180){ right.speed(1); left.speed(-1);} else{right.speed(-1); left.speed(1);} } break; } wait(0.01); } }