James MacLean / Robot_Control

The subsystem design/basis for the final project

Dependencies:   mbed-rtos mbed-src pixylib

robot.cpp@16:73db7ef2deb6, 2016-03-28 (annotated)

Committer:
balsamfir
Date:
Mon Mar 28 21:39:06 2016 +0000
Revision:
16:73db7ef2deb6
Parent:
15:caa5a93a31d7
Child:
17:47e107f9587b
First working ps3 manual control

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