ManualControl
Dependencies: TPixy-Interface
Fork of MbedOS_Robot by
Revision 3:4def4ca68910, committed 2018-02-03
- Comitter:
- asobhy
- Date:
- Sat Feb 03 00:48:47 2018 +0000
- Parent:
- 2:ca2a7430739b
- Child:
- 4:417e475239c7
- Commit message:
- using the control algorithm in the notes
Changed in this revision
PiControlThread.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/PiControlThread.cpp Sat Feb 03 00:37:57 2018 +0000 +++ b/PiControlThread.cpp Sat Feb 03 00:48:47 2018 +0000 @@ -69,9 +69,16 @@ *******************************************************************************/ 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) { @@ -87,29 +94,19 @@ setpoint_mutex.lock(); e = SaturatingSubtract(setpoint, dPosition); // e is the velocity error setpoint_mutex.unlock(); - - // 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. - } + + 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 - - u = (float)(Kp*e/scale) + (float)(Ki*xState/scale); // u is the control signal + uProportional = (float)(Kp*e/scale); + uIntegral = (float)(Ki*xState/scale); - // if u is saturated set the flag to true - if( (u>= U_LIMIT) || (u<= -U_LIMIT) ) saturationFlag = true; - else saturationFlag = false; + uS = SaturatingAdd(uProportional, uIntegral); - // saturate u at values greater than 20 and smaller than -20 - u = SaturateValue(u, U_LIMIT); + u = SaturateValue(uS, U_LIMIT); + if(u==uS) xState=xTemp; if (u >= 0) { @@ -119,7 +116,7 @@ { motorDriver_reverse(u); } - + } }