Shuto Naruse
/
Eurobot2012_IRturret
Eurobot2012_IRturret
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?
User | Revision | Line number | New 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 |