PlayBack
Dependencies: TPixy-Interface
Fork of ObjectFollower by
Diff: PiControlThread.cpp
- Revision:
- 4:417e475239c7
- Parent:
- 3:4def4ca68910
- Child:
- 5:b29220d29022
- Child:
- 6:e7ce340fe91e
--- a/PiControlThread.cpp Sat Feb 03 00:48:47 2018 +0000 +++ b/PiControlThread.cpp Fri Feb 09 18:37:11 2018 +0000 @@ -3,29 +3,17 @@ #include "Drivers/motor_driver.h" #include "Drivers/DE0_driver.h" #include "PiControlThread.h" +#include "PiController.h" -// global speed variable; extern int setpoint; -extern Serial pc; -extern Mutex setpoint_mutex; uint16_t ID, dTime; int dPosition; int vel; - -int32_t e, u, xState; - -bool saturationFlag; - -float Ki; -float Kp; +int32_t U; void PiControlThread(void const *); void PeriodicInterruptISR(void); -int SaturatingSubtract(int , int ); -int SaturatingAdd(int , int ); -int SaturateValue(int , int ); - osThreadId PiControlId; @@ -49,18 +37,16 @@ void PiControlThreadInit() { - DE0_init(); // initialize FPGA - motorDriver_init(); // initialize motorDriver + DE0_init(); // initialize FPGA + motorDriver_init(); // initialize motorDriver + PiController_init(2.2,0.01); // initialize the PI Controller gains and initialize variables PiControlId = osThreadCreate(osThread(PiControlThread), NULL); // Specify address of the PeriodicInt ISR as PiControllerISR, specify the interval // in seconds between interrupts, and start interrupt generation: PeriodicInt.attach(&PeriodicInterruptISR, 0.05); // 50ms sampling rate - - Kp = 2.2; - Ki = 0.01; - + } @@ -70,16 +56,6 @@ void PiControlThread(void const *argument) { - // initialization - saturationFlag = false; - int scale = 40; - xState = 0; - - int32_t xTemp; - int32_t uProportional; - int32_t uIntegral; - int32_t uS; - while (true) { osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received. @@ -91,30 +67,15 @@ // maximum velocity at dPostition = 560 is vel = 703 vel = (float)((6135.92 * dPosition) / dTime) ; - setpoint_mutex.lock(); - e = SaturatingSubtract(setpoint, dPosition); // e is the velocity error - setpoint_mutex.unlock(); - - xTemp = SaturatingAdd(xState, 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 - uProportional = (float)(Kp*e/scale); - uIntegral = (float)(Ki*xState/scale); + U = PiController(setpoint,dPosition); - uS = SaturatingAdd(uProportional, uIntegral); - - u = SaturateValue(uS, U_LIMIT); - if(u==uS) xState=xTemp; - - if (u >= 0) + if (U >= 0) { - motorDriver_forward(u); + motorDriver_forward(U); } - else if (u < 0) + else if (U < 0) { - motorDriver_reverse(u); + motorDriver_reverse(U); } } @@ -132,31 +93,3 @@ 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); -}