ECE4333 - 2018 - Ahmed & Brandon / Mbed OS MbedOS_Robot_Team

ManualControl

Dependencies:   TPixy-Interface

Fork of MbedOS_Robot by ECE4333 - 2018 - Ahmed & Brandon

PiControlThread.cpp@23:1839085ffdcf, 2018-03-29 (annotated)

Committer:
asobhy
Date:
Thu Mar 29 22:33:52 2018 +0000
Revision:
23:1839085ffdcf
Parent:
22:c09acff62e6a 
PLAYBACK SUBSYSTEM WORKING!!!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
asobhy 8:a0890fa79084 1 /******************************************************************************/
asobhy 8:a0890fa79084 2 // ECE4333
asobhy 9:fe56b888985c 3 // LAB Partner 1: Ahmed Sobhy - ID: 3594449
asobhy 9:fe56b888985c 4 // LAB Partner 2: Brandon Kingman - ID: 3470444
asobhy 9:fe56b888985c 5 // Project: Autonomous Robot Design
asobhy 9:fe56b888985c 6 // Instructor: Prof. Chris Diduch
asobhy 8:a0890fa79084 7 /******************************************************************************/
asobhy 8:a0890fa79084 8 // filename: PiControlThread.cpp
asobhy 8:a0890fa79084 9 // file content description:
asobhy 8:a0890fa79084 10 // * Function to Create the PiControl Thread
asobhy 8:a0890fa79084 11 // * PiControl Thread
asobhy 8:a0890fa79084 12 // * PiControl ISR
asobhy 8:a0890fa79084 13 // * Variables related to the functionality of the thread
asobhy 8:a0890fa79084 14 /******************************************************************************/
asobhy 8:a0890fa79084 15
asobhy 0:a355e511bc5d 16 #include "mbed.h"
asobhy 0:a355e511bc5d 17 #include "ui.h"
asobhy 9:fe56b888985c 18 #include "Drivers/motor_driver_r.h"
asobhy 9:fe56b888985c 19 #include "Drivers/motor_driver_l.h"
asobhy 0:a355e511bc5d 20 #include "Drivers/DE0_driver.h"
asobhy 1:3e9684e81312 21 #include "PiControlThread.h"
asobhy 7:73fd05fe556a 22 #include "Drivers/PiController.h"
asobhy 15:cf67f83d5409 23 #include "ui.h"
asobhy 15:cf67f83d5409 24 #include "CameraThread.h"
asobhy 0:a355e511bc5d 25
asobhy 23:1839085ffdcf 26
asobhy 23:1839085ffdcf 27
asobhy 10:8919b1b76243 28 // setpoints
asobhy 22:c09acff62e6a 29 // speed
asobhy 22:c09acff62e6a 30 int setpointR = 0;
asobhy 22:c09acff62e6a 31 int setpointL = 0;
asobhy 15:cf67f83d5409 32
asobhy 23:1839085ffdcf 33 dp_t dpArray;
asobhy 0:a355e511bc5d 34
asobhy 23:1839085ffdcf 35 int iEnd;
asobhy 23:1839085ffdcf 36
asobhy 23:1839085ffdcf 37 bool memoryFull = false;
asobhy 23:1839085ffdcf 38 bool patrolStart = true;
asobhy 23:1839085ffdcf 39 bool reverse = true;
asobhy 23:1839085ffdcf 40 bool do180 = true;
asobhy 23:1839085ffdcf 41 int cnt=0;
asobhy 23:1839085ffdcf 42
asobhy 23:1839085ffdcf 43 //
asobhy 9:fe56b888985c 44 int velR, velL;
asobhy 10:8919b1b76243 45
asobhy 10:8919b1b76243 46 // control signal
asobhy 9:fe56b888985c 47 int32_t U_right, U_left;
asobhy 9:fe56b888985c 48
asobhy 9:fe56b888985c 49 sensors_t sensors;
asobhy 0:a355e511bc5d 50
asobhy 0:a355e511bc5d 51 void PiControlThread(void const *);
asobhy 0:a355e511bc5d 52 void PeriodicInterruptISR(void);
asobhy 0:a355e511bc5d 53
asobhy 10:8919b1b76243 54 // steering gain
asobhy 16:58ec2b891a25 55 float Ks = 0.15;
asobhy 10:8919b1b76243 56 // distance gain
asobhy 16:58ec2b891a25 57 float Kd = 10;
asobhy 10:8919b1b76243 58
asobhy 10:8919b1b76243 59 // overall robot required speed
asobhy 10:8919b1b76243 60 int Setpoint;
asobhy 17:1184df616383 61 Mutex mutexSetpoint;
asobhy 10:8919b1b76243 62
asobhy 0:a355e511bc5d 63 osThreadId PiControlId;
asobhy 0:a355e511bc5d 64
asobhy 0:a355e511bc5d 65 /******************************************************************************/
asobhy 0:a355e511bc5d 66 // osPriorityIdle = -3, ///< priority: idle (lowest)
asobhy 0:a355e511bc5d 67 // osPriorityLow = -2, ///< priority: low
asobhy 0:a355e511bc5d 68 // osPriorityBelowNormal = -1, ///< priority: below normal
asobhy 0:a355e511bc5d 69 // osPriorityNormal = 0, ///< priority: normal (default)
asobhy 0:a355e511bc5d 70 // osPriorityAboveNormal = +1, ///< priority: above normal
asobhy 0:a355e511bc5d 71 // osPriorityHigh = +2, ///< priority: high
asobhy 0:a355e511bc5d 72 // osPriorityRealtime = +3, ///< priority: realtime (highest)
asobhy 0:a355e511bc5d 73 /******************************************************************************/
asobhy 0:a355e511bc5d 74
asobhy 0:a355e511bc5d 75 // Declare PeriodicInterruptThread as a thread/process
asobhy 23:1839085ffdcf 76 osThreadDef(PiControlThread, osPriorityRealtime, 1024);
asobhy 0:a355e511bc5d 77
asobhy 0:a355e511bc5d 78 Ticker PeriodicInt; // Declare a timer interrupt: PeriodicInt
asobhy 0:a355e511bc5d 79
asobhy 7:73fd05fe556a 80 /*******************************************************************************
asobhy 23:1839085ffdcf 81 * @brief function that creates a thread for the PI controller. It initializes
asobhy 23:1839085ffdcf 82 * the PI controller's gains and initializes the DC Motor. It also
asobhy 7:73fd05fe556a 83 * initializes the PIControllerThread runs at 50ms period
asobhy 7:73fd05fe556a 84 * @param none
asobhy 7:73fd05fe556a 85 * @return none
asobhy 7:73fd05fe556a 86 *******************************************************************************/
asobhy 0:a355e511bc5d 87 void PiControlThreadInit()
asobhy 0:a355e511bc5d 88 {
asobhy 4:417e475239c7 89 DE0_init(); // initialize FPGA
asobhy 21:7fee709bb063 90 motorDriver_R_init(); // initialize motorDriver
asobhy 21:7fee709bb063 91 motorDriver_L_init(); // initialize motorDriver
asobhy 6:e7ce340fe91e 92 // Kp,Ki
asobhy 6:e7ce340fe91e 93 PiController_init(1,0.4); // initialize the PI Controller gains and initialize variables
asobhy 23:1839085ffdcf 94
asobhy 0:a355e511bc5d 95 PiControlId = osThreadCreate(osThread(PiControlThread), NULL);
asobhy 0:a355e511bc5d 96
asobhy 0:a355e511bc5d 97 // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval
asobhy 0:a355e511bc5d 98 // in seconds between interrupts, and start interrupt generation:
asobhy 21:7fee709bb063 99 PeriodicInt.attach(&PeriodicInterruptISR, 0.010); // 10ms sampling period -> 100Hz freq
asobhy 23:1839085ffdcf 100
asobhy 0:a355e511bc5d 101 }
asobhy 0:a355e511bc5d 102
asobhy 0:a355e511bc5d 103 /*******************************************************************************
asobhy 23:1839085ffdcf 104 * @brief This is the PI controller thread. It reads several values from the
asobhy 7:73fd05fe556a 105 * FPGA such as speed, time and other sensors data
asobhy 7:73fd05fe556a 106 * @param none
asobhy 7:73fd05fe556a 107 * @return none
asobhy 0:a355e511bc5d 108 *******************************************************************************/
asobhy 0:a355e511bc5d 109 void PiControlThread(void const *argument)
asobhy 1:3e9684e81312 110 {
asobhy 23:1839085ffdcf 111 dpArray.i = 0;
asobhy 23:1839085ffdcf 112 dpArray.size = ARRAY_SIZE;
asobhy 23:1839085ffdcf 113
asobhy 23:1839085ffdcf 114 while (true) {
asobhy 23:1839085ffdcf 115
asobhy 0:a355e511bc5d 116 osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received.
asobhy 23:1839085ffdcf 117
asobhy 14:5777377537a2 118 // Get incremental position and time from QEI
asobhy 9:fe56b888985c 119 DE0_read(&sensors);
asobhy 23:1839085ffdcf 120
asobhy 15:cf67f83d5409 121 // might not be needed
asobhy 20:9118203f7c9c 122 sensors.dp_right = SaturateValue(sensors.dp_right, 112);
asobhy 20:9118203f7c9c 123 sensors.dp_left = SaturateValue(sensors.dp_left, 112);
asobhy 23:1839085ffdcf 124
asobhy 21:7fee709bb063 125 // Maximum angular velocity @ dPostition = 112 is vel = 703 rad/sec
asobhy 23:1839085ffdcf 126 //velR = (float)((6135.92 * sensors.dp_right) / sensors.dt_right) ;
asobhy 23:1839085ffdcf 127
asobhy 21:7fee709bb063 128 // Maximum angular velocity @ dPostition = 112 is vel = 703 rad/sec
asobhy 23:1839085ffdcf 129 //velL = (float)((6135.92 * sensors.dp_left) / sensors.dt_left) ;
asobhy 23:1839085ffdcf 130
asobhy 23:1839085ffdcf 131 // if manual control is ON use Differential drive
asobhy 23:1839085ffdcf 132 if(MC) {
asobhy 23:1839085ffdcf 133 /******************Manual Setpoint and Steering********************/
asobhy 23:1839085ffdcf 134 mutexSetpoint.lock();
asobhy 23:1839085ffdcf 135 setpointR = Setpoint + (Ks*SteeringError);
asobhy 23:1839085ffdcf 136 setpointL = Setpoint - (Ks*SteeringError);
asobhy 23:1839085ffdcf 137 mutexSetpoint.unlock();
asobhy 23:1839085ffdcf 138 /******************Differential End********************************/
asobhy 23:1839085ffdcf 139
asobhy 23:1839085ffdcf 140 // Make sure our limit is not exceeded
asobhy 23:1839085ffdcf 141 setpointR = SaturateValue(setpointR, 112);
asobhy 23:1839085ffdcf 142 setpointL = SaturateValue(setpointL, 112);
asobhy 23:1839085ffdcf 143
asobhy 23:1839085ffdcf 144 /**********************Record Start********************************/
asobhy 23:1839085ffdcf 145 // Record dp if there is room in memory
asobhy 23:1839085ffdcf 146 if( (dpArray.i < dpArray.size) ) {
asobhy 23:1839085ffdcf 147
asobhy 23:1839085ffdcf 148 // only record data when record button is pressed
asobhy 23:1839085ffdcf 149 if(startRecording) {
asobhy 23:1839085ffdcf 150 dpArray.dpR[dpArray.i] = setpointR;
asobhy 23:1839085ffdcf 151 dpArray.dpL[dpArray.i] = setpointL;
asobhy 23:1839085ffdcf 152 dpArray.i++;
asobhy 23:1839085ffdcf 153 patrolStart = true;
asobhy 23:1839085ffdcf 154 }
asobhy 23:1839085ffdcf 155
asobhy 23:1839085ffdcf 156 } else {
asobhy 23:1839085ffdcf 157 // memory is full
asobhy 23:1839085ffdcf 158 memoryFull = true;
asobhy 23:1839085ffdcf 159 }
asobhy 23:1839085ffdcf 160 /************************Record End********************************/
asobhy 17:1184df616383 161
asobhy 23:1839085ffdcf 162 /*********************Playback Start*******************************/
asobhy 23:1839085ffdcf 163 } else { // if manual control is not ON then Playback data
asobhy 23:1839085ffdcf 164
asobhy 23:1839085ffdcf 165 // Check if its the first time to enter the patrol mode
asobhy 23:1839085ffdcf 166 if(patrolStart) {
asobhy 23:1839085ffdcf 167 // Store end index value
asobhy 23:1839085ffdcf 168 iEnd = dpArray.i;
asobhy 23:1839085ffdcf 169 // Reset playback index
asobhy 23:1839085ffdcf 170 dpArray.i = 0;
asobhy 23:1839085ffdcf 171 patrolStart = false;
asobhy 23:1839085ffdcf 172 }
asobhy 23:1839085ffdcf 173
asobhy 23:1839085ffdcf 174 // if do the 180 degree is true then do a 180 rotation
asobhy 23:1839085ffdcf 175 if( do180 == true ) {
asobhy 23:1839085ffdcf 176 // distance = 10ms * cnt * speed
asobhy 23:1839085ffdcf 177 if ( cnt < 300 ) {
asobhy 23:1839085ffdcf 178 setpointR = -10;
asobhy 23:1839085ffdcf 179 setpointL = 10;
asobhy 23:1839085ffdcf 180 cnt++;
asobhy 23:1839085ffdcf 181 }
asobhy 23:1839085ffdcf 182 // exit the 180 code when done
asobhy 23:1839085ffdcf 183 else {
asobhy 23:1839085ffdcf 184 do180 = false;
asobhy 23:1839085ffdcf 185 cnt = 0;
asobhy 23:1839085ffdcf 186 }
asobhy 23:1839085ffdcf 187 }
asobhy 23:1839085ffdcf 188 else if ( dpArray.i < iEnd ) {
asobhy 23:1839085ffdcf 189
asobhy 23:1839085ffdcf 190 // Data Playback normal direction
asobhy 23:1839085ffdcf 191 if(!reverse) {
asobhy 23:1839085ffdcf 192 setpointR = dpArray.dpR[dpArray.i];
asobhy 23:1839085ffdcf 193 setpointL = dpArray.dpL[dpArray.i];
asobhy 23:1839085ffdcf 194 }
asobhy 23:1839085ffdcf 195 // Data Playback in reverse direction
asobhy 23:1839085ffdcf 196 else {
asobhy 23:1839085ffdcf 197 // Wheel dps are swaped to mantain correct direction playback
asobhy 23:1839085ffdcf 198 setpointR = dpArray.dpL[dpArray.i];
asobhy 23:1839085ffdcf 199 setpointL = dpArray.dpR[dpArray.i];
asobhy 23:1839085ffdcf 200 }
asobhy 23:1839085ffdcf 201 dpArray.i++;
asobhy 23:1839085ffdcf 202
asobhy 23:1839085ffdcf 203 } else {
asobhy 23:1839085ffdcf 204 // We reached the end of our playback we need to toggle reverse
asobhy 23:1839085ffdcf 205 reverse = !reverse;
asobhy 23:1839085ffdcf 206 // start from the beginning by reseting the index
asobhy 23:1839085ffdcf 207 dpArray.i = 0;
asobhy 23:1839085ffdcf 208 // Do a 180 degree turn
asobhy 23:1839085ffdcf 209 do180 = true;
asobhy 23:1839085ffdcf 210 }
asobhy 23:1839085ffdcf 211
asobhy 23:1839085ffdcf 212
asobhy 23:1839085ffdcf 213 /***********************Playback End*******************************/
asobhy 23:1839085ffdcf 214 }
asobhy 14:5777377537a2 215
asobhy 9:fe56b888985c 216 U_right = PiControllerR(setpointR,sensors.dp_right);
asobhy 9:fe56b888985c 217 U_left = PiControllerL(setpointL,sensors.dp_left);
asobhy 23:1839085ffdcf 218
asobhy 14:5777377537a2 219 // Set speed and direction for right motor
asobhy 23:1839085ffdcf 220 if (U_right >= 0) {
asobhy 9:fe56b888985c 221 motorDriver_R_forward(U_right);
asobhy 23:1839085ffdcf 222 } else if (U_right < 0) {
asobhy 9:fe56b888985c 223 motorDriver_R_reverse(U_right);
asobhy 0:a355e511bc5d 224 }
asobhy 23:1839085ffdcf 225
asobhy 17:1184df616383 226 // Set speed and direction for left motor
asobhy 23:1839085ffdcf 227 if (U_left >= 0) {
asobhy 9:fe56b888985c 228 motorDriver_L_forward(U_left);
asobhy 23:1839085ffdcf 229 } else if (U_left < 0) {
asobhy 9:fe56b888985c 230 motorDriver_L_reverse(U_left);
asobhy 7:73fd05fe556a 231 }
asobhy 23:1839085ffdcf 232
asobhy 0:a355e511bc5d 233 }
asobhy 23:1839085ffdcf 234
asobhy 0:a355e511bc5d 235 }
asobhy 0:a355e511bc5d 236
asobhy 0:a355e511bc5d 237 /*******************************************************************************
asobhy 23:1839085ffdcf 238 * @brief The ISR below signals the PIControllerThread. it is setup to run
asobhy 22:c09acff62e6a 239 * every 10ms
asobhy 7:73fd05fe556a 240 * @param none
asobhy 7:73fd05fe556a 241 * @return none
asobhy 0:a355e511bc5d 242 *******************************************************************************/
asobhy 0:a355e511bc5d 243 void PeriodicInterruptISR(void)
asobhy 0:a355e511bc5d 244 {
asobhy 0:a355e511bc5d 245 // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread.
asobhy 23:1839085ffdcf 246 osSignalSet(PiControlId,0x1);
asobhy 0:a355e511bc5d 247 }