Sécu LL: on stoppe le robot à la fin du match car le HL est trop gogo…

…l pour se rendre compte qu'on a fini

teensy/loli_teensy/MotionControlSystem.cpp

@@ -23,6 +23,7 @@ endOfMoveMgr(currentMovingSpeed)
 
 	movingState = STOPPED;
 	trajectoryFullyCompleted = true;
+	matchTerminated = false;
 	trajectoryIndex = 0;
 	updateNextStopPoint();
 	updateSideDistanceFactors();
@@ -596,17 +597,24 @@ MotionControlSystem::MovingState MotionControlSystem::getMovingState() const
 
 void MotionControlSystem::gotoNextStopPoint()
 {
-	noInterrupts();
-	if (movingState == MOVING || movingState == MOVE_INIT)
+	if (matchTerminated)
 	{
-		Log::warning("Nested call of MotionControlSystem::gotoNextStopPoint()");
-		return;
+		Log::warning("Move request after match end");
 	}
-	updateNextStopPoint();
-	movingState = MOVE_INIT;
-	trajectoryFullyCompleted = false;
+	else
+	{
+		noInterrupts();
+		if (movingState == MOVING || movingState == MOVE_INIT)
+		{
+			Log::warning("Nested call of MotionControlSystem::gotoNextStopPoint()");
+			return;
+		}
+		updateNextStopPoint();
+		movingState = MOVE_INIT;
+		trajectoryFullyCompleted = false;
 
-	interrupts();
+		interrupts();
+	}
 }
 
 void MotionControlSystem::stop() 
@@ -645,6 +653,16 @@ void MotionControlSystem::highLevelStop()
 	stop();
 }
 
+void MotionControlSystem::endMatchStop()
+{
+	noInterrupts();
+	movingState = STOPPED;
+	clearCurrentTrajectory();
+	interrupts();
+	stop();
+	matchTerminated = true;
+}
+
 bool MotionControlSystem::isStopped() const
 {
 	noInterrupts();

teensy/loli_teensy/MotionControlSystem.h

@@ -201,6 +201,7 @@ class MotionControlSystem : public Singleton<MotionControlSystem>
 private:
 	volatile MovingState movingState;
 	volatile bool trajectoryFullyCompleted; // Indique que tous les points de la dernière trajectoire ont étés rendus obsolètes
+	bool matchTerminated;
 
 	// Variables d'activation des différents PID
 	bool positionControlled;	//  Asservissement en position
@@ -276,6 +277,7 @@ class MotionControlSystem : public Singleton<MotionControlSystem>
 	void gotoNextStopPoint();
 	void stop(); // Met toutes les consignes de l'asservissement à une valeur permettant l'arrêt du robot
 	void highLevelStop(); // Termine la trajectoire courante et stoppe le robot
+	void endMatchStop(); // Termine la trajectoire courante et empêche d'en effectuer une nouvelle
 	bool isStopped() const; // Indique si le robot est physiquement à l'arrêt
 	bool isBreaking() const; // Indique si le robot est en train de ralentir
 	void setMaxMovingSpeed(int32_t); // Règle la vitess maximale de translation (argument passé en mm/s)

teensy/loli_teensy/OrderLong.h

@@ -238,6 +238,7 @@ class StartMatchChrono : public OrderLong, public Singleton<StartMatchChrono>
 		if (millis() - beginTime > 90000 && !matchFinished)
 		{
 			matchFinished = true;
+			motionControlSystem.endMatchStop();
 			actuatorMgr.funnyAction(true);
 		}
 		else if (matchFinished)