ECE4333 - 2018 - Ahmed & Brandon / Mbed OS ManualControlFinal

ManualControl

Dependencies:   TPixy-Interface

Fork of MbedOS_Robot_Team by ECE4333 - 2018 - Ahmed & Brandon

PiControlThread.cpp@7:73fd05fe556a, 2018-02-15 (annotated)

Committer:
asobhy
Date:
Thu Feb 15 01:15:26 2018 +0000
Revision:
7:73fd05fe556a
Parent:
6:e7ce340fe91e
Child:
8:a0890fa79084
Added comments to several files and organized the code a bit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
asobhy 0:a355e511bc5d 1 #include "mbed.h"
asobhy 0:a355e511bc5d 2 #include "ui.h"
asobhy 0:a355e511bc5d 3 #include "Drivers/motor_driver.h"
asobhy 0:a355e511bc5d 4 #include "Drivers/DE0_driver.h"
asobhy 1:3e9684e81312 5 #include "PiControlThread.h"
asobhy 7:73fd05fe556a 6 #include "Drivers/PiController.h"
asobhy 0:a355e511bc5d 7
asobhy 0:a355e511bc5d 8 extern int setpoint;
asobhy 0:a355e511bc5d 9
asobhy 1:3e9684e81312 10 uint16_t ID, dTime;
asobhy 1:3e9684e81312 11 int dPosition;
asobhy 1:3e9684e81312 12 int vel;
asobhy 4:417e475239c7 13 int32_t U;
asobhy 0:a355e511bc5d 14
asobhy 0:a355e511bc5d 15 void PiControlThread(void const *);
asobhy 0:a355e511bc5d 16 void PeriodicInterruptISR(void);
asobhy 0:a355e511bc5d 17
asobhy 0:a355e511bc5d 18 osThreadId PiControlId;
asobhy 0:a355e511bc5d 19
asobhy 0:a355e511bc5d 20 /******************************************************************************/
asobhy 0:a355e511bc5d 21 // osPriorityIdle = -3, ///< priority: idle (lowest)
asobhy 0:a355e511bc5d 22 // osPriorityLow = -2, ///< priority: low
asobhy 0:a355e511bc5d 23 // osPriorityBelowNormal = -1, ///< priority: below normal
asobhy 0:a355e511bc5d 24 // osPriorityNormal = 0, ///< priority: normal (default)
asobhy 0:a355e511bc5d 25 // osPriorityAboveNormal = +1, ///< priority: above normal
asobhy 0:a355e511bc5d 26 // osPriorityHigh = +2, ///< priority: high
asobhy 0:a355e511bc5d 27 // osPriorityRealtime = +3, ///< priority: realtime (highest)
asobhy 0:a355e511bc5d 28 /******************************************************************************/
asobhy 0:a355e511bc5d 29
asobhy 0:a355e511bc5d 30 // Declare PeriodicInterruptThread as a thread/process
asobhy 0:a355e511bc5d 31 osThreadDef(PiControlThread, osPriorityRealtime, 1024);
asobhy 0:a355e511bc5d 32
asobhy 0:a355e511bc5d 33 Ticker PeriodicInt; // Declare a timer interrupt: PeriodicInt
asobhy 0:a355e511bc5d 34
asobhy 0:a355e511bc5d 35 DigitalOut led3(LED3);
asobhy 0:a355e511bc5d 36
asobhy 7:73fd05fe556a 37 /*******************************************************************************
asobhy 7:73fd05fe556a 38 * @brief function that creates a thread for the PI controller. It initializes
asobhy 7:73fd05fe556a 39 * the PI controller's gains and initializes the DC Motor. It also
asobhy 7:73fd05fe556a 40 * initializes the PIControllerThread runs at 50ms period
asobhy 7:73fd05fe556a 41 * @param none
asobhy 7:73fd05fe556a 42 * @return none
asobhy 7:73fd05fe556a 43 *******************************************************************************/
asobhy 0:a355e511bc5d 44 void PiControlThreadInit()
asobhy 0:a355e511bc5d 45 {
asobhy 4:417e475239c7 46 DE0_init(); // initialize FPGA
asobhy 4:417e475239c7 47 motorDriver_init(); // initialize motorDriver
asobhy 6:e7ce340fe91e 48 // Kp,Ki
asobhy 6:e7ce340fe91e 49 PiController_init(1,0.4); // initialize the PI Controller gains and initialize variables
asobhy 0:a355e511bc5d 50
asobhy 0:a355e511bc5d 51 PiControlId = osThreadCreate(osThread(PiControlThread), NULL);
asobhy 0:a355e511bc5d 52
asobhy 0:a355e511bc5d 53 // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval
asobhy 0:a355e511bc5d 54 // in seconds between interrupts, and start interrupt generation:
asobhy 1:3e9684e81312 55 PeriodicInt.attach(&PeriodicInterruptISR, 0.05); // 50ms sampling rate
asobhy 4:417e475239c7 56
asobhy 0:a355e511bc5d 57 }
asobhy 0:a355e511bc5d 58
asobhy 0:a355e511bc5d 59
asobhy 0:a355e511bc5d 60 /*******************************************************************************
asobhy 7:73fd05fe556a 61 * @brief This is the PI controller thread. It reads several values from the
asobhy 7:73fd05fe556a 62 * FPGA such as speed, time and other sensors data
asobhy 7:73fd05fe556a 63 * @param none
asobhy 7:73fd05fe556a 64 * @return none
asobhy 0:a355e511bc5d 65 *******************************************************************************/
asobhy 0:a355e511bc5d 66 void PiControlThread(void const *argument)
asobhy 1:3e9684e81312 67 {
asobhy 3:4def4ca68910 68
asobhy 0:a355e511bc5d 69 while (true)
asobhy 0:a355e511bc5d 70 {
asobhy 0:a355e511bc5d 71 osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received.
asobhy 0:a355e511bc5d 72
asobhy 0:a355e511bc5d 73 // get incremental position and time from QEI
asobhy 0:a355e511bc5d 74 DE0_read(&ID, &dPosition, &dTime);
asobhy 1:3e9684e81312 75 SaturateValue(dPosition, 560);
asobhy 1:3e9684e81312 76
asobhy 1:3e9684e81312 77 // maximum velocity at dPostition = 560 is vel = 703
asobhy 1:3e9684e81312 78 vel = (float)((6135.92 * dPosition) / dTime) ;
asobhy 0:a355e511bc5d 79
asobhy 4:417e475239c7 80 U = PiController(setpoint,dPosition);
asobhy 1:3e9684e81312 81
asobhy 4:417e475239c7 82 if (U >= 0)
asobhy 0:a355e511bc5d 83 {
asobhy 4:417e475239c7 84 motorDriver_forward(U);
asobhy 0:a355e511bc5d 85 }
asobhy 4:417e475239c7 86 else if (U < 0)
asobhy 0:a355e511bc5d 87 {
asobhy 4:417e475239c7 88 motorDriver_reverse(U);
asobhy 7:73fd05fe556a 89 }
asobhy 3:4def4ca68910 90
asobhy 0:a355e511bc5d 91 }
asobhy 1:3e9684e81312 92
asobhy 0:a355e511bc5d 93 }
asobhy 0:a355e511bc5d 94
asobhy 0:a355e511bc5d 95 /*******************************************************************************
asobhy 7:73fd05fe556a 96 * @brief The ISR below signals the PIControllerThread. it is setup to run
asobhy 7:73fd05fe556a 97 * every 50ms
asobhy 7:73fd05fe556a 98 * @param none
asobhy 7:73fd05fe556a 99 * @return none
asobhy 0:a355e511bc5d 100 *******************************************************************************/
asobhy 0:a355e511bc5d 101 void PeriodicInterruptISR(void)
asobhy 0:a355e511bc5d 102 {
asobhy 0:a355e511bc5d 103 // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread.
asobhy 0:a355e511bc5d 104 osSignalSet(PiControlId,0x1);
asobhy 0:a355e511bc5d 105 }
asobhy 1:3e9684e81312 106