j/armchair_fw
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
armchair_fw/board_single/main/control.cpp
jonny_jr9 4f8c421168 Integrate menu into control, Rework prev. button menu 
Armchair functions as before (all tasks enabled).
Note: probably wrong encoder pin set in encoder.hpp

Old button menu works as usual (opimized code).
You can switch to new MENU state with 1x long press
and exit the menu with 1x long press

button.cpp: - use encoder queue instead of evaluated switch
            - simplify code, rework actions

control.cpp: Add MENU state/mode
            -> control task: turns motors off and idles
            -> button task idles  (button menu disabled)
            -> display task switches state to handle menu
control.cpp: Optimize structure:
            Add methods to freeze stick and toggle stick mapping

display.cpp: show status screen or handle menu depending on mode, simpilfy task

main.cpp: re-enable button task, disable buzzer logging

menu.cpp: Change events, Add menu exit condition
2024-02-14 13:40:46 +01:00

523 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"
#include "chairAdjust.hpp"
//used definitions moved from config.hpp:
//#define JOYSTICK_TEST
//tag for logging
static const char * TAG = "control";
const char* controlModeStr[9] = {"IDLE", "JOYSTICK", "MASSAGE", "HTTP", "MQTT", "BLUETOOTH", "AUTO", "ADJUST_CHAIR", "MENU"};
//-----------------------------
//-------- 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;
case controlMode_t::ADJUST_CHAIR:
vTaskDelay(100 / portTICK_PERIOD_MS);
//--- read joystick ---
stickData = joystick_l->getData();
//--- idle motors ---
commands = cmds_bothMotorsIdle;
motorRight->setTarget(commands.right.state, commands.right.duty);
motorLeft->setTarget(commands.left.state, commands.left.duty);
//--- control armchair position with joystick input ---
controlChairAdjustment(joystick_l->getData(), &legRest, &backRest);
break;
case controlMode_t::MENU:
vTaskDelay(1000 / portTICK_PERIOD_MS);
//nothing to do here, display task handles the menu
//--- idle motors ---
commands = cmds_bothMotorsIdle;
motorRight->setTarget(commands.right.state, commands.right.duty);
motorLeft->setTarget(commands.left.state, commands.left.duty);
break;
//TODO: add other modes here
}
//-----------------------
//------ 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
//---------------------------------------
//------ toggleFreezeInputMassage -------
//---------------------------------------
// releases or locks joystick in place when in massage mode
bool controlledArmchair::toggleFreezeInputMassage()
{
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);
ESP_LOGW(TAG, "joystick input is now locked in place");
}
else
{
buzzer->beep(1, 300, 100);
ESP_LOGW(TAG, "joystick input gets updated again");
}
return freezeInput;
}
else
{
ESP_LOGE(TAG, "can not freeze/unfreeze joystick input - not in MASSAGE mode!");
return 0;
}
}
//-------------------------------------
//------- toggleAltStickMapping -------
//-------------------------------------
// toggle between normal and alternative stick mapping (joystick reverse position inverted)
bool controlledArmchair::toggleAltStickMapping()
{
altStickMapping = !altStickMapping;
if (altStickMapping)
{
buzzer->beep(6, 70, 50);
ESP_LOGW(TAG, "changed to alternative stick mapping");
}
else
{
buzzer->beep(1, 500, 100);
ESP_LOGW(TAG, "changed to default stick mapping");
}
return altStickMapping;
}
//-----------------------------------
//---------- resetTimeout -----------
//-----------------------------------
void controlledArmchair::resetTimeout(){
//TODO mutex
timestamp_lastActivity = esp_log_timestamp();
}
//------------------------------------
//---------- 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;
case controlMode_t::IDLE:
#ifdef JOYSTICK_LOG_IN_IDLE
ESP_LOGI(TAG, "disabling debug output for 'evaluatedJoystick'");
esp_log_level_set("evaluatedJoystick", ESP_LOG_WARN); //FIXME: loglevel from config
#endif
buzzer->beep(1,200,100);
break;
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;
case controlMode_t::ADJUST_CHAIR:
ESP_LOGW(TAG, "switching from ADJUST_CHAIR mode => turning off adjustment motors...");
//prevent motors from being always on in case of mode switch while joystick is not in center thus motors currently moving
legRest.setState(REST_OFF);
backRest.setState(REST_OFF);
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::ADJUST_CHAIR:
ESP_LOGW(TAG, "switching to ADJUST_CHAIR mode -> beep");
buzzer->beep(4,200,100);
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