diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 03bac83..99caf6d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,80 +4,248 @@ package frc.robot; -import com.ctre.phoenix6.SignalLogger; - +import edu.wpi.first.epilogue.Epilogue; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import edu.wpi.first.wpilibj.XboxController.Axis; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; -public class Robot extends TimedRobot { - private Command m_autonomousCommand; - - private final Superstructure superstructure; +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; + +import frc.robot.commands.swerve.*; +import frc.robot.constants.*; +import static frc.robot.constants.IDConstants.*; +import static frc.robot.constants.FieldConstants.*; +import static frc.robot.constants.FieldConstants.GeoFencing.*; +import frc.robot.subsystems.*; +import frc.robot.subsystems.vision.*; +import frc.robot.subsystems.vision.Vision.TagPOI; +import frc.robot.util.AutoFactories; +import frc.robot.util.FieldUtils; +import frc.robot.util.SD; +import frc.robot.util.controlTransmutation.*; +import frc.robot.util.libs.Telemetry; + +@Logged +public class Robot extends TimedRobot +{ + /* Enums */ + public enum TargetPosition {Left, Right, Centre, None} + public enum DriveState {Reef, Station, Barge, None} + + /* State */ + private SwerveDriveState swerveState; + private TargetPosition currentTarget; + private DriveState currentDriveState; + private Command autoCommand; + + /* Telemetry and SD */ + private Field2d field = new Field2d(); + private final Telemetry ctreLogger = new Telemetry(Constants.Swerve.maxSpeed); + + /* Subsystems */ + private final static CommandSwerveDrivetrain s_Swerve = TunerConstants.createDrivetrain(); + private final CoralRoller s_Coral = new CoralRoller(); + private final Vision s_Vision = new Vision + ( + (poseEst, timestmp, stdDevs) -> + { + s_Swerve.setVisionMeasurementStdDevs(stdDevs); + s_Swerve.addVisionMeasurement(poseEst, timestmp); + }, + () -> Pair.of(s_Swerve.getPigeon2().getYaw().getValueAsDouble(), swerveState.Speeds.omegaRadiansPerSecond), + new Limelight(foreLimelightName), + new Limelight(aftLimelightName) + ); + + /* Controllers */ + private final CommandXboxController driver = new CommandXboxController(0); + private final CommandXboxController operator = new CommandXboxController(1); + + /* Rumble */ + private final RumbleRequester io_driverRight = new RumbleRequester(driver, RumbleType.kRightRumble, SD.RUMBLE_DRIVER); + private final RumbleRequester io_driverLeft = new RumbleRequester(driver, RumbleType.kLeftRumble, SD.RUMBLE_DRIVER); + private final RumbleRequester io_operatorRight = new RumbleRequester(operator, RumbleType.kRightRumble, SD.RUMBLE_OPERATOR); + private final RumbleRequester io_operatorLeft = new RumbleRequester(operator, RumbleType.kLeftRumble, SD.RUMBLE_OPERATOR); + + /* Input Transmutation */ + private final JoystickTransmuter driverStick = new JoystickTransmuter(driver::getLeftY, driver::getLeftX).invertX().invertY(); + private final Brake driverBrake = new Brake(driver::getRightTriggerAxis, Constants.Control.maxThrottle, Constants.Control.minThrottle); + private final InputCurve driverInputCurve = new InputCurve(2); + private final Deadband driverDeadband = new Deadband(); public Robot() { - superstructure = new Superstructure(); + /* State Initialisation */ + currentDriveState = DriveState.None; + updateSwerveState(); + /* Telemetry and SD */ SignalLogger.enableAutoLogging(false); DataLogManager.start("/home/lvuser/logs"); DriverStation.startDataLog(DataLogManager.getLog()); + Epilogue.bind(this); + SmartDashboard.putData("Field", field); + s_Swerve.registerTelemetry(ctreLogger::telemeterize); + + /* Configure Input Transmutation */ + boolean redAlliance = FieldUtils.isRedAlliance(); + driverStick.rotated(redAlliance); + driverStick.withFieldObjects(GeoFencing.fieldGeoFence).withBrake(driverBrake).withInputCurve(driverInputCurve).withDeadband(driverDeadband); + FieldUtils.activateAllianceFencing(redAlliance); + FieldConstants.GeoFencing.configureAttractors((testTarget, testState) -> currentTarget == testTarget && currentDriveState == testState); + FieldObject.setRobotRadiusSup + (() -> + Math.hypot(swerveState.Speeds.vxMetersPerSecond, swerveState.Speeds.vyMetersPerSecond) >= robotSpeedThreshold ? + robotRadiusCircumscribed : + robotRadiusInscribed + ); + FieldObject.setRobotPosSup(swerveState.Pose::getTranslation); + + /* Bindings */ + bindControls(); + bindRumbles(); } - @Override - public void robotPeriodic() + /* Binding Methods */ + private void bindControls() { - superstructure.updateSwerveState(); - CommandScheduler.getInstance().run(); + /* Default Commands */ + s_Swerve.setDefaultCommand + ( + new ManualDrive + ( + s_Swerve, + driverStick::stickOutput, + () -> -driver.getRightX(), + driver::getRightTriggerAxis + ) + ); + s_Coral.setDefaultCommand(s_Coral.setSpeedCommand(0)); + + /* Setting Drive States */ + driver.povLeft().onTrue(Commands.runOnce(() -> currentTarget = TargetPosition.Left)); + driver.povRight().onTrue(Commands.runOnce(() -> currentTarget = TargetPosition.Right)); + driver.povUp().onTrue(Commands.runOnce(() -> currentTarget = TargetPosition.Centre)); + driver.povDown().onTrue(Commands.runOnce(() -> currentTarget = TargetPosition.None)); + + driver.x().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.Reef)); + driver.a().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.Station)); + driver.y().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.Barge)); + driver.b().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.None)); + driver.axisMagnitudeGreaterThan(Axis.kRightX.value, 0.2).onTrue(Commands.runOnce(() -> currentDriveState = DriveState.None)); + + /* Coral Roller */ + driver.leftTrigger().whileTrue(s_Coral.setSpeedCommand(Constants.Coral.forwardSpeed)); + driver.leftBumper().whileTrue(s_Coral.setSpeedCommand(Constants.Coral.reverseSpeed)); + new Trigger(() -> FieldUtils.atReefLineUp(swerveState.Pose)).whileTrue(s_Coral.setSpeedCommand(Constants.Coral.forwardSpeed)); + + /* Heading Locking */ + new Trigger(() -> currentDriveState == DriveState.None) + .onTrue(Commands.runOnce(() -> s_Vision.setActivePOI(TagPOI.REEF))) + .whileTrue + ( + new ManualDrive + ( + s_Swerve, + driverStick::stickOutput, + () -> -driver.getRightX(), + driver::getRightTriggerAxis + ) + ); + new Trigger(() -> currentDriveState == DriveState.Reef) + .onTrue(Commands.runOnce(() -> s_Vision.setActivePOI(TagPOI.REEF))) + .whileTrue + ( + new TargetScoreDrive + ( + s_Swerve, + driverStick::stickOutput, + Rotation2d.kZero, + swerveState.Pose::getTranslation + ) + ); + new Trigger(() -> currentDriveState == DriveState.Station) + .onTrue(Commands.runOnce(() -> s_Vision.setActivePOI(TagPOI.CORALSTATION))) + .whileTrue + ( + new TargetStationDrive + ( + s_Swerve, + driverStick::stickOutput, + Rotation2d.kZero, + swerveState.Pose::getTranslation + ) + ); + new Trigger(() -> currentDriveState == DriveState.Barge) + .onTrue(Commands.runOnce(() -> s_Vision.setActivePOI(TagPOI.BARGE))) + .whileTrue + ( + new HeadingLockedDrive + ( + s_Swerve, + driverStick::stickOutput, + Rotation2d.kZero, + Rotation2d.kZero, + swerveState.Pose::getTranslation + ) + ); + + /* Other */ + driver.start().onTrue(Commands.runOnce(s_Vision::resetRotation).ignoringDisable(true)); + new Trigger(SD.LL_EXPOSURE_UP::button).onTrue(Commands.runOnce(s_Vision::incrementPipeline)); + new Trigger(SD.LL_EXPOSURE_DOWN::button).onTrue(Commands.runOnce(s_Vision::decrementPipeline)); } - @Override - public void disabledInit() {} - - @Override - public void disabledPeriodic() {} - - @Override - public void disabledExit() {} - - @Override - public void autonomousInit() { - m_autonomousCommand = superstructure.getAutonomousCommand(); - - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); - } + private void bindRumbles() + { + io_operatorLeft.addRumbleTrigger("CoralHeld", new Trigger(s_Coral::getSensor)); + io_operatorRight.addRumbleTrigger("ScoreReady" , new Trigger(() -> FieldUtils.atReefLineUp(swerveState.Pose))); } - @Override - public void autonomousPeriodic() {} + /* Util Methods */ + public static void setYaw(double newYaw) {s_Swerve.getPigeon2().setYaw(newYaw);} - @Override - public void autonomousExit() {} - - @Override - public void teleopInit() { - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); - } + private void updateSwerveState() + { + swerveState = s_Swerve.getState(); + field.setRobotPose(swerveState.Pose); } - + + /* Opmode Methods */ @Override - public void teleopPeriodic() {} + public void robotPeriodic() + { + updateSwerveState(); + CommandScheduler.getInstance().run(); + } @Override - public void teleopExit() {} + public void autonomousInit() + { + autoCommand = AutoFactories.getCommandList(SD.AUTO_STRING.get(), s_Coral, s_Swerve, () -> swerveState); - @Override - public void testInit() { - CommandScheduler.getInstance().cancelAll(); + if (autoCommand != null) autoCommand.schedule(); } @Override - public void testPeriodic() {} + public void teleopInit() + { + if (autoCommand != null) autoCommand.cancel(); + } @Override - public void testExit() {} + public void testInit() {CommandScheduler.getInstance().cancelAll();} } diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java deleted file mode 100644 index e77b68b..0000000 --- a/src/main/java/frc/robot/Superstructure.java +++ /dev/null @@ -1,293 +0,0 @@ -package frc.robot; - -import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.wpilibj.GenericHID.RumbleType; -import edu.wpi.first.wpilibj.XboxController.Axis; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.commands.swerve.HeadingLockedDrive; -import frc.robot.commands.swerve.ManualDrive; -import frc.robot.commands.swerve.TargetScoreDrive; -import frc.robot.commands.swerve.TargetStationDrive; -import frc.robot.constants.Constants; -import frc.robot.constants.FieldConstants; -import frc.robot.constants.TunerConstants; -import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.CoralRoller; -import frc.robot.subsystems.Limelight; -import frc.robot.subsystems.RumbleRequester; -import frc.robot.subsystems.Limelight.TagPOI; -import frc.robot.util.AutoFactories; -import frc.robot.util.FieldUtils; -import frc.robot.util.SD; -import frc.robot.util.Telemetry; -import frc.robot.util.controlTransmutation.Brake; -import frc.robot.util.controlTransmutation.Deadband; -import frc.robot.util.controlTransmutation.FieldObject; -import frc.robot.util.controlTransmutation.InputCurve; -import frc.robot.util.controlTransmutation.JoystickTransmuter; - -public class Superstructure -{ - public enum TargetPosition {Left, Right, Centre, None} - public enum DriveState {Reef, Station, Barge, None} - - private static boolean rotationKnown = false; - private static boolean useLimelights = true; - private static boolean useFence = true; - private static boolean useRestrictors = true; - private boolean redAlliance; - - private final Telemetry logger; - - private SwerveDriveState swerveState; - private Field2d field; - - private static CommandSwerveDrivetrain s_Swerve; - private CoralRoller s_Coral; - private Limelight s_foreLL; - private Limelight s_aftLL; - - private static TargetPosition currentTarget; - private static DriveState currentDriveState; - - private final CommandXboxController driver = new CommandXboxController(0); - private final CommandXboxController operator = new CommandXboxController(1); - private final RumbleRequester io_driverRight = new RumbleRequester(driver, RumbleType.kRightRumble, SD.RUMBLE_D_R::put, SD.IO_RUMBLE_D::get); - private final RumbleRequester io_driverLeft = new RumbleRequester(driver, RumbleType.kLeftRumble, SD.RUMBLE_D_L::put, SD.IO_RUMBLE_D::get); - private final RumbleRequester io_copilotRight = new RumbleRequester(operator, RumbleType.kRightRumble, SD.RUMBLE_C_R::put, SD.IO_RUMBLE_C::get); - private final RumbleRequester io_copilotLeft = new RumbleRequester(operator, RumbleType.kLeftRumble, SD.RUMBLE_C_L::put, SD.IO_RUMBLE_C::get); - - private final JoystickTransmuter driverStick = new JoystickTransmuter(driver::getLeftY, driver::getLeftX).invertX().invertY(); - private final Brake driverBrake = new Brake(driver::getRightTriggerAxis, Constants.Control.maxThrottle, Constants.Control.minThrottle); - private final InputCurve driverInputCurve = new InputCurve(2); - private final Deadband driverDeadband = new Deadband(); - - public Superstructure() - { - field = new Field2d(); - s_Swerve = TunerConstants.createDrivetrain(); - s_Coral = new CoralRoller(); - s_foreLL = new Limelight("limelight-fore"); - s_aftLL = new Limelight("limelight-aft"); - - logger = new Telemetry(Constants.Swerve.maxSpeed); - - redAlliance = FieldUtils.isRedAlliance(); - - setStartPose(redAlliance); - updateSwerveState(); - driverStick.rotated(redAlliance); - FieldUtils.activateAllianceFencing(); - - SmartDashboard.putData("Field", field); - - s_Swerve.registerTelemetry(logger::telemeterize); - driverStick.withFieldObjects(FieldConstants.GeoFencing.fieldGeoFence).withBrake(driverBrake).withInputCurve(driverInputCurve).withDeadband(driverDeadband); - FieldConstants.GeoFencing.fieldGeoFence.setActiveCondition(() -> useFence && useLimelights); - FieldObject.setRobotRadiusSup(this::robotRadiusSup); - FieldObject.setRobotPosSup(this::getPosition); - - bindControls(); - bindRumbles(); - } - - public SwerveDriveState getSwerveState() - {return swerveState;} - - public void updateSwerveState() - { - swerveState = s_Swerve.getState(); - field.setRobotPose(swerveState.Pose); - } - - public Translation2d getPosition() - { - return swerveState.Pose.getTranslation(); - } - - private void bindControls() - { - currentDriveState = DriveState.None; - s_Swerve.setDefaultCommand - ( - new ManualDrive - ( - s_Swerve, - driverStick::stickOutput, - () -> -driver.getRightX(), - driver::getRightTriggerAxis - ) - ); - - new Trigger(() -> {return currentDriveState == DriveState.None;}) - .onTrue(Commands.runOnce(() -> Limelight.setActivePOI(TagPOI.REEF))) - .whileTrue - ( - new ManualDrive - ( - s_Swerve, - driverStick::stickOutput, - () -> -driver.getRightX(), - driver::getRightTriggerAxis - ) - ); - - new Trigger(() -> {return currentDriveState == DriveState.Reef;}) - .onTrue(Commands.runOnce(() -> Limelight.setActivePOI(TagPOI.REEF))) - .whileTrue - ( - new TargetScoreDrive - ( - s_Swerve, - driverStick::stickOutput, - Rotation2d.kZero, - () -> getPosition() - ) - ); - - new Trigger(() -> {return currentDriveState == DriveState.Station;}) - .onTrue(Commands.runOnce(() -> Limelight.setActivePOI(TagPOI.CORALSTATION))) - .whileTrue - ( - new TargetStationDrive - ( - s_Swerve, - driverStick::stickOutput, - Rotation2d.kZero, - () -> getPosition() - ) - ); - - new Trigger(() -> {return currentDriveState == DriveState.Barge;}) - .onTrue(Commands.runOnce(() -> Limelight.setActivePOI(TagPOI.BARGE))) - .whileTrue - ( - new HeadingLockedDrive - ( - s_Swerve, - driverStick::stickOutput, - Rotation2d.kZero, - Rotation2d.kZero, - () -> getPosition() - ) - ); - - - driver.leftTrigger().whileTrue(s_Coral.runCommand(SD.IO_CORALSPEED_F.get())); - driver.leftBumper().whileTrue(s_Coral.runCommand(SD.IO_CORALSPEED_R.get())); - //operator.leftBumper().whileTrue(s_Coral.runCommand(SD.IO_CORALSPEED_R.get())); - // Heading reset - driver.start() - .onTrue - ( - Commands.runOnce - ( - () -> - { - Pigeon2 pigeon = s_Swerve.getPigeon2(); - - pigeon.setYaw(redAlliance ? 0 : 180); - s_Swerve.resetPose(new Pose2d(swerveState.Pose.getTranslation(), new Rotation2d(Math.toRadians(pigeon.getYaw().getValueAsDouble())))); - FieldUtils.activateAllianceFencing(); - rotationKnown = false; - useLimelights = true; - } - ) - .ignoringDisable(true) - .withName("HeadingReset") - ); - driver.back() - .onTrue - ( - Commands.runOnce - ( - () -> - { - Pigeon2 pigeon = s_Swerve.getPigeon2(); - - pigeon.setYaw(redAlliance ? 0 : 180); - s_Swerve.resetPose(new Pose2d(swerveState.Pose.getTranslation(), new Rotation2d(Math.toRadians(pigeon.getYaw().getValueAsDouble())))); - FieldUtils.activateAllianceFencing(); - rotationKnown = false; - useLimelights = false; - } - ) - .ignoringDisable(true) - .withName("DisableNavigation") - ); - - driver.povLeft().onTrue(Commands.runOnce(() -> currentTarget = TargetPosition.Left)); - driver.povRight().onTrue(Commands.runOnce(() -> currentTarget = TargetPosition.Right)); - driver.povUp().onTrue(Commands.runOnce(() -> currentTarget = TargetPosition.Centre)); - driver.povDown().onTrue(Commands.runOnce(() -> currentTarget = TargetPosition.None)); - - driver.x().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.Reef)); - driver.a().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.Station)); - driver.y().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.Barge)); - driver.b().onTrue(Commands.runOnce(() -> currentDriveState = DriveState.None)); - driver.axisMagnitudeGreaterThan(Axis.kRightX.value, 0.2).onTrue(Commands.runOnce(() -> currentDriveState = DriveState.None)); - - new Trigger(() -> FieldUtils.atReefLineUp(swerveState.Pose.getTranslation())).whileTrue(s_Coral.smartRunCommand(Constants.Coral.forwardSpeed)); - } - - private void bindRumbles() - { - io_copilotLeft.addRumbleTrigger("coral held", new Trigger(s_Coral::getSensor)); - io_copilotRight.addRumbleTrigger("ready to score" , new Trigger(() -> FieldUtils.atReefLineUp(swerveState.Pose.getTranslation()))); - } - - public Command getAutonomousCommand() - { - return AutoFactories.getCommandList(SD.IO_AUTO.get(), s_Coral, s_Swerve, () -> swerveState); - } - - public static void addVisionMeasurement(Pose2d poseMeasurement, double timestamp) - { - s_Swerve.addVisionMeasurement(poseMeasurement, timestamp); - } - - public static void setVisionMeasurementStdDevs(Matrix visionMeasurementStdDevs) - { - s_Swerve.setVisionMeasurementStdDevs(visionMeasurementStdDevs); - } - - public double robotRadiusSup() - { - double robotSpeed = Math.hypot(swerveState.Speeds.vxMetersPerSecond, swerveState.Speeds.vyMetersPerSecond); - - return - robotSpeed >= FieldConstants.GeoFencing.robotSpeedThreshold ? - FieldConstants.GeoFencing.robotRadiusCircumscribed : - FieldConstants.GeoFencing.robotRadiusInscribed; - } - - private void setStartPose(boolean isRedAlliance) - { - if (isRedAlliance) - {s_Swerve.resetPose(FieldConstants.redStartLine);} - else - {s_Swerve.resetPose(FieldConstants.blueStartLine);} - } - - public static boolean isRotationKnown() {return rotationKnown;} - public static void setRotationKnown(boolean isKnown) {rotationKnown = isKnown;} - public static boolean isVisionActive() {return useLimelights;} - public static boolean isRestrictorsActive() {return useRestrictors;} - public static void setYaw(double newYaw) {s_Swerve.getPigeon2().setYaw(newYaw);} - public static double getYaw() {return s_Swerve.getPigeon2().getYaw().getValueAsDouble();} - public static boolean checkTargetPosition(TargetPosition testTargetPosition) {return testTargetPosition == currentTarget;} - public static boolean checkDriveState(DriveState testDriveState) {return testDriveState == currentDriveState;} -} diff --git a/src/main/java/frc/robot/commands/swerve/TargetScoreDrive.java b/src/main/java/frc/robot/commands/swerve/TargetScoreDrive.java index 9b9f78c..671d29a 100644 --- a/src/main/java/frc/robot/commands/swerve/TargetScoreDrive.java +++ b/src/main/java/frc/robot/commands/swerve/TargetScoreDrive.java @@ -5,17 +5,13 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; -import frc.robot.constants.FieldConstants; +import static frc.robot.constants.FieldConstants.GeoFencing.*; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.util.FieldUtils; -import frc.robot.util.controlTransmutation.CrossDeadband; -import frc.robot.util.controlTransmutation.Deadband; public class TargetScoreDrive extends HeadingLockedDrive { private Rotation2d rotationOffsetBase; - private int nearestReefFace; - private Deadband crossDeadband = new CrossDeadband(); /** Creates a new TargetScoreDrive. */ public TargetScoreDrive @@ -33,48 +29,45 @@ public class TargetScoreDrive extends HeadingLockedDrive @Override protected void updateTargetHeading() { - if - ( - FieldConstants.GeoFencing.reefBlue.getDistance() >= FieldConstants.GeoFencing.robotRadiusCircumscribed/2 && - FieldConstants.GeoFencing.reefRed.getDistance() >= FieldConstants.GeoFencing.robotRadiusCircumscribed/2 - ) + if (reefBlue.getDistance() >= robotRadiusCircumscribed/2 && reefRed.getDistance() >= robotRadiusCircumscribed/2) { - nearestReefFace = FieldUtils.getNearestReefFace(robotXY); - - switch (nearestReefFace) + switch (FieldUtils.getNearestReefFace(robotXY)) { - case 1: + case 1 -> + { targetHeading = Rotation2d.kZero; super.rotationOffset = this.rotationOffsetBase; - break; + } - case 2: + case 2 -> + { targetHeading = new Rotation2d(Units.degreesToRadians(60)); super.rotationOffset = this.rotationOffsetBase.unaryMinus(); - break; + } - case 3: + case 3 -> + { targetHeading = new Rotation2d(Units.degreesToRadians(120)); super.rotationOffset = this.rotationOffsetBase.unaryMinus(); - break; + } - case 4: + case 4 -> + { targetHeading = Rotation2d.k180deg; super.rotationOffset = this.rotationOffsetBase.unaryMinus(); - break; + } - case 5: + case 5 -> + { targetHeading = new Rotation2d(Units.degreesToRadians(-120)); super.rotationOffset = this.rotationOffsetBase.unaryMinus(); - break; - - case 6: + } + + case 6 -> + { targetHeading = new Rotation2d(Units.degreesToRadians(-60)); super.rotationOffset = this.rotationOffsetBase.unaryMinus(); - break; - - default: - break; + } } } } diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index 6d911bb..8685480 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -7,7 +7,7 @@ public final class Constants public static final class RumblerConstants { public static final double driverDefault = 0.1; - public static final double copilotDefault = 0.1; + public static final double operatorDefault = 0.1; } public static final class Control @@ -33,11 +33,12 @@ public static final class Control public static final double manualClimberScale = 1; public static final double driveSnappingRange = 1.5; public static final double cageFaceDistance = 1.5; + /** Translation lineup tolerance, in meters */ public static final double lineupTolerance = 0.05; + /** Rotation lineup tolerance, in degrees */ + public static final double angleLineupTolerance = 1.5; } - - public static final class Swerve { /** Centre-centre distance (length and width) between wheels, metres */ @@ -72,12 +73,6 @@ public static final class Coral public static final double reverseSpeed = 0.15; } - public static final class Algae - { - public static final double forwardSpeed = 0.5; - public static final double reverseSpeed = -0.5; - } - public static final class Vision { /* public static final int[] validIDs = @@ -114,5 +109,7 @@ public static final class Vision public static final double linearStdDevBaseline = 0.08; /** Baseline 1 meter, 1 tag stddev rotation, in radians */ public static final double rotStdDevBaseline = 999; + /** How many good MT1 readings to get before setting rotation and moving to MT2 */ + public static final int mt1CyclesNeeded = 10; } } diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java index 0332554..cf79341 100644 --- a/src/main/java/frc/robot/constants/FieldConstants.java +++ b/src/main/java/frc/robot/constants/FieldConstants.java @@ -1,14 +1,14 @@ package frc.robot.constants; import java.util.ArrayList; +import java.util.function.BiPredicate; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; -import frc.robot.Superstructure; -import frc.robot.Superstructure.DriveState; -import frc.robot.Superstructure.TargetPosition; +import frc.robot.Robot.DriveState; +import frc.robot.Robot.TargetPosition; import frc.robot.util.controlTransmutation.Attractor; import frc.robot.util.controlTransmutation.ObjectList; import frc.robot.util.controlTransmutation.geoFence.*; @@ -53,52 +53,39 @@ public class FieldConstants public static Pose2d getLineup(String name) { - switch (name) { - case "ra": - return raLineup; - - case "rb": - return rbLineup; - - case "rc": - return rcLineup; - - case "rd": - return rdLineup; - - case "re": - return reLineup; - - case "rf": - return rfLineup; - - case "rg": - return rgLineup; - - case "rh": - return rhLineup; - - case "ri": - return riLineup; - - case "rj": - return rjLineup; - - case "rk": - return rkLineup; - - case "rl": - return rlLineup; - - default: - return raLineup; - } + return switch (name) + { + case "ra" -> raLineup; + case "rb" -> rbLineup; + case "rc" -> rcLineup; + case "rd" -> rdLineup; + case "re" -> reLineup; + case "rf" -> rfLineup; + case "rg" -> rgLineup; + case "rh" -> rhLineup; + case "ri" -> riLineup; + case "rj" -> rjLineup; + case "rk" -> rkLineup; + case "rl" -> rlLineup; + default -> raLineup; + }; } public static final double coralStationRange = 0.6; public static final class GeoFencing { + /** + * Minimum value for object radius, metres

+ * The system is not confirmed to handle negative radii + */ + public static final double minRadius = 0; + /** + * Minimum value for object buffer, metres

+ * A small buffer is required to ensure safe transitions + */ + public static final double minBuffer = 0.1; + // Relative to the centre of the robot, in direction the robot is facing // These values are the distance in metres to the virtual wall the robot will stop at // 0 means the wall is running through the middle of the robot @@ -170,30 +157,30 @@ public static final class GeoFencing public static final Attractor testAttractor = new Attractor(fieldCentre.getX(), fieldCentre.getY(), 0, 5, 1.5); // Set up Attractors and Conditions for GeoFence objects - static + public static void configureAttractors(BiPredicate checkTargetAndState) { - reefRed.addRelativeAttractors(0.4, -0.2, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Left) && Superstructure.checkDriveState(DriveState.Reef)); - reefRed.addRelativeAttractors(0.4, 0, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Centre) && Superstructure.checkDriveState(DriveState.Reef)); - reefRed.addRelativeAttractors(0.4, 0.2, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Right) && Superstructure.checkDriveState(DriveState.Reef)); - reefBlue.addRelativeAttractors(0.4, -0.2, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Left) && Superstructure.checkDriveState(DriveState.Reef)); - reefBlue.addRelativeAttractors(0.4, 0, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Centre) && Superstructure.checkDriveState(DriveState.Reef)); - reefBlue.addRelativeAttractors(0.4, 0.2, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Right) && Superstructure.checkDriveState(DriveState.Reef)); - - cornerSBlue.addRelativeAttractor(true, 0.4, 0.5, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Left) && Superstructure.checkDriveState(DriveState.Station)); - cornerSBlue.addRelativeAttractor(true, 0.4, 0, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Centre) && Superstructure.checkDriveState(DriveState.Station)); - cornerSBlue.addRelativeAttractor(true, 0.4, -0.5, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Right) && Superstructure.checkDriveState(DriveState.Station)); + reefRed.addRelativeAttractors(0.4, -0.2, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Left, DriveState.Reef)) ; + reefRed.addRelativeAttractors(0.4, 0, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Centre, DriveState.Reef)) ; + reefRed.addRelativeAttractors(0.4, 0.2, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Right, DriveState.Reef)) ; + reefBlue.addRelativeAttractors(0.4, -0.2, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Left, DriveState.Reef)) ; + reefBlue.addRelativeAttractors(0.4, 0, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Centre, DriveState.Reef)) ; + reefBlue.addRelativeAttractors(0.4, 0.2, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Right, DriveState.Reef)) ; + + cornerSBlue.addRelativeAttractor(true, 0.4, 0.5, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Left, DriveState.Station)) ; + cornerSBlue.addRelativeAttractor(true, 0.4, 0, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Centre, DriveState.Station)) ; + cornerSBlue.addRelativeAttractor(true, 0.4, -0.5, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Right, DriveState.Station)) ; - cornerNBlue.addRelativeAttractor(false, 0.4, 0.5, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Left) && Superstructure.checkDriveState(DriveState.Station)); - cornerNBlue.addRelativeAttractor(false, 0.4, 0, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Centre) && Superstructure.checkDriveState(DriveState.Station)); - cornerNBlue.addRelativeAttractor(false, 0.4, -0.5, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Right) && Superstructure.checkDriveState(DriveState.Station)); + cornerNBlue.addRelativeAttractor(false, 0.4, 0.5, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Left, DriveState.Station)) ; + cornerNBlue.addRelativeAttractor(false, 0.4, 0, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Centre, DriveState.Station)) ; + cornerNBlue.addRelativeAttractor(false, 0.4, -0.5, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Right, DriveState.Station)) ; - cornerSRed.addRelativeAttractor(false, 0.4, 0.5, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Left) && Superstructure.checkDriveState(DriveState.Station)); - cornerSRed.addRelativeAttractor(false, 0.4, 0, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Centre) && Superstructure.checkDriveState(DriveState.Station)); - cornerSRed.addRelativeAttractor(false, 0.4, -0.5, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Right) && Superstructure.checkDriveState(DriveState.Station)); + cornerSRed.addRelativeAttractor(false, 0.4, 0.5, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Left, DriveState.Station)) ; + cornerSRed.addRelativeAttractor(false, 0.4, 0, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Centre, DriveState.Station)) ; + cornerSRed.addRelativeAttractor(false, 0.4, -0.5, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Right, DriveState.Station)) ; - cornerNRed.addRelativeAttractor(true, 0.4, 0.5, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Left) && Superstructure.checkDriveState(DriveState.Station)); - cornerNRed.addRelativeAttractor(true, 0.4, 0, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Centre) && Superstructure.checkDriveState(DriveState.Station)); - cornerNRed.addRelativeAttractor(true, 0.4, -0.5, 2.5, 1.2, () -> Superstructure.checkTargetPosition(TargetPosition.Right) && Superstructure.checkDriveState(DriveState.Station)); + cornerNRed.addRelativeAttractor(true, 0.4, 0.5, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Left, DriveState.Station)) ; + cornerNRed.addRelativeAttractor(true, 0.4, 0, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Centre, DriveState.Station)) ; + cornerNRed.addRelativeAttractor(true, 0.4, -0.5, 2.5, 1.2, () -> checkTargetAndState.test(TargetPosition.Right, DriveState.Station)) ; } public static final ObjectList fieldBlueGeoFence = new ObjectList @@ -218,5 +205,15 @@ public static final class GeoFencing public static final ObjectList fieldGeoFence = new ObjectList(field, fieldBlueGeoFence, fieldRedGeoFence); + /** Minimum speed limit within a restrictor */ + public static final double minLocalSpeedLimit = 0.05; + } + + public static final class AutoDrive + { + /** Attractor minimum angle tolerance, degrees */ + public static final double minAngleTolerance = 20; + /** Attractor maximum angle tolerance, degrees */ + public static final double maxAngleTolerance = 60; } } diff --git a/src/main/java/frc/robot/constants/IDConstants.java b/src/main/java/frc/robot/constants/IDConstants.java index 31537c6..933a0b1 100644 --- a/src/main/java/frc/robot/constants/IDConstants.java +++ b/src/main/java/frc/robot/constants/IDConstants.java @@ -10,7 +10,6 @@ public final class IDConstants /* Drive */ /* _____ */ - public static final int foreStbdDriveMotorID = 1; public static final int foreStbdAngleMotorID = 2; public static final int foreStbdCANcoderID = 3; @@ -31,11 +30,14 @@ public final class IDConstants /* Mechanism */ /* _________ */ - public static final int coralRollerID = 13; public static final int coralSensorDIO = 0; - public static final int AlgaeRollerID = 14; - public static final int AlgaeSensorDIO = 1; - + public static final int algaeRollerID = 14; + public static final int algaeSensorDIO = 1; + + /* Limelights */ + /* __________ */ + public static final String foreLimelightName = "limelight-fore"; + public static final String aftLimelightName = "limelight-aft"; } diff --git a/src/main/java/frc/robot/subsystems/AlgaeRoller.java b/src/main/java/frc/robot/subsystems/AlgaeRoller.java deleted file mode 100644 index 08f22b5..0000000 --- a/src/main/java/frc/robot/subsystems/AlgaeRoller.java +++ /dev/null @@ -1,47 +0,0 @@ -package frc.robot.subsystems; - -import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.IDConstants; -import frc.robot.util.SD; - -public class AlgaeRoller extends SubsystemBase -{ - private TalonFX m_Algae; - private DigitalInput io_Sensor; - - public AlgaeRoller() - { - m_Algae = new TalonFX(IDConstants.AlgaeRollerID); - io_Sensor = new DigitalInput(IDConstants.AlgaeSensorDIO); - } - - public void setSpeed(double speed) - { - m_Algae.set(speed); - } - - public boolean getSensor() - { - return io_Sensor.get(); - } - - public Command runCommand(double speed) - { - return startEnd(() -> setSpeed(speed), () -> setSpeed(0)); - } - - public Command smartRunCommand(double speed) - { - return runCommand(speed).until(this::getSensor); - } - - @Override - public void periodic() - { - SD.ALGAE_ROLLER.put(m_Algae.get()); - } -} diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 2abfb05..37fe995 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -27,8 +27,9 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.constants.Constants; +import static frc.robot.constants.Constants.Swerve.*; import frc.robot.constants.TunerConstants.TunerSwerveDrivetrain; +import frc.robot.util.FieldUtils; import frc.robot.util.controlTransmutation.PIDDriveTransmuter; /** @@ -37,8 +38,8 @@ */ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem, Sendable { - private final PIDController thetaController = new PIDController(Constants.Swerve.rotationKP, Constants.Swerve.rotationKI, Constants.Swerve.rotationKD); - private final PIDDriveTransmuter pidTransmuter = new PIDDriveTransmuter(Constants.Swerve.driveKP, Constants.Swerve.driveKI, Constants.Swerve.driveKD); + private final PIDController thetaController = new PIDController(rotationKP, rotationKI, rotationKD); + private final PIDDriveTransmuter pidTransmuter = new PIDDriveTransmuter(driveKP, driveKI, driveKD); private static final double kSimLoopPeriod = 0.005; // 5 ms private Notifier m_simNotifier = null; @@ -265,12 +266,9 @@ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) public Command sysIdDynamic(SysIdRoutine.Direction direction) {return m_sysIdRoutineToApply.dynamic(direction);} - public Command poseLockDriveCommand(Supplier targetSupplier, Supplier swerveStateSup) + public Command poseDriveCommand(Supplier targetSupplier, Supplier swerveStateSup) { - final double maxSpeed = Constants.Swerve.maxSpeed; - - final var driveRequest = new SwerveRequest - .ApplyRobotSpeeds(); + final var driveRequest = new SwerveRequest.ApplyRobotSpeeds(); pidTransmuter.withTargetPoseSup(targetSupplier); @@ -281,7 +279,7 @@ public Command poseLockDriveCommand(Supplier targetSupplier, Supplier targetSupplier, Supplier FieldUtils.atPose(swerveStateSup.get().Pose, targetSupplier.get())); } @Override diff --git a/src/main/java/frc/robot/subsystems/CoralRoller.java b/src/main/java/frc/robot/subsystems/CoralRoller.java index 858494f..164ba58 100644 --- a/src/main/java/frc/robot/subsystems/CoralRoller.java +++ b/src/main/java/frc/robot/subsystems/CoralRoller.java @@ -6,7 +6,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.IDConstants; -import frc.robot.util.SD; public class CoralRoller extends SubsystemBase { @@ -19,29 +18,12 @@ public CoralRoller() io_Sensor = new DigitalInput(IDConstants.coralSensorDIO); } - public void setSpeed(double speed) - { - m_Coral.set(speed); - } - public boolean getSensor() - { - return io_Sensor.get(); - } + {return io_Sensor.get();} - public Command runCommand(double speed) - { - return startEnd(() -> setSpeed(speed), () -> setSpeed(0)); - } - - public Command smartRunCommand(double speed) - { - return runCommand(speed).until(this::getSensor); - } + public Command setSpeedCommand(double speed) + {return this.run(() -> m_Coral.set(speed));} @Override - public void periodic() - { - SD.CORAL_ROLLER.put(m_Coral.get()); - } + public void periodic() {} } diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java deleted file mode 100644 index bb5d115..0000000 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ /dev/null @@ -1,196 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import java.util.ArrayList; - -import com.ctre.phoenix6.Utils; - -import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Superstructure; -import frc.robot.constants.Constants; -import frc.robot.util.SD; -import frc.robot.util.LimelightHelpers; - -public class Limelight extends SubsystemBase -{ - private boolean useUpdate; - private LimelightHelpers.PoseEstimate mt2; - private static int[] validIDs = Constants.Vision.reefIDs; - private LimelightHelpers.PoseEstimate mt1; - - private double headingDeg; - private double omegaRps; - private double stdDevFactor; - private double linearStdDev; - private double rotStdDev; - - private final String limelightName; - - private int pipelineIndex = (int)SD.IO_LL_EXPOSURE.defaultValue(); - public static boolean rotationKnown = false; - private ArrayList rotationData = new ArrayList(); - private boolean lastCycleRotationKnown = false; - private final int mt1CyclesNeeded = 10; - - public enum TagPOI - { - REEF, - BARGE, - PROCESSOR, - CORALSTATION - } - - /** Creates a new Limelight. */ - public Limelight(String name) - { - limelightName = name; - LimelightHelpers.setPipelineIndex(limelightName, pipelineIndex); - } - - public void setIMUMode(int mode) - {LimelightHelpers.SetIMUMode(limelightName, mode);} - - public Rotation2d getLimelightRotation() - { - mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName); - - if (mt1 != null && mt1.avgTagDist < 4) - {return mt1.pose.getRotation();} - return Rotation2d.kZero; - } - - public void setThrottle(int throttle) - { - NetworkTableInstance.getDefault().getTable(limelightName).getEntry("").setNumber(throttle); - } - - public static void setActivePOI(TagPOI activePOI) - { - switch (activePOI) - { - default: - case REEF: - validIDs = Constants.Vision.reefIDs; - break; - case BARGE: - validIDs = Constants.Vision.bargeIDs; - break; - case PROCESSOR: - case CORALSTATION: - validIDs = Constants.Vision.humanPlayerStationIDs; - break; - } - } - - public int updateLimelightPipeline() - { - if (SD.IO_LL_EXPOSURE_UP.button()) - { - SD.IO_LL_EXPOSURE.put(MathUtil.clamp(SD.IO_LL_EXPOSURE.get().intValue() + 1, 0, 7)); - } - if (SD.IO_LL_EXPOSURE_DOWN.button()) - { - SD.IO_LL_EXPOSURE.put(MathUtil.clamp(SD.IO_LL_EXPOSURE.get().intValue() - 1, 0, 7)); - } - - return SD.IO_LL_EXPOSURE.get().intValue(); - } - - @Logged - public Pose3d getMT1Pose() - {return LimelightHelpers.getBotPose3d_wpiBlue(limelightName);} - - @Override - public void periodic() - { - if (Superstructure.isVisionActive()) - { - rotationKnown = Superstructure.isRotationKnown(); - - if (!rotationKnown) - { - lastCycleRotationKnown = false; - if (!getLimelightRotation().equals(Rotation2d.kZero)) - { - rotationData.add(0, getLimelightRotation().getDegrees()); - - if (rotationData.size() > mt1CyclesNeeded) - {rotationData.remove(mt1CyclesNeeded);} - - if (rotationData.size() == mt1CyclesNeeded) - { - double lowest = rotationData.get(0).doubleValue(); - double highest = rotationData.get(0).doubleValue(); - - for(int i = 1; i < mt1CyclesNeeded; i++) - { - lowest = Math.min(lowest, rotationData.get(i).doubleValue()); - highest = Math.max(highest, rotationData.get(i).doubleValue()); - } - - if (highest - lowest < 1) - { - rotationKnown = true; - Superstructure.setRotationKnown(true); - //SmartDashboard.putNumber("limelight " + limelightName + " average rotation reading", (highest + lowest) / 2); - Superstructure.setYaw((highest + lowest) / 2); - } - } - } - } - - if (!lastCycleRotationKnown) - { - if (rotationKnown) - { - rotationData.clear(); - lastCycleRotationKnown = true; - //RobotContainer.s_Swerve.resetPose(new Pose2d(RobotContainer.swerveState.Pose.getTranslation(), new Rotation2d(Math.toRadians(RobotContainer.s_Swerve.getPigeon2().getYaw().getValueAsDouble())))); - } - } - - if (updateLimelightPipeline() != pipelineIndex) - { - pipelineIndex = updateLimelightPipeline(); - LimelightHelpers.setPipelineIndex(limelightName, pipelineIndex); - } - - headingDeg = Superstructure.getYaw(); - //omegaRps = Units.radiansToRotations(RobotContainer.swerveState.Speeds.omegaRadiansPerSecond); - - LimelightHelpers.SetRobotOrientation(limelightName, headingDeg, 0, 0, 0, 0, 0); - - LimelightHelpers.SetFiducialIDFiltersOverride(limelightName, validIDs); - - if (SD.IO_LL.get()) //TODO: Replace MT1 references with MT2 when available - { - mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName); - //mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName); - - useUpdate = !(mt2 == null || mt2.tagCount == 0 || omegaRps > 2.0); - - if (useUpdate) - { - stdDevFactor = Math.pow(mt2.avgTagDist, 2.0) / mt2.tagCount; - - linearStdDev = Constants.Vision.linearStdDevBaseline * stdDevFactor; - rotStdDev = Constants.Vision.rotStdDevBaseline * stdDevFactor; - - Superstructure.setVisionMeasurementStdDevs(VecBuilder.fill(linearStdDev, linearStdDev, rotStdDev)); - Superstructure.addVisionMeasurement(mt2.pose, Utils.fpgaToCurrentTime(mt2.timestampSeconds)); - } - } - - SD.SENSOR_GYRO.put(headingDeg); - } - } -} diff --git a/src/main/java/frc/robot/subsystems/RumbleRequester.java b/src/main/java/frc/robot/subsystems/RumbleRequester.java index b213f87..d013e3e 100644 --- a/src/main/java/frc/robot/subsystems/RumbleRequester.java +++ b/src/main/java/frc/robot/subsystems/RumbleRequester.java @@ -1,8 +1,7 @@ package frc.robot.subsystems; -import java.util.ArrayList; -import java.util.function.Consumer; -import java.util.function.DoubleSupplier; +import java.util.HashSet; +import java.util.function.Supplier; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj2.command.Command; @@ -13,57 +12,45 @@ public class RumbleRequester extends SubsystemBase { - private ArrayList queue = new ArrayList(); + private HashSet queue = new HashSet(); private final CommandXboxController controller; private final RumbleType side; - private final DoubleSupplier strengthSup; - private final Consumer displayCns; + private final Supplier strengthSup; - public RumbleRequester(CommandXboxController controller, RumbleType side, Consumer displayCns, DoubleSupplier strengthSup) + public RumbleRequester(CommandXboxController controller, RumbleType side, Supplier strengthSup) { this.controller = controller; this.side = side; - this.displayCns = displayCns; this.strengthSup = strengthSup; } - private void addRequest(String requestID) - { - if(!queue.contains(requestID)) - {queue.add(requestID);} - } + private void add(String rumbleID) + {queue.add(rumbleID);} - private void removeRequest(String requestID) - {queue.remove(requestID);} + private void remove(String rumbleID) + {queue.remove(rumbleID);} + + public void clear() + {queue.clear();} - public Command requestCommand(boolean addRequest, String requestID) - {return addRequest ? Commands.runOnce(() -> addRequest(requestID)) : Commands.runOnce(() -> removeRequest(requestID));} + /** Returns a command that runs a rumble while it's running */ + public Command rumbleWhileCommand(String rumbleID) + {return Commands.startEnd(() -> add(rumbleID), () -> remove(rumbleID));} /** Adds a rumble with the provided ID that runs while the given trigger is true. Returns the subsystem for easier chaining. */ - public RumbleRequester addRumbleTrigger(String requestID, Trigger trigger) + public RumbleRequester addRumbleTrigger(String rumbleID, Trigger trigger) { - trigger.whileTrue(Commands.startEnd(() -> addRequest(requestID), () -> removeRequest(requestID))); + trigger.whileTrue(Commands.startEnd(() -> add(rumbleID), () -> remove(rumbleID))); return this; } - public void clearRequests() - {queue.clear();} - - public Command timedRequestCommand(String requestID, double durationSeconds) - { - return - Commands.sequence - ( - requestCommand(true, requestID), - Commands.waitSeconds(durationSeconds), - requestCommand(false, requestID) - ); - } + /** Returns a command that runs a rumble for the provided duration */ + public Command timedRequestCommand(String rumbleID, double durationSeconds) + {return rumbleWhileCommand(rumbleID).withDeadline(Commands.waitSeconds(durationSeconds));} @Override public void periodic() { - controller.setRumble(side, queue.isEmpty() ? 0 : strengthSup.getAsDouble()); - displayCns.accept(queue.toString()); + controller.setRumble(side, queue.isEmpty() ? 0 : strengthSup.get()); } } diff --git a/src/main/java/frc/robot/subsystems/vision/Limelight.java b/src/main/java/frc/robot/subsystems/vision/Limelight.java new file mode 100644 index 0000000..af89047 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/Limelight.java @@ -0,0 +1,47 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.vision; + +import java.util.Optional; + +import edu.wpi.first.math.geometry.Rotation2d; + +import frc.robot.util.libs.LimelightHelpers; +import frc.robot.util.libs.LimelightHelpers.PoseEstimate; + +public class Limelight +{ + private final String name; + + /** Creates a new Limelight. */ + public Limelight(String name) + {this.name = name;} + + public void setIMUMode(int mode) + {LimelightHelpers.SetIMUMode(name, mode);} + + public Optional getLimelightRotation() + { + var mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(name); + + return (mt1 != null && mt1.avgTagDist < 4) ? + Optional.of(mt1.pose.getRotation()) : + Optional.empty(); + } + + protected void updateValidIDs(int[] validIDs) + {LimelightHelpers.SetFiducialIDFiltersOverride(name, validIDs);} + + protected void updatePipeline(int pipelineIndex) + {LimelightHelpers.setPipelineIndex(name, pipelineIndex);} + + public PoseEstimate getMT2(double headingDeg) + { + LimelightHelpers.SetRobotOrientation(name, headingDeg, 0, 0, 0, 0, 0); + return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name); + } + + public void periodic() {} +} diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java new file mode 100644 index 0000000..ea73a2f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -0,0 +1,162 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.vision; + +import java.util.ArrayList; +import java.util.function.Supplier; + +import com.ctre.phoenix6.Utils; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import frc.robot.Robot; +import frc.robot.util.SD; +import static frc.robot.constants.Constants.Vision.*; + +public class Vision extends SubsystemBase +{ + public enum TagPOI {REEF, BARGE, PROCESSOR, CORALSTATION} + + private final PoseEstimateConsumer estimateConsumer; + private final Supplier> rotationDataSup; + private final Limelight[] lls; + + private int pipelineIndex = (int)SD.LL_EXPOSURE.defaultValue(); + + private ArrayList rotationBuf = new ArrayList(); + private boolean rotationKnown = false; + private boolean lastCycleRotationKnown = false; + + /** Creates a new Vision. */ + public Vision(PoseEstimateConsumer estimateConsumer, Supplier> rotationDataSup, Limelight... lls) + { + this.estimateConsumer = estimateConsumer; + this.rotationDataSup = rotationDataSup; + this.lls = lls; + setActivePOI(TagPOI.REEF); + } + + public void setActivePOI(TagPOI activePOI) + { + var validIDs = switch (activePOI) + { + case REEF -> reefIDs; + case BARGE -> bargeIDs; + case CORALSTATION, PROCESSOR -> humanPlayerStationIDs; + default -> reefIDs; + }; + + for (var ll : lls) ll.updateValidIDs(validIDs); + } + + public void incrementPipeline() + { + pipelineIndex = MathUtil.clamp(pipelineIndex + 1, 0, 7); + for (var ll : lls) {ll.updatePipeline(pipelineIndex);} + SD.LL_EXPOSURE.put((double)pipelineIndex); + } + + public void decrementPipeline() + { + pipelineIndex = MathUtil.clamp(pipelineIndex - 1, 0, 7); + for (var ll : lls) {ll.updatePipeline(pipelineIndex);} + SD.LL_EXPOSURE.put((double)pipelineIndex); + } + + public void resetRotation() {rotationKnown = false;} + + @Override + public void periodic() + { + if (SD.LL_TOGGLE.get()) + { + for (var ll : lls) + { + var rotationData = rotationDataSup.get(); + double heading = rotationData.getFirst(); + double omegaRps = rotationData.getSecond(); + + var mt2 = ll.getMT2(heading); + + boolean useUpdate = !(mt2 == null || mt2.tagCount == 0 || omegaRps > 2.0); + + if (useUpdate) + { + double stdDevFactor = Math.pow(mt2.avgTagDist, 2.0) / mt2.tagCount; + + double linearStdDev = linearStdDevBaseline * stdDevFactor; + double rotStdDev = rotStdDevBaseline * stdDevFactor; + + estimateConsumer.accept(mt2.pose, Utils.fpgaToCurrentTime(mt2.timestampSeconds), VecBuilder.fill(linearStdDev, linearStdDev, rotStdDev)); + } + } + } + + if (!rotationKnown) + { + lastCycleRotationKnown = false; + + for (var ll : lls) + { + ll.getLimelightRotation().ifPresent + ( + rotationReading -> + { + rotationBuf.add(0, rotationReading.getDegrees()); + + if (rotationBuf.size() > mt1CyclesNeeded) + {rotationBuf.remove(mt1CyclesNeeded);} + + if (rotationBuf.size() == mt1CyclesNeeded) + { + double lowest = rotationBuf.get(0).doubleValue(); + double highest = rotationBuf.get(0).doubleValue(); + + for(var reading : rotationBuf) + { + lowest = Math.min(lowest, reading.doubleValue()); + highest = Math.max(highest, reading.doubleValue()); + } + + if (highest - lowest < 1) + { + rotationKnown = true; + Robot.setYaw((highest + lowest) / 2); + } + } + } + ); + } + + if (!lastCycleRotationKnown) + { + if (rotationKnown) + { + rotationBuf.clear(); + lastCycleRotationKnown = true; + //RobotContainer.s_Swerve.resetPose(new Pose2d(RobotContainer.swerveState.Pose.getTranslation(), new Rotation2d(Math.toRadians(RobotContainer.s_Swerve.getPigeon2().getYaw().getValueAsDouble())))); + } + } + } + } + + @FunctionalInterface + public static interface PoseEstimateConsumer + { + public void accept + ( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs + ); + } +} diff --git a/src/main/java/frc/robot/util/AutoFactories.java b/src/main/java/frc/robot/util/AutoFactories.java index 57395a6..f7cd516 100644 --- a/src/main/java/frc/robot/util/AutoFactories.java +++ b/src/main/java/frc/robot/util/AutoFactories.java @@ -7,7 +7,6 @@ import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; @@ -30,7 +29,7 @@ public class AutoFactories */ public static Command getCommandList(String commandInput, CoralRoller s_Coral, CommandSwerveDrivetrain s_Swerve, Supplier swerveStateSup) { - // Removes all space characters from the single-String command phrases, ensures it's all lowercase, and then splits it into individual strings, which are stored in an array + // Removes all whitespace characters from the single-String command phrases, ensures it's all lowercase, and then splits it into individual strings, which are stored in an array String[] splitCommands = commandInput.replaceAll("//s", "").toLowerCase().split(","); // The arraylist that all the commands will be placed into ArrayList commandList = new ArrayList<>(); @@ -40,7 +39,8 @@ public static Command getCommandList(String commandInput, CoralRoller s_Coral, C { switch (splitCommand.charAt(0)) { - case 'g': + case 'g' -> + { int seperatorIndex = splitCommand.indexOf(":"); Translation2d posTarget = @@ -55,47 +55,32 @@ public static Command getCommandList(String commandInput, CoralRoller s_Coral, C new Rotation2d(Units.degreesToRadians(Double.parseDouble(splitCommand.substring(splitCommand.indexOf(";"))))) : swerveStateSup.get().Pose.getRotation().plus(Rotation2d.k180deg); - commandList.add(s_Swerve.poseLockDriveCommand(new AlliancePose2dSup(posTarget, rotationTarget), swerveStateSup)); - break; + commandList.add(s_Swerve.poseDriveCommand(new AlliancePose2dSup(posTarget, rotationTarget), swerveStateSup)); + } - case 'w': - commandList.add(Commands.waitSeconds(Double.parseDouble(splitCommand.substring(1)))); - break; + case 'w' -> commandList.add(Commands.waitSeconds(Double.parseDouble(splitCommand.substring(1)))); - case 't': + case 't' -> + { double targetMatchTimeElapsed = Double.parseDouble(splitCommand.substring(1)); - commandList.add(Commands.waitUntil(() -> Timer.getMatchTime() < (15 - targetMatchTimeElapsed))); - break; + } - case 'r': - commandList.add(s_Swerve.poseLockDriveCommand(new AlliancePose2dSup(FieldConstants.getLineup(splitCommand)), swerveStateSup)); + case 'r' -> + { + commandList.add(s_Swerve.poseDriveCommand(new AlliancePose2dSup(FieldConstants.getLineup(splitCommand)), swerveStateSup)); commandList.add(Commands.waitSeconds(0.1)); - commandList.add(s_Coral.smartRunCommand(Constants.Coral.forwardSpeed)); - break; + commandList.add(s_Coral.setSpeedCommand(Constants.Coral.forwardSpeed).until(s_Coral::getSensor)); + } - case 'c': - commandList.add(s_Swerve.poseLockDriveCommand(new AlliancePose2dSup(FieldConstants.getLineup(splitCommand)), swerveStateSup)); + case 'c' -> + { + commandList.add(s_Swerve.poseDriveCommand(new AlliancePose2dSup(FieldConstants.getLineup(splitCommand)), swerveStateSup)); commandList.add(Commands.waitUntil(s_Coral::getSensor)); - break; - - default: - break; + } } } return new SequentialCommandGroup(commandList.toArray(Command[]::new)).withInterruptBehavior(InterruptionBehavior.kCancelIncoming); } - - public static void displayPose(Pose2d pose, Rotation2d rotation) - { - SD.IO_POSE_X.put(pose.getX()); - SD.IO_POSE_Y.put(pose.getY()); - SD.IO_POSE_R.put(rotation.getDegrees()); - } - - public static void displayPose(Pose2d pose) - { - displayPose(pose, pose.getRotation()); - } } diff --git a/src/main/java/frc/robot/util/FieldUtils.java b/src/main/java/frc/robot/util/FieldUtils.java index 1c36f7b..0cf2fd9 100644 --- a/src/main/java/frc/robot/util/FieldUtils.java +++ b/src/main/java/frc/robot/util/FieldUtils.java @@ -1,9 +1,6 @@ package frc.robot.util; import java.util.ArrayList; import java.util.Arrays; -import java.util.Optional; - -import com.pathplanner.lib.path.PathPlannerPath; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -19,7 +16,7 @@ public class FieldUtils { public static boolean isRedAlliance() { - Optional alliance = DriverStation.getAlliance(); + var alliance = DriverStation.getAlliance(); return alliance.isPresent() && alliance.get() == Alliance.Red; } @@ -62,18 +59,10 @@ public static Pose2d rotatePose(Pose2d pose) return pose; } - public static void activateAllianceFencing() + public static void activateAllianceFencing(boolean redAlliance) { - if (isRedAlliance()) - { - GeoFencing.fieldRedGeoFence.setActiveCondition(() -> true); - GeoFencing.fieldBlueGeoFence.setActiveCondition(() -> false); - } - else - { - GeoFencing.fieldBlueGeoFence.setActiveCondition(() -> true); - GeoFencing.fieldRedGeoFence.setActiveCondition(() -> false); - } + GeoFencing.fieldRedGeoFence.setActiveCondition(() -> redAlliance); + GeoFencing.fieldBlueGeoFence.setActiveCondition(() -> !redAlliance); } public static int getNearestReefFaceAllianceLocked(Translation2d robotPos) @@ -114,22 +103,13 @@ public static Line getNearestCoralStation(Translation2d robotPos) northHalf ? GeoFencing.cornerNBlue : GeoFencing.cornerSBlue; } - public static PathPlannerPath loadPath(String pathName) - { - try { - PathPlannerPath path = PathPlannerPath.fromPathFile(pathName); - return path; - } catch (Exception e) { - DriverStation.reportError(String.format("Unable to load path: %s", pathName), true); - } - return null; - } + public static boolean atReefLineUp(Pose2d robotPose) + {return Arrays.stream(FieldConstants.reefLineups).anyMatch(lineup -> atPose(robotPose, lineup));} - public static boolean atReefLineUp(Translation2d robotPos) + public static boolean atPose(Pose2d robotPose, Pose2d targetPose) { - return - Arrays.stream(FieldConstants.reefLineups) - .anyMatch(lineup -> Math.abs(lineup.getTranslation().minus(robotPos).getNorm()) < Constants.Control.lineupTolerance); + + return robotPose.getTranslation().getDistance(targetPose.getTranslation()) < Constants.Control.lineupTolerance && + Math.abs(robotPose.getRotation().getDegrees() - targetPose.getRotation().getDegrees()) < Constants.Control.angleLineupTolerance; } - } \ No newline at end of file diff --git a/src/main/java/frc/robot/util/SD.java b/src/main/java/frc/robot/util/SD.java index 27a4089..1d7767f 100644 --- a/src/main/java/frc/robot/util/SD.java +++ b/src/main/java/frc/robot/util/SD.java @@ -4,7 +4,8 @@ package frc.robot.util; -import java.util.Set; +import java.util.function.Consumer; +import java.util.function.Supplier; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; @@ -15,60 +16,28 @@ /** Simplified interface for most SmartDashboard interactions */ public class SD { - public static final BooleanKey IO_LL = new BooleanKey("Use Limelight", true); - public static final DoubleKey IO_LL_EXPOSURE = new DoubleKey("Exposure Setting", 0); - public static final BooleanKey IO_LL_EXPOSURE_UP = new BooleanKey("Increase Exposure", false); - public static final BooleanKey IO_LL_EXPOSURE_DOWN = new BooleanKey("Decrease Exposure", false); + public static final StringKey AUTO_STRING = new StringKey("Auto String", ""); - public static final StringKey STATE_HEADING = new StringKey("Heading State", ""); - public static final StringKey STATE_DRIVE = new StringKey("Drive State", "Disabled"); - public static final BooleanKey STATE_HEADING_SNAP = new BooleanKey("Heading Snap Updating", true); + public static final DoubleKey LL_EXPOSURE = new DoubleKey("Exposure Setting", 0); + public static final BooleanKey LL_EXPOSURE_UP = new BooleanKey("Increase Exposure", false); + public static final BooleanKey LL_EXPOSURE_DOWN = new BooleanKey("Decrease Exposure", false); + public static final BooleanKey LL_TOGGLE = new BooleanKey("Use Limelight", true); - public static final BooleanKey IO_GEOFENCE = new BooleanKey("Use Fence", true); - public static final BooleanKey IO_OUTER_GEOFENCE = new BooleanKey("Wall Fence", true); - public static final DoubleKey IO_GEOFENCE_IMPACT = new DoubleKey("Fence Impact", 1); - public static final DoubleKey IO_RUMBLE_D = new DoubleKey("Driver Rumble", Constants.RumblerConstants.driverDefault); - public static final DoubleKey IO_RUMBLE_C = new DoubleKey("Copilot Rumble", Constants.RumblerConstants.copilotDefault); + public static final StringKey STATE_HEADING = new StringKey("Heading State", ""); + public static final StringKey STATE_DRIVE = new StringKey("Drive State", "Disabled"); - public static final DoubleKey SENSOR_GYRO = new DoubleKey("Gyro yaw", 0); - - public static final StringKey RUMBLE_D_R = new StringKey("DriverRight Rumble Queue", ""); - public static final StringKey RUMBLE_D_L = new StringKey("DriverLeft Rumble Queue", ""); - public static final StringKey RUMBLE_C_R = new StringKey("CopilotRight Rumble Queue", ""); - public static final StringKey RUMBLE_C_L = new StringKey("CopilotLeft Rumble Queue", ""); - - public static final DoubleKey IO_POSE_X = new DoubleKey("Pose X", 0.0); - public static final DoubleKey IO_POSE_Y = new DoubleKey("Pose Y", 0.0); - public static final DoubleKey IO_POSE_R = new DoubleKey("Pose Rotation", 0.0); - public static final StringKey IO_AUTO = new StringKey("Auto String", ""); - - public static final DoubleKey IO_CORALSPEED_F = new DoubleKey("Coral Roller Forward Speed", Constants.Coral.forwardSpeed); - public static final DoubleKey IO_CORALSPEED_R = new DoubleKey("Coral Roller Reverse Speed", Constants.Coral.reverseSpeed); - public static final DoubleKey IO_ALGAESPEED_F = new DoubleKey("Algae Roller Forward Speed", Constants.Algae.forwardSpeed); - public static final DoubleKey IO_ALGAESPEED_R = new DoubleKey("Algae Roller Reverse Speed", Constants.Algae.reverseSpeed); - - public static final DoubleKey CORAL_ROLLER = new DoubleKey("Current Coral Roller Speed", 0); - public static final DoubleKey ALGAE_ROLLER = new DoubleKey("Current Algae Roller Speed", 0); + public static final DoubleKey RUMBLE_DRIVER = new DoubleKey("Driver Rumble", Constants.RumblerConstants.driverDefault); + public static final DoubleKey RUMBLE_OPERATOR = new DoubleKey("Operator Rumble", Constants.RumblerConstants.operatorDefault); static { - for - ( - Initable key : - Set.of - ( - IO_POSE_X, - IO_POSE_Y, - IO_POSE_R, - IO_GEOFENCE, - IO_AUTO, - IO_CORALSPEED_F, - IO_CORALSPEED_R - ) - ) + try { - key.init(); - } + for (var field : SD.class.getFields()) + { + if (field.get(null) instanceof Key key) key.init(); + } + } catch (Exception e) {/* This will verifiably never happen */} } public static void initSwerveDisplay(CommandSwerveDrivetrain s_Swerve) @@ -102,18 +71,24 @@ public void initSendable(SendableBuilder builder) ); } - public interface Initable + private interface Key extends Supplier, Consumer { + public default void accept(T value) {put(value);} + + public T get(); + + public void put(T value); + public void init(); } - public record BooleanKey (String label, boolean defaultValue) implements Initable + public record BooleanKey (String label, boolean defaultValue) implements Key { - public boolean get() {return SmartDashboard.getBoolean(label, defaultValue);} + public Boolean get() {return SmartDashboard.getBoolean(label, defaultValue);} public boolean button() { - if (SmartDashboard.getBoolean(label, defaultValue)) + if (get()) { put(false); return true; @@ -124,19 +99,19 @@ public boolean button() public void init() {SmartDashboard.putBoolean(label, defaultValue);} - public void put(boolean value) {SmartDashboard.putBoolean(label, value);} + public void put(Boolean value) {SmartDashboard.putBoolean(label, value);} } - public record DoubleKey (String label, double defaultValue) implements Initable + public record DoubleKey (String label, double defaultValue) implements Key { public Double get() {return SmartDashboard.getNumber(label, defaultValue);} public void init() {SmartDashboard.putNumber(label, defaultValue);} - public void put(double value) {SmartDashboard.putNumber(label, value);} + public void put(Double value) {SmartDashboard.putNumber(label, value);} } - public record StringKey (String label, String defaultValue) implements Initable + public record StringKey (String label, String defaultValue) implements Key { public String get() {return SmartDashboard.getString(label, defaultValue);} diff --git a/src/main/java/frc/robot/util/controlTransmutation/Attractor.java b/src/main/java/frc/robot/util/controlTransmutation/Attractor.java index 0134c7d..8bbcd71 100644 --- a/src/main/java/frc/robot/util/controlTransmutation/Attractor.java +++ b/src/main/java/frc/robot/util/controlTransmutation/Attractor.java @@ -4,6 +4,8 @@ import edu.wpi.first.math.geometry.Translation2d; import frc.robot.util.Conversions; +import static frc.robot.constants.FieldConstants.AutoDrive.*; + /** Guides the robot towards a point along a given heading */ public class Attractor extends FieldObject { @@ -15,10 +17,6 @@ public class Attractor extends FieldObject protected Translation2d frontCheckpoint; /** Point opposite where the approach heading intersects the effect radius */ protected Translation2d backCheckpoint; - /** Minimum angle tollerance, degrees */ - protected static final double minAngleTollerance = 20; - /** Maximum angle tollerance, degrees */ - protected static final double maxAngleTollerance = 60; /** Scalar for how far to back off from the target when approaching from the side */ protected double approachScalar = 0.1; /** Input scale for approaching within the buffer based on distance */ @@ -94,7 +92,7 @@ public boolean checkAngle(Translation2d controlInput) if ( !lastInputAngle.equals(Rotation2d.kZero) && - Conversions.isRotationNear(lastInputAngle, controlInput.getAngle(), minAngleTollerance) + Conversions.isRotationNear(lastInputAngle, controlInput.getAngle(), minAngleTolerance) ) { return true; @@ -102,12 +100,12 @@ public boolean checkAngle(Translation2d controlInput) if (distance <= buffer) { - return Conversions.isRotationNear(approachHeadingRotation, controlInput.getAngle(), maxAngleTollerance); + return Conversions.isRotationNear(approachHeadingRotation, controlInput.getAngle(), maxAngleTolerance); } Rotation2d angleToTarget = centre.minus(robotPos).getAngle(); - double angleTolerance = Conversions.clamp(2*Math.atan(buffer/distance), minAngleTollerance, maxAngleTollerance); + double angleTolerance = Conversions.clamp(2*Math.atan(buffer/distance), minAngleTolerance, maxAngleTolerance); return Conversions.isRotationNear(angleToTarget, controlInput.getAngle(), angleTolerance); } diff --git a/src/main/java/frc/robot/util/controlTransmutation/FieldObject.java b/src/main/java/frc/robot/util/controlTransmutation/FieldObject.java index aed6eab..71425d6 100644 --- a/src/main/java/frc/robot/util/controlTransmutation/FieldObject.java +++ b/src/main/java/frc/robot/util/controlTransmutation/FieldObject.java @@ -30,17 +30,6 @@ public abstract class FieldObject extends InputTransmuter /** Condition for the object to be active, if the return is false the object will return the input */ protected BooleanSupplier activeSupplier = () -> true; - /** - * Global minimum value for object radius, metres

- * The system is not confirmed to handle negative radii - */ - protected static final double minRadius = 0; - /** - * Global minimum value for object buffer, metres

- * A small buffer is required to ensure safe transitions - */ - protected static final double minBuffer = 0.1; - /** * Sets the global robot position supplier for all field objects * @param robotPosSupplier Translation2d Supplier for the robot position (not pose) diff --git a/src/main/java/frc/robot/util/controlTransmutation/JoystickTransmuter.java b/src/main/java/frc/robot/util/controlTransmutation/JoystickTransmuter.java index 3335598..aa7c89b 100644 --- a/src/main/java/frc/robot/util/controlTransmutation/JoystickTransmuter.java +++ b/src/main/java/frc/robot/util/controlTransmutation/JoystickTransmuter.java @@ -41,7 +41,7 @@ public JoystickTransmuter(DoubleSupplier inputX, DoubleSupplier inputY) brake = new Brake(); fieldObjectList = new ObjectList(); - stickOutputSup = () -> stickOutput(); + stickOutputSup = this::stickOutput; } /** diff --git a/src/main/java/frc/robot/util/controlTransmutation/Restrictor.java b/src/main/java/frc/robot/util/controlTransmutation/Restrictor.java index 1fbbe77..b7a554c 100644 --- a/src/main/java/frc/robot/util/controlTransmutation/Restrictor.java +++ b/src/main/java/frc/robot/util/controlTransmutation/Restrictor.java @@ -4,6 +4,8 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import static frc.robot.constants.FieldConstants.GeoFencing.*; + /** * Derived from GeoFence logic, acts as a non-directional speed-limit within the given area

* Using a local speed limit <= 0 allows it to be used as a position/distance check @@ -11,7 +13,6 @@ public class Restrictor extends FieldObject { protected double localSpeedLimit; - protected static final double minLocalSpeedLimit = 0.05; /** * Basic point-type restrictor object diff --git a/src/main/java/frc/robot/util/controlTransmutation/geoFence/Box.java b/src/main/java/frc/robot/util/controlTransmutation/geoFence/Box.java index 3525dd6..9b911dd 100644 --- a/src/main/java/frc/robot/util/controlTransmutation/geoFence/Box.java +++ b/src/main/java/frc/robot/util/controlTransmutation/geoFence/Box.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.geometry.Translation2d; import frc.robot.util.Conversions; import frc.robot.util.controlTransmutation.GeoFence; +import static frc.robot.constants.FieldConstants.GeoFencing.*; /** * Box type GeoFence object

diff --git a/src/main/java/frc/robot/util/controlTransmutation/geoFence/Fence.java b/src/main/java/frc/robot/util/controlTransmutation/geoFence/Fence.java index 5967237..8630703 100644 --- a/src/main/java/frc/robot/util/controlTransmutation/geoFence/Fence.java +++ b/src/main/java/frc/robot/util/controlTransmutation/geoFence/Fence.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.geometry.Translation2d; import frc.robot.util.Conversions; import frc.robot.util.controlTransmutation.GeoFence; +import static frc.robot.constants.FieldConstants.GeoFencing.*; /** * Fence type GeoFence object

diff --git a/src/main/java/frc/robot/util/controlTransmutation/geoFence/Line.java b/src/main/java/frc/robot/util/controlTransmutation/geoFence/Line.java index 85862ad..1257680 100644 --- a/src/main/java/frc/robot/util/controlTransmutation/geoFence/Line.java +++ b/src/main/java/frc/robot/util/controlTransmutation/geoFence/Line.java @@ -11,6 +11,7 @@ import frc.robot.util.Conversions; import frc.robot.util.controlTransmutation.Attractor; import frc.robot.util.controlTransmutation.GeoFence; +import static frc.robot.constants.FieldConstants.GeoFencing.*; /** * Line type GeoFence object

diff --git a/src/main/java/frc/robot/util/controlTransmutation/geoFence/Point.java b/src/main/java/frc/robot/util/controlTransmutation/geoFence/Point.java index 86519fb..82a6758 100644 --- a/src/main/java/frc/robot/util/controlTransmutation/geoFence/Point.java +++ b/src/main/java/frc/robot/util/controlTransmutation/geoFence/Point.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Translation2d; import frc.robot.util.controlTransmutation.GeoFence; +import static frc.robot.constants.FieldConstants.GeoFencing.*; /** * Point type GeoFence object

diff --git a/src/main/java/frc/robot/util/controlTransmutation/geoFence/Polygon.java b/src/main/java/frc/robot/util/controlTransmutation/geoFence/Polygon.java index 4baecc9..885471e 100644 --- a/src/main/java/frc/robot/util/controlTransmutation/geoFence/Polygon.java +++ b/src/main/java/frc/robot/util/controlTransmutation/geoFence/Polygon.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.geometry.Translation2d; import frc.robot.util.Conversions; import frc.robot.util.controlTransmutation.GeoFence; +import static frc.robot.constants.FieldConstants.GeoFencing.*; /** * Polygon type GeoFence object

diff --git a/src/main/java/frc/robot/util/controlTransmutation/restrictor/Box.java b/src/main/java/frc/robot/util/controlTransmutation/restrictor/Box.java index 673905f..40f0bc4 100644 --- a/src/main/java/frc/robot/util/controlTransmutation/restrictor/Box.java +++ b/src/main/java/frc/robot/util/controlTransmutation/restrictor/Box.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Translation2d; import frc.robot.util.controlTransmutation.Restrictor; +import static frc.robot.constants.FieldConstants.GeoFencing.*; /** Add your docs here. */ public class Box extends Restrictor diff --git a/src/main/java/frc/robot/util/controlTransmutation/restrictor/Polygon.java b/src/main/java/frc/robot/util/controlTransmutation/restrictor/Polygon.java index 9cbff9a..9fb451a 100644 --- a/src/main/java/frc/robot/util/controlTransmutation/restrictor/Polygon.java +++ b/src/main/java/frc/robot/util/controlTransmutation/restrictor/Polygon.java @@ -10,6 +10,7 @@ import edu.wpi.first.math.geometry.Translation2d; import frc.robot.util.Conversions; import frc.robot.util.controlTransmutation.Restrictor; +import static frc.robot.constants.FieldConstants.GeoFencing.*; /** Add your docs here. */ public class Polygon extends Restrictor diff --git a/src/main/java/frc/robot/util/LimelightHelpers.java b/src/main/java/frc/robot/util/libs/LimelightHelpers.java similarity index 99% rename from src/main/java/frc/robot/util/LimelightHelpers.java rename to src/main/java/frc/robot/util/libs/LimelightHelpers.java index b6431e6..d4d6fd1 100644 --- a/src/main/java/frc/robot/util/LimelightHelpers.java +++ b/src/main/java/frc/robot/util/libs/LimelightHelpers.java @@ -1,6 +1,6 @@ //LimelightHelpers v1.11 (REQUIRES LLOS 2025.0 OR LATER) -package frc.robot.util; +package frc.robot.util.libs; import edu.wpi.first.networktables.DoubleArrayEntry; import edu.wpi.first.networktables.NetworkTable; diff --git a/src/main/java/frc/robot/util/Telemetry.java b/src/main/java/frc/robot/util/libs/Telemetry.java similarity index 99% rename from src/main/java/frc/robot/util/Telemetry.java rename to src/main/java/frc/robot/util/libs/Telemetry.java index 4d1296c..cb7d226 100644 --- a/src/main/java/frc/robot/util/Telemetry.java +++ b/src/main/java/frc/robot/util/libs/Telemetry.java @@ -1,4 +1,4 @@ -package frc.robot.util; +package frc.robot.util.libs; import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; diff --git a/vendordeps/PathplannerLib-2025.2.7.json b/vendordeps/PathplannerLib-2025.2.7.json deleted file mode 100644 index d3f84e5..0000000 --- a/vendordeps/PathplannerLib-2025.2.7.json +++ /dev/null @@ -1,38 +0,0 @@ -{ - "fileName": "PathplannerLib-2025.2.7.json", - "name": "PathplannerLib", - "version": "2025.2.7", - "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "frcYear": "2025", - "mavenUrls": [ - "https://3015rangerrobotics.github.io/pathplannerlib/repo" - ], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", - "javaDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-java", - "version": "2025.2.7" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-cpp", - "version": "2025.2.7", - "libName": "PathplannerLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxathena", - "linuxarm32", - "linuxarm64" - ] - } - ] -} \ No newline at end of file