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

Debut de documentation doxygen + renommage de module

Browse source
This commit is contained in:
dimercur 2024-01-09 16:59:52 +01:00
parent 4d4882b09d
commit d21c4b2ea3
21 changed files with 4713 additions and 1146 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

3
.gitignore vendored
View file

@ -68,3 +68,6 @@ GUI
/software/dumber3/Release/
/software/dumber3/Debug/
/software/dumber3/Tests/
/software/dumber3/workspace/
/doc/Doc\ robot/doxygen/

233
software/dumber3/Application/application.c
View file

@ -1,8 +1,57 @@
/*
* statemachine.c
/**
******************************************************************************
* @file application.c
* @brief application body
* @author S. DI MERCURIO (dimercur@insa-toulouse.fr)
* @date December 2023
*
******************************************************************************
* @copyright Copyright 2023 INSA-GEI, Toulouse, France. All rights reserved.
* @copyright This project is released under the Lesser GNU Public License (LGPL-3.0-only).
*
* @copyright This file is part of "Dumber" project
*
* @copyright This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 3 of the License, or (at your option) any later version.
*
* @copyright This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
* @copyright You should have received a copy of the GNU Lesser General Public License
* along with this program; if not, write to the Free Software Foundation,
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
******************************************************************************
*/
/**
* @mainpage Dumber 3
*
* Dumber is a robot used at INSA-GEI, Toulouse, France for realtime computer science teaching.
* Robot is basically controlled by a supervisor program and move depending on commands send by supervisor.
* Movements are controlled by a camera.
*
* @copyright Copyright 2023 INSA-GEI, Toulouse, France. All rights reserved.
* @copyright This project is released under the Lesser GNU Public License (LGPL-3.0-only).
*
* @copyright This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 3 of the License, or (at your option) any later version.
*
* @copyright This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
* @copyright You should have received a copy of the GNU Lesser General Public License
* along with this program; if not, write to the Free Software Foundation,
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
* Created on: Sep 12, 2022
* Author: dimercur
*/
#include "application.h"
@ -10,48 +59,63 @@
#include "string.h"
#include <stdlib.h>
#include "moteurs.h"
#include "leds.h"
#include "xbee.h"
#include "batterie.h"
#include "messages.h"
#include "motors.h"
#include "battery.h"
#include "panic.h"
/** @addtogroup Application_Software
* @{
*/
/** @addtogroup APPLICATION
* @{
*/
/** @addtogroup APPLICATION_Private Private
* @{
*/
/** Enumeration class used by application state machine for defining current application state */
typedef enum {
stateStartup=0,
stateIdle,
stateRun,
stateInCharge,
stateInMouvement,
stateWatchdogDisable,
stateLowBatDisable
stateStartup=0, /**< Startup state, after system power on */
stateIdle, /**< Idle state, after system has initialized all peripherals and is ready to handle commands */
stateRun, /**< Run state, after system has received and accepted "StartWithWatchdog" or "StartWithoutWatchdog" command */
stateInCharge, /**< In Charge state, when a charger is plugged */
stateInMouvement, /**< In Movement state, when the robot is moving */
stateWatchdogDisable, /**< Watchdog Disable state, after watchdog has expired */
stateLowBatDisable /**< Low Bat Disable state, when battery is too low */
} APPLICATION_State;
/** Structure containing information about current system state */
typedef struct {
APPLICATION_State state;
uint8_t cmd;
uint16_t batteryState;
char batteryUpdate;
char inCharge;
int32_t distance;
int32_t turns;
int32_t motor_left;
int32_t motor_right;
char endOfMouvement;
char powerOffRequired;
uint16_t senderAddress;
uint8_t rfProblem;
APPLICATION_State state; /**< Store current application state*/
uint8_t cmd; /**< Current received command, CMD_NONE if no command was received */
uint16_t batteryState; /**< Last battery message received from battery driver*/
char batteryUpdate; /**< Battery state has changed and need to be processed*/
char inCharge; /**< Robot is currently plugged for charging*/
int32_t distance; /**< Distance of movement requested with a MOVE command*/
int32_t turns; /**< Number of turn requested with a TURN command*/
int32_t motor_left; /**< Speed to be applied for left motor */
int32_t motor_right; /**< Speed to be applied for right motor*/
char endOfMouvement; /**< Flag indicating last movement request has ended, ready for new movement*/
char powerOffRequired; /**< Flag indicating system power off*/
uint16_t senderAddress; /**< Xbee sender address (not used)*/
uint8_t rfProblem; /**< Xbee RF quality (not used)*/
} APPLICATION_Infos;
/** Structure storing counters used for watchdog and system inactivity.
* Used notably to check if watchdog reset was missed or power down system because of inactivity */
typedef struct {
uint32_t startupCnt;
uint32_t inactivityCnt;
uint32_t watchdogCnt;
char watchdogEnabled;
char watchdogMissedCnt;
uint32_t startupCnt; /**< Counter used during wake up, to allow couple of second for
battery animation to show up before system enters IDLE state*/
uint32_t inactivityCnt; /**< Counter used to check system inactivity (no command received)*/
uint32_t watchdogCnt; /**< Counter used for watchdog check. Reset when RESET_WATCHDOG command is received */
char watchdogEnabled; /**< Flag used to know if watchdog is enabled or not*/
char watchdogMissedCnt; /**< Counter used to store each time watchdog reset is missed*/
} APPLICATION_Timeout;
StaticTask_t xTaskApplicationMain;
@ -66,18 +130,19 @@ StaticTimer_t xBufferTimerTimeout;
TimerHandle_t xHandleTimerTimeout = NULL;
void vTimerTimeoutCallback( TimerHandle_t xTimer );
void LEDS_Tests();
void APPLICATION_MainThread(void* params);
void APPLICATION_TimeoutThread(void* params);
void APPLICATION_Thread(void* params);
void APPLICATION_StateMachine();
LEDS_State APPLICATION_BatteryLevel(uint8_t voltage, APPLICATION_State state);
void APPLICATION_PowerOff();
void APPLICATION_TransitionToNewState(APPLICATION_State new_state);
APPLICATION_Infos systemInfos = {0};
APPLICATION_Timeout systemTimeout = {0};
/**
* @brief Initialization of drivers, modules and application.
* @param None
* @return None
*/
void APPLICATION_Init(void) {
/* Init des messages box */
MESSAGE_Init();
@ -87,13 +152,13 @@ void APPLICATION_Init(void) {
/* Init de la partie RF / reception des messages */
XBEE_Init();
BATTERIE_Init();
MOTEURS_Init();
BATTERY_Init();
MOTORS_Init();
/* Create the task without using any dynamic memory allocation. */
xHandleApplicationMain = xTaskCreateStatic(
APPLICATION_MainThread, /* Function that implements the task. */
"APPLICATION Main", /* Text name for the task. */
APPLICATION_Thread, /* Function that implements the task. */
"APPLICATION Thread", /* Text name for the task. */
STACK_SIZE, /* Number of indexes in the xStack array. */
NULL, /* Parameter passed into the task. */
PriorityApplicationHandler,/* Priority at which the task is created. */
@ -103,8 +168,8 @@ void APPLICATION_Init(void) {
/* Create a periodic task without using any dynamic memory allocation. */
xHandleTimerTimeout = xTimerCreateStatic(
"Seq Timer",
pdMS_TO_TICKS(APPLICATION_PERIODE),
"Counters Timer",
pdMS_TO_TICKS(APPLICATION_COUNTERS_DELAY),
pdTRUE,
( void * ) 0,
vTimerTimeoutCallback,
@ -112,7 +177,16 @@ void APPLICATION_Init(void) {
xTimerStart(xHandleTimerTimeout,0 );
}
void APPLICATION_MainThread(void* params) {
/**
* @brief Application thread (main thread)
*
* This thread mainly waits for messages from others threads or drivers, store informations, set various flags
* and then call state machine function (APPLICATION_StateMachine()) for processing actions.
*
* @param[in] params startup parameters for task (not used)
* @return None
*/
void APPLICATION_Thread(void* params) {
MESSAGE_Typedef msg;
char* receivedCMD;
CMD_Generic* decodedCmd;
@ -206,7 +280,7 @@ void APPLICATION_MainThread(void* params) {
systemInfos.inCharge=0;
systemInfos.batteryState = msg.id;
break;
case MSG_ID_MOTEURS_END_OF_MOUVMENT:
case MSG_ID_MOTORS_END_OF_MOUVMENT:
systemInfos.endOfMouvement= 1;
break;
@ -219,7 +293,16 @@ void APPLICATION_MainThread(void* params) {
}
}
void APPLICATION_StateMachine() {
/**
* @brief State machine processing function
*
* This function processes received messages depending on current system state.
* In case of state transition, function APPLICATION_TransitionToNewState will be called at end for transition and clean up
*
* @param None
* @return None
*/
void APPLICATION_StateMachine(void) {
LEDS_State ledState = leds_off;
if (systemInfos.powerOffRequired)
@ -359,9 +442,16 @@ void APPLICATION_StateMachine() {
systemInfos.powerOffRequired=0;
}
/**
* @brief State machine transition clean up
*
* This function is part of statemachine processing. It's job is to process and cleanup statemachine transition.
*
* @param[in] new_state New state to apply to system
* @return None
*/
void APPLICATION_TransitionToNewState(APPLICATION_State new_state) {
LEDS_State ledState = leds_off;
//int32_t data;
switch (new_state) {
case stateStartup:
@ -371,7 +461,7 @@ void APPLICATION_TransitionToNewState(APPLICATION_State new_state) {
ledState = leds_idle;
LEDS_Set(ledState);
MOTEURS_Stop();
MOTORS_Stop();
systemTimeout.inactivityCnt=0;
systemTimeout.watchdogEnabled=0;
break;
@ -383,21 +473,21 @@ void APPLICATION_TransitionToNewState(APPLICATION_State new_state) {
LEDS_Set(ledState);
MOTEURS_Stop();
MOTORS_Stop();
break;
case stateInMouvement:
ledState = leds_run;
LEDS_Set(ledState);
if (systemInfos.cmd == CMD_MOVE) {
MOTEURS_Avance( systemInfos.distance);
MOTORS_Move( systemInfos.distance);
} else { /* obviously, cmd is CMD_TURN */
MOTEURS_Tourne(systemInfos.turns);
MOTORS_Turn(systemInfos.turns);
}
break;
case stateInCharge:
/* les leds sont gerées dans APPLICATION_StateMachine */
MOTEURS_Stop();
MOTORS_Stop();
systemTimeout.watchdogEnabled=0;
break;
case stateWatchdogDisable:
@ -424,7 +514,15 @@ void APPLICATION_TransitionToNewState(APPLICATION_State new_state) {
systemInfos.state = new_state;
}
void APPLICATION_PowerOff() {
/**
* @brief Power off robot
*
* Disable main regulator and power off system. Used after inactivity or when user press on/off button.
*
* @param None
* @return None
*/
void APPLICATION_PowerOff(void) {
/*
* TODO: a decommenter quand le code sera debuggé
*/
@ -435,10 +533,17 @@ void APPLICATION_PowerOff() {
}
}
/*
* This task is called every 100 ms
* RQ: les constante de temps sont exprimé en ms, d'où la division par 100
*/
/**
* @brief Periodic task used for system counter update
*
* This periodic task is called every 100 ms and is used for updating inactivity, startup and watchdog counters,
* sending messages or triggering transition if necessary.
*
* @remark Time constants are expressed in ms, thus explaining the division by 100 used in comparison.
*
* @param[in] xTimer Handler for periodic task
* @return None
*/
void vTimerTimeoutCallback( TimerHandle_t xTimer ) {
if (systemInfos.state == stateStartup) {
systemTimeout.startupCnt++;
@ -465,3 +570,15 @@ void vTimerTimeoutCallback( TimerHandle_t xTimer ) {
}
}
}
/**
* @}
*/
/**
* @}
*/
/**
* @}
*/

60
software/dumber3/Application/application.h
View file

@ -1,10 +1,32 @@
/*
* statemachine.h
/**
******************************************************************************
* @file application.h
* @brief application header
* @author S. DI MERCURIO (dimercur@insa-toulouse.fr)
* @date December 2023
*
* Created on: Sep 12, 2022
* Author: dimercur
*/
******************************************************************************
* @copyright Copyright 2023 INSA-GEI, Toulouse, France. All rights reserved.
* @copyright This project is released under the Lesser GNU Public License (LGPL-3.0-only).
*
* @copyright This file is part of "Dumber" project
*
* @copyright This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 3 of the License, or (at your option) any later version.
*
* @copyright This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
* @copyright You should have received a copy of the GNU Lesser General Public License
* along with this program; if not, write to the Free Software Foundation,
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
******************************************************************************
*/
#ifndef INC_APPLICATION_H_
#define INC_APPLICATION_H_
@ -14,13 +36,37 @@
#include "messages.h"
#include "leds.h"
#include "moteurs.h"
#include "batterie.h"
#include "battery.h"
#include "motors.h"
#include "xbee.h"
#include "commands.h"
#include "main.h"
/** @addtogroup Application_Software
* @{
*/
/** @addtogroup APPLICATION
* @{
*/
/** @addtogroup APPLICATION_Public Public
* @{
*/
void APPLICATION_Init(void);
/**
* @}
*/
/**
* @}
*/
/**
* @}
*/
#endif /* INC_APPLICATION_H_ */

273
software/dumber3/Application/batterie.c
View file

@ -1,273 +0,0 @@
/*
* batterie.c
*
* Created on: Sep 12, 2022
* Author: dimercur
*/
#include "batterie.h"
#include "stm32l0xx.h"
#include "stm32l0xx_ll_gpio.h"
#include "timers.h"
typedef enum {
CHARGEUR_NOT_PLUGGED,
CHARGEUR_IN_CHARGE,
CHARGEUR_CHARGE_COMPLETE,
CHARGEUR_ERROR
} BATTERIE_StatusChargerTypedef;
#define BATTERIE_MAX_ERROR 3
extern ADC_HandleTypeDef hadc;
uint8_t conversion_complete;
uint16_t adc_raw_value;
StaticTask_t xTaskBatterie;
/* Buffer that the task being created will use as its stack. Note this is
an array of StackType_t variables. The size of StackType_t is dependent on
the RTOS port. */
StackType_t xStackBatterie[ STACK_SIZE ];
TaskHandle_t xHandleBatterie = NULL;
TaskHandle_t task_handler;
TaskHandle_t charger_thread_handler;
/* TimerButton sert à attendre ~ 3secondes avant de prendre en compte les IT bouton
* En effet, au demarrage, le bouton est appuyé pour lancer le systeme. ceci genere alors une IT bouton,
* ammenant à envoyer le message MSG_ID_BUTTON_PRESSED, demandant l'arret du systeme
*
* De ce fait, avec cette tempo, on s'assure de ne pas prendre en compte les IT dans les 3 premieres secondes.
*/
StaticTimer_t xBufferTimerButton;
TimerHandle_t xHandleTimerButton = NULL;
void vTimerButtonCallback( TimerHandle_t xTimer );
uint8_t BUTTON_Inactivity=1; //start with button on/off inactive
void BATTERIE_VoltageThread(void* params);
void BATTERIE_Init(void) {
//task_handler = NULL;
//charger_thread_handler = NULL;
/* Create the task without using any dynamic memory allocation. */
xHandleBatterie = xTaskCreateStatic(
BATTERIE_VoltageThread, /* Function that implements the task. */
"BATTERIE Voltage", /* Text name for the task. */
STACK_SIZE, /* Number of indexes in the xStack array. */
NULL, /* Parameter passed into the task. */
PriorityBatterieHandler,/* Priority at which the task is created. */
xStackBatterie, /* Array to use as the task's stack. */
&xTaskBatterie); /* Variable to hold the task's data structure. */
/* Create a periodic task without using any dynamic memory allocation. */
xHandleTimerButton = xTimerCreateStatic(
"Inactivity Button Timer",
pdMS_TO_TICKS(BUTTON_INACTIVITY_PERIODE),
pdTRUE,
( void * ) 0,
vTimerButtonCallback,
&xBufferTimerButton);
xTimerStart(xHandleTimerButton,0 );
vTaskResume(xHandleBatterie);
}
/*
* Lit les pins GPIO
*/
BATTERIE_StatusChargerTypedef BATTERIE_LireStatusChargeur(void) {
uint32_t st2 = LL_GPIO_ReadInputPort(CHARGER_ST2_GPIO_Port) & CHARGER_ST2_Pin;
uint32_t st1 = LL_GPIO_ReadInputPort(CHARGER_ST1_GPIO_Port) & CHARGER_ST1_Pin;
BATTERIE_StatusChargerTypedef status;
if (st1 && st2)
status = CHARGEUR_NOT_PLUGGED;
else if (st1 && !st2)
status = CHARGEUR_CHARGE_COMPLETE;
else if (!st1 && st2)
status = CHARGEUR_IN_CHARGE;
else /* !st1 && !st2 */
status = CHARGEUR_ERROR;
return status;
}
int BATTERIE_LireTension(uint16_t *val) {
uint32_t ulNotificationValue;
conversion_complete = 0;
adc_raw_value = 0;
task_handler = xTaskGetCurrentTaskHandle();
if (HAL_ADC_Start_IT(&hadc) != HAL_OK)
return -1;
ulNotificationValue = ulTaskNotifyTake( pdTRUE, pdMS_TO_TICKS(100)); // wait max 100 ms
if (ulNotificationValue == 1) {
/* The transmission ended as expected. */
*val = adc_raw_value;
} else {
/* The call to ulTaskNotifyTake() timed out. */
return -2;
}
task_handler = NULL;
return 0;
}
/*
* Il faut considerer ces valeurs comme les seuils de baculement dans une categorie
* ou une autre
*
* Seuil : critical low high
* Tension batterie: 2.9 critic 3.1 low 3.3 med 3.6 high 4.2
*
*/
#ifdef TESTS
uint8_t BATTERIE_LEVEL_CRITICAL=135;
uint8_t BATTERIE_LEVEL_LOW=145;
uint8_t BATTERIE_LEVEL_HIGH=155;
uint8_t BATTERIE_LEVEL_CHARGE_LOW=150;
uint8_t BATTERIE_LEVEL_CHARGE_HIGH=170;
uint8_t BATTERIE_currentValue;
#else
#define BATTERIE_LEVEL_CRITICAL 135
#define BATTERIE_LEVEL_LOW 145
#define BATTERIE_LEVEL_HIGH 155
#define BATTERIE_LEVEL_CHARGE_LOW 150
#define BATTERIE_LEVEL_CHARGE_HIGH 170
#endif /* TESTS */
uint16_t BATTERIE_BatteryLevel(uint8_t voltage, BATTERIE_StatusChargerTypedef chargerStatus) {
uint16_t msgId=0;
#ifdef TESTS
BATTERIE_currentValue=voltage;
#endif /* TESTS */
switch (chargerStatus) {
case CHARGEUR_CHARGE_COMPLETE:
msgId = MSG_ID_BAT_CHARGE_COMPLETE;
break;
case CHARGEUR_IN_CHARGE:
if (voltage<=BATTERIE_LEVEL_CHARGE_LOW)
msgId = MSG_ID_BAT_CHARGE_LOW;
else if (voltage>=BATTERIE_LEVEL_CHARGE_HIGH)
msgId = MSG_ID_BAT_CHARGE_HIGH;
else
msgId = MSG_ID_BAT_CHARGE_MED;
break;
case CHARGEUR_NOT_PLUGGED:
if (voltage<=BATTERIE_LEVEL_CRITICAL)
msgId = MSG_ID_BAT_CRITICAL_LOW;
else if (voltage<=BATTERIE_LEVEL_LOW)
msgId = MSG_ID_BAT_LOW;
else if (voltage>=BATTERIE_LEVEL_HIGH)
msgId = MSG_ID_BAT_HIGH;
else
msgId = MSG_ID_BAT_MED;
break;
default:
msgId = MSG_ID_BAT_CHARGE_ERR;
}
return msgId;
}
void BATTERIE_VoltageThread(void* params) {
static uint16_t tension;
static uint8_t batteryErrorCnt=0;
BATTERIE_StatusChargerTypedef currentStatus;
uint16_t messageID;
TickType_t xLastWakeTime;
// Initialise the xLastWakeTime variable with the current time.
xLastWakeTime = xTaskGetTickCount();
while (1) {
if (BATTERIE_LireTension(&tension) ==0) {
currentStatus = BATTERIE_LireStatusChargeur();
if (currentStatus == CHARGEUR_ERROR) {
batteryErrorCnt++;
if (batteryErrorCnt>=BATTERIE_MAX_ERROR)
MESSAGE_SendMailbox(APPLICATION_Mailbox, MSG_ID_BAT_CHARGE_ERR, (QueueHandle_t)0x0, (void*)NULL);
} else {
messageID = BATTERIE_BatteryLevel(tension, currentStatus);
MESSAGE_SendMailbox(APPLICATION_Mailbox, messageID, (QueueHandle_t)0x0, (void*)NULL);
}
#ifdef TESTS
MESSAGE_SendMailbox(APPLICATION_Mailbox, MSG_ID_BAT_LEVEL, (QueueHandle_t)0x0, (void*)&tension);
#endif /* TESTS*/
} else {
MESSAGE_SendMailbox(APPLICATION_Mailbox, MSG_ID_BAT_ADC_ERR, (QueueHandle_t)0x0, (void*)0x0);
}
// Wait for the next cycle.
vTaskDelayUntil( &xLastWakeTime, pdMS_TO_TICKS(BATTERIE_PERIODE_SCRUTATION));
}
}
void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef *hadc) {
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
adc_raw_value = HAL_ADC_GetValue(hadc);
if (task_handler != NULL) {
/* Notify the task that an event has been emitted. */
vTaskNotifyGiveFromISR(task_handler, &xHigherPriorityTaskWoken );
/* There are no more eventin progress, so no tasks to notify. */
task_handler = NULL;
/* If xHigherPriorityTaskWoken is now set to pdTRUE then a context switch
should be performed to ensure the interrupt returns directly to the highest
priority task. The macro used for this purpose is dependent on the port in
use and may be called portEND_SWITCHING_ISR(). */
portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
}
}
void vTimerButtonCallback( TimerHandle_t xTimer ) {
BUTTON_Inactivity=0;
xTimerStop(xHandleTimerButton,0 );
}
/**
* @brief This function handles EXTI line0 interrupt.
*/
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
// if (GPIO_Pin == USB_SENSE_Pin) { // Le chargeur vient d'etre branché ou debranché
// if (HAL_GPIO_ReadPin(USB_SENSE_GPIO_Port, GPIO_Pin)==GPIO_PIN_SET) // le chargeur est branché
// MESSAGE_SendMailboxFromISR(APPLICATION_Mailbox, MSG_ID_BAT_CHARGE_ON, (QueueHandle_t)0x0, 0x0, &xHigherPriorityTaskWoken);
// else
// MESSAGE_SendMailboxFromISR(APPLICATION_Mailbox, MSG_ID_BAT_CHARGE_OFF, (QueueHandle_t)0x0, 0x0, &xHigherPriorityTaskWoken);
// }
// else
if (GPIO_Pin == BUTTON_SENSE_Pin) { // on vient d'appuyer sur le bouton on/off
if (!BUTTON_Inactivity) {
if (HAL_GPIO_ReadPin(BUTTON_SENSE_GPIO_Port, GPIO_Pin)==GPIO_PIN_RESET) // GPIOB.3 = 0 => le bouton est appuyé
MESSAGE_SendMailboxFromISR(APPLICATION_Mailbox, MSG_ID_BUTTON_PRESSED, (QueueHandle_t)0x0, 0x0, &xHigherPriorityTaskWoken);
}
}
if (xHigherPriorityTaskWoken) {
/* If xHigherPriorityTaskWoken is now set to pdTRUE then a context switch
should be performed to ensure the interrupt returns directly to the highest
priority task. The macro used for this purpose is dependent on the port in
use and may be called portEND_SWITCHING_ISR(). */
portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
}
}

15
software/dumber3/Application/batterie.h
View file

@ -1,15 +0,0 @@
/*
* batterie.h
*
* Created on: Sep 12, 2022
* Author: dimercur
*/
#ifndef INC_BATTERIE_H_
#define INC_BATTERIE_H_
#include "application.h"
void BATTERIE_Init(void);
#endif /* INC_BATTERIE_H_ */

373
software/dumber3/Application/battery.c Normal file
View file

@ -0,0 +1,373 @@
/**
******************************************************************************
* @file battery.c
* @brief battery driver body
* @author S. DI MERCURIO (dimercur@insa-toulouse.fr)
* @date December 2023
*
******************************************************************************
* @copyright Copyright 2023 INSA-GEI, Toulouse, France. All rights reserved.
* @copyright This project is released under the Lesser GNU Public License (LGPL-3.0-only).
*
* @copyright This file is part of "Dumber" project
*
* @copyright This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 3 of the License, or (at your option) any later version.
*
* @copyright This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
* @copyright You should have received a copy of the GNU Lesser General Public License
* along with this program; if not, write to the Free Software Foundation,
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
******************************************************************************
*/
#include "battery.h"
#include "stm32l0xx.h"
#include "stm32l0xx_ll_gpio.h"
#include "timers.h"
/** @addtogroup Application_Software
* @{
*/
/** @addtogroup BATTERY
* Battery driver is in charge of monitoring battery voltage, checking for state change in charger,
* sending message when new events or new voltage acquisitions happened and monitor on/off button
* @{
*/
/** @addtogroup BATTERY_Private Private
* @{
*/
/** Enumeration class defining ST601E battery charger chip states */
typedef enum {
CHARGER_NOT_PLUGGED, /**< Charger is idle, no USB cable is plugged */
CHARGER_IN_CHARGE, /**< Charger is currently charging battery, USB cable is plugged */
CHARGER_CHARGE_COMPLETE, /**< Battery charging is finished, charge has ended but USB cable is still plugged */
CHARGER_ERROR /**< An error occured during battery charging: charge has ended and USB cable is plugged */
} BATTERY_StatusChargerTypedef;
/** Constant used to removed spurious "CHARGER_ERROR" events */
#define BATTERY_MAX_ERROR 3
extern ADC_HandleTypeDef hadc;
uint8_t conversion_complete;
uint16_t adc_raw_value;
StaticTask_t xTaskBattery;
/* Buffer that the task being created will use as its stack. Note this is
an array of StackType_t variables. The size of StackType_t is dependent on
the RTOS port. */
StackType_t xStackBattery[ STACK_SIZE ];
TaskHandle_t xHandleBattery = NULL;
TaskHandle_t task_handler;
TaskHandle_t charger_thread_handler;
/* TimerButton sert à attendre ~ 1.5 secondes avant de prendre en compte les IT bouton
* En effet, au demarrage, le bouton est appuyé pour lancer le systeme. Ceci genere alors une IT bouton,
* ammenant à envoyer le message MSG_ID_BUTTON_PRESSED, demandant l'arret du systeme
*
* De ce fait, avec cette tempo, on s'assure de ne pas prendre en compte les IT dans les 3 premieres secondes.
*/
StaticTimer_t xBufferTimerButton;
TimerHandle_t xHandleTimerButton = NULL;
void vTimerButtonCallback( TimerHandle_t xTimer );
uint8_t BUTTON_Inactivity=1; //start with button on/off inactive
void BATTERY_Thread(void* params);
/**
* @brief Function for initializing battery and on/off button monitoring
*
* @param None
* @retval None
*/
void BATTERY_Init(void) {
/* Create the task without using any dynamic memory allocation. */
xHandleBattery = xTaskCreateStatic(
BATTERY_Thread, /* Function that implements the task. */
"BATTERY Task", /* Text name for the task. */
STACK_SIZE, /* Number of indexes in the xStack array. */
NULL, /* Parameter passed into the task. */
PriorityBatteryHandler,/* Priority at which the task is created. */
xStackBattery, /* Array to use as the task's stack. */
&xTaskBattery); /* Variable to hold the task's data structure. */
/* Create a one-shot timer without using any dynamic memory allocation. */
xHandleTimerButton = xTimerCreateStatic(
"Inactivity Button Timer",
pdMS_TO_TICKS(BUTTON_INACTIVITY_DELAY),
pdTRUE,
( void * ) 0,
vTimerButtonCallback,
&xBufferTimerButton);
xTimerStart(xHandleTimerButton,0 );
vTaskResume(xHandleBattery);
}
/**
* @brief Read charger status pins and return corresponding charger inner state
*
* @param None
* @retval Charger current state
*/
BATTERY_StatusChargerTypedef BATTERY_GetChargerStatus(void) {
uint32_t st2 = LL_GPIO_ReadInputPort(CHARGER_ST2_GPIO_Port) & CHARGER_ST2_Pin;
uint32_t st1 = LL_GPIO_ReadInputPort(CHARGER_ST1_GPIO_Port) & CHARGER_ST1_Pin;
BATTERY_StatusChargerTypedef status;
if (st1 && st2)
status = CHARGER_NOT_PLUGGED;
else if (st1 && !st2)
status = CHARGER_CHARGE_COMPLETE;
else if (!st1 && st2)
status = CHARGER_IN_CHARGE;
else /* !st1 && !st2 */
status = CHARGER_ERROR;
return status;
}
/**
* @brief Start a voltage acquisition and wait for conversion to end
*
* @param[out] val Battery voltage (raw adc value)
* @return
* - 0 in case of success
* - -1 if unable to start ADC
* - -2 if timeout occured waiting for conversion to end
*/
int BATTERY_GetVoltage(uint16_t *val) {
uint32_t ulNotificationValue;
conversion_complete = 0;
adc_raw_value = 0;
task_handler = xTaskGetCurrentTaskHandle();
if (HAL_ADC_Start_IT(&hadc) != HAL_OK)
return -1;
ulNotificationValue = ulTaskNotifyTake( pdTRUE, pdMS_TO_TICKS(100)); // wait max 100 ms
if (ulNotificationValue == 1) {
/* The transmission ended as expected. */
*val = adc_raw_value;
} else {
/* The call to ulTaskNotifyTake() timed out. */
return -2;
}
task_handler = NULL;
return 0;
}
/*
* Il faut considerer ces valeurs comme les seuils de baculement dans une categorie
* ou une autre
*
* Seuil : critical low high
* Tension batterie: 2.9 critic 3.1 low 3.3 med 3.6 high 4.2
*
*/
#ifdef TESTS
uint8_t BATTERY_LEVEL_CRITICAL=135;
uint8_t BATTERY_LEVEL_LOW=145;
uint8_t BATTERY_LEVEL_HIGH=155;
uint8_t BATTERY_LEVEL_CHARGE_LOW=150;
uint8_t BATTERY_LEVEL_CHARGE_HIGH=170;
uint8_t BATTERIE_currentValue;
#else
#define BATTERY_LEVEL_CRITICAL 135
#define BATTERY_LEVEL_LOW 145
#define BATTERY_LEVEL_HIGH 155
#define BATTERY_LEVEL_CHARGE_LOW 150
#define BATTERY_LEVEL_CHARGE_HIGH 170
#endif /* TESTS */
/**
* @brief Convert battery voltage into several ranges (level) depending on current charger status (charging or not)
* and return corresponding message id to send to application.
*
* @param[in] voltage Battery voltage (raw adc value)
* @param[in] charger status
* @return message id to be sent to application mailbox
*/
uint16_t BATTERY_BatteryLevel(uint8_t voltage, BATTERY_StatusChargerTypedef chargerStatus) {
uint16_t msgId=0;
#ifdef TESTS
BATTERY_currentValue=voltage;
#endif /* TESTS */
switch (chargerStatus) {
case CHARGER_CHARGE_COMPLETE:
msgId = MSG_ID_BAT_CHARGE_COMPLETE;
break;
case CHARGER_IN_CHARGE:
if (voltage<=BATTERY_LEVEL_CHARGE_LOW)
msgId = MSG_ID_BAT_CHARGE_LOW;
else if (voltage>=BATTERY_LEVEL_CHARGE_HIGH)
msgId = MSG_ID_BAT_CHARGE_HIGH;
else
msgId = MSG_ID_BAT_CHARGE_MED;
break;
case CHARGER_NOT_PLUGGED:
if (voltage<=BATTERY_LEVEL_CRITICAL)
msgId = MSG_ID_BAT_CRITICAL_LOW;
else if (voltage<=BATTERY_LEVEL_LOW)
msgId = MSG_ID_BAT_LOW;
else if (voltage>=BATTERY_LEVEL_HIGH)
msgId = MSG_ID_BAT_HIGH;
else
msgId = MSG_ID_BAT_MED;
break;
default:
msgId = MSG_ID_BAT_CHARGE_ERR;
}
return msgId;
}
/**
* @brief Battery main thread. Periodically , check charger status and battery level.
* In case of any change (level or charger status), send a message to application mailbox.
* Delay is provided by BATTERY_POLLING_DELAY constant (1 second)
*
* @param[in] params startup parameters for task (not used)
* @return None
*/
void BATTERY_Thread(void* params) {
static uint16_t voltage;
static uint8_t batteryErrorCnt=0;
BATTERY_StatusChargerTypedef currentStatus;
uint16_t messageID;
TickType_t xLastWakeTime;
// Initialise the xLastWakeTime variable with the current time.
xLastWakeTime = xTaskGetTickCount();
while (1) {
if (BATTERY_GetVoltage(&voltage) ==0) {
currentStatus = BATTERY_GetChargerStatus();
if (currentStatus == CHARGER_ERROR) {
batteryErrorCnt++;
if (batteryErrorCnt>=BATTERY_MAX_ERROR)
MESSAGE_SendMailbox(APPLICATION_Mailbox, MSG_ID_BAT_CHARGE_ERR, (QueueHandle_t)0x0, (void*)NULL);
} else {
messageID = BATTERY_BatteryLevel(voltage, currentStatus);
MESSAGE_SendMailbox(APPLICATION_Mailbox, messageID, (QueueHandle_t)0x0, (void*)NULL);
}
#ifdef TESTS
MESSAGE_SendMailbox(APPLICATION_Mailbox, MSG_ID_BAT_LEVEL, (QueueHandle_t)0x0, (void*)&voltage);
#endif /* TESTS*/
} else {
MESSAGE_SendMailbox(APPLICATION_Mailbox, MSG_ID_BAT_ADC_ERR, (QueueHandle_t)0x0, (void*)0x0);
}
// Wait for the next cycle.
vTaskDelayUntil( &xLastWakeTime, pdMS_TO_TICKS(BATTERY_POLLING_DELAY));
}
}
/**
* @brief Callback when ADC conversion end. Store converted value
*
* @param[in] hadc ADC informations
* @return None
*/
void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef *hadc) {
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
adc_raw_value = HAL_ADC_GetValue(hadc);
if (task_handler != NULL) {
/* Notify the task that an event has been emitted. */
vTaskNotifyGiveFromISR(task_handler, &xHigherPriorityTaskWoken );
/* There are no more eventin progress, so no tasks to notify. */
task_handler = NULL;
/* If xHigherPriorityTaskWoken is now set to pdTRUE then a context switch
should be performed to ensure the interrupt returns directly to the highest
priority task. The macro used for this purpose is dependent on the port in
use and may be called portEND_SWITCHING_ISR(). */
portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
}
}
/**
* @brief One-shot timer used to avoid on-off button to trigger event at startup
*
* When system power-up, user may still press on/off button while corresponding EXTI is enabled, triggering an interruption that
* will send a power-off event, disabling system. To avoid this behavior, button events are not accounted during the first 1.5s.
* This timer is used to disable "BUTTON_Inactivity" flag, restoring normal behavior
* @param None
* @retval None
*/
void vTimerButtonCallback( TimerHandle_t xTimer ) {
BUTTON_Inactivity=0;
xTimerStop(xHandleTimerButton,0 );
}
/**
* @brief Interrupt handler for external IT (raised by on/off button).
*
* @param[in] GPIO_Pin GPIO pin number that raised interruption
* @return None
*/
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
// if (GPIO_Pin == USB_SENSE_Pin) { // Le chargeur vient d'etre branché ou debranché
// if (HAL_GPIO_ReadPin(USB_SENSE_GPIO_Port, GPIO_Pin)==GPIO_PIN_SET) // le chargeur est branché
// MESSAGE_SendMailboxFromISR(APPLICATION_Mailbox, MSG_ID_BAT_CHARGE_ON, (QueueHandle_t)0x0, 0x0, &xHigherPriorityTaskWoken);
// else
// MESSAGE_SendMailboxFromISR(APPLICATION_Mailbox, MSG_ID_BAT_CHARGE_OFF, (QueueHandle_t)0x0, 0x0, &xHigherPriorityTaskWoken);
// }
// else
if (GPIO_Pin == BUTTON_SENSE_Pin) { // on vient d'appuyer sur le bouton on/off
if (!BUTTON_Inactivity) {
if (HAL_GPIO_ReadPin(BUTTON_SENSE_GPIO_Port, GPIO_Pin)==GPIO_PIN_RESET) // GPIOB.3 = 0 => le bouton est appuyé
MESSAGE_SendMailboxFromISR(APPLICATION_Mailbox, MSG_ID_BUTTON_PRESSED, (QueueHandle_t)0x0, 0x0, &xHigherPriorityTaskWoken);
}
}
if (xHigherPriorityTaskWoken) {
/* If xHigherPriorityTaskWoken is now set to pdTRUE then a context switch
should be performed to ensure the interrupt returns directly to the highest
priority task. The macro used for this purpose is dependent on the port in
use and may be called portEND_SWITCHING_ISR(). */
portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
}
}
/**
* @}
*/
/**
* @}
*/
/**
* @}
*/

62
software/dumber3/Application/battery.h Normal file
View file

@ -0,0 +1,62 @@
/**
******************************************************************************
* @file battery.c
* @brief battery driver header
* @author S. DI MERCURIO (dimercur@insa-toulouse.fr)
* @date December 2023
*
******************************************************************************
* @copyright Copyright 2023 INSA-GEI, Toulouse, France. All rights reserved.
* @copyright This project is released under the Lesser GNU Public License (LGPL-3.0-only).
*
* @copyright This file is part of "Dumber" project
*
* @copyright This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 3 of the License, or (at your option) any later version.
*
* @copyright This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
* @copyright You should have received a copy of the GNU Lesser General Public License
* along with this program; if not, write to the Free Software Foundation,
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
******************************************************************************
*/
#ifndef INC_BATTERY_H_
#define INC_BATTERY_H_
#include "application.h"
/** @addtogroup Application_Software
* @{
*/
/** @addtogroup BATTERY
* @{
*/
/** @addtogroup BATTERY_Public Public
* @{
*/
void BATTERY_Init(void);
/**
* @}
*/
/**
* @}
*/
/**
* @}
*/
#endif /* INC_BATTERY_H_ */

176
software/dumber3/Application/commands.c
View file

@ -1,8 +1,31 @@
/*
* cmd.c
/**
******************************************************************************
* @file commands.c
* @brief commands handler body
* @author S. DI MERCURIO (dimercur@insa-toulouse.fr)
* @date December 2023
*
* Created on: 3 oct. 2022
* Author: dimercur
******************************************************************************
* @copyright Copyright 2023 INSA-GEI, Toulouse, France. All rights reserved.
* @copyright This project is released under the Lesser GNU Public License (LGPL-3.0-only).
*
* @copyright This file is part of "Dumber" project
*
* @copyright This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 3 of the License, or (at your option) any later version.
*
* @copyright This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
* @copyright You should have received a copy of the GNU Lesser General Public License
* along with this program; if not, write to the Free Software Foundation,
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
******************************************************************************
*/
#include "commands.h"
@ -10,8 +33,23 @@
#include <string.h>
#include <stdio.h>
/* Definition des commandes */
/** @addtogroup Application_Software
* @{
*/
/** @addtogroup COMMANDS
* Commands handler is in charge of decoding received commands and building answer frames.
* @{
*/
/** @addtogroup COMMANDS_Private Private
* @{
*/
/** @name Commands definition
* List of accepted command identifiers
*/
///@{
#define PingCMD 'p'
#define ResetCMD 'r'
#define SetMotorCMD 'm'
@ -26,28 +64,34 @@
#define TestCMD 't'
#define DebugCMD 'a'
#define PowerOffCMD 'z'
///@}
/** @name Answers definition
* List of available answers
*/
///@{
#define OK_ANS "O\r"
#define ERR_ANS "E\r"
#define UNKNOW_ANS "C\r"
#define BAT_OK "2\r"
#define BAT_LOW "1\r"
#define BAT_EMPTY "0\r"
///@}
char* cmdAddChecksum(const char* str);
char cmdVerifyChecksum(char* str);
/** @addtogroup Checksum
* @{
*/
/**
* @brief Inclut le checksum à sendString
* @brief Add checksum to a string
*
* Parcours str pour y calculer le checksum ( xor sur chaque caractére)
* et inclut le resultat en fin de chaine.
* Make a copy of a string and add checksum at its end (xor based).
*
* @param string sans checksum
* @retval string avec checksum
* @warning This function use memory allocation for creating a copy of the original string. Be sure to free memory after use.
*
* @todo Error related to memory allocation (allocation failed) is not managed -> should generate a Panic
*
* @param[in] str string without checksum
* @return string with checksum added
*
*/
char* cmdAddChecksum(const char* str) {
@ -70,14 +114,15 @@ char* cmdAddChecksum(const char* str) {
}
/**
* @brief Verifie le checksum de la variable global recepitString
* @brief Verify checksum of given string
*
* Vérifie le dernier carctére de str sensé être le checksum.
* Si celui-ci est bon, ll retournera 0 et supprimera le checksum de str
* sinon il retournera 1 sans faire de modification.
* @param None
* @retval 0 si le checksum est faux, 1 sinon
* Verify if checksum of given string is correct or not. In case of success,
* function will suppress checksum from the string, otherwise string is not modified.
*
* @param[in] str string to verify checksum
* @return
* - 0 if checksum is not correct
* - 1 if successful
*/
char cmdVerifyChecksum(char* str) {
uint16_t j;
@ -112,9 +157,25 @@ char cmdVerifyChecksum(char* str) {
}
/**
* @}
* @brief Decode received command
*
* Command string is processed and a command structure is filled with information found in string.
*
* @warning This function use memory allocation for returning structure. Be sure to release memory after use.
*
* @param[in] cmd string with command received
* @param[in] length length of command string
* @return
* - NULL if memory allocation has failed
* - CMD_Move if string contains MOVE command
* - CMD_Turn if string contains TURN command
* - CMD_Generic for each other command
*
* Generic structure returned may contains CMD_NONE if command is unknown or CMD_INVALID_CHECKSUM if checksum is not valid
*
* @remark Returned value is always cast on a CMD_Generic type, but may contains CMD_Move or CMD_Turn structure. It is
* user responsibility to check type field inside structure and cast accordingly returned value to correct structure.
*/
CMD_Generic* cmdDecode(char* cmd, uint8_t length) {
CMD_Generic* decodedCmd;
char cmd_type = cmd[0];
@ -214,6 +275,19 @@ CMD_Generic* cmdDecode(char* cmd, uint8_t length) {
return decodedCmd;
}
/**
* @brief Add correct checksum to provided answer and send it to XBEE mailbox
*
* @warning This function use cmdAddChecksum, so indirectly make use of memory allocation for sending message to XBEE mailbox.
* Be sure to release memory in XBEE driver after retrieving message from mailbox.
*
* @todo Error related to memory allocation (allocation failed) is not managed -> should generate a Panic
*
* @todo Maybe duplication between this function and cmdSendString: see if only one function with a macro/wrapper could be used
*
* @param[in] ans string containing answer to send, without checksum
* @return None
*/
void cmdSendAnswer(uint8_t ans) {
char* answer;
@ -232,6 +306,19 @@ void cmdSendAnswer(uint8_t ans) {
MESSAGE_SendMailbox(XBEE_Mailbox, MSG_ID_XBEE_ANS, APPLICATION_Mailbox, (char*) answer);
}
/**
* @brief Send arbitrary answer to XBEE driver and add checksum before
*
* @warning This function use cmdAddChecksum, so indirectly make use of memory allocation for sending message to XBEE mailbox.
* Be sure to release memory in XBEE driver after retrieving message from mailbox.
*
* @todo Error related to memory allocation (allocation failed) is not managed -> should generate a Panic
*
* @todo Maybe duplication between this function and cmdSendAnswer: see if only one function with a macro/wrapper could be used
*
* @param[in] str string containing answer to send without checksum
* @return None
*/
void cmdSendString(char *str) {
char* answer;
int strlength = strlen(str);
@ -247,6 +334,19 @@ void cmdSendString(char *str) {
MESSAGE_SendMailbox(XBEE_Mailbox, MSG_ID_XBEE_ANS, APPLICATION_Mailbox, (char*) answer);
}
/**
* @brief Send battery level answer with correct checksum to XBEE driver
*
* @warning This function use cmdAddChecksum, so indirectly make use of memory allocation for sending message to XBEE mailbox.
* Be sure to release memory in XBEE driver after retrieving message from mailbox.
*
* @todo Error related to memory allocation (allocation failed) is not managed -> should generate a Panic
*
* @todo Check batterystate type used here: seems a bit overkill (16bit for only 6 states) -> enum should be better
*
* @param[in] batteryState current battery state
* @return None
*/
void cmdSendBatteryLevel(uint16_t batteryState) {
char* answer;
@ -268,6 +368,17 @@ void cmdSendBatteryLevel(uint16_t batteryState) {
MESSAGE_SendMailbox(XBEE_Mailbox, MSG_ID_XBEE_ANS, APPLICATION_Mailbox, (char*) answer);
}
/**
* @brief Send version number answer with correct checksum to XBEE driver
*
* @warning This function use cmdAddChecksum, so indirectly make use of memory allocation for sending message to XBEE mailbox.
* Be sure to release memory in XBEE driver after retrieving message from mailbox.
*
* @todo Error related to memory allocation (allocation failed) is not managed -> should generate a Panic
*
* @param None
* @return None
*/
void cmdSendVersion(void) {
int versionlength = strlen(SYSTEM_VERSION_STR);
char versionstr[versionlength+2];
@ -282,6 +393,17 @@ void cmdSendVersion(void) {
MESSAGE_SendMailbox(XBEE_Mailbox, MSG_ID_XBEE_ANS, APPLICATION_Mailbox, (char*) answer);
}
/**
* @brief Send if robot is busy (moving) or not XBEE driver
*
* @warning This function use cmdAddChecksum, so indirectly make use of memory allocation for sending message to XBEE mailbox.
* Be sure to release memory in XBEE driver after retrieving message from mailbox.
*
* @todo Error related to memory allocation (allocation failed) is not managed -> should generate a Panic
*
* @param[in] state current robot state
* @return None
*/
void cmdSendBusyState(uint8_t state) {
char* answer;
@ -292,3 +414,15 @@ void cmdSendBusyState(uint8_t state) {
MESSAGE_SendMailbox(XBEE_Mailbox, MSG_ID_XBEE_ANS, APPLICATION_Mailbox, (char*) answer);
}
/**
* @}
*/
/**
* @}
*/
/**
* @}
*/

146
software/dumber3/Application/commands.h
View file

@ -1,8 +1,31 @@
/*
* cmd.h
/**
******************************************************************************
* @file commands.h
* @brief commands handler header
* @author S. DI MERCURIO (dimercur@insa-toulouse.fr)
* @date December 2023
*
* Created on: 3 oct. 2022
* Author: dimercur
******************************************************************************
* @copyright Copyright 2023 INSA-GEI, Toulouse, France. All rights reserved.
* @copyright This project is released under the Lesser GNU Public License (LGPL-3.0-only).
*
* @copyright This file is part of "Dumber" project
*
* @copyright This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 3 of the License, or (at your option) any later version.
*
* @copyright This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
* @copyright You should have received a copy of the GNU Lesser General Public License
* along with this program; if not, write to the Free Software Foundation,
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
******************************************************************************
*/
#ifndef INC_CMD_H_
@ -10,74 +33,83 @@
#include "application.h"
/** @addtogroup Application_Software
* @{
*/
/** @addtogroup COMMANDS
* @{
*/
/** @addtogroup COMMANDS_Public Public
* @{
*/
/** Enumeration class defining commands to be used in \ref CMD_Generic, \ref CMD_Move, \ref CMD_Turn structures */
typedef enum {
CMD_NONE=0x0,
CMD_PING,
CMD_RESET,
CMD_START_WITH_WATCHDOG,
CMD_RESET_WATCHDOG,
CMD_GET_BATTERY,
CMD_GET_VERSION,
CMD_START_WITHOUT_WATCHDOG,
CMD_MOVE,
CMD_TURN,
CMD_GET_BUSY_STATE,
CMD_TEST,
CMD_DEBUG,
CMD_POWER_OFF,
CMD_INVALID_CHECKSUM=0xFF
CMD_NONE=0x0, /**< Command is unknown */
CMD_PING, /**< PING command */
CMD_RESET, /**< RESET command */
CMD_START_WITH_WATCHDOG, /**< START_WITH_WATCHDOG command */
CMD_RESET_WATCHDOG, /**< RESET_WATCHDOG command */
CMD_GET_BATTERY, /**< GET_BATTERY command */
CMD_GET_VERSION, /**< GET_VERSION command */
CMD_START_WITHOUT_WATCHDOG, /**< START_WITHOUT_WATCHDOG command */
CMD_MOVE, /**< MOVE command */
CMD_TURN, /**< TURN command */
CMD_GET_BUSY_STATE, /**< GET_BUSY_STATE command */
CMD_TEST, /**< TEST command (not used yet) */
CMD_DEBUG, /**< DEBUG command (not used yet) */
CMD_POWER_OFF, /**< POWER_OFF command */
CMD_INVALID_CHECKSUM=0xFF /**< Received command string has an invalid checksum*/
} CMD_CommandsType;
/** Enumeration class defining possible generic answers */
typedef enum {
ANS_OK=0x80,
ANS_ERR,
ANS_UNKNOWN
ANS_OK=0x80, /**< Answer OK, for a correct and processed command */
ANS_ERR, /**< Answer ERR, for rejected command (not accepted in current state), or invalid parameters */
ANS_UNKNOWN /**< Answer UNKNOWN, for unknown command */
} CMD_AnswersType;
/** Enumeration class defining battery levels */
typedef enum {
ANS_BAT_EMPTY=0,
ANS_BAT_LOW,
ANS_BAT_OK
ANS_BAT_EMPTY=0, /**< Battery is empty, system will power off in seconds */
ANS_BAT_LOW, /**< Battery is low, should plug charger quickly */
ANS_BAT_OK /**< Charger is in correct state, charge is not required yet */
} CMD_BatteryLevelType;
/** Enumeration class defining busty states */
typedef enum {
ANS_STATE_NOT_BUSY=0,
ANS_STATE_BUSY
ANS_STATE_NOT_BUSY=0, /**< Robot is not moving, ready for processing new movement */
ANS_STATE_BUSY /**< Robot is moving, not accepting new movement requests */
} CMD_BusyType;
/** Structure class used by cmdDecode function for generic commands
*
* @todo type of "type" field should be \ref CMD_CommandsType and not uint8_t
*/
typedef struct __attribute__((packed)) {
uint8_t type;
uint8_t type; /**< Command type, as found in \ref CMD_CommandsType enum */
} CMD_Generic;
/** Structure class used by cmdDecode function for MOVE command
*
* @todo type of "type" field should be \ref CMD_CommandsType and not uint8_t
*/
typedef struct __attribute__((packed)) {
uint8_t type;
int16_t distance;
uint8_t type; /**< Command type, as found in \ref CMD_CommandsType enum (should be \ref CMD_MOVE)*/
int16_t distance; /**< Distance for movement, positive for forward, negative for backward. Expressed in millimeters */
} CMD_Move;
/** Structure class used by cmdDecode function for TURN command
*
* @todo type of "type" field should be \ref CMD_CommandsType and not uint8_t
*/
typedef struct __attribute__((packed)) {
uint8_t type;
int16_t turns;
uint8_t type; /**< Command type, as found in \ref CMD_CommandsType enum (should be \ref CMD_TURN)*/
int16_t turns; /**< Angle of rotation, positive for clockwise and negative for counter-clockwise. Expressed in degree */
} CMD_Turn;
typedef struct __attribute__((packed)) {
uint8_t ans;
} ANS_Generic;
typedef struct __attribute__((packed)) {
uint8_t ans;
uint16_t version;
} ANS_Version;
typedef struct __attribute__((packed)) {
uint8_t ans;
uint8_t bat_level;
} ANS_Battery;
typedef struct __attribute__((packed)) {
uint8_t ans;
uint8_t state;
} ANS_Busy_State;
CMD_Generic* cmdDecode(char* cmd, uint8_t length);
void cmdSendAnswer(uint8_t ans);
void cmdSendString(char* str);
@ -85,4 +117,16 @@ void cmdSendBatteryLevel(uint16_t batteryState);
void cmdSendVersion(void);
void cmdSendBusyState(uint8_t state);
/**
* @}
*/
/**
* @}
*/
/**
* @}
*/
#endif /* INC_CMD_H_ */

102
software/dumber3/Application/config.h
View file

@ -1,40 +1,90 @@
/*
* config.h
/**
******************************************************************************
* @file config.h
* @brief global configuration header
* @author S. DI MERCURIO (dimercur@insa-toulouse.fr)
* @date December 2023
*
* Created on: 14 sept. 2022
* Author: dimercur
******************************************************************************
* @copyright Copyright 2023 INSA-GEI, Toulouse, France. All rights reserved.
* @copyright This project is released under the Lesser GNU Public License (LGPL-3.0-only).
*
* @copyright This file is part of "Dumber" project
*
* @copyright This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 3 of the License, or (at your option) any later version.
*
* @copyright This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
* @copyright You should have received a copy of the GNU Lesser General Public License
* along with this program; if not, write to the Free Software Foundation,
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
******************************************************************************
*/
#include "stm32l0xx_hal.h"
#include "cmsis_os.h"
// current version is 2.1
/** @addtogroup Application_Software
* @{
*/
/** @addtogroup CONFIG
* Constants for global system configuration.
* @{
*/
/** @name System version
* Version in plain text and as a numeric value
*/
///@{
#define SYSTEM_VERSION_STR "2.1"
#define SYSTEM_VERSION 0x0201 // Upper byte: major version, lower byte: minor version
///@}
#define STACK_SIZE 0x100
/** @name Tasks priority constants
* Priority is based on configMAX_PRIORITIES which represent highest task priority.
* Less priority task is \ref PriorityTestsHandler
*/
///@{
#define PriorityLedsAction (configMAX_PRIORITIES -1)
#define PriorityXbeeRX (configMAX_PRIORITIES -2)
#define PriorityXbeeTX (configMAX_PRIORITIES -3)
#define PriorityLedsHandler (configMAX_PRIORITIES -4)
#define PriorityMoteursAsservissement (configMAX_PRIORITIES -5)
#define PriorityMoteursHandler (configMAX_PRIORITIES -6)
#define PriorityBatterieHandler (configMAX_PRIORITIES -7)
#define PriorityMotorsAsservissement (configMAX_PRIORITIES -5)
#define PriorityMotorsHandler (configMAX_PRIORITIES -6)
#define PriorityBatteryHandler (configMAX_PRIORITIES -7)
#define PriorityApplicationHandler (configMAX_PRIORITIES -8)
#define PrioritySequenceurTimeout (configMAX_PRIORITIES -9)
#define PriorityTestsHandler (configMAX_PRIORITIES -10)
///@}
/* Périodes des taches périodiques (en ms) */
#define MOTEURS_PERIODE_ASSERVISSEMENT (2)
#define APPLICATION_PERIODE (100)
#define BATTERIE_PERIODE_SCRUTATION (1000)
#define LEDS_PERIODE (100)
#define BUTTON_INACTIVITY_PERIODE (1500)
/** @name Periodicities of tasks
* Periodicities are given in ms.
*/
///@{
#define MOTORS_REGULATION_DELAY (2)
#define APPLICATION_COUNTERS_DELAY (100)
#define BATTERY_POLLING_DELAY (1000)
#define LEDS_DELAY (100)
#define BUTTON_INACTIVITY_DELAY (1500)
#define TESTS_PERIODE (5000)
#define TESTS_DELAY (5000)
///@}
/** @name Constants for to XBEE module
* Constants about timeout and buffer lengths
*/
///@{
#define XBEE_TX_SEMAPHORE_WAIT 500
#ifdef TESTS
#define XBEE_RX_BUFFER_MAX_LENGTH 100
@ -43,13 +93,12 @@
#define XBEE_RX_BUFFER_MAX_LENGTH 50
#define XBEE_TX_BUFFER_MAX_LENGTH 50
#endif /* TESTS */
///@}
#define BATTERY_EMPTY_LEVEL 128
#define BATTERY_LOW_LEVEL 140
/*
* Revoir les délais : c'est en 100 ms, pas ms
/** @name Constants relative to timeout
* Delays are given in ms.
*/
///@{
#ifdef TESTS
#define APPLICATION_INACTIVITY_TIMEOUT (0xFFFFFFFF) // Max U32, infinite timeout
#else
@ -59,6 +108,15 @@
#define APPLICATION_WATCHDOG_MIN (900) // minimum time to wait before resetting watchdog, expressed in ms
#define APPLICATION_WATCHDOG_MAX (1100) // maximum time to wait before resetting watchdog, expressed in ms
#define APPLICATION_WATCHDOG_MISSED_MAX (3) // Maximum missed timeout reset before entering watchdog disabled state
#define APPLICATION_STARTUP_DELAY (3*1000) // Startup delay, expressed in ms
#define APPLICATION_STARTUP_DELAY (3*1000) // Startup delay before entering idle state (3s)
///@}
TickType_t msToTicks(TickType_t ms);
/**
* @}
*/
/**
* @}
*/

173
software/dumber3/Application/leds.c
View file

@ -1,39 +1,89 @@
/*
* leds.h
/**
******************************************************************************
* @file leds.c
* @brief leds driver body
* @author S. DI MERCURIO (dimercur@insa-toulouse.fr)
* @date December 2023
*
* Created on: Sep 12, 2022
* Author: dimercur
*/
******************************************************************************
* @copyright Copyright 2023 INSA-GEI, Toulouse, France. All rights reserved.
* @copyright This project is released under the Lesser GNU Public License (LGPL-3.0-only).
*
* @copyright This file is part of "Dumber" project
*
* @copyright This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 3 of the License, or (at your option) any later version.
*
* @copyright This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
* @copyright You should have received a copy of the GNU Lesser General Public License
* along with this program; if not, write to the Free Software Foundation,
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
******************************************************************************
*/
#include "leds.h"
#define LEDS_Allume_Seg_A() HAL_GPIO_WritePin(LED_SEG_A_GPIO_Port, LED_SEG_A_Pin, GPIO_PIN_SET)
#define LEDS_Allume_Seg_B() HAL_GPIO_WritePin(LED_SEG_B_GPIO_Port, LED_SEG_B_Pin, GPIO_PIN_SET)
#define LEDS_Allume_Seg_C() HAL_GPIO_WritePin(LED_SEG_C_GPIO_Port, LED_SEG_C_Pin, GPIO_PIN_SET)
#define LEDS_Allume_Seg_D() HAL_GPIO_WritePin(LED_SEG_D_GPIO_Port, LED_SEG_D_Pin, GPIO_PIN_SET)
#define LEDS_Allume_Seg_E() HAL_GPIO_WritePin(LED_SEG_E_GPIO_Port, LED_SEG_E_Pin, GPIO_PIN_SET)
#define LEDS_Allume_Seg_F() HAL_GPIO_WritePin(LED_SEG_F_GPIO_Port, LED_SEG_F_Pin, GPIO_PIN_SET)
#define LEDS_Allume_Seg_G() HAL_GPIO_WritePin(LED_SEG_G_GPIO_Port, LED_SEG_G_Pin, GPIO_PIN_SET)
#define LEDS_Allume_Seg_DP() HAL_GPIO_WritePin(LED_SEG_DP_GPIO_Port, LED_SEG_DP_Pin, GPIO_PIN_SET)
/** @addtogroup Application_Software
* @{
*/
#define LEDS_Eteint_Seg_A() HAL_GPIO_WritePin(LED_SEG_A_GPIO_Port, LED_SEG_A_Pin, GPIO_PIN_RESET)
#define LEDS_Eteint_Seg_B() HAL_GPIO_WritePin(LED_SEG_B_GPIO_Port, LED_SEG_B_Pin, GPIO_PIN_RESET)
#define LEDS_Eteint_Seg_C() HAL_GPIO_WritePin(LED_SEG_C_GPIO_Port, LED_SEG_C_Pin, GPIO_PIN_RESET)
#define LEDS_Eteint_Seg_D() HAL_GPIO_WritePin(LED_SEG_D_GPIO_Port, LED_SEG_D_Pin, GPIO_PIN_RESET)
#define LEDS_Eteint_Seg_E() HAL_GPIO_WritePin(LED_SEG_E_GPIO_Port, LED_SEG_E_Pin, GPIO_PIN_RESET)
#define LEDS_Eteint_Seg_F() HAL_GPIO_WritePin(LED_SEG_F_GPIO_Port, LED_SEG_F_Pin, GPIO_PIN_RESET)
#define LEDS_Eteint_Seg_G() HAL_GPIO_WritePin(LED_SEG_G_GPIO_Port, LED_SEG_G_Pin, GPIO_PIN_RESET)
#define LEDS_Eteint_Seg_DP() HAL_GPIO_WritePin(LED_SEG_DP_GPIO_Port, LED_SEG_DP_Pin, GPIO_PIN_RESET)
/** @addtogroup LEDS
* Leds handler is in charge of leds animation.
*
* Leds module consiste of two threads:
* - \ref LEDS_HandlerThread in charge of waiting for message in mailbox from application. Depending of the message received, animation is started, modified or stop
* - \ref LEDS_ActionThread, periodic task in charge of animating leds with configured sprites for given animation
* @{
*/
#define LEDS_Eteint_Tout() HAL_GPIO_WritePin(GPIOB, LED_SEG_A_Pin|LED_SEG_B_Pin|LED_SEG_C_Pin, GPIO_PIN_RESET);\
/** @addtogroup LEDS_Private Private
* @{
*/
/** @name Macro for switching ON and OFF individual led segments
*
*/
///@{
#define LEDS_On_Seg_A() HAL_GPIO_WritePin(LED_SEG_A_GPIO_Port, LED_SEG_A_Pin, GPIO_PIN_SET)
#define LEDS_On_Seg_B() HAL_GPIO_WritePin(LED_SEG_B_GPIO_Port, LED_SEG_B_Pin, GPIO_PIN_SET)
#define LEDS_On_Seg_C() HAL_GPIO_WritePin(LED_SEG_C_GPIO_Port, LED_SEG_C_Pin, GPIO_PIN_SET)
#define LEDS_On_Seg_D() HAL_GPIO_WritePin(LED_SEG_D_GPIO_Port, LED_SEG_D_Pin, GPIO_PIN_SET)
#define LEDS_On_Seg_E() HAL_GPIO_WritePin(LED_SEG_E_GPIO_Port, LED_SEG_E_Pin, GPIO_PIN_SET)
#define LEDS_On_Seg_F() HAL_GPIO_WritePin(LED_SEG_F_GPIO_Port, LED_SEG_F_Pin, GPIO_PIN_SET)
#define LEDS_On_Seg_G() HAL_GPIO_WritePin(LED_SEG_G_GPIO_Port, LED_SEG_G_Pin, GPIO_PIN_SET)
#define LEDS_On_Seg_DP() HAL_GPIO_WritePin(LED_SEG_DP_GPIO_Port, LED_SEG_DP_Pin, GPIO_PIN_SET)
#define LEDS_Off_Seg_A() HAL_GPIO_WritePin(LED_SEG_A_GPIO_Port, LED_SEG_A_Pin, GPIO_PIN_RESET)
#define LEDS_Off_Seg_B() HAL_GPIO_WritePin(LED_SEG_B_GPIO_Port, LED_SEG_B_Pin, GPIO_PIN_RESET)
#define LEDS_Off_Seg_C() HAL_GPIO_WritePin(LED_SEG_C_GPIO_Port, LED_SEG_C_Pin, GPIO_PIN_RESET)
#define LEDS_Off_Seg_D() HAL_GPIO_WritePin(LED_SEG_D_GPIO_Port, LED_SEG_D_Pin, GPIO_PIN_RESET)
#define LEDS_Off_Seg_E() HAL_GPIO_WritePin(LED_SEG_E_GPIO_Port, LED_SEG_E_Pin, GPIO_PIN_RESET)
#define LEDS_Off_Seg_F() HAL_GPIO_WritePin(LED_SEG_F_GPIO_Port, LED_SEG_F_Pin, GPIO_PIN_RESET)
#define LEDS_Off_Seg_G() HAL_GPIO_WritePin(LED_SEG_G_GPIO_Port, LED_SEG_G_Pin, GPIO_PIN_RESET)
#define LEDS_Off_Seg_DP() HAL_GPIO_WritePin(LED_SEG_DP_GPIO_Port, LED_SEG_DP_Pin, GPIO_PIN_RESET)
///@}
/** @name Macro for switching all display ON or OFF
*
*/
///@{
#define LEDS_All_Off() HAL_GPIO_WritePin(GPIOB, LED_SEG_A_Pin|LED_SEG_B_Pin|LED_SEG_C_Pin, GPIO_PIN_RESET);\
HAL_GPIO_WritePin(GPIOA, LED_SEG_D_Pin|LED_SEG_E_Pin|LED_SEG_F_Pin|LED_SEG_G_Pin|LED_SEG_DP_Pin, GPIO_PIN_RESET)
#define LEDS_Allume_Tout() HAL_GPIO_WritePin(GPIOB, LED_SEG_A_Pin|LED_SEG_B_Pin|LED_SEG_C_Pin, GPIO_PIN_SET);\
#define LEDS_All_On() HAL_GPIO_WritePin(GPIOB, LED_SEG_A_Pin|LED_SEG_B_Pin|LED_SEG_C_Pin, GPIO_PIN_SET);\
HAL_GPIO_WritePin(GPIOA, LED_SEG_D_Pin|LED_SEG_E_Pin|LED_SEG_F_Pin|LED_SEG_G_Pin|LED_SEG_DP_Pin, GPIO_PIN_SET)
///@}
#define LEDS_Allume_C() HAL_GPIO_WritePin(GPIOB, LED_SEG_A_Pin|LED_SEG_B_Pin|LED_SEG_C_Pin, GPIO_PIN_SET);\
HAL_GPIO_WritePin(GPIOA, LED_SEG_D_Pin, GPIO_PIN_SET)
/** @name List of single sprite (pattern) in animation
*
*/
///@{
#define LED_PATTERN_ALL_OFF 0
#define LED_PATTERN_BAT_SPRITE_0 1
#define LED_PATTERN_BAT_SPRITE_1 2
@ -73,6 +123,7 @@
#define LED_PATTERN_DIGIT_UNKNOWN 36
#define LED_MAX_PATTERNS 37
///@}
/*
* Relation entre segment et nom
@ -97,6 +148,9 @@
*
*/
/** @brief Constant array defining led configuration for all possible sprites used in animation
*
*/
uint16_t LEDS_Patterns [LED_MAX_PATTERNS][4]= {
// GPIOA ON / GPIOB ON / GPIOA OFF / GPIOB OFF
{ 0, 0, LED_SEG_D_Pin|LED_SEG_E_Pin|LED_SEG_F_Pin|LED_SEG_G_Pin|LED_SEG_DP_Pin, LED_SEG_A_Pin|LED_SEG_B_Pin|LED_SEG_C_Pin}, // All Off
@ -162,8 +216,14 @@ void LEDS_ShowPattern(uint8_t pattern);
void LEDS_Tests(void* params);
void LEDS_HandlerThread(void* params);
/**
* @brief Function for initializing leds animation
*
* @param None
* @retval None
*/
void LEDS_Init(void) {
LEDS_Eteint_Tout();
LEDS_All_Off();
LEDS_Animation=leds_off;
LEDS_AnimationAncien =LEDS_Animation;
@ -183,6 +243,15 @@ void LEDS_Init(void) {
vTaskResume(xHandleLedsHandler);
}
/**
* @brief Request an animation, given in parameter
*
* @remark This function wrap a message sending to leds mailbox.
* If multiple module request animation, only the laste requested animation will be taken into account
*
* @param[in] state Led animation requested
* @retval None
*/
void LEDS_Set(LEDS_State state) {
static LEDS_State leds_state;
@ -193,6 +262,12 @@ static LEDS_State leds_state;
}
}
/**
* @brief Apply a pattern to led
*
* @param[in] pattern Pattern to show, defined in \ref LEDS_Patterns. Use macro starting with LED_PATTERN_ as parameter
* @retval None
*/
void LEDS_ShowPattern(uint8_t pattern) {
if (pattern < LED_MAX_PATTERNS) {
HAL_GPIO_WritePin(GPIOA, LEDS_Patterns[pattern][2], GPIO_PIN_RESET);
@ -205,10 +280,18 @@ void LEDS_ShowPattern(uint8_t pattern) {
}
}
/**
* @brief Test task for checking animation correctness
*
* @warning Do not use in normal running condition
*
* @param[in] params Initial task parameters
* @retval None
*/
void LEDS_Tests(void* params) {
LEDS_State ledState = leds_idle;
LEDS_Eteint_Tout();
LEDS_All_Off();
while (1) {
MESSAGE_SendMailbox(LEDS_Mailbox, MSG_ID_LED_ETAT, LEDS_Mailbox, (void*)&ledState);
@ -219,6 +302,16 @@ void LEDS_Tests(void* params) {
}
}
/**
* @brief Message handler task
*
* Get received animation message from application and start, stop or modify animation depending on animation requested
* by controlling \ref LEDS_ActionThread task.
* If requested animation is same of currently running, no modification is done.
*
* @param[in] params Initial task parameters
* @retval None
*/
void LEDS_HandlerThread(void* params) {
MESSAGE_Typedef msg;
@ -252,6 +345,14 @@ void LEDS_HandlerThread(void* params) {
}
}
/**
* @brief Animation task
*
* Periodic task (100 ms) used for animating led. Started and stopped from \ref LEDS_HandlerThread
*
* @param[in] params Initial task parameters
* @retval None
*/
void LEDS_ActionThread(void* params) {
uint8_t cnt=0;
TickType_t xLastWakeTime;
@ -259,13 +360,13 @@ void LEDS_ActionThread(void* params) {
// Initialise the xLastWakeTime variable with the current time.
xLastWakeTime = xTaskGetTickCount();
LEDS_Eteint_Tout();
LEDS_All_Off();
while (1) {
switch (LEDS_Animation) {
case leds_off:
LEDS_Eteint_Tout();
LEDS_All_Off();
break;
case leds_idle:
if (cnt<5)
@ -362,7 +463,7 @@ void LEDS_ActionThread(void* params) {
else
cnt=0;
break;
case leds_erreur_1:
case leds_error_1:
if (cnt<5)
LEDS_ShowPattern(LED_PATTERN_ERROR);
else if (cnt<10)
@ -370,7 +471,7 @@ void LEDS_ActionThread(void* params) {
else
cnt=0;
break;
case leds_erreur_2:
case leds_error_2:
if (cnt<5)
LEDS_ShowPattern(LED_PATTERN_ERROR);
else if (cnt<10)
@ -378,7 +479,7 @@ void LEDS_ActionThread(void* params) {
else
cnt=0;
break;
case leds_erreur_3:
case leds_error_3:
if (cnt<5)
LEDS_ShowPattern(LED_PATTERN_ERROR);
else if (cnt<10)
@ -386,7 +487,7 @@ void LEDS_ActionThread(void* params) {
else
cnt=0;
break;
case leds_erreur_4:
case leds_error_4:
if (cnt<5)
LEDS_ShowPattern(LED_PATTERN_ERROR);
else if (cnt<10)
@ -394,7 +495,7 @@ void LEDS_ActionThread(void* params) {
else
cnt=0;
break;
case leds_erreur_5:
case leds_error_5:
if (cnt<5)
LEDS_ShowPattern(LED_PATTERN_ERROR);
else if (cnt<10)
@ -415,7 +516,7 @@ void LEDS_ActionThread(void* params) {
}
// Wait for the next cycle.
vTaskDelayUntil( &xLastWakeTime, pdMS_TO_TICKS(LEDS_PERIODE));
vTaskDelayUntil( &xLastWakeTime, pdMS_TO_TICKS(LEDS_DELAY));
cnt++;
}

94
software/dumber3/Application/leds.h
View file

@ -1,8 +1,31 @@
/*
* leds.h
/**
******************************************************************************
* @file leds.h
* @brief leds driver header
* @author S. DI MERCURIO (dimercur@insa-toulouse.fr)
* @date December 2023
*
* Created on: Sep 12, 2022
* Author: dimercur
******************************************************************************
* @copyright Copyright 2023 INSA-GEI, Toulouse, France. All rights reserved.
* @copyright This project is released under the Lesser GNU Public License (LGPL-3.0-only).
*
* @copyright This file is part of "Dumber" project
*
* @copyright This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 3 of the License, or (at your option) any later version.
*
* @copyright This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
* @copyright You should have received a copy of the GNU Lesser General Public License
* along with this program; if not, write to the Free Software Foundation,
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
******************************************************************************
*/
#ifndef INC_LEDS_H_
@ -10,29 +33,54 @@
#include "application.h"
/** @addtogroup Application_Software
* @{
*/
/** @addtogroup LEDS
* @{
*/
/** @addtogroup LEDS_Public Public
* @{
*/
/** Enumeration class defining possible leds animations */
typedef enum {
leds_off=0,
leds_idle,
leds_run,
leds_run_with_watchdog,
leds_bat_critical_low,
leds_bat_low,
leds_bat_med,
leds_bat_high,
leds_bat_charge_low,
leds_bat_charge_med,
leds_bat_charge_high,
leds_bat_charge_complete,
leds_watchdog_expired,
leds_erreur_1,
leds_erreur_2,
leds_erreur_3,
leds_erreur_4,
leds_erreur_5,
leds_state_unknown
leds_off=0, /**< No animation */
leds_idle, /**< Idle animation (only dot point blinking) */
leds_run, /**< Run animation (leds animate in circle) */
leds_run_with_watchdog, /**< Run with watchdog animation (leds animate in circle, with dot point blinking) */
leds_bat_critical_low, /**< Critical low bat animation (C,L and B lettres) */
leds_bat_low, /**< Low bat animation */
leds_bat_med, /**< Medium charged bat animation */
leds_bat_high, /**< Full charged bat animation */
leds_bat_charge_low, /**< Charge in progress (low bat level) animation */
leds_bat_charge_med, /**< Charge in progress (medium bat level) animation */
leds_bat_charge_high, /**< Charge in progress (high bat level) animation */
leds_bat_charge_complete, /**< Charge complete animation */
leds_watchdog_expired, /**< Watchdog expired animation (squares moving) */
leds_error_1, /**< Error 1 animation */
leds_error_2, /**< Error 2 animation */
leds_error_3, /**< Error 3 animation */
leds_error_4, /**< Error 4 animation */
leds_error_5, /**< Error 5 animation */
leds_state_unknown /**< Unknown animation */
} LEDS_State;
void LEDS_Init(void);
void LEDS_Set(LEDS_State state);
/**
* @}
*/
/**
* @}
*/
/**
* @}
*/
#endif /* INC_LEDS_H_ */

4
software/dumber3/Application/messages.c
View file

@ -11,13 +11,13 @@
#define QUEUE_SIZE 5
QueueHandle_t LEDS_Mailbox;
QueueHandle_t MOTEURS_Mailbox;
QueueHandle_t MOTORS_Mailbox;
QueueHandle_t APPLICATION_Mailbox;
QueueHandle_t XBEE_Mailbox;
void MESSAGE_Init(void) {
LEDS_Mailbox = xQueueCreate(QUEUE_SIZE, sizeof(MESSAGE_Typedef));
MOTEURS_Mailbox = xQueueCreate(QUEUE_SIZE, sizeof(MESSAGE_Typedef));
MOTORS_Mailbox = xQueueCreate(QUEUE_SIZE, sizeof(MESSAGE_Typedef));
APPLICATION_Mailbox = xQueueCreate(QUEUE_SIZE, sizeof(MESSAGE_Typedef));
XBEE_Mailbox = xQueueCreate(QUEUE_SIZE, sizeof(MESSAGE_Typedef));
}

10
software/dumber3/Application/messages.h
View file

@ -18,7 +18,7 @@ typedef struct {
} MESSAGE_Typedef;
extern QueueHandle_t LEDS_Mailbox;
extern QueueHandle_t MOTEURS_Mailbox;
extern QueueHandle_t MOTORS_Mailbox;
extern QueueHandle_t APPLICATION_Mailbox;
extern QueueHandle_t XBEE_Mailbox;
@ -43,10 +43,10 @@ extern QueueHandle_t XBEE_Mailbox;
#define MSG_ID_BUTTON_PRESSED 0x30
#define MSG_ID_MOTEURS_STOP 0x40
#define MSG_ID_MOTEURS_MOVE 0x41
#define MSG_ID_MOTEURS_TURN 0x42
#define MSG_ID_MOTEURS_END_OF_MOUVMENT 0x43
#define MSG_ID_MOTORS_STOP 0x40
#define MSG_ID_MOTORS_MOVE 0x41
#define MSG_ID_MOTORS_TURN 0x42
#define MSG_ID_MOTORS_END_OF_MOUVMENT 0x43
#define MSG_ID_XBEE_CMD 0x50
#define MSG_ID_XBEE_ANS 0x51

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

@ -1,595 +0,0 @@
/*
* moteurs.c
*
* Created on: Sep 12, 2022
* Author: dimercur
*/
#include "moteurs.h"
#include "timers.h"
#include "stm32l0xx_ll_gpio.h"
#include "stm32l0xx_ll_tim.h"
#include <limits.h>
/*
* Global informations
* Main clock: 6 Mhz
* TIM2 PWM Input (CH1): Encodeur droit PHB : 0 -> 65535
* TIM21 PWM Input (CH1): Encodeur Gauche PHA: 0 -> 65535
* TIM3: PWM Output moteur (0->200) (~30 Khz)
*/
extern TIM_HandleTypeDef htim2;
extern TIM_HandleTypeDef htim21;
extern TIM_HandleTypeDef htim3;
#define MOTEURS_MAX_COMMANDE 200
#define MOTEURS_MAX_ENCODEUR USHRT_MAX
typedef struct {
int16_t commande;
int16_t consigne;
uint16_t encodeur;
uint16_t encodeurFront;
uint8_t moteurLent;
} MOTEURS_EtatMoteur;
typedef struct {
uint8_t type;
int16_t commande;
int16_t consigne;
int32_t distance;
int32_t tours;
} MOTEURS_EtatDiff;
MOTEURS_EtatMoteur MOTEURS_EtatMoteurGauche, MOTEURS_EtatMoteurDroit = { 0 };
MOTEURS_EtatDiff MOTEURS_EtatDifferentiel = { 0 };
#define MOTEUR_Kp 300
/***** Tasks part *****/
/* Tache moteurs (gestion des messages) */
StaticTask_t xTaskMoteurs;
StackType_t xStackMoteurs[STACK_SIZE];
TaskHandle_t xHandleMoteurs = NULL;
void MOTEURS_TachePrincipale(void *params);
/* Tache moteurs périodique (asservissement) */
StaticTask_t xTaskMoteursAsservissement;
StackType_t xStackMoteursAsservissement[STACK_SIZE];
TaskHandle_t xHandleMoteursAsservissement = NULL;
void MOTEURS_TacheAsservissement(void *params);
/* Fonctions diverses */
void MOTEURS_Set(int16_t cmdGauche, int16_t cmdDroit);
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
*
*/
void MOTEURS_Init(void) {
/* Désactive les alimentations des moteurs */
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. */
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. */
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. */
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. */
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(int32_t distance) {
static int32_t dist;
dist = distance*15;
if (dist) {
MOTEURS_ActiveAlim();
MESSAGE_SendMailbox(MOTEURS_Mailbox, MSG_ID_MOTEURS_MOVE,
APPLICATION_Mailbox, (void*) &dist);
} else
MOTEURS_Stop();
}
void MOTEURS_Tourne(int32_t tours) {
static int32_t turns;
turns = tours;
if (turns) {
MOTEURS_ActiveAlim();
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);
}
/*
* @brief Tache de supervision des moteurs
* Gestion de la boite aux lettres moteurs, et supervision generale
* @params params non utilisé
*/
void MOTEURS_TachePrincipale(void *params) {
MESSAGE_Typedef msg;
int32_t distance, tours;
while (1) {
msg = MESSAGE_ReadMailbox(MOTEURS_Mailbox);
switch (msg.id) {
case MSG_ID_MOTEURS_MOVE:
distance = *((int32_t*) msg.data);
MOTEURS_EtatDifferentiel.distance = distance;
MOTEURS_EtatDifferentiel.tours = 0;
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 = *((int32_t*) msg.data);
MOTEURS_EtatDifferentiel.distance = 0;
MOTEURS_EtatDifferentiel.tours = tours;
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:
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)) {
// Les moteurs sont déjà arrêtés
vTaskSuspend(xHandleMoteursAsservissement);
MESSAGE_SendMailbox(APPLICATION_Mailbox, MSG_ID_MOTEURS_END_OF_MOUVMENT,
MOTEURS_Mailbox, (void*) NULL);
} else
// Les moteurs tournent encore
vTaskResume(xHandleMoteursAsservissement);
break;
default:
break;
}
}
}
/*
* @brief Tache d'asservissement, périodique (10ms)
*
* @params params non utilisé
*/
void MOTEURS_TacheAsservissement(void *params) {
TickType_t xLastWakeTime;
int16_t erreurG, erreurD = 0;
int16_t encodeurGauche, encodeurDroit;
int32_t locCmdG, locCmdD;
// Initialise the xLastWakeTime variable with the current time.
xLastWakeTime = xTaskGetTickCount();
for (;;) {
// Wait for the next cycle.
vTaskDelayUntil(&xLastWakeTime,
pdMS_TO_TICKS(MOTEURS_PERIODE_ASSERVISSEMENT));
#ifdef TESTS
//StartMeasure();
#endif /* TESTS */
encodeurGauche = MOTEURS_CorrectionEncodeur(MOTEURS_EtatMoteurGauche);
encodeurDroit = MOTEURS_CorrectionEncodeur(MOTEURS_EtatMoteurDroit);
/*
* encodeur est entre -32768 et +32767, selon le sens de rotation du moteur
* consigne est entre -32768 et + 32767 selon le sens de rotation du moteur
* erreur est entre -32768 et 32767 selon la difference à apporter à la commande
*/
erreurG = MOTEURS_EtatMoteurGauche.consigne - encodeurGauche;
erreurD = MOTEURS_EtatMoteurDroit.consigne - encodeurDroit;
if (((MOTEURS_EtatMoteurDroit.consigne == 0)
&& (MOTEURS_EtatMoteurGauche.consigne == 0))
&& ((erreurD == 0) && (erreurG == 0))) {
MOTEURS_DesactiveAlim();
MESSAGE_SendMailbox(APPLICATION_Mailbox, MSG_ID_MOTEURS_END_OF_MOUVMENT,
MOTEURS_Mailbox, (void*) NULL);
vTaskSuspend(xHandleMoteursAsservissement);
}
if (MOTEURS_EtatMoteurGauche.consigne == 0)
MOTEURS_EtatMoteurGauche.commande = 0;
else {
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;
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 (MOTEURS_EtatMoteurDroit.consigne == 0)
MOTEURS_EtatMoteurDroit.commande = 0;
else {
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;
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;
}
}
}
/* Finalement, on applique les commandes aux moteurs */
MOTEURS_Set(MOTEURS_EtatMoteurGauche.commande,
MOTEURS_EtatMoteurDroit.commande);
#ifdef TESTS
//EndMeasure();
#endif /* TESTS */
}
}
typedef struct {
uint16_t encodeur;
uint16_t correction;
} MOTEURS_CorrectionPoint;
#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
};
/*
* @brief Fonction de conversion des valeurs brutes de l'encodeur en valeur linearisées
*
* @param encodeur valeur brute de l'encodeur
* @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;
uint16_t encodeur = etat.encodeur;
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)) {
// valeur trouvée, on sort
break;
} else
index++;
}
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;
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)
correction = -correction;
return correction;
}
/**
*
*/
void MOTEURS_DesactiveAlim(void) {
LL_TIM_DisableCounter(TIM3);
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(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);
LL_TIM_DisableIT_UPDATE(TIM2);
LL_TIM_DisableIT_UPDATE(TIM21);
LL_GPIO_SetOutputPin(GPIOB, SHUTDOWN_ENCODERS_Pin);
LL_GPIO_ResetOutputPin(GPIOB, SHUTDOWN_5V_Pin);
}
/**
*
*/
void MOTEURS_ActiveAlim(void) {
LL_TIM_EnableCounter(TIM3);
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(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);
LL_TIM_EnableIT_UPDATE(TIM2);
LL_TIM_EnableIT_UPDATE(TIM21);
LL_GPIO_ResetOutputPin(GPIOB, SHUTDOWN_ENCODERS_Pin);
LL_GPIO_SetOutputPin(GPIOB, SHUTDOWN_5V_Pin);
}
/**
* @brief Active les encodeurs et le régulateur des moteur si nécessaire et
* règle la commande du moteur (entre -MOTEURS_MAX_COMMANDE et +MOTEURS_MAX_COMMANDE)
* On applique une "regle de 3"
* pour SHRT_MAX -> MOTEURS_MAX_COMMANDE
* pour 0 -> 0
* pour une commande C dans l'interval [0 .. 32767], la commande est
* commande = (C * MOTEURS_MAX_COMMANDE)/32767
*/
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));
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);
} else {
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);
} else {
LL_TIM_OC_SetCompareCH4(TIM3, (uint32_t) 0);
LL_TIM_OC_SetCompareCH3(TIM3, (uint32_t) locValGauche);
}
}
/*
* @brief Recupere les mesures brutes des encodeurs et les enregistre dans la structure moteur correspondante
*
* @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->Channel == HAL_TIM_ACTIVE_CHANNEL_1) {
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);
}
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) {
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);
}
if (LL_TIM_IsActiveFlag_UPDATE(TIM2))
LL_TIM_ClearFlag_UPDATE(TIM2);
MOTEURS_EtatMoteurDroit.moteurLent = 0;
}
}
}
/*
* @brief Gestionnaire d'interruption "overflow"
* Lorsque deux interruptions "overflow" sont arrivées sans que l'interruption capture n'arrive,
* cela signifie que le moteur est à l'arret.
* On met la valeur de l'encodeur à MOTEURS_MAX_ENCODEUR
*
* @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) {
MOTEURS_EtatMoteurGauche.encodeur = MOTEURS_MAX_ENCODEUR;
MOTEURS_EtatMoteurGauche.moteurLent = 1;
}
} else { /* moteur droit */
if ((MOTEURS_EtatMoteurDroit.moteurLent++) >= 1) {
MOTEURS_EtatMoteurDroit.encodeur = MOTEURS_MAX_ENCODEUR;
MOTEURS_EtatMoteurDroit.moteurLent = 1;
}
}
}

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

@ -1,22 +0,0 @@
/*
* moteurs.h
*
* Created on: Sep 12, 2022
* Author: dimercur
*/
#ifndef INC_MOTEURS_H_
#define INC_MOTEURS_H_
#include "application.h"
void MOTEURS_Init(void);
//void MOTEURS_Set(uint8_t mot, int16_t val);
//void MOTEURS_Test (void);
void MOTEURS_Avance(int32_t distance);
void MOTEURS_Tourne(int32_t tours);
void MOTEURS_Stop(void);
void MOTEURS_TimerEncodeurUpdate (TIM_HandleTypeDef *htim);
#endif /* INC_MOTEURS_H_ */

675
software/dumber3/Application/motors.c Normal file
View file

@ -0,0 +1,675 @@
/**
******************************************************************************
* @file motors.c
* @brief motors driver body
* @author S. DI MERCURIO (dimercur@insa-toulouse.fr)
* @date December 2023
*
******************************************************************************
* @copyright Copyright 2023 INSA-GEI, Toulouse, France. All rights reserved.
* @copyright This project is released under the Lesser GNU Public License (LGPL-3.0-only).
*
* @copyright This file is part of "Dumber" project
*
* @copyright This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 3 of the License, or (at your option) any later version.
*
* @copyright This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
* @copyright You should have received a copy of the GNU Lesser General Public License
* along with this program; if not, write to the Free Software Foundation,
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
******************************************************************************
*/
#include "motors.h"
#include "timers.h"
#include "stm32l0xx_ll_gpio.h"
#include "stm32l0xx_ll_tim.h"
#include <limits.h>
/** @addtogroup Application_Software
* @{
*/
/** @addtogroup MOTORS
* Motors driver is in charge of controlling motors and applying a regulation to ensure a linear trajectory
*
* Global informations about peripherals
* - Main clock: 6 Mhz
* - TIM2 PWM Input (CH1): right encoder PHB : 0 -> 65535
* - TIM21 PWM Input (CH1): left encoder PHA: 0 -> 65535
* - TIM3: PWM Output motor (0->200) (~30 Khz)
* @{
*/
/** @addtogroup MOTORS_Private Private
* @{
*/
extern TIM_HandleTypeDef htim2;
extern TIM_HandleTypeDef htim21;
extern TIM_HandleTypeDef htim3;
#define MOTORS_MAX_COMMAND 200
#define MOTORS_MAX_ENCODER USHRT_MAX
/** Structure for storing motore (left or right) regulation state
* Used during regulation task for controlling motor */
typedef struct {
int16_t output; /**< */
int16_t set_point; /**< Xbee RF quality (not used)*/
uint16_t encoder; /**< Xbee RF quality (not used)*/
uint16_t encoderEdge; /**< Xbee RF quality (not used)*/
uint8_t slowMotor; /**< Xbee RF quality (not used)*/
} MOTORS_MotorState;
/** Structure storing counters used for watchdog and system inactivity.
* Used notably to check if watchdog reset was missed or power down system because of inactivity */
typedef struct {
uint8_t type; /**< Xbee RF quality (not used)*/
int16_t output; /**< Xbee RF quality (not used)*/
int16_t set_point; /**< Xbee RF quality (not used)*/
int32_t distance; /**< Xbee RF quality (not used)*/
int32_t turns; /**< Xbee RF quality (not used)*/
} MOTORS_DifferentialState;
MOTORS_MotorState MOTORS_LeftMotorState, MOTORS_RightMotorState = { 0 };
MOTORS_DifferentialState MOTORS_DiffState = { 0 };
#define MOTOR_Kp 300
/***** Tasks part *****/
/* Tache moteurs (gestion des messages) */
StaticTask_t xTaskMotors;
StackType_t xStackMotors[STACK_SIZE];
TaskHandle_t xHandleMotors = NULL;
void MOTORS_HandlerTask(void *params);
/* Tache moteurs périodique (asservissement) */
StaticTask_t xTaskMotorsControl;
StackType_t xStackMotorsControl[STACK_SIZE];
TaskHandle_t xHandleMotorsControl = NULL;
void MOTORS_ControlTask(void *params);
/* Fonctions diverses */
void MOTORS_Set(int16_t leftMotor, int16_t rightMotor);
void MOTORS_PowerOff(void);
void MOTORS_PowerOn(void);
int16_t MOTORS_EncoderCorrection(MOTORS_MotorState state);
#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 Function for initializing motors driver
*
* @param None
* @retval None
*/
void MOTORS_Init(void) {
/* Désactive les alimentations des moteurs */
MOTORS_PowerOff();
/* Create the task without using any dynamic memory allocation. */
xHandleMotors = xTaskCreateStatic(MOTORS_HandlerTask, /* Function that implements the task. */
"MOTORS Handler", /* Text name for the task. */
STACK_SIZE, /* Number of indexes in the xStack array. */
NULL, /* Parameter passed into the task. */
PriorityMotorsHandler,/* Priority at which the task is created. */
xStackMotors, /* Array to use as the task's stack. */
&xTaskMotors); /* Variable to hold the task's data structure. */
vTaskResume(xHandleMotors);
/* Create the task without using any dynamic memory allocation. */
xHandleMotorsControl = xTaskCreateStatic(
MOTORS_ControlTask, /* Function that implements the task. */
"MOTORS Control", /* Text name for the task. */
STACK_SIZE, /* Number of indexes in the xStack array. */
NULL, /* Parameter passed into the task. */
PriorityMotorsAsservissement,/* Priority at which the task is created. */
xStackMotorsControl, /* Array to use as the task's stack. */
&xTaskMotorsControl); /* Variable to hold the task's data structure. */
vTaskSuspend(xHandleMotorsControl); // On ne lance la tache d'asservissement que lorsque'une commande moteur arrive
MOTORS_PowerOff();
#ifdef TESTS
Init_Systick();
#endif /* TESTS */
}
/**
* @brief Function for initializing motors driver
*
* @param None
* @retval None
*/
void MOTORS_Move(int32_t distance) {
static int32_t dist;
dist = distance*15;
if (dist) {
MOTORS_PowerOn();
MESSAGE_SendMailbox(MOTORS_Mailbox, MSG_ID_MOTORS_MOVE,
APPLICATION_Mailbox, (void*) &dist);
} else
MOTORS_Stop();
}
/**
* @brief Function for initializing motors driver
*
* @param None
* @retval None
*/
void MOTORS_Turn(int32_t tours) {
static int32_t turns;
turns = tours;
if (turns) {
MOTORS_PowerOn();
MESSAGE_SendMailbox(MOTORS_Mailbox, MSG_ID_MOTORS_TURN,
APPLICATION_Mailbox, (void*) &turns);
} else
MOTORS_Stop();
}
/**
* @brief Function for initializing motors driver
*
* @param None
* @retval None
*/
void MOTORS_Stop(void) {
MOTORS_PowerOff();
MESSAGE_SendMailbox(MOTORS_Mailbox, MSG_ID_MOTORS_STOP,
APPLICATION_Mailbox, (void*) NULL);
}
/*
* @brief Tache de supervision des moteurs
* Gestion de la boite aux lettres moteurs, et supervision generale
* @param params non utilisé
*/
void MOTORS_HandlerTask(void *params) {
MESSAGE_Typedef msg;
int32_t distance, tours;
while (1) {
msg = MESSAGE_ReadMailbox(MOTORS_Mailbox);
switch (msg.id) {
case MSG_ID_MOTORS_MOVE:
distance = *((int32_t*) msg.data);
MOTORS_DiffState.distance = distance;
MOTORS_DiffState.turns = 0;
if (distance > 0) {
MOTORS_LeftMotorState.set_point = 50;
MOTORS_RightMotorState.set_point = 50;
} else {
MOTORS_LeftMotorState.set_point = -50;
MOTORS_RightMotorState.set_point = -50;
}
vTaskResume(xHandleMotorsControl);
break;
case MSG_ID_MOTORS_TURN:
tours = *((int32_t*) msg.data);
MOTORS_DiffState.distance = 0;
MOTORS_DiffState.turns = tours;
if (tours > 0) {
MOTORS_LeftMotorState.set_point = -50;
MOTORS_RightMotorState.set_point = 50;
} else {
MOTORS_LeftMotorState.set_point = 50;
MOTORS_RightMotorState.set_point = -50;
}
vTaskResume(xHandleMotorsControl);
break;
case MSG_ID_MOTORS_STOP:
MOTORS_DiffState.distance = 0;
MOTORS_DiffState.turns = 0;
MOTORS_LeftMotorState.set_point = 0;
MOTORS_RightMotorState.set_point = 0;
if ((MOTORS_EncoderCorrection(MOTORS_LeftMotorState) == 0)
&& (MOTORS_EncoderCorrection(MOTORS_RightMotorState) == 0)) {
// Les moteurs sont déjà arrêtés
vTaskSuspend(xHandleMotorsControl);
MESSAGE_SendMailbox(APPLICATION_Mailbox, MSG_ID_MOTORS_END_OF_MOUVMENT,
MOTORS_Mailbox, (void*) NULL);
} else
// Les moteurs tournent encore
vTaskResume(xHandleMotorsControl);
break;
default:
break;
}
}
}
/*
* @brief Tache d'asservissement, périodique (10ms)
*
* @param params non utilisé
*/
void MOTORS_ControlTask(void *params) {
TickType_t xLastWakeTime;
int16_t leftError, rightError = 0;
int16_t leftEncoder, rightEncoder;
int32_t locCmdG, locCmdD;
// Initialise the xLastWakeTime variable with the current time.
xLastWakeTime = xTaskGetTickCount();
for (;;) {
// Wait for the next cycle.
vTaskDelayUntil(&xLastWakeTime,
pdMS_TO_TICKS(MOTORS_REGULATION_DELAY));
#ifdef TESTS
//StartMeasure();
#endif /* TESTS */
leftEncoder = MOTORS_EncoderCorrection(MOTORS_LeftMotorState);
rightEncoder = MOTORS_EncoderCorrection(MOTORS_RightMotorState);
/*
* encodeur est entre -32768 et +32767, selon le sens de rotation du moteur
* consigne est entre -32768 et + 32767 selon le sens de rotation du moteur
* erreur est entre -32768 et 32767 selon la difference à apporter à la commande
*/
leftError = MOTORS_LeftMotorState.set_point - leftEncoder;
rightError = MOTORS_RightMotorState.set_point - rightEncoder;
if (((MOTORS_RightMotorState.set_point == 0)
&& (MOTORS_LeftMotorState.set_point == 0))
&& ((rightError == 0) && (leftError == 0))) {
MOTORS_PowerOff();
MESSAGE_SendMailbox(APPLICATION_Mailbox, MSG_ID_MOTORS_END_OF_MOUVMENT,
MOTORS_Mailbox, (void*) NULL);
vTaskSuspend(xHandleMotorsControl);
}
if (MOTORS_LeftMotorState.set_point == 0)
MOTORS_LeftMotorState.output = 0;
else {
if (leftError != 0) {
//locCmdG = (int32_t)MOTEURS_EtatMoteurGauche.commande + ((int32_t)MOTEUR_Kp*(int32_t)erreurG)/100;
locCmdG = ((int32_t) MOTOR_Kp * (int32_t) leftError) / 100;
if (MOTORS_LeftMotorState.set_point >= 0) {
if (locCmdG < 0)
MOTORS_LeftMotorState.output = 0;
else if (locCmdG > SHRT_MAX)
MOTORS_LeftMotorState.output = SHRT_MAX;
else
MOTORS_LeftMotorState.output = (int16_t) locCmdG;
} else {
if (locCmdG > 0)
MOTORS_LeftMotorState.output = 0;
else if (locCmdG < SHRT_MIN)
MOTORS_LeftMotorState.output = SHRT_MIN;
else
MOTORS_LeftMotorState.output = (int16_t) locCmdG;
}
}
}
if (MOTORS_RightMotorState.set_point == 0)
MOTORS_RightMotorState.output = 0;
else {
if (rightError != 0) {
//locCmdD = (int32_t)MOTEURS_EtatMoteurDroit.commande + ((int32_t)MOTEUR_Kp*(int32_t)erreurD)/100;
locCmdD = ((int32_t) MOTOR_Kp * (int32_t) rightError) / 100;
if (MOTORS_RightMotorState.set_point >= 0) {
if (locCmdD < 0)
MOTORS_RightMotorState.output = 0;
else if (locCmdD > SHRT_MAX)
MOTORS_RightMotorState.output = SHRT_MAX;
else
MOTORS_RightMotorState.output = (int16_t) locCmdD;
} else {
if (locCmdD > 0)
MOTORS_RightMotorState.output = 0;
else if (locCmdD < SHRT_MIN)
MOTORS_RightMotorState.output = SHRT_MIN;
else
MOTORS_RightMotorState.output = (int16_t) locCmdD;
}
}
}
/* Finalement, on applique les commandes aux moteurs */
MOTORS_Set(MOTORS_LeftMotorState.output,
MOTORS_RightMotorState.output);
#ifdef TESTS
//EndMeasure();
#endif /* TESTS */
}
}
typedef struct {
uint16_t encoder;
uint16_t correction;
} MOTORS_CorrectionPoint;
#define MOTORS_MAX_CORRECTION_POINTS 16
const MOTORS_CorrectionPoint MOTORS_CorrectionPoints[MOTORS_MAX_CORRECTION_POINTS] =
{ { MOTORS_MAX_ENCODER - 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
};
/*
* @brief Fonction de conversion des valeurs brutes de l'encodeur en valeur linearisées
*
* @param encodeur valeur brute de l'encodeur
* @return valeur linéarisée (entre -32768 et 32767)
*/
int16_t MOTORS_EncoderCorrection(MOTORS_MotorState state) {
int16_t correction = 0;
uint8_t index = 0;
uint32_t A, B, C;
uint16_t encoder = state.encoder;
if (encoder == MOTORS_MAX_ENCODER)
correction = 0;
else { // recherche par dichotomie de l'intervale
while (index < MOTORS_MAX_CORRECTION_POINTS) {
if ((MOTORS_CorrectionPoints[index].encoder >= encoder)
&& (MOTORS_CorrectionPoints[index + 1].encoder < encoder)) {
// valeur trouvée, on sort
break;
} else
index++;
}
if (index >= MOTORS_MAX_CORRECTION_POINTS)
correction = SHRT_MAX;
else {
A = encoder - MOTORS_CorrectionPoints[index + 1].encoder;
B = MOTORS_CorrectionPoints[index + 1].correction
- MOTORS_CorrectionPoints[index].correction;
C = MOTORS_CorrectionPoints[index].encoder
- MOTORS_CorrectionPoints[index + 1].encoder;
correction =
(int16_t) (MOTORS_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 (state.set_point < 0)
correction = -correction;
return correction;
}
/**
* @brief Function for initializing motors driver
*
* @param None
* @retval None
*/
void MOTORS_PowerOff(void) {
LL_TIM_DisableCounter(TIM3);
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(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);
LL_TIM_DisableIT_UPDATE(TIM2);
LL_TIM_DisableIT_UPDATE(TIM21);
LL_GPIO_SetOutputPin(GPIOB, SHUTDOWN_ENCODERS_Pin);
LL_GPIO_ResetOutputPin(GPIOB, SHUTDOWN_5V_Pin);
}
/**
* @brief Function for initializing motors driver
*
* @param None
* @retval None
*/
void MOTORS_PowerOn(void) {
LL_TIM_EnableCounter(TIM3);
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(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);
LL_TIM_EnableIT_UPDATE(TIM2);
LL_TIM_EnableIT_UPDATE(TIM21);
LL_GPIO_ResetOutputPin(GPIOB, SHUTDOWN_ENCODERS_Pin);
LL_GPIO_SetOutputPin(GPIOB, SHUTDOWN_5V_Pin);
}
/**
* @brief Active les encodeurs et le régulateur des moteur si nécessaire et
* règle la commande du moteur (entre -MOTEURS_MAX_COMMANDE et +MOTEURS_MAX_COMMANDE)
* On applique une "regle de 3"
* pour SHRT_MAX -> MOTEURS_MAX_COMMANDE
* pour 0 -> 0
* pour une commande C dans l'interval [0 .. 32767], la commande est
* commande = (C * MOTEURS_MAX_COMMANDE)/32767
*
* @param[in] cmdGauche blablabla
* @param[in] cmdDroit blablabla
*/
void MOTORS_Set(int16_t leftMotor, int16_t rightMotor) {
int32_t leftValue, rightValue;
leftValue = (int32_t) (((int32_t) leftMotor * (int32_t) SHRT_MAX)
/ ((int32_t) MOTORS_MAX_COMMAND));
rightValue = (int32_t) (((int32_t) rightMotor * (int32_t) SHRT_MAX)
/ ((int32_t) MOTORS_MAX_COMMAND));
if (LL_GPIO_IsOutputPinSet(GPIOB, SHUTDOWN_5V_Pin) == GPIO_PIN_RESET)
MOTORS_PowerOn();
// Moteur droit
if (rightMotor >= 0) {
LL_TIM_OC_SetCompareCH2(TIM3, (uint32_t) rightValue);
LL_TIM_OC_SetCompareCH1(TIM3, (uint32_t) 0);
} else {
LL_TIM_OC_SetCompareCH2(TIM3, (uint32_t) 0);
LL_TIM_OC_SetCompareCH1(TIM3, (uint32_t) rightValue);
}
// Moteur gauche
if (leftMotor >= 0) {
LL_TIM_OC_SetCompareCH4(TIM3, (uint32_t) leftValue);
LL_TIM_OC_SetCompareCH3(TIM3, (uint32_t) 0);
} else {
LL_TIM_OC_SetCompareCH4(TIM3, (uint32_t) 0);
LL_TIM_OC_SetCompareCH3(TIM3, (uint32_t) leftValue);
}
}
/*
* @brief Recupere les mesures brutes des encodeurs et les enregistre dans la structure moteur correspondante
*
* @param[in] 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->Channel == HAL_TIM_ACTIVE_CHANNEL_1) {
if (MOTORS_LeftMotorState.slowMotor != 0) {
MOTORS_LeftMotorState.encoder = MOTORS_MAX_ENCODER;
MOTORS_LeftMotorState.encoderEdge = MOTORS_MAX_ENCODER;
} else {
MOTORS_LeftMotorState.encoder =
(uint16_t) LL_TIM_IC_GetCaptureCH1(TIM21);
MOTORS_LeftMotorState.encoderEdge =
(uint16_t) LL_TIM_IC_GetCaptureCH2(TIM21);
}
if (LL_TIM_IsActiveFlag_UPDATE(TIM21))
LL_TIM_ClearFlag_UPDATE(TIM21);
MOTORS_LeftMotorState.slowMotor = 0;
if (MOTORS_DiffState.distance) {
if (MOTORS_DiffState.distance>0) MOTORS_DiffState.distance--;
else MOTORS_DiffState.distance++;
if (MOTORS_DiffState.distance==0) {
MOTORS_LeftMotorState.set_point=0;
MOTORS_RightMotorState.set_point=0;
}
}
if (MOTORS_DiffState.turns) {
if (MOTORS_DiffState.turns>0) MOTORS_DiffState.turns--;
else MOTORS_DiffState.turns++;
if (MOTORS_DiffState.turns==0) {
MOTORS_LeftMotorState.set_point=0;
MOTORS_RightMotorState.set_point=0;
}
}
}
} else { /* moteur droit */
if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1) {
if (MOTORS_RightMotorState.slowMotor != 0) {
MOTORS_RightMotorState.encoder = MOTORS_MAX_ENCODER;
MOTORS_RightMotorState.encoderEdge = MOTORS_MAX_ENCODER;
} else {
MOTORS_RightMotorState.encoder =
(uint16_t) LL_TIM_IC_GetCaptureCH1(TIM2);
MOTORS_RightMotorState.encoderEdge =
(uint16_t) LL_TIM_IC_GetCaptureCH2(TIM2);
}
if (LL_TIM_IsActiveFlag_UPDATE(TIM2))
LL_TIM_ClearFlag_UPDATE(TIM2);
MOTORS_RightMotorState.slowMotor = 0;
}
}
}
/*
* @brief Gestionnaire d'interruption "overflow"
* Lorsque deux interruptions "overflow" sont arrivées sans que l'interruption capture n'arrive,
* cela signifie que le moteur est à l'arret.
* On met la valeur de l'encodeur à MOTEURS_MAX_ENCODEUR
*
* @param[in] htim pointeur sur la reference du timer qui generé l'interruption
*/
void MOTORS_TimerEncodeurUpdate(TIM_HandleTypeDef *htim) {
if (htim->Instance == TIM21) { /* moteur gauche */
if ((MOTORS_LeftMotorState.slowMotor++) >= 1) {
MOTORS_LeftMotorState.encoder = MOTORS_MAX_ENCODER;
MOTORS_LeftMotorState.slowMotor = 1;
}
} else { /* moteur droit */
if ((MOTORS_RightMotorState.slowMotor++) >= 1) {
MOTORS_RightMotorState.encoder = MOTORS_MAX_ENCODER;
MOTORS_RightMotorState.slowMotor = 1;
}
}
}
/**
* @}
*/
/**
* @}
*/
/**
* @}
*/

67
software/dumber3/Application/motors.h Normal file
View file

@ -0,0 +1,67 @@
/**
******************************************************************************
* @file motors.c
* @brief motors driver header
* @author S. DI MERCURIO (dimercur@insa-toulouse.fr)
* @date December 2023
*
******************************************************************************
* @copyright Copyright 2023 INSA-GEI, Toulouse, France. All rights reserved.
* @copyright This project is released under the Lesser GNU Public License (LGPL-3.0-only).
*
* @copyright This file is part of "Dumber" project
*
* @copyright This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 3 of the License, or (at your option) any later version.
*
* @copyright This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
* @copyright You should have received a copy of the GNU Lesser General Public License
* along with this program; if not, write to the Free Software Foundation,
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
******************************************************************************
*/
#ifndef INC_MOTORS_H_
#define INC_MOTORS_H_
#include "application.h"
/** @addtogroup Application_Software
* @{
*/
/** @addtogroup MOTORS
* @{
*/
/** @addtogroup MOTORS_Public Public
* @{
*/
void MOTORS_Init(void);
void MOTORS_Move(int32_t distance);
void MOTORS_Turn(int32_t tours);
void MOTORS_Stop(void);
void MOTORS_TimerEncodeurUpdate (TIM_HandleTypeDef *htim);
/**
* @}
*/
/**
* @}
*/
/**
* @}
*/
#endif /* INC_MOTORS_H_ */

30
software/dumber3/Application/panic.c
View file

@ -12,32 +12,32 @@
#include "leds.h"
void PANIC_StopTasksAndWait(void);
void MOTEURS_DesactiveAlim(void);
void MOTORS_PowerOff(void);
extern TaskHandle_t xHandleLedsHandler;
extern TaskHandle_t xHandleLedsAction;
extern TaskHandle_t xHandleBatterie;
extern TaskHandle_t xHandleBattery;
extern TimerHandle_t xHandleTimerButton;
extern TaskHandle_t xHandleApplicationMain;
extern TimerHandle_t xHandleTimerTimeout;
extern TaskHandle_t xHandleMoteurs;
extern TaskHandle_t xHandleMoteursAsservissement;
extern TaskHandle_t xHandleMotors;
extern TaskHandle_t xHandleMotorsControl;
extern TaskHandle_t xHandleXbeeTXHandler;
extern TaskHandle_t xHandleXbeeRX;
void PANIC_Raise(PANIC_Typedef panicId) {
switch (panicId) {
case panic_adc_err:
LEDS_Set(leds_erreur_1);
LEDS_Set(leds_error_1);
break;
case panic_charger_err:
LEDS_Set(leds_erreur_2);
LEDS_Set(leds_error_2);
break;
case panic_malloc:
LEDS_Set(leds_erreur_3);
LEDS_Set(leds_error_3);
break;
default:
LEDS_Set(leds_erreur_5);
LEDS_Set(leds_error_5);
break;
}
@ -66,21 +66,21 @@ void PANIC_StopTasksAndWait(void){
if (currentTask != xHandleXbeeTXHandler)
vTaskSuspend(xHandleXbeeTXHandler);
if (currentTask != xHandleMoteurs)
vTaskSuspend(xHandleMoteurs);
if (currentTask != xHandleMotors)
vTaskSuspend(xHandleMotors);
if (currentTask != xHandleMoteursAsservissement)
vTaskSuspend(xHandleMoteursAsservissement);
if (currentTask != xHandleMotorsControl)
vTaskSuspend(xHandleMotorsControl);
if (currentTask != xHandleBatterie)
vTaskSuspend(xHandleBatterie);
if (currentTask != xHandleBattery)
vTaskSuspend(xHandleBattery);
if (currentTask != xHandleApplicationMain)
vTaskSuspend(xHandleApplicationMain);
/* Stop des alim moteurs */
MOTEURS_DesactiveAlim();
MOTORS_PowerOff();
/* disable XBEE */
HAL_GPIO_WritePin(XBEE_RESET_GPIO_Port, XBEE_RESET_Pin, GPIO_PIN_RESET);

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

@ -681,7 +681,7 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
}
/* USER CODE BEGIN Callback 1 */
else if ((htim->Instance == TIM2) || (htim->Instance == TIM21))
MOTEURS_TimerEncodeurUpdate (htim);
MOTORS_TimerEncodeurUpdate (htim);
/* USER CODE END Callback 1 */
}

2744
software/dumber3/Doxyfile Normal file
View file

File diff suppressed because it is too large Load diff