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

600 lines
19 KiB
C
Raw Blame History

/**
******************************************************************************
* @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 version 3.1
*
* 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.
*
* @warning Very important information: robot is use a 2.5V power supply so
* XBEE MODULES FROM S1 GENERATION DON'T WORK
* Use, at least, module from S2 generation
*
* @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.
*
*/
#include "application.h"
#include "timers.h"
#include "string.h"
#include <stdlib.h>
#include "leds.h"
#include "xbee.h"
#include "messages.h"
#include "motors.h"
#include "battery.h"
#include "panic.h"
#include "rtos_support.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, /**< 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; /**< 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; /**< 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;
/* 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 xStackApplicationMain[ STACK_SIZE*2 ];
TaskHandle_t xHandleApplicationMain = NULL;
StaticTimer_t xBufferTimerTimeout;
TimerHandle_t xHandleTimerTimeout = NULL;
void vTimerTimeoutCallback( TimerHandle_t xTimer );
void APPLICATION_Thread(void* params);
void APPLICATION_StateMachine();
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();
/* Init de l'afficheur */
LEDS_Init();
/* Init de la partie RF / reception des messages */
XBEE_Init();
BATTERY_Init();
MOTORS_Init();
/* Create the task without using any dynamic memory allocation. */
xHandleApplicationMain = xTaskCreateStatic(
APPLICATION_Thread, /* Function that implements the task. */
"APPLICATION Thread", /* Text name for the task. */
STACK_SIZE*2, /* Number of indexes in the xStack array. */
NULL, /* Parameter passed into the task. */
PriorityApplicationHandler,/* Priority at which the task is created. */
xStackApplicationMain, /* Array to use as the task's stack. */
&xTaskApplicationMain); /* Variable to hold the task's data structure. */
vTaskResume(xHandleApplicationMain);
/* Create a periodic task without using any dynamic memory allocation. */
xHandleTimerTimeout = xTimerCreateStatic(
"Counters Timer",
pdMS_TO_TICKS(APPLICATION_COUNTERS_DELAY),
pdTRUE,
( void * ) 0,
vTimerTimeoutCallback,
&xBufferTimerTimeout);
xTimerStart(xHandleTimerTimeout,0 );
}
/**
* @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;
while (1) {
msg = MESSAGE_ReadMailbox(APPLICATION_Mailbox);
switch (msg.id) {
case MSG_ID_XBEE_CMD:
receivedCMD = (char*)msg.data;
if (receivedCMD != NULL) {
decodedCmd = cmdDecode(receivedCMD,strlen(receivedCMD));
if (decodedCmd != NULL) {
if (decodedCmd->type==CMD_NONE) {
cmdSendAnswer(ANS_UNKNOWN);
} else if (decodedCmd->type == CMD_INVALID_CHECKSUM) {
cmdSendAnswer(ANS_ERR);
} else {
systemInfos.cmd = decodedCmd->type;
systemTimeout.inactivityCnt = 0;
/* Manage answer to command, when possible
* (further treatment of the command will be done in APPLICATION_StateMachine) */
switch (decodedCmd->type) {
case CMD_PING:
case CMD_TEST:
case CMD_DEBUG:
cmdSendAnswer(ANS_OK);
break;
case CMD_POWER_OFF:
systemInfos.powerOffRequired=1;
cmdSendAnswer(ANS_OK);
break;
case CMD_GET_BATTERY:
cmdSendBatteryLevel(systemInfos.batteryState);
break;
case CMD_GET_VERSION:
cmdSendVersion();
break;
case CMD_GET_BUSY_STATE:
if (systemInfos.state == stateInMouvement)
cmdSendBusyState(0x1);
else
cmdSendBusyState(0x0);
break;
case CMD_MOVE:
systemInfos.distance = ((CMD_Move*)decodedCmd)->distance;
break;
case CMD_TURN:
systemInfos.turns = ((CMD_Turn*)decodedCmd)->turns;
break;
default:
/* All others commands must be processed in state machine
* in order to find what action to do and what answer to give
*/
break;
}
}
free(receivedCMD);
free(decodedCmd);
}
}
break;
case MSG_ID_BAT_ADC_ERR:
/* depart en panic: error 2 */
PANIC_Raise(panic_adc_err);
break;
case MSG_ID_BAT_CHARGE_ERR:
/* depart en panic: error 3 */
PANIC_Raise(panic_charger_err);
break;
case MSG_ID_BAT_CHARGE_COMPLETE:
case MSG_ID_BAT_CHARGE_LOW:
case MSG_ID_BAT_CHARGE_MED:
case MSG_ID_BAT_CHARGE_HIGH:
systemInfos.batteryUpdate=1;
systemInfos.inCharge=1;
systemInfos.batteryState = msg.id;
break;
case MSG_ID_BAT_CRITICAL_LOW:
case MSG_ID_BAT_LOW:
case MSG_ID_BAT_MED:
case MSG_ID_BAT_HIGH:
systemInfos.batteryUpdate=1;
systemInfos.inCharge=0;
systemInfos.batteryState = msg.id;
break;
case MSG_ID_MOTORS_END_OF_MOUVMENT:
systemInfos.endOfMouvement= 1;
break;
case MSG_ID_BUTTON_PRESSED:
systemInfos.powerOffRequired =1;
break;
default:
break;
}
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)
APPLICATION_PowerOff(); // system will halt here
if ((systemInfos.inCharge) && (systemInfos.state != stateInCharge)) {
APPLICATION_TransitionToNewState(stateInCharge);
}
if (systemInfos.batteryUpdate) {
if (systemInfos.batteryState==MSG_ID_BAT_CRITICAL_LOW) {
ledState = leds_bat_critical_low;
APPLICATION_TransitionToNewState(stateLowBatDisable);
LEDS_Set(ledState);
} else if (systemInfos.state == stateInCharge) {
switch (systemInfos.batteryState) {
case MSG_ID_BAT_CHARGE_COMPLETE:
ledState= leds_bat_charge_complete;
break;
case MSG_ID_BAT_CHARGE_HIGH:
ledState= leds_bat_charge_high;
break;
case MSG_ID_BAT_CHARGE_MED:
ledState= leds_bat_charge_med;
break;
case MSG_ID_BAT_CHARGE_LOW:
ledState= leds_bat_charge_low;
break;
}
LEDS_Set(ledState);
} else if (systemInfos.state==stateStartup) {
switch (systemInfos.batteryState) {
case MSG_ID_BAT_HIGH:
ledState= leds_bat_high;
break;
case MSG_ID_BAT_MED:
ledState= leds_bat_med;
break;
case MSG_ID_BAT_LOW:
ledState= leds_bat_low;
break;
}
LEDS_Set(ledState);
}
}
if (systemInfos.cmd != CMD_NONE) {
/* a newer command just arrived, process it
* in this switch/case, we only care about state transition and command execution
*/
switch (systemInfos.cmd) {
case CMD_RESET:
if ((systemInfos.state == stateIdle) ||
(systemInfos.state == stateRun) ||
(systemInfos.state == stateInMouvement) ||
(systemInfos.state == stateWatchdogDisable)) {
/* command RESET is only applicable in those 3 states, otherwise it is rejected */
cmdSendAnswer(ANS_OK);
APPLICATION_TransitionToNewState(stateIdle);
} else
cmdSendAnswer(ANS_ERR);
break;
case CMD_START_WITH_WATCHDOG:
case CMD_START_WITHOUT_WATCHDOG:
if (systemInfos.state == stateIdle) {
/* only state where START cmd is accepted */
cmdSendAnswer(ANS_OK);
if (systemInfos.cmd == CMD_START_WITH_WATCHDOG) {
systemTimeout.watchdogEnabled = 1;
systemTimeout.watchdogCnt=0;
systemTimeout.watchdogMissedCnt=0;
}
APPLICATION_TransitionToNewState(stateRun);
} else
cmdSendAnswer(ANS_ERR);
break;
case CMD_RESET_WATCHDOG:
if ((systemInfos.state == stateRun) || (systemInfos.state == stateInMouvement)) {
if ((systemTimeout.watchdogEnabled ==0 ) ||
((systemTimeout.watchdogCnt>=(APPLICATION_WATCHDOG_MIN/100)) && (systemTimeout.watchdogCnt<=(APPLICATION_WATCHDOG_MAX/100)))) {
systemTimeout.watchdogMissedCnt=0; // gg, watchog is reset correctly
cmdSendAnswer(ANS_OK);
} else {
systemTimeout.watchdogMissedCnt++; // If you reset at the wrong time, it still count as missed watchdog reset
cmdSendAnswer(ANS_ERR);
}
systemTimeout.watchdogCnt =0;
} else
cmdSendAnswer(ANS_ERR);
break;
case CMD_MOVE:
case CMD_TURN:
if (systemInfos.state == stateRun) { // only state where MOVE or TURN cmds are accepted
if (((systemInfos.cmd == CMD_MOVE) && (systemInfos.distance !=0)) ||
((systemInfos.cmd == CMD_TURN) && (systemInfos.turns !=0))) {
systemInfos.endOfMouvement = 0;
APPLICATION_TransitionToNewState(stateInMouvement);
} else {
if (((systemInfos.cmd == CMD_MOVE) && (systemInfos.distance ==0)) || // This case (M=0 or T=0) correspond to STOP
((systemInfos.cmd == CMD_TURN) && (systemInfos.turns ==0))) {
systemInfos.endOfMouvement = 1;
}
}
cmdSendAnswer(ANS_OK);
} else if (systemInfos.state == stateInMouvement) { // in this state, MOVE and TURN cmds are accepted only if they come with no parameter (0)
if (((systemInfos.cmd == CMD_MOVE) && (systemInfos.distance ==0)) ||
((systemInfos.cmd == CMD_TURN) && (systemInfos.turns ==0))) {
systemInfos.endOfMouvement = 1;
cmdSendAnswer(ANS_OK);
} else { // If MOVE and TURN cmds come with parameters, reject them
cmdSendAnswer(ANS_ERR);
}
} else
cmdSendAnswer(ANS_ERR);
break;
default: // commands no common for every states
break;
}
}
if ((systemInfos.state==stateInMouvement) && (systemInfos.endOfMouvement)) {
APPLICATION_TransitionToNewState(stateRun);
}
if (systemInfos.state== stateInCharge) {
if (!systemInfos.inCharge)
APPLICATION_TransitionToNewState(stateIdle);
else if (systemInfos.batteryUpdate) {
APPLICATION_TransitionToNewState(stateInCharge);
}
}
systemInfos.batteryUpdate = 0;
systemInfos.cmd = CMD_NONE;
systemInfos.endOfMouvement =0;
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;
switch (new_state) {
case stateStartup:
// nothing to do here
break;
case stateIdle:
ledState = leds_idle;
LEDS_Set(ledState);
MOTORS_Stop();
systemTimeout.inactivityCnt=0;
systemTimeout.watchdogEnabled=0;
break;
case stateRun:
if (systemTimeout.watchdogEnabled)
ledState = leds_run_with_watchdog;
else
ledState = leds_run;
LEDS_Set(ledState);
MOTORS_Stop();
break;
case stateInMouvement:
// evoxx-bug-watchdog-avec-moteurs : mauvaise gestion des leds dans le cas où le watchdog est actif
if (systemTimeout.watchdogEnabled)
ledState = leds_run_with_watchdog;
else
ledState = leds_run;
LEDS_Set(ledState);
if (systemInfos.cmd == CMD_MOVE) {
MOTORS_Move( systemInfos.distance);
} else { /* obviously, cmd is CMD_TURN */
MOTORS_Turn(systemInfos.turns);
}
break;
case stateInCharge:
/* les leds sont gerées dans APPLICATION_StateMachine */
MOTORS_Stop();
systemTimeout.watchdogEnabled=0;
break;
case stateWatchdogDisable:
ledState = leds_watchdog_expired;
LEDS_Set(ledState);
MOTORS_Stop(); // evoxx-bug-watchdog-avec-moteurs : oublie d'arreter les moteurs
systemTimeout.watchdogEnabled=0;
break;
case stateLowBatDisable:
ledState = leds_bat_critical_low;
LEDS_Set(ledState);
MOTORS_Stop(); // evoxx-bug-watchdog-avec-moteurs : oublie d'arreter les moteurs
systemTimeout.watchdogEnabled=0;
// evoxx-bug-watchdog-avec-moteurs : le systeme ne s'arrete pas quand la batterie est tres faible:
// envoi du message en priorité, sinon la mailbox se remplie de message et celui-ci est perdu
/* send a message Button_Pressed to enable power off */
MESSAGE_SendMailbox(APPLICATION_Mailbox, MSG_ID_BUTTON_PRESSED, APPLICATION_Mailbox, (void*)NULL);
vTaskDelay(pdMS_TO_TICKS(4000)); // wait 4s
break;
default:
break;
}
systemInfos.state = new_state;
}
/**
* @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) {
HAL_GPIO_WritePin(SHUTDOWN_GPIO_Port, SHUTDOWN_Pin, GPIO_PIN_RESET);
while (1){
__WFE(); // Attente infinie que le regulateur se coupe.
}
}
/**
* @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++;
if (systemTimeout.startupCnt++>=(APPLICATION_STARTUP_DELAY/100))
APPLICATION_TransitionToNewState(stateIdle);
}
if (systemInfos.state != stateInCharge) {
systemTimeout.inactivityCnt++;
if (systemTimeout.inactivityCnt>=(APPLICATION_INACTIVITY_TIMEOUT/100))
/* send a message Button_Pressed to enable power off */
MESSAGE_SendMailbox(APPLICATION_Mailbox, MSG_ID_BUTTON_PRESSED, APPLICATION_Mailbox, (void*)NULL);
if (systemTimeout.watchdogEnabled) {
systemTimeout.watchdogCnt++;
if (systemTimeout.watchdogCnt>(APPLICATION_WATCHDOG_MAX/100)) {
systemTimeout.watchdogCnt=0;
systemTimeout.watchdogMissedCnt++;
}
if (systemTimeout.watchdogMissedCnt>=APPLICATION_WATCHDOG_MISSED_MAX)
APPLICATION_TransitionToNewState(stateWatchdogDisable); /* TODO: Reprendre pour en faire un envoi de message */
}
}
}
/**
* @}
*/
/**
* @}
*/
/**
* @}
*/
Reference in a new issue View git blame Copy permalink