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

Fix bug right motor always on; Optimize comments

Browse Source 
Bug was fixed in previous commit already
Only changed code slightly (better readable) work with duty instead of data

Optimize comments in motordriver
This commit is contained in:
jonny_jr9 2023-09-13 13:49:52 +02:00
parent 5f54681bbf
commit 0037f4a836
2 changed files with 33 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
common/motordrivers.cpp
View File

@ -179,6 +179,20 @@ void single100a::set(motorstate_t state_f, float duty_f){
//===== sabertooth 2x60A motor driver ====== //===== 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 -------- //-------- constructor --------
//----------------------------- //-----------------------------
@ -261,27 +275,24 @@ void sabertooth2x60a::sendByte(char data){
//between 1 and 127 will control motor 1. 1 is full reverse, 64 is stop and 127 is full forward. //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. //Character 0 (hex 0x00) shut down both motors.
void sabertooth2x60a::setLeft(float dutyPerSigned){ void sabertooth2x60a::setLeft(float dutyPerSigned){
uint8_t data = 0; uint8_t data = 64;
if (dutyPerSigned <= -100.0) { if (dutyPerSigned <= -100.0) {
//full reverse for motor 1 data = 1; //1 -> motor1 full reverse
data = 1;
} else if (dutyPerSigned >= 100) { } else if (dutyPerSigned >= 100) {
//full forward data = 127; //127 -> motor1 full forward
data = 127;
} else if (dutyPerSigned == 0.0) { } else if (dutyPerSigned == 0.0) {
// Stop motor 1 data = 64; //64 -> motor1 off
data = 64;
} else if (dutyPerSigned < 0.0) { } else if (dutyPerSigned < 0.0) {
//scale negative values between -100 and 0 to 1-63 //REV: scale negative values between -100 and 0 to 1-63
data = static_cast<int>(64 - (- dutyPerSigned / 100.0) * 63); data = static_cast<int>(64 - (- dutyPerSigned / 100.0) * 63);
} else if (dutyPerSigned <= 100.0) { } else if (dutyPerSigned <= 100.0) {
//scale positive values between 0 and 100 to control motor 1 //FWD: scale positive values between 0 and 100 to 65-127
data = static_cast<int>(64 + (dutyPerSigned / 100.0) * 63); data = static_cast<int>(64 + (dutyPerSigned / 100.0) * 63);
} }
//check if both motors are off -> special off command //check if both motors are off -> special off command
lastDataLeft = data; dutyLeft = dutyPerSigned;
if (data == 64 && lastDataRight == 192) data = 0; if (dutyLeft == 0 && dutyRight == 0) data = 0;
ESP_LOGI(TAG, "set left motor to duty=%.2f, (data=%d)", dutyPerSigned, data); ESP_LOGI(TAG, "set left motor to duty=%.2f, (data=%d)", dutyPerSigned, data);
sendByte(data); sendByte(data);
@ -295,27 +306,25 @@ void sabertooth2x60a::setLeft(float dutyPerSigned){
//between 128 and 255 will control motor 2. 128 is full reverse, 192 is stop and 255 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. //Character 0 (hex 0x00) shut down both motors.
void sabertooth2x60a::setRight(float dutyPerSigned) { void sabertooth2x60a::setRight(float dutyPerSigned) {
uint8_t data = 0; uint8_t data = 192;
if (dutyPerSigned <= -100.0) { if (dutyPerSigned <= -100.0) {
// Full reverse for motor 2 data = 128; //128 -> motor2 full reverse
data = 128;
} else if (dutyPerSigned >= 100.0) { } else if (dutyPerSigned >= 100.0) {
// Full forward for motor 2 data = 255; //255 -> motor2 full forward
data = 255;
} else if (dutyPerSigned == 0.0) { } else if (dutyPerSigned == 0.0) {
// Stop motor 2 data = 192; //192 -> motor2 off
data = 192; // Assuming 192 represents the stop value for motor 2 //NOTE: having issue with driver - after previously being on motor2 still spins slowly at command 192
} else if (dutyPerSigned < 0.0) { } else if (dutyPerSigned < 0.0) {
// Scale negative values between -100 and 0 to control motor 2 //REV: scale negative values between -100 and 0 to 128-191
data = static_cast<uint8_t>(192 - (-dutyPerSigned / 100.0) * 64); data = static_cast<uint8_t>(192 - (-dutyPerSigned / 100.0) * 64);
} else if (dutyPerSigned <= 100.0) { } else if (dutyPerSigned <= 100.0) {
// Scale positive values between 0 and 100 to control motor 2 //FWD: scale positive values between 0 and 100 to 193-255
data = static_cast<uint8_t>(192 + (dutyPerSigned / 100.0) * 63); data = static_cast<uint8_t>(192 + (dutyPerSigned / 100.0) * 63);
} }
//check if both motors are off -> special off command //check if both motors are off -> special off command
lastDataRight = data; dutyRight = dutyPerSigned;
if (data == 192 && lastDataLeft == 64) data = 0; if (dutyRight == 0 && dutyLeft == 0) data = 0;
ESP_LOGI(TAG, "set right motor to duty=%.2f, (data=%d)", dutyPerSigned, data); ESP_LOGI(TAG, "set right motor to duty=%.2f, (data=%d)", dutyPerSigned, data);
sendByte(data); sendByte(data);

6
common/motordrivers.hpp
View File

@ -122,10 +122,8 @@ class sabertooth2x60a {
motorstate_t state = motorstate_t::IDLE; motorstate_t state = motorstate_t::IDLE;
bool uart_isInitialized = false; bool uart_isInitialized = false;
SemaphoreHandle_t uart_mutex; SemaphoreHandle_t uart_mutex;
uint8_t lastDataLeft = 0; float dutyLeft = 0;
uint8_t lastDataRight = 0; float dutyRight = 0;
}; };