29/10/2019
Dependencies: Encoder_Nucleo_16_bits
PID.cpp
- Committer:
- haarkon
- Date:
- 2018-05-20
- Revision:
- 0:259a31d968a6
- Child:
- 1:24c444f3716b
File content as of revision 0:259a31d968a6:
#include "PID.h" PID::PID(TIM_TypeDef * _TIM, PinName outPWM, PinName outA, PinName outB, double scale, double loopTime) : _encoder (_TIM), _pwm(outPWM), _outA(outA), _outB(outB) { Ticker _tick; if (loopTime > 0.0001) _loopTime = loopTime; else _loopTime = 0.001; if (scale > 0) _scale = scale ; else _scale = 31.8309886184; _pwm.period(1/20000); _pwm = 0; _Kp = 1; _Ki = 0; _Kd = 0; _tick.attach(this, &PID::controlLoop, _loopTime); } 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 (float setPoint) { _consigne = setPoint; } void PID::controlLoop() { static int firstTime = 1; static float integralError = 0; static float oldError; double deltaD, error, commande; long position; static long int oldPosition; position = _encoder.GetCounter(); if (firstTime) { oldPosition = position; firstTime = 0; } else { deltaD = 1000.0 * ((double)position - (double)oldPosition) / _scale; oldPosition = position; error = _consigne - deltaD; integralError += error; if (integralError > 10000) { RobotIsStuck = 1; integralError = 10000; } if (integralError < -10000) { RobotIsStuck = 1; integralError = -10000; } commande = _Kp * error + _Ki * integralError + _Kd * (error - oldError); oldError = error; _PwmValue = commande/1300; if (_PwmValue > 1) _PwmValue = 1; if (_PwmValue < -1) _PwmValue = -1; if (_PwmValue < 0) { _pwm = -_PwmValue; _outA = 1; _outB = 0; } else { _pwm = _PwmValue; _outA = 0; _outB = 1; } } }