/** ****************************************************************************** * @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 #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 */ } } } /** * @} */ /** * @} */ /** * @} */