James MacLean / Robot_Control

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?

UserRevisionLine numberNew 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 }