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

send, receive, apply motorCommands works (proof of concept); add timeout

Browse Source 
- board_control successfully sends motor commands to board_motorctl
- board_motorctl receives and applies motor commands
note: control pcb currently switches to HTTP mode after startup for testing
with data from ui

- partially commented in code that has to be reworked
- control: send commands via uart instead of to motor objects
- board motorctl handled motor: add timeout when no target data
  received (e.g. control pcb offline / uart bugged)
- board motorctl uart: receive motorCommands_t struct and apply data to
  target state of handled motors
- types: fix issue with global motorstateStr variable
This commit is contained in:
jonny_jr9 2023-08-30 09:01:44 +02:00
parent 59a4f8c13f
commit 1e544613ee
13 changed files with 562 additions and 500 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
board_control/main/config.cpp
View File

@ -71,7 +71,7 @@ buzzer_t buzzer(GPIO_NUM_12, 100);
httpJoystick httpJoystickMain(configHttpJoystickMain);
//create global control object (control.hpp)
//controlledArmchair control(configControl, &buzzer, &motorLeft, &motorRight, &joystick, &httpJoystickMain);
controlledArmchair control(configControl, &buzzer, &joystick, &httpJoystickMain);
//create global automatedArmchair object (for auto-mode) (auto.hpp)
automatedArmchair armchair;

2
board_control/main/config.hpp
View File

@ -26,7 +26,7 @@ extern gpio_evaluatedSwitch buttonJoystick;
extern buzzer_t buzzer;
//create global control object
//extern controlledArmchair control;
extern controlledArmchair control;
//create global automatedArmchair object (for auto-mode)
extern automatedArmchair armchair;

725
board_control/main/control.cpp
View File

