29/10/2019
Dependencies: Encoder_Nucleo_16_bits
PID.cpp
- Committer:
- haarkon
- Date:
- 2019-10-25
- Revision:
- 15:5dfa92698884
- Parent:
- 13:0227a21c181a
- Child:
- 16:a8a0a1e3ddb1
File content as of revision 15:5dfa92698884:
#include "PID.h" 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) { _tick.attach(callback(this, &PID::controlLoop), 0.001); _Lpwm.period_us(50); _Lpwm = 0; _Rpwm.period_us(50); _Rpwm = 0; _Kp = 2.0; _Ki = 0.4; _Kd = 1.0; } float PID::setProportionnalValue (float KpValue) { _Kp = KpValue; return _Kp; } float PID::setintegralValue (float KiValue) { _Ki = KiValue; return _Ki; } float PID::setDerivativeValue (float KdValue) { _Kd = KdValue; return _Kd; } void PID::setSpeed (double left, double right) { _SpeedG = left; _SpeedD = right; } void PID::getSpeed (double *vG, double *vD) { *vG = _measSpeedG; *vD = _measSpeedD; } void PID::getPosition (double *x, double *y, double *theta) { *x = _X; *y = _Y; *theta = _THETA; } void PID::resetPosition (void) { _X = 0; _Y = 0; _THETA = 0; } void PID::controlLoop() { long PositionG, PositionD; static float integralErrorG = 0, integralErrorD = 0; static float oldErrorG, oldErrorD; double errorG, commandeG; double errorD, commandeD; double meanDist, diffDist, deltaX, deltaY, deltaTheta; static long oldPositionG=0, oldPositionD=0; PositionG = _Lencoder.GetCounter(); PositionD = -_Rencoder.GetCounter(); // As we use mm/s for speed unit and we convert all mesure to this unit // Converting step to millimeters _measDistG = ((double)PositionG - (double)oldPositionG) / 54.881; _measDistD = ((double)PositionD - (double)oldPositionD) / 54.881; // Converting position to speed _measSpeedG = 1000.0 * _measDistG; _measSpeedD = 1000.0 * _measDistD; // Computing errors errorG = _SpeedG - _measSpeedG; errorD = _SpeedD - _measSpeedD; integralErrorG += errorG; integralErrorD += errorD; // Limiting integral error if (integralErrorG > 10000) { _WheelStuckG = 1; integralErrorG = 10000; } else { if (integralErrorG < -10000) { _WheelStuckG = 1; integralErrorG = -10000; } else { _WheelStuckG = 0; } } if (integralErrorD > 10000) { _WheelStuckD = 1; integralErrorD = 10000; } else { if (integralErrorD < -10000) { _WheelStuckD = 1; integralErrorD = -10000; } else { _WheelStuckD = 0; } } // Correcting system (empiric test tells me that Kp = 4, Ki = 1 and Kd = 2, but this is probably not the good setting) commandeG = _Kp * errorG + _Ki * integralErrorG + _Kd * (errorG - oldErrorG); commandeD = _Kp * errorD + _Ki * integralErrorD + _Kd * (errorD - oldErrorD); // Convert to PWM _PwmValueG = commandeG / 1300.0; if (_PwmValueG > 1) _PwmValueG = 1; if (_PwmValueG < -1) _PwmValueG = -1; if (_PwmValueG < 0) { _Lpwm = -_PwmValueG; _LdirA = 0; _LdirB = 1; } else { _Lpwm = _PwmValueG; _LdirA = 1; _LdirB = 0; } _PwmValueD = commandeD/1300.0; if (_PwmValueD > 1) _PwmValueD = 1; if (_PwmValueD < -1) _PwmValueD = -1; if (_PwmValueD < 0) { _Rpwm = -_PwmValueD; _RdirA = 1; _RdirB = 0; } else { _Rpwm = _PwmValueD; _RdirA = 0; _RdirB = 1; } //odometry (segments approx.) meanDist = (_measDistG + _measDistD) / 2.0; diffDist = _measDistD - _measDistG; deltaX = meanDist * cos (_THETA); deltaY = meanDist * sin (_THETA); deltaTheta = atan2(diffDist,230.0); _X += deltaX; _Y += deltaY; _THETA += deltaTheta; //if (_THETA > 3.141592653589793) _THETA -= 6.283185307179586; //if (_THETA < -3.141592653589793) _THETA += 6.283185307179586; oldPositionG = PositionG; oldPositionD = PositionD; oldErrorG = errorG; oldErrorD = errorD; }