ECE4333 - 2018 - Ahmed & Brandon / Mbed OS MbedOS_Robot_Team

ManualControl

Dependencies:   TPixy-Interface

Fork of MbedOS_Robot by ECE4333 - 2018 - Ahmed & Brandon

PiControlThread.cpp@1:3e9684e81312, 2018-02-03 (annotated)

Committer:
asobhy
Date:
Sat Feb 03 00:05:08 2018 +0000
Revision:
1:3e9684e81312
Parent:
0:a355e511bc5d
Child:
2:ca2a7430739b
setpoint represent velocity

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 1:3e9684e81312 61 Kp = 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 1:3e9684e81312 72 // initialization
asobhy 1:3e9684e81312 73 saturationFlag = false;
asobhy 1:3e9684e81312 74 int scale = 40;
asobhy 1:3e9684e81312 75
asobhy 0:a355e511bc5d 76 while (true)
asobhy 0:a355e511bc5d 77 {
asobhy 0:a355e511bc5d 78 osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received.
asobhy 0:a355e511bc5d 79
asobhy 0:a355e511bc5d 80 // get incremental position and time from QEI
asobhy 0:a355e511bc5d 81 DE0_read(&ID, &dPosition, &dTime);
asobhy 1:3e9684e81312 82 SaturateValue(dPosition, 560);
asobhy 1:3e9684e81312 83
asobhy 1:3e9684e81312 84 // maximum velocity at dPostition = 560 is vel = 703
asobhy 1:3e9684e81312 85 vel = (float)((6135.92 * dPosition) / dTime) ;
asobhy 0:a355e511bc5d 86
asobhy 0:a355e511bc5d 87 setpoint_mutex.lock();
asobhy 1:3e9684e81312 88 e = SaturatingSubtract(setpoint, vel); // e is the velocity error
asobhy 0:a355e511bc5d 89 setpoint_mutex.unlock();
asobhy 0:a355e511bc5d 90
asobhy 1:3e9684e81312 91 // disable integration if u is saturated
asobhy 1:3e9684e81312 92 if(saturationFlag)
asobhy 1:3e9684e81312 93 {
asobhy 1:3e9684e81312 94 xState = xState;
asobhy 1:3e9684e81312 95 }
asobhy 1:3e9684e81312 96 else
asobhy 1:3e9684e81312 97 {
asobhy 1:3e9684e81312 98 xState = SaturatingAdd(xState, e); // x is the Euler approximation to the integral of e.
asobhy 1:3e9684e81312 99 }
asobhy 1:3e9684e81312 100
asobhy 1:3e9684e81312 101 // the maximum value that 'u' can get to is 20
asobhy 1:3e9684e81312 102 // the maximum value that dPosition can get to 560
asobhy 1:3e9684e81312 103 // scaling factor is 560/20 = 28
asobhy 1:3e9684e81312 104
asobhy 1:3e9684e81312 105 u = (float)(Kp*e/scale) + (float)(Ki*xState/scale); // u is the control signal
asobhy 1:3e9684e81312 106
asobhy 1:3e9684e81312 107 // if u is saturated set the flag to true
asobhy 1:3e9684e81312 108 if( (u>= U_LIMIT) || (u<= -U_LIMIT) ) saturationFlag = true;
asobhy 1:3e9684e81312 109 else saturationFlag = false;
asobhy 1:3e9684e81312 110
asobhy 1:3e9684e81312 111 // saturate u at values greater than 20 and smaller than -20
asobhy 1:3e9684e81312 112 u = SaturateValue(u, U_LIMIT);
asobhy 0:a355e511bc5d 113
asobhy 0:a355e511bc5d 114 if (u >= 0)
asobhy 0:a355e511bc5d 115 {
asobhy 0:a355e511bc5d 116 motorDriver_forward(u);
asobhy 0:a355e511bc5d 117 }
asobhy 0:a355e511bc5d 118 else if (u < 0)
asobhy 0:a355e511bc5d 119 {
asobhy 0:a355e511bc5d 120 motorDriver_reverse(u);
asobhy 1:3e9684e81312 121 }
asobhy 1:3e9684e81312 122
asobhy 0:a355e511bc5d 123 }
asobhy 1:3e9684e81312 124
asobhy 0:a355e511bc5d 125 }
asobhy 0:a355e511bc5d 126
asobhy 0:a355e511bc5d 127 /*******************************************************************************
asobhy 0:a355e511bc5d 128 * the interrupt below occures every 250ms as setup in the main function during
asobhy 0:a355e511bc5d 129 * initialization
asobhy 0:a355e511bc5d 130 * ******** Period Timer Interrupt Handler ********
asobhy 0:a355e511bc5d 131 *******************************************************************************/
asobhy 0:a355e511bc5d 132 void PeriodicInterruptISR(void)
asobhy 0:a355e511bc5d 133 {
asobhy 0:a355e511bc5d 134 // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread.
asobhy 0:a355e511bc5d 135 osSignalSet(PiControlId,0x1);
asobhy 0:a355e511bc5d 136 }
asobhy 1:3e9684e81312 137
asobhy 1:3e9684e81312 138
asobhy 1:3e9684e81312 139 /*****************************************************************************/
asobhy 1:3e9684e81312 140 int SaturatingSubtract(int x, int y)
asobhy 1:3e9684e81312 141 {
asobhy 1:3e9684e81312 142 int z;
asobhy 1:3e9684e81312 143 z = x - y; // 32-bit overflow detection and saturating arithmetic
asobhy 1:3e9684e81312 144 if((x > 0) && (y < 0) && (z < 0)) z = 0x7FFFFFFF;
asobhy 1:3e9684e81312 145 else if((x < 0) && (y > 0) && (z > 0)) z = 0x80000000;
asobhy 1:3e9684e81312 146 return z;
asobhy 1:3e9684e81312 147 }
asobhy 1:3e9684e81312 148
asobhy 1:3e9684e81312 149 /*****************************************************************************/
asobhy 1:3e9684e81312 150 int SaturatingAdd(int x, int y)
asobhy 1:3e9684e81312 151 {
asobhy 1:3e9684e81312 152 int z;
asobhy 1:3e9684e81312 153 z = x + y; // 32-bit overflow detection and saturating arithmetic
asobhy 1:3e9684e81312 154 if((x > 0) && (y > 0) && (z < 0)) z = 0x7FFFFFFF;
asobhy 1:3e9684e81312 155 else if((x < 0) && (y < 0) && (z > 0)) z = 0x80000000;
asobhy 1:3e9684e81312 156 return z;
asobhy 1:3e9684e81312 157 }
asobhy 1:3e9684e81312 158
asobhy 1:3e9684e81312 159 /*****************************************************************************/
asobhy 1:3e9684e81312 160 int SaturateValue(int x, int Limit)
asobhy 1:3e9684e81312 161 {
asobhy 1:3e9684e81312 162 if(x > Limit) return(Limit); // Impose maximum limit on x
asobhy 1:3e9684e81312 163 else if(x < -Limit) return(-Limit);
asobhy 1:3e9684e81312 164 else return(x);
asobhy 1:3e9684e81312 165 }