From b3bec36a3f5eba29b30e26fed51b58a84e38b8a1 Mon Sep 17 00:00:00 2001 From: ParkerMeyers Date: Thu, 2 Nov 2023 12:45:54 -0400 Subject: [PATCH 1/3] Update VendorDeps to work with firmware & remove Unused libraries --- src/main/java/bhs/devilbotz/Constants.java | 4 +- .../devilbotz/subsystems/ModuleIOFalcon.java | 183 ------------------ .../subsystems/ModuleIOSparkMAX.java | 4 +- .../{Phoenix.json => Phoenix6And5.json} | 145 ++++++++------ vendordeps/SdsSwerveLib.json | 19 -- 5 files changed, 94 insertions(+), 261 deletions(-) delete mode 100644 src/main/java/bhs/devilbotz/subsystems/ModuleIOFalcon.java rename vendordeps/{Phoenix.json => Phoenix6And5.json} (76%) delete mode 100644 vendordeps/SdsSwerveLib.json diff --git a/src/main/java/bhs/devilbotz/Constants.java b/src/main/java/bhs/devilbotz/Constants.java index 6c3a441..e299a4e 100644 --- a/src/main/java/bhs/devilbotz/Constants.java +++ b/src/main/java/bhs/devilbotz/Constants.java @@ -11,12 +11,12 @@ public class Constants { // 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 = false; + public static final boolean tuningMode = true; // Which robot to use? // Options: // - ROBOT_2024S // - ROBOT_2024SIM - private static final RobotType robot = RobotType.ROBOT_2024SIM; + private static final RobotType robot = RobotType.ROBOT_2024S; // Get the current robot public static RobotType getRobot() { diff --git a/src/main/java/bhs/devilbotz/subsystems/ModuleIOFalcon.java b/src/main/java/bhs/devilbotz/subsystems/ModuleIOFalcon.java deleted file mode 100644 index bc3ff41..0000000 --- a/src/main/java/bhs/devilbotz/subsystems/ModuleIOFalcon.java +++ /dev/null @@ -1,183 +0,0 @@ -package bhs.devilbotz.subsystems; - -import bhs.devilbotz.Constants; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; -import com.ctre.phoenix.motorcontrol.TalonFXSensorCollection; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.ctre.phoenix.sensors.CANCoder; -import com.swervedrivespecialties.swervelib.Mk4SwerveModuleHelper; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; - -import java.util.Objects; - -/** - * Hardware interface for a swerve module with two Falcon 500s as drive and steer motors - */ -public class ModuleIOFalcon implements ModuleIO { - // 2 Motor and controller combos - private final WPI_TalonFX driveFalcon; - private final WPI_TalonFX turnFalcon; - - // Encoder initialization - private final CANCoder turnAbsoluteEncoder; - private final Rotation2d absoluteEncoderOffset; - - // Gear ratios for the module - private final double driveGearRatio = Mk4SwerveModuleHelper.GearRatio.L3.getConfiguration().getDriveReduction(); - private final double turnGearRatio = Mk4SwerveModuleHelper.GearRatio.L3.getConfiguration().getSteerReduction(); - - // 2 integrated Encoders for the motors - private final TalonFXSensorCollection driveDefaultEncoder; - private final TalonFXSensorCollection turnRelativeEncoder; - - // Encoder resolution - private final double driveEncoderTicksPerRevolution = 2048.0; - private final double turnEncoderTicksPerRevolution = 2048.0; - private final double turnAbsoluteEncoderTicksPerRevolution = 4096.0; - - /** - * Initialize the Module - * - * @param index which module to initialize - */ - public ModuleIOFalcon(int index) { - // Change the hardware depending on the type of robot - if (Objects.requireNonNull(Constants.getRobot()) == Constants.RobotType.ROBOT_2024S) { - // Depending on the module, initialize the hardware - switch (index) { - case 0: - driveFalcon = new WPI_TalonFX(11); - turnFalcon = new WPI_TalonFX(12); - turnAbsoluteEncoder = new CANCoder(13); - absoluteEncoderOffset = new Rotation2d(0.0); - break; - case 1: - driveFalcon = new WPI_TalonFX(14); - turnFalcon = new WPI_TalonFX(15); - turnAbsoluteEncoder = new CANCoder(16); - absoluteEncoderOffset = new Rotation2d(0.0); - break; - case 2: - driveFalcon = new WPI_TalonFX(17); - turnFalcon = new WPI_TalonFX(18); - turnAbsoluteEncoder = new CANCoder(19); - absoluteEncoderOffset = new Rotation2d(0.0); - break; - case 3: - driveFalcon = new WPI_TalonFX(20); - turnFalcon = new WPI_TalonFX(21); - turnAbsoluteEncoder = new CANCoder(22); - absoluteEncoderOffset = new Rotation2d(0.0); - break; - default: - throw new RuntimeException( - "Invalid module index for ModuleIOFalcon"); - } - } else { - throw new RuntimeException("Invalid robot for ModuleIOFalcon"); - } - - // Reset the falcons - driveFalcon.configFactoryDefault(); - turnFalcon.configFactoryDefault(); - - // Invert the turn falcon - turnFalcon.setInverted(true); - - // Set current limit to 30 amps for drive and 30 amps for turn - driveFalcon.configSupplyCurrentLimit( - new SupplyCurrentLimitConfiguration( - true, 30, 30, 0.1)); - turnFalcon.configSupplyCurrentLimit( - new SupplyCurrentLimitConfiguration( - true, 30, 30, 0.1)); - - // Set voltage compensation to 12 volts - driveFalcon.configVoltageCompSaturation(12.0); - driveFalcon.enableVoltageCompensation(true); - turnFalcon.configVoltageCompSaturation(12.0); - turnFalcon.enableVoltageCompensation(true); - - // Assign the encoder collection - driveDefaultEncoder = driveFalcon.getSensorCollection(); - turnRelativeEncoder = turnFalcon.getSensorCollection(); - - // Reset the encoders - resetEncoders(); - } - - /** - * Update the AK hardware inputs - * - * @param inputs the inputs to update - */ - @Override - public void updateInputs(ModuleIOInputs inputs) { - inputs.drivePositionRad = Units.rotationsToRadians( - driveDefaultEncoder.getIntegratedSensorPosition() / driveEncoderTicksPerRevolution); - inputs.driveVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond( - driveDefaultEncoder.getIntegratedSensorVelocity() / driveEncoderTicksPerRevolution * 60.0); - inputs.drivePositionMeters = inputs.drivePositionRad * driveGearRatio * Mk4SwerveModuleHelper.GearRatio.L2.getConfiguration().getWheelDiameter(); - - inputs.driveVolts = driveFalcon.getMotorOutputVoltage(); - inputs.driveCurrentAmps = driveFalcon.getStatorCurrent(); - inputs.driveTempCelcius = driveFalcon.getTemperature(); - - inputs.turnAbsolutePositionRad = Units.rotationsToRadians( - turnAbsoluteEncoder.getAbsolutePosition() / turnAbsoluteEncoderTicksPerRevolution); - inputs.turnPositionRad = Units.rotationsToRadians( - turnRelativeEncoder.getIntegratedSensorPosition() / turnEncoderTicksPerRevolution); - inputs.turnVelocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond( - turnRelativeEncoder.getIntegratedSensorVelocity() / turnEncoderTicksPerRevolution * 60.0); - - inputs.turnVolts = turnFalcon.getMotorOutputVoltage(); - inputs.turnCurrentAmps = turnFalcon.getStatorCurrent(); - inputs.turnTempCelcius = turnFalcon.getTemperature(); - } - - /** - * Set the drive voltage of the Falcon - * - * @param volts The voltage to run the falcon at - */ - public void setDriveVoltage(double volts) { - driveFalcon.setVoltage(volts); - } - - /** - * Set the turn voltage of the Falcon - * - * @param volts The voltage to run the falcon at - */ - public void setTurnVoltage(double volts) { - turnFalcon.setVoltage(volts); - } - - /** - * Set the brake mode of the drive Falcon - * - * @param enable whether to enable the brake mode or not - */ - public void setDriveBrakeMode(boolean enable) { - driveFalcon.setNeutralMode(enable ? NeutralMode.Brake : NeutralMode.Coast); - } - - /** - * Set the brake mode of the turn Falcon - * - * @param enable whether to enable the brake mode or not - */ - public void setTurnBrakeMode(boolean enable) { - turnFalcon.setNeutralMode(enable ? NeutralMode.Brake : NeutralMode.Coast); - } - - /** - * Reset the encoders - */ - public void resetEncoders() { - driveDefaultEncoder.setIntegratedSensorPosition(0, 0); - turnRelativeEncoder.setIntegratedSensorPosition(0, 0); - } -} \ No newline at end of file diff --git a/src/main/java/bhs/devilbotz/subsystems/ModuleIOSparkMAX.java b/src/main/java/bhs/devilbotz/subsystems/ModuleIOSparkMAX.java index 1e08a90..e9ea4f6 100644 --- a/src/main/java/bhs/devilbotz/subsystems/ModuleIOSparkMAX.java +++ b/src/main/java/bhs/devilbotz/subsystems/ModuleIOSparkMAX.java @@ -138,8 +138,8 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.driveCurrentAmps = driveSparkMax.getOutputCurrent(); inputs.driveTempCelcius = driveSparkMax.getMotorTemperature(); - inputs.turnAbsolutePositionRad = Units.rotationsToRadians( - turnAbsoluteEncoder.getAbsolutePosition() / turnAbsoluteEncoderTicksPerRevolution); + inputs.turnAbsolutePositionRad = + Units.degreesToRadians(turnAbsoluteEncoder.getAbsolutePosition()) - absoluteEncoderOffset.getRadians(); inputs.turnPositionRad = Units.rotationsToRadians(turnRelativeEncoder.getPosition()) diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix6And5.json similarity index 76% rename from vendordeps/Phoenix.json rename to vendordeps/Phoenix6And5.json index 282d536..c74983c 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix6And5.json @@ -1,30 +1,35 @@ { - "fileName": "Phoenix.json", - "name": "CTRE-Phoenix (v5)", - "version": "5.30.4+23.0.12", + "fileName": "Phoenix6And5.json", + "name": "CTRE-Phoenix (v6 And v5)", + "version": "23.2.2", "frcYear": 2023, - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2023-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6And5-frc2023-latest.json", "javaDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.30.4" + "version": "5.31.0" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.30.4" + "version": "5.31.0" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "23.2.2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.30.4", + "version": "5.31.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -37,7 +42,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.30.4", + "version": "5.31.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -48,9 +53,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro", + "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "23.0.12", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -61,9 +66,9 @@ "simMode": "hwsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "23.0.12", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -74,9 +79,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "23.0.12", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -87,9 +92,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "23.0.12", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -100,9 +105,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "23.0.12", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -113,9 +118,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "23.0.12", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -126,9 +131,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "23.0.12", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -139,9 +144,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "23.0.12", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,9 +157,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "23.0.12", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -165,9 +170,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "23.0.12", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -182,7 +187,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.30.4", + "version": "5.31.0", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -197,7 +202,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.30.4", + "version": "5.31.0", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": true, @@ -212,7 +217,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.30.4", + "version": "5.31.0", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": true, @@ -225,9 +230,9 @@ "simMode": "hwsim" }, { - "groupId": "com.ctre.phoenixpro", + "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "23.0.12", + "version": "23.2.2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -242,7 +247,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "wpiapi-cpp-sim", - "version": "5.30.4", + "version": "5.31.0", "libName": "CTRE_Phoenix_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -257,7 +262,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "api-cpp-sim", - "version": "5.30.4", + "version": "5.31.0", "libName": "CTRE_PhoenixSim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +277,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.30.4", + "version": "5.31.0", "libName": "CTRE_PhoenixCCISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -285,9 +290,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "23.0.12", + "version": "23.2.2", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -300,9 +305,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "23.0.12", + "version": "23.2.2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -315,9 +320,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "23.0.12", + "version": "23.2.2", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -330,9 +335,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "23.0.12", + "version": "23.2.2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -345,9 +350,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "23.0.12", + "version": "23.2.2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -360,9 +365,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "23.0.12", + "version": "23.2.2", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -375,9 +380,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "23.0.12", + "version": "23.2.2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -390,9 +395,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "23.0.12", + "version": "23.2.2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -405,9 +410,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "23.0.12", + "version": "23.2.2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -418,6 +423,36 @@ "osxuniversal" ], "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "23.2.2", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "23.2.2", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" } ] } \ No newline at end of file diff --git a/vendordeps/SdsSwerveLib.json b/vendordeps/SdsSwerveLib.json deleted file mode 100644 index e14c5e4..0000000 --- a/vendordeps/SdsSwerveLib.json +++ /dev/null @@ -1,19 +0,0 @@ -{ - "fileName": "SdsSwerveLib.json", - "name": "swerve-lib", - "version": "1.0.0", - "uuid": "9619F7EA-7F96-4236-9D94-02338DFED592", - "mavenUrls": [ - "https://jitpack.io" - ], - "jsonUrl": "https://raw.githubusercontent.com/SwerveDriveSpecialties/swerve-lib/master/SdsSwerveLib.json", - "javaDependencies": [ - { - "groupId": "com.github.SwerveDriveSpecialties", - "artifactId": "swerve-lib", - "version": "1.0.0" - } - ], - "jniDependencies": [], - "cppDependencies": [] -} \ No newline at end of file From 2583fc9418dd2ec1c5d7954c561d93525f357ee1 Mon Sep 17 00:00:00 2001 From: ParkerMeyers Date: Thu, 2 Nov 2023 13:09:21 -0400 Subject: [PATCH 2/3] 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); + } +} From e37fd50482647173a6a58d217adfaa94634f482f Mon Sep 17 00:00:00 2001 From: Parker Meyers <74273030+ParkerMeyers@users.noreply.github.com> Date: Thu, 2 Nov 2023 14:14:12 -0400 Subject: [PATCH 3/3] Update src/main/java/bhs/devilbotz/Constants.java --- src/main/java/bhs/devilbotz/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/bhs/devilbotz/Constants.java b/src/main/java/bhs/devilbotz/Constants.java index c15fa46..a3d8d95 100644 --- a/src/main/java/bhs/devilbotz/Constants.java +++ b/src/main/java/bhs/devilbotz/Constants.java @@ -11,7 +11,7 @@ public class Constants { // 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; + public static final boolean tuningMode = false; // Which robot to use? // Options: // - ROBOT_2024S