From 2583fc9418dd2ec1c5d7954c561d93525f357ee1 Mon Sep 17 00:00:00 2001 From: ParkerMeyers Date: Thu, 2 Nov 2023 13:09:21 -0400 Subject: [PATCH] Spotless Apply --- src/main/java/bhs/devilbotz/Constants.java | 108 +++--- .../subsystems/ModuleIOSparkMAX.java | 329 +++++++++--------- 2 files changed, 220 insertions(+), 217 deletions(-) diff --git a/src/main/java/bhs/devilbotz/Constants.java b/src/main/java/bhs/devilbotz/Constants.java index e299a4e..c15fa46 100644 --- a/src/main/java/bhs/devilbotz/Constants.java +++ b/src/main/java/bhs/devilbotz/Constants.java @@ -7,64 +7,70 @@ import edu.wpi.first.wpilibj.RobotBase; public class Constants { - // How often to run a loop of the robot - // Default is 20ms (do not change unless you know what you're doing - public static final double loopPeriodSecs = 0.02; - // Tuning mode toggle - public static final boolean tuningMode = true; - // Which robot to use? - // Options: - // - ROBOT_2024S - // - ROBOT_2024SIM - private static final RobotType robot = RobotType.ROBOT_2024S; + // How often to run a loop of the robot + // Default is 20ms (do not change unless you know what you're doing + public static final double loopPeriodSecs = 0.02; + // Tuning mode toggle + public static final boolean tuningMode = true; + // Which robot to use? + // Options: + // - ROBOT_2024S + // - ROBOT_2024SIM + private static final RobotType robot = RobotType.ROBOT_2024S; - // Get the current robot - public static RobotType getRobot() { - if (RobotBase.isReal()) { - if (robot == RobotType.ROBOT_2024SIM) { // Invalid robot selected - return RobotType.ROBOT_2024S; - } else { - return robot; - } - } else { - return robot; - } + // Get the current robot + public static RobotType getRobot() { + if (RobotBase.isReal()) { + if (robot == RobotType.ROBOT_2024SIM) { // Invalid robot selected + return RobotType.ROBOT_2024S; + } else { + return robot; + } + } else { + return robot; } + } - // Get the current mode - public static Mode getMode() { - switch (getRobot()) { - case ROBOT_2024S: - return RobotBase.isReal() ? Mode.REAL : Mode.REPLAY; + // Get the current mode + public static Mode getMode() { + switch (getRobot()) { + case ROBOT_2024S: + return RobotBase.isReal() ? Mode.REAL : Mode.REPLAY; - case ROBOT_2024SIM: - return Mode.SIM; + case ROBOT_2024SIM: + return Mode.SIM; - default: - return Mode.REAL; - } + default: + return Mode.REAL; } + } - // Robot Type Enum - public enum RobotType { - ROBOT_2024S, ROBOT_2024SIM - } + // Robot Type Enum + public enum RobotType { + ROBOT_2024S, + ROBOT_2024SIM + } - // Robot Type Mode - public enum Mode { - REAL, REPLAY, SIM - } + // Robot Type Mode + public enum Mode { + REAL, + REPLAY, + SIM + } - // Vision Constants - public static class VisionConstants { - public static final Transform3d robotToCam = - new Transform3d( - new Translation3d(0.5, 0.0, 0.5), - new Rotation3d( - Units.degreesToRadians(0), Units.degreesToRadians(0), - Units.degreesToRadians(0))); // Cam mounted facing forward, half a meter forward of center, half a meter up - // from center. - // TODO: Get camera name - public static final String cameraName = "YOUR CAMERA NAME"; - } + // Vision Constants + public static class VisionConstants { + public static final Transform3d robotToCam = + new Transform3d( + new Translation3d(0.5, 0.0, 0.5), + new Rotation3d( + Units.degreesToRadians(0), + Units.degreesToRadians(0), + Units.degreesToRadians( + 0))); // Cam mounted facing forward, half a meter forward of center, half a + // meter up + // from center. + // TODO: Get camera name + public static final String cameraName = "YOUR CAMERA NAME"; + } } diff --git a/src/main/java/bhs/devilbotz/subsystems/ModuleIOSparkMAX.java b/src/main/java/bhs/devilbotz/subsystems/ModuleIOSparkMAX.java index e9ea4f6..67d8da3 100644 --- a/src/main/java/bhs/devilbotz/subsystems/ModuleIOSparkMAX.java +++ b/src/main/java/bhs/devilbotz/subsystems/ModuleIOSparkMAX.java @@ -16,175 +16,172 @@ * Hardware interface for a swerve module with two Spark MAXs as drive and steer motor controllers */ public class ModuleIOSparkMAX implements ModuleIO { - // 2 Controllers for 2 NEOs - private final CANSparkMax driveSparkMax; - private final CANSparkMax turnSparkMax; - - // Encoder Initialization - private final SparkMaxDerivedVelocityController driveDerivedVelocityController; - private final RelativeEncoder driveDefaultEncoder; - private final RelativeEncoder turnRelativeEncoder; - private final CANCoder turnAbsoluteEncoder; - private final Rotation2d absoluteEncoderOffset; - - // Encoder resolution - private final double driveAfterEncoderReduction = - (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - private final double turnAfterEncoderReduction = 150.0 / 7.0; - - // Booleans for setup - private final boolean isTurnMotorInverted = true; - private final double turnAbsoluteEncoderTicksPerRevolution = 4096.0; - - /** - * Initialize the Module - * - * @param index which module to initialize - */ - public ModuleIOSparkMAX(int index) { - // Change the hardware depending on the type of robot - switch (Constants.getRobot()) { - case ROBOT_2024S: - // Depending on the module, initialize the hardware - switch (index) { - case 0: - driveSparkMax = new CANSparkMax(10, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(14, MotorType.kBrushless); - turnAbsoluteEncoder = new CANCoder(18); - // TODO: Hardware: Measure absolute encoder offset - absoluteEncoderOffset = new Rotation2d(Units.degreesToRadians(0)); - break; - case 1: - driveSparkMax = new CANSparkMax(11, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(15, MotorType.kBrushless); - turnAbsoluteEncoder = new CANCoder(19); - // TODO: Hardware: Measure absolute encoder offset - absoluteEncoderOffset = new Rotation2d(Units.degreesToRadians(0)); - break; - case 2: - driveSparkMax = new CANSparkMax(12, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(16, MotorType.kBrushless); - turnAbsoluteEncoder = new CANCoder(20); - // TODO: Hardware: Measure absolute encoder offset - absoluteEncoderOffset = new Rotation2d(Units.degreesToRadians(0)); - break; - case 3: - driveSparkMax = new CANSparkMax(13, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(17, MotorType.kBrushless); - turnAbsoluteEncoder = new CANCoder(21); - // TODO: Hardware: Measure absolute encoder offset - absoluteEncoderOffset = new Rotation2d(Units.degreesToRadians(0)); - break; - default: - throw new RuntimeException( - "Invalid module index for ModuleIOSparkMAX"); - } - break; - default: - throw new RuntimeException("Invalid robot for ModuleIOSparkMAX"); + // 2 Controllers for 2 NEOs + private final CANSparkMax driveSparkMax; + private final CANSparkMax turnSparkMax; + + // Encoder Initialization + private final SparkMaxDerivedVelocityController driveDerivedVelocityController; + private final RelativeEncoder driveDefaultEncoder; + private final RelativeEncoder turnRelativeEncoder; + private final CANCoder turnAbsoluteEncoder; + private final Rotation2d absoluteEncoderOffset; + + // Encoder resolution + private final double driveAfterEncoderReduction = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); + private final double turnAfterEncoderReduction = 150.0 / 7.0; + + // Booleans for setup + private final boolean isTurnMotorInverted = true; + private final double turnAbsoluteEncoderTicksPerRevolution = 4096.0; + + /** + * Initialize the Module + * + * @param index which module to initialize + */ + public ModuleIOSparkMAX(int index) { + // Change the hardware depending on the type of robot + switch (Constants.getRobot()) { + case ROBOT_2024S: + // Depending on the module, initialize the hardware + switch (index) { + case 0: + driveSparkMax = new CANSparkMax(10, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(14, MotorType.kBrushless); + turnAbsoluteEncoder = new CANCoder(18); + // TODO: Hardware: Measure absolute encoder offset + absoluteEncoderOffset = new Rotation2d(Units.degreesToRadians(0)); + break; + case 1: + driveSparkMax = new CANSparkMax(11, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(15, MotorType.kBrushless); + turnAbsoluteEncoder = new CANCoder(19); + // TODO: Hardware: Measure absolute encoder offset + absoluteEncoderOffset = new Rotation2d(Units.degreesToRadians(0)); + break; + case 2: + driveSparkMax = new CANSparkMax(12, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(16, MotorType.kBrushless); + turnAbsoluteEncoder = new CANCoder(20); + // TODO: Hardware: Measure absolute encoder offset + absoluteEncoderOffset = new Rotation2d(Units.degreesToRadians(0)); + break; + case 3: + driveSparkMax = new CANSparkMax(13, MotorType.kBrushless); + turnSparkMax = new CANSparkMax(17, MotorType.kBrushless); + turnAbsoluteEncoder = new CANCoder(21); + // TODO: Hardware: Measure absolute encoder offset + absoluteEncoderOffset = new Rotation2d(Units.degreesToRadians(0)); + break; + default: + throw new RuntimeException("Invalid module index for ModuleIOSparkMAX"); } - - // Burn the Spark MAXs depending on their status - if (SparkMAXBurnManager.shouldBurn()) { - driveSparkMax.restoreFactoryDefaults(); - turnSparkMax.restoreFactoryDefaults(); - } - - // Invert the turn falcon - turnSparkMax.setInverted(isTurnMotorInverted); - - // Set the current limit to 30 amps - driveSparkMax.setSmartCurrentLimit(30); - turnSparkMax.setSmartCurrentLimit(30); - // Enable voltage compensation - driveSparkMax.enableVoltageCompensation(12.0); - turnSparkMax.enableVoltageCompensation(12.0); - - // Assign the encoders - driveDerivedVelocityController = - new SparkMaxDerivedVelocityController(driveSparkMax); - driveDefaultEncoder = driveSparkMax.getEncoder(); - turnRelativeEncoder = turnSparkMax.getEncoder(); - turnRelativeEncoder.setPosition(0.0); - - driveSparkMax.setCANTimeout(0); - turnSparkMax.setCANTimeout(0); - - // Burn the spark maxes if necessary - if (SparkMAXBurnManager.shouldBurn()) { - driveSparkMax.burnFlash(); - turnSparkMax.burnFlash(); - } - } - - /** - * Update the AK hardware inputs - * - * @param inputs the inputs to update - */ - @Override - public void updateInputs(ModuleIOInputs inputs) { - inputs.drivePositionRad = - Units.rotationsToRadians(driveDerivedVelocityController.getPosition()) - / driveAfterEncoderReduction; - inputs.driveVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond( - driveDerivedVelocityController.getVelocity()) - / driveAfterEncoderReduction; - inputs.driveVelocityFilteredRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond( - driveDefaultEncoder.getVelocity()) / driveAfterEncoderReduction; - inputs.driveAppliedVolts = - driveSparkMax.getAppliedOutput() * RobotController.getBatteryVoltage(); - inputs.driveCurrentAmps = driveSparkMax.getOutputCurrent(); - inputs.driveTempCelcius = driveSparkMax.getMotorTemperature(); - - inputs.turnAbsolutePositionRad = - Units.degreesToRadians(turnAbsoluteEncoder.getAbsolutePosition()) - absoluteEncoderOffset.getRadians(); - - inputs.turnPositionRad = - Units.rotationsToRadians(turnRelativeEncoder.getPosition()) - / turnAfterEncoderReduction; - inputs.turnVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond( - turnRelativeEncoder.getVelocity()) / turnAfterEncoderReduction; - inputs.turnAppliedVolts = - turnSparkMax.getAppliedOutput() * RobotController.getBatteryVoltage(); - inputs.turnCurrentAmps = turnSparkMax.getOutputCurrent(); - inputs.turnTempCelcius = turnSparkMax.getMotorTemperature(); - } - - /** - * Set the drive voltage of the Spark MAX - * - * @param volts The voltage to run the Spark Max at - */ - public void setDriveVoltage(double volts) { - driveSparkMax.setVoltage(volts); - } - - /** - * Set the turn voltage of the Spark MAX - * - * @param volts The voltage to run the Spark Max at - */ - public void setTurnVoltage(double volts) { - turnSparkMax.setVoltage(volts); + break; + default: + throw new RuntimeException("Invalid robot for ModuleIOSparkMAX"); } - /** - * Set the brake mode of the drive NEO - * - * @param enable whether to enable the brake mode or not - */ - public void setDriveBrakeMode(boolean enable) { - driveSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + // Burn the Spark MAXs depending on their status + if (SparkMAXBurnManager.shouldBurn()) { + driveSparkMax.restoreFactoryDefaults(); + turnSparkMax.restoreFactoryDefaults(); } - /** - * Set the brake mode of the turn NEO - * - * @param enable whether to enable the brake mode or not - */ - public void setTurnBrakeMode(boolean enable) { - turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + // Invert the turn falcon + turnSparkMax.setInverted(isTurnMotorInverted); + + // Set the current limit to 30 amps + driveSparkMax.setSmartCurrentLimit(30); + turnSparkMax.setSmartCurrentLimit(30); + // Enable voltage compensation + driveSparkMax.enableVoltageCompensation(12.0); + turnSparkMax.enableVoltageCompensation(12.0); + + // Assign the encoders + driveDerivedVelocityController = new SparkMaxDerivedVelocityController(driveSparkMax); + driveDefaultEncoder = driveSparkMax.getEncoder(); + turnRelativeEncoder = turnSparkMax.getEncoder(); + turnRelativeEncoder.setPosition(0.0); + + driveSparkMax.setCANTimeout(0); + turnSparkMax.setCANTimeout(0); + + // Burn the spark maxes if necessary + if (SparkMAXBurnManager.shouldBurn()) { + driveSparkMax.burnFlash(); + turnSparkMax.burnFlash(); } -} \ No newline at end of file + } + + /** + * Update the AK hardware inputs + * + * @param inputs the inputs to update + */ + @Override + public void updateInputs(ModuleIOInputs inputs) { + inputs.drivePositionRad = + Units.rotationsToRadians(driveDerivedVelocityController.getPosition()) + / driveAfterEncoderReduction; + inputs.driveVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(driveDerivedVelocityController.getVelocity()) + / driveAfterEncoderReduction; + inputs.driveVelocityFilteredRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(driveDefaultEncoder.getVelocity()) + / driveAfterEncoderReduction; + inputs.driveAppliedVolts = + driveSparkMax.getAppliedOutput() * RobotController.getBatteryVoltage(); + inputs.driveCurrentAmps = driveSparkMax.getOutputCurrent(); + inputs.driveTempCelcius = driveSparkMax.getMotorTemperature(); + + inputs.turnAbsolutePositionRad = + Units.degreesToRadians(turnAbsoluteEncoder.getAbsolutePosition()) + - absoluteEncoderOffset.getRadians(); + + inputs.turnPositionRad = + Units.rotationsToRadians(turnRelativeEncoder.getPosition()) / turnAfterEncoderReduction; + inputs.turnVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) + / turnAfterEncoderReduction; + inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * RobotController.getBatteryVoltage(); + inputs.turnCurrentAmps = turnSparkMax.getOutputCurrent(); + inputs.turnTempCelcius = turnSparkMax.getMotorTemperature(); + } + + /** + * Set the drive voltage of the Spark MAX + * + * @param volts The voltage to run the Spark Max at + */ + public void setDriveVoltage(double volts) { + driveSparkMax.setVoltage(volts); + } + + /** + * Set the turn voltage of the Spark MAX + * + * @param volts The voltage to run the Spark Max at + */ + public void setTurnVoltage(double volts) { + turnSparkMax.setVoltage(volts); + } + + /** + * Set the brake mode of the drive NEO + * + * @param enable whether to enable the brake mode or not + */ + public void setDriveBrakeMode(boolean enable) { + driveSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } + + /** + * Set the brake mode of the turn NEO + * + * @param enable whether to enable the brake mode or not + */ + public void setTurnBrakeMode(boolean enable) { + turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); + } +}