diff --git a/build.gradle b/build.gradle index e17d1e30..53bf715a 100644 --- a/build.gradle +++ b/build.gradle @@ -30,15 +30,23 @@ deploy { frcJava(getArtifactTypeClass('FRCJavaArtifact')) { // https://www.chiefdelphi.com/t/2024-wpilib-feedback/464322/141 - final MAX_JAVA_HEAP_SIZE_MB = 100; - // jvmArgs.add("-XX:+UnlockExperimentalVMOptions") + final MAX_JAVA_HEAP_SIZE_MB = 200; + jvmArgs.add("-XX:+UnlockExperimentalVMOptions") // Set the minimum heap size to the maximum heap size to avoid resizing - // jvmArgs.add("-Xmx" + MAX_JAVA_HEAP_SIZE_MB + "M") - //jvmArgs.add("-Xms" + MAX_JAVA_HEAP_SIZE_MB + "M") - //jvmArgs.add("-XX:GCTimeRatio=5") - //jvmArgs.add("-XX:+UseSerialGC") - //jvmArgs.add("-XX:MaxGCPauseMillis=50") + jvmArgs.add("-Xmx" + MAX_JAVA_HEAP_SIZE_MB + "M") + jvmArgs.add("-Xms" + MAX_JAVA_HEAP_SIZE_MB + "M") + jvmArgs.add("-XX:GCTimeRatio=5") + jvmArgs.add("-XX:+UseSerialGC") + jvmArgs.add("-XX:MaxGCPauseMillis=50") + + // VisualVM stuff (comment out in match) + // jvmArgs.add("-Dcom.sun.management.jmxremote=true") + // jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") + // jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") + // jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") + // jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") + // jvmArgs.add("-Djava.rmi.server.hostname=10.4.1.2") } // Static files artifact diff --git a/simgui-ds.json b/simgui-ds.json index 442546ae..8bcd0c7b 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -79,12 +79,12 @@ "axisCount": 2, "buttonCount": 6, "buttonKeys": [ - 260, - 268, - 266, - 261, - 269, - 267 + 49, + 50, + 51, + 52, + 53, + 54 ], "povCount": 0 }, @@ -96,10 +96,14 @@ ], "robotJoysticks": [ { - "guid": "Keyboard0" + "guid": "030000006d04000014c2000000000000" }, { - "guid": "Keyboard1" + "guid": "030000006d04000014c2000000000000" + }, + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true } ] } diff --git a/src/main/deploy/auto/ExampleAutoPath.json b/src/main/deploy/auto/ExampleAutoPath.json index 54908a25..2b0cd2ed 100644 --- a/src/main/deploy/auto/ExampleAutoPath.json +++ b/src/main/deploy/auto/ExampleAutoPath.json @@ -1,11 +1,11 @@ { "scoringLocations": [ - "Reef2", + "Reef5", "Reef5" ], "scoringLevels": [ - "L2", - "L4" + "L1", + "L1" ], "intakeLocation": "CoralStationRight" } diff --git a/src/main/deploy/constants/ScoringSetpoints.json b/src/main/deploy/constants/ScoringSetpoints.json index 9f856c38..b9e31509 100644 --- a/src/main/deploy/constants/ScoringSetpoints.json +++ b/src/main/deploy/constants/ScoringSetpoints.json @@ -35,7 +35,7 @@ "L2": { "name": "L2", "elevatorHeight": { - "value": 0.25, + "value": 0.29, "unit": "Meter" }, "wristAngle": { @@ -112,7 +112,7 @@ "processor": { "name": "processor", "elevatorHeight": { - "value": 0.2, + "value": 0.18, "unit": "Meter" }, "wristAngle": { diff --git a/src/main/deploy/constants/comp/ClawConstants.json b/src/main/deploy/constants/comp/ClawConstants.json index 2b6132bc..55930abc 100644 --- a/src/main/deploy/constants/comp/ClawConstants.json +++ b/src/main/deploy/constants/comp/ClawConstants.json @@ -21,8 +21,8 @@ }, "coralMinSignalStrengthForValidMeasurement": 2500.0, "algaeProximityThreshold": { - "value": 5.0, - "unit": "Centimeter" + "value": 2.0, + "unit": "Inch" }, "algaeProximityHysteresis": { "value": 0.5, @@ -38,7 +38,11 @@ "unit": "Volt" }, "coralScoreVoltage": { - "value": 12.0, + "value": 10.0, + "unit": "Volt" + }, + "coralL23ScoreVoltage": { + "value": 4.0, "unit": "Volt" }, "algaeScoreVoltage": { diff --git a/src/main/deploy/constants/comp/ClimbConstants.json b/src/main/deploy/constants/comp/ClimbConstants.json index 44c6c32f..c9a858a6 100644 --- a/src/main/deploy/constants/comp/ClimbConstants.json +++ b/src/main/deploy/constants/comp/ClimbConstants.json @@ -17,5 +17,6 @@ "climbkI": 0.0, "climbkD": 0.0, "climbCurrentLimit": 60, - "invertFollowerClimbMotor": true + "invertFollowerClimbMotor": true, + "climbFFVolts": -2.0 } diff --git a/src/main/deploy/constants/comp/ElevatorConstants.json b/src/main/deploy/constants/comp/ElevatorConstants.json index fb18ec87..d236382d 100644 --- a/src/main/deploy/constants/comp/ElevatorConstants.json +++ b/src/main/deploy/constants/comp/ElevatorConstants.json @@ -23,7 +23,7 @@ "elevatorLargeCANCoderDirection": "Clockwise_Positive", "elevatorSmallCANCoderID": 15, "elevatorSmallCANCoderDirection": "CounterClockwise_Positive", - "elevatorkP": 40.0, + "elevatorkP": 50.0, "elevatorkI": 0.0, "elevatorkD": 4.0, "elevatorkS": 0.0, diff --git a/src/main/deploy/constants/config.json b/src/main/deploy/constants/config.json index 6352756e..9055af5d 100644 --- a/src/main/deploy/constants/config.json +++ b/src/main/deploy/constants/config.json @@ -1,5 +1,5 @@ { - "environment": "comp", + "environment": "test_drivebase", "defaults": "comp", "environments": [ { diff --git a/src/main/deploy/constants/test_drivebase/ElevatorConstants.json b/src/main/deploy/constants/test_drivebase/ElevatorConstants.json index b21df5ee..8cd68dd8 100644 --- a/src/main/deploy/constants/test_drivebase/ElevatorConstants.json +++ b/src/main/deploy/constants/test_drivebase/ElevatorConstants.json @@ -20,7 +20,7 @@ }, "elevatorCANCoderDiscontinuityPoint": 1.0, "elevatorLargeCANCoderID": 32, - "elevatorLargeCANCoderDirection": "CounterClockwise_Positive", + "elevatorLargeCANCoderDirection": "Clockwise_Positive", "elevatorSmallCANCoderID": 33, "elevatorSmallCANCoderDirection": "CounterClockwise_Positive", "elevatorkP": 60.0, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index f52ddd56..72f0bf72 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -3,14 +3,14 @@ /** Automatically generated file containing build version information. */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2025-Robot-Code-3"; + public static final String MAVEN_NAME = "2025-Robot-Code"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 68; - public static final String GIT_SHA = "9899bcb2854cc592c52ca78edd1259dc08feb736"; - public static final String GIT_DATE = "2025-03-08 09:57:29 EST"; - public static final String GIT_BRANCH = "156-ramp-work"; - public static final String BUILD_DATE = "2025-03-08 10:03:36 EST"; - public static final long BUILD_UNIX_TIME = 1741446216963L; + public static final int GIT_REVISION = 97; + public static final String GIT_SHA = "11e804357aa0e68a9318f719feff477e4b6b8d97"; + public static final String GIT_DATE = "2025-03-17 19:17:23 EDT"; + public static final String GIT_BRANCH = "maplesim-intake"; + public static final String BUILD_DATE = "2025-03-17 19:45:03 EDT"; + public static final long BUILD_UNIX_TIME = 1742255103915L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/frc/robot/InitBindings.java b/src/main/java/frc/robot/InitBindings.java index 0adbad9c..3bd22991 100644 --- a/src/main/java/frc/robot/InitBindings.java +++ b/src/main/java/frc/robot/InitBindings.java @@ -291,6 +291,8 @@ public static void initScoringBindings(ScoringSubsystem scoring) { .onTrue( new InstantCommand( () -> { + scoring.setTarget(FieldTarget.Net); + scoring.setGamePiece(GamePiece.Algae); scoring.setClawRollerVoltage(JsonConstants.clawConstants.coralScoreVoltage); })) .onFalse( @@ -298,6 +300,26 @@ public static void initScoringBindings(ScoringSubsystem scoring) { () -> { scoring.setClawRollerVoltage(Volts.zero()); })); + + driverController + .leftBumper() + .onTrue( + new InstantCommand( + () -> { + scoring.setTarget(FieldTarget.Ground); + scoring.setGamePiece(GamePiece.Coral); + })); + + rightJoystick + .top() + .onTrue( + new InstantCommand( + () -> { + if (ScoringSubsystem.getInstance().getGamePiece() == GamePiece.Algae + && ScoringSubsystem.getInstance().getTarget() == FieldTarget.Processor) { + ScoringSubsystem.getInstance().fireTrigger(ScoringTrigger.WarmupReady); + } + })); } /** diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index df7fc3a5..6c5b1837 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,7 +4,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.constants.ModeConstants; -import org.ironmaple.simulation.SimulatedArena; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -151,8 +150,7 @@ public void testPeriodic() { /** This function is called once when the robot is first started up. */ @Override public void simulationInit() { - SimulatedArena.getInstance().resetFieldForAuto(); - // SimulatedArena.overrideInstance(new Arena2025Reefscape()); + robotContainer.resetMapleSim(); } /** This function is called periodically whilst in simulation. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 934f1dad..0c600eef 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,7 +21,6 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.StrategyManager.AutonomyMode; import frc.robot.commands.drive.AkitDriveCommands; import frc.robot.constants.AutoStrategy; import frc.robot.constants.AutoStrategyContainer; @@ -34,9 +33,11 @@ import frc.robot.subsystems.led.LED; import frc.robot.subsystems.ramp.RampSubsystem; import frc.robot.subsystems.scoring.ScoringSubsystem; +import frc.robot.subsystems.scoring.ScoringSubsystemMapleSim; import java.io.File; import org.ironmaple.simulation.SimulatedArena; import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeAlgaeOnFly; import org.littletonrobotics.junction.Logger; /** @@ -140,8 +141,8 @@ public void updateRobotModel() { new Pose3d( new Translation3d(-0.16, 0.31, 0.115), new Rotation3d(climb_rotation, 0.0, 0.0)), new Pose3d( - new Translation3d(0.34, 0.12, height + 0.35), - new Rotation3d(0.0, -claw_rotation + 0.465719787 * 180.0, 0.0)), + new Translation3d(0.34, 0.12, height + 0.4), + new Rotation3d(0.0, -claw_rotation + 0.465719787 * 180.0 + 3.14, 0.0)), new Pose3d(new Translation3d(0.0, 0.0, height), new Rotation3d(0.0, 0.0, 0.0)), new Pose3d(new Translation3d(0.0, 0.0, stage_two_height), new Rotation3d(0.0, 0.0, 0.0)), new Pose3d(new Translation3d(0.0, 0.0, stage_one_height), new Rotation3d(0.0, 0.0, 0.0)) @@ -207,10 +208,14 @@ public void configureSubsystems() { if (FeatureFlags.synced.getObject().runDrive) { scoringSubsystem.setIsDriveLinedUpSupplier( () -> { - if (strategyManager.getAutonomyMode() != AutonomyMode.Manual) { - return drive.isDriveAlignmentFinished(); - } else { - return InitBindings.isManualScorePressed(); + switch (strategyManager.getAutonomyMode()) { + case Manual: + return InitBindings.isManualScorePressed(); + case Mixed: + return drive.isDriveAlignmentFinished() || InitBindings.isManualScorePressed(); + case Full: + default: + return drive.isDriveAlignmentFinished(); } }); scoringSubsystem.setReefDistanceSupplier( @@ -224,6 +229,7 @@ public void configureSubsystems() { } else { scoringSubsystem.setIsDriveLinedUpSupplier(() -> true); } + scoringSubsystem.setIsDriveLinedUpSupplier(() -> true); } if (FeatureFlags.synced.getObject().runLEDs) { @@ -276,10 +282,10 @@ public void periodic() { public void autonomousInit() { drive.autonomousInit(); - setSubsystemsToBrake(); - if (FeatureFlags.synced.getObject().runLEDs) { - led.setLedOn(true); - } + // setSubsystemsToBrake(); + // if (FeatureFlags.synced.getObject().runLEDs) { + // led.setLedOn(true); + // } // load chosen strategy strategyManager.autonomousInit(autoChooser.getSelected()); @@ -378,10 +384,24 @@ public void disabledPeriodic() { strategyManager.logActions(); checkSwitchForDisabled(); checkLedSwitch(); + + Logger.recordOutput("Switches/brake", brakeSwitch.get()); + Logger.recordOutput("Switches/led", ledSwitch.get()); } public void disabledInit() { CommandScheduler.getInstance().cancelAll(); + + if (FeatureFlags.synced.getObject().runClimb) { + climbSubsystem.setBrakeMode(true); + } + } + + public void resetMapleSim() { + SimulatedArena.getInstance().resetFieldForAuto(); + ReefscapeAlgaeOnFly.setHitNetCallBack(() -> System.out.println("ALGAE hits NET!")); + ScoringSubsystemMapleSim.configDrive(drive, driveSim); + ScoringSubsystemMapleSim.configScoring(scoringSubsystem); } public void updateMapleSim() { diff --git a/src/main/java/frc/robot/constants/ClawConstants.java b/src/main/java/frc/robot/constants/ClawConstants.java index 6db4943e..f0e84262 100644 --- a/src/main/java/frc/robot/constants/ClawConstants.java +++ b/src/main/java/frc/robot/constants/ClawConstants.java @@ -48,6 +48,7 @@ public class ClawConstants { public final Voltage coralIntakeVoltage = Volts.of(3.0); public final Voltage algaeIntakeVoltage = Volts.of(-3.0); public final Voltage coralScoreVoltage = Volts.of(3.0); + public final Voltage coralL23ScoreVoltage = Volts.of(6.0); public final Voltage algaeScoreVoltage = Volts.of(-3.0); /** Voltage the claw should use to hold the algae in place while idling */ diff --git a/src/main/java/frc/robot/constants/ClimbConstants.java b/src/main/java/frc/robot/constants/ClimbConstants.java index 861e6802..584626b5 100644 --- a/src/main/java/frc/robot/constants/ClimbConstants.java +++ b/src/main/java/frc/robot/constants/ClimbConstants.java @@ -40,6 +40,8 @@ public class ClimbConstants { public final InvertedValue climbInvertValue = InvertedValue.Clockwise_Positive; public final Boolean invertFollowerClimbMotor = true; + public final Double climbFFVolts = -10.0; + public static final class Sim { @JSONExclude public static final JSONSync synced = diff --git a/src/main/java/frc/robot/constants/LEDConstants.java b/src/main/java/frc/robot/constants/LEDConstants.java index 3c4a4ce9..fb79e700 100644 --- a/src/main/java/frc/robot/constants/LEDConstants.java +++ b/src/main/java/frc/robot/constants/LEDConstants.java @@ -13,9 +13,10 @@ public final class LEDConstants { public static final int ledPort = 9; - public static final int halfLength = 14; // alternate number to test = 504 - public static final int totalLength = 28; // alternate number to test = 1008 - public static final double rainbowSpeed = 50; + public static final int halfLength = 14; + public static final int totalLength = 28; + public static final int totalLengthRainbow = 50; + public static final double rainbowSpeed = 20; public static final Color off = Color.kBlack; // LED Patterns diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIO.java b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java index 7fd3b3bf..72b96532 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbIO.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java @@ -36,5 +36,7 @@ public default void setBrakeMode(boolean brake) {} public default void setPID(double p, double i, double d) {} + public default void setFF(double newFF) {} + public default void setFF(double kS, double kV, double kA, double kG) {} } diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/frc/robot/subsystems/climb/ClimbIOTalonFX.java index 5c7a0176..a541a728 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbIOTalonFX.java @@ -96,10 +96,19 @@ public void updateInputs(ClimbInputs inputs) { inputs.motorAngle.mut_replace(climbAngleCoder.getAbsolutePosition().getValue()); } + private double feedforward = 0.0; + + public void setFF(double newFF) { + this.feedforward = newFF; + } + @Override public void applyOutputs(ClimbOutputs outputs) { - - calculator.withPosition(goalAngle.in(Rotations)); + if (goalAngle.lt(climbAngleCoder.getAbsolutePosition().getValue())) { + calculator.withPosition(goalAngle.in(Rotations)).withFeedForward(feedforward); + } else { + calculator.withPosition(goalAngle.in(Rotations)).withFeedForward(0.0); + } Logger.recordOutput("climb/calculatorAngle", leadMotor.getPosition().getValueAsDouble()); @@ -108,6 +117,7 @@ public void applyOutputs(ClimbOutputs outputs) { } else { leadMotor.setControl(calculator); } + // tried to replace the above if statement with leadMotor.setControl(new VoltageOut(5));, // doesn't work // the getMotorVoltage continuously returns 0 despite leadMotor's literal voltage being set to 5 diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java index 7632a216..db21ab27 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java @@ -211,4 +211,8 @@ public void testPeriodic() { break; } } + + public void setFeedforward(double newFF) { + io.setFF(newFF); + } } diff --git a/src/main/java/frc/robot/subsystems/climb/states/IdleState.java b/src/main/java/frc/robot/subsystems/climb/states/IdleState.java index 77e63b09..a0fc5b75 100644 --- a/src/main/java/frc/robot/subsystems/climb/states/IdleState.java +++ b/src/main/java/frc/robot/subsystems/climb/states/IdleState.java @@ -13,6 +13,7 @@ public IdleState(ClimbSubsystem climbSubsystem) { @Override public void periodic() { + climbSubsystem.setFeedforward(0.0); climbSubsystem.setGoalAngle(ClimbConstants.synced.getObject().restingAngle); } } diff --git a/src/main/java/frc/robot/subsystems/climb/states/LiftingState.java b/src/main/java/frc/robot/subsystems/climb/states/LiftingState.java index dcdc2c65..47daf878 100644 --- a/src/main/java/frc/robot/subsystems/climb/states/LiftingState.java +++ b/src/main/java/frc/robot/subsystems/climb/states/LiftingState.java @@ -2,6 +2,7 @@ import coppercore.controls.state_machine.state.PeriodicStateInterface; import frc.robot.constants.ClimbConstants; +import frc.robot.constants.JsonConstants; import frc.robot.subsystems.climb.ClimbSubsystem; public class LiftingState implements PeriodicStateInterface { @@ -14,6 +15,7 @@ public LiftingState(ClimbSubsystem climbSubsystem) { @Override public void periodic() { + climbSubsystem.setFeedforward(JsonConstants.climbConstants.climbFFVolts); climbSubsystem.setGoalAngle(ClimbConstants.synced.getObject().finalHangingAngle); } } diff --git a/src/main/java/frc/robot/subsystems/climb/states/SearchingState.java b/src/main/java/frc/robot/subsystems/climb/states/SearchingState.java index b0fdc239..4ce90dc3 100644 --- a/src/main/java/frc/robot/subsystems/climb/states/SearchingState.java +++ b/src/main/java/frc/robot/subsystems/climb/states/SearchingState.java @@ -14,6 +14,7 @@ public SearchingState(ClimbSubsystem climbSubsystem) { @Override public void periodic() { + climbSubsystem.setFeedforward(0.0); climbSubsystem.setGoalAngle(ClimbConstants.synced.getObject().searchingAngle); if (climbSubsystem.getLockedToCage()) { climbSubsystem.fireTrigger(ClimbAction.CLIMB); diff --git a/src/main/java/frc/robot/subsystems/climb/states/WaitingState.java b/src/main/java/frc/robot/subsystems/climb/states/WaitingState.java index d20c0da9..b3152c85 100644 --- a/src/main/java/frc/robot/subsystems/climb/states/WaitingState.java +++ b/src/main/java/frc/robot/subsystems/climb/states/WaitingState.java @@ -14,6 +14,7 @@ public WaitingState(ClimbSubsystem climbSubsystem) { @Override public void periodic() { + climbSubsystem.setFeedforward(0.0); climbSubsystem.setGoalAngle(ClimbConstants.synced.getObject().restingAngle); if (climbSubsystem.getRampClear()) { climbSubsystem.fireTrigger(ClimbAction.CLIMB); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 97884599..d8193728 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -235,6 +235,9 @@ public enum DriveTrigger { private LocalADStarAK localADStar = new LocalADStarAK(); + private Command pathfindWarmupSpoofCommand; + private Command warmupCommand = PathfindingCommand.warmupCommand(); + public Drive( GyroIO gyroIO, ModuleIO flModuleIO, @@ -276,7 +279,64 @@ public Drive( }); // warm up java processing for faster pathfind later - PathfindingCommand.warmupCommand().schedule(); + if (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red) { + localADStar.setDynamicObstacles( + List.of( + new Pair( + JsonConstants.redFieldLocations.coralAlgaeStackLeftTopCorner, + JsonConstants.redFieldLocations.coralAlgaeStackLeftBottomCorner), + new Pair( + JsonConstants.redFieldLocations.coralAlgaeStackMiddleTopCorner, + JsonConstants.redFieldLocations.coralAlgaeStackMiddleBottomCorner), + new Pair( + JsonConstants.redFieldLocations.coralAlgaeStackRightTopCorner, + JsonConstants.redFieldLocations.coralAlgaeStackRightBottomCorner)), + getPose().getTranslation()); + } else { + localADStar.setDynamicObstacles( + List.of( + new Pair( + JsonConstants.blueFieldLocations.coralAlgaeStackLeftTopCorner, + JsonConstants.blueFieldLocations.coralAlgaeStackLeftBottomCorner), + new Pair( + JsonConstants.blueFieldLocations.coralAlgaeStackMiddleTopCorner, + JsonConstants.blueFieldLocations.coralAlgaeStackMiddleBottomCorner), + new Pair( + JsonConstants.blueFieldLocations.coralAlgaeStackRightTopCorner, + JsonConstants.blueFieldLocations.coralAlgaeStackRightBottomCorner)), + getPose().getTranslation()); + } + + warmupCommand.schedule(); + + // // DriveState.OTF.getState().getDriveToPoseCommand(). + // // Manually create an OTF command on robot init to avoid massive lagspike when auto begins: + // Pose2d goalPose = + // isAllianceRed() + // ? new Pose2d( + // JsonConstants.redFieldLocations.redReef23Translation, + // JsonConstants.redFieldLocations.redReef23Rotation) + // : new Pose2d( + // JsonConstants.blueFieldLocations.blueReef23Translation, + // JsonConstants.blueFieldLocations.blueReef23Rotation); + + // // Create the constraints to use while pathfinding + // PathConstraints constraints = + // new PathConstraints( + // JsonConstants.drivetrainConstants.OTFMaxLinearVelocity, + // JsonConstants.drivetrainConstants.OTFMaxLinearAccel, + // JsonConstants.drivetrainConstants.OTFMaxAngularVelocity, + // JsonConstants.drivetrainConstants.OTFMaxAngularAccel); + + // pathfindWarmupSpoofCommand = + // AutoBuilder.pathfindToPose( + // goalPose, constraints, JsonConstants.drivetrainConstants.otfPoseEndingVelocity); + + // pathfindWarmupSpoofCommand.schedule(); + + // // Manually call execute because the 1st execute seems to take 0.9 seconds in auto + // pathfindWarmupSpoofCommand.execute(); + angleController.enableContinuousInput(-Math.PI, Math.PI); // Configure SysId sysId = @@ -324,34 +384,13 @@ public void configureStates() { stateMachine = new StateMachine<>(stateMachineConfiguration, DriveState.Joystick); } - /** add algae coral stack obstacles for on the fly */ public void autonomousInit() { - if (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red) { - localADStar.setDynamicObstacles( - List.of( - new Pair( - JsonConstants.redFieldLocations.coralAlgaeStackLeftTopCorner, - JsonConstants.redFieldLocations.coralAlgaeStackLeftBottomCorner), - new Pair( - JsonConstants.redFieldLocations.coralAlgaeStackMiddleTopCorner, - JsonConstants.redFieldLocations.coralAlgaeStackMiddleBottomCorner), - new Pair( - JsonConstants.redFieldLocations.coralAlgaeStackRightTopCorner, - JsonConstants.redFieldLocations.coralAlgaeStackRightBottomCorner)), - getPose().getTranslation()); - } else { - localADStar.setDynamicObstacles( - List.of( - new Pair( - JsonConstants.blueFieldLocations.coralAlgaeStackLeftTopCorner, - JsonConstants.blueFieldLocations.coralAlgaeStackLeftBottomCorner), - new Pair( - JsonConstants.blueFieldLocations.coralAlgaeStackMiddleTopCorner, - JsonConstants.blueFieldLocations.coralAlgaeStackMiddleBottomCorner), - new Pair( - JsonConstants.blueFieldLocations.coralAlgaeStackRightTopCorner, - JsonConstants.blueFieldLocations.coralAlgaeStackRightBottomCorner)), - getPose().getTranslation()); + // if (pathfindWarmupSpoofCommand != null && pathfindWarmupSpoofCommand.isScheduled()) { + // pathfindWarmupSpoofCommand.cancel(); + // } + + if (warmupCommand != null && warmupCommand.isScheduled()) { + warmupCommand.cancel(); } } @@ -448,6 +487,12 @@ else if (!DriverStation.isDisabled()) { Logger.recordOutput("Drive/driveLinedUp", driveLinedUp); } + public void disabledPeriodic() { + if (warmupCommand != null) { + Logger.recordOutput("Drive/warmupScheduled", warmupCommand.isScheduled()); + } + } + /** * sets desired speeds of robot * diff --git a/src/main/java/frc/robot/subsystems/led/LED.java b/src/main/java/frc/robot/subsystems/led/LED.java index 1f0e1c8e..361f7ba8 100644 --- a/src/main/java/frc/robot/subsystems/led/LED.java +++ b/src/main/java/frc/robot/subsystems/led/LED.java @@ -24,11 +24,13 @@ public class LED extends SubsystemBase { private List rightPatterns = new ArrayList<>(); private final AddressableLED led = new AddressableLED(LEDConstants.ledPort); - public final AddressableLEDBuffer ledStrip = new AddressableLEDBuffer(LEDConstants.totalLength); + public AddressableLEDBuffer ledStrip = new AddressableLEDBuffer(LEDConstants.totalLength); - public final AddressableLEDBufferView leftData = - ledStrip.createView(0, LEDConstants.halfLength - 1); - private final AddressableLEDBufferView rightData = + public AddressableLEDBufferView leftData = + ledStrip + .createView(0, LEDConstants.halfLength - 1) + .reversed(); // these should both be reversed + private AddressableLEDBufferView rightData = ledStrip.createView(LEDConstants.halfLength, LEDConstants.totalLength - 1).reversed(); private Supplier visionWorkingSupplier = () -> true; @@ -37,6 +39,8 @@ public class LED extends SubsystemBase { private ClimbSubsystem climbSubsystem; private Drive driveSubsystem; + private boolean enabled = false; + /** * Constructs the LED subsystem. * @@ -52,10 +56,25 @@ public LED(ScoringSubsystem scoringSubsystem, ClimbSubsystem climbSubsystem, Dri led.start(); } + private void setEnabled(boolean enabled) { + if (enabled != this.enabled) { + this.enabled = enabled; + if (enabled) { + ledStrip = new AddressableLEDBuffer(LEDConstants.totalLength); + leftData = ledStrip.createView(0, LEDConstants.halfLength - 1); + rightData = + ledStrip.createView(LEDConstants.halfLength, LEDConstants.totalLength - 1).reversed(); + led.setLength(LEDConstants.totalLength); + } else { + ledStrip = new AddressableLEDBuffer(LEDConstants.totalLengthRainbow); + led.setLength(LEDConstants.totalLengthRainbow); + } + } + } + /** Periodic method called every loop cycle. Updates LED patterns based on robot state. */ @Override public void periodic() { - if (!DriverStation.isDisabled()) { // Drive Subsystem Checks @@ -111,11 +130,12 @@ public void periodic() { // } else { // rainbow - all thirds - addPattern(LEDConstants.rainbowPattern); - if (!visionWorkingSupplier.get()) { - addPattern(LEDConstants.isBeeLinkWorkingPattern); - } - applyPatterns(); + // addPattern(LEDConstants.rainbowPattern); + // if (!visionWorkingSupplier.get()) { + // addPattern(LEDConstants.isBeeLinkWorkingPattern); + // } + // applyPatterns(); + LEDConstants.rainbowPattern.applyTo(ledStrip); } led.setData(ledStrip); } diff --git a/src/main/java/frc/robot/subsystems/scoring/ClawIOSim.java b/src/main/java/frc/robot/subsystems/scoring/ClawIOSim.java index 66b9fb3b..1d542752 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ClawIOSim.java +++ b/src/main/java/frc/robot/subsystems/scoring/ClawIOSim.java @@ -75,11 +75,11 @@ private void updateSimState() { .times(Seconds.of(0.02))); if (has == HasState.NONE) { - if (outputVoltage.in(Volts) > 0.0 && algaeAvailable) { + if (outputVoltage.in(Volts) > 0.0 || algaeAvailable) { has = HasState.ALGAE; piecePos = 1.0; } - if (outputVoltage.in(Volts) > 0.0 && coralAvailable) { + if (outputVoltage.in(Volts) > 0.0 || coralAvailable) { has = HasState.CORAL; piecePos = 0.0; } diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index f14ce234..b3001390 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -278,7 +278,7 @@ && canFarWarmup()) .permitIf( ScoringTrigger.WarmupReady, ScoringState.Score, - () -> isDriveLinedUpSupplier.getAsBoolean()) + () -> true) // () -> isDriveLinedUpSupplier.getAsBoolean()) .permit(ScoringTrigger.ReturnToIdle, ScoringState.Idle) .permit(ScoringTrigger.CancelWarmup, ScoringState.Idle); @@ -633,7 +633,8 @@ public boolean shouldWaitOnScore() { hasGamePiece = clawMechanism.isAlgaeDetected(); break; } - return ((stateMachine.getCurrentState() == ScoringState.Warmup) + return ((stateMachine.getCurrentState() == ScoringState.Init) + || (stateMachine.getCurrentState() == ScoringState.Warmup) || (stateMachine.getCurrentState() == ScoringState.Score)) && hasGamePiece; } diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java new file mode 100644 index 00000000..0e19b0d5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystemMapleSim.java @@ -0,0 +1,138 @@ +package frc.robot.subsystems.scoring; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Radians; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.subsystems.drive.Drive; +import java.util.List; +import org.ironmaple.simulation.IntakeSimulation; +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeAlgaeOnFly; +import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeCoralOnFly; +import org.littletonrobotics.junction.Logger; + +public class ScoringSubsystemMapleSim { + + public static Drive drive; + public static ScoringSubsystem scoring; + + private static IntakeSimulation coralIntakeSimulation; + private static IntakeSimulation algaeIntakeSimulation; + + public static void shootAlgae() { + if (drive == null || scoring == null) { + return; + } + SimulatedArena.getInstance() + .addGamePieceProjectile( + new ReefscapeAlgaeOnFly( + drive.getPose().getTranslation(), + new Translation2d(0.5, 0), + drive.getChassisSpeeds(), + drive.getPose().getRotation(), + Meters.of( + scoring.getElevatorHeight().in(Meters) + + 0.5), // initial height of the ball, in meters + MetersPerSecond.of(5), // initial velocity, in m/s + scoring.getWristAngle()) // shooter angle + .withProjectileTrajectoryDisplayCallBack( + (poses) -> + Logger.recordOutput( + "successfulShotsTrajectory", poses.toArray(Pose3d[]::new)), + (poses) -> + Logger.recordOutput( + "missedShotsTrajectory", poses.toArray(Pose3d[]::new)))); + } + + public static void shootCoral() { + if (drive == null || scoring == null) { + return; + } + SimulatedArena.getInstance() + .addGamePieceProjectile( + new ReefscapeCoralOnFly( + drive.getPose().getTranslation(), + new Translation2d(0.45, 0), + drive.getChassisSpeeds(), + drive.getPose().getRotation(), + Meters.of( + scoring.getElevatorHeight().in(Meters) + + 0.4), // initial height of the ball, in meters + MetersPerSecond.of(4), // initial velocity, in m/s + Degrees.of( + (scoring.getWristAngle().in(Radians) + + 0.465719787 * 180.0 + + 3.14))) // shooter angle + .withProjectileTrajectoryDisplayCallBack( + (poses) -> {}, + (poses) -> + Logger.recordOutput("coralShotsTrajectory", poses.toArray(Pose3d[]::new)))); + } + + public static void intakeCoral() { + if (scoring.isCoralDetected() == false) { + List corals = SimulatedArena.getInstance().getGamePiecesByType("Coral"); + int indexOfMinHeightIntake; + double distanceOfMinHeightIntake = Integer.MAX_VALUE; + + for (int coralIndex = 0; coralIndex < corals.size(); coralIndex++) { + double heightDistance = + Math.abs(scoring.getElevatorHeight().in(Meters) + 0.5 - corals.get(coralIndex).getZ()); + if (distanceOfMinHeightIntake > heightDistance) { + indexOfMinHeightIntake = coralIndex; + distanceOfMinHeightIntake = heightDistance; + } + } + + if (distanceOfMinHeightIntake < 0.1) { + coralIntakeSimulation.startIntake(); + SmartDashboard.putBoolean("clawSim/coralAvailable", true); + coralIntakeSimulation.stopIntake(); + } + } + } + + public static void intakeAlgae() { + if (scoring.isCoralDetected() == false) { + List algae = SimulatedArena.getInstance().getGamePiecesByType("Algae"); + int indexOfMinHeightIntake; + double distanceOfMinHeightIntake = Integer.MAX_VALUE; + + for (int algaeIndex = 0; algaeIndex < algae.size(); algaeIndex++) { + double heightDistance = + Math.abs(scoring.getElevatorHeight().in(Meters) + 0.5 - algae.get(algaeIndex).getZ()); + if (distanceOfMinHeightIntake > heightDistance) { + indexOfMinHeightIntake = algaeIndex; + distanceOfMinHeightIntake = heightDistance; + } + } + + if (distanceOfMinHeightIntake < 0.1) { + algaeIntakeSimulation.startIntake(); + SmartDashboard.putBoolean("clawSim/algaeAvailable", true); + algaeIntakeSimulation.stopIntake(); + } + } + } + + public static void configDrive(Drive driveSet, SwerveDriveSimulation driveSim) { + drive = driveSet; + coralIntakeSimulation = + IntakeSimulation.InTheFrameIntake( + "Coral", driveSim, Meters.of(0.2), IntakeSimulation.IntakeSide.FRONT, 1); + + algaeIntakeSimulation = + IntakeSimulation.InTheFrameIntake( + "Algae", driveSim, Meters.of(0.2), IntakeSimulation.IntakeSide.FRONT, 1); + } + + public static void configScoring(ScoringSubsystem scoringSet) { + scoring = scoringSet; + } +} diff --git a/src/main/java/frc/robot/subsystems/scoring/states/ScoreState.java b/src/main/java/frc/robot/subsystems/scoring/states/ScoreState.java index e4574b64..6d7c72aa 100644 --- a/src/main/java/frc/robot/subsystems/scoring/states/ScoreState.java +++ b/src/main/java/frc/robot/subsystems/scoring/states/ScoreState.java @@ -10,6 +10,7 @@ import frc.robot.subsystems.scoring.ScoringSubsystem.FieldTarget; import frc.robot.subsystems.scoring.ScoringSubsystem.GamePiece; import frc.robot.subsystems.scoring.ScoringSubsystem.ScoringTrigger; +import frc.robot.subsystems.scoring.ScoringSubsystemMapleSim; public class ScoreState implements PeriodicStateInterface { private ScoringSubsystem scoringSubsystem; @@ -36,8 +37,14 @@ public void periodic() { switch (scoringSubsystem.getGamePiece()) { case Coral: - scoringSubsystem.setClawRollerVoltage(JsonConstants.clawConstants.coralScoreVoltage); + if (scoringSubsystem.getTarget() == FieldTarget.L2 + || scoringSubsystem.getTarget() == FieldTarget.L3) { + scoringSubsystem.setClawRollerVoltage(JsonConstants.clawConstants.coralL23ScoreVoltage); + } else { + scoringSubsystem.setClawRollerVoltage(JsonConstants.clawConstants.coralScoreVoltage); + } if (!scoringSubsystem.isCoralDetected()) { + ScoringSubsystemMapleSim.shootCoral(); scoringSubsystem.fireTrigger(ScoringTrigger.ScoredPiece); } break; @@ -63,6 +70,7 @@ public void periodic() { } if (!scoringSubsystem.isAlgaeDetected()) { + ScoringSubsystemMapleSim.shootAlgae(); scoringSubsystem.fireTrigger(ScoringTrigger.ScoredPiece); } break;