Shuto Naruse / Mbed 2 deprecated Eurobot2012_IRturret

Eurobot2012_IRturret

Dependencies:   mbed

main.cpp@0:b17919247a31, 2012-10-17 (annotated)

Committer:
narshu
Date:
Wed Oct 17 22:27:33 2012 +0000
Revision:
0:b17919247a31
Commit before publishing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
narshu 0:b17919247a31 1 /*************************************************************
narshu 0:b17919247a31 2 Eurobot 2012
narshu 0:b17919247a31 3 IR Localization Beacon Turret Control Code
narshu 0:b17919247a31 4
narshu 0:b17919247a31 5 author: Shuto
narshu 0:b17919247a31 6 *************************************************************/
narshu 0:b17919247a31 7
narshu 0:b17919247a31 8 /*
narshu 0:b17919247a31 9 NOTES
narshu 0:b17919247a31 10
narshu 0:b17919247a31 11 IR identification is based on beacon period => may need to change this
narshu 0:b17919247a31 12 + ON time is coded
narshu 0:b17919247a31 13 + OFF time used to confirm read,
narshu 0:b17919247a31 14
narshu 0:b17919247a31 15 ticker based stepper control could be unstable
narshu 0:b17919247a31 16
narshu 0:b17919247a31 17 may need to slow down stepper, microstep less
narshu 0:b17919247a31 18
narshu 0:b17919247a31 19
narshu 0:b17919247a31 20
narshu 0:b17919247a31 21 iterative average predictions
narshu 0:b17919247a31 22
narshu 0:b17919247a31 23
narshu 0:b17919247a31 24 */
narshu 0:b17919247a31 25
narshu 0:b17919247a31 26
narshu 0:b17919247a31 27 #include "mbed.h"
narshu 0:b17919247a31 28
narshu 0:b17919247a31 29 // Pin Definitions ============================================================
narshu 0:b17919247a31 30
narshu 0:b17919247a31 31 // IR sensor input
narshu 0:b17919247a31 32 InterruptIn irt_pin_sensor (p30); // pull up to supply
narshu 0:b17919247a31 33
narshu 0:b17919247a31 34 // stepper control pins
narshu 0:b17919247a31 35 DigitalOut irt_pin_step (p11);
narshu 0:b17919247a31 36 DigitalOut irt_pin_dir (p12);
narshu 0:b17919247a31 37 DigitalOut StepperDisable (p23);// Connects to Stepper controller !EN pin
narshu 0:b17919247a31 38
narshu 0:b17919247a31 39
narshu 0:b17919247a31 40 // data output
narshu 0:b17919247a31 41 Serial mainMbed (p13,p14);
narshu 0:b17919247a31 42
narshu 0:b17919247a31 43 // start interrupt
narshu 0:b17919247a31 44 DigitalIn startTrig (p21);
narshu 0:b17919247a31 45
narshu 0:b17919247a31 46 // start location signal
narshu 0:b17919247a31 47 //DigitalIn startOnRed (p22);
narshu 0:b17919247a31 48
narshu 0:b17919247a31 49 // debug
narshu 0:b17919247a31 50 Serial pc (USBTX, USBRX); // tx, rx
narshu 0:b17919247a31 51 DigitalOut rx0 (LED1);
narshu 0:b17919247a31 52 DigitalOut rx1 (LED2);
narshu 0:b17919247a31 53 DigitalOut rx2 (LED3);
narshu 0:b17919247a31 54 DigitalOut serialTx (LED4);
narshu 0:b17919247a31 55
narshu 0:b17919247a31 56
narshu 0:b17919247a31 57 // IR Beacon Settings =========================================================
narshu 0:b17919247a31 58
narshu 0:b17919247a31 59 // IR signal coding: ON time in us
narshu 0:b17919247a31 60 // TODO need to adjust
narshu 0:b17919247a31 61 #define IR0_ON 1000
narshu 0:b17919247a31 62 #define IR1_ON 750
narshu 0:b17919247a31 63 #define IR2_ON 500
narshu 0:b17919247a31 64
narshu 0:b17919247a31 65 #define IR0_OFF 500
narshu 0:b17919247a31 66 #define IR1_OFF 750
narshu 0:b17919247a31 67 #define IR2_OFF 1000
narshu 0:b17919247a31 68
narshu 0:b17919247a31 69 #define IR_TOR 100 // torrerence
narshu 0:b17919247a31 70
narshu 0:b17919247a31 71 #define IR_BUF_SIZE 100 // buffer size for sensor readings
narshu 0:b17919247a31 72 #define IR_INVALID 100000 // invalid angle
narshu 0:b17919247a31 73
narshu 0:b17919247a31 74 #define IR_ANGLE_TOR 10
narshu 0:b17919247a31 75
narshu 0:b17919247a31 76 #define MIN_SC 5 // minimum sample count for valid recieve
narshu 0:b17919247a31 77 #define NO_SIG_WINDOW 32 // need to adjust! (NO_SIG_WINDOW * 1.8/16)
narshu 0:b17919247a31 78
narshu 0:b17919247a31 79 // Stepper Motor Settings =====================================================
narshu 0:b17919247a31 80
narshu 0:b17919247a31 81 #define STEPPER_DIV 3200 // number of steps per cycle = default * microstep
narshu 0:b17919247a31 82 #define STEP_PULSE 0.00005 // step pulse duration
narshu 0:b17919247a31 83
narshu 0:b17919247a31 84
narshu 0:b17919247a31 85 // Variables ==================================================================
narshu 0:b17919247a31 86
narshu 0:b17919247a31 87 // stepper
narshu 0:b17919247a31 88 Ticker irt_stepTicker; // ticker triggering stepper motor step
narshu 0:b17919247a31 89
narshu 0:b17919247a31 90 float irt_STEPANGLE = (float)360 / STEPPER_DIV; // step angle of stepper
narshu 0:b17919247a31 91
narshu 0:b17919247a31 92 float irt_rps = 5;
narshu 0:b17919247a31 93 float irt_angle = 0; // current direction which the stepper is facing
narshu 0:b17919247a31 94
narshu 0:b17919247a31 95 bool irt_spin_dir = true;
narshu 0:b17919247a31 96
narshu 0:b17919247a31 97 // IR sensor
narshu 0:b17919247a31 98 Timer irt_timer_on, irt_timer_off; // timer used to measure signal duration
narshu 0:b17919247a31 99
narshu 0:b17919247a31 100 bool irt_rx0_ON = false;
narshu 0:b17919247a31 101 bool irt_rx1_ON = false;
narshu 0:b17919247a31 102 bool irt_rx2_ON = false;
narshu 0:b17919247a31 103
narshu 0:b17919247a31 104 //data buffer
narshu 0:b17919247a31 105 int data0[IR_BUF_SIZE];
narshu 0:b17919247a31 106 int data1[IR_BUF_SIZE];
narshu 0:b17919247a31 107 int data2[IR_BUF_SIZE];
narshu 0:b17919247a31 108 int sc0 = 0;
narshu 0:b17919247a31 109 int sc1 = 0;
narshu 0:b17919247a31 110 int sc2 = 0;
narshu 0:b17919247a31 111 int acc0 = 0;
narshu 0:b17919247a31 112 int acc1 = 0;
narshu 0:b17919247a31 113 int acc2 = 0;
narshu 0:b17919247a31 114
narshu 0:b17919247a31 115 int nosig0 = 0;
narshu 0:b17919247a31 116 int nosig1 = 0;
narshu 0:b17919247a31 117 int nosig2 = 0;
narshu 0:b17919247a31 118
narshu 0:b17919247a31 119 // game end
narshu 0:b17919247a31 120 Timeout gameEnd;
narshu 0:b17919247a31 121 bool inGame = false;
narshu 0:b17919247a31 122
narshu 0:b17919247a31 123 // functions ==================================================================
narshu 0:b17919247a31 124
narshu 0:b17919247a31 125 // prototypes
narshu 0:b17919247a31 126 void irt_stepperStep ();
narshu 0:b17919247a31 127 void irt_sensorSetup ();
narshu 0:b17919247a31 128 void irt_sigStart ();
narshu 0:b17919247a31 129 void irt_sigEnd ();
narshu 0:b17919247a31 130 void sendSerial (int ID, float center, float var);
narshu 0:b17919247a31 131 void stopRobot ();
narshu 0:b17919247a31 132 float step2rad (int step);
narshu 0:b17919247a31 133 float radFormat (float in);
narshu 0:b17919247a31 134
narshu 0:b17919247a31 135 //--- debug main --------------------------------
narshu 0:b17919247a31 136 int main () {
narshu 0:b17919247a31 137 mainMbed.baud(115200);
narshu 0:b17919247a31 138 mainMbed.format(8,Serial::Odd,1);
narshu 0:b17919247a31 139
narshu 0:b17919247a31 140 StepperDisable = 0; //Enable stepper
narshu 0:b17919247a31 141
narshu 0:b17919247a31 142 irt_sensorSetup();
narshu 0:b17919247a31 143
narshu 0:b17919247a31 144 /*
narshu 0:b17919247a31 145 // change spin direction for different start locations
narshu 0:b17919247a31 146 if (startOnRed){
narshu 0:b17919247a31 147 irt_spin_dir = true;
narshu 0:b17919247a31 148 }else{
narshu 0:b17919247a31 149 irt_spin_dir = false;
narshu 0:b17919247a31 150 }
narshu 0:b17919247a31 151 */
narshu 0:b17919247a31 152 irt_pin_dir = irt_spin_dir;
narshu 0:b17919247a31 153
narshu 0:b17919247a31 154
narshu 0:b17919247a31 155 // reset buffer
narshu 0:b17919247a31 156 sc0 = 0;
narshu 0:b17919247a31 157 sc1 = 0;
narshu 0:b17919247a31 158 sc2 = 0;
narshu 0:b17919247a31 159 for (int i = 0; i < IR_BUF_SIZE; i++) {
narshu 0:b17919247a31 160 data0[i] = IR_INVALID; // fill with invalid angle values
narshu 0:b17919247a31 161 data1[i] = IR_INVALID;
narshu 0:b17919247a31 162 data2[i] = IR_INVALID;
narshu 0:b17919247a31 163 }
narshu 0:b17919247a31 164
narshu 0:b17919247a31 165 // calibration 5 sec
narshu 0:b17919247a31 166 for (int cv = 0; cv < (STEPPER_DIV * irt_rps)*5 ; cv++) {
narshu 0:b17919247a31 167 irt_stepperStep();
narshu 0:b17919247a31 168 wait(1/(STEPPER_DIV * irt_rps));
narshu 0:b17919247a31 169 rx0 = false;
narshu 0:b17919247a31 170 rx1 = false;
narshu 0:b17919247a31 171 rx2 = false;
narshu 0:b17919247a31 172 }
narshu 0:b17919247a31 173
narshu 0:b17919247a31 174 // wait for start signal
narshu 0:b17919247a31 175 while(!startTrig){}
narshu 0:b17919247a31 176
narshu 0:b17919247a31 177 // start off 90sec kill timer
narshu 0:b17919247a31 178 gameEnd.attach(&stopRobot, 87.0);
narshu 0:b17919247a31 179 inGame = true;
narshu 0:b17919247a31 180
narshu 0:b17919247a31 181 // spin loop
narshu 0:b17919247a31 182 while (inGame) {
narshu 0:b17919247a31 183 irt_stepperStep();
narshu 0:b17919247a31 184 wait(1/(STEPPER_DIV * irt_rps));
narshu 0:b17919247a31 185 rx0 = false;
narshu 0:b17919247a31 186 rx1 = false;
narshu 0:b17919247a31 187 rx2 = false;
narshu 0:b17919247a31 188 }
narshu 0:b17919247a31 189
narshu 0:b17919247a31 190 }
narshu 0:b17919247a31 191
narshu 0:b17919247a31 192
narshu 0:b17919247a31 193 //--- Stepper Control Functions -----------------
narshu 0:b17919247a31 194 void irt_stepperStep () {
narshu 0:b17919247a31 195 // function to handle stepper stepping
narshu 0:b17919247a31 196 // this function is repeatedly called by the ticker
narshu 0:b17919247a31 197
narshu 0:b17919247a31 198 // generate step pulse
narshu 0:b17919247a31 199 irt_pin_step = true;
narshu 0:b17919247a31 200 wait(STEP_PULSE);
narshu 0:b17919247a31 201 irt_pin_step = false;
narshu 0:b17919247a31 202
narshu 0:b17919247a31 203 // increment
narshu 0:b17919247a31 204 irt_angle ++;
narshu 0:b17919247a31 205 if (irt_angle >= STEPPER_DIV) {
narshu 0:b17919247a31 206 irt_angle = 0;
narshu 0:b17919247a31 207 }
narshu 0:b17919247a31 208
narshu 0:b17919247a31 209 // check for signal end
narshu 0:b17919247a31 210 if (sc0) {
narshu 0:b17919247a31 211 nosig0++;
narshu 0:b17919247a31 212
narshu 0:b17919247a31 213 // if signal not recieved for long enough
narshu 0:b17919247a31 214 if (nosig0 > NO_SIG_WINDOW) {
narshu 0:b17919247a31 215
narshu 0:b17919247a31 216 // if enough samples obtained, valid signal
narshu 0:b17919247a31 217 if (sc0 > MIN_SC) {
narshu 0:b17919247a31 218 float range = step2rad(data0[sc0-1] - data0[0]);
narshu 0:b17919247a31 219
narshu 0:b17919247a31 220 // if robot facing a beacon signal will be recieved accross 0
narshu 0:b17919247a31 221 if (range < 0) {
narshu 0:b17919247a31 222 acc0 = 0;
narshu 0:b17919247a31 223 for (int i = 0; i < sc0; i++) {
narshu 0:b17919247a31 224 if (data0[i] > STEPPER_DIV/2) {
narshu 0:b17919247a31 225 acc0 += data0[i] - STEPPER_DIV;
narshu 0:b17919247a31 226 } else {
narshu 0:b17919247a31 227 acc0 += data0[i];
narshu 0:b17919247a31 228 }
narshu 0:b17919247a31 229 }
narshu 0:b17919247a31 230 }
narshu 0:b17919247a31 231
narshu 0:b17919247a31 232 // send data packet
narshu 0:b17919247a31 233 sendSerial (0, step2rad((float)acc0/sc0), range*range);
narshu 0:b17919247a31 234 }
narshu 0:b17919247a31 235 // if not, probably noise
narshu 0:b17919247a31 236
narshu 0:b17919247a31 237 // reset
narshu 0:b17919247a31 238 nosig0 = 0;
narshu 0:b17919247a31 239 sc0 = 0;
narshu 0:b17919247a31 240 acc0 = 0;
narshu 0:b17919247a31 241 }
narshu 0:b17919247a31 242 }
narshu 0:b17919247a31 243
narshu 0:b17919247a31 244 if (sc1) {
narshu 0:b17919247a31 245 nosig1++;
narshu 0:b17919247a31 246
narshu 0:b17919247a31 247 // if signal not recieved for long enough
narshu 0:b17919247a31 248 if (nosig1 > NO_SIG_WINDOW) {
narshu 0:b17919247a31 249
narshu 0:b17919247a31 250 // if enough samples obtained, valid signal
narshu 0:b17919247a31 251 if (sc1 > MIN_SC) {
narshu 0:b17919247a31 252 float range = step2rad(data1[sc1-1] - data1[0]);
narshu 0:b17919247a31 253
narshu 0:b17919247a31 254 // if robot facing a beacon signal will be recieved accross 0
narshu 0:b17919247a31 255 if (range < 0) {
narshu 0:b17919247a31 256 acc1 = 0;
narshu 0:b17919247a31 257 for (int i = 0; i < sc1; i++) {
narshu 0:b17919247a31 258 if (data1[i] > STEPPER_DIV/2) {
narshu 0:b17919247a31 259 acc1 += data1[i] - STEPPER_DIV;
narshu 0:b17919247a31 260 } else {
narshu 0:b17919247a31 261 acc1 += data1[i];
narshu 0:b17919247a31 262 }
narshu 0:b17919247a31 263 }
narshu 0:b17919247a31 264 }
narshu 0:b17919247a31 265
narshu 0:b17919247a31 266 // send data packet
narshu 0:b17919247a31 267 sendSerial (1, step2rad((float)acc1/sc1), range*range);
narshu 0:b17919247a31 268 }
narshu 0:b17919247a31 269 // if not, probably noise
narshu 0:b17919247a31 270
narshu 0:b17919247a31 271 // reset
narshu 0:b17919247a31 272 nosig1 = 0;
narshu 0:b17919247a31 273 sc1 = 0;
narshu 0:b17919247a31 274 acc1 = 0;
narshu 0:b17919247a31 275 }
narshu 0:b17919247a31 276 }
narshu 0:b17919247a31 277
narshu 0:b17919247a31 278 if (sc2) {
narshu 0:b17919247a31 279 nosig2++;
narshu 0:b17919247a31 280
narshu 0:b17919247a31 281 // if signal not recieved for long enough
narshu 0:b17919247a31 282 if (nosig2 > NO_SIG_WINDOW) {
narshu 0:b17919247a31 283
narshu 0:b17919247a31 284 // if enough samples obtained, valid signal
narshu 0:b17919247a31 285 if (sc2 > MIN_SC) {
narshu 0:b17919247a31 286 float range = step2rad(data2[sc2-1] - data2[0]);
narshu 0:b17919247a31 287
narshu 0:b17919247a31 288 // if robot facing a beacon signal will be recieved accross 0
narshu 0:b17919247a31 289 if (range < 0) {
narshu 0:b17919247a31 290 acc2 = 0;
narshu 0:b17919247a31 291 for (int i = 0; i < sc2; i++) {
narshu 0:b17919247a31 292 if (data2[i] > STEPPER_DIV/2) {
narshu 0:b17919247a31 293 acc2 += data2[i] - STEPPER_DIV;
narshu 0:b17919247a31 294 } else {
narshu 0:b17919247a31 295 acc2 += data2[i];
narshu 0:b17919247a31 296 }
narshu 0:b17919247a31 297 }
narshu 0:b17919247a31 298 }
narshu 0:b17919247a31 299
narshu 0:b17919247a31 300 // send data packet
narshu 0:b17919247a31 301 sendSerial (2, step2rad((float)acc2/sc2), range*range);
narshu 0:b17919247a31 302 }
narshu 0:b17919247a31 303 // if not, probably noise
narshu 0:b17919247a31 304
narshu 0:b17919247a31 305 // reset
narshu 0:b17919247a31 306 nosig2 = 0;
narshu 0:b17919247a31 307 sc2 = 0;
narshu 0:b17919247a31 308 acc2 = 0;
narshu 0:b17919247a31 309 }
narshu 0:b17919247a31 310 }
narshu 0:b17919247a31 311
narshu 0:b17919247a31 312
narshu 0:b17919247a31 313 return;
narshu 0:b17919247a31 314 }
narshu 0:b17919247a31 315
narshu 0:b17919247a31 316
narshu 0:b17919247a31 317 //--- IR Signal Decode Functions ----------------
narshu 0:b17919247a31 318 void irt_sensorSetup () {
narshu 0:b17919247a31 319 // function to setup sensor ISRs
narshu 0:b17919247a31 320
narshu 0:b17919247a31 321 // attach ISRs to sensor input
narshu 0:b17919247a31 322 irt_pin_sensor.fall(&irt_sigStart); // ISR for sensor active (low value)
narshu 0:b17919247a31 323 irt_pin_sensor.rise(&irt_sigEnd); // ISR for sensor inactive (high)
narshu 0:b17919247a31 324
narshu 0:b17919247a31 325 // reset timer
narshu 0:b17919247a31 326 irt_timer_on.reset();
narshu 0:b17919247a31 327 irt_timer_off.reset();
narshu 0:b17919247a31 328
narshu 0:b17919247a31 329
narshu 0:b17919247a31 330
narshu 0:b17919247a31 331 return;
narshu 0:b17919247a31 332 }
narshu 0:b17919247a31 333
narshu 0:b17919247a31 334
narshu 0:b17919247a31 335 void irt_sigStart () {
narshu 0:b17919247a31 336 // high to low transition
narshu 0:b17919247a31 337
narshu 0:b17919247a31 338 // start ON timer
narshu 0:b17919247a31 339 irt_timer_on.reset();
narshu 0:b17919247a31 340 irt_timer_on.start();
narshu 0:b17919247a31 341
narshu 0:b17919247a31 342 // stop OFF timer
narshu 0:b17919247a31 343 irt_timer_off.stop();
narshu 0:b17919247a31 344
narshu 0:b17919247a31 345 // read timer
narshu 0:b17919247a31 346 int off_time = irt_timer_off.read_us();
narshu 0:b17919247a31 347
narshu 0:b17919247a31 348 // check if signal recieved
narshu 0:b17919247a31 349
narshu 0:b17919247a31 350 if (irt_angle != 0) {
narshu 0:b17919247a31 351 if (abs(off_time - IR0_OFF) < IR_TOR) {
narshu 0:b17919247a31 352 rx0 = irt_rx0_ON;
narshu 0:b17919247a31 353 if (irt_rx0_ON) {
narshu 0:b17919247a31 354 data0[sc0] = irt_angle;
narshu 0:b17919247a31 355 acc0 += irt_angle;
narshu 0:b17919247a31 356 sc0++;
narshu 0:b17919247a31 357 nosig0 = 0;
narshu 0:b17919247a31 358 if (sc0 >= IR_BUF_SIZE) {
narshu 0:b17919247a31 359 //pc.printf("buffer overflow \n");
narshu 0:b17919247a31 360 acc0 = 0;
narshu 0:b17919247a31 361 sc0 = 0;
narshu 0:b17919247a31 362 }
narshu 0:b17919247a31 363 }
narshu 0:b17919247a31 364 }
narshu 0:b17919247a31 365
narshu 0:b17919247a31 366 if (abs(off_time - IR1_OFF) < IR_TOR) {
narshu 0:b17919247a31 367 rx1 = irt_rx1_ON;
narshu 0:b17919247a31 368 if (irt_rx1_ON) {
narshu 0:b17919247a31 369 data1[sc1] = irt_angle;
narshu 0:b17919247a31 370 acc1 += irt_angle;
narshu 0:b17919247a31 371 sc1++;
narshu 0:b17919247a31 372 nosig1 = 0;
narshu 0:b17919247a31 373 if (sc1 >= IR_BUF_SIZE) {
narshu 0:b17919247a31 374 //pc.printf("buffer overflow \n");
narshu 0:b17919247a31 375 acc1 = 0;
narshu 0:b17919247a31 376 sc1 = 0;
narshu 0:b17919247a31 377 }
narshu 0:b17919247a31 378 }
narshu 0:b17919247a31 379 }
narshu 0:b17919247a31 380
narshu 0:b17919247a31 381 if (abs(off_time - IR2_OFF) < IR_TOR) {
narshu 0:b17919247a31 382 rx2 = irt_rx2_ON;
narshu 0:b17919247a31 383 if (irt_rx2_ON) {
narshu 0:b17919247a31 384 data2[sc2] = irt_angle;
narshu 0:b17919247a31 385 acc2 += irt_angle;
narshu 0:b17919247a31 386 sc2++;
narshu 0:b17919247a31 387 nosig2 = 0;
narshu 0:b17919247a31 388 if (sc2 >= IR_BUF_SIZE) {
narshu 0:b17919247a31 389 //pc.printf("buffer overflow \n");
narshu 0:b17919247a31 390 acc2 = 0;
narshu 0:b17919247a31 391 sc2 = 0;
narshu 0:b17919247a31 392 }
narshu 0:b17919247a31 393 }
narshu 0:b17919247a31 394 }
narshu 0:b17919247a31 395 }
narshu 0:b17919247a31 396
narshu 0:b17919247a31 397
narshu 0:b17919247a31 398
narshu 0:b17919247a31 399 return;
narshu 0:b17919247a31 400 }
narshu 0:b17919247a31 401
narshu 0:b17919247a31 402 void irt_sigEnd () {
narshu 0:b17919247a31 403 // low to high transition
narshu 0:b17919247a31 404
narshu 0:b17919247a31 405 // start OFF timer
narshu 0:b17919247a31 406 irt_timer_off.reset();
narshu 0:b17919247a31 407 irt_timer_off.start();
narshu 0:b17919247a31 408
narshu 0:b17919247a31 409 // stop ON timer
narshu 0:b17919247a31 410 irt_timer_on.stop();
narshu 0:b17919247a31 411
narshu 0:b17919247a31 412 // read timer
narshu 0:b17919247a31 413 int on_time = irt_timer_on.read_us();
narshu 0:b17919247a31 414
narshu 0:b17919247a31 415 // check
narshu 0:b17919247a31 416 if (abs(on_time - IR0_ON) < IR_TOR) {
narshu 0:b17919247a31 417 irt_rx0_ON = true;
narshu 0:b17919247a31 418 } else {
narshu 0:b17919247a31 419 irt_rx0_ON = false;
narshu 0:b17919247a31 420 }
narshu 0:b17919247a31 421
narshu 0:b17919247a31 422 if (abs(on_time - IR1_ON) < IR_TOR) {
narshu 0:b17919247a31 423 irt_rx1_ON = true;
narshu 0:b17919247a31 424 } else {
narshu 0:b17919247a31 425 irt_rx1_ON = false;
narshu 0:b17919247a31 426 }
narshu 0:b17919247a31 427
narshu 0:b17919247a31 428 if (abs(on_time - IR2_ON) < IR_TOR) {
narshu 0:b17919247a31 429 irt_rx2_ON = true;
narshu 0:b17919247a31 430 } else {
narshu 0:b17919247a31 431 irt_rx2_ON = false;
narshu 0:b17919247a31 432 }
narshu 0:b17919247a31 433
narshu 0:b17919247a31 434 return;
narshu 0:b17919247a31 435 }
narshu 0:b17919247a31 436
narshu 0:b17919247a31 437
narshu 0:b17919247a31 438 // send serial ----------------------------------------------------------------
narshu 0:b17919247a31 439 void sendSerial (int ID, float center, float var) {
narshu 0:b17919247a31 440
narshu 0:b17919247a31 441 // bytes packing for IR turret serial comm
narshu 0:b17919247a31 442 union IRValue_t {
narshu 0:b17919247a31 443 int IR_ints[4];
narshu 0:b17919247a31 444 float IR_floats[4];
narshu 0:b17919247a31 445 unsigned char IR_chars[16];
narshu 0:b17919247a31 446 } IRValues;
narshu 0:b17919247a31 447
narshu 0:b17919247a31 448 IRValues.IR_chars[0] = 0xFF;
narshu 0:b17919247a31 449 IRValues.IR_chars[1] = 0xFE;
narshu 0:b17919247a31 450 IRValues.IR_chars[2] = 0xFD;
narshu 0:b17919247a31 451 IRValues.IR_chars[3] = 0xFC;
narshu 0:b17919247a31 452
narshu 0:b17919247a31 453 IRValues.IR_ints[1] = ID; // beacon ID
narshu 0:b17919247a31 454 IRValues.IR_floats[2] = center; // center
narshu 0:b17919247a31 455 IRValues.IR_floats[3] = var; // (max - min)^2
narshu 0:b17919247a31 456
narshu 0:b17919247a31 457 // output sample to main board
narshu 0:b17919247a31 458 for (int i = 0; i < 16; i++) {
narshu 0:b17919247a31 459 mainMbed.putc(IRValues.IR_chars[i]);
narshu 0:b17919247a31 460 }
narshu 0:b17919247a31 461
narshu 0:b17919247a31 462 // debug
narshu 0:b17919247a31 463 //pc.printf("%d; %f; %f; \n",ID,center,var);
narshu 0:b17919247a31 464 }
narshu 0:b17919247a31 465
narshu 0:b17919247a31 466 //--- end game ----------------------------------------------------------------
narshu 0:b17919247a31 467 void stopRobot (){
narshu 0:b17919247a31 468 inGame = false;
narshu 0:b17919247a31 469 StepperDisable = 1; //Disable Stepper Disable pin set High
narshu 0:b17919247a31 470 }
narshu 0:b17919247a31 471
narshu 0:b17919247a31 472
narshu 0:b17919247a31 473 //--- util --------------------------------------------------------------------
narshu 0:b17919247a31 474 float step2rad (int step) {
narshu 0:b17919247a31 475 // convert moter step count to rad format with correct range (defined in radFormat)
narshu 0:b17919247a31 476 return radFormat(step * irt_STEPANGLE * 3.14 / 180);
narshu 0:b17919247a31 477 }
narshu 0:b17919247a31 478
narshu 0:b17919247a31 479 float radFormat (float in) {
narshu 0:b17919247a31 480 return in;
narshu 0:b17919247a31 481 // format angle range => currently -PI to PI
narshu 0:b17919247a31 482 /*
narshu 0:b17919247a31 483 if (in > 3.14) {
narshu 0:b17919247a31 484 return in - 6.28;
narshu 0:b17919247a31 485 }
narshu 0:b17919247a31 486 if (in < -3.14) {
narshu 0:b17919247a31 487 return in + 6.28;
narshu 0:b17919247a31 488 }
narshu 0:b17919247a31 489 return in;
narshu 0:b17919247a31 490 */
narshu 0:b17919247a31 491 }
narshu 0:b17919247a31 492
narshu 0:b17919247a31 493
narshu 0:b17919247a31 494
narshu 0:b17919247a31 495
narshu 0:b17919247a31 496
narshu 0:b17919247a31 497
narshu 0:b17919247a31 498 // notes about the stepper controller =========================================
narshu 0:b17919247a31 499 /*
narshu 0:b17919247a31 500 *** timing info **********************************************
narshu 0:b17919247a31 501 STEP input pulse min widths 1us
narshu 0:b17919247a31 502 setup and hold for MS inputs and RESET/DIR 0.2us
narshu 0:b17919247a31 503
narshu 0:b17919247a31 504 FULL Step config !MS1 !MS2 !MS3
narshu 0:b17919247a31 505 STEP number (for full step config) ==> 200
narshu 0:b17919247a31 506
narshu 0:b17919247a31 507 *** stepper lead colors **************************************
narshu 0:b17919247a31 508 -> steps clockwise for DIR true
narshu 0:b17919247a31 509 1A BLUE
narshu 0:b17919247a31 510 1B RED
narshu 0:b17919247a31 511 2A BLACK
narshu 0:b17919247a31 512 2B GREEN
narshu 0:b17919247a31 513
narshu 0:b17919247a31 514 *** stepper driver pin connection ****************************
narshu 0:b17919247a31 515 GND ground
narshu 0:b17919247a31 516 5V float
narshu 0:b17919247a31 517 Vdd jump to 3.3v
narshu 0:b17919247a31 518 3V3 (jump to Vdd)
narshu 0:b17919247a31 519 GND ground
narshu 0:b17919247a31 520 REF floating
narshu 0:b17919247a31 521 !EN GND
narshu 0:b17919247a31 522 MS1/2/3 floating (have pull down resistors)
narshu 0:b17919247a31 523 !RST pull up to 3.3v (10k resistor)
narshu 0:b17919247a31 524 !SLP connect to !RST (pull up to 3.3v)
narshu 0:b17919247a31 525 STEP STEP data pin
narshu 0:b17919247a31 526 DIR DIR data pin
narshu 0:b17919247a31 527
narshu 0:b17919247a31 528 VMOT connect to 12v supply
narshu 0:b17919247a31 529 GND ground
narshu 0:b17919247a31 530 2B motor lead - GREEN
narshu 0:b17919247a31 531 2A motor lead - BLACK
narshu 0:b17919247a31 532 1A motor lead - BLUE
narshu 0:b17919247a31 533 1B motor lead - RED (clock wise direction when DIR true)
narshu 0:b17919247a31 534 Vdd float
narshu 0:b17919247a31 535 GND ground
narshu 0:b17919247a31 536
narshu 0:b17919247a31 537 *** current limit setting ************************************
narshu 0:b17919247a31 538 coil resistance = 3.3ohm
narshu 0:b17919247a31 539
narshu 0:b17919247a31 540 check voltage output to motor ==> set it to ~1v
narshu 0:b17919247a31 541 this will limit current to 0.333A
narshu 0:b17919247a31 542 (total current 0.66A, since two phase)
narshu 0:b17919247a31 543
narshu 0:b17919247a31 544 higher currents needed for higher rotation speeds
narshu 0:b17919247a31 545 2v for 5 rps
narshu 0:b17919247a31 546 */
narshu 0:b17919247a31 547
narshu 0:b17919247a31 548
narshu 0:b17919247a31 549
narshu 0:b17919247a31 550
narshu 0:b17919247a31 551
narshu 0:b17919247a31 552
narshu 0:b17919247a31 553
narshu 0:b17919247a31 554
narshu 0:b17919247a31 555
narshu 0:b17919247a31 556
narshu 0:b17919247a31 557
narshu 0:b17919247a31 558
narshu 0:b17919247a31 559
narshu 0:b17919247a31 560
narshu 0:b17919247a31 561
narshu 0:b17919247a31 562
narshu 0:b17919247a31 563
narshu 0:b17919247a31 564
narshu 0:b17919247a31 565
narshu 0:b17919247a31 566