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

Merge branch 'fade-down' into dev

Browse Source 
New fade-down feature (aka deceleration limit) works reliably and as
expected.
Continuing on dev branch.
This commit is contained in:
jonny_l480 2022-07-23 17:19:54 +02:00
commit d1dcf726aa
6 changed files with 247 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
main/button.cpp
View File

@ -19,11 +19,13 @@ static const char * TAG = "button";
//-----------------------------
//-------- constructor --------
//-----------------------------
buttonCommands::buttonCommands(gpio_evaluatedSwitch * button_f, controlledArmchair * control_f, buzzer_t * buzzer_f ){
buttonCommands::buttonCommands(gpio_evaluatedSwitch * button_f, controlledArmchair * control_f, buzzer_t * buzzer_f, controlledMotor * motorLeft_f, controlledMotor * motorRight_f){
//copy object pointers
button = button_f;
control = control_f;
buzzer = buzzer_f;
motorLeft = motorLeft_f;
motorRight = motorRight_f;
//TODO declare / configure evaluatedSwitch here instead of config (unnecessary that button object is globally available - only used here)?
}
@ -63,6 +65,19 @@ void buttonCommands::action (uint8_t count){
ESP_LOGW(TAG, "cmd %d: toggle between MASSAGE and JOYSTICK", count);
control->toggleModes(controlMode_t::MASSAGE, controlMode_t::JOYSTICK); //toggle between MASSAGE and JOYSTICK mode
break;
case 8:
//toggle deceleration fading between on and off
bool decelEnabled = motorLeft->toggleFade(fadeType_t::DECEL);
motorRight->toggleFade(fadeType_t::DECEL);
ESP_LOGW(TAG, "cmd %d: toggle deceleration fading to: %d", count, (int)decelEnabled);
if (decelEnabled){
buzzer->beep(3, 60, 50);
} else {
buzzer->beep(1, 1000, 1);
}
}
}

7
main/button.hpp
View File

