add arduino sketch plus ps2 steering

README.md

@@ -80,10 +80,14 @@ http://pi-ip/
     sudo chmod -R 755 /opt/face
 
 Start:
+
     sudo systemctl daemon-reload
     sudo systemctl enable --now face.service
     sudo systemctl status face.service --no-pager
 
+### Face Control UI
+    http://localhost/control/
+
 ### Debug Face
 
 #### SSE-Stream going to browser?
@@ -120,4 +124,60 @@ Start:
 
 ## Arduino Motor Steering
 
+### Pin assignment Arduino -> 2 pcs Motor-Driver-Boards (BTS7960)
+- one BTS7960 is for the two left 12V DC motors in parallel 
+- one BTS7960 is for the two right 12V DC Motors
+
+Caution! This drivers only work for DC Motors. For stepper motors you will need different motor drivers.
+
+
+#### Left BTS#1
+Power
+
+- VCC -> Arduino 5V
+- GND -> Arduino GND
+
+Steering - NEVER activate both directions at once!!!
+
+- RPWM -> Arduino D5 (PWM)
+- LPWM -> Arduino D6 (PWM)
+
+Enabled - either directly 5V or steer it from arduino
+
+- R_EN -> Arduino D7
+- L_EN -> Ardiono D8
+
+
+#### Right BTS#2
+Power
+
+- VCC -> Arduino 5V
+- GND -> Arduino GND
+
+Steering - NEVER activate both directions at once!!!
+
+- RPWM -> Arduino D9 (PWM)
+- LPWM -> Arduino D10 (PWM)
+
+Enabled - either directly 5V or steer it from arduino
+
+- R_EN -> Arduino D11
+- L_EN -> Ardiono D12
+
+
+### Arduino Steering Code
+
+Upload to Arduino:
+    drive/arduino-sketch/arduino-sketch.ino
+
+### Test with Arduino Serial Monitor
+
+slow forward
+
+    f 60
+
+slow backward
+
+    r 60
+
 

drive/arduino-sketch/arduino-sketch-live.ino

@@ -0,0 +1,303 @@
+/*
+  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));
+  }
+}

drive/arduino-sketch/arduino-sketch-test.ino

@@ -0,0 +1,165 @@
+/*
+  Single BTS7960 test on Arduino UNO
+  Pins:
+    D5 -> LPWM
+    D6 -> RPWM
+    D7 -> LEN
+    D8 -> REN
+
+  Test:
+    - automatically: forward -> stop -> backward -> stop (loop)
+    - optional serial commands:
+        f <0..255>
+        b <0..255>
+        s
+*/
+
+#include <Arduino.h>
+
+const uint8_t LPWM = 5;   // PWM
+const uint8_t RPWM = 6;   // PWM
+const uint8_t LEN  = 7;   // Enable
+const uint8_t REN  = 8;   // Enable
+
+const int PWM_MAX = 255;
+
+// Ramp tuning (PWM steps per second)
+const float RAMP_UP_PER_SEC   = 320.0f;
+const float RAMP_DOWN_PER_SEC = 520.0f;
+
+const uint32_t CONTROL_INTERVAL_MS = 10;
+
+int target = 0;        // -255..255
+float current = 0.0f;  // ramped output
+uint32_t lastControlMs = 0;
+
+static int clampInt(int v, int lo, int hi) {
+  if (v < lo) return lo;
+  if (v > hi) return hi;
+  return v;
+}
+
+static float rampTo(float cur, float tgt, float dtSec) {
+  float diff = tgt - cur;
+  if (diff == 0.0f) return cur;
+
+  float rate = (abs(tgt) > abs(cur)) ? RAMP_UP_PER_SEC : RAMP_DOWN_PER_SEC;
+  float step = rate * dtSec;
+
+  if (abs(diff) <= step) return tgt;
+  return cur + (diff > 0 ? step : -step);
+}
+
+static void driveBTS(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 >0 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 setTarget(int speed) {
+  target = clampInt(speed, -PWM_MAX, PWM_MAX);
+}
+
+static void stopMotor() {
+  setTarget(0);
+}
+
+static void handleSerialLine(String line) {
+  line.trim();
+  if (line.length() == 0) return;
+
+  char c = tolower(line.charAt(0));
+  int v = 0;
+
+  if (c == 's') {
+    stopMotor();
+    Serial.println(F("STOP"));
+    return;
+  }
+
+  if (sscanf(line.c_str(), "%c %d", &c, &v) < 2) return;
+  v = clampInt(v, 0, PWM_MAX);
+
+  if (c == 'f') {
+    setTarget(+v);
+    Serial.print(F("FWD ")); Serial.println(v);
+  } else if (c == 'b') {
+    setTarget(-v);
+    Serial.print(F("BWD ")); Serial.println(v);
+  }
+}
+
+void setup() {
+  pinMode(LPWM, OUTPUT);
+  pinMode(RPWM, OUTPUT);
+  pinMode(LEN, OUTPUT);
+  pinMode(REN, OUTPUT);
+
+  analogWrite(LPWM, 0);
+  analogWrite(RPWM, 0);
+  digitalWrite(LEN, HIGH);
+  digitalWrite(REN, HIGH);
+
+  Serial.begin(115200);
+  Serial.println(F("BTS7960 single-driver test ready."));
+  Serial.println(F("Serial: f <0..255>, b <0..255>, s"));
+  lastControlMs = millis();
+}
+
+void loop() {
+  // --- Serial line input ---
+  static String buf;
+  while (Serial.available()) {
+    char ch = (char)Serial.read();
+    if (ch == '\n' || ch == '\r') {
+      if (buf.length() > 0) {
+        handleSerialLine(buf);
+        buf = "";
+      }
+    } else {
+      if (buf.length() < 60) buf += ch;
+    }
+  }
+
+  // --- Automatic test sequence (only if no serial commands used) ---
+  // Comment this block out if you want ONLY serial control.
+  static uint32_t t0 = millis();
+  static int phase = 0;
+
+  uint32_t now = millis();
+  if (now - t0 > 2000) { // change every 2s
+    t0 = now;
+    phase = (phase + 1) % 4;
+
+    if (phase == 0) setTarget(+80);   // forward slow
+    if (phase == 1) stopMotor();      // stop
+    if (phase == 2) setTarget(-80);   // backward slow
+    if (phase == 3) stopMotor();      // stop
+  }
+
+  // --- Control update (ramp + output) ---
+  now = millis();
+  if (now - lastControlMs >= CONTROL_INTERVAL_MS) {
+    float dt = (now - lastControlMs) / 1000.0f;
+    lastControlMs = now;
+
+    current = rampTo(current, (float)target, dt);
+    driveBTS((int)lround(current));
+  }
+}