Jack Daniels
/
SantaSeeker
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: HMC6352 Motor mbed
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Sun Jul 24 2022 21:07:16 by
1.7.2