From 1e544613ee33d083f56d66a4826e650363ae2f67 Mon Sep 17 00:00:00 2001 From: jonny_jr9 Date: Wed, 30 Aug 2023 09:01:44 +0200 Subject: [PATCH] send, receive, apply motorCommands works (proof of concept); add timeout - 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 --- board_control/main/config.cpp | 2 +- board_control/main/config.hpp | 2 +- board_control/main/control.cpp | 725 ++++++++++++++++--------------- board_control/main/control.hpp | 198 +++++---- board_control/main/main.cpp | 61 +-- board_control/main/uart.cpp | 9 +- board_motorctl/main/main.cpp | 7 +- board_motorctl/main/motorctl.cpp | 12 + board_motorctl/main/motorctl.hpp | 3 + board_motorctl/main/uart.cpp | 30 +- common/CMakeLists.txt | 1 + common/types.hpp | 2 +- common/uart_common.hpp | 10 +- 13 files changed, 562 insertions(+), 500 deletions(-) diff --git a/board_control/main/config.cpp b/board_control/main/config.cpp index 4d28e65..876fe14 100644 --- a/board_control/main/config.cpp +++ b/board_control/main/config.cpp @@ -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; diff --git a/board_control/main/config.hpp b/board_control/main/config.hpp index 77ddb80..d0e2edd 100644 --- a/board_control/main/config.hpp +++ b/board_control/main/config.hpp @@ -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; diff --git a/board_control/main/control.cpp b/board_control/main/control.cpp index 3900550..0d0d0a0 100644 --- a/board_control/main/control.cpp +++ b/board_control/main/control.cpp @@ -1,3 +1,4 @@ +#include "types.hpp" extern "C" { #include @@ -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(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(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(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(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(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); + } + } diff --git a/board_control/main/control.hpp b/board_control/main/control.hpp index 9be6775..d3dbe5f 100644 --- a/board_control/main/control.hpp +++ b/board_control/main/control.hpp @@ -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; + }; + + diff --git a/board_control/main/main.cpp b/board_control/main/main.cpp index 86aa88f..901c5fd 100644 --- a/board_control/main/main.cpp +++ b/board_control/main/main.cpp @@ -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); //--------------------------------- diff --git a/board_control/main/uart.cpp b/board_control/main/uart.cpp index 14aacaa..b5edcb5 100644 --- a/board_control/main/uart.cpp +++ b/board_control/main/uart.cpp @@ -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(data); //change data values diff --git a/board_motorctl/main/main.cpp b/board_motorctl/main/main.cpp index d0c051c..a901b94 100644 --- a/board_motorctl/main/main.cpp +++ b/board_motorctl/main/main.cpp @@ -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 --- diff --git a/board_motorctl/main/motorctl.cpp b/board_motorctl/main/motorctl.cpp index df544ea..f2baac5 100644 --- a/board_motorctl/main/motorctl.cpp +++ b/board_motorctl/main/motorctl.cpp @@ -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 diff --git a/board_motorctl/main/motorctl.hpp b/board_motorctl/main/motorctl.hpp index d960d11..32018be 100644 --- a/board_motorctl/main/motorctl.hpp +++ b/board_motorctl/main/motorctl.hpp @@ -77,4 +77,7 @@ class controlledMotor { struct motorCommand_t commandReceive = {}; struct motorCommand_t commandSend = {}; + + uint32_t timestamp_commandReceived = 0; + bool receiveTimeout = false; }; diff --git a/board_motorctl/main/uart.cpp b/board_motorctl/main/uart.cpp index 2121422..90585d8 100644 --- a/board_motorctl/main/uart.cpp +++ b/board_motorctl/main/uart.cpp @@ -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(receivedData); - ESP_LOGW(TAG, "received uartDataStruct len=%d DATA: timestamp=%d, id=%d, value=%.1f", len, testData.timestamp, testData.id, testData.value); + dataTest = serialData2Struct(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(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); } diff --git a/common/CMakeLists.txt b/common/CMakeLists.txt index a47af70..a49da26 100644 --- a/common/CMakeLists.txt +++ b/common/CMakeLists.txt @@ -3,6 +3,7 @@ idf_component_register( "wifi.c" "buzzer.cpp" "uart_common.cpp" + "types.cpp" INCLUDE_DIRS "." PRIV_REQUIRES nvs_flash diff --git a/common/types.hpp b/common/types.hpp index e7986c4..6cf868e 100644 --- a/common/types.hpp +++ b/common/types.hpp @@ -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]; diff --git a/common/uart_common.hpp b/common/uart_common.hpp index 6858c1f..ce34567 100644 --- a/common/uart_common.hpp +++ b/common/uart_common.hpp @@ -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);