304 lines
8.2 KiB
C++
304 lines
8.2 KiB
C++
/*
|
|
4WD Robot Motor Control (2x BTS7960: Left + Right)
|
|
- Soft start/stop via ramping (slew-rate limiter)
|
|
- Differential drive mixing: throttle + turn
|
|
- Serial control (USB): f/b/l/r/s or m <throttle> <turn>
|
|
- Watchdog: stops if no command received in time
|
|
|
|
Wiring (per BTS7960 module):
|
|
- LPWM -> Arduino PWM pin
|
|
- RPWM -> Arduino PWM pin
|
|
- LEN/REN (or L_EN/R_EN) -> Arduino digital pins (or permanently HIGH to 5V)
|
|
- VCC -> 5V (logic)
|
|
- GND -> GND common with Arduino and motor supply
|
|
- B+ / B- -> motor supply (e.g. 12V)
|
|
- M+ / M- -> motor output
|
|
|
|
NOTE:
|
|
Each side is one BTS7960 module driving two motors in parallel (front+rear) on that side.
|
|
*/
|
|
|
|
#include <Arduino.h>
|
|
#include <PS2X_lib.h>
|
|
PS2X ps2x;
|
|
|
|
// ------ PS2 Controller Pins -------------
|
|
const uint8_t PS2_CLK = 2;
|
|
const uint8_t PS2_ATT = 3;
|
|
const uint8_t PS2_CMD = 4;
|
|
const uint8_t PS2_DAT = 12;
|
|
|
|
// ------- USE PS2 Controller or Serial Input --------------
|
|
#define USE_PS2 1
|
|
|
|
// ----------------------------- Pins (ANPASSEN) -----------------------------
|
|
// LEFT BTS7960
|
|
const uint8_t L_LPWM = 5; // PWM pin
|
|
const uint8_t L_RPWM = 6; // PWM pin
|
|
const uint8_t L_LEN = 7; // enable pin (LEN)
|
|
const uint8_t L_REN = 8; // enable pin (REN)
|
|
|
|
// RIGHT BTS7960
|
|
const uint8_t R_LPWM = 9; // PWM pin
|
|
const uint8_t R_RPWM = 10; // PWM pin
|
|
const uint8_t R_LEN = 11; // enable pin (LEN)
|
|
const uint8_t R_REN = 12; // enable pin (REN)
|
|
|
|
// --------------------------- Tuning / Limits -------------------------------
|
|
// PWM range: 0..255
|
|
const int PWM_MAX = 255;
|
|
|
|
// Ramp speed: PWM units per second (z.B. 300 => ~0.85s von 0 auf 255)
|
|
const float RAMP_UP_PER_SEC = 320.0f;
|
|
const float RAMP_DOWN_PER_SEC = 520.0f; // meist darf bremsen/stoppen schneller sein
|
|
|
|
// Deadband: kleine Werte ignorieren (gegen "Zittern")
|
|
const int INPUT_DEADBAND = 10;
|
|
|
|
// Watchdog: wenn so lange kein Command kommt -> STOP
|
|
const uint32_t COMMAND_TIMEOUT_MS = 600;
|
|
|
|
// Loop interval (Ramping arbeitet zeitbasiert; häufig genug aufrufen)
|
|
const uint32_t CONTROL_INTERVAL_MS = 10;
|
|
|
|
// ---------------------------- State ----------------------------------------
|
|
volatile int targetLeft = 0; // -255..255
|
|
volatile int targetRight = 0; // -255..255
|
|
|
|
float currentLeft = 0.0f; // ramped output -255..255
|
|
float currentRight = 0.0f;
|
|
|
|
uint32_t lastCmdMs = 0;
|
|
uint32_t lastControlMs = 0;
|
|
|
|
// ------------------------- Helpers -----------------------------------------
|
|
static int clampInt(int v, int lo, int hi) {
|
|
if (v < lo) return lo;
|
|
if (v > hi) return hi;
|
|
return v;
|
|
}
|
|
|
|
static int applyDeadband(int v, int db) {
|
|
if (abs(v) < db) return 0;
|
|
return v;
|
|
}
|
|
|
|
// Ramp current towards target based on dt
|
|
static float rampTo(float current, float target, float dtSec) {
|
|
float diff = target - current;
|
|
if (diff == 0.0f) return current;
|
|
|
|
const float rate = (abs(target) > abs(current)) ? RAMP_UP_PER_SEC : RAMP_DOWN_PER_SEC;
|
|
float step = rate * dtSec;
|
|
|
|
if (abs(diff) <= step) return target;
|
|
return current + (diff > 0 ? step : -step);
|
|
}
|
|
|
|
// Send signed speed (-255..255) to one BTS7960
|
|
static void driveBTS7960(uint8_t lpwm, uint8_t rpwm, uint8_t len, uint8_t ren, int speed) {
|
|
speed = clampInt(speed, -PWM_MAX, PWM_MAX);
|
|
|
|
// Enable driver
|
|
digitalWrite(len, HIGH);
|
|
digitalWrite(ren, HIGH);
|
|
|
|
int pwm = abs(speed);
|
|
|
|
// IMPORTANT: never drive both PWM pins at the same time
|
|
if (speed > 0) {
|
|
analogWrite(lpwm, pwm);
|
|
analogWrite(rpwm, 0);
|
|
} else if (speed < 0) {
|
|
analogWrite(lpwm, 0);
|
|
analogWrite(rpwm, pwm);
|
|
} else {
|
|
analogWrite(lpwm, 0);
|
|
analogWrite(rpwm, 0);
|
|
}
|
|
}
|
|
|
|
static void stopAll() {
|
|
targetLeft = 0;
|
|
targetRight = 0;
|
|
}
|
|
|
|
// Differential drive mixing:
|
|
// throttle: -255..255, turn: -255..255
|
|
static void setMix(int throttle, int turn) {
|
|
throttle = clampInt(throttle, -PWM_MAX, PWM_MAX);
|
|
turn = clampInt(turn, -PWM_MAX, PWM_MAX);
|
|
|
|
throttle = applyDeadband(throttle, INPUT_DEADBAND);
|
|
turn = applyDeadband(turn, INPUT_DEADBAND);
|
|
|
|
// Classic mix
|
|
int left = throttle + turn;
|
|
int right = throttle - turn;
|
|
|
|
// Normalize if exceeds range
|
|
int maxMag = max(abs(left), abs(right));
|
|
if (maxMag > PWM_MAX) {
|
|
// scale down proportionally
|
|
left = (int)((float)left * ((float)PWM_MAX / (float)maxMag));
|
|
right = (int)((float)right * ((float)PWM_MAX / (float)maxMag));
|
|
}
|
|
|
|
targetLeft = left;
|
|
targetRight = right;
|
|
}
|
|
|
|
// ------------------------- Serial command parser ---------------------------
|
|
// Commands:
|
|
// f <pwm> -> forward
|
|
// b <pwm> -> backward
|
|
// l <pwm> -> turn left (in place)
|
|
// r <pwm> -> turn right (in place)
|
|
// s -> stop
|
|
// m <throttle> <turn> -> mix mode (-255..255 each)
|
|
// Examples:
|
|
// f 140
|
|
// m 120 -40
|
|
// s
|
|
static void handleSerialLine(String line) {
|
|
line.trim();
|
|
if (line.length() == 0) return;
|
|
|
|
char cmd = tolower(line.charAt(0));
|
|
|
|
// Update watchdog timestamp on any valid-looking input
|
|
lastCmdMs = millis();
|
|
|
|
if (cmd == 's') {
|
|
stopAll();
|
|
return;
|
|
}
|
|
|
|
// Split by spaces
|
|
// Simple parsing:
|
|
// cmd + integers
|
|
int a = 0, b = 0;
|
|
int n = 0;
|
|
|
|
// Try parse "m a b"
|
|
if (cmd == 'm') {
|
|
n = sscanf(line.c_str(), "m %d %d", &a, &b);
|
|
if (n == 2) {
|
|
setMix(a, b);
|
|
}
|
|
return;
|
|
}
|
|
|
|
// Parse "f a" / "b a" / "l a" / "r a"
|
|
n = sscanf(line.c_str(), "%c %d", &cmd, &a);
|
|
if (n < 2) return;
|
|
|
|
a = clampInt(a, 0, PWM_MAX);
|
|
|
|
switch (tolower(cmd)) {
|
|
case 'f': setMix(+a, 0); break;
|
|
case 'b': setMix(-a, 0); break;
|
|
case 'l': setMix(0, +a); break; // left turn in place
|
|
case 'r': setMix(0, -a); break; // right turn in place
|
|
default: break;
|
|
}
|
|
}
|
|
|
|
// ------------------------------ Arduino ------------------------------------
|
|
void setup() {
|
|
int err = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_ATT, PS2_DAT, false, false);
|
|
Serial.print("PS2 init: "); Serial.println(err);
|
|
|
|
pinMode(L_LPWM, OUTPUT);
|
|
pinMode(L_RPWM, OUTPUT);
|
|
pinMode(L_LEN, OUTPUT);
|
|
pinMode(L_REN, OUTPUT);
|
|
|
|
pinMode(R_LPWM, OUTPUT);
|
|
pinMode(R_RPWM, OUTPUT);
|
|
pinMode(R_LEN, OUTPUT);
|
|
pinMode(R_REN, OUTPUT);
|
|
|
|
// Init: disabled PWM = 0, enables on
|
|
analogWrite(L_LPWM, 0); analogWrite(L_RPWM, 0);
|
|
analogWrite(R_LPWM, 0); analogWrite(R_RPWM, 0);
|
|
|
|
digitalWrite(L_LEN, HIGH); digitalWrite(L_REN, HIGH);
|
|
digitalWrite(R_LEN, HIGH); digitalWrite(R_REN, HIGH);
|
|
|
|
Serial.begin(115200);
|
|
Serial.println(F("BTS7960 Robot Control ready."));
|
|
Serial.println(F("Commands: f/b/l/r <0..255>, m <throttle -255..255> <turn -255..255>, s"));
|
|
|
|
lastCmdMs = millis();
|
|
lastControlMs = millis();
|
|
}
|
|
|
|
void loop() {
|
|
#if USE_PS2
|
|
// -------- PS2 Controller Input -------
|
|
ps2x.read_gamepad(false, 0);
|
|
|
|
// Not-Stop z.B. START
|
|
if (ps2x.ButtonPressed(PSB_START)) {
|
|
stopAll();
|
|
lastCmdMs = millis(); // watchdog “füttern”
|
|
}
|
|
|
|
// Sticks lesen
|
|
int ly = ps2x.Analog(PSS_LY); // 0..255
|
|
int rx = ps2x.Analog(PSS_RX); // 0..255
|
|
|
|
// 0..255 -> -255..255
|
|
auto stickToSigned = [](int v) {
|
|
int x = (v - 128) * 2;
|
|
if (x > 255) x = 255;
|
|
if (x < -255) x = -255;
|
|
return x;
|
|
};
|
|
|
|
// Mapping: LY nach oben = vorwärts (invertieren)
|
|
int throttle = -stickToSigned(ly);
|
|
int turn = stickToSigned(rx);
|
|
|
|
lastCmdMs = millis(); // watchdog “füttern”
|
|
setMix(throttle, turn); // das ist dein Original-Mixer
|
|
|
|
#else
|
|
// -------- Serial input (line based) --------
|
|
static String buf;
|
|
while (Serial.available()) {
|
|
char c = (char)Serial.read();
|
|
if (c == '\n' || c == '\r') {
|
|
if (buf.length() > 0) {
|
|
handleSerialLine(buf);
|
|
buf = "";
|
|
}
|
|
} else {
|
|
// avoid huge line
|
|
if (buf.length() < 80) buf += c;
|
|
}
|
|
}
|
|
#ENDIF
|
|
|
|
// -------- Watchdog --------
|
|
if (millis() - lastCmdMs > COMMAND_TIMEOUT_MS) {
|
|
stopAll();
|
|
}
|
|
|
|
// -------- Control update (ramping + output) --------
|
|
uint32_t now = millis();
|
|
if (now - lastControlMs >= CONTROL_INTERVAL_MS) {
|
|
float dt = (now - lastControlMs) / 1000.0f;
|
|
lastControlMs = now;
|
|
|
|
// ramp current values towards target
|
|
currentLeft = rampTo(currentLeft, (float)targetLeft, dt);
|
|
currentRight = rampTo(currentRight, (float)targetRight, dt);
|
|
|
|
// apply to drivers
|
|
driveBTS7960(L_LPWM, L_RPWM, L_LEN, L_REN, (int)round(currentLeft));
|
|
driveBTS7960(R_LPWM, R_RPWM, R_LEN, R_REN, (int)round(currentRight));
|
|
}
|
|
}
|