diff --git a/README.md b/README.md index 9fda71e..a96e78e 100644 --- a/README.md +++ b/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 + diff --git a/drive/arduino-sketch/arduino-sketch-live.ino b/drive/arduino-sketch/arduino-sketch-live.ino new file mode 100644 index 0000000..0dd243b --- /dev/null +++ b/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 + - 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 +#include +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 -> 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; + + 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 , 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)); + } +} diff --git a/drive/arduino-sketch/arduino-sketch-test.ino b/drive/arduino-sketch/arduino-sketch-test.ino new file mode 100644 index 0000000..f001a2b --- /dev/null +++ b/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 + +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)); + } +}