James MacLean / Robot_Control

The subsystem design/basis for the final project

Dependencies:   mbed-rtos mbed-src pixylib

robot.cpp@14:2d609d465f00, 2016-03-26 (annotated)

Committer:
balsamfir
Date:
Sat Mar 26 17:52:34 2016 +0000
Revision:
14:2d609d465f00
Parent:
7:5ef312aa2678
Child:
15:caa5a93a31d7
Everything tuned and working nice

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 3:dfb6733ae397 4 // Global Definitions
balsamfir 3:dfb6733ae397 5 // ----------------------------------------------------------------
balsamfir 2:2bc519e14bae 6
balsamfir 6:52686c25e4af 7 // Function prototypes
balsamfir 6:52686c25e4af 8 // ----------------------------------------------------------------
balsamfir 6:52686c25e4af 9 void MotorISR(void);
balsamfir 6:52686c25e4af 10 void NavigationISR(void);
balsamfir 6:52686c25e4af 11 void CollisionISR(void);
balsamfir 6:52686c25e4af 12 void WatchdogISR(void const *n);
balsamfir 6:52686c25e4af 13 void MotorThread(void const *argument);
balsamfir 6:52686c25e4af 14 void NavigationThread(void const *argument);
balsamfir 14:2d609d465f00 15 void UpdateGains(char key, float *increment);
balsamfir 14:2d609d465f00 16 void ShutDown(void);
balsamfir 6:52686c25e4af 17
balsamfir 3:dfb6733ae397 18 // Interrupt and thread control
balsamfir 3:dfb6733ae397 19 osThreadId motorId, navigationId, wdtId;
balsamfir 3:dfb6733ae397 20 osThreadDef(MotorThread, osPriorityRealtime, DEFAULT_STACK_SIZE);
balsamfir 3:dfb6733ae397 21 osThreadDef(NavigationThread, osPriorityAboveNormal, DEFAULT_STACK_SIZE);
balsamfir 3:dfb6733ae397 22 osTimerDef(Wdtimer, WatchdogISR);
balsamfir 3:dfb6733ae397 23 int32_t motorSignal, navigationSignal;
balsamfir 3:dfb6733ae397 24 Ticker motorPeriodicInt;
balsamfir 3:dfb6733ae397 25 Ticker sensorPeriodicInt;
balsamfir 2:2bc519e14bae 26
balsamfir 3:dfb6733ae397 27 // Mutex to protect left and right motor setpoints
balsamfir 3:dfb6733ae397 28 osMutexId motorMutex;
balsamfir 3:dfb6733ae397 29 osMutexDef(motorMutex);
balsamfir 2:2bc519e14bae 30
balsamfir 3:dfb6733ae397 31 // Set points and display variables
balsamfir 3:dfb6733ae397 32 float leftMotor, rightMotor;
balsamfir 6:52686c25e4af 33 int x, height;
balsamfir 3:dfb6733ae397 34 float speed, steering;
balsamfir 2:2bc519e14bae 35
balsamfir 3:dfb6733ae397 36 // Functions
balsamfir 3:dfb6733ae397 37 // ----------------------------------------------------------------
balsamfir 14:2d609d465f00 38 void AutoTrack(bool isTunning) {
balsamfir 14:2d609d465f00 39 float increment = 1;
balsamfir 14:2d609d465f00 40 char key;
balsamfir 2:2bc519e14bae 41
balsamfir 3:dfb6733ae397 42 leftPwm.period(PWM_PERIOD);
balsamfir 3:dfb6733ae397 43 leftPwm.pulsewidth(0);
balsamfir 3:dfb6733ae397 44 rightPwm.period(PWM_PERIOD);
balsamfir 3:dfb6733ae397 45 rightPwm.pulsewidth(0);
balsamfir 3:dfb6733ae397 46
balsamfir 3:dfb6733ae397 47 motorMutex = osMutexCreate(osMutex(motorMutex));
balsamfir 3:dfb6733ae397 48
balsamfir 3:dfb6733ae397 49 bumper.rise(&CollisionISR); // Attach interrupt handler to rising edge of Bumper
balsamfir 2:2bc519e14bae 50
balsamfir 3:dfb6733ae397 51 // Start execution of the threads MotorThread, and NavigationThread:
balsamfir 3:dfb6733ae397 52 navigationId = osThreadCreate(osThread(NavigationThread), NULL);
balsamfir 3:dfb6733ae397 53 motorId = osThreadCreate(osThread(MotorThread), NULL);
balsamfir 3:dfb6733ae397 54
balsamfir 2:2bc519e14bae 55 // Start the watch dog timer and enable the watch dog interrupt
balsamfir 3:dfb6733ae397 56 osTimerId oneShotId = osTimerCreate(osTimer(Wdtimer), osTimerOnce, (void *)0);
balsamfir 3:dfb6733ae397 57 led3 = 0; // Clear watch dog led3 status
balsamfir 2:2bc519e14bae 58
balsamfir 3:dfb6733ae397 59 //SPI Initialization
balsamfir 3:dfb6733ae397 60 pc.printf("\n\rStarting SPI...");
balsamfir 3:dfb6733ae397 61 deSpi.format(16,1); // 16 bit mode 1
balsamfir 3:dfb6733ae397 62 ioReset = 0; ioReset = 1;
balsamfir 3:dfb6733ae397 63 wait_us(10); ioReset = 0; spiReset = 0; spiReset = 1;
balsamfir 3:dfb6733ae397 64 wait_us(10); spiReset = 0;
balsamfir 14:2d609d465f00 65 pc.printf("\n\rDevice Id: %d \r\n", deSpi.write(0x8004)); // Read count & time for both motors
balsamfir 2:2bc519e14bae 66
balsamfir 3:dfb6733ae397 67 // Specify periodic ISRs and their intervals in seconds
balsamfir 2:2bc519e14bae 68 // TODO: Optimize interrupt time for motor... Currently too fast
balsamfir 6:52686c25e4af 69 motorPeriodicInt.attach(&MotorISR, MOTOR_PERIOD);
balsamfir 6:52686c25e4af 70 sensorPeriodicInt.attach(&NavigationISR, NAVIGATION_PERIOD);
balsamfir 2:2bc519e14bae 71
balsamfir 2:2bc519e14bae 72 while (1) {
balsamfir 2:2bc519e14bae 73
balsamfir 3:dfb6733ae397 74 osTimerStart(oneShotId, 2000); // Start or restart the watchdog timer interrupt and set to 2000ms.
balsamfir 14:2d609d465f00 75
balsamfir 14:2d609d465f00 76 if (isTunning) {
balsamfir 14:2d609d465f00 77 if (pc.readable()) {
balsamfir 14:2d609d465f00 78 key = pc.getc();
balsamfir 14:2d609d465f00 79 if (key == 'q') {
balsamfir 14:2d609d465f00 80 ShutDown();
balsamfir 14:2d609d465f00 81 return;
balsamfir 14:2d609d465f00 82 }
balsamfir 14:2d609d465f00 83 UpdateGains(key, &increment);
balsamfir 14:2d609d465f00 84 pc.printf("Increment \t %f \r\n", increment);
balsamfir 14:2d609d465f00 85 pc.printf("Motor \t KP: %f, KI: %f \r\n", leftMotorPI.kP, leftMotorPI.kI);
balsamfir 14:2d609d465f00 86 pc.printf("Steering \t KP: %f, KI: %f \r\n", xPI.kP, xPI.kI);
balsamfir 14:2d609d465f00 87 pc.printf("Speed \t KP: %f, KI: %f \r\n\r\n\r\n", heightPI.kP, heightPI.kI);
balsamfir 14:2d609d465f00 88 }
balsamfir 14:2d609d465f00 89 } else {
balsamfir 14:2d609d465f00 90 if (pc.readable()) {
balsamfir 14:2d609d465f00 91 key = pc.getc();
balsamfir 14:2d609d465f00 92 if (key == 'q') {
balsamfir 14:2d609d465f00 93 ShutDown();
balsamfir 14:2d609d465f00 94 return;
balsamfir 14:2d609d465f00 95 }
balsamfir 14:2d609d465f00 96 }
balsamfir 14:2d609d465f00 97 pc.printf("X Coordinate: %d, Height: %d \r\n", x, height);
balsamfir 14:2d609d465f00 98 pc.printf("Speed Set: %f, Steering Set: %f \r\n", speed, steering);
balsamfir 14:2d609d465f00 99 pc.printf("Left Motor Set: %f, Right Motor Set: %f \n\r\r\n\r\n", leftMotor, rightMotor);
balsamfir 14:2d609d465f00 100 }
balsamfir 2:2bc519e14bae 101
balsamfir 2:2bc519e14bae 102 Thread::wait(500); // Go to sleep for 500 ms
balsamfir 2:2bc519e14bae 103 }
balsamfir 2:2bc519e14bae 104 }
balsamfir 2:2bc519e14bae 105
balsamfir 2:2bc519e14bae 106 void ManualControl(void) {
balsamfir 2:2bc519e14bae 107
balsamfir 2:2bc519e14bae 108 }
balsamfir 2:2bc519e14bae 109
balsamfir 14:2d609d465f00 110 void ShutDown(void) {
balsamfir 14:2d609d465f00 111 osThreadTerminate(navigationId);
balsamfir 14:2d609d465f00 112 osThreadTerminate(motorId);
balsamfir 14:2d609d465f00 113 leftPwm.pulsewidth(0);
balsamfir 14:2d609d465f00 114 rightPwm.pulsewidth(0);
balsamfir 14:2d609d465f00 115 }
balsamfir 14:2d609d465f00 116
balsamfir 3:dfb6733ae397 117
balsamfir 3:dfb6733ae397 118 // Threads
balsamfir 3:dfb6733ae397 119 // ----------------------------------------------------------------
balsamfir 3:dfb6733ae397 120 void NavigationThread(void const *argument) {
balsamfir 3:dfb6733ae397 121 int count;
balsamfir 14:2d609d465f00 122 int missingCount;
balsamfir 3:dfb6733ae397 123
balsamfir 3:dfb6733ae397 124 while (1) {
balsamfir 3:dfb6733ae397 125 osSignalWait(navigationSignal, osWaitForever); // Go to sleep until navigation signal is set high by ISR
balsamfir 3:dfb6733ae397 126
balsamfir 3:dfb6733ae397 127 count = pixy.getBlocks(1);
balsamfir 3:dfb6733ae397 128
balsamfir 3:dfb6733ae397 129 // If target returned
balsamfir 3:dfb6733ae397 130 if (count && (pixy.blocks[0].signature == TARGET_DECIMAL)) {
balsamfir 14:2d609d465f00 131 missingCount = 0;
balsamfir 6:52686c25e4af 132 height = pixy.blocks[0].height; //use this for now
balsamfir 6:52686c25e4af 133 x = pixy.blocks[0].x;
balsamfir 5:f655435d0782 134
balsamfir 6:52686c25e4af 135 speed = heightPI.Run(HEIGHT_SETPOINT-height, SPEED_MAX);
balsamfir 7:5ef312aa2678 136 steering = xPI.Run(X_SETPOINT-x, SPEED_MAX);
balsamfir 3:dfb6733ae397 137
balsamfir 3:dfb6733ae397 138 // Give setpoints to MotorThread
balsamfir 3:dfb6733ae397 139 osMutexWait(motorMutex, osWaitForever);
balsamfir 7:5ef312aa2678 140 if (speed >= 0) {
balsamfir 7:5ef312aa2678 141 leftMotor = speed - steering;
balsamfir 7:5ef312aa2678 142 rightMotor = speed + steering;
balsamfir 7:5ef312aa2678 143 } else {
balsamfir 7:5ef312aa2678 144 leftMotor = speed - steering;
balsamfir 7:5ef312aa2678 145 rightMotor = speed + steering;
balsamfir 7:5ef312aa2678 146 }
balsamfir 3:dfb6733ae397 147 osMutexRelease(motorMutex);
balsamfir 14:2d609d465f00 148 } else {
balsamfir 14:2d609d465f00 149 if (missingCount >= 30) {
balsamfir 14:2d609d465f00 150 osMutexWait(motorMutex, osWaitForever);
balsamfir 14:2d609d465f00 151 leftMotor = (leftMotor > rightMotor)? SPEED_MAX/3 : -SPEED_MAX/3;
balsamfir 14:2d609d465f00 152 rightMotor = (rightMotor > leftMotor)? SPEED_MAX/3 : -SPEED_MAX/3;
balsamfir 14:2d609d465f00 153 osMutexRelease(motorMutex);
balsamfir 14:2d609d465f00 154 } else {
balsamfir 14:2d609d465f00 155 missingCount++;
balsamfir 14:2d609d465f00 156 }
balsamfir 3:dfb6733ae397 157 }
balsamfir 3:dfb6733ae397 158 }
balsamfir 3:dfb6733ae397 159 }
balsamfir 3:dfb6733ae397 160
balsamfir 3:dfb6733ae397 161 void MotorThread(void const *argument) {
balsamfir 3:dfb6733ae397 162 float leftSet, rightSet, angVel;
balsamfir 14:2d609d465f00 163 float timeOnLeft, timeOnRight;
balsamfir 14:2d609d465f00 164 short dP, dt;
balsamfir 3:dfb6733ae397 165
balsamfir 3:dfb6733ae397 166 while (1) {
balsamfir 3:dfb6733ae397 167 osSignalWait(motorSignal, osWaitForever); // Go to sleep until motor signal is set high by ISR
balsamfir 3:dfb6733ae397 168 led2= !led2; // Alive status - led2 toggles each time MotorControlThread is signaled.
balsamfir 3:dfb6733ae397 169
balsamfir 3:dfb6733ae397 170 // Get setpoints from navigation
balsamfir 3:dfb6733ae397 171 osMutexWait(motorMutex, osWaitForever);
balsamfir 3:dfb6733ae397 172 leftSet = leftMotor;
balsamfir 3:dfb6733ae397 173 rightSet = rightMotor;
balsamfir 3:dfb6733ae397 174 osMutexRelease(motorMutex);
balsamfir 3:dfb6733ae397 175
balsamfir 3:dfb6733ae397 176 // Run PI control on left motor
balsamfir 3:dfb6733ae397 177 dP = deSpi.write(0);
balsamfir 3:dfb6733ae397 178 dt = deSpi.write(0);
balsamfir 3:dfb6733ae397 179 angVel = QE2RadsPerSec(dP, dt);
balsamfir 6:52686c25e4af 180 timeOnLeft = leftMotorPI.Run(leftSet-angVel, PWM_PERIOD);
balsamfir 3:dfb6733ae397 181
balsamfir 3:dfb6733ae397 182 // Run PI control on right motor
balsamfir 14:2d609d465f00 183 dP = deSpi.write(0);
balsamfir 14:2d609d465f00 184 dt = deSpi.write(0);
balsamfir 14:2d609d465f00 185 angVel = -QE2RadsPerSec(dP, dt); // motors have opposite orientations
balsamfir 7:5ef312aa2678 186 timeOnRight = rightMotorPI.Run(rightSet-angVel, PWM_PERIOD); //
balsamfir 3:dfb6733ae397 187
balsamfir 7:5ef312aa2678 188 // Output new PWM and direction
balsamfir 6:52686c25e4af 189 if (timeOnLeft >= 0) leftDir = 1;
balsamfir 3:dfb6733ae397 190 else leftDir = 0;
balsamfir 3:dfb6733ae397 191
balsamfir 6:52686c25e4af 192 if (timeOnRight >= 0) rightDir = 0;
balsamfir 3:dfb6733ae397 193 else rightDir = 1;
balsamfir 3:dfb6733ae397 194
balsamfir 6:52686c25e4af 195 leftPwm.pulsewidth(fabs(timeOnLeft));
balsamfir 6:52686c25e4af 196 rightPwm.pulsewidth(fabs(timeOnRight));
balsamfir 3:dfb6733ae397 197 }
balsamfir 3:dfb6733ae397 198 }
balsamfir 3:dfb6733ae397 199
balsamfir 3:dfb6733ae397 200
balsamfir 3:dfb6733ae397 201 // Interrupt Service Routines
balsamfir 3:dfb6733ae397 202 // ----------------------------------------------------------------
balsamfir 3:dfb6733ae397 203
balsamfir 3:dfb6733ae397 204 // TODO: Shutdown system
balsamfir 3:dfb6733ae397 205 void WatchdogISR(void const *n) {
balsamfir 3:dfb6733ae397 206 led3=1; // Activated when the watchdog timer times out
balsamfir 3:dfb6733ae397 207 }
balsamfir 3:dfb6733ae397 208
balsamfir 3:dfb6733ae397 209 void MotorISR(void) {
balsamfir 3:dfb6733ae397 210 osSignalSet(motorId,0x1); // Activate the signal, MotorControl, with each periodic timer interrupt
balsamfir 3:dfb6733ae397 211 }
balsamfir 3:dfb6733ae397 212
balsamfir 3:dfb6733ae397 213 void NavigationISR(void) {
balsamfir 3:dfb6733ae397 214 osSignalSet(navigationId,0x1); // Activate the signal, SensorControl, with each periodic timer interrupt
balsamfir 3:dfb6733ae397 215 }
balsamfir 3:dfb6733ae397 216
balsamfir 3:dfb6733ae397 217 // TODO: Shutdown system
balsamfir 3:dfb6733ae397 218 void CollisionISR(void) {
balsamfir 3:dfb6733ae397 219 led4 = 1; // Activated on collision
balsamfir 14:2d609d465f00 220
balsamfir 14:2d609d465f00 221 }
balsamfir 14:2d609d465f00 222
balsamfir 14:2d609d465f00 223 // Nasty case function to update gain values
balsamfir 14:2d609d465f00 224 void UpdateGains(char key, float *increment) {
balsamfir 14:2d609d465f00 225 switch(key) {
balsamfir 14:2d609d465f00 226 case 'e':
balsamfir 14:2d609d465f00 227 leftMotorPI.kP = leftMotorPI.kP + *increment;
balsamfir 14:2d609d465f00 228 break;
balsamfir 14:2d609d465f00 229 case 'd':
balsamfir 14:2d609d465f00 230 leftMotorPI.kP = leftMotorPI.kP - *increment;
balsamfir 14:2d609d465f00 231 break;
balsamfir 14:2d609d465f00 232 case 'r':
balsamfir 14:2d609d465f00 233 leftMotorPI.kI = leftMotorPI.kI + *increment;
balsamfir 14:2d609d465f00 234 break;
balsamfir 14:2d609d465f00 235 case 'f':
balsamfir 14:2d609d465f00 236 leftMotorPI.kI = leftMotorPI.kI - *increment;
balsamfir 14:2d609d465f00 237 break;
balsamfir 14:2d609d465f00 238 case 't':
balsamfir 14:2d609d465f00 239 xPI.kP = xPI.kP + *increment;
balsamfir 14:2d609d465f00 240 break;
balsamfir 14:2d609d465f00 241 case 'g':
balsamfir 14:2d609d465f00 242 xPI.kP = xPI.kP - *increment;
balsamfir 14:2d609d465f00 243 break;
balsamfir 14:2d609d465f00 244 case 'y':
balsamfir 14:2d609d465f00 245 xPI.kI = xPI.kI + *increment;
balsamfir 14:2d609d465f00 246 break;
balsamfir 14:2d609d465f00 247 case 'h':
balsamfir 14:2d609d465f00 248 xPI.kI = xPI.kI - *increment;
balsamfir 14:2d609d465f00 249 break;
balsamfir 14:2d609d465f00 250 case 'u':
balsamfir 14:2d609d465f00 251 heightPI.kP = heightPI.kP + *increment;
balsamfir 14:2d609d465f00 252 break;
balsamfir 14:2d609d465f00 253 case 'j':
balsamfir 14:2d609d465f00 254 heightPI.kP = heightPI.kP - *increment;
balsamfir 14:2d609d465f00 255 break;
balsamfir 14:2d609d465f00 256 case 'i':
balsamfir 14:2d609d465f00 257 heightPI.kI = heightPI.kI + *increment;
balsamfir 14:2d609d465f00 258 break;
balsamfir 14:2d609d465f00 259 case 'k':
balsamfir 14:2d609d465f00 260 heightPI.kI = heightPI.kI - *increment;
balsamfir 14:2d609d465f00 261 break;
balsamfir 14:2d609d465f00 262 case 'o':
balsamfir 14:2d609d465f00 263 *increment = *increment * 10;
balsamfir 14:2d609d465f00 264 break;
balsamfir 14:2d609d465f00 265 case 'l':
balsamfir 14:2d609d465f00 266 *increment = *increment / 10;
balsamfir 14:2d609d465f00 267 break;
balsamfir 14:2d609d465f00 268 }
balsamfir 2:2bc519e14bae 269 }