0037f4a836
Bug was fixed in previous commit already Only changed code slightly (better readable) work with duty instead of data Optimize comments in motordriver
347 lines
12 KiB
C++
347 lines
12 KiB
C++
#include "motordrivers.hpp"
|
|
#include "esp_log.h"
|
|
#include "types.hpp"
|
|
|
|
//TODO: move from ledc to mcpwm?
|
|
//https://docs.espressif.com/projects/esp-idf/en/v4.3/esp32/api-reference/peripherals/mcpwm.html#
|
|
//https://github.com/espressif/esp-idf/tree/v4.3/examples/peripherals/mcpwm/mcpwm_basic_config
|
|
|
|
//Note fade functionality provided by LEDC would be very useful but unfortunately is not usable here because:
|
|
//"Due to hardware limitations, there is no way to stop a fade before it reaches its target duty."
|
|
|
|
//tag for logging
|
|
static const char * TAG = "motordriver";
|
|
|
|
//ms to wait in IDLE before BRAKE until relay actually switched
|
|
#define BRAKE_RELAY_DELAY_MS 300
|
|
|
|
|
|
//====================================
|
|
//===== single100a motor driver ======
|
|
//====================================
|
|
|
|
//-----------------------------
|
|
//-------- constructor --------
|
|
//-----------------------------
|
|
//copy provided struct with all configuration and run init function
|
|
single100a::single100a(single100a_config_t config_f){
|
|
config = config_f;
|
|
init();
|
|
}
|
|
|
|
|
|
|
|
//----------------------------
|
|
//---------- init ------------
|
|
//----------------------------
|
|
//function to initialize pwm output, gpio pins and calculate maxDuty
|
|
void single100a::init(){
|
|
|
|
//--- configure ledc timer ---
|
|
ledc_timer_config_t ledc_timer;
|
|
ledc_timer.speed_mode = LEDC_HIGH_SPEED_MODE;
|
|
ledc_timer.timer_num = config.ledc_timer;
|
|
ledc_timer.duty_resolution = config.resolution; //13bit gives max 5khz freq
|
|
ledc_timer.freq_hz = config.pwmFreq;
|
|
ledc_timer.clk_cfg = LEDC_AUTO_CLK;
|
|
//apply configuration
|
|
ledc_timer_config(&ledc_timer);
|
|
|
|
//--- configure ledc channel ---
|
|
ledc_channel_config_t ledc_channel;
|
|
ledc_channel.channel = config.ledc_channel;
|
|
ledc_channel.duty = 0;
|
|
ledc_channel.gpio_num = config.gpio_pwm;
|
|
ledc_channel.speed_mode = LEDC_HIGH_SPEED_MODE;
|
|
ledc_channel.hpoint = 0;
|
|
ledc_channel.timer_sel = config.ledc_timer;
|
|
ledc_channel.intr_type = LEDC_INTR_DISABLE;
|
|
ledc_channel.flags.output_invert = 0; //TODO: add config option to invert the pwm output?
|
|
//apply configuration
|
|
ledc_channel_config(&ledc_channel);
|
|
|
|
//--- define gpio pins as outputs ---
|
|
gpio_pad_select_gpio(config.gpio_a);
|
|
gpio_set_direction(config.gpio_a, GPIO_MODE_OUTPUT);
|
|
gpio_pad_select_gpio(config.gpio_b);
|
|
gpio_set_direction(config.gpio_b, GPIO_MODE_OUTPUT);
|
|
gpio_pad_select_gpio(config.gpio_brakeRelay);
|
|
gpio_set_direction(config.gpio_brakeRelay, GPIO_MODE_OUTPUT);
|
|
|
|
//--- calculate max duty according to selected resolution ---
|
|
dutyMax = pow(2, ledc_timer.duty_resolution) -1;
|
|
ESP_LOGW(TAG, "initialized single100a driver");
|
|
ESP_LOGW(TAG, "resolution=%dbit, dutyMax value=%d, resolution=%.4f %%", ledc_timer.duty_resolution, dutyMax, 100/(float)dutyMax);
|
|
}
|
|
|
|
|
|
|
|
//---------------------------
|
|
//----------- 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
|
|
uint32_t dutyScaled;
|
|
if (duty_f > 100) { //target duty above 100%
|
|
dutyScaled = dutyMax;
|
|
} else if (duty_f < 0) { //target at or below 0%
|
|
state_f = motorstate_t::IDLE;
|
|
dutyScaled = 0;
|
|
} else { //target duty 0-100%
|
|
//scale duty to available resolution
|
|
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);
|
|
|
|
//TODO: only when previous mode was BRAKE?
|
|
if (state_f != motorstate_t::BRAKE){
|
|
//reset brake wait state
|
|
brakeWaitingForRelay = false;
|
|
//turn of brake relay
|
|
gpio_set_level(config.gpio_brakeRelay, 0);
|
|
}
|
|
|
|
//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()...
|
|
switch (state_f){
|
|
case motorstate_t::IDLE:
|
|
ledc_set_duty(LEDC_HIGH_SPEED_MODE, config.ledc_channel, dutyScaled);
|
|
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?
|
|
//no brake: (freewheel)
|
|
//gpio_set_level(config.gpio_a, config.aEnabledPinState);
|
|
//gpio_set_level(config.gpio_b, !config.bEnabledPinState);
|
|
gpio_set_level(config.gpio_a, config.aEnabledPinState);
|
|
gpio_set_level(config.gpio_b, config.bEnabledPinState);
|
|
break;
|
|
|
|
case motorstate_t::BRAKE:
|
|
//prevent full short (no brake resistors) due to slow relay, also reduces switching load
|
|
if (!brakeWaitingForRelay){
|
|
ESP_LOGW(TAG, "BRAKE: turned on relay, waiting in IDLE for %d ms, then apply brake", BRAKE_RELAY_DELAY_MS);
|
|
//switch driver to IDLE for now
|
|
gpio_set_level(config.gpio_a, config.aEnabledPinState);
|
|
gpio_set_level(config.gpio_b, config.bEnabledPinState);
|
|
//start brake relay
|
|
gpio_set_level(config.gpio_brakeRelay, 1);
|
|
timestamp_brakeRelayPowered = esp_log_timestamp();
|
|
brakeWaitingForRelay = true;
|
|
}
|
|
//check if delay for relay to switch has passed
|
|
else if ((esp_log_timestamp() - timestamp_brakeRelayPowered) > BRAKE_RELAY_DELAY_MS) {
|
|
ESP_LOGD(TAG, "applying brake with brake-resistors at duty=%.2f%%", duty_f);
|
|
//switch driver to BRAKE
|
|
gpio_set_level(config.gpio_a, !config.aEnabledPinState);
|
|
gpio_set_level(config.gpio_b, !config.bEnabledPinState);
|
|
//apply duty
|
|
//FIXME switch between BREAK and IDLE with PWM and ignore pwm-pin? (needs test)
|
|
ledc_set_duty(LEDC_HIGH_SPEED_MODE, config.ledc_channel, dutyScaled);
|
|
ledc_update_duty(LEDC_HIGH_SPEED_MODE, config.ledc_channel);
|
|
} else {
|
|
//waiting... stay in IDLE
|
|
ESP_LOGD(TAG, "waiting for brake relay to close (IDLE)...");
|
|
gpio_set_level(config.gpio_a, config.aEnabledPinState);
|
|
gpio_set_level(config.gpio_b, config.bEnabledPinState);
|
|
}
|
|
break;
|
|
|
|
case motorstate_t::FWD:
|
|
ledc_set_duty(LEDC_HIGH_SPEED_MODE, config.ledc_channel, dutyScaled);
|
|
ledc_update_duty(LEDC_HIGH_SPEED_MODE, config.ledc_channel);
|
|
//forward:
|
|
gpio_set_level(config.gpio_a, config.aEnabledPinState);
|
|
gpio_set_level(config.gpio_b, !config.bEnabledPinState);
|
|
break;
|
|
|
|
case motorstate_t::REV:
|
|
ledc_set_duty(LEDC_HIGH_SPEED_MODE, config.ledc_channel, dutyScaled);
|
|
ledc_update_duty(LEDC_HIGH_SPEED_MODE, config.ledc_channel);
|
|
//reverse:
|
|
gpio_set_level(config.gpio_a, !config.aEnabledPinState);
|
|
gpio_set_level(config.gpio_b, config.bEnabledPinState);
|
|
break;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
//==========================================
|
|
//===== sabertooth 2x60A motor driver ======
|
|
//==========================================
|
|
|
|
/*
|
|
fom driver documentation:
|
|
>Sabertooth controls two motors with one 8 byte character, when operating in Simplified Serial mode
|
|
|
|
>Sending a character between 1 and 127 will control motor 1.
|
|
>1 is full reverse, 64 is stop and 127 is full forward.
|
|
|
|
>Sending a character between 128 and 255 will control motor 2.
|
|
>128 is full reverse, 192 is stop and 255 is full forward.
|
|
|
|
>Character 0 (hex 0x00) is a special case. Sending this character will shut down both motors.
|
|
*/
|
|
|
|
|
|
//-----------------------------
|
|
//-------- 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 = 9600, //dip switches: 101011
|
|
.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_mutex = xSemaphoreCreateMutex();
|
|
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
|
|
#define UART_MUTEX_TIMEOUT_MS 5000
|
|
void sabertooth2x60a::sendByte(char data){
|
|
//TODO mutex?
|
|
if (!uart_isInitialized){
|
|
ESP_LOGE(TAG, "uart not initialized, not sending command %d...", data);
|
|
return;
|
|
}
|
|
if (xSemaphoreTake(uart_mutex, pdMS_TO_TICKS(UART_MUTEX_TIMEOUT_MS)) == pdTRUE) {
|
|
uart_write_bytes(config.uart_num, &data, 1);
|
|
ESP_LOGD(TAG, "sent data=%d to sabertooth driver via uart", data);
|
|
//note 1byte takes 833us at 9600 baud
|
|
ets_delay_us(400); //wait for min time between each message
|
|
xSemaphoreGive(uart_mutex);
|
|
} else ESP_LOGE(TAG, "timeout waiting for uart mutex");
|
|
}
|
|
|
|
|
|
|
|
//---------------------------
|
|
//--------- 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 = 64;
|
|
if (dutyPerSigned <= -100.0) {
|
|
data = 1; //1 -> motor1 full reverse
|
|
} else if (dutyPerSigned >= 100) {
|
|
data = 127; //127 -> motor1 full forward
|
|
} else if (dutyPerSigned == 0.0) {
|
|
data = 64; //64 -> motor1 off
|
|
} else if (dutyPerSigned < 0.0) {
|
|
//REV: scale negative values between -100 and 0 to 1-63
|
|
data = static_cast<int>(64 - (- dutyPerSigned / 100.0) * 63);
|
|
} else if (dutyPerSigned <= 100.0) {
|
|
//FWD: scale positive values between 0 and 100 to 65-127
|
|
data = static_cast<int>(64 + (dutyPerSigned / 100.0) * 63);
|
|
}
|
|
|
|
//check if both motors are off -> special off command
|
|
dutyLeft = dutyPerSigned;
|
|
if (dutyLeft == 0 && dutyRight == 0) data = 0;
|
|
|
|
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 = 192;
|
|
if (dutyPerSigned <= -100.0) {
|
|
data = 128; //128 -> motor2 full reverse
|
|
} else if (dutyPerSigned >= 100.0) {
|
|
data = 255; //255 -> motor2 full forward
|
|
} else if (dutyPerSigned == 0.0) {
|
|
data = 192; //192 -> motor2 off
|
|
//NOTE: having issue with driver - after previously being on motor2 still spins slowly at command 192
|
|
} else if (dutyPerSigned < 0.0) {
|
|
//REV: scale negative values between -100 and 0 to 128-191
|
|
data = static_cast<uint8_t>(192 - (-dutyPerSigned / 100.0) * 64);
|
|
} else if (dutyPerSigned <= 100.0) {
|
|
//FWD: scale positive values between 0 and 100 to 193-255
|
|
data = static_cast<uint8_t>(192 + (dutyPerSigned / 100.0) * 63);
|
|
}
|
|
|
|
//check if both motors are off -> special off command
|
|
dutyRight = dutyPerSigned;
|
|
if (dutyRight == 0 && dutyLeft == 0) data = 0;
|
|
|
|
ESP_LOGI(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);
|
|
}
|