j/armchair_fw
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
armchair_fw/main/control.cpp
jonny_l480 73325e08ce Fix bug in scaling, Fix http cmd gen, Fix overflow 
Fix several bugs noticed while testing the preceding commits in dev
branch:

- Fix bug in function scaleCoordinate
  - scaling was wrong resulting in negative/inverted values at start of axis

- Adjust timeout value from 30s to 5min

- Fix http joystick behaivor
  - calculate angle, radius and evaluate position AFTER the coordinates
    have been scaled in control.cpp (bug introduced when switching applying tolerance
    on controller instead of in the app)

- Add independent toleranceZero for X and Y axis -> unnecessary to have
  large tolerance for x axis... makes turning more sensitive

- Fix stack overflow in control task
  - controller crashed repeatedly when logging output was enabled in
    control task
  - -> doubled stack size at task creation in main.cpp

currently works, position hast to be evaluated AFTER coordinate scaling
2022-06-22 09:09:40 +02:00

329 lines
13 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 "http.hpp"
//tag for logging
static const char * TAG = "control";
const char* controlModeStr[7] = {"IDLE", "JOYSTICK", "MASSAGE", "HTTP", "MQTT", "BLUETOOTH", "AUTO"};
//-----------------------------
//-------- constructor --------
//-----------------------------
controlledArmchair::controlledArmchair (
control_config_t config_f,
buzzer_t * buzzer_f,
controlledMotor* motorLeft_f,
controlledMotor* motorRight_f
){
//copy configuration
config = config_f;
//copy object pointers
buzzer = buzzer_f;
motorLeft = motorLeft_f;
motorRight = motorRight_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
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);
break;
case controlMode_t::JOYSTICK:
//generate motor commands
//pass joystick data from getData method of evaluatedJoystick to generateCommandsDriving function
commands = joystick_generateCommandsDriving(joystick.getData());
//TODO: pass pointer to joystick object to control class instead of accessing it directly globally
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
vTaskDelay(20 / portTICK_PERIOD_MS);
break;
case controlMode_t::MASSAGE:
motorRight->setTarget(motorstate_t::IDLE, 0);
motorLeft->setTarget(motorstate_t::IDLE, 0);
//TODO add actual command generation here
vTaskDelay(20 / portTICK_PERIOD_MS);
break;
case controlMode_t::HTTP:
//TODO: outsource this code to http.cpp?
//create emptry struct for receiving data from http function
joystickData_t dataRead = { };
//--- get joystick data from queue ---
if( xQueueReceive( joystickDataQueue, &dataRead, pdMS_TO_TICKS(500) ) ) {
//reset timestamp lastAction
http_timestamp_lastData = esp_log_timestamp();
ESP_LOGD(TAG, "received data (from queue): x=%.3f y=%.3f radius=%.3f angle=%.3f",
dataRead.x, dataRead.y, dataRead.radius, dataRead.angle);
//--- scale coordinates ---
//note: scaleCoordinate function currently can not handle negative input -> added offset to input
// scaleCoordinate(input, min, max, center, tolerance_zero_per, tolerance_end_per)
dataRead.x = scaleCoordinate(dataRead.x+1, 0, 2, 1, config.http_toleranceZeroX_Per, config.http_toleranceEndPer);
dataRead.y = scaleCoordinate(dataRead.y+1, 0, 2, 1, config.http_toleranceZeroY_Per, config.http_toleranceEndPer);
//--- re-calculate radius, angle and position with new/scaled coordinates ---
dataRead.radius = sqrt(pow(dataRead.x,2) + pow(dataRead.y,2));
dataRead.angle = (atan(dataRead.y/dataRead.x) * 180) / 3.141;
dataRead.position = joystick_evaluatePosition(dataRead.x, dataRead.y);
ESP_LOGD(TAG, "processed/scaled data: x=%.3f y=%.3f radius=%.3f", dataRead.x, dataRead.y, dataRead.radius);
//--- generate motor commands ---
//pass received joystick data from http queue to generatecommands function from joystick.hpp
ESP_LOGV(TAG, "generating commands...");
ESP_LOGD(TAG, "generating commands from x=%.3f y=%.3f radius=%.3f angle=%.3f", dataRead.x, dataRead.y, dataRead.radius, dataRead.angle);
commands = joystick_generateCommandsDriving(dataRead);
//--- 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);
}
//--- timeout ---
//turn off motors when motor still on and no new data received for some time
if (
(esp_log_timestamp() - http_timestamp_lastData > config.http_timeoutMs) //no data received for x seconds
&& (commands.left.state != motorstate_t::IDLE || commands.right.state != motorstate_t::IDLE) //at least one motor is still running
){
ESP_LOGE(TAG, "TIMEOUT - no data received for 3s -> stopping motors");
//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);
}
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
//-----------------------------------
//---------- resetTimeout -----------
//-----------------------------------
void controlledArmchair::resetTimeout(){
//TODO mutex
timestamp_lastActivity = esp_log_timestamp();
}
//------------------------------------
//---------- handleTimeout -----------
//------------------------------------
float inactivityTolerance = 10; //percentage the duty can vary since last timeout check and still counts as incative
//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: detected [activity] since last check -> reset");
//reset last duty and timestamp
timestamp_lastActivity = esp_log_timestamp();
dutyLeft_lastActivity = dutyLeftNow;
dutyRight_lastActivity = dutyRightNow;
}
//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 seconds ago", (float)(esp_log_timestamp() - timestamp_lastActivity)/1000);
}
}
}
//-----------------------------------
//----------- changeMode ------------
//-----------------------------------
//function to change to a specified control mode
void controlledArmchair::changeMode(controlMode_t modeNew) {
//reset timeout timer
resetTimeout();
//copy previous mode
controlMode_t modePrevious = mode;
ESP_LOGW(TAG, "=== changing mode from %s to %s ===", controlModeStr[(int)mode], controlModeStr[(int)modeNew]);
//--- 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::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;
}
//--- 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);
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;
}
//--- update mode to new mode ---
//TODO: add mutex
mode = modeNew;
}
//-----------------------------------
//----------- toggleIdle ------------
//-----------------------------------
//function to toggle between IDLE and previous active mode
void controlledArmchair::toggleIdle() {
if (mode == controlMode_t::IDLE){
changeMode(modePrevious); //restore previous mode, or default if not switched yet
buzzer->beep(2, 200, 100);
ESP_LOGW(TAG, "toggle idle: switched mode from IDLE to %s", controlModeStr[(int)mode]);
} else {
modePrevious = mode; //store current mode
changeMode(controlMode_t::IDLE); //set mode to IDLE
ESP_LOGW(TAG, "toggle idle: switched mode from %s to IDLE", controlModeStr[(int)mode]);
}
}
//------------------------------------
//----------- 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);
}
}
Reference in New Issue View Git Blame Copy Permalink