diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ae155a9..46af315 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); @@ -88,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(); @@ -129,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/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/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 01f91f7..8b8fc0e 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -4,6 +4,9 @@ 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; @@ -36,13 +39,20 @@ /** * 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 = 48.0; + private final DrivetrainContext context; + // Scale from desired wheel speed (meters-per-second) to motor voltage (V) + private final double simVoltsMetPerSec; + private final Field2d field = new Field2d(); private final SwerveDriveKinematics swerveDriveKinematics; @@ -50,27 +60,26 @@ 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; private final SwerveDrivePoseEstimator swerveDrivePoseEstimator; - // TODO: These publishers should private final StructArrayPublisher desiredStatePublisher; private final StructArrayPublisher currentStatePublisher; @@ -87,7 +96,7 @@ public class Drivetrain extends SubsystemBase { private boolean wheelLock = false; - private boolean fieldRelativeEnable = true; + private boolean fieldRelativeEnable = false; private Pose2d goalPose; @@ -98,8 +107,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} */ @@ -110,14 +117,15 @@ 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"); this.context = context; + this.simVoltsMetPerSec = NOMINAL_BATT_VOLTS / this.getMaxSpeed(); + NetworkTableInstance nti = NetworkTableInstance.getDefault(); this.desiredStatePublisher = nti.getStructArrayTopic("DesiredStates", SwerveModuleState.struct) @@ -193,15 +201,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.getFrSwerveContext()); + this.flSwerve = new SwerveModule(this.context.getFlSwerveContext()); + this.rlSwerve = new SwerveModule(this.context.getRlSwerveContext()); + this.rrSwerve = new SwerveModule(this.context.getRrSwerveContext()); - 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.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( @@ -215,10 +223,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); } @@ -242,9 +250,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(); @@ -259,20 +267,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()); } /** @@ -283,10 +291,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() }; } @@ -299,10 +307,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; @@ -402,10 +412,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() }; } @@ -419,33 +429,43 @@ 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(); } /** * 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); @@ -530,57 +550,80 @@ public void periodic() { public void simulationPeriodic() { // 1. Compute elapsed time since last loop double currentTime = Timer.getFPGATimestamp(); - double dt = currentTime - lastSimTime; + double dt = currentTime - this.lastSimTime; this.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); - - frontRightSwerveModuleSim.setDriveVoltage(frontRightSwerveModule.getLastDrivePercent() * 12.0); - frontRightSwerveModuleSim.setTurnVoltage(frontRightSwerveModule.getLastTurnPercent() * 12.0); - 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); + // 2. Skip if no desired states yet (e.g., before first drive command) + if (this.desiredStates == null) { + return; + } - // 3. Build simulated module states for kinematics + // 3. Compute each module’s commanded (optimized) state + 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) + 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 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( - this.frontLeftSwerveModuleSim.getWheelSpeedMetersPerSecond(), - this.frontLeftSwerveModuleSim.getTurnAngle()), - new SwerveModuleState( - this.frontRightSwerveModuleSim.getWheelSpeedMetersPerSecond(), - this.frontRightSwerveModuleSim.getTurnAngle()), - new SwerveModuleState( - this.rearLeftSwerveModuleSim.getWheelSpeedMetersPerSecond(), - this.rearLeftSwerveModuleSim.getTurnAngle()), - new SwerveModuleState( - this.rearRightSwerveModuleSim.getWheelSpeedMetersPerSecond(), - this.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()) }; - // 4. Convert to chassis speeds and integrate heading + // 8. 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); + this.simYaw = this.simYaw.plus(Rotation2d.fromRadians(chassisSpeeds.omegaRadiansPerSecond * dt)); - // 5. Update pose estimator with sim yaw and module positions + // 9. 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() + this.flSwerveSim.getPosition(), + this.frSwerveSim.getPosition(), + this.rlSwerveSim.getPosition(), + this.rrSwerveSim.getPosition() }); - // 6. Push pose to Field2d for visualization - this.field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition()); + // 10. Push pose to Field2d for visualization + this.field.setRobotPose(this.swerveDrivePoseEstimator.getEstimatedPosition()); } @Override @@ -617,10 +660,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); 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;