Motion Control library for 2, 3 inputs Full Bridge + Quadrature Encoder, motor system (also called a 2 wheels robot)
Dependencies: Encoder_Nucleo_16_bits
Dependents: FRC_2018 FRC2018_Bis 0hackton_08_06_18 lib_FRC_2019 ... more
Diff: PID.cpp
- Revision:
- 8:4553677e8b99
- Parent:
- 7:78cdcb637927
- Child:
- 12:13f42fe66fb1
diff -r 78cdcb637927 -r 4553677e8b99 PID.cpp --- a/PID.cpp Tue Jun 05 12:27:28 2018 +0000 +++ b/PID.cpp Tue Jun 05 13:33:28 2018 +0000 @@ -2,7 +2,7 @@ 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) { - Ticker _tick; + _tick.attach(callback(this, &PID::controlLoop), 0.001); _Lpwm.period_us(50); _Lpwm = 0; @@ -13,8 +13,6 @@ _Kp = 2.0; _Ki = 0.4; _Kd = 1.0; - - _tick.attach(callback(this, &PID::controlLoop), 0.001); } float PID::setProportionnalValue (float KpValue) @@ -150,7 +148,7 @@ //odometry (segments approx.) meanDist = (_measDistG + _measDistD) / 2.0; - diffDist = _measDistG - _measDistD; + diffDist = _measDistD - _measDistG; deltaX = meanDist * cos (_THETA); deltaY = meanDist * sin (_THETA);