From 71b63ebbd3a50ecaf79b39a8019c0e7bd216efec Mon Sep 17 00:00:00 2001 From: jonny_jr9 Date: Fri, 8 Sep 2023 12:03:33 +0200 Subject: [PATCH] Make drivers interchangeable, Switch to sabertooth driver - make motorctl compatible with different drivers - pass set function instead of specific motor object - add lambda function in config.cpp - update config to use one new sabertooth driver instead of two single100a - main test controlled motor --- board_single/main/config.cpp | 27 ++- board_single/main/main.cpp | 312 +++++++++++++++++------------------ common/motorctl.cpp | 9 +- common/motorctl.hpp | 9 +- common/motordrivers.cpp | 6 +- common/motordrivers.hpp | 6 +- 6 files changed, 194 insertions(+), 175 deletions(-) diff --git a/board_single/main/config.cpp b/board_single/main/config.cpp index 36593f1..4449f9a 100644 --- a/board_single/main/config.cpp +++ b/board_single/main/config.cpp @@ -3,6 +3,7 @@ //=================================== //======= motor configuration ======= //=================================== +/* ==> currently using other driver //--- configure left motor (hardware) --- single100a_config_t configDriverLeft = { .gpio_pwm = GPIO_NUM_26, @@ -27,6 +28,13 @@ single100a_config_t configDriverRight = { .bEnabledPinState = true, //-> not inverted (direct) .resolution = LEDC_TIMER_11_BIT, .pwmFreq = 10000 + }; + */ + +//--- configure sabertooth driver --- (controls both motors in one instance) +sabertooth2x60_config_t sabertoothConfig = { + .gpio_TX = GPIO_NUM_23, + .uart_num = UART_NUM_2 }; @@ -125,10 +133,24 @@ fan_config_t configCooling = { //===== create global objects ===== //================================= //TODO outsource global variables to e.g. global.cpp and only config options here? +//create sabertooth motor driver instance +sabertooth2x60a sabertoothDriver(sabertoothConfig); + +//--- controlledMotor --- +//functions for updating the duty via certain/current driver that can then be passed to controlledMotor +//-> makes it possible to easily use different motor drivers +//note: ignoring warning "capture of variable 'sabertoothDriver' with non-automatic storage duration", since sabertoothDriver object does not get destroyed anywhere - no lifetime issue +motorSetCommandFunc_t setLeftFunc = [&sabertoothDriver](motorCommand_t cmd) { + sabertoothDriver.setLeft(cmd); +}; +motorSetCommandFunc_t setRightFunc = [&sabertoothDriver](motorCommand_t cmd) { + sabertoothDriver.setRight(cmd); +}; //create controlled motor instances (motorctl.hpp) -controlledMotor motorLeft(configDriverLeft, configMotorControlLeft); -controlledMotor motorRight(configDriverRight, configMotorControlRight); +controlledMotor motorLeft(setLeftFunc, configMotorControlLeft); +controlledMotor motorRight(setRightFunc, configMotorControlRight); + //create global joystic instance (joystick.hpp) evaluatedJoystick joystick(configJoystick); @@ -149,3 +171,4 @@ controlledArmchair control(configControl, &buzzer, &motorLeft, &motorRight, &joy automatedArmchair armchair; + diff --git a/board_single/main/main.cpp b/board_single/main/main.cpp index d69407f..f56f7cd 100644 --- a/board_single/main/main.cpp +++ b/board_single/main/main.cpp @@ -164,174 +164,162 @@ void sendByte(char data){ //=========== app_main ============ //================================= extern "C" void app_main(void) { - //TEST SABERTOOTH driver: - //config - sabertooth2x60_config_t sabertoothConfig = { - .gpio_TX = GPIO_NUM_23, - .uart_num = UART_NUM_2 - }; + //enable 5V volate regulator + gpio_pad_select_gpio(GPIO_NUM_17); + gpio_set_direction(GPIO_NUM_17, GPIO_MODE_OUTPUT); + gpio_set_level(GPIO_NUM_17, 1); - //create instance - sabertooth2x60a motors(sabertoothConfig); + //---- define log levels ---- + setLoglevels(); + + //---------------------------------------------- + //--- create task for controlling the motors --- + //---------------------------------------------- + //task that receives commands, handles ramp and current limit and executes commands using the motordriver function + xTaskCreate(&task_motorctl, "task_motor-control", 2048, NULL, 6, NULL); + +// //------------------------------ +// //--- create task for buzzer --- +// //------------------------------ +// xTaskCreate(&task_buzzer, "task_buzzer", 2048, NULL, 2, NULL); +// +// //------------------------------- +// //--- create task for control --- +// //------------------------------- +// //task that generates motor commands depending on the current mode and sends those to motorctl task +// xTaskCreate(&task_control, "task_control", 4096, NULL, 5, NULL); +// +// //------------------------------ +// //--- create task for button --- +// //------------------------------ +// //task that evaluates and processes the button input and runs the configured commands +// xTaskCreate(&task_button, "task_button", 4096, NULL, 4, NULL); +// +// //----------------------------------- +// //--- create task for fan control --- +// //----------------------------------- +// //task that evaluates and processes the button input and runs the configured commands +// xTaskCreate(&task_fans, "task_fans", 2048, NULL, 1, NULL); +// +// +// //beep at startup +// buzzer.beep(3, 70, 50); +// +// //--- initialize nvs-flash and netif (needed for wifi) --- +// wifi_initNvs_initNetif(); +// +// //--- initialize spiffs --- +// init_spiffs(); + + //--- initialize and start wifi --- + //FIXME: run wifi_init_client or wifi_init_ap as intended from control.cpp when switching state + //currently commented out because of error "assert failed: xQueueSemaphoreTake queue.c:1549 (pxQueue->uxItemSize == 0)" when calling control->changeMode from button.cpp + //when calling control.changeMode(http) from main.cpp it worked without error for some reason? + ESP_LOGI(TAG,"starting wifi..."); + //wifi_init_client(); //connect to existing wifi + wifi_init_ap(); //start access point + ESP_LOGI(TAG,"done starting wifi"); - //run test commands + //--- testing http server --- + // wifi_init_client(); //connect to existing wifi + // vTaskDelay(2000 / portTICK_PERIOD_MS); + // ESP_LOGI(TAG, "initializing http server"); + // http_init_server(); + + + //--- testing force http mode after startup --- + //control.changeMode(controlMode_t::HTTP); + + + //--- main loop --- + //does nothing except for testing things while(1){ - motors.setLeft({motorstate_t::FWD, 70}); + //test sabertooth driver + // motors.setLeft({motorstate_t::FWD, 70}); + // vTaskDelay(1000 / portTICK_PERIOD_MS); + // motors.setLeft({motorstate_t::IDLE, 0}); + // vTaskDelay(1000 / portTICK_PERIOD_MS); + // motors.setLeft({motorstate_t::REV, 50}); + // vTaskDelay(1000 / portTICK_PERIOD_MS); + // motors.setLeft(-90); + // vTaskDelay(1000 / portTICK_PERIOD_MS); + // motors.setLeft(90); + // vTaskDelay(1000 / portTICK_PERIOD_MS); + // motors.setLeft(0); + // vTaskDelay(1000 / portTICK_PERIOD_MS); + + + //--- test controlledMotor --- (ramp) + // //brake for 1 s + //motorLeft.setTarget(motorstate_t::BRAKE); + //vTaskDelay(1000 / portTICK_PERIOD_MS); + //command 90% - reverse + motorLeft.setTarget(motorstate_t::REV, 90); + vTaskDelay(5000 / portTICK_PERIOD_MS); + motorLeft.setTarget(motorstate_t::FWD, 80); vTaskDelay(1000 / portTICK_PERIOD_MS); - motors.setLeft({motorstate_t::IDLE, 0}); - vTaskDelay(1000 / portTICK_PERIOD_MS); - motors.setLeft({motorstate_t::REV, 50}); - vTaskDelay(1000 / portTICK_PERIOD_MS); - motors.setLeft(-90); - vTaskDelay(1000 / portTICK_PERIOD_MS); - motors.setLeft(90); - vTaskDelay(1000 / portTICK_PERIOD_MS); - motors.setLeft(0); + motorLeft.setTarget(motorstate_t::IDLE, 90); vTaskDelay(1000 / portTICK_PERIOD_MS); + + + //--------------------------------- + //-------- TESTING section -------- + //--------------------------------- + // //--- test functions at mode change HTTP --- + // control.changeMode(controlMode_t::HTTP); + // vTaskDelay(10000 / portTICK_PERIOD_MS); + // control.changeMode(controlMode_t::IDLE); + // vTaskDelay(10000 / portTICK_PERIOD_MS); + + + //--- test wifi functions --- + // ESP_LOGI(TAG, "creating AP"); + // wifi_init_ap(); //start accesspoint + // vTaskDelay(15000 / portTICK_PERIOD_MS); + // ESP_LOGI(TAG, "stopping wifi"); + // wifi_deinit_ap(); //stop wifi access point + // vTaskDelay(5000 / portTICK_PERIOD_MS); + // ESP_LOGI(TAG, "connecting to wifi"); + // wifi_init_client(); //connect to existing wifi + // vTaskDelay(10000 / portTICK_PERIOD_MS); + // ESP_LOGI(TAG, "stopping wifi"); + // wifi_deinit_client(); //stop wifi client + // vTaskDelay(5000 / portTICK_PERIOD_MS); + + + //--- test button --- + //buttonJoystick.handle(); + // if (buttonJoystick.risingEdge){ + // ESP_LOGI(TAG, "button pressed, was released for %d ms", buttonJoystick.msReleased); + // buzzer.beep(2, 100, 50); + + // }else if (buttonJoystick.fallingEdge){ + // ESP_LOGI(TAG, "button released, was pressed for %d ms", buttonJoystick.msPressed); + // buzzer.beep(1, 200, 0); + // } + + + //--- test joystick commands --- + // motorCommands_t commands = joystick_generateCommandsDriving(joystick); + // motorRight.setTarget(commands.right.state, commands.right.duty); //TODO make motorctl.setTarget also accept motorcommand struct directly + // motorLeft.setTarget(commands.left.state, commands.left.duty); //TODO make motorctl.setTarget also accept motorcommand struct directly + // //motorRight.setTarget(commands.right.state, commands.right.duty); + + + //--- test joystick class --- + //joystickData_t data = joystick.getData(); + //ESP_LOGI(TAG, "position=%s, x=%.1f%%, y=%.1f%%, radius=%.1f%%, angle=%.2f", + // joystickPosStr[(int)data.position], data.x*100, data.y*100, data.radius*100, data.angle); + + //--- test the motor driver --- + //fade up duty - forward + // for (int duty=0; duty<=100; duty+=5) { + // motorLeft.setTarget(motorstate_t::FWD, duty); + // vTaskDelay(100 / portTICK_PERIOD_MS); + // } + + } -// //enable 5V volate regulator -// gpio_pad_select_gpio(GPIO_NUM_17); -// gpio_set_direction(GPIO_NUM_17, GPIO_MODE_OUTPUT); -// gpio_set_level(GPIO_NUM_17, 1); -// -// //---- define log levels ---- -// setLoglevels(); -// -// //---------------------------------------------- -// //--- create task for controlling the motors --- -// //---------------------------------------------- -// //task that receives commands, handles ramp and current limit and executes commands using the motordriver function -// xTaskCreate(&task_motorctl, "task_motor-control", 2048, NULL, 6, NULL); -// -// //------------------------------ -// //--- create task for buzzer --- -// //------------------------------ -// xTaskCreate(&task_buzzer, "task_buzzer", 2048, NULL, 2, NULL); -// -// //------------------------------- -// //--- create task for control --- -// //------------------------------- -// //task that generates motor commands depending on the current mode and sends those to motorctl task -// xTaskCreate(&task_control, "task_control", 4096, NULL, 5, NULL); -// -// //------------------------------ -// //--- create task for button --- -// //------------------------------ -// //task that evaluates and processes the button input and runs the configured commands -// xTaskCreate(&task_button, "task_button", 4096, NULL, 4, NULL); -// -// //----------------------------------- -// //--- create task for fan control --- -// //----------------------------------- -// //task that evaluates and processes the button input and runs the configured commands -// xTaskCreate(&task_fans, "task_fans", 2048, NULL, 1, NULL); -// -// -// //beep at startup -// buzzer.beep(3, 70, 50); -// -// //--- initialize nvs-flash and netif (needed for wifi) --- -// wifi_initNvs_initNetif(); -// -// //--- initialize spiffs --- -// init_spiffs(); -// -// //--- initialize and start wifi --- -// //FIXME: run wifi_init_client or wifi_init_ap as intended from control.cpp when switching state -// //currently commented out because of error "assert failed: xQueueSemaphoreTake queue.c:1549 (pxQueue->uxItemSize == 0)" when calling control->changeMode from button.cpp -// //when calling control.changeMode(http) from main.cpp it worked without error for some reason? -// ESP_LOGI(TAG,"starting wifi..."); -// //wifi_init_client(); //connect to existing wifi -// wifi_init_ap(); //start access point -// ESP_LOGI(TAG,"done starting wifi"); -// -// -// //--- testing http server --- -// // wifi_init_client(); //connect to existing wifi -// // vTaskDelay(2000 / portTICK_PERIOD_MS); -// // ESP_LOGI(TAG, "initializing http server"); -// // http_init_server(); -// -// -// //--- testing force http mode after startup --- -// //control.changeMode(controlMode_t::HTTP); -// -// -// //--- main loop --- -// //does nothing except for testing things -// while(1){ -// vTaskDelay(1000 / portTICK_PERIOD_MS); -// -// //--------------------------------- -// //-------- TESTING section -------- -// //--------------------------------- -// // //--- test functions at mode change HTTP --- -// // control.changeMode(controlMode_t::HTTP); -// // vTaskDelay(10000 / portTICK_PERIOD_MS); -// // control.changeMode(controlMode_t::IDLE); -// // vTaskDelay(10000 / portTICK_PERIOD_MS); -// -// -// //--- test wifi functions --- -// // ESP_LOGI(TAG, "creating AP"); -// // wifi_init_ap(); //start accesspoint -// // vTaskDelay(15000 / portTICK_PERIOD_MS); -// // ESP_LOGI(TAG, "stopping wifi"); -// // wifi_deinit_ap(); //stop wifi access point -// // vTaskDelay(5000 / portTICK_PERIOD_MS); -// // ESP_LOGI(TAG, "connecting to wifi"); -// // wifi_init_client(); //connect to existing wifi -// // vTaskDelay(10000 / portTICK_PERIOD_MS); -// // ESP_LOGI(TAG, "stopping wifi"); -// // wifi_deinit_client(); //stop wifi client -// // vTaskDelay(5000 / portTICK_PERIOD_MS); -// -// -// //--- test button --- -// //buttonJoystick.handle(); -// // if (buttonJoystick.risingEdge){ -// // ESP_LOGI(TAG, "button pressed, was released for %d ms", buttonJoystick.msReleased); -// // buzzer.beep(2, 100, 50); -// -// // }else if (buttonJoystick.fallingEdge){ -// // ESP_LOGI(TAG, "button released, was pressed for %d ms", buttonJoystick.msPressed); -// // buzzer.beep(1, 200, 0); -// // } -// -// -// //--- test joystick commands --- -// // motorCommands_t commands = joystick_generateCommandsDriving(joystick); -// // motorRight.setTarget(commands.right.state, commands.right.duty); //TODO make motorctl.setTarget also accept motorcommand struct directly -// // motorLeft.setTarget(commands.left.state, commands.left.duty); //TODO make motorctl.setTarget also accept motorcommand struct directly -// // //motorRight.setTarget(commands.right.state, commands.right.duty); -// -// -// //--- test joystick class --- -// //joystickData_t data = joystick.getData(); -// //ESP_LOGI(TAG, "position=%s, x=%.1f%%, y=%.1f%%, radius=%.1f%%, angle=%.2f", -// // joystickPosStr[(int)data.position], data.x*100, data.y*100, data.radius*100, data.angle); -// -// //--- test the motor driver --- -// //fade up duty - forward -// // for (int duty=0; duty<=100; duty+=5) { -// // motorLeft.setTarget(motorstate_t::FWD, duty); -// // vTaskDelay(100 / portTICK_PERIOD_MS); -// // } -// -// -// //--- test controlledMotor --- (ramp) -// // //brake for 1 s -// // motorLeft.setTarget(motorstate_t::BRAKE); -// // vTaskDelay(1000 / portTICK_PERIOD_MS); -// // //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); -// -// } - } diff --git a/common/motorctl.cpp b/common/motorctl.cpp index b560ad4..fdfdd7f 100644 --- a/common/motorctl.cpp +++ b/common/motorctl.cpp @@ -11,11 +11,12 @@ static const char * TAG = "motor-control"; //======== constructor ======== //============================= //constructor, simultaniously initialize instance of motor driver 'motor' and current sensor 'cSensor' with provided config (see below lines after ':') -controlledMotor::controlledMotor(single100a_config_t config_driver, motorctl_config_t config_control): - motor(config_driver), +controlledMotor::controlledMotor(motorSetCommandFunc_t setCommandFunc, motorctl_config_t config_control): 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; @@ -139,7 +140,7 @@ void controlledMotor::handle(){ //brake immediately, update state, duty and exit this cycle of handle function if (state == motorstate_t::BRAKE){ ESP_LOGD(TAG, "braking - skip fading"); - motor.set(motorstate_t::BRAKE, dutyTarget); + motorSetCommand({motorstate_t::BRAKE, dutyTarget}); //dutyNow = 0; return; //no need to run the fade algorithm } @@ -229,7 +230,7 @@ void controlledMotor::handle(){ //--- apply new target to motor --- - motor.set(state, fabs(dutyNow)); + motorSetCommand({state, (float)fabs(dutyNow)}); //note: BRAKE state is handled earlier diff --git a/common/motorctl.hpp b/common/motorctl.hpp index 32018be..bea2060 100644 --- a/common/motorctl.hpp +++ b/common/motorctl.hpp @@ -19,7 +19,7 @@ extern "C" //outsourced to common/types.hpp #include "types.hpp" - +typedef void (*motorSetCommandFunc_t)(motorCommand_t cmd); //=================================== @@ -28,7 +28,7 @@ extern "C" 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 + controlledMotor(motorSetCommandFunc_t setCommandFunc, 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 motorCommand_t getStatus(); //get current status of the motor (returns struct with state and duty) @@ -44,13 +44,14 @@ class controlledMotor { void init(); //creates currentsensor objects, motordriver objects and queue //--- objects --- - //motor driver - single100a motor; //queue for sending commands to the separate task running the handle() function very fast QueueHandle_t commandQueue = NULL; //current sensor currentSensor cSensor; + //function pointer that sets motor duty (driver) + motorSetCommandFunc_t motorSetCommand; + //--- variables --- //struct for storing control specific parameters motorctl_config_t config; diff --git a/common/motordrivers.cpp b/common/motordrivers.cpp index aaf8549..3392c76 100644 --- a/common/motordrivers.cpp +++ b/common/motordrivers.cpp @@ -80,6 +80,10 @@ void single100a::init(){ //----------- set ----------- //--------------------------- //function to put the h-bridge module in the desired state and duty cycle +void single100a::set(motorCommand_t cmd){ + set(cmd.state, cmd.duty); +} + void single100a::set(motorstate_t state_f, float duty_f){ //scale provided target duty in percent to available resolution for ledc @@ -190,7 +194,7 @@ sabertooth2x60a::sabertooth2x60a(sabertooth2x60_config_t config_f){ void sabertooth2x60a::init(){ ESP_LOGW(TAG, "initializing uart..."); uart_config_t uart_config = { - .baud_rate = 38400, + .baud_rate = 9600, //dip switches: 101011 .data_bits = UART_DATA_8_BITS, .parity = UART_PARITY_DISABLE, .stop_bits = UART_STOP_BITS_1, diff --git a/common/motordrivers.hpp b/common/motordrivers.hpp index c4efca8..894013d 100644 --- a/common/motordrivers.hpp +++ b/common/motordrivers.hpp @@ -51,6 +51,7 @@ class single100a { //--- functions --- void set(motorstate_t state, float duty_f = 0); //set mode and duty of the motor (see motorstate_t above) + void set(motorCommand_t cmd); //TODO: add functions to get the current state and duty private: @@ -79,18 +80,19 @@ class single100a { //===== sabertooth 2x60A motor driver ====== //========================================== -//struct with all config parameters for single100a motor driver +//struct with all config parameters for sabertooth2x60a driver typedef struct { gpio_num_t gpio_TX; uart_port_t uart_num; //(UART_NUM_1/2) } sabertooth2x60_config_t; +//controll via simplified serial +//=> set dip switches to 'simplified serial 9600 Baud' according to manual 101011 class sabertooth2x60a { public: //--- constructor --- sabertooth2x60a(sabertooth2x60_config_t config_f); //provide config struct (see above) - //--- functions --- //set motor speed with float value from -100 to 100 void setLeft(float dutyPerSigned);