Shuto Naruse
Eurobot2012_IRturret
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /************************************************************* 00002 Eurobot 2012 00003 IR Localization Beacon Turret Control Code 00004 00005 author: Shuto 00006 *************************************************************/ 00007 00008 /* 00009 NOTES 00010 00011 IR identification is based on beacon period => may need to change this 00012 + ON time is coded 00013 + OFF time used to confirm read, 00014 00015 ticker based stepper control could be unstable 00016 00017 may need to slow down stepper, microstep less 00018 00019 00020 00021 iterative average predictions 00022 00023 00024 */ 00025 00026 00027 #include "mbed.h" 00028 00029 // Pin Definitions ============================================================ 00030 00031 // IR sensor input 00032 InterruptIn irt_pin_sensor (p30); // pull up to supply 00033 00034 // stepper control pins 00035 DigitalOut irt_pin_step (p11); 00036 DigitalOut irt_pin_dir (p12); 00037 DigitalOut StepperDisable (p23);// Connects to Stepper controller !EN pin 00038 00039 00040 // data output 00041 Serial mainMbed (p13,p14); 00042 00043 // start interrupt 00044 DigitalIn startTrig (p21); 00045 00046 // start location signal 00047 //DigitalIn startOnRed (p22); 00048 00049 // debug 00050 Serial pc (USBTX, USBRX); // tx, rx 00051 DigitalOut rx0 (LED1); 00052 DigitalOut rx1 (LED2); 00053 DigitalOut rx2 (LED3); 00054 DigitalOut serialTx (LED4); 00055 00056 00057 // IR Beacon Settings ========================================================= 00058 00059 // IR signal coding: ON time in us 00060 // TODO need to adjust 00061 #define IR0_ON 1000 00062 #define IR1_ON 750 00063 #define IR2_ON 500 00064 00065 #define IR0_OFF 500 00066 #define IR1_OFF 750 00067 #define IR2_OFF 1000 00068 00069 #define IR_TOR 100 // torrerence 00070 00071 #define IR_BUF_SIZE 100 // buffer size for sensor readings 00072 #define IR_INVALID 100000 // invalid angle 00073 00074 #define IR_ANGLE_TOR 10 00075 00076 #define MIN_SC 5 // minimum sample count for valid recieve 00077 #define NO_SIG_WINDOW 32 // need to adjust! (NO_SIG_WINDOW * 1.8/16) 00078 00079 // Stepper Motor Settings ===================================================== 00080 00081 #define STEPPER_DIV 3200 // number of steps per cycle = default * microstep 00082 #define STEP_PULSE 0.00005 // step pulse duration 00083 00084 00085 // Variables ================================================================== 00086 00087 // stepper 00088 Ticker irt_stepTicker; // ticker triggering stepper motor step 00089 00090 float irt_STEPANGLE = (float)360 / STEPPER_DIV; // step angle of stepper 00091 00092 float irt_rps = 5; 00093 float irt_angle = 0; // current direction which the stepper is facing 00094 00095 bool irt_spin_dir = true; 00096 00097 // IR sensor 00098 Timer irt_timer_on, irt_timer_off; // timer used to measure signal duration 00099 00100 bool irt_rx0_ON = false; 00101 bool irt_rx1_ON = false; 00102 bool irt_rx2_ON = false; 00103 00104 //data buffer 00105 int data0[IR_BUF_SIZE]; 00106 int data1[IR_BUF_SIZE]; 00107 int data2[IR_BUF_SIZE]; 00108 int sc0 = 0; 00109 int sc1 = 0; 00110 int sc2 = 0; 00111 int acc0 = 0; 00112 int acc1 = 0; 00113 int acc2 = 0; 00114 00115 int nosig0 = 0; 00116 int nosig1 = 0; 00117 int nosig2 = 0; 00118 00119 // game end 00120 Timeout gameEnd; 00121 bool inGame = false; 00122 00123 // functions ================================================================== 00124 00125 // prototypes 00126 void irt_stepperStep (); 00127 void irt_sensorSetup (); 00128 void irt_sigStart (); 00129 void irt_sigEnd (); 00130 void sendSerial (int ID, float center, float var); 00131 void stopRobot (); 00132 float step2rad (int step); 00133 float radFormat (float in); 00134 00135 //--- debug main -------------------------------- 00136 int main () { 00137 mainMbed.baud(115200); 00138 mainMbed.format(8,Serial::Odd,1); 00139 00140 StepperDisable = 0; //Enable stepper 00141 00142 irt_sensorSetup(); 00143 00144 /* 00145 // change spin direction for different start locations 00146 if (startOnRed){ 00147 irt_spin_dir = true; 00148 }else{ 00149 irt_spin_dir = false; 00150 } 00151 */ 00152 irt_pin_dir = irt_spin_dir; 00153 00154 00155 // reset buffer 00156 sc0 = 0; 00157 sc1 = 0; 00158 sc2 = 0; 00159 for (int i = 0; i < IR_BUF_SIZE; i++) { 00160 data0[i] = IR_INVALID; // fill with invalid angle values 00161 data1[i] = IR_INVALID; 00162 data2[i] = IR_INVALID; 00163 } 00164 00165 // calibration 5 sec 00166 for (int cv = 0; cv < (STEPPER_DIV * irt_rps)*5 ; cv++) { 00167 irt_stepperStep(); 00168 wait(1/(STEPPER_DIV * irt_rps)); 00169 rx0 = false; 00170 rx1 = false; 00171 rx2 = false; 00172 } 00173 00174 // wait for start signal 00175 while(!startTrig){} 00176 00177 // start off 90sec kill timer 00178 gameEnd.attach(&stopRobot, 87.0); 00179 inGame = true; 00180 00181 // spin loop 00182 while (inGame) { 00183 irt_stepperStep(); 00184 wait(1/(STEPPER_DIV * irt_rps)); 00185 rx0 = false; 00186 rx1 = false; 00187 rx2 = false; 00188 } 00189 00190 } 00191 00192 00193 //--- Stepper Control Functions ----------------- 00194 void irt_stepperStep () { 00195 // function to handle stepper stepping 00196 // this function is repeatedly called by the ticker 00197 00198 // generate step pulse 00199 irt_pin_step = true; 00200 wait(STEP_PULSE); 00201 irt_pin_step = false; 00202 00203 // increment 00204 irt_angle ++; 00205 if (irt_angle >= STEPPER_DIV) { 00206 irt_angle = 0; 00207 } 00208 00209 // check for signal end 00210 if (sc0) { 00211 nosig0++; 00212 00213 // if signal not recieved for long enough 00214 if (nosig0 > NO_SIG_WINDOW) { 00215 00216 // if enough samples obtained, valid signal 00217 if (sc0 > MIN_SC) { 00218 float range = step2rad(data0[sc0-1] - data0[0]); 00219 00220 // if robot facing a beacon signal will be recieved accross 0 00221 if (range < 0) { 00222 acc0 = 0; 00223 for (int i = 0; i < sc0; i++) { 00224 if (data0[i] > STEPPER_DIV/2) { 00225 acc0 += data0[i] - STEPPER_DIV; 00226 } else { 00227 acc0 += data0[i]; 00228 } 00229 } 00230 } 00231 00232 // send data packet 00233 sendSerial (0, step2rad((float)acc0/sc0), range*range); 00234 } 00235 // if not, probably noise 00236 00237 // reset 00238 nosig0 = 0; 00239 sc0 = 0; 00240 acc0 = 0; 00241 } 00242 } 00243 00244 if (sc1) { 00245 nosig1++; 00246 00247 // if signal not recieved for long enough 00248 if (nosig1 > NO_SIG_WINDOW) { 00249 00250 // if enough samples obtained, valid signal 00251 if (sc1 > MIN_SC) { 00252 float range = step2rad(data1[sc1-1] - data1[0]); 00253 00254 // if robot facing a beacon signal will be recieved accross 0 00255 if (range < 0) { 00256 acc1 = 0; 00257 for (int i = 0; i < sc1; i++) { 00258 if (data1[i] > STEPPER_DIV/2) { 00259 acc1 += data1[i] - STEPPER_DIV; 00260 } else { 00261 acc1 += data1[i]; 00262 } 00263 } 00264 } 00265 00266 // send data packet 00267 sendSerial (1, step2rad((float)acc1/sc1), range*range); 00268 } 00269 // if not, probably noise 00270 00271 // reset 00272 nosig1 = 0; 00273 sc1 = 0; 00274 acc1 = 0; 00275 } 00276 } 00277 00278 if (sc2) { 00279 nosig2++; 00280 00281 // if signal not recieved for long enough 00282 if (nosig2 > NO_SIG_WINDOW) { 00283 00284 // if enough samples obtained, valid signal 00285 if (sc2 > MIN_SC) { 00286 float range = step2rad(data2[sc2-1] - data2[0]); 00287 00288 // if robot facing a beacon signal will be recieved accross 0 00289 if (range < 0) { 00290 acc2 = 0; 00291 for (int i = 0; i < sc2; i++) { 00292 if (data2[i] > STEPPER_DIV/2) { 00293 acc2 += data2[i] - STEPPER_DIV; 00294 } else { 00295 acc2 += data2[i]; 00296 } 00297 } 00298 } 00299 00300 // send data packet 00301 sendSerial (2, step2rad((float)acc2/sc2), range*range); 00302 } 00303 // if not, probably noise 00304 00305 // reset 00306 nosig2 = 0; 00307 sc2 = 0; 00308 acc2 = 0; 00309 } 00310 } 00311 00312 00313 return; 00314 } 00315 00316 00317 //--- IR Signal Decode Functions ---------------- 00318 void irt_sensorSetup () { 00319 // function to setup sensor ISRs 00320 00321 // attach ISRs to sensor input 00322 irt_pin_sensor.fall(&irt_sigStart); // ISR for sensor active (low value) 00323 irt_pin_sensor.rise(&irt_sigEnd); // ISR for sensor inactive (high) 00324 00325 // reset timer 00326 irt_timer_on.reset(); 00327 irt_timer_off.reset(); 00328 00329 00330 00331 return; 00332 } 00333 00334 00335 void irt_sigStart () { 00336 // high to low transition 00337 00338 // start ON timer 00339 irt_timer_on.reset(); 00340 irt_timer_on.start(); 00341 00342 // stop OFF timer 00343 irt_timer_off.stop(); 00344 00345 // read timer 00346 int off_time = irt_timer_off.read_us(); 00347 00348 // check if signal recieved 00349 00350 if (irt_angle != 0) { 00351 if (abs(off_time - IR0_OFF) < IR_TOR) { 00352 rx0 = irt_rx0_ON; 00353 if (irt_rx0_ON) { 00354 data0[sc0] = irt_angle; 00355 acc0 += irt_angle; 00356 sc0++; 00357 nosig0 = 0; 00358 if (sc0 >= IR_BUF_SIZE) { 00359 //pc.printf("buffer overflow \n"); 00360 acc0 = 0; 00361 sc0 = 0; 00362 } 00363 } 00364 } 00365 00366 if (abs(off_time - IR1_OFF) < IR_TOR) { 00367 rx1 = irt_rx1_ON; 00368 if (irt_rx1_ON) { 00369 data1[sc1] = irt_angle; 00370 acc1 += irt_angle; 00371 sc1++; 00372 nosig1 = 0; 00373 if (sc1 >= IR_BUF_SIZE) { 00374 //pc.printf("buffer overflow \n"); 00375 acc1 = 0; 00376 sc1 = 0; 00377 } 00378 } 00379 } 00380 00381 if (abs(off_time - IR2_OFF) < IR_TOR) { 00382 rx2 = irt_rx2_ON; 00383 if (irt_rx2_ON) { 00384 data2[sc2] = irt_angle; 00385 acc2 += irt_angle; 00386 sc2++; 00387 nosig2 = 0; 00388 if (sc2 >= IR_BUF_SIZE) { 00389 //pc.printf("buffer overflow \n"); 00390 acc2 = 0; 00391 sc2 = 0; 00392 } 00393 } 00394 } 00395 } 00396 00397 00398 00399 return; 00400 } 00401 00402 void irt_sigEnd () { 00403 // low to high transition 00404 00405 // start OFF timer 00406 irt_timer_off.reset(); 00407 irt_timer_off.start(); 00408 00409 // stop ON timer 00410 irt_timer_on.stop(); 00411 00412 // read timer 00413 int on_time = irt_timer_on.read_us(); 00414 00415 // check 00416 if (abs(on_time - IR0_ON) < IR_TOR) { 00417 irt_rx0_ON = true; 00418 } else { 00419 irt_rx0_ON = false; 00420 } 00421 00422 if (abs(on_time - IR1_ON) < IR_TOR) { 00423 irt_rx1_ON = true; 00424 } else { 00425 irt_rx1_ON = false; 00426 } 00427 00428 if (abs(on_time - IR2_ON) < IR_TOR) { 00429 irt_rx2_ON = true; 00430 } else { 00431 irt_rx2_ON = false; 00432 } 00433 00434 return; 00435 } 00436 00437 00438 // send serial ---------------------------------------------------------------- 00439 void sendSerial (int ID, float center, float var) { 00440 00441 // bytes packing for IR turret serial comm 00442 union IRValue_t { 00443 int IR_ints[4]; 00444 float IR_floats[4]; 00445 unsigned char IR_chars[16]; 00446 } IRValues; 00447 00448 IRValues.IR_chars[0] = 0xFF; 00449 IRValues.IR_chars[1] = 0xFE; 00450 IRValues.IR_chars[2] = 0xFD; 00451 IRValues.IR_chars[3] = 0xFC; 00452 00453 IRValues.IR_ints[1] = ID; // beacon ID 00454 IRValues.IR_floats[2] = center; // center 00455 IRValues.IR_floats[3] = var; // (max - min)^2 00456 00457 // output sample to main board 00458 for (int i = 0; i < 16; i++) { 00459 mainMbed.putc(IRValues.IR_chars[i]); 00460 } 00461 00462 // debug 00463 //pc.printf("%d; %f; %f; \n",ID,center,var); 00464 } 00465 00466 //--- end game ---------------------------------------------------------------- 00467 void stopRobot (){ 00468 inGame = false; 00469 StepperDisable = 1; //Disable Stepper Disable pin set High 00470 } 00471 00472 00473 //--- util -------------------------------------------------------------------- 00474 float step2rad (int step) { 00475 // convert moter step count to rad format with correct range (defined in radFormat) 00476 return radFormat(step * irt_STEPANGLE * 3.14 / 180); 00477 } 00478 00479 float radFormat (float in) { 00480 return in; 00481 // format angle range => currently -PI to PI 00482 /* 00483 if (in > 3.14) { 00484 return in - 6.28; 00485 } 00486 if (in < -3.14) { 00487 return in + 6.28; 00488 } 00489 return in; 00490 */ 00491 } 00492 00493 00494 00495 00496 00497 00498 // notes about the stepper controller ========================================= 00499 /* 00500 *** timing info ********************************************** 00501 STEP input pulse min widths 1us 00502 setup and hold for MS inputs and RESET/DIR 0.2us 00503 00504 FULL Step config !MS1 !MS2 !MS3 00505 STEP number (for full step config) ==> 200 00506 00507 *** stepper lead colors ************************************** 00508 -> steps clockwise for DIR true 00509 1A BLUE 00510 1B RED 00511 2A BLACK 00512 2B GREEN 00513 00514 *** stepper driver pin connection **************************** 00515 GND ground 00516 5V float 00517 Vdd jump to 3.3v 00518 3V3 (jump to Vdd) 00519 GND ground 00520 REF floating 00521 !EN GND 00522 MS1/2/3 floating (have pull down resistors) 00523 !RST pull up to 3.3v (10k resistor) 00524 !SLP connect to !RST (pull up to 3.3v) 00525 STEP STEP data pin 00526 DIR DIR data pin 00527 00528 VMOT connect to 12v supply 00529 GND ground 00530 2B motor lead - GREEN 00531 2A motor lead - BLACK 00532 1A motor lead - BLUE 00533 1B motor lead - RED (clock wise direction when DIR true) 00534 Vdd float 00535 GND ground 00536 00537 *** current limit setting ************************************ 00538 coil resistance = 3.3ohm 00539 00540 check voltage output to motor ==> set it to ~1v 00541 this will limit current to 0.333A 00542 (total current 0.66A, since two phase) 00543 00544 higher currents needed for higher rotation speeds 00545 2v for 5 rps 00546 */ 00547 00548 00549 00550 00551 00552 00553 00554 00555 00556 00557 00558 00559 00560 00561 00562 00563 00564 00565 00566
Generated on Tue Jul 12 2022 20:58:09 by
1.7.2