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

Create class sabertooth2x60a - new driver functional

Browse Source 
currently repeatedly sends test commands from main using a created
instance of that new class
This commit is contained in:
jonny_jr9 2023-09-07 16:37:14 +02:00
parent a30ec01818
commit 98956e2bf8
3 changed files with 334 additions and 152 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
board_single/main/main.cpp
View File

@ -1,3 +1,6 @@
#include "hal/uart_types.h"
#include "motordrivers.hpp"
#include "types.hpp"
extern "C" extern "C"
{ {
#include <stdio.h> #include <stdio.h>
@ -161,37 +164,31 @@ void sendByte(char data){
//=========== app_main ============ //=========== app_main ============
//================================= //=================================
extern "C" void app_main(void) { extern "C" void app_main(void) {
//TEST SAVERTOOTH driver: //TEST SABERTOOTH driver:
//init uart: //config
sabertooth2x60_config_t sabertoothConfig = {
ESP_LOGW(TAG, "initializing uart1..."); .gpio_TX = GPIO_NUM_23,
uart_config_t uart1_config = { .uart_num = UART_NUM_2
.baud_rate = 38400,
.data_bits = UART_DATA_8_BITS,
.parity = UART_PARITY_DISABLE,
.stop_bits = UART_STOP_BITS_1,
.flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
}; };
ESP_LOGI(TAG, "configure...");
ESP_ERROR_CHECK(uart_param_config(UART_NUM_1, &uart1_config));
ESP_LOGI(TAG, "setpins...");
ESP_ERROR_CHECK(uart_set_pin(UART_NUM_1, 23, 22, 0, 0));
ESP_LOGI(TAG, "init...");
ESP_ERROR_CHECK(uart_driver_install(UART_NUM_1, 1024, 1024, 10, NULL, 0));
bool uart_isInitialized = true;
//between 1 and 127 will control motor 1. 1 is full reverse, 64 is stop and 127 is full forward. //create instance
//between 128 and 255 will control motor 2. 128 is full reverse, 192 is stop and 255 is full forward. sabertooth2x60a motors(sabertoothConfig);
//Character 0 (hex 0x00) shut down both motors.
//run test commands
while(1){ while(1){
sendByte(0); motors.setLeft({motorstate_t::FWD, 70});
vTaskDelay(1000 / portTICK_PERIOD_MS); vTaskDelay(1000 / portTICK_PERIOD_MS);
sendByte(10); motors.setLeft({motorstate_t::IDLE, 0});
vTaskDelay(2000 / portTICK_PERIOD_MS); vTaskDelay(1000 / portTICK_PERIOD_MS);
sendByte(64); 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); vTaskDelay(1000 / portTICK_PERIOD_MS);
sendByte(120);
vTaskDelay(2000 / portTICK_PERIOD_MS);
} }
// //enable 5V volate regulator // //enable 5V volate regulator

292
common/motordrivers.cpp
View File

@ -25,8 +25,8 @@ static const char * TAG = "motordriver";
//----------------------------- //-----------------------------
//copy provided struct with all configuration and run init function //copy provided struct with all configuration and run init function
single100a::single100a(single100a_config_t config_f){ single100a::single100a(single100a_config_t config_f){
config = config_f; config = config_f;
init(); init();
} }
@ -37,41 +37,41 @@ single100a::single100a(single100a_config_t config_f){
//function to initialize pwm output, gpio pins and calculate maxDuty //function to initialize pwm output, gpio pins and calculate maxDuty
void single100a::init(){ void single100a::init(){
//--- configure ledc timer --- //--- configure ledc timer ---
ledc_timer_config_t ledc_timer; ledc_timer_config_t ledc_timer;
ledc_timer.speed_mode = LEDC_HIGH_SPEED_MODE; ledc_timer.speed_mode = LEDC_HIGH_SPEED_MODE;
ledc_timer.timer_num = config.ledc_timer; ledc_timer.timer_num = config.ledc_timer;
ledc_timer.duty_resolution = config.resolution; //13bit gives max 5khz freq ledc_timer.duty_resolution = config.resolution; //13bit gives max 5khz freq
ledc_timer.freq_hz = config.pwmFreq; ledc_timer.freq_hz = config.pwmFreq;
ledc_timer.clk_cfg = LEDC_AUTO_CLK; ledc_timer.clk_cfg = LEDC_AUTO_CLK;
//apply configuration //apply configuration
ledc_timer_config(&ledc_timer); ledc_timer_config(&ledc_timer);
//--- configure ledc channel --- //--- configure ledc channel ---
ledc_channel_config_t ledc_channel; ledc_channel_config_t ledc_channel;
ledc_channel.channel = config.ledc_channel; ledc_channel.channel = config.ledc_channel;
ledc_channel.duty = 0; ledc_channel.duty = 0;
ledc_channel.gpio_num = config.gpio_pwm; ledc_channel.gpio_num = config.gpio_pwm;
ledc_channel.speed_mode = LEDC_HIGH_SPEED_MODE; ledc_channel.speed_mode = LEDC_HIGH_SPEED_MODE;
ledc_channel.hpoint = 0; ledc_channel.hpoint = 0;
ledc_channel.timer_sel = config.ledc_timer; ledc_channel.timer_sel = config.ledc_timer;
ledc_channel.intr_type = LEDC_INTR_DISABLE; ledc_channel.intr_type = LEDC_INTR_DISABLE;
ledc_channel.flags.output_invert = 0; //TODO: add config option to invert the pwm output? ledc_channel.flags.output_invert = 0; //TODO: add config option to invert the pwm output?
//apply configuration //apply configuration
ledc_channel_config(&ledc_channel); ledc_channel_config(&ledc_channel);
//--- define gpio pins as outputs --- //--- define gpio pins as outputs ---
gpio_pad_select_gpio(config.gpio_a); gpio_pad_select_gpio(config.gpio_a);
gpio_set_direction(config.gpio_a, GPIO_MODE_OUTPUT); gpio_set_direction(config.gpio_a, GPIO_MODE_OUTPUT);
gpio_pad_select_gpio(config.gpio_b); gpio_pad_select_gpio(config.gpio_b);
gpio_set_direction(config.gpio_b, GPIO_MODE_OUTPUT); gpio_set_direction(config.gpio_b, GPIO_MODE_OUTPUT);
gpio_pad_select_gpio(config.gpio_brakeRelay); gpio_pad_select_gpio(config.gpio_brakeRelay);
gpio_set_direction(config.gpio_brakeRelay, GPIO_MODE_OUTPUT); gpio_set_direction(config.gpio_brakeRelay, GPIO_MODE_OUTPUT);
//--- calculate max duty according to selected resolution --- //--- calculate max duty according to selected resolution ---
dutyMax = pow(2, ledc_timer.duty_resolution) -1; dutyMax = pow(2, ledc_timer.duty_resolution) -1;
ESP_LOGW(TAG, "initialized single100a driver"); ESP_LOGW(TAG, "initialized single100a driver");
ESP_LOGW(TAG, "resolution=%dbit, dutyMax value=%d, resolution=%.4f %%", ledc_timer.duty_resolution, dutyMax, 100/(float)dutyMax); ESP_LOGW(TAG, "resolution=%dbit, dutyMax value=%d, resolution=%.4f %%", ledc_timer.duty_resolution, dutyMax, 100/(float)dutyMax);
} }
@ -82,19 +82,19 @@ void single100a::init(){
//function to put the h-bridge module in the desired state and duty cycle //function to put the h-bridge module in the desired state and duty cycle
void single100a::set(motorstate_t state_f, float duty_f){ void single100a::set(motorstate_t state_f, float duty_f){
//scale provided target duty in percent to available resolution for ledc //scale provided target duty in percent to available resolution for ledc
uint32_t dutyScaled; uint32_t dutyScaled;
if (duty_f > 100) { //target duty above 100% if (duty_f > 100) { //target duty above 100%
dutyScaled = dutyMax; dutyScaled = dutyMax;
} else if (duty_f < 0) { //target at or below 0% } else if (duty_f < 0) { //target at or below 0%
state_f = motorstate_t::IDLE; state_f = motorstate_t::IDLE;
dutyScaled = 0; dutyScaled = 0;
} else { //target duty 0-100% } else { //target duty 0-100%
//scale duty to available resolution //scale duty to available resolution
dutyScaled = duty_f / 100 * dutyMax; dutyScaled = duty_f / 100 * dutyMax;
} }
ESP_LOGV(TAG, "target-state=%s, duty=%d/%d, duty_input=%.3f%%", motorstateStr[(int)state_f], dutyScaled, dutyMax, duty_f); ESP_LOGV(TAG, "target-state=%s, duty=%d/%d, duty_input=%.3f%%", motorstateStr[(int)state_f], dutyScaled, dutyMax, duty_f);
//TODO: only when previous mode was BRAKE? //TODO: only when previous mode was BRAKE?
if (state_f != motorstate_t::BRAKE){ if (state_f != motorstate_t::BRAKE){
@ -104,19 +104,19 @@ void single100a::set(motorstate_t state_f, float duty_f){
gpio_set_level(config.gpio_brakeRelay, 0); gpio_set_level(config.gpio_brakeRelay, 0);
} }
//put the single100a h-bridge module in the desired state update duty-cycle //put the single100a h-bridge module in the desired state update duty-cycle
//TODO maybe outsource mode code from once switch case? e.g. idle() brake()... //TODO maybe outsource mode code from once switch case? e.g. idle() brake()...
switch (state_f){ switch (state_f){
case motorstate_t::IDLE: case motorstate_t::IDLE:
ledc_set_duty(LEDC_HIGH_SPEED_MODE, config.ledc_channel, dutyScaled); ledc_set_duty(LEDC_HIGH_SPEED_MODE, config.ledc_channel, dutyScaled);
ledc_update_duty(LEDC_HIGH_SPEED_MODE, config.ledc_channel); ledc_update_duty(LEDC_HIGH_SPEED_MODE, config.ledc_channel);
//TODO: to fix bugged state of h-bridge module when idle and start again, maybe try to leave pwm signal on for some time before updating a/b pins? //TODO: to fix bugged state of h-bridge module when idle and start again, maybe try to leave pwm signal on for some time before updating a/b pins?
//no brake: (freewheel) //no brake: (freewheel)
//gpio_set_level(config.gpio_a, config.aEnabledPinState); //gpio_set_level(config.gpio_a, config.aEnabledPinState);
//gpio_set_level(config.gpio_b, !config.bEnabledPinState); //gpio_set_level(config.gpio_b, !config.bEnabledPinState);
gpio_set_level(config.gpio_a, config.aEnabledPinState); gpio_set_level(config.gpio_a, config.aEnabledPinState);
gpio_set_level(config.gpio_b, config.bEnabledPinState); gpio_set_level(config.gpio_b, config.bEnabledPinState);
break; break;
case motorstate_t::BRAKE: case motorstate_t::BRAKE:
//prevent full short (no brake resistors) due to slow relay, also reduces switching load //prevent full short (no brake resistors) due to slow relay, also reduces switching load
@ -148,20 +148,168 @@ void single100a::set(motorstate_t state_f, float duty_f){
} }
break; break;
case motorstate_t::FWD: case motorstate_t::FWD:
ledc_set_duty(LEDC_HIGH_SPEED_MODE, config.ledc_channel, dutyScaled); ledc_set_duty(LEDC_HIGH_SPEED_MODE, config.ledc_channel, dutyScaled);
ledc_update_duty(LEDC_HIGH_SPEED_MODE, config.ledc_channel); ledc_update_duty(LEDC_HIGH_SPEED_MODE, config.ledc_channel);
//forward: //forward:
gpio_set_level(config.gpio_a, config.aEnabledPinState); gpio_set_level(config.gpio_a, config.aEnabledPinState);
gpio_set_level(config.gpio_b, !config.bEnabledPinState); gpio_set_level(config.gpio_b, !config.bEnabledPinState);
break; break;
case motorstate_t::REV: case motorstate_t::REV:
ledc_set_duty(LEDC_HIGH_SPEED_MODE, config.ledc_channel, dutyScaled); ledc_set_duty(LEDC_HIGH_SPEED_MODE, config.ledc_channel, dutyScaled);
ledc_update_duty(LEDC_HIGH_SPEED_MODE, config.ledc_channel); ledc_update_duty(LEDC_HIGH_SPEED_MODE, config.ledc_channel);
//reverse: //reverse:
gpio_set_level(config.gpio_a, !config.aEnabledPinState); gpio_set_level(config.gpio_a, !config.aEnabledPinState);
gpio_set_level(config.gpio_b, config.bEnabledPinState); gpio_set_level(config.gpio_b, config.bEnabledPinState);
break; break;
} }
}
//==========================================
//===== sabertooth 2x60A motor driver ======
//==========================================
//-----------------------------
//-------- constructor --------
//-----------------------------
sabertooth2x60a::sabertooth2x60a(sabertooth2x60_config_t config_f){
config = config_f;
init();
}
//----------------------------
//----------- init -----------
//----------------------------
void sabertooth2x60a::init(){
ESP_LOGW(TAG, "initializing uart...");
uart_config_t uart_config = {
.baud_rate = 38400,
.data_bits = UART_DATA_8_BITS,
.parity = UART_PARITY_DISABLE,
.stop_bits = UART_STOP_BITS_1,
.flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
};
ESP_LOGI(TAG, "configure...");
ESP_ERROR_CHECK(uart_param_config(config.uart_num, &uart_config));
ESP_LOGI(TAG, "setpins...");
ESP_ERROR_CHECK(uart_set_pin(config.uart_num, config.gpio_TX, GPIO_NUM_NC, 0, 0));
ESP_LOGI(TAG, "init...");
ESP_ERROR_CHECK(uart_driver_install(config.uart_num, 1024, 1024, 10, NULL, 0)); //todo ring buffer less / 0
uart_isInitialized = true;
}
//---------- motorCommand to signedDuty ----------
float motorCommandToSignedDuty(motorCommand_t cmd){
//if (cmd.duty > 100) cmd.duty = 100;
//else if (cmd.duty < 0) cmd.duty = 0;
float duty = 0;
switch (cmd.state){
case motorstate_t::FWD:
duty = cmd.duty;
break;
case motorstate_t::REV:
duty = - cmd.duty;
break;
case motorstate_t::IDLE:
case motorstate_t::BRAKE:
duty = 0;
break;
}
return duty;
}
//-------- send byte --------
//send byte via uart to sabertooth driver
void sabertooth2x60a::sendByte(char data){
if (!uart_isInitialized){
ESP_LOGE(TAG, "uart not initialized, not sending command %d...", data);
return;
}
uart_write_bytes(config.uart_num, &data, 1);
ESP_LOGI(TAG, "sent data=%d to sabertooth driver via uart", data);
}
//---------------------------
//--------- setLeft ---------
//---------------------------
//between 1 and 127 will control motor 1. 1 is full reverse, 64 is stop and 127 is full forward.
//Character 0 (hex 0x00) shut down both motors.
void sabertooth2x60a::setLeft(float dutyPerSigned){
uint8_t data = 0;
if (dutyPerSigned <= -100.0) {
//full reverse for motor 1
data = 1;
} else if (dutyPerSigned >= 100) {
//full forward
data = 127;
} else if (dutyPerSigned == 0.0) {
// Stop motor 1
data = 64;
} else if (dutyPerSigned < 0.0) {
//scale negative values between -100 and 0 to 1-63
data = static_cast<int>(64 - (- dutyPerSigned / 100.0) * 63);
} else if (dutyPerSigned <= 100.0) {
//scale positive values between 0 and 100 to control motor 1
data = static_cast<int>(64 + (dutyPerSigned / 100.0) * 63);
}
ESP_LOGI(TAG, "set left motor to duty=%.2f, (data=%d)", dutyPerSigned, data);
sendByte(data);
}
//----------------------------
//--------- setRight ---------
//----------------------------
//between 128 and 255 will control motor 2. 128 is full reverse, 192 is stop and 255 is full forward.
//Character 0 (hex 0x00) shut down both motors.
void sabertooth2x60a::setRight(float dutyPerSigned) {
uint8_t data = 0;
if (dutyPerSigned <= -100.0) {
// Full reverse for motor 2
data = 128;
} else if (dutyPerSigned >= 100.0) {
// Full forward for motor 2
data = 255;
} else if (dutyPerSigned == 0.0) {
// Stop motor 2
data = 192; // Assuming 192 represents the stop value for motor 2
} else if (dutyPerSigned < 0.0) {
// Scale negative values between -100 and 0 to control motor 2
data = static_cast<uint8_t>(192 - (-dutyPerSigned / 100.0) * 64);
} else if (dutyPerSigned <= 100.0) {
// Scale positive values between 0 and 100 to control motor 2
data = static_cast<uint8_t>(192 + (dutyPerSigned / 100.0) * 63);
}
ESP_LOGW(TAG, "set right motor to duty=%.2f, (data=%d)", dutyPerSigned, data);
sendByte(data);
}
//-------------------------
//--- setLeft, setRight ---
//-------------------------
//using motorCommand_t struct
void sabertooth2x60a::setLeft(motorCommand_t motorCommand_f){
float dutyTarget = motorCommandToSignedDuty(motorCommand_f);
setLeft(dutyTarget);
}
void sabertooth2x60a::setRight(motorCommand_t motorCommand_f){
float dutyTarget = motorCommandToSignedDuty(motorCommand_f);
setRight(dutyTarget);
} }

145
common/motordrivers.hpp
View File

@ -9,6 +9,7 @@ extern "C"
#include "driver/ledc.h" #include "driver/ledc.h"
#include "esp_err.h" #include "esp_err.h"
#include "driver/uart.h"
} }
#include <cmath> #include <cmath>
@ -26,16 +27,16 @@ extern "C"
//struct with all config parameters for single100a motor driver //struct with all config parameters for single100a motor driver
typedef struct single100a_config_t { typedef struct single100a_config_t {
gpio_num_t gpio_pwm; gpio_num_t gpio_pwm;
gpio_num_t gpio_a; gpio_num_t gpio_a;
gpio_num_t gpio_b; gpio_num_t gpio_b;
gpio_num_t gpio_brakeRelay; gpio_num_t gpio_brakeRelay;
ledc_timer_t ledc_timer; ledc_timer_t ledc_timer;
ledc_channel_t ledc_channel; ledc_channel_t ledc_channel;
bool aEnabledPinState; bool aEnabledPinState;
bool bEnabledPinState; bool bEnabledPinState;
ledc_timer_bit_t resolution; ledc_timer_bit_t resolution;
int pwmFreq; int pwmFreq;
} single100a_config_t; } single100a_config_t;
@ -44,58 +45,94 @@ typedef struct single100a_config_t {
//------- single100a class ------- //------- single100a class -------
//-------------------------------- //--------------------------------
class single100a { class single100a {
public: public:
//--- constructor --- //--- constructor ---
single100a(single100a_config_t config_f); //provide config struct (see above) single100a(single100a_config_t config_f); //provide config struct (see above)
//--- functions --- //--- functions ---
void set(motorstate_t state, float duty_f = 0); //set mode and duty of the motor (see motorstate_t above) void set(motorstate_t state, float duty_f = 0); //set mode and duty of the motor (see motorstate_t above)
//TODO: add functions to get the current state and duty //TODO: add functions to get the current state and duty
private: private:
//--- functions --- //--- functions ---
void init(); //initialize pwm and gpio outputs, calculate maxDuty void init(); //initialize pwm and gpio outputs, calculate maxDuty
//--- variables --- //--- variables ---
single100a_config_t config; single100a_config_t config;
uint32_t dutyMax; uint32_t dutyMax;
motorstate_t state = motorstate_t::IDLE; motorstate_t state = motorstate_t::IDLE;
bool brakeWaitingForRelay = false; bool brakeWaitingForRelay = false;
uint32_t timestamp_brakeRelayPowered; uint32_t timestamp_brakeRelayPowered;
}; };
////struct with all config parameters for single100a motor driver
//typedef struct sabertooth2x60_config_t {
// gpio_num_t gpio_TX;
// ledc_timer_t ledc_timer;
// ledc_channel_t ledc_channel;
// bool aEnabledPinState;
// bool bEnabledPinState;
// ledc_timer_bit_t resolution;
// int pwmFreq;
//} single100a_config_t; //==========================================
// //===== sabertooth 2x60A motor driver ======
////================================= //==========================================
////======= sabertooth 2x60a ========
////================================= //struct with all config parameters for single100a motor driver
//class single100a { typedef struct {
// public: gpio_num_t gpio_TX;
// //--- constructor --- uart_port_t uart_num; //(UART_NUM_1/2)
// single100a(single100a_config_t config_f); //provide config struct (see above) } sabertooth2x60_config_t;
//
// //--- functions ---
// void set(motorstate_t state, float duty_f = 0); //set mode and duty of the motor (see motorstate_t above) class sabertooth2x60a {
// //TODO: add functions to get the current state and duty public:
// //--- constructor ---
// private: sabertooth2x60a(sabertooth2x60_config_t config_f); //provide config struct (see above)
// //--- functions ---
// void init(); //initialize pwm and gpio outputs, calculate maxDuty //--- functions ---
// //set motor speed with float value from -100 to 100
// //--- variables --- void setLeft(float dutyPerSigned);
// single100a_config_t config; void setRight(float dutyPerSigned);
// uint32_t dutyMax; //set mode and duty of the motor (see motorstate_t above)
// motorstate_t state = motorstate_t::IDLE; void setLeft(motorCommand_t command);
// bool brakeWaitingForRelay = false; void setRight(motorCommand_t command);
// uint32_t timestamp_brakeRelayPowered; //TODO: add functions to get the current state and duty
//};
private:
//--- functions ---
void sendByte(char data);
void init(); //initialize uart
//--- variables ---
sabertooth2x60_config_t config;
uint32_t dutyMax;
motorstate_t state = motorstate_t::IDLE;
bool uart_isInitialized = false;
};
// //wrap motor drivers in a common class
// //-> different drivers can be used easily without changing code in motorctl etc
// //TODO add UART as driver?
// enum driverType_t {SINGLE100A, SABERTOOTH};
//
// template <typename driverType> class motors_t {
// public:
// motors_t (single100a_config_t config_left, single100a_config_t config_right);
// motors_t (sabertooth2x60_config_t);
//
// setLeft(motorstate_t state);
// setRight(motorstate_t state);
// set(motorCommands_t);
// private:
// enum driverType_t driverType;
// sabertooth2x60_config_t config_saber;
// single100a_config_t config_single;
// }
//
//