From 24d89b96cc524b4969e10dcca49acd9f0beab505 Mon Sep 17 00:00:00 2001
From: jonny_ji7 <jonny@wwad.de>
Date: Thu, 11 Aug 2022 09:30:07 +0200
Subject: [PATCH] Add example command, Improvements
- automaticArmchair:
- add method addCommands for adding an array of
commands to queue
- add keys to simpleCommand struct
- fadeDecel (not used yet)
- fadeAcel (not used yet)
- instructions (new enum for running other commands in control task
, not used yet)
- button.cpp
- add example command to 1x button press (comment out previous cmd
temporarily)
- control.cpp
- update changeMode function: dont do anything when current mode is
already target mode
---
main/auto.cpp | 15 +++++++++++----
main/auto.hpp | 25 ++++++++++++++++++-------
main/button.cpp | 48 +++++++++++++++++++++++++++++++++++++++++++++---
main/button.hpp | 2 ++
main/control.cpp | 6 ++++++
5 files changed, 82 insertions(+), 14 deletions(-)
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;