ECE4333 - 2018 - Ahmed & Brandon / Mbed OS ManualControlFinal

ManualControl

Dependencies:   TPixy-Interface

Fork of MbedOS_Robot_Team by ECE4333 - 2018 - Ahmed & Brandon

PiControlThread.cpp@5:b29220d29022, 2018-02-09 (annotated)

Committer:
asobhy
Date:
Fri Feb 09 20:26:04 2018 +0000
Revision:
5:b29220d29022
Parent:
4:417e475239c7 
time constant calculation

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 5:b29220d29022 9 extern bool start_counting;
asobhy 5:b29220d29022 10 extern Serial bluetooth;
asobhy 0:a355e511bc5d 11
asobhy 1:3e9684e81312 12 uint16_t ID, dTime;
asobhy 1:3e9684e81312 13 int dPosition;
asobhy 1:3e9684e81312 14 int vel;
asobhy 4:417e475239c7 15 int32_t U;
asobhy 5:b29220d29022 16 int counter =0 ;
asobhy 5:b29220d29022 17
asobhy 5:b29220d29022 18 int dPosition_values[1000];
asobhy 0:a355e511bc5d 19
asobhy 0:a355e511bc5d 20 void PiControlThread(void const *);
asobhy 0:a355e511bc5d 21 void PeriodicInterruptISR(void);
asobhy 0:a355e511bc5d 22
asobhy 0:a355e511bc5d 23 osThreadId PiControlId;
asobhy 0:a355e511bc5d 24
asobhy 0:a355e511bc5d 25 /******************************************************************************/
asobhy 0:a355e511bc5d 26 // osPriorityIdle = -3, ///< priority: idle (lowest)
asobhy 0:a355e511bc5d 27 // osPriorityLow = -2, ///< priority: low
asobhy 0:a355e511bc5d 28 // osPriorityBelowNormal = -1, ///< priority: below normal
asobhy 0:a355e511bc5d 29 // osPriorityNormal = 0, ///< priority: normal (default)
asobhy 0:a355e511bc5d 30 // osPriorityAboveNormal = +1, ///< priority: above normal
asobhy 0:a355e511bc5d 31 // osPriorityHigh = +2, ///< priority: high
asobhy 0:a355e511bc5d 32 // osPriorityRealtime = +3, ///< priority: realtime (highest)
asobhy 0:a355e511bc5d 33 /******************************************************************************/
asobhy 0:a355e511bc5d 34
asobhy 0:a355e511bc5d 35 // Declare PeriodicInterruptThread as a thread/process
asobhy 5:b29220d29022 36 osThreadDef(PiControlThread, osPriorityRealtime, 1024);
asobhy 0:a355e511bc5d 37
asobhy 0:a355e511bc5d 38 Ticker PeriodicInt; // Declare a timer interrupt: PeriodicInt
asobhy 0:a355e511bc5d 39
asobhy 0:a355e511bc5d 40 DigitalOut led3(LED3);
asobhy 0:a355e511bc5d 41
asobhy 0:a355e511bc5d 42
asobhy 0:a355e511bc5d 43 void PiControlThreadInit()
asobhy 0:a355e511bc5d 44 {
asobhy 4:417e475239c7 45 DE0_init(); // initialize FPGA
asobhy 4:417e475239c7 46 motorDriver_init(); // initialize motorDriver
asobhy 4:417e475239c7 47 PiController_init(2.2,0.01); // initialize the PI Controller gains and initialize variables
asobhy 5:b29220d29022 48
asobhy 0:a355e511bc5d 49 PiControlId = osThreadCreate(osThread(PiControlThread), NULL);
asobhy 0:a355e511bc5d 50
asobhy 0:a355e511bc5d 51 // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval
asobhy 0:a355e511bc5d 52 // in seconds between interrupts, and start interrupt generation:
asobhy 5:b29220d29022 53 PeriodicInt.attach(&PeriodicInterruptISR, 0.001); // 50ms sampling rate
asobhy 5:b29220d29022 54
asobhy 0:a355e511bc5d 55 }
asobhy 0:a355e511bc5d 56
asobhy 0:a355e511bc5d 57
asobhy 0:a355e511bc5d 58 /*******************************************************************************
asobhy 0:a355e511bc5d 59 * ******** Periodic Timer Interrupt Thread ********
asobhy 0:a355e511bc5d 60 *******************************************************************************/
asobhy 0:a355e511bc5d 61 void PiControlThread(void const *argument)
asobhy 1:3e9684e81312 62 {
asobhy 5:b29220d29022 63
asobhy 5:b29220d29022 64 // time constant calc
asobhy 5:b29220d29022 65 setpoint = 140;
asobhy 5:b29220d29022 66
asobhy 5:b29220d29022 67 while (true) {
asobhy 0:a355e511bc5d 68 osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received.
asobhy 5:b29220d29022 69
asobhy 0:a355e511bc5d 70 // get incremental position and time from QEI
asobhy 0:a355e511bc5d 71 DE0_read(&ID, &dPosition, &dTime);
asobhy 1:3e9684e81312 72 SaturateValue(dPosition, 560);
asobhy 5:b29220d29022 73
asobhy 1:3e9684e81312 74 // maximum velocity at dPostition = 560 is vel = 703
asobhy 1:3e9684e81312 75 vel = (float)((6135.92 * dPosition) / dTime) ;
asobhy 5:b29220d29022 76
asobhy 5:b29220d29022 77 //U = PiController(setpoint,dPosition);
asobhy 5:b29220d29022 78
asobhy 5:b29220d29022 79 // time constnant calc.
asobhy 5:b29220d29022 80 U = setpoint/28;
asobhy 5:b29220d29022 81
asobhy 5:b29220d29022 82 if (U >= 0) {
asobhy 4:417e475239c7 83 motorDriver_forward(U);
asobhy 5:b29220d29022 84 } else if (U < 0) {
asobhy 5:b29220d29022 85 motorDriver_reverse(U);
asobhy 0:a355e511bc5d 86 }
asobhy 5:b29220d29022 87
asobhy 5:b29220d29022 88
asobhy 5:b29220d29022 89 // start our calculations
asobhy 5:b29220d29022 90 if( start_counting == true ) {
asobhy 5:b29220d29022 91 float time_constant = 0;
asobhy 5:b29220d29022 92 dPosition_values[counter] = dPosition;
asobhy 5:b29220d29022 93 counter++;
asobhy 5:b29220d29022 94
asobhy 5:b29220d29022 95 // if we reach our setpoint
asobhy 5:b29220d29022 96 if (dPosition > 8 ) {
asobhy 5:b29220d29022 97 // stop counting time
asobhy 5:b29220d29022 98 start_counting = false;
asobhy 5:b29220d29022 99 // find the value where dPosition = 316
asobhy 5:b29220d29022 100 for(int i=0; i<1000; i++) {
asobhy 5:b29220d29022 101 if( dPosition_values[i] >= 7 ) {
asobhy 5:b29220d29022 102 time_constant = counter;
asobhy 5:b29220d29022 103 //time_constant = time_constant;
asobhy 5:b29220d29022 104 bluetooth.printf("\ntime constant: %f", time_constant);
asobhy 5:b29220d29022 105 }
asobhy 5:b29220d29022 106 }
asobhy 5:b29220d29022 107 }
asobhy 5:b29220d29022 108 }
asobhy 5:b29220d29022 109
asobhy 5:b29220d29022 110
asobhy 5:b29220d29022 111
asobhy 5:b29220d29022 112
asobhy 5:b29220d29022 113
asobhy 0:a355e511bc5d 114 }
asobhy 5:b29220d29022 115
asobhy 0:a355e511bc5d 116 }
asobhy 0:a355e511bc5d 117
asobhy 0:a355e511bc5d 118 /*******************************************************************************
asobhy 5:b29220d29022 119 * the interrupt below occures every 250ms as setup in the main function during
asobhy 0:a355e511bc5d 120 * initialization
asobhy 0:a355e511bc5d 121 * ******** Period Timer Interrupt Handler ********
asobhy 0:a355e511bc5d 122 *******************************************************************************/
asobhy 0:a355e511bc5d 123 void PeriodicInterruptISR(void)
asobhy 0:a355e511bc5d 124 {
asobhy 0:a355e511bc5d 125 // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread.
asobhy 5:b29220d29022 126 osSignalSet(PiControlId,0x1);
asobhy 0:a355e511bc5d 127 }
asobhy 1:3e9684e81312 128