From 2b7bb1224195b9e001e60b895ee04d63abdfc513 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 17 Jul 2025 09:15:19 -0600 Subject: [PATCH] Attempt to implement advantagekit for CTRE swerve --- src/main/java/frc4388/robot/Robot.java | 12 +- .../java/frc4388/robot/RobotContainer.java | 28 +-- src/main/java/frc4388/robot/RobotMap.java | 160 +++++++++++------- .../robot/commands/MoveForTimeCommand.java | 2 +- .../robot/commands/MoveUntilSuply.java | 2 +- .../robot/commands/alignment/DriveToReef.java | 2 +- .../commands/alignment/DriveUntilLiDAR.java | 2 +- .../robot/commands/alignment/LidarAlign.java | 2 +- .../robot/commands/swerve/RotateToAngle.java | 2 +- .../commands/swerve/neoJoystickPlayback.java | 2 +- .../commands/swerve/neoJoystickRecorder.java | 2 +- .../robot/constants/BuildConstants.java | 10 +- .../frc4388/robot/subsystems/DiffDrive.java | 4 +- .../frc4388/robot/subsystems/lidar/LiDAR.java | 3 + .../subsystems/{ => swerve}/SwerveDrive.java | 158 ++++++++--------- .../swerve/SwerveDriveConstants.java} | 4 +- .../robot/subsystems/swerve/SwerveIO.java | 33 ++++ .../subsystems/swerve/SwervePhoenix.java | 65 +++++++ .../robot/subsystems/vision/Vision.java | 19 ++- .../utility/compute/TimesNegativeOne.java | 18 +- 20 files changed, 326 insertions(+), 204 deletions(-) rename src/main/java/frc4388/robot/subsystems/{ => swerve}/SwerveDrive.java (73%) rename src/main/java/frc4388/robot/{constants/DriveConstants.java => subsystems/swerve/SwerveDriveConstants.java} (99%) create mode 100644 src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java create mode 100644 src/main/java/frc4388/robot/subsystems/swerve/SwervePhoenix.java diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 0a31049..c147692 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -205,13 +205,13 @@ public class Robot extends LoggedRobot { // } - @Override - public void simulationPeriodic() { - m_robotContainer.m_robotSwerveDrive.updateSim(RobotController.getBatteryVoltage()); - // visionSim.update(m_robotContainer.m_robotSwerveDrive.getPose2d()); + // @Override + // public void simulationPeriodic() { + // m_robotContainer.m_robotSwerveDrive.updateSim(RobotController.getBatteryVoltage()); + // // visionSim.update(m_robotContainer.m_robotSwerveDrive.getPose2d()); - // m_robotContainer.m_robotSwerveDrive. - } + // // m_robotContainer.m_robotSwerveDrive. + // } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index aeb3808..4a6f4d8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -48,6 +48,7 @@ import frc4388.robot.constants.Constants; import frc4388.robot.constants.Constants.AutoConstants; import frc4388.robot.constants.Constants.LiDARConstants; import frc4388.robot.constants.Constants.OIConstants; +import frc4388.robot.constants.Constants.SimConstants.Mode; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; @@ -56,8 +57,8 @@ import frc4388.robot.subsystems.Elevator; // Subsystems import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Elevator.CoordinationState; -// import frc4388.robot.subsystems.Endeffector; -import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.lidar.LiDAR; +import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; // Utilites import frc4388.utility.DeferredBlock; @@ -74,7 +75,7 @@ import frc4388.utility.compute.ReefPositionHelper.Side; public class RobotContainer { /* RobotMap */ - public final RobotMap m_robotMap = new RobotMap(); + public final RobotMap m_robotMap = new RobotMap(Mode.REAL); /* Subsystems */ public final LED m_robotLED = new LED(); @@ -83,6 +84,9 @@ public class RobotContainer { public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); + public final LiDAR reefLidar = new LiDAR(m_robotMap.reefLidar, "Reef"); + public final LiDAR reverseLidar = new LiDAR(m_robotMap.reverseLidar, "Reverse"); + /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); @@ -129,7 +133,7 @@ public class RobotContainer { new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), + new LidarAlign(m_robotSwerveDrive, reefLidar), waitDebuger.asProxy(), new ParallelRaceGroup( new WaitCommand(1), @@ -292,7 +296,7 @@ public class RobotContainer { new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), + new LidarAlign(m_robotSwerveDrive, reefLidar), waitDebuger.asProxy(), new ParallelRaceGroup( new WaitCommand(1), @@ -339,7 +343,7 @@ public class RobotContainer { new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), + new LidarAlign(m_robotSwerveDrive, reefLidar), // waitDebuger.asProxy(), // new ParallelRaceGroup( // new WaitCommand(1), @@ -380,7 +384,7 @@ public class RobotContainer { new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE-Units.inchesToMeters(1), Side.LEFT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), + new LidarAlign(m_robotSwerveDrive, reefLidar), waitDebuger.asProxy(), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator), new waitEndefectorRefrence(m_robotElevator), @@ -409,7 +413,7 @@ public class RobotContainer { new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), + new LidarAlign(m_robotSwerveDrive, reefLidar), waitDebuger.asProxy(), // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator), @@ -429,7 +433,7 @@ public class RobotContainer { new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), + new LidarAlign(m_robotSwerveDrive, reefLidar), waitDebuger.asProxy(), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), @@ -450,7 +454,7 @@ public class RobotContainer { new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT, true), waitDebuger.asProxy(), - new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar), + new LidarAlign(m_robotSwerveDrive, reefLidar), waitDebuger.asProxy(), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator), new waitEndefectorRefrence(m_robotElevator), @@ -558,7 +562,7 @@ public class RobotContainer { NamedCommands.registerCommand("await-coral", new waitFeedCoral(m_robotElevator)); NamedCommands.registerCommand("feed-driveback", new DriveUntilLiDAR(m_robotSwerveDrive, - new Translation2d(-1,0), new Translation2d(), m_robotMap.reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE)); + new Translation2d(-1,0), new Translation2d(), reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE)); // NamedCommands.registerCommand("feed-driveback", Commands.none()); NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.isStopped())); @@ -722,7 +726,7 @@ public class RobotContainer { .onTrue(thrustIntake.asProxy()); new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive, m_robotMap.reefLidar)); + .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive, reefLidar)); // ? /* Operator Buttons */ diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 1e0cb87..45eb62a 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -19,14 +19,18 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DigitalInput; import frc4388.robot.constants.Constants.ElevatorConstants; import frc4388.robot.constants.Constants.LiDARConstants; +import frc4388.robot.constants.Constants.SimConstants; import frc4388.robot.constants.Constants.VisionConstants; -import frc4388.robot.constants.DriveConstants; import frc4388.robot.subsystems.lidar.LiDAR; import frc4388.robot.subsystems.lidar.LidarIO; import frc4388.robot.subsystems.lidar.LidarLiteV2; +import frc4388.robot.subsystems.swerve.SwerveDriveConstants; +import frc4388.robot.subsystems.swerve.SwerveIO; +import frc4388.robot.subsystems.swerve.SwervePhoenix; import frc4388.robot.subsystems.vision.VisionIO; import frc4388.robot.subsystems.vision.VisionPhotonvision; import frc4388.utility.status.FaultCANCoder; +import frc4388.utility.status.FaultPhotonCamera; import frc4388.utility.status.FaultPidgeon2; import frc4388.utility.status.FaultTalonFX; @@ -38,24 +42,20 @@ public class RobotMap { // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id); // public RobotGyro gyro = new RobotGyro(m_pigeon2); - public final VisionIO leftCamera = new VisionPhotonvision(new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME), VisionConstants.LEFT_CAMERA_POS); - public final VisionIO rightCamera = new VisionPhotonvision(new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME), VisionConstants.RIGHT_CAMERA_POS); + public final VisionIO leftCamera; + public final VisionIO rightCamera; // public final LiDAR lidar = new - public final LiDAR reefLidar = new LiDAR((LidarIO) new LidarLiteV2(LiDARConstants.REEF_LIDAR_DIO_CHANNEL), "Reef"); - public final LiDAR reverseLidar = new LiDAR((LidarIO) new LidarLiteV2(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL), "Reverse"); + public final LidarIO reefLidar; + public final LidarIO reverseLidar; /* LED Subsystem */ // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); /* Swreve Drive Subsystem */ - public final SwerveDrivetrain swerveDrivetrain = new SwerveDrivetrain (TalonFX::new, TalonFX::new, CANcoder::new, - DriveConstants.DrivetrainConstants, - DriveConstants.FRONT_LEFT, DriveConstants.FRONT_RIGHT, - DriveConstants.BACK_LEFT, DriveConstants.BACK_RIGHT - ); + public final SwerveIO swerveDrivetrain; /* Elevator Subsystem */ public final TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id); @@ -66,71 +66,101 @@ public class RobotMap { public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); public final DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); - void configureDriveMotorControllers() { - // endeffector.saf + public RobotMap(SimConstants.Mode mode) { + switch (mode) { + case REAL: + // Configure cameras + PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME); + PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME); + + leftCamera = new VisionPhotonvision(leftCameraReal, VisionConstants.LEFT_CAMERA_POS); ; + rightCamera = new VisionPhotonvision(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS); + + FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera"); + FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera"); + + // Configure LiDAR + reefLidar = new LidarLiteV2(LiDARConstants.REEF_LIDAR_DIO_CHANNEL); + reverseLidar = new LidarLiteV2(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL); + + // Configure swerve drive train + + SwerveDrivetrain swerveDrivetrainReal = new SwerveDrivetrain (TalonFX::new, TalonFX::new, CANcoder::new, + SwerveDriveConstants.DrivetrainConstants, + SwerveDriveConstants.FRONT_LEFT, SwerveDriveConstants.FRONT_RIGHT, + SwerveDriveConstants.BACK_LEFT, SwerveDriveConstants.BACK_RIGHT + ); + + swerveDrivetrain = new SwervePhoenix(swerveDrivetrainReal); + + + + FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro"); + + FaultTalonFX.addDevice(elevator, "Elevator"); + FaultTalonFX.addDevice(endeffector, "Endeffector"); + + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer"); + FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(0).getEncoder(), "Module 0 CANCoder"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getDriveMotor(), "Module 1 Drive"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getSteerMotor(), "Module 1 Steer"); + FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(1).getEncoder(), "Module 1 CANCoder"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getDriveMotor(), "Module 2 Drive"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getSteerMotor(), "Module 2 Steer"); + FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(2).getEncoder(), "Module 2 CANCoder"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getDriveMotor(), "Module 3 Drive"); + FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getSteerMotor(), "Module 3 Steer"); + FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder"); + + break; + // case SIM: + // break; + default: + leftCamera = new VisionIO() {}; + rightCamera = new VisionIO() {}; + reefLidar = new LidarIO() {}; + reverseLidar = new LidarIO() {}; + swerveDrivetrain = new SwerveIO() {}; + break; + } } + // public class RobotMapSim { + // public PhotonCameraSim leftCamera; + // public PhotonCameraSim rightCamera; + // } - public RobotMap() { - configureDriveMotorControllers(); + // public RobotMapSim configureSim() { + // RobotMapSim sim = new RobotMapSim(); - // FaultPhotonCamera.addDevice(leftCamera, "Left Camera"); - // FaultPhotonCamera.addDevice(rightCamera, "Right Camera"); + // // The simulated camera properties + // SimCameraProperties cameraProp = new SimCameraProperties(); + // // A 640 x 480 camera with a 100 degree diagonal FOV. + // cameraProp.setCalibration(640, 480, Rotation2d.fromDegrees(100)); + // // Approximate detection noise with average and standard deviation error in pixels. + // cameraProp.setCalibError(0.25, 0.08); + // // Set the camera image capture framerate (Note: this is limited by robot loop rate). + // cameraProp.setFPS(20); + // // The average and standard deviation in milliseconds of image data latency. + // cameraProp.setAvgLatencyMs(35); + // cameraProp.setLatencyStdDevMs(5); - FaultPidgeon2.addDevice(swerveDrivetrain.getPigeon2(), "Gyro"); - - FaultTalonFX.addDevice(elevator, "Elevator"); - FaultTalonFX.addDevice(endeffector, "Endeffector"); - - FaultTalonFX.addDevice(swerveDrivetrain.getModule(0).getDriveMotor(), "Module 0 Drive"); - FaultTalonFX.addDevice(swerveDrivetrain.getModule(0).getSteerMotor(), "Module 0 Steer"); - FaultCANCoder.addDevice(swerveDrivetrain.getModule(0).getEncoder(), "Module 0 CANCoder"); - FaultTalonFX.addDevice(swerveDrivetrain.getModule(1).getDriveMotor(), "Module 1 Drive"); - FaultTalonFX.addDevice(swerveDrivetrain.getModule(1).getSteerMotor(), "Module 1 Steer"); - FaultCANCoder.addDevice(swerveDrivetrain.getModule(1).getEncoder(), "Module 1 CANCoder"); - FaultTalonFX.addDevice(swerveDrivetrain.getModule(2).getDriveMotor(), "Module 2 Drive"); - FaultTalonFX.addDevice(swerveDrivetrain.getModule(2).getSteerMotor(), "Module 2 Steer"); - FaultCANCoder.addDevice(swerveDrivetrain.getModule(2).getEncoder(), "Module 2 CANCoder"); - FaultTalonFX.addDevice(swerveDrivetrain.getModule(3).getDriveMotor(), "Module 3 Drive"); - FaultTalonFX.addDevice(swerveDrivetrain.getModule(3).getSteerMotor(), "Module 3 Steer"); - FaultCANCoder.addDevice(swerveDrivetrain.getModule(3).getEncoder(), "Module 3 CANCoder"); - } - - public class RobotMapSim { - public PhotonCameraSim leftCamera; - public PhotonCameraSim rightCamera; - } - - public RobotMapSim configureSim() { - RobotMapSim sim = new RobotMapSim(); - - // The simulated camera properties - SimCameraProperties cameraProp = new SimCameraProperties(); - // A 640 x 480 camera with a 100 degree diagonal FOV. - cameraProp.setCalibration(640, 480, Rotation2d.fromDegrees(100)); - // Approximate detection noise with average and standard deviation error in pixels. - cameraProp.setCalibError(0.25, 0.08); - // Set the camera image capture framerate (Note: this is limited by robot loop rate). - cameraProp.setFPS(20); - // The average and standard deviation in milliseconds of image data latency. - cameraProp.setAvgLatencyMs(35); - cameraProp.setLatencyStdDevMs(5); - - // sim.leftCamera = new PhotonCameraSim(leftCamera, cameraProp); - // sim.rightCamera = new PhotonCameraSim(rightCamera, cameraProp); + // // sim.leftCamera = new PhotonCameraSim(leftCamera, cameraProp); + // // sim.rightCamera = new PhotonCameraSim(rightCamera, cameraProp); - sim.leftCamera.enableRawStream(true); - sim.leftCamera.enableProcessedStream(true); - sim.leftCamera.enableDrawWireframe(true); + // sim.leftCamera.enableRawStream(true); + // sim.leftCamera.enableProcessedStream(true); + // sim.leftCamera.enableDrawWireframe(true); - sim.rightCamera.enableRawStream(true); - sim.rightCamera.enableProcessedStream(true); - sim.rightCamera.enableDrawWireframe(true); + // sim.rightCamera.enableRawStream(true); + // sim.rightCamera.enableProcessedStream(true); + // sim.rightCamera.enableDrawWireframe(true); - return sim; + // return sim; - } + // } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java b/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java index 6a085ec..3a4e043 100644 --- a/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java +++ b/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java @@ -4,7 +4,7 @@ import java.time.Instant; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.swerve.SwerveDrive; // Command to repeat a joystick movement for a specific time. public class MoveForTimeCommand extends Command { diff --git a/src/main/java/frc4388/robot/commands/MoveUntilSuply.java b/src/main/java/frc4388/robot/commands/MoveUntilSuply.java index aa1b210..c5b5e53 100644 --- a/src/main/java/frc4388/robot/commands/MoveUntilSuply.java +++ b/src/main/java/frc4388/robot/commands/MoveUntilSuply.java @@ -4,7 +4,7 @@ import java.util.function.Supplier; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.swerve.SwerveDrive; // Command to repeat a joystick movement for a specific time. public class MoveUntilSuply extends Command { diff --git a/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java b/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java index 3d13052..6a11e85 100644 --- a/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java +++ b/src/main/java/frc4388/robot/commands/alignment/DriveToReef.java @@ -6,7 +6,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.constants.Constants.AutoConstants; -import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.compute.ReefPositionHelper; import frc4388.utility.compute.TimesNegativeOne; diff --git a/src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java b/src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java index d6ca489..648a7d8 100644 --- a/src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java +++ b/src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java @@ -2,8 +2,8 @@ package frc4388.robot.commands.alignment; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.lidar.LiDAR; +import frc4388.robot.subsystems.swerve.SwerveDrive; // Command to repeat a joystick movement for a specific time. public class DriveUntilLiDAR extends Command { diff --git a/src/main/java/frc4388/robot/commands/alignment/LidarAlign.java b/src/main/java/frc4388/robot/commands/alignment/LidarAlign.java index 668d3fa..19efd85 100644 --- a/src/main/java/frc4388/robot/commands/alignment/LidarAlign.java +++ b/src/main/java/frc4388/robot/commands/alignment/LidarAlign.java @@ -8,8 +8,8 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.lidar.LiDAR; +import frc4388.robot.subsystems.swerve.SwerveDrive; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class LidarAlign extends Command { diff --git a/src/main/java/frc4388/robot/commands/swerve/RotateToAngle.java b/src/main/java/frc4388/robot/commands/swerve/RotateToAngle.java index fea8b77..50e616c 100644 --- a/src/main/java/frc4388/robot/commands/swerve/RotateToAngle.java +++ b/src/main/java/frc4388/robot/commands/swerve/RotateToAngle.java @@ -6,7 +6,7 @@ package frc4388.robot.commands.swerve; import edu.wpi.first.math.geometry.Translation2d; import frc4388.robot.commands.PID; -import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.swerve.SwerveDrive; public class RotateToAngle extends PID { diff --git a/src/main/java/frc4388/robot/commands/swerve/neoJoystickPlayback.java b/src/main/java/frc4388/robot/commands/swerve/neoJoystickPlayback.java index f1c865e..bff5105 100644 --- a/src/main/java/frc4388/robot/commands/swerve/neoJoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/swerve/neoJoystickPlayback.java @@ -6,7 +6,7 @@ import java.util.function.Supplier; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.utility.compute.DataUtils; import frc4388.utility.controller.VirtualController; import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame; diff --git a/src/main/java/frc4388/robot/commands/swerve/neoJoystickRecorder.java b/src/main/java/frc4388/robot/commands/swerve/neoJoystickRecorder.java index 1df53b6..4cf3159 100644 --- a/src/main/java/frc4388/robot/commands/swerve/neoJoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/swerve/neoJoystickRecorder.java @@ -7,7 +7,7 @@ import java.util.function.Supplier; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.utility.compute.DataUtils; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame; diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index f131ea0..f1e17b4 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025RidgeScape"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 171; - public static final String GIT_SHA = "3130f647c83cc82b45a5299e19108f9eec45e6f6"; - public static final String GIT_DATE = "2025-07-15 11:07:01 MDT"; + public static final int GIT_REVISION = 172; + public static final String GIT_SHA = "8e34bfe35427e1fb86ab2af21dd706d387bf4874"; + public static final String GIT_DATE = "2025-07-15 13:42:25 MDT"; public static final String GIT_BRANCH = "advantagekit"; - public static final String BUILD_DATE = "2025-07-15 13:40:35 MDT"; - public static final long BUILD_UNIX_TIME = 1752608435113L; + public static final String BUILD_DATE = "2025-07-16 16:20:33 MDT"; + public static final long BUILD_UNIX_TIME = 1752704433011L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/DiffDrive.java b/src/main/java/frc4388/robot/subsystems/DiffDrive.java index df9f29e..4aff045 100644 --- a/src/main/java/frc4388/robot/subsystems/DiffDrive.java +++ b/src/main/java/frc4388/robot/subsystems/DiffDrive.java @@ -13,7 +13,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.constants.DriveConstants; +import frc4388.robot.subsystems.swerve.SwerveDriveConstants; // import frc4388.utility.RobotGyro; import frc4388.utility.compute.RobotTime; import frc4388.utility.status.Status; @@ -56,7 +56,7 @@ public class DiffDrive extends SubsystemBase implements Queryable { @Override public void periodic() { - if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) { + if (m_robotTime.m_frameNumber % SwerveDriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) { updateSmartDashboard(); } } diff --git a/src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java b/src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java index dbbb024..2945c2d 100644 --- a/src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java +++ b/src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java @@ -1,5 +1,7 @@ package frc4388.robot.subsystems.lidar; +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants.LiDARConstants; import frc4388.utility.status.Status; @@ -23,6 +25,7 @@ public class LiDAR extends SubsystemBase implements Queryable { @Override public void periodic() { io.updateInputs(state); + Logger.processInputs("LiDAR/"+name, state); } // @AutoLogOutput(key = "Lidar/{name}") diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java similarity index 73% rename from src/main/java/frc4388/robot/subsystems/SwerveDrive.java rename to src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index e37de34..4a5f4cc 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -2,7 +2,7 @@ // 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 frc4388.robot.subsystems; +package frc4388.robot.subsystems.swerve; import java.util.Optional; @@ -22,7 +22,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants.AutoConstants; -import frc4388.robot.constants.DriveConstants; +import frc4388.robot.subsystems.swerve.SwerveIO.SwerveState; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.compute.TimesNegativeOne; import frc4388.utility.status.Status; @@ -36,18 +36,22 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.RobotConfig; public class SwerveDrive extends SubsystemBase implements Queryable { - private SwerveDrivetrain swerveDriveTrain; + // private SwerveDrivetrain swerveDriveTrain; + + private SwerveIO io; + private SwerveStateAutoLogged state; + private Vision vision; - // @AutoLog - // public class SwerveDriveState { - public int gear_index = DriveConstants.STARTING_GEAR; + + + public int gear_index = SwerveDriveConstants.STARTING_GEAR; public boolean stopped = false; public boolean robotKnowsWhereItIs = false; - public double speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * DriveConstants.GEARS[gear_index]; - public double rotSpeedAdjust = DriveConstants.MAX_ROT_SPEED; - public double autoSpeedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to + public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index]; + public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED; + public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to // 25% public double lastOdomSpeed; @@ -58,17 +62,16 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public double rotTarget = 0.0; public Rotation2d orientRotTarget = new Rotation2d(); public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); - // } - - // public SwerveDriveState state = new SwerveDriveState(); /** Creates a new SwerveDrive. */ - public SwerveDrive(SwerveDrivetrain swerveDriveTrain, Vision vision) { + public SwerveDrive(SwerveIO swerveDriveTrain, Vision vision) { // public SwerveDrive(SwerveDrivetrain // swerveDriveTrain) { FaultReporter.register(this); - this.swerveDriveTrain = swerveDriveTrain; + this.io = swerveDriveTrain; + this.state = new SwerveStateAutoLogged(); + this.vision = vision; RobotConfig config; @@ -81,12 +84,12 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // DoubleSupplier a = () -> 1.d; AutoBuilder.configure( () -> { - return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(initalPose2d); + return getPose2d(); }, // Robot pose supplier this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting // pose) - () -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - (speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds() + () -> state.speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + (speeds, feedforwards) -> io.setControl(new SwerveRequest.ApplyRobotSpeeds() .withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. // Also optionally outputs individual module feedforwards new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for @@ -139,7 +142,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public void setOdoPose(Pose2d pose) { if (pose == null) return; initalPose2d = pose; - swerveDriveTrain.resetPose(pose); + io.resetPose(pose); } // public void oneModuleTest(SwerveModule module, Translation2d leftStick, @@ -172,12 +175,15 @@ public class SwerveDrive extends SubsystemBase implements Queryable { rightStick = TimesNegativeOne.invert(rightStick, TimesNegativeOne.RotAxis); // ! drift correction - if (rightStick.getNorm() > 0.05 || !DriveConstants.DRIFT_CORRECTION_ENABLED) { - rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees(); - swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() + if (rightStick.getNorm() > 0.05 || !SwerveDriveConstants.DRIFT_CORRECTION_ENABLED) { + + rotTarget = state.currentPose.getRotation().getDegrees(); + + io.setControl(new SwerveRequest.FieldCentric() .withVelocityX(leftStick.getX() * speedAdjust) .withVelocityY(leftStick.getY() * speedAdjust) .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); + // .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective)); SmartDashboard.putBoolean("drift correction", false); } else { @@ -186,17 +192,17 @@ public class SwerveDrive extends SubsystemBase implements Queryable { .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(Rotation2d.fromDegrees(rotTarget)); ctrl.HeadingController.setPID( - DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP, - DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI, - DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD + SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP, + SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI, + SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD ); - swerveDriveTrain.setControl(ctrl); + io.setControl(ctrl); SmartDashboard.putBoolean("drift correction", true); } } else { // Create robot-relative speeds. - swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() + io.setControl(new SwerveRequest.RobotCentric() .withVelocityX(leftStick.getX() * speedAdjust) .withVelocityY(-leftStick.getY() * speedAdjust) .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); @@ -207,9 +213,9 @@ public class SwerveDrive extends SubsystemBase implements Queryable { stopped = false; // Create robot-relative speeds. if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0); - swerveDriveTrain.setControl(new SwerveRequest.RobotCentric() - .withVelocityX(leftStick.getX() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) - .withVelocityY(-leftStick.getY() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) + io.setControl(new SwerveRequest.RobotCentric() + .withVelocityX(leftStick.getX() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) + .withVelocityY(-leftStick.getY() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput) .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); } @@ -229,7 +235,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { leftStick.rotateBy(TimesNegativeOne.ForwardOffset); - swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() + io.setControl(new SwerveRequest.FieldCentricFacingAngle() .withVelocityX(leftStick.getX() * speedAdjust) .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(rightStick.getAngle())); @@ -243,11 +249,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable { .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(heading); ctrl.HeadingController.setPID( - DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, - DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, - DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD + SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, + SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, + SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD ); - swerveDriveTrain.setControl(ctrl); + io.setControl(ctrl); } public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) { @@ -262,31 +268,19 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, // DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD // ); - swerveDriveTrain.setControl(ctrl); - } - - public void setLimits(double limitInAmps) { - for (SwerveModule module : swerveDriveTrain.getModules()) { - var talonFXConfigurator = module.getDriveMotor().getConfigurator(); - var talonFXConfigs = new TalonFXConfiguration(); - - talonFXConfigurator.refresh(talonFXConfigs); - talonFXConfigs.CurrentLimits.StatorCurrentLimit = limitInAmps; - talonFXConfigs.CurrentLimits.SupplyCurrentLimit = limitInAmps+10; - talonFXConfigurator.apply(talonFXConfigs); - } + io.setControl(ctrl); } public void activateLuigiMode() { - setLimits(20); + io.setLimits(20); } public void deactivateLuigiMode() { - setLimits(DriveConstants.Configurations.SLIP_CURRENT); + io.setLimits(SwerveDriveConstants.Configurations.SLIP_CURRENT); } public boolean rotateToTarget(double angle) { - swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() + io.setControl(new SwerveRequest.FieldCentricFacingAngle() .withVelocityX(0) .withVelocityY(0) .withTargetDirection(Rotation2d.fromDegrees(angle))); @@ -312,7 +306,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); - swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() + io.setControl(new SwerveRequest.FieldCentricFacingAngle() .withVelocityX(leftStick.getX() * -speedAdjust) .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(rot)); @@ -324,11 +318,13 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } public Pose2d getPose2d() { - return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(initalPose2d); + if(state.currentPose == null) + return initalPose2d; + return state.currentPose; } public void resetGyro() { - swerveDriveTrain.tareEverything(); + io.tareEverything(); robotKnowsWhereItIs = false; rotTarget = 0; // vision.resetRotations(); @@ -337,7 +333,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public void softStop() { stopped = true; - swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() + io.setControl(new SwerveRequest.FieldCentric() .withVelocityX(0) .withVelocityY(0) .withRotationalRate(0) @@ -356,14 +352,10 @@ public class SwerveDrive extends SubsystemBase implements Queryable { SmartDashboard.putNumber("Gyro", (getGyroAngle() * 180) / Math.PI); SmartDashboard.putNumber("RotTartget", rotTarget); - double time = Vision.getTime(); - double freq = swerveDriveTrain.getOdometryFrequency(); - - Optional curpose = swerveDriveTrain.samplePoseAt(time); - Optional lastPose = swerveDriveTrain.samplePoseAt(time - freq); + io.updateInputs(state); - vision.setLastOdomPose(curpose); - setLastOdomSpeed(curpose, lastPose, freq); + vision.setLastOdomPose(state.currentPose); + setLastOdomSpeed(state.currentPose, state.lastPose, state.frequency); if (vision.isTag()) { Pose2d pose = vision.getPose2d(); @@ -373,66 +365,66 @@ public class SwerveDrive extends SubsystemBase implements Queryable { rotTarget += pose.getRotation().getDegrees() - curPose.getRotation().getDegrees(); } - vision.addVisionMeasurement(swerveDriveTrain); + io.addVisionMeasurement(vision.getPosesToAdd()); } // if(e.isPresent()) } private void reset_index() { - gear_index = DriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the + gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the // robot start in?) } public void shiftDown() { - if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length) + if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index int i = gear_index - 1; if (i == -1) i = 0; - setPercentOutput(DriveConstants.GEARS[i]); + setPercentOutput(SwerveDriveConstants.GEARS[i]); gear_index = i; } public void shiftUp() { - if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length) + if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index int i = gear_index + 1; - if (i == DriveConstants.GEARS.length) - i = DriveConstants.GEARS.length - 1; - setPercentOutput(DriveConstants.GEARS[i]); + if (i == SwerveDriveConstants.GEARS.length) + i = SwerveDriveConstants.GEARS.length - 1; + setPercentOutput(SwerveDriveConstants.GEARS[i]); gear_index = i; } public void setPercentOutput(double speed) { - speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; + speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; gear_index = -1; } public void setToSlow() { - setPercentOutput(DriveConstants.SLOW_SPEED); + setPercentOutput(SwerveDriveConstants.SLOW_SPEED); gear_index = 0; } public void setToFast() { - setPercentOutput(DriveConstants.FAST_SPEED); + setPercentOutput(SwerveDriveConstants.FAST_SPEED); gear_index = 1; } public void setToTurbo() { - setPercentOutput(DriveConstants.TURBO_SPEED); + setPercentOutput(SwerveDriveConstants.TURBO_SPEED); gear_index = 2; } public void shiftUpRot() { - rotSpeedAdjust = DriveConstants.ROTATION_SPEED; + rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED; } public void shiftDownRot() { - rotSpeedAdjust = DriveConstants.MIN_ROT_SPEED; + rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; } - private int tmp_gear_index = DriveConstants.STARTING_GEAR; + private int tmp_gear_index = SwerveDriveConstants.STARTING_GEAR; public void startSlowPeriod() { tmp_gear_index = gear_index; @@ -445,15 +437,15 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } public void endSlowPeriod() { - setPercentOutput(DriveConstants.GEARS[tmp_gear_index]); + setPercentOutput(SwerveDriveConstants.GEARS[tmp_gear_index]); gear_index = tmp_gear_index; } - public void setLastOdomSpeed(Optional curPose, Optional lastPose, double freq){ - if(curPose.isPresent() && lastPose.isPresent()){ - lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq; + public void setLastOdomSpeed(Pose2d curPose, Pose2d lastPose, double freq){ + if(curPose != null && lastPose != null){ + lastOdomSpeed = curPose.getTranslation().getDistance(lastPose.getTranslation())/freq; } } @@ -473,11 +465,5 @@ public class SwerveDrive extends SubsystemBase implements Queryable { return status; } - - - // Update CTRE simulation, if used. - public void updateSim(double voltage) { - swerveDriveTrain.updateSimState(0.02, voltage); - } } diff --git a/src/main/java/frc4388/robot/constants/DriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java similarity index 99% rename from src/main/java/frc4388/robot/constants/DriveConstants.java rename to src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java index 0021eae..35b02db 100644 --- a/src/main/java/frc4388/robot/constants/DriveConstants.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -1,4 +1,4 @@ -package frc4388.robot.constants; +package frc4388.robot.subsystems.swerve; import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Rotations; @@ -28,7 +28,7 @@ import frc4388.utility.structs.Gains; // No mans land // Beware, there be dragons. -public final class DriveConstants { +public final class SwerveDriveConstants { public static final double MAX_ROT_SPEED = Math.PI * 2; public static final double AUTO_MAX_ROT_SPEED = 1.5; public static final double MIN_ROT_SPEED = 1.0; diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java new file mode 100644 index 0000000..5debd5a --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java @@ -0,0 +1,33 @@ +package frc4388.robot.subsystems.swerve; + +import java.util.List; + +import org.littletonrobotics.junction.AutoLog; + +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; + +public interface SwerveIO { + @AutoLog + public class SwerveState { + public Pose2d currentPose = null; + public Pose2d lastPose = null; + public ChassisSpeeds speeds = null; + public double frequency = 1; + } + + public default void setControl(SwerveRequest ctrl) {} + + public default void setLimits(double limitInAmps) {} + + public default void tareEverything() {} + + public default void resetPose(Pose2d pose) {} + + public default void addVisionMeasurement(List poses) {} + + public default void updateInputs(SwerveState state) {} +} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwervePhoenix.java b/src/main/java/frc4388/robot/subsystems/swerve/SwervePhoenix.java new file mode 100644 index 0000000..84bbcd9 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwervePhoenix.java @@ -0,0 +1,65 @@ +package frc4388.robot.subsystems.swerve; + +import java.util.List; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.swerve.SwerveDrivetrain; +import com.ctre.phoenix6.swerve.SwerveModule; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.math.geometry.Pose2d; +import frc4388.robot.subsystems.lidar.LidarIO.LidarState; +import frc4388.robot.subsystems.vision.Vision; +import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; + +public class SwervePhoenix implements SwerveIO { + SwerveDrivetrain swerveDriveTrain; + + public SwervePhoenix(SwerveDrivetrain swerveDriveTrain) { + this.swerveDriveTrain = swerveDriveTrain; + swerveDriveTrain.getOdometryFrequency(); + } + + @Override + public void updateInputs(SwerveState state) { + double time = Vision.getTime(); + state.frequency = swerveDriveTrain.getOdometryFrequency(); + state.currentPose = swerveDriveTrain.samplePoseAt(time).orElse(null); + state.lastPose = swerveDriveTrain.samplePoseAt(time - state.frequency).orElse(null); + state.speeds = swerveDriveTrain.getState().Speeds; + } + + @Override + public void setControl(SwerveRequest ctrl) { + swerveDriveTrain.setControl(ctrl); + } + + @Override + public void tareEverything() { + swerveDriveTrain.tareEverything(); + } + + @Override + public void setLimits(double limitInAmps) { + for (SwerveModule module : swerveDriveTrain.getModules()) { + var talonFXConfigurator = module.getDriveMotor().getConfigurator(); + var talonFXConfigs = new TalonFXConfiguration(); + + talonFXConfigurator.refresh(talonFXConfigs); + talonFXConfigs.CurrentLimits.StatorCurrentLimit = limitInAmps; + talonFXConfigs.CurrentLimits.SupplyCurrentLimit = limitInAmps+10; + talonFXConfigurator.apply(talonFXConfigs); + } + } + + @Override + public void addVisionMeasurement(List poses) { + for(PoseObservation pose : poses) { + swerveDriveTrain.addVisionMeasurement(pose.pose(), Utils.fpgaToCurrentTime(pose.timestamp())); + } + } + +} diff --git a/src/main/java/frc4388/robot/subsystems/vision/Vision.java b/src/main/java/frc4388/robot/subsystems/vision/Vision.java index d24ee3f..a069af7 100644 --- a/src/main/java/frc4388/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/vision/Vision.java @@ -1,5 +1,7 @@ package frc4388.robot.subsystems.vision; +import java.util.ArrayList; +import java.util.List; import java.util.Optional; import org.littletonrobotics.junction.AutoLogOutput; @@ -43,21 +45,20 @@ public class Vision extends SubsystemBase implements Queryable { } } - public void addVisionMeasurement(SwerveDrivetrain drivetrain){ - // for(EstimatedRobotPose pose : poses){ - // - // } + public List getPosesToAdd(){ + List poses = new ArrayList<>(); for(int i = 0; i < state.length; i++) { if(state[i].lastEstimatedPose != null) { - PoseObservation pose = state[i].lastEstimatedPose; - drivetrain.addVisionMeasurement(pose.pose(), Utils.fpgaToCurrentTime(pose.timestamp())); + poses.add(state[i].lastEstimatedPose); } } + + return poses; } - public void setLastOdomPose(Optional pose){ - if(pose.isPresent()) - lastPhysOdomPose = pose.get(); + public void setLastOdomPose(Pose2d pose){ + if(pose != null) + lastPhysOdomPose = pose; } public boolean isTag(){ diff --git a/src/main/java/frc4388/utility/compute/TimesNegativeOne.java b/src/main/java/frc4388/utility/compute/TimesNegativeOne.java index 9b7da0e..2ba0d55 100644 --- a/src/main/java/frc4388/utility/compute/TimesNegativeOne.java +++ b/src/main/java/frc4388/utility/compute/TimesNegativeOne.java @@ -7,16 +7,16 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc4388.robot.constants.DriveConstants; +import frc4388.robot.subsystems.swerve.SwerveDriveConstants; // Class that holds weather the drivers sticks should be inverted public class TimesNegativeOne { - public static boolean XAxis = DriveConstants.INVERT_X; - public static boolean YAxis = DriveConstants.INVERT_Y; - public static boolean RotAxis = DriveConstants.INVERT_ROTATION; + public static boolean XAxis = SwerveDriveConstants.INVERT_X; + public static boolean YAxis = SwerveDriveConstants.INVERT_Y; + public static boolean RotAxis = SwerveDriveConstants.INVERT_ROTATION; public static boolean isRed = false; - public static Rotation2d ForwardOffset = Rotation2d.fromDegrees(DriveConstants.FORWARD_OFFSET); + public static Rotation2d ForwardOffset = Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET); private static boolean isRed() { Optional alliance = DriverStation.getAlliance(); @@ -26,10 +26,10 @@ public class TimesNegativeOne { public static void update(){ isRed = isRed(); - XAxis = DriveConstants.INVERT_X ^ isRed; - YAxis = DriveConstants.INVERT_Y ^ isRed; - RotAxis = DriveConstants.INVERT_ROTATION; - ForwardOffset = Rotation2d.fromDegrees((DriveConstants.FORWARD_OFFSET + (isRed ? 0 : 0))); + XAxis = SwerveDriveConstants.INVERT_X ^ isRed; + YAxis = SwerveDriveConstants.INVERT_Y ^ isRed; + RotAxis = SwerveDriveConstants.INVERT_ROTATION; + ForwardOffset = Rotation2d.fromDegrees((SwerveDriveConstants.FORWARD_OFFSET + (isRed ? 0 : 0))); SmartDashboard.putBoolean("Is red alliance", isRed); }