ECE4333 - 2018 - Ahmed & Brandon / Mbed OS ManualControlFinal

ManualControl

Dependencies:   TPixy-Interface

Fork of MbedOS_Robot_Team by ECE4333 - 2018 - Ahmed & Brandon

PiControlThread.cpp@6:e7ce340fe91e, 2018-02-10 (annotated)

Committer:
asobhy
Date:
Sat Feb 10 19:35:21 2018 +0000
Revision:
6:e7ce340fe91e
Parent:
4:417e475239c7
Child:
7:73fd05fe556a
02/10/2018

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 4:417e475239c7 6 #include "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 0:a355e511bc5d 37
asobhy 0:a355e511bc5d 38 void PiControlThreadInit()
asobhy 0:a355e511bc5d 39 {
asobhy 4:417e475239c7 40 DE0_init(); // initialize FPGA
asobhy 4:417e475239c7 41 motorDriver_init(); // initialize motorDriver
asobhy 6:e7ce340fe91e 42 // Kp,Ki
asobhy 6:e7ce340fe91e 43 PiController_init(1,0.4); // initialize the PI Controller gains and initialize variables
asobhy 0:a355e511bc5d 44
asobhy 0:a355e511bc5d 45 PiControlId = osThreadCreate(osThread(PiControlThread), NULL);
asobhy 0:a355e511bc5d 46
asobhy 0:a355e511bc5d 47 // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval
asobhy 0:a355e511bc5d 48 // in seconds between interrupts, and start interrupt generation:
asobhy 1:3e9684e81312 49 PeriodicInt.attach(&PeriodicInterruptISR, 0.05); // 50ms sampling rate
asobhy 4:417e475239c7 50
asobhy 0:a355e511bc5d 51 }
asobhy 0:a355e511bc5d 52
asobhy 0:a355e511bc5d 53
asobhy 0:a355e511bc5d 54 /*******************************************************************************
asobhy 0:a355e511bc5d 55 * ******** Periodic Timer Interrupt Thread ********
asobhy 0:a355e511bc5d 56 *******************************************************************************/
asobhy 0:a355e511bc5d 57 void PiControlThread(void const *argument)
asobhy 1:3e9684e81312 58 {
asobhy 3:4def4ca68910 59
asobhy 0:a355e511bc5d 60 while (true)
asobhy 0:a355e511bc5d 61 {
asobhy 0:a355e511bc5d 62 osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received.
asobhy 0:a355e511bc5d 63
asobhy 0:a355e511bc5d 64 // get incremental position and time from QEI
asobhy 0:a355e511bc5d 65 DE0_read(&ID, &dPosition, &dTime);
asobhy 1:3e9684e81312 66 SaturateValue(dPosition, 560);
asobhy 1:3e9684e81312 67
asobhy 1:3e9684e81312 68 // maximum velocity at dPostition = 560 is vel = 703
asobhy 1:3e9684e81312 69 vel = (float)((6135.92 * dPosition) / dTime) ;
asobhy 0:a355e511bc5d 70
asobhy 4:417e475239c7 71 U = PiController(setpoint,dPosition);
asobhy 1:3e9684e81312 72
asobhy 4:417e475239c7 73 if (U >= 0)
asobhy 0:a355e511bc5d 74 {
asobhy 4:417e475239c7 75 motorDriver_forward(U);
asobhy 0:a355e511bc5d 76 }
asobhy 4:417e475239c7 77 else if (U < 0)
asobhy 0:a355e511bc5d 78 {
asobhy 4:417e475239c7 79 motorDriver_reverse(U);
asobhy 1:3e9684e81312 80 }
asobhy 3:4def4ca68910 81
asobhy 0:a355e511bc5d 82 }
asobhy 1:3e9684e81312 83
asobhy 0:a355e511bc5d 84 }
asobhy 0:a355e511bc5d 85
asobhy 0:a355e511bc5d 86 /*******************************************************************************
asobhy 0:a355e511bc5d 87 * the interrupt below occures every 250ms as setup in the main function during
asobhy 0:a355e511bc5d 88 * initialization
asobhy 0:a355e511bc5d 89 * ******** Period Timer Interrupt Handler ********
asobhy 0:a355e511bc5d 90 *******************************************************************************/
asobhy 0:a355e511bc5d 91 void PeriodicInterruptISR(void)
asobhy 0:a355e511bc5d 92 {
asobhy 0:a355e511bc5d 93 // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread.
asobhy 0:a355e511bc5d 94 osSignalSet(PiControlId,0x1);
asobhy 0:a355e511bc5d 95 }
asobhy 1:3e9684e81312 96