Switch to tank drive - one input per motor, no mixing

Arcade drive mixed forward and turn onto the left stick, so one stick
commanded both track motors. Replaced with tank drive and moved the head
off the right stick onto the d-pad, so every control now maps to exactly
one motor.

- left stick Y  -> left track      (was: mixed into both)
- right stick Y -> right track     (was: head tilt)
- d-pad U/D     -> head tilt       (was: right stick Y)
- d-pad L/R     -> head turn       (was: right stick X)
- Square/Circle -> left arm        (was: d-pad U/D)
- Triangle/Cross-> right arm       (unchanged)
- L2/R2         -> body lift       (unchanged)
- L1+R1 together-> all stop        (was: Circle)

Head is digital now rather than proportional, so HEAD_MAX drops to 45.
Stick X axes are unused. Also added a commented-out forgetBluetoothKeys()
call for clearing stale NVS pairings.
This commit is contained in:
2026-09-01 23:43:11 +10:00
parent 1ca4bda441
commit fffb4fcb0d
+26 -19
View File
@@ -7,6 +7,9 @@
*
* Model: Johnny 5 (Short Circuit) MOC, 7 motors across 2 hubs.
*
* CONTROL SCHEME: tank drive. Every input drives exactly one motor - no
* mixing. See docs/CONTROLS.md.
*
* Board package: esp32-bluepad32 4.1.0 (NOT the plain esp32 package)
* Libraries: none. No Legoino, no NimBLE - they would be a second host
* stack and that is exactly what we are avoiding.
@@ -76,11 +79,11 @@ static const int DEADZONE = 40; // raw stick counts, Bluepad32 range is +/-5
// stop, and there is no position feedback here - holding a direction at a stop
// stalls the motor. Keep these conservative; lower them if an axis feels forceful.
static const int TRACK_MAX = 100; // LWP3 power range is -100..100
static const int HEAD_MAX = 50;
static const int HEAD_MAX = 45; // digital now, so this is the only speed
static const int LIFT_MAX = 60;
static const int ARM_MAX = 45;
// Drive scaling: normal, L1 held (precision), R1 held (full).
// Track scaling: normal, L1 held (precision), R1 held (full).
static const int SCALE_NORMAL = 75;
static const int SCALE_PRECISION = 40;
static const int SCALE_FULL = 100;
@@ -522,6 +525,11 @@ void setup() {
BP32.setup(&onConnectedController, &onDisconnectedController);
BP32.enableVirtualDevice(false);
// Uncomment, flash once, re-pair, then comment out again if you ever end
// up with stale pairings. Bluepad32 keeps these in NVS and reflashing the
// sketch does not clear them.
// BP32.forgetBluetoothKeys();
// Hubs first. Gamepad discovery gets switched on once they are up.
BP32.enableNewBluetoothConnections(false);
@@ -556,7 +564,7 @@ void loop() {
if (gp == nullptr) {
clearDesired();
} else if (gp->b()) { // Circle - all stop
} else if (gp->l1() && gp->r1()) { // both shoulders together - all stop
clearDesired();
} else {
#if DEBUG_BUTTONS
@@ -570,31 +578,30 @@ void loop() {
}
#endif
// Arcade drive on the left stick. One stick for driving frees the
// right one entirely for the head, which is what Johnny emotes with.
int fwd = stickToPower(-gp->axisY(), TRACK_MAX);
int turn = stickToPower(gp->axisX(), TRACK_MAX);
// Tank drive. One stick per track, vertical axis only. Nothing is
// mixed, so a stick can never command two motors.
int scale = SCALE_NORMAL;
if (gp->l1()) scale = SCALE_PRECISION;
if (gp->r1()) scale = SCALE_FULL;
fwd = fwd * scale / 100;
turn = turn * scale / 100;
gDesired.leftTrack = constrain(fwd + turn, -TRACK_MAX, TRACK_MAX);
gDesired.rightTrack = constrain(fwd - turn, -TRACK_MAX, TRACK_MAX);
gDesired.leftTrack = stickToPower(-gp->axisY(), TRACK_MAX) * scale / 100;
gDesired.rightTrack = stickToPower(-gp->axisRY(), TRACK_MAX) * scale / 100;
gDesired.headTurn = stickToPower(gp->axisRX(), HEAD_MAX);
gDesired.headTilt = stickToPower(-gp->axisRY(), HEAD_MAX);
// Head on the d-pad. Up/down tilts, left/right turns. Pressing a
// diagonal will move both, but only because you asked it to.
uint8_t d = gp->dpad();
gDesired.headTilt = (d & DPAD_UP) ? HEAD_MAX
: (d & DPAD_DOWN) ? -HEAD_MAX : 0;
gDesired.headTurn = (d & DPAD_RIGHT) ? HEAD_MAX
: (d & DPAD_LEFT) ? -HEAD_MAX : 0;
// R2 raises, L2 lowers. Both are analog, 0..1023. Proportional control
// suits the heavy slow axis better than the sticks would.
// R2 raises, L2 lowers. Both are analog, 0..1023, so the lift keeps
// proportional control - it is the heavy axis that benefits most.
gDesired.bodyLift = constrain(
(gp->throttle() - gp->brake()) * LIFT_MAX / 1023, -LIFT_MAX, LIFT_MAX);
// Arms are pose-and-hold rather than modulate, so digital is fine.
uint8_t d = gp->dpad();
gDesired.leftArm = (d & DPAD_UP) ? ARM_MAX : (d & DPAD_DOWN) ? -ARM_MAX : 0;
// Arms on the face buttons. Square/Circle left, Triangle/Cross right.
gDesired.leftArm = gp->x() ? ARM_MAX : gp->b() ? -ARM_MAX : 0;
gDesired.rightArm = gp->y() ? ARM_MAX : gp->a() ? -ARM_MAX : 0;
}