INSA/dumber
1
0
Fork 0
mirror of https://github.com/yoboujon/dumber.git synced 2025-06-08 13:50:49 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Fonction moteur marche a peu pres correctement. Asservissement a revoir cependant

Browse source
This commit is contained in:
dimercur 2023-10-10 16:03:25 +02:00
parent 149a56fbac
commit 7d9e38f3ef
4 changed files with 257 additions and 144 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

15
software/dumber3/.cproject
View file

@ -327,16 +327,15 @@
<outputType id="org.eclipse.cdt.managedbuilder.ui.rcbs.outputtype.1151842665.1610208999" name="Resource Custom Build Step Output Type"/> <outputType id="org.eclipse.cdt.managedbuilder.ui.rcbs.outputtype.1151842665.1610208999" name="Resource Custom Build Step Output Type"/>
</tool> </tool>
</fileInfo> </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"> <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.772184159" name="MCU GCC Compiler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.1017015985"> <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">
<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.422540263" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c"/>
<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"/>
</tool> </tool>
<tool customBuildStep="true" id="org.eclipse.cdt.managedbuilder.ui.rcbs.1151549896" name="Resource Custom Build Step"> <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.1295176218" name="Resource Custom Build Step Input Type"> <inputType id="org.eclipse.cdt.managedbuilder.ui.rcbs.inputtype.1316072342" name="Resource Custom Build Step Input Type">
<additionalInput kind="additionalinputdependency" paths=""/> <additionalInput kind="additionalinputdependency"/>
</inputType> </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> </tool>
</fileInfo> </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"> <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">

380
software/dumber3/Application/moteurs.c
View file

