j/armchair_fw
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
armchair_fw/board_control/main/control.cpp
jonny_jr9 76e8bac113 2 boards: split code, outsource common, remove duplicate files 
- outsoruce common files and separate common types from source files (new
  common/types.hpp)
- split source files to 2 board folders (relevant only, no duplicate)
- currently boards compile successfull but not functional at all
- comment out currently incompatible code
2023-08-30 09:01:13 +02:00

481 lines
20 KiB
C++
Raw Blame History

extern "C"
{
#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "esp_log.h"
#include "freertos/queue.h"
//custom C libraries
#include "wifi.h"
}
#include "config.hpp"
#include "control.hpp"
//used definitions moved from config.hpp:
//#define JOYSTICK_TEST
//tag for logging
static const char * TAG = "control";
const char* controlModeStr[7] = {"IDLE", "JOYSTICK", "MASSAGE", "HTTP", "MQTT", "BLUETOOTH", "AUTO"};
//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);
// //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);
//
// //process received instruction
// switch (instruction) {
// case auto_instruction_t::NONE:
// break;
// case auto_instruction_t::SWITCH_PREV_MODE:
// toggleMode(controlMode_t::AUTO);
// break;
// case auto_instruction_t::SWITCH_JOYSTICK_MODE:
// changeMode(controlMode_t::JOYSTICK);
// break;
// case auto_instruction_t::RESET_ACCEL_DECEL:
// //enable downfading (set to default value)
// motorLeft->setFade(fadeType_t::DECEL, true);
// motorRight->setFade(fadeType_t::DECEL, true);
// //set upfading to default value
// motorLeft->setFade(fadeType_t::ACCEL, true);
// motorRight->setFade(fadeType_t::ACCEL, true);
// break;
// case auto_instruction_t::RESET_ACCEL:
// //set upfading to default value
// motorLeft->setFade(fadeType_t::ACCEL, true);
// motorRight->setFade(fadeType_t::ACCEL, true);
// break;
// case auto_instruction_t::RESET_DECEL:
// //enable downfading (set to default value)
// motorLeft->setFade(fadeType_t::DECEL, true);
// motorRight->setFade(fadeType_t::DECEL, true);
// break;
// }
// break;
//
//
// //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(){
// //check for timeout only when not idling already
// if (mode != controlMode_t::IDLE) {
// //get current duty from controlled motor objects
// float dutyLeftNow = motorLeft->getStatus().duty;
// float dutyRightNow = motorRight->getStatus().duty;
//
// //activity detected on any of the two motors
// if (validateActivity(dutyLeft_lastActivity, dutyLeftNow, inactivityTolerance)
// || validateActivity(dutyRight_lastActivity, dutyRightNow, inactivityTolerance)
// ){
// ESP_LOGD(TAG, "timeout check: [activity] detected since last check -> reset");
// //reset last duty and timestamp
// dutyLeft_lastActivity = dutyLeftNow;
// dutyRight_lastActivity = dutyRightNow;
// resetTimeout();
// }
// //no activity on any motor and msTimeout exceeded
// else if (esp_log_timestamp() - timestamp_lastActivity > config.timeoutMs){
// ESP_LOGI(TAG, "timeout check: [TIMEOUT], no activity for more than %.ds -> switch to idle", config.timeoutMs/1000);
// //toggle to idle mode
// toggleIdle();
// }
// else {
// 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
//
// case controlMode_t::HTTP:
// ESP_LOGW(TAG, "switching from http mode -> disabling http and wifi");
// //stop http server
// ESP_LOGI(TAG, "disabling http server...");
// http_stop_server();
//
// //FIXME: make wifi function work here - currently starting wifi at startup (see notes main.cpp)
// //stop wifi
// //TODO: decide whether ap or client is currently used - which has to be disabled?
// //ESP_LOGI(TAG, "deinit wifi...");
// //wifi_deinit_client();
// //wifi_deinit_ap();
// ESP_LOGI(TAG, "done stopping http mode");
// break;
//
// case controlMode_t::MASSAGE:
// ESP_LOGW(TAG, "switching from MASSAGE mode -> restoring fading, reset frozen input");
// //TODO: fix issue when downfading was disabled before switching to massage mode - currently it gets enabled again here...
// //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);
// //reset frozen input state
// freezeInput = false;
// break;
//
// case controlMode_t::AUTO:
// ESP_LOGW(TAG, "switching from AUTO mode -> restoring fading to default");
// //TODO: fix issue when downfading was disabled before switching to auto mode - currently it gets enabled again here...
// //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;
// }
//
//
// //========== 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
//
// //disable downfading (max. deceleration)
// motorLeft->setFade(fadeType_t::DECEL, false);
// motorRight->setFade(fadeType_t::DECEL, false);
// //reduce upfading (increase acceleration)
// 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);
// }
// }
Reference in New Issue View Git Blame Copy Permalink