diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9e33d5d..5d4111c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -19,6 +19,7 @@ import static edu.wpi.first.units.Units.*; +import com.ctre.phoenix6.CANBus; import com.pathplanner.lib.config.ModuleConfig; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; @@ -45,6 +46,9 @@ import frc.robot.util.RBSIEnum.SwerveType; import frc.robot.util.RBSIEnum.VisionType; import frc.robot.util.RobotDeviceId; +import java.util.Collections; +import java.util.HashMap; +import java.util.Map; import org.photonvision.simulation.SimCameraProperties; import swervelib.math.Matter; @@ -161,13 +165,56 @@ public static final class PowerConstants { public static final double kVoltageCritical = 6.5; } + /************************************************************************* */ + /** List of Robot CAN Busses ********************************************* */ + public static final class CANBuses { + + // ---- Bus names (single source of truth) ---- + public static final String DRIVE = "DriveTrain"; + public static final String RIO = ""; + // In 2027 and later, you'll be able to have even more CAN Busses! + + // ---- Singleton instances (exactly one per bus) ---- + public static final CANBus DRIVE_BUS = new CANBus(DRIVE); + public static final CANBus RIO_BUS = new CANBus(RIO); + + // ---- Lookup table: name -> CANBus singleton ---- + private static final Map BY_NAME; + + static { + Map m = new HashMap<>(); + m.put(DRIVE, DRIVE_BUS); + m.put(RIO, RIO_BUS); + BY_NAME = Collections.unmodifiableMap(m); + } + + /** + * Get the singleton CANBus for a given bus name. + * + *

Usage: CANBus bus = Constants.CANBuses.get(Constants.CANBuses.DRIVE); + */ + public static CANBus get(String name) { + CANBus bus = BY_NAME.get(name); + if (bus == null) { + throw new IllegalArgumentException( + "Unknown CAN bus name '" + name + "'. Known buses: " + BY_NAME.keySet()); + } + return bus; + } + + /** Expose known bus names for debugging. */ + public static Map all() { + return BY_NAME; + } + } + /************************************************************************* */ /** List of Robot Device CAN and Power Distribution Circuit IDs ********** */ public static class RobotDevices { /* DRIVETRAIN CAN DEVICE IDS */ // Input the correct Power Distribution Module port for each motor!!!! - // NOTE: The CAN ID and bus are set in the Swerve Generator (Phoenix Tuner or YAGSL) + // NOTE: The CAN ID's are set in the Swerve Generator (Phoenix Tuner or YAGSL) // Front Left public static final RobotDeviceId FL_DRIVE = @@ -204,8 +251,8 @@ public static class RobotDevices { /* SUBSYSTEM CAN DEVICE IDS */ // This is where mechanism subsystem devices are defined (Including ID, bus, and power port) // Example: - public static final RobotDeviceId FLYWHEEL_LEADER = new RobotDeviceId(3, "", 8); - public static final RobotDeviceId FLYWHEEL_FOLLOWER = new RobotDeviceId(4, "", 9); + public static final RobotDeviceId FLYWHEEL_LEADER = new RobotDeviceId(3, CANBuses.RIO, 8); + public static final RobotDeviceId FLYWHEEL_FOLLOWER = new RobotDeviceId(4, CANBuses.RIO, 9); /* BEAM BREAK and/or LIMIT SWITCH DIO CHANNELS */ // This is where digital I/O feedback devices are defined @@ -262,19 +309,27 @@ public static final class DrivebaseConstants { // of YOUR ROBOT, and replace the estimate here with your measured value! public static final double kMaxLinearSpeed = Feet.of(18).in(Meters); - // Set 3/4 of a rotation per second as the max angular velocity (radians/sec) - public static final double kMaxAngularSpeed = 1.5 * Math.PI; + // Slip Current -- the current draw when the wheels start to slip + // Measure this against a wall. CHECK WITH THE CARPET AT AN ACTUAL EVENT!!! + public static final double kSlipCurrent = 20.0; // Amps + + // Characterized Wheel Radius (using the "Drive Wheel Radius Characterization" auto routine) + public static final double kWheelRadiusMeters = Inches.of(2.000).in(Meters); // Maximum chassis accelerations desired for robot motion -- metric / radians // TODO: Compute the maximum linear acceleration given the PHYSICS of the ROBOT! public static final double kMaxLinearAccel = 4.0; // m/s/s - public static final double kMaxAngularAccel = Degrees.of(720).in(Radians); - // For Profiled PID Motion, these are the rotational PID Constants: - // TODO: Need tuning! - public static final double kPTheta = 5.0; - public static final double kITheta = 0.0; - public static final double kDTheta = 0.0; + // For Profiled PID Motion -- NEED TUNING! + // Used in a variety of contexts, including PathPlanner and AutoPilot + // Chassis (not module) across-the-field strafing motion + public static final double kPStrafe = 5.0; + public static final double kIStrafe = 0.0; + public static final double kDStrafe = 0.0; + // Chassis (not module) solid-body rotation + public static final double kPSPin = 5.0; + public static final double kISPin = 0.0; + public static final double kDSpin = 0.0; // Hold time on motor brakes when disabled public static final double kWheelLockTime = 10; // seconds @@ -326,17 +381,21 @@ public static final class FlywheelConstants { // MODE == REAL / REPLAY // Feedforward constants - public static final double kStaticGainReal = 0.1; - public static final double kVelocityGainReal = 0.05; + public static final double kSreal = 0.1; + public static final double kVreal = 0.05; + public static final double kAreal = 0.0; // Feedback (PID) constants - public static final PIDConstants pidReal = new PIDConstants(1.0, 0.0, 0.0); + public static final double kPreal = 1.0; + public static final double kDreal = 0.0; // MODE == SIM // Feedforward constants - public static final double kStaticGainSim = 0.0; - public static final double kVelocityGainSim = 0.03; + public static final double kSsim = 0.0; + public static final double kVsim = 0.03; + public static final double kAsim = 0.0; // Feedback (PID) constants - public static final PIDConstants pidSim = new PIDConstants(1.0, 0.0, 0.0); + public static final double kPsim = 0.0; + public static final double kDsim = 0.0; } /************************************************************************* */ @@ -350,21 +409,17 @@ public static final class FlywheelConstants { public static final class AutoConstants { // ********** PATHPLANNER CONSTANTS ******************* - // Drive and Turn PID constants used for PathPlanner - public static final PIDConstants kPPdrivePID = new PIDConstants(5.0, 0.0, 0.0); - public static final PIDConstants kPPsteerPID = new PIDConstants(5.0, 0.0, 0.0); - // PathPlanner Config constants public static final RobotConfig kPathPlannerConfig = new RobotConfig( RobotConstants.kRobotMass.in(Kilograms), RobotConstants.kRobotMOI, new ModuleConfig( - SwerveConstants.kWheelRadiusMeters, + DrivebaseConstants.kWheelRadiusMeters, DrivebaseConstants.kMaxLinearSpeed, RobotConstants.kWheelCOF, DCMotor.getKrakenX60Foc(1).withReduction(SwerveConstants.kDriveGearRatio), - SwerveConstants.kDriveSlipCurrent, + DrivebaseConstants.kSlipCurrent, 1), Drive.getModuleTranslations()); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 83d5b19..e9034e2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -112,35 +112,60 @@ public Robot() { // Create a timer to disable motor brake a few seconds after disable. This will let the robot // stop immediately when disabled, but then also let it be pushed more m_disabledTimer = new Timer(); + + // Switch thread to high priority to improve loop timing + if (isReal()) { + Threads.setCurrentThreadPriority(true, 99); + } } - /** This function is called periodically during all modes. */ + // /** This function is called periodically during all modes. */ + // @Override + // public void robotPeriodic() { + + // // Run all virtual subsystems each time through the loop + // VirtualSubsystem.periodicAll(); + + // // Runs the Scheduler. This is responsible for polling buttons, adding + // // newly-scheduled commands, running already-scheduled commands, removing + // // finished or interrupted commands, and running subsystem periodic() methods. + // // This must be called from the robot's periodic block in order for anything in + // // the Command-based framework to work. + // CommandScheduler.getInstance().run(); + // } + + /** TESTING VERSION OF ROBOTPERIODIC FOR OVERRUN SOURCES */ @Override public void robotPeriodic() { - // Switch thread to high priority to improve loop timing + final long t0 = System.nanoTime(); + if (isReal()) { Threads.setCurrentThreadPriority(true, 99); } + final long t1 = System.nanoTime(); - // Run all virtual subsystems each time through the loop VirtualSubsystem.periodicAll(); + final long t2 = System.nanoTime(); - // Runs the Scheduler. This is responsible for polling buttons, adding - // newly-scheduled commands, running already-scheduled commands, removing - // finished or interrupted commands, and running subsystem periodic() methods. - // This must be called from the robot's periodic block in order for anything in - // the Command-based framework to work. CommandScheduler.getInstance().run(); + final long t3 = System.nanoTime(); - // Return to normal thread priority Threads.setCurrentThreadPriority(false, 10); + final long t4 = System.nanoTime(); + + Logger.recordOutput("Loop/RobotPeriodic_ms", (t4 - t0) / 1e6); + Logger.recordOutput("Loop/ThreadBoost_ms", (t1 - t0) / 1e6); + Logger.recordOutput("Loop/Virtual_ms", (t2 - t1) / 1e6); + Logger.recordOutput("Loop/Scheduler_ms", (t3 - t2) / 1e6); + Logger.recordOutput("Loop/ThreadRestore_ms", (t4 - t3) / 1e6); } /** This function is called once when the robot is disabled. */ @Override public void disabledInit() { // Set the brakes to stop robot motion - m_robotContainer.setMotorBrake(true); + m_robotContainer.getDrivebase().setMotorBrake(true); + m_robotContainer.getDrivebase().resetHeadingController(); m_disabledTimer.reset(); m_disabledTimer.start(); } @@ -150,7 +175,7 @@ public void disabledInit() { public void disabledPeriodic() { // After WHEEL_LOCK_TIME has elapsed, release the drive brakes if (m_disabledTimer.hasElapsed(Constants.DrivebaseConstants.kWheelLockTime)) { - m_robotContainer.setMotorBrake(false); + m_robotContainer.getDrivebase().setMotorBrake(false); m_disabledTimer.stop(); } } @@ -161,7 +186,8 @@ public void autonomousInit() { // Just in case, cancel all running commands CommandScheduler.getInstance().cancelAll(); - m_robotContainer.setMotorBrake(true); + m_robotContainer.getDrivebase().setMotorBrake(true); + m_robotContainer.getDrivebase().resetHeadingController(); // TODO: Make sure Gyro inits here with whatever is in the path planning thingie switch (Constants.getAutoType()) { @@ -202,7 +228,8 @@ public void teleopInit() { } else { CommandScheduler.getInstance().cancelAll(); } - m_robotContainer.setMotorBrake(true); + m_robotContainer.getDrivebase().setMotorBrake(true); + m_robotContainer.getDrivebase().resetHeadingController(); // In case this got set in sequential practice sessions or whatever FieldState.wonAuto = null; @@ -246,6 +273,7 @@ public void teleopPeriodic() { public void testInit() { // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); + m_robotContainer.getDrivebase().resetHeadingController(); } /** This function is called periodically during test mode. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ce35545..adef129 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -47,6 +47,7 @@ import frc.robot.subsystems.flywheel_example.Flywheel; import frc.robot.subsystems.flywheel_example.FlywheelIO; import frc.robot.subsystems.flywheel_example.FlywheelIOSim; +import frc.robot.subsystems.imu.Imu; import frc.robot.subsystems.imu.ImuIO; import frc.robot.subsystems.imu.ImuIONavX; import frc.robot.subsystems.imu.ImuIOPigeon2; @@ -63,8 +64,8 @@ import frc.robot.util.LoggedTunableNumber; import frc.robot.util.OverrideSwitches; import frc.robot.util.RBSIEnum.AutoType; +import frc.robot.util.RBSIEnum.DriveStyle; import frc.robot.util.RBSIEnum.Mode; -import frc.robot.util.RBSIPowerMonitor; import java.util.Set; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.photonvision.PhotonCamera; @@ -89,17 +90,18 @@ public class RobotContainer { // These are the "Active Subsystems" that the robot controls private final Drive m_drivebase; - private final ImuIO m_imu; private final Flywheel m_flywheel; // ... Add additional subsystems here (e.g., elevator, arm, etc.) // These are "Virtual Subsystems" that report information but have no motors + private final Imu m_imu; + @SuppressWarnings("unused") private final Accelerometer m_accel; - @SuppressWarnings("unused") - private final RBSIPowerMonitor m_power; + // @SuppressWarnings("unused") + // private final RBSIPowerMonitor m_power; @SuppressWarnings("unused") private final Vision m_vision; @@ -108,6 +110,9 @@ public class RobotContainer { // AutoChoosers for both supported path planning types private final LoggedDashboardChooser autoChooserPathPlanner; + private final LoggedDashboardChooser driveStyle = + new LoggedDashboardChooser<>("Drive Style"); + private final AutoChooser autoChooserChoreo; private final AutoFactory autoFactoryChoreo; // Input estimated battery capacity (if full, use printed value) @@ -133,17 +138,13 @@ public RobotContainer() { // YAGSL drivebase, get config from deploy directory // Get the IMU instance - switch (SwerveConstants.kImuType) { - case "pigeon2": - m_imu = new ImuIOPigeon2(); - break; - case "navx": - case "navx_spi": - m_imu = new ImuIONavX(); - break; - default: - throw new RuntimeException("Invalid IMU type"); - } + ImuIO imuIO = + switch (SwerveConstants.kImuType) { + case "pigeon2" -> new ImuIOPigeon2(); + case "navx", "navx_spi" -> new ImuIONavX(); + default -> throw new RuntimeException("Invalid IMU type"); + }; + m_imu = new Imu(imuIO); m_drivebase = new Drive(m_imu); m_flywheel = new Flywheel(new FlywheelIOSim()); // new Flywheel(new FlywheelIOTalonFX()); @@ -170,7 +171,7 @@ public RobotContainer() { case SIM: // Sim robot, instantiate physics sim IO implementations - m_imu = new ImuIOSim(); + m_imu = new Imu(new ImuIOSim() {}); m_drivebase = new Drive(m_imu); m_flywheel = new Flywheel(new FlywheelIOSim() {}); m_vision = @@ -204,7 +205,7 @@ public RobotContainer() { default: // Replayed robot, disable IO implementations - m_imu = new ImuIOSim(); + m_imu = new Imu(new ImuIOSim() {}); m_drivebase = new Drive(m_imu); m_flywheel = new Flywheel(new FlywheelIO() {}); m_vision = @@ -217,7 +218,7 @@ public RobotContainer() { // In addition to the initial battery capacity from the Dashbaord, ``RBSIPowerMonitor`` takes // all the non-drivebase subsystems for which you wish to have power monitoring; DO NOT // include ``m_drivebase``, as that is automatically monitored. - m_power = new RBSIPowerMonitor(batteryCapacity, m_flywheel); + // m_power = new RBSIPowerMonitor(batteryCapacity, m_flywheel); // Set up the SmartDashboard Auto Chooser based on auto type switch (Constants.getAutoType()) { @@ -260,6 +261,10 @@ public RobotContainer() { "Incorrect AUTO type selected in Constants: " + Constants.getAutoType()); } + // Get drive style from the Dashboard Chooser + driveStyle.addDefaultOption("TANK", DriveStyle.TANK); + driveStyle.addOption("GAMER", DriveStyle.GAMER); + // Define Auto commands defineAutoCommands(); // Define SysIs Routines @@ -286,6 +291,8 @@ private void configureBindings() { GetJoystickValue driveStickY; GetJoystickValue driveStickX; GetJoystickValue turnStickX; + // OPTIONAL: Use the DashboardChooser rather than the Constants file for Drive Style + // switch (driveStyle.get()) { switch (OperatorConstants.kDriveStyle) { case GAMER: driveStickY = driverController::getRightY; @@ -442,11 +449,6 @@ public void getAutonomousCommandChoreo() { RobotModeTriggers.autonomous().whileTrue(autoChooserChoreo.selectedCommandScheduler()); } - /** Set the motor neutral mode to BRAKE / COAST for T/F */ - public void setMotorBrake(boolean brake) { - m_drivebase.setMotorBrake(brake); - } - /** Updates the alerts. */ public void updateAlerts() { // AprilTag layout alert @@ -460,7 +462,7 @@ public void updateAlerts() { } } - /** Drivetrain getter method */ + /** Drivetrain getter method for use with Robot.java */ public Drive getDrivebase() { return m_drivebase; } diff --git a/src/main/java/frc/robot/commands/AutopilotCommands.java b/src/main/java/frc/robot/commands/AutopilotCommands.java index 3419d5e..80901f8 100644 --- a/src/main/java/frc/robot/commands/AutopilotCommands.java +++ b/src/main/java/frc/robot/commands/AutopilotCommands.java @@ -21,16 +21,13 @@ import com.therekrab.autopilot.APTarget; import com.therekrab.autopilot.Autopilot.APResult; -import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.Constants.AutoConstants; -import frc.robot.Constants.DrivebaseConstants; import frc.robot.subsystems.drive.Drive; import org.littletonrobotics.junction.Logger; @@ -178,16 +175,6 @@ public static Command runAutopilot( */ private static Command autopilotToTarget(Drive drive, APTarget target) { - // Create PID controller - ProfiledPIDController angleController = - new ProfiledPIDController( - AutoConstants.kPPsteerPID.kP, - AutoConstants.kPPsteerPID.kI, - AutoConstants.kPPsteerPID.kD, - new TrapezoidProfile.Constraints( - DrivebaseConstants.kMaxAngularSpeed, DrivebaseConstants.kMaxAngularAccel)); - angleController.enableContinuousInput(-Math.PI, Math.PI); - return Commands.run( () -> { ChassisSpeeds robotRelativeSpeeds = drive.getChassisSpeeds(); @@ -214,15 +201,19 @@ private static Command autopilotToTarget(Drive drive, APTarget target) { output.vx(), output.vy(), RadiansPerSecond.of( - angleController.calculate( - drive.getHeading().getRadians(), output.targetAngle().getRadians()))); + drive + .getAngleController() + .calculate( + drive.getHeading().getRadians(), + output.targetAngle().getRadians()))); drive.runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds(speeds, drive.getHeading())); }, drive) - // Reset PID controller when command starts - .beforeStarting(() -> angleController.reset(drive.getHeading().getRadians())) - .until(() -> AutoConstants.kAutopilot.atTarget(drive.getPose(), target)); + // Reset PID controller when command starts & ends; run until we're at target + .beforeStarting(() -> drive.resetHeadingController()) + .until(() -> AutoConstants.kAutopilot.atTarget(drive.getPose(), target)) + .finallyDo(() -> drive.resetHeadingController()); } } diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 9b538e1..0a2a5e7 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -10,22 +10,18 @@ package frc.robot.commands; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.Constants.AutoConstants; -import frc.robot.Constants.DrivebaseConstants; import frc.robot.Constants.OperatorConstants; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.SwerveConstants; @@ -117,16 +113,6 @@ public static Command fieldRelativeDriveAtAngle( DoubleSupplier ySupplier, Supplier rotationSupplier) { - // Create PID controller - ProfiledPIDController angleController = - new ProfiledPIDController( - AutoConstants.kPPsteerPID.kP, - AutoConstants.kPPsteerPID.kI, - AutoConstants.kPPsteerPID.kD, - new TrapezoidProfile.Constraints( - DrivebaseConstants.kMaxAngularSpeed, DrivebaseConstants.kMaxAngularAccel)); - angleController.enableContinuousInput(-Math.PI, Math.PI); - // Construct command return Commands.run( () -> { @@ -136,8 +122,10 @@ public static Command fieldRelativeDriveAtAngle( // Calculate angular speed double omega = - angleController.calculate( - drive.getHeading().getRadians(), rotationSupplier.get().getRadians()); + drive + .getAngleController() + .calculate( + drive.getHeading().getRadians(), rotationSupplier.get().getRadians()); // Convert to field relative speeds & send command ChassisSpeeds speeds = @@ -157,8 +145,9 @@ public static Command fieldRelativeDriveAtAngle( }, drive) - // Reset PID controller when command starts - .beforeStarting(() -> angleController.reset(drive.getHeading().getRadians())); + // Reset PID controller when command starts & ends; + .beforeStarting(() -> drive.resetHeadingController()) + .finallyDo(() -> drive.resetHeadingController()); } /** Utility functions needed by commands in this module ****************** */ diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java old mode 100755 new mode 100644 index 59ecd1b..3e23fec --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -15,7 +15,7 @@ // import frc.robot.subsystems.CommandSwerveDrivetrain; -// Generated by the Tuner X Swerve Project Generator +// Generated by the 2026 Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { // Both sets of gains need to be tuned to your individual robot. diff --git a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java index 7039b86..7752dd4 100644 --- a/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java +++ b/src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java @@ -18,8 +18,9 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj.BuiltInAccelerometer; +import edu.wpi.first.wpilibj.Timer; import frc.robot.Constants; -import frc.robot.subsystems.imu.ImuIO; +import frc.robot.subsystems.imu.Imu; import frc.robot.util.VirtualSubsystem; import org.littletonrobotics.junction.Logger; @@ -34,36 +35,39 @@ public class Accelerometer extends VirtualSubsystem { private final BuiltInAccelerometer rioAccel = new BuiltInAccelerometer(); - private final ImuIO imuIO; - private final ImuIO.ImuIOInputs imuInputs = new ImuIO.ImuIOInputs(); + private final Imu imu; + + // RIO and IMU rotations with respect to the robot + private static final Rotation3d kRioRot = new Rotation3d(0, 0, kRioOrientation.getRadians()); + private static final Rotation3d kImuRot = new Rotation3d(0, 0, kIMUOrientation.getRadians()); private Translation3d prevRioAccel = Translation3d.kZero; - public Accelerometer(ImuIO imuIO) { - this.imuIO = imuIO; + public Accelerometer(Imu imu) { + this.imu = imu; } @Override - public void periodic() { - // --- Update IMU readings --- - imuIO.updateInputs(imuInputs); + public void rbsiPeriodic() { + long t0 = System.nanoTime(); + // --- Updated IMU readings --- + final var imuInputs = imu.getInputs(); // --- Apply orientation corrections --- Translation3d rioAccVector = new Translation3d(rioAccel.getX(), rioAccel.getY(), rioAccel.getZ()) - .rotateBy(new Rotation3d(0., 0., kRioOrientation.getRadians())) + .rotateBy(kRioRot) .times(9.81); // convert to m/s^2 Translation3d imuAccVector = imuInputs .linearAccel - .rotateBy(new Rotation3d(0., 0., kIMUOrientation.getRadians())) + .rotateBy(kImuRot) .times(1.00); // already converted to m/s^2 in ImuIO implementation // --- Compute jerks --- Translation3d rioJerk = rioAccVector.minus(prevRioAccel).div(Constants.loopPeriodSecs); - Translation3d imuJerk = - imuInputs.jerk.rotateBy(new Rotation3d(0.0, 0.0, kIMUOrientation.getRadians())); + Translation3d imuJerk = imuInputs.jerk.rotateBy(kImuRot); // --- Log to AdvantageKit --- Logger.recordOutput("Accel/Rio/Accel_mps2", rioAccVector); @@ -72,13 +76,15 @@ public void periodic() { Logger.recordOutput("Accel/IMU/Jerk_mps3", imuJerk); // --- Log IMU latency --- - if (imuInputs.odometryYawTimestamps.length > 0) { - double latencySeconds = - System.currentTimeMillis() / 1000.0 - - imuInputs.odometryYawTimestamps[imuInputs.odometryYawTimestamps.length - 1]; - Logger.recordOutput("IMU/LatencySec", latencySeconds); + final double[] ts = imuInputs.odometryYawTimestamps; + if (ts.length > 0) { + double latencySeconds = Timer.getFPGATimestamp() - ts[ts.length - 1]; + Logger.recordOutput("IMU/OdometryLatencySec", latencySeconds); } prevRioAccel = rioAccVector; + + long t1 = System.nanoTime(); + Logger.recordOutput("Loop/Accel/total_ms", (t1 - t0) / 1e6); } } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 91129b7..a539b53 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -15,6 +15,7 @@ import choreo.trajectory.SwerveSample; import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.pathfinding.Pathfinding; import com.pathplanner.lib.util.PathPlannerLogging; @@ -48,7 +49,7 @@ import frc.robot.Constants.AutoConstants; import frc.robot.Constants.DrivebaseConstants; import frc.robot.Constants.RobotConstants; -import frc.robot.subsystems.imu.ImuIO; +import frc.robot.subsystems.imu.Imu; import frc.robot.util.LocalADStarAK; import frc.robot.util.RBSIEnum.Mode; import frc.robot.util.RBSIParsing; @@ -61,15 +62,13 @@ public class Drive extends SubsystemBase { static final Lock odometryLock = new ReentrantLock(); - private final ImuIO imuIO; - private final ImuIO.ImuIOInputs imuInputs = new ImuIO.ImuIOInputs(); + private final Imu imu; private final Module[] modules = new Module[4]; // FL, FR, BL, BR private final SysIdRoutine sysId; private final Alert gyroDisconnectedAlert = new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); - private Rotation2d rawGyroRotation = imuInputs.yawPosition; private SwerveModulePosition[] lastModulePositions = // For delta tracking new SwerveModulePosition[] { new SwerveModulePosition(), @@ -78,22 +77,27 @@ public class Drive extends SubsystemBase { new SwerveModulePosition() }; private SwerveDrivePoseEstimator m_PoseEstimator = - new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, Pose2d.kZero); + new SwerveDrivePoseEstimator(kinematics, Rotation2d.kZero, lastModulePositions, Pose2d.kZero); - private final ProfiledPIDController thetaController = - new ProfiledPIDController( - DrivebaseConstants.kPTheta, - DrivebaseConstants.kITheta, - DrivebaseConstants.kDTheta, - new TrapezoidProfile.Constraints( - DrivebaseConstants.kMaxAngularSpeed, DrivebaseConstants.kMaxAngularAccel)); + private ProfiledPIDController angleController; private DriveSimPhysics simPhysics; // Constructor - public Drive(ImuIO imuIO) { - this.imuIO = imuIO; - + public Drive(Imu imu) { + this.imu = imu; + + // Define the Angle Controller + angleController = + new ProfiledPIDController( + DrivebaseConstants.kPSPin, + DrivebaseConstants.kISPin, + DrivebaseConstants.kDSpin, + new TrapezoidProfile.Constraints( + getMaxAngularSpeedRadPerSec(), getMaxLinearAccelMetersPerSecPerSec())); + angleController.enableContinuousInput(-Math.PI, Math.PI); + + // If REAL (i.e., NOT simulation), parse out the module types if (Constants.getMode() == Mode.REAL) { // Case out the swerve types because Az-RBSI supports a lot @@ -111,7 +115,7 @@ public Drive(ImuIO imuIO) { for (int i = 0; i < 4; i++) { switch (modType) { case 0b00000000: // ALL-CTRE - if (kImuType == "navx" || kImuType == "navx_spi") { + if (kImuType.equals("navx") || kImuType.equals("navx_spi")) { modules[i] = new Module(new ModuleIOTalonFX(i), i); } else { throw new RuntimeException( @@ -167,7 +171,15 @@ public Drive(ImuIO imuIO) { this::resetPose, this::getChassisSpeeds, (speeds, feedforwards) -> runVelocity(speeds), - new PPHolonomicDriveController(AutoConstants.kPPdrivePID, AutoConstants.kPPsteerPID), + new PPHolonomicDriveController( + new PIDConstants( + DrivebaseConstants.kPStrafe, + DrivebaseConstants.kIStrafe, + DrivebaseConstants.kDStrafe), + new PIDConstants( + DrivebaseConstants.kPSPin, + DrivebaseConstants.kISPin, + DrivebaseConstants.kDSpin)), AutoConstants.kPathPlannerConfig, () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, this); @@ -207,7 +219,12 @@ public Drive(ImuIO imuIO) { /** Periodic function that is called each robot cycle by the command scheduler */ @Override public void periodic() { + final long t0 = System.nanoTime(); odometryLock.lock(); + final long t1 = System.nanoTime(); + + final var imuInputs = imu.getInputs(); + final long t2 = System.nanoTime(); // Stop modules & log empty setpoint states if disabled if (DriverStation.isDisabled()) { @@ -217,20 +234,18 @@ public void periodic() { Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); } } - - // Update the IMU inputs -- logging happens automatically - imuIO.updateInputs(imuInputs); + final long t3 = System.nanoTime(); // Feed historical samples into odometry if REAL robot if (Constants.getMode() != Mode.SIM) { double[] sampleTimestamps = modules[0].getOdometryTimestamps(); int sampleCount = sampleTimestamps.length; - for (int i = 0; i < sampleCount; i++) { - // Read wheel positions and deltas from each module - SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; - SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + // Read wheel positions and deltas from each module + SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; + SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + for (int i = 0; i < sampleCount; i++) { for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i]; moduleDeltas[moduleIndex] = @@ -252,15 +267,33 @@ public void periodic() { } Logger.recordOutput("Drive/Pose", m_PoseEstimator.getEstimatedPosition()); } + final long t4 = System.nanoTime(); // Module periodic updates for (var module : modules) { module.periodic(); } + final long t5 = System.nanoTime(); odometryLock.unlock(); + final long t6 = System.nanoTime(); + + Logger.recordOutput("Loop/Drive/total_ms", (t6 - t0) / 1e6); + Logger.recordOutput("Loop/Drive/lockWait_ms", (t1 - t0) / 1e6); + Logger.recordOutput("Loop/Drive/getImuInputs_ms", (t2 - t1) / 1e6); + Logger.recordOutput("Loop/Drive/disabled_ms", (t3 - t2) / 1e6); + Logger.recordOutput("Loop/Drive/odometry_ms", (t4 - t3) / 1e6); + Logger.recordOutput("Loop/Drive/modules_ms", (t5 - t4) / 1e6); + Logger.recordOutput("Loop/Drive/unlock_ms", (t6 - t5) / 1e6); + + double driveMs = (t6 - t0) / 1e6; + Logger.recordOutput("Loop/Drive/total_ms", driveMs); + + if (driveMs > 20.0) { + Logger.recordOutput("LoopSpike/Drive/odometry_ms", (t4 - t3) / 1e6); + Logger.recordOutput("LoopSpike/Drive/modules_ms", (t5 - t4) / 1e6); + } - // Update gyro/IMU alert gyroDisconnectedAlert.set(!imuInputs.connected && Constants.getMode() != Mode.SIM); } @@ -282,9 +315,9 @@ public void simulationPeriodic() { simPhysics.update(moduleStates, dt); // 4) Feed IMU from authoritative physics - imuIO.simulationSetYaw(simPhysics.getYaw()); - imuIO.simulationSetOmega(simPhysics.getOmegaRadPerSec()); - imuIO.setLinearAccel( + imu.simulationSetYaw(simPhysics.getYaw()); + imu.simulationSetOmega(simPhysics.getOmegaRadPerSec()); + imu.setLinearAccel( new Translation3d( simPhysics.getLinearAccel().getX(), simPhysics.getLinearAccel().getY(), 0.0)); @@ -354,7 +387,7 @@ public void runVelocity(ChassisSpeeds speeds) { // Calculate module setpoints ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, Constants.loopPeriodSecs); SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); - SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, DrivebaseConstants.kMaxLinearSpeed); + SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, getMaxLinearSpeedMetersPerSec()); // Log unoptimized setpoints and setpoint speeds Logger.recordOutput("SwerveStates/Setpoints", setpointStates); @@ -377,14 +410,17 @@ public void runCharacterization(double output) { } /** - * Reset the heading ProfiledPIDController - * - *

TODO: CALL THIS FUNCTION!!! + * Reset the heading for the ProfiledPIDController * *

Call this when: (A) robot is disabled, (B) gyro is zeroed, (C) autonomous starts */ public void resetHeadingController() { - thetaController.reset(getHeading().getRadians()); + angleController.reset(getHeading().getRadians()); + } + + /** Getter function for the angle controller */ + public ProfiledPIDController getAngleController() { + return angleController; } /** SysId Characterization Routines ************************************** */ @@ -444,7 +480,7 @@ public Rotation2d getHeading() { if (Constants.getMode() == Mode.SIM) { return simPhysics.getYaw(); } - return imuInputs.yawPosition; + return imu.getInputs().yawPosition; } /** Returns an array of module translations. */ @@ -510,24 +546,36 @@ public double getMaxAngularSpeedRadPerSec() { return getMaxLinearSpeedMetersPerSec() / kDriveBaseRadiusMeters; } + /** Returns the maximum linear acceleration in meters per sec per sec. */ + public double getMaxLinearAccelMetersPerSecPerSec() { + return DrivebaseConstants.kMaxLinearAccel; + } + + /** Returns the maximum angular acceleration in radians per sec per sec */ + public double getMaxAngularAccelRadPerSecPerSec() { + return getMaxLinearAccelMetersPerSecPerSec() / kDriveBaseRadiusMeters; + } + /* Setter Functions ****************************************************** */ /** Resets the current odometry pose. */ public void resetPose(Pose2d pose) { - m_PoseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); + m_PoseEstimator.resetPosition(getHeading(), getModulePositions(), pose); } /** Zeros the gyro based on alliance color */ public void zeroHeadingForAlliance() { - imuIO.zeroYaw( + imu.zeroYaw( DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? Rotation2d.kZero : Rotation2d.k180deg); + resetHeadingController(); } /** Zeros the heading */ public void zeroHeading() { - imuIO.zeroYaw(Rotation2d.kZero); + imu.zeroYaw(Rotation2d.kZero); + resetHeadingController(); } /** Adds a new timestamped vision measurement. */ diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 4bd9017..abc40ee 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -9,14 +9,13 @@ package frc.robot.subsystems.drive; -import static frc.robot.subsystems.drive.SwerveConstants.*; - import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; +import frc.robot.Constants.DrivebaseConstants; import org.littletonrobotics.junction.Logger; public class Module { @@ -53,7 +52,8 @@ public void periodic() { int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together odometryPositions = new SwerveModulePosition[sampleCount]; for (int i = 0; i < sampleCount; i++) { - double positionMeters = inputs.odometryDrivePositionsRad[i] * kWheelRadiusMeters; + double positionMeters = + inputs.odometryDrivePositionsRad[i] * DrivebaseConstants.kWheelRadiusMeters; Rotation2d angle = inputs.odometryTurnPositions[i]; odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); } @@ -71,7 +71,7 @@ public void runSetpoint(SwerveModuleState state) { state.cosineScale(inputs.turnPosition); // Apply setpoints - io.setDriveVelocity(state.speedMetersPerSecond / kWheelRadiusMeters); + io.setDriveVelocity(state.speedMetersPerSecond / DrivebaseConstants.kWheelRadiusMeters); io.setTurnPosition(state.angle); } @@ -100,12 +100,12 @@ public Rotation2d getAngle() { /** Returns the current drive position of the module in meters. */ public double getPositionMeters() { - return inputs.drivePositionRad * kWheelRadiusMeters; + return inputs.drivePositionRad * DrivebaseConstants.kWheelRadiusMeters; } /** Returns the current drive velocity of the module in meters per second. */ public double getVelocityMetersPerSec() { - return inputs.driveVelocityRadPerSec * kWheelRadiusMeters; + return inputs.driveVelocityRadPerSec * DrivebaseConstants.kWheelRadiusMeters; } /** Returns the module position (turn angle and drive position). */ diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java index 10ff019..6319902 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java @@ -10,9 +10,9 @@ package frc.robot.subsystems.drive; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static frc.robot.subsystems.drive.SwerveConstants.*; import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; @@ -52,6 +52,7 @@ import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotController; import frc.robot.Constants; +import frc.robot.Constants.CANBuses; import frc.robot.Constants.DrivebaseConstants; import frc.robot.util.PhoenixUtil; import frc.robot.util.SparkUtil; @@ -137,54 +138,55 @@ public ModuleIOBlended(int module) { switch (module) { case 0 -> ConstantCreator.createModuleConstants( - kFLSteerMotorId, - kFLDriveMotorId, - kFLEncoderId, - kFLEncoderOffset, - kFLXPosMeters, - kFLYPosMeters, - kFLDriveInvert, - kFLSteerInvert, - kFLEncoderInvert); + SwerveConstants.kFLSteerMotorId, + SwerveConstants.kFLDriveMotorId, + SwerveConstants.kFLEncoderId, + SwerveConstants.kFLEncoderOffset, + SwerveConstants.kFLXPosMeters, + SwerveConstants.kFLYPosMeters, + SwerveConstants.kFLDriveInvert, + SwerveConstants.kFLSteerInvert, + SwerveConstants.kFLEncoderInvert); case 1 -> ConstantCreator.createModuleConstants( - kFRSteerMotorId, - kFRDriveMotorId, - kFREncoderId, - kFREncoderOffset, - kFRXPosMeters, - kFRYPosMeters, - kFRDriveInvert, - kFRSteerInvert, - kFREncoderInvert); + SwerveConstants.kFRSteerMotorId, + SwerveConstants.kFRDriveMotorId, + SwerveConstants.kFREncoderId, + SwerveConstants.kFREncoderOffset, + SwerveConstants.kFRXPosMeters, + SwerveConstants.kFRYPosMeters, + SwerveConstants.kFRDriveInvert, + SwerveConstants.kFRSteerInvert, + SwerveConstants.kFREncoderInvert); case 2 -> ConstantCreator.createModuleConstants( - kBLSteerMotorId, - kBLDriveMotorId, - kBLEncoderId, - kBLEncoderOffset, - kBLXPosMeters, - kBLYPosMeters, - kBLDriveInvert, - kBLSteerInvert, - kBLEncoderInvert); + SwerveConstants.kBLSteerMotorId, + SwerveConstants.kBLDriveMotorId, + SwerveConstants.kBLEncoderId, + SwerveConstants.kBLEncoderOffset, + SwerveConstants.kBLXPosMeters, + SwerveConstants.kBLYPosMeters, + SwerveConstants.kBLDriveInvert, + SwerveConstants.kBLSteerInvert, + SwerveConstants.kBLEncoderInvert); case 3 -> ConstantCreator.createModuleConstants( - kBRSteerMotorId, - kBRDriveMotorId, - kBREncoderId, - kBREncoderOffset, - kBRXPosMeters, - kBRYPosMeters, - kBRDriveInvert, - kBRSteerInvert, - kBREncoderInvert); + SwerveConstants.kBRSteerMotorId, + SwerveConstants.kBRDriveMotorId, + SwerveConstants.kBREncoderId, + SwerveConstants.kBREncoderOffset, + SwerveConstants.kBRXPosMeters, + SwerveConstants.kBRYPosMeters, + SwerveConstants.kBRDriveInvert, + SwerveConstants.kBRSteerInvert, + SwerveConstants.kBREncoderInvert); default -> throw new IllegalArgumentException("Invalid module index"); }; - driveTalon = new TalonFX(constants.DriveMotorId, kCANBus); + CANBus canBus = CANBuses.get(SwerveConstants.kCANbusName); + driveTalon = new TalonFX(constants.DriveMotorId, canBus); turnSpark = new SparkMax(constants.SteerMotorId, MotorType.kBrushless); - cancoder = new CANcoder(constants.EncoderId, kCANBus); + cancoder = new CANcoder(constants.EncoderId, canBus); turnController = turnSpark.getClosedLoopController(); @@ -199,8 +201,8 @@ public ModuleIOBlended(int module) { .withKV(DrivebaseConstants.kDriveV) .withKA(DrivebaseConstants.kDriveA); driveConfig.Feedback.SensorToMechanismRatio = SwerveConstants.kDriveGearRatio; - driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = SwerveConstants.kDriveSlipCurrent; - driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -SwerveConstants.kDriveSlipCurrent; + driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = DrivebaseConstants.kSlipCurrent; + driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -DrivebaseConstants.kSlipCurrent; driveConfig.CurrentLimits.StatorCurrentLimit = SwerveConstants.kDriveCurrentLimit; driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; // Build the OpenLoopRampsConfigs and ClosedLoopRampsConfigs for current smoothing @@ -229,21 +231,22 @@ public ModuleIOBlended(int module) { turnConfig .absoluteEncoder .inverted(constants.EncoderInverted) - .positionConversionFactor(turnEncoderPositionFactor) - .velocityConversionFactor(turnEncoderVelocityFactor) + .positionConversionFactor(SwerveConstants.turnEncoderPositionFactor) + .velocityConversionFactor(SwerveConstants.turnEncoderVelocityFactor) .averageDepth(2); turnConfig .closedLoop .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .positionWrappingEnabled(true) - .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) + .positionWrappingInputRange( + SwerveConstants.turnPIDMinInput, SwerveConstants.turnPIDMaxInput) .pid(DrivebaseConstants.kSteerP, 0.0, DrivebaseConstants.kSteerD) .feedForward .kV(0.0); turnConfig .signals .absoluteEncoderPositionAlwaysOn(true) - .absoluteEncoderPositionPeriodMs((int) (1000.0 / kOdometryFrequency)) + .absoluteEncoderPositionPeriodMs((int) (1000.0 / SwerveConstants.kOdometryFrequency)) .absoluteEncoderVelocityAlwaysOn(true) .absoluteEncoderVelocityPeriodMs((int) (Constants.loopPeriodSecs * 1000.)) .appliedOutputPeriodMs((int) (Constants.loopPeriodSecs * 1000.)) @@ -434,8 +437,8 @@ public void setTurnPosition(Rotation2d rotation) { double setpoint = MathUtil.inputModulus( rotation.plus(Rotation2d.fromRotations(constants.EncoderOffset)).getRadians(), - turnPIDMinInput, - turnPIDMaxInput); + SwerveConstants.turnPIDMinInput, + SwerveConstants.turnPIDMaxInput); turnController.setSetpoint(setpoint, ControlType.kPosition); } @@ -444,12 +447,12 @@ public void setTurnPosition(Rotation2d rotation) { ConstantCreator = new SwerveModuleConstantsFactory< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadiusMeters) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); + .withDriveMotorGearRatio(SwerveConstants.kDriveGearRatio) + .withSteerMotorGearRatio(SwerveConstants.kSteerGearRatio) + .withCouplingGearRatio(SwerveConstants.kCoupleRatio) + .withWheelRadius(DrivebaseConstants.kWheelRadiusMeters) + .withSteerInertia(SwerveConstants.kSteerInertia) + .withDriveInertia(SwerveConstants.kDriveInertia) + .withSteerFrictionVoltage(SwerveConstants.kSteerFrictionVoltage) + .withDriveFrictionVoltage(SwerveConstants.kDriveFrictionVoltage); } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java index c2fdc35..7573861 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSpark.java @@ -9,8 +9,6 @@ package frc.robot.subsystems.drive; -import static frc.robot.subsystems.drive.SwerveConstants.*; - import com.revrobotics.AbsoluteEncoder; import com.revrobotics.PersistMode; import com.revrobotics.RelativeEncoder; @@ -78,46 +76,46 @@ public class ModuleIOSpark implements ModuleIO { public ModuleIOSpark(int module) { zeroRotation = switch (module) { - case 0 -> new Rotation2d(kFLEncoderOffset); - case 1 -> new Rotation2d(kFREncoderOffset); - case 2 -> new Rotation2d(kBLEncoderOffset); - case 3 -> new Rotation2d(kBREncoderOffset); + case 0 -> new Rotation2d(SwerveConstants.kFLEncoderOffset); + case 1 -> new Rotation2d(SwerveConstants.kFREncoderOffset); + case 2 -> new Rotation2d(SwerveConstants.kBLEncoderOffset); + case 3 -> new Rotation2d(SwerveConstants.kBREncoderOffset); default -> Rotation2d.kZero; }; driveSpark = new SparkFlex( switch (module) { - case 0 -> kFLDriveMotorId; - case 1 -> kFRDriveMotorId; - case 2 -> kBLDriveMotorId; - case 3 -> kBRDriveMotorId; + case 0 -> SwerveConstants.kFLDriveMotorId; + case 1 -> SwerveConstants.kFRDriveMotorId; + case 2 -> SwerveConstants.kBLDriveMotorId; + case 3 -> SwerveConstants.kBRDriveMotorId; default -> 0; }, MotorType.kBrushless); turnSpark = new SparkMax( switch (module) { - case 0 -> kFLSteerMotorId; - case 1 -> kFRSteerMotorId; - case 2 -> kBLSteerMotorId; - case 3 -> kBRSteerMotorId; + case 0 -> SwerveConstants.kFLSteerMotorId; + case 1 -> SwerveConstants.kFRSteerMotorId; + case 2 -> SwerveConstants.kBLSteerMotorId; + case 3 -> SwerveConstants.kBRSteerMotorId; default -> 0; }, MotorType.kBrushless); turnInverted = switch (module) { - case 0 -> kFLSteerInvert; - case 1 -> kFRSteerInvert; - case 2 -> kBLSteerInvert; - case 3 -> kBRSteerInvert; + case 0 -> SwerveConstants.kFLSteerInvert; + case 1 -> SwerveConstants.kFRSteerInvert; + case 2 -> SwerveConstants.kBLSteerInvert; + case 3 -> SwerveConstants.kBRSteerInvert; default -> false; }; turnEncoderInverted = switch (module) { - case 0 -> kFLEncoderInvert; - case 1 -> kFREncoderInvert; - case 2 -> kBLEncoderInvert; - case 3 -> kBREncoderInvert; + case 0 -> SwerveConstants.kFLEncoderInvert; + case 1 -> SwerveConstants.kFREncoderInvert; + case 2 -> SwerveConstants.kBLEncoderInvert; + case 3 -> SwerveConstants.kBREncoderInvert; default -> false; }; driveEncoder = driveSpark.getEncoder(); @@ -129,12 +127,12 @@ public ModuleIOSpark(int module) { var driveConfig = new SparkFlexConfig(); driveConfig .idleMode(IdleMode.kBrake) - .smartCurrentLimit((int) kDriveCurrentLimit) + .smartCurrentLimit((int) SwerveConstants.kDriveCurrentLimit) .voltageCompensation(DrivebaseConstants.kOptimalVoltage); driveConfig .encoder - .positionConversionFactor(driveEncoderPositionFactor) - .velocityConversionFactor(driveEncoderVelocityFactor) + .positionConversionFactor(SwerveConstants.driveEncoderPositionFactor) + .velocityConversionFactor(SwerveConstants.driveEncoderVelocityFactor) .uvwMeasurementPeriod(10) .uvwAverageDepth(2); driveConfig @@ -147,7 +145,7 @@ public ModuleIOSpark(int module) { driveConfig .signals .primaryEncoderPositionAlwaysOn(true) - .primaryEncoderPositionPeriodMs((int) (1000.0 / kOdometryFrequency)) + .primaryEncoderPositionPeriodMs((int) (1000.0 / SwerveConstants.kOdometryFrequency)) .primaryEncoderVelocityAlwaysOn(true) .primaryEncoderVelocityPeriodMs((int) (Constants.loopPeriodSecs * 1000.)) .appliedOutputPeriodMs((int) (Constants.loopPeriodSecs * 1000.)) @@ -174,21 +172,22 @@ public ModuleIOSpark(int module) { turnConfig .absoluteEncoder .inverted(turnEncoderInverted) - .positionConversionFactor(turnEncoderPositionFactor) - .velocityConversionFactor(turnEncoderVelocityFactor) + .positionConversionFactor(SwerveConstants.turnEncoderPositionFactor) + .velocityConversionFactor(SwerveConstants.turnEncoderVelocityFactor) .averageDepth(2); turnConfig .closedLoop .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .positionWrappingEnabled(true) - .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) + .positionWrappingInputRange( + SwerveConstants.turnPIDMinInput, SwerveConstants.turnPIDMaxInput) .pid(DrivebaseConstants.kSteerP, 0.0, DrivebaseConstants.kSteerD) .feedForward .kV(0.0); turnConfig .signals .absoluteEncoderPositionAlwaysOn(true) - .absoluteEncoderPositionPeriodMs((int) (1000.0 / kOdometryFrequency)) + .absoluteEncoderPositionPeriodMs((int) (1000.0 / SwerveConstants.kOdometryFrequency)) .absoluteEncoderVelocityAlwaysOn(true) .absoluteEncoderVelocityPeriodMs((int) (Constants.loopPeriodSecs * 1000.)) .appliedOutputPeriodMs((int) (Constants.loopPeriodSecs * 1000.)) @@ -332,7 +331,9 @@ public void setDriveVelocity(double velocityRadPerSec) { public void setTurnPosition(Rotation2d rotation) { double setpoint = MathUtil.inputModulus( - rotation.plus(zeroRotation).getRadians(), turnPIDMinInput, turnPIDMaxInput); + rotation.plus(zeroRotation).getRadians(), + SwerveConstants.turnPIDMinInput, + SwerveConstants.turnPIDMaxInput); turnController.setSetpoint(setpoint, ControlType.kPosition); } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java index 8f47a65..73270a7 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkCANcoder.java @@ -9,8 +9,6 @@ package frc.robot.subsystems.drive; -import static frc.robot.subsystems.drive.SwerveConstants.*; - import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.hardware.CANcoder; @@ -37,6 +35,7 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.RobotController; import frc.robot.Constants; +import frc.robot.Constants.CANBuses; import frc.robot.Constants.DrivebaseConstants; import frc.robot.util.SparkUtil; import java.util.Queue; @@ -90,59 +89,59 @@ public class ModuleIOSparkCANcoder implements ModuleIO { public ModuleIOSparkCANcoder(int module) { zeroRotation = switch (module) { - case 0 -> new Rotation2d(kFLEncoderOffset); - case 1 -> new Rotation2d(kFREncoderOffset); - case 2 -> new Rotation2d(kBLEncoderOffset); - case 3 -> new Rotation2d(kBREncoderOffset); + case 0 -> new Rotation2d(SwerveConstants.kFLEncoderOffset); + case 1 -> new Rotation2d(SwerveConstants.kFREncoderOffset); + case 2 -> new Rotation2d(SwerveConstants.kBLEncoderOffset); + case 3 -> new Rotation2d(SwerveConstants.kBREncoderOffset); default -> Rotation2d.kZero; }; driveSpark = new SparkFlex( switch (module) { - case 0 -> kFLDriveMotorId; - case 1 -> kFRDriveMotorId; - case 2 -> kBLDriveMotorId; - case 3 -> kBRDriveMotorId; + case 0 -> SwerveConstants.kFLDriveMotorId; + case 1 -> SwerveConstants.kFRDriveMotorId; + case 2 -> SwerveConstants.kBLDriveMotorId; + case 3 -> SwerveConstants.kBRDriveMotorId; default -> 0; }, MotorType.kBrushless); turnSpark = new SparkMax( switch (module) { - case 0 -> kFLSteerMotorId; - case 1 -> kFRSteerMotorId; - case 2 -> kBLSteerMotorId; - case 3 -> kBRSteerMotorId; + case 0 -> SwerveConstants.kFLSteerMotorId; + case 1 -> SwerveConstants.kFRSteerMotorId; + case 2 -> SwerveConstants.kBLSteerMotorId; + case 3 -> SwerveConstants.kBRSteerMotorId; default -> 0; }, MotorType.kBrushless); turnInverted = switch (module) { - case 0 -> kFLSteerInvert; - case 1 -> kFRSteerInvert; - case 2 -> kBLSteerInvert; - case 3 -> kBRSteerInvert; + case 0 -> SwerveConstants.kFLSteerInvert; + case 1 -> SwerveConstants.kFRSteerInvert; + case 2 -> SwerveConstants.kBLSteerInvert; + case 3 -> SwerveConstants.kBRSteerInvert; default -> false; }; turnEncoderInverted = switch (module) { - case 0 -> kFLEncoderInvert; - case 1 -> kFREncoderInvert; - case 2 -> kBLEncoderInvert; - case 3 -> kBREncoderInvert; + case 0 -> SwerveConstants.kFLEncoderInvert; + case 1 -> SwerveConstants.kFREncoderInvert; + case 2 -> SwerveConstants.kBLEncoderInvert; + case 3 -> SwerveConstants.kBREncoderInvert; default -> false; }; driveEncoder = driveSpark.getEncoder(); cancoder = new CANcoder( switch (module) { - case 0 -> kFLEncoderId; - case 1 -> kFREncoderId; - case 2 -> kBLEncoderId; - case 3 -> kBREncoderId; + case 0 -> SwerveConstants.kFLEncoderId; + case 1 -> SwerveConstants.kFREncoderId; + case 2 -> SwerveConstants.kBLEncoderId; + case 3 -> SwerveConstants.kBREncoderId; default -> 0; }, - kCANBus); + CANBuses.get(SwerveConstants.kCANbusName)); driveController = driveSpark.getClosedLoopController(); turnController = turnSpark.getClosedLoopController(); @@ -150,12 +149,12 @@ public ModuleIOSparkCANcoder(int module) { var driveConfig = new SparkFlexConfig(); driveConfig .idleMode(IdleMode.kBrake) - .smartCurrentLimit((int) kDriveCurrentLimit) + .smartCurrentLimit((int) SwerveConstants.kDriveCurrentLimit) .voltageCompensation(DrivebaseConstants.kOptimalVoltage); driveConfig .encoder - .positionConversionFactor(driveEncoderPositionFactor) - .velocityConversionFactor(driveEncoderVelocityFactor) + .positionConversionFactor(SwerveConstants.driveEncoderPositionFactor) + .velocityConversionFactor(SwerveConstants.driveEncoderVelocityFactor) .uvwMeasurementPeriod(10) .uvwAverageDepth(2); driveConfig @@ -168,7 +167,7 @@ public ModuleIOSparkCANcoder(int module) { driveConfig .signals .primaryEncoderPositionAlwaysOn(true) - .primaryEncoderPositionPeriodMs((int) (1000.0 / kOdometryFrequency)) + .primaryEncoderPositionPeriodMs((int) (1000.0 / SwerveConstants.kOdometryFrequency)) .primaryEncoderVelocityAlwaysOn(true) .primaryEncoderVelocityPeriodMs(20) .appliedOutputPeriodMs(20) @@ -192,21 +191,22 @@ public ModuleIOSparkCANcoder(int module) { turnConfig .absoluteEncoder .inverted(turnEncoderInverted) - .positionConversionFactor(turnEncoderPositionFactor) - .velocityConversionFactor(turnEncoderVelocityFactor) + .positionConversionFactor(SwerveConstants.turnEncoderPositionFactor) + .velocityConversionFactor(SwerveConstants.turnEncoderVelocityFactor) .averageDepth(2); turnConfig .closedLoop .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .positionWrappingEnabled(true) - .positionWrappingInputRange(turnPIDMinInput, turnPIDMaxInput) + .positionWrappingInputRange( + SwerveConstants.turnPIDMinInput, SwerveConstants.turnPIDMaxInput) .pid(DrivebaseConstants.kSteerP, 0.0, DrivebaseConstants.kSteerD) .feedForward .kV(0.0); turnConfig .signals .absoluteEncoderPositionAlwaysOn(true) - .absoluteEncoderPositionPeriodMs((int) (1000.0 / kOdometryFrequency)) + .absoluteEncoderPositionPeriodMs((int) (1000.0 / SwerveConstants.kOdometryFrequency)) .absoluteEncoderVelocityAlwaysOn(true) .absoluteEncoderVelocityPeriodMs((int) (Constants.loopPeriodSecs * 1000.)) .appliedOutputPeriodMs((int) (Constants.loopPeriodSecs * 1000.)) @@ -362,7 +362,9 @@ public void setDriveVelocity(double velocityRadPerSec) { public void setTurnPosition(Rotation2d rotation) { double setpoint = MathUtil.inputModulus( - rotation.plus(zeroRotation).getRadians(), turnPIDMinInput, turnPIDMaxInput); + rotation.plus(zeroRotation).getRadians(), + SwerveConstants.turnPIDMinInput, + SwerveConstants.turnPIDMaxInput); turnController.setSetpoint(setpoint, ControlType.kPosition); } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index a8470d8..b616b0b 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -10,9 +10,9 @@ package frc.robot.subsystems.drive; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static frc.robot.subsystems.drive.SwerveConstants.*; import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; @@ -44,6 +44,7 @@ import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotController; import frc.robot.Constants; +import frc.robot.Constants.CANBuses; import frc.robot.Constants.DrivebaseConstants; import frc.robot.generated.TunerConstants; import frc.robot.util.PhoenixUtil; @@ -135,9 +136,10 @@ public ModuleIOTalonFX(int module) { default -> throw new IllegalArgumentException("Invalid module index"); }; - driveTalon = new TalonFX(constants.DriveMotorId, kCANBus); - turnTalon = new TalonFX(constants.SteerMotorId, kCANBus); - cancoder = new CANcoder(constants.EncoderId, kCANBus); + CANBus canBus = CANBuses.get(SwerveConstants.kCANbusName); + driveTalon = new TalonFX(constants.DriveMotorId, canBus); + turnTalon = new TalonFX(constants.SteerMotorId, canBus); + cancoder = new CANcoder(constants.EncoderId, canBus); // Configure drive motor driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -150,8 +152,8 @@ public ModuleIOTalonFX(int module) { .withKV(DrivebaseConstants.kDriveV) .withKA(DrivebaseConstants.kDriveA); driveConfig.Feedback.SensorToMechanismRatio = SwerveConstants.kDriveGearRatio; - driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = SwerveConstants.kDriveSlipCurrent; - driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -SwerveConstants.kDriveSlipCurrent; + driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = DrivebaseConstants.kSlipCurrent; + driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -DrivebaseConstants.kSlipCurrent; driveConfig.CurrentLimits.StatorCurrentLimit = SwerveConstants.kDriveCurrentLimit; driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; // Build the OpenLoopRampsConfigs and ClosedLoopRampsConfigs for current smoothing diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java index 9e101cd..34bdf72 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java @@ -10,9 +10,9 @@ package frc.robot.subsystems.drive; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static frc.robot.subsystems.drive.SwerveConstants.*; import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANdiConfiguration; import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; @@ -45,6 +45,7 @@ import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotController; import frc.robot.Constants; +import frc.robot.Constants.CANBuses; import frc.robot.Constants.DrivebaseConstants; import frc.robot.util.PhoenixUtil; import java.util.Queue; @@ -124,9 +125,10 @@ public class ModuleIOTalonFXS implements ModuleIO { public ModuleIOTalonFXS( SwerveModuleConstants constants) { - driveTalon = new TalonFXS(constants.DriveMotorId, kCANBus); - turnTalon = new TalonFXS(constants.SteerMotorId, kCANBus); - candi = new CANdi(constants.EncoderId, kCANBus); + CANBus canBus = CANBuses.get(SwerveConstants.kCANbusName); + driveTalon = new TalonFXS(constants.DriveMotorId, canBus); + turnTalon = new TalonFXS(constants.SteerMotorId, canBus); + candi = new CANdi(constants.EncoderId, canBus); // Configure drive motor driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -147,7 +149,7 @@ public ModuleIOTalonFXS( driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; driveConfig.Slot0 = constants.DriveMotorGains; driveConfig.ExternalFeedback.SensorToMechanismRatio = constants.DriveMotorGearRatio; - driveConfig.CurrentLimits.StatorCurrentLimit = constants.SlipCurrent; + driveConfig.CurrentLimits.StatorCurrentLimit = DrivebaseConstants.kSlipCurrent; driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; // Build the OpenLoopRampsConfigs and ClosedLoopRampsConfigs for current smoothing OpenLoopRampsConfigs openRamps = new OpenLoopRampsConfigs(); diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java index 61201dd..98f775f 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java @@ -16,6 +16,7 @@ import com.ctre.phoenix6.CANBus; import edu.wpi.first.math.util.Units; import frc.robot.Constants; +import frc.robot.Constants.CANBuses; import frc.robot.generated.TunerConstants; import frc.robot.util.YagslConstants; @@ -32,10 +33,7 @@ public class SwerveConstants { public static final double kCoupleRatio; public static final double kDriveGearRatio; public static final double kSteerGearRatio; - public static final double kWheelRadiusInches; - public static final double kWheelRadiusMeters; public static final String kCANbusName; - public static final CANBus kCANBus; public static final int kPigeonId; public static final double kSteerInertia; public static final double kDriveInertia; @@ -43,7 +41,6 @@ public class SwerveConstants { public static final double kDriveFrictionVoltage; public static final double kSteerCurrentLimit; public static final double kDriveCurrentLimit; - public static final double kDriveSlipCurrent; public static final int kFLDriveMotorId; public static final int kFLSteerMotorId; public static final int kFLEncoderId; @@ -113,10 +110,7 @@ public class SwerveConstants { kCoupleRatio = TunerConstants.FrontLeft.CouplingGearRatio; kDriveGearRatio = TunerConstants.FrontLeft.DriveMotorGearRatio; kSteerGearRatio = TunerConstants.FrontLeft.SteerMotorGearRatio; - kWheelRadiusMeters = TunerConstants.FrontLeft.WheelRadius; - kWheelRadiusInches = Units.metersToInches(kWheelRadiusMeters); - kCANbusName = TunerConstants.DrivetrainConstants.CANBusName; - kCANBus = new CANBus(TunerConstants.DrivetrainConstants.CANBusName); + kCANbusName = CANBuses.DRIVE; kPigeonId = TunerConstants.DrivetrainConstants.Pigeon2Id; kSteerInertia = TunerConstants.FrontLeft.SteerInertia; kDriveInertia = TunerConstants.FrontLeft.DriveInertia; @@ -124,14 +118,13 @@ public class SwerveConstants { kDriveFrictionVoltage = 0.1; kSteerCurrentLimit = 40.0; // Example from CTRE documentation kDriveCurrentLimit = 120.0; // Example from CTRE documentation - kDriveSlipCurrent = TunerConstants.FrontLeft.SlipCurrent; // Front Left kFLDriveMotorId = TunerConstants.FrontLeft.DriveMotorId; kFLSteerMotorId = TunerConstants.FrontLeft.SteerMotorId; kFLEncoderId = TunerConstants.FrontLeft.EncoderId; - kFLDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kFLSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kFLEncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kFLDriveCanbus = kCANbusName; + kFLSteerCanbus = kCANbusName; + kFLEncoderCanbus = kCANbusName; kFLDriveType = "kraken"; kFLSteerType = "kraken"; kFLEncoderType = "cancoder"; @@ -146,9 +139,9 @@ public class SwerveConstants { kFRDriveMotorId = TunerConstants.FrontRight.DriveMotorId; kFRSteerMotorId = TunerConstants.FrontRight.SteerMotorId; kFREncoderId = TunerConstants.FrontRight.EncoderId; - kFRDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kFRSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kFREncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kFRDriveCanbus = kCANbusName; + kFRSteerCanbus = kCANbusName; + kFREncoderCanbus = kCANbusName; kFRDriveType = "kraken"; kFRSteerType = "kraken"; kFREncoderType = "cancoder"; @@ -162,9 +155,9 @@ public class SwerveConstants { kBLDriveMotorId = TunerConstants.BackLeft.DriveMotorId; kBLSteerMotorId = TunerConstants.BackLeft.SteerMotorId; kBLEncoderId = TunerConstants.BackLeft.EncoderId; - kBLDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kBLSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kBLEncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kBLDriveCanbus = kCANbusName; + kBLSteerCanbus = kCANbusName; + kBLEncoderCanbus = kCANbusName; kBLDriveType = "kraken"; kBLSteerType = "kraken"; kBLEncoderType = "cancoder"; @@ -179,9 +172,9 @@ public class SwerveConstants { kBRDriveMotorId = TunerConstants.BackRight.DriveMotorId; kBRSteerMotorId = TunerConstants.BackRight.SteerMotorId; kBREncoderId = TunerConstants.BackRight.EncoderId; - kBRDriveCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kBRSteerCanbus = TunerConstants.DrivetrainConstants.CANBusName; - kBREncoderCanbus = TunerConstants.DrivetrainConstants.CANBusName; + kBRDriveCanbus = kCANbusName; + kBRSteerCanbus = kCANbusName; + kBREncoderCanbus = kCANbusName; kBRDriveType = "kraken"; kBRSteerType = "kraken"; kBREncoderType = "cancoder"; @@ -198,10 +191,7 @@ public class SwerveConstants { kCoupleRatio = YagslConstants.kCoupleRatio; kDriveGearRatio = YagslConstants.kDriveGearRatio; kSteerGearRatio = YagslConstants.kSteerGearRatio; - kWheelRadiusInches = YagslConstants.kWheelRadiusInches; - kWheelRadiusMeters = Units.inchesToMeters(kWheelRadiusInches); kCANbusName = YagslConstants.kCANbusName; - kCANBus = new CANBus(YagslConstants.kCANbusName); kPigeonId = YagslConstants.kPigeonId; kSteerInertia = YagslConstants.kSteerInertia; kDriveInertia = YagslConstants.kDriveInertia; @@ -209,7 +199,6 @@ public class SwerveConstants { kDriveFrictionVoltage = YagslConstants.kDriveFrictionVoltage; kSteerCurrentLimit = YagslConstants.kSteerCurrentLimit; kDriveCurrentLimit = YagslConstants.kDriveCurrentLimit; - kDriveSlipCurrent = 120.0; // Front Left kFLDriveMotorId = YagslConstants.kFrontLeftDriveMotorId; kFLSteerMotorId = YagslConstants.kFrontLeftSteerMotorId; diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java index be7ab97..80406e5 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java @@ -12,7 +12,6 @@ import static edu.wpi.first.units.Units.Volts; import static frc.robot.Constants.FlywheelConstants.*; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -24,7 +23,6 @@ public class Flywheel extends RBSISubsystem { private final FlywheelIO io; private final FlywheelIOInputsAutoLogged inputs = new FlywheelIOInputsAutoLogged(); - private final SimpleMotorFeedforward ffModel; private final SysIdRoutine sysId; /** Creates a new Flywheel. */ @@ -36,17 +34,11 @@ public Flywheel(FlywheelIO io) { switch (Constants.getMode()) { case REAL: case REPLAY: - ffModel = new SimpleMotorFeedforward(kStaticGainReal, kVelocityGainReal); - io.configurePID(pidReal.kP, pidReal.kI, pidReal.kD); - io.configureFF(kStaticGainReal, kVelocityGainReal); + io.configureGains(kPreal, 0.0, kDreal, kSreal, kVreal, kAreal); break; case SIM: - ffModel = new SimpleMotorFeedforward(kStaticGainSim, kVelocityGainSim); - io.configurePID(pidSim.kP, pidSim.kI, pidSim.kD); - io.configureFF(kStaticGainSim, kVelocityGainSim); - break; default: - ffModel = new SimpleMotorFeedforward(0.0, 0.0); + io.configureGains(kPsim, 0.0, kDsim, kSsim, kVsim, kAsim); break; } @@ -61,8 +53,9 @@ public Flywheel(FlywheelIO io) { new SysIdRoutine.Mechanism((voltage) -> runVolts(voltage.in(Volts)), null, this)); } + /** Periodic function -- inherits timing logic from RBSISubsystem */ @Override - public void periodic() { + protected void rbsiPeriodic() { io.updateInputs(inputs); Logger.processInputs("Flywheel", inputs); } @@ -75,7 +68,7 @@ public void runVolts(double volts) { /** Run closed loop at the specified velocity. */ public void runVelocity(double velocityRPM) { var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); - io.setVelocity(velocityRadPerSec, ffModel.calculate(velocityRadPerSec)); + io.setVelocity(velocityRadPerSec); // Log flywheel setpoint Logger.recordOutput("Flywheel/SetpointRPM", velocityRPM); diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java index c1b6901..47fa874 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java @@ -26,14 +26,12 @@ public static class FlywheelIOInputs { public default void updateInputs(FlywheelIOInputs inputs) {} /** Run closed loop at the specified velocity. */ - public default void setVelocity(double velocityRadPerSec, double ffVolts) {} + public default void setVelocity(double velocityRadPerSec) {} - /** Set velocity PID constants. */ - public default void configurePID(double kP, double kI, double kD) {} + /** Set gain constants */ + public default void configureGains(double kP, double kI, double kD, double kS, double kV) {} - /** Set velocity FeedForward constants. */ - public default void configureFF(double kS, double kV) {} - - /** Set velocity FeedForward constants. */ - public default void configureFF(double kS, double kV, double kA) {} + /** Set gain constants */ + public default void configureGains( + double kP, double kI, double kD, double kS, double kV, double kA) {} } diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java index d65eade..a8e8c20 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java @@ -11,6 +11,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.plant.DCMotor; @@ -33,6 +34,7 @@ public class FlywheelIOSim implements FlywheelIO { private final FlywheelSim sim = new FlywheelSim(m_plant, m_gearbox); private PIDController pid = new PIDController(0.0, 0.0, 0.0); + private SimpleMotorFeedforward ff = new SimpleMotorFeedforward(0.0, 0.0, 0.0); private boolean closedLoop = false; private double ffVolts = 0.0; @@ -62,10 +64,10 @@ public void setVoltage(double volts) { } @Override - public void setVelocity(double velocityRadPerSec, double ffVolts) { + public void setVelocity(double velocityRadPerSec) { closedLoop = true; pid.setSetpoint(velocityRadPerSec); - this.ffVolts = ffVolts; + this.ffVolts = ff.calculate(velocityRadPerSec); } @Override @@ -73,8 +75,20 @@ public void stop() { setVoltage(0.0); } + /** Set gain constants -- no kA */ @Override - public void configurePID(double kP, double kI, double kD) { + public void configureGains(double kP, double kI, double kD, double kS, double kV) { pid.setPID(kP, kI, kD); + ff.setKs(kS); + ff.setKv(kV); + ff.setKa(0.0); + } + + /** Set gain constants - with kA */ + public void configureGains(double kP, double kI, double kD, double kS, double kV, double kA) { + pid.setPID(kP, kI, kD); + ff.setKs(kS); + ff.setKv(kV); + ff.setKa(kA); } } diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java index cef38ce..ee89859 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java @@ -30,6 +30,7 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkFlexConfig; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.util.Units; import frc.robot.Constants; import frc.robot.Constants.DrivebaseConstants; @@ -54,6 +55,7 @@ public class FlywheelIOSpark implements FlywheelIO { public final int[] powerPorts = { FLYWHEEL_LEADER.getPowerPort(), FLYWHEEL_FOLLOWER.getPowerPort() }; + private final SimpleMotorFeedforward ff = new SimpleMotorFeedforward(kSreal, kVreal, kAreal); public FlywheelIOSpark() { @@ -71,10 +73,11 @@ public FlywheelIOSpark() { leaderConfig .closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - .pid(pidReal.kP, pidReal.kI, pidReal.kD) + .pid(kPreal, 0.0, kDreal) .feedForward - .kS(kStaticGainReal) - .kV(kVelocityGainReal); + .kS(kSreal) + .kV(kVreal) + .kA(kAreal); leaderConfig .signals .primaryEncoderPositionAlwaysOn(true) @@ -118,7 +121,8 @@ public void setVoltage(double volts) { } @Override - public void setVelocity(double velocityRadPerSec, double ffVolts) { + public void setVelocity(double velocityRadPerSec) { + double ffVolts = ff.calculate(velocityRadPerSec); pid.setSetpoint( Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * kFlywheelGearRatio, ControlType.kVelocity, @@ -133,14 +137,14 @@ public void stop() { } /** - * Configure the closed-loop PID + * Configure the closed-loop control gains * *

TODO: This functionality is no longer supported by the REVLib SparkClosedLoopController * class. In order to keep control of the flywheel's underlying funtionality, shift everything to * SmartMotion control. */ @Override - public void configurePID(double kP, double kI, double kD) { + public void configureGains(double kP, double kI, double kD, double kS, double kV) { // pid.setP(kP, 0); // pid.setI(kI, 0); // pid.setD(kD, 0); @@ -148,8 +152,10 @@ public void configurePID(double kP, double kI, double kD) { } @Override - public void configureFF(double kS, double kV) {} - - @Override - public void configureFF(double kS, double kV, double kA) {} + public void configureGains(double kP, double kI, double kD, double kS, double kV, double kA) { + // pid.setP(kP, 0); + // pid.setI(kI, 0); + // pid.setD(kD, 0); + // pid.setFF(0, 0); + } } diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java index 04391d6..1b964f5 100644 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java @@ -18,8 +18,9 @@ import com.ctre.phoenix6.configs.OpenLoopRampsConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.controls.MotionMagicDutyCycle; +import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -28,7 +29,10 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants; +import frc.robot.Constants.PowerConstants; import frc.robot.util.PhoenixUtil; +import frc.robot.util.RBSIEnum.CTREPro; public class FlywheelIOTalonFX implements FlywheelIO { @@ -49,9 +53,10 @@ public class FlywheelIOTalonFX implements FlywheelIO { private final StatusSignal followerCurrent = follower.getSupplyCurrent(); private final TalonFXConfiguration config = new TalonFXConfiguration(); + private final boolean isCTREPro = Constants.getPhoenixPro() == CTREPro.LICENSED; public FlywheelIOTalonFX() { - config.CurrentLimits.SupplyCurrentLimit = 30.0; + config.CurrentLimits.SupplyCurrentLimit = PowerConstants.kMotorPortMaxCurrent; config.CurrentLimits.SupplyCurrentLimitEnable = true; config.MotorOutput.NeutralMode = switch (kFlywheelIdleMode) { @@ -69,10 +74,15 @@ public FlywheelIOTalonFX() { closedRamps.TorqueClosedLoopRampPeriod = kFlywheelClosedLoopRampPeriod; // Apply the open- and closed-loop ramp configuration for current smoothing config.withClosedLoopRamps(closedRamps).withOpenLoopRamps(openRamps); + // set Motion Magic Velocity settings + var motionMagicConfigs = config.MotionMagic; + motionMagicConfigs.MotionMagicAcceleration = + 400; // Target acceleration of 400 rps/s (0.25 seconds to max) + motionMagicConfigs.MotionMagicJerk = 4000; // Target jerk of 4000 rps/s/s (0.1 seconds) // Apply the configurations to the flywheel motors - leader.getConfigurator().apply(config); - follower.getConfigurator().apply(config); + PhoenixUtil.tryUntilOk(5, () -> leader.getConfigurator().apply(config, 0.25)); + PhoenixUtil.tryUntilOk(5, () -> follower.getConfigurator().apply(config, 0.25)); // If follower rotates in the opposite direction, set "MotorAlignmentValue" to Opposed follower.setControl(new Follower(leader.getDeviceID(), MotorAlignmentValue.Aligned)); @@ -97,12 +107,25 @@ public void updateInputs(FlywheelIOInputs inputs) { @Override public void setVoltage(double volts) { - leader.setControl(new VoltageOut(volts)); + final MotionMagicVoltage m_request = new MotionMagicVoltage(volts); + m_request.withEnableFOC(isCTREPro); + leader.setControl(m_request); + } + + @Override + public void setVelocity(double velocityRadPerSec) { + // create a Motion Magic Velocity request, voltage output + final MotionMagicVelocityVoltage m_request = new MotionMagicVelocityVoltage(0); + m_request.withEnableFOC(isCTREPro); + leader.setControl(m_request.withVelocity(Units.radiansToRotations(velocityRadPerSec))); } @Override - public void setVelocity(double velocityRadPerSec, double ffVolts) { - leader.setControl(new VelocityVoltage(Units.radiansToRotations(velocityRadPerSec))); + public void setPercent(double percent) { + // create a Motion Magic DutyCycle request, voltage output + final MotionMagicDutyCycle m_request = new MotionMagicDutyCycle(percent); + m_request.withEnableFOC(isCTREPro); + leader.setControl(m_request); } @Override @@ -111,41 +134,40 @@ public void stop() { } /** - * Set the PID portion of the Slot0 closed-loop configuration + * Set the gains of the Slot0 closed-loop configuration * * @param kP Proportional gain * @param kI Integral gain * @param kD Differential gain + * @param kS Static gain + * @param kV Velocity gain */ @Override - public void configurePID(double kP, double kI, double kD) { + public void configureGains(double kP, double kI, double kD, double kS, double kV) { config.Slot0.kP = kP; config.Slot0.kI = kI; config.Slot0.kD = kD; - PhoenixUtil.tryUntilOk(5, () -> leader.getConfigurator().apply(config, 0.25)); - } - - /** - * Set the FeedForward portion of the Slot0 closed-loop configuration - * - * @param kS Static gain - * @param kV Velocity gain - */ - @Override - public void configureFF(double kS, double kV) { config.Slot0.kS = kS; config.Slot0.kV = kV; + config.Slot0.kA = 0.0; PhoenixUtil.tryUntilOk(5, () -> leader.getConfigurator().apply(config, 0.25)); } /** - * Set the FeedForward portion of the Slot0 closed-loop configuration + * Set the gains of the Slot0 closed-loop configuration * + * @param kP Proportional gain + * @param kI Integral gain + * @param kD Differential gain * @param kS Static gain * @param kV Velocity gain * @param kA Acceleration gain */ - public void configureFF(double kS, double kV, double kA) { + @Override + public void configureGains(double kP, double kI, double kD, double kS, double kV, double kA) { + config.Slot0.kP = kP; + config.Slot0.kI = kI; + config.Slot0.kD = kD; config.Slot0.kS = kS; config.Slot0.kV = kV; config.Slot0.kA = kA; diff --git a/src/main/java/frc/robot/subsystems/imu/Imu.java b/src/main/java/frc/robot/subsystems/imu/Imu.java new file mode 100644 index 0000000..c69a06b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/imu/Imu.java @@ -0,0 +1,73 @@ +// Copyright (c) 2024-2026 Az-FIRST +// http://github.com/AZ-First +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// 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.imu; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation3d; +import frc.robot.util.VirtualSubsystem; +import org.littletonrobotics.junction.Logger; + +public class Imu extends VirtualSubsystem { + + private final ImuIO io; + private final ImuIOInputsAutoLogged inputs = new ImuIOInputsAutoLogged(); + + public Imu(ImuIO io) { + this.io = io; + } + + @Override + protected void rbsiPeriodic() { + final long t0 = System.nanoTime(); + io.updateInputs(inputs); + final long t1 = System.nanoTime(); + Logger.processInputs("IMU", inputs); + final long t2 = System.nanoTime(); + + Logger.recordOutput("Loop/IMU/update_ms", (t1 - t0) / 1e6); + Logger.recordOutput("Loop/IMU/log_ms", (t2 - t1) / 1e6); + Logger.recordOutput("Loop/IMU/total_ms", (t2 - t0) / 1e6); + + double totalMs = (t2 - t0) / 1e6; + Logger.recordOutput("Loop/IMU/total_ms", totalMs); + if (totalMs > 2.0) { + Logger.recordOutput("LoopSpike/IMU/update_ms", (t1 - t0) / 1e6); + } + } + + public ImuIOInputsAutoLogged getInputs() { + return inputs; + } + + // Pass-throughs so Drive can still control the IMU + public void zeroYaw(Rotation2d yaw) { + io.zeroYaw(yaw); + } + + public void simulationSetYaw(Rotation2d yaw) { + io.simulationSetYaw(yaw); + } + + public void simulationSetOmega(double omegaRadPerSec) { + io.simulationSetOmega(omegaRadPerSec); + } + + public void setLinearAccel(Translation3d accelMps2) { + io.setLinearAccel(accelMps2); + } +} diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIO.java b/src/main/java/frc/robot/subsystems/imu/ImuIO.java index 9273f0c..d8c05f9 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIO.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIO.java @@ -22,12 +22,13 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.util.RBSIIO; import org.littletonrobotics.junction.AutoLog; /** * Single IMU interface exposing all relevant state: orientation, rates, linear accel, and odometry. */ -public interface ImuIO { +public interface ImuIO extends RBSIIO { @AutoLog public static class ImuIOInputs { diff --git a/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java b/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java index d954f6b..664dafb 100644 --- a/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/imu/ImuIOPigeon2.java @@ -28,6 +28,7 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.Constants.CANBuses; import frc.robot.subsystems.drive.PhoenixOdometryThread; import frc.robot.subsystems.drive.SwerveConstants; import java.util.Queue; @@ -35,7 +36,8 @@ /** IMU IO for CTRE Pigeon2 */ public class ImuIOPigeon2 implements ImuIO { - private final Pigeon2 pigeon = new Pigeon2(SwerveConstants.kPigeonId, SwerveConstants.kCANBus); + private final Pigeon2 pigeon = + new Pigeon2(SwerveConstants.kPigeonId, CANBuses.get(SwerveConstants.kCANbusName)); private final StatusSignal yawSignal = pigeon.getYaw(); private final StatusSignal yawRateSignal = pigeon.getAngularVelocityZWorld(); private final Queue odomTimestamps; diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 6b71f49..68183f1 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -27,8 +27,7 @@ import frc.robot.Constants.Cameras; import frc.robot.FieldConstants; import frc.robot.subsystems.vision.VisionIO.PoseObservationType; -import java.util.LinkedList; -import java.util.List; +import java.util.ArrayList; import org.littletonrobotics.junction.Logger; public class Vision extends SubsystemBase { @@ -71,29 +70,30 @@ public Rotation2d getTargetX(int cameraIndex) { @Override public void periodic() { + // Update inputs + process inputs first (cheap, and keeps AK logs consistent) for (int i = 0; i < io.length; i++) { io[i].updateInputs(inputs[i]); - Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]); + Logger.processInputs("Vision/Camera" + i, inputs[i]); + disconnectedAlerts[i].set(!inputs[i].connected); } - // Initialize logging values - List allTagPoses = new LinkedList<>(); - List allRobotPoses = new LinkedList<>(); - List allRobotPosesAccepted = new LinkedList<>(); - List allRobotPosesRejected = new LinkedList<>(); + // Reusable scratch buffers (ArrayList avoids LinkedList churn) + // Tune these capacities if you know typical sizes + final ArrayList allTagPoses = new ArrayList<>(32); + final ArrayList allRobotPoses = new ArrayList<>(64); + final ArrayList allRobotPosesAccepted = new ArrayList<>(64); + final ArrayList allRobotPosesRejected = new ArrayList<>(64); // Loop over cameras for (int cameraIndex = 0; cameraIndex < io.length; cameraIndex++) { - // Update disconnected alert - disconnectedAlerts[cameraIndex].set(!inputs[cameraIndex].connected); - // Initialize logging values - List tagPoses = new LinkedList<>(); - List robotPoses = new LinkedList<>(); - List robotPosesAccepted = new LinkedList<>(); - List robotPosesRejected = new LinkedList<>(); + // Per-camera scratch buffers + final ArrayList tagPoses = new ArrayList<>(16); + final ArrayList robotPoses = new ArrayList<>(32); + final ArrayList robotPosesAccepted = new ArrayList<>(32); + final ArrayList robotPosesRejected = new ArrayList<>(32); - // Add tag poses + // Add tag poses from ids for (int tagId : inputs[cameraIndex].tagIds) { var tagPose = FieldConstants.aprilTagLayout.getTagPose(tagId); if (tagPose.isPresent()) { @@ -151,19 +151,19 @@ public void periodic() { VecBuilder.fill(linearStdDev, linearStdDev, angularStdDev)); } - // Log camera datadata + // Log camera data Logger.recordOutput( - "Vision/Camera" + Integer.toString(cameraIndex) + "/TagPoses", - tagPoses.toArray(new Pose3d[0])); + "Vision/Camera" + cameraIndex + "/TagPoses", tagPoses.toArray(new Pose3d[0])); Logger.recordOutput( - "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPoses", - robotPoses.toArray(new Pose3d[0])); + "Vision/Camera" + cameraIndex + "/RobotPoses", robotPoses.toArray(new Pose3d[0])); Logger.recordOutput( - "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPosesAccepted", + "Vision/Camera" + cameraIndex + "/RobotPosesAccepted", robotPosesAccepted.toArray(new Pose3d[0])); Logger.recordOutput( - "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPosesRejected", + "Vision/Camera" + cameraIndex + "/RobotPosesRejected", robotPosesRejected.toArray(new Pose3d[0])); + + // Summary aggregation allTagPoses.addAll(tagPoses); allRobotPoses.addAll(robotPoses); allRobotPosesAccepted.addAll(robotPosesAccepted); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index 13f93b3..f8d9cdd 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -14,9 +14,8 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; +import java.util.ArrayList; import java.util.HashSet; -import java.util.LinkedList; -import java.util.List; import java.util.Set; import org.photonvision.PhotonCamera; @@ -40,10 +39,20 @@ public VisionIOPhotonVision(String name, Transform3d robotToCamera) { public void updateInputs(VisionIOInputs inputs) { inputs.connected = camera.isConnected(); - // Read new camera observations + // Cap the number of unread results processed per loop + final int kMaxUnread = 5; + + // Use HashSet/ArrayList to avoid LinkedList churn Set tagIds = new HashSet<>(); - List poseObservations = new LinkedList<>(); + ArrayList poseObservations = new ArrayList<>(kMaxUnread); + + int processed = 0; for (var result : camera.getAllUnreadResults()) { + // Hard cap + if (processed++ >= kMaxUnread) { + break; + } + // Update latest target observation if (result.hasTargets()) { inputs.latestTargetObservation = @@ -72,6 +81,10 @@ public void updateInputs(VisionIOInputs inputs) { // Add tag IDs tagIds.addAll(multitagResult.fiducialIDsUsed); + // Guard against divide-by-zero if targets is empty (defensive) + double avgTagDistance = + result.targets.isEmpty() ? 0.0 : (totalTagDistance / result.targets.size()); + // Add observation poseObservations.add( new PoseObservation( @@ -79,7 +92,7 @@ public void updateInputs(VisionIOInputs inputs) { robotPose, // 3D pose estimate multitagResult.estimatedPose.ambiguity, // Ambiguity multitagResult.fiducialIDsUsed.size(), // Tag count - totalTagDistance / result.targets.size(), // Average tag distance + avgTagDistance, // Average tag distance PoseObservationType.PHOTONVISION)); // Observation type } else if (!result.targets.isEmpty()) { // Single tag result diff --git a/src/main/java/frc/robot/util/RBSIPowerMonitor.java b/src/main/java/frc/robot/util/RBSIPowerMonitor.java index 54972cf..4d84b8f 100644 --- a/src/main/java/frc/robot/util/RBSIPowerMonitor.java +++ b/src/main/java/frc/robot/util/RBSIPowerMonitor.java @@ -51,29 +51,33 @@ public class RBSIPowerMonitor extends VirtualSubsystem { RobotDevices.BR_ROTATION.getPowerPort() }; - // Class method definition, including inputs of optional subsystems + private final Alert totalCurrentAlert = + new Alert("Total current draw exceeds limit!", AlertType.WARNING); + + private final Alert[] portAlerts = new Alert[24]; // or pdh.getNumChannels() after construct + + // Constructor, including inputs of optional subsystems public RBSIPowerMonitor(LoggedTunableNumber batteryCapacityAh, RBSISubsystem... subsystems) { this.batteryCapacityAh = batteryCapacityAh; this.subsystems = subsystems; + + for (int i = 0; i < portAlerts.length; i++) { + portAlerts[i] = new Alert("Port " + i + " current exceeds limit!", AlertType.WARNING); + } } /** Periodic Method */ @Override - public void periodic() { + public void rbsiPeriodic() { // --- Read voltage & total current --- double voltage = m_pdm.getVoltage(); double totalCurrent = m_pdm.getTotalCurrent(); // --- Safety alerts --- - if (totalCurrent > PowerConstants.kTotalMaxCurrent) { - new Alert("Total current draw exceeds limit!", AlertType.WARNING).set(true); - } + totalCurrentAlert.set(totalCurrent > PowerConstants.kTotalMaxCurrent); for (int ch = 0; ch < m_pdm.getNumChannels(); ch++) { - double current = m_pdm.getCurrent(ch); - if (current > PowerConstants.kMotorPortMaxCurrent) { - new Alert("Port " + ch + " current exceeds limit!", AlertType.WARNING).set(true); - } + portAlerts[ch].set(m_pdm.getCurrent(ch) > PowerConstants.kMotorPortMaxCurrent); } // if (voltage < PowerConstants.kVoltageWarning) { diff --git a/src/main/java/frc/robot/util/RBSISubsystem.java b/src/main/java/frc/robot/util/RBSISubsystem.java index 9755675..3aa3b8b 100644 --- a/src/main/java/frc/robot/util/RBSISubsystem.java +++ b/src/main/java/frc/robot/util/RBSISubsystem.java @@ -13,6 +13,7 @@ package frc.robot.util; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; /** * This class is designed to include Az-RBSI specific methods on top of the standard WPILib @@ -20,7 +21,29 @@ * etc.) should subclass ``RBSISubsystem`` rather than ``SubsystemBase`` in order to gain access to * added functionality. */ -public class RBSISubsystem extends SubsystemBase { +public abstract class RBSISubsystem extends SubsystemBase { + private final String name = getClass().getSimpleName(); + + /** + * Guaranteed timing wrapper (cannot be bypassed by subclasses). + * + *

DO NOT OVERRIDE. + * + *

Subsystems must implement {@link #rbsiPeriodic()} instead. + * + *

If you see a compiler error here, remove your periodic() override and move your logic into + * rbsiPeriodic(). + */ + @Deprecated(forRemoval = false) + public final void periodic() { + long start = System.nanoTime(); + rbsiPeriodic(); + long end = System.nanoTime(); + Logger.recordOutput("Loop/Mech/" + name + "_ms", (end - start) / 1e6); + } + + /** Subclasses must implement this instead of periodic(). */ + protected abstract void rbsiPeriodic(); /** * Gets the power ports associated with this Subsystem. diff --git a/src/main/java/frc/robot/util/RobotDeviceId.java b/src/main/java/frc/robot/util/RobotDeviceId.java index 18005ff..e51ccba 100644 --- a/src/main/java/frc/robot/util/RobotDeviceId.java +++ b/src/main/java/frc/robot/util/RobotDeviceId.java @@ -50,6 +50,7 @@ public String getBus() { /** Get the CTRE CANBus object for a named device */ public CANBus getCANBus() { + return new CANBus(m_CANBus); } diff --git a/src/main/java/frc/robot/util/VirtualSubsystem.java b/src/main/java/frc/robot/util/VirtualSubsystem.java index 534aed0..2996aa3 100644 --- a/src/main/java/frc/robot/util/VirtualSubsystem.java +++ b/src/main/java/frc/robot/util/VirtualSubsystem.java @@ -11,12 +11,14 @@ import java.util.ArrayList; import java.util.List; +import org.littletonrobotics.junction.Logger; /** * Base class for virtual subsystems -- not robot hardware -- that should be treated as subsystems */ public abstract class VirtualSubsystem { - private static List subsystems = new ArrayList<>(); + private static final List subsystems = new ArrayList<>(); + private final String name = getClass().getSimpleName(); // Load all defined virtual subsystems into a list public VirtualSubsystem() { @@ -30,6 +32,23 @@ public static void periodicAll() { } } - // Each virtual subsystem must implement its own periodic() method - public abstract void periodic(); + /** + * Guaranteed timing wrapper (cannot be bypassed by subclasses). + * + *

DO NOT OVERRIDE. + * + *

Subsystems must implement {@link #rbsiPeriodic()} instead. + * + *

If you see a compiler error here, remove your periodic() override and move your logic into + * rbsiPeriodic(). + */ + @Deprecated(forRemoval = false) + public final void periodic() { + long start = System.nanoTime(); + rbsiPeriodic(); + Logger.recordOutput("Loop/Virtual/" + name + "_ms", (System.nanoTime() - start) / 1e6); + } + + /** Subclasses must implement this instead of periodic(). */ + protected abstract void rbsiPeriodic(); } diff --git a/src/main/java/frc/robot/util/YagslConstants.java b/src/main/java/frc/robot/util/YagslConstants.java index 9a79f9f..1542f85 100644 --- a/src/main/java/frc/robot/util/YagslConstants.java +++ b/src/main/java/frc/robot/util/YagslConstants.java @@ -91,8 +91,6 @@ public class YagslConstants { physicalPropertiesJson.conversionFactors.drive.gearRatio; public static final double kSteerGearRatio = physicalPropertiesJson.conversionFactors.angle.gearRatio; - public static final double kWheelRadiusInches = - physicalPropertiesJson.conversionFactors.drive.diameter / 2.0; public static final String kCANbusName = swerveDriveJson.imu.canbus; public static final int kPigeonId = swerveDriveJson.imu.id;