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

Add Duty, Current, Speed control modes, Add traction-control [testing]

Browse Source 
very experimental needs testing/debugging,
other control modes can currently be selected by editing the class definition in motorctl.hpp

menu/config
  - add menu item to enable/disable traction control system

main: pass ptr to other motor to motor object

speedsensor: add method to get last update time

motorctl: handle loop:
  - re-arrange some code sections
  - add several methods to get current status (needed from other motor for tcs)
  - add sketchy code for different control modes DUTY, CURRENT, SPEED (very basic implementation)
  - add experimental code for traction control
This commit is contained in:
jonny_jr9
2024-02-28 10:21:53 +01:00
parent 2bf2276dbe
commit 2abeefde07
8 changed files with 316 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files

304
common/motorctl.cpp
View File

@@ -28,7 +28,8 @@ void task_motorctl( void * ptrControlledMotor ){
//======== constructor ========
//=============================
//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) {
//copy parameters for controlling the motor
config = config_control;
@@ -36,6 +37,10 @@ controlledMotor::controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl
motorSetCommand = setCommandFunc;
//pointer to nvs handle
nvsHandle = nvsHandle_f;
//pointer to other motor object
ppOtherMotor = otherMotor_f;
//pointer to speed sensor
sSensor = speedSensor_f;
//create queue, initialize config values
init();
@@ -105,7 +110,7 @@ void controlledMotor::handle(){
//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
{
ESP_LOGV(TAG, "[%s] Read command from queue: state=%s, duty=%.2f", config.name, motorstateStr[(int)commandReceive.state], commandReceive.duty);
@@ -114,49 +119,135 @@ void controlledMotor::handle(){
receiveTimeout = false;
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
#define CURRENT_CONTROL_ALLOWED_AMPERE_DIFF 2
#define SPEED_CONTROL_MAX_SPEED_KMH 9
#define SPEED_CONTROL_ALLOWED_KMH_DIFF 1
//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);
receiveTimeout = true;
state = motorstate_t::IDLE;
dutyTarget = 0;
case motorControlMode_t::DUTY: // regulate to desired duty (as originally)
//--- convert duty ---
// define target duty (-100 to 100) from provided duty and motorstate
// 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;
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
ampereDiff = ampereTarget - ampereNow;
ESP_LOGV("TESTING", "CURRENT-CONTROL: ampereNow=%.2f, ampereTarget=%.2f, diff=%.2f", ampereNow, ampereTarget, ampereDiff); // todo handle brake
if (fabs(ampereDiff) > CURRENT_CONTROL_ALLOWED_AMPERE_DIFF)
{
if (ampereDiff > 0 && commandReceive.state == motorstate_t::FWD) // 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::REV) // backward need to increase current (more negative)
{
dutyTarget = -100;
}
else // fwd too much, rev too much -> decrease
{
dutyTarget = 0;
}
ESP_LOGV("TESTING", "CURRENT-CONTROL: set target to %.0f%%", dutyTarget);
}
else
{
dutyTarget = dutyNow; // target current reached
ESP_LOGD("TESTING", "CURRENT-CONTROL: target current %.3f reached", dutyTarget);
}
break;
case motorControlMode_t::SPEED: // regulate to desired speed
speedNow = sSensor->getKmph();
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;
speedDiff = speedTarget - speedNow;
ESP_LOGV("TESTING", "SPEED-CONTROL: target-speed=%.2f, current-speed=%.2f, diff=%.3f", speedTarget, speedNow, speedDiff);
if (fabs(speedDiff) > SPEED_CONTROL_ALLOWED_KMH_DIFF)
{
if (speedDiff > 0 && commandReceive.state == motorstate_t::FWD) // 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
}
else if (speedDiff < 0 && commandReceive.state == motorstate_t::REV) // backward need to increase speed (more negative)
{
dutyTarget = -100;
}
else // fwd too much, rev too much -> decrease
{
dutyTarget = 0;
}
ESP_LOGV("TESTING", "CURRENT-CONTROL: set target to %.0f%%", dutyTarget);
}
else
{
dutyTarget = dutyNow; // target current reached
ESP_LOGD("TESTING", "SPEED-CONTROL: target speed %.3f reached", 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)
{
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;
state = motorstate_t::IDLE;
dutyTarget = 0; // todo put this in else section of queue (no data received) and add control mode "timeout"?
}
//--- CALCULATE DUTY-DIFF ---
dutyDelta = dutyTarget - dutyNow;
//positive: need to increase by that value
//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
//-> 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 ((dutyDelta == 0 && !config.currentLimitEnabled && !config.tractionControlSystemEnabled) || (dutyTarget == 0 && dutyNow == 0)) //when current limit or tcs enabled only slow down when duty is 0
{
//increase timeout once when duty is the same (once)
//increase queue timeout when duty is the same (once)
if (timeoutWaitForCommand == 0)
{ // TODO verify if state matches too?
ESP_LOGI(TAG, "[%s] already at target duty %.2f, slowing down...", config.name, dutyTarget);
@@ -175,23 +266,6 @@ void controlledMotor::handle(){
//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 immediately, update state, duty and exit this cycle of handle function
if (state == motorstate_t::BRAKE){
@@ -203,9 +277,25 @@ void controlledMotor::handle(){
}
//----- 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)
//TODO: this needs optimization (can be more clear and/or simpler)
if (dutyDelta > 0) { //difference positive -> increasing duty (-100 -> 100)
@@ -226,7 +316,7 @@ void controlledMotor::handle(){
}
//----- CURRENT LIMIT -----
//----- CURRENT LIMIT -----
currentNow = cSensor.read();
if ((config.currentLimitEnabled) && (dutyDelta != 0)){
if (fabs(currentNow) > config.currentMax){
@@ -244,7 +334,73 @@ void controlledMotor::handle(){
}
}
//----- 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
if (config.tractionControlSystemEnabled && sSensor->getTimeLastUpdate() != tcs_timestampLastSpeedUpdate){
//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->getRpm();
float speedNowOther = (*ppOtherMotor)->getDuty();
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;
ESP_LOGD("TESTING", "[%s] TCS: ratioSpeedTarget=%.3f, ratioSpeedNow=%.3f, ratioDutyNow=%.3f, diff=%.3f", config.name, ratioSpeedTarget, ratioSpeedNow, ratioDutyNow, ratioDiff);
//-- handle rotating faster than expected --
//TODO also increase duty when other motor is slipping? (diff negative)
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)
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
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
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;
}
}
//--- define new motorstate --- (-100 to 100 => direction)
state=getStateFromDuty(dutyNow);
@@ -254,25 +410,27 @@ void controlledMotor::handle(){
//FWD -> IDLE -> FWD continue without waiting
//FWD -> IDLE -> REV wait for dead-time in IDLE
//TODO check when changed only?
if ( //not enough time between last direction state
( 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
deadTimeWaiting = true;
ESP_LOGI(TAG, "starting dead-time... %s -> %s", motorstateStr[(int)statePrev], motorstateStr[(int)state]);
}
//force IDLE state during wait
state = motorstate_t::IDLE;
dutyNow = 0;
} else {
if (deadTimeWaiting){ //log end
deadTimeWaiting = false;
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 (config.deadTimeMs > 0) { //deadTime is enabled
if ( //not enough time between last direction state
( 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
deadTimeWaiting = true;
ESP_LOGI(TAG, "starting dead-time... %s -> %s", motorstateStr[(int)statePrev], motorstateStr[(int)state]);
}
//force IDLE state during wait
state = motorstate_t::IDLE;
dutyNow = 0;
} else {
if (deadTimeWaiting){ //log end
deadTimeWaiting = false;
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);
}
}
//--- save current actual motorstate and timestamp ---

29
common/motorctl.hpp
View File

@@ -13,6 +13,7 @@ extern "C"
#include "motordrivers.hpp"
#include "currentsensor.hpp"
#include "speedsensor.hpp"
//=======================================
@@ -30,11 +31,18 @@ typedef void (*motorSetCommandFunc_t)(motorCommand_t cmd);
class controlledMotor {
public:
//--- 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 setTarget(motorstate_t state_f, float duty_f = 0); //adds target command to queue for handle function
void setTarget(motorCommand_t command);
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;};
void enableTractionControlSystem() {config.tractionControlSystemEnabled = true;};
void disableTractionControlSystem() {config.tractionControlSystemEnabled = false; tcs_isExceeded = false;};
bool getTractionControlSystemStatus() {return config.tractionControlSystemEnabled;};
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
@@ -60,6 +68,11 @@ class controlledMotor {
QueueHandle_t commandQueue = NULL;
//current sensor
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)
motorSetCommandFunc_t motorSetCommand;
@@ -69,14 +82,21 @@ class controlledMotor {
//struct for storing control specific parameters
motorctl_config_t config;
motorstate_t state = motorstate_t::IDLE;
motorControlMode_t mode = motorControlMode_t::DUTY;
//handle for using the nvs flash (persistent config variables)
nvs_handle_t * nvsHandle;
float currentMax;
float currentNow;
//speed mode
float speedTarget = 0;
float speedNow = 0;
float dutyTarget = 0;
float dutyNow = 0;
float dutyIncrementAccel;
float dutyIncrementDecel;
float dutyDelta;
@@ -97,6 +117,13 @@ class controlledMotor {
uint32_t timestamp_commandReceived = 0;
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
uint64_t pulseSum = pulse1 + pulse2 + pulse3;
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 getMps(); //meters per second
float getRpm(); //rotations per minute
float getTimeLastUpdate() {return timeLastUpdate;};
//variables for handling the encoder (public because ISR needs access)
speedSensor_config_t config;
@@ -45,6 +46,7 @@ public:
int debugCount = 0;
uint32_t debug_countIgnoredSequencesTooShort = 0;
double currentRpm = 0;
uint32_t timeLastUpdate = 0;
private:
static bool isrIsInitialized; // default false due to static

2
common/types.hpp
View File

@@ -22,6 +22,7 @@ enum class motorstate_t {IDLE, FWD, REV, BRAKE};
//definition of string array to be able to convert state enum to readable string (defined in motordrivers.cpp)
extern const char* motorstateStr[4];
enum class motorControlMode_t {DUTY, CURRENT, SPEED};
//===========================
@@ -45,6 +46,7 @@ typedef struct motorctl_config_t {
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%)
bool currentLimitEnabled;
bool tractionControlSystemEnabled;
adc1_channel_t currentSensor_adc;
float currentSensor_ratedCurrent;
float currentMax;