PID.cpp

00001 #include "PID.h"
00002 
00003 PID::PID (TIM_TypeDef *LTIM, TIM_TypeDef *RTIM, PinName LPWM, PinName RPWM, PinName LDIRA, PinName LDIRB, PinName RDIRA, PinName RDIRB) : _Lencoder(LTIM), _Rencoder(RTIM), _Lpwm(LPWM), _Rpwm(RPWM), _LdirA(LDIRA), _LdirB(LDIRB), _RdirA(RDIRA), _RdirB(RDIRB)
00004 {
00005     _tick.attach(callback(this, &PID::controlLoop), 0.001);
00006 
00007     _Lpwm.period_us(50);
00008     _Lpwm = 0;
00009 
00010     _Rpwm.period_us(50);
00011     _Rpwm = 0;
00012 
00013     _Kp = 2.0;
00014     _Ki = 0.4;
00015     _Kd = 1.0;
00016 }
00017 
00018 float PID::setProportionnalValue (float KpValue)
00019 {
00020     _Kp = KpValue;
00021     return _Kp;
00022 }
00023 
00024 float PID::setintegralValue (float KiValue)
00025 {
00026     _Ki = KiValue;
00027     return _Ki;
00028 }
00029 
00030 float PID::setDerivativeValue (float KdValue)
00031 {
00032     _Kd = KdValue;
00033     return _Kd;
00034 }
00035 
00036 void PID::setSpeed (double left, double right)
00037 {
00038     _SpeedG = left;
00039     _SpeedD = right;
00040 }
00041 
00042 void PID::getSpeed (double *vG, double *vD)
00043 {
00044     *vG = _measSpeedG;
00045     *vD = _measSpeedD;
00046 }
00047 
00048 void PID::getPosition (double *x, double *y, double *theta)
00049 {
00050     *x = _X;
00051     *y = _Y;
00052     *theta = _THETA;
00053 }
00054 
00055 void PID::resetPosition (void)
00056 {
00057     _X = 0;
00058     _Y = 0;
00059     _THETA = 0;
00060 }
00061 
00062 void PID::controlLoop()
00063 {
00064     long            PositionG, PositionD;
00065     static float    integralErrorG = 0, integralErrorD = 0;
00066     static float    oldErrorG, oldErrorD;
00067     double          errorG, commandeG;
00068     double          errorD, commandeD;
00069     double          meanDist, diffDist, deltaX, deltaY, deltaTheta;
00070     static long     oldPositionG=0, oldPositionD=0;
00071 
00072     PositionG =  _Lencoder.GetCounter();
00073     PositionD = -_Rencoder.GetCounter();
00074 
00075     // As we use mm/s for speed unit and we convert all mesure to this unit
00076 
00077     // Converting step to millimeters
00078     _measDistG = ((double)PositionG - (double)oldPositionG) / 54.881;
00079     _measDistD = ((double)PositionD - (double)oldPositionD) / 54.881;
00080 
00081     // Converting position to speed
00082     _measSpeedG = 1000.0 * _measDistG;
00083     _measSpeedD = 1000.0 * _measDistD;
00084 
00085     // Computing errors
00086     errorG = _SpeedG - _measSpeedG;
00087     errorD = _SpeedD - _measSpeedD;
00088     integralErrorG += errorG;
00089     integralErrorD += errorD;
00090 
00091     // Limiting integral error
00092     if (integralErrorG > 10000) {
00093         _WheelStuckG = 1;
00094         integralErrorG = 10000;
00095     } else {
00096         if (integralErrorG < -10000) {
00097             _WheelStuckG = 1;
00098             integralErrorG = -10000;
00099         } else {
00100             _WheelStuckG = 0;
00101         }
00102     }
00103 
00104     if (integralErrorD > 10000) {
00105         _WheelStuckD = 1;
00106         integralErrorD = 10000;
00107     } else {
00108         if (integralErrorD < -10000) {
00109             _WheelStuckD = 1;
00110             integralErrorD = -10000;
00111         } else {
00112             _WheelStuckD = 0;
00113         }
00114     }
00115 
00116     // Correcting system (empiric test tells me that Kp = 4, Ki = 1 and Kd = 2, but this is probably not the good setting)
00117     commandeG = _Kp * errorG + _Ki * integralErrorG + _Kd * (errorG - oldErrorG);
00118     commandeD = _Kp * errorD + _Ki * integralErrorD + _Kd * (errorD - oldErrorD);
00119 
00120     // Convert to PWM
00121     _PwmValueG = commandeG / 1300.0;
00122     if (_PwmValueG >  1) _PwmValueG =  1;
00123     if (_PwmValueG < -1) _PwmValueG = -1;
00124 
00125     if (_PwmValueG < 0) {
00126         _Lpwm  = -_PwmValueG;
00127         _LdirA = 0;
00128         _LdirB = 1;
00129     } else {
00130         _Lpwm = _PwmValueG;
00131         _LdirA = 1;
00132         _LdirB = 0;
00133     }
00134 
00135     _PwmValueD = commandeD/1300.0;
00136     if (_PwmValueD >  1) _PwmValueD =  1;
00137     if (_PwmValueD < -1) _PwmValueD = -1;
00138 
00139     if (_PwmValueD < 0) {
00140         _Rpwm  = -_PwmValueD;
00141         _RdirA = 1;
00142         _RdirB = 0;
00143     } else {
00144         _Rpwm = _PwmValueD;
00145         _RdirA = 0;
00146         _RdirB = 1;
00147     }
00148 
00149     //odometry (segments approx.)
00150     meanDist = (_measDistG + _measDistD) / 2.0;
00151     diffDist =  _measDistD - _measDistG;
00152 
00153     deltaX = meanDist * cos (_THETA);
00154     deltaY = meanDist * sin (_THETA);
00155     deltaTheta = atan2(diffDist,230.0);
00156 
00157     _X += deltaX;
00158     _Y += deltaY;
00159     _THETA += deltaTheta;
00160     //if (_THETA > 3.141592653589793) _THETA -= 6.283185307179586;
00161     //if (_THETA < -3.141592653589793) _THETA += 6.283185307179586;
00162 
00163     oldPositionG = PositionG;
00164     oldPositionD = PositionD;
00165     oldErrorG = errorG;
00166     oldErrorD = errorD;
00167 }