From fb9e9ede25a1bac11fea7a3d5c67074bf8f22c5f Mon Sep 17 00:00:00 2001 From: jonny_l480 Date: Thu, 11 Aug 2022 13:13:05 +0200 Subject: [PATCH] Add working command instructions, optimize control auto.cpp: - forward instructions from commands to newly passed pointer control.cpp: - handle instructions returned at generation of commands in auto mode - add method toggleMode - remove dupliate code in toggleIdle method button.cpp: - optimize command for enabling leg support, add instruction --- main/auto.cpp | 6 +++- main/auto.hpp | 6 ++-- main/button.cpp | 14 +++++----- main/control.cpp | 71 +++++++++++++++++++++++++++++++++++++++--------- main/control.hpp | 5 ++++ 5 files changed, 79 insertions(+), 23 deletions(-) diff --git a/main/auto.cpp b/main/auto.cpp index d5b2246..4cf4a71 100644 --- a/main/auto.cpp +++ b/main/auto.cpp @@ -18,12 +18,16 @@ automatedArmchair::automatedArmchair(void) { //============================== //====== generateCommands ====== //============================== -motorCommands_t automatedArmchair::generateCommands() { +motorCommands_t automatedArmchair::generateCommands(auto_instruction_t * instruction) { + //reset instruction + *instruction = auto_instruction_t::NONE; //check if previous command is finished if ( esp_log_timestamp() > timestampCmdFinished ) { //get next command from queue if( xQueueReceive( commandQueue, &cmdCurrent, pdMS_TO_TICKS(500) ) ) { ESP_LOGI(TAG, "running next command from queue..."); + //copy instruction to be provided to control task + *instruction = cmdCurrent.instruction; //set acceleration / fading parameters according to command motorLeft.setFade(fadeType_t::DECEL, cmdCurrent.fadeDecel); motorRight.setFade(fadeType_t::DECEL, cmdCurrent.fadeDecel); diff --git a/main/auto.hpp b/main/auto.hpp index 061b518..8b799fb 100644 --- a/main/auto.hpp +++ b/main/auto.hpp @@ -18,7 +18,7 @@ extern "C" //---- struct, enum, variable declarations --- //-------------------------------------------- //enum for special instructions / commands to be run in control task -enum class auto_instruction_t { NONE, SWITCH_PREV_MODE, RESET_ACCEL, RESET_DECEL }; +enum class auto_instruction_t { NONE, SWITCH_PREV_MODE, SWITCH_JOYSTICK_MODE, RESET_ACCEL_DECEL, RESET_ACCEL, RESET_DECEL }; //struct for a simple command //e.g. put motors in a certain state for certain time @@ -46,7 +46,9 @@ class automatedArmchair { // - 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 handles automatic driving and returns motor commands + //also provides instructions to be executed in control task via pointer + motorCommands_t generateCommands(auto_instruction_t * instruction); //function that adds a basic command to the queue void addCommand(commandSimple_t command); diff --git a/main/button.cpp b/main/button.cpp index 4829fdc..ef94e94 100644 --- a/main/button.cpp +++ b/main/button.cpp @@ -66,20 +66,20 @@ void buttonCommands::action (uint8_t count){ .left = {motorstate_t::REV, 90}, .right = {motorstate_t::REV, 90} }, - .msDuration = 700, + .msDuration = 1200, .fadeDecel = 800, - .fadeAccel = 1200, + .fadeAccel = 1300, .instruction = auto_instruction_t::NONE }; cmds[1] = { .motorCmds = { - .left = {motorstate_t::FWD, 50}, - .right = {motorstate_t::FWD, 50} + .left = {motorstate_t::FWD, 70}, + .right = {motorstate_t::FWD, 70} }, - .msDuration = 100, + .msDuration = 70, .fadeDecel = 0, - .fadeAccel = 400, + .fadeAccel = 300, .instruction = auto_instruction_t::NONE }; cmds[2] = @@ -91,7 +91,7 @@ void buttonCommands::action (uint8_t count){ .msDuration = 10, .fadeDecel = 800, .fadeAccel = 1300, - .instruction = auto_instruction_t::NONE + .instruction = auto_instruction_t::SWITCH_JOYSTICK_MODE }; //send commands to automatedArmchair command queue diff --git a/main/control.cpp b/main/control.cpp index 9e44895..80b5393 100644 --- a/main/control.cpp +++ b/main/control.cpp @@ -125,11 +125,41 @@ void controlledArmchair::startHandleLoop() { case controlMode_t::AUTO: vTaskDelay(20 / portTICK_PERIOD_MS); //generate commands - commands = armchair.generateCommands(); + commands = armchair.generateCommands(&instruction); //--- apply commands to motors --- //TODO make motorctl.setTarget also accept motorcommand struct directly - motorRight->setTarget(commands.right.state, commands.right.duty); - motorLeft->setTarget(commands.left.state, commands.left.duty); + motorRight->setTarget(commands.right.state, commands.right.duty); + motorLeft->setTarget(commands.left.state, commands.left.duty); + + //process received instruction + switch (instruction) { + case auto_instruction_t::NONE: + break; + case auto_instruction_t::SWITCH_PREV_MODE: + toggleMode(controlMode_t::AUTO); + break; + case auto_instruction_t::SWITCH_JOYSTICK_MODE: + changeMode(controlMode_t::JOYSTICK); + break; + case auto_instruction_t::RESET_ACCEL_DECEL: + //enable downfading (set to default value) + motorLeft->setFade(fadeType_t::DECEL, true); + motorRight->setFade(fadeType_t::DECEL, true); + //set upfading to default value + motorLeft->setFade(fadeType_t::ACCEL, true); + motorRight->setFade(fadeType_t::ACCEL, true); + break; + case auto_instruction_t::RESET_ACCEL: + //set upfading to default value + motorLeft->setFade(fadeType_t::ACCEL, true); + motorRight->setFade(fadeType_t::ACCEL, true); + break; + case auto_instruction_t::RESET_DECEL: + //enable downfading (set to default value) + motorLeft->setFade(fadeType_t::DECEL, true); + motorRight->setFade(fadeType_t::DECEL, true); + break; + } break; @@ -274,7 +304,7 @@ void controlledArmchair::changeMode(controlMode_t modeNew) { } //copy previous mode - controlMode_t modePrevious = mode; + modePrevious = mode; ESP_LOGW(TAG, "=== changing mode from %s to %s ===", controlModeStr[(int)mode], controlModeStr[(int)modeNew]); @@ -383,15 +413,8 @@ void controlledArmchair::changeMode(controlMode_t modeNew) { //----------------------------------- //function to toggle between IDLE and previous active mode void controlledArmchair::toggleIdle() { - if (mode == controlMode_t::IDLE){ - changeMode(modePrevious); //restore previous mode, or default if not switched yet - buzzer->beep(2, 200, 100); - ESP_LOGW(TAG, "toggle idle: switched mode from IDLE to %s", controlModeStr[(int)mode]); - } else { - modePrevious = mode; //store current mode - changeMode(controlMode_t::IDLE); //set mode to IDLE - ESP_LOGW(TAG, "toggle idle: switched mode from %s to IDLE", controlModeStr[(int)mode]); - } + //toggle between IDLE and previous mode + toggleMode(controlMode_t::IDLE); } @@ -415,3 +438,25 @@ void controlledArmchair::toggleModes(controlMode_t modePrimary, controlMode_t mo changeMode(modePrimary); } } + + + +//----------------------------------- +//----------- toggleMode ------------ +//----------------------------------- +//function that toggles between certain mode and previous mode +void controlledArmchair::toggleMode(controlMode_t modePrimary){ + + //switch to previous mode when primary is already active + if (mode == modePrimary){ + ESP_LOGW(TAG, "toggleMode: switching from primaryMode %s to previousMode %s", controlModeStr[(int)mode], controlModeStr[(int)modePrevious]); + //buzzer->beep(2,200,100); + changeMode(modePrevious); //switch to previous mode + } + //switch to primary mode when any other mode is active + else { + ESP_LOGW(TAG, "toggleModes: switching from %s to primary mode %s", controlModeStr[(int)mode], controlModeStr[(int)modePrimary]); + //buzzer->beep(4,200,100); + changeMode(modePrimary); + } +} diff --git a/main/control.hpp b/main/control.hpp index 7c3df17..9715a33 100644 --- a/main/control.hpp +++ b/main/control.hpp @@ -55,6 +55,8 @@ class controlledArmchair { //function that toggles between two modes, but prefers first argument if entirely different mode is currently active void toggleModes(controlMode_t modePrimary, controlMode_t modeSecondary); + //toggle between certain mode and previous mode + void toggleMode(controlMode_t modePrimary); //function that restarts timer which initiates the automatic timeout (switch to IDLE) after certain time of inactivity void resetTimeout(); @@ -91,6 +93,9 @@ class controlledArmchair { //variables for MASSAGE mode bool freezeInput = false; + + //variables for AUTO mode + auto_instruction_t instruction = auto_instruction_t::NONE; //variable to receive instructions from automatedArmchair //variable to store button event uint8_t buttonCount = 0;