133 lines
3.3 KiB
C++
133 lines
3.3 KiB
C++
// ===== Arduino UNO: Differential Drive über 2x BTS7960 =====
|
|
// Serial command format: "L <int> R <int>\n" where int in [-255..255]
|
|
// Soft-start ramping + watchdog stop
|
|
|
|
// --- Left BTS7960 pins ---
|
|
const uint8_t L_RPWM = 5; // PWM
|
|
const uint8_t L_LPWM = 6; // PWM
|
|
const uint8_t L_REN = 7; // enable
|
|
const uint8_t L_LEN = 8; // enable
|
|
|
|
// --- Right BTS7960 pins ---
|
|
const uint8_t R_RPWM = 9; // PWM
|
|
const uint8_t R_LPWM = 10; // PWM
|
|
const uint8_t R_REN = 11; // enable
|
|
const uint8_t R_LEN = 12; // enable
|
|
|
|
// Ramping
|
|
const uint8_t RAMP_STEP = 6; // speed change per loop (0..255)
|
|
const uint16_t LOOP_MS = 15; // ramp update interval
|
|
const uint16_t CMD_TIMEOUT_MS = 300; // stop if no command within x ms
|
|
|
|
int targetL = 0, targetR = 0;
|
|
int currentL = 0, currentR = 0;
|
|
|
|
unsigned long lastCmdMs = 0;
|
|
unsigned long lastLoopMs = 0;
|
|
|
|
void setupPins() {
|
|
pinMode(L_RPWM, OUTPUT); pinMode(L_LPWM, OUTPUT);
|
|
pinMode(L_REN, OUTPUT); pinMode(L_LEN, OUTPUT);
|
|
|
|
pinMode(R_RPWM, OUTPUT); pinMode(R_LPWM, OUTPUT);
|
|
pinMode(R_REN, OUTPUT); pinMode(R_LEN, OUTPUT);
|
|
|
|
digitalWrite(L_REN, HIGH); digitalWrite(L_LEN, HIGH);
|
|
digitalWrite(R_REN, HIGH); digitalWrite(R_LEN, HIGH);
|
|
|
|
analogWrite(L_RPWM, 0); analogWrite(L_LPWM, 0);
|
|
analogWrite(R_RPWM, 0); analogWrite(R_LPWM, 0);
|
|
}
|
|
|
|
static int clamp255(int v) {
|
|
if (v > 255) return 255;
|
|
if (v < -255) return -255;
|
|
return v;
|
|
}
|
|
|
|
void driveOne(int speed, uint8_t rpwm, uint8_t lpwm) {
|
|
speed = clamp255(speed);
|
|
if (speed > 0) {
|
|
analogWrite(rpwm, (uint8_t)speed);
|
|
analogWrite(lpwm, 0);
|
|
} else if (speed < 0) {
|
|
analogWrite(rpwm, 0);
|
|
analogWrite(lpwm, (uint8_t)(-speed));
|
|
} else {
|
|
analogWrite(rpwm, 0);
|
|
analogWrite(lpwm, 0);
|
|
}
|
|
}
|
|
|
|
static int rampTowards(int current, int target, uint8_t step) {
|
|
if (current < target) {
|
|
int n = current + step;
|
|
return (n > target) ? target : n;
|
|
}
|
|
if (current > target) {
|
|
int n = current - step;
|
|
return (n < target) ? target : n;
|
|
}
|
|
return current;
|
|
}
|
|
|
|
bool parseLine(const String& line, int &outL, int &outR) {
|
|
// expected: "L <int> R <int>"
|
|
int idxL = line.indexOf('L');
|
|
int idxR = line.indexOf('R');
|
|
if (idxL < 0 || idxR < 0) return false;
|
|
|
|
// crude parse; robust enough for our simple format
|
|
String partL = line.substring(idxL + 1, idxR);
|
|
String partR = line.substring(idxR + 1);
|
|
|
|
partL.trim();
|
|
partR.trim();
|
|
|
|
outL = partL.toInt();
|
|
outR = partR.toInt();
|
|
outL = clamp255(outL);
|
|
outR = clamp255(outR);
|
|
return true;
|
|
}
|
|
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
setupPins();
|
|
lastCmdMs = millis();
|
|
lastLoopMs = millis();
|
|
}
|
|
|
|
void loop() {
|
|
// --- read serial lines ---
|
|
while (Serial.available()) {
|
|
String line = Serial.readStringUntil('\n');
|
|
line.trim();
|
|
if (line.length() == 0) continue;
|
|
|
|
int l, r;
|
|
if (parseLine(line, l, r)) {
|
|
targetL = l;
|
|
targetR = r;
|
|
lastCmdMs = millis();
|
|
}
|
|
}
|
|
|
|
// --- watchdog stop ---
|
|
if (millis() - lastCmdMs > CMD_TIMEOUT_MS) {
|
|
targetL = 0;
|
|
targetR = 0;
|
|
}
|
|
|
|
// --- ramp update ---
|
|
if (millis() - lastLoopMs >= LOOP_MS) {
|
|
lastLoopMs = millis();
|
|
|
|
currentL = rampTowards(currentL, targetL, RAMP_STEP);
|
|
currentR = rampTowards(currentR, targetR, RAMP_STEP);
|
|
|
|
driveOne(currentL, L_RPWM, L_LPWM);
|
|
driveOne(currentR, R_RPWM, R_LPWM);
|
|
}
|
|
}
|