ECE4333 - 2018 - Ahmed & Brandon / Mbed OS ManualControlFinal

ManualControl

Dependencies:   TPixy-Interface

Fork of MbedOS_Robot_Team by ECE4333 - 2018 - Ahmed & Brandon

PiControlThread.cpp@4:417e475239c7, 2018-02-09 (annotated)

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