add arduino sketch plus ps2 steering
This commit is contained in:
max
parent
35535f1792
commit
ac4425bb42
60
README.md
60
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
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
}
|
||||
|
|
@ -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));
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue