ECE4333 - 2018 - Ahmed & Brandon / Mbed OS ObjectFollower

ObjectFollower

Dependencies:   TPixy-Interface

Fork of PlayBack by ECE4333 - 2018 - Ahmed & Brandon

PiControlThread.cpp@15:cf67f83d5409, 2018-03-03 (annotated)

Committer:
asobhy
Date:
Sat Mar 03 04:47:26 2018 +0000
Revision:
15:cf67f83d5409
Parent:
14:5777377537a2
Child:
16:58ec2b891a25
following robot almost done. Need to get a better object to follow. Need to use a torch to light up the object. Need to adjust width of object in camera settings to match new object then test.

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 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 15:cf67f83d5409 43 int Ks = 0.5;
asobhy 10:8919b1b76243 44
asobhy 10:8919b1b76243 45 // distance gain
asobhy 15:cf67f83d5409 46 int Kd = 4;
asobhy 10:8919b1b76243 47
asobhy 10:8919b1b76243 48 // overall robot required speed
asobhy 10:8919b1b76243 49 int Setpoint;
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 15:cf67f83d5409 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 14:5777377537a2 145
asobhy 14:5777377537a2 146 // Make sure our limit is not exceeded
asobhy 14:5777377537a2 147 setpointR = SaturateValue(setpointR, 560);
asobhy 14:5777377537a2 148 setpointL = SaturateValue(setpointL, 560);
asobhy 14:5777377537a2 149
asobhy 14:5777377537a2 150 /*********************Differential End*********************************/
asobhy 14:5777377537a2 151
asobhy 9:fe56b888985c 152 U_right = PiControllerR(setpointR,sensors.dp_right);
asobhy 9:fe56b888985c 153 U_left = PiControllerL(setpointL,sensors.dp_left);
asobhy 1:3e9684e81312 154
asobhy 14:5777377537a2 155 // Set speed and direction for right motor
asobhy 9:fe56b888985c 156 if (U_right >= 0)
asobhy 0:a355e511bc5d 157 {
asobhy 9:fe56b888985c 158 motorDriver_R_forward(U_right);
asobhy 9:fe56b888985c 159 }
asobhy 9:fe56b888985c 160 else if (U_right < 0)
asobhy 9:fe56b888985c 161 {
asobhy 9:fe56b888985c 162 motorDriver_R_reverse(U_right);
asobhy 0:a355e511bc5d 163 }
asobhy 9:fe56b888985c 164
asobhy 14:5777377537a2 165 // Set speed and direction of left motor
asobhy 9:fe56b888985c 166 if (U_left >= 0)
asobhy 0:a355e511bc5d 167 {
asobhy 9:fe56b888985c 168 motorDriver_L_forward(U_left);
asobhy 9:fe56b888985c 169 }
asobhy 9:fe56b888985c 170 else if (U_left < 0)
asobhy 9:fe56b888985c 171 {
asobhy 9:fe56b888985c 172 motorDriver_L_reverse(U_left);
asobhy 7:73fd05fe556a 173 }
asobhy 3:4def4ca68910 174
asobhy 0:a355e511bc5d 175 }
asobhy 1:3e9684e81312 176
asobhy 0:a355e511bc5d 177 }
asobhy 0:a355e511bc5d 178
asobhy 0:a355e511bc5d 179 /*******************************************************************************
asobhy 7:73fd05fe556a 180 * @brief The ISR below signals the PIControllerThread. it is setup to run
asobhy 7:73fd05fe556a 181 * every 50ms
asobhy 7:73fd05fe556a 182 * @param none
asobhy 7:73fd05fe556a 183 * @return none
asobhy 0:a355e511bc5d 184 *******************************************************************************/
asobhy 0:a355e511bc5d 185 void PeriodicInterruptISR(void)
asobhy 0:a355e511bc5d 186 {
asobhy 0:a355e511bc5d 187 // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread.
asobhy 0:a355e511bc5d 188 osSignalSet(PiControlId,0x1);
asobhy 0:a355e511bc5d 189 }
asobhy 1:3e9684e81312 190