From 7d9e38f3ef549a6a0f8e8ff4bc11cfa7b076d647 Mon Sep 17 00:00:00 2001
From: dimercur <dimercur@insa-toulouse.fr>
Date: Tue, 10 Oct 2023 16:03:25 +0200
Subject: [PATCH] Fonction moteur marche a peu pres correctement.
 Asservissement a revoir cependant

---
 software/dumber3/.cproject                    |  15 +-
 software/dumber3/Application/moteurs.c        | 380 ++++++++++++------
 software/dumber3/Application/moteurs.h        |   4 +-
 software/dumber3/Core/Src/stm32l0xx_hal_msp.c |   2 +-
 4 files changed, 257 insertions(+), 144 deletions(-)

diff --git a/software/dumber3/.cproject b/software/dumber3/.cproject
index de7a132..59bb15e 100644
--- a/software/dumber3/.cproject
+++ b/software/dumber3/.cproject
@@ -327,16 +327,15 @@
 							<outputType id="org.eclipse.cdt.managedbuilder.ui.rcbs.outputtype.1151842665.1610208999" name="Resource Custom Build Step Output Type"/>
 						</tool>
 					</fileInfo>
-					<fileInfo id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.1504381080.727722516.1512822849" name="moteurs.c" rcbsApplicability="disable" resourcePath="Application/moteurs.c" toolsToInvoke="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.1017015985.772184159">
-						<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.1017015985.772184159" name="MCU GCC Compiler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.1017015985">
-							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.1561927129" name="Optimization level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.optimization.level.value.o0" valueType="enumerated"/>
-							<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.786982597" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c"/>
+					<fileInfo id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.1504381080.727722516.584766423" name="moteurs.c" rcbsApplicability="disable" resourcePath="Application/moteurs.c" toolsToInvoke="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.1017015985.1839636316">
+						<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.1017015985.1839636316" name="MCU GCC Compiler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.1017015985">
+							<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.422540263" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c"/>
 						</tool>
-						<tool customBuildStep="true" id="org.eclipse.cdt.managedbuilder.ui.rcbs.1151549896" name="Resource Custom Build Step">
-							<inputType id="org.eclipse.cdt.managedbuilder.ui.rcbs.inputtype.1295176218" name="Resource Custom Build Step Input Type">
-								<additionalInput kind="additionalinputdependency" paths=""/>
+						<tool customBuildStep="true" id="org.eclipse.cdt.managedbuilder.ui.rcbs.1811836732" name="Resource Custom Build Step">
+							<inputType id="org.eclipse.cdt.managedbuilder.ui.rcbs.inputtype.1316072342" name="Resource Custom Build Step Input Type">
+								<additionalInput kind="additionalinputdependency"/>
 							</inputType>
-							<outputType id="org.eclipse.cdt.managedbuilder.ui.rcbs.outputtype.1189979175" name="Resource Custom Build Step Output Type"/>
+							<outputType id="org.eclipse.cdt.managedbuilder.ui.rcbs.outputtype.861774528" name="Resource Custom Build Step Output Type"/>
 						</tool>
 					</fileInfo>
 					<fileInfo id="com.st.stm32cube.ide.mcu.gnu.managedbuild.config.exe.debug.1504381080.727722516.Application/application.c" name="application.c" rcbsApplicability="disable" resourcePath="Application/application.c" toolsToInvoke="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.983087978">
diff --git a/software/dumber3/Application/moteurs.c b/software/dumber3/Application/moteurs.c
index 72c7e66..11afe78 100644
--- a/software/dumber3/Application/moteurs.c
+++ b/software/dumber3/Application/moteurs.c
@@ -40,12 +40,12 @@ typedef struct {
 	uint8_t type;
 	int16_t commande;
 	int16_t consigne;
-	uint32_t distance;
-	uint32_t tours;
+	int32_t distance;
+	int32_t tours;
 } MOTEURS_EtatDiff;
 
-MOTEURS_EtatMoteur MOTEURS_EtatMoteurGauche, MOTEURS_EtatMoteurDroit = {0};
-MOTEURS_EtatDiff MOTEURS_EtatDifferentiel = {0};
+MOTEURS_EtatMoteur MOTEURS_EtatMoteurGauche, MOTEURS_EtatMoteurDroit = { 0 };
+MOTEURS_EtatDiff MOTEURS_EtatDifferentiel = { 0 };
 
 #define MOTEUR_Kp 		300
 
@@ -53,15 +53,15 @@ MOTEURS_EtatDiff MOTEURS_EtatDifferentiel = {0};
 
 /* Tache moteurs (gestion des messages) */
 StaticTask_t xTaskMoteurs;
-StackType_t xStackMoteurs[ STACK_SIZE ];
+StackType_t xStackMoteurs[STACK_SIZE];
 TaskHandle_t xHandleMoteurs = NULL;
-void MOTEURS_TachePrincipale(void* params);
+void MOTEURS_TachePrincipale(void *params);
 
 /* Tache moteurs périodique (asservissement) */
 StaticTask_t xTaskMoteursAsservissement;
