The subsystem design/basis for the final project
Dependencies: mbed-rtos mbed-src pixylib
robot.cpp@19:05b8123905fb, 2017-04-24 (annotated)
- Committer:
- balsamfir
- Date:
- Mon Apr 24 21:37:50 2017 +0000
- Revision:
- 19:05b8123905fb
- Parent:
- 18:501f1007a572
Commit before share
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
balsamfir | 2:2bc519e14bae | 1 | #include "robot.h" |
balsamfir | 2:2bc519e14bae | 2 | #include "global.h" |
balsamfir | 3:dfb6733ae397 | 3 | |
balsamfir | 17:47e107f9587b | 4 | enum System { |
balsamfir | 17:47e107f9587b | 5 | MOTOR = '0', |
balsamfir | 17:47e107f9587b | 6 | SPEED = '1', |
balsamfir | 17:47e107f9587b | 7 | STEERING = '2' |
balsamfir | 17:47e107f9587b | 8 | }; |
balsamfir | 2:2bc519e14bae | 9 | |
balsamfir | 6:52686c25e4af | 10 | // Function prototypes |
balsamfir | 6:52686c25e4af | 11 | // ---------------------------------------------------------------- |
balsamfir | 6:52686c25e4af | 12 | void MotorISR(void); |
balsamfir | 6:52686c25e4af | 13 | void NavigationISR(void); |
balsamfir | 6:52686c25e4af | 14 | void CollisionISR(void); |
balsamfir | 6:52686c25e4af | 15 | void WatchdogISR(void const *n); |
balsamfir | 6:52686c25e4af | 16 | void MotorThread(void const *argument); |
balsamfir | 6:52686c25e4af | 17 | void NavigationThread(void const *argument); |
balsamfir | 14:2d609d465f00 | 18 | void ShutDown(void); |
balsamfir | 6:52686c25e4af | 19 | |
balsamfir | 3:dfb6733ae397 | 20 | // Interrupt and thread control |
balsamfir | 15:caa5a93a31d7 | 21 | osTimerId oneShotId; |
balsamfir | 3:dfb6733ae397 | 22 | osThreadId motorId, navigationId, wdtId; |
balsamfir | 3:dfb6733ae397 | 23 | osThreadDef(MotorThread, osPriorityRealtime, DEFAULT_STACK_SIZE); |
balsamfir | 3:dfb6733ae397 | 24 | osThreadDef(NavigationThread, osPriorityAboveNormal, DEFAULT_STACK_SIZE); |
balsamfir | 3:dfb6733ae397 | 25 | osTimerDef(Wdtimer, WatchdogISR); |
balsamfir | 3:dfb6733ae397 | 26 | int32_t motorSignal, navigationSignal; |
balsamfir | 3:dfb6733ae397 | 27 | Ticker motorPeriodicInt; |
balsamfir | 3:dfb6733ae397 | 28 | Ticker sensorPeriodicInt; |
balsamfir | 2:2bc519e14bae | 29 | |
balsamfir | 3:dfb6733ae397 | 30 | // Mutex to protect left and right motor setpoints |
balsamfir | 3:dfb6733ae397 | 31 | osMutexId motorMutex; |
balsamfir | 3:dfb6733ae397 | 32 | osMutexDef(motorMutex); |
balsamfir | 2:2bc519e14bae | 33 | |
balsamfir | 17:47e107f9587b | 34 | // Variables |
balsamfir | 17:47e107f9587b | 35 | // ---------------------------------------------------------------- |
balsamfir | 17:47e107f9587b | 36 | |
balsamfir | 17:47e107f9587b | 37 | // For tunning and response measurement |
balsamfir | 17:47e107f9587b | 38 | bool isTunning; |
balsamfir | 17:47e107f9587b | 39 | int motorSample; |
balsamfir | 17:47e107f9587b | 40 | int navigationSample; |
balsamfir | 17:47e107f9587b | 41 | float leftMotorResponse[MOTOR_SAMPLES]; |
balsamfir | 17:47e107f9587b | 42 | float rightMotorResponse[MOTOR_SAMPLES]; |
balsamfir | 17:47e107f9587b | 43 | float speedResponse[NAVIGATION_SAMPLES]; |
balsamfir | 17:47e107f9587b | 44 | float steeringResponse[NAVIGATION_SAMPLES]; |
balsamfir | 17:47e107f9587b | 45 | |
balsamfir | 17:47e107f9587b | 46 | |
balsamfir | 3:dfb6733ae397 | 47 | // Set points and display variables |
balsamfir | 6:52686c25e4af | 48 | int x, height; |
balsamfir | 3:dfb6733ae397 | 49 | float speed, steering; |
balsamfir | 17:47e107f9587b | 50 | float leftMotor, rightMotor; |
balsamfir | 17:47e107f9587b | 51 | |
balsamfir | 2:2bc519e14bae | 52 | |
balsamfir | 3:dfb6733ae397 | 53 | // Functions |
balsamfir | 3:dfb6733ae397 | 54 | // ---------------------------------------------------------------- |
balsamfir | 15:caa5a93a31d7 | 55 | void InitRobot(void) { |
balsamfir | 3:dfb6733ae397 | 56 | leftPwm.period(PWM_PERIOD); |
balsamfir | 3:dfb6733ae397 | 57 | leftPwm.pulsewidth(0); |
balsamfir | 3:dfb6733ae397 | 58 | rightPwm.period(PWM_PERIOD); |
balsamfir | 3:dfb6733ae397 | 59 | rightPwm.pulsewidth(0); |
balsamfir | 3:dfb6733ae397 | 60 | |
balsamfir | 3:dfb6733ae397 | 61 | motorMutex = osMutexCreate(osMutex(motorMutex)); |
balsamfir | 2:2bc519e14bae | 62 | |
balsamfir | 3:dfb6733ae397 | 63 | // Start execution of the threads MotorThread, and NavigationThread: |
balsamfir | 3:dfb6733ae397 | 64 | navigationId = osThreadCreate(osThread(NavigationThread), NULL); |
balsamfir | 3:dfb6733ae397 | 65 | motorId = osThreadCreate(osThread(MotorThread), NULL); |
balsamfir | 3:dfb6733ae397 | 66 | |
balsamfir | 2:2bc519e14bae | 67 | // Start the watch dog timer and enable the watch dog interrupt |
balsamfir | 15:caa5a93a31d7 | 68 | oneShotId = osTimerCreate(osTimer(Wdtimer), osTimerOnce, (void *)0); |
balsamfir | 3:dfb6733ae397 | 69 | led3 = 0; // Clear watch dog led3 status |
balsamfir | 2:2bc519e14bae | 70 | |
balsamfir | 3:dfb6733ae397 | 71 | //SPI Initialization |
balsamfir | 3:dfb6733ae397 | 72 | pc.printf("\n\rStarting SPI..."); |
balsamfir | 3:dfb6733ae397 | 73 | deSpi.format(16,1); // 16 bit mode 1 |
balsamfir | 19:05b8123905fb | 74 | deSpi.frequency(500000); |
balsamfir | 3:dfb6733ae397 | 75 | ioReset = 0; ioReset = 1; |
balsamfir | 3:dfb6733ae397 | 76 | wait_us(10); ioReset = 0; spiReset = 0; spiReset = 1; |
balsamfir | 3:dfb6733ae397 | 77 | wait_us(10); spiReset = 0; |
balsamfir | 14:2d609d465f00 | 78 | pc.printf("\n\rDevice Id: %d \r\n", deSpi.write(0x8004)); // Read count & time for both motors |
balsamfir | 19:05b8123905fb | 79 | FlushBuffer(); |
balsamfir | 15:caa5a93a31d7 | 80 | } |
balsamfir | 15:caa5a93a31d7 | 81 | |
balsamfir | 19:05b8123905fb | 82 | // Setup robot for auto tracking and listen for commands |
balsamfir | 15:caa5a93a31d7 | 83 | void AutoTrack(void) { |
balsamfir | 15:caa5a93a31d7 | 84 | char key; |
balsamfir | 6:52686c25e4af | 85 | motorPeriodicInt.attach(&MotorISR, MOTOR_PERIOD); |
balsamfir | 6:52686c25e4af | 86 | sensorPeriodicInt.attach(&NavigationISR, NAVIGATION_PERIOD); |
balsamfir | 2:2bc519e14bae | 87 | |
balsamfir | 2:2bc519e14bae | 88 | while (1) { |
balsamfir | 3:dfb6733ae397 | 89 | osTimerStart(oneShotId, 2000); // Start or restart the watchdog timer interrupt and set to 2000ms. |
balsamfir | 15:caa5a93a31d7 | 90 | if (pc.readable()) { |
balsamfir | 15:caa5a93a31d7 | 91 | key = pc.getc(); |
balsamfir | 15:caa5a93a31d7 | 92 | if (key == 'q') { |
balsamfir | 18:501f1007a572 | 93 | ShutDown(); |
balsamfir | 18:501f1007a572 | 94 | return; |
balsamfir | 19:05b8123905fb | 95 | } else if (key == 'm') { |
balsamfir | 18:501f1007a572 | 96 | ShutDown(); |
balsamfir | 18:501f1007a572 | 97 | ManualControl(); |
balsamfir | 19:05b8123905fb | 98 | } |
balsamfir | 14:2d609d465f00 | 99 | } |
balsamfir | 18:501f1007a572 | 100 | |
balsamfir | 19:05b8123905fb | 101 | pc.printf("X: %d, Height: %d \r\n", x, height); |
balsamfir | 19:05b8123905fb | 102 | pc.printf("Speed: %f \r\nSteering: %f \r\n", speed, steering); |
balsamfir | 19:05b8123905fb | 103 | |
balsamfir | 18:501f1007a572 | 104 | Thread::wait(250); |
balsamfir | 2:2bc519e14bae | 105 | } |
balsamfir | 2:2bc519e14bae | 106 | } |
balsamfir | 2:2bc519e14bae | 107 | |
balsamfir | 19:05b8123905fb | 108 | // Setup robot for manual control and listen for commands |
balsamfir | 2:2bc519e14bae | 109 | void ManualControl(void) { |
balsamfir | 18:501f1007a572 | 110 | char key; |
balsamfir | 15:caa5a93a31d7 | 111 | motorPeriodicInt.attach(&MotorISR, MOTOR_PERIOD); |
balsamfir | 16:73db7ef2deb6 | 112 | float speedRate, steeringRate; |
balsamfir | 16:73db7ef2deb6 | 113 | |
balsamfir | 15:caa5a93a31d7 | 114 | while (1) { |
balsamfir | 15:caa5a93a31d7 | 115 | osTimerStart(oneShotId, 2000); // Start or restart the watchdog timer interrupt and set to 2000ms. |
balsamfir | 17:47e107f9587b | 116 | if (pc.readable()) { |
balsamfir | 18:501f1007a572 | 117 | key = pc.getc(); |
balsamfir | 18:501f1007a572 | 118 | if (key == 'q') { |
balsamfir | 18:501f1007a572 | 119 | ShutDown(); |
balsamfir | 18:501f1007a572 | 120 | return; |
balsamfir | 19:05b8123905fb | 121 | } else if (key == 'a') { |
balsamfir | 18:501f1007a572 | 122 | ShutDown(); |
balsamfir | 18:501f1007a572 | 123 | AutoTrack(); |
balsamfir | 19:05b8123905fb | 124 | } else if (key == '8') { |
balsamfir | 19:05b8123905fb | 125 | speedRate = (speedRate < 1)? speedRate + 0.1 : 1; |
balsamfir | 19:05b8123905fb | 126 | } else if (key == '5') { |
balsamfir | 19:05b8123905fb | 127 | speedRate = (speedRate > -1)? speedRate - 0.1 : -1; |
balsamfir | 19:05b8123905fb | 128 | } else if (key == '6') { |
balsamfir | 19:05b8123905fb | 129 | steeringRate = (steeringRate < 1)? steeringRate + 0.1 : 1; |
balsamfir | 19:05b8123905fb | 130 | } else if (key == '4') { |
balsamfir | 19:05b8123905fb | 131 | steeringRate = (steeringRate > -1)? steeringRate - 0.1 : -1; |
balsamfir | 18:501f1007a572 | 132 | } |
balsamfir | 19:05b8123905fb | 133 | |
balsamfir | 19:05b8123905fb | 134 | pc.printf("%.2f,%.2f\r\n", speedRate, steeringRate); |
balsamfir | 18:501f1007a572 | 135 | } |
balsamfir | 18:501f1007a572 | 136 | |
balsamfir | 19:05b8123905fb | 137 | osMutexWait(motorMutex, osWaitForever); |
balsamfir | 19:05b8123905fb | 138 | leftMotor = SPEED_MAX*(speedRate + steeringRate); |
balsamfir | 19:05b8123905fb | 139 | rightMotor = SPEED_MAX*(speedRate - steeringRate); |
balsamfir | 15:caa5a93a31d7 | 140 | osMutexRelease(motorMutex); |
balsamfir | 15:caa5a93a31d7 | 141 | } |
balsamfir | 15:caa5a93a31d7 | 142 | } |
balsamfir | 2:2bc519e14bae | 143 | |
balsamfir | 15:caa5a93a31d7 | 144 | void Tunning(void) { |
balsamfir | 17:47e107f9587b | 145 | System system; |
balsamfir | 17:47e107f9587b | 146 | int i; |
balsamfir | 15:caa5a93a31d7 | 147 | char key; |
balsamfir | 15:caa5a93a31d7 | 148 | float increment = 1; |
balsamfir | 17:47e107f9587b | 149 | |
balsamfir | 17:47e107f9587b | 150 | isTunning = true; |
balsamfir | 17:47e107f9587b | 151 | |
balsamfir | 17:47e107f9587b | 152 | pc.printf("\e[1;1H\e[2J"); |
balsamfir | 17:47e107f9587b | 153 | pc.printf("Motor (0), Speed (1), Steering (2), Print (p) \r\n"); |
balsamfir | 17:47e107f9587b | 154 | pc.printf("=> "); |
balsamfir | 15:caa5a93a31d7 | 155 | |
balsamfir | 15:caa5a93a31d7 | 156 | while (1) { |
balsamfir | 17:47e107f9587b | 157 | osTimerStart(oneShotId, 20000); // Start or restart the watchdog timer interrupt and set to 20000ms. |
balsamfir | 15:caa5a93a31d7 | 158 | if (pc.readable()) { |
balsamfir | 15:caa5a93a31d7 | 159 | key = pc.getc(); |
balsamfir | 17:47e107f9587b | 160 | if ((key=='0')||(key=='1')||(key=='2')) { |
balsamfir | 17:47e107f9587b | 161 | system = (System)key; |
balsamfir | 17:47e107f9587b | 162 | } else if (key == 'q') { |
balsamfir | 17:47e107f9587b | 163 | ShutDown(); |
balsamfir | 17:47e107f9587b | 164 | return; |
balsamfir | 17:47e107f9587b | 165 | } else if (key == 'p') { |
balsamfir | 17:47e107f9587b | 166 | |
balsamfir | 17:47e107f9587b | 167 | pc.printf("\r\n\r\n Left Motor \t Right Motor \t Speed \t Steering \r\n"); |
balsamfir | 17:47e107f9587b | 168 | |
balsamfir | 17:47e107f9587b | 169 | while ((i < MOTOR_SAMPLES)||(i < NAVIGATION_SAMPLES)) { |
balsamfir | 17:47e107f9587b | 170 | if (i < MOTOR_SAMPLES) { |
balsamfir | 17:47e107f9587b | 171 | pc.printf(" %f, \t %f, \t ", |
balsamfir | 17:47e107f9587b | 172 | rightMotorResponse[i], |
balsamfir | 17:47e107f9587b | 173 | leftMotorResponse[i] |
balsamfir | 17:47e107f9587b | 174 | ); |
balsamfir | 17:47e107f9587b | 175 | } else pc.printf(" \t \t"); |
balsamfir | 17:47e107f9587b | 176 | |
balsamfir | 17:47e107f9587b | 177 | if (i < NAVIGATION_SAMPLES) { |
balsamfir | 17:47e107f9587b | 178 | pc.printf("%f, \t %f ", |
balsamfir | 17:47e107f9587b | 179 | speedResponse[i], |
balsamfir | 17:47e107f9587b | 180 | steeringResponse[i] |
balsamfir | 17:47e107f9587b | 181 | ); |
balsamfir | 17:47e107f9587b | 182 | } else pc.printf(" \t "); |
balsamfir | 17:47e107f9587b | 183 | pc.printf("\r\n"); |
balsamfir | 17:47e107f9587b | 184 | i++; |
balsamfir | 17:47e107f9587b | 185 | } |
balsamfir | 17:47e107f9587b | 186 | |
balsamfir | 17:47e107f9587b | 187 | } else { |
balsamfir | 17:47e107f9587b | 188 | switch (system) { |
balsamfir | 17:47e107f9587b | 189 | case MOTOR: |
balsamfir | 17:47e107f9587b | 190 | if (key == 'a') leftMotorPI.kP = leftMotorPI.kP + increment; |
balsamfir | 17:47e107f9587b | 191 | else if (key == 'z') leftMotorPI.kP = leftMotorPI.kP - increment; |
balsamfir | 17:47e107f9587b | 192 | else if (key == 's') leftMotorPI.kI = leftMotorPI.kI + increment; |
balsamfir | 17:47e107f9587b | 193 | else if (key == 'x') leftMotorPI.kI = leftMotorPI.kI - increment; |
balsamfir | 17:47e107f9587b | 194 | break; |
balsamfir | 17:47e107f9587b | 195 | case SPEED: |
balsamfir | 17:47e107f9587b | 196 | if (key == 'a') heightPI.kP = heightPI.kP + increment; |
balsamfir | 17:47e107f9587b | 197 | else if (key == 'z') heightPI.kP = heightPI.kP - increment; |
balsamfir | 17:47e107f9587b | 198 | else if (key == 's') heightPI.kI = heightPI.kI + increment; |
balsamfir | 17:47e107f9587b | 199 | else if (key == 'x') heightPI.kI = heightPI.kI - increment; |
balsamfir | 17:47e107f9587b | 200 | break; |
balsamfir | 17:47e107f9587b | 201 | case STEERING: |
balsamfir | 17:47e107f9587b | 202 | if (key == 'a') xPI.kP = xPI.kP + increment; |
balsamfir | 17:47e107f9587b | 203 | else if (key == 'z') xPI.kP = xPI.kP - increment; |
balsamfir | 17:47e107f9587b | 204 | else if (key == 's') xPI.kI = xPI.kI + increment; |
balsamfir | 17:47e107f9587b | 205 | else if (key == 'x') xPI.kI = xPI.kI - increment; |
balsamfir | 17:47e107f9587b | 206 | break; |
balsamfir | 17:47e107f9587b | 207 | } |
balsamfir | 17:47e107f9587b | 208 | |
balsamfir | 17:47e107f9587b | 209 | if (key == 'd') increment = increment*10; |
balsamfir | 17:47e107f9587b | 210 | else if (key == 'c') increment = increment/10; |
balsamfir | 15:caa5a93a31d7 | 211 | } |
balsamfir | 17:47e107f9587b | 212 | |
balsamfir | 17:47e107f9587b | 213 | pc.printf("\e[1;1H\e[2J"); |
balsamfir | 17:47e107f9587b | 214 | pc.printf("Current system: %c \r\n\r\n", system); |
balsamfir | 17:47e107f9587b | 215 | |
balsamfir | 17:47e107f9587b | 216 | pc.printf("Motor (0) \t Speed (1) \t Steering (2) \t Print (p) \r\n"); |
balsamfir | 17:47e107f9587b | 217 | pc.printf("Kp (a/z) \t Ki (s/x) \t inc (d/c) \r\n\r\n"); |
balsamfir | 17:47e107f9587b | 218 | |
balsamfir | 15:caa5a93a31d7 | 219 | pc.printf("Increment \t %f \r\n", increment); |
balsamfir | 15:caa5a93a31d7 | 220 | pc.printf("Motor \t KP: %f, KI: %f \r\n", leftMotorPI.kP, leftMotorPI.kI); |
balsamfir | 17:47e107f9587b | 221 | pc.printf("Speed \t KP: %f, KI: %f \r\n", heightPI.kP, heightPI.kI); |
balsamfir | 17:47e107f9587b | 222 | pc.printf("Steering \t KP: %f, KI: %f \r\n\r\n", xPI.kP, xPI.kI); |
balsamfir | 17:47e107f9587b | 223 | pc.printf("=> "); |
balsamfir | 17:47e107f9587b | 224 | |
balsamfir | 17:47e107f9587b | 225 | if (key != 'p') { |
balsamfir | 17:47e107f9587b | 226 | motorPeriodicInt.attach(&MotorISR, MOTOR_PERIOD); |
balsamfir | 17:47e107f9587b | 227 | sensorPeriodicInt.attach(&NavigationISR, NAVIGATION_PERIOD); |
balsamfir | 17:47e107f9587b | 228 | motorSample = 0; |
balsamfir | 17:47e107f9587b | 229 | navigationSample = 0; |
balsamfir | 17:47e107f9587b | 230 | i = 0; |
balsamfir | 17:47e107f9587b | 231 | } |
balsamfir | 17:47e107f9587b | 232 | } |
balsamfir | 17:47e107f9587b | 233 | |
balsamfir | 17:47e107f9587b | 234 | // Stop when all samples are collected |
balsamfir | 17:47e107f9587b | 235 | if ((navigationSample >= NAVIGATION_SAMPLES)&&(motorSample >= MOTOR_SAMPLES)) { |
balsamfir | 17:47e107f9587b | 236 | motorPeriodicInt.detach(); |
balsamfir | 17:47e107f9587b | 237 | sensorPeriodicInt.detach(); |
balsamfir | 17:47e107f9587b | 238 | leftPwm.pulsewidth(0); |
balsamfir | 17:47e107f9587b | 239 | rightPwm.pulsewidth(0); |
balsamfir | 17:47e107f9587b | 240 | xPI.Reset(); |
balsamfir | 17:47e107f9587b | 241 | heightPI.Reset(); |
balsamfir | 17:47e107f9587b | 242 | leftMotorPI.Reset(); |
balsamfir | 17:47e107f9587b | 243 | rightMotorPI.Reset(); |
balsamfir | 15:caa5a93a31d7 | 244 | } |
balsamfir | 15:caa5a93a31d7 | 245 | Thread::wait(500); // Go to sleep for 500 ms |
balsamfir | 15:caa5a93a31d7 | 246 | } |
balsamfir | 2:2bc519e14bae | 247 | } |
balsamfir | 2:2bc519e14bae | 248 | |
balsamfir | 14:2d609d465f00 | 249 | void ShutDown(void) { |
balsamfir | 15:caa5a93a31d7 | 250 | motorPeriodicInt.detach(); |
balsamfir | 15:caa5a93a31d7 | 251 | sensorPeriodicInt.detach(); |
balsamfir | 14:2d609d465f00 | 252 | leftPwm.pulsewidth(0); |
balsamfir | 14:2d609d465f00 | 253 | rightPwm.pulsewidth(0); |
balsamfir | 17:47e107f9587b | 254 | isTunning = false; |
balsamfir | 17:47e107f9587b | 255 | motorSample = 0; |
balsamfir | 17:47e107f9587b | 256 | navigationSample = 0; |
balsamfir | 17:47e107f9587b | 257 | xPI.Reset(); |
balsamfir | 17:47e107f9587b | 258 | heightPI.Reset(); |
balsamfir | 17:47e107f9587b | 259 | leftMotorPI.Reset(); |
balsamfir | 17:47e107f9587b | 260 | rightMotorPI.Reset(); |
balsamfir | 17:47e107f9587b | 261 | pc.printf("Robot Shutdown... \r\n\r\n"); |
balsamfir | 19:05b8123905fb | 262 | FlushBuffer(); |
balsamfir | 14:2d609d465f00 | 263 | } |
balsamfir | 14:2d609d465f00 | 264 | |
balsamfir | 3:dfb6733ae397 | 265 | // Threads |
balsamfir | 3:dfb6733ae397 | 266 | // ---------------------------------------------------------------- |
balsamfir | 3:dfb6733ae397 | 267 | void NavigationThread(void const *argument) { |
balsamfir | 3:dfb6733ae397 | 268 | int count; |
balsamfir | 3:dfb6733ae397 | 269 | |
balsamfir | 3:dfb6733ae397 | 270 | while (1) { |
balsamfir | 3:dfb6733ae397 | 271 | osSignalWait(navigationSignal, osWaitForever); // Go to sleep until navigation signal is set high by ISR |
balsamfir | 3:dfb6733ae397 | 272 | |
balsamfir | 3:dfb6733ae397 | 273 | count = pixy.getBlocks(1); |
balsamfir | 3:dfb6733ae397 | 274 | |
balsamfir | 3:dfb6733ae397 | 275 | // If target returned |
balsamfir | 3:dfb6733ae397 | 276 | if (count && (pixy.blocks[0].signature == TARGET_DECIMAL)) { |
balsamfir | 19:05b8123905fb | 277 | height = pixy.blocks[0].height; |
balsamfir | 6:52686c25e4af | 278 | x = pixy.blocks[0].x; |
balsamfir | 5:f655435d0782 | 279 | |
balsamfir | 6:52686c25e4af | 280 | speed = heightPI.Run(HEIGHT_SETPOINT-height, SPEED_MAX); |
balsamfir | 7:5ef312aa2678 | 281 | steering = xPI.Run(X_SETPOINT-x, SPEED_MAX); |
balsamfir | 3:dfb6733ae397 | 282 | |
balsamfir | 3:dfb6733ae397 | 283 | // Give setpoints to MotorThread |
balsamfir | 3:dfb6733ae397 | 284 | osMutexWait(motorMutex, osWaitForever); |
balsamfir | 19:05b8123905fb | 285 | leftMotor = speed - steering; |
balsamfir | 19:05b8123905fb | 286 | rightMotor = speed + steering; |
balsamfir | 3:dfb6733ae397 | 287 | osMutexRelease(motorMutex); |
balsamfir | 19:05b8123905fb | 288 | } |
balsamfir | 17:47e107f9587b | 289 | |
balsamfir | 17:47e107f9587b | 290 | if (isTunning && (navigationSample < NAVIGATION_SAMPLES)) { |
balsamfir | 17:47e107f9587b | 291 | speedResponse[navigationSample] = speed; |
balsamfir | 17:47e107f9587b | 292 | steeringResponse[navigationSample] = steering; |
balsamfir | 17:47e107f9587b | 293 | navigationSample++; |
balsamfir | 17:47e107f9587b | 294 | } |
balsamfir | 3:dfb6733ae397 | 295 | } |
balsamfir | 3:dfb6733ae397 | 296 | } |
balsamfir | 3:dfb6733ae397 | 297 | |
balsamfir | 3:dfb6733ae397 | 298 | void MotorThread(void const *argument) { |
balsamfir | 3:dfb6733ae397 | 299 | float leftSet, rightSet, angVel; |
balsamfir | 14:2d609d465f00 | 300 | float timeOnLeft, timeOnRight; |
balsamfir | 19:05b8123905fb | 301 | short loop; |
balsamfir | 14:2d609d465f00 | 302 | short dP, dt; |
balsamfir | 3:dfb6733ae397 | 303 | |
balsamfir | 3:dfb6733ae397 | 304 | while (1) { |
balsamfir | 3:dfb6733ae397 | 305 | osSignalWait(motorSignal, osWaitForever); // Go to sleep until motor signal is set high by ISR |
balsamfir | 3:dfb6733ae397 | 306 | |
balsamfir | 3:dfb6733ae397 | 307 | // Get setpoints from navigation |
balsamfir | 3:dfb6733ae397 | 308 | osMutexWait(motorMutex, osWaitForever); |
balsamfir | 3:dfb6733ae397 | 309 | leftSet = leftMotor; |
balsamfir | 3:dfb6733ae397 | 310 | rightSet = rightMotor; |
balsamfir | 3:dfb6733ae397 | 311 | osMutexRelease(motorMutex); |
balsamfir | 3:dfb6733ae397 | 312 | |
balsamfir | 19:05b8123905fb | 313 | // Run PI control on right motor |
balsamfir | 19:05b8123905fb | 314 | dP = deSpi.write(0); |
balsamfir | 19:05b8123905fb | 315 | dt = deSpi.write(0); |
balsamfir | 19:05b8123905fb | 316 | angVel = RadsPerSec(dP, dt); // motors have opposite orientations |
balsamfir | 19:05b8123905fb | 317 | timeOnRight = rightMotorPI.Run(rightSet-angVel, PWM_PERIOD/2); |
balsamfir | 19:05b8123905fb | 318 | |
balsamfir | 3:dfb6733ae397 | 319 | // Run PI control on left motor |
balsamfir | 3:dfb6733ae397 | 320 | dP = deSpi.write(0); |
balsamfir | 3:dfb6733ae397 | 321 | dt = deSpi.write(0); |
balsamfir | 19:05b8123905fb | 322 | angVel = -RadsPerSec(dP, dt); |
balsamfir | 19:05b8123905fb | 323 | timeOnLeft = leftMotorPI.Run(leftSet-angVel, PWM_PERIOD/2); |
balsamfir | 3:dfb6733ae397 | 324 | |
balsamfir | 3:dfb6733ae397 | 325 | |
balsamfir | 7:5ef312aa2678 | 326 | // Output new PWM and direction |
balsamfir | 19:05b8123905fb | 327 | if (timeOnLeft >= 0) leftDir = 0; |
balsamfir | 19:05b8123905fb | 328 | else leftDir = 1; |
balsamfir | 3:dfb6733ae397 | 329 | |
balsamfir | 6:52686c25e4af | 330 | if (timeOnRight >= 0) rightDir = 0; |
balsamfir | 3:dfb6733ae397 | 331 | else rightDir = 1; |
balsamfir | 3:dfb6733ae397 | 332 | |
balsamfir | 6:52686c25e4af | 333 | leftPwm.pulsewidth(fabs(timeOnLeft)); |
balsamfir | 6:52686c25e4af | 334 | rightPwm.pulsewidth(fabs(timeOnRight)); |
balsamfir | 17:47e107f9587b | 335 | |
balsamfir | 17:47e107f9587b | 336 | if (isTunning && (motorSample < MOTOR_SAMPLES)) { |
balsamfir | 17:47e107f9587b | 337 | leftMotorResponse[motorSample] = timeOnLeft; |
balsamfir | 17:47e107f9587b | 338 | rightMotorResponse[motorSample] = timeOnRight; |
balsamfir | 17:47e107f9587b | 339 | motorSample++; |
balsamfir | 17:47e107f9587b | 340 | } |
balsamfir | 19:05b8123905fb | 341 | |
balsamfir | 19:05b8123905fb | 342 | #ifdef DEBUG |
balsamfir | 19:05b8123905fb | 343 | loop = (loop < 1/MOTOR_PERIOD)? loop + 1 : 0; |
balsamfir | 19:05b8123905fb | 344 | #endif |
balsamfir | 3:dfb6733ae397 | 345 | } |
balsamfir | 3:dfb6733ae397 | 346 | } |
balsamfir | 3:dfb6733ae397 | 347 | |
balsamfir | 3:dfb6733ae397 | 348 | |
balsamfir | 3:dfb6733ae397 | 349 | // Interrupt Service Routines |
balsamfir | 3:dfb6733ae397 | 350 | // ---------------------------------------------------------------- |
balsamfir | 3:dfb6733ae397 | 351 | |
balsamfir | 3:dfb6733ae397 | 352 | void WatchdogISR(void const *n) { |
balsamfir | 3:dfb6733ae397 | 353 | led3=1; // Activated when the watchdog timer times out |
balsamfir | 19:05b8123905fb | 354 | ShutDown(); |
balsamfir | 3:dfb6733ae397 | 355 | } |
balsamfir | 3:dfb6733ae397 | 356 | |
balsamfir | 3:dfb6733ae397 | 357 | void MotorISR(void) { |
balsamfir | 3:dfb6733ae397 | 358 | osSignalSet(motorId,0x1); // Activate the signal, MotorControl, with each periodic timer interrupt |
balsamfir | 3:dfb6733ae397 | 359 | } |
balsamfir | 3:dfb6733ae397 | 360 | |
balsamfir | 3:dfb6733ae397 | 361 | void NavigationISR(void) { |
balsamfir | 3:dfb6733ae397 | 362 | osSignalSet(navigationId,0x1); // Activate the signal, SensorControl, with each periodic timer interrupt |
balsamfir | 2:2bc519e14bae | 363 | } |