![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
PlayBack
Dependencies: TPixy-Interface
Fork of ManualControlFinal by
PiControlThread.cpp@17:1184df616383, 2018-03-11 (annotated)
- Committer:
- asobhy
- Date:
- Sun Mar 11 01:26:23 2018 +0000
- Revision:
- 17:1184df616383
- Parent:
- 16:58ec2b891a25
- Child:
- 19:4fbffc755ed7
MANUAL CONTROL - TESTING AND TUNNING REQUIRED
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 | 10:8919b1b76243 | 26 | // setpoints |
asobhy | 15:cf67f83d5409 | 27 | |
asobhy | 0:a355e511bc5d | 28 | |
asobhy | 10:8919b1b76243 | 29 | // |
asobhy | 9:fe56b888985c | 30 | int velR, velL; |
asobhy | 10:8919b1b76243 | 31 | |
asobhy | 10:8919b1b76243 | 32 | // control signal |
asobhy | 9:fe56b888985c | 33 | int32_t U_right, U_left; |
asobhy | 9:fe56b888985c | 34 | |
asobhy | 9:fe56b888985c | 35 | sensors_t sensors; |
asobhy | 0:a355e511bc5d | 36 | |
asobhy | 8:a0890fa79084 | 37 | int time_passed = 0; |
asobhy | 8:a0890fa79084 | 38 | |
asobhy | 0:a355e511bc5d | 39 | void PiControlThread(void const *); |
asobhy | 0:a355e511bc5d | 40 | void PeriodicInterruptISR(void); |
asobhy | 0:a355e511bc5d | 41 | |
asobhy | 10:8919b1b76243 | 42 | // steering gain |
asobhy | 16:58ec2b891a25 | 43 | float Ks = 0.15; |
asobhy | 10:8919b1b76243 | 44 | // distance gain |
asobhy | 16:58ec2b891a25 | 45 | float Kd = 10; |
asobhy | 10:8919b1b76243 | 46 | |
asobhy | 10:8919b1b76243 | 47 | // overall robot required speed |
asobhy | 10:8919b1b76243 | 48 | int Setpoint; |
asobhy | 17:1184df616383 | 49 | Mutex mutexSetpoint; |
asobhy | 10:8919b1b76243 | 50 | |
asobhy | 0:a355e511bc5d | 51 | osThreadId PiControlId; |
asobhy | 0:a355e511bc5d | 52 | |
asobhy | 0:a355e511bc5d | 53 | /******************************************************************************/ |
asobhy | 0:a355e511bc5d | 54 | // osPriorityIdle = -3, ///< priority: idle (lowest) |
asobhy | 0:a355e511bc5d | 55 | // osPriorityLow = -2, ///< priority: low |
asobhy | 0:a355e511bc5d | 56 | // osPriorityBelowNormal = -1, ///< priority: below normal |
asobhy | 0:a355e511bc5d | 57 | // osPriorityNormal = 0, ///< priority: normal (default) |
asobhy | 0:a355e511bc5d | 58 | // osPriorityAboveNormal = +1, ///< priority: above normal |
asobhy | 0:a355e511bc5d | 59 | // osPriorityHigh = +2, ///< priority: high |
asobhy | 0:a355e511bc5d | 60 | // osPriorityRealtime = +3, ///< priority: realtime (highest) |
asobhy | 0:a355e511bc5d | 61 | /******************************************************************************/ |
asobhy | 0:a355e511bc5d | 62 | |
asobhy | 0:a355e511bc5d | 63 | // Declare PeriodicInterruptThread as a thread/process |
asobhy | 0:a355e511bc5d | 64 | osThreadDef(PiControlThread, osPriorityRealtime, 1024); |
asobhy | 0:a355e511bc5d | 65 | |
asobhy | 0:a355e511bc5d | 66 | Ticker PeriodicInt; // Declare a timer interrupt: PeriodicInt |
asobhy | 0:a355e511bc5d | 67 | |
asobhy | 0:a355e511bc5d | 68 | DigitalOut led3(LED3); |
asobhy | 0:a355e511bc5d | 69 | |
asobhy | 7:73fd05fe556a | 70 | /******************************************************************************* |
asobhy | 7:73fd05fe556a | 71 | * @brief function that creates a thread for the PI controller. It initializes |
asobhy | 7:73fd05fe556a | 72 | * the PI controller's gains and initializes the DC Motor. It also |
asobhy | 7:73fd05fe556a | 73 | * initializes the PIControllerThread runs at 50ms period |
asobhy | 7:73fd05fe556a | 74 | * @param none |
asobhy | 7:73fd05fe556a | 75 | * @return none |
asobhy | 7:73fd05fe556a | 76 | *******************************************************************************/ |
asobhy | 0:a355e511bc5d | 77 | void PiControlThreadInit() |
asobhy | 0:a355e511bc5d | 78 | { |
asobhy | 4:417e475239c7 | 79 | DE0_init(); // initialize FPGA |
asobhy | 9:fe56b888985c | 80 | motorDriver_R_init(); // initialize motorDriver |
asobhy | 9:fe56b888985c | 81 | motorDriver_L_init(); // initialize motorDriver |
asobhy | 6:e7ce340fe91e | 82 | // Kp,Ki |
asobhy | 6:e7ce340fe91e | 83 | PiController_init(1,0.4); // initialize the PI Controller gains and initialize variables |
asobhy | 0:a355e511bc5d | 84 | |
asobhy | 0:a355e511bc5d | 85 | PiControlId = osThreadCreate(osThread(PiControlThread), NULL); |
asobhy | 0:a355e511bc5d | 86 | |
asobhy | 0:a355e511bc5d | 87 | // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval |
asobhy | 0:a355e511bc5d | 88 | // in seconds between interrupts, and start interrupt generation: |
asobhy | 1:3e9684e81312 | 89 | PeriodicInt.attach(&PeriodicInterruptISR, 0.05); // 50ms sampling rate |
asobhy | 4:417e475239c7 | 90 | |
asobhy | 0:a355e511bc5d | 91 | } |
asobhy | 0:a355e511bc5d | 92 | |
asobhy | 0:a355e511bc5d | 93 | /******************************************************************************* |
asobhy | 7:73fd05fe556a | 94 | * @brief This is the PI controller thread. It reads several values from the |
asobhy | 7:73fd05fe556a | 95 | * FPGA such as speed, time and other sensors data |
asobhy | 7:73fd05fe556a | 96 | * @param none |
asobhy | 7:73fd05fe556a | 97 | * @return none |
asobhy | 0:a355e511bc5d | 98 | *******************************************************************************/ |
asobhy | 0:a355e511bc5d | 99 | void PiControlThread(void const *argument) |
asobhy | 1:3e9684e81312 | 100 | { |
asobhy | 3:4def4ca68910 | 101 | |
asobhy | 0:a355e511bc5d | 102 | while (true) |
asobhy | 0:a355e511bc5d | 103 | { |
asobhy | 0:a355e511bc5d | 104 | osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received. |
asobhy | 0:a355e511bc5d | 105 | |
asobhy | 14:5777377537a2 | 106 | // Get incremental position and time from QEI |
asobhy | 9:fe56b888985c | 107 | DE0_read(&sensors); |
asobhy | 9:fe56b888985c | 108 | |
asobhy | 15:cf67f83d5409 | 109 | // might not be needed |
asobhy | 15:cf67f83d5409 | 110 | sensors.dp_right = SaturateValue(sensors.dp_right, 560); |
asobhy | 15:cf67f83d5409 | 111 | sensors.dp_left = SaturateValue(sensors.dp_left, 560); |
asobhy | 9:fe56b888985c | 112 | |
asobhy | 14:5777377537a2 | 113 | // Maximum velocity at dPostition = 560 is vel = 703 |
asobhy | 9:fe56b888985c | 114 | velR = (float)((6135.92 * sensors.dp_right) / sensors.dt_right) ; |
asobhy | 1:3e9684e81312 | 115 | |
asobhy | 14:5777377537a2 | 116 | // Maximum velocity at dPostition = 560 is vel = 703 |
asobhy | 9:fe56b888985c | 117 | velL = (float)((6135.92 * sensors.dp_left) / sensors.dt_left) ; |
asobhy | 9:fe56b888985c | 118 | |
asobhy | 10:8919b1b76243 | 119 | /*********************Differential Start*******************************/ |
asobhy | 10:8919b1b76243 | 120 | // Inputs are Speed Setpoint and Steering Setpoint |
asobhy | 10:8919b1b76243 | 121 | // The Inputs for the Steering are specified in the CameraThread |
asobhy | 10:8919b1b76243 | 122 | // and set at the center. |
asobhy | 10:8919b1b76243 | 123 | // The distance between the object and the image should be set to 1 meter |
asobhy | 10:8919b1b76243 | 124 | // If distance decrease then speed should stop. |
asobhy | 10:8919b1b76243 | 125 | // If distance increase then speed should increase. |
asobhy | 10:8919b1b76243 | 126 | |
asobhy | 14:5777377537a2 | 127 | // If object is moving away from the the robot increase robot speed |
asobhy | 17:1184df616383 | 128 | /*cameraData_mutex.lock(); |
asobhy | 14:5777377537a2 | 129 | if(DistanceError > 0) |
asobhy | 10:8919b1b76243 | 130 | { |
asobhy | 14:5777377537a2 | 131 | // Proportional controller |
asobhy | 10:8919b1b76243 | 132 | Setpoint = (Kd*DistanceError); |
asobhy | 15:cf67f83d5409 | 133 | Setpoint = SaturateValue(Setpoint,560); |
asobhy | 10:8919b1b76243 | 134 | } |
asobhy | 14:5777377537a2 | 135 | // If object is at the set distance limit or less then do not move. |
asobhy | 10:8919b1b76243 | 136 | else if(DistanceError <= 0) |
asobhy | 10:8919b1b76243 | 137 | { |
asobhy | 10:8919b1b76243 | 138 | Setpoint = 0; |
asobhy | 10:8919b1b76243 | 139 | } |
asobhy | 10:8919b1b76243 | 140 | |
asobhy | 14:5777377537a2 | 141 | // Decoupled drive |
asobhy | 10:8919b1b76243 | 142 | setpointR = Setpoint + (Ks*SteeringError); |
asobhy | 10:8919b1b76243 | 143 | setpointL = Setpoint - (Ks*SteeringError); |
asobhy | 15:cf67f83d5409 | 144 | cameraData_mutex.unlock(); |
asobhy | 17:1184df616383 | 145 | */ |
asobhy | 14:5777377537a2 | 146 | |
asobhy | 17:1184df616383 | 147 | /********************Manual Setpoint and Steering**********************/ |
asobhy | 17:1184df616383 | 148 | mutexSetpoint.lock(); |
asobhy | 17:1184df616383 | 149 | setpointR = Setpoint + (Ks*SteeringError); |
asobhy | 17:1184df616383 | 150 | setpointL = Setpoint - (Ks*SteeringError); |
asobhy | 17:1184df616383 | 151 | mutexSetpoint.unlock(); |
asobhy | 17:1184df616383 | 152 | |
asobhy | 14:5777377537a2 | 153 | // Make sure our limit is not exceeded |
asobhy | 14:5777377537a2 | 154 | setpointR = SaturateValue(setpointR, 560); |
asobhy | 14:5777377537a2 | 155 | setpointL = SaturateValue(setpointL, 560); |
asobhy | 14:5777377537a2 | 156 | |
asobhy | 14:5777377537a2 | 157 | /*********************Differential End*********************************/ |
asobhy | 14:5777377537a2 | 158 | |
asobhy | 9:fe56b888985c | 159 | U_right = PiControllerR(setpointR,sensors.dp_right); |
asobhy | 9:fe56b888985c | 160 | U_left = PiControllerL(setpointL,sensors.dp_left); |
asobhy | 1:3e9684e81312 | 161 | |
asobhy | 14:5777377537a2 | 162 | // Set speed and direction for right motor |
asobhy | 9:fe56b888985c | 163 | if (U_right >= 0) |
asobhy | 0:a355e511bc5d | 164 | { |
asobhy | 9:fe56b888985c | 165 | motorDriver_R_forward(U_right); |
asobhy | 9:fe56b888985c | 166 | } |
asobhy | 9:fe56b888985c | 167 | else if (U_right < 0) |
asobhy | 9:fe56b888985c | 168 | { |
asobhy | 9:fe56b888985c | 169 | motorDriver_R_reverse(U_right); |
asobhy | 0:a355e511bc5d | 170 | } |
asobhy | 9:fe56b888985c | 171 | |
asobhy | 17:1184df616383 | 172 | // Set speed and direction for left motor |
asobhy | 9:fe56b888985c | 173 | if (U_left >= 0) |
asobhy | 0:a355e511bc5d | 174 | { |
asobhy | 9:fe56b888985c | 175 | motorDriver_L_forward(U_left); |
asobhy | 9:fe56b888985c | 176 | } |
asobhy | 9:fe56b888985c | 177 | else if (U_left < 0) |
asobhy | 9:fe56b888985c | 178 | { |
asobhy | 9:fe56b888985c | 179 | motorDriver_L_reverse(U_left); |
asobhy | 7:73fd05fe556a | 180 | } |
asobhy | 3:4def4ca68910 | 181 | |
asobhy | 0:a355e511bc5d | 182 | } |
asobhy | 1:3e9684e81312 | 183 | |
asobhy | 0:a355e511bc5d | 184 | } |
asobhy | 0:a355e511bc5d | 185 | |
asobhy | 0:a355e511bc5d | 186 | /******************************************************************************* |
asobhy | 7:73fd05fe556a | 187 | * @brief The ISR below signals the PIControllerThread. it is setup to run |
asobhy | 7:73fd05fe556a | 188 | * every 50ms |
asobhy | 7:73fd05fe556a | 189 | * @param none |
asobhy | 7:73fd05fe556a | 190 | * @return none |
asobhy | 0:a355e511bc5d | 191 | *******************************************************************************/ |
asobhy | 0:a355e511bc5d | 192 | void PeriodicInterruptISR(void) |
asobhy | 0:a355e511bc5d | 193 | { |
asobhy | 0:a355e511bc5d | 194 | // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread. |
asobhy | 0:a355e511bc5d | 195 | osSignalSet(PiControlId,0x1); |
asobhy | 0:a355e511bc5d | 196 | } |
asobhy | 1:3e9684e81312 | 197 |