-StackType_t xStackMoteursAsservissement[ STACK_SIZE ];
+StackType_t xStackMoteursAsservissement[STACK_SIZE];
 TaskHandle_t xHandleMoteursAsservissement = NULL;
-void MOTEURS_TacheAsservissement( void* params ) ;
+void MOTEURS_TacheAsservissement(void *params);
 
 /* Fonctions diverses */
 void MOTEURS_Set(int16_t cmdGauche, int16_t cmdDroit);
@@ -69,6 +69,53 @@ void MOTEURS_DesactiveAlim(void);
 void MOTEURS_ActiveAlim(void);
 int16_t MOTEURS_CorrectionEncodeur(MOTEURS_EtatMoteur etat);
 
+#ifdef TESTS
+TIM_HandleTypeDef htim7;
+
+volatile uint32_t DEBUG_startTime = 0;
+volatile uint32_t DEBUG_endTime = 0;
+volatile uint32_t DEBUG_duration = 0;
+volatile uint32_t DEBUG_worstCase = 0;
+
+void Init_Systick(void) {
+//	TIM_ClockConfigTypeDef sClockSourceConfig = { 0 };
+//
+//	__HAL_RCC_TIM7_CLK_ENABLE();
+//
+//	htim7.Instance = TIM2;
+//	htim7.Init.Prescaler = 0;
+//	htim7.Init.CounterMode = TIM_COUNTERMODE_UP;
+//	htim7.Init.Period = 65535;
+//	htim7.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+//	htim7.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+//	if (HAL_TIM_Base_Init(&htim7) != HAL_OK) {
+//		Error_Handler();
+//	}
+//
+//	sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+//	if (HAL_TIM_ConfigClockSource(&htim7, &sClockSourceConfig) != HAL_OK) {
+//		Error_Handler();
+//	}
+//
+//	LL_TIM_EnableCounter(TIM7);
+}
+
+void StartMeasure(void) {
+//	DEBUG_startTime = LL_TIM_GetCounter(TIM7);
+}
+
+void EndMeasure(void) {
+//	DEBUG_endTime = LL_TIM_GetCounter(TIM7);
+//
+//	if (DEBUG_startTime >= DEBUG_endTime)
+//		DEBUG_duration = 65533 - DEBUG_startTime + DEBUG_endTime;
+//	else
+//		DEBUG_duration = DEBUG_endTime - DEBUG_startTime;
+//
+//	if (DEBUG_duration > DEBUG_worstCase)
+//		DEBUG_worstCase = DEBUG_duration;
+}
+#endif /* TESTS */
 /**
  * @brief Fonction d'initialisation des moteurs
  *
@@ -78,55 +125,63 @@ void MOTEURS_Init(void) {
 	MOTEURS_DesactiveAlim();
 
 	/* Create the task without using any dynamic memory allocation. */
-	xHandleMoteurs = xTaskCreateStatic(
-			MOTEURS_TachePrincipale,       /* Function that implements the task. */
-			"MOTEURS Principale",          /* Text name for the task. */
-			STACK_SIZE,      /* Number of indexes in the xStack array. */
-			NULL,    /* Parameter passed into the task. */
+	xHandleMoteurs = xTaskCreateStatic(MOTEURS_TachePrincipale, /* Function that implements the task. */
+			"MOTEURS Principale", /* Text name for the task. */
+			STACK_SIZE, /* Number of indexes in the xStack array. */
+			NULL, /* Parameter passed into the task. */
 			PriorityMoteursHandler,/* Priority at which the task is created. */
-			xStackMoteurs,          /* Array to use as the task's stack. */
-			&xTaskMoteurs);  /* Variable to hold the task's data structure. */
+			xStackMoteurs, /* Array to use as the task's stack. */
+			&xTaskMoteurs); /* Variable to hold the task's data structure. */
 	vTaskResume(xHandleMoteurs);
 
 	/* Create the task without using any dynamic memory allocation. */
 	xHandleMoteursAsservissement = xTaskCreateStatic(
-			MOTEURS_TacheAsservissement,       /* Function that implements the task. */
-			"MOTEURS Asservissement",          /* Text name for the task. */
-			STACK_SIZE,      /* Number of indexes in the xStack array. */
-			NULL,    /* Parameter passed into the task. */
+			MOTEURS_TacheAsservissement, /* Function that implements the task. */
+			"MOTEURS Asservissement", /* Text name for the task. */
+			STACK_SIZE, /* Number of indexes in the xStack array. */
+			NULL, /* Parameter passed into the task. */
 			PriorityMoteursAsservissement,/* Priority at which the task is created. */
-			xStackMoteursAsservissement,          /* Array to use as the task's stack. */
-			&xTaskMoteursAsservissement);  /* Variable to hold the task's data structure. */
+			xStackMoteursAsservissement, /* Array to use as the task's stack. */
+			&xTaskMoteursAsservissement); /* Variable to hold the task's data structure. */
 	vTaskSuspend(xHandleMoteursAsservissement); // On ne lance la tache d'asservissement que lorsque'une commande moteur arrive
 
 	MOTEURS_DesactiveAlim();
