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:
15:5dfa92698884
Parent:
13:0227a21c181a
--- a/PID.cpp	Fri Oct 25 08:16:23 2019 +0000
+++ b/PID.cpp	Fri Oct 25 09:41:31 2019 +0000
@@ -75,8 +75,8 @@
     // As we use mm/s for speed unit and we convert all mesure to this unit
 
     // Converting step to millimeters
-    _measDistG = 20.0*(((double)PositionG - (double)oldPositionG) / 63.662)/23.0;
-    _measDistD = 20.0*(((double)PositionD - (double)oldPositionD) / 63.662)/23.0;
+    _measDistG = ((double)PositionG - (double)oldPositionG) / 54.881;
+    _measDistD = ((double)PositionD - (double)oldPositionD) / 54.881;
 
     // Converting position to speed
     _measSpeedG = 1000.0 * _measDistG;
@@ -157,8 +157,8 @@
     _X += deltaX;
     _Y += deltaY;
     _THETA += deltaTheta;
-    if (_THETA > 3.141592653589793) _THETA -= 6.283185307179586;
-    if (_THETA < -3.141592653589793) _THETA += 6.283185307179586;
+    //if (_THETA > 3.141592653589793) _THETA -= 6.283185307179586;
+    //if (_THETA < -3.141592653589793) _THETA += 6.283185307179586;
 
     oldPositionG = PositionG;
     oldPositionD = PositionD;