Create class sabertooth2x60a - new driver functional
currently repeatedly sends test commands from main using a created instance of that new class
This commit is contained in:
jonny_jr9
parent
a30ec01818
commit
98956e2bf8
@ -1,3 +1,6 @@
|
||||
#include "hal/uart_types.h"
|
||||
#include "motordrivers.hpp"
|
||||
#include "types.hpp"
|
||||
extern "C"
|
||||
{
|
||||
#include <stdio.h>
|
||||
@ -161,37 +164,31 @@ void sendByte(char data){
|
||||
//=========== app_main ============
|
||||
//=================================
|
||||
extern "C" void app_main(void) {
|
||||
//TEST SAVERTOOTH driver:
|
||||
//init uart:
|
||||
|
||||
ESP_LOGW(TAG, "initializing uart1...");
|
||||
uart_config_t uart1_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,
|
||||
//TEST SABERTOOTH driver:
|
||||
//config
|
||||
sabertooth2x60_config_t sabertoothConfig = {
|
||||
.gpio_TX = GPIO_NUM_23,
|
||||
.uart_num = UART_NUM_2
|
||||
};
|
||||
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.
|
||||
//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.
|
||||
//create instance
|
||||
sabertooth2x60a motors(sabertoothConfig);
|
||||
|
||||
|
||||
//run test commands
|
||||
while(1){
|
||||
sendByte(0);
|
||||
motors.setLeft({motorstate_t::FWD, 70});
|
||||
vTaskDelay(1000 / portTICK_PERIOD_MS);
|
||||
sendByte(10);
|
||||
vTaskDelay(2000 / portTICK_PERIOD_MS);
|
||||
sendByte(64);
|
||||
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);
|
||||
sendByte(120);
|
||||
vTaskDelay(2000 / portTICK_PERIOD_MS);
|
||||
}
|
||||
|
||||
// //enable 5V volate regulator
|
||||
|
||||
@ -165,3 +165,151 @@ void single100a::set(motorstate_t state_f, float duty_f){
|
||||
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);
|
||||
}
|
||||
|
||||
@ -9,6 +9,7 @@ extern "C"
|
||||
|
||||
#include "driver/ledc.h"
|
||||
#include "esp_err.h"
|
||||
#include "driver/uart.h"
|
||||
}
|
||||
|
||||
#include <cmath>
|
||||
@ -65,37 +66,73 @@ class single100a {
|
||||
};
|
||||
|
||||
|
||||
////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 ======
|
||||
//==========================================
|
||||
|
||||
//struct with all config parameters for single100a motor driver
|
||||
typedef struct {
|
||||
gpio_num_t gpio_TX;
|
||||
uart_port_t uart_num; //(UART_NUM_1/2)
|
||||
} sabertooth2x60_config_t;
|
||||
|
||||
|
||||
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);
|
||||
void setRight(float dutyPerSigned);
|
||||
//set mode and duty of the motor (see motorstate_t above)
|
||||
void setLeft(motorCommand_t command);
|
||||
void setRight(motorCommand_t command);
|
||||
//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};
|
||||
//
|
||||
////=================================
|
||||
////======= sabertooth 2x60a ========
|
||||
////=================================
|
||||
//class single100a {
|
||||
// template <typename driverType> class motors_t {
|
||||
// public:
|
||||
// //--- constructor ---
|
||||
// single100a(single100a_config_t config_f); //provide config struct (see above)
|
||||
//
|
||||
// //--- functions ---
|
||||
// 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
|
||||
// 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:
|
||||
// //--- functions ---
|
||||
// void init(); //initialize pwm and gpio outputs, calculate maxDuty
|
||||
// enum driverType_t driverType;
|
||||
// sabertooth2x60_config_t config_saber;
|
||||
// single100a_config_t config_single;
|
||||
// }
|
||||
//
|
||||
// //--- variables ---
|
||||
// single100a_config_t config;
|
||||
// uint32_t dutyMax;
|
||||
// motorstate_t state = motorstate_t::IDLE;
|
||||
// bool brakeWaitingForRelay = false;
|
||||
// uint32_t timestamp_brakeRelayPowered;
|
||||
//};
|
||||
//
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user