From 44351afdc8bcc48a8b236bbe0a41fe931bbbd8ec Mon Sep 17 00:00:00 2001 From: SeanM Date: Fri, 3 Oct 2025 23:35:40 -0400 Subject: [PATCH 1/3] the beginnings of a viable simulated drivetrain --- src/main/java/frc/robot/RobotContainer.java | 1 + .../swervedrive/ControllerDelegate.java | 7 + .../swervedrive/SwerveDriveCommand.java | 11 -- .../java/frc/robot/sim/SwerveModuleSim.java | 2 +- .../java/frc/robot/subsystems/LedStrand.java | 1 - .../subsystems/drivetrain/Drivetrain.java | 160 +++++++++++------- 6 files changed, 112 insertions(+), 70 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 72a11be..937c22d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -148,6 +148,7 @@ public class RobotContainer { private final CommandXboxController manipController = new CommandXboxController(Constants.Controller.MANIPULATION_CONTROLLER_CHANNEL); + private final CommandXboxController debugController = new CommandXboxController(Constants.Controller.DEBUG_CONTROLLER_CHANNEL); diff --git a/src/main/java/frc/robot/commands/swervedrive/ControllerDelegate.java b/src/main/java/frc/robot/commands/swervedrive/ControllerDelegate.java index c73e9bc..ba711cd 100644 --- a/src/main/java/frc/robot/commands/swervedrive/ControllerDelegate.java +++ b/src/main/java/frc/robot/commands/swervedrive/ControllerDelegate.java @@ -41,6 +41,13 @@ public ControllerDelegate( this.driver = driver; } + public void logRawAxes() { + for (int i = 0; i < 10; i++) { + double value = edu.wpi.first.wpilibj.DriverStation.getStickAxis(0, i); + System.out.println("Axis[" + i + "]: " + value); + } + } + public double getLeftX() { return this.leftXSupplier.getAsDouble(); } diff --git a/src/main/java/frc/robot/commands/swervedrive/SwerveDriveCommand.java b/src/main/java/frc/robot/commands/swervedrive/SwerveDriveCommand.java index d3a983d..72dc545 100644 --- a/src/main/java/frc/robot/commands/swervedrive/SwerveDriveCommand.java +++ b/src/main/java/frc/robot/commands/swervedrive/SwerveDriveCommand.java @@ -63,17 +63,6 @@ public void execute() { ySpeed = x * this.driveMaxSpeed * acceleration * elevatorDecelerationRatio; } - // TODO: x and y are assigned to and never used again. I'm assuming this is just an oversight in the code. - // To be clear, this is an issue because x and y and on the local scope of this method... did the author intend - // for their values to be normalized and retained across executions... because they're not, as they are - // stack-level - // variables so commenting this out... - // double normalizingFactor = Math.hypot(x, y); - // if (normalizingFactor > 0) { - // x /= normalizingFactor; - // y /= normalizingFactor; - // } - if (this.controllerDelegate.isHalfSpeed()) { xSpeed /= 2; ySpeed /= 2; diff --git a/src/main/java/frc/robot/sim/SwerveModuleSim.java b/src/main/java/frc/robot/sim/SwerveModuleSim.java index fc83a0f..62cb66b 100644 --- a/src/main/java/frc/robot/sim/SwerveModuleSim.java +++ b/src/main/java/frc/robot/sim/SwerveModuleSim.java @@ -41,7 +41,7 @@ public void update(double dtSeconds) { this.driveDistanceMeters += wheelSpeed * dtSeconds; double turnRotations = turnMotorSim.getAngularPositionRotations(); - this.turnAngle = Rotation2d.fromRotations(turnRotations % 1.0); + this.turnAngle = Rotation2d.fromRotations(turnRotations); } public void setDriveVoltage(double volts) { diff --git a/src/main/java/frc/robot/subsystems/LedStrand.java b/src/main/java/frc/robot/subsystems/LedStrand.java index 7023c38..184f927 100644 --- a/src/main/java/frc/robot/subsystems/LedStrand.java +++ b/src/main/java/frc/robot/subsystems/LedStrand.java @@ -31,7 +31,6 @@ public void stopLed() { } public void changeLed(int r, int g, int b) { - System.out.println("Here"); sendSerialCommand("R" + r + "G" + g + "B" + b); } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 01f91f7..cabeb64 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -10,6 +10,7 @@ import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.studica.frc.AHRS; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -41,6 +42,8 @@ */ public class Drivetrain extends SubsystemBase { + private static final double NOMINAL_BATT_VOLTS = 12.0; + private final DrivetrainContext context; private final Field2d field = new Field2d(); @@ -87,7 +90,7 @@ public class Drivetrain extends SubsystemBase { private boolean wheelLock = false; - private boolean fieldRelativeEnable = true; + private boolean fieldRelativeEnable = false; private Pose2d goalPose; @@ -110,8 +113,7 @@ public Drivetrain() { /** * Instantiates a new Drivetrain subsystem with the specified settings * - * @param context - * The DrivetrainSettings to apply to this instance + * @param context The DrivetrainSettings to apply to this instance */ public Drivetrain(final DrivetrainContext context) { requireNonNull(context, "DrivetrainContext cannot be null"); @@ -242,9 +244,9 @@ public void setFieldRelativeEnable(boolean enable) { * Gets our current position in meters on the field. * * @return A current position on the field. - *

- * A translation2d (X and Y on the field) -> {@link #swerveDriveKinematics} + A rotation2d (Rot X and Y - * on the field) -> {@link #navXSensorModule} + *

+ * A translation2d (X and Y on the field) -> {@link #swerveDriveKinematics} + A rotation2d (Rot X and Y + * on the field) -> {@link #navXSensorModule} */ private Pose2d getPose2d() { return this.swerveDriveOdometry.getPoseMeters(); @@ -299,10 +301,12 @@ private void driveRobotRelative(final ChassisSpeeds robotRelativeSpeeds) { * Updates our current Odometry */ private void updateOdometry() { + if (RobotBase.isSimulation()) return; this.swerveDriveOdometry.update(this.navXSensorModule.getRotation2d(), this.getSwerveModulePositions()); } private void updatePoseEstimatorOdometry() { + if (RobotBase.isSimulation()) return; this.swerveDrivePoseEstimator.update(this.getHeading(), this.getSwerveModulePositions()); boolean doRejectUpdate = false; @@ -428,24 +432,34 @@ public void stopModules() { /** * Method to drive the robot using joystick info. * - * @param xSpeed - * Speed of the robot in the x direction (forward). - * @param ySpeed - * Speed of the robot in the y direction (sideways). - * @param rot - * Angular rate of the robot. + * @param xSpeed Speed of the robot in the x direction (forward). + * @param ySpeed Speed of the robot in the y direction (sideways). + * @param rot Angular rate of the robot. */ public void drive(double xSpeed, double ySpeed, double rot) { - SmartDashboard.putNumber(getName() + "/Command/X Speed", xSpeed); - SmartDashboard.putNumber(getName() + "/Command/Y Speed", ySpeed); - SmartDashboard.putNumber(getName() + "/Command/Rot Speed", rot); - SmartDashboard.putBoolean(getName() + "/Command/RobotRelative", this.fieldRelativeEnable); - Rotation2d robotRotation = - new Rotation2d(navXSensorModule.getRotation2d().getRadians()); - this.desiredStates = this.swerveDriveKinematics.toSwerveModuleStates( - this.fieldRelativeEnable - ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, robotRotation) - : new ChassisSpeeds(xSpeed, ySpeed, rot)); + // Commanded inputs (m/s, rad/s) + SmartDashboard.putNumber(getName() + "/Cmd/xSpeed_in", xSpeed); + SmartDashboard.putNumber(getName() + "/Cmd/ySpeed_in", ySpeed); + SmartDashboard.putNumber(getName() + "/Cmd/rot_in", rot); + SmartDashboard.putBoolean(getName() + "/Cmd/FieldRelative", this.fieldRelativeEnable); + + Rotation2d rawHeading = getHeading(); + + ChassisSpeeds speeds = fieldRelativeEnable + ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, rawHeading) + : new ChassisSpeeds(xSpeed, ySpeed, rot); + + // IMPORTANT: NavX is CW+, WPILib is CCW+. Negate heading for fromFieldRelativeSpeeds. + SmartDashboard.putNumber(getName() + "/HeadingUsedDeg", rawHeading.getDegrees()); + + // What we will pass to kinematics + SmartDashboard.putNumber(getName() + "/Chassis/vx", speeds.vxMetersPerSecond); + SmartDashboard.putNumber(getName() + "/Chassis/vy", speeds.vyMetersPerSecond); + SmartDashboard.putNumber(getName() + "/Chassis/omega", speeds.omegaRadiansPerSecond); + + // Compute and apply + this.desiredStates = this.swerveDriveKinematics.toSwerveModuleStates(speeds); + SwerveDriveKinematics.desaturateWheelSpeeds(this.desiredStates, SwerveModule.DRIVE_MAX_SPEED); if (!this.wheelLock) { this.setModuleStates(this.desiredStates); @@ -531,56 +545,88 @@ public void simulationPeriodic() { // 1. Compute elapsed time since last loop double currentTime = Timer.getFPGATimestamp(); double dt = currentTime - lastSimTime; - this.lastSimTime = currentTime; + lastSimTime = currentTime; - // 2. Update each simulated swerve module using stored percent outputs - frontLeftSwerveModuleSim.setDriveVoltage(frontLeftSwerveModule.getLastDrivePercent() * 12.0); - frontLeftSwerveModuleSim.setTurnVoltage(frontLeftSwerveModule.getLastTurnPercent() * 12.0); - frontLeftSwerveModuleSim.update(dt); + // 2. Skip if no desired states yet (e.g., before first drive command) + if (desiredStates == null) { + return; + } - frontRightSwerveModuleSim.setDriveVoltage(frontRightSwerveModule.getLastDrivePercent() * 12.0); - frontRightSwerveModuleSim.setTurnVoltage(frontRightSwerveModule.getLastTurnPercent() * 12.0); + // --- constants for sim behavior --- + final double kMaxSpeed = this.getMaxSpeed(); // m/s + final double kAzimuthP = 6.0; // V per radian (tune 4–10) + final double kDriveVPerMS = NOMINAL_BATT_VOLTS / kMaxSpeed; // volts per (m/s) + + // 3. Compute each module’s commanded (optimized) state + var flCmd = SwerveModuleState.optimize(desiredStates[0], frontLeftSwerveModuleSim.getTurnAngle()); + var frCmd = SwerveModuleState.optimize(desiredStates[1], frontRightSwerveModuleSim.getTurnAngle()); + var blCmd = SwerveModuleState.optimize(desiredStates[2], rearLeftSwerveModuleSim.getTurnAngle()); + var brCmd = SwerveModuleState.optimize(desiredStates[3], rearRightSwerveModuleSim.getTurnAngle()); + + // 4. Apply drive voltages (scale m/s → ±12 V) + frontLeftSwerveModuleSim.setDriveVoltage(Math.copySign( + Math.min(Math.abs(flCmd.speedMetersPerSecond) * kDriveVPerMS, NOMINAL_BATT_VOLTS), + flCmd.speedMetersPerSecond)); + frontRightSwerveModuleSim.setDriveVoltage(Math.copySign( + Math.min(Math.abs(frCmd.speedMetersPerSecond) * kDriveVPerMS, NOMINAL_BATT_VOLTS), + frCmd.speedMetersPerSecond)); + rearLeftSwerveModuleSim.setDriveVoltage(Math.copySign( + Math.min(Math.abs(blCmd.speedMetersPerSecond) * kDriveVPerMS, NOMINAL_BATT_VOLTS), + blCmd.speedMetersPerSecond)); + rearRightSwerveModuleSim.setDriveVoltage(Math.copySign( + Math.min(Math.abs(brCmd.speedMetersPerSecond) * kDriveVPerMS, NOMINAL_BATT_VOLTS), + brCmd.speedMetersPerSecond)); + + // 5. Apply turn voltages (simple proportional control on angle error) + double flErr = + flCmd.angle.minus(frontLeftSwerveModuleSim.getTurnAngle()).getRadians(); + double frErr = + frCmd.angle.minus(frontRightSwerveModuleSim.getTurnAngle()).getRadians(); + double blErr = blCmd.angle.minus(rearLeftSwerveModuleSim.getTurnAngle()).getRadians(); + double brErr = + brCmd.angle.minus(rearRightSwerveModuleSim.getTurnAngle()).getRadians(); + + frontLeftSwerveModuleSim.setTurnVoltage( + MathUtil.clamp(kAzimuthP * flErr, -NOMINAL_BATT_VOLTS, NOMINAL_BATT_VOLTS)); + frontRightSwerveModuleSim.setTurnVoltage( + MathUtil.clamp(kAzimuthP * frErr, -NOMINAL_BATT_VOLTS, NOMINAL_BATT_VOLTS)); + rearLeftSwerveModuleSim.setTurnVoltage( + MathUtil.clamp(kAzimuthP * blErr, -NOMINAL_BATT_VOLTS, NOMINAL_BATT_VOLTS)); + rearRightSwerveModuleSim.setTurnVoltage( + MathUtil.clamp(kAzimuthP * brErr, -NOMINAL_BATT_VOLTS, NOMINAL_BATT_VOLTS)); + + // 6. Update each simulated module + frontLeftSwerveModuleSim.update(dt); frontRightSwerveModuleSim.update(dt); - - rearLeftSwerveModuleSim.setDriveVoltage(rearLeftSwerveModule.getLastDrivePercent() * 12.0); - rearLeftSwerveModuleSim.setTurnVoltage(rearLeftSwerveModule.getLastTurnPercent() * 12.0); rearLeftSwerveModuleSim.update(dt); - - rearRightSwerveModuleSim.setDriveVoltage(rearRightSwerveModule.getLastDrivePercent() * 12.0); - rearRightSwerveModuleSim.setTurnVoltage(rearRightSwerveModule.getLastTurnPercent() * 12.0); rearRightSwerveModuleSim.update(dt); - // 3. Build simulated module states for kinematics + // 7. Build module states for kinematics SwerveModuleState[] states = new SwerveModuleState[] { new SwerveModuleState( - this.frontLeftSwerveModuleSim.getWheelSpeedMetersPerSecond(), - this.frontLeftSwerveModuleSim.getTurnAngle()), + frontLeftSwerveModuleSim.getWheelSpeedMetersPerSecond(), frontLeftSwerveModuleSim.getTurnAngle()), new SwerveModuleState( - this.frontRightSwerveModuleSim.getWheelSpeedMetersPerSecond(), - this.frontRightSwerveModuleSim.getTurnAngle()), + frontRightSwerveModuleSim.getWheelSpeedMetersPerSecond(), frontRightSwerveModuleSim.getTurnAngle()), new SwerveModuleState( - this.rearLeftSwerveModuleSim.getWheelSpeedMetersPerSecond(), - this.rearLeftSwerveModuleSim.getTurnAngle()), + rearLeftSwerveModuleSim.getWheelSpeedMetersPerSecond(), rearLeftSwerveModuleSim.getTurnAngle()), new SwerveModuleState( - this.rearRightSwerveModuleSim.getWheelSpeedMetersPerSecond(), - this.rearRightSwerveModuleSim.getTurnAngle()) + rearRightSwerveModuleSim.getWheelSpeedMetersPerSecond(), rearRightSwerveModuleSim.getTurnAngle()) }; - // 4. Convert to chassis speeds and integrate heading - ChassisSpeeds chassisSpeeds = this.swerveDriveKinematics.toChassisSpeeds(states); - Rotation2d delta = new Rotation2d(chassisSpeeds.omegaRadiansPerSecond * dt); - this.simYaw = this.simYaw.rotateBy(delta); - - // 5. Update pose estimator with sim yaw and module positions - this.swerveDrivePoseEstimator.update(this.simYaw, new SwerveModulePosition[] { - this.frontLeftSwerveModuleSim.getPosition(), - this.frontRightSwerveModuleSim.getPosition(), - this.rearLeftSwerveModuleSim.getPosition(), - this.rearRightSwerveModuleSim.getPosition() + // 8. Convert to chassis speeds and integrate heading + ChassisSpeeds chassisSpeeds = swerveDriveKinematics.toChassisSpeeds(states); + simYaw = simYaw.plus(Rotation2d.fromRadians(chassisSpeeds.omegaRadiansPerSecond * dt)); + + // 9. Update pose estimator with sim yaw and module positions + swerveDrivePoseEstimator.update(simYaw, new SwerveModulePosition[] { + frontLeftSwerveModuleSim.getPosition(), + frontRightSwerveModuleSim.getPosition(), + rearLeftSwerveModuleSim.getPosition(), + rearRightSwerveModuleSim.getPosition() }); - // 6. Push pose to Field2d for visualization - this.field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition()); + // 10. Push pose to Field2d for visualization + field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition()); } @Override From cedcb8ff5f908cb86bd13d1ad1c3dc6ae3269b40 Mon Sep 17 00:00:00 2001 From: SeanM Date: Sun, 5 Oct 2025 09:03:30 -0400 Subject: [PATCH 2/3] increase SIM_VOLTS_PER_RADIAN_TURN_VOLTAGE constant so that turn voltage in sim results in a tighter turn when changing driving direction. additional variable naming factor and constants --- src/main/java/frc/robot/Constants.java | 1 + .../subsystems/drivetrain/Drivetrain.java | 218 +++++++++--------- 2 files changed, 109 insertions(+), 110 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ae155a9..e2a1133 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -25,6 +25,7 @@ public static final class DriverLabels { } public static class Drive { + // (x, y) position of each module relative to the robot center (center of rotation) public static final Translation2d SMFrontRightLocation = new Translation2d(0.285, -0.285); public static final Translation2d SMFrontLeftLocation = new Translation2d(0.285, 0.285); public static final Translation2d SMBackLeftLocation = new Translation2d(-0.285, 0.285); diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index cabeb64..1fbbad5 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -4,13 +4,15 @@ package frc.robot.subsystems.drivetrain; +import static edu.wpi.first.math.MathUtil.clamp; +import static edu.wpi.first.math.kinematics.SwerveModuleState.optimize; +import static java.lang.Math.min; import static java.util.Objects.requireNonNull; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.studica.frc.AHRS; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -37,13 +39,18 @@ /** * Represents a swerve drive style drivetrain. In here, we initialize our swerve modules (example -> - * {@link #frontLeftSwerveModule}), Get input from autonomous and initialize our odometry -> + * {@link #flSwerve}), Get input from autonomous and initialize our odometry -> * {@link #swerveDriveOdometry}. Various other DriveTrain Related thing are initialized here too. */ public class Drivetrain extends SubsystemBase { private static final double NOMINAL_BATT_VOLTS = 12.0; + private final double SIM_VOLTS_PER_RADIAN_TURN_VOLTAGE = 24.0; + + // Scale from desired wheel speed (meters-per-second) to motor voltage (V) + private final double simVoltsMetPerSec; + private final DrivetrainContext context; private final Field2d field = new Field2d(); @@ -53,21 +60,21 @@ public class Drivetrain extends SubsystemBase { // The gyro object. Gyro gives the robots rotation/ where the robot is pointed. private final AHRS navXSensorModule; - public final SwerveModule frontLeftSwerveModule; + public final SwerveModule flSwerve; - public final SwerveModule frontRightSwerveModule; + public final SwerveModule frSwerve; - public final SwerveModule rearLeftSwerveModule; + public final SwerveModule rlSwerve; - public final SwerveModule rearRightSwerveModule; + public final SwerveModule rrSwerve; - private final SwerveModuleSim frontLeftSwerveModuleSim; + private final SwerveModuleSim flSwerveSim; - private final SwerveModuleSim frontRightSwerveModuleSim; + private final SwerveModuleSim frSwerveSim; - private final SwerveModuleSim rearLeftSwerveModuleSim; + private final SwerveModuleSim rlSwerveSim; - private final SwerveModuleSim rearRightSwerveModuleSim; + private final SwerveModuleSim rrSwerveSim; private final SwerveDriveOdometry swerveDriveOdometry; @@ -101,8 +108,6 @@ public class Drivetrain extends SubsystemBase { private double lastSimTime = Timer.getFPGATimestamp(); - private SwerveModuleState[] CurrentStates; // TODO: Unused? Eliminate - /** * Instantiates a new Drivetrain with default {@link DrivetrainContext} */ @@ -120,6 +125,8 @@ public Drivetrain(final DrivetrainContext context) { this.context = context; + this.simVoltsMetPerSec = NOMINAL_BATT_VOLTS / this.getMaxSpeed(); + NetworkTableInstance nti = NetworkTableInstance.getDefault(); this.desiredStatePublisher = nti.getStructArrayTopic("DesiredStates", SwerveModuleState.struct) @@ -195,15 +202,15 @@ public Drivetrain(final DrivetrainContext context) { Constants.Drive.SMBackLeftLocation, Constants.Drive.SMBackRightLocation); - this.frontRightSwerveModule = new SwerveModule(this.context.getFrontRightSwerveModuleContext()); - this.frontLeftSwerveModule = new SwerveModule(this.context.getFrontLeftSwerveModuleContext()); - this.rearLeftSwerveModule = new SwerveModule(this.context.getRearLeftSwerveModuleContext()); - this.rearRightSwerveModule = new SwerveModule(this.context.getRearRightSwerveModuleContext()); + this.frSwerve = new SwerveModule(this.context.getFrontRightSwerveModuleContext()); + this.flSwerve = new SwerveModule(this.context.getFrontLeftSwerveModuleContext()); + this.rlSwerve = new SwerveModule(this.context.getRearLeftSwerveModuleContext()); + this.rrSwerve = new SwerveModule(this.context.getRearRightSwerveModuleContext()); - this.frontRightSwerveModuleSim = new SwerveModuleSim(context.getFrontRightSwerveModuleContext()); - this.frontLeftSwerveModuleSim = new SwerveModuleSim(context.getFrontLeftSwerveModuleContext()); - this.rearLeftSwerveModuleSim = new SwerveModuleSim(context.getRearLeftSwerveModuleContext()); - this.rearRightSwerveModuleSim = new SwerveModuleSim(context.getRearRightSwerveModuleContext()); + this.frSwerveSim = new SwerveModuleSim(context.getFrontRightSwerveModuleContext()); + this.flSwerveSim = new SwerveModuleSim(context.getFrontLeftSwerveModuleContext()); + this.rlSwerveSim = new SwerveModuleSim(context.getRearLeftSwerveModuleContext()); + this.rrSwerveSim = new SwerveModuleSim(context.getRearRightSwerveModuleContext()); // initializes odometry this.swerveDriveOdometry = new SwerveDriveOdometry( @@ -217,10 +224,10 @@ public Drivetrain(final DrivetrainContext context) { this.context.getStateStdDevs(), this.context.getVisionMeasurementStdDevs()); - this.addChild(frontLeftSwerveModule.getName(), frontLeftSwerveModule); - this.addChild(frontRightSwerveModule.getName(), frontRightSwerveModule); - this.addChild(rearLeftSwerveModule.getName(), rearLeftSwerveModule); - this.addChild(rearRightSwerveModule.getName(), rearRightSwerveModule); + this.addChild(flSwerve.getName(), flSwerve); + this.addChild(frSwerve.getName(), frSwerve); + this.addChild(rlSwerve.getName(), rlSwerve); + this.addChild(rrSwerve.getName(), rrSwerve); this.addChild("navx", this.navXSensorModule); } @@ -261,20 +268,20 @@ private Pose2d getPose2dEstimator() { */ private void setModuleStates(SwerveModuleState[] swerveModuleStates) { SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, SwerveModule.DRIVE_MAX_SPEED); - this.frontLeftSwerveModule.setDesiredState(swerveModuleStates[0]); - this.frontRightSwerveModule.setDesiredState(swerveModuleStates[1]); - this.rearLeftSwerveModule.setDesiredState(swerveModuleStates[2]); - this.rearRightSwerveModule.setDesiredState(swerveModuleStates[3]); + this.flSwerve.setDesiredState(swerveModuleStates[0]); + this.frSwerve.setDesiredState(swerveModuleStates[1]); + this.rlSwerve.setDesiredState(swerveModuleStates[2]); + this.rrSwerve.setDesiredState(swerveModuleStates[3]); } /** * Tells our wheels to go to the Wheel Locking position (0 m/s, forming an X) */ private void lockWheels() { - this.rearLeftSwerveModule.setDesiredState(this.context.getFullStopAt135Degrees()); - this.frontLeftSwerveModule.setDesiredState(this.context.getFullStopAt45Degrees()); - this.rearRightSwerveModule.setDesiredState(this.context.getFullStopAt45Degrees()); - this.frontRightSwerveModule.setDesiredState(this.context.getFullStopAt135Degrees()); + this.rlSwerve.setDesiredState(this.context.getFullStopAt135Degrees()); + this.flSwerve.setDesiredState(this.context.getFullStopAt45Degrees()); + this.rrSwerve.setDesiredState(this.context.getFullStopAt45Degrees()); + this.frSwerve.setDesiredState(this.context.getFullStopAt135Degrees()); } /** @@ -285,10 +292,10 @@ private void lockWheels() { */ private SwerveModulePosition[] getSwerveModulePositions() { return new SwerveModulePosition[] { - this.frontLeftSwerveModule.getModulePosition(), - this.frontRightSwerveModule.getModulePosition(), - this.rearLeftSwerveModule.getModulePosition(), - this.rearRightSwerveModule.getModulePosition() + this.flSwerve.getModulePosition(), + this.frSwerve.getModulePosition(), + this.rlSwerve.getModulePosition(), + this.rrSwerve.getModulePosition() }; } @@ -406,10 +413,10 @@ private ChassisSpeeds getChassisSpeeds() { */ private SwerveModuleState[] getSwerveModuleStates() { return new SwerveModuleState[] { - this.frontLeftSwerveModule.getModuleState(), - this.frontRightSwerveModule.getModuleState(), - this.rearLeftSwerveModule.getModuleState(), - this.rearRightSwerveModule.getModuleState() + this.flSwerve.getModuleState(), + this.frSwerve.getModuleState(), + this.rlSwerve.getModuleState(), + this.rrSwerve.getModuleState() }; } @@ -423,10 +430,10 @@ private Pose2d refreshGoalPose2d() { * Stops all the motors on the SwerveModules */ public void stopModules() { - this.frontLeftSwerveModule.stopMotors(); - this.frontRightSwerveModule.stopMotors(); - this.rearLeftSwerveModule.stopMotors(); - this.rearRightSwerveModule.stopMotors(); + this.flSwerve.stopMotors(); + this.frSwerve.stopMotors(); + this.rlSwerve.stopMotors(); + this.rrSwerve.stopMotors(); } /** @@ -544,89 +551,80 @@ public void periodic() { public void simulationPeriodic() { // 1. Compute elapsed time since last loop double currentTime = Timer.getFPGATimestamp(); - double dt = currentTime - lastSimTime; - lastSimTime = currentTime; + double dt = currentTime - this.lastSimTime; + this.lastSimTime = currentTime; // 2. Skip if no desired states yet (e.g., before first drive command) - if (desiredStates == null) { + if (this.desiredStates == null) { return; } - // --- constants for sim behavior --- - final double kMaxSpeed = this.getMaxSpeed(); // m/s - final double kAzimuthP = 6.0; // V per radian (tune 4–10) - final double kDriveVPerMS = NOMINAL_BATT_VOLTS / kMaxSpeed; // volts per (m/s) - // 3. Compute each module’s commanded (optimized) state - var flCmd = SwerveModuleState.optimize(desiredStates[0], frontLeftSwerveModuleSim.getTurnAngle()); - var frCmd = SwerveModuleState.optimize(desiredStates[1], frontRightSwerveModuleSim.getTurnAngle()); - var blCmd = SwerveModuleState.optimize(desiredStates[2], rearLeftSwerveModuleSim.getTurnAngle()); - var brCmd = SwerveModuleState.optimize(desiredStates[3], rearRightSwerveModuleSim.getTurnAngle()); + SwerveModuleState frontLeftOptimized = optimize(this.desiredStates[0], this.flSwerveSim.getTurnAngle()); + SwerveModuleState frontRightOptimized = optimize(this.desiredStates[1], this.frSwerveSim.getTurnAngle()); + SwerveModuleState rearLeftOptimized = optimize(this.desiredStates[2], this.rlSwerveSim.getTurnAngle()); + SwerveModuleState rearRightOptimized = optimize(this.desiredStates[3], this.rrSwerveSim.getTurnAngle()); // 4. Apply drive voltages (scale m/s → ±12 V) - frontLeftSwerveModuleSim.setDriveVoltage(Math.copySign( - Math.min(Math.abs(flCmd.speedMetersPerSecond) * kDriveVPerMS, NOMINAL_BATT_VOLTS), - flCmd.speedMetersPerSecond)); - frontRightSwerveModuleSim.setDriveVoltage(Math.copySign( - Math.min(Math.abs(frCmd.speedMetersPerSecond) * kDriveVPerMS, NOMINAL_BATT_VOLTS), - frCmd.speedMetersPerSecond)); - rearLeftSwerveModuleSim.setDriveVoltage(Math.copySign( - Math.min(Math.abs(blCmd.speedMetersPerSecond) * kDriveVPerMS, NOMINAL_BATT_VOLTS), - blCmd.speedMetersPerSecond)); - rearRightSwerveModuleSim.setDriveVoltage(Math.copySign( - Math.min(Math.abs(brCmd.speedMetersPerSecond) * kDriveVPerMS, NOMINAL_BATT_VOLTS), - brCmd.speedMetersPerSecond)); + this.flSwerveSim.setDriveVoltage(Math.copySign( + min(Math.abs(frontLeftOptimized.speedMetersPerSecond) * this.simVoltsMetPerSec, NOMINAL_BATT_VOLTS), + frontLeftOptimized.speedMetersPerSecond)); + this.frSwerveSim.setDriveVoltage(Math.copySign( + min(Math.abs(frontRightOptimized.speedMetersPerSecond) * this.simVoltsMetPerSec, NOMINAL_BATT_VOLTS), + frontRightOptimized.speedMetersPerSecond)); + this.rlSwerveSim.setDriveVoltage(Math.copySign( + min(Math.abs(rearLeftOptimized.speedMetersPerSecond) * this.simVoltsMetPerSec, NOMINAL_BATT_VOLTS), + rearLeftOptimized.speedMetersPerSecond)); + this.rrSwerveSim.setDriveVoltage(Math.copySign( + min(Math.abs(rearRightOptimized.speedMetersPerSecond) * this.simVoltsMetPerSec, NOMINAL_BATT_VOLTS), + rearRightOptimized.speedMetersPerSecond)); // 5. Apply turn voltages (simple proportional control on angle error) - double flErr = - flCmd.angle.minus(frontLeftSwerveModuleSim.getTurnAngle()).getRadians(); - double frErr = - frCmd.angle.minus(frontRightSwerveModuleSim.getTurnAngle()).getRadians(); - double blErr = blCmd.angle.minus(rearLeftSwerveModuleSim.getTurnAngle()).getRadians(); - double brErr = - brCmd.angle.minus(rearRightSwerveModuleSim.getTurnAngle()).getRadians(); - - frontLeftSwerveModuleSim.setTurnVoltage( - MathUtil.clamp(kAzimuthP * flErr, -NOMINAL_BATT_VOLTS, NOMINAL_BATT_VOLTS)); - frontRightSwerveModuleSim.setTurnVoltage( - MathUtil.clamp(kAzimuthP * frErr, -NOMINAL_BATT_VOLTS, NOMINAL_BATT_VOLTS)); - rearLeftSwerveModuleSim.setTurnVoltage( - MathUtil.clamp(kAzimuthP * blErr, -NOMINAL_BATT_VOLTS, NOMINAL_BATT_VOLTS)); - rearRightSwerveModuleSim.setTurnVoltage( - MathUtil.clamp(kAzimuthP * brErr, -NOMINAL_BATT_VOLTS, NOMINAL_BATT_VOLTS)); - - // 6. Update each simulated module - frontLeftSwerveModuleSim.update(dt); - frontRightSwerveModuleSim.update(dt); - rearLeftSwerveModuleSim.update(dt); - rearRightSwerveModuleSim.update(dt); + double frontLeftError = + frontLeftOptimized.angle.minus(flSwerveSim.getTurnAngle()).getRadians(); + double frontRightError = + frontRightOptimized.angle.minus(frSwerveSim.getTurnAngle()).getRadians(); + double rearLeftError = + rearLeftOptimized.angle.minus(rlSwerveSim.getTurnAngle()).getRadians(); + double rearRightError = + rearRightOptimized.angle.minus(rrSwerveSim.getTurnAngle()).getRadians(); + + this.flSwerveSim.setTurnVoltage( + clamp(SIM_VOLTS_PER_RADIAN_TURN_VOLTAGE * frontLeftError, -NOMINAL_BATT_VOLTS, NOMINAL_BATT_VOLTS)); + this.frSwerveSim.setTurnVoltage( + clamp(SIM_VOLTS_PER_RADIAN_TURN_VOLTAGE * frontRightError, -NOMINAL_BATT_VOLTS, NOMINAL_BATT_VOLTS)); + this.rlSwerveSim.setTurnVoltage( + clamp(SIM_VOLTS_PER_RADIAN_TURN_VOLTAGE * rearLeftError, -NOMINAL_BATT_VOLTS, NOMINAL_BATT_VOLTS)); + this.rrSwerveSim.setTurnVoltage( + clamp(SIM_VOLTS_PER_RADIAN_TURN_VOLTAGE * rearRightError, -NOMINAL_BATT_VOLTS, NOMINAL_BATT_VOLTS)); + + flSwerveSim.update(dt); + frSwerveSim.update(dt); + rlSwerveSim.update(dt); + rrSwerveSim.update(dt); // 7. Build module states for kinematics SwerveModuleState[] states = new SwerveModuleState[] { - new SwerveModuleState( - frontLeftSwerveModuleSim.getWheelSpeedMetersPerSecond(), frontLeftSwerveModuleSim.getTurnAngle()), - new SwerveModuleState( - frontRightSwerveModuleSim.getWheelSpeedMetersPerSecond(), frontRightSwerveModuleSim.getTurnAngle()), - new SwerveModuleState( - rearLeftSwerveModuleSim.getWheelSpeedMetersPerSecond(), rearLeftSwerveModuleSim.getTurnAngle()), - new SwerveModuleState( - rearRightSwerveModuleSim.getWheelSpeedMetersPerSecond(), rearRightSwerveModuleSim.getTurnAngle()) + new SwerveModuleState(this.flSwerveSim.getWheelSpeedMetersPerSecond(), this.flSwerveSim.getTurnAngle()), + new SwerveModuleState(this.frSwerveSim.getWheelSpeedMetersPerSecond(), this.frSwerveSim.getTurnAngle()), + new SwerveModuleState(this.rlSwerveSim.getWheelSpeedMetersPerSecond(), this.rlSwerveSim.getTurnAngle()), + new SwerveModuleState(this.rrSwerveSim.getWheelSpeedMetersPerSecond(), this.rrSwerveSim.getTurnAngle()) }; // 8. Convert to chassis speeds and integrate heading - ChassisSpeeds chassisSpeeds = swerveDriveKinematics.toChassisSpeeds(states); - simYaw = simYaw.plus(Rotation2d.fromRadians(chassisSpeeds.omegaRadiansPerSecond * dt)); + ChassisSpeeds chassisSpeeds = this.swerveDriveKinematics.toChassisSpeeds(states); + this.simYaw = this.simYaw.plus(Rotation2d.fromRadians(chassisSpeeds.omegaRadiansPerSecond * dt)); // 9. Update pose estimator with sim yaw and module positions - swerveDrivePoseEstimator.update(simYaw, new SwerveModulePosition[] { - frontLeftSwerveModuleSim.getPosition(), - frontRightSwerveModuleSim.getPosition(), - rearLeftSwerveModuleSim.getPosition(), - rearRightSwerveModuleSim.getPosition() + this.swerveDrivePoseEstimator.update(this.simYaw, new SwerveModulePosition[] { + this.flSwerveSim.getPosition(), + this.frSwerveSim.getPosition(), + this.rlSwerveSim.getPosition(), + this.rrSwerveSim.getPosition() }); // 10. Push pose to Field2d for visualization - field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition()); + this.field.setRobotPose(this.swerveDrivePoseEstimator.getEstimatedPosition()); } @Override @@ -663,10 +661,10 @@ public void initSendable(SendableBuilder builder) { builder.addDoubleProperty( "GOALPOSE/ROT", () -> this.refreshGoalPose2d().getRotation().getRadians(), null); - SmartDashboard.putData("DriveTrain/" + this.frontLeftSwerveModule.getName(), this.frontLeftSwerveModule); - SmartDashboard.putData("DriveTrain/" + this.frontRightSwerveModule.getName(), this.frontRightSwerveModule); - SmartDashboard.putData("DriveTrain/" + this.rearLeftSwerveModule.getName(), this.rearLeftSwerveModule); - SmartDashboard.putData("DriveTrain/" + this.rearRightSwerveModule.getName(), this.rearRightSwerveModule); + SmartDashboard.putData("DriveTrain/" + this.flSwerve.getName(), this.flSwerve); + SmartDashboard.putData("DriveTrain/" + this.frSwerve.getName(), this.frSwerve); + SmartDashboard.putData("DriveTrain/" + this.rlSwerve.getName(), this.rlSwerve); + SmartDashboard.putData("DriveTrain/" + this.rrSwerve.getName(), this.rrSwerve); SmartDashboard.putData("field", this.field); builder.addDoubleProperty("GYRO ANGLE", this.navXSensorModule::getAngle, null); From db1ad983cacd67004f317f7ccf1401ceffc23f46 Mon Sep 17 00:00:00 2001 From: SeanM Date: Sun, 5 Oct 2025 14:07:39 -0400 Subject: [PATCH 3/3] naming refactor/cleanup. increase SIM_VOLTS_PER_RADIAN_TURN_VOLTAGE constant again for tighter turns in sim. Bypass crash when running ClimbMechanism in sim by using ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder --- src/main/java/frc/robot/Constants.java | 42 +++++++++---------- .../subsystems/climb/ClimbMechanism.java | 32 ++++++++++---- .../climb/ClimbMechanismContext.java | 12 ++++-- .../subsystems/coral/CoralMechanism.java | 4 +- .../subsystems/drivetrain/Drivetrain.java | 23 +++++----- .../drivetrain/DrivetrainContext.java | 40 +++++++++--------- .../robot/subsystems/elevator/Elevator.java | 2 +- .../subsystems/elevator/ElevatorContext.java | 12 +++--- 8 files changed, 94 insertions(+), 73 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e2a1133..46af315 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -89,23 +89,23 @@ public static class AprilTagID { } public static class Port { - public static final int blSteerMtrC = 1; - public static final int blDriveMtrC = 2; - public static final int flDriveMtrC = 3; - public static final int flSteerMtrC = 4; - public static final int frSteerMtrC = 5; - public static final int frDriveMtrC = 6; - public static final int brDriveMtrC = 7; - public static final int brSteerMtrC = 8; - public static final int elevatorMotorChannel = 11; - public static final int climbMotorChannel = 12; - public static final int coralMotorRightChannel = 13; - public static final int coralMotorLeftChannel = 14; - public static final int elevatorFollowerMotorChannel = 15; - public static final int blTurnEncoderDIOC = DIOChannel.ZERO.getChannel(); - public static final int flTurnEncoderDIOC = DIOChannel.ONE.getChannel(); - public static final int frTurnEncoderDIOC = DIOChannel.TWO.getChannel(); - public static final int brTurnEncoderDIOC = DIOChannel.THREE.getChannel(); + public static final int REAR_LEFT_TURN_CHANNEL = 1; + public static final int REAR_LEFT_DRIVE_CHANNEL = 2; + public static final int FRONT_LEFT_DRIVE_CHANNEL = 3; + public static final int FRONT_LEFT_STEER_CHANNEL = 4; + public static final int FRONT_RIGHT_STEER_CHANNEL = 5; + public static final int FRONT_RIGHT_DRIVE_CHANNEL = 6; + public static final int REAR_RIGHT_DRIVE_CHANNEL = 7; + public static final int REAR_RIGHT_STEER_CHANNEL = 8; + public static final int ELEVATOR_DRIVE_CHANNEL = 11; + public static final int CLIMB_DRIVE_CHANNEL = 12; + public static final int RIGHT_CORAL_DRIVE_CHANNEL = 13; + public static final int LEFT_CORAL_DRIVE_CHANNEL = 14; + public static final int ELEVATOR_FOLLOWER_DRIVE_CHANNEL = 15; + public static final int REAR_LEFT_TURN_ENCODER_DIO_CHANNEL = DIOChannel.ZERO.getChannel(); + public static final int FRONT_LEFT_TURN_ENCODER_DIO_CHANNEL = DIOChannel.ONE.getChannel(); + public static final int FRONT_RIGHT_TURN_ENCODER_DIO_CHANNEL = DIOChannel.TWO.getChannel(); + public static final int REAR_RIGHT_TURN_ENCODER_DIO_CHANNEL = DIOChannel.THREE.getChannel(); public static final int climbMagSwitchDIOC = DIOChannel.FOUR.getChannel(); ; public static final int hangerLeftMagSwitchDIOC = DIOChannel.SEVEN.getChannel(); @@ -130,15 +130,15 @@ public abstract class RobotVersionConstants { public class RobotVersion2025 extends RobotVersionConstants { public static final double flTurnEncoderOffset = 3.84 - .04 + Math.PI; public static final double frTurnEncoderOffset = 1.7 + Math.PI - .03 + Math.PI; - public static final double blTurnEncoderOffset = 3.284 + Math.PI; - public static final double brTurnEncoderOffset = 4.49 + Math.PI; + public static final double rlTurnEncoderOffset = 3.284 + Math.PI; + public static final double rrTurnEncoderOffset = 4.49 + Math.PI; } public class RobotVersion2023 extends RobotVersionConstants { public static final double flTurnEncoderOffset = 5.3038; public static final double frTurnEncoderOffset = Math.PI / 2 - 0.1242 - .05759; - public static final double blTurnEncoderOffset = 4.2 + 0.0385; - public static final double brTurnEncoderOffset = 2.736 - .06098; + public static final double rlTurnEncoderOffset = 4.2 + 0.0385; + public static final double rrTurnEncoderOffset = 2.736 - .06098; } public static final class Poses { diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbMechanism.java b/src/main/java/frc/robot/subsystems/climb/ClimbMechanism.java index 06e81ce..240b5bf 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbMechanism.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbMechanism.java @@ -1,10 +1,13 @@ package frc.robot.subsystems.climb; +import static edu.wpi.first.wpilibj.RobotBase.isSimulation; import static java.util.Objects.requireNonNull; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.config.AlternateEncoderConfig; +import com.revrobotics.spark.config.ClosedLoopConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.util.sendable.Sendable; @@ -13,6 +16,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.support.PIDSettings; import frc.robot.support.sparkmax.TeamSparkMax; /** @@ -36,8 +40,7 @@ public ClimbMechanism() { /** * Instantiates a new ClimbMechanism subsystem with the specified settings * - * @param context - * The ClimbMechanismSettings to apply to this instance + * @param context The ClimbMechanismSettings to apply to this instance */ public ClimbMechanism(final ClimbMechanismContext context) { requireNonNull(context, "ClimbMechanismContext cannot be null"); @@ -59,10 +62,24 @@ private SparkMaxConfig assembleClimbMotorConfig() { config.encoder .positionConversionFactor(this.context.getClimbPositionConversionFactor()) .velocityConversionFactor(this.context.getClimbVelocityConversionFactor()); - // PIDSettings pidSettings = this.context.getClimbControllerPIDSettings(); - // config - // .closedLoop.feedbackSensor(FeedbackSensor.kAlternateOrExternalEncoder) - // .pid(pidSettings.p(), pidSettings.i(), pidSettings.d()); + PIDSettings pidSettings = this.context.getClimbControllerPIDSettings(); + + if (isSimulation()) { + // Eliminate data-port dependency in sim + config.closedLoop + .feedbackSensor(ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder) + .pid(pidSettings.p(), pidSettings.i(), pidSettings.d()); + } else { + // Use the alternate encoder in real + AlternateEncoderConfig altConfig = new AlternateEncoderConfig() + .setSparkMaxDataPortConfig() + .countsPerRevolution(context.getClimbAltEncoderCPR()); + + config.apply(altConfig); + config.closedLoop + .feedbackSensor(ClosedLoopConfig.FeedbackSensor.kAlternateOrExternalEncoder) + .pid(pidSettings.p(), pidSettings.i(), pidSettings.d()); + } return config; } @@ -152,8 +169,7 @@ private double getCurrentClimbPosition() { /** * {@link Sendable#initSendable(SendableBuilder)} implementation * - * @param builder - * The sendable builder + * @param builder The sendable builder */ @Override public void initSendable(SendableBuilder builder) { diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbMechanismContext.java b/src/main/java/frc/robot/subsystems/climb/ClimbMechanismContext.java index 61ede99..9a59a97 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbMechanismContext.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbMechanismContext.java @@ -24,14 +24,14 @@ public static ClimbMechanismContext defaults() { .climbMotor( (Robot.isReal()) ? new TeamSparkMaxImpl( - Constants.Port.climbMotorChannel, SparkLowLevel.MotorType.kBrushless) + Constants.Port.CLIMB_DRIVE_CHANNEL, SparkLowLevel.MotorType.kBrushless) : new TeamSparkMaxSimImpl( - Constants.Port.climbMotorChannel, SparkLowLevel.MotorType.kBrushless)) + Constants.Port.CLIMB_DRIVE_CHANNEL, SparkLowLevel.MotorType.kBrushless)) .build(); } @Builder.Default - private int climbMotorChannel = Constants.Port.climbMotorChannel; + private int climbMotorChannel = Constants.Port.CLIMB_DRIVE_CHANNEL; @Builder.Default private int climbMagSwitchChannel = Constants.Port.climbMagSwitchDIOC; @@ -51,5 +51,11 @@ public static ClimbMechanismContext defaults() { @Builder.Default private double reverseIncrement = -.75; + @Builder.Default + // TODO: Look up the datasheet for the exact encoder you installed on the climb mechanism. + // If we're not using a quadrature encoder at all, then don’t pick kAlternateOrExternalEncoder + // Instead use the primary encoder or the appropriate sensor type. + private int climbAltEncoderCPR = (int) Constants.Conversion.MagEncoderCountsPerRev; + private TeamSparkMax climbMotor; } diff --git a/src/main/java/frc/robot/subsystems/coral/CoralMechanism.java b/src/main/java/frc/robot/subsystems/coral/CoralMechanism.java index 2fec0a3..d8411f4 100644 --- a/src/main/java/frc/robot/subsystems/coral/CoralMechanism.java +++ b/src/main/java/frc/robot/subsystems/coral/CoralMechanism.java @@ -43,8 +43,8 @@ public CoralMechanism() { public CoralMechanism(final CoralMechanismContext context) { requireNonNull(context, "CoralMechanismContext cannot be null"); this.context = context; - this.coralMotorRight = new TalonSRX(Constants.Port.coralMotorRightChannel); - this.coralMotorLeft = new TalonSRX(Constants.Port.coralMotorLeftChannel); + this.coralMotorRight = new TalonSRX(Constants.Port.RIGHT_CORAL_DRIVE_CHANNEL); + this.coralMotorLeft = new TalonSRX(Constants.Port.LEFT_CORAL_DRIVE_CHANNEL); this.rearSensor = new AnalogInput(this.context.getRearSensorChannel()); this.frontSensor = new AnalogInput(this.context.getFrontSensorChannel()); } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 1fbbad5..8b8fc0e 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -46,13 +46,13 @@ public class Drivetrain extends SubsystemBase { private static final double NOMINAL_BATT_VOLTS = 12.0; - private final double SIM_VOLTS_PER_RADIAN_TURN_VOLTAGE = 24.0; + private final double SIM_VOLTS_PER_RADIAN_TURN_VOLTAGE = 48.0; + + private final DrivetrainContext context; // Scale from desired wheel speed (meters-per-second) to motor voltage (V) private final double simVoltsMetPerSec; - private final DrivetrainContext context; - private final Field2d field = new Field2d(); private final SwerveDriveKinematics swerveDriveKinematics; @@ -80,7 +80,6 @@ public class Drivetrain extends SubsystemBase { private final SwerveDrivePoseEstimator swerveDrivePoseEstimator; - // TODO: These publishers should private final StructArrayPublisher desiredStatePublisher; private final StructArrayPublisher currentStatePublisher; @@ -202,15 +201,15 @@ public Drivetrain(final DrivetrainContext context) { Constants.Drive.SMBackLeftLocation, Constants.Drive.SMBackRightLocation); - this.frSwerve = new SwerveModule(this.context.getFrontRightSwerveModuleContext()); - this.flSwerve = new SwerveModule(this.context.getFrontLeftSwerveModuleContext()); - this.rlSwerve = new SwerveModule(this.context.getRearLeftSwerveModuleContext()); - this.rrSwerve = new SwerveModule(this.context.getRearRightSwerveModuleContext()); + this.frSwerve = new SwerveModule(this.context.getFrSwerveContext()); + this.flSwerve = new SwerveModule(this.context.getFlSwerveContext()); + this.rlSwerve = new SwerveModule(this.context.getRlSwerveContext()); + this.rrSwerve = new SwerveModule(this.context.getRrSwerveContext()); - this.frSwerveSim = new SwerveModuleSim(context.getFrontRightSwerveModuleContext()); - this.flSwerveSim = new SwerveModuleSim(context.getFrontLeftSwerveModuleContext()); - this.rlSwerveSim = new SwerveModuleSim(context.getRearLeftSwerveModuleContext()); - this.rrSwerveSim = new SwerveModuleSim(context.getRearRightSwerveModuleContext()); + this.frSwerveSim = new SwerveModuleSim(context.getFrSwerveContext()); + this.flSwerveSim = new SwerveModuleSim(context.getFlSwerveContext()); + this.rlSwerveSim = new SwerveModuleSim(context.getRlSwerveContext()); + this.rrSwerveSim = new SwerveModuleSim(context.getRrSwerveContext()); // initializes odometry this.swerveDriveOdometry = new SwerveDriveOdometry( diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainContext.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainContext.java index d6a7498..4bdc68b 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainContext.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainContext.java @@ -34,39 +34,39 @@ public static DrivetrainContext defaults() { private PIDSettings rotationPIDSettings = new PIDSettings(3, 0, 0); @Builder.Default - private SwerveModuleContext frontLeftSwerveModuleContext = SwerveModuleContext.builder() + private SwerveModuleContext flSwerveContext = SwerveModuleContext.builder() .name("Swerve Module/Front Left") - .driveMotorId(Constants.Port.flDriveMtrC) - .turningMotorId(Constants.Port.flSteerMtrC) - .turnEncoderPWMChannel(Constants.Port.flTurnEncoderDIOC) + .driveMotorId(Constants.Port.FRONT_LEFT_DRIVE_CHANNEL) + .turningMotorId(Constants.Port.FRONT_LEFT_STEER_CHANNEL) + .turnEncoderPWMChannel(Constants.Port.FRONT_LEFT_TURN_ENCODER_DIO_CHANNEL) .turnOffset(Constants.RobotVersion2025.flTurnEncoderOffset) .build(); @Builder.Default - private SwerveModuleContext frontRightSwerveModuleContext = SwerveModuleContext.builder() + private SwerveModuleContext frSwerveContext = SwerveModuleContext.builder() .name("Swerve Module/Front Right") - .driveMotorId(Constants.Port.frDriveMtrC) - .turningMotorId(Constants.Port.frSteerMtrC) - .turnEncoderPWMChannel(Constants.Port.frTurnEncoderDIOC) + .driveMotorId(Constants.Port.FRONT_RIGHT_DRIVE_CHANNEL) + .turningMotorId(Constants.Port.FRONT_RIGHT_STEER_CHANNEL) + .turnEncoderPWMChannel(Constants.Port.FRONT_RIGHT_TURN_ENCODER_DIO_CHANNEL) .turnOffset(Constants.RobotVersion2025.frTurnEncoderOffset) .build(); @Builder.Default - private SwerveModuleContext rearLeftSwerveModuleContext = SwerveModuleContext.builder() + private SwerveModuleContext rlSwerveContext = SwerveModuleContext.builder() .name("Swerve Module/Back Left") - .driveMotorId(Constants.Port.blDriveMtrC) - .turningMotorId(Constants.Port.blSteerMtrC) - .turnEncoderPWMChannel(Constants.Port.blTurnEncoderDIOC) - .turnOffset(Constants.RobotVersion2025.blTurnEncoderOffset) + .driveMotorId(Constants.Port.REAR_LEFT_DRIVE_CHANNEL) + .turningMotorId(Constants.Port.REAR_LEFT_TURN_CHANNEL) + .turnEncoderPWMChannel(Constants.Port.REAR_LEFT_TURN_ENCODER_DIO_CHANNEL) + .turnOffset(Constants.RobotVersion2025.rlTurnEncoderOffset) .build(); @Builder.Default - private SwerveModuleContext rearRightSwerveModuleContext = SwerveModuleContext.builder() + private SwerveModuleContext rrSwerveContext = SwerveModuleContext.builder() .name("Swerve Module/Back Right") - .driveMotorId(Constants.Port.brDriveMtrC) - .turningMotorId(Constants.Port.brSteerMtrC) - .turnEncoderPWMChannel(Constants.Port.brTurnEncoderDIOC) - .turnOffset(Constants.RobotVersion2025.brTurnEncoderOffset) + .driveMotorId(Constants.Port.REAR_RIGHT_DRIVE_CHANNEL) + .turningMotorId(Constants.Port.REAR_RIGHT_STEER_CHANNEL) + .turnEncoderPWMChannel(Constants.Port.REAR_RIGHT_TURN_ENCODER_DIO_CHANNEL) + .turnOffset(Constants.RobotVersion2025.rrTurnEncoderOffset) .build(); @Builder.Default @@ -76,10 +76,10 @@ public static DrivetrainContext defaults() { private double frTurnOffset = Constants.RobotVersion2025.frTurnEncoderOffset; @Builder.Default - private double blTurnOffset = Constants.RobotVersion2025.blTurnEncoderOffset; + private double rlTurnOffset = Constants.RobotVersion2025.rlTurnEncoderOffset; @Builder.Default - private double brTurnOffset = Constants.RobotVersion2025.brTurnEncoderOffset; + private double rrTurnOffset = Constants.RobotVersion2025.rrTurnEncoderOffset; @Builder.Default private Matrix stateStdDevs = VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)); diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index face673..36eeb5e 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -83,7 +83,7 @@ private SparkMaxConfig assembleElevatorMotorConfig() { private SparkMaxConfig assembleElevatorFollowerMotorConfig() { SparkMaxConfig config = new SparkMaxConfig(); - config.follow(Constants.Port.elevatorMotorChannel, true); + config.follow(Constants.Port.ELEVATOR_DRIVE_CHANNEL, true); return config; } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorContext.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorContext.java index 21b3143..989e9f2 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorContext.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorContext.java @@ -21,14 +21,14 @@ public static ElevatorContext defaults() { return ElevatorContext.builder() .elevatorMotor( (Robot.isReal() - ? new TeamSparkMaxImpl(Constants.Port.elevatorMotorChannel, MotorType.kBrushless) - : new TeamSparkMaxSimImpl(Constants.Port.elevatorMotorChannel, MotorType.kBrushless))) + ? new TeamSparkMaxImpl(Constants.Port.ELEVATOR_DRIVE_CHANNEL, MotorType.kBrushless) + : new TeamSparkMaxSimImpl(Constants.Port.ELEVATOR_DRIVE_CHANNEL, MotorType.kBrushless))) .elevatorFollowerMotor( (Robot.isReal() ? new TeamSparkMaxImpl( - Constants.Port.elevatorFollowerMotorChannel, MotorType.kBrushless) + Constants.Port.ELEVATOR_FOLLOWER_DRIVE_CHANNEL, MotorType.kBrushless) : new TeamSparkMaxSimImpl( - Constants.Port.elevatorFollowerMotorChannel, MotorType.kBrushless))) + Constants.Port.ELEVATOR_FOLLOWER_DRIVE_CHANNEL, MotorType.kBrushless))) .build(); } @@ -42,10 +42,10 @@ public static ElevatorContext defaults() { private double elevatorControllerTolerance = .16; @Builder.Default - private int elevatorMotorChannel = Constants.Port.elevatorMotorChannel; + private int elevatorMotorChannel = Constants.Port.ELEVATOR_DRIVE_CHANNEL; @Builder.Default - private int elevatorFollowerMotorChannel = Constants.Port.elevatorFollowerMotorChannel; + private int elevatorFollowerMotorChannel = Constants.Port.ELEVATOR_FOLLOWER_DRIVE_CHANNEL; @Builder.Default private double downPosition = 0;