diff --git a/main/auto.cpp b/main/auto.cpp index 92f7e14..beff71d 100644 --- a/main/auto.cpp +++ b/main/auto.cpp @@ -21,15 +21,15 @@ motorCommands_t automatedArmchair::generateCommands() { //check if previous command is finished if ( esp_log_timestamp() > timestampCmdFinished ) { //get next command from queue - if( xQueueReceive( commandQueue, &cmdCurrent, pdMS_TO_TICKS(0) ) ) { + if( xQueueReceive( commandQueue, &cmdCurrent, pdMS_TO_TICKS(500) ) ) { ESP_LOGI(TAG, "running next command from queue..."); //calculate timestamp the command is finished timestampCmdFinished = esp_log_timestamp() + cmdCurrent.msDuration; //copy the new commands - motorCommands = cmdCurrent.commands; + motorCommands = cmdCurrent.motorCmds; } else { //queue empty ESP_LOGD(TAG, "no new command in queue -> set motors to IDLE"); - motorCommands = cmds_bothMotorsIdle; + motorCommands = motorCmds_bothMotorsIdle; } } else { //previous command still running ESP_LOGD(TAG, "command still running -> no change"); @@ -53,6 +53,12 @@ void automatedArmchair::addCommand(commandSimple_t command) { } } +void automatedArmchair::addCommands(commandSimple_t commands[], size_t count) { + for (int i = 0; i < count; i++) { + ESP_LOGI(TAG, "Reading command no. %d from provided array", i); + addCommand(commands[i]); + } +} //=============================== @@ -65,6 +71,7 @@ motorCommands_t automatedArmchair::clearCommands() { xQueueReset( commandQueue ); ESP_LOGW(TAG, "command queue was successfully emptied"); //return commands for idling both motors - return cmds_bothMotorsIdle; + motorCommands = motorCmds_bothMotorsIdle; + return motorCmds_bothMotorsIdle; } diff --git a/main/auto.hpp b/main/auto.hpp index f4db8a6..7a43ce1 100644 --- a/main/auto.hpp +++ b/main/auto.hpp @@ -17,11 +17,17 @@ extern "C" //-------------------------------------------- //---- struct, enum, variable declarations --- //-------------------------------------------- +//enum for special instructions / commands to be run in control task +enum class instructions_t { NONE, SWITCH_PREV_MODE, RESET_ACCEL, RESET_DECEL }; + //struct for a simple command //e.g. put motors in a certain state for certain time -typedef struct { - motorCommands_t commands; +typedef struct commandSimple_t{ + motorCommands_t motorCmds; uint32_t msDuration; + uint32_t fadeDecel; + uint32_t fadeAcel; + instructions_t instructions; } commandSimple_t; @@ -36,11 +42,16 @@ class automatedArmchair { automatedArmchair(void); //function to generate motor commands //can be also seen as handle function - //TODO: maybe create separate task for handling at mode switch and communicate with queue? + //TODO: go with other approach: separate task for handling auto mode + // - receive commands with queue anyways + // - => use delay function + // - have a queue that outputs current motor state/commands -> repeatedly check the queue in control task motorCommands_t generateCommands(); //repeatedly called by control task //function that adds a basic command to the queue void addCommand(commandSimple_t command); + //function that adds an array of basic commands to queue + void addCommands(commandSimple_t commands[], size_t count); //function that deletes all pending/queued commands motorCommands_t clearCommands(); @@ -61,13 +72,13 @@ class automatedArmchair { motorCommands_t motorCommands; //command preset for idling motors - const motorCommand_t cmd_motorIdle = { + const motorCommand_t motorCmd_motorIdle = { .state = motorstate_t::IDLE, .duty = 0 }; - const motorCommands_t cmds_bothMotorsIdle = { - .left = cmd_motorIdle, - .right = cmd_motorIdle + const motorCommands_t motorCmds_bothMotorsIdle = { + .left = motorCmd_motorIdle, + .right = motorCmd_motorIdle }; }; diff --git a/main/button.cpp b/main/button.cpp index b717328..0f03db8 100644 --- a/main/button.cpp +++ b/main/button.cpp @@ -38,6 +38,7 @@ buttonCommands::buttonCommands(gpio_evaluatedSwitch * button_f, controlledArmcha void buttonCommands::action (uint8_t count){ //--- variable declarations --- bool decelEnabled; //for different beeping when toggling + commandSimple_t cmds[8]; //array for commands for automatedArmchair //--- actions based on count --- switch (count){ @@ -54,9 +55,50 @@ void buttonCommands::action (uint8_t count){ esp_restart(); case 1: - ESP_LOGW(TAG, "cmd %d: sending button event to control task", count); - //-> define joystick center or toggle freeze input (executed in control task) - control->sendButtonEvent(count); //TODO: always send button event to control task (not just at count=1) -> control.cpp has to be changed + ////////ESP_LOGW(TAG, "cmd %d: sending button event to control task", count); + //////////-> define joystick center or toggle freeze input (executed in control task) + ////////control->sendButtonEvent(count); //TODO: always send button event to control task (not just at count=1) -> control.cpp has to be changed + //====== TESTING: send cmd to auto mode ===== + //define commands + cmds[1] = + { + .motorCmds = { + .left = {motorstate_t::FWD, 40}, + .right = {motorstate_t::FWD, 40} + }, + .msDuration = 2000, + .fadeDecel = 800, + .fadeAcel = 1300, + .instructions = instructions_t::NONE + }; + cmds[2] = + { + .motorCmds = { + .left = {motorstate_t::IDLE, 0}, + .right = {motorstate_t::IDLE, 0} + }, + .msDuration = 1000, + .fadeDecel = 800, + .fadeAcel = 1300, + .instructions = instructions_t::NONE + }; + cmds[3] = + { + .motorCmds = { + .left = {motorstate_t::REV, 40}, + .right = {motorstate_t::REV, 40} + }, + .msDuration = 1000, + .fadeDecel = 800, + .fadeAcel = 1300, + .instructions = instructions_t::NONE + }; + + //send commands to automatedArmchair command queue + armchair.addCommands(cmds, 3); + + //change mode to AUTO + control->changeMode(controlMode_t::AUTO); break; case 3: diff --git a/main/button.hpp b/main/button.hpp index b5d5900..7afa62e 100644 --- a/main/button.hpp +++ b/main/button.hpp @@ -4,6 +4,8 @@ #include "buzzer.hpp" #include "control.hpp" #include "motorctl.hpp" +#include "auto.hpp" +#include "config.hpp" diff --git a/main/control.cpp b/main/control.cpp index c0ac627..9984570 100644 --- a/main/control.cpp +++ b/main/control.cpp @@ -263,6 +263,12 @@ void controlledArmchair::changeMode(controlMode_t modeNew) { //reset timeout timer resetTimeout(); + //exit if target mode is already active + if (mode == modeNew) { + ESP_LOGE(TAG, "changeMode: Already in target mode '%s' -> nothing to change", controlModeStr[(int)mode]); + return; + } + //copy previous mode controlMode_t modePrevious = mode;