James MacLean / Robot_Control

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed-rtos mbed-src pixylib

robot.cpp@9:62fbb69b612c, 2016-03-25 (annotated)

Committer:
balsamfir
Date:
Fri Mar 25 21:20:10 2016 +0000
Revision:
9:62fbb69b612c
Parent:
8:b0478286ad21 
commit before revert

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