diff --git a/drive/arduino-sketch/arduino-sketch-live.ino b/drive/arduino-sketch/arduino-sketch-live.ino index 0dd243b..090354a 100644 --- a/drive/arduino-sketch/arduino-sketch-live.ino +++ b/drive/arduino-sketch/arduino-sketch-live.ino @@ -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 - - 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 R (-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 -#include -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 -> forward -// b -> backward -// l -> turn left (in place) -// r -> turn right (in place) -// s -> stop -// m -> 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 , s")); + setupPins(); + delay(200); + + Serial.println(F("Drive Ready. Send: L R (-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); } } diff --git a/drive/arduino-sketch/arduino-sketch-ps2-controller.ino/arduino-sketch-ps2-controller.ino.ino b/drive/arduino-sketch/arduino-sketch-ps2-controller.ino/arduino-sketch-ps2-controller.ino.ino deleted file mode 100644 index d54efad..0000000 --- a/drive/arduino-sketch/arduino-sketch-ps2-controller.ino/arduino-sketch-ps2-controller.ino.ino +++ /dev/null @@ -1,132 +0,0 @@ -// ===== Arduino UNO: Differential Drive über 2x BTS7960 ===== -// Serial command format: "L R \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 R " - 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); - } -} diff --git a/drive/var/www/drive/index.html b/drive/var/www/drive/index.html index 62607f0..98ed111 100644 --- a/drive/var/www/drive/index.html +++ b/drive/var/www/drive/index.html @@ -3,12 +3,12 @@ - Robot Dual Stick + Robot Dual Stick (Ramping) @@ -56,46 +61,180 @@
-
-
Max Speed: 180
+
+
+ + 180 +
-
T 0 | Turn 0 → L 0 | R 0
+ +
+
+ + 2.2/s +
+ + +
+ + 3.6/s +
+ + +
+ + 6.0/s +
+ + +
+ + 0.35 +
+ +
-
+ +
+
+
+ target: T 0.00 | Turn 0.00 || smooth: T 0.00 | Turn 0.00 || L 0 | R 0 +
+
- Loslassen = STOP. Wenn Verbindung/Tab weg: STOP (Watchdog). + Loslassen = STOP. Tab/Verbindung weg = STOP. (Web sendet laufend, damit Arduino-Watchdog nicht stoppt.)