diff --git a/.vscode/settings.json b/.vscode/settings.json index 9a299ae5..446902b5 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -57,5 +57,5 @@ "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", ], - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable" + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx8G -Xms100m -Xlog:disable" } diff --git a/build.gradle b/build.gradle index 3677b87b..f6d13581 100644 --- a/build.gradle +++ b/build.gradle @@ -81,7 +81,7 @@ dependencies { implementation 'ch.qos.logback:logback-classic:1.3.5' //logging implementation("com.opencsv:opencsv:5.6") - implementation 'org.strykeforce:wallEYE:25.0.0' + implementation 'org.strykeforce:wallEYE:25.0.1' implementation 'net.jafama:jafama:2.3.2' //fastMath } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2c9d9469..a1f618b1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,11 +9,17 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.commands.algae.IntakeAlgaeCommand; import frc.robot.commands.algae.OpenLoopAlgaeCommand; +import frc.robot.commands.algae.ProcessorAlgaeCommand; +import frc.robot.commands.algae.ToggleHasAlgaeCommand; +import frc.robot.commands.biscuit.HoldBiscuitCommand; +import frc.robot.commands.biscuit.JogBiscuitCommand; import frc.robot.commands.coral.EnableEjectBeamCommand; import frc.robot.commands.coral.OpenLoopCoralCommand; import frc.robot.commands.drive.DriveTeleopCommand; @@ -22,9 +28,19 @@ import frc.robot.commands.elevator.JogElevatorCommand; import frc.robot.commands.elevator.SetElevatorPositionCommand; import frc.robot.commands.elevator.ZeroElevatorCommand; -import frc.robot.commands.robotState.ScoreReefManualCommand; +import frc.robot.commands.robotState.FloorAlgaeCommand; +import frc.robot.commands.robotState.HPAlgaeCommand; +import frc.robot.commands.robotState.InterruptAutoCommand; +import frc.robot.commands.robotState.ReefCycleCommand; +import frc.robot.commands.robotState.ScoreAlgaeCommand; +import frc.robot.commands.robotState.SetScoreSideRightCommand; import frc.robot.commands.robotState.SetScoringLevelCommand; import frc.robot.commands.robotState.StowCommand; +import frc.robot.commands.robotState.ToggleAlgaeHeightCommand; +import frc.robot.commands.robotState.ToggleAutoCommand; +import frc.robot.commands.robotState.ToggleGetAlgaeCommand; +import frc.robot.commands.robotState.setScoreSideLeftCommand; +import frc.robot.constants.BiscuitConstants; import frc.robot.constants.ElevatorConstants; import frc.robot.constants.RobotConstants; import frc.robot.controllers.FlyskyJoystick; @@ -119,7 +135,7 @@ public RobotContainer() { ledIO = new LEDIO(); ledSubsystem = new LEDSubsystem(); - visionSubsystem = new VisionSubsystem(); + visionSubsystem = new VisionSubsystem(driveSubsystem); tagAlignSubsystem = new TagAlignSubsystem(driveSubsystem, visionSubsystem); @@ -142,6 +158,8 @@ public RobotContainer() { configureTelemetry(); configureDriverBindings(); configureOperatorBindings(); + // configureTestOperatorBindings(); + configurePitDashboard(); } private void configureTelemetry() { @@ -150,6 +168,7 @@ private void configureTelemetry() { algaeSubsystem.registerWith(telemetryService); elevatorSubsystem.registerWith(telemetryService); funnelSubsystem.registerWith(telemetryService); + biscuitSubsystem.registerWith(telemetryService); telemetryService.start(); } @@ -158,13 +177,79 @@ private void configureDriverBindings() { new DriveTeleopCommand( () -> flysky.getFwd(), () -> flysky.getStr(), () -> flysky.getYaw(), driveSubsystem)); - // Reset Gyro Command + // Reset Gyro Command, stow, interrupt Auton, and zero elev new JoystickButton(driveJoystick, Button.M_SWC.id).onTrue(new ResetGyroCommand(driveSubsystem)); + new JoystickButton(driveJoystick, Button.SWD.id) + .onTrue( + new StowCommand( + robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem)) + .onFalse( + new StowCommand( + robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem)); + new JoystickButton(driveJoystick, Button.SWA.id) + .onTrue(new InterruptAutoCommand(robotStateSubsystem)) + .onFalse(new InterruptAutoCommand(robotStateSubsystem)); + new JoystickButton(driveJoystick, Button.SWB_UP.id) + .onTrue(new ZeroElevatorCommand(elevatorSubsystem)) + .onFalse(new ZeroElevatorCommand(elevatorSubsystem)); + new JoystickButton(driveJoystick, Button.SWB_DWN.id) + .onTrue(new ZeroElevatorCommand(elevatorSubsystem)) + .onFalse(new ZeroElevatorCommand(elevatorSubsystem)); + + // other stuff new JoystickButton(driveJoystick, Button.M_SWH.id) - .onTrue(new ScoreReefManualCommand(robotStateSubsystem, elevatorSubsystem, coralSubsystem)); + .onTrue( + new ReefCycleCommand( + robotStateSubsystem, + elevatorSubsystem, + coralSubsystem, + biscuitSubsystem, + algaeSubsystem)); + new JoystickButton(driveJoystick, Button.M_SWE.id) + .onTrue( + new ScoreAlgaeCommand( + robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem)); + new JoystickButton(driveJoystick, Button.SWF_UP.id) + .onTrue( + new FloorAlgaeCommand( + robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem)) + .onFalse( + new FloorAlgaeCommand( + robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem)); + new JoystickButton(driveJoystick, Button.SWF_DWN.id) + .onTrue( + new FloorAlgaeCommand( + robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem)) + .onFalse( + new FloorAlgaeCommand( + robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem)); } private void configureOperatorBindings() { + // Move biscuit + new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband)) + .onTrue( + new JogBiscuitCommand( + biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations))) + .onFalse(new HoldBiscuitCommand(biscuitSubsystem)); + new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband)) + .onTrue( + new JogBiscuitCommand( + biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations))) + .onFalse(new HoldBiscuitCommand(biscuitSubsystem)); + + // Move elevator + new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband)) + .onTrue( + new JogElevatorCommand( + elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations))) + .onFalse(new HoldElevatorCommand(elevatorSubsystem)); + new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband)) + .onTrue( + new JogElevatorCommand( + elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations))) + .onFalse(new HoldElevatorCommand(elevatorSubsystem)); + // Set Levels new Trigger(() -> xboxController.getLeftTriggerAxis() > RobotConstants.kTriggerDeadband) .onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L1)); @@ -181,12 +266,56 @@ private void configureOperatorBindings() { new StowCommand( robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem)); - // zero elevator, slated for removal + // Algae + new JoystickButton(xboxController, XboxController.Button.kY.value) + .onTrue(new ToggleAlgaeHeightCommand(robotStateSubsystem)); new JoystickButton(xboxController, XboxController.Button.kX.value) - .onTrue(new ZeroElevatorCommand(elevatorSubsystem)); + .onTrue(new ToggleGetAlgaeCommand(robotStateSubsystem)); + new JoystickButton(xboxController, XboxController.Button.kB.value) + .onTrue( + new HPAlgaeCommand( + robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem)); + + // Scoring + new JoystickButton(xboxController, XboxController.Button.kA.value) + .onTrue(new ToggleAutoCommand(robotStateSubsystem)); + + new JoystickButton(xboxController, XboxController.Button.kRightStick.value) + .onTrue(new SetScoreSideRightCommand(robotStateSubsystem)); + new JoystickButton(xboxController, XboxController.Button.kLeftStick.value) + .onTrue(new setScoreSideLeftCommand(robotStateSubsystem)); } private void configureTestOperatorBindings() { + new JoystickButton(xboxController, XboxController.Button.kLeftBumper.value) + .onTrue(new IntakeAlgaeCommand(algaeSubsystem)); + new JoystickButton(xboxController, XboxController.Button.kRightBumper.value) + .onTrue(new ProcessorAlgaeCommand(algaeSubsystem)); + + // Move biscuit + new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband)) + .onTrue( + new JogBiscuitCommand( + biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations))) + .onFalse(new HoldBiscuitCommand(biscuitSubsystem)); + new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband)) + .onTrue( + new JogBiscuitCommand( + biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations))) + .onFalse(new HoldBiscuitCommand(biscuitSubsystem)); + + // Move elevator + new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband)) + .onTrue( + new JogElevatorCommand( + elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations))) + .onFalse(new HoldElevatorCommand(elevatorSubsystem)); + new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband)) + .onTrue( + new JogElevatorCommand( + elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations))) + .onFalse(new HoldElevatorCommand(elevatorSubsystem)); + // Stop Coral new JoystickButton(xboxController, XboxController.Button.kB.value) .onTrue(new OpenLoopCoralCommand(coralSubsystem, 0)); @@ -201,13 +330,25 @@ private void configureTestOperatorBindings() { .onTrue(new OpenLoopCoralCommand(coralSubsystem, 1)) .onTrue(new EnableEjectBeamCommand(false, coralSubsystem)); - // Move Elevator + // Move biscuit new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband)) + .onTrue( + new JogBiscuitCommand( + biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations))) + .onFalse(new HoldBiscuitCommand(biscuitSubsystem)); + new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband)) + .onTrue( + new JogBiscuitCommand( + biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations))) + .onFalse(new HoldBiscuitCommand(biscuitSubsystem)); + + // Move elevator + new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband)) .onTrue( new JogElevatorCommand( elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations))) .onFalse(new HoldElevatorCommand(elevatorSubsystem)); - new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband)) + new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband)) .onTrue( new JogElevatorCommand( elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations))) @@ -243,6 +384,19 @@ private void configureTestOperatorBindings() { new SetElevatorPositionCommand(elevatorSubsystem, ElevatorConstants.kL4CoralSetpoint)); } + public void configurePitDashboard() { + + Shuffleboard.getTab("Pit") + .add("Toggle Has Algae", new ToggleHasAlgaeCommand(algaeSubsystem)) + .withPosition(3, 2) + .withSize(1, 1); + + Shuffleboard.getTab("Pit") + .add("Zero Elevator", new ZeroElevatorCommand(elevatorSubsystem)) + .withPosition(2, 1) + .withSize(1, 1); + } + public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } diff --git a/src/main/java/frc/robot/commands/algae/IntakeAlgaeCommand.java b/src/main/java/frc/robot/commands/algae/IntakeAlgaeCommand.java new file mode 100644 index 00000000..2b94e06e --- /dev/null +++ b/src/main/java/frc/robot/commands/algae/IntakeAlgaeCommand.java @@ -0,0 +1,17 @@ +package frc.robot.commands.algae; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.algae.AlgaeSubsystem; + +public class IntakeAlgaeCommand extends InstantCommand { + private AlgaeSubsystem algaeSubsystem; + + public IntakeAlgaeCommand(AlgaeSubsystem algaeSubsystem) { + this.algaeSubsystem = algaeSubsystem; + } + + @Override + public void initialize() { + algaeSubsystem.intake(); + } +} diff --git a/src/main/java/frc/robot/commands/algae/ProcessorAlgaeCommand.java b/src/main/java/frc/robot/commands/algae/ProcessorAlgaeCommand.java new file mode 100644 index 00000000..101bf221 --- /dev/null +++ b/src/main/java/frc/robot/commands/algae/ProcessorAlgaeCommand.java @@ -0,0 +1,17 @@ +package frc.robot.commands.algae; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.algae.AlgaeSubsystem; + +public class ProcessorAlgaeCommand extends InstantCommand { + private AlgaeSubsystem algaeSubsystem; + + public ProcessorAlgaeCommand(AlgaeSubsystem algaeSubsystem) { + this.algaeSubsystem = algaeSubsystem; + } + + @Override + public void initialize() { + algaeSubsystem.scoreProcessor(); + } +} diff --git a/src/main/java/frc/robot/commands/algae/ToggleHasAlgaeCommand.java b/src/main/java/frc/robot/commands/algae/ToggleHasAlgaeCommand.java new file mode 100644 index 00000000..4b28ef4e --- /dev/null +++ b/src/main/java/frc/robot/commands/algae/ToggleHasAlgaeCommand.java @@ -0,0 +1,19 @@ +package frc.robot.commands.algae; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.algae.AlgaeSubsystem.AlgaeStates; + +public class ToggleHasAlgaeCommand extends InstantCommand { + private AlgaeSubsystem algaeSubsystem; + + public ToggleHasAlgaeCommand(AlgaeSubsystem algaeSubsystem) { + this.algaeSubsystem = algaeSubsystem; + } + + @Override + public void initialize() { + algaeSubsystem.setState( + algaeSubsystem.getState() == AlgaeStates.EMPTY ? AlgaeStates.HAS_ALGAE : AlgaeStates.EMPTY); + } +} diff --git a/src/main/java/frc/robot/commands/biscuit/HoldBiscuitCommand.java b/src/main/java/frc/robot/commands/biscuit/HoldBiscuitCommand.java new file mode 100644 index 00000000..10ddb00e --- /dev/null +++ b/src/main/java/frc/robot/commands/biscuit/HoldBiscuitCommand.java @@ -0,0 +1,18 @@ +package frc.robot.commands.biscuit; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.biscuit.BiscuitSubsystem; + +public class HoldBiscuitCommand extends InstantCommand { + private BiscuitSubsystem biscuitSubsystem; + + public HoldBiscuitCommand(BiscuitSubsystem biscuitSubsystem) { + this.biscuitSubsystem = biscuitSubsystem; + addRequirements(biscuitSubsystem); + } + + @Override + public void initialize() { + biscuitSubsystem.setPosition(biscuitSubsystem.getPosition()); + } +} diff --git a/src/main/java/frc/robot/commands/biscuit/JogBiscuitCommand.java b/src/main/java/frc/robot/commands/biscuit/JogBiscuitCommand.java new file mode 100644 index 00000000..fa73d747 --- /dev/null +++ b/src/main/java/frc/robot/commands/biscuit/JogBiscuitCommand.java @@ -0,0 +1,32 @@ +package frc.robot.commands.biscuit; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.biscuit.BiscuitSubsystem; + +public class JogBiscuitCommand extends Command { + + private BiscuitSubsystem biscuitSubsystem; + private Angle positionChange; + + public JogBiscuitCommand(BiscuitSubsystem biscuitSubsystem, Angle positionChange) { + this.biscuitSubsystem = biscuitSubsystem; + this.positionChange = positionChange; + addRequirements(biscuitSubsystem); + } + + @Override + public void initialize() { + biscuitSubsystem.setPosition(biscuitSubsystem.getPosition().plus(positionChange)); + } + + @Override + public void execute() { + biscuitSubsystem.setPosition(biscuitSubsystem.getPosition().plus(positionChange)); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/robotState/ClimbCommand.java b/src/main/java/frc/robot/commands/robotState/ClimbCommand.java new file mode 100644 index 00000000..ba1609a5 --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/ClimbCommand.java @@ -0,0 +1,17 @@ +package frc.robot.commands.robotState; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.robotState.RobotStateSubsystem; + +public class ClimbCommand extends InstantCommand { + RobotStateSubsystem robotState; + + public ClimbCommand(RobotStateSubsystem robotState) { + this.robotState = robotState; + } + + @Override + public void initialize() { + robotState.toClimb(); + } +} diff --git a/src/main/java/frc/robot/commands/robotState/ClimbPrepCommand.java b/src/main/java/frc/robot/commands/robotState/ClimbPrepCommand.java new file mode 100644 index 00000000..bf400e9e --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/ClimbPrepCommand.java @@ -0,0 +1,17 @@ +package frc.robot.commands.robotState; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.robotState.RobotStateSubsystem; + +public class ClimbPrepCommand extends InstantCommand { + RobotStateSubsystem robotState; + + public ClimbPrepCommand(RobotStateSubsystem robotState) { + this.robotState = robotState; + } + + @Override + public void initialize() { + robotState.toPrepClimb(); + } +} diff --git a/src/main/java/frc/robot/commands/robotState/FloorAlgaeCommand.java b/src/main/java/frc/robot/commands/robotState/FloorAlgaeCommand.java new file mode 100644 index 00000000..cf0e8c4b --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/FloorAlgaeCommand.java @@ -0,0 +1,37 @@ +package frc.robot.commands.robotState; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.biscuit.BiscuitSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates; + +public class FloorAlgaeCommand extends Command { + RobotStateSubsystem robotState; + boolean hasTriedToPickup = false; + + public FloorAlgaeCommand( + RobotStateSubsystem robotState, + ElevatorSubsystem elevatorSubsystem, + BiscuitSubsystem biscuitSubsystem, + AlgaeSubsystem algaeSubsystem) { + this.robotState = robotState; + addRequirements(elevatorSubsystem, biscuitSubsystem, algaeSubsystem); + } + + @Override + public void initialize() { + hasTriedToPickup = false; + robotState.toAlgaeFloorPickup(); + } + + @Override + public boolean isFinished() { + if (robotState.getState() == RobotStates.FLOOR_ALGAE + || robotState.getState() == RobotStates.MIC_ALGAE) { + hasTriedToPickup = true; + } + return robotState.getState() == RobotStates.STOW && hasTriedToPickup; + } +} diff --git a/src/main/java/frc/robot/commands/robotState/HPAlgaeCommand.java b/src/main/java/frc/robot/commands/robotState/HPAlgaeCommand.java new file mode 100644 index 00000000..6f25e4c0 --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/HPAlgaeCommand.java @@ -0,0 +1,36 @@ +package frc.robot.commands.robotState; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.biscuit.BiscuitSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates; + +public class HPAlgaeCommand extends Command { + RobotStateSubsystem robotState; + private boolean hasTriedToGrab = false; + + public HPAlgaeCommand( + RobotStateSubsystem robotState, + ElevatorSubsystem elevatorSubsystem, + BiscuitSubsystem biscuitSubsystem, + AlgaeSubsystem algaeSubsystem) { + this.robotState = robotState; + addRequirements(elevatorSubsystem, biscuitSubsystem, algaeSubsystem); + } + + @Override + public void initialize() { + hasTriedToGrab = false; + robotState.toHpAlgae(); + } + + @Override + public boolean isFinished() { + if (robotState.getState() == RobotStates.HP_ALGAE) { + hasTriedToGrab = true; + } + return robotState.getState() == RobotStates.STOW && hasTriedToGrab; + } +} diff --git a/src/main/java/frc/robot/commands/robotState/InterruptAutoCommand.java b/src/main/java/frc/robot/commands/robotState/InterruptAutoCommand.java new file mode 100644 index 00000000..d7790eb2 --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/InterruptAutoCommand.java @@ -0,0 +1,18 @@ +package frc.robot.commands.robotState; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.robotState.RobotStateSubsystem; + +public class InterruptAutoCommand + extends InstantCommand { // TODO fix this command, it dose not work + RobotStateSubsystem robotState; + + public InterruptAutoCommand(RobotStateSubsystem robotState) { + this.robotState = robotState; + } + + @Override + public void initialize() { + robotState.toInterrupted(); + } +} diff --git a/src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java b/src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java new file mode 100644 index 00000000..4b0695bb --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/ReefCycleCommand.java @@ -0,0 +1,53 @@ +package frc.robot.commands.robotState; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.biscuit.BiscuitSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates; + +public class ReefCycleCommand extends Command { + private RobotStateSubsystem robotStateSubsystem; + private ElevatorSubsystem elevatorSubsystem; + private RobotStates startingRobotState; + private boolean startingElevatorFinished; + private boolean isAutoPlacing; + + public ReefCycleCommand( + RobotStateSubsystem robotStateSubsystem, + ElevatorSubsystem elevatorSubsystem, + CoralSubsystem coralSubsystem, + BiscuitSubsystem biscuitSubsystem, + AlgaeSubsystem algaeSubsystem) { + this.robotStateSubsystem = robotStateSubsystem; + this.elevatorSubsystem = elevatorSubsystem; + addRequirements(elevatorSubsystem, coralSubsystem, biscuitSubsystem, algaeSubsystem); + } + + @Override + public void initialize() { + startingRobotState = robotStateSubsystem.getState(); + startingElevatorFinished = elevatorSubsystem.isFinished(); + isAutoPlacing = robotStateSubsystem.getAutoPlaceOnCycle(); + robotStateSubsystem.toPrepCoral(); + } + + @Override + public boolean isFinished() { + if (isAutoPlacing) { + return robotStateSubsystem.getState() == RobotStates.FUNNEL_LOAD; + } else { + if (startingRobotState == RobotStates.PRESTAGE || startingRobotState == RobotStates.STOW) { + return robotStateSubsystem.getState() == RobotStates.REEF_ALIGN_CORAL; + } + if (startingRobotState == RobotStates.REEF_ALIGN_CORAL) { + return robotStateSubsystem.getState() == RobotStates.FUNNEL_LOAD + || robotStateSubsystem.getState() == RobotStates.LOADING_CORAL + || !startingElevatorFinished; + } + } + return false; + } +} diff --git a/src/main/java/frc/robot/commands/robotState/ScoreAlgaeCommand.java b/src/main/java/frc/robot/commands/robotState/ScoreAlgaeCommand.java new file mode 100644 index 00000000..380863f7 --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/ScoreAlgaeCommand.java @@ -0,0 +1,49 @@ +package frc.robot.commands.robotState; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.biscuit.BiscuitSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates; + +public class ScoreAlgaeCommand extends Command { + private RobotStateSubsystem robotStateSubsystem; + private ElevatorSubsystem elevatorSubsystem; + private RobotStates startState; + private boolean startingElevatorFinished; + + public ScoreAlgaeCommand( + RobotStateSubsystem robotStateSubsystem, + ElevatorSubsystem elevatorSubsystem, + BiscuitSubsystem biscuitSubsystem, + AlgaeSubsystem algaeSubsystem) { + this.robotStateSubsystem = robotStateSubsystem; + this.elevatorSubsystem = elevatorSubsystem; + addRequirements(elevatorSubsystem, biscuitSubsystem, algaeSubsystem); + } + + @Override + public void initialize() { + startState = robotStateSubsystem.getState(); + startingElevatorFinished = elevatorSubsystem.isFinished(); + if (startState == RobotStates.BARGE_ALGAE || startState == RobotStates.PROCESSOR_ALGAE) { + robotStateSubsystem.releaseAlgae(); + } else { + robotStateSubsystem.toScoreAlgae(); + } + } + + @Override + public boolean isFinished() { + if (startState == RobotStates.BARGE_ALGAE || startState == RobotStates.PROCESSOR_ALGAE) { + return robotStateSubsystem.getState() == RobotStates.FUNNEL_LOAD + || robotStateSubsystem.getState() == RobotStates.STOW + || !startingElevatorFinished; + } else { + return robotStateSubsystem.getState() == RobotStates.BARGE_ALGAE + || robotStateSubsystem.getState() == RobotStates.PROCESSOR_ALGAE + || !robotStateSubsystem.isBargeSafe; + } + } +} diff --git a/src/main/java/frc/robot/commands/robotState/ScoreReefManualCommand.java b/src/main/java/frc/robot/commands/robotState/ScoreReefManualCommand.java index e792f64e..700f5638 100644 --- a/src/main/java/frc/robot/commands/robotState/ScoreReefManualCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ScoreReefManualCommand.java @@ -25,7 +25,7 @@ public ScoreReefManualCommand( public void initialize() { startingRobotState = robotStateSubsystem.getState(); startingElevatorFinished = elevatorSubsystem.isFinished(); - robotStateSubsystem.setIsAuto(false); + robotStateSubsystem.setAutoPlaceOnCycle(false); robotStateSubsystem.setGetAlgaeOnCycle(false); robotStateSubsystem.toPrepCoral(); } diff --git a/src/main/java/frc/robot/commands/robotState/SetScoreSideRightCommand.java b/src/main/java/frc/robot/commands/robotState/SetScoreSideRightCommand.java new file mode 100644 index 00000000..f35a8259 --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/SetScoreSideRightCommand.java @@ -0,0 +1,18 @@ +package frc.robot.commands.robotState; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide; + +public class SetScoreSideRightCommand extends InstantCommand { + RobotStateSubsystem robotState; + + public SetScoreSideRightCommand(RobotStateSubsystem robotState) { + this.robotState = robotState; + } + + @Override + public void initialize() { + robotState.setScoreSide(ScoreSide.RIGHT); + } +} diff --git a/src/main/java/frc/robot/commands/robotState/ToggleAlgaeHeightCommand.java b/src/main/java/frc/robot/commands/robotState/ToggleAlgaeHeightCommand.java new file mode 100644 index 00000000..cdc5a457 --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/ToggleAlgaeHeightCommand.java @@ -0,0 +1,17 @@ +package frc.robot.commands.robotState; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.robotState.RobotStateSubsystem; + +public class ToggleAlgaeHeightCommand extends InstantCommand { + RobotStateSubsystem robotState; + + public ToggleAlgaeHeightCommand(RobotStateSubsystem robotState) { + this.robotState = robotState; + } + + @Override + public void initialize() { + robotState.toggleAlgaeHeight(); + } +} diff --git a/src/main/java/frc/robot/commands/robotState/ToggleAutoCommand.java b/src/main/java/frc/robot/commands/robotState/ToggleAutoCommand.java new file mode 100644 index 00000000..3199458c --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/ToggleAutoCommand.java @@ -0,0 +1,21 @@ +package frc.robot.commands.robotState; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.robotState.RobotStateSubsystem; + +public class ToggleAutoCommand extends InstantCommand { + RobotStateSubsystem robotState; + + public ToggleAutoCommand(RobotStateSubsystem robotState) { + this.robotState = robotState; + } + + @Override + public void initialize() { + if (robotState.getAutoPlaceOnCycle() == false) { + robotState.setAutoPlaceOnCycle(true); + } else { + robotState.setAutoPlaceOnCycle(false); + } + } +} diff --git a/src/main/java/frc/robot/commands/robotState/ToggleGetAlgaeCommand.java b/src/main/java/frc/robot/commands/robotState/ToggleGetAlgaeCommand.java new file mode 100644 index 00000000..7f397221 --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/ToggleGetAlgaeCommand.java @@ -0,0 +1,17 @@ +package frc.robot.commands.robotState; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.robotState.RobotStateSubsystem; + +public class ToggleGetAlgaeCommand extends InstantCommand { + RobotStateSubsystem robotStateSubsystem; + + public ToggleGetAlgaeCommand(RobotStateSubsystem robotStateSubsystem) { + this.robotStateSubsystem = robotStateSubsystem; + } + + @Override + public void initialize() { + robotStateSubsystem.ToggleGetAlgaeOnCycle(); + } +} diff --git a/src/main/java/frc/robot/commands/robotState/setScoreSideLeftCommand.java b/src/main/java/frc/robot/commands/robotState/setScoreSideLeftCommand.java new file mode 100644 index 00000000..3acbf9d9 --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/setScoreSideLeftCommand.java @@ -0,0 +1,18 @@ +package frc.robot.commands.robotState; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide; + +public class setScoreSideLeftCommand extends InstantCommand { + RobotStateSubsystem robotState; + + public setScoreSideLeftCommand(RobotStateSubsystem robotState) { + this.robotState = robotState; + } + + @Override + public void initialize() { + robotState.setScoreSide(ScoreSide.LEFT); + } +} diff --git a/src/main/java/frc/robot/constants/AlgaeConstants.java b/src/main/java/frc/robot/constants/AlgaeConstants.java index 71e31cd3..45b3c0f5 100644 --- a/src/main/java/frc/robot/constants/AlgaeConstants.java +++ b/src/main/java/frc/robot/constants/AlgaeConstants.java @@ -1,48 +1,52 @@ package frc.robot.constants; -import static edu.wpi.first.units.Units.RotationsPerSecond; - +import com.ctre.phoenix6.configs.CommutationConfigs; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.ExternalFeedbackConfigs; import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TalonFXSConfiguration; +import com.ctre.phoenix6.signals.ExternalFeedbackSensorSourceValue; import com.ctre.phoenix6.signals.ForwardLimitSourceValue; import com.ctre.phoenix6.signals.ForwardLimitTypeValue; import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorArrangementValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.ReverseLimitSourceValue; import com.ctre.phoenix6.signals.ReverseLimitTypeValue; -import edu.wpi.first.units.measure.AngularVelocity; public class AlgaeConstants { public static final int kFxId = 30; - public static final AngularVelocity kCloseEnough = RotationsPerSecond.of(0.1); - public static final AngularVelocity kMaxFwd = RotationsPerSecond.of(100); - public static final AngularVelocity kMaxRev = RotationsPerSecond.of(-100); + public static final double kCloseEnough = 0.1; + public static final double kMaxFwd = 100; + public static final double kMaxRev = -100; + + public static final double kHoldSpeed = -0.1; + public static final double kBargeScoreSpeed = 1; + public static final double kProcessorScoreSpeed = 1; + public static final double kIntakingSpeed = -1; - public static final AngularVelocity kHoldSpeed = RotationsPerSecond.of(-0.1); - public static final AngularVelocity kBargeScoreSpeed = RotationsPerSecond.of(1); - public static final AngularVelocity kProcessorScoreSpeed = RotationsPerSecond.of(1); - public static final AngularVelocity kIntakingSpeed = RotationsPerSecond.of(-1); + public static final double kHasAlgaeVelThreshold = 10; + public static final double kHasAlgaeCounts = 1; // Example Talon FX Config - public static TalonFXConfiguration getFXConfig() { - TalonFXConfiguration fxConfig = new TalonFXConfiguration(); + public static TalonFXSConfiguration getFXConfig() { + TalonFXSConfiguration fxsConfig = new TalonFXSConfiguration(); CurrentLimitsConfigs current = new CurrentLimitsConfigs() - .withStatorCurrentLimit(10) - .withStatorCurrentLimitEnable(false) - .withStatorCurrentLimit(20) + .withStatorCurrentLimit(40) + .withStatorCurrentLimitEnable(true) .withSupplyCurrentLimit(10) - .withSupplyCurrentLowerLimit(8) - .withSupplyCurrentLowerTime(0.02) + .withSupplyCurrentLowerLimit(2) + .withSupplyCurrentLowerTime(1) .withSupplyCurrentLimitEnable(true); - fxConfig.CurrentLimits = current; + fxsConfig.CurrentLimits = current; HardwareLimitSwitchConfigs hwLimit = new HardwareLimitSwitchConfigs() @@ -54,15 +58,13 @@ public static TalonFXConfiguration getFXConfig() { .withReverseLimitEnable(false) .withReverseLimitType(ReverseLimitTypeValue.NormallyOpen) .withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin); - fxConfig.HardwareLimitSwitch = hwLimit; + fxsConfig.HardwareLimitSwitch = hwLimit; SoftwareLimitSwitchConfigs swLimit = new SoftwareLimitSwitchConfigs() - .withForwardSoftLimitEnable(true) - .withForwardSoftLimitThreshold(kMaxFwd.in(RotationsPerSecond)) - .withReverseSoftLimitEnable(true) - .withReverseSoftLimitThreshold(kMaxRev.in(RotationsPerSecond)); - fxConfig.SoftwareLimitSwitch = swLimit; + .withForwardSoftLimitEnable(false) + .withReverseSoftLimitEnable(false); + fxsConfig.SoftwareLimitSwitch = swLimit; Slot0Configs slot0 = new Slot0Configs() @@ -74,7 +76,7 @@ public static TalonFXConfiguration getFXConfig() { .withKS(0) .withKV(0) .withKA(0); - fxConfig.Slot0 = slot0; + fxsConfig.Slot0 = slot0; MotionMagicConfigs motionMagic = new MotionMagicConfigs() @@ -83,14 +85,24 @@ public static TalonFXConfiguration getFXConfig() { .withMotionMagicExpo_kA(0) .withMotionMagicExpo_kV(0) .withMotionMagicJerk(0); - fxConfig.MotionMagic = motionMagic; + fxsConfig.MotionMagic = motionMagic; MotorOutputConfigs motorOut = new MotorOutputConfigs() .withDutyCycleNeutralDeadband(0.01) - .withNeutralMode(NeutralModeValue.Coast); - fxConfig.MotorOutput = motorOut; + .withInverted(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake); + fxsConfig.MotorOutput = motorOut; + + CommutationConfigs commutationConfigs = + new CommutationConfigs().withMotorArrangement(MotorArrangementValue.Minion_JST); + fxsConfig.Commutation = commutationConfigs; + + ExternalFeedbackConfigs externalFeedbackConfigs = + new ExternalFeedbackConfigs() + .withExternalFeedbackSensorSource(ExternalFeedbackSensorSourceValue.Commutation); + fxsConfig.ExternalFeedback = externalFeedbackConfigs; - return fxConfig; + return fxsConfig; } } diff --git a/src/main/java/frc/robot/constants/BiscuitConstants.java b/src/main/java/frc/robot/constants/BiscuitConstants.java index 7c3e76fa..e408d970 100644 --- a/src/main/java/frc/robot/constants/BiscuitConstants.java +++ b/src/main/java/frc/robot/constants/BiscuitConstants.java @@ -2,7 +2,9 @@ import static edu.wpi.first.units.Units.Rotations; +import com.ctre.phoenix6.configs.CommutationConfigs; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.ExternalFeedbackConfigs; import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; @@ -10,55 +12,61 @@ import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.TalonFXSConfiguration; import com.ctre.phoenix6.configs.VoltageConfigs; +import com.ctre.phoenix6.signals.ExternalFeedbackSensorSourceValue; import com.ctre.phoenix6.signals.ForwardLimitSourceValue; import com.ctre.phoenix6.signals.ForwardLimitTypeValue; import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.MotorArrangementValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.ReverseLimitSourceValue; import com.ctre.phoenix6.signals.ReverseLimitTypeValue; import edu.wpi.first.units.measure.Angle; public class BiscuitConstants { - // These are all wrong right now because we don't have any actual info - public static Angle kZero = Rotations.of(42); // Will need to be experimentally determined - public static int talonID = 25; - public static double kCloseEnough = 0.05; // This was a little out of wack. - public static final Angle kMaxFwd = Rotations.of(100); - public static final Angle kMaxRev = Rotations.of(-100); + public static final double kZero = .36; + public static final double kTicksPerRot = 160; + public static final int talonID = 25; + public static final double kCloseEnough = 0.05; + public static final Angle kMaxFwd = Rotations.of(51.04735 + 5); + public static final Angle kMaxRev = Rotations.of(-12.3489 - 5); + public static final double kSafeToStowUpper = 40; + public static final double kSafeToStowLower = -5; // Setpoints // Idle - public static final Angle kStowSetpoint = Rotations.of(0.0); - public static final Angle kFunnelSetpoint = Rotations.of(0.0); - public static final Angle kPrestageSetpoint = Rotations.of(0.0); + public static final Angle kStowSetpoint = Rotations.of(1.862); + public static final Angle kFunnelSetpoint = kStowSetpoint; + public static final Angle kPrestageSetpoint = kStowSetpoint; // Algae removal - public static final Angle kL2AlgaeSetpoint = Rotations.of(0.0); - public static final Angle kL3AlgaeSetpoint = Rotations.of(0.0); + public static final Angle kL2AlgaeSetpoint = Rotations.of(20.848); + public static final Angle kL3AlgaeSetpoint = Rotations.of(24.562); public static final Angle kL2AlgaeRemovalSetpoint = Rotations.of(0.0); public static final Angle kL3AlgaeRemovalSetpoint = Rotations.of(0.0); - public static final Angle kSafeAlgaeRemovalSetpoint = Rotations.of(0.0); - public static final Angle kSafeAlgaeRemovalRotateSetpoint = Rotations.of(0.0); - // Coral score - public static final Angle kL1CoralSetpoint = Rotations.of(0.0); - public static final Angle kL2CoralSetpoint = Rotations.of(0.0); - public static final Angle kL3CoralSetpoint = Rotations.of(0.0); - public static final Angle kL4CoralSetpoint = Rotations.of(0.0); + public static final Angle kL1CoralSetpoint = kStowSetpoint; + public static final Angle kL2CoralSetpoint = kStowSetpoint; + public static final Angle kL3CoralSetpoint = kStowSetpoint; + public static final Angle kL4CoralSetpoint = kStowSetpoint; // Algae obtaining - public static final Angle kFloorAlgaeSetpoint = Rotations.of(0.0); - public static final Angle kMicAlgaeSetpoint = Rotations.of(0.0); - public static final Angle kHpAlgaeSetpoint = Rotations.of(0.0); + public static final Angle kFloorAlgaeSetpoint = Rotations.of(49.627); + public static final Angle kMicAlgaeSetpoint = Rotations.of(51.61872); + public static final Angle kHpAlgaeSetpoint = Rotations.of(16.97559); // Algae scoring - public static final Angle kProcessorSetpoint = Rotations.of(0.0); - public static final Angle kBargeSetpoint = Rotations.of(0.0); + public static final Angle kProcessorSetpoint = Rotations.of(41.193); + public static final Angle kBargeSetpoint = Rotations.of(12.3489); + public static final Angle kBargeBackwardSetpoint = Rotations.of(-12.3489); + + // jogging + public static final double kJogAmountUp = 10; + public static final double kJogAmountDown = -10; - // Disables the TalonFXS by setting it's voltage to zero. Not very shocking. + // Disables the TalonFXS by setting its voltage to zero. public static VoltageConfigs disableTalon() { VoltageConfigs voltage = new VoltageConfigs().withPeakForwardVoltage(0.0).withPeakReverseVoltage(0.0); @@ -71,12 +79,11 @@ public static TalonFXSConfiguration getFXSConfig() { CurrentLimitsConfigs current = new CurrentLimitsConfigs() - .withStatorCurrentLimit(10) + .withStatorCurrentLimit(0) .withStatorCurrentLimitEnable(false) - .withStatorCurrentLimit(20) - .withSupplyCurrentLimit(10) - .withSupplyCurrentLowerLimit(8) - .withSupplyCurrentLowerTime(0.02) + .withSupplyCurrentLimit(20) + .withSupplyCurrentLowerLimit(5) + .withSupplyCurrentLowerTime(2) .withSupplyCurrentLimitEnable(true); fxsConfig.CurrentLimits = current; @@ -102,31 +109,40 @@ public static TalonFXSConfiguration getFXSConfig() { Slot0Configs slot0 = new Slot0Configs() - .withKP(0) + .withKP(2) .withKI(0) .withKD(0) .withGravityType(GravityTypeValue.Elevator_Static) .withKG(0) .withKS(0) - .withKV(0) + .withKV(0.1) .withKA(0); fxsConfig.Slot0 = slot0; MotionMagicConfigs motionMagic = new MotionMagicConfigs() - .withMotionMagicAcceleration(0) - .withMotionMagicCruiseVelocity(0) + .withMotionMagicAcceleration(500) + .withMotionMagicCruiseVelocity(100) .withMotionMagicExpo_kA(0) .withMotionMagicExpo_kV(0) - .withMotionMagicJerk(0); + .withMotionMagicJerk(1000); fxsConfig.MotionMagic = motionMagic; MotorOutputConfigs motorOut = new MotorOutputConfigs() .withDutyCycleNeutralDeadband(0.01) - .withNeutralMode(NeutralModeValue.Coast); + .withNeutralMode(NeutralModeValue.Brake); fxsConfig.MotorOutput = motorOut; + CommutationConfigs commutation = + new CommutationConfigs().withMotorArrangement(MotorArrangementValue.Minion_JST); + fxsConfig.Commutation = commutation; + + ExternalFeedbackConfigs feedBack = + new ExternalFeedbackConfigs() + .withExternalFeedbackSensorSource(ExternalFeedbackSensorSourceValue.Commutation); + fxsConfig.ExternalFeedback = feedBack; + return fxsConfig; } } diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java index a97b3763..9c54826b 100644 --- a/src/main/java/frc/robot/constants/DriveConstants.java +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -10,19 +10,29 @@ import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; public class DriveConstants { public static final double kAlgaeRemovalSpeed = 0; + public static final double kBargeScoreStickMultiplier = 0.5; public static final double kDeadbandAllStick = 0.075; public static final double kExpoScaleYawFactor = 0.75; public static final double kRateLimitFwdStr = 3.5; public static final double kRateLimitYaw = 8.0; - public static final double kDriveGearRatio = 6.5; + public static final double kDriveMotorOutputGear = 22; + public static final double kDriveInputGear = 52; + public static final double kBevelInputGear = 15; + public static final double kBevelOutputGear = 45; + + public static final double kDriveGearRatio = + (kDriveMotorOutputGear / kDriveInputGear) * (kBevelInputGear / kBevelOutputGear); + public static final double kWheelDiameterInches = 4.0; - public static final double kMaxSpeedMetersPerSecond = 12.0; + public static final double kMaxSpeedMetersPerSecond = 3.384; public static final double kSpeedStillThreshold = 0.1; // meters per second public static final double kGyroRateStillThreshold = 10.0; // 25 5 degrees per second public static final double kGyroDifferentThreshold = 5.0; // 5 degrees @@ -31,6 +41,7 @@ public class DriveConstants { public static final double kRobotLength = 0.6223; public static final double kRobotWidth = 0.6223; public static final double kFieldMaxX = 17.526; + public static final double kCenterLineX = 8.763; public static final double kPOmega = 4.5; public static final double kIOmega = 0.0; @@ -60,6 +71,9 @@ public static Translation2d[] getWheelLocationMeters() { public static final double kRecoverTemp = 1290; public static final double kNotifyTemp = 1295; + public static final Pose2d kResetOdomPose = + new Pose2d(new Translation2d(0.5, 3.62), Rotation2d.fromDegrees(67)); + // public static TalonFXSConfiguration // getAzimuthTalonConfig() { // will be changed to a TalonFXConfiguration // // constructor sets encoder to Quad/CTRE_MagEncoder_Relative diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index cfd304a6..13ce04c0 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -21,7 +21,7 @@ public class ElevatorConstants { public static final double kCloseEnoughRotations = 0.1; - public static final double kMaxFwd = 53; // TODO all of these fields need to be filled out + public static final double kMaxFwd = 53; public static final double kMaxRev = 2; public static final int kZeroMultiple = 0; // some constant to multiply, add by to turn the analog input into a position @@ -43,14 +43,11 @@ public class ElevatorConstants { public static final Angle kStowSetpoint = kFunnelSetpoint; // Algae removal - public static final Angle kL2AlgaeSetpoint = Rotations.of(0.0); - public static final Angle kL3AlgaeSetpoint = Rotations.of(0.0); + public static final Angle kL2AlgaeSetpoint = Rotations.of(2.8677); + public static final Angle kL3AlgaeSetpoint = Rotations.of(17.513); - public static final Angle kL2AlgaeRemovalSetpoint = Rotations.of(0.0); - public static final Angle kL3AlgaeRemovalSetpoint = Rotations.of(0.0); - - public static final Angle kSafeAlgaeRemovalSetpoint = Rotations.of(0.0); - public static final Angle kSafeAlgaeRemovalRotateSetpoint = Rotations.of(0.0); + public static final Angle kL2AlgaeRemovalSetpoint = kL2AlgaeSetpoint; + public static final Angle kL3AlgaeRemovalSetpoint = kL3AlgaeSetpoint; // Coral score public static final Angle kL1CoralSetpoint = Rotations.of(13.04053); @@ -62,13 +59,14 @@ public class ElevatorConstants { public static final Angle kPrestageSetpoint = kL3CoralSetpoint; // Algae obtaining - public static final Angle kFloorAlgaeSetpoint = Rotations.of(0.0); - public static final Angle kMicAlgaeSetpoint = Rotations.of(0.0); - public static final Angle kHpAlgaeSetpoint = Rotations.of(0.0); + public static final Angle kFloorAlgaeSetpoint = Rotations.of(6.66); + public static final Angle kMicAlgaeSetpoint = Rotations.of(17.07); + public static final Angle kHpAlgaeSetpoint = Rotations.of(14.9063); // Algae scoring - public static final Angle kProcessorSetpoint = Rotations.of(0.0); - public static final Angle kBargeSetpoint = Rotations.of(0.0); + public static final Angle kProcessorSetpoint = Rotations.of(4.297); + public static final Angle kBargeSetpoint = Rotations.of(41.936); // 40.913 + public static final Angle kBargeHigherThan = Rotations.of(31.0901); public static TalonFXConfiguration getBothFXConfig() { TalonFXConfiguration fxConfig = new TalonFXConfiguration(); diff --git a/src/main/java/frc/robot/constants/RobotStateConstants.java b/src/main/java/frc/robot/constants/RobotStateConstants.java index 06df4d69..1ddc8768 100644 --- a/src/main/java/frc/robot/constants/RobotStateConstants.java +++ b/src/main/java/frc/robot/constants/RobotStateConstants.java @@ -4,8 +4,8 @@ public class RobotStateConstants { public static final double[] kNodeAngles = {0.0, 60.0, 120.0, 180.0, -120.0, -60.0}; public static final double kAlgaeRetreatDistance = 0; - public static final double kRedBargeSafeX = 9.5; - public static final double kBlueBargeSafeX = 8; + public static final double kBlueBargeSafeX = 7.6; + public static final double kRedBargeSafeX = DriveConstants.kFieldMaxX - kBlueBargeSafeX; public static final double kCoralEjectTimer = 0.5; } diff --git a/src/main/java/frc/robot/constants/TagServoingConstants.java b/src/main/java/frc/robot/constants/TagServoingConstants.java index 424f7a57..fe9cf9e4 100644 --- a/src/main/java/frc/robot/constants/TagServoingConstants.java +++ b/src/main/java/frc/robot/constants/TagServoingConstants.java @@ -6,11 +6,16 @@ public class TagServoingConstants { // Cameras public static final int kLeftServoCam = 0; // Score RIGHT coral - public static final int kRightServoCam = 1; // Score LEFT coral + public static final int kRightServoCam = 2; // Score LEFT coral + + // Offsets + public static final double kLeftCamOffset = VisionConstants.kCam1Pose.getY(); + public static final double kRightCamOffset = VisionConstants.kCam2Pose.getY(); // Targets public static final double kHorizontalTarget = 800; - public static final double kAreaTarget = 800; + public static final double kLeftCamDiagTarget = 1180; + public static final double kRightCamDiagTarget = 985; public static final double[] kAngleTarget = { Units.degreesToRadians(0), @@ -26,19 +31,21 @@ public class TagServoingConstants { public static final int[] kRedTargetTag = {7, 8, 9, 10, 11, 6}; // Tag align - public static final double kAreaCloseEnough = 0; - public static final double kHorizontalCloseEnough = 0; + public static final double kHorizontalCloseEnough = 20; public static final double kAngleCloseEnough = Units.degreesToRadians(3.0); + public static final double kDiagCloseEnough = 20; + public static final double kNoUpdateMicrosec = 500_000; // Drive public static final double kInitialDriveRadius = 1.5; - public static final double kStopXDriveRadius = 1.2; // Should be closer to reef than target pose - public static final double kDriveCloseEnough = 0.3; + public static final double kStopXDriveRadius = + kInitialDriveRadius; // Should be closer to reef than target pose + public static final double kMinStopXDriveRadius = 1.35; + public static final double kDriveCloseEnough = 0.1; + public static final double kMinVelX = 0.85; // Reef - public static final Translation2d kBlueReefPose = - new Translation2d(Units.inchesToMeters(223.5), Units.inchesToMeters(158.5)); + public static final Translation2d kBlueReefPose = new Translation2d(4.524, 4.033); - public static final Translation2d kRedReefPose = - kBlueReefPose.plus(new Translation2d(Units.inchesToMeters(337.39), 0)); + public static final Translation2d kRedReefPose = new Translation2d(13.084, 4.033); } diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index cbb8c7e3..1e6a8065 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -19,6 +19,7 @@ public final class VisionConstants { public static final double kMaxAmbig = 1.0; public static final int kMaxTimesOffWheels = 5; public static final double kBumperPixelLine = 87; // 100 + public static final double kRobotHeight = 0.5; // public static final double kThetaStdDevUsed = Units.degreesToRadians(0.02); // public static final double kThetaStdDevRejected = Units.degreesToRadians(360); @@ -29,7 +30,7 @@ public final class VisionConstants { public static final double kOffsetOnVelFilter = 0.10; public static final double kSquaredCoeffOnVelFilter = 0.1; - public static Matrix kStateStdDevs = VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(0)); + public static Matrix kStateStdDevs = VecBuilder.fill(0.1, 0.1, 0); public static final double kTimeStampOffset = 0.0; @@ -37,63 +38,85 @@ public final class VisionConstants { public static final double singleTagCoeff = 25.0 / 100.0; public static final double multiTagCoeff = 18.0 / 100.0; public static final double baseNumber = Math.E; - public static final double powerNumber = 4.0; + public static final double powerNumber = 2.0; + public static final double baseTrust = 3.0; + public static final double FOV45MultiTagCoeff = 16.0 / 100.0; public static final double FOV45powerNumber = 4.5; - public static final double FOV45SinlgeTagCoeff = 21.0 / 100.0; + public static final double FOV45SingleTagCoeff = 21.0 / 100.0; + public static final double FOV45BaseTrust = 2.0; + public static final double FOV58MJPGMultiTagCoeff = 16.0 / 100.0; public static final double FOV58MJPGPowerNumber = 3.5; public static final double FOV58MJPGSingleTagCoeff = 21.0 / 100.0; + public static final double FOV58MJPGBaseTrust = 3.0; + public static final double FOV58YUYVMultiTagCoeff = 17.0 / 100.0; - public static final double FOV58YUYVPowerNumber = 4.0; + public static final double FOV58YUYVPowerNumber = 2.0; public static final double FOV58YUYVSingleTagCoeff = 22.0 / 100.0; + public static final double FOV58YUYVBaseTrust = 3.0; + + public static final double FOV75YUYVMultiTagCoeff = 17.0 / 100.0; + public static final double FOV75YUYVPowerNumber = 2.0; + public static final double FOV75YUYVSingleTagCoeff = 22.0 / 100.0; + public static final double FOV75YUYVBaseTrust = 3.0; + + // Gyro error scaling + public static final double kYawErrorThreshold = Units.degreesToRadians(45); // Constants for cameras - public static final int kNumCams = 4; + public static final int kNumCams = 5; + public static final int kNumPis = 3; + public static final int[] kUdpIndex = {0, 1, 2}; + + // Camera Ports + public static final int[] kCamPorts = {5802, 5802, 5803, 5803, 5804}; // Names - public static final String kCam1Name = "Shooter"; - public static final String kCam2Name = "Intake"; - public static final String kCam3Name = "AngledShooterLeft"; - public static final String kCam4Name = "AngledShooterRight"; + public static final String kCam1Name = "Left Servo"; + public static final String kCam2Name = "Left High"; + public static final String kCam3Name = "Right Servo"; + public static final String kCam4Name = "Right High"; + public static final String kCam5Name = "Rear"; - public static final String kPi1Name = "Shooter"; - public static final String kPi2Name = "Intake"; - public static final String kPi3Name = "AngledShooters"; + public static final String kPi1Name = "Left"; + public static final String kPi2Name = "Right"; + public static final String kPi3Name = "Rear"; // Indexs public static final int kCam1Idx = 0; - public static final int kCam2Idx = 0; + public static final int kCam2Idx = 1; public static final int kCam3Idx = 0; public static final int kCam4Idx = 1; + public static final int kCam5Idx = 0; public static final double kLoopTime = 0.02; public static final int kCircularBufferSize = 1000; // Poses public static final Pose3d kCam1Pose = - new Pose3d( - new Translation3d(-0.27, 0.055, 0.20), - new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(180.0))); + new Pose3d(new Translation3d(0.28, 0.02, 0.30), new Rotation3d()); + public static final Pose3d kCam2Pose = new Pose3d( new Translation3d(-0.21, -0.31, 0.44), new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(0.0))); public static final Pose3d kCam3Pose = - new Pose3d( - new Translation3d(-0.23, 0.33, 0.56), - new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(-132.0))); + new Pose3d(new Translation3d(0.09, -0.31, 0.36), new Rotation3d()); public static final Pose3d kCam4Pose = new Pose3d( new Translation3d(-0.22, -0.335, 0.50), new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(138.0))); - + public static final Pose3d kCam5Pose = + new Pose3d( + new Translation3d(-0.22, -0.335, 0.50), + new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(138.0))); // Increase these numbers to trust sensor readings from encoders and gyros less. This matrix is // in the form [theta], with units in radians. public static Matrix kLocalMeasurementStdDevs = VecBuilder.fill(Units.degreesToRadians(0.01)); // Increase these numbers to trust global measurements from vision less. This matrix is in the - // form [x, y, theta], with units in meters and radians. + // form [x, y, theta]ᵀ, with units in meters and radians. // Vision Odometry Standard devs public static Matrix kVisionMeasurementStdDevs = VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)); diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java b/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java index 826c63c0..7d7ecdbd 100644 --- a/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java +++ b/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java @@ -1,20 +1,18 @@ package frc.robot.subsystems.algae; -import edu.wpi.first.units.measure.AngularVelocity; import org.littletonrobotics.junction.AutoLog; import org.strykeforce.telemetry.TelemetryService; public interface AlgaeIO { @AutoLog public static class AlgaeIOInputs { - public AngularVelocity velocity; - public boolean isFwdLimitSwitchClosed; - public boolean isRevLimitSwitchClosed; + public double velocity; + public boolean isBeamBroken; } public default void updateInputs(AlgaeIOInputs inputs) {} - public default void setSpeed(AngularVelocity speed) {} + public default void setSpeed(double speed) {} public default void setPct(double pct) {} diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeIOFX.java b/src/main/java/frc/robot/subsystems/algae/AlgaeIOFX.java index c99b9aff..c185760a 100644 --- a/src/main/java/frc/robot/subsystems/algae/AlgaeIOFX.java +++ b/src/main/java/frc/robot/subsystems/algae/AlgaeIOFX.java @@ -2,9 +2,11 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXSConfiguration; +import com.ctre.phoenix6.configs.TalonFXSConfigurator; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.hardware.TalonFXS; import com.ctre.phoenix6.signals.ForwardLimitValue; import com.ctre.phoenix6.signals.ReverseLimitValue; import edu.wpi.first.units.measure.AngularVelocity; @@ -15,9 +17,11 @@ public class AlgaeIOFX implements AlgaeIO { private Logger logger; - private TalonFX talonFX; + private TalonFXS talonFXS; private AlgaeIOInputs inputs; + private TalonFXSConfigurator configurator; + // FX Access objects private StatusSignal curVelocity; private StatusSignal fwdLimitSwitch; @@ -27,36 +31,36 @@ public class AlgaeIOFX implements AlgaeIO { public AlgaeIOFX() { logger = LoggerFactory.getLogger(this.getClass()); - talonFX = new TalonFX(AlgaeConstants.kFxId); - fwdLimitSwitch = talonFX.getForwardLimit(); - revLimitSwitch = talonFX.getReverseLimit(); - curVelocity = talonFX.getVelocity(); + talonFXS = new TalonFXS(AlgaeConstants.kFxId); + + configurator = talonFXS.getConfigurator(); + configurator.apply(new TalonFXSConfiguration()); // Factory default motor controller + configurator.apply(AlgaeConstants.getFXConfig()); + + fwdLimitSwitch = talonFXS.getForwardLimit(); + revLimitSwitch = talonFXS.getReverseLimit(); + curVelocity = talonFXS.getVelocity(); } @Override public void updateInputs(AlgaeIOInputs inputs) { BaseStatusSignal.refreshAll(curVelocity, fwdLimitSwitch, revLimitSwitch); - inputs.velocity = curVelocity.refresh().getValue(); - inputs.isFwdLimitSwitchClosed = fwdLimitSwitch.getValue().value == 1; // FIXME check right value - inputs.isRevLimitSwitchClosed = revLimitSwitch.getValue().value == 0; // FIXME check right value + inputs.velocity = curVelocity.getValueAsDouble(); + inputs.isBeamBroken = fwdLimitSwitch.getValue().value == 0; } @Override - public void setSpeed(AngularVelocity speed) { - talonFX.setControl(speedRequest.withVelocity(speed)); + public void setSpeed(double speed) { + talonFXS.setControl(speedRequest.withVelocity(speed)); } @Override public void setPct(double pct) { - talonFX.setControl(dutyCycleRequest.withOutput(pct)); - } - - public AngularVelocity AngularVelocity() { - return talonFX.getVelocity().getValue(); + talonFXS.setControl(dutyCycleRequest.withOutput(pct)); } @Override public void registerWith(TelemetryService telemetryService) { - telemetryService.register(talonFX, true); + telemetryService.register(talonFXS, true); } } diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java b/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java index 58d8f65a..4628be7a 100644 --- a/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java @@ -1,26 +1,23 @@ package frc.robot.subsystems.algae; -import static edu.wpi.first.units.Units.RotationsPerSecond; - -import edu.wpi.first.units.measure.AngularVelocity; import frc.robot.constants.AlgaeConstants; -import frc.robot.standards.ClosedLoopSpeedSubsystem; -import frc.robot.subsystems.algae.AlgaeIO.AlgaeIOInputs; import java.util.Set; +import net.jafama.FastMath; import org.littletonrobotics.junction.Logger; import org.slf4j.LoggerFactory; import org.strykeforce.telemetry.TelemetryService; import org.strykeforce.telemetry.measurable.MeasurableSubsystem; import org.strykeforce.telemetry.measurable.Measure; -public class AlgaeSubsystem extends MeasurableSubsystem implements ClosedLoopSpeedSubsystem { +public class AlgaeSubsystem extends MeasurableSubsystem { private org.slf4j.Logger logger = LoggerFactory.getLogger(AlgaeSubsystem.class); private final AlgaeIO io; - private final AlgaeIOInputs inputs = new AlgaeIOInputs(); - private AngularVelocity desiredSpeed = RotationsPerSecond.of(0); + private final AlgaeIOInputsAutoLogged inputs = new AlgaeIOInputsAutoLogged(); + private double desiredSpeed = 0; + private double slowCounts = 0; - private AlgaeStates curState = AlgaeStates.IDLE; + private AlgaeStates curState = AlgaeStates.EMPTY; public AlgaeSubsystem(AlgaeIO io) { this.io = io; @@ -36,59 +33,74 @@ public void setState(AlgaeStates newState) { } public void intake() { - setSpeed(AlgaeConstants.kIntakingSpeed); + // setSpeed(AlgaeConstants.kIntakingSpeed); + setPct(0.5); + slowCounts = 0; } public void scoreProcessor() { - setSpeed(AlgaeConstants.kProcessorScoreSpeed); + // setSpeed(AlgaeConstants.kProcessorScoreSpeed); + setPct(-1); } public void scoreBarge() { - setSpeed(AlgaeConstants.kBargeScoreSpeed); + // setSpeed(AlgaeConstants.kBargeScoreSpeed); + setPct(-1); } public void hold() { - setSpeed(AlgaeConstants.kHoldSpeed); + // setSpeed(AlgaeConstants.kHoldSpeed); + setPct(-0); } - @Override - public void setSpeed(AngularVelocity speed) { - io.setSpeed(speed); - desiredSpeed = speed; + public boolean hasAlgae() { + return curState == AlgaeStates.HAS_ALGAE; + } + + public void setSpeed(double speed) { + // io.setSpeed(speed); + // desiredSpeed = speed; } public void setPct(double pct) { io.setPct(pct); } - @Override - public AngularVelocity getSpeed() { + public double getSpeed() { return inputs.velocity; } - @Override public boolean atSpeed() { - return inputs.velocity.minus(desiredSpeed).abs(RotationsPerSecond) - < AlgaeConstants.kCloseEnough.in(RotationsPerSecond); + return FastMath.abs(inputs.velocity - desiredSpeed) < AlgaeConstants.kCloseEnough; } @Override public void periodic() { io.updateInputs(inputs); + Logger.processInputs(getName(), inputs); Logger.recordOutput("Algae/state", curState); - Logger.recordOutput("Algae/setpoint", desiredSpeed.in(RotationsPerSecond)); + Logger.recordOutput("Algae/setpoint", desiredSpeed); switch (curState) { case HAS_ALGAE -> { - if (!inputs.isFwdLimitSwitchClosed) { + if (!inputs.isBeamBroken) { setState(AlgaeStates.EMPTY); - setSpeed(RotationsPerSecond.of(0)); + // setPct(0); + // setSpeed(RotationsPerSecond.of(0)); } } case EMPTY -> { - if (inputs.isRevLimitSwitchClosed) { // FIXME: correct? - hold(); - setState(AlgaeStates.HAS_ALGAE); + if (inputs.isBeamBroken) { + if (FastMath.abs(inputs.velocity) < AlgaeConstants.kHasAlgaeVelThreshold) { + slowCounts++; + } else { + slowCounts = 0; + } + + if (slowCounts >= AlgaeConstants.kHasAlgaeCounts) { + hold(); + setState(AlgaeStates.HAS_ALGAE); + } } } case IDLE -> {} @@ -111,9 +123,4 @@ public enum AlgaeStates { EMPTY, IDLE } - - public boolean hasAlgae() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'hasAlgae'"); - } } diff --git a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIO.java b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIO.java index 02d8743e..18c38d3b 100644 --- a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIO.java +++ b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIO.java @@ -1,10 +1,6 @@ package frc.robot.subsystems.biscuit; -import static edu.wpi.first.units.Units.Rotations; -import static edu.wpi.first.units.Units.RotationsPerSecond; - import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; import org.littletonrobotics.junction.AutoLog; import org.strykeforce.telemetry.TelemetryService; @@ -12,8 +8,8 @@ public interface BiscuitIO { @AutoLog public class BiscuitIOInputs { - public Angle position = Rotations.of(0); - public AngularVelocity velocity = RotationsPerSecond.of(0); + public double position = 0.0; + public double velocity = 0.0; public boolean fwdLimitSwitchOpen = false; public boolean didZero; } diff --git a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java index 1d456f50..c8acc2dd 100644 --- a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java +++ b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java @@ -7,6 +7,7 @@ import com.ctre.phoenix6.controls.MotionMagicDutyCycle; import com.ctre.phoenix6.hardware.TalonFXS; import com.ctre.phoenix6.signals.ForwardLimitTypeValue; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.Alert; @@ -26,6 +27,8 @@ public class BiscuitIOFX implements BiscuitIO { private StatusSignal position; private StatusSignal velocity; private StatusSignal fwdLimitSwitch; + private StatusSignal rawQuadrature; + private StatusSignal rawPulseWidth; private boolean didZero; private boolean fwdLimitSwitchOpen; private Angle offset; @@ -51,6 +54,11 @@ public BiscuitIOFX() { // Set our variables velocity = talon.getVelocity(); position = talon.getPosition(); + rawQuadrature = talon.getRawQuadraturePosition(); + rawQuadrature.setUpdateFrequency(20); + rawPulseWidth = talon.getRawPulseWidthPosition(); + rawPulseWidth.setUpdateFrequency(20); + zero(); } @Override @@ -60,11 +68,10 @@ public void setPosition(Angle position) { @Override public void updateInputs(BiscuitIOInputs inputs) { - BaseStatusSignal.refreshAll(velocity, position, fwdLimitSwitch); - inputs.velocity = velocity.getValue(); - inputs.position = position.getValue(); - inputs.fwdLimitSwitchOpen = fwdLimitSwitch.getValueAsDouble() == 1; + inputs.velocity = velocity.getValueAsDouble(); + inputs.position = position.getValueAsDouble(); inputs.didZero = didZero; + BaseStatusSignal.refreshAll(velocity, position); } @Override @@ -75,14 +82,10 @@ public void registerWith(TelemetryService telemetry) { @Override public void zero() { didZero = false; - if (fwdLimitSwitchOpen == true) { - Angle pos = position.getValue(); - offset = BiscuitConstants.kZero.minus(pos); - didZero = true; - } else { - rangeAlert.set(true); - logger.error("Biscuit overextended! Shutting down movement!"); - configurator.apply(BiscuitConstants.disableTalon()); - } + double pos = MathUtil.inputModulus(rawPulseWidth.getValueAsDouble(), 0, 1); + double setPos = BiscuitConstants.kTicksPerRot * (BiscuitConstants.kZero - pos); + talon.setPosition(setPos); + logger.info("set Biscuit position to " + setPos); + didZero = true; } } diff --git a/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java b/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java index 16121b23..2ef3ea40 100644 --- a/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java +++ b/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java @@ -3,6 +3,7 @@ package frc.robot.subsystems.biscuit; import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; @@ -32,11 +33,11 @@ public void setPosition(Angle position) { @Override public Angle getPosition() { - return inputs.position; + return Rotations.of(inputs.position); } public AngularVelocity getVelocity(AngularVelocity velocity) { - return inputs.velocity; + return RotationsPerSecond.of(inputs.velocity); } @Override @@ -46,15 +47,20 @@ public void zero() { @Override public boolean isFinished() { - return setPoint.minus(inputs.position).abs(Rotations) < BiscuitConstants.kCloseEnough; + return Math.abs(getPosition().minus(setPoint).in(Rotations)) < BiscuitConstants.kCloseEnough; + } + + public boolean isSafeToStow() { + return getPosition().in(Rotations) < BiscuitConstants.kSafeToStowUpper + && getPosition().in(Rotations) > BiscuitConstants.kSafeToStowLower; } @Override public void periodic() { - // io.updateInputs(inputs); + io.updateInputs(inputs); Logger.processInputs(getName(), inputs); - Logger.recordOutput("Biscuit setPoint", setPoint); - Logger.recordOutput("Is Biscuit Finished", isFinished() ? 1.0 : 0.0); + Logger.recordOutput("Biscuit setPoint", setPoint.in(Rotations)); + Logger.recordOutput("Is Biscuit Finished", isFinished()); } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index da8d9f67..7128d394 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -2,6 +2,7 @@ import choreo.trajectory.SwerveSample; import choreo.trajectory.Trajectory; +import edu.wpi.first.math.Matrix; import edu.wpi.first.math.controller.HolonomicDriveController; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; @@ -9,8 +10,11 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.RobotController; import frc.robot.constants.DriveConstants; import frc.robot.subsystems.robotState.RobotStateSubsystem; import java.util.Set; @@ -40,6 +44,8 @@ public class DriveSubsystem extends MeasurableSubsystem { private Trajectory autoTrajectory; private double trajectoryActive = 0.0; + private double driveMultiplier = 1.0; + private int gyroDifferentCount = 0; private RobotStateSubsystem robotStateSubsystem; @@ -77,7 +83,7 @@ public DriveSubsystem(SwerveIO io) { // Open-Loop Swerve Movements public void drive(double vXmps, double vYmps, double vOmegaRadps) { - io.drive(vXmps, vYmps, vOmegaRadps, true); + io.drive(vXmps * driveMultiplier, vYmps * driveMultiplier, vOmegaRadps, true); } public void setAzimuthVel(double vel) { @@ -164,6 +170,29 @@ public Trajectory getAutoTrajectory() { } } + public void addVisionMeasurement(Pose2d pose, double timestamp) { + org.littletonrobotics.junction.Logger.recordOutput("DriveSubsystem/Pose from vision", pose); + org.littletonrobotics.junction.Logger.recordOutput( + "DriveSubsystem/Time Since Vision Update", + RobotController.getFPGATime() / 1_000_000 - timestamp); + io.addVisionMeasurement(pose, timestamp); + } + + public void addVisionMeasurement(Pose2d pose, double timestamp, Matrix stdDevvs) { + org.littletonrobotics.junction.Logger.recordOutput("DriveSubsystem/Pose from vision", pose); + org.littletonrobotics.junction.Logger.recordOutput( + "DriveSubsystem/stdDevvs", stdDevvs.get(0, 0)); + org.littletonrobotics.junction.Logger.recordOutput( + "DriveSubsystem/Time Since Vision Update", + RobotController.getFPGATime() / 1_000_000.0 - timestamp); + + org.littletonrobotics.junction.Logger.recordOutput( + "DriveSubsystem/FPGA Seconds", RobotController.getFPGATime() / 1_000_000.0); + org.littletonrobotics.junction.Logger.recordOutput("DriveSubsystem/Result Seconds", timestamp); + + io.addVisionMeasurement(pose, timestamp, stdDevvs); + } + public void resetHolonomicController(double yaw) { xController.reset(); yController.reset(); @@ -319,6 +348,18 @@ public PIDController getomegaControllerNonProfiled() { omegaController.getP(), omegaController.getI(), omegaController.getD()); } + public void setDriveMultiplier(double multiplier) { + driveMultiplier = multiplier; + } + + public double getDriveMultiplier() { + return driveMultiplier; + } + + public void removeDriveMultiplier() { + driveMultiplier = 1.0; + } + @Override public void periodic() { io.updateInputs(inputs); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 0e924d06..f09ed333 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -48,6 +48,11 @@ public boolean isFinished() { && currState != ElevatorStates.ZEROING; } + public boolean isHigherThan(Angle higherThanPos) { + return getPosition().in(Rotations) > higherThanPos.in(Rotations) + && currState != ElevatorStates.ZEROING; + } + @Override public void zero() { currState = ElevatorStates.ZEROING; diff --git a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java index 9d2f6de4..c60ad888 100644 --- a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java +++ b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java @@ -90,6 +90,7 @@ public void setStartNode(Character startNode) { public void startPathHandler() { nodeNames.add(0, startNode); isHandling = true; + robotStateSubsystem.setAutoPlaceOnCycle(false); curState = PathStates.DRIVE_FETCH; } @@ -189,6 +190,7 @@ public void killPathHandler() { isHandling = false; curState = PathStates.DONE; runningPath = false; + robotStateSubsystem.setAutoPlaceOnCycle(true); timer.stop(); timer.reset(); } diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index ab663ad0..a34b19a9 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.robotState; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import frc.robot.constants.BiscuitConstants; @@ -45,21 +46,25 @@ public class RobotStateSubsystem extends MeasurableSubsystem { private RobotStates curState = RobotStates.IDLE; private RobotStates nextState; private RobotStates futureState; + private boolean isAuto = false; private ScoringLevel scoringLevel = ScoringLevel.L4; private ScoringLevel currentLevel = ScoringLevel.L4; - private ScoreSide scoreSide; - private AlgaeHeight algaeHeight; - private CoralLoc coralLoc; + private ScoreSide scoreSide = ScoreSide.LEFT; + private AlgaeHeight algaeHeight = AlgaeHeight.LOW; + private AlgaeHeight currentAlgaeHeight = AlgaeHeight.LOW; + private CoralLoc coralLoc = CoralLoc.CORAL; private boolean isAutoPlacing = false; private boolean getAlgaeOnCycle = false; private boolean isCurrentLimiting = false; - private boolean isAuto = true; + private boolean autoPlaceOnCycle = false; private Timer scoringTimer = new Timer(); + private boolean isEjectingAlgae = false; private Pose2d algaeRemovalPose; + public boolean isBargeSafe = true; public RobotStateSubsystem( AlgaeSubsystem algaeSubsystem, @@ -108,7 +113,7 @@ public Alliance getAllianceColor() { } public ScoringLevel getAlgaeLevel() { - return tagAlignSubsystem.computeHexant(allianceColor) % 2 == 0 + return (tagAlignSubsystem.computeHexant(allianceColor) % 2) == 0 ? ScoringLevel.L3 : ScoringLevel.L2; } @@ -133,8 +138,8 @@ private void setState(RobotStates robotState, boolean transfer) { curState = RobotStates.TRANSFER; } else { logger.info("{} -> {}", this.curState, robotState); + curState = nextState = robotState; } - curState = nextState = robotState; } } @@ -154,7 +159,11 @@ public void setAlgaeHeight(AlgaeHeight algaeHeight) { this.algaeHeight = algaeHeight; } - public void setAutoPlacing(boolean isAutoPlacing) { + public void toggleAlgaeHeight() { + algaeHeight = algaeHeight == AlgaeHeight.LOW ? AlgaeHeight.HIGH : AlgaeHeight.LOW; + } + + public void setIsAutoPlacing(boolean isAutoPlacing) { this.isAutoPlacing = isAutoPlacing; } @@ -162,12 +171,24 @@ public void setGetAlgaeOnCycle(boolean getAlgaeOnCycle) { this.getAlgaeOnCycle = getAlgaeOnCycle; } + public boolean getGetAlgaeOnCycle() { + return getAlgaeOnCycle; + } + + public void ToggleGetAlgaeOnCycle() { + getAlgaeOnCycle = !getAlgaeOnCycle; + } + public void setCurrentLimiting(boolean isCurrentLimiting) { this.isCurrentLimiting = isCurrentLimiting; } - public void setIsAuto(boolean isAuto) { - this.isAuto = isAuto; + public void setAutoPlaceOnCycle(boolean isAuto) { + this.autoPlaceOnCycle = isAuto; + } + + public boolean getAutoPlaceOnCycle() { + return autoPlaceOnCycle; } public void setAutoPlacingLed(boolean isAutoPlacing) { @@ -175,57 +196,72 @@ public void setAutoPlacingLed(boolean isAutoPlacing) { } private boolean needSafeAlgaeTransfer(RobotStates nextState) { - // if (algaeSubsystem.hasAlgae()) { - // switch (curState) { - // case FLOOR_ALGAE, MIC_ALGAE, PROCESSOR_ALGAE, BARGE_ALGAE, HP_ALGAE, INTERRUPTED -> { - // switch (nextState) { - // // These are all the states that could possibly be entered that are also possibly - // // dangerous - // // Each futureState must be handled in STOW - // case REEF_ALIGN_ALGAE, REEF_ALIGN_CORAL, PREP_CLIMB -> { - // futureState = nextState; - // toStow(); - // return true; - // } - // default -> {} - // } - // } - // default -> {} - // } - - // switch (curState) { - // case REEF_ALIGN_ALGAE, - // REEF_ALIGN_CORAL, - // REMOVE_ALGAE, - // SAFE_REMOVE_ALGAE_ABOVE, - // SAFE_REMOVE_ALGAE_ROTATE, - // PLACE_CORAL, - // INTERRUPTED -> { - // switch (nextState) { - // // These are all the states that could possibly be entered that are also - // // dangerous - // // Each futureState must be handled in STOW - // case FLOOR_ALGAE, MIC_ALGAE, PROCESSOR_ALGAE, BARGE_ALGAE, HP_ALGAE, PREP_CLIMB -> { - // futureState = nextState; - // toStow(); - // return true; - // } - // default -> {} - // } - // } - // default -> {} - // } - // } + if (algaeSubsystem.hasAlgae()) { + switch (curState) { + case FLOOR_ALGAE, MIC_ALGAE, PROCESSOR_ALGAE, BARGE_ALGAE, HP_ALGAE, INTERRUPTED -> { + switch (nextState) { + // These are all the states that could possibly be entered that are also possibly + // dangerous + // Each futureState must be handled in STOW + case REEF_ALIGN_ALGAE, REEF_ALIGN_CORAL, PREP_CLIMB -> { + futureState = nextState; + toStow(); + return true; + } + default -> {} + } + } + default -> {} + } + + switch (curState) { + case REEF_ALIGN_ALGAE, REEF_ALIGN_CORAL, REMOVE_ALGAE, PLACE_CORAL, INTERRUPTED -> { + switch (nextState) { + // These are all the states that could possibly be entered that are also + // dangerous + // Each futureState must be handled in STOW + case FLOOR_ALGAE, MIC_ALGAE, PROCESSOR_ALGAE, BARGE_ALGAE, HP_ALGAE, PREP_CLIMB -> { + futureState = nextState; + toStow(); + return true; + } + default -> {} + } + } + default -> {} + } + } return false; } public void toStow() { - // biscuitSubsystem.setPosition(BiscuitConstants.kStowSetpoint); - elevatorSubsystem.setPosition(ElevatorConstants.kStowSetpoint); - // algaeSubsystem.hold(); + if (biscuitSubsystem.isSafeToStow()) { + biscuitSubsystem.setPosition(BiscuitConstants.kStowSetpoint); + elevatorSubsystem.setPosition(ElevatorConstants.kStowSetpoint); + driveSubsystem.removeDriveMultiplier(); + algaeSubsystem.hold(); - setState(RobotStates.TO_STOW); + setState(RobotStates.TO_STOW); + } else { + toStowSafe(); + } + } + + public void toStowSafe() { + biscuitSubsystem.setPosition(BiscuitConstants.kStowSetpoint); + driveSubsystem.removeDriveMultiplier(); + algaeSubsystem.hold(); + + setState(RobotStates.TO_STOW_SAFE); + } + + public void toStowSequential() { + biscuitSubsystem.setPosition(BiscuitConstants.kStowSetpoint); + driveSubsystem.removeDriveMultiplier(); + algaeSubsystem.hold(); + + setState(RobotStates.TO_STOW_SEQUENTIAL); } public void toPrepCoral() { @@ -233,15 +269,13 @@ public void toPrepCoral() { if (elevatorSubsystem.isFinished()) { toPlaceCoral(); } - } else if (isAuto) { - toReefAlign(false, false); } else { - toReefAlign(getAlgaeOnCycle, false); // Manual scoring + toReefAlign(getAlgaeOnCycle, autoPlaceOnCycle); } } private void toFunnelLoad() { - // biscuitSubsystem.setPosition(BiscuitConstants.kFunnelSetpoint); + biscuitSubsystem.setPosition(BiscuitConstants.kFunnelSetpoint); coralSubsystem.intake(); elevatorSubsystem.setPosition(ElevatorConstants.kFunnelSetpoint); @@ -253,7 +287,9 @@ public void toReefAlign() { } private void toReefAlign(boolean getAlgae, boolean drive) { - if (getAlgae) { + if ((getAlgae || !coralSubsystem.hasCoral()) + && (scoreSide == ScoreSide.LEFT || !autoPlaceOnCycle) + && !algaeSubsystem.hasAlgae()) { if (needSafeAlgaeTransfer(RobotStates.REEF_ALIGN_ALGAE)) { return; } @@ -273,33 +309,28 @@ private void toReefAlign(boolean getAlgae, boolean drive) { default -> logger.error("Invalid algae level: {}", getAlgaeLevel()); } } else { - if (!coralSubsystem.hasCoral()) { - toStow(); + if (needSafeAlgaeTransfer(RobotStates.REEF_ALIGN_CORAL)) { return; - } else { - if (needSafeAlgaeTransfer(RobotStates.REEF_ALIGN_CORAL)) { - return; - } - setState(RobotStates.REEF_ALIGN_CORAL); + } + setState(RobotStates.REEF_ALIGN_CORAL); - currentLevel = scoringLevel; - switch (scoringLevel) { - case L1 -> { - // biscuitSubsystem.setPosition(BiscuitConstants.kL1CoralSetpoint); - elevatorSubsystem.setPosition(ElevatorConstants.kL1CoralSetpoint); - } - case L2 -> { - // biscuitSubsystem.setPosition(BiscuitConstants.kL2CoralSetpoint); - elevatorSubsystem.setPosition(ElevatorConstants.kL2CoralSetpoint); - } - case L3 -> { - // biscuitSubsystem.setPosition(BiscuitConstants.kL3CoralSetpoint); - elevatorSubsystem.setPosition(ElevatorConstants.kL3CoralSetpoint); - } - case L4 -> { - // biscuitSubsystem.setPosition(BiscuitConstants.kL4CoralSetpoint); - elevatorSubsystem.setPosition(ElevatorConstants.kL4CoralSetpoint); - } + currentLevel = scoringLevel; + switch (scoringLevel) { + case L1 -> { + biscuitSubsystem.setPosition(BiscuitConstants.kL1CoralSetpoint); + elevatorSubsystem.setPosition(ElevatorConstants.kL1CoralSetpoint); + } + case L2 -> { + biscuitSubsystem.setPosition(BiscuitConstants.kL2CoralSetpoint); + elevatorSubsystem.setPosition(ElevatorConstants.kL2CoralSetpoint); + } + case L3 -> { + biscuitSubsystem.setPosition(BiscuitConstants.kL3CoralSetpoint); + elevatorSubsystem.setPosition(ElevatorConstants.kL3CoralSetpoint); + } + case L4 -> { + biscuitSubsystem.setPosition(BiscuitConstants.kL4CoralSetpoint); + elevatorSubsystem.setPosition(ElevatorConstants.kL4CoralSetpoint); } } } @@ -320,6 +351,7 @@ public void toPlaceCoral() { public void toAlgaeFloorPickup() { algaeSubsystem.intake(); + currentAlgaeHeight = algaeHeight; switch (algaeHeight) { case LOW -> { @@ -346,6 +378,14 @@ public void toAlgaeFloorPickup() { } public void toScoreAlgae() { + currentAlgaeHeight = algaeHeight; + if (curState == RobotStates.BARGE_ALGAE && algaeHeight == AlgaeHeight.LOW) { + futureState = RobotStates.PROCESSOR_ALGAE; + toStowSafe(); + } else if (curState == RobotStates.PROCESSOR_ALGAE && algaeHeight == AlgaeHeight.HIGH) { + futureState = RobotStates.BARGE_ALGAE; + toStowSafe(); + } switch (algaeHeight) { case LOW -> { toProcessor(); @@ -355,20 +395,28 @@ public void toScoreAlgae() { return; } + driveSubsystem.setDriveMultiplier(DriveConstants.kBargeScoreStickMultiplier); + double poseX = driveSubsystem.getPoseMeters().getX(); - if (poseX > RobotStateConstants.kRedBargeSafeX - || poseX < RobotStateConstants.kBlueBargeSafeX) { - biscuitSubsystem.setPosition(BiscuitConstants.kBargeSetpoint); + isBargeSafe = + poseX > RobotStateConstants.kRedBargeSafeX + || poseX < RobotStateConstants.kBlueBargeSafeX; + + if (isBargeSafe) { elevatorSubsystem.setPosition(ElevatorConstants.kBargeSetpoint); - setState(RobotStates.BARGE_ALGAE, true); + setState(RobotStates.TO_BARGE_ALGAE); } } } } public void releaseAlgae() { + scoringTimer.stop(); + scoringTimer.reset(); + scoringTimer.start(); + isEjectingAlgae = true; switch (algaeHeight) { case LOW -> { algaeSubsystem.scoreProcessor(); @@ -382,6 +430,7 @@ public void releaseAlgae() { } public void toHpAlgae() { + currentAlgaeHeight = algaeHeight; if (needSafeAlgaeTransfer(RobotStates.HP_ALGAE)) { return; } @@ -395,6 +444,7 @@ public void toHpAlgae() { } private void toProcessor() { + currentAlgaeHeight = algaeHeight; if (needSafeAlgaeTransfer(RobotStates.PROCESSOR_ALGAE)) { return; } @@ -444,12 +494,13 @@ public void periodic() { Logger.recordOutput("RobotState/state", curState); Logger.recordOutput("RobotState/hasCoral", hasCoral()); Logger.recordOutput("RobotState/scoringLevel", scoringLevel); + Logger.recordOutput("RobotState/algeaHeight", algaeHeight); + Logger.recordOutput("RobotState/algeaLevelSideBased", getAlgaeLevel()); + Logger.recordOutput("RobotState/getAlgea", getAlgaeOnCycle); switch (curState) { case TRANSFER -> { - if ( - // biscuitSubsystem.isFinished() && - elevatorSubsystem.isFinished() + if (biscuitSubsystem.isFinished() && elevatorSubsystem.isFinished() // && climbSubsystem.isFinished() ) { setState(nextState); @@ -457,17 +508,30 @@ public void periodic() { } case TO_STOW -> { - if ( - // biscuitSubsystem.isFinished() && - elevatorSubsystem.isFinished()) { + if (biscuitSubsystem.isFinished() && elevatorSubsystem.isFinished()) { setState(RobotStates.STOW); } } + + case TO_STOW_SAFE -> { + if (biscuitSubsystem.isSafeToStow()) { + elevatorSubsystem.setPosition(ElevatorConstants.kStowSetpoint); + setState(RobotStates.TO_STOW); + } + } + + case TO_STOW_SEQUENTIAL -> { + if (biscuitSubsystem.isFinished()) { + elevatorSubsystem.setPosition(ElevatorConstants.kStowSetpoint); + setState(RobotStates.TO_STOW); + } + } + case STOW -> { if (futureState != null) { switch (futureState) { - case REEF_ALIGN_ALGAE -> toReefAlign(true, isAutoPlacing); - case REEF_ALIGN_CORAL -> toReefAlign(false, isAutoPlacing); + case REEF_ALIGN_ALGAE -> toReefAlign(true, autoPlaceOnCycle); + case REEF_ALIGN_CORAL -> toReefAlign(false, autoPlaceOnCycle); case HP_ALGAE -> toHpAlgae(); case PROCESSOR_ALGAE, BARGE_ALGAE -> toScoreAlgae(); case MIC_ALGAE, FLOOR_ALGAE -> toAlgaeFloorPickup(); @@ -489,23 +553,13 @@ public void periodic() { // } // } case REEF_ALIGN_ALGAE -> { - if (!isAutoPlacing + if (!autoPlaceOnCycle || tagAlignSubsystem.getState() == TagAlignSubsystem.TagAlignStates.DONE) { if (algaeSubsystem.hasAlgae()) { - algaeRemovalPose = driveSubsystem.getPoseMeters(); - switch (getAlgaeLevel()) { case L2 -> { biscuitSubsystem.setPosition(BiscuitConstants.kL2AlgaeRemovalSetpoint); elevatorSubsystem.setPosition(ElevatorConstants.kL2AlgaeRemovalSetpoint); - - // FIXME: if driving to remove algae - switch (scoringLevel) { - case L1, L2 -> { - driveSubsystem.move(DriveConstants.kAlgaeRemovalSpeed, 0, 0, false); - } - default -> {} - } } case L3 -> { biscuitSubsystem.setPosition(BiscuitConstants.kL3AlgaeRemovalSetpoint); @@ -519,50 +573,24 @@ public void periodic() { } } case REEF_ALIGN_CORAL -> { - if (tagAlignSubsystem.getState() == TagAlignSubsystem.TagAlignStates.DONE) { - if (isAutoPlacing) { - toPlaceCoral(); - } else if (currentLevel != scoringLevel) { - toReefAlign(getAlgaeOnCycle, false); - } + if (currentLevel != scoringLevel) { + toReefAlign(false, false); + } + if (autoPlaceOnCycle + && tagAlignSubsystem.getState() == TagAlignSubsystem.TagAlignStates.DONE) { + toPlaceCoral(); } } case REMOVE_ALGAE -> { - if (getAlgaeLevel() == ScoringLevel.L2) { - switch (scoringLevel) { - case L1, L2 -> { - // FIXME: if driving to remove algae - if (driveSubsystem.getPoseMeters().minus(algaeRemovalPose).getTranslation().getNorm() - > RobotStateConstants.kAlgaeRetreatDistance) { - driveSubsystem.move(0, 0, 0, false); - toReefAlign(false, true); - } - // FIXME: if not driving to remove algae - biscuitSubsystem.setPosition(BiscuitConstants.kSafeAlgaeRemovalSetpoint); - elevatorSubsystem.setPosition(ElevatorConstants.kSafeAlgaeRemovalSetpoint); - setState(RobotStates.SAFE_REMOVE_ALGAE_ABOVE, true); - } - case L3, L4 -> { - if (biscuitSubsystem.isFinished() && elevatorSubsystem.isFinished()) { - toReefAlign(false, false); - } - } + if (biscuitSubsystem.isFinished() && elevatorSubsystem.isFinished()) { + if (coralSubsystem.hasCoral()) { + toReefAlign(false, false); + } else { + toStowSequential(); } - } else if (biscuitSubsystem.isFinished() && elevatorSubsystem.isFinished()) { - toReefAlign(false, false); } } - case SAFE_REMOVE_ALGAE_ABOVE -> { - biscuitSubsystem.setPosition(BiscuitConstants.kSafeAlgaeRemovalRotateSetpoint); - elevatorSubsystem.setPosition(ElevatorConstants.kSafeAlgaeRemovalRotateSetpoint); - setState(RobotStates.SAFE_REMOVE_ALGAE_ROTATE, true); - } - - case SAFE_REMOVE_ALGAE_ROTATE -> { - toReefAlign(false, false); - } - case PLACE_CORAL -> { if (!coralSubsystem.hasCoral() && scoringTimer.hasElapsed(RobotStateConstants.kCoralEjectTimer)) { @@ -583,7 +611,7 @@ public void periodic() { case LOADING_CORAL -> { if (coralSubsystem.hasCoral()) { coralLoc = CoralLoc.CORAL; - // biscuitSubsystem.setPosition(BiscuitConstants.kPrestageSetpoint); + biscuitSubsystem.setPosition(BiscuitConstants.kPrestageSetpoint); elevatorSubsystem.setPosition(ElevatorConstants.kPrestageSetpoint); funnelSubsystem.stopMotor(); @@ -597,23 +625,57 @@ public void periodic() { } } case PROCESSOR_ALGAE -> { - if (!algaeSubsystem.hasAlgae()) { - toStow(); + if (!algaeSubsystem.hasAlgae() + && scoringTimer.hasElapsed(RobotStateConstants.kCoralEjectTimer) + && isEjectingAlgae) { + isEjectingAlgae = false; + toStowSafe(); + } else if (algaeHeight != currentAlgaeHeight) { + toScoreAlgae(); + } + } + case TO_BARGE_ALGAE -> { + if (elevatorSubsystem.isHigherThan(ElevatorConstants.kBargeHigherThan)) { + Angle BiscuitSetpoint = ElevatorConstants.kBargeSetpoint; + if (driveSubsystem.getPoseMeters().getX() <= DriveConstants.kCenterLineX) { + double yaw = driveSubsystem.getPoseMeters().getRotation().getDegrees(); + BiscuitSetpoint = + yaw < 90 && yaw > -90 + ? BiscuitConstants.kBargeSetpoint + : BiscuitConstants.kBargeBackwardSetpoint; + } else if (driveSubsystem.getPoseMeters().getX() > DriveConstants.kCenterLineX) { + double yaw = driveSubsystem.getPoseMeters().getRotation().getDegrees(); + BiscuitSetpoint = + yaw < -90 || yaw > 90 + ? BiscuitConstants.kBargeSetpoint + : BiscuitConstants.kBargeBackwardSetpoint; + } + biscuitSubsystem.setPosition(BiscuitSetpoint); + curState = RobotStates.BARGE_ALGAE; } } case BARGE_ALGAE -> { - if (!algaeSubsystem.hasAlgae()) { - toStow(); + if (!algaeSubsystem.hasAlgae() + && scoringTimer.hasElapsed(RobotStateConstants.kCoralEjectTimer) + && isEjectingAlgae) { + isEjectingAlgae = false; + toStowSafe(); + } else if (algaeHeight != currentAlgaeHeight) { + toScoreAlgae(); } } case MIC_ALGAE -> { if (algaeSubsystem.hasAlgae()) { - toStow(); + toStowSafe(); + } else if (algaeHeight != currentAlgaeHeight) { + toAlgaeFloorPickup(); } } case FLOOR_ALGAE -> { if (algaeSubsystem.hasAlgae()) { - toStow(); + toStowSafe(); + } else if (algaeHeight != currentAlgaeHeight) { + toAlgaeFloorPickup(); } } case PREP_CLIMB -> {} @@ -637,18 +699,19 @@ public void registerWith(TelemetryService telemetryService) { public enum RobotStates { STOW, TO_STOW, + TO_STOW_SAFE, + TO_STOW_SEQUENTIAL, PREP_CORAL, REEF_ALIGN_ALGAE, REEF_ALIGN_CORAL, REMOVE_ALGAE, - SAFE_REMOVE_ALGAE_ABOVE, - SAFE_REMOVE_ALGAE_ROTATE, PLACE_CORAL, FUNNEL_LOAD, LOADING_CORAL, PRESTAGE, HP_ALGAE, PROCESSOR_ALGAE, + TO_BARGE_ALGAE, BARGE_ALGAE, MIC_ALGAE, FLOOR_ALGAE, diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index 703e2a9f..13ab380e 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java @@ -5,10 +5,12 @@ import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.RobotController; import frc.robot.constants.TagServoingConstants; import frc.robot.subsystems.drive.DriveSubsystem; import frc.robot.subsystems.vision.VisionSubsystem; @@ -37,26 +39,31 @@ public class TagAlignSubsystem extends MeasurableSubsystem { private int targetCamId; private int targetTagId; private int fieldRelHexant; - private Alliance alliance; + private Alliance alliance = Alliance.Blue; + private double goalTargetDiag; + + private long startServoTime; public TagAlignSubsystem(DriveSubsystem driveSubsystem, VisionSubsystem visionSubsystem) { this.driveSubsystem = driveSubsystem; this.visionSubsystem = visionSubsystem; // FIXME: need sane constants - this.driveX = new ProfiledPIDController(0.0019, 0, 0, new Constraints(3.0, 3.0)); - this.driveY = new ProfiledPIDController(0.00001, 0, 0, new Constraints(2.0, 1.0)); + this.driveX = new ProfiledPIDController(3, 0, 0, new Constraints(1, 1.0)); + this.driveY = new ProfiledPIDController(5, 0, 0, new Constraints(2, 3)); this.driveOmega = new ProfiledPIDController(5.0, 0, 0, new Constraints(1.0, 1.0)); this.driveOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180)); - this.alignX = new ProfiledPIDController(0.0019, 0, 0, new Constraints(3.0, 3.0)); - this.alignY = new ProfiledPIDController(0.00001, 0, 0, new Constraints(2.0, 1.0)); - this.alignOmega = new ProfiledPIDController(5.0, 0, 0, new Constraints(1.0, 1.0)); + this.alignX = new ProfiledPIDController(0.0017, 0, 0, new Constraints(1.0, 1.0)); // 0.0015 + this.alignY = new ProfiledPIDController(0.0017, 0, 0, new Constraints(1.0, 1.0)); + this.alignOmega = new ProfiledPIDController(5.5, 0, 0, new Constraints(1.0, 1.0)); this.alignOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180)); - Logger.recordOutput("TagAlignSubsystem/TargetArea", -1); + Logger.recordOutput("TagAlignSubsystem/TargetDiag", -1); Logger.recordOutput("TagAlignSubsystem/TargetTag", -1); Logger.recordOutput("TagAlignSubsystem/TargetCenterX", -1); + Logger.recordOutput("TagAlignSubsystem/Hexant", -1); + Logger.recordOutput("TagAlignSubsystem/GoalTargetDiag", -1); } public int computeHexant(Alliance color) { @@ -66,19 +73,23 @@ public int computeHexant(Alliance color) { : TagServoingConstants.kRedReefPose; double offset = Units.degreesToRadians(30); - return (((int) - (FastMath.normalizeZeroTwoPi( + int hexant = + (((int) + (FastMath.normalizeZeroTwoPi( driveSubsystem .getPoseMeters() .getTranslation() .minus(reefT) .getAngle() .getRadians() - - offset) - / Units.degreesToRadians(60) - + offset)) - + (color == Alliance.Blue ? 0 : 3)) - % 6; + + offset) + / Units.degreesToRadians(60))) + + (color == Alliance.Blue ? 3 : 0)) + % 6; + + Logger.recordOutput("TagAlignSubsystem/Hexant", hexant); + + return hexant; } // Red reef numbered like blue (red 0 is facing the same direction as blue 0) @@ -86,7 +97,7 @@ private int computeFieldRelHexant(Alliance color) { return (computeHexant(color) + (color == Alliance.Blue ? 0 : 3)) % 6; } - private Pose2d getTargetDrivePose(Alliance color) { + private Pose2d getTargetDrivePose(Alliance color, boolean scoreLeft) { Translation2d reefT = color == Alliance.Blue ? TagServoingConstants.kBlueReefPose @@ -95,10 +106,16 @@ private Pose2d getTargetDrivePose(Alliance color) { Translation2d offset = new Translation2d( TagServoingConstants.kInitialDriveRadius, - Rotation2d.fromDegrees(computeFieldRelHexant(color) * 60)); + Rotation2d.fromDegrees(computeFieldRelHexant(color) * 60 + 180)); + + Translation2d sideOffset = + new Translation2d( + scoreLeft ? TagServoingConstants.kRightCamOffset : TagServoingConstants.kLeftCamOffset, + Rotation2d.fromDegrees(computeFieldRelHexant(color) * 60 + 180 + 90)); return new Pose2d( - reefT.plus(offset), Rotation2d.fromDegrees(computeFieldRelHexant(color) * 60)); + reefT.plus(offset).plus(sideOffset), + Rotation2d.fromDegrees(computeFieldRelHexant(color) * 60)); } private double getCurRadius(Alliance color) { @@ -106,7 +123,9 @@ private double getCurRadius(Alliance color) { color == Alliance.Blue ? TagServoingConstants.kBlueReefPose : TagServoingConstants.kRedReefPose; - return driveSubsystem.getPoseMeters().getTranslation().minus(reefT).getNorm(); + + Translation2d reefRelative = driveSubsystem.getPoseMeters().getTranslation().minus(reefT); + return FastMath.hypot(reefRelative.getX(), reefRelative.getY()); } public TagAlignStates getState() { @@ -114,8 +133,12 @@ public TagAlignStates getState() { } public void setup(Alliance alliance, boolean scoreLeft) { - targetPose = getTargetDrivePose(alliance); + targetPose = getTargetDrivePose(alliance, scoreLeft); this.alliance = alliance; + this.goalTargetDiag = + scoreLeft + ? TagServoingConstants.kRightCamDiagTarget + : TagServoingConstants.kLeftCamDiagTarget; // Inverted, scoring left coral means aligning right camera targetCamId = @@ -128,6 +151,8 @@ public void setup(Alliance alliance, boolean scoreLeft) { fieldRelHexant = computeFieldRelHexant(alliance); Logger.recordOutput("TagAlignSubsystem/TargetTag", targetTagId); + Logger.recordOutput("TagAlignSubsystem/GoalTargetDiag", goalTargetDiag); + Logger.recordOutput("TagAlignSubsystem/TargetPose", targetPose); Pose2d current = driveSubsystem.getPoseMeters(); @@ -135,8 +160,8 @@ public void setup(Alliance alliance, boolean scoreLeft) { driveY.reset(current.getY()); driveOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); - alignX.reset(current.getX()); - alignY.reset(current.getY()); + alignX.reset(0); + alignY.reset(0); alignOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); } @@ -144,6 +169,10 @@ public void setup(Alliance alliance, boolean scoreLeft) { public double calculateAlignY() { WallEyeTagResult result = visionSubsystem.getLastResult(targetCamId); + if (result == null) { + return 2767; + } + int tagIndex = -1; int[] tags = result.getTagIDs(); @@ -160,10 +189,12 @@ public double calculateAlignY() { } Point center = result.getTagCenters().get(tagIndex); + double diag = result.getTagDiags()[tagIndex]; + Logger.recordOutput("TagAlignSubsystem/TargetDiag", diag); Logger.recordOutput("TagAlignSubsystem/TargetCenterX", center.x()); - double vY = alignY.calculate(center.x(), TagServoingConstants.kHorizontalTarget); + double vY = -alignY.calculate(TagServoingConstants.kHorizontalTarget - center.x(), 0); return vY; } @@ -176,12 +207,14 @@ public void start(Alliance alliance, boolean scoreLeft) { public void terminate() { driveSubsystem.move(0, 0, 0, false); + driveSubsystem.drive(0, 0, 0); curState = TagAlignStates.DONE; } @Override public void periodic() { Logger.recordOutput("TagAlignSubsystem/State", curState.toString()); + // Logger.recordOutput("TagAlignSubsystem/Hexant", computeHexant(alliance)); switch (curState) { case DRIVE -> { @@ -190,9 +223,13 @@ public void periodic() { double vX = driveX.calculate(current.getX(), targetPose.getX()); double vY = driveY.calculate(current.getY(), targetPose.getY()); double vOmega = - driveX.calculate( + driveOmega.calculate( current.getRotation().getRadians(), targetPose.getRotation().getRadians()); + Logger.recordOutput("TagAlignSubsystem/DriveXError", driveX.getPositionError()); + Logger.recordOutput("TagAlignSubsystem/DriveYError", driveY.getPositionError()); + Logger.recordOutput("TagAlignSubsystem/DriveOmegaError", driveOmega.getPositionError()); + Translation2d tagRelVel = new Translation2d(vX, vY) .rotateBy( @@ -201,21 +238,43 @@ public void periodic() { double tagRelX = tagRelVel.getX(); double tagRelY = tagRelVel.getY(); - if (getCurRadius(alliance) < TagServoingConstants.kStopXDriveRadius) { + double radius = getCurRadius(alliance); + boolean ignoreX = radius < TagServoingConstants.kStopXDriveRadius; + + tagRelX = tagRelX < TagServoingConstants.kMinVelX ? TagServoingConstants.kMinVelX : tagRelX; + + if (radius < TagServoingConstants.kStopXDriveRadius) { tagRelX = 0; } - if (Math.abs(driveOmega.getPositionError()) < TagServoingConstants.kAngleCloseEnough - && targetPose.minus(current).getTranslation().getNorm() - < TagServoingConstants.kDriveCloseEnough) { - alignX.reset(current.getX()); - alignY.reset(current.getY()); + Translation2d tagRelError = + targetPose + .minus(current) + .getTranslation() + .rotateBy( + Rotation2d.fromRadians(-TagServoingConstants.kAngleTarget[fieldRelHexant])); + + Logger.recordOutput("TagAlignSubsystem/Tag Rel Y Error", tagRelError.getY()); + + Transform2d poseError = targetPose.minus(current); + + if (FastMath.abs(driveOmega.getPositionError()) < TagServoingConstants.kAngleCloseEnough + && (FastMath.hypot(poseError.getX(), poseError.getY()) + < TagServoingConstants.kDriveCloseEnough + || ignoreX + && FastMath.abs(tagRelError.getY()) < TagServoingConstants.kDriveCloseEnough)) { + alignX.reset(0); + alignY.reset(0); alignOmega.reset(driveSubsystem.getGyroRotation2d().getRadians()); curState = TagAlignStates.TAG_ALIGN; + startServoTime = RobotController.getFPGATime(); break; } + Logger.recordOutput("TagAlignSubsystem/Tag Rel vX", tagRelX); + Logger.recordOutput("TagAlignSubsystem/Tag Rel vY", tagRelY); + Translation2d adjusted = new Translation2d(tagRelX, tagRelY) .rotateBy( @@ -227,6 +286,10 @@ public void periodic() { case TAG_ALIGN -> { WallEyeTagResult result = visionSubsystem.getLastResult(targetCamId); + if (result == null) { + return; + } + int tagIndex = -1; int[] tags = result.getTagIDs(); @@ -242,19 +305,33 @@ public void periodic() { } Point center = result.getTagCenters().get(tagIndex); - double area = result.getTagAreas()[tagIndex]; + double diag = result.getTagDiags()[tagIndex]; - Logger.recordOutput("TagAlignSubsystem/TargetArea", area); + Logger.recordOutput("TagAlignSubsystem/TargetDiag", diag); Logger.recordOutput("TagAlignSubsystem/TargetCenterX", center.x()); - double vX = alignX.calculate(area, TagServoingConstants.kAreaTarget); - double vY = alignY.calculate(center.x(), TagServoingConstants.kHorizontalTarget); + double vX = -alignX.calculate(goalTargetDiag - diag, 0); + double vY = -alignY.calculate(TagServoingConstants.kHorizontalTarget - center.x(), 0); + + Logger.recordOutput("TagAlignSubsystem/X Error", alignX.getPositionError()); + Logger.recordOutput("TagAlignSubsystem/Y Error", alignY.getPositionError()); + Logger.recordOutput( + "TagAlignSubsystem/Last result delay", + (RobotController.getFPGATime() - result.getTimeStamp()) / 1000); + + if (RobotController.getFPGATime() - result.getTimeStamp() + > TagServoingConstants.kNoUpdateMicrosec + && RobotController.getFPGATime() - startServoTime + > TagServoingConstants.kNoUpdateMicrosec) { + terminate(); + break; + } - if (TagServoingConstants.kAreaTarget - area < TagServoingConstants.kAngleCloseEnough - || area > TagServoingConstants.kAreaTarget) { + if (FastMath.abs(goalTargetDiag - diag) < TagServoingConstants.kDiagCloseEnough + || diag > goalTargetDiag) { vX = 0; - if (Math.abs(TagServoingConstants.kHorizontalTarget - center.x()) + if (FastMath.abs(TagServoingConstants.kHorizontalTarget - center.x()) < TagServoingConstants.kHorizontalCloseEnough) { terminate(); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 48e2e5a7..bc423cd9 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -1,13 +1,478 @@ package frc.robot.subsystems.vision; -import WallEye.WallEyeTagResult; +import static edu.wpi.first.units.Units.*; -public class VisionSubsystem { - public VisionSubsystem() {} +import WallEye.*; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.numbers.*; +import edu.wpi.first.util.CircularBuffer; +import edu.wpi.first.wpilibj.RobotController; +import frc.robot.constants.VisionConstants; +import frc.robot.subsystems.drive.DriveSubsystem; +import java.io.IOException; +import java.util.ArrayList; +import java.util.Set; +import net.jafama.FastMath; +import org.littletonrobotics.junction.Logger; +import org.slf4j.LoggerFactory; +import org.strykeforce.telemetry.TelemetryService; +import org.strykeforce.telemetry.measurable.MeasurableSubsystem; +import org.strykeforce.telemetry.measurable.Measure; + +public class VisionSubsystem extends MeasurableSubsystem { + + // Array of cameras + private WallEyeCam[] cams; + + // Array of camera positions + private Translation3d[] camPositions = { + VisionConstants.kCam1Pose.getTranslation(), + VisionConstants.kCam2Pose.getTranslation(), + VisionConstants.kCam3Pose.getTranslation(), + VisionConstants.kCam4Pose.getTranslation(), + VisionConstants.kCam5Pose.getTranslation() + }; + + // Array of camera rotations + private Rotation3d[] camRotations = { + VisionConstants.kCam1Pose.getRotation(), + VisionConstants.kCam2Pose.getRotation(), + VisionConstants.kCam3Pose.getRotation(), + VisionConstants.kCam4Pose.getRotation(), + VisionConstants.kCam5Pose.getRotation() + }; + + // Array of camera names + private String[] camNames = { + VisionConstants.kCam1Name, + VisionConstants.kCam2Name, + VisionConstants.kCam3Name, + VisionConstants.kCam4Name, + VisionConstants.kCam5Name + }; + + // Array of orange pi names + private String[] piNames = { + VisionConstants.kPi1Name, + VisionConstants.kPi1Name, + VisionConstants.kPi2Name, + VisionConstants.kPi2Name, + VisionConstants.kPi3Name + }; + + // Array of camera indexs + private int[] camIndex = { + VisionConstants.kCam1Idx, + VisionConstants.kCam2Idx, + VisionConstants.kCam3Idx, + VisionConstants.kCam4Idx, + VisionConstants.kCam5Idx + }; + + private DriveSubsystem driveSubsystem; + /*Because we use two seperate loggers we can import one and then define the + other here.*/ + private org.slf4j.Logger textLogger; + private UdpSubscriber[] udpSubscriber; + private AprilTagFieldLayout field; + private boolean visionUpdating = true; + private int minTags; + private CircularBuffer gyroBuffer = + new CircularBuffer(VisionConstants.kCircularBufferSize); + private double timeSinceLastUpdate; + private int updatesToWheels; + private Matrix adaptiveMatrix; + private ArrayList> validResults = new ArrayList<>(); + private WallEyeTagResult[] lastResult = new WallEyeTagResult[VisionConstants.kNumCams]; + private Matrix adativeMatrix; + private Matrix stdMatrix; + + public VisionSubsystem(DriveSubsystem driveSubsystem) { + this.driveSubsystem = driveSubsystem; + textLogger = LoggerFactory.getLogger("Vision"); + + cams = new WallEyeCam[VisionConstants.kNumCams]; + udpSubscriber = new UdpSubscriber[VisionConstants.kNumPis]; + // We copy the matrix from our constants + adaptiveMatrix = VisionConstants.kVisionMeasurementStdDevs.copy(); + // We then copy the adaptive matrix because they will differ later + stdMatrix = adaptiveMatrix.copy(); + // I'm not sure why we put this in a try catch maybe could be removed + try { + field = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2025Reefscape.m_resourceFile); + } catch (IOException e) { + textLogger.error("BAD NEWS BEARS NO APRIL TAG LAYOUT"); + } + // Fill our camera array + for (int i = 0; i < VisionConstants.kNumCams; i++) { + cams[i] = new WallEyeCam(piNames[i], camIndex[i], -1); + } + // Initialize our udpSubscribers + udpSubscriber[0] = new UdpSubscriber(5802, cams[0], cams[1]); + udpSubscriber[1] = new UdpSubscriber(5803, cams[2], cams[3]); + udpSubscriber[2] = new UdpSubscriber(5804, cams[4]); + } + // Setter Methods + public void setVisionUpdating(boolean updating) { + this.visionUpdating = updating; + } + + public void setMinTags(int minTags) { + this.minTags = minTags; + } + // Getter Methods + public boolean isVisionUpdating() { + return visionUpdating; + } + + public boolean cameraConnected(int index) { + return cams[index].isCameraConnected(); + } + + private double getSeconds() { + return RobotController.getFPGATime() / 1_000_000; + } + + private double minTagDistance(WallEyePoseResult result) { + + Pose3d camLoc = result.getCameraPose(); + int[] ids = result.getTagIDs(); + // This number's value doesn't really matter it just needs to be large + double minDistance = 2000; + + // Go through the camera locations and finds the one closest to the tag + for (int id : ids) { + Pose3d tagPose = field.getTagPose(id).get(); + + double dist = FastMath.hypot(tagPose.getX() - camLoc.getX(), tagPose.getY() - camLoc.getY()); + + if (dist < minDistance) { + minDistance = dist; + } + } + return minDistance; + } + + private double averageTagDistance(WallEyePoseResult result) { + + Pose3d camLoc = result.getCameraPose(); + + int[] ids = result.getTagIDs(); + double totalDistance = 0.0; + + // Goes through the camera locations and gets the average distance + for (int id : ids) { + Pose3d tagPose = field.getTagPose(id).get(); + totalDistance += + FastMath.hypot(tagPose.getX() - camLoc.getX(), tagPose.getY() - camLoc.getY()); + } + return totalDistance / ids.length; + } + + // Filters + private boolean camsAgreeWithWheels(Translation3d pose, WallEyeTagResult result) { + + ChassisSpeeds vel = driveSubsystem.getFieldRelSpeed(); + Pose2d curPose = driveSubsystem.getPoseMeters(); + + Translation2d disp = (curPose.getTranslation().minus(pose.toTranslation2d())); + + double velMagnitude = + FastMath.sqrt( + FastMath.pow(vel.vxMetersPerSecond, 2) + FastMath.pow(vel.vyMetersPerSecond, 2)); + + double dispMagnitude = + FastMath.sqrt(FastMath.pow(disp.getX(), 2) + FastMath.pow(disp.getY(), 2)); + + /*This gets our displacement and compares it to who much we could + have moved.It does this by getting the velocity and plotting it on a + graph. The graph will be in the docs.*/ + return result.getNumTags() >= minTags + && dispMagnitude + <= (velMagnitude * VisionConstants.kLinearCoeffOnVelFilter + + VisionConstants.kOffsetOnVelFilter + + FastMath.pow(velMagnitude * VisionConstants.kSquaredCoeffOnVelFilter, 2)); + } + + private boolean camsWithinField(Translation3d pose, WallEyePoseResult result) { + return (result.getNumTags() >= 2 || result.getAmbiguity() < VisionConstants.kMaxAmbig) + && pose.getX() < field.getFieldLength() + && pose.getX() > 0 + && pose.getY() < field.getFieldWidth() + && pose.getY() > 0; + } + + /*Large switch case to see get the standard deviation factor based on camera, how many + tags we see and distance. An example of this graph will be in the docs.*/ + private double getStdDevFactor(double distance, int numTags, String camName) { + switch (camName) { + case VisionConstants.kCam2Name, VisionConstants.kCam4Name -> { + if (numTags == 1) + return 1 + / (VisionConstants.FOV58YUYVBaseTrust + * FastMath.pow( + VisionConstants.baseNumber, + FastMath.pow( + VisionConstants.FOV58YUYVSingleTagCoeff * distance, + VisionConstants.FOV58YUYVPowerNumber))); + + return 1 + / (VisionConstants.FOV58YUYVBaseTrust + * FastMath.pow( + VisionConstants.baseNumber, + FastMath.pow( + VisionConstants.FOV58YUYVMultiTagCoeff * distance, + VisionConstants.FOV58YUYVPowerNumber))); + } + case VisionConstants.kCam5Name -> { + if (numTags == 1) + return 1 + / (VisionConstants.FOV58YUYVBaseTrust + * FastMath.pow( + VisionConstants.baseNumber, + FastMath.pow( + VisionConstants.FOV58MJPGSingleTagCoeff * distance, + VisionConstants.FOV58YUYVPowerNumber))); + + return 1 + / (VisionConstants.FOV58YUYVBaseTrust + * FastMath.pow( + VisionConstants.baseNumber, + FastMath.pow( + VisionConstants.FOV58MJPGSingleTagCoeff * distance, + VisionConstants.FOV58YUYVPowerNumber))); + } + + case VisionConstants.kCam1Name, VisionConstants.kCam3Name -> { + if (numTags == 1) + return 1 + / (VisionConstants.FOV75YUYVBaseTrust + * FastMath.pow( + VisionConstants.baseNumber, + FastMath.pow( + VisionConstants.FOV75YUYVSingleTagCoeff * distance, + VisionConstants.FOV75YUYVPowerNumber))); + return 1 + / (VisionConstants.FOV75YUYVBaseTrust + * FastMath.pow( + VisionConstants.baseNumber, + FastMath.pow( + VisionConstants.FOV75YUYVMultiTagCoeff * distance, + VisionConstants.FOV75YUYVPowerNumber))); + } + + default -> { + if (numTags == 1) + return 1 + / (VisionConstants.baseTrust + * FastMath.pow( + VisionConstants.baseNumber, + FastMath.pow( + VisionConstants.singleTagCoeff * distance, VisionConstants.powerNumber))); + return 1 + / (VisionConstants.baseTrust + * FastMath.pow( + VisionConstants.baseNumber, + FastMath.pow( + VisionConstants.multiTagCoeff * distance, VisionConstants.powerNumber))); + } + } + } + + private Pose3d getCloserPose(Pose3d pose1, Pose3d pose2, double rotation) { + /*Which pose rotation is closer to our gyro. We subtract the absolute value of the gyro + from the rotation of the pose and compare the two*/ + double pose1Error = + FastMath.abs( + Rotation2d.fromRadians(rotation) + .minus(pose1.getRotation().toRotation2d()) + .getRadians()); + double pose2Error = + FastMath.abs( + Rotation2d.fromRadians(rotation) + .minus(pose2.getRotation().toRotation2d()) + .getRadians()); + + Logger.recordOutput("Vision/Pose 1 Yaw Error", pose1Error); + Logger.recordOutput("Vision/Pose 2 Yaw Error", pose2Error); + Logger.recordOutput("Vision/Historical yaw", rotation); + + if (pose1Error < pose2Error) { + if (pose1Error > VisionConstants.kYawErrorThreshold) {} + + return pose1; + } else { + return pose2; + } + } + + private Pose3d getCorrectPose(Pose3d pose1, Pose3d pose2, double time, int camIndex) { + // double dist1 = Math.abs(camHeights[camIndex] - pose1.getZ()); + // double dist2 = Math.abs(camHeights[camIndex] - pose2.getZ()); + // // This filters out results by seeing if they are close to the right height + // if (dist1 < dist2 && dist1 < 0.5) { + // return pose1; + // } + // if (dist2 < dist1 && dist2 < 0.5) { + // return pose2; + // } + // If we don't have enough data in the gyro buffer we default to returning a pose + if (gyroBuffer.size() < VisionConstants.kCircularBufferSize) return pose1; + + // See what pose is closer the the gyro at the time of the photo's capture. + double rotation = + gyroBuffer.get( + FastMath.floorToInt( + (((RobotController.getFPGATime() - time) / 1_000_000.0) + / VisionConstants.kLoopTime))); + Logger.recordOutput( + "Vision/Gyro Queried Loop Count", + FastMath.floorToInt(((time / 1_000_000.0) / VisionConstants.kLoopTime))); + return getCloserPose(pose1, pose2, rotation); + } + + @Override + public void periodic() { + double gyroData = FastMath.normalizeMinusPiPi(driveSubsystem.getGyroRotation2d().getRadians()); + gyroBuffer.addFirst(gyroData); + + if (gyroBuffer.size() >= 1000) { + Logger.recordOutput("Vision/Gyro Buffer Delayed", gyroBuffer.get(25)); + } + + Logger.recordOutput("Vision/Gyro Buffer", gyroData); + + if (getSeconds() - timeSinceLastUpdate > VisionConstants.kMaxTimeNoVision) { + updatesToWheels = 0; + } + + validResults.clear(); + + for (int i = 0; i < VisionConstants.kNumCams; i++) { + if (cams[i].hasNewUpdate()) { + timeSinceLastUpdate = getSeconds(); + validResults.add(new Pair(cams[i].getResults(), i)); + lastResult[i] = (WallEyeTagResult) cams[i].getResults(); + } + } + + if (getSeconds() - timeSinceLastUpdate >= VisionConstants.kTimeToDecayDev) { + // Decrease the thresholds required for a good pose over time. A graph is in the docs + for (int i = 0; i < 2; i++) { + double scaledWeight = + VisionConstants.kVisionMeasurementStdDevs.get(i, 0) + + VisionConstants.kStdDevDecayCoeff + * ((getSeconds() - timeSinceLastUpdate) - VisionConstants.kTimeToDecayDev); + + adaptiveMatrix.set( + i, + 0, + scaledWeight >= VisionConstants.kMinStdDev ? scaledWeight : VisionConstants.kMinStdDev); + } + } + + for (Pair res : validResults) { + if (res.getFirst() instanceof WallEyePoseResult) { + /*Reset the adaptive matrix to a stricter value once we get a result. + We set it to a stricter value then initially.*/ + adaptiveMatrix.set(0, 0, .1); + adaptiveMatrix.set(1, 0, .1); + + WallEyePoseResult result = (WallEyePoseResult) res.getFirst(); + + if (result.getCameraPose() == null) { // TODO figure out why it sometimes is null + textLogger.error("Pose is null, skipping!"); + continue; + } + + int idx = res.getSecond(); + + for (int i = 0; i < 2; i++) { + stdMatrix.set( + i, + 0, + .1 / getStdDevFactor(minTagDistance(result), result.getNumTags(), camNames[idx])); + } + + Pose3d cameraPose; + Translation3d robotTranslation = new Translation3d(); + Rotation3d cameraRotation = new Rotation3d(); + + if (result.getNumTags() > 1) { + // If there our more then one tag in an image we can get pose possible pose + cameraPose = result.getCameraPose(); + + robotTranslation = + cameraPose + .getTranslation() + .minus( + camPositions[idx] + .rotateBy(cameraPose.getRotation()) + .rotateBy(camRotations[idx])); + + cameraRotation = cameraPose.getRotation().rotateBy(camRotations[idx]); + } else { + // If there is one we get two possible poses and have to filter them. + Pose3d cam1Pose = result.getFirstPose(); + Pose3d cam2Pose = result.getSecondPose(); + + cam1Pose = + new Pose3d( + cam1Pose.getTranslation(), cam1Pose.getRotation().rotateBy(camRotations[idx])); + cam2Pose = + new Pose3d( + cam2Pose.getTranslation(), cam2Pose.getRotation().rotateBy(camRotations[idx])); + + Logger.recordOutput("Vision/Raw Camera 1 " + camNames[idx], cam1Pose); + Logger.recordOutput("Vision/Raw Camera 2 " + camNames[idx], cam2Pose); + + cameraPose = getCorrectPose(cam1Pose, cam2Pose, result.getTimeStamp(), idx); + robotTranslation = + cameraPose + .getTranslation() + .minus(camPositions[idx].rotateBy(cameraPose.getRotation())) + .rotateBy(camRotations[idx]); + cameraRotation = cameraPose.getRotation(); + } + Pose2d robotPose = + new Pose2d(robotTranslation.toTranslation2d(), cameraRotation.toRotation2d()); + if (camsWithinField(robotTranslation, result)) { + // Is the pose in the field? If so, enjoy a updated position drive subsystem + updatesToWheels++; + Logger.recordOutput("Vision/Accepted Cam " + camNames[idx], robotPose); + // However we do have to be accepting the poses to use them + if (visionUpdating) { + driveSubsystem.addVisionMeasurement( + robotPose, result.getTimeStamp() / 1_000_000.0, stdMatrix); + } + } else { + Logger.recordOutput("Vision/Rejected Cam " + camNames[idx], robotPose); + } + } + } + } - // FIXME: see walleye-testing branch from crescendo for correct implementation but - // FIXME: USE WalleyeTagResult INSTEAD OF WalleyeResult! public WallEyeTagResult getLastResult(int index) { - return null; + return lastResult[index]; + } + + @Override + public Set getMeasures() { + return Set.of(); + } + + @Override + public void registerWith(TelemetryService telemetryService) { + super.registerWith(telemetryService); } } diff --git a/vendordeps/Phoenix6-replay-frc2025-latest.json b/vendordeps/Phoenix6-replay-25.2.2.json similarity index 92% rename from vendordeps/Phoenix6-replay-frc2025-latest.json rename to vendordeps/Phoenix6-replay-25.2.2.json index f2ff57eb..f94c1084 100644 --- a/vendordeps/Phoenix6-replay-frc2025-latest.json +++ b/vendordeps/Phoenix6-replay-25.2.2.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-replay-frc2025-latest.json", + "fileName": "Phoenix6-replay-25.2.2.json", "name": "CTRE-Phoenix (v6) Replay", - "version": "25.2.1", + "version": "25.2.2", "frcYear": "2025", "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.2.1" + "version": "25.2.2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -48,7 +48,7 @@ { "groupId": "com.ctre.phoenix6.replay", "artifactId": "api-cpp-replay", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -62,7 +62,7 @@ { "groupId": "com.ctre.phoenix6.replay", "artifactId": "tools-replay", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -76,7 +76,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -90,7 +90,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -118,7 +118,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -132,7 +132,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -146,7 +146,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -160,7 +160,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -174,7 +174,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -202,7 +202,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -216,7 +216,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -232,7 +232,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -245,7 +245,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -258,7 +258,7 @@ { "groupId": "com.ctre.phoenix6.replay", "artifactId": "wpiapi-cpp-replay", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_Phoenix6_WPIReplay", "headerClassifier": "headers", "sharedLibrary": true, @@ -274,7 +274,7 @@ { "groupId": "com.ctre.phoenix6.replay", "artifactId": "tools-replay", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_PhoenixTools_Replay", "headerClassifier": "headers", "sharedLibrary": true, @@ -290,7 +290,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -306,7 +306,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -322,7 +322,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -338,7 +338,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -354,7 +354,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -370,7 +370,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -386,7 +386,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -402,7 +402,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -418,7 +418,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -434,7 +434,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -450,7 +450,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/thirdcoast.json b/vendordeps/thirdcoast.json index d142a4ca..b1be2433 100644 --- a/vendordeps/thirdcoast.json +++ b/vendordeps/thirdcoast.json @@ -1,7 +1,7 @@ { "fileName": "thirdcoast.json", "name": "StrykeForce-ThirdCoast", - "version": "25.0.1", + "version": "25.0.4", "uuid": "13c4f4b5-a1c0-4670-8f8d-b7b03628c0d3", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "org.strykeforce", "artifactId": "thirdcoast", - "version": "25.0.1" + "version": "25.0.4" } ], "jniDependencies": [],