ObjectFollower
Dependencies: TPixy-Interface
Fork of PlayBack by
Diff: PiControlThread.cpp
- Revision:
- 1:3e9684e81312
- Parent:
- 0:a355e511bc5d
- Child:
- 2:ca2a7430739b
--- a/PiControlThread.cpp Thu Feb 01 03:58:00 2018 +0000 +++ b/PiControlThread.cpp Sat Feb 03 00:05:08 2018 +0000 @@ -2,19 +2,30 @@ #include "ui.h" #include "Drivers/motor_driver.h" #include "Drivers/DE0_driver.h" +#include "PiControlThread.h" // global speed variable; extern int setpoint; extern Serial pc; extern Mutex setpoint_mutex; -uint16_t ID, dPosition, dTime; -int e, u, xState; +uint16_t ID, dTime; +int dPosition; +int vel; -float Ki, Kp; +int32_t e, u, xState; + +bool saturationFlag; + +float Ki; +float Kp; void PiControlThread(void const *); void PeriodicInterruptISR(void); +int SaturatingSubtract(int , int ); +int SaturatingAdd(int , int ); +int SaturateValue(int , int ); + osThreadId PiControlId; @@ -45,7 +56,11 @@ // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval // in seconds between interrupts, and start interrupt generation: - PeriodicInt.attach(&PeriodicInterruptISR, 0.05); + PeriodicInt.attach(&PeriodicInterruptISR, 0.05); // 50ms sampling rate + + Kp = 2; + Ki = 0.01; + } @@ -53,23 +68,48 @@ * ******** Periodic Timer Interrupt Thread ******** *******************************************************************************/ void PiControlThread(void const *argument) -{ +{ + // initialization + saturationFlag = false; + int scale = 40; + while (true) { - osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received. - led3= !led3; // Alive status - led3 toggles each time PieriodicZInterruptsThread is signaled. // get incremental position and time from QEI DE0_read(&ID, &dPosition, &dTime); + SaturateValue(dPosition, 560); + + // maximum velocity at dPostition = 560 is vel = 703 + vel = (float)((6135.92 * dPosition) / dTime) ; setpoint_mutex.lock(); - e = setpoint-dPosition; // e is the velocity error + e = SaturatingSubtract(setpoint, vel); // e is the velocity error setpoint_mutex.unlock(); - xState = xState + e; // x is the Euler approximation to the integral of e. - u = Kp*e + Ki*xState; // u is the control signal - + // disable integration if u is saturated + if(saturationFlag) + { + xState = xState; + } + else + { + xState = SaturatingAdd(xState, e); // x is the Euler approximation to the integral of e. + } + + // the maximum value that 'u' can get to is 20 + // the maximum value that dPosition can get to 560 + // scaling factor is 560/20 = 28 + + u = (float)(Kp*e/scale) + (float)(Ki*xState/scale); // u is the control signal + + // if u is saturated set the flag to true + if( (u>= U_LIMIT) || (u<= -U_LIMIT) ) saturationFlag = true; + else saturationFlag = false; + + // saturate u at values greater than 20 and smaller than -20 + u = SaturateValue(u, U_LIMIT); if (u >= 0) { @@ -78,12 +118,10 @@ else if (u < 0) { motorDriver_reverse(u); - } - else - { - pc.printf("\r\nerror!!!"); - } + } + } + } /******************************************************************************* @@ -96,3 +134,32 @@ // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread. osSignalSet(PiControlId,0x1); } + + +/*****************************************************************************/ +int SaturatingSubtract(int x, int y) +{ + int z; + z = x - y; // 32-bit overflow detection and saturating arithmetic + if((x > 0) && (y < 0) && (z < 0)) z = 0x7FFFFFFF; + else if((x < 0) && (y > 0) && (z > 0)) z = 0x80000000; + return z; +} + +/*****************************************************************************/ +int SaturatingAdd(int x, int y) +{ + int z; + z = x + y; // 32-bit overflow detection and saturating arithmetic + if((x > 0) && (y > 0) && (z < 0)) z = 0x7FFFFFFF; + else if((x < 0) && (y < 0) && (z > 0)) z = 0x80000000; + return z; +} + +/*****************************************************************************/ +int SaturateValue(int x, int Limit) +{ + if(x > Limit) return(Limit); // Impose maximum limit on x + else if(x < -Limit) return(-Limit); + else return(x); +}