Extension graph

src/main/java/frc/robot/Robot.java

@@ -48,6 +48,7 @@
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.subsystems.ExtensionKinematics;
 import frc.robot.subsystems.ExtensionKinematics.ExtensionState;
+import frc.robot.subsystems.ExtensionPathing;
 import frc.robot.subsystems.FunnelSubsystem;
 import frc.robot.subsystems.ManipulatorSubsystem;
 import frc.robot.subsystems.Superstructure;
@@ -123,6 +124,9 @@ private RobotHardware(SwerveConstants swerveConstants) {
   public static final RobotType ROBOT_TYPE = Robot.isReal() ? RobotType.REAL : RobotType.SIM;
   // For replay to work properly this should match the hardware used in the log
   public static final RobotHardware ROBOT_HARDWARE = RobotHardware.KELPIE;
+  // for testing class loading
+  public static final ExtensionState test =
+      ExtensionPathing.getNearest(new ExtensionState(0.0, Rotation2d.kZero, Rotation2d.kZero));
 
   public static enum ReefTarget {
     L1(
@@ -338,7 +342,7 @@ public static enum AlgaeScoreTarget {
                       .withMotionMagic(WristSubsystem.DEFAULT_MOTION_MAGIC)
                       .withCurrentLimits(
                           new CurrentLimitsConfigs()
-                              .withStatorCurrentLimit(30.0)
+                              .withStatorCurrentLimit(50.0)
                               .withStatorCurrentLimitEnable(true)
                               .withSupplyCurrentLimit(20.0)
                               .withSupplyCurrentLimitEnable(true))
@@ -618,6 +622,60 @@ public Robot() {
                 })
             .ignoringDisable(true));
 
+    System.out.println("Node Count " + ExtensionPathing.graph.nodes().size());
+
+    SmartDashboard.putData(
+        "Traverse Extension Graph",
+        superstructure
+            .extendWithClearance(
+                () ->
+                    new ExtensionState(
+                        ElevatorSubsystem.HP_EXTENSION_METERS,
+                        ShoulderSubsystem.SHOULDER_HP_POS,
+                        WristSubsystem.WRIST_HP_POS))
+            .until(
+                () ->
+                    elevator.isNearExtension(ElevatorSubsystem.HP_EXTENSION_METERS)
+                        && shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_HP_POS)
+                        && wrist.isNearAngle(WristSubsystem.WRIST_HP_POS))
+            .andThen(
+                Commands.sequence(
+                    ExtensionPathing.graph.nodes().stream()
+                        .map(
+                            (node) ->
+                                superstructure
+                                    .extendWithClearance(() -> node)
+                                    .alongWith(
+                                        Commands.print("Traversing to " + node),
+                                        Commands.runOnce(
+                                            () -> Logger.recordOutput("Traversal Target", node)))
+                                    .until(
+                                        () ->
+                                            elevator.isNearExtension(node.elevatorHeightMeters())
+                                                && shoulder.isNearAngle(node.shoulderAngle())
+                                                && wrist.isNearAngle(node.wristAngle()))
+                                    .finallyDo(() -> System.out.println("done"))
+                                    .andThen(
+                                        Commands.waitSeconds(0.5),
+                                        superstructure
+                                            .extendWithClearance(
+                                                () ->
+                                                    new ExtensionState(
+                                                        ElevatorSubsystem.HP_EXTENSION_METERS,
+                                                        ShoulderSubsystem.SHOULDER_HP_POS,
+                                                        WristSubsystem.WRIST_HP_POS))
+                                            .alongWith(Commands.print("Retracting"))
+                                            .until(
+                                                () ->
+                                                    elevator.isNearExtension(
+                                                            ElevatorSubsystem.HP_EXTENSION_METERS)
+                                                        && shoulder.isNearAngle(
+                                                            ShoulderSubsystem.SHOULDER_HP_POS)
+                                                        && wrist.isNearAngle(
+                                                            WristSubsystem.WRIST_HP_POS)),
+                                        Commands.waitSeconds(0.5)))
+                        .toArray(Command[]::new))));
+
     // Run auto when auto starts. Matches Choreolib's defer impl
     RobotModeTriggers.autonomous()
         .whileTrue(Commands.defer(() -> autoChooser.get().asProxy(), Set.of()));
@@ -701,9 +759,9 @@ public Robot() {
 
     manipulator.setDefaultCommand(manipulator.hold());
 
-    shoulder.setDefaultCommand(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_RETRACTED_POS));
+    shoulder.setDefaultCommand(shoulder.hold());
 
-    wrist.setDefaultCommand(wrist.setTargetAngle(WristSubsystem.WRIST_RETRACTED_POS));
+    wrist.setDefaultCommand(wrist.hold());
 
     funnel.setDefaultCommand(funnel.setVoltage(0.0));
 

src/main/java/frc/robot/subsystems/ExtensionPathing.java

@@ -0,0 +1,199 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import com.google.common.collect.Sets;
+import com.google.common.graph.GraphBuilder;
+import com.google.common.graph.MutableGraph;
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.geometry.Rotation2d;
+import frc.robot.subsystems.ExtensionKinematics.ExtensionState;
+import frc.robot.subsystems.elevator.ElevatorSubsystem;
+import frc.robot.subsystems.shoulder.ShoulderSubsystem;
+import frc.robot.subsystems.wrist.WristSubsystem;
+import java.util.ArrayList;
+import java.util.Comparator;
+import java.util.HashMap;
+import java.util.List;
+import java.util.Map;
+import java.util.Set;
+
+public class ExtensionPathing {
+  public static final MutableGraph<ExtensionState> graph =
+      GraphBuilder.undirected().allowsSelfLoops(true).build();
+  // TODO make this cache distances so we can do partial caches
+  private static final Map<Pair<ExtensionState, ExtensionState>, List<ExtensionState>> cache =
+      new HashMap<>();
+
+  static {
+    final var hp =
+        new ExtensionState(
+            ElevatorSubsystem.HP_EXTENSION_METERS,
+            ShoulderSubsystem.SHOULDER_HP_POS,
+            WristSubsystem.WRIST_HP_POS);
+    graph.addNode(hp);
+    final var tucked =
+        new ExtensionState(
+            0.0,
+            ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS,
+            WristSubsystem.WRIST_TUCKED_CLEARANCE_POS);
+    graph.addNode(tucked);
+    graph.putEdge(hp, tucked);
+    final var l2Tucked =
+        new ExtensionState(
+            ExtensionKinematics.L2_EXTENSION.elevatorHeightMeters(),
+            ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS,
+            WristSubsystem.WRIST_TUCKED_CLEARANCE_POS);
+    graph.addNode(l2Tucked);
+    graph.putEdge(tucked, l2Tucked);
+    final var l3Tucked =
+        new ExtensionState(
+            ExtensionKinematics.L3_EXTENSION.elevatorHeightMeters(),
+            ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS,
+            WristSubsystem.WRIST_TUCKED_CLEARANCE_POS);
+    graph.addNode(l3Tucked);
+    graph.putEdge(tucked, l3Tucked);
+    graph.putEdge(l3Tucked, l2Tucked);
+    final var l4Tucked =
+        new ExtensionState(
+            ExtensionKinematics.L4_EXTENSION.elevatorHeightMeters(),
+            ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS,
+            WristSubsystem.WRIST_TUCKED_CLEARANCE_POS);
+    graph.addNode(l4Tucked);
+    graph.putEdge(tucked, l4Tucked);
+    graph.putEdge(l4Tucked, l3Tucked);
+    graph.putEdge(l4Tucked, l2Tucked);
+    final var l4TuckedOut =
+        new ExtensionState(
+            ExtensionKinematics.L4_EXTENSION.elevatorHeightMeters(),
+            Rotation2d.fromDegrees(25.0),
+            Rotation2d.fromDegrees(120.0));
+    graph.addNode(l4TuckedOut);
+    graph.putEdge(l4Tucked, l4TuckedOut);
+
+    final var betweenTucked =
+        new ExtensionState(0.0, Rotation2d.fromDegrees(35.0), WristSubsystem.WRIST_CLEARANCE_POS);
+    graph.addNode(betweenTucked);
+    graph.putEdge(tucked, betweenTucked);
+
+    final var untucked =
+        new ExtensionState(
+            0.0, ShoulderSubsystem.SHOULDER_CLEARANCE_POS, WristSubsystem.WRIST_CLEARANCE_POS);
+    graph.addNode(untucked);
+    graph.putEdge(betweenTucked, untucked);
+
+    final var preCoralGround =
+        new ExtensionState(
+            ElevatorSubsystem.GROUND_EXTENSION_METERS,
+            ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS,
+            Rotation2d.kCCW_90deg);
+    graph.addNode(preCoralGround);
+    graph.putEdge(tucked, preCoralGround);
+
+    final var coralGround =
+        new ExtensionState(
+            ElevatorSubsystem.GROUND_EXTENSION_METERS,
+            ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS,
+            WristSubsystem.WRIST_CORAL_GROUND);
+    graph.addNode(coralGround);
+    graph.putEdge(preCoralGround, coralGround);
+
+    final var retracted =
+        new ExtensionState(
+            0.0, ShoulderSubsystem.SHOULDER_RETRACTED_POS, WristSubsystem.WRIST_RETRACTED_POS);
+    graph.addNode(retracted);
+    graph.putEdge(untucked, retracted);
+    graph.putEdge(betweenTucked, retracted);
+
+    final var l1 = ExtensionKinematics.L1_EXTENSION;
+    graph.addNode(l1);
+    graph.putEdge(l1, betweenTucked);
+  }
+
+  private ExtensionPathing() {}
+
+  public static ExtensionState getNearest(ExtensionState state) {
+    return graph.nodes().stream()
+        .min(
+            (a, b) -> {
+              var aFK = ExtensionKinematics.solveFK(a);
+              var bFK = ExtensionKinematics.solveFK(b);
+              var stateFK = ExtensionKinematics.solveFK(state);
+              return (int)
+                  (1000
+                          * (aFK.getTranslation().getDistance(stateFK.getTranslation())
+                              + Math.abs(
+                                  aFK.getRotation().getRotations()
+                                      - stateFK.getRotation().getRotations()))
+                      - 1000
+                          * (bFK.getTranslation().getDistance(stateFK.getTranslation())
+                              + Math.abs(
+                                  bFK.getRotation().getRotations()
+                                      - stateFK.getRotation().getRotations())));
+            })
+        .get();
+  }
+
+  private record TotalMotion(double elevator, double shoulderRotations, double wristRotations) {
+    public TotalMotion plus(double elevator, double shoulderRotations, double wristRotations) {
+      return new TotalMotion(
+          this.elevator + elevator,
+          this.shoulderRotations + shoulderRotations,
+          this.wristRotations + wristRotations);
+    }
+  }
+
+  private static Pair<List<ExtensionState>, TotalMotion> search(
+      ExtensionState current, ExtensionState target, Set<ExtensionState> visited) {
+    if (current == target) {
+      List<ExtensionState> result = new ArrayList<>();
+      result.add(target);
+      return Pair.of(result, new TotalMotion(0, 0, 0));
+    }
+
+    final var edges = Sets.difference(graph.successors(current), visited);
+    final List<Pair<List<ExtensionState>, TotalMotion>> result = new ArrayList<>();
+    for (var edge : edges) {
+      final var path = search(edge, target, Sets.union(visited, Set.of(current)));
+      if (path != null) result.add(path);
+    }
+    if (result.size() == 0) return null;
+    final var best =
+        result.stream()
+            .min(
+                Comparator.comparing(
+                        (Pair<List<ExtensionState>, TotalMotion> s) -> s.getSecond().elevator)
+                    .thenComparing((s) -> s.getSecond().wristRotations)
+                    .thenComparing((s) -> s.getSecond().shoulderRotations))
+            .get();
+    final List<ExtensionState> newList = new ArrayList<>();
+    newList.add(current);
+    newList.addAll(best.getFirst());
+    return Pair.of(
+        newList,
+        best.getSecond()
+            .plus(
+                Math.abs(
+                    current.elevatorHeightMeters() - best.getFirst().get(0).elevatorHeightMeters()),
+                Math.abs(
+                    current.shoulderAngle().getRotations()
+                        - best.getFirst().get(0).shoulderAngle().getRotations()),
+                Math.abs(
+                    current.wristAngle().getRotations()
+                        - best.getFirst().get(0).wristAngle().getRotations())));
+  }
+
+  public static List<ExtensionState> getPath(ExtensionState current, ExtensionState target) {
+    final var nearestCurrent = getNearest(current);
+    final var nearestTarget = getNearest(target);
+    final var path =
+        cache.containsKey(Pair.of(nearestCurrent, nearestTarget))
+            ? cache.get(Pair.of(nearestCurrent, nearestTarget))
+            : search(nearestCurrent, nearestTarget, Set.of()).getFirst();
+    path.add(target);
+    cache.putIfAbsent(Pair.of(nearestCurrent, nearestTarget), path);
+    return path;
+  }
+}

src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java

@@ -78,12 +78,12 @@ public void periodic() {
     if (Robot.ROBOT_TYPE != RobotType.REAL)
       Logger.recordOutput(NAME + "/Filtered Current", currentFilterValue);
 
-    if (firstBBInputs.get && !secondBBInputs.get) {
+    if (getFirstBeambreak() && !getSecondBeambreak()) {
       Tracer.trace("Manipulator/Zero", () -> io.resetEncoder(0.0));
       zeroTimer.reset();
     }
 
-    if (!firstBBInputs.get && secondBBInputs.get) {
+    if (!getFirstBeambreak() && getSecondBeambreak()) {
       // Number calculated from coral length, may need tuning
       Tracer.trace("Manipulator/Zero", () -> io.resetEncoder(1.0));
       zeroTimer.reset();
@@ -95,16 +95,16 @@ public void periodic() {
   public Command index() {
     return Commands.sequence(
         setVelocity(9.0)
-            .until(() -> firstBBInputs.get || secondBBInputs.get)
-            .unless(() -> firstBBInputs.get),
-        setVelocity(3.0).until(() -> secondBBInputs.get).unless(() -> secondBBInputs.get),
+            .until(() -> getFirstBeambreak() || getSecondBeambreak())
+            .unless(() -> getFirstBeambreak()),
+        setVelocity(3.0).until(() -> getSecondBeambreak()).unless(() -> getSecondBeambreak()),
         setVelocity(-3.0)
-            .until(() -> firstBBInputs.get && !secondBBInputs.get)
+            .until(() -> getFirstBeambreak() && !getSecondBeambreak())
             .unless(() -> zeroTimer.get() < 0.25),
         // TODO tune timeout
         // Commands.runOnce(() -> io.resetEncoder(0.0)),
         Commands.run(() -> io.setPosition(Rotation2d.fromRotations(1.1)))
-            .until(() -> !firstBBInputs.get && !secondBBInputs.get));
+            .until(() -> !getFirstBeambreak() && !getSecondBeambreak()));
   } // TODO check if anything got lost in merge?
 
   public Command jog(double rotations) {
@@ -146,26 +146,26 @@ public Command intakeCoral() {
   public Command intakeCoralAir(double vel) {
     return Commands.sequence(
         setVelocity(vel)
-            .until(() -> secondBBInputs.get)
+            .until(() -> getSecondBeambreak())
             .finallyDo(
                 () -> {
                   io.setPosition(Rotation2d.fromRotations(0.63));
                   positionSetpoint = 0.63;
                 }),
-        setVoltage(2.0).until(() -> !firstBBInputs.get),
-        jog(CORAL_HOLD_POS).until(() -> !secondBBInputs.get && !firstBBInputs.get));
+        setVoltage(2.0).until(() -> !getFirstBeambreak()),
+        jog(CORAL_HOLD_POS).until(() -> !getSecondBeambreak() && !getFirstBeambreak()));
   }
 
   public Command intakeCoral(double vel) {
     return Commands.sequence(
-        setVelocity(vel).until(new Trigger(() -> secondBBInputs.get).debounce(0.5)),
+        setVelocity(vel).until(new Trigger(() -> getSecondBeambreak()).debounce(0.5)),
         Commands.runOnce(
             () -> {
               io.setPosition(Rotation2d.fromRotations(0.5));
               positionSetpoint = 0.5;
             }),
-        setVelocity(1.0).until(() -> !firstBBInputs.get),
-        jog(CORAL_HOLD_POS).until(() -> !secondBBInputs.get && !firstBBInputs.get));
+        setVelocity(1.0).until(() -> !getFirstBeambreak()),
+        jog(CORAL_HOLD_POS).until(() -> !getSecondBeambreak() && !getFirstBeambreak()));
   }
 
   public Command intakeAlgae() {