PlayBack
Dependencies: TPixy-Interface
Fork of ObjectFollower by
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?
User | Revision | Line number | New 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 | } |