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

Merge branch 'dev'- turn-behaivor, exp motor-ctl-modes, screensaver [WIP]

Browse Source 
- boost when turning significantly improved driving experience
- display screensaver (timeouts, moving text, brightness)
- bugfixes
- different approach in motorctl implementing different control modes (needs testing)

Stable to use for now, but still WIP due to some bugs
needs optimizations and issues to be fixed before release
This commit is contained in:
jonny 2024-03-26 16:41:34 +01:00
commit e5fad27899
23 changed files with 1143 additions and 563 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
board_single/main/CMakeLists.txt
View File

@ -1,7 +1,6 @@
idf_component_register( idf_component_register(
SRCS SRCS
"main.cpp" "main.cpp"
"config.cpp"
"control.cpp" "control.cpp"
"button.cpp" "button.cpp"
"fan.cpp" "fan.cpp"

1
board_single/main/auto.cpp
View File

@ -1,5 +1,4 @@
#include "auto.hpp" #include "auto.hpp"
#include "config.hpp"
//tag for logging //tag for logging
static const char * TAG = "automatedArmchair"; static const char * TAG = "automatedArmchair";

36
board_single/main/button.cpp
View File

@ -75,7 +75,10 @@ void buttonCommands::action (uint8_t count, bool lastPressLong){
if (lastPressLong) if (lastPressLong)
{ {
control->changeMode(controlMode_t::MENU); control->changeMode(controlMode_t::MENU);
ESP_LOGW(TAG, "1x long press -> change to menu mode"); ESP_LOGW(TAG, "1x long press -> clear encoder queue and change to menu mode");
// clear encoder event queue (prevent menu from exiting immediately due to long press event just happend)
rotary_encoder_event_t ev;
while (xQueueReceive(encoderQueue, &ev, 0) == pdPASS);
buzzer->beep(20, 20, 10); buzzer->beep(20, 20, 10);
vTaskDelay(500 / portTICK_PERIOD_MS); vTaskDelay(500 / portTICK_PERIOD_MS);
} }
@ -96,15 +99,16 @@ void buttonCommands::action (uint8_t count, bool lastPressLong){
// ## switch to ADJUST_CHAIR mode ## // ## switch to ADJUST_CHAIR mode ##
if (lastPressLong) if (lastPressLong)
{ {
ESP_LOGW(TAG, "cmd %d: toggle ADJUST_CHAIR", count); ESP_LOGW(TAG, "cmd %d: switch to ADJUST_CHAIR", count);
control->toggleMode(controlMode_t::ADJUST_CHAIR); control->changeMode(controlMode_t::ADJUST_CHAIR);
} }
// ## toggle IDLE ## // ## toggle IDLE ##
else { else
{
ESP_LOGW(TAG, "cmd %d: toggle IDLE", count); ESP_LOGW(TAG, "cmd %d: toggle IDLE", count);
control->toggleIdle(); //toggle between idle and previous/default mode control->toggleIdle(); // toggle between idle and previous/default mode
} }
break; break;
case 3: case 3:
// ## switch to JOYSTICK mode ## // ## switch to JOYSTICK mode ##
@ -114,14 +118,14 @@ void buttonCommands::action (uint8_t count, bool lastPressLong){
case 4: case 4:
// ## switch to HTTP mode ## // ## switch to HTTP mode ##
ESP_LOGW(TAG, "cmd %d: toggle between HTTP and JOYSTICK", count); ESP_LOGW(TAG, "cmd %d: switch to HTTP", count);
control->toggleModes(controlMode_t::HTTP, controlMode_t::JOYSTICK); //toggle between HTTP and JOYSTICK mode control->changeMode(controlMode_t::HTTP); //switch to HTTP mode
break; break;
case 6: case 6:
// ## switch to MASSAGE mode ## // ## switch to MASSAGE mode ##
ESP_LOGW(TAG, "cmd %d: toggle between MASSAGE and JOYSTICK", count); ESP_LOGW(TAG, "switch to MASSAGE");
control->toggleModes(controlMode_t::MASSAGE, controlMode_t::JOYSTICK); //toggle between MASSAGE and JOYSTICK mode control->changeMode(controlMode_t::MASSAGE); //switch to MASSAGE mode
break; break;
case 8: case 8:
@ -129,7 +133,7 @@ void buttonCommands::action (uint8_t count, bool lastPressLong){
//toggle deceleration fading between on and off //toggle deceleration fading between on and off
//decelEnabled = motorLeft->toggleFade(fadeType_t::DECEL); //decelEnabled = motorLeft->toggleFade(fadeType_t::DECEL);
//motorRight->toggleFade(fadeType_t::DECEL); //motorRight->toggleFade(fadeType_t::DECEL);
decelEnabled = motorLeft->toggleFade(fadeType_t::ACCEL); decelEnabled = motorLeft->toggleFade(fadeType_t::ACCEL); //TODO remove/simplify this using less functions
motorRight->toggleFade(fadeType_t::ACCEL); motorRight->toggleFade(fadeType_t::ACCEL);
ESP_LOGW(TAG, "cmd %d: toggle deceleration fading to: %d", count, (int)decelEnabled); ESP_LOGW(TAG, "cmd %d: toggle deceleration fading to: %d", count, (int)decelEnabled);
if (decelEnabled){ if (decelEnabled){
@ -155,7 +159,7 @@ void buttonCommands::action (uint8_t count, bool lastPressLong){
// when not in MENU mode, repeatedly receives events from encoder button // when not in MENU mode, repeatedly receives events from encoder button
// and takes the corresponding action // and takes the corresponding action
// this function has to be started once in a separate task // this function has to be started once in a separate task
#define INPUT_TIMEOUT 800 // duration of no button events, after which action is run (implicitly also is 'long-press' time) #define INPUT_TIMEOUT 500 // duration of no button events, after which action is run (implicitly also is 'long-press' time)
void buttonCommands::startHandleLoop() void buttonCommands::startHandleLoop()
{ {
//-- variables -- //-- variables --
@ -178,7 +182,7 @@ void buttonCommands::startHandleLoop()
//-- get events from encoder -- //-- get events from encoder --
if (xQueueReceive(encoderQueue, &ev, INPUT_TIMEOUT / portTICK_PERIOD_MS)) if (xQueueReceive(encoderQueue, &ev, INPUT_TIMEOUT / portTICK_PERIOD_MS))
{ {
control->resetTimeout(); //reset inactivity IDLE timeout control->resetTimeout(); // user input -> reset switch to IDLE timeout
switch (ev.type) switch (ev.type)
{ {
break; break;

1
board_single/main/button.hpp
View File

@ -5,7 +5,6 @@
#include "control.hpp" #include "control.hpp"
#include "motorctl.hpp" #include "motorctl.hpp"
#include "auto.hpp" #include "auto.hpp"
#include "config.hpp"
#include "joystick.hpp" #include "joystick.hpp"

50
board_single/main/config.cpp
View File

@ -45,6 +45,13 @@ void setLoglevels(void)
esp_log_level_set("chair-adjustment", ESP_LOG_INFO); esp_log_level_set("chair-adjustment", ESP_LOG_INFO);
esp_log_level_set("menu", ESP_LOG_INFO); esp_log_level_set("menu", ESP_LOG_INFO);
esp_log_level_set("encoder", ESP_LOG_INFO); esp_log_level_set("encoder", ESP_LOG_INFO);
esp_log_level_set("TESTING", ESP_LOG_ERROR);
} }
//================================== //==================================
@ -91,9 +98,11 @@ sabertooth2x60_config_t sabertoothConfig = {
//--- configure left motor (contol) --- //--- configure left motor (contol) ---
motorctl_config_t configMotorControlLeft = { motorctl_config_t configMotorControlLeft = {
.name = "left", .name = "left",
.loggingEnabled = true,
.msFadeAccel = 1500, // acceleration of the motor (ms it takes from 0% to 100%) .msFadeAccel = 1500, // acceleration of the motor (ms it takes from 0% to 100%)
.msFadeDecel = 1000, // deceleration of the motor (ms it takes from 100% to 0%) .msFadeDecel = 1000, // deceleration of the motor (ms it takes from 100% to 0%)
.currentLimitEnabled = false, .currentLimitEnabled = false,
.tractionControlSystemEnabled = false,
.currentSensor_adc = ADC1_CHANNEL_4, // GPIO32 .currentSensor_adc = ADC1_CHANNEL_4, // GPIO32
.currentSensor_ratedCurrent = 50, .currentSensor_ratedCurrent = 50,
.currentMax = 30, .currentMax = 30,
@ -105,9 +114,11 @@ motorctl_config_t configMotorControlLeft = {
//--- configure right motor (contol) --- //--- configure right motor (contol) ---
motorctl_config_t configMotorControlRight = { motorctl_config_t configMotorControlRight = {
.name = "right", .name = "right",
.loggingEnabled = false,
.msFadeAccel = 1500, // acceleration of the motor (ms it takes from 0% to 100%) .msFadeAccel = 1500, // acceleration of the motor (ms it takes from 0% to 100%)
.msFadeDecel = 1000, // deceleration of the motor (ms it takes from 100% to 0%) .msFadeDecel = 1000, // deceleration of the motor (ms it takes from 100% to 0%)
.currentLimitEnabled = false, .currentLimitEnabled = false,
.tractionControlSystemEnabled = false,
.currentSensor_adc = ADC1_CHANNEL_5, // GPIO33 .currentSensor_adc = ADC1_CHANNEL_5, // GPIO33
.currentSensor_ratedCurrent = 50, .currentSensor_ratedCurrent = 50,
.currentMax = 30, .currentMax = 30,
@ -121,11 +132,9 @@ motorctl_config_t configMotorControlRight = {
//------------------------------ //------------------------------
control_config_t configControl = { control_config_t configControl = {
.defaultMode = controlMode_t::JOYSTICK, // default mode after startup and toggling IDLE .defaultMode = controlMode_t::JOYSTICK, // default mode after startup and toggling IDLE
//--- timeout --- //--- timeouts ---
.timeoutMs = 3 * 60 * 1000, // time of inactivity after which the mode gets switched to IDLE .timeoutSwitchToIdleMs = 5 * 60 * 1000, // time of inactivity after which the mode gets switched to IDLE
.timeoutTolerancePer = 5, // percentage the duty can vary between timeout checks considered still inactive .timeoutNotifyPowerStillOnMs = 6 * 60 * 60 * 1000 // time in IDLE after which buzzer beeps in certain interval (notify "forgot to turn off")
//--- http mode ---
}; };
//------------------------------- //-------------------------------
@ -203,16 +212,21 @@ speedSensor_config_t speedRight_config{
//------------------------- //-------------------------
//-------- display -------- //-------- display --------
//------------------------- //-------------------------
display_config_t display_config { display_config_t display_config{
// hardware initialization
.gpio_scl = GPIO_NUM_22, .gpio_scl = GPIO_NUM_22,
.gpio_sda = GPIO_NUM_23, .gpio_sda = GPIO_NUM_23,
.gpio_reset = -1, //negative number disables reset feature .gpio_reset = -1, // negative number disables reset feature
.width = 128, .width = 128,
.height = 64, .height = 64,
.offsetX = 2, .offsetX = 2,
.flip = false, .flip = false,
.contrast = 0xff, //max: 255 .contrastNormal = 170, // max: 255
}; // display task
.contrastReduced = 30, // max: 255
.timeoutReduceContrastMs = 5 * 60 * 1000, // actions at certain inactivity
.timeoutSwitchToScreensaverMs = 30 * 60 * 1000
};
@ -237,7 +251,19 @@ rotary_encoder_t encoder_config = {
//----------------------------------- //-----------------------------------
//configure parameters for motor command generation from joystick data //configure parameters for motor command generation from joystick data
joystickGenerateCommands_config_t joystickGenerateCommands_config{ joystickGenerateCommands_config_t joystickGenerateCommands_config{
.maxDuty = 100, //-- maxDuty --
.dutyOffset = 5, // duty at which motors start immediately // max duty when both motors are at equal ratio e.g. driving straight forward
.altStickMapping = false, // better to be set less than 100% to have some reserve for boosting the outer tire when turning
.maxDutyStraight = 75,
//-- maxBoost --
// boost is amount of duty added to maxDutyStraight to outer tire while turning
// => turning: inner tire gets slower, outer tire gets faster
// 0: boost = 0 (disabled)
// 100: boost = maxDutyStraight (e.g. when maxDuty is 50, outer motor can still reach 100 (50+50))
.maxRelativeBoostPercentOfMaxDuty = 60,
// 60: when maxDuty is set above 62% (equals 0.6*62 = 38% boost) the outer tire can still reach 100% - below 62 maxDuty the boosted speed is also reduced.
// => setting this value lower prevents desired low max duty configuration from being way to fast in curves.
.dutyOffset = 5, // duty at which motors start immediately
.ratioSnapToOneThreshold = 0.9, // threshold ratio snaps to 1 to have some area of max turning before entering X-Axis-full-rotate mode
.altStickMapping = false // invert reverse direction
}; };

6
board_single/main/config.h Normal file
View File

@ -0,0 +1,6 @@
#pragma once
// outsourced macros / definitions
//-- control.cpp --
//#define JOYSTICK_LOG_IN_IDLE

57
board_single/main/config.hpp
View File

@ -1,57 +0,0 @@
#pragma once
#include "motordrivers.hpp"
#include "motorctl.hpp"
#include "joystick.hpp"
#include "gpio_evaluateSwitch.hpp"
#include "buzzer.hpp"
#include "control.hpp"
#include "fan.hpp"
#include "http.hpp"
#include "auto.hpp"
#include "speedsensor.hpp"
#include "chairAdjust.hpp"
//
////in IDLE mode: set loglevel for evaluatedJoystick to DEBUG
////and repeatedly read joystick e.g. for manually calibrating / testing joystick
////#define JOYSTICK_LOG_IN_IDLE
//
//
////TODO outsource global variables to e.g. global.cpp and only config options here?
//
////create global controlledMotor instances for both motors
//extern controlledMotor motorLeft;
//extern controlledMotor motorRight;
//
////create global joystic instance
//extern evaluatedJoystick joystick;
//
////create global evaluated switch instance for button next to joystick
//extern gpio_evaluatedSwitch buttonJoystick;
//
////create global buzzer object
//extern buzzer_t buzzer;
//
////create global control object
//extern controlledArmchair control;
//
////create global automatedArmchair object (for auto-mode)
//extern automatedArmchair_c armchair;
//
////create global httpJoystick object
////extern httpJoystick httpJoystickMain;
//
////configuration for fans / cooling
//extern fan_config_t configCooling;
//
////create global objects for measuring speed
//extern speedSensor speedLeft;
//extern speedSensor speedRight;
//
////create global objects for controlling the chair position
//extern cControlledRest legRest;
//extern cControlledRest backRest;
//
//

563
board_single/main/control.cpp
View File

@ -9,13 +9,14 @@ extern "C"
#include "wifi.h" #include "wifi.h"
} }
#include "config.hpp" #include "config.h"
#include "control.hpp" #include "control.hpp"
#include "chairAdjust.hpp" #include "chairAdjust.hpp"
#include "display.hpp" // needed for getBatteryPercent()
//used definitions moved from config.hpp: //used definitions moved from config.h:
//#define JOYSTICK_TEST //#define JOYSTICK_LOG_IN_IDLE
//tag for logging //tag for logging
@ -58,6 +59,9 @@ controlledArmchair::controlledArmchair(
// override default config value if maxDuty is found in nvs // override default config value if maxDuty is found in nvs
loadMaxDuty(); loadMaxDuty();
// create semaphore for preventing race condition: mode-change operations while currently still executing certain mode
handleIteration_mutex = xSemaphoreCreateMutex();
} }
@ -70,182 +74,209 @@ controlledArmchair::controlledArmchair(
void task_control( void * pvParameters ){ void task_control( void * pvParameters ){
controlledArmchair * control = (controlledArmchair *)pvParameters; controlledArmchair * control = (controlledArmchair *)pvParameters;
ESP_LOGW(TAG, "Initializing controlledArmchair and starting handle loop"); ESP_LOGW(TAG, "Initializing controlledArmchair and starting handle loop");
//start handle loop (control object declared in config.hpp)
control->startHandleLoop(); control->startHandleLoop();
} }
//---------------------------------- //----------------------------------
//---------- Handle loop ----------- //---------- Handle loop -----------
//---------------------------------- //----------------------------------
//function that repeatedly generates motor commands depending on the current mode // start endless loop that repeatedly calls handle() and handleTimeout() methods
void controlledArmchair::startHandleLoop() { void controlledArmchair::startHandleLoop()
while (1){ {
ESP_LOGV(TAG, "control loop executing... mode=%s", controlModeStr[(int)mode]); while (1)
{
// mutex to prevent race condition with actions beeing run at mode change and previous mode still beeing executed
if (xSemaphoreTake(handleIteration_mutex, portMAX_DELAY) == pdTRUE)
{
//--- handle current mode ---
ESP_LOGV(TAG, "control loop executing... mode='%s'", controlModeStr[(int)mode]);
handle();
switch(mode) { xSemaphoreGive(handleIteration_mutex);
default: } // end mutex
mode = controlMode_t::IDLE;
break;
case controlMode_t::IDLE: //--- slow loop ---
//copy preset commands for idling both motors - now done once at mode change // this section is run approx every 5s (+500ms)
//commands = cmds_bothMotorsIdle; if (esp_log_timestamp() - timestamp_SlowLoopLastRun > 5000)
//motorRight->setTarget(commands.right.state, commands.right.duty); {
//motorLeft->setTarget(commands.left.state, commands.left.duty); ESP_LOGV(TAG, "running slow loop... time since last run: %.1fs", (float)(esp_log_timestamp() - timestamp_SlowLoopLastRun) / 1000);
vTaskDelay(500 / portTICK_PERIOD_MS);
#ifdef JOYSTICK_LOG_IN_IDLE
//get joystick data here (without using it)
//since loglevel is DEBUG, calculation details are output
joystick_l->getData();
#endif
break;
case controlMode_t::JOYSTICK:
vTaskDelay(50 / portTICK_PERIOD_MS);
//get current joystick data with getData method of evaluatedJoystick
stickDataLast = stickData;
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
// only generate when the stick data actually changed (e.g. stick stayed in center)
if (stickData.x != stickDataLast.x || stickData.y != stickDataLast.y)
{
commands = joystick_generateCommandsDriving(stickData, &joystickGenerateCommands_config);
// apply motor commands
motorRight->setTarget(commands.right);
motorLeft->setTarget(commands.left);
}
else
{
vTaskDelay(20 / portTICK_PERIOD_MS);
ESP_LOGD(TAG, "analog joystick data unchanged at %s not updating commands", joystickPosStr[(int)stickData.position]);
}
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);
motorLeft->setTarget(commands.left);
break;
case controlMode_t::HTTP:
//--- get joystick data from queue ---
stickDataLast = stickData;
stickData = httpJoystickMain_l->getData(); //get last stored data from receive queue (waits up to 500ms for new event to arrive)
//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 ---
//only generate when the stick data actually changed (e.g. no new data recevied via http)
if (stickData.x != stickDataLast.x || stickData.y != stickDataLast.y ){
// Note: timeout (no data received) is handled in getData method
commands = joystick_generateCommandsDriving(stickData, &joystickGenerateCommands_config);
//--- apply commands to motors ---
motorRight->setTarget(commands.right);
motorLeft->setTarget(commands.left);
}
else
{
ESP_LOGD(TAG, "http joystick data unchanged at %s not updating commands", joystickPosStr[(int)stickData.position]);
}
break;
case controlMode_t::AUTO:
vTaskDelay(20 / portTICK_PERIOD_MS);
//generate commands
commands = automatedArmchair->generateCommands(&instruction);
//--- apply commands to motors ---
motorRight->setTarget(commands.right);
motorLeft->setTarget(commands.left);
//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; - now done once at mode change
//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; - now done once at mode change
//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(); timestamp_SlowLoopLastRun = esp_log_timestamp();
//--- handle timeouts ---
//run function that detects timeout (switch to idle) // run function that detects timeouts (switch to idle, or notify "forgot to turn off")
handleTimeout(); handleTimeout();
} }
vTaskDelay(5 / portTICK_PERIOD_MS); // small delay necessary to give modeChange() a chance to take the mutex
// TODO: move mode specific delays from handle() to here, to prevent unnecessary long mutex lock
}
}
}//end while(1)
}//end startHandleLoop
//-------------------------------------
//---------- Handle control -----------
//-------------------------------------
// function that repeatedly generates motor commands and runs actions depending on the current mode
void controlledArmchair::handle()
{
switch (mode)
{
default:
//switch to IDLE mode when current mode is not implemented
changeMode(controlMode_t::IDLE);
break;
//------- handle IDLE -------
case controlMode_t::IDLE:
vTaskDelay(500 / portTICK_PERIOD_MS);
// TODO repeatedly set motors to idle, in case driver bugs? Currently 15s motorctl timeout would have to pass
#ifdef JOYSTICK_LOG_IN_IDLE
// get joystick data and log it
joystickData_t data joystick_l->getData();
ESP_LOGI("JOYSTICK_LOG_IN_IDLE", "x=%.3f, y=%.3f, radius=%.3f, angle=%.3f, pos=%s, adcx=%d, adcy=%d",
data.x, data.y, data.radius, data.angle,
joystickPosStr[(int)data.position],
objects->joystick->getRawX(), objects->joystick->getRawY());
#endif
break;
//------- handle JOYSTICK mode -------
case controlMode_t::JOYSTICK:
vTaskDelay(50 / portTICK_PERIOD_MS);
// get current joystick data with getData method of evaluatedJoystick
stickDataLast = stickData;
stickData = joystick_l->getData();
// additionaly scale coordinates (more detail in slower area)
joystick_scaleCoordinatesLinear(&stickData, 0.7, 0.45); // TODO: add scaling parameters to config
// generate motor commands
// only generate when the stick data actually changed (e.g. stick stayed in center)
if (stickData.x != stickDataLast.x || stickData.y != stickDataLast.y)
{
resetTimeout(); // user input -> reset switch to IDLE timeout
commands = joystick_generateCommandsDriving(stickData, &joystickGenerateCommands_config);
// apply motor commands
motorRight->setTarget(commands.right);
motorLeft->setTarget(commands.left);
}
else
{
vTaskDelay(20 / portTICK_PERIOD_MS);
ESP_LOGV(TAG, "analog joystick data unchanged at %s not updating commands", joystickPosStr[(int)stickData.position]);
}
break;
//------- handle MASSAGE mode -------
case controlMode_t::MASSAGE:
vTaskDelay(10 / portTICK_PERIOD_MS);
//--- read joystick ---
// only update joystick data when input not frozen
stickDataLast = stickData;
if (!freezeInput)
stickData = joystick_l->getData();
// reset timeout when joystick data changed
if (stickData.x != stickDataLast.x || stickData.y != stickDataLast.y)
resetTimeout(); // user input -> reset switch to IDLE timeout
//--- 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);
motorLeft->setTarget(commands.left);
break;
//------- handle HTTP mode -------
case controlMode_t::HTTP:
//--- get joystick data from queue ---
stickDataLast = stickData;
stickData = httpJoystickMain_l->getData(); // get last stored data from receive queue (waits up to 500ms for new event to arrive)
// 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 ---
// only generate when the stick data actually changed (e.g. no new data recevied via http)
if (stickData.x != stickDataLast.x || stickData.y != stickDataLast.y)
{
resetTimeout(); // user input -> reset switch to IDLE timeout
// Note: timeout (no data received) is handled in getData method
commands = joystick_generateCommandsDriving(stickData, &joystickGenerateCommands_config);
//--- apply commands to motors ---
motorRight->setTarget(commands.right);
motorLeft->setTarget(commands.left);
}
else
{
ESP_LOGD(TAG, "http joystick data unchanged at %s not updating commands", joystickPosStr[(int)stickData.position]);
}
break;
//------- handle AUTO mode -------
case controlMode_t::AUTO:
vTaskDelay(20 / portTICK_PERIOD_MS);
// generate commands
commands = automatedArmchair->generateCommands(&instruction);
//--- apply commands to motors ---
motorRight->setTarget(commands.right);
motorLeft->setTarget(commands.left);
// 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;
//------- handle ADJUST_CHAIR mode -------
case controlMode_t::ADJUST_CHAIR:
vTaskDelay(100 / portTICK_PERIOD_MS);
//--- read joystick ---
stickDataLast = stickData;
stickData = joystick_l->getData();
//--- control armchair position with joystick input ---
// dont update when stick data did not change
if (stickData.x != stickDataLast.x || stickData.y != stickDataLast.y)
{
resetTimeout(); // user input -> reset switch to IDLE timeout
controlChairAdjustment(joystick_l->getData(), legRest, backRest);
}
break;
//------- handle MENU mode -------
case controlMode_t::MENU:
// nothing to do here, display task handles the menu
vTaskDelay(1000 / portTICK_PERIOD_MS);
break;
// TODO: add other modes here
}
} // end - handle method
@ -310,59 +341,58 @@ void controlledArmchair::idleBothMotors(){
motorLeft->setTarget(cmd_motorIdle); motorLeft->setTarget(cmd_motorIdle);
} }
//----------------------------------- //-----------------------------------
//---------- resetTimeout ----------- //---------- resetTimeout -----------
//----------------------------------- //-----------------------------------
void controlledArmchair::resetTimeout(){ void controlledArmchair::resetTimeout(){
//TODO mutex //TODO mutex
timestamp_lastActivity = esp_log_timestamp(); timestamp_lastActivity = esp_log_timestamp();
ESP_LOGV(TAG, "timeout: activity detected, resetting timeout");
} }
//------------------------------------ //------------------------------------
//---------- handleTimeout ----------- //---------- handleTimeout -----------
//------------------------------------ //------------------------------------
//percentage the duty can vary since last timeout check and still counts as incative // switch to IDLE when no activity (prevent accidential movement)
//TODO: add this to config // notify "power still on" when in IDLE for a very long time (prevent battery drain when forgotten to turn off)
float inactivityTolerance = 10; // this function has to be run repeatedly (can be slow interval)
#define TIMEOUT_POWER_STILL_ON_BEEP_INTERVAL_MS 5 * 60 * 1000 // beep every 5 minutes for someone to notice
#define TIMEOUT_POWER_STILL_ON_BATTERY_THRESHOLD_PERCENT 96 // only notify/beep when below certain percentage (prevent beeping when connected to charger)
// note: timeout durations are configured in config.cpp
void controlledArmchair::handleTimeout()
{
uint32_t noActivityDurationMs = esp_log_timestamp() - timestamp_lastActivity;
// log current inactivity and configured timeouts
ESP_LOGD(TAG, "timeout check: last activity %dmin and %ds ago - timeout IDLE after %ds - notify after power on after %dh",
noActivityDurationMs / 1000 / 60,
noActivityDurationMs / 1000 % 60,
config.timeoutSwitchToIdleMs / 1000,
config.timeoutNotifyPowerStillOnMs / 1000 / 60 / 60);
//local function that checks whether two values differ more than a given tolerance // -- timeout switch to IDLE --
bool validateActivity(float dutyOld, float dutyNow, float tolerance){ // timeout to IDLE when not idling already
float dutyDelta = dutyNow - dutyOld; if (mode != controlMode_t::IDLE && noActivityDurationMs > config.timeoutSwitchToIdleMs)
if (fabs(dutyDelta) < tolerance) { {
return false; //no significant activity detected ESP_LOGW(TAG, "timeout check: [TIMEOUT], no activity for more than %ds -> switch to IDLE", config.timeoutSwitchToIdleMs / 1000);
} else { changeMode(controlMode_t::IDLE);
return true; //there was activity //TODO switch to previous status-screen when activity detected
} }
}
//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 // -- timeout notify "forgot to turn off" --
void controlledArmchair::handleTimeout(){ // repeatedly notify via buzzer when in IDLE for a very long time to prevent battery drain ("forgot to turn off")
//check for timeout only when not idling already // also battery charge-level has to be below certain threshold to prevent beeping in case connected to charger
if (mode != controlMode_t::IDLE) { // note: ignores user input while in IDLE (e.g. encoder rotation)
//get current duty from controlled motor objects else if ((esp_log_timestamp() - timestamp_lastModeChange) > config.timeoutNotifyPowerStillOnMs && getBatteryPercent() < TIMEOUT_POWER_STILL_ON_BATTERY_THRESHOLD_PERCENT)
float dutyLeftNow = motorLeft->getStatus().duty; {
float dutyRightNow = motorRight->getStatus().duty; // beep in certain intervals
if ((esp_log_timestamp() - timestamp_lastTimeoutBeep) > TIMEOUT_POWER_STILL_ON_BEEP_INTERVAL_MS)
//activity detected on any of the two motors {
if (validateActivity(dutyLeft_lastActivity, dutyLeftNow, inactivityTolerance) ESP_LOGW(TAG, "timeout: [TIMEOUT] in IDLE since %.3f hours -> beeping", (float)(esp_log_timestamp() - timestamp_lastModeChange) / 1000 / 60 / 60);
|| validateActivity(dutyRight_lastActivity, dutyRightNow, inactivityTolerance) // TODO dont beep at certain time ranges (e.g. at night)
){ timestamp_lastTimeoutBeep = esp_log_timestamp();
ESP_LOGD(TAG, "timeout check: [activity] detected since last check -> reset"); buzzer->beep(6, 100, 50);
//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);
} }
} }
} }
@ -373,73 +403,85 @@ void controlledArmchair::handleTimeout(){
//----------- changeMode ------------ //----------- changeMode ------------
//----------------------------------- //-----------------------------------
//function to change to a specified control mode //function to change to a specified control mode
void controlledArmchair::changeMode(controlMode_t modeNew) { void controlledArmchair::changeMode(controlMode_t modeNew)
//reset timeout timer {
resetTimeout();
//exit if target mode is already active // exit if target mode is already active
if (mode == modeNew) { if (mode == modeNew)
{
ESP_LOGE(TAG, "changeMode: Already in target mode '%s' -> nothing to change", controlModeStr[(int)mode]); ESP_LOGE(TAG, "changeMode: Already in target mode '%s' -> nothing to change", controlModeStr[(int)mode]);
return; return;
} }
//copy previous mode // mutex to wait for current handle iteration (control-task) to finish
modePrevious = mode; // prevents race conditions where operations when changing mode are run but old mode gets handled still
ESP_LOGI(TAG, "changeMode: waiting for current handle() iteration to finish...");
if (xSemaphoreTake(handleIteration_mutex, portMAX_DELAY) == pdTRUE)
{
// copy previous mode
modePrevious = mode;
// store time changed (needed for timeout)
timestamp_lastModeChange = esp_log_timestamp();
ESP_LOGW(TAG, "=== changing mode from %s to %s ===", controlModeStr[(int)mode], controlModeStr[(int)modeNew]); ESP_LOGW(TAG, "=== changing mode from %s to %s ===", controlModeStr[(int)mode], controlModeStr[(int)modeNew]);
//========== commands change FROM mode ========== //========== commands change FROM mode ==========
//run functions when changing FROM certain mode // run functions when changing FROM certain mode
switch(modePrevious){ switch (modePrevious)
default: {
ESP_LOGI(TAG, "noting to execute when changing FROM this mode"); default:
break; ESP_LOGI(TAG, "noting to execute when changing FROM this mode");
break;
case controlMode_t::IDLE: case controlMode_t::IDLE:
#ifdef JOYSTICK_LOG_IN_IDLE #ifdef JOYSTICK_LOG_IN_IDLE
ESP_LOGI(TAG, "disabling debug output for 'evaluatedJoystick'"); ESP_LOGI(TAG, "disabling debug output for 'evaluatedJoystick'");
esp_log_level_set("evaluatedJoystick", ESP_LOG_WARN); //FIXME: loglevel from config esp_log_level_set("evaluatedJoystick", ESP_LOG_WARN); // FIXME: loglevel from config
#endif #endif
buzzer->beep(1,200,100); buzzer->beep(1, 200, 100);
break; break;
case controlMode_t::HTTP:
ESP_LOGW(TAG, "switching from HTTP mode -> stopping wifi-ap");
wifi_stop_ap();
break;
case controlMode_t::MASSAGE: case controlMode_t::MASSAGE:
ESP_LOGW(TAG, "switching from MASSAGE mode -> restoring fading, reset frozen input"); 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... // TODO: fix issue when downfading was disabled before switching to massage mode - currently it gets enabled again here...
//enable downfading (set to default value) // enable downfading (set to default value)
motorLeft->setFade(fadeType_t::DECEL, true); motorLeft->setFade(fadeType_t::DECEL, true);
motorRight->setFade(fadeType_t::DECEL, true); motorRight->setFade(fadeType_t::DECEL, true);
//set upfading to default value // set upfading to default value
motorLeft->setFade(fadeType_t::ACCEL, true); motorLeft->setFade(fadeType_t::ACCEL, true);
motorRight->setFade(fadeType_t::ACCEL, true); motorRight->setFade(fadeType_t::ACCEL, true);
//reset frozen input state // reset frozen input state
freezeInput = false; freezeInput = false;
break; break;
case controlMode_t::AUTO: case controlMode_t::AUTO:
ESP_LOGW(TAG, "switching from AUTO mode -> restoring fading to default"); 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... // TODO: fix issue when downfading was disabled before switching to auto mode - currently it gets enabled again here...
//enable downfading (set to default value) // enable downfading (set to default value)
motorLeft->setFade(fadeType_t::DECEL, true); motorLeft->setFade(fadeType_t::DECEL, true);
motorRight->setFade(fadeType_t::DECEL, true); motorRight->setFade(fadeType_t::DECEL, true);
//set upfading to default value // set upfading to default value
motorLeft->setFade(fadeType_t::ACCEL, true); motorLeft->setFade(fadeType_t::ACCEL, true);
motorRight->setFade(fadeType_t::ACCEL, true); motorRight->setFade(fadeType_t::ACCEL, true);
break; break;
case controlMode_t::ADJUST_CHAIR: case controlMode_t::ADJUST_CHAIR:
ESP_LOGW(TAG, "switching from ADJUST_CHAIR mode => turning off adjustment motors..."); 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 // 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); legRest->setState(REST_OFF);
backRest->setState(REST_OFF); backRest->setState(REST_OFF);
break; break;
}
} //========== commands change TO mode ==========
// run functions when changing TO certain mode
switch (modeNew)
//========== commands change TO mode ========== {
//run functions when changing TO certain mode
switch(modeNew){
default: default:
ESP_LOGI(TAG, "noting to execute when changing TO this mode"); ESP_LOGI(TAG, "noting to execute when changing TO this mode");
break; break;
@ -448,9 +490,11 @@ void controlledArmchair::changeMode(controlMode_t modeNew) {
ESP_LOGW(TAG, "switching to IDLE mode: turning both motors off, beep"); ESP_LOGW(TAG, "switching to IDLE mode: turning both motors off, beep");
idleBothMotors(); idleBothMotors();
buzzer->beep(1, 900, 0); buzzer->beep(1, 900, 0);
#ifdef JOYSTICK_LOG_IN_IDLE break;
esp_log_level_set("evaluatedJoystick", ESP_LOG_DEBUG);
#endif case controlMode_t::HTTP:
ESP_LOGW(TAG, "switching to HTTP mode -> starting wifi-ap");
wifi_start_ap();
break; break;
case controlMode_t::ADJUST_CHAIR: case controlMode_t::ADJUST_CHAIR:
@ -465,24 +509,25 @@ void controlledArmchair::changeMode(controlMode_t modeNew) {
case controlMode_t::MASSAGE: case controlMode_t::MASSAGE:
ESP_LOGW(TAG, "switching to MASSAGE mode -> reducing fading"); ESP_LOGW(TAG, "switching to MASSAGE mode -> reducing fading");
uint32_t shake_msFadeAccel = 500; //TODO: move this to config uint32_t shake_msFadeAccel = 500; // TODO: move this to config
//disable downfading (max. deceleration) // disable downfading (max. deceleration)
motorLeft->setFade(fadeType_t::DECEL, false); motorLeft->setFade(fadeType_t::DECEL, false);
motorRight->setFade(fadeType_t::DECEL, false); motorRight->setFade(fadeType_t::DECEL, false);
//reduce upfading (increase acceleration) // reduce upfading (increase acceleration)
motorLeft->setFade(fadeType_t::ACCEL, shake_msFadeAccel); motorLeft->setFade(fadeType_t::ACCEL, shake_msFadeAccel);
motorRight->setFade(fadeType_t::ACCEL, shake_msFadeAccel); motorRight->setFade(fadeType_t::ACCEL, shake_msFadeAccel);
break; break;
}
} //--- update mode to new mode ---
mode = modeNew;
//--- update mode to new mode --- // unlock mutex for control task to continue handling modes
//TODO: add mutex xSemaphoreGive(handleIteration_mutex);
mode = modeNew; } // end mutex
} }
//TODO simplify the following 3 functions? can be replaced by one? //TODO simplify the following 3 functions? can be replaced by one?
//----------------------------------- //-----------------------------------
@ -509,7 +554,7 @@ void controlledArmchair::toggleModes(controlMode_t modePrimary, controlMode_t mo
} }
//switch to primary mode when any other mode is active //switch to primary mode when any other mode is active
else { else {
ESP_LOGW(TAG, "toggleModes: switching from %s to primary mode %s", controlModeStr[(int)mode], controlModeStr[(int)modePrimary]); ESP_LOGW(TAG, "toggleModes: switching from '%s' to primary mode '%s'", controlModeStr[(int)mode], controlModeStr[(int)modePrimary]);
//buzzer->beep(4,200,100); //buzzer->beep(4,200,100);
changeMode(modePrimary); changeMode(modePrimary);
} }
@ -525,13 +570,13 @@ void controlledArmchair::toggleMode(controlMode_t modePrimary){
//switch to previous mode when primary is already active //switch to previous mode when primary is already active
if (mode == modePrimary){ if (mode == modePrimary){
ESP_LOGW(TAG, "toggleMode: switching from primaryMode %s to previousMode %s", controlModeStr[(int)mode], controlModeStr[(int)modePrevious]); ESP_LOGW(TAG, "toggleMode: switching from primaryMode '%s' to previousMode '%s'", controlModeStr[(int)mode], controlModeStr[(int)modePrevious]);
//buzzer->beep(2,200,100); //buzzer->beep(2,200,100);
changeMode(modePrevious); //switch to previous mode changeMode(modePrevious); //switch to previous mode
} }
//switch to primary mode when any other mode is active //switch to primary mode when any other mode is active
else { else {
ESP_LOGW(TAG, "toggleModes: switching from %s to primary mode %s", controlModeStr[(int)mode], controlModeStr[(int)modePrimary]); ESP_LOGW(TAG, "toggleModes: switching from '%s' to primary mode '%s'", controlModeStr[(int)mode], controlModeStr[(int)modePrimary]);
//buzzer->beep(4,200,100); //buzzer->beep(4,200,100);
changeMode(modePrimary); changeMode(modePrimary);
} }
@ -540,9 +585,9 @@ void controlledArmchair::toggleMode(controlMode_t modePrimary){
//------------------------------- //-----------------------------
//------ loadDecelDuration ------ //-------- loadMaxDuty --------
//------------------------------- //-----------------------------
// update local config value when maxDuty is stored in nvs // update local config value when maxDuty is stored in nvs
void controlledArmchair::loadMaxDuty(void) void controlledArmchair::loadMaxDuty(void)
{ {
@ -553,11 +598,11 @@ void controlledArmchair::loadMaxDuty(void)
switch (err) switch (err)
{ {
case ESP_OK: case ESP_OK:
ESP_LOGW(TAG, "Successfully read value '%s' from nvs. Overriding default value %.2f with %.2f", "c-maxDuty", joystickGenerateCommands_config.maxDuty, valueRead/100.0); ESP_LOGW(TAG, "Successfully read value '%s' from nvs. Overriding default value %.2f with %.2f", "c-maxDuty", joystickGenerateCommands_config.maxDutyStraight, valueRead/100.0);
joystickGenerateCommands_config.maxDuty = (float)(valueRead/100.0); joystickGenerateCommands_config.maxDutyStraight = (float)(valueRead/100.0);
break; break;
case ESP_ERR_NVS_NOT_FOUND: case ESP_ERR_NVS_NOT_FOUND:
ESP_LOGW(TAG, "nvs: the value '%s' is not initialized yet, keeping default value %.2f", "c-maxDuty", joystickGenerateCommands_config.maxDuty); ESP_LOGW(TAG, "nvs: the value '%s' is not initialized yet, keeping default value %.2f", "c-maxDuty", joystickGenerateCommands_config.maxDutyStraight);
break; break;
default: default:
ESP_LOGE(TAG, "Error (%s) reading nvs!", esp_err_to_name(err)); ESP_LOGE(TAG, "Error (%s) reading nvs!", esp_err_to_name(err));
@ -572,12 +617,12 @@ void controlledArmchair::loadMaxDuty(void)
// note: duty percentage gets stored as uint with factor 100 (to get more precision) // note: duty percentage gets stored as uint with factor 100 (to get more precision)
void controlledArmchair::writeMaxDuty(float newValue){ void controlledArmchair::writeMaxDuty(float newValue){
// check if unchanged // check if unchanged
if(joystickGenerateCommands_config.maxDuty == newValue){ if(joystickGenerateCommands_config.maxDutyStraight == newValue){
ESP_LOGW(TAG, "value unchanged at %.2f, not writing to nvs", newValue); ESP_LOGW(TAG, "value unchanged at %.2f, not writing to nvs", newValue);
return; return;
} }
// update nvs value // update nvs value
ESP_LOGW(TAG, "updating nvs value '%s' from %.2f to %.2f", "c-maxDuty", joystickGenerateCommands_config.maxDuty, newValue) ; ESP_LOGW(TAG, "updating nvs value '%s' from %.2f to %.2f", "c-maxDuty", joystickGenerateCommands_config.maxDutyStraight, newValue) ;
esp_err_t err = nvs_set_u16(*nvsHandle, "c-maxDuty", (uint16_t)(newValue*100)); esp_err_t err = nvs_set_u16(*nvsHandle, "c-maxDuty", (uint16_t)(newValue*100));
if (err != ESP_OK) if (err != ESP_OK)
ESP_LOGE(TAG, "nvs: failed writing"); ESP_LOGE(TAG, "nvs: failed writing");
@ -587,5 +632,5 @@ void controlledArmchair::writeMaxDuty(float newValue){
else else
ESP_LOGI(TAG, "nvs: successfully committed updates"); ESP_LOGI(TAG, "nvs: successfully committed updates");
// update variable // update variable
joystickGenerateCommands_config.maxDuty = newValue; joystickGenerateCommands_config.maxDutyStraight = newValue;
} }

27
board_single/main/control.hpp
View File

@ -27,8 +27,8 @@ extern const char* controlModeStr[9];
typedef struct control_config_t { typedef struct control_config_t {
controlMode_t defaultMode; //default mode after startup and toggling IDLE controlMode_t defaultMode; //default mode after startup and toggling IDLE
//timeout options //timeout options
uint32_t timeoutMs; //time of inactivity after which the mode gets switched to IDLE uint32_t timeoutSwitchToIdleMs; //time of inactivity after which the mode gets switched to IDLE
float timeoutTolerancePer; //percentage the duty can vary between timeout checks considered still inactive uint32_t timeoutNotifyPowerStillOnMs;
} control_config_t; } control_config_t;
@ -37,7 +37,7 @@ typedef struct control_config_t {
//======================================= //=======================================
//task that controls the armchair modes and initiates commands generation and applies them to driver //task that controls the armchair modes and initiates commands generation and applies them to driver
//parameter: pointer to controlledArmchair object //parameter: pointer to controlledArmchair object
void task_control( void * pvParameters ); void task_control( void * controlledArmchair );
@ -64,7 +64,7 @@ class controlledArmchair {
); );
//--- functions --- //--- functions ---
//task that repeatedly generates motor commands depending on the current mode //endless loop that repeatedly calls handle() and handleTimeout() methods respecting mutex
void startHandleLoop(); void startHandleLoop();
//function that changes to a specified control mode //function that changes to a specified control mode
@ -94,11 +94,19 @@ class controlledArmchair {
// configure max dutycycle (in joystick or http mode) // configure max dutycycle (in joystick or http mode)
void setMaxDuty(float maxDutyNew) { writeMaxDuty(maxDutyNew); }; void setMaxDuty(float maxDutyNew) { writeMaxDuty(maxDutyNew); };
float getMaxDuty() const {return joystickGenerateCommands_config.maxDuty; }; float getMaxDuty() const {return joystickGenerateCommands_config.maxDutyStraight; };
// configure max boost (in joystick or http mode)
void setMaxRelativeBoostPer(float newValue) { joystickGenerateCommands_config.maxRelativeBoostPercentOfMaxDuty = newValue; };
float getMaxRelativeBoostPer() const {return joystickGenerateCommands_config.maxRelativeBoostPercentOfMaxDuty; };
uint32_t getInactivityDurationMs() {return esp_log_timestamp() - timestamp_lastActivity;};
private: private:
//--- functions --- //--- functions ---
//generate motor commands or run actions depending on the current mode
void handle();
//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 //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(); void handleTimeout();
@ -144,6 +152,9 @@ class controlledArmchair {
//struct with config parameters //struct with config parameters
control_config_t config; control_config_t config;
//mutex to prevent race condition between handle() and changeMode()
SemaphoreHandle_t handleIteration_mutex;
//store joystick data //store joystick data
joystickData_t stickData = joystickData_center; joystickData_t stickData = joystickData_center;
joystickData_t stickDataLast = joystickData_center; joystickData_t stickDataLast = joystickData_center;
@ -169,10 +180,10 @@ class controlledArmchair {
//variable for slow loop //variable for slow loop
uint32_t timestamp_SlowLoopLastRun = 0; uint32_t timestamp_SlowLoopLastRun = 0;
//variables for detecting timeout (switch to idle, after inactivity) //variables for detecting timeout (switch to idle, or notify "forgot to turn off" after inactivity
float dutyLeft_lastActivity = 0; uint32_t timestamp_lastModeChange = 0;
float dutyRight_lastActivity = 0;
uint32_t timestamp_lastActivity = 0; uint32_t timestamp_lastActivity = 0;
uint32_t timestamp_lastTimeoutBeep = 0;
}; };

167
board_single/main/display.cpp
View File

@ -12,7 +12,16 @@ extern "C"{
#define STARTUP_MSG_TIMEOUT 2000 #define STARTUP_MSG_TIMEOUT 2000
#define ADC_BATT_VOLTAGE ADC1_CHANNEL_6 #define ADC_BATT_VOLTAGE ADC1_CHANNEL_6
#define BAT_CELL_COUNT 7 #define BAT_CELL_COUNT 7
// continously vary display contrast from 0 to 250 in OVERVIEW status screen
//#define BRIGHTNESS_TEST
// if display and driver support hardware scrolling the SCREENSAVER status-screen will be smoother:
//#define HARDWARE_SCROLL_AVAILABLE
//=== variables ===
// every function can access the display configuration from config.cpp
static display_config_t displayConfig;
//-------------------------- //--------------------------
@ -61,7 +70,10 @@ void display_init(display_config_t config){
ssd1306_init(&dev, config.width, config.height, config.offsetX); ssd1306_init(&dev, config.width, config.height, config.offsetX);
ssd1306_clear_screen(&dev, false); ssd1306_clear_screen(&dev, false);
ssd1306_contrast(&dev, config.contrast); ssd1306_contrast(&dev, config.contrastNormal);
//store configuration locally (e.g. for accessing timeouts)
displayConfig = config;
} }
@ -233,8 +245,8 @@ float getBatteryPercent()
//############################# //#############################
//shows overview on entire display: //shows overview on entire display:
//Battery percentage, voltage, current, mode, rpm, speed //Battery percentage, voltage, current, mode, rpm, speed
#define STATUS_SCREEN_OVERVIEW_UPDATE_INTERVAL 500 #define STATUS_SCREEN_OVERVIEW_UPDATE_INTERVAL 400
void showStatusScreenOverview(display_task_parameters_t * objects) void showStatusScreenOverview(display_task_parameters_t *objects)
{ {
//-- battery percentage -- //-- battery percentage --
// TODO update when no load (currentsensors = ~0A) only // TODO update when no load (currentsensors = ~0A) only
@ -263,6 +275,17 @@ void showStatusScreenOverview(display_task_parameters_t * objects)
objects->speedLeft->getRpm(), objects->speedLeft->getRpm(),
objects->speedRight->getRpm()); objects->speedRight->getRpm());
vTaskDelay(STATUS_SCREEN_OVERVIEW_UPDATE_INTERVAL / portTICK_PERIOD_MS); vTaskDelay(STATUS_SCREEN_OVERVIEW_UPDATE_INTERVAL / portTICK_PERIOD_MS);
//-- brightness test --
#ifdef BRIGHTNESS_TEST
// continously vary brightness/contrast for testing
displayConfig.contrastNormal += 10;
if (displayConfig.contrastNormal > 255)
displayConfig.contrastNormal = 0;
ssd1306_contrast(&dev, displayConfig.contrastNormal);
vTaskDelay(100 / portTICK_PERIOD_MS);
ESP_LOGW(TAG, "TEST BRIGHTNESS, setting to %d", displayConfig.contrastNormal);
#endif
} }
@ -315,8 +338,6 @@ void showStatusScreenJoystick(display_task_parameters_t * objects)
#define STATUS_SCREEN_MOTORS_UPDATE_INTERVAL 150 #define STATUS_SCREEN_MOTORS_UPDATE_INTERVAL 150
void showStatusScreenMotors(display_task_parameters_t *objects) void showStatusScreenMotors(display_task_parameters_t *objects)
{ {
// print all joystick data
joystickData_t data = objects->joystick->getData();
displayTextLine(&dev, 0, true, false, "%-4.0fW ", fabs(objects->motorLeft->getCurrentA()) * getBatteryVoltage()); displayTextLine(&dev, 0, true, false, "%-4.0fW ", fabs(objects->motorLeft->getCurrentA()) * getBatteryVoltage());
displayTextLine(&dev, 3, true, false, "%-4.0fW ", fabs(objects->motorRight->getCurrentA()) * getBatteryVoltage()); displayTextLine(&dev, 3, true, false, "%-4.0fW ", fabs(objects->motorRight->getCurrentA()) * getBatteryVoltage());
//displayTextLine(&dev, 0, true, false, "L:%02.0f%%", objects->motorLeft->getStatus().duty); //displayTextLine(&dev, 0, true, false, "L:%02.0f%%", objects->motorLeft->getStatus().duty);
@ -331,6 +352,71 @@ void showStatusScreenMotors(display_task_parameters_t *objects)
} }
// ################################
// #### showScreen Screensaver ####
// ################################
// show inactivity duration and battery perventage scrolling across screen the entire screen to prevent burn in
#define STATUS_SCREEN_SCREENSAVER_DELAY_NEXT_LINE_MS 10 * 1000
#define STATUS_SCREEN_SCREENSAVER_UPDATE_INTERVAL 500
#define DISPLAY_HORIZONTAL_CHARACTER_COUNT 16
#define DISPLAY_VERTICAL_LINE_COUNT 8
void showStatusScreenScreensaver(display_task_parameters_t *objects)
{
//-- variables for line rotation --
static int msPassed = 0;
static int currentLine = 0;
static bool lineChanging = false;
// clear display once when rotating to next line
if (lineChanging)
{
ssd1306_clear_screen(&dev, false);
lineChanging = false;
}
//-- print 2 lines scrolling horizontally --
#ifdef HARDWARE_SCROLL_AVAILABLE // when display supports hardware scrolling -> only the content has to be updated
// note: scrolling is enabled at screen change (display_selectStatusPage())
// update text every iteration to prevent empty screen at start
displayTextLine(&dev, currentLine, false, false, "IDLE since:");
displayTextLine(&dev, currentLine + 1, false, false, "%.1fh, B:%02.0f%%",
(float)objects->control->getInactivityDurationMs() / 1000 / 60 / 60,
getBatteryPercent());
// note: scrolling is disabled at screen change (display_selectStatusPage())
#else // custom implementation to scroll the text 1 character to the right every iteration (also wraps over the end to beginning)
static int offset = DISPLAY_HORIZONTAL_CHARACTER_COUNT;
char buf1[64], buf2[64];
// scroll text left to right (taken window of the string moves to the left => offset 16->0, 16->0 ...)
offset -= 1;
if (offset < 0)
offset = DISPLAY_HORIZONTAL_CHARACTER_COUNT - 1; // 0 = no crop -> start over with crop
// note: these strings have to be symetrical and 2x display character count long
snprintf(buf1, 64, "IDLE since: IDLE since: ");
snprintf(buf2, 64, "%.1fh, B:%02.0f%% %.1fh, B:%02.0f%% ",
(float)objects->control->getInactivityDurationMs() / 1000 / 60 / 60,
getBatteryPercent(),
(float)objects->control->getInactivityDurationMs() / 1000 / 60 / 60,
getBatteryPercent());
// print strings on display while limiting to certain window (ignore certain count of characters at start)
displayTextLine(&dev, currentLine, false, false, "%s", buf1 + offset);
displayTextLine(&dev, currentLine + 1, false, false, "%s", buf2 + offset);
#endif
//-- handle line rotation --
// to not block the display task for several seconds returning every e.g. 500ms here
// -> ensures detection of activity (exit condition) in task loop is handled regularly
if (msPassed > STATUS_SCREEN_SCREENSAVER_DELAY_NEXT_LINE_MS) // switch to next line is due
{
msPassed = 0; // rest seconds count
// increment / rotate to next line
if (++currentLine >= DISPLAY_VERTICAL_LINE_COUNT - 1) // rotate to next line
currentLine = 0;
lineChanging = true; // clear screen in next run
}
//-- wait update interval --
// wait and increment passed time after each run
vTaskDelay(STATUS_SCREEN_SCREENSAVER_UPDATE_INTERVAL / portTICK_PERIOD_MS);
msPassed += STATUS_SCREEN_SCREENSAVER_UPDATE_INTERVAL;
}
//######################## //########################
//#### showStartupMsg #### //#### showStartupMsg ####
//######################## //########################
@ -353,16 +439,44 @@ void showStartupMsg(){
//============================ //============================
//===== selectStatusPage ===== //===== selectStatusPage =====
//============================ //============================
void display_selectStatusPage(displayStatusPage_t newStatusPage){ void display_selectStatusPage(displayStatusPage_t newStatusPage)
{
//-- run commands when switching FROM certain mode --
switch (selectedStatusPage)
{
#ifdef HARDWARE_SCROLL_AVAILABLE
case STATUS_SCREEN_SCREENSAVER:
ssd1306_hardware_scroll(&dev, SCROLL_STOP); // disable scrolling when exiting screensaver
break;
#endif
default:
break;
}
ESP_LOGW(TAG, "switching statusPage from %d to %d", (int)selectedStatusPage, (int)newStatusPage); ESP_LOGW(TAG, "switching statusPage from %d to %d", (int)selectedStatusPage, (int)newStatusPage);
selectedStatusPage = newStatusPage; selectedStatusPage = newStatusPage;
//-- run commands when switching TO certain mode --
switch (selectedStatusPage)
{
case STATUS_SCREEN_SCREENSAVER:
ssd1306_clear_screen(&dev, false); // clear screen when switching
#ifdef HARDWARE_SCROLL_AVAILABLE
ssd1306_hardware_scroll(&dev, SCROLL_RIGHT);
#endif
break;
default:
break;
}
} }
//============================ //============================
//======= display task ======= //======= display task =======
//============================ //============================
// TODO: separate task for each loop? // TODO: separate task for each loop?
void display_task(void *pvParameters) void display_task(void *pvParameters)
{ {
ESP_LOGW(TAG, "Initializing display and starting handle loop"); ESP_LOGW(TAG, "Initializing display and starting handle loop");
@ -403,6 +517,47 @@ void display_task(void *pvParameters)
case STATUS_SCREEN_MOTORS: case STATUS_SCREEN_MOTORS:
showStatusScreenMotors(objects); showStatusScreenMotors(objects);
break; break;
case STATUS_SCREEN_SCREENSAVER:
showStatusScreenScreensaver(objects);
break;
}
//--- handle timeouts ---
uint32_t inactiveMs = objects->control->getInactivityDurationMs();
//-- screensaver --
// handle switch to screensaver when no user input for a long time
if (inactiveMs > displayConfig.timeoutSwitchToScreensaverMs) // timeout - switch to screensaver is due
{
if (selectedStatusPage != STATUS_SCREEN_SCREENSAVER){ // switch/log only once at change
ESP_LOGW(TAG, "no activity for more than %d min, switching to screensaver", inactiveMs / 1000 / 60);
display_selectStatusPage(STATUS_SCREEN_SCREENSAVER);
}
}
else if (selectedStatusPage == STATUS_SCREEN_SCREENSAVER) // exit screensaver when there was recent activity
{
ESP_LOGW(TAG, "recent activity detected, disabling screensaver");
display_selectStatusPage(STATUS_SCREEN_OVERVIEW);
}
//-- reduce brightness --
// handle brightness reduction when no user input for some time
static bool brightnessIsReduced = false;
if (inactiveMs > displayConfig.timeoutReduceContrastMs) // threshold exceeded - reduction of brightness is due
{
if (!brightnessIsReduced) //change / log only once at change
{
// reduce display brightness (less burn in)
ESP_LOGW(TAG, "no activity for more than %d min, reducing display brightness to %d/255", inactiveMs / 1000 / 60, displayConfig.contrastReduced);
ssd1306_contrast(&dev, displayConfig.contrastReduced);
brightnessIsReduced = true;
}
}
else if (brightnessIsReduced) // threshold not exceeded anymore, but still reduced
{
// increase display brighness again
ESP_LOGW(TAG, "recent activity detected, increasing brightness again");
ssd1306_contrast(&dev, displayConfig.contrastNormal);
brightnessIsReduced = false;
} }
} }
// TODO add pages and menus // TODO add pages and menus

12
board_single/main/display.hpp
View File

@ -22,6 +22,7 @@ extern "C" {
// configuration for initializing display (passed to task as well) // configuration for initializing display (passed to task as well)
typedef struct display_config_t { typedef struct display_config_t {
// initialization
gpio_num_t gpio_scl; gpio_num_t gpio_scl;
gpio_num_t gpio_sda; gpio_num_t gpio_sda;
int gpio_reset; // negative number means reset pin is not connected or not used int gpio_reset; // negative number means reset pin is not connected or not used
@ -29,7 +30,11 @@ typedef struct display_config_t {
int height; int height;
int offsetX; int offsetX;
bool flip; bool flip;
int contrast; // display-task
int contrastNormal;
int contrastReduced;
uint32_t timeoutReduceContrastMs;
uint32_t timeoutSwitchToScreensaverMs;
} display_config_t; } display_config_t;
@ -49,11 +54,14 @@ typedef struct display_task_parameters_t {
// enum for selecting the currently shown status page (display content when not in MENU mode) // enum for selecting the currently shown status page (display content when not in MENU mode)
typedef enum displayStatusPage_t {STATUS_SCREEN_OVERVIEW=0, STATUS_SCREEN_SPEED, STATUS_SCREEN_JOYSTICK, STATUS_SCREEN_MOTORS} displayStatusPage_t; typedef enum displayStatusPage_t {STATUS_SCREEN_OVERVIEW=0, STATUS_SCREEN_SPEED, STATUS_SCREEN_JOYSTICK, STATUS_SCREEN_MOTORS, STATUS_SCREEN_SCREENSAVER} displayStatusPage_t;
// get precise battery voltage (using lookup table) // get precise battery voltage (using lookup table)
float getBatteryVoltage(); float getBatteryVoltage();
// get battery charge level in percent (using lookup table as discharge curve)
float getBatteryPercent();
// function to select one of the defined status screens which are shown on display when not in MENU mode // function to select one of the defined status screens which are shown on display when not in MENU mode
void display_selectStatusPage(displayStatusPage_t newStatusPage); void display_selectStatusPage(displayStatusPage_t newStatusPage);

46
board_single/main/main.cpp
View File

@ -27,7 +27,6 @@ extern "C"
#include "motorctl.hpp" #include "motorctl.hpp"
//folder single_board //folder single_board
#include "config.hpp"
#include "control.hpp" #include "control.hpp"
#include "button.hpp" #include "button.hpp"
#include "display.hpp" #include "display.hpp"
@ -108,7 +107,6 @@ nvs_handle_t nvsHandle;
//================================= //=================================
//initialize spi flash filesystem (used for webserver) //initialize spi flash filesystem (used for webserver)
void init_spiffs(){ void init_spiffs(){
ESP_LOGW(TAG, "initializing spiffs...");
esp_vfs_spiffs_conf_t esp_vfs_spiffs_conf = { esp_vfs_spiffs_conf_t esp_vfs_spiffs_conf = {
.base_path = "/spiffs", .base_path = "/spiffs",
.partition_label = NULL, .partition_label = NULL,
@ -142,16 +140,16 @@ void createObjects()
// with configuration above // with configuration above
//sabertoothDriver = new sabertooth2x60a(sabertoothConfig); //sabertoothDriver = new sabertooth2x60a(sabertoothConfig);
// create controlled motor instances (motorctl.hpp)
// with configurations from config.cpp
motorLeft = new controlledMotor(setLeftFunc, configMotorControlLeft, &nvsHandle);
motorRight = new controlledMotor(setRightFunc, configMotorControlRight, &nvsHandle);
// create speedsensor instances // create speedsensor instances
// with configurations from config.cpp // with configurations from config.cpp
speedLeft = new speedSensor(speedLeft_config); speedLeft = new speedSensor(speedLeft_config);
speedRight = new speedSensor(speedRight_config); speedRight = new speedSensor(speedRight_config);
// create controlled motor instances (motorctl.hpp)
// with configurations from config.cpp
motorLeft = new controlledMotor(setLeftFunc, configMotorControlLeft, &nvsHandle, speedLeft, &motorRight); //note: ptr to ptr of controlledMotor since it isnt defined yet
motorRight = new controlledMotor(setRightFunc, configMotorControlRight, &nvsHandle, speedRight, &motorLeft);
// create joystick instance (joystick.hpp) // create joystick instance (joystick.hpp)
joystick = new evaluatedJoystick(configJoystick, &nvsHandle); joystick = new evaluatedJoystick(configJoystick, &nvsHandle);
@ -195,35 +193,29 @@ extern "C" void app_main(void) {
gpio_set_direction(GPIO_NUM_17, GPIO_MODE_OUTPUT); gpio_set_direction(GPIO_NUM_17, GPIO_MODE_OUTPUT);
gpio_set_level(GPIO_NUM_17, 1); gpio_set_level(GPIO_NUM_17, 1);
//--- initialize nvs-flash and netif (needed for wifi) --- //--- initialize nvs-flash and netif ---
ESP_LOGW(TAG,"initializing wifi..."); ESP_LOGW(TAG,"initializing NVS...");
wifi_initNvs_initNetif(); wifi_initNvs(); //needed for wifi and persistent config variables
ESP_LOGW(TAG,"initializing NETIF...");
wifi_initNetif(); // needed for wifi
//--- initialize spiffs --- //--- initialize spiffs ---
init_spiffs(); ESP_LOGW(TAG, "initializing SPIFFS...");
init_spiffs(); // used by httpd server
//--- initialize and start wifi --- //--- initialize and start wifi ---
ESP_LOGW(TAG,"starting wifi..."); // Note: now started only when switching to HTTP mode in control.cpp
//wifi_init_client(); //connect to existing wifi // ESP_LOGW(TAG,"starting wifi...");
wifi_init_ap(); //start access point // wifi_start_client(); //connect to existing wifi (dropped)
ESP_LOGD(TAG,"done starting wifi"); // wifi_start_ap(); //start access point
//--- initialize encoder --- //--- initialize encoder ---
const QueueHandle_t encoderQueue = encoder_init(&encoder_config); const QueueHandle_t encoderQueue = encoder_init(&encoder_config);
//--- initialize nvs-flash --- (for persistant config values)
ESP_LOGW(TAG, "initializing nvs-flash...");
esp_err_t err = nvs_flash_init();
if (err == ESP_ERR_NVS_NO_FREE_PAGES || err == ESP_ERR_NVS_NEW_VERSION_FOUND)
{
ESP_LOGE(TAG, "NVS truncated -> deleting flash");
// Retry nvs_flash_init
ESP_ERROR_CHECK(nvs_flash_erase());
err = nvs_flash_init();
}
ESP_ERROR_CHECK(err);
//--- open nvs-flash --- //--- open nvs-flash ---
err = nvs_open("storage", NVS_READWRITE, &nvsHandle); // note: nvs already initialized in wifi_initNvs()
ESP_LOGW(TAG, "opening NVS-handle...");
esp_err_t err = nvs_open("storage", NVS_READWRITE, &nvsHandle); // this handle is passed to all tasks for accessing nvs
if (err != ESP_OK) if (err != ESP_OK)
ESP_LOGE(TAG, "Error (%s) opening NVS handle!\n", esp_err_to_name(err)); ESP_LOGE(TAG, "Error (%s) opening NVS handle!\n", esp_err_to_name(err));

122
board_single/main/menu.cpp
View File

@ -11,7 +11,6 @@ extern "C"{
#include "menu.hpp" #include "menu.hpp"
#include "encoder.hpp" #include "encoder.hpp"
#include "config.hpp"
#include "motorctl.hpp" #include "motorctl.hpp"
@ -105,7 +104,7 @@ void item_calibrateJoystick_action(display_task_parameters_t *objects, SSD1306_t
// save and next when button clicked, exit when long pressed // save and next when button clicked, exit when long pressed
if (xQueueReceive(objects->encoderQueue, &event, CALIBRATE_JOYSTICK_UPDATE_INTERVAL / portTICK_PERIOD_MS)) if (xQueueReceive(objects->encoderQueue, &event, CALIBRATE_JOYSTICK_UPDATE_INTERVAL / portTICK_PERIOD_MS))
{ {
objects->control->resetTimeout(); objects->control->resetTimeout(); // user input -> reset switch to IDLE timeout
switch (event.type) switch (event.type)
{ {
case RE_ET_BTN_CLICKED: case RE_ET_BTN_CLICKED:
@ -215,7 +214,7 @@ void item_debugJoystick_action(display_task_parameters_t * objects, SSD1306_t *
// exit when button pressed // exit when button pressed
if (xQueueReceive(objects->encoderQueue, &event, DEBUG_JOYSTICK_UPDATE_INTERVAL / portTICK_PERIOD_MS)) if (xQueueReceive(objects->encoderQueue, &event, DEBUG_JOYSTICK_UPDATE_INTERVAL / portTICK_PERIOD_MS))
{ {
objects->control->resetTimeout(); objects->control->resetTimeout(); // user input -> reset switch to IDLE timeout
switch (event.type) switch (event.type)
{ {
case RE_ET_BTN_CLICKED: case RE_ET_BTN_CLICKED:
@ -277,6 +276,34 @@ menuItem_t item_maxDuty = {
}; };
//##################################
//##### set max relative boost #####
//##################################
void maxRelativeBoost_action(display_task_parameters_t * objects, SSD1306_t * display, int value)
{
objects->control->setMaxRelativeBoostPer(value);
}
int maxRelativeBoost_currentValue(display_task_parameters_t * objects)
{
return (int)objects->control->getMaxRelativeBoostPer();
}
menuItem_t item_maxRelativeBoost = {
maxRelativeBoost_action, // function action
maxRelativeBoost_currentValue, // function get initial value or NULL(show in line 2)
NULL, // function get default value or NULL(dont set value, show msg)
0, // valueMin
150, // valueMax
1, // valueIncrement
"Set max Boost ", // title
"Set max Boost % ", // line1 (above value)
"for outer tire ", // line2 (above value)
"", // line4 * (below value)
"", // line5 *
" % of max duty ", // line6
"added on turning", // line7
};
//###################### //######################
//##### accelLimit ##### //##### accelLimit #####
//###################### //######################
@ -343,6 +370,85 @@ menuItem_t item_decelLimit = {
}; };
//###############################
//### select motorControlMode ###
//###############################
void item_motorControlMode_action(display_task_parameters_t *objects, SSD1306_t *display, int value)
{
switch (value)
{
case 1:
default:
objects->motorLeft->setControlMode(motorControlMode_t::DUTY);
objects->motorRight->setControlMode(motorControlMode_t::DUTY);
break;
case 2:
objects->motorLeft->setControlMode(motorControlMode_t::CURRENT);
objects->motorRight->setControlMode(motorControlMode_t::CURRENT);
break;
case 3:
objects->motorLeft->setControlMode(motorControlMode_t::SPEED);
objects->motorRight->setControlMode(motorControlMode_t::SPEED);
break;
}
}
int item_motorControlMode_value(display_task_parameters_t *objects)
{
return 1; // initial value shown / changed from //TODO get actual mode
}
menuItem_t item_motorControlMode = {
item_motorControlMode_action, // function action
item_motorControlMode_value, // function get initial value or NULL(show in line 2)
NULL, // function get default value or NULL(dont set value, show msg)
1, // valueMin
3, // valueMax
1, // valueIncrement
"Control mode ", // title
" sel. motor ", // line1 (above value)
" control mode ", // line2 (above value)
"1: DUTY (defaul)", // line4 * (below value)
"2: CURRENT", // line5 *
"3: SPEED", // line6
"", // line7
};
//###################################
//##### Traction Control System #####
//###################################
void tractionControlSystem_action(display_task_parameters_t * objects, SSD1306_t * display, int value)
{
if (value == 1){
objects->motorLeft->enableTractionControlSystem();
objects->motorRight->enableTractionControlSystem();
ESP_LOGW(TAG, "enabled Traction Control System");
} else {
objects->motorLeft->disableTractionControlSystem();
objects->motorRight->disableTractionControlSystem();
ESP_LOGW(TAG, "disabled Traction Control System");
}
}
int tractionControlSystem_currentValue(display_task_parameters_t * objects)
{
return (int)objects->motorLeft->getTractionControlSystemStatus();
}
menuItem_t item_tractionControlSystem = {
tractionControlSystem_action, // function action
tractionControlSystem_currentValue, // function get initial value or NULL(show in line 2)
NULL, // function get default value or NULL(dont set value, show msg)
0, // valueMin
1, // valueMax
1, // valueIncrement
"TCS / ASR ", // title
"Traction Control", // line1 (above value)
" System ", // line2 (above value)
"1: enable ", // line4 * (below value)
"0: disable ", // line5 *
"note: requires ", // line6
"speed ctl-mode ", // line7
};
//##################### //#####################
//####### RESET ####### //####### RESET #######
//##################### //#####################
@ -472,8 +578,8 @@ menuItem_t item_last = {
//#################################################### //####################################################
//### store all configured menu items in one array ### //### store all configured menu items in one array ###
//#################################################### //####################################################
const menuItem_t menuItems[] = {item_centerJoystick, item_calibrateJoystick, item_debugJoystick, item_maxDuty, item_accelLimit, item_decelLimit, item_statusScreen, item_reset, item_example, item_last}; const menuItem_t menuItems[] = {item_centerJoystick, item_calibrateJoystick, item_debugJoystick, item_statusScreen, item_maxDuty, item_maxRelativeBoost, item_accelLimit, item_decelLimit, item_motorControlMode, item_tractionControlSystem, item_reset, item_example, item_last};
const int itemCount = 8; const int itemCount = 11;
@ -621,7 +727,7 @@ void handleMenu(display_task_parameters_t * objects, SSD1306_t *display)
{ {
// reset menu- and control-timeout on any encoder event // reset menu- and control-timeout on any encoder event
lastActivity = esp_log_timestamp(); lastActivity = esp_log_timestamp();
objects->control->resetTimeout(); objects->control->resetTimeout(); // user input -> reset switch to IDLE timeout
switch (event.type) switch (event.type)
{ {
case RE_ET_CHANGED: case RE_ET_CHANGED:
@ -692,7 +798,7 @@ void handleMenu(display_task_parameters_t * objects, SSD1306_t *display)
// wait for encoder event // wait for encoder event
if (xQueueReceive(objects->encoderQueue, &event, QUEUE_TIMEOUT / portTICK_PERIOD_MS)) if (xQueueReceive(objects->encoderQueue, &event, QUEUE_TIMEOUT / portTICK_PERIOD_MS))
{ {
objects->control->resetTimeout(); objects->control->resetTimeout(); // user input -> reset switch to IDLE timeout
switch (event.type) switch (event.type)
{ {
case RE_ET_CHANGED: case RE_ET_CHANGED:
@ -732,7 +838,7 @@ void handleMenu(display_task_parameters_t * objects, SSD1306_t *display)
} }
// reset menu- and control-timeout on any encoder event // reset menu- and control-timeout on any encoder event
lastActivity = esp_log_timestamp(); lastActivity = esp_log_timestamp();
objects->control->resetTimeout(); objects->control->resetTimeout(); // user input -> reset switch to IDLE timeout
} }
break; break;
} }

2
board_single/sdkconfig
View File

@ -1252,7 +1252,7 @@ CONFIG_WPA_MBEDTLS_CRYPTO=y
# #
CONFIG_RE_MAX=1 CONFIG_RE_MAX=1
CONFIG_RE_INTERVAL_US=1000 CONFIG_RE_INTERVAL_US=1000
CONFIG_RE_BTN_DEAD_TIME_US=10000 CONFIG_RE_BTN_DEAD_TIME_US=40000
CONFIG_RE_BTN_PRESSED_LEVEL_0=y CONFIG_RE_BTN_PRESSED_LEVEL_0=y
# CONFIG_RE_BTN_PRESSED_LEVEL_1 is not set # CONFIG_RE_BTN_PRESSED_LEVEL_1 is not set
CONFIG_RE_BTN_LONG_PRESS_TIME_US=500000 CONFIG_RE_BTN_LONG_PRESS_TIME_US=500000

85
common/joystick.cpp
View File

@ -306,30 +306,38 @@ joystickPos_t joystick_evaluatePosition(float x, float y){
//function that generates commands for both motors from the joystick data //function that generates commands for both motors from the joystick data
motorCommands_t joystick_generateCommandsDriving(joystickData_t data, joystickGenerateCommands_config_t * config){ motorCommands_t joystick_generateCommandsDriving(joystickData_t data, joystickGenerateCommands_config_t * config){
//struct with current data of the joystick //--- interpret config parameters ---
//typedef struct joystickData_t { float dutyOffset = config->dutyOffset; // immediately starts with this duty
// joystickPos_t position; float dutyRange = config->maxDutyStraight - config->dutyOffset; //duty at max radius
// float x; // calculate configured boost duty (added when turning)
// float y; float dutyBoost = config->maxDutyStraight * config->maxRelativeBoostPercentOfMaxDuty/100;
// float radius; // limit to maximum possible duty
// float angle; float dutyAvailable = 100 - config->maxDutyStraight;
//} joystickData_t; if (dutyBoost > dutyAvailable) dutyBoost = dutyAvailable;
//--- variables ---
motorCommands_t commands;
float dutyOffset = 5; //immediately starts with this duty, TODO add this to config
float dutyRange = config->maxDuty - config->dutyOffset;
float ratio = fabs(data.angle) / 90; //90degree = x=0 || 0degree = y=0
//--- snap ratio to max at angle threshold ---
//(-> more joystick area where inner wheel is off when turning) //--- calculate paramaters with current data ---
/* motorCommands_t commands; // store new motor commands
//FIXME works, but armchair unsusable because of current bug with motor driver (inner motor freezes after turn)
float ratioClipThreshold = 0.3; // -- calculate ratio --
if (ratio < ratioClipThreshold) ratio = 0; // get current ratio from stick angle
else if (ratio > 1-ratioClipThreshold) ratio = 1; float ratioActual = fabs(data.angle) / 90; //x=0 -> 90deg -> ratio=1 || y=0 -> 0deg -> ratio=0
//TODO subtract this clip threshold from available joystick range at ratio usage ratioActual = 1 - ratioActual; // invert ratio
*/ // scale and clip ratio according to configured tolerance
// to have some joystick area at max ratio before reaching X-Axis-full-turn-mode
float ratio = ratioActual / (config->ratioSnapToOneThreshold); //0->0 threshold->1
// limit to 1 when above threshold (inside area max ratio)
if (ratio > 1) ratio = 1; // >threshold -> 1
// -- calculate outer tire boost --
#define BOOST_RATIO_MANIPULATION_SCALE 1.05 // >1 to apply boost slightly faster, this slightly compensates that available boost is most times less than reduction of inner duty, so for small turns the total speed feels more equal
float boostAmountOuter = data.radius*dutyBoost* ratio *BOOST_RATIO_MANIPULATION_SCALE;
// limit to max amount
if (boostAmountOuter > dutyBoost) boostAmountOuter = dutyBoost;
// -- calculate inner tire reduction --
float reductionAmountInner = (data.radius * dutyRange + dutyOffset) * ratio;
//--- experimental alternative control mode --- //--- experimental alternative control mode ---
if (config->altStickMapping == true){ if (config->altStickMapping == true){
@ -380,36 +388,43 @@ motorCommands_t joystick_generateCommandsDriving(joystickData_t data, joystickGe
case joystickPos_t::TOP_RIGHT: case joystickPos_t::TOP_RIGHT:
commands.left.state = motorstate_t::FWD; commands.left.state = motorstate_t::FWD;
commands.right.state = motorstate_t::FWD; commands.right.state = motorstate_t::FWD;
commands.left.duty = data.radius * dutyRange + dutyOffset; commands.left.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset;
commands.right.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset; commands.right.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset;
break; break;
case joystickPos_t::TOP_LEFT: case joystickPos_t::TOP_LEFT:
commands.left.state = motorstate_t::FWD; commands.left.state = motorstate_t::FWD;
commands.right.state = motorstate_t::FWD; commands.right.state = motorstate_t::FWD;
commands.left.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset; commands.left.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset;
commands.right.duty = data.radius * dutyRange + dutyOffset; commands.right.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset;
break; break;
case joystickPos_t::BOTTOM_LEFT: case joystickPos_t::BOTTOM_LEFT:
commands.left.state = motorstate_t::REV; commands.left.state = motorstate_t::REV;
commands.right.state = motorstate_t::REV; commands.right.state = motorstate_t::REV;
commands.left.duty = data.radius * dutyRange + dutyOffset; commands.left.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset;
commands.right.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset; commands.right.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset;
break; break;
case joystickPos_t::BOTTOM_RIGHT: case joystickPos_t::BOTTOM_RIGHT:
commands.left.state = motorstate_t::REV; commands.left.state = motorstate_t::REV;
commands.right.state = motorstate_t::REV; commands.right.state = motorstate_t::REV;
commands.left.duty = data.radius * dutyRange - (data.radius*dutyRange + dutyOffset)*(1-ratio) + dutyOffset; commands.left.duty = data.radius * dutyRange - reductionAmountInner + dutyOffset;
commands.right.duty = data.radius * dutyRange + dutyOffset; commands.right.duty = data.radius * dutyRange + boostAmountOuter + dutyOffset;
break; break;
} }
ESP_LOGI(TAG_CMD, "generated commands from data: state=%s, angle=%.3f, ratio=%.3f/%.3f, radius=%.2f, x=%.2f, y=%.2f", // log input data
joystickPosStr[(int)data.position], data.angle, ratio, (1-ratio), data.radius, data.x, data.y); ESP_LOGD(TAG_CMD, "in: pos='%s', angle=%.3f, ratioActual/Scaled=%.2f/%.2f, r=%.2f, x=%.2f, y=%.2f",
ESP_LOGI(TAG_CMD, "motor left: state=%s, duty=%.3f", motorstateStr[(int)commands.left.state], commands.left.duty); joystickPosStr[(int)data.position], data.angle, ratioActual, ratio, data.radius, data.x, data.y);
ESP_LOGI(TAG_CMD, "motor right: state=%s, duty=%.3f", motorstateStr[(int)commands.right.state], commands.right.duty); // log generation details
ESP_LOGI(TAG_CMD, "left=%.2f, right=%.2f -- BoostOuter=%.1f, ReductionInner=%.1f, maxDuty=%.0f, maxBoost=%.0f, dutyOffset=%.0f",
commands.left.duty, commands.right.duty,
boostAmountOuter, reductionAmountInner,
config->maxDutyStraight, dutyBoost, dutyOffset);
// log generated motor commands
ESP_LOGD(TAG_CMD, "motor left: state=%s, duty=%.3f", motorstateStr[(int)commands.left.state], commands.left.duty);
ESP_LOGD(TAG_CMD, "motor right: state=%s, duty=%.3f", motorstateStr[(int)commands.right.state], commands.right.duty);
return commands; return commands;
} }

14
common/joystick.hpp
View File

@ -69,15 +69,17 @@ typedef struct joystickData_t {
float angle; float angle;
} joystickData_t; } joystickData_t;
// struct with parameters provided to joystick_GenerateCommandsDriving() // struct with parameters provided to joystick_GenerateCommandsDriving()
typedef struct joystickGenerateCommands_config_t { typedef struct joystickGenerateCommands_config_t
float maxDuty; {
float dutyOffset; float maxDutyStraight; // max duty applied when driving with ratio=1 (when turning it might increase by Boost)
bool altStickMapping; float maxRelativeBoostPercentOfMaxDuty; // max duty percent added to outer tire when turning (max actual is 100-maxDutyStraight) - set 0 to disable
// note: to be able to reduce the overall driving speed boost has to be limited as well otherwise outer tire when turning would always be 100% no matter of maxDuty
float dutyOffset; // motors immediately start with this duty (duty movement starts)
float ratioSnapToOneThreshold; // have some area around X-Axis where inner tire is completely off - set 1 to disable
bool altStickMapping; // swap reverse direction
} joystickGenerateCommands_config_t; } joystickGenerateCommands_config_t;
//------------------------------------ //------------------------------------
//----- evaluatedJoystick class ----- //----- evaluatedJoystick class -----
//------------------------------------ //------------------------------------

373
common/motorctl.cpp
View File

@ -28,14 +28,20 @@ void task_motorctl( void * ptrControlledMotor ){
//======== constructor ======== //======== constructor ========
//============================= //=============================
//constructor, simultaniously initialize instance of motor driver 'motor' and current sensor 'cSensor' with provided config (see below lines after ':') //constructor, simultaniously initialize instance of motor driver 'motor' and current sensor 'cSensor' with provided config (see below lines after ':')
controlledMotor::controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl_config_t config_control, nvs_handle_t * nvsHandle_f): controlledMotor::controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl_config_t config_control, nvs_handle_t * nvsHandle_f, speedSensor * speedSensor_f, controlledMotor ** otherMotor_f):
//create current sensor
cSensor(config_control.currentSensor_adc, config_control.currentSensor_ratedCurrent, config_control.currentSnapToZeroThreshold, config_control.currentInverted) { cSensor(config_control.currentSensor_adc, config_control.currentSensor_ratedCurrent, config_control.currentSnapToZeroThreshold, config_control.currentInverted) {
//copy parameters for controlling the motor //copy parameters for controlling the motor
config = config_control; config = config_control;
log = config.loggingEnabled;
//pointer to update motot dury method //pointer to update motot dury method
motorSetCommand = setCommandFunc; motorSetCommand = setCommandFunc;
//pointer to nvs handle //pointer to nvs handle
nvsHandle = nvsHandle_f; nvsHandle = nvsHandle_f;
//pointer to other motor object
ppOtherMotor = otherMotor_f;
//pointer to speed sensor
sSensor = speedSensor_f;
//create queue, initialize config values //create queue, initialize config values
init(); init();
@ -105,61 +111,185 @@ void controlledMotor::handle(){
//TODO: History: skip fading when motor was running fast recently / alternatively add rot-speed sensor //TODO: History: skip fading when motor was running fast recently / alternatively add rot-speed sensor
//--- receive commands from queue --- //--- RECEIVE DATA FROM QUEUE ---
if( xQueueReceive( commandQueue, &commandReceive, timeoutWaitForCommand / portTICK_PERIOD_MS ) ) //wait time is always 0 except when at target duty already if( xQueueReceive( commandQueue, &commandReceive, timeoutWaitForCommand / portTICK_PERIOD_MS ) ) //wait time is always 0 except when at target duty already
{ {
ESP_LOGV(TAG, "[%s] Read command from queue: state=%s, duty=%.2f", config.name, motorstateStr[(int)commandReceive.state], commandReceive.duty); if(log) ESP_LOGV(TAG, "[%s] Read command from queue: state=%s, duty=%.2f", config.name, motorstateStr[(int)commandReceive.state], commandReceive.duty);
state = commandReceive.state; state = commandReceive.state;
dutyTarget = commandReceive.duty; dutyTarget = commandReceive.duty;
receiveTimeout = false; receiveTimeout = false;
timestamp_commandReceived = esp_log_timestamp(); timestamp_commandReceived = esp_log_timestamp();
//--- convert duty ---
//define target duty (-100 to 100) from provided duty and motorstate
//this value is more suitable for the fading algorithm
switch(commandReceive.state){
case motorstate_t::BRAKE:
//update state
state = motorstate_t::BRAKE;
//dutyTarget = 0;
dutyTarget = fabs(commandReceive.duty);
break;
case motorstate_t::IDLE:
dutyTarget = 0;
break;
case motorstate_t::FWD:
dutyTarget = fabs(commandReceive.duty);
break;
case motorstate_t::REV:
dutyTarget = - fabs(commandReceive.duty);
break;
}
} }
//--- timeout, no data ---
// turn motors off if no data received for a long time (e.g. no uart data or control task offline)
if (dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_IDLE_WHEN_NO_COMMAND && !receiveTimeout)
// ----- EXPERIMENTAL, DIFFERENT MODES -----
// define target duty differently depending on current contro-mode
//declare variables used inside switch
float ampereNow, ampereTarget, ampereDiff;
float speedDiff;
switch (mode)
{ {
ESP_LOGE(TAG, "[%s] TIMEOUT, motor active, but no target data received for more than %ds -> switch from duty=%.2f to IDLE", config.name, TIMEOUT_IDLE_WHEN_NO_COMMAND / 1000, dutyTarget); case motorControlMode_t::DUTY: // regulate to desired duty (as originally)
receiveTimeout = true; //--- convert duty ---
state = motorstate_t::IDLE; // define target duty (-100 to 100) from provided duty and motorstate
dutyTarget = 0; // this value is more suitable for t
// todo scale target input with DUTY-MAX here instead of in joysick cmd generationhe fading algorithm
switch (commandReceive.state)
{
case motorstate_t::BRAKE:
// update state
state = motorstate_t::BRAKE;
// dutyTarget = 0;
dutyTarget = fabs(commandReceive.duty);
break;
case motorstate_t::IDLE:
dutyTarget = 0;
break;
case motorstate_t::FWD:
dutyTarget = fabs(commandReceive.duty);
break;
case motorstate_t::REV:
dutyTarget = -fabs(commandReceive.duty);
break;
}
break;
#define CURRENT_CONTROL_ALLOWED_AMPERE_DIFF 1 //difference from target where no change is made yet
#define CURRENT_CONTROL_MIN_AMPERE 0.7 //current where motor is turned off
//TODO define different, fixed fading configuration in current mode, fade down can be significantly less (500/500ms fade up worked fine)
case motorControlMode_t::CURRENT: // regulate to desired current flow
ampereNow = cSensor.read();
ampereTarget = config.currentMax * commandReceive.duty / 100; // TODO ensure input data is 0-100 (no duty max), add currentMax to menu/config
if (commandReceive.state == motorstate_t::REV) ampereTarget = - ampereTarget; //target is negative when driving reverse
ampereDiff = ampereTarget - ampereNow;
if(log) ESP_LOGV("TESTING", "[%s] CURRENT-CONTROL: ampereNow=%.2f, ampereTarget=%.2f, diff=%.2f", config.name, ampereNow, ampereTarget, ampereDiff); // todo handle brake
//--- when IDLE to keep the current at target zero motor needs to be on for some duty (to compensate generator current)
if (commandReceive.duty == 0 && fabs(ampereNow) < CURRENT_CONTROL_MIN_AMPERE){ //stop motors completely when current is very low already
dutyTarget = 0;
}
else if (fabs(ampereDiff) > CURRENT_CONTROL_ALLOWED_AMPERE_DIFF || commandReceive.duty == 0) //#### BOOST BY 1 A
{
if (ampereDiff > 0 && commandReceive.state != motorstate_t::REV) // forward need to increase current
{
dutyTarget = 100; // todo add custom fading depending on diff? currently very dependent of fade times
}
else if (ampereDiff < 0 && commandReceive.state != motorstate_t::FWD) // backward need to increase current (more negative)
{
dutyTarget = -100;
}
else // fwd too much, rev too much -> decrease
{
dutyTarget = 0;
}
if(log) ESP_LOGV("TESTING", "[%s] CURRENT-CONTROL: set target to %.0f%%", config.name, dutyTarget);
}
else
{
dutyTarget = dutyNow; // target current reached
if(log) ESP_LOGD("TESTING", "[%s] CURRENT-CONTROL: target current %.3f reached", config.name, dutyTarget);
}
break;
#define SPEED_CONTROL_MAX_SPEED_KMH 10
#define SPEED_CONTROL_ALLOWED_KMH_DIFF 0.6
#define SPEED_CONTROL_MIN_SPEED 0.7 //" start from standstill" always accelerate to this speed, ignoring speedsensor data
case motorControlMode_t::SPEED: // regulate to desired speed
speedNow = sSensor->getKmph();
//caculate target speed from input
speedTarget = SPEED_CONTROL_MAX_SPEED_KMH * commandReceive.duty / 100; // TODO add maxSpeed to config
// target speed negative when driving reverse
if (commandReceive.state == motorstate_t::REV)
speedTarget = -speedTarget;
if (sSensor->getTimeLastUpdate() != timestamp_speedLastUpdate ){ //only modify duty when new speed data available
timestamp_speedLastUpdate = sSensor->getTimeLastUpdate(); //TODO get time only once
speedDiff = speedTarget - speedNow;
} else {
if(log) ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: no new speed data, not changing duty", config.name);
speedDiff = 0;
}
if(log) ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: target-speed=%.2f, current-speed=%.2f, diff=%.3f", config.name, speedTarget, speedNow, speedDiff);
//stop when target is 0
if (commandReceive.duty == 0) { //TODO add IDLE, BRAKE state
if(log) ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: OFF, target is 0... current-speed=%.2f, diff=%.3f", config.name, speedNow, speedDiff);
dutyTarget = 0;
}
else if (fabs(speedNow) < SPEED_CONTROL_MIN_SPEED){ //start from standstill or too slow (not enough speedsensor data)
if (log)
ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: starting from standstill -> increase duty... target-speed=%.2f, current-speed=%.2f, diff=%.3f", config.name, speedTarget, speedNow, speedDiff);
if (commandReceive.state == motorstate_t::FWD)
dutyTarget = 100;
else if (commandReceive.state == motorstate_t::REV)
dutyTarget = -100;
}
else if (fabs(speedDiff) > SPEED_CONTROL_ALLOWED_KMH_DIFF) //speed too fast/slow
{
if (speedDiff > 0 && commandReceive.state != motorstate_t::REV) // forward need to increase speed
{
// TODO retain max duty here
dutyTarget = 100; // todo add custom fading depending on diff? currently very dependent of fade times
if(log) ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: speed to low (fwd), diff=%.2f, increasing set target from %.1f%% to %.1f%%", config.name, speedDiff, dutyNow, dutyTarget);
}
else if (speedDiff < 0 && commandReceive.state != motorstate_t::FWD) // backward need to increase speed (more negative)
{
dutyTarget = -100;
if(log) ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: speed to low (rev), diff=%.2f, increasing set target from %.1f%% to %.1f%%", config.name, speedDiff, dutyNow, dutyTarget);
}
else // fwd too much, rev too much -> decrease
{
dutyTarget = 0;
if(log) ESP_LOGV("TESTING", "[%s] SPEED-CONTROL: speed to high, diff=%.2f, decreasing set target from %.1f%% to %.1f%%", config.name, speedDiff, dutyNow, dutyTarget);
}
}
else
{
dutyTarget = dutyNow; // target speed reached
if(log) ESP_LOGD("TESTING", "[%s] SPEED-CONTROL: target speed %.3f reached", config.name, speedTarget);
}
break;
} }
//--- calculate difference ---
dutyDelta = dutyTarget - dutyNow;
//--- TIMEOUT NO DATA ---
// turn motors off if no data received for a long time (e.g. no uart data or control task offline)
if ( dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_IDLE_WHEN_NO_COMMAND && !receiveTimeout)
{
if(log) ESP_LOGE(TAG, "[%s] TIMEOUT, motor active, but no target data received for more than %ds -> switch from duty=%.2f to IDLE", config.name, TIMEOUT_IDLE_WHEN_NO_COMMAND / 1000, dutyTarget);
receiveTimeout = true;
// set target and last command to IDLE
state = motorstate_t::IDLE;
commandReceive.state = motorstate_t::IDLE;
dutyTarget = 0; // todo put this in else section of queue (no data received) and add control mode "timeout"?
commandReceive.duty = 0;
}
//--- CALCULATE DUTY-DIFF ---
dutyDelta = dutyTarget - dutyNow;
//positive: need to increase by that value //positive: need to increase by that value
//negative: need to decrease //negative: need to decrease
//--- already at target ---
//--- DETECT ALREADY AT TARGET ---
// when already at exact target duty there is no need to run very fast to handle fading // when already at exact target duty there is no need to run very fast to handle fading
//-> slow down loop by waiting significantly longer for new commands to arrive //-> slow down loop by waiting significantly longer for new commands to arrive
if ((dutyDelta == 0 && !config.currentLimitEnabled) || (dutyTarget == 0 && dutyNow == 0)) //when current limit enabled only slow down when duty is 0 if (mode != motorControlMode_t::CURRENT //dont slow down when in CURRENT mode at all
&& ((dutyDelta == 0 && !config.currentLimitEnabled && !config.tractionControlSystemEnabled && mode != motorControlMode_t::SPEED) //when neither of current-limit, tractioncontrol or speed-mode is enabled slow down when target reached
|| (dutyTarget == 0 && dutyNow == 0))) //otherwise only slow down when when actually off
{ {
//increase timeout once when duty is the same (once) //increase queue timeout when duty is the same (once)
if (timeoutWaitForCommand == 0) if (timeoutWaitForCommand == 0)
{ // TODO verify if state matches too? { // TODO verify if state matches too?
ESP_LOGI(TAG, "[%s] already at target duty %.2f, slowing down...", config.name, dutyTarget); if(log) ESP_LOGI(TAG, "[%s] already at target duty %.2f, slowing down...", config.name, dutyTarget);
timeoutWaitForCommand = TIMEOUT_QUEUE_WHEN_AT_TARGET; // wait in queue very long, for new command to arrive timeoutWaitForCommand = TIMEOUT_QUEUE_WHEN_AT_TARGET; // wait in queue very long, for new command to arrive
} }
vTaskDelay(20 / portTICK_PERIOD_MS); // add small additional delay overall, in case the same commands get spammed vTaskDelay(20 / portTICK_PERIOD_MS); // add small additional delay overall, in case the same commands get spammed
@ -168,44 +298,43 @@ void controlledMotor::handle(){
else if (timeoutWaitForCommand != 0) else if (timeoutWaitForCommand != 0)
{ {
timeoutWaitForCommand = 0; // dont wait additional time for new commands, handle fading fast timeoutWaitForCommand = 0; // dont wait additional time for new commands, handle fading fast
ESP_LOGI(TAG, "[%s] duty changed to %.2f, resuming at full speed", config.name, dutyTarget); if(log) ESP_LOGI(TAG, "[%s] duty changed to %.2f, resuming at full speed", config.name, dutyTarget);
// adjust lastRun timestamp to not mess up fading, due to much time passed but with no actual duty change // adjust lastRun timestamp to not mess up fading, due to much time passed but with no actual duty change
timestampLastRunUs = esp_timer_get_time() - 20*1000; //subtract approx 1 cycle delay timestampLastRunUs = esp_timer_get_time() - 20*1000; //subtract approx 1 cycle delay
} }
//TODO skip rest of the handle function below using return? Some regular driver updates sound useful though //TODO skip rest of the handle function below using return? Some regular driver updates sound useful though
//--- calculate increment ---
//calculate increment for fading UP with passed time since last run and configured fade time
int64_t usPassed = esp_timer_get_time() - timestampLastRunUs;
if (msFadeAccel > 0){
dutyIncrementAccel = ( usPassed / ((float)msFadeAccel * 1000) ) * 100; //TODO define maximum increment - first run after startup (or long) pause can cause a very large increment
} else {
dutyIncrementAccel = 100;
}
//calculate increment for fading DOWN with passed time since last run and configured fade time
if (msFadeDecel > 0){
dutyIncrementDecel = ( usPassed / ((float)msFadeDecel * 1000) ) * 100;
} else {
dutyIncrementDecel = 100;
}
//--- BRAKE --- //--- BRAKE ---
//brake immediately, update state, duty and exit this cycle of handle function //brake immediately, update state, duty and exit this cycle of handle function
if (state == motorstate_t::BRAKE){ if (state == motorstate_t::BRAKE){
ESP_LOGD(TAG, "braking - skip fading"); if(log) ESP_LOGD(TAG, "braking - skip fading");
motorSetCommand({motorstate_t::BRAKE, dutyTarget}); motorSetCommand({motorstate_t::BRAKE, dutyTarget});
ESP_LOGD(TAG, "[%s] Set Motordriver: state=%s, duty=%.2f - Measurements: current=%.2f, speed=N/A", config.name, motorstateStr[(int)state], dutyNow, currentNow); if(log) ESP_LOGD(TAG, "[%s] Set Motordriver: state=%s, duty=%.2f - Measurements: current=%.2f, speed=N/A", config.name, motorstateStr[(int)state], dutyNow, currentNow);
//dutyNow = 0; //dutyNow = 0;
return; //no need to run the fade algorithm return; //no need to run the fade algorithm
} }
//----- FADING ----- //----- FADING -----
//calculate passed time since last run
int64_t usPassed = esp_timer_get_time() - timestampLastRunUs;
//--- calculate increment ---
//calculate increment for fading UP with passed time since last run and configured fade time
if (tcs_isExceeded) // disable acceleration when slippage is currently detected
dutyIncrementAccel = 0;
else if (msFadeAccel > 0)
dutyIncrementAccel = (usPassed / ((float)msFadeAccel * 1000)) * 100; // TODO define maximum increment - first run after startup (or long) pause can cause a very large increment
else //no accel limit (immediately set to 100)
dutyIncrementAccel = 100;
//calculate increment for fading DOWN with passed time since last run and configured fade time
if (msFadeDecel > 0)
dutyIncrementDecel = ( usPassed / ((float)msFadeDecel * 1000) ) * 100;
else //no decel limit (immediately reduce to 0)
dutyIncrementDecel = 100;
//fade duty to target (up and down) //fade duty to target (up and down)
//TODO: this needs optimization (can be more clear and/or simpler) //TODO: this needs optimization (can be more clear and/or simpler)
if (dutyDelta > 0) { //difference positive -> increasing duty (-100 -> 100) if (dutyDelta > 0) { //difference positive -> increasing duty (-100 -> 100)
@ -226,7 +355,7 @@ void controlledMotor::handle(){
} }
//----- CURRENT LIMIT ----- //----- CURRENT LIMIT -----
currentNow = cSensor.read(); currentNow = cSensor.read();
if ((config.currentLimitEnabled) && (dutyDelta != 0)){ if ((config.currentLimitEnabled) && (dutyDelta != 0)){
if (fabs(currentNow) > config.currentMax){ if (fabs(currentNow) > config.currentMax){
@ -240,11 +369,89 @@ void controlledMotor::handle(){
} else if (dutyNow > currentLimitDecrement) { } else if (dutyNow > currentLimitDecrement) {
dutyNow -= currentLimitDecrement; dutyNow -= currentLimitDecrement;
} }
ESP_LOGW(TAG, "[%s] current limit exceeded! now=%.3fA max=%.1fA => decreased duty from %.3f to %.3f", config.name, currentNow, config.currentMax, dutyOld, dutyNow); if(log) ESP_LOGW(TAG, "[%s] current limit exceeded! now=%.3fA max=%.1fA => decreased duty from %.3f to %.3f", config.name, currentNow, config.currentMax, dutyOld, dutyNow);
} }
} }
//----- TRACTION CONTROL -----
//reduce duty when turning faster than expected
//TODO only run this when speed sensors actually updated
//handle tcs when enabled and new speed sensor data is available TODO: currently assumes here that speed sensor data of other motor updated as well
#define TCS_MAX_ALLOWED_RATIO_DIFF 0.1 //when motor speed ratio differs more than that, one motor is slowed down
#define TCS_NO_SPEED_DATA_TIMEOUT_US 200*1000
#define TCS_MIN_SPEED_KMH 1 //must be at least that fast for TCS to be enabled
//TODO rework this: clearer structure (less nested if statements)
if (config.tractionControlSystemEnabled && mode == motorControlMode_t::SPEED && sSensor->getTimeLastUpdate() != tcs_timestampLastSpeedUpdate && (esp_timer_get_time() - tcs_timestampLastRun < TCS_NO_SPEED_DATA_TIMEOUT_US)){
//update last speed update received
tcs_timestampLastSpeedUpdate = sSensor->getTimeLastUpdate(); //TODO: re-use tcs_timestampLastRun in if statement, instead of having additional variable SpeedUpdate
//calculate time passed since last run
uint32_t tcs_usPassed = esp_timer_get_time() - tcs_timestampLastRun; // passed time since last time handled
tcs_timestampLastRun = esp_timer_get_time();
//get motor stats
float speedNowThis = sSensor->getKmph();
float speedNowOther = (*ppOtherMotor)->getCurrentSpeed();
float speedTargetThis = speedTarget;
float speedTargetOther = (*ppOtherMotor)->getTargetSpeed();
float dutyTargetOther = (*ppOtherMotor)->getTargetDuty();
float dutyTargetThis = dutyTarget;
float dutyNowOther = (*ppOtherMotor)->getDuty();
float dutyNowThis = dutyNow;
//calculate expected ratio
float ratioSpeedTarget = speedTargetThis / speedTargetOther;
//calculate current ratio of actual measured rotational speed
float ratioSpeedNow = speedNowThis / speedNowOther;
//calculate current duty ration (logging only)
float ratioDutyNow = dutyNowThis / dutyNowOther;
//calculate unexpected difference
float ratioDiff = ratioSpeedNow - ratioSpeedTarget;
if(log) ESP_LOGD("TESTING", "[%s] TCS: speedThis=%.3f, speedOther=%.3f, ratioSpeedTarget=%.3f, ratioSpeedNow=%.3f, ratioDutyNow=%.3f, diff=%.3f", config.name, speedNowThis, speedNowOther, ratioSpeedTarget, ratioSpeedNow, ratioDutyNow, ratioDiff);
//-- handle rotating faster than expected --
//TODO also increase duty when other motor is slipping? (diff negative)
if (speedNowThis < TCS_MIN_SPEED_KMH) { //disable / turn off TCS when currently too slow (danger of deadlock)
tcs_isExceeded = false;
tcs_usExceeded = 0;
}
else if (ratioDiff > TCS_MAX_ALLOWED_RATIO_DIFF ) // motor turns too fast compared to expected target ratio
{
if (!tcs_isExceeded) // just started being too fast
{
tcs_timestampBeginExceeded = esp_timer_get_time();
tcs_isExceeded = true; //also blocks further acceleration (fade)
if(log) ESP_LOGW("TESTING", "[%s] TCS: now exceeding max allowed ratio diff! diff=%.2f max=%.2f", config.name, ratioDiff, TCS_MAX_ALLOWED_RATIO_DIFF);
}
else
{ // too fast for more than 2 cycles already
tcs_usExceeded = esp_timer_get_time() - tcs_timestampBeginExceeded; //time too fast already
if(log) ESP_LOGI("TESTING", "[%s] TCS: faster than expected since %dms, current ratioDiff=%.2f -> slowing down", config.name, tcs_usExceeded/1000, ratioDiff);
// calculate amount duty gets decreased
float dutyDecrement = (tcs_usPassed / ((float)msFadeDecel * 1000)) * 100; //TODO optimize dynamic increment: P:scale with ratio-difference, I: scale with duration exceeded
// decrease duty
if(log) ESP_LOGI("TESTING", "[%s] TCS: msPassed=%.3f, reducing duty by %.3f%%", config.name, (float)tcs_usPassed/1000, dutyDecrement);
fade(&dutyNow, 0, -dutyDecrement); //reduce duty but not less than 0
}
}
else
{ // not exceeded
tcs_isExceeded = false;
tcs_usExceeded = 0;
}
}
else // TCS mode not active or timed out
{ // not exceeded
tcs_isExceeded = false;
tcs_usExceeded = 0;
}
//--- define new motorstate --- (-100 to 100 => direction) //--- define new motorstate --- (-100 to 100 => direction)
state=getStateFromDuty(dutyNow); state=getStateFromDuty(dutyNow);
@ -254,25 +461,27 @@ void controlledMotor::handle(){
//FWD -> IDLE -> FWD continue without waiting //FWD -> IDLE -> FWD continue without waiting
//FWD -> IDLE -> REV wait for dead-time in IDLE //FWD -> IDLE -> REV wait for dead-time in IDLE
//TODO check when changed only? //TODO check when changed only?
if ( //not enough time between last direction state if (config.deadTimeMs > 0) { //deadTime is enabled
( state == motorstate_t::FWD && (esp_log_timestamp() - timestampsModeLastActive[(int)motorstate_t::REV] < config.deadTimeMs)) if ( //not enough time between last direction state
|| (state == motorstate_t::REV && (esp_log_timestamp() - timestampsModeLastActive[(int)motorstate_t::FWD] < config.deadTimeMs)) ( state == motorstate_t::FWD && (esp_log_timestamp() - timestampsModeLastActive[(int)motorstate_t::REV] < config.deadTimeMs))
){ || (state == motorstate_t::REV && (esp_log_timestamp() - timestampsModeLastActive[(int)motorstate_t::FWD] < config.deadTimeMs))
ESP_LOGD(TAG, "waiting dead-time... dir change %s -> %s", motorstateStr[(int)statePrev], motorstateStr[(int)state]); ){
if (!deadTimeWaiting){ //log start if(log) ESP_LOGD(TAG, "waiting dead-time... dir change %s -> %s", motorstateStr[(int)statePrev], motorstateStr[(int)state]);
deadTimeWaiting = true; if (!deadTimeWaiting){ //log start
ESP_LOGI(TAG, "starting dead-time... %s -> %s", motorstateStr[(int)statePrev], motorstateStr[(int)state]); deadTimeWaiting = true;
} if(log) ESP_LOGI(TAG, "starting dead-time... %s -> %s", motorstateStr[(int)statePrev], motorstateStr[(int)state]);
//force IDLE state during wait }
state = motorstate_t::IDLE; //force IDLE state during wait
dutyNow = 0; state = motorstate_t::IDLE;
} else { dutyNow = 0;
if (deadTimeWaiting){ //log end } else {
deadTimeWaiting = false; if (deadTimeWaiting){ //log end
ESP_LOGI(TAG, "dead-time ended - continue with %s", motorstateStr[(int)state]); deadTimeWaiting = false;
} if(log) ESP_LOGI(TAG, "dead-time ended - continue with %s", motorstateStr[(int)state]);
ESP_LOGV(TAG, "deadtime: no change below deadtime detected... dir=%s, duty=%.1f", motorstateStr[(int)state], dutyNow); }
} if(log) ESP_LOGV(TAG, "deadtime: no change below deadtime detected... dir=%s, duty=%.1f", motorstateStr[(int)state], dutyNow);
}
}
//--- save current actual motorstate and timestamp --- //--- save current actual motorstate and timestamp ---
@ -284,7 +493,7 @@ void controlledMotor::handle(){
//--- apply new target to motor --- //--- apply new target to motor ---
motorSetCommand({state, (float)fabs(dutyNow)}); motorSetCommand({state, (float)fabs(dutyNow)});
ESP_LOGI(TAG, "[%s] Set Motordriver: state=%s, duty=%.2f - Measurements: current=%.2f, speed=N/A", config.name, motorstateStr[(int)state], dutyNow, currentNow); if(log) ESP_LOGI(TAG, "[%s] Set Motordriver: state=%s, duty=%.2f - Measurements: current=%.2f, speed=N/A", config.name, motorstateStr[(int)state], dutyNow, currentNow);
//note: BRAKE state is handled earlier //note: BRAKE state is handled earlier
@ -300,11 +509,11 @@ void controlledMotor::handle(){
//function to set the target mode and duty of a motor //function to set the target mode and duty of a motor
//puts the provided command in a queue for the handle function running in another task //puts the provided command in a queue for the handle function running in another task
void controlledMotor::setTarget(motorCommand_t commandSend){ void controlledMotor::setTarget(motorCommand_t commandSend){
ESP_LOGI(TAG, "[%s] setTarget: Inserting command to queue: state='%s'(%d), duty=%.2f", config.name, motorstateStr[(int)commandSend.state], (int)commandSend.state, commandSend.duty); if(log) ESP_LOGI(TAG, "[%s] setTarget: Inserting command to queue: state='%s'(%d), duty=%.2f", config.name, motorstateStr[(int)commandSend.state], (int)commandSend.state, commandSend.duty);
//send command to queue (overwrite if an old command is still in the queue and not processed) //send command to queue (overwrite if an old command is still in the queue and not processed)
xQueueOverwrite( commandQueue, ( void * )&commandSend); xQueueOverwrite( commandQueue, ( void * )&commandSend);
//xQueueSend( commandQueue, ( void * )&commandSend, ( TickType_t ) 0 ); //xQueueSend( commandQueue, ( void * )&commandSend, ( TickType_t ) 0 );
ESP_LOGD(TAG, "finished inserting new command"); if(log) ESP_LOGD(TAG, "finished inserting new command");
} }
// accept target state and duty as separate agrguments: // accept target state and duty as separate agrguments:

34
common/motorctl.hpp
View File

@ -13,6 +13,7 @@ extern "C"
#include "motordrivers.hpp" #include "motordrivers.hpp"
#include "currentsensor.hpp" #include "currentsensor.hpp"
#include "speedsensor.hpp"
//======================================= //=======================================
@ -23,6 +24,7 @@ extern "C"
typedef void (*motorSetCommandFunc_t)(motorCommand_t cmd); typedef void (*motorSetCommandFunc_t)(motorCommand_t cmd);
enum class motorControlMode_t {DUTY, CURRENT, SPEED};
//=================================== //===================================
//====== controlledMotor class ====== //====== controlledMotor class ======
@ -30,11 +32,20 @@ typedef void (*motorSetCommandFunc_t)(motorCommand_t cmd);
class controlledMotor { class controlledMotor {
public: public:
//--- functions --- //--- functions ---
controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl_config_t config_control, nvs_handle_t * nvsHandle); //constructor with structs for configuring motordriver and parameters for control TODO: add configuration for currentsensor //TODO move speedsensor object creation in this class to (pass through / wrap methods)
controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl_config_t config_control, nvs_handle_t * nvsHandle, speedSensor * speedSensor, controlledMotor ** otherMotor); //constructor with structs for configuring motordriver and parameters for control TODO: add configuration for currentsensor
void handle(); //controls motor duty with fade and current limiting feature (has to be run frequently by another task) void handle(); //controls motor duty with fade and current limiting feature (has to be run frequently by another task)
void setTarget(motorstate_t state_f, float duty_f = 0); //adds target command to queue for handle function void setTarget(motorstate_t state_f, float duty_f = 0); //adds target command to queue for handle function
void setTarget(motorCommand_t command); void setTarget(motorCommand_t command);
motorCommand_t getStatus(); //get current status of the motor (returns struct with state and duty) motorCommand_t getStatus(); //get current status of the motor (returns struct with state and duty)
float getDuty() {return dutyNow;};
float getTargetDuty() {return dutyTarget;};
float getTargetSpeed() {return speedTarget;};
float getCurrentSpeed() {return sSensor->getKmph();};
void enableTractionControlSystem() {config.tractionControlSystemEnabled = true;};
void disableTractionControlSystem() {config.tractionControlSystemEnabled = false; tcs_isExceeded = false;};
bool getTractionControlSystemStatus() {return config.tractionControlSystemEnabled;};
void setControlMode(motorControlMode_t newMode) {mode = newMode;};
uint32_t getFade(fadeType_t fadeType); //get currently set acceleration or deceleration fading time uint32_t getFade(fadeType_t fadeType); //get currently set acceleration or deceleration fading time
uint32_t getFadeDefault(fadeType_t fadeType); //get acceleration or deceleration fading time from config uint32_t getFadeDefault(fadeType_t fadeType); //get acceleration or deceleration fading time from config
@ -60,6 +71,11 @@ class controlledMotor {
QueueHandle_t commandQueue = NULL; QueueHandle_t commandQueue = NULL;
//current sensor //current sensor
currentSensor cSensor; currentSensor cSensor;
//speed sensor
speedSensor * sSensor;
//other motor (needed for traction control)
controlledMotor ** ppOtherMotor; //ptr to ptr of controlledMotor (because not created at initialization yet)
//function pointer that sets motor duty (driver) //function pointer that sets motor duty (driver)
motorSetCommandFunc_t motorSetCommand; motorSetCommandFunc_t motorSetCommand;
@ -68,15 +84,24 @@ class controlledMotor {
//TODO add name for logging? //TODO add name for logging?
//struct for storing control specific parameters //struct for storing control specific parameters
motorctl_config_t config; motorctl_config_t config;
bool log = false;
motorstate_t state = motorstate_t::IDLE; motorstate_t state = motorstate_t::IDLE;
motorControlMode_t mode = motorControlMode_t::DUTY; //default control mode
//handle for using the nvs flash (persistent config variables) //handle for using the nvs flash (persistent config variables)
nvs_handle_t * nvsHandle; nvs_handle_t * nvsHandle;
float currentMax; float currentMax;
float currentNow; float currentNow;
//speed mode
float speedTarget = 0;
float speedNow = 0;
uint32_t timestamp_speedLastUpdate = 0;
float dutyTarget = 0; float dutyTarget = 0;
float dutyNow = 0; float dutyNow = 0;
float dutyIncrementAccel; float dutyIncrementAccel;
float dutyIncrementDecel; float dutyIncrementDecel;
float dutyDelta; float dutyDelta;
@ -97,6 +122,13 @@ class controlledMotor {
uint32_t timestamp_commandReceived = 0; uint32_t timestamp_commandReceived = 0;
bool receiveTimeout = false; bool receiveTimeout = false;
//traction control system
uint32_t tcs_timestampLastSpeedUpdate = 0; //track speedsensor update
int64_t tcs_timestampBeginExceeded = 0; //track start of event
uint32_t tcs_usExceeded = 0; //sum up time
bool tcs_isExceeded = false; //is currently too fast
int64_t tcs_timestampLastRun = 0;
}; };
//==================================== //====================================

1
common/speedsensor.cpp
View File

@ -93,6 +93,7 @@ void IRAM_ATTR onEncoderRising(void *arg)
// calculate rotational speed // calculate rotational speed
uint64_t pulseSum = pulse1 + pulse2 + pulse3; uint64_t pulseSum = pulse1 + pulse2 + pulse3;
sensor->currentRpm = direction * (sensor->config.degreePerGroup / 360.0 * 60.0 / ((double)pulseSum / 1000000.0)); sensor->currentRpm = direction * (sensor->config.degreePerGroup / 360.0 * 60.0 / ((double)pulseSum / 1000000.0));
sensor->timeLastUpdate = currentTime;
} }
} }

2
common/speedsensor.hpp
View File

@ -33,6 +33,7 @@ public:
float getKmph(); //kilometers per hour float getKmph(); //kilometers per hour
float getMps(); //meters per second float getMps(); //meters per second
float getRpm(); //rotations per minute float getRpm(); //rotations per minute
uint32_t getTimeLastUpdate() {return timeLastUpdate;};
//variables for handling the encoder (public because ISR needs access) //variables for handling the encoder (public because ISR needs access)
speedSensor_config_t config; speedSensor_config_t config;
@ -45,6 +46,7 @@ public:
int debugCount = 0; int debugCount = 0;
uint32_t debug_countIgnoredSequencesTooShort = 0; uint32_t debug_countIgnoredSequencesTooShort = 0;
double currentRpm = 0; double currentRpm = 0;
uint32_t timeLastUpdate = 0;
private: private:
static bool isrIsInitialized; // default false due to static static bool isrIsInitialized; // default false due to static

2
common/types.hpp
View File

@ -42,9 +42,11 @@ typedef struct motorCommands_t {
//struct with all config parameters for a motor regarding ramp and current limit //struct with all config parameters for a motor regarding ramp and current limit
typedef struct motorctl_config_t { typedef struct motorctl_config_t {
char * name; //name for unique nvs storage-key prefix and logging char * name; //name for unique nvs storage-key prefix and logging
bool loggingEnabled; //enable/disable ALL log output (mostly better to debug only one instance)
uint32_t msFadeAccel; //acceleration of the motor (ms it takes from 0% to 100%) uint32_t msFadeAccel; //acceleration of the motor (ms it takes from 0% to 100%)
uint32_t msFadeDecel; //deceleration of the motor (ms it takes from 100% to 0%) uint32_t msFadeDecel; //deceleration of the motor (ms it takes from 100% to 0%)
bool currentLimitEnabled; bool currentLimitEnabled;
bool tractionControlSystemEnabled;
adc1_channel_t currentSensor_adc; adc1_channel_t currentSensor_adc;
float currentSensor_ratedCurrent; float currentSensor_ratedCurrent;
float currentMax; float currentMax;

84
common/wifi.c
View File

@ -21,26 +21,45 @@ static const char *TAG = "wifi";
static esp_event_handler_instance_t instance_any_id; static esp_event_handler_instance_t instance_any_id;
//============================================ //##########################################
//============ init nvs and netif ============ //############ common functions ############
//============================================ //##########################################
//initialize nvs-flash and netif (needed for both AP and CLIENT)
//============================
//========= init nvs =========
//============================
//initialize nvs-flash (needed for both AP and CLIENT)
//has to be run once at startup //has to be run once at startup
void wifi_initNvs_initNetif(){ void wifi_initNvs(){
//Initialize NVS (needed for wifi) //Initialize NVS (needed for wifi)
esp_err_t ret = nvs_flash_init(); esp_err_t err = nvs_flash_init();
if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) { if (err == ESP_ERR_NVS_NO_FREE_PAGES || err == ESP_ERR_NVS_NEW_VERSION_FOUND)
ESP_ERROR_CHECK(nvs_flash_erase()); {
ret = nvs_flash_init(); ESP_LOGE(TAG, "NVS truncated -> deleting flash");
} // Retry nvs_flash_init
ESP_ERROR_CHECK(nvs_flash_erase());
err = nvs_flash_init();
}
ESP_ERROR_CHECK(err);
}
//==============================
//========= init netif =========
//==============================
//initialize netif (needed for both AP and CLIENT)
//has to be run once at startup
void wifi_initNetif(){
ESP_ERROR_CHECK(esp_netif_init()); ESP_ERROR_CHECK(esp_netif_init());
ESP_ERROR_CHECK(esp_event_loop_create_default()); ESP_ERROR_CHECK(esp_event_loop_create_default());
} }
//===========================================
//============ init access point ============
//=========================================== //############################################
//############### access point ###############
//############################################
//-------------------------------------------- //--------------------------------------------
//------ configuration / declarations -------- //------ configuration / declarations --------
@ -66,10 +85,12 @@ static void wifi_event_handler_ap(void* arg, esp_event_base_t event_base,
} }
} }
//-----------------------
//------ init ap --------
//----------------------- //========================
void wifi_init_ap(void) //====== start AP ========
//========================
void wifi_start_ap(void)
{ {
ap = esp_netif_create_default_wifi_ap(); ap = esp_netif_create_default_wifi_ap();
@ -107,9 +128,9 @@ void wifi_init_ap(void)
//============================= //=============================
//========= deinit AP ========= //========== stop AP ==========
//============================= //=============================
void wifi_deinit_ap(void) void wifi_stop_ap(void)
{ {
ESP_ERROR_CHECK(esp_event_handler_instance_unregister(WIFI_EVENT, ESP_EVENT_ANY_ID, instance_any_id)); ESP_ERROR_CHECK(esp_event_handler_instance_unregister(WIFI_EVENT, ESP_EVENT_ANY_ID, instance_any_id));
ESP_ERROR_CHECK(esp_event_handler_instance_unregister(WIFI_EVENT, ESP_EVENT_ANY_ID, instance_any_id)); ESP_ERROR_CHECK(esp_event_handler_instance_unregister(WIFI_EVENT, ESP_EVENT_ANY_ID, instance_any_id));
@ -123,9 +144,9 @@ void wifi_deinit_ap(void)
//=========================================== //##########################################
//=============== init client =============== //################# client #################
//=========================================== //##########################################
//-------------------------------------------- //--------------------------------------------
//------ configuration / declarations -------- //------ configuration / declarations --------
@ -168,10 +189,13 @@ static void event_handler(void* arg, esp_event_base_t event_base,
xEventGroupSetBits(s_wifi_event_group, WIFI_CONNECTED_BIT); xEventGroupSetBits(s_wifi_event_group, WIFI_CONNECTED_BIT);
} }
} }
//---------------------------
//------ init client --------
//---------------------------
void wifi_init_client(void) //===========================
//====== init client ========
//===========================
void wifi_start_client(void)
{ {
s_wifi_event_group = xEventGroupCreate(); s_wifi_event_group = xEventGroupCreate();
sta = esp_netif_create_default_wifi_sta(); sta = esp_netif_create_default_wifi_sta();
@ -249,10 +273,10 @@ void wifi_init_client(void)
//================================= //===============================
//========= deinit client ========= //========= stop client =========
//================================= //===============================
void wifi_deinit_client(void) void wifi_stop_client(void)
{ {
/* The event will not be processed after unregister */ /* The event will not be processed after unregister */
ESP_ERROR_CHECK(esp_event_handler_instance_unregister(IP_EVENT, IP_EVENT_STA_GOT_IP, instance_got_ip)); ESP_ERROR_CHECK(esp_event_handler_instance_unregister(IP_EVENT, IP_EVENT_STA_GOT_IP, instance_got_ip));

20
common/wifi.h
View File

@ -3,20 +3,20 @@
//TODO: currently wifi names and passwords are configured in wifi.c -> move this to config? //TODO: currently wifi names and passwords are configured in wifi.c -> move this to config?
//initialize nvs-flash and netif (needed for both AP and CLIENT) //initialize nvs-flash and netif (needed for both AP and CLIENT)
//has to be run once at startup //both functions have to be run once at startup
//Note: this cant be put in wifi_init functions because this may not be in deinit functions void wifi_initNvs();
void wifi_initNvs_initNetif(); void wifi_initNetif();
//function to start an access point //function to start an access point (config in wifi.c)
void wifi_init_ap(void); void wifi_start_ap(void);
//function to disable/deinit access point //function to disable/stop access point
void wifi_deinit_ap(void); void wifi_stop_ap(void);
//function to connect to existing wifi network //function to connect to existing wifi network (config in wifi.c)
void wifi_init_client(void); void wifi_start_client(void);
//function to disable/deinit client //function to disable/deinit client
void wifi_deinit_client(void); void wifi_stop_client(void);