@ -40,12 +40,12 @@ typedef struct {
uint8_t type; uint8_t type;
int16_t commande; int16_t commande;
int16_t consigne; int16_t consigne;
uint32_t distance; int32_t distance;
uint32_t tours; int32_t tours;
} MOTEURS_EtatDiff; } MOTEURS_EtatDiff;
MOTEURS_EtatMoteur MOTEURS_EtatMoteurGauche, MOTEURS_EtatMoteurDroit = {0}; MOTEURS_EtatMoteur MOTEURS_EtatMoteurGauche, MOTEURS_EtatMoteurDroit = { 0 };
MOTEURS_EtatDiff MOTEURS_EtatDifferentiel = {0}; MOTEURS_EtatDiff MOTEURS_EtatDifferentiel = { 0 };
#define MOTEUR_Kp 300 #define MOTEUR_Kp 300
@ -53,15 +53,15 @@ MOTEURS_EtatDiff MOTEURS_EtatDifferentiel = {0};
/* Tache moteurs (gestion des messages) */ /* Tache moteurs (gestion des messages) */
StaticTask_t xTaskMoteurs; StaticTask_t xTaskMoteurs;
StackType_t xStackMoteurs[ STACK_SIZE ]; StackType_t xStackMoteurs[STACK_SIZE];
TaskHandle_t xHandleMoteurs = NULL; TaskHandle_t xHandleMoteurs = NULL;
void MOTEURS_TachePrincipale(void* params); void MOTEURS_TachePrincipale(void *params);
/* Tache moteurs périodique (asservissement) */ /* Tache moteurs périodique (asservissement) */
StaticTask_t xTaskMoteursAsservissement; StaticTask_t xTaskMoteursAsservissement;
StackType_t xStackMoteursAsservissement[ STACK_SIZE ]; StackType_t xStackMoteursAsservissement[STACK_SIZE];
TaskHandle_t xHandleMoteursAsservissement = NULL; TaskHandle_t xHandleMoteursAsservissement = NULL;
void MOTEURS_TacheAsservissement( void* params ) ; void MOTEURS_TacheAsservissement(void *params);
/* Fonctions diverses */ /* Fonctions diverses */
void MOTEURS_Set(int16_t cmdGauche, int16_t cmdDroit); void MOTEURS_Set(int16_t cmdGauche, int16_t cmdDroit);
@ -69,6 +69,53 @@ void MOTEURS_DesactiveAlim(void);
void MOTEURS_ActiveAlim(void); void MOTEURS_ActiveAlim(void);
int16_t MOTEURS_CorrectionEncodeur(MOTEURS_EtatMoteur etat); 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 * @brief Fonction d'initialisation des moteurs
* *
@ -78,55 +125,63 @@ void MOTEURS_Init(void) {
MOTEURS_DesactiveAlim(); MOTEURS_DesactiveAlim();
/* Create the task without using any dynamic memory allocation. */ /* Create the task without using any dynamic memory allocation. */
xHandleMoteurs = xTaskCreateStatic( xHandleMoteurs = xTaskCreateStatic(MOTEURS_TachePrincipale, /* Function that implements the task. */
MOTEURS_TachePrincipale, /* Function that implements the task. */ "MOTEURS Principale", /* Text name for the task. */
"MOTEURS Principale", /* Text name for the task. */ STACK_SIZE, /* Number of indexes in the xStack array. */
STACK_SIZE, /* Number of indexes in the xStack array. */ NULL, /* Parameter passed into the task. */
NULL, /* Parameter passed into the task. */
PriorityMoteursHandler,/* Priority at which the task is created. */ PriorityMoteursHandler,/* Priority at which the task is created. */
xStackMoteurs, /* Array to use as the task's stack. */ xStackMoteurs, /* Array to use as the task's stack. */
&xTaskMoteurs); /* Variable to hold the task's data structure. */ &xTaskMoteurs); /* Variable to hold the task's data structure. */
vTaskResume(xHandleMoteurs); vTaskResume(xHandleMoteurs);
/* Create the task without using any dynamic memory allocation. */ /* Create the task without using any dynamic memory allocation. */
xHandleMoteursAsservissement = xTaskCreateStatic( xHandleMoteursAsservissement = xTaskCreateStatic(
MOTEURS_TacheAsservissement, /* Function that implements the task. */ MOTEURS_TacheAsservissement, /* Function that implements the task. */
"MOTEURS Asservissement", /* Text name for the task. */ "MOTEURS Asservissement", /* Text name for the task. */
STACK_SIZE, /* Number of indexes in the xStack array. */ STACK_SIZE, /* Number of indexes in the xStack array. */
NULL, /* Parameter passed into the task. */ NULL, /* Parameter passed into the task. */
PriorityMoteursAsservissement,/* Priority at which the task is created. */ PriorityMoteursAsservissement,/* Priority at which the task is created. */
xStackMoteursAsservissement, /* Array to use as the task's stack. */ xStackMoteursAsservissement, /* Array to use as the task's stack. */
&xTaskMoteursAsservissement); /* Variable to hold the task's data structure. */ &xTaskMoteursAsservissement); /* Variable to hold the task's data structure. */
vTaskSuspend(xHandleMoteursAsservissement); // On ne lance la tache d'asservissement que lorsque'une commande moteur arrive vTaskSuspend(xHandleMoteursAsservissement); // On ne lance la tache d'asservissement que lorsque'une commande moteur arrive
MOTEURS_DesactiveAlim(); MOTEURS_DesactiveAlim();
#ifdef TESTS
Init_Systick();
#endif /* TESTS */
} }
void MOTEURS_Avance(uint32_t distance) { void MOTEURS_Avance(int32_t distance) {
static uint32_t dist; static int32_t dist;
if (distance) { dist = distance*15;
dist = distance;
if (dist) {
MOTEURS_ActiveAlim(); 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 } else
MOTEURS_Stop(); MOTEURS_Stop();
} }
void MOTEURS_Tourne(uint32_t tours) { void MOTEURS_Tourne(int32_t tours) {
static uint32_t turns; static int32_t turns;
if (tours) { turns = tours;
turns = tours;
if (turns) {
MOTEURS_ActiveAlim(); 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 } else
MOTEURS_Stop(); MOTEURS_Stop();
} }
void MOTEURS_Stop(void) { void MOTEURS_Stop(void) {
MOTEURS_DesactiveAlim(); 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 * Gestion de la boite aux lettres moteurs, et supervision generale
* @params params non utilisé * @params params non utilisé
*/ */
void MOTEURS_TachePrincipale(void* params) { void MOTEURS_TachePrincipale(void *params) {
MESSAGE_Typedef msg; MESSAGE_Typedef msg;
uint32_t distance, tours; int32_t distance, tours;
while (1) { while (1) {
msg = MESSAGE_ReadMailbox(MOTEURS_Mailbox); msg = MESSAGE_ReadMailbox(MOTEURS_Mailbox);
switch (msg.id) { switch (msg.id) {
case MSG_ID_MOTEURS_MOVE: case MSG_ID_MOTEURS_MOVE:
distance = *((uint32_t*)msg.data); distance = *((int32_t*) msg.data);
MOTEURS_EtatDifferentiel.distance = distance; MOTEURS_EtatDifferentiel.distance = distance;
MOTEURS_EtatDifferentiel.tours = 0; MOTEURS_EtatDifferentiel.tours = 0;
MOTEURS_EtatMoteurGauche.consigne=50; if (distance > 0) {
MOTEURS_EtatMoteurDroit.consigne=50; MOTEURS_EtatMoteurGauche.consigne = 50;
MOTEURS_EtatMoteurDroit.consigne = 50;
} else {
MOTEURS_EtatMoteurGauche.consigne = -50;
MOTEURS_EtatMoteurDroit.consigne = -50;
}
vTaskResume(xHandleMoteursAsservissement); vTaskResume(xHandleMoteursAsservissement);
break; break;
case MSG_ID_MOTEURS_TURN: case MSG_ID_MOTEURS_TURN:
tours = *((uint32_t*)msg.data); tours = *((int32_t*) msg.data);
MOTEURS_EtatDifferentiel.distance = 0; MOTEURS_EtatDifferentiel.distance = 0;
MOTEURS_EtatDifferentiel.tours = tours; MOTEURS_EtatDifferentiel.tours = tours;
MOTEURS_EtatMoteurGauche.consigne=50; if (tours > 0) {
MOTEURS_EtatMoteurDroit.consigne=50; MOTEURS_EtatMoteurGauche.consigne = -50;
MOTEURS_EtatMoteurDroit.consigne = 50;
} else {
MOTEURS_EtatMoteurGauche.consigne = 50;
MOTEURS_EtatMoteurDroit.consigne = -50;
}
vTaskResume(xHandleMoteursAsservissement); vTaskResume(xHandleMoteursAsservissement);
break; break;
case MSG_ID_MOTEURS_STOP:
case MSG_ID_MOTEURS_STOP:
MOTEURS_EtatDifferentiel.distance = 0; MOTEURS_EtatDifferentiel.distance = 0;
MOTEURS_EtatDifferentiel.tours = 0; MOTEURS_EtatDifferentiel.tours = 0;
MOTEURS_EtatMoteurGauche.consigne=0; MOTEURS_EtatMoteurGauche.consigne = 0;
MOTEURS_EtatMoteurDroit.consigne=0; MOTEURS_EtatMoteurDroit.consigne = 0;
if ((MOTEURS_CorrectionEncodeur(MOTEURS_EtatMoteurGauche) ==0) && if ((MOTEURS_CorrectionEncodeur(MOTEURS_EtatMoteurGauche) == 0)
(MOTEURS_CorrectionEncodeur(MOTEURS_EtatMoteurDroit) ==0)) && (MOTEURS_CorrectionEncodeur(MOTEURS_EtatMoteurDroit) == 0))
// Les moteurs sont déjà arrêtés // Les moteurs sont déjà arrêtés
vTaskSuspend(xHandleMoteursAsservissement); vTaskSuspend(xHandleMoteursAsservissement);
else else
@ -188,9 +254,9 @@ void MOTEURS_TachePrincipale(void* params) {
* *
* @params params non utilisé * @params params non utilisé
*/ */
void MOTEURS_TacheAsservissement( void* params ) { void MOTEURS_TacheAsservissement(void *params) {
TickType_t xLastWakeTime; TickType_t xLastWakeTime;
int16_t erreurG, erreurD =0; int16_t erreurG, erreurD = 0;
int16_t encodeurGauche, encodeurDroit; int16_t encodeurGauche, encodeurDroit;
int32_t locCmdG, locCmdD; int32_t locCmdG, locCmdD;
@ -199,7 +265,12 @@ void MOTEURS_TacheAsservissement( void* params ) {
for (;;) { for (;;) {
// Wait for the next cycle. // 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); encodeurGauche = MOTEURS_CorrectionEncodeur(MOTEURS_EtatMoteurGauche);
encodeurDroit = MOTEURS_CorrectionEncodeur(MOTEURS_EtatMoteurDroit); encodeurDroit = MOTEURS_CorrectionEncodeur(MOTEURS_EtatMoteurDroit);
@ -213,53 +284,71 @@ void MOTEURS_TacheAsservissement( void* params ) {
erreurG = MOTEURS_EtatMoteurGauche.consigne - encodeurGauche; erreurG = MOTEURS_EtatMoteurGauche.consigne - encodeurGauche;
erreurD = MOTEURS_EtatMoteurDroit.consigne - encodeurDroit; erreurD = MOTEURS_EtatMoteurDroit.consigne - encodeurDroit;
if (((MOTEURS_EtatMoteurDroit.consigne ==0) && (MOTEURS_EtatMoteurGauche.consigne ==0)) && if (((MOTEURS_EtatMoteurDroit.consigne == 0)
((erreurD==0) && (erreurG==0))) { && (MOTEURS_EtatMoteurGauche.consigne == 0))
&& ((erreurD == 0) && (erreurG == 0))) {
MOTEURS_DesactiveAlim(); MOTEURS_DesactiveAlim();
vTaskSuspend(xHandleMoteursAsservissement); vTaskSuspend(xHandleMoteursAsservissement);
} }
if (MOTEURS_EtatMoteurGauche.consigne ==0) if (MOTEURS_EtatMoteurGauche.consigne == 0)
MOTEURS_EtatMoteurGauche.commande =0; MOTEURS_EtatMoteurGauche.commande = 0;
else { else {
if (erreurG !=0) { if (erreurG != 0) {
//locCmdG = (int32_t)MOTEURS_EtatMoteurGauche.commande + ((int32_t)MOTEUR_Kp*(int32_t)erreurG)/100; //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 (MOTEURS_EtatMoteurGauche.consigne >= 0) {
if (locCmdG<0) MOTEURS_EtatMoteurGauche.commande=0; if (locCmdG < 0)
else if (locCmdG>SHRT_MAX) MOTEURS_EtatMoteurGauche.commande=SHRT_MAX; MOTEURS_EtatMoteurGauche.commande = 0;
else MOTEURS_EtatMoteurGauche.commande=(int16_t)locCmdG; else if (locCmdG > SHRT_MAX)
MOTEURS_EtatMoteurGauche.commande = SHRT_MAX;
else
MOTEURS_EtatMoteurGauche.commande = (int16_t) locCmdG;
} else { } else {
if (locCmdG>0) MOTEURS_EtatMoteurGauche.commande=0; if (locCmdG > 0)
else if (locCmdG<SHRT_MIN) MOTEURS_EtatMoteurGauche.commande=SHRT_MIN; MOTEURS_EtatMoteurGauche.commande = 0;
else MOTEURS_EtatMoteurGauche.commande=(int16_t)locCmdG; else if (locCmdG < SHRT_MIN)
MOTEURS_EtatMoteurGauche.commande = SHRT_MIN;
else
MOTEURS_EtatMoteurGauche.commande = (int16_t) locCmdG;
} }
} }
} }
if (MOTEURS_EtatMoteurDroit.consigne ==0) if (MOTEURS_EtatMoteurDroit.consigne == 0)
MOTEURS_EtatMoteurDroit.commande =0; MOTEURS_EtatMoteurDroit.commande = 0;
else { else {
if (erreurD !=0) { if (erreurD != 0) {
//locCmdD = (int32_t)MOTEURS_EtatMoteurDroit.commande + ((int32_t)MOTEUR_Kp*(int32_t)erreurD)/100; //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 (MOTEURS_EtatMoteurDroit.consigne >= 0) {
if (locCmdD<0) MOTEURS_EtatMoteurDroit.commande=0; if (locCmdD < 0)
else if (locCmdD>SHRT_MAX) MOTEURS_EtatMoteurDroit.commande=SHRT_MAX; MOTEURS_EtatMoteurDroit.commande = 0;
else MOTEURS_EtatMoteurDroit.commande=(int16_t)locCmdD; else if (locCmdD > SHRT_MAX)
MOTEURS_EtatMoteurDroit.commande = SHRT_MAX;
else
MOTEURS_EtatMoteurDroit.commande = (int16_t) locCmdD;
} else { } else {
if (locCmdD>0) MOTEURS_EtatMoteurDroit.commande=0; if (locCmdD > 0)
else if (locCmdD<SHRT_MIN) MOTEURS_EtatMoteurDroit.commande=SHRT_MIN; MOTEURS_EtatMoteurDroit.commande = 0;
else MOTEURS_EtatMoteurDroit.commande=(int16_t)locCmdD; else if (locCmdD < SHRT_MIN)
MOTEURS_EtatMoteurDroit.commande = SHRT_MIN;
else
MOTEURS_EtatMoteurDroit.commande = (int16_t) locCmdD;
} }
} }
} }
/* Finalement, on applique les commandes aux moteurs */ /* 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 #define MOTEURS_MAX_CORRECTION_POINTS 16
const MOTEURS_CorrectionPoint MOTEURS_CorrectionPoints[MOTEURS_MAX_CORRECTION_POINTS]= const MOTEURS_CorrectionPoint MOTEURS_CorrectionPoints[MOTEURS_MAX_CORRECTION_POINTS] =
{ { { MOTEURS_MAX_ENCODEUR - 1, 1 }, { 42000, 100 }, { 22000, 2500 }, {
{MOTEURS_MAX_ENCODEUR-1, 1}, 18000, 5000 }, { 16500, 7500 }, { 15500, 10000 },
{42000, 100}, { 14500, 12500 }, { 13000, 15000 }, { 12500, 17500 }, { 12200,
{22000, 2500}, 20000 }, { 11500, 22500 }, { 11100, 25000 }, { 11000,
{18000, 5000}, 27500 }, { 10900, 29000 }, { 10850, 30500 }, { 10800,
{16500, 7500}, SHRT_MAX } // 32767
{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) * @return valeur linéarisée (entre -32768 et 32767)
*/ */
int16_t MOTEURS_CorrectionEncodeur(MOTEURS_EtatMoteur etat) { int16_t MOTEURS_CorrectionEncodeur(MOTEURS_EtatMoteur etat) {
int16_t correction=0; int16_t correction = 0;
uint8_t index=0; uint8_t index = 0;
uint32_t A,B,C; uint32_t A, B, C;
uint16_t encodeur = etat.encodeur; uint16_t encodeur = etat.encodeur;
if (encodeur ==MOTEURS_MAX_ENCODEUR) if (encodeur == MOTEURS_MAX_ENCODEUR)
correction =0; correction = 0;
else { // recherche par dichotomie de l'intervale else { // recherche par dichotomie de l'intervale
while (index <MOTEURS_MAX_CORRECTION_POINTS) { while (index < MOTEURS_MAX_CORRECTION_POINTS) {
if ((MOTEURS_CorrectionPoints[index].encodeur>=encodeur) && (MOTEURS_CorrectionPoints[index+1].encodeur<encodeur)) { if ((MOTEURS_CorrectionPoints[index].encodeur >= encodeur)
&& (MOTEURS_CorrectionPoints[index + 1].encodeur < encodeur)) {
// valeur trouvée, on sort // valeur trouvée, on sort
break; break;
} else } else
@ -316,18 +395,22 @@ int16_t MOTEURS_CorrectionEncodeur(MOTEURS_EtatMoteur etat) {
if (index >= MOTEURS_MAX_CORRECTION_POINTS) if (index >= MOTEURS_MAX_CORRECTION_POINTS)
correction = SHRT_MAX; correction = SHRT_MAX;
else { else {
A = encodeur-MOTEURS_CorrectionPoints[index+1].encodeur; A = encodeur - MOTEURS_CorrectionPoints[index + 1].encodeur;
B = MOTEURS_CorrectionPoints[index+1].correction-MOTEURS_CorrectionPoints[index].correction; B = MOTEURS_CorrectionPoints[index + 1].correction
C = MOTEURS_CorrectionPoints[index].encodeur-MOTEURS_CorrectionPoints[index+1].encodeur; - 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 * 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; correction = -correction;
return correction; return correction;
@ -341,10 +424,12 @@ void MOTEURS_DesactiveAlim(void) {
LL_TIM_DisableCounter(TIM2); LL_TIM_DisableCounter(TIM2);
LL_TIM_DisableCounter(TIM21); 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(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(TIM21, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH2);
LL_TIM_DisableIT_CC1(TIM2); LL_TIM_DisableIT_CC1(TIM2);
LL_TIM_DisableIT_CC1(TIM21); LL_TIM_DisableIT_CC1(TIM21);
@ -363,10 +448,12 @@ void MOTEURS_ActiveAlim(void) {
LL_TIM_EnableCounter(TIM2); LL_TIM_EnableCounter(TIM2);
LL_TIM_EnableCounter(TIM21); 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(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(TIM21, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH2);
LL_TIM_EnableIT_CC1(TIM2); LL_TIM_EnableIT_CC1(TIM2);
LL_TIM_EnableIT_CC1(TIM21); LL_TIM_EnableIT_CC1(TIM21);
@ -389,28 +476,30 @@ void MOTEURS_ActiveAlim(void) {
void MOTEURS_Set(int16_t cmdGauche, int16_t cmdDroit) { void MOTEURS_Set(int16_t cmdGauche, int16_t cmdDroit) {
int32_t locValGauche, locValDroit; int32_t locValGauche, locValDroit;
locValGauche = (int32_t)(((int32_t)cmdGauche * (int32_t)SHRT_MAX)/((int32_t)MOTEURS_MAX_COMMANDE)); locValGauche = (int32_t) (((int32_t) cmdGauche * (int32_t) SHRT_MAX)
locValDroit = (int32_t)(((int32_t)cmdDroit * (int32_t)SHRT_MAX)/((int32_t)MOTEURS_MAX_COMMANDE)); / ((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(); MOTEURS_ActiveAlim();
// Moteur droit // Moteur droit
if (cmdDroit >=0) { if (cmdDroit >= 0) {
LL_TIM_OC_SetCompareCH2(TIM3, (uint32_t)locValDroit); LL_TIM_OC_SetCompareCH2(TIM3, (uint32_t) locValDroit);
LL_TIM_OC_SetCompareCH1(TIM3, (uint32_t)0); LL_TIM_OC_SetCompareCH1(TIM3, (uint32_t) 0);
} else { } else {
LL_TIM_OC_SetCompareCH2(TIM3, (uint32_t)0); LL_TIM_OC_SetCompareCH2(TIM3, (uint32_t) 0);
LL_TIM_OC_SetCompareCH1(TIM3, (uint32_t)locValDroit); LL_TIM_OC_SetCompareCH1(TIM3, (uint32_t) locValDroit);
} }
// Moteur gauche // Moteur gauche
if (cmdGauche >=0) { if (cmdGauche >= 0) {
LL_TIM_OC_SetCompareCH4(TIM3, (uint32_t)locValGauche); LL_TIM_OC_SetCompareCH4(TIM3, (uint32_t) locValGauche);
LL_TIM_OC_SetCompareCH3(TIM3, (uint32_t)0); LL_TIM_OC_SetCompareCH3(TIM3, (uint32_t) 0);
} else { } else {
LL_TIM_OC_SetCompareCH4(TIM3, (uint32_t)0); LL_TIM_OC_SetCompareCH4(TIM3, (uint32_t) 0);
LL_TIM_OC_SetCompareCH3(TIM3, (uint32_t)locValGauche); 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 * @param htim pointeur sur la reference du timer qui generé l'interruption
*/ */
void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) { 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 (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.encodeur = MOTEURS_MAX_ENCODEUR;
MOTEURS_EtatMoteurGauche.encodeurFront = MOTEURS_MAX_ENCODEUR; MOTEURS_EtatMoteurGauche.encodeurFront = MOTEURS_MAX_ENCODEUR;
} else { } else {
MOTEURS_EtatMoteurGauche.encodeur = (uint16_t)LL_TIM_IC_GetCaptureCH1(TIM21); MOTEURS_EtatMoteurGauche.encodeur =
MOTEURS_EtatMoteurGauche.encodeurFront = (uint16_t)LL_TIM_IC_GetCaptureCH2(TIM21); (uint16_t) LL_TIM_IC_GetCaptureCH1(TIM21);
MOTEURS_EtatMoteurGauche.encodeurFront =
(uint16_t) LL_TIM_IC_GetCaptureCH2(TIM21);
} }
if (LL_TIM_IsActiveFlag_UPDATE(TIM21)) if (LL_TIM_IsActiveFlag_UPDATE(TIM21))
LL_TIM_ClearFlag_UPDATE(TIM21); LL_TIM_ClearFlag_UPDATE(TIM21);
MOTEURS_EtatMoteurGauche.moteurLent = 0; 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 */ } else { /* moteur droit */
if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1) { 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.encodeur = MOTEURS_MAX_ENCODEUR;
MOTEURS_EtatMoteurDroit.encodeurFront = MOTEURS_MAX_ENCODEUR; MOTEURS_EtatMoteurDroit.encodeurFront = MOTEURS_MAX_ENCODEUR;
} else { } else {
MOTEURS_EtatMoteurDroit.encodeur = (uint16_t)LL_TIM_IC_GetCaptureCH1(TIM2); MOTEURS_EtatMoteurDroit.encodeur =
MOTEURS_EtatMoteurDroit.encodeurFront = (uint16_t)LL_TIM_IC_GetCaptureCH2(TIM2); (uint16_t) LL_TIM_IC_GetCaptureCH1(TIM2);
MOTEURS_EtatMoteurDroit.encodeurFront =
(uint16_t) LL_TIM_IC_GetCaptureCH2(TIM2);
} }
if (LL_TIM_IsActiveFlag_UPDATE(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 * @param htim pointeur sur la reference du timer qui generé l'interruption
*/ */
void MOTEURS_TimerEncodeurUpdate (TIM_HandleTypeDef *htim) { void MOTEURS_TimerEncodeurUpdate(TIM_HandleTypeDef *htim) {
if (htim->Instance==TIM21) { /* moteur gauche */ if (htim->Instance == TIM21) { /* moteur gauche */
if ((MOTEURS_EtatMoteurGauche.moteurLent++) >=1) { if ((MOTEURS_EtatMoteurGauche.moteurLent++) >= 1) {
MOTEURS_EtatMoteurGauche.encodeur = MOTEURS_MAX_ENCODEUR; MOTEURS_EtatMoteurGauche.encodeur = MOTEURS_MAX_ENCODEUR;
MOTEURS_EtatMoteurGauche.moteurLent = 1; MOTEURS_EtatMoteurGauche.moteurLent = 1;
} }
} else { /* moteur droit */ } else { /* moteur droit */
if ((MOTEURS_EtatMoteurDroit.moteurLent++) >=1) { if ((MOTEURS_EtatMoteurDroit.moteurLent++) >= 1) {
MOTEURS_EtatMoteurDroit.encodeur = MOTEURS_MAX_ENCODEUR; MOTEURS_EtatMoteurDroit.encodeur = MOTEURS_MAX_ENCODEUR;
MOTEURS_EtatMoteurDroit.moteurLent = 1; MOTEURS_EtatMoteurDroit.moteurLent = 1;
} }

4
software/dumber3/Application/moteurs.h
View file

@ -13,8 +13,8 @@
void MOTEURS_Init(void); void MOTEURS_Init(void);
//void MOTEURS_Set(uint8_t mot, int16_t val); //void MOTEURS_Set(uint8_t mot, int16_t val);
//void MOTEURS_Test (void); //void MOTEURS_Test (void);
void MOTEURS_Avance(uint32_t distance); void MOTEURS_Avance(int32_t distance);
void MOTEURS_Tourne(uint32_t tours); void MOTEURS_Tourne(int32_t tours);
void MOTEURS_Stop(void); void MOTEURS_Stop(void);
void MOTEURS_TimerEncodeurUpdate (TIM_HandleTypeDef *htim); void MOTEURS_TimerEncodeurUpdate (TIM_HandleTypeDef *htim);

2
software/dumber3/Core/Src/stm32l0xx_hal_msp.c
View file

@ -181,7 +181,7 @@ void HAL_UART_MspInit(UART_HandleTypeDef* huart)
HAL_NVIC_SetPriority(LPUART1_IRQn, 3, 0); HAL_NVIC_SetPriority(LPUART1_IRQn, 3, 0);
HAL_NVIC_EnableIRQ(LPUART1_IRQn); HAL_NVIC_EnableIRQ(LPUART1_IRQn);
/* USER CODE BEGIN LPUART1_MspInit 1 */ /* 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 */ /* USER CODE END LPUART1_MspInit 1 */
} }