@ -1,3 +1,4 @@
#include "types.hpp"
extern "C"
{
#include <stdio.h>
@ -11,6 +12,7 @@ extern "C"
#include "config.hpp"
#include "control.hpp"
#include "uart.hpp"
//used definitions moved from config.hpp:
@ -23,120 +25,121 @@ const char* controlModeStr[7] = {"IDLE", "JOYSTICK", "MASSAGE", "HTTP", "MQTT",
//FIXME controlledMotor class not available for this pcb, rework
// //-----------------------------
// //-------- constructor --------
// //-----------------------------
// controlledArmchair::controlledArmchair (
// control_config_t config_f,
// buzzer_t * buzzer_f,
// controlledMotor* motorLeft_f,
// controlledMotor* motorRight_f,
// evaluatedJoystick* joystick_f,
// httpJoystick* httpJoystick_f
// ){
//
// //copy configuration
// config = config_f;
// //copy object pointers
// buzzer = buzzer_f;
// motorLeft = motorLeft_f;
// motorRight = motorRight_f;
// joystick_l = joystick_f,
// httpJoystickMain_l = httpJoystick_f;
// //set default mode from config
// modePrevious = config.defaultMode;
//
// //TODO declare / configure controlled motors here instead of config (unnecessary that button object is globally available - only used here)?
// }
//
//
//
// //----------------------------------
// //---------- Handle loop -----------
// //----------------------------------
// //function that repeatedly generates motor commands depending on the current mode
// //also handles fading and current-limit
// void controlledArmchair::startHandleLoop() {
// while (1){
// ESP_LOGV(TAG, "control task executing... mode=%s", controlModeStr[(int)mode]);
//
// switch(mode) {
// default:
// mode = controlMode_t::IDLE;
// break;
//
// case controlMode_t::IDLE:
// //copy preset commands for idling both motors
// commands = cmds_bothMotorsIdle;
// motorRight->setTarget(commands.right.state, commands.right.duty);
// motorLeft->setTarget(commands.left.state, commands.left.duty);
// vTaskDelay(200 / portTICK_PERIOD_MS);
// #ifdef JOYSTICK_LOG_IN_IDLE
// //get joystick data here (without using it)
// //since loglevel is DEBUG, calculateion details is output
// joystick_l->getData(); //get joystick data here
// #endif
// break;
//
//
// case controlMode_t::JOYSTICK:
// vTaskDelay(20 / portTICK_PERIOD_MS);
// //get current joystick data with getData method of evaluatedJoystick
// stickData = joystick_l->getData();
// //additionaly scale coordinates (more detail in slower area)
// joystick_scaleCoordinatesLinear(&stickData, 0.6, 0.35); //TODO: add scaling parameters to config
// //generate motor commands
// commands = joystick_generateCommandsDriving(stickData, altStickMapping);
// //apply motor commands
// motorRight->setTarget(commands.right.state, commands.right.duty);
// motorLeft->setTarget(commands.left.state, commands.left.duty);
// //TODO make motorctl.setTarget also accept motorcommand struct directly
// break;
//
//
// case controlMode_t::MASSAGE:
// vTaskDelay(10 / portTICK_PERIOD_MS);
// //--- read joystick ---
// //only update joystick data when input not frozen
// if (!freezeInput){
// stickData = joystick_l->getData();
// }
// //--- generate motor commands ---
// //pass joystick data from getData method of evaluatedJoystick to generateCommandsShaking function
// commands = joystick_generateCommandsShaking(stickData);
// //apply motor commands
// motorRight->setTarget(commands.right.state, commands.right.duty);
// motorLeft->setTarget(commands.left.state, commands.left.duty);
// break;
//
//
// case controlMode_t::HTTP:
// //--- get joystick data from queue ---
// //Note this function waits several seconds (httpconfig.timeoutMs) for data to arrive, otherwise Center data or NULL is returned
// //TODO: as described above, when changing modes it might delay a few seconds for the change to apply
// stickData = httpJoystickMain_l->getData();
// //scale coordinates additionally (more detail in slower area)
// joystick_scaleCoordinatesLinear(&stickData, 0.6, 0.4); //TODO: add scaling parameters to config
// ESP_LOGD(TAG, "generating commands from x=%.3f y=%.3f radius=%.3f angle=%.3f", stickData.x, stickData.y, stickData.radius, stickData.angle);
// //--- generate motor commands ---
// //Note: timeout (no data received) is handled in getData method
// commands = joystick_generateCommandsDriving(stickData, altStickMapping);
//
// //--- 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);
// break;
//
//
// case controlMode_t::AUTO:
// vTaskDelay(20 / portTICK_PERIOD_MS);
//-----------------------------
//-------- constructor --------
//-----------------------------
controlledArmchair::controlledArmchair (
control_config_t config_f,
buzzer_t * buzzer_f,
evaluatedJoystick* joystick_f,
httpJoystick* httpJoystick_f
){
//copy configuration
config = config_f;
//copy object pointers
buzzer = buzzer_f;
joystick_l = joystick_f,
httpJoystickMain_l = httpJoystick_f;
//set default mode from config
modePrevious = config.defaultMode;
//TODO declare / configure controlled motors here instead of config (unnecessary that button object is globally available - only used here)?
}
//----------------------------------
//---------- Handle loop -----------
//----------------------------------
//function that repeatedly generates motor commands depending on the current mode
//also handles fading and current-limit
void controlledArmchair::startHandleLoop() {
while (1){
ESP_LOGV(TAG, "control task executing... mode=%s", controlModeStr[(int)mode]);
switch(mode) {
default:
mode = controlMode_t::IDLE;
break;
case controlMode_t::IDLE:
//copy preset commands for idling both motors
commands = cmds_bothMotorsIdle;
uart_sendStruct<motorCommands_t>(commands);
//motorRight->setTarget(commands.right.state, commands.right.duty);
//motorLeft->setTarget(commands.left.state, commands.left.duty);
vTaskDelay(200 / portTICK_PERIOD_MS);
#ifdef JOYSTICK_LOG_IN_IDLE
//get joystick data here (without using it)
//since loglevel is DEBUG, calculateion details is output
joystick_l->getData(); //get joystick data here
#endif
break;
case controlMode_t::JOYSTICK:
vTaskDelay(20 / portTICK_PERIOD_MS);
//get current joystick data with getData method of evaluatedJoystick
stickData = joystick_l->getData();
//additionaly scale coordinates (more detail in slower area)
joystick_scaleCoordinatesLinear(&stickData, 0.6, 0.35); //TODO: add scaling parameters to config
//generate motor commands
commands = joystick_generateCommandsDriving(stickData, altStickMapping);
//apply motor commands
uart_sendStruct<motorCommands_t>(commands);
//motorRight->setTarget(commands.right.state, commands.right.duty);
//motorLeft->setTarget(commands.left.state, commands.left.duty);
//TODO make motorctl.setTarget also accept motorcommand struct directly
break;
case controlMode_t::MASSAGE:
vTaskDelay(10 / portTICK_PERIOD_MS);
//--- read joystick ---
//only update joystick data when input not frozen
if (!freezeInput){
stickData = joystick_l->getData();
}
//--- generate motor commands ---
//pass joystick data from getData method of evaluatedJoystick to generateCommandsShaking function
commands = joystick_generateCommandsShaking(stickData);
//apply motor commands
uart_sendStruct<motorCommands_t>(commands);
//motorRight->setTarget(commands.right.state, commands.right.duty);
//motorLeft->setTarget(commands.left.state, commands.left.duty);
break;
case controlMode_t::HTTP:
//--- get joystick data from queue ---
//Note this function waits several seconds (httpconfig.timeoutMs) for data to arrive, otherwise Center data or NULL is returned
//TODO: as described above, when changing modes it might delay a few seconds for the change to apply
stickData = httpJoystickMain_l->getData();
//scale coordinates additionally (more detail in slower area)
joystick_scaleCoordinatesLinear(&stickData, 0.6, 0.4); //TODO: add scaling parameters to config
ESP_LOGD(TAG, "generating commands from x=%.3f y=%.3f radius=%.3f angle=%.3f", stickData.x, stickData.y, stickData.radius, stickData.angle);
//--- generate motor commands ---
//Note: timeout (no data received) is handled in getData method
commands = joystick_generateCommandsDriving(stickData, altStickMapping);
//--- apply commands to motors ---
//TODO make motorctl.setTarget also accept motorcommand struct directly
uart_sendStruct<motorCommands_t>(commands);
//motorRight->setTarget(commands.right.state, commands.right.duty);
//motorLeft->setTarget(commands.left.state, commands.left.duty);
break;
case controlMode_t::AUTO:
vTaskDelay(20 / portTICK_PERIOD_MS);
// //generate commands
// 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);
// uart_sendStruct<motorCommands_t>(commands);
// //motorRight->setTarget(commands.right.state, commands.right.duty);
// //motorLeft->setTarget(commands.left.state, commands.left.duty);
//
// //process received instruction
// switch (instruction) {
@ -149,124 +152,125 @@ const char* controlModeStr[7] = {"IDLE", "JOYSTICK", "MASSAGE", "HTTP", "MQTT",
// 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;
//// //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;
//// //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);
//// //enable downfading (set to default value)
//// motorLeft->setFade(fadeType_t::DECEL, true);
//// motorRight->setFade(fadeType_t::DECEL, true);
// break;
// }
// break;
//
//
// //TODO: add other modes here
// }
//
//
// //--- run actions based on received button button event ---
// //note: buttonCount received by sendButtonEvent method called from button.cpp
// //TODO: what if variable gets set from other task during this code? -> mutex around this code
// switch (buttonCount) {
// case 1: //define joystick center or freeze input
// if (mode == controlMode_t::JOYSTICK){
// //joystick mode: calibrate joystick
// joystick_l->defineCenter();
// } else if (mode == controlMode_t::MASSAGE){
// //massage mode: toggle freeze of input (lock joystick at current values)
// freezeInput = !freezeInput;
// if (freezeInput){
// buzzer->beep(5, 40, 25);
// } else {
// buzzer->beep(1, 300, 100);
// }
// }
// break;
//
// case 12: //toggle alternative joystick mapping (reverse swapped)
// altStickMapping = !altStickMapping;
// if (altStickMapping){
// buzzer->beep(6, 70, 50);
// } else {
// buzzer->beep(1, 500, 100);
// }
// break;
// }
// //--- reset button event --- (only one action per run)
// if (buttonCount > 0){
// ESP_LOGI(TAG, "resetting button event/count");
// buttonCount = 0;
// }
//
//
//
// //-----------------------
// //------ slow loop ------
// //-----------------------
// //this section is run about every 5s (+500ms)
// if (esp_log_timestamp() - timestamp_SlowLoopLastRun > 5000) {
// ESP_LOGV(TAG, "running slow loop... time since last run: %.1fs", (float)(esp_log_timestamp() - timestamp_SlowLoopLastRun)/1000);
// timestamp_SlowLoopLastRun = esp_log_timestamp();
//
// //run function which detects timeout (switch to idle)
// handleTimeout();
// }
//
// }//end while(1)
// }//end startHandleLoop
//
//
//
// //-----------------------------------
// //---------- resetTimeout -----------
// //-----------------------------------
// void controlledArmchair::resetTimeout(){
// //TODO mutex
// timestamp_lastActivity = esp_log_timestamp();
// }
//
//
//
// //------------------------------------
// //--------- sendButtonEvent ----------
// //------------------------------------
// void controlledArmchair::sendButtonEvent(uint8_t count){
// //TODO mutex - if not replaced with queue
// ESP_LOGI(TAG, "setting button event");
// buttonCount = count;
// }
//
//
//
// //------------------------------------
// //---------- handleTimeout -----------
// //------------------------------------
// //percentage the duty can vary since last timeout check and still counts as incative
// //TODO: add this to config
// float inactivityTolerance = 10;
//
// //local function that checks whether two values differ more than a given tolerance
// bool validateActivity(float dutyOld, float dutyNow, float tolerance){
// float dutyDelta = dutyNow - dutyOld;
// if (fabs(dutyDelta) < tolerance) {
// return false; //no significant activity detected
// } else {
// return true; //there was activity
// }
// }
//
// //function that evaluates whether there is no activity/change on the motor duty for a certain time. If so, a switch to IDLE is issued. - has to be run repeatedly in a slow interval
// void controlledArmchair::handleTimeout(){
break;
//TODO: add other modes here
}
//--- run actions based on received button button event ---
//note: buttonCount received by sendButtonEvent method called from button.cpp
//TODO: what if variable gets set from other task during this code? -> mutex around this code
switch (buttonCount) {
case 1: //define joystick center or freeze input
if (mode == controlMode_t::JOYSTICK){
//joystick mode: calibrate joystick
joystick_l->defineCenter();
} else if (mode == controlMode_t::MASSAGE){
//massage mode: toggle freeze of input (lock joystick at current values)
freezeInput = !freezeInput;
if (freezeInput){
buzzer->beep(5, 40, 25);
} else {
buzzer->beep(1, 300, 100);
}
}
break;
case 12: //toggle alternative joystick mapping (reverse swapped)
altStickMapping = !altStickMapping;
if (altStickMapping){
buzzer->beep(6, 70, 50);
} else {
buzzer->beep(1, 500, 100);
}
break;
}
//--- reset button event --- (only one action per run)
if (buttonCount > 0){
ESP_LOGI(TAG, "resetting button event/count");
buttonCount = 0;
}
//-----------------------
//------ slow loop ------
//-----------------------
//this section is run about every 5s (+500ms)
if (esp_log_timestamp() - timestamp_SlowLoopLastRun > 5000) {
ESP_LOGV(TAG, "running slow loop... time since last run: %.1fs", (float)(esp_log_timestamp() - timestamp_SlowLoopLastRun)/1000);
timestamp_SlowLoopLastRun = esp_log_timestamp();
//run function which detects timeout (switch to idle)
handleTimeout();
}
}//end while(1)
}//end startHandleLoop
//-----------------------------------
//---------- resetTimeout -----------
//-----------------------------------
void controlledArmchair::resetTimeout(){
//TODO mutex
timestamp_lastActivity = esp_log_timestamp();
}
//------------------------------------
//--------- sendButtonEvent ----------
//------------------------------------
void controlledArmchair::sendButtonEvent(uint8_t count){
//TODO mutex - if not replaced with queue
ESP_LOGI(TAG, "setting button event");
buttonCount = count;
}
//------------------------------------
//---------- handleTimeout -----------
//------------------------------------
//percentage the duty can vary since last timeout check and still counts as incative
//TODO: add this to config
float inactivityTolerance = 10;
//local function that checks whether two values differ more than a given tolerance
bool validateActivity(float dutyOld, float dutyNow, float tolerance){
float dutyDelta = dutyNow - dutyOld;
if (fabs(dutyDelta) < tolerance) {
return false; //no significant activity detected
} else {
return true; //there was activity
}
}
//function that evaluates whether there is no activity/change on the motor duty for a certain time. If so, a switch to IDLE is issued. - has to be run repeatedly in a slow interval
//FIXME rework timout to work via uart duy or control input only
void controlledArmchair::handleTimeout(){
// //check for timeout only when not idling already
// if (mode != controlMode_t::IDLE) {
// //get current duty from controlled motor objects
@ -293,43 +297,44 @@ const char* controlModeStr[7] = {"IDLE", "JOYSTICK", "MASSAGE", "HTTP", "MQTT",
// ESP_LOGD(TAG, "timeout check: [inactive], last activity %.1f s ago, timeout after %d s", (float)(esp_log_timestamp() - timestamp_lastActivity)/1000, config.timeoutMs/1000);
// }
// }
// }
//
//
//
// //-----------------------------------
// //----------- changeMode ------------
// //-----------------------------------
// //function to change to a specified control mode
// 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
// modePrevious = mode;
//
// ESP_LOGW(TAG, "=== changing mode from %s to %s ===", controlModeStr[(int)mode], controlModeStr[(int)modeNew]);
//
// //========== commands change FROM mode ==========
// //run functions when changing FROM certain mode
// switch(modePrevious){
// default:
// ESP_LOGI(TAG, "noting to execute when changing FROM this mode");
// break;
//
// #ifdef JOYSTICK_LOG_IN_IDLE
// case controlMode_t::IDLE:
// ESP_LOGI(TAG, "disabling debug output for 'evaluatedJoystick'");
// esp_log_level_set("evaluatedJoystick", ESP_LOG_WARN); //FIXME: loglevel from config
// break;
// #endif
//
}
//-----------------------------------
//----------- changeMode ------------
//-----------------------------------
//function to change to a specified control mode
//FIXME FIXME: replace change with motorLeft object with update config via uart
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
modePrevious = mode;
ESP_LOGW(TAG, "=== changing mode from %s to %s ===", controlModeStr[(int)mode], controlModeStr[(int)modeNew]);
//========== commands change FROM mode ==========
//run functions when changing FROM certain mode
switch(modePrevious){
default:
ESP_LOGI(TAG, "noting to execute when changing FROM this mode");
break;
#ifdef JOYSTICK_LOG_IN_IDLE
case controlMode_t::IDLE:
ESP_LOGI(TAG, "disabling debug output for 'evaluatedJoystick'");
esp_log_level_set("evaluatedJoystick", ESP_LOG_WARN); //FIXME: loglevel from config
break;
#endif
// case controlMode_t::HTTP:
// ESP_LOGW(TAG, "switching from http mode -> disabling http and wifi");
// //stop http server
@ -368,43 +373,43 @@ const char* controlModeStr[7] = {"IDLE", "JOYSTICK", "MASSAGE", "HTTP", "MQTT",
// motorLeft->setFade(fadeType_t::ACCEL, true);
// motorRight->setFade(fadeType_t::ACCEL, true);
// break;
// }
//
//
// //========== commands change TO mode ==========
// //run functions when changing TO certain mode
// switch(modeNew){
// default:
// ESP_LOGI(TAG, "noting to execute when changing TO this mode");
// break;
//
// case controlMode_t::IDLE:
// buzzer->beep(1, 1500, 0);
// #ifdef JOYSTICK_LOG_IN_IDLE
// esp_log_level_set("evaluatedJoystick", ESP_LOG_DEBUG);
// #endif
// break;
//
// case controlMode_t::HTTP:
// ESP_LOGW(TAG, "switching to http mode -> enabling http and wifi");
// //start wifi
// //TODO: decide wether ap or client should be started
// ESP_LOGI(TAG, "init wifi...");
//
// //FIXME: make wifi function work here - currently starting wifi at startup (see notes main.cpp)
// //wifi_init_client();
// //wifi_init_ap();
//
// //wait for wifi
// //ESP_LOGI(TAG, "waiting for wifi...");
// //vTaskDelay(1000 / portTICK_PERIOD_MS);
//
// //start http server
// ESP_LOGI(TAG, "init http server...");
// http_init_server();
// ESP_LOGI(TAG, "done initializing http mode");
// break;
//
}
//========== commands change TO mode ==========
//run functions when changing TO certain mode
switch(modeNew){
default:
ESP_LOGI(TAG, "noting to execute when changing TO this mode");
break;
case controlMode_t::IDLE:
buzzer->beep(1, 1500, 0);
#ifdef JOYSTICK_LOG_IN_IDLE
esp_log_level_set("evaluatedJoystick", ESP_LOG_DEBUG);
#endif
break;
case controlMode_t::HTTP:
ESP_LOGW(TAG, "switching to http mode -> enabling http and wifi");
//start wifi
//TODO: decide wether ap or client should be started
ESP_LOGI(TAG, "init wifi...");
//FIXME: make wifi function work here - currently starting wifi at startup (see notes main.cpp)
//wifi_init_client();
//wifi_init_ap();
//wait for wifi
//ESP_LOGI(TAG, "waiting for wifi...");
//vTaskDelay(1000 / portTICK_PERIOD_MS);
//start http server
ESP_LOGI(TAG, "init http server...");
http_init_server();
ESP_LOGI(TAG, "done initializing http mode");
break;
// case controlMode_t::MASSAGE:
// ESP_LOGW(TAG, "switching to MASSAGE mode -> reducing fading");
// uint32_t shake_msFadeAccel = 500; //TODO: move this to config
@ -416,65 +421,65 @@ const char* controlModeStr[7] = {"IDLE", "JOYSTICK", "MASSAGE", "HTTP", "MQTT",
// motorLeft->setFade(fadeType_t::ACCEL, shake_msFadeAccel);
// motorRight->setFade(fadeType_t::ACCEL, shake_msFadeAccel);
// break;
//
// }
//
// //--- update mode to new mode ---
// //TODO: add mutex
// mode = modeNew;
// }
//
//
// //TODO simplify the following 3 functions? can be replaced by one?
//
// //-----------------------------------
// //----------- toggleIdle ------------
// //-----------------------------------
// //function to toggle between IDLE and previous active mode
// void controlledArmchair::toggleIdle() {
// //toggle between IDLE and previous mode
// toggleMode(controlMode_t::IDLE);
// }
//
//
//
// //------------------------------------
// //----------- toggleModes ------------
// //------------------------------------
// //function to toggle between two modes, but prefer first argument if entirely different mode is currently active
// void controlledArmchair::toggleModes(controlMode_t modePrimary, controlMode_t modeSecondary) {
// //switch to secondary mode when primary is already active
// if (mode == modePrimary){
// ESP_LOGW(TAG, "toggleModes: switching from primaryMode %s to secondarMode %s", controlModeStr[(int)mode], controlModeStr[(int)modeSecondary]);
// buzzer->beep(2,200,100);
// changeMode(modeSecondary); //switch to secondary 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);
// }
// }
//
//
//
// //-----------------------------------
// //----------- 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);
// }
// }
}
//--- update mode to new mode ---
//TODO: add mutex
mode = modeNew;
}
//TODO simplify the following 3 functions? can be replaced by one?
//-----------------------------------
//----------- toggleIdle ------------
//-----------------------------------
//function to toggle between IDLE and previous active mode
void controlledArmchair::toggleIdle() {
//toggle between IDLE and previous mode
toggleMode(controlMode_t::IDLE);
}
//------------------------------------
//----------- toggleModes ------------
//------------------------------------
//function to toggle between two modes, but prefer first argument if entirely different mode is currently active
void controlledArmchair::toggleModes(controlMode_t modePrimary, controlMode_t modeSecondary) {
//switch to secondary mode when primary is already active
if (mode == modePrimary){
ESP_LOGW(TAG, "toggleModes: switching from primaryMode %s to secondarMode %s", controlModeStr[(int)mode], controlModeStr[(int)modeSecondary]);
buzzer->beep(2,200,100);
changeMode(modeSecondary); //switch to secondary 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);
}
}
//-----------------------------------
//----------- 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);
}
}

198
board_control/main/control.hpp
View File

@ -27,104 +27,100 @@
// //==================================
// //========= control class ==========
// //==================================
// //controls the mode the armchair operates
// //repeatedly generates the motor commands corresponding to current mode and sends those to motorcontrol
// class controlledArmchair {
// public:
// //--- constructor ---
// controlledArmchair (
// control_config_t config_f,
// buzzer_t* buzzer_f,
// controlledMotor* motorLeft_f,
// controlledMotor* motorRight_f,
// evaluatedJoystick* joystick_f,
// httpJoystick* httpJoystick_f
// );
//
// //--- functions ---
// //task that repeatedly generates motor commands depending on the current mode
// void startHandleLoop();
//
// //function that changes to a specified control mode
// void changeMode(controlMode_t modeNew);
//
// //function that toggle between IDLE and previous active mode (or default if not switched to certain mode yet)
// void toggleIdle();
//
// //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();
//
// //function for sending a button event (e.g. from button task at event) to control task
// //TODO: use queue instead?
// void sendButtonEvent(uint8_t count);
//
// private:
//
// //--- functions ---
// //function that evaluates whether there is no activity/change on the motor duty for a certain time, if so a switch to IDLE is issued. - has to be run repeatedly in a slow interval
// void handleTimeout();
//
// //--- objects ---
// buzzer_t* buzzer;
// controlledMotor* motorLeft;
// controlledMotor* motorRight;
// httpJoystick* httpJoystickMain_l;
// evaluatedJoystick* joystick_l;
//
// //---variables ---
// //struct for motor commands returned by generate functions of each mode
// motorCommands_t commands;
// //struct with config parameters
// control_config_t config;
//
// //store joystick data
// joystickData_t stickData;
// bool altStickMapping; //alternative joystick mapping (reverse mapped differently)
//
// //variables for http mode
// uint32_t http_timestamp_lastData = 0;
//
// //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;
//
// //definition of mode enum
// controlMode_t mode = controlMode_t::IDLE;
//
// //variable to store mode when toggling IDLE mode
// controlMode_t modePrevious; //default mode
//
// //command preset for idling motors
// const motorCommand_t cmd_motorIdle = {
// .state = motorstate_t::IDLE,
// .duty = 0
// };
// const motorCommands_t cmds_bothMotorsIdle = {
// .left = cmd_motorIdle,
// .right = cmd_motorIdle
// };
//
// //variable for slow loop
// uint32_t timestamp_SlowLoopLastRun = 0;
//
// //variables for detecting timeout (switch to idle, after inactivity)
// float dutyLeft_lastActivity = 0;
// float dutyRight_lastActivity = 0;
// uint32_t timestamp_lastActivity = 0;
// };
//
//
//==================================
//========= control class ==========
//==================================
//controls the mode the armchair operates
//repeatedly generates the motor commands corresponding to current mode and sends those to motorcontrol
class controlledArmchair {
public:
//--- constructor ---
controlledArmchair (
control_config_t config_f,
buzzer_t* buzzer_f,
evaluatedJoystick* joystick_f,
httpJoystick* httpJoystick_f
);
//--- functions ---
//task that repeatedly generates motor commands depending on the current mode
void startHandleLoop();
//function that changes to a specified control mode
void changeMode(controlMode_t modeNew);
//function that toggle between IDLE and previous active mode (or default if not switched to certain mode yet)
void toggleIdle();
//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();
//function for sending a button event (e.g. from button task at event) to control task
//TODO: use queue instead?
void sendButtonEvent(uint8_t count);
private:
//--- functions ---
//function that evaluates whether there is no activity/change on the motor duty for a certain time, if so a switch to IDLE is issued. - has to be run repeatedly in a slow interval
void handleTimeout();
//--- objects ---
buzzer_t* buzzer;
httpJoystick* httpJoystickMain_l;
evaluatedJoystick* joystick_l;
//---variables ---
//struct for motor commands returned by generate functions of each mode
motorCommands_t commands;
//struct with config parameters
control_config_t config;
//store joystick data
joystickData_t stickData;
bool altStickMapping; //alternative joystick mapping (reverse mapped differently)
//variables for http mode
uint32_t http_timestamp_lastData = 0;
//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;
//definition of mode enum
controlMode_t mode = controlMode_t::IDLE;
//variable to store mode when toggling IDLE mode
controlMode_t modePrevious; //default mode
//command preset for idling motors
const motorCommand_t cmd_motorIdle = {
.state = motorstate_t::IDLE,
.duty = 0
};
const motorCommands_t cmds_bothMotorsIdle = {
.left = cmd_motorIdle,
.right = cmd_motorIdle
};
//variable for slow loop
uint32_t timestamp_SlowLoopLastRun = 0;
//variables for detecting timeout (switch to idle, after inactivity)
float dutyLeft_lastActivity = 0;
float dutyRight_lastActivity = 0;
uint32_t timestamp_lastActivity = 0;
};

61
board_control/main/main.cpp
View File

@ -13,7 +13,7 @@ extern "C"
//custom C files
//#include "wifi.h"
#include "wifi.h"
}
@ -25,7 +25,7 @@ extern "C"
//=========================
//only run uart test code at the end
//disables other functionality
#define UART_TEST_ONLY
//#define UART_TEST_ONLY
//tag for logging
@ -71,13 +71,13 @@ void task_control( void * pvParameters ){
//============ button task =============
//======================================
//task that handles the button interface/commands
void task_button( void * pvParameters ){
ESP_LOGI(TAG, "Initializing command-button and starting handle loop");
//create button instance
buttonCommands commandButton(&buttonJoystick, &joystick, &control, &buzzer, &motorLeft, &motorRight);
//start handle loop
commandButton.startHandleLoop();
}
//void task_button( void * pvParameters ){
// ESP_LOGI(TAG, "Initializing command-button and starting handle loop");
// //create button instance
// buttonCommands commandButton(&buttonJoystick, &joystick, &control, &buzzer, &motorLeft, &motorRight);
// //start handle loop
// commandButton.startHandleLoop();
//}
@ -85,16 +85,16 @@ void task_button( void * pvParameters ){
//============== fan task ===============
//=======================================
//task that controlls fans for cooling the drivers
void task_fans( void * pvParameters ){
ESP_LOGI(TAG, "Initializing fans and starting fan handle loop");
//create fan instances with config defined in config.cpp
controlledFan fan(configCooling, &motorLeft, &motorRight);
//repeatedly run fan handle function in a slow loop
while(1){
fan.handle();
vTaskDelay(500 / portTICK_PERIOD_MS);
}
}
//void task_fans( void * pvParameters ){
// ESP_LOGI(TAG, "Initializing fans and starting fan handle loop");
// //create fan instances with config defined in config.cpp
// controlledFan fan(configCooling, &motorLeft, &motorRight);
// //repeatedly run fan handle function in a slow loop
// while(1){
// fan.handle();
// vTaskDelay(500 / portTICK_PERIOD_MS);
// }
//}
@ -142,7 +142,10 @@ void setLoglevels(void){
esp_log_level_set("wifi", ESP_LOG_INFO);
esp_log_level_set("http", ESP_LOG_INFO);
esp_log_level_set("automatedArmchair", ESP_LOG_DEBUG);
//esp_log_level_set("current-sensors", ESP_LOG_INFO);
esp_log_level_set("uart_common", ESP_LOG_INFO);
esp_log_level_set("uart", ESP_LOG_INFO);
//
}
@ -166,25 +169,25 @@ extern "C" void app_main(void) {
//------------------------------
//--- create task for buzzer ---
//------------------------------
xTaskCreate(&task_buzzer, "task_buzzer", 2048, NULL, 2, NULL);
//xTaskCreate(&task_buzzer, "task_buzzer", 2048, NULL, 2, NULL);
//-------------------------------
//--- create task for control ---
//-------------------------------
//task that generates motor commands depending on the current mode and sends those to motorctl task
xTaskCreate(&task_control, "task_control", 4096, NULL, 5, NULL);
xTaskCreate(&task_control, "task_control", 5*4096, NULL, 5, NULL);
//------------------------------
//--- create task for button ---
//------------------------------
//task that evaluates and processes the button input and runs the configured commands
xTaskCreate(&task_button, "task_button", 4096, NULL, 4, NULL);
//xTaskCreate(&task_button, "task_button", 4096, NULL, 4, NULL);
//-----------------------------------
//--- create task for fan control ---
//-----------------------------------
//task that evaluates and processes the button input and runs the configured commands
xTaskCreate(&task_fans, "task_fans", 2048, NULL, 1, NULL);
//xTaskCreate(&task_fans, "task_fans", 2048, NULL, 1, NULL);
//beep at startup
@ -213,16 +216,22 @@ extern "C" void app_main(void) {
// http_init_server();
//--- testing force http mode after startup ---
//control.changeMode(controlMode_t::HTTP);
#endif
//-------------------------------------------
//--- create tasks for uart communication ---
//-------------------------------------------
uart_init();
xTaskCreate(task_uartReceive, "task_uartReceive", 4096, NULL, 10, NULL);
xTaskCreate(task_uartSend, "task_uartSend", 4096, NULL, 10, NULL);
//--- main loop ---
//does nothing except for testing things
//--- testing force http mode after startup ---
vTaskDelay(5000 / portTICK_PERIOD_MS);
control.changeMode(controlMode_t::HTTP);
while(1){
vTaskDelay(1000 / portTICK_PERIOD_MS);
//---------------------------------

9
board_control/main/uart.cpp
View File

@ -20,12 +20,19 @@ extern "C"
static const char * TAG = "uart";
//TESTING
#include "control.hpp"
#include "config.hpp"
//==============================
//====== task_uartReceive ======
//==============================
//TODO copy receive task from board_motorctl/uart.cpp
void task_uartReceive(void *arg){
//--- testing force http mode after startup ---
//TESTING
vTaskDelay(5000 / portTICK_PERIOD_MS);
control.changeMode(controlMode_t::HTTP);
while (1) {
vTaskDelay(200 / portTICK_PERIOD_MS);
}
@ -42,7 +49,7 @@ void task_uartSend(void *arg){
uartData_test_t data = {123, 0, 1.1};
ESP_LOGW(TAG, "startloop...");
while (1) {
vTaskDelay(500 / portTICK_PERIOD_MS);
vTaskDelay(10000 / portTICK_PERIOD_MS);
uart_sendStruct<uartData_test_t>(data);
//change data values

7
board_motorctl/main/main.cpp
View File

@ -103,6 +103,8 @@ void setLoglevels(void){
esp_log_level_set("wifi", ESP_LOG_INFO);
esp_log_level_set("http", ESP_LOG_INFO);
esp_log_level_set("automatedArmchair", ESP_LOG_DEBUG);
esp_log_level_set("uart_common", ESP_LOG_INFO);
esp_log_level_set("uart", ESP_LOG_INFO);
//esp_log_level_set("current-sensors", ESP_LOG_INFO);
}
#endif
@ -146,11 +148,12 @@ extern "C" void app_main(void) {
#ifdef UART_TEST_ONLY
//-------------------------------------------
//--- create tasks for uart communication ---
//-------------------------------------------
uart_init();
xTaskCreate(task_uartReceive, "task_uartReceive", 4096, NULL, 10, NULL);
xTaskCreate(task_uartSend, "task_uartSend", 4096, NULL, 10, NULL);
#endif
//--- main loop ---

12
board_motorctl/main/motorctl.cpp
View File

@ -1,4 +1,6 @@
#include "motorctl.hpp"
#include "esp_log.h"
#include "types.hpp"
//tag for logging
static const char * TAG = "motor-control";
@ -80,6 +82,8 @@ void controlledMotor::handle(){
ESP_LOGD(TAG, "Read command from queue: state=%s, duty=%.2f", motorstateStr[(int)commandReceive.state], commandReceive.duty);
state = commandReceive.state;
dutyTarget = commandReceive.duty;
receiveTimeout = false;
timestamp_commandReceived = esp_log_timestamp();
//--- convert duty ---
//define target duty (-100 to 100) from provided duty and motorstate
@ -102,6 +106,14 @@ void controlledMotor::handle(){
}
}
//--- timeout, no data ---
//turn motors off if no data received for long time (e.g. no uart data / control offline)
if ((esp_log_timestamp() - timestamp_commandReceived) > 3000 && !receiveTimeout){
receiveTimeout = true;
state = motorstate_t::IDLE;
dutyTarget = 0;
ESP_LOGE(TAG, "TIMEOUT, no target data received for more than 3s -> switch to IDLE");
}
//--- calculate increment ---
//calculate increment for fading UP with passed time since last run and configured fade time

3
board_motorctl/main/motorctl.hpp
View File

@ -77,4 +77,7 @@ class controlledMotor {
struct motorCommand_t commandReceive = {};
struct motorCommand_t commandSend = {};
uint32_t timestamp_commandReceived = 0;
bool receiveTimeout = false;
};

30
board_motorctl/main/uart.cpp
View File

@ -1,4 +1,6 @@
#include "uart.hpp"
#include "config.hpp"
#include "types.hpp"
//===== uart board MOTORCTL =====
static const char * TAG = "uart";
@ -8,24 +10,39 @@ static const char * TAG = "uart";
//====== task_uartReceive ======
//==============================
void task_uartReceive(void *arg){
ESP_LOGW(TAG, "receive task started");
//receive data from uart, detect associated struct and copy/handle the data
//TODO use queue instead of check interval?
uartData_test_t testData;
uartData_test_t dataTest;
motorCommands_t dataMotorCommands;
uint8_t receivedData[1024-1];
while(1){
//note: check has to be more frequent than pause time between sending
vTaskDelay(200 / portTICK_PERIOD_MS);
int len = uart_read_bytes(UART_NUM_1, receivedData, sizeof(uartData_test_t), 20 / portTICK_PERIOD_MS);
vTaskDelay(50 / portTICK_PERIOD_MS);
//read bytes (max 1023) until 20ms pause is happening
int len = uart_read_bytes(UART_NUM_1, receivedData, sizeof(receivedData), 20 / portTICK_PERIOD_MS);
uart_flush_input(UART_NUM_1);
if (len < 1) continue;
switch (len){
case sizeof(uartData_test_t):
testData = serialData2Struct<uartData_test_t>(receivedData);
ESP_LOGW(TAG, "received uartDataStruct len=%d DATA: timestamp=%d, id=%d, value=%.1f", len, testData.timestamp, testData.id, testData.value);
dataTest = serialData2Struct<uartData_test_t>(receivedData);
ESP_LOGW(TAG, "received uartDataStruct len=%d DATA: timestamp=%d, id=%d, value=%.1f", len, dataTest.timestamp, dataTest.id, dataTest.value);
break;
case sizeof(motorCommands_t):
dataMotorCommands = serialData2Struct<motorCommands_t>(receivedData);
ESP_LOGI(TAG, "received motorCommands struct len=%d left=%.2f%% right=%.2f%%, update target...", len, dataMotorCommands.left.duty, dataMotorCommands.right.duty);
//update target motor state and duty
motorLeft.setTarget(dataMotorCommands.left.state,
dataMotorCommands.left.duty);
motorRight.setTarget(dataMotorCommands.right.state,
dataMotorCommands.right.duty);
break;
//TODO add other received structs here
default:
ESP_LOGW(TAG, "received data len=%d cant be associated with configures struct", len);
ESP_LOGE(TAG, "received data len=%d cant be associated with configures struct", len);
break;
}
}
@ -38,6 +55,7 @@ void task_uartReceive(void *arg){
//=============================
//TODO copy send task from board_control/uart.cpp
void task_uartSend(void *arg){
ESP_LOGW(TAG, "send task started");
while (1) {
vTaskDelay(500 / portTICK_PERIOD_MS);
}

1
common/CMakeLists.txt
View File

@ -3,6 +3,7 @@ idf_component_register(
"wifi.c"
"buzzer.cpp"
"uart_common.cpp"
"types.cpp"
INCLUDE_DIRS
"."
PRIV_REQUIRES nvs_flash

2
common/types.hpp
View File

@ -19,7 +19,7 @@ extern "C"
//===============================
enum class motorstate_t {IDLE, FWD, REV, BRAKE};
//definition of string array to be able to convert state enum to readable string (defined in motordrivers.cpp)
const char* motorstateStr[] = {"IDLE", "FWD", "REV", "BRAKE"};
extern const char* motorstateStr[4];

10
common/uart_common.hpp
View File

@ -15,8 +15,9 @@ extern "C"
#include "freertos/queue.h"
#include "driver/uart.h"
}
#include "types.hpp"
//struct for testin uart
//struct for testing uart
typedef struct {
uint32_t timestamp;
int id;
@ -24,6 +25,13 @@ typedef struct {
} uartData_test_t;
//unnecessary, using commands struct directly
typedef struct {
uint32_t timestamp;
motorCommands_t commands;
} uartData_motorCommands_t;
//===== uart_init =====
//should be run once at startup
void uart_init(void);