FRC - Hackathon / PID

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
--- 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);