Framework for doing the figure 8 track.
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "stdlib.h" 00003 00004 //Outputs 00005 DigitalOut led1(LED1); 00006 DigitalOut clk(PTD5); 00007 DigitalOut si(PTD4); 00008 PwmOut motor1(PTA4); 00009 PwmOut motor2(PTA12); 00010 DigitalOut break1(PTC12); 00011 DigitalOut break2(PTC13); 00012 PwmOut servo(PTA5); 00013 00014 //Inputs 00015 AnalogIn camData(PTC2); 00016 00017 //Encoder setup and variables 00018 InterruptIn interrupt(PTA13); 00019 00020 //Line Tracking Variables -------------------------------- 00021 float ADCdata [128]; 00022 float minAccum; 00023 float minCount; 00024 float approxPos; 00025 float minVal; 00026 int minLoc; 00027 00028 //Servo turning parameters 00029 float straight = 0.00155f; 00030 float hardLeft = 0.0013f; 00031 float slightLeft = 0.00145f; 00032 float hardRight = 0.0018f; 00033 float slightRight = 0.00165f; 00034 00035 //End of Line Tracking Variables ------------------------- 00036 00037 //Encoder and Motor Driver Variables --------------------- 00038 00039 //Intervals used during encoder data collection to measure velocity 00040 int interval1=0; 00041 int interval2=0; 00042 int interval3=0; 00043 int avg_interval=0; 00044 int lastchange1 = 0; 00045 int lastchange2 = 0; 00046 int lastchange3 = 0; 00047 int lastchange4 = 0; 00048 00049 //Variables used to for velocity control 00050 float avg_speed = 0; 00051 float stall_check = 0; 00052 float tuning_val = 1; 00053 00054 Timer t; 00055 Timer servoTimer; 00056 00057 //Observed average speeds for each duty cycle 00058 const float DESIRED_SPEED = 0.5; 00059 const float TUNING_CONSTANT_10 = 1.10; 00060 const float TUNING_CONSTANT_20 = 3.00; 00061 const float TUNING_CONSTANT_30 = 4.30; 00062 const float TUNING_CONSTANT_50 = 6.880; 00063 const float PI = 3.14159; 00064 const float WHEEL_CIRCUMFERENCE = .05*PI; 00065 00066 //Velocity Control Tuning Constants 00067 const float TUNE_THRESH = 0.5f; 00068 const float TUNE_AMT = 0.1f; 00069 00070 //Parameters specifying sample sizes and delays for small and large average speed samples 00071 float num_samples_small = 3.0f; 00072 float delay_small = 0.05f; 00073 float num_samples_large = 100.0f; 00074 float delay_large = 0.1f; 00075 00076 //Large and small arrays used to get average velocity values 00077 float large_avg_speed_list [100]; 00078 float small_avg_speed_list [10]; 00079 00080 //End of Encoder and Motor Driver Variables ---------------------- 00081 00082 //Line Tracker Functions~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 00083 00084 int find_track(float line[]){ 00085 int track_location = -1; 00086 float slope_threshold = .05; 00087 bool downslope_indices [128] = {false}; 00088 bool upslope_indices [128] = {false}; 00089 for(int i=10; i<118; i++){ 00090 if(line[i+1] - line[i] < -slope_threshold && line[i+2] - line[i+1] < -slope_threshold){ 00091 downslope_indices[i] = true; 00092 } 00093 if(line[i+1] - line[i] > slope_threshold && line[i+2] - line[i+1] > slope_threshold){ 00094 upslope_indices[i] = true; 00095 } 00096 } 00097 int numDownslopes = 0; 00098 int numUpslopes = 0; 00099 for(int i=0; i<128; i++){ 00100 if(downslope_indices[i] == true){ 00101 numDownslopes ++; 00102 } 00103 if(upslope_indices[i] == true){ 00104 numUpslopes ++; 00105 } 00106 } 00107 int downslope_locs [numDownslopes]; 00108 int upslope_locs [numUpslopes]; 00109 int dsctr = 0; 00110 int usctr = 0; 00111 00112 for (int i=0; i<128; i++){ 00113 if(downslope_indices[i] == true){ 00114 downslope_locs[dsctr] = i; 00115 dsctr++; 00116 } 00117 if(upslope_indices[i] == true){ 00118 upslope_locs[usctr] = i; 00119 usctr++; 00120 } 00121 } 00122 00123 for(int i=0; i<numDownslopes; i++){ 00124 for(int j=0; j<numUpslopes; j++){ 00125 if(upslope_locs[j] - downslope_locs[i] >=4 && upslope_locs[j] - downslope_locs[i] <=5){ 00126 track_location = downslope_locs[i] + 2 ; 00127 } 00128 } 00129 } 00130 00131 return track_location; 00132 } 00133 00134 00135 // Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 00136 00137 float get_speed(){ 00138 float revPerSec = (1.0f/((float)avg_interval*.000001))*.25f; 00139 float linearSpeed = revPerSec * WHEEL_CIRCUMFERENCE; 00140 return linearSpeed; 00141 } 00142 00143 float get_avg_speed(float num_samples, float delay) { 00144 00145 float avg_avg_speed = 0; 00146 00147 for (int c = 0; c < num_samples; c++) { 00148 if (num_samples == num_samples_small){ 00149 small_avg_speed_list[c] = get_speed(); 00150 } else if (num_samples == num_samples_large){ 00151 large_avg_speed_list[c] = get_speed(); 00152 } 00153 //wait(delay); 00154 } 00155 00156 for (int c = 1; c < num_samples; c++) { 00157 if (num_samples == num_samples_small){ 00158 avg_avg_speed += small_avg_speed_list[c]; 00159 } else if (num_samples == num_samples_large){ 00160 avg_avg_speed += large_avg_speed_list[c]; 00161 } 00162 } 00163 return avg_avg_speed/num_samples; 00164 } 00165 00166 void velocity_control(float duty_cyc, float tuning_const) { 00167 00168 avg_speed = get_speed();//get_avg_speed(num_samples_small, delay_small); 00169 00170 if (avg_speed == stall_check) { 00171 avg_speed = 0; 00172 tuning_val += TUNE_AMT; 00173 } else if((avg_speed - tuning_const) > TUNE_THRESH){ 00174 tuning_val -= TUNE_AMT; 00175 stall_check = avg_speed; 00176 } else if (tuning_const - avg_speed > TUNE_THRESH){ 00177 //tuning_val += TUNE_AMT; 00178 stall_check = avg_speed; 00179 } else { 00180 //tuning_val = 1; 00181 stall_check = avg_speed; 00182 } 00183 motor1.pulsewidth(.0025 * duty_cyc * tuning_val); 00184 motor2.pulsewidth(.0025 * duty_cyc * tuning_val); 00185 00186 } 00187 00188 void fallInterrupt(){ 00189 00190 int time = t.read_us(); 00191 interval1 = time - lastchange2; 00192 interval2 = lastchange1-lastchange3; 00193 interval3 = lastchange2 - lastchange4; 00194 avg_interval = (interval1 + interval2 + interval3)/3; 00195 00196 lastchange4 = lastchange3; 00197 lastchange3 = lastchange2; 00198 lastchange2 = lastchange1; 00199 lastchange1 = time; 00200 } 00201 00202 void riseInterrupt(){ 00203 int time = t.read_us(); 00204 interval1 = time - lastchange2; 00205 interval2 = lastchange1-lastchange3; 00206 interval3 = lastchange2 - lastchange4; 00207 avg_interval = (interval1 + interval2 + interval3)/3; 00208 00209 lastchange4 = lastchange3; 00210 lastchange3 = lastchange2; 00211 lastchange2 = lastchange1; 00212 lastchange1 = time; 00213 } 00214 00215 00216 //End of Encoder and Motor Driver Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 00217 00218 int main() { 00219 //Line Tracker Initializations 00220 int integrationCounter = 0; 00221 00222 // Motor Driver Initializations 00223 motor1.period(.0025); 00224 motor2.period(.0025); 00225 //interrupt.fall(&fallInterrupt); 00226 //interrupt.rise(&riseInterrupt); 00227 00228 wait(5); 00229 00230 motor1.pulsewidth(.0025*.105); 00231 motor2.pulsewidth(.0025*.105); 00232 break1 = 0; 00233 break2 = 0; 00234 00235 //t.start(); 00236 00237 //wait(5); 00238 00239 while(1) { 00240 00241 if(integrationCounter % 151== 0){ 00242 //Disable interrupts 00243 //__disable_irq(); 00244 //interrupt.fall(NULL); 00245 //interrupt.rise(NULL); 00246 00247 //Send start of integration signal 00248 si = 1; 00249 clk = 1; 00250 00251 si = 0; 00252 clk = 0; 00253 00254 //Reset timing counter for integration 00255 integrationCounter = 0; 00256 00257 //Reset line tracking variables 00258 minAccum = 0; 00259 minCount = 0; 00260 approxPos = 0; 00261 00262 //Reset Encoder and Motor Driver Variables 00263 interval1=0; 00264 interval2=0; 00265 interval3=0; 00266 avg_interval=0; 00267 lastchange1 = 0; 00268 lastchange2 = 0; 00269 lastchange3 = 0; 00270 lastchange4 = 0; 00271 00272 t.reset(); 00273 00274 } 00275 else if (integrationCounter > 129){ 00276 //Enable interrupts 00277 //__enable_irq(); 00278 //interrupt.fall(&fallInterrupt); 00279 //interrupt.rise(&riseInterrupt); 00280 00281 minVal = ADCdata[15]; 00282 for (int c = 10; c < 118; c++) { 00283 if (ADCdata[c] > minVal){ 00284 minVal = ADCdata[c]; 00285 minLoc = c; 00286 } 00287 } 00288 00289 for (int c = 10; c < 118; c++) { 00290 if (ADCdata[c] <= minVal && minVal - ADCdata[c] < 0.07f && ADCdata[c] > 0.1f){ 00291 minAccum += c; 00292 minCount++; 00293 } 00294 } 00295 00296 approxPos = (float)minAccum/(float)minCount; 00297 00298 00299 //approxPos = find_track(ADCdata); 00300 00301 if(approxPos > 0 && approxPos <= 25){ 00302 servo.pulsewidth(hardLeft); 00303 motor1.pulsewidth(.0025*.1); 00304 motor2.pulsewidth(.0025*.1); 00305 } else if (approxPos > 25 && approxPos <= 40){ 00306 servo.pulsewidth(slightLeft); 00307 motor1.pulsewidth(.0025*.105); 00308 motor2.pulsewidth(.0025*.105); 00309 } else if (approxPos > 40 && approxPos <= 90){ 00310 servo.pulsewidth(straight); 00311 motor1.pulsewidth(.0025*.11); 00312 motor2.pulsewidth(.0025*.11); 00313 } else if (approxPos > 90 && approxPos <= 105){ 00314 servo.pulsewidth(slightRight); 00315 motor1.pulsewidth(.0025*.105); 00316 motor2.pulsewidth(.0025*.105); 00317 } else if (approxPos > 105 && approxPos <= 128){ 00318 servo.pulsewidth(hardRight); 00319 motor1.pulsewidth(.0025*.1); 00320 motor2.pulsewidth(.0025*.1); 00321 } 00322 /* 00323 if(approxPos > 0 && approxPos <= 45){ 00324 servo.pulsewidth(hardLeft); 00325 } else if (approxPos > 45 && approxPos <= 95){ 00326 servo.pulsewidth(straight); 00327 } else { 00328 servo.pulsewidth(hardRight); 00329 }*/ 00330 //velocity_control(0.05f, DESIRED_SPEED); 00331 00332 integrationCounter = 150; 00333 } 00334 else{ 00335 clk = 1; 00336 wait_us(220); 00337 ADCdata[integrationCounter - 1] = camData; 00338 clk = 0; 00339 } 00340 00341 //clk = 0; 00342 integrationCounter++; 00343 //camData. 00344 00345 } 00346 }
Generated on Mon Mar 20 2023 22:12:43 by
1.7.2