Preempt stale manual drive controls

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.
+ *
+ * <p>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;
+  }
+}

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;

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());
+  }
+}