@ -3,6 +3,7 @@
#include "gpio_evaluateSwitch.hpp"
#include "buzzer.hpp"
#include "control.hpp"
#include "motorctl.hpp"
@ -16,7 +17,9 @@ class buttonCommands {
buttonCommands (
gpio_evaluatedSwitch * button_f,
controlledArmchair * control_f,
buzzer_t * buzzer_f
buzzer_t * buzzer_f,
controlledMotor * motorLeft_f,
controlledMotor * motorRight_f
);
//--- functions ---
@ -32,6 +35,8 @@ class buttonCommands {
gpio_evaluatedSwitch* button;
controlledArmchair * control;
buzzer_t* buzzer;
controlledMotor * motorLeft;
controlledMotor * motorRight;
//--- variables ---
uint8_t count = 0;

3
main/config.cpp
View File

@ -29,7 +29,8 @@ single100a_config_t configDriverRight = {
//--- configure motor contol ---
motorctl_config_t configMotorControl = {
.msFade = 900,
.msFadeAccel = 1300, //acceleration of the motor (ms it takes from 0% to 100%)
.msFadeDecel = 800, //deceleration of the motor (ms it takes from 100% to 0%)
.currentMax = 10
};

2
main/main.cpp
View File

@ -75,7 +75,7 @@ void task_control( void * pvParameters ){
void task_button( void * pvParameters ){
ESP_LOGI(TAG, "Initializing command-button and starting handle loop");
//create button instance
buttonCommands commandButton(&buttonJoystick, &control, &buzzer);
buttonCommands commandButton(&buttonJoystick, &control, &buzzer, &motorLeft, &motorRight);
//start handle loop
commandButton.startHandleLoop();
}

218
main/motorctl.cpp
View File

@ -11,6 +11,9 @@ static const char * TAG = "motor-control";
controlledMotor::controlledMotor(single100a_config_t config_driver, motorctl_config_t config_control): motor(config_driver) {
//copy parameters for controlling the motor
config = config_control;
//copy configured default fading durations to actually used variables
msFadeAccel = config.msFadeAccel;
msFadeDecel = config.msFadeDecel;
init();
//TODO: add currentsensor object here
@ -27,6 +30,26 @@ void controlledMotor::init(){
//----------------
//----- fade -----
//----------------
//local function that fades a variable
//- increments a variable (pointer) by given value
//- sets to target if already closer than increment
//TODO this needs testing
void fade(float * dutyNow, float dutyTarget, float dutyIncrement){
float dutyDelta = dutyTarget - *dutyNow;
if ( fabs(dutyDelta) > fabs(dutyIncrement) ) { //check if already close to target
*dutyNow = *dutyNow + dutyIncrement;
}
//already closer to target than increment
else {
*dutyNow = dutyTarget;
}
}
//==============================
//=========== handle ===========
//==============================
@ -43,33 +66,129 @@ void controlledMotor::handle(){
ESP_LOGD(TAG, "Read command from queue: state=%s, duty=%.2f", motorstateStr[(int)commandReceive.state], commandReceive.duty);
state = commandReceive.state;
dutyTarget = commandReceive.duty;
//--- 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;
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;
}
}
//--- calculate increment ---
//calculate increment with passed time since last run and configured fade time
//calculate increment for fading UP with passed time since last run and configured fade time
int64_t usPassed = esp_timer_get_time() - timestampLastRunUs;
dutyIncrement = ( usPassed / ((float)config.msFade * 1000) ) * 100; //TODO define maximum increment - first run after startup (or long) pause can cause a very large increment
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 immediately, update state, duty and exit this cycle of handle function
if (state == motorstate_t::BRAKE){
motor.set(motorstate_t::BRAKE, 0);
dutyNow = 0;
return; //no need to run the fade algorithm
}
//--- calculate difference ---
dutyDelta = dutyTarget - dutyNow;
//positive: need to increase by that value
//negative: need to decrease
//--- fade up ---
if(dutyDelta > dutyIncrement){ //target duty his higher than current duty -> fade up
ESP_LOGV(TAG, "*fading up*: target=%.2f%% - previous=%.2f%% - increment=%.6f%% - usSinceLastRun=%d", dutyTarget, dutyNow, dutyIncrement, (int)usPassed);
dutyNow += dutyIncrement; //increase duty by increment
//--- set lower ---
} else { //target duty is lower than current duty -> immediately set to target
ESP_LOGV(TAG, "*setting to target*: target=%.2f%% - previous=%.2f%% ", dutyTarget, dutyNow);
dutyNow = dutyTarget; //set target duty
//--- fade duty to target (up and down) ---
//TODO: this needs optimization (can be more clear and/or simpler)
if (dutyDelta > 0) { //difference positive -> increasing duty (-100 -> 100)
if (dutyNow < 0) { //reverse, decelerating
fade(&dutyNow, dutyTarget, dutyIncrementDecel);
}
else if (dutyNow >= 0) { //forward, accelerating
fade(&dutyNow, dutyTarget, dutyIncrementAccel);
}
}
else if (dutyDelta < 0) { //difference negative -> decreasing duty (100 -> -100)
if (dutyNow <= 0) { //reverse, accelerating
fade(&dutyNow, dutyTarget, - dutyIncrementAccel);
}
else if (dutyNow > 0) { //forward, decelerating
fade(&dutyNow, dutyTarget, - dutyIncrementDecel);
}
}
//previous approach: (resulted in bug where accel/decel fade is swaped in reverse)
// //--- fade up ---
// //dutyDelta is higher than IncrementUp -> fade up
// if(dutyDelta > dutyIncrementUp){
// ESP_LOGV(TAG, "*fading up*: target=%.2f%% - previous=%.2f%% - increment=%.6f%% - usSinceLastRun=%d", dutyTarget, dutyNow, dutyIncrementUp, (int)usPassed);
// dutyNow += dutyIncrementUp; //increase duty by increment
// }
//
// //--- fade down ---
// //dutyDelta is more negative than -IncrementDown -> fade down
// else if (dutyDelta < -dutyIncrementDown){
// ESP_LOGV(TAG, "*fading down*: target=%.2f%% - previous=%.2f%% - increment=%.6f%% - usSinceLastRun=%d", dutyTarget, dutyNow, dutyIncrementDown, (int)usPassed);
//
// dutyNow -= dutyIncrementDown; //decrease duty by increment
// }
//
// //--- set to target ---
// //duty is already very close to target (closer than IncrementUp or IncrementDown)
// else{
// //immediately set to target duty
// dutyNow = dutyTarget;
// }
//define motorstate from converted duty -100 to 100
//apply target duty and state to motor driver
//forward
if(dutyNow > 0){
state = motorstate_t::FWD;
}
//reverse
else if (dutyNow < 0){
state = motorstate_t::REV;
}
//idle
else {
state = motorstate_t::IDLE;
}
//--- apply to motor ---
//apply target duty and state to motor driver
motor.set(state, dutyNow);
motor.set(state, fabs(dutyNow));
//note: BRAKE state is handled earlier
//--- update timestamp ---
timestampLastRunUs = esp_timer_get_time(); //update timestamp last run with current timestamp in microseconds
@ -110,3 +229,76 @@ motorCommand_t controlledMotor::getStatus(){
return status;
};
//===============================
//=========== setFade ===========
//===============================
//function for editing or enabling the fading/ramp of the motor control
//set/update fading duration/amount
void controlledMotor::setFade(fadeType_t fadeType, uint32_t msFadeNew){
//TODO: mutex for msFade variable also used in handle function
switch(fadeType){
case fadeType_t::ACCEL:
msFadeAccel = msFadeNew;
break;
case fadeType_t::DECEL:
msFadeDecel = msFadeNew;
break;
}
}
//enable (set to default value) or disable fading
void controlledMotor::setFade(fadeType_t fadeType, bool enabled){
uint32_t msFadeNew = 0; //define new fade time as default disabled
if(enabled){ //enable
//set to default/configured value
switch(fadeType){
case fadeType_t::ACCEL:
msFadeNew = config.msFadeAccel;
break;
case fadeType_t::DECEL:
msFadeNew = config.msFadeDecel;
break;
}
}
//apply new Fade value
setFade(fadeType, msFadeNew);
}
//==================================
//=========== toggleFade ===========
//==================================
//toggle fading between OFF and default value
bool controlledMotor::toggleFade(fadeType_t fadeType){
uint32_t msFadeNew = 0;
bool enabled = false;
switch(fadeType){
case fadeType_t::ACCEL:
if (msFadeAccel == 0){
msFadeNew = config.msFadeAccel;
enabled = true;
} else {
msFadeNew = 0;
}
break;
case fadeType_t::DECEL:
if (msFadeDecel == 0){
msFadeNew = config.msFadeAccel;
enabled = true;
} else {
msFadeNew = 0;
}
break;
}
//apply new Fade value
setFade(fadeType, msFadeNew);
//return new state (fading enabled/disabled)
return enabled;
}

23
main/motorctl.hpp
View File

@ -13,7 +13,7 @@ extern "C"
//-------------------------------------
//-------- struct declarations -------
//-------- struct/type declarations -------
//-------------------------------------
//struct for sending command for one motor in the queue
@ -30,10 +30,15 @@ typedef struct motorCommands_t {
//struct with all config parameters for a motor regarding ramp and current limit
typedef struct motorctl_config_t {
uint32_t msFade; //acceleration of the motor (ms it should take 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%)
float currentMax;
} motorctl_config_t;
//enum fade type (acceleration, deceleration)
//e.g. used for specifying which fading should be modified with setFade, togleFade functions
enum class fadeType_t {ACCEL, DECEL};
class controlledMotor {
@ -44,6 +49,10 @@ class controlledMotor {
void setTarget(motorstate_t state_f, float duty_f = 0); //adds target command to queue for handle function
motorCommand_t getStatus(); //get current status of the motor (returns struct with state and duty)
void setFade(fadeType_t fadeType, bool enabled); //enable/disable acceleration or deceleration fading
void setFade(fadeType_t fadeType, uint32_t msFadeNew); //set acceleration or deceleration fade time
bool toggleFade(fadeType_t fadeType); //toggle acceleration or deceleration on/off
private:
//--- functions ---
@ -59,7 +68,7 @@ class controlledMotor {
//--- variables ---
//struct for storing control specific parameters
motorctl_config_t config;
motorstate_t state = motorstate_t::IDLE;
float currentMax;
@ -67,15 +76,17 @@ class controlledMotor {
float dutyTarget;
float dutyNow;
float dutyIncrement;
float dutyIncrementAccel;
float dutyIncrementDecel;
float dutyDelta;
uint32_t msFadeAccel;
uint32_t msFadeDecel;
uint32_t ramp;
int64_t timestampLastRunUs;
struct motorCommand_t commandReceive = {};
struct motorCommand_t commandSend = {};
};