j/armchair_fw
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add working command instructions, optimize control

Browse Source 
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
This commit is contained in:
jonny_l480 2022-08-11 13:13:05 +02:00
parent 6dfae934d5
commit fb9e9ede25
5 changed files with 79 additions and 23 deletions

6
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);

6
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);

14
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

71
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);
}
}

5
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;