diff --git a/simgui-ds.json b/simgui-ds.json index 44bd084c..30247095 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -110,8 +110,7 @@ "guid": "Keyboard0" }, { - "guid": "78696e70757401000000000000000000", - "useGamepad": true + "guid": "Keyboard1" } ], "useEnableDisableHotkeys": true diff --git a/simgui-window.json b/simgui-window.json index d193a1b0..ca0df641 100644 --- a/simgui-window.json +++ b/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": { diff --git a/simgui.json b/simgui.json index f2da99fb..e453c9b8 100644 --- a/simgui.json +++ b/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 } } diff --git a/src/main/java/frc/robot/RobotStates.java b/src/main/java/frc/robot/RobotStates.java index 63855678..dd651bb6 100644 --- a/src/main/java/frc/robot/RobotStates.java +++ b/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"); } diff --git a/src/main/java/frc/robot/configs/PM2025.java b/src/main/java/frc/robot/configs/PM2025.java index 410564c1..c4285a15 100644 --- a/src/main/java/frc/robot/configs/PM2025.java +++ b/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); } } diff --git a/src/main/java/frc/robot/pilot/Pilot.java b/src/main/java/frc/robot/pilot/Pilot.java index e122fc24..db771861 100644 --- a/src/main/java/frc/robot/pilot/Pilot.java +++ b/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); diff --git a/src/main/java/frc/robot/twist/TwistStates.java b/src/main/java/frc/robot/twist/TwistStates.java index 0c902453..a3158587 100644 --- a/src/main/java/frc/robot/twist/TwistStates.java +++ b/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)); } diff --git a/src/main/java/frc/robot/vision/Vision.java b/src/main/java/frc/robot/vision/Vision.java index e5a6d021..5c188598 100644 --- a/src/main/java/frc/robot/vision/Vision.java +++ b/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 // ------------------------------------------------------------------------------ diff --git a/src/main/java/frc/robot/vision/VisionSystem.java b/src/main/java/frc/robot/vision/VisionSystem.java index 776a47ac..87af2e34 100644 --- a/src/main/java/frc/robot/vision/VisionSystem.java +++ b/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 diff --git a/src/main/java/frc/spectrumLib/SpectrumState.java b/src/main/java/frc/spectrumLib/SpectrumState.java index e8413408..11e09a82 100644 --- a/src/main/java/frc/spectrumLib/SpectrumState.java +++ b/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 *