From dae39c30a089a97406a4fb64ec80eff4a476ce97 Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Sat, 24 Jan 2026 18:02:05 -0500 Subject: [PATCH] Build Session 1/24, swerve work, need to revalidate individual module CAN IDs --- src/main/java/frc/robot/RobotContainer.java | 4 +-- .../robot/constants/DrivetrainConstants.java | 29 +++++++++---------- .../frc/robot/constants/ModuleConstants.java | 13 ++++----- .../java/frc/robot/subsystems/Drivetrain.java | 26 ++++++++++++----- .../frc/robot/utilities/SwerveModule.java | 24 ++++++++++++--- 5 files changed, 59 insertions(+), 37 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3f63495..91d7883 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,8 +11,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.PrintCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -52,6 +50,8 @@ public class RobotContainer { ) ); + //drivetrain.setDefaultCommand(drivetrain.disableOutputs()); + configureShiftDisplay(); } diff --git a/src/main/java/frc/robot/constants/DrivetrainConstants.java b/src/main/java/frc/robot/constants/DrivetrainConstants.java index 0d592db..10749f7 100644 --- a/src/main/java/frc/robot/constants/DrivetrainConstants.java +++ b/src/main/java/frc/robot/constants/DrivetrainConstants.java @@ -9,9 +9,8 @@ public class DrivetrainConstants { public static final double kMaxSpeedMetersPerSecond = 4.125; public static final double kMaxAngularSpeed = 2 * Math.PI; - // TODO Replace zeros with real numbers - public static final double kTrackWidth = Units.inchesToMeters(0); - public static final double kWheelBase = Units.inchesToMeters(0); + public static final double kTrackWidth = Units.inchesToMeters(23.75); + public static final double kWheelBase = Units.inchesToMeters(18.75); // TODO Replace zeros with real numbers // These values should be determinable by writing the magnetic encoder output @@ -24,26 +23,24 @@ public class DrivetrainConstants { public static final double kRearLeftMagEncoderOffset = 0; public static final double kRearRightMagEncoderOffset = 0; - // Kraken CAN IDs - // TODO Real CAN IDs + // Kraken CAN IDs public static final int kFrontLeftDrivingCANID = 0; - public static final int kFrontRightDrivingCANID = 0; - public static final int kRearLeftDrivingCANID = 0; - public static final int kRearRightDrivingCANID = 0; + public static final int kFrontRightDrivingCANID = 1;//3; + public static final int kRearLeftDrivingCANID = 3;//1; + public static final int kRearRightDrivingCANID = 2; // SparkMAX CAN IDs - // TODO Real CAN IDs - public static final int kFrontLeftTurningCANID = 0; - public static final int kFrontRightTurningCANID = 0; - public static final int kRearLeftTurningCANID = 0; - public static final int kRearRightTurningCANID = 0; + public static final int kFrontLeftTurningCANID = 8; + public static final int kFrontRightTurningCANID = 7;//9; + public static final int kRearLeftTurningCANID = 9;//7; + public static final int kRearRightTurningCANID = 6; // Analog Encoder Input Ports // TODO Real Port IDs public static final int kFrontLeftAnalogInPort = 0; - public static final int kFrontRightAnalogInPort = 0; - public static final int kRearLeftAnalogInPort = 0; - public static final int kRearRightAnalogInPort = 0; + public static final int kFrontRightAnalogInPort = 1; + public static final int kRearLeftAnalogInPort = 2; + public static final int kRearRightAnalogInPort = 3; public static final boolean kGyroReversed = true; diff --git a/src/main/java/frc/robot/constants/ModuleConstants.java b/src/main/java/frc/robot/constants/ModuleConstants.java index 35d13d8..f422f20 100644 --- a/src/main/java/frc/robot/constants/ModuleConstants.java +++ b/src/main/java/frc/robot/constants/ModuleConstants.java @@ -11,13 +11,11 @@ import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.util.Units; public class ModuleConstants { // DRIVING MOTOR CONFIG (Kraken) - // TODO Replace with something other than 0 - public static final double kDrivingMotorReduction = 0; + public static final double kDrivingMotorReduction = (14.0 * 28.0 * 15.0) / (50 * 16 * 45); public static final double kDrivingMotorFeedSpeedRPS = KrakenMotorConstants.kFreeSpeedRPM / 60; public static final double kWheelDiameterMeters = Units.inchesToMeters(4); @@ -44,14 +42,15 @@ public class ModuleConstants { public static final NeutralModeValue kDriveIdleMode = NeutralModeValue.Brake; // TURNING MOTOR CONFIG (NEO) - - // TODO Hold over from 2025, adjust? - public static final double kTurningMotorReduction = 0; + public static final double kTurningMotorReduction = 150.0/7.0; public static final double kTurningFactor = 2 * Math.PI / kTurningMotorReduction; + // TODO Adjust? Let over from 2025 public static final double kTurnP = 1; public static final double kTurnI = 0; public static final double kTurnD = 0; + public static final boolean kIsEncoderInverted = false; + // TODO How sensitive is too sensitive? public static final double kAutoResetPositionDeadband = Units.degreesToRadians(.25); @@ -93,7 +92,7 @@ public class ModuleConstants { .idleMode(kTurnIdleMode) .smartCurrentLimit(kTurnMotorCurrentLimit); turningConfig.encoder - .inverted(true) + //.inverted(true) .positionConversionFactor(kTurningFactor) .velocityConversionFactor(kTurningFactor / 60.0); turningConfig.closedLoop diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 538e315..81a5413 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -48,34 +48,34 @@ public class Drivetrain extends SubsystemBase { frontLeft = new SwerveModule( "FrontLeft", DrivetrainConstants.kFrontLeftDrivingCANID, - DrivetrainConstants.kFrontLeftTurningCANID, + DrivetrainConstants.kFrontLeftTurningCANID);/*, DrivetrainConstants.kFrontLeftAnalogInPort, DrivetrainConstants.kFrontLeftMagEncoderOffset - ); + ); */ frontRight = new SwerveModule( "FrontRight", DrivetrainConstants.kFrontRightDrivingCANID, - DrivetrainConstants.kFrontRightTurningCANID, + DrivetrainConstants.kFrontRightTurningCANID);/*, DrivetrainConstants.kFrontRightAnalogInPort, DrivetrainConstants.kFrontRightMagEncoderOffset - ); + );*/ rearLeft = new SwerveModule( "RearLeft", DrivetrainConstants.kRearLeftDrivingCANID, - DrivetrainConstants.kRearLeftTurningCANID, + DrivetrainConstants.kRearLeftTurningCANID);/*, DrivetrainConstants.kRearLeftAnalogInPort, DrivetrainConstants.kRearLeftMagEncoderOffset - ); + );*/ rearRight = new SwerveModule( "RearRight", DrivetrainConstants.kRearRightDrivingCANID, - DrivetrainConstants.kRearRightTurningCANID, + DrivetrainConstants.kRearRightTurningCANID);/*, DrivetrainConstants.kRearRightAnalogInPort, DrivetrainConstants.kRearRightMagEncoderOffset - ); + );*/ gyro = new AHRS(NavXComType.kMXP_SPI); @@ -130,9 +130,19 @@ public class Drivetrain extends SubsystemBase { rearRight.periodic(); Logger.recordOutput("Drivetrain/Pose", getPose()); + Logger.recordOutput("Drivetrain/Gyro Angle", getGyroValue()); Logger.recordOutput("Drivetrain/Heading", getHeading()); } + public Command disableOutputs() { + return run(() -> { + frontLeft.disableOutput(); + frontRight.disableOutput(); + rearLeft.disableOutput(); + rearRight.disableOutput(); + }); + } + // TODO check both cameras public Command driveAprilTagLock(DoubleSupplier xSpeed, DoubleSupplier ySpeed, double deadband, int tagID) { if (camera1 == null) { diff --git a/src/main/java/frc/robot/utilities/SwerveModule.java b/src/main/java/frc/robot/utilities/SwerveModule.java index 2447c90..e5a67c4 100644 --- a/src/main/java/frc/robot/utilities/SwerveModule.java +++ b/src/main/java/frc/robot/utilities/SwerveModule.java @@ -114,6 +114,8 @@ public class SwerveModule { PersistMode.kPersistParameters ); + driveVelocityRequest = new VelocityVoltage(0); + if(isAbsoluteEncoderDisabled){ turningRelativeEncoder.setPosition(0); } else { @@ -130,9 +132,14 @@ public class SwerveModule { } public void periodic() { - Logger.recordOutput(moduleName + "/AbsoluteEncoder/Position", turningAbsoluteEncoder.get()); + if(!isAbsoluteEncoderDisabled) { + Logger.recordOutput(moduleName + "/AbsoluteEncoder/Position", turningAbsoluteEncoder.get()); + } + + Logger.recordOutput(moduleName + "ModuleTargetState", lastTargetState); Logger.recordOutput(moduleName + "/SwerveModuleState", getState()); Logger.recordOutput(moduleName + "/SwerveModulePosition", getPosition()); + Logger.recordOutput(moduleName + "/RelativeEncoderPosition", getTurningEncoderPosition()); if(!isAbsoluteEncoderDisabled && turningEncoderAutoRezeroEnabled) { if(Math.abs(getState().angle.getRadians() - lastTargetState.angle.getRadians()) <= ModuleConstants.kAutoResetPositionDeadband) { @@ -144,22 +151,27 @@ public class SwerveModule { public SwerveModuleState getState() { return new SwerveModuleState( drive.getVelocity().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters, - new Rotation2d(turningRelativeEncoder.getPosition()) + new Rotation2d(getTurningEncoderPosition()) ); } public SwerveModulePosition getPosition() { return new SwerveModulePosition( drive.getPosition().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters, - new Rotation2d(turningRelativeEncoder.getPosition()) + new Rotation2d(getTurningEncoderPosition()) ); } + public void disableOutput() { + drive.disable(); + turning.disable(); + } + public void setDesiredState(SwerveModuleState desiredState) { // TODO is this really necessary, the offset is managed by the Absolute Encoder // and its "source of truth" behavior in relation to the relative encoder // Probably doesn't *hurt* that it's here, but it may not be needed - desiredState.optimize(new Rotation2d(turningRelativeEncoder.getPosition())); + desiredState.optimize(new Rotation2d(getTurningEncoderPosition())); lastTargetState = desiredState; @@ -177,6 +189,10 @@ public class SwerveModule { ); } + public double getTurningEncoderPosition() { + return turningRelativeEncoder.getPosition() * (ModuleConstants.kIsEncoderInverted ? -1 : 1); + } + public void resetEncoders() { drive.setPosition(0);