ECE4333 - 2018 - Ahmed & Brandon / Mbed OS ManualControlFinal

ManualControl

Dependencies:   TPixy-Interface

Fork of MbedOS_Robot_Team by ECE4333 - 2018 - Ahmed & Brandon

PiControlThread.cpp@3:4def4ca68910, 2018-02-03 (annotated)

Committer:
asobhy
Date:
Sat Feb 03 00:48:47 2018 +0000
Revision:
3:4def4ca68910
Parent:
2:ca2a7430739b
Child:
4:417e475239c7
using the control algorithm in the notes

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 0:a355e511bc5d 6
asobhy 0:a355e511bc5d 7 // global speed variable;
asobhy 0:a355e511bc5d 8 extern int setpoint;
asobhy 0:a355e511bc5d 9 extern Serial pc;
asobhy 0:a355e511bc5d 10 extern Mutex setpoint_mutex;
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 0:a355e511bc5d 15
asobhy 1:3e9684e81312 16 int32_t e, u, xState;
asobhy 1:3e9684e81312 17
asobhy 1:3e9684e81312 18 bool saturationFlag;
asobhy 1:3e9684e81312 19
asobhy 1:3e9684e81312 20 float Ki;
asobhy 1:3e9684e81312 21 float Kp;
asobhy 0:a355e511bc5d 22
asobhy 0:a355e511bc5d 23 void PiControlThread(void const *);
asobhy 0:a355e511bc5d 24 void PeriodicInterruptISR(void);
asobhy 1:3e9684e81312 25 int SaturatingSubtract(int , int );
asobhy 1:3e9684e81312 26 int SaturatingAdd(int , int );
asobhy 1:3e9684e81312 27 int SaturateValue(int , int );
asobhy 1:3e9684e81312 28
asobhy 0:a355e511bc5d 29
asobhy 0:a355e511bc5d 30 osThreadId PiControlId;
asobhy 0:a355e511bc5d 31
asobhy 0:a355e511bc5d 32 /******************************************************************************/
asobhy 0:a355e511bc5d 33 // osPriorityIdle = -3, ///< priority: idle (lowest)
asobhy 0:a355e511bc5d 34 // osPriorityLow = -2, ///< priority: low
asobhy 0:a355e511bc5d 35 // osPriorityBelowNormal = -1, ///< priority: below normal
asobhy 0:a355e511bc5d 36 // osPriorityNormal = 0, ///< priority: normal (default)
asobhy 0:a355e511bc5d 37 // osPriorityAboveNormal = +1, ///< priority: above normal
asobhy 0:a355e511bc5d 38 // osPriorityHigh = +2, ///< priority: high
asobhy 0:a355e511bc5d 39 // osPriorityRealtime = +3, ///< priority: realtime (highest)
asobhy 0:a355e511bc5d 40 /******************************************************************************/
asobhy 0:a355e511bc5d 41
asobhy 0:a355e511bc5d 42 // Declare PeriodicInterruptThread as a thread/process
asobhy 0:a355e511bc5d 43 osThreadDef(PiControlThread, osPriorityRealtime, 1024);
asobhy 0:a355e511bc5d 44
asobhy 0:a355e511bc5d 45 Ticker PeriodicInt; // Declare a timer interrupt: PeriodicInt
asobhy 0:a355e511bc5d 46
asobhy 0:a355e511bc5d 47 DigitalOut led3(LED3);
asobhy 0:a355e511bc5d 48
asobhy 0:a355e511bc5d 49
asobhy 0:a355e511bc5d 50 void PiControlThreadInit()
asobhy 0:a355e511bc5d 51 {
asobhy 0:a355e511bc5d 52 DE0_init(); // initialize FPGA
asobhy 0:a355e511bc5d 53 motorDriver_init(); // initialize motorDriver
asobhy 0:a355e511bc5d 54
asobhy 0:a355e511bc5d 55 PiControlId = osThreadCreate(osThread(PiControlThread), NULL);
asobhy 0:a355e511bc5d 56
asobhy 0:a355e511bc5d 57 // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval
asobhy 0:a355e511bc5d 58 // in seconds between interrupts, and start interrupt generation:
asobhy 1:3e9684e81312 59 PeriodicInt.attach(&PeriodicInterruptISR, 0.05); // 50ms sampling rate
asobhy 1:3e9684e81312 60
asobhy 2:ca2a7430739b 61 Kp = 2.2;
asobhy 1:3e9684e81312 62 Ki = 0.01;
asobhy 1:3e9684e81312 63
asobhy 0:a355e511bc5d 64 }
asobhy 0:a355e511bc5d 65
asobhy 0:a355e511bc5d 66
asobhy 0:a355e511bc5d 67 /*******************************************************************************
asobhy 0:a355e511bc5d 68 * ******** Periodic Timer Interrupt Thread ********
asobhy 0:a355e511bc5d 69 *******************************************************************************/
asobhy 0:a355e511bc5d 70 void PiControlThread(void const *argument)
asobhy 1:3e9684e81312 71 {
asobhy 3:4def4ca68910 72
asobhy 1:3e9684e81312 73 // initialization
asobhy 1:3e9684e81312 74 saturationFlag = false;
asobhy 1:3e9684e81312 75 int scale = 40;
asobhy 3:4def4ca68910 76 xState = 0;
asobhy 3:4def4ca68910 77
asobhy 3:4def4ca68910 78 int32_t xTemp;
asobhy 3:4def4ca68910 79 int32_t uProportional;
asobhy 3:4def4ca68910 80 int32_t uIntegral;
asobhy 3:4def4ca68910 81 int32_t uS;
asobhy 1:3e9684e81312 82
asobhy 0:a355e511bc5d 83 while (true)
asobhy 0:a355e511bc5d 84 {
asobhy 0:a355e511bc5d 85 osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received.
asobhy 0:a355e511bc5d 86
asobhy 0:a355e511bc5d 87 // get incremental position and time from QEI
asobhy 0:a355e511bc5d 88 DE0_read(&ID, &dPosition, &dTime);
asobhy 1:3e9684e81312 89 SaturateValue(dPosition, 560);
asobhy 1:3e9684e81312 90
asobhy 1:3e9684e81312 91 // maximum velocity at dPostition = 560 is vel = 703
asobhy 1:3e9684e81312 92 vel = (float)((6135.92 * dPosition) / dTime) ;
asobhy 0:a355e511bc5d 93
asobhy 0:a355e511bc5d 94 setpoint_mutex.lock();
asobhy 2:ca2a7430739b 95 e = SaturatingSubtract(setpoint, dPosition); // e is the velocity error
asobhy 0:a355e511bc5d 96 setpoint_mutex.unlock();
asobhy 3:4def4ca68910 97
asobhy 3:4def4ca68910 98 xTemp = SaturatingAdd(xState, e);
asobhy 1:3e9684e81312 99
asobhy 1:3e9684e81312 100 // the maximum value that 'u' can get to is 20
asobhy 1:3e9684e81312 101 // the maximum value that dPosition can get to 560
asobhy 1:3e9684e81312 102 // scaling factor is 560/20 = 28
asobhy 3:4def4ca68910 103 uProportional = (float)(Kp*e/scale);
asobhy 3:4def4ca68910 104 uIntegral = (float)(Ki*xState/scale);
asobhy 1:3e9684e81312 105
asobhy 3:4def4ca68910 106 uS = SaturatingAdd(uProportional, uIntegral);
asobhy 1:3e9684e81312 107
asobhy 3:4def4ca68910 108 u = SaturateValue(uS, U_LIMIT);
asobhy 3:4def4ca68910 109 if(u==uS) xState=xTemp;
asobhy 0:a355e511bc5d 110
asobhy 0:a355e511bc5d 111 if (u >= 0)
asobhy 0:a355e511bc5d 112 {
asobhy 0:a355e511bc5d 113 motorDriver_forward(u);
asobhy 0:a355e511bc5d 114 }
asobhy 0:a355e511bc5d 115 else if (u < 0)
asobhy 0:a355e511bc5d 116 {
asobhy 0:a355e511bc5d 117 motorDriver_reverse(u);
asobhy 1:3e9684e81312 118 }
asobhy 3:4def4ca68910 119
asobhy 0:a355e511bc5d 120 }
asobhy 1:3e9684e81312 121
asobhy 0:a355e511bc5d 122 }
asobhy 0:a355e511bc5d 123
asobhy 0:a355e511bc5d 124 /*******************************************************************************
asobhy 0:a355e511bc5d 125 * the interrupt below occures every 250ms as setup in the main function during
asobhy 0:a355e511bc5d 126 * initialization
asobhy 0:a355e511bc5d 127 * ******** Period Timer Interrupt Handler ********
asobhy 0:a355e511bc5d 128 *******************************************************************************/
asobhy 0:a355e511bc5d 129 void PeriodicInterruptISR(void)
asobhy 0:a355e511bc5d 130 {
asobhy 0:a355e511bc5d 131 // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread.
asobhy 0:a355e511bc5d 132 osSignalSet(PiControlId,0x1);
asobhy 0:a355e511bc5d 133 }
asobhy 1:3e9684e81312 134
asobhy 1:3e9684e81312 135
asobhy 1:3e9684e81312 136 /*****************************************************************************/
asobhy 1:3e9684e81312 137 int SaturatingSubtract(int x, int y)
asobhy 1:3e9684e81312 138 {
asobhy 1:3e9684e81312 139 int z;
asobhy 1:3e9684e81312 140 z = x - y; // 32-bit overflow detection and saturating arithmetic
asobhy 1:3e9684e81312 141 if((x > 0) && (y < 0) && (z < 0)) z = 0x7FFFFFFF;
asobhy 1:3e9684e81312 142 else if((x < 0) && (y > 0) && (z > 0)) z = 0x80000000;
asobhy 1:3e9684e81312 143 return z;
asobhy 1:3e9684e81312 144 }
asobhy 1:3e9684e81312 145
asobhy 1:3e9684e81312 146 /*****************************************************************************/
asobhy 1:3e9684e81312 147 int SaturatingAdd(int x, int y)
asobhy 1:3e9684e81312 148 {
asobhy 1:3e9684e81312 149 int z;
asobhy 1:3e9684e81312 150 z = x + y; // 32-bit overflow detection and saturating arithmetic
asobhy 1:3e9684e81312 151 if((x > 0) && (y > 0) && (z < 0)) z = 0x7FFFFFFF;
asobhy 1:3e9684e81312 152 else if((x < 0) && (y < 0) && (z > 0)) z = 0x80000000;
asobhy 1:3e9684e81312 153 return z;
asobhy 1:3e9684e81312 154 }
asobhy 1:3e9684e81312 155
asobhy 1:3e9684e81312 156 /*****************************************************************************/
asobhy 1:3e9684e81312 157 int SaturateValue(int x, int Limit)
asobhy 1:3e9684e81312 158 {
asobhy 1:3e9684e81312 159 if(x > Limit) return(Limit); // Impose maximum limit on x
asobhy 1:3e9684e81312 160 else if(x < -Limit) return(-Limit);
asobhy 1:3e9684e81312 161 else return(x);
asobhy 1:3e9684e81312 162 }