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

Merge branch 'dev'

Browse Source
This commit is contained in:
jonny_ji7 2022-07-24 10:15:49 +02:00
commit ca41213e22
10 changed files with 499 additions and 45 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;

15
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
};
@ -57,10 +58,10 @@ control_config_t configControl = {
//----- httpJoystick config -----
//-------------------------------
httpJoystick_config_t configHttpJoystickMain{
.toleranceZeroX_Per = 3, //percentage around joystick axis the coordinate snaps to 0
.toleranceZeroY_Per = 10,
.toleranceZeroX_Per = 1, //percentage around joystick axis the coordinate snaps to 0
.toleranceZeroY_Per = 6,
.toleranceEndPer = 2, //percentage before joystick end the coordinate snaps to 1/-1
.timeoutMs = 3000 //time no new data was received before the motors get turned off
.timeoutMs = 2500 //time no new data was received before the motors get turned off
};
@ -72,8 +73,8 @@ joystick_config_t configJoystick = {
.adc_x = ADC1_CHANNEL_3, //GPIO39
.adc_y = ADC1_CHANNEL_0, //GPIO36
//percentage of joystick range the coordinate of the axis snaps to 0 (0-100)
.tolerance_zeroX_per = 7,
.tolerance_zeroY_per = 3,
.tolerance_zeroX_per = 3,
.tolerance_zeroY_per = 7,
//percentage of joystick range the coordinate snaps to -1 or 1 before configured "_max" or "_min" threshold (mechanical end) is reached (0-100)
.tolerance_end_per = 5,
//threshold the radius jumps to 1 before the stick is at max radius (range 0-1)
@ -123,7 +124,7 @@ buzzer_t buzzer(GPIO_NUM_12, 100);
httpJoystick httpJoystickMain(configHttpJoystickMain);
//create global control object
controlledArmchair control(configControl, &buzzer, &motorLeft, &motorRight, &httpJoystickMain);
controlledArmchair control(configControl, &buzzer, &motorLeft, &motorRight, &joystick, &httpJoystickMain);

37
main/control.cpp
View File

@ -27,6 +27,7 @@ controlledArmchair::controlledArmchair (
buzzer_t * buzzer_f,
controlledMotor* motorLeft_f,
controlledMotor* motorRight_f,
evaluatedJoystick* joystick_f,
httpJoystick* httpJoystick_f
){
@ -36,6 +37,7 @@ controlledArmchair::controlledArmchair (
buzzer = buzzer_f;
motorLeft = motorLeft_f;
motorRight = motorRight_f;
joystick_l = joystick_f,
httpJoystickMain_l = httpJoystick_f;
//set default mode from config
modePrevious = config.defaultMode;
@ -68,10 +70,13 @@ void controlledArmchair::startHandleLoop() {
break;
case controlMode_t::JOYSTICK:
//get current joystick data with getData method of evaluatedJoystick
stickData = joystick_l->getData();
//additionaly scale coordinates (more detail in slower area)
joystick_scaleCoordinatesLinear(&stickData, 0.6, 0.35); //TODO: add scaling parameters to config
//generate motor commands
//pass joystick data from getData method of evaluatedJoystick to generateCommandsDriving function
commands = joystick_generateCommandsDriving(joystick.getData());
//TODO: pass pointer to joystick object to control class instead of accessing it directly globally
commands = joystick_generateCommandsDriving(stickData);
//apply motor commands
motorRight->setTarget(commands.right.state, commands.right.duty);
motorLeft->setTarget(commands.left.state, commands.left.duty);
//TODO make motorctl.setTarget also accept motorcommand struct directly
@ -79,24 +84,26 @@ void controlledArmchair::startHandleLoop() {
break;
case controlMode_t::MASSAGE:
motorRight->setTarget(motorstate_t::IDLE, 0);
motorLeft->setTarget(motorstate_t::IDLE, 0);
//TODO add actual command generation here
//generate motor commands
//pass joystick data from getData method of evaluatedJoystick to generateCommandsShaking function
commands = joystick_generateCommandsShaking(joystick_l->getData());
//apply motor commands
motorRight->setTarget(commands.right.state, commands.right.duty);
motorLeft->setTarget(commands.left.state, commands.left.duty);
vTaskDelay(20 / portTICK_PERIOD_MS);
break;
case controlMode_t::HTTP:
//create emptry struct for receiving data from http function
joystickData_t dataRead = { };
//--- get joystick data from queue ---
//Note this function waits several seconds (httpconfig.timeoutMs) for data to arrive, otherwise Center data or NULL is returned
//TODO: as described above, when changing modes it might delay a few seconds for the change to apply
dataRead = httpJoystickMain_l->getData();
stickData = httpJoystickMain_l->getData();
//scale coordinates additionally (more detail in slower area)
joystick_scaleCoordinatesLinear(&stickData, 0.6, 0.4); //TODO: add scaling parameters to config
ESP_LOGD(TAG, "generating commands from x=%.3f y=%.3f radius=%.3f angle=%.3f", stickData.x, stickData.y, stickData.radius, stickData.angle);
//--- generate motor commands ---
ESP_LOGD(TAG, "generating commands from x=%.3f y=%.3f radius=%.3f angle=%.3f", dataRead.x, dataRead.y, dataRead.radius, dataRead.angle);
//Note: timeout (no data received) is handled in getData method
commands = joystick_generateCommandsDriving(dataRead);
commands = joystick_generateCommandsDriving(stickData);
//--- apply commands to motors ---
//TODO make motorctl.setTarget also accept motorcommand struct directly
@ -162,11 +169,11 @@ void controlledArmchair::handleTimeout(){
if (validateActivity(dutyLeft_lastActivity, dutyLeftNow, inactivityTolerance)
|| validateActivity(dutyRight_lastActivity, dutyRightNow, inactivityTolerance)
){
ESP_LOGD(TAG, "timeout check: detected [activity] since last check -> reset");
ESP_LOGD(TAG, "timeout check: [activity] detected since last check -> reset");
//reset last duty and timestamp
timestamp_lastActivity = esp_log_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){
@ -175,7 +182,7 @@ void controlledArmchair::handleTimeout(){
toggleIdle();
}
else {
ESP_LOGD(TAG, "timeout check: [inactive], last activity %.1f seconds ago", (float)(esp_log_timestamp() - timestamp_lastActivity)/1000);
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);
}
}
}

5
main/control.hpp
View File

@ -38,6 +38,7 @@ class controlledArmchair {
buzzer_t* buzzer_f,
controlledMotor* motorLeft_f,
controlledMotor* motorRight_f,
evaluatedJoystick* joystick_f,
httpJoystick* httpJoystick_f
);
@ -68,6 +69,7 @@ class controlledArmchair {
controlledMotor* motorLeft;
controlledMotor* motorRight;
httpJoystick* httpJoystickMain_l;
evaluatedJoystick* joystick_l;
//---variables ---
//struct for motor commands returned by generate functions of each mode
@ -75,6 +77,9 @@ class controlledArmchair {
//struct with config parameters
control_config_t config;
//store joystick data
joystickData_t stickData;
//variables for http mode
uint32_t http_timestamp_lastData = 0;

187
main/joystick.cpp
View File

@ -163,6 +163,73 @@ float scaleCoordinate(float input, float min, float max, float center, float tol
//===========================================
//====== joystick_scaleCoordinatesExp =======
//===========================================
//local function that scales the absolute value of a variable exponentionally
float scaleExp(float value, float exponent){
float result = powf(fabs(value), exponent);
if (value >= 0) {
return result;
} else {
return -result;
}
}
//function that updates a joystickData object with exponentionally scaling applied to coordinates
void joystick_scaleCoordinatesExp(joystickData_t * data, float exponent){
//scale x and y coordinate
data->x = scaleExp(data->x, exponent);
data->y = scaleExp(data->y, exponent);
//re-calculate radius
data->radius = sqrt(pow(data->x,2) + pow(data->y,2));
if (data->radius > 1-0.07) {//FIXME hardcoded radius tolerance
data->radius = 1;
}
}
//==============================================
//====== joystick_scaleCoordinatesLinear =======
//==============================================
//local function that scales value from -1-1 to -1-1 with two different slopes before and after a specified point
//slope1: for value from 0 to pointX -> scale linear from 0 to pointY
//slope2: for value from pointX to 1 -> scale linear from pointY to 1
float scaleLinPoint(float value, float pointX, float pointY){
float result;
if (fabs(value) <= pointX) {
//--- scale on line from 0 to point ---
result = fabs(value) * (pointY/pointX);
} else {
//--- scale on line from point to 1 ---
float m = (1-pointY) / (1-pointX);
result = fabs(value) * m + (1 - m);
}
//--- return result with same sign as input ---
if (value >= 0) {
return result;
} else {
return -result;
}
}
//function that updates a joystickData object with linear scaling applied to coordinates
//e.g. use to use more joystick resolution for lower speeds
void joystick_scaleCoordinatesLinear(joystickData_t * data, float pointX, float pointY){
//scale x and y coordinate
data->x = scaleLinPoint(data->x, pointX, pointY);
data->y = scaleLinPoint(data->y, pointX, pointY);
//re-calculate radius
data->radius = sqrt(pow(data->x,2) + pow(data->y,2));
if (data->radius > 1-0.07) {//FIXME hardcoded radius tolerance
data->radius = 1;
}
}
//=============================================
//========= joystick_evaluatePosition =========
//=============================================
@ -227,7 +294,7 @@ motorCommands_t joystick_generateCommandsDriving(joystickData_t data){
motorCommands_t commands;
float dutyMax = 94; //TODO add this to config, make changeable during runtime
float dutyOffset = 5; //immedeately starts with this duty, TODO add this to config
float dutyOffset = 10; //immedeately starts with this duty, TODO add this to config
float dutyRange = dutyMax - dutyOffset;
float ratio = fabs(data.angle) / 90; //90degree = x=0 || 0degree = y=0
@ -299,3 +366,121 @@ motorCommands_t joystick_generateCommandsDriving(joystickData_t data){
ESP_LOGI(TAG_CMD, "motor right: state=%s, duty=%.3f", motorstateStr[(int)commands.right.state], commands.right.duty);
return commands;
}
//============================================
//========= joystick_CommandsShaking =========
//============================================
//--- variable declarations ---
uint32_t shake_timestamp_turnedOn = 0;
uint32_t shake_timestamp_turnedOff = 0;
bool shake_state = false;
//--- configure shake mode --- TODO: move this to config
uint32_t shake_msOffMax = 90;
uint32_t shake_msOnMax = 180;
float dutyShake = 60;
//function that generates commands for both motors from the joystick data
motorCommands_t joystick_generateCommandsShaking(joystickData_t data){
//struct with current data of the joystick
//typedef struct joystickData_t {
// joystickPos_t position;
// float x;
// float y;
// float radius;
// float angle;
//} joystickData_t;
motorCommands_t commands;
float ratio = fabs(data.angle) / 90; //90degree = x=0 || 0degree = y=0
//--- calculate on/off duration ---
uint32_t msOn = shake_msOnMax * data.radius;
uint32_t msOff = shake_msOffMax * data.radius;
//--- evaluate state (on/off) ---
if (data.radius > 0 ){
//currently off
if (shake_state == false){
//off long enough
if (esp_log_timestamp() - shake_timestamp_turnedOff > msOff) {
//turn on
shake_state = true;
shake_timestamp_turnedOn = esp_log_timestamp();
}
}
//currently on
else {
//on long enough
if (esp_log_timestamp() - shake_timestamp_turnedOn > msOn) {
//turn off
shake_state = false;
shake_timestamp_turnedOff = esp_log_timestamp();
}
}
}
//joystick is at center
else {
shake_state = false;
shake_timestamp_turnedOff = esp_log_timestamp();
}
if (shake_state){
switch (data.position){
default:
commands.left.state = motorstate_t::IDLE;
commands.right.state = motorstate_t::IDLE;
commands.left.duty = 0;
commands.right.duty = 0;
break;
case joystickPos_t::Y_AXIS:
if (data.y > 0){
commands.left.state = motorstate_t::FWD;
commands.right.state = motorstate_t::FWD;
} else {
commands.left.state = motorstate_t::REV;
commands.right.state = motorstate_t::REV;
}
//set duty to shake
commands.left.duty = dutyShake;
commands.right.duty = dutyShake;
break;
case joystickPos_t::X_AXIS:
if (data.x > 0) {
commands.left.state = motorstate_t::FWD;
commands.right.state = motorstate_t::REV;
} else {
commands.left.state = motorstate_t::REV;
commands.right.state = motorstate_t::FWD;
}
//set duty to shake
commands.left.duty = dutyShake;
commands.right.duty = dutyShake;
break;
}
} else { //shake state off
commands.left.state = motorstate_t::IDLE;
commands.right.state = motorstate_t::IDLE;
commands.left.duty = 0;
commands.right.duty = 0;
}
ESP_LOGI(TAG_CMD, "generated commands from data: state=%s, angle=%.3f, ratio=%.3f/%.3f, radius=%.2f, x=%.2f, y=%.2f",
joystickPosStr[(int)data.position], data.angle, ratio, (1-ratio), data.radius, data.x, data.y);
ESP_LOGI(TAG_CMD, "motor left: state=%s, duty=%.3f", motorstateStr[(int)commands.left.state], commands.left.duty);
ESP_LOGI(TAG_CMD, "motor right: state=%s, duty=%.3f", motorstateStr[(int)commands.right.state], commands.right.duty);
return commands;
}

33
main/joystick.hpp
View File

@ -110,6 +110,16 @@ class evaluatedJoystick {
motorCommands_t joystick_generateCommandsDriving(joystickData_t data );
//============================================
//========= joystick_CommandsShaking =========
//============================================
//function that generates commands for both motors from the joystick data
//motorCommands_t joystick_generateCommandsDriving(evaluatedJoystick joystick);
motorCommands_t joystick_generateCommandsShaking(joystickData_t data );
//==============================
//====== scaleCoordinate =======
//==============================
@ -117,6 +127,29 @@ motorCommands_t joystick_generateCommandsDriving(joystickData_t data );
float scaleCoordinate(float input, float min, float max, float center, float tolerance_zero_per, float tolerance_end_per);
//===========================================
//====== joystick_scaleCoordinatesExp =======
//===========================================
//function that updates a joystickData object with exponentionally scaling applied to coordinates
//e.g. use to use more joystick resolution for lower speeds
void joystick_scaleCoordinatesExp(joystickData_t * data, float exponent);
//==============================================
//====== joystick_scaleCoordinatesLinear =======
//==============================================
//function that updates a joystickData object with linear scaling applied to coordinates
//scales coordinates with two different slopes before and after a specified point
//slope1: for value from 0 to pointX -> scale linear from 0 to pointY
//slope2: for value from pointX to 1 -> scale linear from pointY to 1
//=> best to draw the lines and point in a graph
//e.g. use to use more joystick resolution for lower speeds
void joystick_scaleCoordinatesLinear(joystickData_t * data, float pointX, float pointY);
//=============================================
//========= joystick_evaluatePosition =========
//=============================================

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 = {};
};