From ee6da38730a3cf02017470b3e34361a7243f38f5 Mon Sep 17 00:00:00 2001
From: Sylvain <sylvain.gaultier@telecom-sudparis.eu>
Date: Wed, 26 Apr 2017 22:09:54 +0200
Subject: [PATCH] =?UTF-8?q?bug=20fix:=20robot=20qui=20part=20en=20couille?=
 =?UTF-8?q?=20=C3=A0=20fond=20la=20caisse?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 teensy/loli_teensy/MotionControlSystem.cpp | 35 ++++++++++++++--------
 teensy/loli_teensy/PID.h                   |  3 +-
 2 files changed, 24 insertions(+), 14 deletions(-)

diff --git a/teensy/loli_teensy/MotionControlSystem.cpp b/teensy/loli_teensy/MotionControlSystem.cpp
index e4c96250..8523d9e0 100644
--- a/teensy/loli_teensy/MotionControlSystem.cpp
+++ b/teensy/loli_teensy/MotionControlSystem.cpp
@@ -413,6 +413,7 @@ void MotionControlSystem::updateTranslationSetpoint()
 
 	if (nextStopPoint == UINT16_MAX)
 	{
+		translationSetpoint = currentTranslation + UINT8_MAX * TRAJECTORY_STEP;
 		if (!undefinedStopPoint)
 		{
 			forwardTranslationPID.resetDerivativeError();
@@ -421,19 +422,9 @@ void MotionControlSystem::updateTranslationSetpoint()
 			backwardTranslationPID.resetIntegralError();
 			undefinedStopPoint = true;
 		}
-		translationSetpoint = currentTranslation + UINT8_MAX * TRAJECTORY_STEP;
 	}
 	else
 	{
-		if (undefinedStopPoint)
-		{
-			forwardTranslationPID.resetDerivativeError();
-			forwardTranslationPID.resetIntegralError();
-			backwardTranslationPID.resetDerivativeError();
-			backwardTranslationPID.resetIntegralError();
-			undefinedStopPoint = false;
-		}
-
 		uint8_t nbPointsToTravel = nextStopPoint - trajectoryIndex;
 		translationSetpoint = currentTranslation + nbPointsToTravel * TRAJECTORY_STEP;
 		if (nbPointsToTravel > 10)
@@ -443,13 +434,31 @@ void MotionControlSystem::updateTranslationSetpoint()
 		else
 		{
 			Position posConsigne = currentTrajectory[trajectoryIndex].getPosition();
-			translationSetpoint += 
+			float offset = 
 				ABS(
 					(
 						(position.x - posConsigne.x) * cosf(posConsigne.orientation) + 
 						(position.y - posConsigne.y) * sinf(posConsigne.orientation)
 					) / TICK_TO_MM
 				);
+			if (offset < TRAJECTORY_STEP)
+			{
+				translationSetpoint += (int32_t)offset;
+			}
+			else
+			{
+				translationSetpoint += TRAJECTORY_STEP / 2;
+				Log::warning("Position courante tres loin du point de trajectoire courant");
+			}
+		}
+
+		if (undefinedStopPoint)
+		{
+			forwardTranslationPID.resetDerivativeError();
+			forwardTranslationPID.resetIntegralError();
+			backwardTranslationPID.resetDerivativeError();
+			backwardTranslationPID.resetIntegralError();
+			undefinedStopPoint = false;
 		}
 	}
 }
@@ -1059,8 +1068,8 @@ void MotionControlSystem::loadDefaultParameters()
 
 	maxAcceleration = 25;
 
-	forwardTranslationPID.setTunings(2.75, 0, 1);
-	backwardTranslationPID.setTunings(2.75, 0, 1);
+	forwardTranslationPID.setTunings(2.75, 0, 1.5);
+	backwardTranslationPID.setTunings(1.75, 0, 1);
 	leftSpeedPID.setTunings(0.6, 0.01, 20);
 	rightSpeedPID.setTunings(0.6, 0.01, 20);
 	curvatureCorrectorK1 = 0.1;
diff --git a/teensy/loli_teensy/PID.h b/teensy/loli_teensy/PID.h
index 94b88351..308a38b6 100644
--- a/teensy/loli_teensy/PID.h
+++ b/teensy/loli_teensy/PID.h
@@ -69,7 +69,8 @@ class PID : public Printable
 	}
 
 	void resetDerivativeError() {
-		pre_error = 0;
+		// pre_error = error :
+		pre_error = (*setPoint) - (*input);
 	}
 	void resetIntegralError() {
 		integral = 0;