+
+#ifdef TESTS
+	Init_Systick();
+#endif /* TESTS */
 }
 
-void MOTEURS_Avance(uint32_t distance) {
-	static uint32_t dist;
+void MOTEURS_Avance(int32_t distance) {
+	static int32_t dist;
 
-	if (distance) {
-		dist = distance;
+	dist = distance*15;
+
+	if (dist) {
 		MOTEURS_ActiveAlim();
-		MESSAGE_SendMailbox(MOTEURS_Mailbox, MSG_ID_MOTEURS_MOVE, APPLICATION_Mailbox, (void*)&dist);
+		MESSAGE_SendMailbox(MOTEURS_Mailbox, MSG_ID_MOTEURS_MOVE,
+				APPLICATION_Mailbox, (void*) &dist);
 	} else
 		MOTEURS_Stop();
 }
 
-void MOTEURS_Tourne(uint32_t tours) {
-	static uint32_t turns;
+void MOTEURS_Tourne(int32_t tours) {
+	static int32_t turns;
 
-	if (tours) {
-		turns = tours;
+	turns = tours;
+
+	if (turns) {
 		MOTEURS_ActiveAlim();
-		MESSAGE_SendMailbox(MOTEURS_Mailbox, MSG_ID_MOTEURS_TURN, APPLICATION_Mailbox, (void*)&turns);
+		MESSAGE_SendMailbox(MOTEURS_Mailbox, MSG_ID_MOTEURS_TURN,
+				APPLICATION_Mailbox, (void*) &turns);
 	} else
 		MOTEURS_Stop();
 }
 
 void MOTEURS_Stop(void) {
 	MOTEURS_DesactiveAlim();
-	MESSAGE_SendMailbox(MOTEURS_Mailbox, MSG_ID_MOTEURS_STOP, APPLICATION_Mailbox, (void*)NULL);
+	MESSAGE_SendMailbox(MOTEURS_Mailbox, MSG_ID_MOTEURS_STOP,
+			APPLICATION_Mailbox, (void*) NULL);
 }
 
 /*
@@ -134,43 +189,54 @@ void MOTEURS_Stop(void) {
  * 		  Gestion de la boite aux lettres moteurs, et supervision generale
  * @params params non utilisé
  */
-void MOTEURS_TachePrincipale(void* params) {
+void MOTEURS_TachePrincipale(void *params) {
 	MESSAGE_Typedef msg;
-	uint32_t distance, tours;
+	int32_t distance, tours;
 
 	while (1) {
 		msg = MESSAGE_ReadMailbox(MOTEURS_Mailbox);
 
 		switch (msg.id) {
 		case MSG_ID_MOTEURS_MOVE:
-			distance = *((uint32_t*)msg.data);
+			distance = *((int32_t*) msg.data);
 			MOTEURS_EtatDifferentiel.distance = distance;
 			MOTEURS_EtatDifferentiel.tours = 0;
 
-			MOTEURS_EtatMoteurGauche.consigne=50;
-			MOTEURS_EtatMoteurDroit.consigne=50;
+			if (distance > 0) {
+				MOTEURS_EtatMoteurGauche.consigne = 50;
+				MOTEURS_EtatMoteurDroit.consigne = 50;
+			} else {
+				MOTEURS_EtatMoteurGauche.consigne = -50;
+				MOTEURS_EtatMoteurDroit.consigne = -50;
+			}
 
 			vTaskResume(xHandleMoteursAsservissement);
 			break;
+
 		case MSG_ID_MOTEURS_TURN:
-			tours = *((uint32_t*)msg.data);
+			tours = *((int32_t*) msg.data);
 			MOTEURS_EtatDifferentiel.distance = 0;
 			MOTEURS_EtatDifferentiel.tours = tours;
 
-			MOTEURS_EtatMoteurGauche.consigne=50;
-			MOTEURS_EtatMoteurDroit.consigne=50;
+			if (tours > 0) {
+				MOTEURS_EtatMoteurGauche.consigne = -50;
+				MOTEURS_EtatMoteurDroit.consigne = 50;
+			} else {
+				MOTEURS_EtatMoteurGauche.consigne = 50;
+				MOTEURS_EtatMoteurDroit.consigne = -50;
+			}
 
 			vTaskResume(xHandleMoteursAsservissement);
 			break;
-		case MSG_ID_MOTEURS_STOP:
 
+		case MSG_ID_MOTEURS_STOP:
 			MOTEURS_EtatDifferentiel.distance = 0;
 			MOTEURS_EtatDifferentiel.tours = 0;
 
-			MOTEURS_EtatMoteurGauche.consigne=0;
-			MOTEURS_EtatMoteurDroit.consigne=0;
-			if ((MOTEURS_CorrectionEncodeur(MOTEURS_EtatMoteurGauche) ==0) &&
-					(MOTEURS_CorrectionEncodeur(MOTEURS_EtatMoteurDroit) ==0))
+			MOTEURS_EtatMoteurGauche.consigne = 0;
+			MOTEURS_EtatMoteurDroit.consigne = 0;
+			if ((MOTEURS_CorrectionEncodeur(MOTEURS_EtatMoteurGauche) == 0)
+					&& (MOTEURS_CorrectionEncodeur(MOTEURS_EtatMoteurDroit) == 0))
 				// Les moteurs sont déjà arrêtés
 				vTaskSuspend(xHandleMoteursAsservissement);
 			else
@@ -188,9 +254,9 @@ void MOTEURS_TachePrincipale(void* params) {
  *
  * @params params non utilisé
  */
-void MOTEURS_TacheAsservissement( void* params ) {
+void MOTEURS_TacheAsservissement(void *params) {
 	TickType_t xLastWakeTime;
-	int16_t erreurG, erreurD =0;
+	int16_t erreurG, erreurD = 0;
 	int16_t encodeurGauche, encodeurDroit;
 	int32_t locCmdG, locCmdD;
 
@@ -199,7 +265,12 @@ void MOTEURS_TacheAsservissement( void* params ) {
 
 	for (;;) {
 		// Wait for the next cycle.
-		vTaskDelayUntil( &xLastWakeTime, pdMS_TO_TICKS(MOTEURS_PERIODE_ASSERVISSEMENT));
+		vTaskDelayUntil(&xLastWakeTime,
+				pdMS_TO_TICKS(MOTEURS_PERIODE_ASSERVISSEMENT));
+
+#ifdef TESTS
+		//StartMeasure();
+#endif /* TESTS */
 
 		encodeurGauche = MOTEURS_CorrectionEncodeur(MOTEURS_EtatMoteurGauche);
 		encodeurDroit = MOTEURS_CorrectionEncodeur(MOTEURS_EtatMoteurDroit);
@@ -213,53 +284,71 @@ void MOTEURS_TacheAsservissement( void* params ) {
 		erreurG = MOTEURS_EtatMoteurGauche.consigne - encodeurGauche;
 		erreurD = MOTEURS_EtatMoteurDroit.consigne - encodeurDroit;
 
-		if (((MOTEURS_EtatMoteurDroit.consigne ==0) && (MOTEURS_EtatMoteurGauche.consigne ==0)) &&
-				((erreurD==0) && (erreurG==0))) {
+		if (((MOTEURS_EtatMoteurDroit.consigne == 0)
+				&& (MOTEURS_EtatMoteurGauche.consigne == 0))
+				&& ((erreurD == 0) && (erreurG == 0))) {
 
 			MOTEURS_DesactiveAlim();
 			vTaskSuspend(xHandleMoteursAsservissement);
 		}
 
-		if (MOTEURS_EtatMoteurGauche.consigne ==0)
-			MOTEURS_EtatMoteurGauche.commande =0;
+		if (MOTEURS_EtatMoteurGauche.consigne == 0)
+			MOTEURS_EtatMoteurGauche.commande = 0;
 		else {
-			if (erreurG !=0) {
+			if (erreurG != 0) {
 				//locCmdG = (int32_t)MOTEURS_EtatMoteurGauche.commande + ((int32_t)MOTEUR_Kp*(int32_t)erreurG)/100;
-				locCmdG = ((int32_t)MOTEUR_Kp*(int32_t)erreurG)/100;
+				locCmdG = ((int32_t) MOTEUR_Kp * (int32_t) erreurG) / 100;
 
-				if (MOTEURS_EtatMoteurGauche.consigne>=0) {
-					if (locCmdG<0) MOTEURS_EtatMoteurGauche.commande=0;
-					else if (locCmdG>SHRT_MAX) MOTEURS_EtatMoteurGauche.commande=SHRT_MAX;
-					else MOTEURS_EtatMoteurGauche.commande=(int16_t)locCmdG;
+				if (MOTEURS_EtatMoteurGauche.consigne >= 0) {
+					if (locCmdG < 0)
+						MOTEURS_EtatMoteurGauche.commande = 0;
+					else if (locCmdG > SHRT_MAX)
+						MOTEURS_EtatMoteurGauche.commande = SHRT_MAX;
+					else
+						MOTEURS_EtatMoteurGauche.commande = (int16_t) locCmdG;
 				} else {
-					if (locCmdG>0) MOTEURS_EtatMoteurGauche.commande=0;
-					else if (locCmdG<SHRT_MIN) MOTEURS_EtatMoteurGauche.commande=SHRT_MIN;
-					else MOTEURS_EtatMoteurGauche.commande=(int16_t)locCmdG;
+					if (locCmdG > 0)
+						MOTEURS_EtatMoteurGauche.commande = 0;
+					else if (locCmdG < SHRT_MIN)
+						MOTEURS_EtatMoteurGauche.commande = SHRT_MIN;
+					else
+						MOTEURS_EtatMoteurGauche.commande = (int16_t) locCmdG;
 				}
 			}
 		}
 
-		if (MOTEURS_EtatMoteurDroit.consigne ==0)
-			MOTEURS_EtatMoteurDroit.commande =0;
+		if (MOTEURS_EtatMoteurDroit.consigne == 0)
+			MOTEURS_EtatMoteurDroit.commande = 0;
 		else {
-			if (erreurD !=0) {
+			if (erreurD != 0) {
 				//locCmdD = (int32_t)MOTEURS_EtatMoteurDroit.commande + ((int32_t)MOTEUR_Kp*(int32_t)erreurD)/100;
-				locCmdD = ((int32_t)MOTEUR_Kp*(int32_t)erreurD)/100;
+				locCmdD = ((int32_t) MOTEUR_Kp * (int32_t) erreurD) / 100;
 
-				if (MOTEURS_EtatMoteurDroit.consigne>=0) {
-					if (locCmdD<0) MOTEURS_EtatMoteurDroit.commande=0;
-					else if (locCmdD>SHRT_MAX) MOTEURS_EtatMoteurDroit.commande=SHRT_MAX;
-					else MOTEURS_EtatMoteurDroit.commande=(int16_t)locCmdD;
+				if (MOTEURS_EtatMoteurDroit.consigne >= 0) {
+					if (locCmdD < 0)
+						MOTEURS_EtatMoteurDroit.commande = 0;
+					else if (locCmdD > SHRT_MAX)
+						MOTEURS_EtatMoteurDroit.commande = SHRT_MAX;
+					else
+						MOTEURS_EtatMoteurDroit.commande = (int16_t) locCmdD;
 				} else {
-					if (locCmdD>0) MOTEURS_EtatMoteurDroit.commande=0;
-					else if (locCmdD<SHRT_MIN) MOTEURS_EtatMoteurDroit.commande=SHRT_MIN;
-					else MOTEURS_EtatMoteurDroit.commande=(int16_t)locCmdD;
+					if (locCmdD > 0)
+						MOTEURS_EtatMoteurDroit.commande = 0;
+					else if (locCmdD < SHRT_MIN)
+						MOTEURS_EtatMoteurDroit.commande = SHRT_MIN;
+					else
+						MOTEURS_EtatMoteurDroit.commande = (int16_t) locCmdD;
 				}
 			}
 		}
 
 		/* Finalement, on applique les commandes aux moteurs */
-		MOTEURS_Set(MOTEURS_EtatMoteurGauche.commande, MOTEURS_EtatMoteurDroit.commande);
+		MOTEURS_Set(MOTEURS_EtatMoteurGauche.commande,
+				MOTEURS_EtatMoteurDroit.commande);
+
+#ifdef TESTS
+		//EndMeasure();
+#endif /* TESTS */
 	}
 }
 
@@ -270,24 +359,13 @@ typedef struct {
 
 #define MOTEURS_MAX_CORRECTION_POINTS 16
 
-const MOTEURS_CorrectionPoint MOTEURS_CorrectionPoints[MOTEURS_MAX_CORRECTION_POINTS]=
-{
-		{MOTEURS_MAX_ENCODEUR-1, 1},
-		{42000,   100},
-		{22000,  2500},
-		{18000,  5000},
-		{16500,  7500},
-		{15500, 10000},
-		{14500, 12500},
-		{13000, 15000},
-		{12500, 17500},
-		{12200, 20000},
-		{11500, 22500},
-		{11100, 25000},
-		{11000, 27500},
-		{10900, 29000},
-		{10850, 30500},
-		{10800, SHRT_MAX} // 32767
+const MOTEURS_CorrectionPoint MOTEURS_CorrectionPoints[MOTEURS_MAX_CORRECTION_POINTS] =
+{ { MOTEURS_MAX_ENCODEUR - 1, 1 }, { 42000, 100 }, { 22000, 2500 }, {
+		18000, 5000 }, { 16500, 7500 }, { 15500, 10000 },
+		{ 14500, 12500 }, { 13000, 15000 }, { 12500, 17500 }, { 12200,
+				20000 }, { 11500, 22500 }, { 11100, 25000 }, { 11000,
+						27500 }, { 10900, 29000 }, { 10850, 30500 }, { 10800,
+								SHRT_MAX } // 32767
 };
 
 /*
@@ -297,16 +375,17 @@ const MOTEURS_CorrectionPoint MOTEURS_CorrectionPoints[MOTEURS_MAX_CORRECTION_PO
  * @return valeur linéarisée (entre -32768 et 32767)
  */
 int16_t MOTEURS_CorrectionEncodeur(MOTEURS_EtatMoteur etat) {
-	int16_t correction=0;
-	uint8_t index=0;
-	uint32_t A,B,C;
+	int16_t correction = 0;
+	uint8_t index = 0;
+	uint32_t A, B, C;
 	uint16_t encodeur = etat.encodeur;
 
-	if (encodeur ==MOTEURS_MAX_ENCODEUR)
-		correction =0;
+	if (encodeur == MOTEURS_MAX_ENCODEUR)
+		correction = 0;
 	else { // recherche par dichotomie de l'intervale
-		while (index <MOTEURS_MAX_CORRECTION_POINTS) {
-			if ((MOTEURS_CorrectionPoints[index].encodeur>=encodeur) && (MOTEURS_CorrectionPoints[index+1].encodeur<encodeur)) {
+		while (index < MOTEURS_MAX_CORRECTION_POINTS) {
+			if ((MOTEURS_CorrectionPoints[index].encodeur >= encodeur)
+					&& (MOTEURS_CorrectionPoints[index + 1].encodeur < encodeur)) {
 				// valeur trouvée, on sort
 				break;
 			} else
@@ -316,18 +395,22 @@ int16_t MOTEURS_CorrectionEncodeur(MOTEURS_EtatMoteur etat) {
 		if (index >= MOTEURS_MAX_CORRECTION_POINTS)
 			correction = SHRT_MAX;
 		else {
-			A = encodeur-MOTEURS_CorrectionPoints[index+1].encodeur;
-			B = MOTEURS_CorrectionPoints[index+1].correction-MOTEURS_CorrectionPoints[index].correction;
-			C = MOTEURS_CorrectionPoints[index].encodeur-MOTEURS_CorrectionPoints[index+1].encodeur;
+			A = encodeur - MOTEURS_CorrectionPoints[index + 1].encodeur;
+			B = MOTEURS_CorrectionPoints[index + 1].correction
+					- MOTEURS_CorrectionPoints[index].correction;
+			C = MOTEURS_CorrectionPoints[index].encodeur
+					- MOTEURS_CorrectionPoints[index + 1].encodeur;
 
-			correction = (int16_t)(MOTEURS_CorrectionPoints[index+1].correction - (uint16_t)((A*B)/C));
+			correction =
+					(int16_t) (MOTEURS_CorrectionPoints[index + 1].correction
+							- (uint16_t) ((A * B) / C));
 		}
 	}
 
 	/*
 	 * Selon le sens de rotation du moteur (commande > 0 ou < 0), on corrige le signe du capteur
 	 */
-	if (etat.consigne<0)
+	if (etat.consigne < 0)
 		correction = -correction;
 
 	return correction;
@@ -341,10 +424,12 @@ void MOTEURS_DesactiveAlim(void) {
 	LL_TIM_DisableCounter(TIM2);
 	LL_TIM_DisableCounter(TIM21);
 
-	LL_TIM_CC_DisableChannel(TIM3, LL_TIM_CHANNEL_CH1|LL_TIM_CHANNEL_CH2|LL_TIM_CHANNEL_CH3|LL_TIM_CHANNEL_CH4);
+	LL_TIM_CC_DisableChannel(TIM3,
+			LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH2 | LL_TIM_CHANNEL_CH3
+			| LL_TIM_CHANNEL_CH4);
 
-	LL_TIM_CC_DisableChannel(TIM2, LL_TIM_CHANNEL_CH1|LL_TIM_CHANNEL_CH2);
-	LL_TIM_CC_DisableChannel(TIM21, LL_TIM_CHANNEL_CH1|LL_TIM_CHANNEL_CH2);
+	LL_TIM_CC_DisableChannel(TIM2, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH2);
+	LL_TIM_CC_DisableChannel(TIM21, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH2);
 
 	LL_TIM_DisableIT_CC1(TIM2);
 	LL_TIM_DisableIT_CC1(TIM21);
@@ -363,10 +448,12 @@ void MOTEURS_ActiveAlim(void) {
 	LL_TIM_EnableCounter(TIM2);
 	LL_TIM_EnableCounter(TIM21);
 
-	LL_TIM_CC_EnableChannel(TIM3, LL_TIM_CHANNEL_CH1|LL_TIM_CHANNEL_CH2|LL_TIM_CHANNEL_CH3|LL_TIM_CHANNEL_CH4);
+	LL_TIM_CC_EnableChannel(TIM3,
+			LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH2 | LL_TIM_CHANNEL_CH3
+			| LL_TIM_CHANNEL_CH4);
 
-	LL_TIM_CC_EnableChannel(TIM2, LL_TIM_CHANNEL_CH1|LL_TIM_CHANNEL_CH2);
-	LL_TIM_CC_EnableChannel(TIM21, LL_TIM_CHANNEL_CH1|LL_TIM_CHANNEL_CH2);
+	LL_TIM_CC_EnableChannel(TIM2, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH2);
+	LL_TIM_CC_EnableChannel(TIM21, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH2);
 
 	LL_TIM_EnableIT_CC1(TIM2);
 	LL_TIM_EnableIT_CC1(TIM21);
@@ -389,28 +476,30 @@ void MOTEURS_ActiveAlim(void) {
 void MOTEURS_Set(int16_t cmdGauche, int16_t cmdDroit) {
 	int32_t locValGauche, locValDroit;
 
-	locValGauche = (int32_t)(((int32_t)cmdGauche * (int32_t)SHRT_MAX)/((int32_t)MOTEURS_MAX_COMMANDE));
-	locValDroit = (int32_t)(((int32_t)cmdDroit * (int32_t)SHRT_MAX)/((int32_t)MOTEURS_MAX_COMMANDE));
+	locValGauche = (int32_t) (((int32_t) cmdGauche * (int32_t) SHRT_MAX)
+			/ ((int32_t) MOTEURS_MAX_COMMANDE));
+	locValDroit = (int32_t) (((int32_t) cmdDroit * (int32_t) SHRT_MAX)
+			/ ((int32_t) MOTEURS_MAX_COMMANDE));
 
-	if (LL_GPIO_IsOutputPinSet(GPIOB, SHUTDOWN_5V_Pin)==GPIO_PIN_RESET)
+	if (LL_GPIO_IsOutputPinSet(GPIOB, SHUTDOWN_5V_Pin) == GPIO_PIN_RESET)
 		MOTEURS_ActiveAlim();
 
 	// Moteur droit
-	if (cmdDroit >=0) {
-		LL_TIM_OC_SetCompareCH2(TIM3, (uint32_t)locValDroit);
-		LL_TIM_OC_SetCompareCH1(TIM3, (uint32_t)0);
+	if (cmdDroit >= 0) {
+		LL_TIM_OC_SetCompareCH2(TIM3, (uint32_t) locValDroit);
+		LL_TIM_OC_SetCompareCH1(TIM3, (uint32_t) 0);
 	} else {
-		LL_TIM_OC_SetCompareCH2(TIM3, (uint32_t)0);
-		LL_TIM_OC_SetCompareCH1(TIM3, (uint32_t)locValDroit);
+		LL_TIM_OC_SetCompareCH2(TIM3, (uint32_t) 0);
+		LL_TIM_OC_SetCompareCH1(TIM3, (uint32_t) locValDroit);
 	}
 
 	// Moteur gauche
-	if (cmdGauche >=0) {
-		LL_TIM_OC_SetCompareCH4(TIM3, (uint32_t)locValGauche);
-		LL_TIM_OC_SetCompareCH3(TIM3, (uint32_t)0);
+	if (cmdGauche >= 0) {
+		LL_TIM_OC_SetCompareCH4(TIM3, (uint32_t) locValGauche);
+		LL_TIM_OC_SetCompareCH3(TIM3, (uint32_t) 0);
 	} else {
-		LL_TIM_OC_SetCompareCH4(TIM3, (uint32_t)0);
-		LL_TIM_OC_SetCompareCH3(TIM3, (uint32_t)locValGauche);
+		LL_TIM_OC_SetCompareCH4(TIM3, (uint32_t) 0);
+		LL_TIM_OC_SetCompareCH3(TIM3, (uint32_t) locValGauche);
 	}
 }
 
@@ -420,29 +509,54 @@ void MOTEURS_Set(int16_t cmdGauche, int16_t cmdDroit) {
  * @param htim pointeur sur la reference du timer qui generé l'interruption
  */
 void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) {
-	if (htim->Instance==TIM21) { /* moteur gauche */
+	if (htim->Instance == TIM21) { /* moteur gauche */
 		if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1) {
-			if (MOTEURS_EtatMoteurGauche.moteurLent !=0) {
+			if (MOTEURS_EtatMoteurGauche.moteurLent != 0) {
 				MOTEURS_EtatMoteurGauche.encodeur = MOTEURS_MAX_ENCODEUR;
 				MOTEURS_EtatMoteurGauche.encodeurFront = MOTEURS_MAX_ENCODEUR;
 			} else {
-				MOTEURS_EtatMoteurGauche.encodeur = (uint16_t)LL_TIM_IC_GetCaptureCH1(TIM21);
-				MOTEURS_EtatMoteurGauche.encodeurFront = (uint16_t)LL_TIM_IC_GetCaptureCH2(TIM21);
+				MOTEURS_EtatMoteurGauche.encodeur =
+						(uint16_t) LL_TIM_IC_GetCaptureCH1(TIM21);
+				MOTEURS_EtatMoteurGauche.encodeurFront =
+						(uint16_t) LL_TIM_IC_GetCaptureCH2(TIM21);
 			}
 
 			if (LL_TIM_IsActiveFlag_UPDATE(TIM21))
 				LL_TIM_ClearFlag_UPDATE(TIM21);
 
 			MOTEURS_EtatMoteurGauche.moteurLent = 0;
+
+			if (MOTEURS_EtatDifferentiel.distance) {
+				if (MOTEURS_EtatDifferentiel.distance>0) MOTEURS_EtatDifferentiel.distance--;
+				else MOTEURS_EtatDifferentiel.distance++;
+
+				if (MOTEURS_EtatDifferentiel.distance==0) {
+					MOTEURS_EtatMoteurGauche.consigne=0;
+					MOTEURS_EtatMoteurDroit.consigne=0;
+				}
+			}
+
+			if (MOTEURS_EtatDifferentiel.tours) {
+				if (MOTEURS_EtatDifferentiel.tours>0) MOTEURS_EtatDifferentiel.tours--;
+				else MOTEURS_EtatDifferentiel.tours++;
+
+				if (MOTEURS_EtatDifferentiel.tours==0) {
+					MOTEURS_EtatMoteurGauche.consigne=0;
+					MOTEURS_EtatMoteurDroit.consigne=0;
+				}
+			}
+
 		}
 	} else { /* moteur droit */
 		if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1) {
-			if (MOTEURS_EtatMoteurDroit.moteurLent !=0) {
+			if (MOTEURS_EtatMoteurDroit.moteurLent != 0) {
 				MOTEURS_EtatMoteurDroit.encodeur = MOTEURS_MAX_ENCODEUR;
 				MOTEURS_EtatMoteurDroit.encodeurFront = MOTEURS_MAX_ENCODEUR;
 			} else {
-				MOTEURS_EtatMoteurDroit.encodeur = (uint16_t)LL_TIM_IC_GetCaptureCH1(TIM2);
-				MOTEURS_EtatMoteurDroit.encodeurFront = (uint16_t)LL_TIM_IC_GetCaptureCH2(TIM2);
+				MOTEURS_EtatMoteurDroit.encodeur =
+						(uint16_t) LL_TIM_IC_GetCaptureCH1(TIM2);
+				MOTEURS_EtatMoteurDroit.encodeurFront =
+						(uint16_t) LL_TIM_IC_GetCaptureCH2(TIM2);
 			}
 
 			if (LL_TIM_IsActiveFlag_UPDATE(TIM2))
@@ -461,14 +575,14 @@ void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) {
  *
  * @param htim pointeur sur la reference du timer qui generé l'interruption
  */
-void MOTEURS_TimerEncodeurUpdate (TIM_HandleTypeDef *htim) {
-	if (htim->Instance==TIM21) { /* moteur gauche */
-		if ((MOTEURS_EtatMoteurGauche.moteurLent++) >=1) {
+void MOTEURS_TimerEncodeurUpdate(TIM_HandleTypeDef *htim) {
+	if (htim->Instance == TIM21) { /* moteur gauche */
+		if ((MOTEURS_EtatMoteurGauche.moteurLent++) >= 1) {
 			MOTEURS_EtatMoteurGauche.encodeur = MOTEURS_MAX_ENCODEUR;
 			MOTEURS_EtatMoteurGauche.moteurLent = 1;
 		}
 	} else { /* moteur droit */
-		if ((MOTEURS_EtatMoteurDroit.moteurLent++) >=1) {
+		if ((MOTEURS_EtatMoteurDroit.moteurLent++) >= 1) {
 			MOTEURS_EtatMoteurDroit.encodeur = MOTEURS_MAX_ENCODEUR;
 			MOTEURS_EtatMoteurDroit.moteurLent = 1;
 		}
diff --git a/software/dumber3/Application/moteurs.h b/software/dumber3/Application/moteurs.h
index d4724e7..f14120d 100644
--- a/software/dumber3/Application/moteurs.h
+++ b/software/dumber3/Application/moteurs.h
@@ -13,8 +13,8 @@
 void MOTEURS_Init(void);
 //void MOTEURS_Set(uint8_t mot, int16_t val);
 //void MOTEURS_Test (void);
-void MOTEURS_Avance(uint32_t distance);
-void MOTEURS_Tourne(uint32_t tours);
+void MOTEURS_Avance(int32_t distance);
+void MOTEURS_Tourne(int32_t tours);
 void MOTEURS_Stop(void);
 
 void MOTEURS_TimerEncodeurUpdate (TIM_HandleTypeDef *htim);
diff --git a/software/dumber3/Core/Src/stm32l0xx_hal_msp.c b/software/dumber3/Core/Src/stm32l0xx_hal_msp.c
index b72334d..76649e4 100644
--- a/software/dumber3/Core/Src/stm32l0xx_hal_msp.c
+++ b/software/dumber3/Core/Src/stm32l0xx_hal_msp.c
@@ -181,7 +181,7 @@ void HAL_UART_MspInit(UART_HandleTypeDef* huart)
     HAL_NVIC_SetPriority(LPUART1_IRQn, 3, 0);
     HAL_NVIC_EnableIRQ(LPUART1_IRQn);
   /* USER CODE BEGIN LPUART1_MspInit 1 */
-    //HAL_NVIC_SetPriority(LPUART1_IRQn, 2, 0);
+    HAL_NVIC_SetPriority(LPUART1_IRQn, 2, 0);
   /* USER CODE END LPUART1_MspInit 1 */
   }