diff --git a/android/robot/src/main/java/org/openbot/cartfollow/ManualControlArbiter.java b/android/robot/src/main/java/org/openbot/cartfollow/ManualControlArbiter.java new file mode 100644 index 000000000..74fb0a468 --- /dev/null +++ b/android/robot/src/main/java/org/openbot/cartfollow/ManualControlArbiter.java @@ -0,0 +1,61 @@ +package org.openbot.cartfollow; + +/** + * Owns the single active manual direction control for the real-cart screen. + * + *

A newly pressed direction replaces the preceding one. Releases are accepted only from the + * currently active direction, so a delayed release from an old button cannot stop or overwrite a + * newer command. + */ +final class ManualControlArbiter { + enum Control { + FORWARD, + BACKWARD, + LEFT, + RIGHT + } + + static final class PressResult { + final boolean replacedActiveControl; + final long generation; + + private PressResult(boolean replacedActiveControl, long generation) { + this.replacedActiveControl = replacedActiveControl; + this.generation = generation; + } + } + + private Control activeControl; + private long generation; + + synchronized PressResult press(Control control) { + if (control == null) throw new IllegalArgumentException("control must not be null"); + + boolean replaced = activeControl != null && activeControl != control; + if (activeControl != control) generation++; + activeControl = control; + return new PressResult(replaced, generation); + } + + synchronized boolean release(Control control) { + if (control == null || activeControl != control) return false; + activeControl = null; + generation++; + return true; + } + + synchronized boolean clear() { + if (activeControl == null) return false; + activeControl = null; + generation++; + return true; + } + + synchronized Control getActiveControl() { + return activeControl; + } + + synchronized long getGeneration() { + return generation; + } +} diff --git a/android/robot/src/main/java/org/openbot/cartfollow/RealCartFollowFragment.java b/android/robot/src/main/java/org/openbot/cartfollow/RealCartFollowFragment.java index 539c64481..c94e259bc 100644 --- a/android/robot/src/main/java/org/openbot/cartfollow/RealCartFollowFragment.java +++ b/android/robot/src/main/java/org/openbot/cartfollow/RealCartFollowFragment.java @@ -16,11 +16,13 @@ public class RealCartFollowFragment extends BaseCartFollowFragment { private static final long AUTO_UNLOCK_HOLD_MS = 2000L; private final RealCartSafetyController safetyController = new RealCartSafetyController(); + private final ManualControlArbiter manualControlArbiter = new ManualControlArbiter(); private final Handler mainHandler = new Handler(Looper.getMainLooper()); private volatile RealCartSafetyController.Output latestOutput = RealCartSafetyController.stop("idle"); private boolean schedulerRunning; private long lastHandshakeRequestMs; + private View activeManualButton; private final Runnable commandScheduler = new Runnable() { @@ -62,18 +64,22 @@ protected void onCartFollowViewCreated() { installDeadMan( binding.driveForward, + ManualControlArbiter.Control.FORWARD, RealCartSafetyController.MANUAL_FORWARD, RealCartSafetyController.MANUAL_FORWARD); installDeadMan( binding.driveBackward, + ManualControlArbiter.Control.BACKWARD, -RealCartSafetyController.MANUAL_REVERSE, -RealCartSafetyController.MANUAL_REVERSE); installDeadMan( binding.driveLeft, + ManualControlArbiter.Control.LEFT, -RealCartSafetyController.MANUAL_TURN, RealCartSafetyController.MANUAL_TURN); installDeadMan( binding.driveRight, + ManualControlArbiter.Control.RIGHT, RealCartSafetyController.MANUAL_TURN, -RealCartSafetyController.MANUAL_TURN); @@ -83,7 +89,7 @@ protected void onCartFollowViewCreated() { binding.emergencyStop.setOnClickListener( v -> { safetyController.latchEmergency(); - latestOutput = RealCartSafetyController.stop("emergency_stop"); + invalidateManualControl("emergency_stop", true); vehicle.emergencyStop(); binding.startSwitch.setChecked(false); binding.startSwitch.setEnabled(false); @@ -104,9 +110,7 @@ public synchronized void onResume() { @Override protected void onCartFollowPause() { safetyController.setForeground(false); - latestOutput = RealCartSafetyController.stop("paused"); - sendOutput(latestOutput); - vehicle.stopBot(); + invalidateManualControl("paused", true); vehicle.stopHeartbeat(); schedulerRunning = false; mainHandler.removeCallbacks(commandScheduler); @@ -145,8 +149,7 @@ protected void processUSBData(String data) { } private void setMode(RealCartSafetyController.Mode mode) { - latestOutput = RealCartSafetyController.stop("mode_change"); - sendOutput(latestOutput); + invalidateManualControl("mode_change", true); safetyController.setMode(mode); stateMachine.cancel(); binding.startSwitch.setChecked(false); @@ -158,20 +161,43 @@ private void setMode(RealCartSafetyController.Mode mode) { refreshRealUi(); } - private void installDeadMan(View button, int left, int right) { + private void installDeadMan( + View button, ManualControlArbiter.Control control, int left, int right) { button.setOnTouchListener( (view, event) -> { switch (event.getActionMasked()) { case MotionEvent.ACTION_DOWN: - latestOutput = safetyController.manual(left, right); + RealCartSafetyController.Output nextOutput = safetyController.manual(left, right); + if (nextOutput.isStop()) { + invalidateManualControl("manual_blocked", true); + view.setPressed(false); + return true; + } + + ManualControlArbiter.PressResult pressResult = manualControlArbiter.press(control); + if (pressResult.replacedActiveControl) { + // c0,0 bypasses the firmware ramp. Send it before the new target so the AT8236 + // starts the replacement direction from a known zero output. + latestOutput = RealCartSafetyController.stop("manual_replace"); + sendOutput(latestOutput); + if (activeManualButton != null && activeManualButton != view) { + activeManualButton.setPressed(false); + } + } + activeManualButton = view; + latestOutput = nextOutput; + sendOutput(latestOutput); view.setPressed(true); return true; case MotionEvent.ACTION_UP: case MotionEvent.ACTION_CANCEL: case MotionEvent.ACTION_OUTSIDE: - latestOutput = RealCartSafetyController.stop("manual_release"); - sendOutput(latestOutput); view.setPressed(false); + if (manualControlArbiter.release(control)) { + activeManualButton = null; + latestOutput = RealCartSafetyController.stop("manual_release"); + sendOutput(latestOutput); + } return true; default: return true; @@ -205,11 +231,19 @@ private void updateConnectionState() { boolean serialReady = vehicle != null && vehicle.isBleSerialReady(); boolean firmwareReady = vehicle != null && vehicle.isCartFirmwareReady(); safetyController.setConnection(serialReady, firmwareReady); - if (!firmwareReady && latestOutput != null && !latestOutput.isStop()) { - latestOutput = RealCartSafetyController.stop("ble_not_ready"); + if (!firmwareReady) { + invalidateManualControl("ble_not_ready", false); } } + private void invalidateManualControl(String reason, boolean sendStop) { + manualControlArbiter.clear(); + if (activeManualButton != null) activeManualButton.setPressed(false); + activeManualButton = null; + latestOutput = RealCartSafetyController.stop(reason); + if (sendStop) sendOutput(latestOutput); + } + private void startScheduler() { if (schedulerRunning) return; schedulerRunning = true; diff --git a/android/robot/src/test/java/org/openbot/cartfollow/ManualControlArbiterTest.java b/android/robot/src/test/java/org/openbot/cartfollow/ManualControlArbiterTest.java new file mode 100644 index 000000000..d24e8dcfe --- /dev/null +++ b/android/robot/src/test/java/org/openbot/cartfollow/ManualControlArbiterTest.java @@ -0,0 +1,62 @@ +package org.openbot.cartfollow; + +import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertFalse; +import static org.junit.Assert.assertTrue; + +import org.junit.Test; + +public class ManualControlArbiterTest { + @Test + public void firstPressBecomesActiveWithoutReplacement() { + ManualControlArbiter arbiter = new ManualControlArbiter(); + + ManualControlArbiter.PressResult result = + arbiter.press(ManualControlArbiter.Control.FORWARD); + + assertFalse(result.replacedActiveControl); + assertEquals(ManualControlArbiter.Control.FORWARD, arbiter.getActiveControl()); + assertEquals(1L, result.generation); + } + + @Test + public void differentPressReplacesThePreviousControl() { + ManualControlArbiter arbiter = new ManualControlArbiter(); + arbiter.press(ManualControlArbiter.Control.FORWARD); + + ManualControlArbiter.PressResult result = arbiter.press(ManualControlArbiter.Control.LEFT); + + assertTrue(result.replacedActiveControl); + assertEquals(ManualControlArbiter.Control.LEFT, arbiter.getActiveControl()); + assertEquals(2L, result.generation); + } + + @Test + public void delayedReleaseFromReplacedControlIsIgnored() { + ManualControlArbiter arbiter = new ManualControlArbiter(); + arbiter.press(ManualControlArbiter.Control.FORWARD); + arbiter.press(ManualControlArbiter.Control.LEFT); + + assertFalse(arbiter.release(ManualControlArbiter.Control.FORWARD)); + assertEquals(ManualControlArbiter.Control.LEFT, arbiter.getActiveControl()); + } + + @Test + public void activeReleaseStopsManualOwnership() { + ManualControlArbiter arbiter = new ManualControlArbiter(); + arbiter.press(ManualControlArbiter.Control.RIGHT); + + assertTrue(arbiter.release(ManualControlArbiter.Control.RIGHT)); + assertEquals(null, arbiter.getActiveControl()); + } + + @Test + public void clearInvalidatesAnyLaterRelease() { + ManualControlArbiter arbiter = new ManualControlArbiter(); + arbiter.press(ManualControlArbiter.Control.BACKWARD); + + assertTrue(arbiter.clear()); + assertFalse(arbiter.release(ManualControlArbiter.Control.BACKWARD)); + assertEquals(null, arbiter.getActiveControl()); + } +}