changed drive ramping from arduino to steering for better control
This commit is contained in:
max
@@ -1,303 +1,163 @@
|
||||
/*
|
||||
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.
|
||||
Arduino Drive "Safe Executor"
|
||||
- Receives: L <int> R <int> (-255..255)
|
||||
- Outputs PWM immediately (no ramping here)
|
||||
- Safety: watchdog stop if no command
|
||||
- Deadzone + Min PWM mapping to avoid motor buzzing at low PWM
|
||||
- Optional direction-change protection (short stop)
|
||||
*/
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <PS2X_lib.h>
|
||||
PS2X ps2x;
|
||||
// ===== Left BTS7960 pins =====
|
||||
const uint8_t L_RPWM = 5;
|
||||
const uint8_t L_LPWM = 6;
|
||||
const uint8_t L_REN = 7;
|
||||
const uint8_t L_LEN = 8;
|
||||
|
||||
// ------ 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;
|
||||
// ===== Right BTS7960 pins =====
|
||||
const uint8_t R_RPWM = 9;
|
||||
const uint8_t R_LPWM = 10;
|
||||
const uint8_t R_REN = 11;
|
||||
const uint8_t R_LEN = 12;
|
||||
|
||||
// ------- USE PS2 Controller or Serial Input --------------
|
||||
#define USE_PS2 1
|
||||
// ===== Safety / feel =====
|
||||
const uint16_t CMD_TIMEOUT_MS = 600; // Web sendet regelmäßig; 600ms ist entspannt
|
||||
const uint8_t DEADZONE = 10; // kleine Werte -> 0
|
||||
const uint8_t MIN_PWM = 70; // anpassen: 60..110 typisch (gegen "brummen")
|
||||
|
||||
// ----------------------------- 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)
|
||||
// Optional: beim Richtungswechsel kurz stoppen (schont Treiber/Getriebe)
|
||||
const bool PROTECT_DIR_CHANGE = true;
|
||||
const uint16_t DIR_CHANGE_STOP_MS = 60;
|
||||
|
||||
// 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)
|
||||
int targetL = 0, targetR = 0;
|
||||
unsigned long lastCmdMs = 0;
|
||||
|
||||
// --------------------------- Tuning / Limits -------------------------------
|
||||
// PWM range: 0..255
|
||||
const int PWM_MAX = 255;
|
||||
int lastOutL = 0, lastOutR = 0;
|
||||
|
||||
// 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;
|
||||
static int clamp255(int v) {
|
||||
if (v > 255) return 255;
|
||||
if (v < -255) return -255;
|
||||
return v;
|
||||
}
|
||||
|
||||
static int applyDeadband(int v, int db) {
|
||||
if (abs(v) < db) return 0;
|
||||
return v;
|
||||
int applyMinPwm(int v) {
|
||||
v = clamp255(v);
|
||||
int a = abs(v);
|
||||
if (a <= DEADZONE) return 0;
|
||||
|
||||
int s = (v >= 0) ? 1 : -1;
|
||||
|
||||
// map: DEADZONE..255 -> MIN_PWM..255
|
||||
long mapped = MIN_PWM + (long)(a - DEADZONE) * (255 - MIN_PWM) / (255 - DEADZONE);
|
||||
if (mapped > 255) mapped = 255;
|
||||
|
||||
return s * (int)mapped;
|
||||
}
|
||||
|
||||
// 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
|
||||
void setBTS7960(int speed, uint8_t rpwm, uint8_t lpwm) {
|
||||
speed = clamp255(speed);
|
||||
if (speed > 0) {
|
||||
analogWrite(lpwm, pwm);
|
||||
analogWrite(rpwm, 0);
|
||||
analogWrite(rpwm, (uint8_t)speed);
|
||||
analogWrite(lpwm, 0);
|
||||
} else if (speed < 0) {
|
||||
analogWrite(lpwm, 0);
|
||||
analogWrite(rpwm, pwm);
|
||||
} else {
|
||||
analogWrite(lpwm, 0);
|
||||
analogWrite(rpwm, 0);
|
||||
analogWrite(lpwm, (uint8_t)(-speed));
|
||||
} else {
|
||||
analogWrite(rpwm, 0);
|
||||
analogWrite(lpwm, 0);
|
||||
}
|
||||
}
|
||||
|
||||
static void stopAll() {
|
||||
targetLeft = 0;
|
||||
targetRight = 0;
|
||||
bool parseLine(const String& line, int &outL, int &outR) {
|
||||
int idxL = line.indexOf('L');
|
||||
int idxR = line.indexOf('R');
|
||||
if (idxL < 0 || idxR < 0) return false;
|
||||
|
||||
String partL = line.substring(idxL + 1, idxR);
|
||||
String partR = line.substring(idxR + 1);
|
||||
partL.trim(); partR.trim();
|
||||
|
||||
outL = clamp255(partL.toInt());
|
||||
outR = clamp255(partR.toInt());
|
||||
return true;
|
||||
}
|
||||
|
||||
// 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);
|
||||
void setupPins() {
|
||||
pinMode(L_RPWM, OUTPUT); pinMode(L_LPWM, OUTPUT);
|
||||
pinMode(L_REN, OUTPUT); pinMode(L_LEN, OUTPUT);
|
||||
|
||||
throttle = applyDeadband(throttle, INPUT_DEADBAND);
|
||||
turn = applyDeadband(turn, INPUT_DEADBAND);
|
||||
pinMode(R_RPWM, OUTPUT); pinMode(R_LPWM, OUTPUT);
|
||||
pinMode(R_REN, OUTPUT); pinMode(R_LEN, OUTPUT);
|
||||
|
||||
// Classic mix
|
||||
int left = throttle + turn;
|
||||
int right = throttle - turn;
|
||||
digitalWrite(L_REN, HIGH); digitalWrite(L_LEN, HIGH);
|
||||
digitalWrite(R_REN, HIGH); digitalWrite(R_LEN, HIGH);
|
||||
|
||||
// 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;
|
||||
setBTS7960(0, L_RPWM, L_LPWM);
|
||||
setBTS7960(0, R_RPWM, R_LPWM);
|
||||
}
|
||||
|
||||
// ------------------------- 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;
|
||||
void outputLR(int l, int r) {
|
||||
l = applyMinPwm(l);
|
||||
r = applyMinPwm(r);
|
||||
|
||||
char cmd = tolower(line.charAt(0));
|
||||
if (PROTECT_DIR_CHANGE) {
|
||||
// if sign changes across 0 while moving -> brief stop
|
||||
auto sign = [](int v) -> int { return (v > 0) - (v < 0); };
|
||||
|
||||
// Update watchdog timestamp on any valid-looking input
|
||||
lastCmdMs = millis();
|
||||
bool lFlip = (sign(lastOutL) != 0) && (sign(l) != 0) && (sign(lastOutL) != sign(l));
|
||||
bool rFlip = (sign(lastOutR) != 0) && (sign(r) != 0) && (sign(lastOutR) != sign(r));
|
||||
|
||||
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);
|
||||
if (lFlip || rFlip) {
|
||||
setBTS7960(0, L_RPWM, L_LPWM);
|
||||
setBTS7960(0, R_RPWM, R_LPWM);
|
||||
delay(DIR_CHANGE_STOP_MS);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// Parse "f a" / "b a" / "l a" / "r a"
|
||||
n = sscanf(line.c_str(), "%c %d", &cmd, &a);
|
||||
if (n < 2) return;
|
||||
setBTS7960(l, L_RPWM, L_LPWM);
|
||||
setBTS7960(r, R_RPWM, R_LPWM);
|
||||
|
||||
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;
|
||||
}
|
||||
lastOutL = l;
|
||||
lastOutR = r;
|
||||
}
|
||||
|
||||
// ------------------------------ 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"));
|
||||
setupPins();
|
||||
delay(200);
|
||||
|
||||
Serial.println(F("Drive Ready. Send: L <val> R <val> (-255..255)"));
|
||||
Serial.print(F("DEADZONE=")); Serial.print(DEADZONE);
|
||||
Serial.print(F(" MIN_PWM=")); Serial.println(MIN_PWM);
|
||||
|
||||
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 = "";
|
||||
}
|
||||
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();
|
||||
|
||||
outputLR(targetL, targetR);
|
||||
|
||||
Serial.print(F("RX/OUT L=")); Serial.print(lastOutL);
|
||||
Serial.print(F(" R=")); Serial.println(lastOutR);
|
||||
} else {
|
||||
// avoid huge line
|
||||
if (buf.length() < 80) buf += c;
|
||||
Serial.println(F("Parse error -> STOP"));
|
||||
targetL = 0; targetR = 0;
|
||||
lastCmdMs = millis();
|
||||
outputLR(0, 0);
|
||||
}
|
||||
}
|
||||
#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));
|
||||
// Watchdog stop
|
||||
if (millis() - lastCmdMs > CMD_TIMEOUT_MS) {
|
||||
outputLR(0, 0);
|
||||
}
|
||||
}
|
||||
|
||||
-132
@@ -1,132 +0,0 @@
|
||||
// ===== 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);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user