diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9e33d5d..92c4536 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -262,19 +262,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 +334,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 +362,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..0ebfbe5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -140,7 +140,8 @@ public void robotPeriodic() { @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 +151,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 +162,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 +204,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 +249,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..8bb188e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -64,7 +64,6 @@ import frc.robot.util.OverrideSwitches; import frc.robot.util.RBSIEnum.AutoType; 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; @@ -98,8 +97,8 @@ public class RobotContainer { @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; @@ -217,7 +216,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()) { @@ -442,11 +441,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 +454,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/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 91129b7..286a863 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; @@ -80,13 +81,7 @@ public class Drive extends SubsystemBase { private SwerveDrivePoseEstimator m_PoseEstimator = new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, 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; @@ -94,6 +89,17 @@ public class Drive extends SubsystemBase { public Drive(ImuIO imuIO) { this.imuIO = imuIO; + // 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 @@ -167,7 +173,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); @@ -354,7 +368,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 +391,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 ************************************** */ @@ -510,6 +527,16 @@ 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. */ @@ -523,11 +550,13 @@ public void zeroHeadingForAlliance() { DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? Rotation2d.kZero : Rotation2d.k180deg); + resetHeadingController(); } /** Zeros the heading */ public void zeroHeading() { imuIO.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..d902948 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOBlended.java @@ -199,8 +199,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 @@ -447,7 +447,7 @@ public void setTurnPosition(Rotation2d rotation) { .withDriveMotorGearRatio(kDriveGearRatio) .withSteerMotorGearRatio(kSteerGearRatio) .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadiusMeters) + .withWheelRadius(DrivebaseConstants.kWheelRadiusMeters) .withSteerInertia(kSteerInertia) .withDriveInertia(kDriveInertia) .withSteerFrictionVoltage(kSteerFrictionVoltage) diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index a8470d8..b9614d2 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -150,8 +150,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..5a80997 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXS.java @@ -147,7 +147,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..57e89d5 100644 --- a/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/SwerveConstants.java @@ -32,8 +32,6 @@ 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; @@ -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,8 +110,6 @@ 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); kPigeonId = TunerConstants.DrivetrainConstants.Pigeon2Id; @@ -124,7 +119,6 @@ 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; @@ -198,8 +192,6 @@ 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; @@ -209,7 +201,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/util/RBSISubsystem.java b/src/main/java/frc/robot/util/RBSISubsystem.java index 9755675..2eee8a6 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,28 @@ * 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(); + Logger.recordOutput("LoggedRobot/" + name + "CodeMS", System.nanoTime() - 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/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;