James MacLean / Robot_Control

The subsystem design/basis for the final project

Dependencies:   mbed-rtos mbed-src pixylib

robot.cpp@4:01252f56e0e5, 2016-03-18 (annotated)

Committer:
balsamfir
Date:
Fri Mar 18 17:05:22 2016 +0000
Revision:
4:01252f56e0e5
Parent:
3:dfb6733ae397
Child:
5:f655435d0782
Commit before adding features

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 2:2bc519e14bae 7 #define MAX_BLOCKS 1
balsamfir 3:dfb6733ae397 8 #define TARGET_DECIMAL 10
balsamfir 3:dfb6733ae397 9 #define PWM_PERIOD 0.001
balsamfir 2:2bc519e14bae 10
balsamfir 3:dfb6733ae397 11 #define MOTOR_KP 0.000120
balsamfir 3:dfb6733ae397 12 #define MOTOR_KI 0.0000000001
balsamfir 2:2bc519e14bae 13
balsamfir 3:dfb6733ae397 14 #define X_SETPOINT 160
balsamfir 3:dfb6733ae397 15 #define STEERING_KP 0.01
balsamfir 3:dfb6733ae397 16 #define STEERING_KI 0.005
balsamfir 2:2bc519e14bae 17
balsamfir 3:dfb6733ae397 18 #define HEIGHT_SETPOINT 100
balsamfir 3:dfb6733ae397 19 #define SPEED_KP 0.01
balsamfir 3:dfb6733ae397 20 #define SPEED_KI 0.01
balsamfir 3:dfb6733ae397 21 #define SPEED_MAX 10
balsamfir 2:2bc519e14bae 22
balsamfir 3:dfb6733ae397 23 // Interrupt and thread control
balsamfir 3:dfb6733ae397 24 osThreadId motorId, navigationId, wdtId;
balsamfir 3:dfb6733ae397 25 osThreadDef(MotorThread, osPriorityRealtime, DEFAULT_STACK_SIZE);
balsamfir 3:dfb6733ae397 26 osThreadDef(NavigationThread, osPriorityAboveNormal, DEFAULT_STACK_SIZE);
balsamfir 3:dfb6733ae397 27 osTimerDef(Wdtimer, WatchdogISR);
balsamfir 3:dfb6733ae397 28 int32_t motorSignal, navigationSignal;
balsamfir 3:dfb6733ae397 29 Ticker motorPeriodicInt;
balsamfir 3:dfb6733ae397 30 Ticker sensorPeriodicInt;
balsamfir 2:2bc519e14bae 31
balsamfir 3:dfb6733ae397 32 // Mutex to protect left and right motor setpoints
balsamfir 3:dfb6733ae397 33 osMutexId motorMutex;
balsamfir 3:dfb6733ae397 34 osMutexDef(motorMutex);
balsamfir 2:2bc519e14bae 35
balsamfir 3:dfb6733ae397 36 // Set points and display variables
balsamfir 3:dfb6733ae397 37 float leftMotor, rightMotor;
balsamfir 3:dfb6733ae397 38 int xTracked, heightTracked;
balsamfir 3:dfb6733ae397 39 float speed, steering;
balsamfir 2:2bc519e14bae 40
balsamfir 3:dfb6733ae397 41 // PI internal variables (TODO: move to object)
balsamfir 3:dfb6733ae397 42 float steeringIntegral, speedIntegral;
balsamfir 3:dfb6733ae397 43 float leftMotorIntegral, rightMotorIntegral;
balsamfir 2:2bc519e14bae 44
balsamfir 2:2bc519e14bae 45
balsamfir 3:dfb6733ae397 46 // Functions
balsamfir 3:dfb6733ae397 47 // ----------------------------------------------------------------
balsamfir 3:dfb6733ae397 48 void AutoTrack() {
balsamfir 2:2bc519e14bae 49
balsamfir 3:dfb6733ae397 50 leftPwm.period(PWM_PERIOD);
balsamfir 3:dfb6733ae397 51 leftPwm.pulsewidth(0);
balsamfir 3:dfb6733ae397 52 rightPwm.period(PWM_PERIOD);
balsamfir 3:dfb6733ae397 53 rightPwm.pulsewidth(0);
balsamfir 3:dfb6733ae397 54
balsamfir 3:dfb6733ae397 55 motorMutex = osMutexCreate(osMutex(motorMutex));
balsamfir 3:dfb6733ae397 56
balsamfir 3:dfb6733ae397 57 bumper.rise(&CollisionISR); // Attach interrupt handler to rising edge of Bumper
balsamfir 2:2bc519e14bae 58
balsamfir 3:dfb6733ae397 59 // Start execution of the threads MotorThread, and NavigationThread:
balsamfir 3:dfb6733ae397 60 navigationId = osThreadCreate(osThread(NavigationThread), NULL);
balsamfir 3:dfb6733ae397 61 motorId = osThreadCreate(osThread(MotorThread), NULL);
balsamfir 3:dfb6733ae397 62
balsamfir 2:2bc519e14bae 63 // Start the watch dog timer and enable the watch dog interrupt
balsamfir 3:dfb6733ae397 64 osTimerId oneShotId = osTimerCreate(osTimer(Wdtimer), osTimerOnce, (void *)0);
balsamfir 3:dfb6733ae397 65 led3 = 0; // Clear watch dog led3 status
balsamfir 2:2bc519e14bae 66
balsamfir 3:dfb6733ae397 67 //SPI Initialization
balsamfir 3:dfb6733ae397 68 pc.printf("\n\rStarting SPI...");
balsamfir 3:dfb6733ae397 69 deSpi.format(16,1); // 16 bit mode 1
balsamfir 3:dfb6733ae397 70 ioReset = 0; ioReset = 1;
balsamfir 3:dfb6733ae397 71 wait_us(10); ioReset = 0; spiReset = 0; spiReset = 1;
balsamfir 3:dfb6733ae397 72 wait_us(10); spiReset = 0;
balsamfir 3:dfb6733ae397 73 pc.printf("\n\rDevice Id: %d", deSpi.write(0x8004)); // Read count & time for both motors
balsamfir 2:2bc519e14bae 74
balsamfir 3:dfb6733ae397 75 // Specify periodic ISRs and their intervals in seconds
balsamfir 2:2bc519e14bae 76 // TODO: Optimize interrupt time for motor... Currently too fast
balsamfir 3:dfb6733ae397 77 motorPeriodicInt.attach(&MotorISR, .001);
balsamfir 3:dfb6733ae397 78 sensorPeriodicInt.attach(&NavigationISR, 0.1); //trigger sensor thread 60 times/sec
balsamfir 2:2bc519e14bae 79
balsamfir 2:2bc519e14bae 80 while (1) {
balsamfir 2:2bc519e14bae 81
balsamfir 3:dfb6733ae397 82 osTimerStart(oneShotId, 2000); // Start or restart the watchdog timer interrupt and set to 2000ms.
balsamfir 2:2bc519e14bae 83
balsamfir 3:dfb6733ae397 84 pc.printf("X Coordinate: %d, Height: %d \r\n", xTracked, heightTracked);
balsamfir 3:dfb6733ae397 85 pc.printf("Speed Set: %f, Steering Set: %f \r\n", speed, steering);
balsamfir 3:dfb6733ae397 86 pc.printf("Speed Int: %f, Steering Int: %f \r\n", speedIntegral, steeringIntegral);
balsamfir 3:dfb6733ae397 87 pc.printf("Left Motor Set: %f, Right Motor Set: %f \n\r\r\n", leftMotor, rightMotor);
balsamfir 2:2bc519e14bae 88
balsamfir 2:2bc519e14bae 89 Thread::wait(500); // Go to sleep for 500 ms
balsamfir 2:2bc519e14bae 90 }
balsamfir 2:2bc519e14bae 91 }
balsamfir 2:2bc519e14bae 92
balsamfir 2:2bc519e14bae 93 void ManualControl(void) {
balsamfir 2:2bc519e14bae 94
balsamfir 2:2bc519e14bae 95 }
balsamfir 2:2bc519e14bae 96
balsamfir 2:2bc519e14bae 97 void PixySubsystem(void) {
balsamfir 2:2bc519e14bae 98
balsamfir 3:dfb6733ae397 99 }
balsamfir 3:dfb6733ae397 100
balsamfir 3:dfb6733ae397 101
balsamfir 3:dfb6733ae397 102 // Threads
balsamfir 3:dfb6733ae397 103 // ----------------------------------------------------------------
balsamfir 3:dfb6733ae397 104 void NavigationThread(void const *argument) {
balsamfir 3:dfb6733ae397 105 int count;
balsamfir 3:dfb6733ae397 106
balsamfir 3:dfb6733ae397 107 while (1) {
balsamfir 3:dfb6733ae397 108 osSignalWait(navigationSignal, osWaitForever); // Go to sleep until navigation signal is set high by ISR
balsamfir 3:dfb6733ae397 109
balsamfir 3:dfb6733ae397 110 count = pixy.getBlocks(1);
balsamfir 3:dfb6733ae397 111
balsamfir 3:dfb6733ae397 112 // If target returned
balsamfir 3:dfb6733ae397 113 if (count && (pixy.blocks[0].signature == TARGET_DECIMAL)) {
balsamfir 3:dfb6733ae397 114 heightTracked = pixy.blocks[0].height; //use this for now
balsamfir 3:dfb6733ae397 115 xTracked = pixy.blocks[0].x;
balsamfir 3:dfb6733ae397 116
balsamfir 3:dfb6733ae397 117 PI(HEIGHT_SETPOINT-heightTracked, &speed, &speedIntegral, SPEED_KP, SPEED_KI, SPEED_MAX);
balsamfir 3:dfb6733ae397 118 PI(X_SETPOINT-xTracked, &steering, &steeringIntegral, STEERING_KP, STEERING_KI, 2*SPEED_MAX);
balsamfir 3:dfb6733ae397 119
balsamfir 3:dfb6733ae397 120 // Give setpoints to MotorThread
balsamfir 3:dfb6733ae397 121 osMutexWait(motorMutex, osWaitForever);
balsamfir 3:dfb6733ae397 122 leftMotor = speed - steering;
balsamfir 3:dfb6733ae397 123 rightMotor = speed + steering;
balsamfir 3:dfb6733ae397 124 osMutexRelease(motorMutex);
balsamfir 3:dfb6733ae397 125
balsamfir 3:dfb6733ae397 126 }
balsamfir 3:dfb6733ae397 127 }
balsamfir 3:dfb6733ae397 128 }
balsamfir 3:dfb6733ae397 129
balsamfir 3:dfb6733ae397 130 void MotorThread(void const *argument) {
balsamfir 3:dfb6733ae397 131 float leftSet, rightSet, angVel;
balsamfir 3:dfb6733ae397 132 short dP, dt;
balsamfir 3:dfb6733ae397 133 float uL, uR;
balsamfir 3:dfb6733ae397 134
balsamfir 3:dfb6733ae397 135 while (1) {
balsamfir 3:dfb6733ae397 136 osSignalWait(motorSignal, osWaitForever); // Go to sleep until motor signal is set high by ISR
balsamfir 3:dfb6733ae397 137 led2= !led2; // Alive status - led2 toggles each time MotorControlThread is signaled.
balsamfir 3:dfb6733ae397 138
balsamfir 3:dfb6733ae397 139 // Get setpoints from navigation
balsamfir 3:dfb6733ae397 140 osMutexWait(motorMutex, osWaitForever);
balsamfir 3:dfb6733ae397 141 leftSet = leftMotor;
balsamfir 3:dfb6733ae397 142 rightSet = rightMotor;
balsamfir 3:dfb6733ae397 143 osMutexRelease(motorMutex);
balsamfir 3:dfb6733ae397 144
balsamfir 3:dfb6733ae397 145 // Run PI control on left motor
balsamfir 3:dfb6733ae397 146 dP = deSpi.write(0);
balsamfir 3:dfb6733ae397 147 dt = deSpi.write(0);
balsamfir 3:dfb6733ae397 148 angVel = QE2RadsPerSec(dP, dt);
balsamfir 3:dfb6733ae397 149 PI(leftSet-angVel, &uL, &leftMotorIntegral, MOTOR_KP, MOTOR_KI, PWM_PERIOD);
balsamfir 3:dfb6733ae397 150
balsamfir 3:dfb6733ae397 151 // Run PI control on right motor
balsamfir 3:dfb6733ae397 152 dP = deSpi.write(0);
balsamfir 3:dfb6733ae397 153 dt = deSpi.write(0);
balsamfir 3:dfb6733ae397 154 angVel = QE2RadsPerSec(dP, dt);
balsamfir 3:dfb6733ae397 155 PI(rightSet-angVel, &uR, &rightMotorIntegral, MOTOR_KP, MOTOR_KI, PWM_PERIOD);
balsamfir 3:dfb6733ae397 156
balsamfir 3:dfb6733ae397 157 // Output new PWM and direction (motors have opposite orientations)
balsamfir 3:dfb6733ae397 158 if (uL >= 0) leftDir = 1;
balsamfir 3:dfb6733ae397 159 else leftDir = 0;
balsamfir 3:dfb6733ae397 160
balsamfir 3:dfb6733ae397 161 if (uR >= 0) rightDir = 0;
balsamfir 3:dfb6733ae397 162 else rightDir = 1;
balsamfir 3:dfb6733ae397 163
balsamfir 3:dfb6733ae397 164 leftPwm.pulsewidth(fabs(uL));
balsamfir 3:dfb6733ae397 165 rightPwm.pulsewidth(fabs(uR));
balsamfir 3:dfb6733ae397 166 }
balsamfir 3:dfb6733ae397 167 }
balsamfir 3:dfb6733ae397 168
balsamfir 3:dfb6733ae397 169
balsamfir 3:dfb6733ae397 170 // Interrupt Service Routines
balsamfir 3:dfb6733ae397 171 // ----------------------------------------------------------------
balsamfir 3:dfb6733ae397 172
balsamfir 3:dfb6733ae397 173 // TODO: Shutdown system
balsamfir 3:dfb6733ae397 174 void WatchdogISR(void const *n) {
balsamfir 3:dfb6733ae397 175 led3=1; // Activated when the watchdog timer times out
balsamfir 3:dfb6733ae397 176 }
balsamfir 3:dfb6733ae397 177
balsamfir 3:dfb6733ae397 178 void MotorISR(void) {
balsamfir 3:dfb6733ae397 179 osSignalSet(motorId,0x1); // Activate the signal, MotorControl, with each periodic timer interrupt
balsamfir 3:dfb6733ae397 180 }
balsamfir 3:dfb6733ae397 181
balsamfir 3:dfb6733ae397 182 void NavigationISR(void) {
balsamfir 3:dfb6733ae397 183 osSignalSet(navigationId,0x1); // Activate the signal, SensorControl, with each periodic timer interrupt
balsamfir 3:dfb6733ae397 184 }
balsamfir 3:dfb6733ae397 185
balsamfir 3:dfb6733ae397 186 // TODO: Shutdown system
balsamfir 3:dfb6733ae397 187 void CollisionISR(void) {
balsamfir 3:dfb6733ae397 188 led4 = 1; // Activated on collision
balsamfir 2:2bc519e14bae 189 }