James MacLean / Robot_Control

The subsystem design/basis for the final project

Dependencies:   mbed-rtos mbed-src pixylib

robot.cpp@6:52686c25e4af, 2016-03-25 (annotated)

Committer:
balsamfir
Date:
Fri Mar 25 15:18:33 2016 +0000
Revision:
6:52686c25e4af
Parent:
5:f655435d0782
Child:
7:5ef312aa2678
Commit cuz slow;

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