IT GOES NORTH MOST OF THE TIME

Dependencies:   HMC6352 Motor mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "Motor.h"
00003 #include "HMC6352.h"
00004 
00005 //e
00006 Motor right(p21, p23, p22); // pwm, fwd, rev
00007 Motor left(p26, p24, p25); // pwm, fwd, rev
00008 AnalogIn irLeft(p20);
00009 AnalogIn irFront(p19);
00010 AnalogIn irRight(p18);
00011 AnalogIn dirLeft(p17);
00012 AnalogIn dirRight(p16);
00013 HMC6352 compass(p9, p10);
00014 
00015 enum direction{ Right, Forward, Left, TurnNorth };
00016 
00017 int main() {
00018     enum direction dir=Forward;
00019     compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
00020     
00021     //printf("Front Right Left\n\r");
00022     int j=0;
00023     float readLeft;
00024     float readRight;
00025     float readFront;
00026     float diagRight;
00027     float diagLeft;
00028     float stable[5][4];
00029         readLeft=irLeft;
00030         readRight=irRight;
00031         readFront=irFront;
00032         diagRight=dirRight;
00033         diagLeft=dirLeft;
00034    
00035     int NorthOn=0;
00036     int NorthCount=0;
00037         for(j=0;j<2;j++) stable[0][j] = readLeft;
00038         for(j=0;j<2;j++) stable[1][j] = readRight;
00039         for(j=0;j<2;j++) stable[2][j] = diagLeft;
00040         for(j=0;j<2;j++) stable[3][j] = diagRight;
00041         for(j=0;j<2;j++) stable[4][j] = readFront;
00042         j=1;
00043     while (1){
00044         if(j>=2) j=0;
00045        
00046         readLeft=irLeft;
00047         readRight=irRight;
00048         readFront=irFront;
00049         diagRight=dirRight;
00050         diagLeft=dirLeft;
00051         readLeft=21/readLeft;
00052         readRight=21/readRight;
00053         readFront=21/readFront;
00054         diagRight=21/diagRight;
00055         diagLeft=21/diagLeft;
00056         float avgFront = 0, avgRight = 0, avgLeft = 0;
00057         avgFront = readFront;
00058         avgRight = readRight;
00059         avgLeft  = readLeft;
00060 stable[0][j] = avgLeft;
00061 stable[1][j] = avgRight;
00062 stable[2][j] = diagLeft;
00063 stable[3][j] = diagRight;
00064 stable[4][j] = avgFront;
00065 
00066 j++;
00067         //printf("Front: %5f Right: %5f Left: %5f dLeft: %5f dRight: %5f", avgFront, avgRight, avgLeft,diagLeft,diagRight);
00068         float globalDir = compass.sample()/10.0;
00069         //printf(" Heading: %f\n\r", globalDir);
00070 
00071         switch(dir){
00072             case Forward:
00073             
00074                 NorthCount++;
00075                 if ((avgFront<35&&stable[4][0]<35&&stable[4][1]<35)/*&&stable[4][2]<35&&stable[4][3]<35)*/||
00076                     (diagLeft<35&&stable[2][0]<35&&stable[2][1]<35/*&&stable[2][2]<35&&stable[2][3]<35)*/
00077                     /*(avgLeft<40&&stable[0][0]<40&&stable[0][1]<40&&stable[0][2]<40&&stable[0][3]<40)*/))
00078                 {
00079                     right.speed(0);
00080                     left.speed(0);
00081                     //printf("stopped!\n\r");
00082                     //wait(0.1);
00083                     NorthOn=0;
00084                     dir=Right;
00085                 }
00086                 else if((diagRight<35&&stable[3][0]<35&&stable[3][1]<35/*&&stable[3][2]<35&&stable[3][3]<35)*/
00087                         /*(avgRight<40&&stable[1][0]<40&&stable[1][1]<40&&stable[1][2]<40&&stable[1][3]<40))*/))
00088                 {
00089                     right.speed(0);
00090                     left.speed(0);
00091                     //printf("stopped2!\n\r");
00092                     //wait(0.1);
00093                     NorthOn=0;
00094                     dir=Left;
00095                 }
00096                 else if(((globalDir >= 30 && globalDir <= 330))&&NorthOn)
00097                 
00098                 {
00099                             right.speed(0);
00100                             left.speed(0);
00101                             dir = TurnNorth;
00102                 }
00103                 else {right.speed(1); left.speed(1);
00104                 //printf("CHARGE!\n\r");
00105                 if((avgLeft>35&&stable[0][0]>35&&stable[0][1]>35)
00106                 
00107                    &&(avgRight>35&&stable[1][0]>35&&stable[1][1]>35)
00108                    &&NorthCount>=350){ NorthOn=1; NorthCount=0;}
00109 
00110                 }
00111             break;
00112             case Right:
00113                 ////printf("TURN Right!\n\r");
00114                 if (
00115                    (avgFront<=35||stable[4][0]<35||stable[4][1]<35)/*||stable[4][2]<35||stable[4][3]<35)*/
00116                 || (diagLeft<=35||stable[2][0]<35||stable[2][1]<35)/*||stable[2][2]<35||stable[2][3]<35)*/
00117                 )
00118                 {
00119                     right.speed(-1);
00120                     left.speed(0);
00121                    // //printf("turning!\n\r");
00122                 }
00123                 else 
00124                 {
00125                     right.speed(0);
00126                     left.speed(0);
00127                   //  //printf("stopped!\n\r");
00128                     //wait(0.1);
00129                     dir=Forward;
00130                 }
00131             break;
00132             
00133             case Left:
00134                 //printf("TURN Left!\n\r");
00135                 if (
00136                    /*(avgRight<=31.7 ||stable[1][0]<30||stable[1][1]<30||stable[1][2]<30||stable[1][3]<30)||*/
00137                    (avgFront<=35||(stable[4][0]<35||stable[4][1]<35))/*&&(stable[4][2]<35||stable[4][3]<35))*/
00138                     ||(diagRight<=35||(stable[3][0]<35||stable[3][1]<35))/*&&(stable[3][2]<35||stable[3][3]<35))*/
00139                    )
00140                 {
00141                     right.speed(0);
00142                     left.speed(-1);
00143                    // //printf("turning!\n\r");
00144                 }
00145                 else 
00146                 {
00147                 
00148                     right.speed(0);
00149                     left.speed(0);
00150                   //  //printf("stopped!\n\r");
00151                     //wait(0.1);
00152                     dir=Forward;
00153                 }
00154             break;
00155             case TurnNorth:
00156                 //printf("TO THE NORTH!\n\r");
00157                 if(
00158                    (avgLeft<35&&stable[0][0]<35&&stable[0][1]<35)
00159                    ||
00160                    (avgRight<35&&stable[1][0]<35&&stable[1][1]<35)
00161                    ||
00162                    (diagLeft<35&&stable[2][0]<35&&stable[2][1]<35)
00163                    ||
00164                    (diagRight<35&&stable[3][0]<35&&stable[3][1]<35)
00165                    ||
00166                    (avgFront<35&&stable[4][0]<35&&stable[4][1]<35)
00167                    )
00168                    {right.speed(0);
00169                     left.speed(0);
00170                     dir=Forward;}    
00171                 if (globalDir>330 || globalDir < 30)
00172                 {
00173                     //It's going north!!
00174                     right.speed(0);
00175                     left.speed(0);
00176                     dir=Forward;
00177                 }
00178                 else
00179                 {   
00180                     //If it's not going north, choose which way to turn to get north
00181                     if(globalDir<=180)
00182                     {
00183                         right.speed(1);
00184                         left.speed(-1);
00185                     }
00186                     else
00187                     {
00188                         right.speed(-1);
00189                         left.speed(1);
00190                     }
00191                 }
00192             break;
00193         }
00194         wait(0.01);
00195     }
00196 }