ECE4333 - 2018 - Ahmed & Brandon / Mbed OS MbedOS_Robot_Team

ManualControl

Dependencies:   TPixy-Interface

Fork of MbedOS_Robot by ECE4333 - 2018 - Ahmed & Brandon

PiControlThread.cpp@21:7fee709bb063, 2018-03-23 (annotated)

Committer:
asobhy
Date:
Fri Mar 23 22:32:38 2018 +0000
Revision:
21:7fee709bb063
Parent:
20:9118203f7c9c
Child:
22:c09acff62e6a
Manual Control - Latest Update - some code clean up

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 0:a355e511bc5d 37 void PiControlThread(void const *);
asobhy 0:a355e511bc5d 38 void PeriodicInterruptISR(void);
asobhy 0:a355e511bc5d 39
asobhy 10:8919b1b76243 40 // steering gain
asobhy 16:58ec2b891a25 41 float Ks = 0.15;
asobhy 10:8919b1b76243 42 // distance gain
asobhy 16:58ec2b891a25 43 float Kd = 10;
asobhy 10:8919b1b76243 44
asobhy 10:8919b1b76243 45 // overall robot required speed
asobhy 10:8919b1b76243 46 int Setpoint;
asobhy 17:1184df616383 47 Mutex mutexSetpoint;
asobhy 10:8919b1b76243 48
asobhy 0:a355e511bc5d 49 osThreadId PiControlId;
asobhy 0:a355e511bc5d 50
asobhy 0:a355e511bc5d 51 /******************************************************************************/
asobhy 0:a355e511bc5d 52 // osPriorityIdle = -3, ///< priority: idle (lowest)
asobhy 0:a355e511bc5d 53 // osPriorityLow = -2, ///< priority: low
asobhy 0:a355e511bc5d 54 // osPriorityBelowNormal = -1, ///< priority: below normal
asobhy 0:a355e511bc5d 55 // osPriorityNormal = 0, ///< priority: normal (default)
asobhy 0:a355e511bc5d 56 // osPriorityAboveNormal = +1, ///< priority: above normal
asobhy 0:a355e511bc5d 57 // osPriorityHigh = +2, ///< priority: high
asobhy 0:a355e511bc5d 58 // osPriorityRealtime = +3, ///< priority: realtime (highest)
asobhy 0:a355e511bc5d 59 /******************************************************************************/
asobhy 0:a355e511bc5d 60
asobhy 0:a355e511bc5d 61 // Declare PeriodicInterruptThread as a thread/process
asobhy 0:a355e511bc5d 62 osThreadDef(PiControlThread, osPriorityRealtime, 1024);
asobhy 0:a355e511bc5d 63
asobhy 0:a355e511bc5d 64 Ticker PeriodicInt; // Declare a timer interrupt: PeriodicInt
asobhy 0:a355e511bc5d 65
asobhy 7:73fd05fe556a 66 /*******************************************************************************
asobhy 7:73fd05fe556a 67 * @brief function that creates a thread for the PI controller. It initializes
asobhy 7:73fd05fe556a 68 * the PI controller's gains and initializes the DC Motor. It also
asobhy 7:73fd05fe556a 69 * initializes the PIControllerThread runs at 50ms period
asobhy 7:73fd05fe556a 70 * @param none
asobhy 7:73fd05fe556a 71 * @return none
asobhy 7:73fd05fe556a 72 *******************************************************************************/
asobhy 0:a355e511bc5d 73 void PiControlThreadInit()
asobhy 0:a355e511bc5d 74 {
asobhy 4:417e475239c7 75 DE0_init(); // initialize FPGA
asobhy 21:7fee709bb063 76 motorDriver_R_init(); // initialize motorDriver
asobhy 21:7fee709bb063 77 motorDriver_L_init(); // initialize motorDriver
asobhy 6:e7ce340fe91e 78 // Kp,Ki
asobhy 6:e7ce340fe91e 79 PiController_init(1,0.4); // initialize the PI Controller gains and initialize variables
asobhy 0:a355e511bc5d 80
asobhy 0:a355e511bc5d 81 PiControlId = osThreadCreate(osThread(PiControlThread), NULL);
asobhy 0:a355e511bc5d 82
asobhy 0:a355e511bc5d 83 // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval
asobhy 0:a355e511bc5d 84 // in seconds between interrupts, and start interrupt generation:
asobhy 21:7fee709bb063 85 PeriodicInt.attach(&PeriodicInterruptISR, 0.010); // 10ms sampling period -> 100Hz freq
asobhy 4:417e475239c7 86
asobhy 0:a355e511bc5d 87 }
asobhy 0:a355e511bc5d 88
asobhy 0:a355e511bc5d 89 /*******************************************************************************
asobhy 7:73fd05fe556a 90 * @brief This is the PI controller thread. It reads several values from the
asobhy 7:73fd05fe556a 91 * FPGA such as speed, time and other sensors data
asobhy 7:73fd05fe556a 92 * @param none
asobhy 7:73fd05fe556a 93 * @return none
asobhy 0:a355e511bc5d 94 *******************************************************************************/
asobhy 0:a355e511bc5d 95 void PiControlThread(void const *argument)
asobhy 1:3e9684e81312 96 {
asobhy 3:4def4ca68910 97
asobhy 0:a355e511bc5d 98 while (true)
asobhy 0:a355e511bc5d 99 {
asobhy 0:a355e511bc5d 100 osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received.
asobhy 0:a355e511bc5d 101
asobhy 14:5777377537a2 102 // Get incremental position and time from QEI
asobhy 9:fe56b888985c 103 DE0_read(&sensors);
asobhy 9:fe56b888985c 104
asobhy 15:cf67f83d5409 105 // might not be needed
asobhy 20:9118203f7c9c 106 sensors.dp_right = SaturateValue(sensors.dp_right, 112);
asobhy 20:9118203f7c9c 107 sensors.dp_left = SaturateValue(sensors.dp_left, 112);
asobhy 9:fe56b888985c 108
asobhy 21:7fee709bb063 109 // Maximum angular velocity @ dPostition = 112 is vel = 703 rad/sec
asobhy 9:fe56b888985c 110 velR = (float)((6135.92 * sensors.dp_right) / sensors.dt_right) ;
asobhy 1:3e9684e81312 111
asobhy 21:7fee709bb063 112 // Maximum angular velocity @ dPostition = 112 is vel = 703 rad/sec
asobhy 9:fe56b888985c 113 velL = (float)((6135.92 * sensors.dp_left) / sensors.dt_left) ;
asobhy 9:fe56b888985c 114
asobhy 17:1184df616383 115 /********************Manual Setpoint and Steering**********************/
asobhy 17:1184df616383 116 mutexSetpoint.lock();
asobhy 17:1184df616383 117 setpointR = Setpoint + (Ks*SteeringError);
asobhy 17:1184df616383 118 setpointL = Setpoint - (Ks*SteeringError);
asobhy 17:1184df616383 119 mutexSetpoint.unlock();
asobhy 17:1184df616383 120
asobhy 14:5777377537a2 121 // Make sure our limit is not exceeded
asobhy 20:9118203f7c9c 122 setpointR = SaturateValue(setpointR, 112);
asobhy 20:9118203f7c9c 123 setpointL = SaturateValue(setpointL, 112);
asobhy 14:5777377537a2 124
asobhy 14:5777377537a2 125 /*********************Differential End*********************************/
asobhy 14:5777377537a2 126
asobhy 9:fe56b888985c 127 U_right = PiControllerR(setpointR,sensors.dp_right);
asobhy 9:fe56b888985c 128 U_left = PiControllerL(setpointL,sensors.dp_left);
asobhy 1:3e9684e81312 129
asobhy 14:5777377537a2 130 // Set speed and direction for right motor
asobhy 9:fe56b888985c 131 if (U_right >= 0)
asobhy 0:a355e511bc5d 132 {
asobhy 9:fe56b888985c 133 motorDriver_R_forward(U_right);
asobhy 9:fe56b888985c 134 }
asobhy 9:fe56b888985c 135 else if (U_right < 0)
asobhy 9:fe56b888985c 136 {
asobhy 9:fe56b888985c 137 motorDriver_R_reverse(U_right);
asobhy 0:a355e511bc5d 138 }
asobhy 9:fe56b888985c 139
asobhy 17:1184df616383 140 // Set speed and direction for left motor
asobhy 9:fe56b888985c 141 if (U_left >= 0)
asobhy 0:a355e511bc5d 142 {
asobhy 9:fe56b888985c 143 motorDriver_L_forward(U_left);
asobhy 9:fe56b888985c 144 }
asobhy 9:fe56b888985c 145 else if (U_left < 0)
asobhy 9:fe56b888985c 146 {
asobhy 9:fe56b888985c 147 motorDriver_L_reverse(U_left);
asobhy 7:73fd05fe556a 148 }
asobhy 3:4def4ca68910 149
asobhy 0:a355e511bc5d 150 }
asobhy 1:3e9684e81312 151
asobhy 0:a355e511bc5d 152 }
asobhy 0:a355e511bc5d 153
asobhy 0:a355e511bc5d 154 /*******************************************************************************
asobhy 7:73fd05fe556a 155 * @brief The ISR below signals the PIControllerThread. it is setup to run
asobhy 7:73fd05fe556a 156 * every 50ms
asobhy 7:73fd05fe556a 157 * @param none
asobhy 7:73fd05fe556a 158 * @return none
asobhy 0:a355e511bc5d 159 *******************************************************************************/
asobhy 0:a355e511bc5d 160 void PeriodicInterruptISR(void)
asobhy 0:a355e511bc5d 161 {
asobhy 0:a355e511bc5d 162 // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread.
asobhy 0:a355e511bc5d 163 osSignalSet(PiControlId,0x1);
asobhy 0:a355e511bc5d 164 }
asobhy 1:3e9684e81312 165