From 84bfe211accd68e48793d59b995556e0cd18cc40 Mon Sep 17 00:00:00 2001 From: jonny_ji7 Date: Sun, 5 Jun 2022 16:36:57 +0200 Subject: [PATCH] Create class 'controlledMotor', Create config - Create class 'controlledMotor': - handles 'fading / ramp' of the pwm duty - handles current limit **not implemented yet** - has .handle function that is intended to be run very fast in another task commands are sent via queue - Create config.hpp - Globally available instance motorLeft of controlledMotor class - Create config.cpp - Configuration of motordriver and control parameters for motorLeft - Add config.cpp and motorctl.cpp to cmakelists - main.cpp: - create 'task_motorctl' which repeatedly runs motorLeft.handle() - modify testing code for testing the new class - comments The fading/ramp capability of the new class was tested successfully using a breakoutboard with an led. --- main/CMakeLists.txt | 2 +- main/config.cpp | 22 ++++++++++ main/config.hpp | 7 ++++ main/main.cpp | 74 ++++++++++++++++++++++------------ main/motorctl.cpp | 97 +++++++++++++++++++++++++++++++++++++++++++++ main/motorctl.hpp | 70 ++++++++++++++++++++++++++++++++ 6 files changed, 246 insertions(+), 26 deletions(-) create mode 100644 main/config.cpp create mode 100644 main/config.hpp create mode 100644 main/motorctl.cpp create mode 100644 main/motorctl.hpp diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index f547649..5f23fe3 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -1,2 +1,2 @@ -idf_component_register(SRCS "main.cpp" "motordrivers.cpp" +idf_component_register(SRCS "main.cpp" "motordrivers.cpp" "motorctl.cpp" "config.cpp" INCLUDE_DIRS ".") diff --git a/main/config.cpp b/main/config.cpp new file mode 100644 index 0000000..186617d --- /dev/null +++ b/main/config.cpp @@ -0,0 +1,22 @@ +#include "config.hpp" + + +//configure motor driver +single100a_config_t configDriverLeft = { + .gpio_pwm = GPIO_NUM_14, + .gpio_a = GPIO_NUM_12, + .gpio_b = GPIO_NUM_13, + .ledc_timer = LEDC_TIMER_0, + .ledc_channel = LEDC_CHANNEL_0, + .abInverted = true, + .resolution = LEDC_TIMER_11_BIT, + .pwmFreq = 10000 +}; + +motorctl_config_t configControlLeft = { + .msFade = 5000, + .currentMax = 10 +}; + +//create controlled motor +controlledMotor motorLeft(configDriverLeft, configControlLeft); diff --git a/main/config.hpp b/main/config.hpp new file mode 100644 index 0000000..fdfaf5c --- /dev/null +++ b/main/config.hpp @@ -0,0 +1,7 @@ +#pragma once + +#include "motordrivers.hpp" +#include "motorctl.hpp" + +//create global controlledMotor instances for both motors +extern controlledMotor motorLeft; diff --git a/main/main.cpp b/main/main.cpp index d7112e2..ec19c3f 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -15,48 +15,72 @@ extern "C" } #include "motordrivers.hpp" +#include "motorctl.hpp" +#include "config.hpp" + +//tag for logging +static const char * TAG = "main"; + +//==================================== +//========== motorctl task =========== +//==================================== +//task for handling the motors (ramp, current limit, driver) +void task_motorctl( void * pvParameters ){ + ESP_LOGI("motorctl-task", "starting handle loop..."); + while(1){ + motorLeft.handle(); + //10khz -> T=100us + vTaskDelay(10 / portTICK_PERIOD_MS); + } +} + + + +//================================= +//=========== app_main ============ +//================================= extern "C" void app_main(void) { + //------------------------------- + //---------- log level ---------- + //------------------------------- //set loglevel for all tags: esp_log_level_set("*", ESP_LOG_INFO); - //set loglevel for motordriver to DEBUG for testing - esp_log_level_set("motordriver", ESP_LOG_DEBUG); - //configure motor driver - single100a_config_t configDriverLeft = { - .gpio_pwm = GPIO_NUM_14, - .gpio_a = GPIO_NUM_12, - .gpio_b = GPIO_NUM_13, - .ledc_timer = LEDC_TIMER_0, - .ledc_channel = LEDC_CHANNEL_0, - .abInverted = true, - .resolution = LEDC_TIMER_11_BIT, - .pwmFreq = 10000 - }; + //set loglevel for individual tags: + //esp_log_level_set("motordriver", ESP_LOG_DEBUG); + esp_log_level_set("motor-control", ESP_LOG_DEBUG); - //create driver instance for motor left - single100a motorLeft(configDriverLeft); + + + //---------------------------------------------- + //--- create task for controlling the motors --- + //---------------------------------------------- + xTaskCreate(&task_motorctl, "task_motor-control", 2048, NULL, 5, NULL); while(1){ //--- testing the motor driver --- //fade up duty - forward - for (int duty=0; duty<=100; duty+=5) { - motorLeft.set(motorstate_t::FWD, duty); - vTaskDelay(100 / portTICK_PERIOD_MS); - } + // for (int duty=0; duty<=100; duty+=5) { + // motorLeft.setTarget(motorstate_t::FWD, duty); + // vTaskDelay(100 / portTICK_PERIOD_MS); + // } + // + //--- testing controlledMotor --- (ramp) //brake for 1 s - motorLeft.set(motorstate_t::BRAKE); + motorLeft.setTarget(motorstate_t::BRAKE); vTaskDelay(1000 / portTICK_PERIOD_MS); - //stay at 100% - reverse - motorLeft.set(motorstate_t::REV, 150); + //command 90% - reverse + motorLeft.setTarget(motorstate_t::REV, 90); + vTaskDelay(5000 / portTICK_PERIOD_MS); + //command 100% - forward + motorLeft.setTarget(motorstate_t::FWD, 100); vTaskDelay(1000 / portTICK_PERIOD_MS); - //stay at idle (default duty) - motorLeft.set(motorstate_t::IDLE); - vTaskDelay(2000 / portTICK_PERIOD_MS); } + } diff --git a/main/motorctl.cpp b/main/motorctl.cpp new file mode 100644 index 0000000..7ca779f --- /dev/null +++ b/main/motorctl.cpp @@ -0,0 +1,97 @@ +#include "motorctl.hpp" + +//tag for logging +static const char * TAG = "motor-control"; + + +//============================= +//======== constructor ======== +//============================= +//constructor, simultaniously initialize instance of motor driver 'motor' with provided config (see below line after ':') +controlledMotor::controlledMotor(single100a_config_t config_driver, motorctl_config_t config_control): motor(config_driver) { + //copy parameters for controlling the motor + config = config_control; + + init(); + //TODO: add currentsensor object here +} + + + +//============================ +//========== init ============ +//============================ +void controlledMotor::init(){ + commandQueue = xQueueCreate( 1, sizeof( struct motorCommand_t ) ); +} + + + +//============================== +//=========== handle =========== +//============================== +//function that controls the motor driver and handles fading/ramp and current limit +void controlledMotor::handle(){ + + //TODO: current sensor + //TODO: delay when switching direction? + //TODO: History: skip fading when motor was running fast recently + + //--- 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); + state = commandReceive.state; + dutyTarget = commandReceive.duty; + } + + //--- calculate increment --- + //calculate increment 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 + + //--- 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 + } + + //--- apply to motor --- + //apply target duty and state to motor driver + motor.set(state, dutyNow); + + //--- update timestamp --- + timestampLastRunUs = esp_timer_get_time(); //update timestamp last run with current timestamp in microseconds + +} + + + +//=============================== +//========== setTarget ========== +//=============================== +//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 +void controlledMotor::setTarget(motorstate_t state_f, float duty_f){ + commandSend = { + .state = state_f, + .duty = duty_f + }; + + ESP_LOGD(TAG, "Inserted command to queue: state=%s, duty=%.2f", motorstateStr[(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 ); + +} + diff --git a/main/motorctl.hpp b/main/motorctl.hpp new file mode 100644 index 0000000..6cdd7d6 --- /dev/null +++ b/main/motorctl.hpp @@ -0,0 +1,70 @@ +#pragma once + +extern "C" +{ +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "freertos/queue.h" +#include "esp_log.h" +#include "esp_timer.h" +} + +#include "motordrivers.hpp" + + + + +typedef struct motorctl_config_t { + uint32_t msFade; //acceleration of the motor (ms it should take from 0 to 100%) + float currentMax; +} motorctl_config_t; + + + +class controlledMotor { + public: + //--- functions --- + controlledMotor(single100a_config_t config_driver, motorctl_config_t config_control); //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 + + + private: + //--- functions --- + void init(); //creates currentsensor objects, motordriver objects and queue + + //--- objects --- + //TODO: add currentsensor object + //motor driver + single100a motor; + //queue for sending commands to the separate task running the handle() function very fast + QueueHandle_t commandQueue = NULL; + + //--- variables --- + //struct for storing control specific parameters + motorctl_config_t config; + + motorstate_t state = motorstate_t::IDLE; + + float currentMax; + float currentNow; + + float dutyTarget; + float dutyNow; + float dutyIncrement; + float dutyDelta; + + uint32_t ramp; + int64_t timestampLastRunUs; + + //structs for sending commands in the queue + struct motorCommand_t { + motorstate_t state; + float duty; + }; + struct motorCommand_t commandReceive = {}; + struct motorCommand_t commandSend = {}; + + + +};