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

Add nvs in motorctl: Store and Load accel/decel conf

Browse Source 
Changes in menu for accel/decel time are now persistent after
restarts

main: initialize nvs and pass pointer to motorctl task
motorctl:
    - add method to get default configured value
    - add name to config -> adjust logging
    - add methods to read and write msFadeAccel and msFadeDecel from nvs
This commit is contained in:
jonny_jr9 2024-02-20 16:34:42 +01:00
parent dab6a437c0
commit 268018832d
5 changed files with 178 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
board_single/main/config.cpp
View File

@ -90,6 +90,7 @@ sabertooth2x60_config_t sabertoothConfig = {
// TODO add motor name string -> then use as log tag?
//--- configure left motor (contol) ---
motorctl_config_t configMotorControlLeft = {
.name = "left",
.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%)
.currentLimitEnabled = false,
@ -101,6 +102,7 @@ motorctl_config_t configMotorControlLeft = {
//--- configure right motor (contol) ---
motorctl_config_t configMotorControlRight = {
.name = "right",
.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%)
.currentLimitEnabled = false,

26
board_single/main/main.cpp
View File

@ -4,6 +4,7 @@ extern "C"
#include <esp_system.h>
#include <esp_event.h>
#include <nvs_flash.h>
#include "nvs.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"
@ -93,9 +94,12 @@ esp_err_t on_joystick_url(httpd_req_t *req)
return (httpJoystickMain->*pointerToReceiveFunc)(req);
}
//-- tag for logging --
//--- tag for logging ---
static const char * TAG = "main";
//-- handle passed to tasks for accessing nvs --
nvs_handle_t nvsHandle;
@ -140,8 +144,8 @@ void createObjects()
// create controlled motor instances (motorctl.hpp)
// with configurations from config.cpp
motorLeft = new controlledMotor(setLeftFunc, configMotorControlLeft);
motorRight = new controlledMotor(setRightFunc, configMotorControlRight);
motorLeft = new controlledMotor(setLeftFunc, configMotorControlLeft, &nvsHandle);
motorRight = new controlledMotor(setRightFunc, configMotorControlRight, &nvsHandle);
// create speedsensor instances
// with configurations from config.cpp
@ -207,6 +211,22 @@ extern "C" void app_main(void) {
//--- initialize encoder ---
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 ---
err = nvs_open("storage", NVS_READWRITE, &nvsHandle);
if (err != ESP_OK)
ESP_LOGE(TAG, "Error (%s) opening NVS handle!\n", esp_err_to_name(err));
printf("\n");

158
common/motorctl.cpp
View File

@ -15,7 +15,7 @@ static const char * TAG = "motor-control";
//task for handling the motors (ramp, current limit, driver)
void task_motorctl( void * task_motorctl_parameters ){
task_motorctl_parameters_t *objects = (task_motorctl_parameters_t *)task_motorctl_parameters;
ESP_LOGW(TAG, "Task-motorctl: starting handle loop...");
ESP_LOGW(TAG, "Task-motorctl: starting handle loops for left and right motor...");
while(1){
objects->motorRight->handle();
objects->motorLeft->handle();
@ -29,19 +29,17 @@ void task_motorctl( void * task_motorctl_parameters ){
//======== 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):
controlledMotor::controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl_config_t config_control, nvs_handle_t * nvsHandle_f):
cSensor(config_control.currentSensor_adc, config_control.currentSensor_ratedCurrent) {
//copy parameters for controlling the motor
config = config_control;
//pointer to update motot dury method
motorSetCommand = setCommandFunc;
//copy configured default fading durations to actually used variables
msFadeAccel = config.msFadeAccel;
msFadeDecel = config.msFadeDecel;
//pointer to nvs handle
nvsHandle = nvsHandle_f;
//create queue, initialize config values
init();
//TODO: add currentsensor object here
//currentSensor cSensor(config.currentSensor_adc, config.currentSensor_ratedCurrent);
}
@ -54,7 +52,11 @@ void controlledMotor::init(){
if (commandQueue == NULL)
ESP_LOGE(TAG, "Failed to create command-queue");
else
ESP_LOGW(TAG, "Initialized command-queue");
ESP_LOGI(TAG, "[%s] Initialized command-queue", config.name);
// load config values from nvs, otherwise use default from config object
loadAccelDuration();
loadDecelDuration();
//cSensor.calibrateZeroAmpere(); //currently done in currentsensor constructor TODO do this regularly e.g. in idle?
}
@ -104,7 +106,7 @@ void controlledMotor::handle(){
//--- receive commands from queue ---
if( xQueueReceive( commandQueue, &commandReceive, ( TickType_t ) 0 ) )
{
ESP_LOGD(TAG, "Read command from queue: state=%s, duty=%.2f", motorstateStr[(int)commandReceive.state], commandReceive.duty);
ESP_LOGD(TAG, "[%s] Read command from queue: state=%s, duty=%.2f", config.name, motorstateStr[(int)commandReceive.state], commandReceive.duty);
state = commandReceive.state;
dutyTarget = commandReceive.duty;
receiveTimeout = false;
@ -139,7 +141,7 @@ void controlledMotor::handle(){
receiveTimeout = true;
state = motorstate_t::IDLE;
dutyTarget = 0;
ESP_LOGE(TAG, "TIMEOUT, no target data received for more than %ds -> switch to IDLE", TIMEOUT_IDLE_WHEN_NO_COMMAND/1000);
ESP_LOGE(TAG, "[%s] TIMEOUT, no target data received for more than %ds -> switch to IDLE", config.name, TIMEOUT_IDLE_WHEN_NO_COMMAND/1000);
}
//--- calculate increment ---
@ -164,7 +166,7 @@ void controlledMotor::handle(){
if (state == motorstate_t::BRAKE){
ESP_LOGD(TAG, "braking - skip fading");
motorSetCommand({motorstate_t::BRAKE, dutyTarget});
ESP_LOGI(TAG, "Set Motordriver: state=%s, duty=%.2f - Measurements: current=%.2f, speed=N/A", motorstateStr[(int)state], dutyNow, currentNow);
ESP_LOGI(TAG, "[%s] Set Motordriver: state=%s, duty=%.2f - Measurements: current=%.2f, speed=N/A", config.name, motorstateStr[(int)state], dutyNow, currentNow);
//dutyNow = 0;
return; //no need to run the fade algorithm
}
@ -211,7 +213,7 @@ void controlledMotor::handle(){
} else if (dutyNow > currentLimitDecrement) {
dutyNow -= currentLimitDecrement;
}
ESP_LOGW(TAG, "current limit exceeded! now=%.3fA max=%.1fA => decreased duty from %.3f to %.3f", currentNow, config.currentMax, dutyOld, dutyNow);
ESP_LOGW(TAG, "[%s] current limit exceeded! now=%.3fA max=%.1fA => decreased duty from %.3f to %.3f", config.name, currentNow, config.currentMax, dutyOld, dutyNow);
}
}
@ -255,7 +257,7 @@ void controlledMotor::handle(){
//--- apply new target to motor ---
motorSetCommand({state, (float)fabs(dutyNow)});
ESP_LOGI(TAG, "Set Motordriver: state=%s, duty=%.2f - Measurements: current=%.2f, speed=N/A", motorstateStr[(int)state], dutyNow, currentNow);
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
@ -275,7 +277,7 @@ void controlledMotor::setTarget(motorstate_t state_f, float duty_f){
.state = state_f,
.duty = duty_f
};
ESP_LOGI(TAG, "setTarget: Inserting command to queue: state='%s'(%d), duty=%.2f", motorstateStr[(int)commandSend.state], (int)commandSend.state, commandSend.duty);
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)
xQueueOverwrite( commandQueue, ( void * )&commandSend);
//xQueueSend( commandQueue, ( void * )&commandSend, ( TickType_t ) 0 );
@ -316,6 +318,22 @@ uint32_t controlledMotor::getFade(fadeType_t fadeType){
return 0;
}
//==============================
//======= getFadeDefault =======
//==============================
//return default accel / decel time (from config)
uint32_t controlledMotor::getFadeDefault(fadeType_t fadeType){
switch(fadeType){
case fadeType_t::ACCEL:
return config.msFadeAccel;
break;
case fadeType_t::DECEL:
return config.msFadeDecel;
break;
}
return 0;
}
//===============================
@ -328,12 +346,13 @@ 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:
ESP_LOGW(TAG, "changed fade-up time from %d to %d", msFadeAccel, msFadeNew);
msFadeAccel = msFadeNew;
ESP_LOGW(TAG, "[%s] changed fade-up time from %d to %d", config.name, msFadeAccel, msFadeNew);
writeAccelDuration(msFadeNew);
break;
case fadeType_t::DECEL:
ESP_LOGW(TAG, "changed fade-down time from %d to %d", msFadeDecel, msFadeNew);
ESP_LOGW(TAG, "[%s] changed fade-down time from %d to %d",config.name, msFadeDecel, msFadeNew);
msFadeDecel = msFadeNew;
writeDecelDuration(msFadeNew);
break;
}
}
@ -390,3 +409,108 @@ bool controlledMotor::toggleFade(fadeType_t fadeType){
return enabled;
}
//-----------------------------
//----- loadAccelDuration -----
//-----------------------------
// load stored value from nvs if not successfull uses config default value
void controlledMotor::loadAccelDuration(void)
{
// load default value
msFadeAccel = config.msFadeAccel;
// read from nvs
uint32_t valueNew;
char key[15];
snprintf(key, 15, "m-%s-accel", config.name);
esp_err_t err = nvs_get_u32(*nvsHandle, key, &valueNew);
switch (err)
{
case ESP_OK:
ESP_LOGW(TAG, "Successfully read value '%s' from nvs. Overriding default value %d with %d", key, config.msFadeAccel, valueNew);
msFadeAccel = valueNew;
break;
case ESP_ERR_NVS_NOT_FOUND:
ESP_LOGW(TAG, "nvs: the value '%s' is not initialized yet, keeping default value %d", key, msFadeAccel);
break;
default:
ESP_LOGE(TAG, "Error (%s) reading nvs!", esp_err_to_name(err));
}
}
//-----------------------------
//----- loadDecelDuration -----
//-----------------------------
void controlledMotor::loadDecelDuration(void)
{
// load default value
msFadeDecel = config.msFadeDecel;
// read from nvs
uint32_t valueNew;
char key[15];
snprintf(key, 15, "m-%s-decel", config.name);
esp_err_t err = nvs_get_u32(*nvsHandle, key, &valueNew);
switch (err)
{
case ESP_OK:
ESP_LOGW(TAG, "Successfully read value '%s' from nvs. Overriding default value %d with %d", key, config.msFadeDecel, valueNew);
msFadeDecel = valueNew;
break;
case ESP_ERR_NVS_NOT_FOUND:
ESP_LOGW(TAG, "nvs: the value '%s' is not initialized yet, keeping default value %d", key, msFadeDecel);
break;
default:
ESP_LOGE(TAG, "Error (%s) reading nvs!", esp_err_to_name(err));
}
}
//------------------------------
//----- writeAccelDuration -----
//------------------------------
// write provided value to nvs to be persistent and update the local variable msFadeAccel
void controlledMotor::writeAccelDuration(uint32_t newValue)
{
// generate nvs storage key
char key[15];
snprintf(key, 15, "m-%s-accel", config.name);
// update nvs value
ESP_LOGW(TAG, "[%s] updating nvs value '%s' from %d to %d", config.name, key, msFadeAccel, newValue);
esp_err_t err = nvs_set_u32(*nvsHandle, key, newValue);
if (err != ESP_OK)
ESP_LOGE(TAG, "nvs: failed writing");
err = nvs_commit(*nvsHandle);
if (err != ESP_OK)
ESP_LOGE(TAG, "nvs: failed committing updates");
else
ESP_LOGI(TAG, "nvs: successfully committed updates");
// update variable
msFadeAccel = newValue;
}
//------------------------------
//----- writeDecelDuration -----
//------------------------------
// write provided value to nvs to be persistent and update the local variable msFadeDecel
// TODO: reduce duplicate code
void controlledMotor::writeDecelDuration(uint32_t newValue)
{
// generate nvs storage key
char key[15];
snprintf(key, 15, "m-%s-decel", config.name);
// update nvs value
ESP_LOGW(TAG, "[%s] updating nvs value '%s' from %d to %d", config.name, key, msFadeDecel, newValue);
esp_err_t err = nvs_set_u32(*nvsHandle, key, newValue);
if (err != ESP_OK)
ESP_LOGE(TAG, "nvs: failed writing");
err = nvs_commit(*nvsHandle);
if (err != ESP_OK)
ESP_LOGE(TAG, "nvs: failed committing updates");
else
ESP_LOGI(TAG, "nvs: successfully committed updates");
// update variable
msFadeDecel = newValue;
}

13
common/motorctl.hpp
View File

@ -7,6 +7,8 @@ extern "C"
#include "freertos/queue.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "nvs_flash.h"
#include "nvs.h"
}
#include "motordrivers.hpp"
@ -28,12 +30,13 @@ typedef void (*motorSetCommandFunc_t)(motorCommand_t cmd);
class controlledMotor {
public:
//--- functions ---
controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl_config_t config_control); //constructor with structs for configuring motordriver and parameters for control TODO: add configuration for currentsensor
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
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
motorCommand_t getStatus(); //get current status of the motor (returns struct with state and duty)
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
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
@ -44,7 +47,11 @@ class controlledMotor {
private:
//--- functions ---
void init(); //creates currentsensor objects, motordriver objects and queue
void init(); // creates command-queue and initializes config values
void loadAccelDuration(void); // load stored value for msFadeAccel from nvs
void loadDecelDuration(void);
void writeAccelDuration(uint32_t newValue); // write value to nvs and update local variable
void writeDecelDuration(uint32_t newValue);
//--- objects ---
//queue for sending commands to the separate task running the handle() function very fast
@ -60,6 +67,8 @@ class controlledMotor {
//struct for storing control specific parameters
motorctl_config_t config;
motorstate_t state = motorstate_t::IDLE;
//handle for using the nvs flash (persistent config variables)
nvs_handle_t * nvsHandle;
float currentMax;
float currentNow;

1
common/types.hpp
View File

@ -41,6 +41,7 @@ typedef struct motorCommands_t {
//struct with all config parameters for a motor regarding ramp and current limit
typedef struct motorctl_config_t {
char * name; //name for unique nvs storage-key prefix and logging
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;