algae removal after scoring

simgui-ds.json

@@ -110,8 +110,7 @@
       "guid": "Keyboard0"
     },
     {
-      "guid": "78696e70757401000000000000000000",
-      "useGamepad": true
+      "guid": "Keyboard1"
     }
   ],
   "useEnableDisableHotkeys": true

simgui-window.json

@@ -18,6 +18,11 @@
     }
   },
   "Table": {
+    "0x965B9C9E,2": {
+      "Column 0  Width": "160",
+      "Column 1  Weight": "1.0000",
+      "RefScale": "16"
+    },
     "0xE56EC1C2,4": {
       "Column 0  Weight": "1.0000",
       "Column 1  Weight": "1.0000",
@@ -43,12 +48,12 @@
     },
     "###/SmartDashboard/Field2d": {
       "Collapsed": "0",
-      "Pos": "8,639",
+      "Pos": "11,639",
       "Size": "650,348"
     },
     "###/SmartDashboard/LeftView": {
       "Collapsed": "0",
-      "Pos": "1675,640",
+      "Pos": "1684,663",
       "Size": "544,547"
     },
     "###/SmartDashboard/Scheduler": {
@@ -73,7 +78,7 @@
     },
     "###/SmartDashboard/frontView": {
       "Collapsed": "0",
-      "Pos": "1299,640",
+      "Pos": "1297,660",
       "Size": "373,555"
     },
     "###Addressable LEDs": {

simgui.json

@@ -58,15 +58,27 @@
         },
         "right": 3466,
         "top": 291,
-        "width": 17.54825210571289
+        "width": 17.54825210571289,
+        "window": {
+          "visible": true
+        }
       },
       "/SmartDashboard/Field2d": {
-        "bottom": 1476,
-        "height": 8.210550308227539,
-        "left": 150,
-        "right": 2961,
-        "top": 79,
-        "width": 16.541748046875,
+        "bottom": 1638,
+        "builtin": "2025 Reefscape",
+        "height": 8.051901817321777,
+        "left": 534,
+        "limelight-back": {
+          "arrows": false,
+          "style": "Hidden"
+        },
+        "limelight-front": {
+          "arrows": false,
+          "style": "Hidden"
+        },
+        "right": 3466,
+        "top": 291,
+        "width": 17.54825210571289,
         "window": {
           "visible": true
         }
@@ -134,6 +146,15 @@
   "NetworkTables": {
     "transitory": {
       "CameraPublisher": {
+        "limelight-back-processed": {
+          "open": true
+        },
+        "limelight-back-raw": {
+          "open": true
+        },
+        "limelight-front-processed": {
+          "open": true
+        },
         "open": true
       },
       "Robot": {
@@ -157,6 +178,9 @@
     }
   },
   "NetworkTables Info": {
+    "Server": {
+      "open": true
+    },
     "visible": true
   }
 }

src/main/java/frc/robot/RobotStates.java

@@ -23,7 +23,8 @@ public class RobotStates {
     private static final Pilot pilot = Robot.getPilot();
     private static final Operator operator = Robot.getOperator();
 
-    @Getter private static double scoreTime = 3.0;
+    @Getter private static double scoreTime = 1.0;
+    @Getter private static double scoreTimeAlgae = 0.5;
 
     // Robot States
     // These are states that aren't directly tied to hardware or buttons, etc.
@@ -39,10 +40,13 @@ public class RobotStates {
     public static final SpectrumState rightScore = new SpectrumState("rightScore");
     public static final SpectrumState reverse = new SpectrumState("reverse");
     public static final SpectrumState actionPrepState = new SpectrumState("actionPrepState");
+    public static final SpectrumState algaeAfterAction = new SpectrumState("algaeAfterAction");
     public static final SpectrumState actionState = new SpectrumState("actionState");
     public static final SpectrumState homeAll = new SpectrumState("homeAll");
     public static final SpectrumState autonStationIntake = new SpectrumState("autonStationIntake");
 
+    public static final SpectrumState testL3 = new SpectrumState("testL3");
+
     /**
      * Define Robot States here and how they can be triggered States should be triggers that command
      * multiple mechanism or can be used in teleop or auton Use onTrue/whileTrue to run a command
@@ -97,6 +101,9 @@ public class RobotStates {
     public static final Trigger isAtHome =
             ElevatorStates.isHome.and(ElbowStates.isHome, ShoulderStates.isHome);
 
+    public static final Trigger isL3Algae = new Trigger(() -> Robot.getVision().isL3Algae());
+    public static final Trigger seesTag = new Trigger(() -> Robot.getVision().seesTag());
+
     // reset triggers
     public static final Trigger homeElevator = operator.homeElevator_A;
 
@@ -112,9 +119,12 @@ public static void setupStates() {
         pilot.home_select.or(operator.home_select).onFalse(clearStates());
         autonClearStates.whileTrue(clearStates());
 
-        actionState
-                .or(operator.staged)
-                .onChangeToFalse(coral.setFalse().alongWith(algae.setFalse()));
+        actionState.or(operator.staged).onChangeToFalse(coral.setFalse());
+        algaeAfterAction
+                .not()
+                .and(actionState.or(operator.staged))
+                .onChangeToFalse(algae.setFalse());
+        algae.and(actionState).onTrue(algaeAfterAction.setFalse());
         isAtHome.onTrue(homeAll.setFalse());
 
         pilot.coastOn_dB.or(operator.coastOn_dB).onTrue(coastMode.setTrue().ignoringDisable(true));
@@ -138,6 +148,8 @@ public static void setupStates() {
 
         operator.algaeStage.or(operator.coralStage).onTrue(actionState.setFalse());
 
+        pilot.algaeRemovalAfterScore.onTrue(algaeAfterAction.setTrue());
+
         // *********************************
         // Intaking States
         stationIntaking.whileTrue(coral.toggleToTrue(), algae.setFalse());
@@ -157,6 +169,32 @@ public static void setupStates() {
                 algae.setTrue(), coral.setFalse(), l3.setTrue(), actionPrepState.setTrue());
         pilot.l3AlgaeRemoval.onFalse(l3.setFalse(), actionPrepState.setFalse());
 
+        seesTag.onTrue(testL3.setTrue());
+        seesTag.onFalse(testL3.setFalse());
+
+        actionState
+                .and(algaeAfterAction, isL3Algae)
+                .onTrue(
+                        l3.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 0.1)),
+                        algae.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 0.1)),
+                        actionPrepState.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 0.1)),
+                        coral.setFalseAfterTime(() -> (RobotStates.getScoreTimeAlgae())),
+                        l1.setFalseAfterTime(RobotStates::getScoreTime),
+                        l2.setFalseAfterTime(RobotStates::getScoreTime),
+                        l4.setFalseAfterTime(RobotStates::getScoreTime),
+                        homeAll.setTrueAfterTime(() -> RobotStates.getScoreTimeAlgae() + 0.2));
+        actionState
+                .and(algaeAfterAction, isL3Algae.not())
+                .onTrue(
+                        l2.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 0.1)),
+                        algae.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 0.1)),
+                        actionPrepState.setTrueAfterTime(() -> (RobotStates.getScoreTime() + 0.1)),
+                        coral.setFalseAfterTime(() -> (RobotStates.getScoreTimeAlgae())),
+                        l1.setFalseAfterTime(RobotStates::getScoreTime),
+                        l3.setFalseAfterTime(RobotStates::getScoreTime),
+                        l4.setFalseAfterTime(RobotStates::getScoreTime),
+                        homeAll.setTrueAfterTime(() -> RobotStates.getScoreTimeAlgae() + 0.2));
+
         // **********************************
         // Staging and Scoring
 
@@ -262,6 +300,7 @@ public static Command clearStaged() {
                         coral.setFalse(),
                         algae.setFalse(),
                         shrinkState.setFalse(),
+                        algaeAfterAction.setFalse(),
                         autonStationIntake.setFalse())
                 .withName("Clear Staged");
     }

src/main/java/frc/robot/configs/PM2025.java

@@ -16,6 +16,6 @@ public PM2025() {
         elbow.setAttached(true);
         twist.setAttached(true);
         intake.setAttached(true);
-        climb.setAttached(true);
+        climb.setAttached(false);
     }
 }

src/main/java/frc/robot/pilot/Pilot.java

@@ -28,6 +28,7 @@ public class Pilot extends Gamepad {
     public final Trigger l2AlgaeRemoval = X.and(teleop);
     public final Trigger l3AlgaeRemoval = Y.and(teleop);
     public final Trigger photonRemoveL3Algae = rightTrigger.and(fn, teleop, photon);
+    public final Trigger algaeRemovalAfterScore = B.and(teleop);
 
     public final Trigger climbRoutine_start = start.and(noFn, teleop);
 

src/main/java/frc/robot/twist/TwistStates.java

@@ -44,7 +44,7 @@ public static void setStates() {
         branch.and((rightScore.not().or(Robot.getOperator().leftScore)), actionPrepState)
                 .whileTrue(move(config::getLeftCoral, config::getStageDelay, "Twist.leftCoral"));
 
-        twistL4R.onTrue(move(config::getRightCoral, "Twist.RightCoral"));
+        twistL4R.onTrue(move(config::getRightCoral, false, "Twist.RightCoral"));
         twistL4L.onTrue(move(config::getLeftCoral, "Twist.leftCoral"));
 
         climbPrep.whileTrue(move(config::getClimbPrep, "Twist.climbPrep"));
@@ -54,6 +54,10 @@ public static Command move(DoubleSupplier degrees, String name) {
         return twist.move(degrees).withName(name);
     }
 
+    public static Command move(DoubleSupplier degrees, Boolean clockwise, String name) {
+        return twist.move(degrees, clockwise).withName(name);
+    }
+
     public static Command move(DoubleSupplier degrees, DoubleSupplier delay, String name) {
         return new WaitCommand(delay.getAsDouble()).andThen(move(degrees, name).withName(name));
     }

src/main/java/frc/robot/vision/Vision.java

@@ -751,6 +751,31 @@ public static double indexOfSmallest(double[] array) {
     //     return new Translation2d(x, y);
     // }
 
+    public boolean isL3Algae() {
+        int tag = (int) getClosestTagID();
+        boolean isL3 = false;
+
+        if (Field.isBlue()) {
+            if (tag == 22 || tag == 20 || tag == 18) {
+                isL3 = true;
+            } else {
+                isL3 = false;
+            }
+        } else {
+            if (tag == 11 || tag == 9 || tag == 7) {
+                isL3 = true;
+            } else {
+                isL3 = false;
+            }
+        }
+
+        return isL3;
+    }
+
+    public boolean seesTag() {
+        return frontLL.targetInView() || backLL.targetInView();
+    }
+
     // ------------------------------------------------------------------------------
     // VisionStates Commands
     // ------------------------------------------------------------------------------

src/main/java/frc/robot/vision/VisionSystem.java

@@ -10,26 +10,19 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import java.io.IOException;
 import org.photonvision.PhotonCamera;
+import org.photonvision.simulation.PhotonCameraSim;
 import org.photonvision.simulation.SimCameraProperties;
 import org.photonvision.simulation.VisionSystemSim;
 
 public class VisionSystem extends SubsystemBase {
     @SuppressWarnings("unused")
     private final PhotonCamera camera = new PhotonCamera("cameraName");
-    // private final PhotonCamera frontCam = new PhotonCamera(VisionConfig.FRONT_LL);
-    // private final PhotonCamera backCam = new PhotonCamera(VisionConfig.RIGHT_LL);
+
+    private final PhotonCamera frontCam = new PhotonCamera("limelight-front");
+    private final PhotonCamera backCam = new PhotonCamera("limelight-back");
     private final VisionSystemSim visionSim = new VisionSystemSim("main");
     private final Pose2dSupplier getSimPose;
 
-    Transform3d robotToFrontCamera =
-            new Transform3d(
-                    new Translation3d(0, 0, 0.5), // Centered on the robot, 0.5m up
-                    new Rotation3d(0, Math.toRadians(-15), 0)); // Pitched 15 deg up
-    Transform3d robotToBackCamera =
-            new Transform3d(
-                    new Translation3d(0, 0, 0.5), // Centered on the robot, 0.5m up
-                    new Rotation3d(0, Math.toRadians(-15), 0)); // Pitched 15 deg up
-
     @FunctionalInterface
     public interface Pose2dSupplier {
         Pose2d getPose2d();
@@ -46,15 +39,20 @@ public VisionSystem(Pose2dSupplier getSimPose) {
         props.setLatencyStdDevMs(5.0);
 
         // Setup simulated camera
-        // PhotonCameraSim cameraSimFront = new PhotonCameraSim(frontCam, props);
-        // PhotonCameraSim cameraSimBack = new PhotonCameraSim(backCam, props);
-        // Draw field wireframe in simulated camera view
-        // cameraSimFront.enableDrawWireframe(true);
-        // cameraSimBack.enableDrawWireframe(false);
+        PhotonCameraSim cameraSimFront = new PhotonCameraSim(frontCam, props);
+        Translation3d robotToFrontCameraTrl = new Translation3d(0.215, 0, 0.188);
+        Rotation3d robotToFrontCameraRot = new Rotation3d(0, Math.toRadians(0), 0);
+        Transform3d robotToFrontCamera =
+                new Transform3d(robotToFrontCameraTrl, robotToFrontCameraRot);
+
+        PhotonCameraSim cameraSimBack = new PhotonCameraSim(backCam, props);
+        Translation3d robotToBackCameraTrl = new Translation3d(-0.215, 0.0, 0.188);
+        Rotation3d robotToBackCameraRot = new Rotation3d(0, Math.toRadians(0), Math.toRadians(180));
+        Transform3d robotToBackCamera = new Transform3d(robotToBackCameraTrl, robotToBackCameraRot);
 
         // // Add simulated camera to vision sim
-        // visionSim.addCamera(cameraSimFront, robotToFrontCamera);
-        // visionSim.addCamera(cameraSimBack, robotToBackCamera);
+        visionSim.addCamera(cameraSimFront, robotToFrontCamera);
+        visionSim.addCamera(cameraSimBack, robotToBackCamera);
 
         // Add AprilTags to vision sim
         try {
@@ -65,6 +63,14 @@ public VisionSystem(Pose2dSupplier getSimPose) {
         } catch (IOException e) {
             System.err.println(e);
         }
+
+        visionSim.getDebugField();
+        cameraSimFront.enableRawStream(true);
+        cameraSimFront.enableProcessedStream(true);
+
+        // Draw field wireframe in simulated camera view
+        cameraSimFront.enableDrawWireframe(true);
+        cameraSimBack.enableDrawWireframe(false);
     }
 
     @Override

src/main/java/frc/spectrumLib/SpectrumState.java

@@ -72,6 +72,20 @@ public Command setTrueForTime(DoubleSupplier time) {
                 .withName(name + " state: SetTrueForTime->" + time.getAsDouble());
     }
 
+    public Command setTrueAfterTime(DoubleSupplier time) {
+        return new WaitCommand(time.getAsDouble())
+                .andThen(setTrue())
+                .ignoringDisable(true)
+                .withName(name + " state: SetTrueAfterTime->" + time.getAsDouble());
+    }
+
+    public Command setFalseAfterTime(DoubleSupplier time) {
+        return new WaitCommand(time.getAsDouble())
+                .andThen(setFalse())
+                .ignoringDisable(true)
+                .withName(name + " state: SetTrueAfterTime->" + time.getAsDouble());
+    }
+
     /**
      * Command to set state to false, and then to true, ensuring your state will trigger actions
      *