ECE4333 - 2018 - Ahmed & Brandon / Mbed OS ObjectFollower

ObjectFollower

Dependencies:   TPixy-Interface

Fork of PlayBack by ECE4333 - 2018 - Ahmed & Brandon

PiControlThread.cpp@14:5777377537a2, 2018-03-03 (annotated)

Committer:
asobhy
Date:
Sat Mar 03 02:14:40 2018 +0000
Revision:
14:5777377537a2
Parent:
11:9135e5bc2fcf
Child:
15:cf67f83d5409
averaging code added to following robot but results are not that good

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