29/10/2019
Dependencies: Encoder_Nucleo_16_bits
Diff: PID.cpp
- Revision:
- 15:5dfa92698884
- Parent:
- 13:0227a21c181a
- Child:
- 16:a8a0a1e3ddb1
diff -r 35f197ad6002 -r 5dfa92698884 PID.cpp --- 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;