diff --git a/README.md b/README.md index a96e78e..49e9862 100644 --- a/README.md +++ b/README.md @@ -180,4 +180,40 @@ slow backward r 60 +## Web Control + +### Setup + + ln -s /opt/helva-robot/drive/opt/drive-ctl/ /opt/ + sudo apt update + sudo apt install -y python3-venv + python3 -m venv /opt/drive-ctl-venv + source /opt/drive-ctl-venv/bin/activate + pip install fastapi uvicorn pyserial + + +### Activate Web Control Interface + + sudo ln -s /opt/helva-robot/drive/etc/nginx/sites-available/drive /etc/nginx/sites-enabled/drive + sudo nginx -t && sudo systemctl reload nginx + + +### Systemd Service for Backend + + ln -s /opt/helva-robot/drive/etc/systemd/system/drive-ctl.service /etc/systemd/system/ + sudo systemctl daemon-reload + sudo systemctl enable --now drive-ctl + sudo systemctl status drive-ctl --no-pager -l + +Check Serial device is stable, otherwise create a udev rule + + ls -l /dev/ttyACM0 /dev/ttyUSB0 + dmesg | tail -n 50 + + + +### Open Web Control UI + + http://pi-ip/drive/ + 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 new file mode 100644 index 0000000..d54efad --- /dev/null +++ b/drive/arduino-sketch/arduino-sketch-ps2-controller.ino/arduino-sketch-ps2-controller.ino.ino @@ -0,0 +1,132 @@ +// ===== 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/etc/nginx/snippets/drive.conf b/drive/etc/nginx/snippets/drive.conf new file mode 100644 index 0000000..b4e9bd2 --- /dev/null +++ b/drive/etc/nginx/snippets/drive.conf @@ -0,0 +1,18 @@ + + location /drive/ { + alias /opt/helva-robot/drive/var/www/drive/; + try_files $uri $uri/ /drive/index.html; + } + + location /drive-ws { + proxy_pass http://127.0.0.1:8002/ws; + proxy_http_version 1.1; + proxy_set_header Upgrade $http_upgrade; + proxy_set_header Connection "Upgrade"; + proxy_set_header Host $host; + } + + location /drive-health { + proxy_pass http://127.0.0.1:8002/health; + } + diff --git a/drive/etc/systemd/system/drive-ctl.service b/drive/etc/systemd/system/drive-ctl.service new file mode 100644 index 0000000..a1ab46e --- /dev/null +++ b/drive/etc/systemd/system/drive-ctl.service @@ -0,0 +1,13 @@ +[Unit] +Description=Robot Drive Controller (FastAPI -> Serial) +After=network.target + +[Service] +WorkingDirectory=/opt/drive-ctl +ExecStart=/opt/drive-ctl-venv/bin/uvicorn server:app --host 127.0.0.1 --port 8002 --app-dir /opt/drive-ctl +Restart=always +RestartSec=1 + +[Install] +WantedBy=multi-user.target + diff --git a/drive/images/ps2-controller.gif b/drive/images/ps2-controller.gif new file mode 100644 index 0000000..fea3b9c Binary files /dev/null and b/drive/images/ps2-controller.gif differ diff --git a/drive/opt/drive-ctl/server.py b/drive/opt/drive-ctl/server.py new file mode 100644 index 0000000..955f76f --- /dev/null +++ b/drive/opt/drive-ctl/server.py @@ -0,0 +1,65 @@ +import asyncio +import json +import time +import serial +from fastapi import FastAPI, WebSocket, WebSocketDisconnect +from fastapi.responses import HTMLResponse + +SERIAL_PORT = "/dev/ttyACM0" # ggf. /dev/ttyUSB0 +BAUD = 115200 + +# Safety: if websocket stops sending, we also stop. +STOP_AFTER_SEC = 0.35 + +app = FastAPI() + +ser = serial.Serial(SERIAL_PORT, BAUD, timeout=0) + +last_msg_ts = 0.0 + +def clamp255(v: int) -> int: + return max(-255, min(255, int(v))) + +def send_lr(l: int, r: int): + l = clamp255(l) + r = clamp255(r) + line = f"L {l} R {r}\n".encode("ascii") + ser.write(line) + +@app.get("/health") +def health(): + return {"ok": True, "serial": SERIAL_PORT} + +@app.websocket("/ws") +async def ws(websocket: WebSocket): + global last_msg_ts + await websocket.accept() + + async def watchdog(): + while True: + await asyncio.sleep(0.05) + if time.time() - last_msg_ts > STOP_AFTER_SEC: + send_lr(0, 0) + + wd_task = asyncio.create_task(watchdog()) + + try: + while True: + msg = await websocket.receive_text() + last_msg_ts = time.time() + + try: + data = json.loads(msg) + l = clamp255(data.get("l", 0)) + r = clamp255(data.get("r", 0)) + send_lr(l, r) + except Exception: + # On parse errors: stop + send_lr(0, 0) + + await websocket.send_text("ok") + except WebSocketDisconnect: + send_lr(0, 0) + finally: + wd_task.cancel() + diff --git a/drive/var/www/drive/index.html b/drive/var/www/drive/index.html new file mode 100644 index 0000000..d305c59 --- /dev/null +++ b/drive/var/www/drive/index.html @@ -0,0 +1,315 @@ + + + + + + Robot Dual Stick + + + +
+
+
๐ŸŽฎ Dual Stick Drive
+
offline
+
+ +
+
+
+ +
Links: Gas (โ†‘/โ†“)
+
+
+ +
Rechts: Lenken (โ†/โ†’)
+
+
+
+ +
+
+
Max Speed: 180
+ +
T 0 | Turn 0 โ†’ L 0 | R 0
+
+
+ + +
+
+ +
+ Loslassen = STOP. Wenn Verbindung/Tab weg: STOP (Watchdog). +
+
+ + + + + diff --git a/face/etc/nginx/sites-available/face b/face/etc/nginx/sites-available/face index acd1aa3..db0e50c 100644 --- a/face/etc/nginx/sites-available/face +++ b/face/etc/nginx/sites-available/face @@ -30,5 +30,7 @@ server { add_header Cache-Control no-cache; } + + include /opt/helva-robot/drive/etc/nginx/snippets/drive.conf; }