diff --git a/ps4_lego_onebrain/ps4_lego_onebrain.ino b/ps4_lego_onebrain/ps4_lego_onebrain.ino index fe78fff..112d9c2 100644 --- a/ps4_lego_onebrain/ps4_lego_onebrain.ino +++ b/ps4_lego_onebrain/ps4_lego_onebrain.ino @@ -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; }