diff --git a/Robot2024/.gitignore b/Robot2024/.gitignore index 5528d4f..22f2c13 100644 --- a/Robot2024/.gitignore +++ b/Robot2024/.gitignore @@ -162,7 +162,6 @@ bin/ *.iml *.ipr *.iws -.idea/ out/ # Fleet diff --git a/Robot2024/build.gradle b/Robot2024/build.gradle index d663d93..0a5b832 100644 --- a/Robot2024/build.gradle +++ b/Robot2024/build.gradle @@ -71,6 +71,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testImplementation 'junit:junit:4.13.2' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' } @@ -100,6 +101,14 @@ deployArtifact.jarTask = jar wpi.java.configureExecutableTasks(jar) wpi.java.configureTestTasks(test) +// the magic line that makes tests work :) +wpi.java.configureTestTasks(test) + +tasks.named('test') { + // Use JUnit 4 for unit tests. + useJUnit() +} + // Configure string concat to always inline compile tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' diff --git a/Robot2024/lib/build.gradle b/Robot2024/lib/build.gradle index 14b2ed2..8509765 100644 --- a/Robot2024/lib/build.gradle +++ b/Robot2024/lib/build.gradle @@ -13,6 +13,11 @@ plugins { id 'maven-publish' } +java { + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 +} + tasks.withType(JavaCompile) { // Configure string concat to always inline compile options.compilerArgs.add '-XDstringConcat=inline' @@ -43,7 +48,7 @@ dependencies { implementation 'edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:2023.4.3' - testImplementation 'junit:junit:4.13' + testImplementation 'junit:junit:4.13.2' } // the magic line that makes tests work :) diff --git a/Robot2024/lib/src/main/java/com/team2813/lib2813/control/imu/Pigeon2Wrapper.java b/Robot2024/lib/src/main/java/com/team2813/lib2813/control/imu/Pigeon2Wrapper.java index 83cd5e5..3553aa9 100644 --- a/Robot2024/lib/src/main/java/com/team2813/lib2813/control/imu/Pigeon2Wrapper.java +++ b/Robot2024/lib/src/main/java/com/team2813/lib2813/control/imu/Pigeon2Wrapper.java @@ -4,6 +4,7 @@ import com.ctre.phoenix.sensors.Pigeon2Configuration; import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame; import com.team2813.lib2813.util.ConfigUtils; +import com.team2813.lib2813.util.Port; public class Pigeon2Wrapper extends Pigeon2 { @@ -13,12 +14,12 @@ public class Pigeon2Wrapper extends Pigeon2 { /** * Constructor - * @param deviceNumber [0,62] + * @param port The port to configure. * @param canbus Name of the CANbus; can be a SocketCAN interface (on Linux), * or a CANivore device name or serial number */ - public Pigeon2Wrapper(int deviceNumber, String canbus) { - super(deviceNumber, canbus); + public Pigeon2Wrapper(Port port, String canbus) { + super(port.getCanId(), canbus); canivore = true; ConfigUtils.ctreConfig(() -> configAllSettings(new Pigeon2Configuration())); @@ -26,10 +27,10 @@ public Pigeon2Wrapper(int deviceNumber, String canbus) { /** * Constructor - * @param deviceNumber [0,62] + * @param The port to configure. */ - public Pigeon2Wrapper(int deviceNumber) { - super(deviceNumber); + public Pigeon2Wrapper(Port port) { + super(port.getCanId()); canivore = false; ConfigUtils.ctreConfig(() -> configAllSettings(new Pigeon2Configuration())); diff --git a/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/drive/Falcon500DriveController.java b/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/drive/Falcon500DriveController.java index 702119b..f3b7963 100644 --- a/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/drive/Falcon500DriveController.java +++ b/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/drive/Falcon500DriveController.java @@ -6,6 +6,7 @@ import com.swervedrivespecialties.swervelib.Mk4ModuleConfiguration; import com.swervedrivespecialties.swervelib.ModuleConfiguration; import com.team2813.lib2813.util.ConfigUtils; +import com.team2813.lib2813.util.Port; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardContainer; @@ -18,7 +19,7 @@ public class Falcon500DriveController implements DriveController { private SimpleMotorFeedforward feedforward; private boolean hasPidConstants = false; - public Falcon500DriveController(int id, ModuleConfiguration moduleConfiguration, Mk4ModuleConfiguration mk4Configuration) { + public Falcon500DriveController(Port port, ModuleConfiguration moduleConfiguration, Mk4ModuleConfiguration mk4Configuration) { maxVelocity = 6380.0 / 60.0 * moduleConfiguration.getDriveReduction() * moduleConfiguration.getWheelDiameter() * Math.PI; TalonFXConfiguration motorConfiguration = new TalonFXConfiguration(); @@ -30,7 +31,7 @@ public Falcon500DriveController(int id, ModuleConfiguration moduleConfiguration, motorConfiguration.supplyCurrLimit.currentLimit = mk4Configuration.getDriveCurrentLimit(); motorConfiguration.supplyCurrLimit.enable = true; - motor = new TalonFX(id); + motor = new TalonFX(port.getCanId()); ConfigUtils.ctreConfig(() -> motor.configAllSettings(motorConfiguration)); motor.enableVoltageCompensation(true); @@ -48,7 +49,7 @@ public Falcon500DriveController(int id, ModuleConfiguration moduleConfiguration, ); } - public Falcon500DriveController(int id, String canbus, ModuleConfiguration moduleConfiguration, Mk4ModuleConfiguration mk4Configuration) { + public Falcon500DriveController(Port port, String canbus, ModuleConfiguration moduleConfiguration, Mk4ModuleConfiguration mk4Configuration) { maxVelocity = 6380.0 / 60.0 * moduleConfiguration.getDriveReduction() * moduleConfiguration.getWheelDiameter() * Math.PI; TalonFXConfiguration motorConfiguration = new TalonFXConfiguration(); @@ -60,7 +61,7 @@ public Falcon500DriveController(int id, String canbus, ModuleConfiguration modul motorConfiguration.supplyCurrLimit.currentLimit = mk4Configuration.getDriveCurrentLimit(); motorConfiguration.supplyCurrLimit.enable = true; - motor = new TalonFX(id, canbus); + motor = new TalonFX(port.getCanId(), canbus); ConfigUtils.ctreConfig(() -> motor.configAllSettings(motorConfiguration)); motor.enableVoltageCompensation(true); diff --git a/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/drive/NeoDriveController.java b/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/drive/NeoDriveController.java index 3d8ad90..1689847 100644 --- a/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/drive/NeoDriveController.java +++ b/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/drive/NeoDriveController.java @@ -4,6 +4,7 @@ import com.swervedrivespecialties.swervelib.Mk4ModuleConfiguration; import com.swervedrivespecialties.swervelib.ModuleConfiguration; import com.team2813.lib2813.util.ConfigUtils; +import com.team2813.lib2813.util.Port; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardContainer; @@ -16,10 +17,10 @@ public class NeoDriveController implements DriveController { private SimpleMotorFeedforward feedforward; private boolean hasPidConstants = false; - public NeoDriveController(int id, ModuleConfiguration moduleConfiguration, Mk4ModuleConfiguration mk4Configuration) { + public NeoDriveController(Port port, ModuleConfiguration moduleConfiguration, Mk4ModuleConfiguration mk4Configuration) { maxVelocity = 5676.0 / 60.0 * moduleConfiguration.getDriveReduction() * moduleConfiguration.getWheelDiameter(); - motor = new CANSparkMax(id, CANSparkLowLevel.MotorType.kBrushless); + motor = new CANSparkMax(port.getCanId(), CANSparkLowLevel.MotorType.kBrushless); motor.setInverted(moduleConfiguration.isDriveInverted()); ConfigUtils.revConfig(() -> motor.enableVoltageCompensation(mk4Configuration.getNominalVoltage())); diff --git a/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/steer/Falcon500SteerConfiguration.java b/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/steer/Falcon500SteerConfiguration.java index 0a16836..5435a1f 100644 --- a/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/steer/Falcon500SteerConfiguration.java +++ b/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/steer/Falcon500SteerConfiguration.java @@ -1,18 +1,19 @@ package com.team2813.lib2813.swerve.controllers.steer; import com.swervedrivespecialties.swervelib.ctre.CanCoderAbsoluteConfiguration; +import com.team2813.lib2813.util.Port; public class Falcon500SteerConfiguration extends SteerConfiguration { private final String canbus; - public Falcon500SteerConfiguration(int motorPort, String canbus, CanCoderAbsoluteConfiguration encoderConfiguration) { + public Falcon500SteerConfiguration(Port motorPort, String canbus, CanCoderAbsoluteConfiguration encoderConfiguration) { super(motorPort, encoderConfiguration); this.canbus = canbus; } - public Falcon500SteerConfiguration(int motorPort, CanCoderAbsoluteConfiguration encoderConfiguration) { + public Falcon500SteerConfiguration(Port motorPort, CanCoderAbsoluteConfiguration encoderConfiguration) { this(motorPort, "", encoderConfiguration); } diff --git a/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/steer/Falcon500SteerController.java b/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/steer/Falcon500SteerController.java index 5df3936..043514b 100644 --- a/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/steer/Falcon500SteerController.java +++ b/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/steer/Falcon500SteerController.java @@ -55,7 +55,7 @@ public Falcon500SteerController( motorConfiguration.supplyCurrLimit.currentLimit = mk4Configuration.getSteerCurrentLimit(); motorConfiguration.supplyCurrLimit.enable = true; - motor = new TalonFX(steerConfiguration.getMotorPort(), steerConfiguration.getCanbus()); + motor = new TalonFX(steerConfiguration.getMotorPort().getCanId(), steerConfiguration.getCanbus()); ConfigUtils.ctreConfig(() -> motor.configAllSettings(motorConfiguration, 250)); motor.enableVoltageCompensation(true); diff --git a/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/steer/NeoSteerController.java b/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/steer/NeoSteerController.java index 2735b50..b8a7a37 100644 --- a/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/steer/NeoSteerController.java +++ b/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/steer/NeoSteerController.java @@ -44,7 +44,7 @@ public NeoSteerController(SteerConfiguration steerConfiguration, ModuleConfigura absoluteEncoder = new SteerEncoder(cancoder); - motor = new CANSparkMax(steerConfiguration.getMotorPort(), CANSparkLowLevel.MotorType.kBrushless); + motor = new CANSparkMax(steerConfiguration.getMotorPort().getCanId(), CANSparkLowLevel.MotorType.kBrushless); ConfigUtils.revConfig(() -> motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus0, 100)); ConfigUtils.revConfig(() -> motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus1, 20)); ConfigUtils.revConfig(() -> motor.setPeriodicFramePeriod(CANSparkLowLevel.PeriodicFrame.kStatus2, 20)); diff --git a/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/steer/SteerConfiguration.java b/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/steer/SteerConfiguration.java index 34707e0..5cd1048 100644 --- a/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/steer/SteerConfiguration.java +++ b/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/controllers/steer/SteerConfiguration.java @@ -1,19 +1,20 @@ package com.team2813.lib2813.swerve.controllers.steer; import com.swervedrivespecialties.swervelib.ctre.CanCoderAbsoluteConfiguration; +import com.team2813.lib2813.util.Port; public class SteerConfiguration { - private final int motorPort; + private final Port motorPort; private final CanCoderAbsoluteConfiguration encoderConfiguration; - public SteerConfiguration(int motorPort, CanCoderAbsoluteConfiguration encoderConfiguration) { + public SteerConfiguration(Port motorPort, CanCoderAbsoluteConfiguration encoderConfiguration) { this.motorPort = motorPort; this.encoderConfiguration = encoderConfiguration; } - public int getMotorPort() { + public Port getMotorPort() { return motorPort; } diff --git a/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/helpers/Mk4SwerveModuleHelper.java b/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/helpers/Mk4SwerveModuleHelper.java index 0ef0a85..4b696f2 100644 --- a/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/helpers/Mk4SwerveModuleHelper.java +++ b/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/helpers/Mk4SwerveModuleHelper.java @@ -8,6 +8,7 @@ import com.team2813.lib2813.swerve.controllers.SwerveModule; import com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerController; import com.team2813.lib2813.swerve.controllers.steer.NeoSteerController; +import com.team2813.lib2813.util.Port; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; @@ -34,9 +35,9 @@ public static SwerveModule createFalcon500( ShuffleboardLayout container, Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { Falcon500DriveController driveController = new Falcon500DriveController(driveMotorPort, gearRatio.getConfiguration(), configuration); @@ -45,7 +46,7 @@ public static SwerveModule createFalcon500( Falcon500SteerController steerController = new Falcon500SteerController( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -76,9 +77,9 @@ public static SwerveModule createFalcon500( Mk4ModuleConfiguration configuration, GearRatio gearRatio, String canbus, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { Falcon500DriveController driveController = new Falcon500DriveController(driveMotorPort, canbus, gearRatio.getConfiguration(), configuration); @@ -88,7 +89,7 @@ public static SwerveModule createFalcon500( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, canbus, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -114,9 +115,9 @@ public static SwerveModule createFalcon500( public static SwerveModule createFalcon500( ShuffleboardLayout container, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { return createFalcon500( @@ -149,9 +150,9 @@ public static SwerveModule createFalcon500( ShuffleboardLayout container, GearRatio gearRatio, String canbus, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { return createFalcon500( @@ -181,9 +182,9 @@ public static SwerveModule createFalcon500( public static SwerveModule createFalcon500( Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { Falcon500DriveController driveController = new Falcon500DriveController(driveMotorPort, gearRatio.getConfiguration(), configuration); @@ -191,7 +192,7 @@ public static SwerveModule createFalcon500( Falcon500SteerController steerController = new Falcon500SteerController( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -218,9 +219,9 @@ public static SwerveModule createFalcon500( Mk4ModuleConfiguration configuration, GearRatio gearRatio, String canbus, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { Falcon500DriveController driveController = new Falcon500DriveController(driveMotorPort, canbus, gearRatio.getConfiguration(), configuration); @@ -229,7 +230,7 @@ public static SwerveModule createFalcon500( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, canbus, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -251,9 +252,9 @@ public static SwerveModule createFalcon500( */ public static SwerveModule createFalcon500( GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { return createFalcon500( @@ -282,9 +283,9 @@ public static SwerveModule createFalcon500( public static SwerveModule createFalcon500( GearRatio gearRatio, String canbus, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { return createFalcon500( @@ -319,9 +320,9 @@ public static SwerveModule createFalcon500( ShuffleboardLayout container, Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -334,7 +335,7 @@ public static SwerveModule createFalcon500( Falcon500SteerController steerController = new Falcon500SteerController( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -368,9 +369,9 @@ public static SwerveModule createFalcon500( Mk4ModuleConfiguration configuration, GearRatio gearRatio, String canbus, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -384,7 +385,7 @@ public static SwerveModule createFalcon500( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, canbus, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -413,9 +414,9 @@ public static SwerveModule createFalcon500( public static SwerveModule createFalcon500( ShuffleboardLayout container, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -457,9 +458,9 @@ public static SwerveModule createFalcon500( ShuffleboardLayout container, GearRatio gearRatio, String canbus, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -498,9 +499,9 @@ public static SwerveModule createFalcon500( public static SwerveModule createFalcon500( Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -512,7 +513,7 @@ public static SwerveModule createFalcon500( Falcon500SteerController steerController = new Falcon500SteerController( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -542,9 +543,9 @@ public static SwerveModule createFalcon500( Mk4ModuleConfiguration configuration, GearRatio gearRatio, String canbus, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -557,7 +558,7 @@ public static SwerveModule createFalcon500( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, canbus, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -582,9 +583,9 @@ public static SwerveModule createFalcon500( */ public static SwerveModule createFalcon500( GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -622,9 +623,9 @@ public static SwerveModule createFalcon500( public static SwerveModule createFalcon500( GearRatio gearRatio, String canbus, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -666,9 +667,9 @@ public static SwerveModule createFalcon500( ShuffleboardLayout container, Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -683,7 +684,7 @@ public static SwerveModule createFalcon500( Falcon500SteerController steerController = new Falcon500SteerController( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -718,9 +719,9 @@ public static SwerveModule createFalcon500( Mk4ModuleConfiguration configuration, GearRatio gearRatio, String canbus, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -736,7 +737,7 @@ public static SwerveModule createFalcon500( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, canbus, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -766,9 +767,9 @@ public static SwerveModule createFalcon500( public static SwerveModule createFalcon500( ShuffleboardLayout container, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -813,9 +814,9 @@ public static SwerveModule createFalcon500( ShuffleboardLayout container, GearRatio gearRatio, String canbus, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -857,9 +858,9 @@ public static SwerveModule createFalcon500( public static SwerveModule createFalcon500( Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -873,7 +874,7 @@ public static SwerveModule createFalcon500( Falcon500SteerController steerController = new Falcon500SteerController( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -904,9 +905,9 @@ public static SwerveModule createFalcon500( Mk4ModuleConfiguration configuration, GearRatio gearRatio, String canbus, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -921,7 +922,7 @@ public static SwerveModule createFalcon500( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, canbus, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -947,9 +948,9 @@ public static SwerveModule createFalcon500( */ public static SwerveModule createFalcon500( GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -990,9 +991,9 @@ public static SwerveModule createFalcon500( public static SwerveModule createFalcon500( GearRatio gearRatio, String canbus, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1031,9 +1032,9 @@ public static SwerveModule createNeo( ShuffleboardLayout container, Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { NeoDriveController driveController = new NeoDriveController(driveMotorPort, gearRatio.getConfiguration(), configuration); @@ -1042,7 +1043,7 @@ public static SwerveModule createNeo( NeoSteerController steerController = new NeoSteerController( new com.team2813.lib2813.swerve.controllers.steer.SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -1067,9 +1068,9 @@ public static SwerveModule createNeo( public static SwerveModule createNeo( ShuffleboardLayout container, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { return createNeo( @@ -1097,9 +1098,9 @@ public static SwerveModule createNeo( public static SwerveModule createNeo( Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { NeoDriveController driveController = new NeoDriveController(driveMotorPort, gearRatio.getConfiguration(), configuration); @@ -1107,7 +1108,7 @@ public static SwerveModule createNeo( NeoSteerController steerController = new NeoSteerController( new com.team2813.lib2813.swerve.controllers.steer.SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -1128,9 +1129,9 @@ public static SwerveModule createNeo( */ public static SwerveModule createNeo( GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { return createNeo( @@ -1163,9 +1164,9 @@ public static SwerveModule createNeo( ShuffleboardLayout container, Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1178,7 +1179,7 @@ public static SwerveModule createNeo( NeoSteerController steerController = new NeoSteerController( new com.team2813.lib2813.swerve.controllers.steer.SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -1206,9 +1207,9 @@ public static SwerveModule createNeo( public static SwerveModule createNeo( ShuffleboardLayout container, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1245,9 +1246,9 @@ public static SwerveModule createNeo( public static SwerveModule createNeo( Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1259,7 +1260,7 @@ public static SwerveModule createNeo( NeoSteerController steerController = new NeoSteerController( new com.team2813.lib2813.swerve.controllers.steer.SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -1283,9 +1284,9 @@ public static SwerveModule createNeo( */ public static SwerveModule createNeo( GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1325,9 +1326,9 @@ public static SwerveModule createNeo( ShuffleboardLayout container, Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1342,7 +1343,7 @@ public static SwerveModule createNeo( NeoSteerController steerController = new NeoSteerController( new com.team2813.lib2813.swerve.controllers.steer.SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -1371,9 +1372,9 @@ public static SwerveModule createNeo( public static SwerveModule createNeo( ShuffleboardLayout container, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1413,9 +1414,9 @@ public static SwerveModule createNeo( public static SwerveModule createNeo( Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1429,7 +1430,7 @@ public static SwerveModule createNeo( NeoSteerController steerController = new NeoSteerController( new com.team2813.lib2813.swerve.controllers.steer.SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -1454,9 +1455,9 @@ public static SwerveModule createNeo( */ public static SwerveModule createNeo( GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1495,9 +1496,9 @@ public static SwerveModule createFalcon500Neo( ShuffleboardLayout container, Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { Falcon500DriveController driveController = new Falcon500DriveController(driveMotorPort, gearRatio.getConfiguration(), configuration); @@ -1506,7 +1507,7 @@ public static SwerveModule createFalcon500Neo( NeoSteerController steerController = new NeoSteerController( new com.team2813.lib2813.swerve.controllers.steer.SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -1532,9 +1533,9 @@ public static SwerveModule createFalcon500Neo( public static SwerveModule createFalcon500Neo( ShuffleboardLayout container, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { return createFalcon500Neo( @@ -1563,9 +1564,9 @@ public static SwerveModule createFalcon500Neo( public static SwerveModule createFalcon500Neo( Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { Falcon500DriveController driveController = new Falcon500DriveController(driveMotorPort, gearRatio.getConfiguration(), configuration); @@ -1573,7 +1574,7 @@ public static SwerveModule createFalcon500Neo( NeoSteerController steerController = new NeoSteerController( new com.team2813.lib2813.swerve.controllers.steer.SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -1595,9 +1596,9 @@ public static SwerveModule createFalcon500Neo( */ public static SwerveModule createFalcon500Neo( GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { return createFalcon500Neo( @@ -1631,9 +1632,9 @@ public static SwerveModule createFalcon500Neo( ShuffleboardLayout container, Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1646,7 +1647,7 @@ public static SwerveModule createFalcon500Neo( NeoSteerController steerController = new NeoSteerController( new com.team2813.lib2813.swerve.controllers.steer.SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -1675,9 +1676,9 @@ public static SwerveModule createFalcon500Neo( public static SwerveModule createFalcon500Neo( ShuffleboardLayout container, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1715,9 +1716,9 @@ public static SwerveModule createFalcon500Neo( public static SwerveModule createFalcon500Neo( Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1729,7 +1730,7 @@ public static SwerveModule createFalcon500Neo( NeoSteerController steerController = new NeoSteerController( new com.team2813.lib2813.swerve.controllers.steer.SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -1754,9 +1755,9 @@ public static SwerveModule createFalcon500Neo( */ public static SwerveModule createFalcon500Neo( GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1797,9 +1798,9 @@ public static SwerveModule createFalcon500Neo( ShuffleboardLayout container, Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1814,7 +1815,7 @@ public static SwerveModule createFalcon500Neo( NeoSteerController steerController = new NeoSteerController( new com.team2813.lib2813.swerve.controllers.steer.SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -1844,9 +1845,9 @@ public static SwerveModule createFalcon500Neo( public static SwerveModule createFalcon500Neo( ShuffleboardLayout container, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1887,9 +1888,9 @@ public static SwerveModule createFalcon500Neo( public static SwerveModule createFalcon500Neo( Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1903,7 +1904,7 @@ public static SwerveModule createFalcon500Neo( NeoSteerController steerController = new NeoSteerController( new com.team2813.lib2813.swerve.controllers.steer.SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -1929,9 +1930,9 @@ public static SwerveModule createFalcon500Neo( */ public static SwerveModule createFalcon500Neo( GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1969,9 +1970,9 @@ public static SwerveModule createNeoFalcon500( ShuffleboardLayout container, Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { NeoDriveController driveController = new NeoDriveController(driveMotorPort, gearRatio.getConfiguration(), configuration); @@ -1980,7 +1981,7 @@ public static SwerveModule createNeoFalcon500( Falcon500SteerController steerController = new Falcon500SteerController( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -2005,9 +2006,9 @@ public static SwerveModule createNeoFalcon500( public static SwerveModule createNeoFalcon500( ShuffleboardLayout container, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { return createNeoFalcon500( @@ -2035,9 +2036,9 @@ public static SwerveModule createNeoFalcon500( public static SwerveModule createNeoFalcon500( Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { NeoDriveController driveController = new NeoDriveController(driveMotorPort, gearRatio.getConfiguration(), configuration); @@ -2045,7 +2046,7 @@ public static SwerveModule createNeoFalcon500( Falcon500SteerController steerController = new Falcon500SteerController( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -2066,9 +2067,9 @@ public static SwerveModule createNeoFalcon500( */ public static SwerveModule createNeoFalcon500( GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { return createNeoFalcon500( @@ -2101,9 +2102,9 @@ public static SwerveModule createNeoFalcon500( ShuffleboardLayout container, Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -2116,7 +2117,7 @@ public static SwerveModule createNeoFalcon500( Falcon500SteerController steerController = new Falcon500SteerController( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -2144,9 +2145,9 @@ public static SwerveModule createNeoFalcon500( public static SwerveModule createNeoFalcon500( ShuffleboardLayout container, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -2183,9 +2184,9 @@ public static SwerveModule createNeoFalcon500( public static SwerveModule createNeoFalcon500( Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -2197,7 +2198,7 @@ public static SwerveModule createNeoFalcon500( Falcon500SteerController steerController = new Falcon500SteerController( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -2221,9 +2222,9 @@ public static SwerveModule createNeoFalcon500( */ public static SwerveModule createNeoFalcon500( GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -2263,9 +2264,9 @@ public static SwerveModule createNeoFalcon500( ShuffleboardLayout container, Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -2280,7 +2281,7 @@ public static SwerveModule createNeoFalcon500( Falcon500SteerController steerController = new Falcon500SteerController( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -2309,9 +2310,9 @@ public static SwerveModule createNeoFalcon500( public static SwerveModule createNeoFalcon500( ShuffleboardLayout container, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -2351,9 +2352,9 @@ public static SwerveModule createNeoFalcon500( public static SwerveModule createNeoFalcon500( Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -2367,7 +2368,7 @@ public static SwerveModule createNeoFalcon500( Falcon500SteerController steerController = new Falcon500SteerController( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -2392,9 +2393,9 @@ public static SwerveModule createNeoFalcon500( */ public static SwerveModule createNeoFalcon500( GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, diff --git a/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/helpers/Mk4iSwerveModuleHelper.java b/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/helpers/Mk4iSwerveModuleHelper.java index 0ed6e5a..573a390 100644 --- a/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/helpers/Mk4iSwerveModuleHelper.java +++ b/Robot2024/lib/src/main/java/com/team2813/lib2813/swerve/helpers/Mk4iSwerveModuleHelper.java @@ -3,6 +3,7 @@ import com.swervedrivespecialties.swervelib.Mk4ModuleConfiguration; import com.swervedrivespecialties.swervelib.Mk4iSwerveModuleHelper.GearRatio; import com.swervedrivespecialties.swervelib.ctre.CanCoderAbsoluteConfiguration; +import com.team2813.lib2813.util.Port; import com.team2813.lib2813.swerve.controllers.SwerveModule; import com.team2813.lib2813.swerve.controllers.drive.Falcon500DriveController; import com.team2813.lib2813.swerve.controllers.drive.NeoDriveController; @@ -34,9 +35,9 @@ public static SwerveModule createFalcon500( ShuffleboardLayout container, Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { Falcon500DriveController driveController = new Falcon500DriveController(driveMotorPort, gearRatio.getConfiguration(), configuration); @@ -45,7 +46,7 @@ public static SwerveModule createFalcon500( Falcon500SteerController steerController = new Falcon500SteerController( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -76,9 +77,9 @@ public static SwerveModule createFalcon500( Mk4ModuleConfiguration configuration, GearRatio gearRatio, String canbus, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { Falcon500DriveController driveController = new Falcon500DriveController(driveMotorPort, canbus, gearRatio.getConfiguration(), configuration); @@ -88,7 +89,7 @@ public static SwerveModule createFalcon500( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, canbus, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -114,9 +115,9 @@ public static SwerveModule createFalcon500( public static SwerveModule createFalcon500( ShuffleboardLayout container, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { return createFalcon500( @@ -149,9 +150,9 @@ public static SwerveModule createFalcon500( ShuffleboardLayout container, GearRatio gearRatio, String canbus, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { return createFalcon500( @@ -181,9 +182,9 @@ public static SwerveModule createFalcon500( public static SwerveModule createFalcon500( Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { Falcon500DriveController driveController = new Falcon500DriveController(driveMotorPort, gearRatio.getConfiguration(), configuration); @@ -191,7 +192,7 @@ public static SwerveModule createFalcon500( Falcon500SteerController steerController = new Falcon500SteerController( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -218,9 +219,9 @@ public static SwerveModule createFalcon500( Mk4ModuleConfiguration configuration, GearRatio gearRatio, String canbus, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { Falcon500DriveController driveController = new Falcon500DriveController(driveMotorPort, canbus, gearRatio.getConfiguration(), configuration); @@ -229,7 +230,7 @@ public static SwerveModule createFalcon500( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, canbus, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -251,9 +252,9 @@ public static SwerveModule createFalcon500( */ public static SwerveModule createFalcon500( GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { return createFalcon500( @@ -282,9 +283,9 @@ public static SwerveModule createFalcon500( public static SwerveModule createFalcon500( GearRatio gearRatio, String canbus, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { return createFalcon500( @@ -319,9 +320,9 @@ public static SwerveModule createFalcon500( ShuffleboardLayout container, Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -334,7 +335,7 @@ public static SwerveModule createFalcon500( Falcon500SteerController steerController = new Falcon500SteerController( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -368,9 +369,9 @@ public static SwerveModule createFalcon500( Mk4ModuleConfiguration configuration, GearRatio gearRatio, String canbus, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -384,7 +385,7 @@ public static SwerveModule createFalcon500( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, canbus, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -413,9 +414,9 @@ public static SwerveModule createFalcon500( public static SwerveModule createFalcon500( ShuffleboardLayout container, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -457,9 +458,9 @@ public static SwerveModule createFalcon500( ShuffleboardLayout container, GearRatio gearRatio, String canbus, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -498,9 +499,9 @@ public static SwerveModule createFalcon500( public static SwerveModule createFalcon500( Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -512,7 +513,7 @@ public static SwerveModule createFalcon500( Falcon500SteerController steerController = new Falcon500SteerController( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -542,9 +543,9 @@ public static SwerveModule createFalcon500( Mk4ModuleConfiguration configuration, GearRatio gearRatio, String canbus, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -557,7 +558,7 @@ public static SwerveModule createFalcon500( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, canbus, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -582,9 +583,9 @@ public static SwerveModule createFalcon500( */ public static SwerveModule createFalcon500( GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -622,9 +623,9 @@ public static SwerveModule createFalcon500( public static SwerveModule createFalcon500( GearRatio gearRatio, String canbus, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -666,9 +667,9 @@ public static SwerveModule createFalcon500( ShuffleboardLayout container, Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -683,7 +684,7 @@ public static SwerveModule createFalcon500( Falcon500SteerController steerController = new Falcon500SteerController( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -718,9 +719,9 @@ public static SwerveModule createFalcon500( Mk4ModuleConfiguration configuration, GearRatio gearRatio, String canbus, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -736,7 +737,7 @@ public static SwerveModule createFalcon500( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, canbus, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -766,9 +767,9 @@ public static SwerveModule createFalcon500( public static SwerveModule createFalcon500( ShuffleboardLayout container, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -813,9 +814,9 @@ public static SwerveModule createFalcon500( ShuffleboardLayout container, GearRatio gearRatio, String canbus, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -857,9 +858,9 @@ public static SwerveModule createFalcon500( public static SwerveModule createFalcon500( Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -873,7 +874,7 @@ public static SwerveModule createFalcon500( Falcon500SteerController steerController = new Falcon500SteerController( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -904,9 +905,9 @@ public static SwerveModule createFalcon500( Mk4ModuleConfiguration configuration, GearRatio gearRatio, String canbus, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -921,7 +922,7 @@ public static SwerveModule createFalcon500( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, canbus, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -947,9 +948,9 @@ public static SwerveModule createFalcon500( */ public static SwerveModule createFalcon500( GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -990,9 +991,9 @@ public static SwerveModule createFalcon500( public static SwerveModule createFalcon500( GearRatio gearRatio, String canbus, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1031,9 +1032,9 @@ public static SwerveModule createNeo( ShuffleboardLayout container, Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { NeoDriveController driveController = new NeoDriveController(driveMotorPort, gearRatio.getConfiguration(), configuration); @@ -1042,7 +1043,7 @@ public static SwerveModule createNeo( NeoSteerController steerController = new NeoSteerController( new com.team2813.lib2813.swerve.controllers.steer.SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -1067,9 +1068,9 @@ public static SwerveModule createNeo( public static SwerveModule createNeo( ShuffleboardLayout container, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { return createNeo( @@ -1097,9 +1098,9 @@ public static SwerveModule createNeo( public static SwerveModule createNeo( Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { NeoDriveController driveController = new NeoDriveController(driveMotorPort, gearRatio.getConfiguration(), configuration); @@ -1107,7 +1108,7 @@ public static SwerveModule createNeo( NeoSteerController steerController = new NeoSteerController( new com.team2813.lib2813.swerve.controllers.steer.SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -1128,9 +1129,9 @@ public static SwerveModule createNeo( */ public static SwerveModule createNeo( GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { return createNeo( @@ -1163,9 +1164,9 @@ public static SwerveModule createNeo( ShuffleboardLayout container, Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1178,7 +1179,7 @@ public static SwerveModule createNeo( NeoSteerController steerController = new NeoSteerController( new com.team2813.lib2813.swerve.controllers.steer.SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -1206,9 +1207,9 @@ public static SwerveModule createNeo( public static SwerveModule createNeo( ShuffleboardLayout container, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1245,9 +1246,9 @@ public static SwerveModule createNeo( public static SwerveModule createNeo( Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1259,7 +1260,7 @@ public static SwerveModule createNeo( NeoSteerController steerController = new NeoSteerController( new com.team2813.lib2813.swerve.controllers.steer.SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -1283,9 +1284,9 @@ public static SwerveModule createNeo( */ public static SwerveModule createNeo( GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1325,9 +1326,9 @@ public static SwerveModule createNeo( ShuffleboardLayout container, Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1342,7 +1343,7 @@ public static SwerveModule createNeo( NeoSteerController steerController = new NeoSteerController( new com.team2813.lib2813.swerve.controllers.steer.SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -1371,9 +1372,9 @@ public static SwerveModule createNeo( public static SwerveModule createNeo( ShuffleboardLayout container, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1413,9 +1414,9 @@ public static SwerveModule createNeo( public static SwerveModule createNeo( Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1429,7 +1430,7 @@ public static SwerveModule createNeo( NeoSteerController steerController = new NeoSteerController( new com.team2813.lib2813.swerve.controllers.steer.SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -1454,9 +1455,9 @@ public static SwerveModule createNeo( */ public static SwerveModule createNeo( GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1495,9 +1496,9 @@ public static SwerveModule createFalcon500Neo( ShuffleboardLayout container, Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { Falcon500DriveController driveController = new Falcon500DriveController(driveMotorPort, gearRatio.getConfiguration(), configuration); @@ -1506,7 +1507,7 @@ public static SwerveModule createFalcon500Neo( NeoSteerController steerController = new NeoSteerController( new com.team2813.lib2813.swerve.controllers.steer.SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -1532,9 +1533,9 @@ public static SwerveModule createFalcon500Neo( public static SwerveModule createFalcon500Neo( ShuffleboardLayout container, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { return createFalcon500Neo( @@ -1563,9 +1564,9 @@ public static SwerveModule createFalcon500Neo( public static SwerveModule createFalcon500Neo( Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { Falcon500DriveController driveController = new Falcon500DriveController(driveMotorPort, gearRatio.getConfiguration(), configuration); @@ -1573,7 +1574,7 @@ public static SwerveModule createFalcon500Neo( NeoSteerController steerController = new NeoSteerController( new com.team2813.lib2813.swerve.controllers.steer.SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -1595,9 +1596,9 @@ public static SwerveModule createFalcon500Neo( */ public static SwerveModule createFalcon500Neo( GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { return createFalcon500Neo( @@ -1631,9 +1632,9 @@ public static SwerveModule createFalcon500Neo( ShuffleboardLayout container, Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1646,7 +1647,7 @@ public static SwerveModule createFalcon500Neo( NeoSteerController steerController = new NeoSteerController( new com.team2813.lib2813.swerve.controllers.steer.SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -1675,9 +1676,9 @@ public static SwerveModule createFalcon500Neo( public static SwerveModule createFalcon500Neo( ShuffleboardLayout container, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1715,9 +1716,9 @@ public static SwerveModule createFalcon500Neo( public static SwerveModule createFalcon500Neo( Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1729,7 +1730,7 @@ public static SwerveModule createFalcon500Neo( NeoSteerController steerController = new NeoSteerController( new com.team2813.lib2813.swerve.controllers.steer.SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -1754,9 +1755,9 @@ public static SwerveModule createFalcon500Neo( */ public static SwerveModule createFalcon500Neo( GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1797,9 +1798,9 @@ public static SwerveModule createFalcon500Neo( ShuffleboardLayout container, Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1814,7 +1815,7 @@ public static SwerveModule createFalcon500Neo( NeoSteerController steerController = new NeoSteerController( new com.team2813.lib2813.swerve.controllers.steer.SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -1844,9 +1845,9 @@ public static SwerveModule createFalcon500Neo( public static SwerveModule createFalcon500Neo( ShuffleboardLayout container, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1887,9 +1888,9 @@ public static SwerveModule createFalcon500Neo( public static SwerveModule createFalcon500Neo( Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1903,7 +1904,7 @@ public static SwerveModule createFalcon500Neo( NeoSteerController steerController = new NeoSteerController( new com.team2813.lib2813.swerve.controllers.steer.SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -1929,9 +1930,9 @@ public static SwerveModule createFalcon500Neo( */ public static SwerveModule createFalcon500Neo( GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -1969,9 +1970,9 @@ public static SwerveModule createNeoFalcon500( ShuffleboardLayout container, Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { NeoDriveController driveController = new NeoDriveController(driveMotorPort, gearRatio.getConfiguration(), configuration); @@ -1980,7 +1981,7 @@ public static SwerveModule createNeoFalcon500( Falcon500SteerController steerController = new Falcon500SteerController( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -2005,9 +2006,9 @@ public static SwerveModule createNeoFalcon500( public static SwerveModule createNeoFalcon500( ShuffleboardLayout container, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { return createNeoFalcon500( @@ -2035,9 +2036,9 @@ public static SwerveModule createNeoFalcon500( public static SwerveModule createNeoFalcon500( Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { NeoDriveController driveController = new NeoDriveController(driveMotorPort, gearRatio.getConfiguration(), configuration); @@ -2045,7 +2046,7 @@ public static SwerveModule createNeoFalcon500( Falcon500SteerController steerController = new Falcon500SteerController( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -2066,9 +2067,9 @@ public static SwerveModule createNeoFalcon500( */ public static SwerveModule createNeoFalcon500( GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double steerOffset ) { return createNeoFalcon500( @@ -2101,9 +2102,9 @@ public static SwerveModule createNeoFalcon500( ShuffleboardLayout container, Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -2116,7 +2117,7 @@ public static SwerveModule createNeoFalcon500( Falcon500SteerController steerController = new Falcon500SteerController( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -2144,9 +2145,9 @@ public static SwerveModule createNeoFalcon500( public static SwerveModule createNeoFalcon500( ShuffleboardLayout container, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -2183,9 +2184,9 @@ public static SwerveModule createNeoFalcon500( public static SwerveModule createNeoFalcon500( Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -2197,7 +2198,7 @@ public static SwerveModule createNeoFalcon500( Falcon500SteerController steerController = new Falcon500SteerController( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -2221,9 +2222,9 @@ public static SwerveModule createNeoFalcon500( */ public static SwerveModule createNeoFalcon500( GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -2263,9 +2264,9 @@ public static SwerveModule createNeoFalcon500( ShuffleboardLayout container, Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -2280,7 +2281,7 @@ public static SwerveModule createNeoFalcon500( Falcon500SteerController steerController = new Falcon500SteerController( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -2309,9 +2310,9 @@ public static SwerveModule createNeoFalcon500( public static SwerveModule createNeoFalcon500( ShuffleboardLayout container, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -2351,9 +2352,9 @@ public static SwerveModule createNeoFalcon500( public static SwerveModule createNeoFalcon500( Mk4ModuleConfiguration configuration, GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, @@ -2367,7 +2368,7 @@ public static SwerveModule createNeoFalcon500( Falcon500SteerController steerController = new Falcon500SteerController( new com.team2813.lib2813.swerve.controllers.steer.Falcon500SteerConfiguration( steerMotorPort, - new CanCoderAbsoluteConfiguration(steerEncoderPort, steerOffset) + new CanCoderAbsoluteConfiguration(steerEncoderPort.getCanId(), steerOffset) ), gearRatio.getConfiguration(), configuration @@ -2392,9 +2393,9 @@ public static SwerveModule createNeoFalcon500( */ public static SwerveModule createNeoFalcon500( GearRatio gearRatio, - int driveMotorPort, - int steerMotorPort, - int steerEncoderPort, + Port driveMotorPort, + Port steerMotorPort, + Port steerEncoderPort, double drive_kP, double drive_kI, double drive_kD, diff --git a/Robot2024/lib/src/main/java/com/team2813/lib2813/util/InputValidation.java b/Robot2024/lib/src/main/java/com/team2813/lib2813/util/InputValidation.java index 25a495b..be46573 100644 --- a/Robot2024/lib/src/main/java/com/team2813/lib2813/util/InputValidation.java +++ b/Robot2024/lib/src/main/java/com/team2813/lib2813/util/InputValidation.java @@ -9,10 +9,10 @@ private InputValidation() { } /** - * Check if a given value is between the bounds + * Check if a given value is between the bounds. * @param the type to compare - * @param lower an object that {@code actual} should be comparativly less than or equal to - * @param upper an object that {@code actual} should be comparativly greater than or equal to + * @param lower an object that {@code actual} should be comparatively less than or equal to + * @param upper an object that {@code actual} should be comparatively greater than or equal to * @param actual the actual value * @param throwable a function that takes the actual value and returns an unchecked exception * @throws RuntimeException when actual is not between lower and upper, based on natural ordering. @@ -25,22 +25,22 @@ static > void checkBounds(T lower, T upper, T actual, Fu } } /** - * Check if the given value is in the bounds - * @param lower the lower bound - * @param upper the upper bound + * Checks if the given value is in the bounds. + * @param lower the lower bound (inclusive) + * @param upper the upper bound (inclusive) * @param actual the actual value * @param throwable a function that takes the actual value and returns an unchecked exception - * @throws RuntimeException when the actual is not inbetween the bounds. + * @throws IllegalArgumentException when the actual is not in between the bounds. * exception is provided by {@code throwable} */ - static void checkBounds(int lower, int upper, int actual, IntFunction throwable) { + static void checkBounds(int lower, int upper, int actual, IntFunction throwable) { assert lower <= upper; if (!(lower <= actual && actual <= upper)) { throw throwable.apply(actual); } } /** - * Checks if the input is a valid {@index CAN} Id, and throws an exception if it isn't + * Checks if the input is a valid {@index CAN} Id, and throws an exception if it isn't. * @param id the can id, between 0 and 62, inclusive * @return the {@code id} * @throws InvalidCanIdException if the id is invalid diff --git a/Robot2024/lib/src/main/java/com/team2813/lib2813/util/InvalidCanIdException.java b/Robot2024/lib/src/main/java/com/team2813/lib2813/util/InvalidCanIdException.java index 18df084..a0582b5 100644 --- a/Robot2024/lib/src/main/java/com/team2813/lib2813/util/InvalidCanIdException.java +++ b/Robot2024/lib/src/main/java/com/team2813/lib2813/util/InvalidCanIdException.java @@ -5,24 +5,32 @@ /** * Signifies that a {@index CAN} Id was given to a function, and it was invalid */ -public class InvalidCanIdException extends RuntimeException { +public class InvalidCanIdException extends IllegalArgumentException { /** * The CAN id that is invalid * @serial an integer that is not between 0 and 62 */ - private int canId; + private final int canId; + + public InvalidCanIdException(int canId, String message) { + super(message); + this.canId = canId; + } + public InvalidCanIdException(int canId) { - super(String.format( - "%d is not a valid can id (a valid can id is between 0 and 62, inclusive)", + this(canId, String.format( + "%d is not a valid can ID (a valid can id is between 0 and 62, inclusive)", canId)); if (0 <= canId && canId <= 62) { throw new IllegalArgumentException(String.format( - "%s is a valid can id (it is between 0 and 62, inclusive)", canId)); + "%s is a valid can ID (it is between 0 and 62, inclusive)", canId)); } } + public int getCanId() { return canId; } + @Override public boolean equals(Object o) { if (!(o instanceof InvalidCanIdException)) @@ -30,6 +38,7 @@ public boolean equals(Object o) { InvalidCanIdException other = (InvalidCanIdException) o; return getMessage().equals(other.getMessage()); } + @Override public int hashCode() { return getMessage().hashCode(); diff --git a/Robot2024/lib/src/main/java/com/team2813/lib2813/util/Port.java b/Robot2024/lib/src/main/java/com/team2813/lib2813/util/Port.java new file mode 100644 index 0000000..2c597e7 --- /dev/null +++ b/Robot2024/lib/src/main/java/com/team2813/lib2813/util/Port.java @@ -0,0 +1,32 @@ +package com.team2813.lib2813.util; + +import java.util.HashSet; +import java.util.Set; + +import static com.team2813.lib2813.util.InputValidation.checkCanId; + +/** + * Type-safe wrapper for a can port ID. + */ +public final class Port { + private static final Set _GLOBAL_REGISTRY = new HashSet<>(); + private final int id; + + public Port(int id) { + this(id, _GLOBAL_REGISTRY); + } + + /** Visible for testing. */ + Port(int id, Set canIdRegistry) { + this.id = checkCanId(id); + if (!canIdRegistry.add(id)) { + throw new InvalidCanIdException( + id, String.format("Can ID %d already registered", id)); + } + } + + /** The ID (device number) for this port. */ + public int getCanId() { + return id; + } +} diff --git a/Robot2024/lib/src/test/java/com/team2813/lib2813/util/PortTest.java b/Robot2024/lib/src/test/java/com/team2813/lib2813/util/PortTest.java new file mode 100644 index 0000000..ebca568 --- /dev/null +++ b/Robot2024/lib/src/test/java/com/team2813/lib2813/util/PortTest.java @@ -0,0 +1,60 @@ +package com.team2813.lib2813.util; + +import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertThrows; +import static org.junit.Assert.assertTrue; + +import com.team2813.lib2813.theories.Between; +import org.junit.Test; +import org.junit.experimental.theories.Theories; +import org.junit.experimental.theories.Theory; +import org.junit.runner.RunWith; +import org.junit.runners.JUnit4; + +import java.util.HashSet; +import java.util.Set; + +import com.team2813.lib2813.util.Port; + +@RunWith(Theories.class) +public class PortTest { + private final Set canRegistry = new HashSet<>(); + + @Test + public void getCanIdReturnsCanId() { + Port port = new Port(3, canRegistry); + assertEquals(3, port.getCanId()); + } + + @Test + public void multipleCanIds() { + new Port(1, canRegistry); + assertEquals(1, canRegistry.size()); + new Port(2, canRegistry);; + assertEquals(2, canRegistry.size()); + } + + @Test + public void duplicateCanIdRaises() { + new Port(1, canRegistry); + assertThrows(IllegalArgumentException.class, () -> new Port(1, canRegistry)); + assertEquals(1, canRegistry.size()); + } + + @Test + public void invalidCanIdRaises() { + assertThrows(InvalidCanIdException.class, () -> new Port(-1, canRegistry)); + assertThrows(InvalidCanIdException.class, () -> new Port(63, canRegistry)); + assertTrue(canRegistry.isEmpty()); + + new Port(0, canRegistry); + assertEquals(1, canRegistry.size()); + new Port(62, canRegistry); + assertEquals(2, canRegistry.size()); + } + + @Theory + public void validCanID(@Between(first = 0, last = 62) int canID) { + new Port(canID, canRegistry); + } +} diff --git a/Robot2024/src/main/java/com/team2813/Constants.java b/Robot2024/src/main/java/com/team2813/Constants.java index 1785760..07080b5 100644 --- a/Robot2024/src/main/java/com/team2813/Constants.java +++ b/Robot2024/src/main/java/com/team2813/Constants.java @@ -7,6 +7,8 @@ import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller; import edu.wpi.first.wpilibj2.command.button.Trigger; +import com.team2813.lib2813.util.Port; + /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean * constants. This class should not be used for any other purpose. All constants should be declared @@ -26,27 +28,27 @@ public static class DriverConstants { } // front right swerve module - public static final int FRONT_RIGHT_DRIVE_ID = 1; - public static final int FRONT_RIGHT_ENCODER_ID = 2; - public static final int FRONT_RIGHT_STEER_ID = 3; + public static final Port FRONT_RIGHT_DRIVE_PORT = new Port(1); + public static final Port FRONT_RIGHT_ENCODER_PORT = new Port(2); + public static final Port FRONT_RIGHT_STEER_PORT = new Port(3); // front left swerve module - public static final int FRONT_LEFT_DRIVE_ID = 4; - public static final int FRONT_LEFT_ENCODER_ID = 5; - public static final int FRONT_LEFT_STEER_ID = 6; + public static final Port FRONT_LEFT_DRIVE_PORT = new Port(4); + public static final Port FRONT_LEFT_ENCODER_PORT = new Port(5); + public static final Port FRONT_LEFT_STEER_PORT = new Port(6); // back right swerve module - public static final int BACK_RIGHT_DRIVE_ID = 7; - public static final int BACK_RIGHT_ENCODER_ID = 8; - public static final int BACK_RIGHT_STEER_ID = 9; + public static final Port BACK_RIGHT_DRIVE_PORT = new Port(7);; + public static final Port BACK_RIGHT_ENCODER_PORT = new Port(8); + public static final Port BACK_RIGHT_STEER_PORT = new Port(9); // back left swerve module - public static final int BACK_LEFT_DRIVE_ID = 10; - public static final int BACK_LEFT_ENCODER_ID = 11; - public static final int BACK_LEFT_STEER_ID = 12; + public static final Port BACK_LEFT_DRIVE_PORT = new Port(10);; + public static final Port BACK_LEFT_ENCODER_PORT = new Port(11); + public static final Port BACK_LEFT_STEER_PORT = new Port(12); // pigeon - public static final int PIGEON_ID = 13; + public static final Port PIGEON_PORT = new Port(13); //Mechanism CAN IDs public static final int INTAKE = 14; diff --git a/Robot2024/src/main/java/com/team2813/subsystems/Drive.java b/Robot2024/src/main/java/com/team2813/subsystems/Drive.java index 04c5447..660179c 100644 --- a/Robot2024/src/main/java/com/team2813/subsystems/Drive.java +++ b/Robot2024/src/main/java/com/team2813/subsystems/Drive.java @@ -7,11 +7,9 @@ import com.pathplanner.lib.util.ReplanningConfig; import com.swervedrivespecialties.swervelib.Mk4iSwerveModuleHelper.GearRatio; import com.swervedrivespecialties.swervelib.SdsModuleConfigurations; - import com.team2813.lib2813.control.imu.Pigeon2Wrapper; import com.team2813.lib2813.swerve.controllers.SwerveModule; import com.team2813.lib2813.swerve.helpers.Mk4iSwerveModuleHelper; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -56,7 +54,7 @@ public class Drive extends SubsystemBase { private final SwerveModule backLeftModule; private final SwerveModule backRightModule; - private final Pigeon2Wrapper pigeon = new Pigeon2Wrapper(PIGEON_ID); + private final Pigeon2Wrapper pigeon = new Pigeon2Wrapper(PIGEON_PORT); private ChassisSpeeds chassisSpeedDemand = new ChassisSpeeds(0, 0, 0); @@ -82,9 +80,9 @@ public Drive() { .withSize(2, 4).withPosition(0, 0), GearRatio.L2, canbus, - FRONT_LEFT_DRIVE_ID, - FRONT_LEFT_STEER_ID, - FRONT_LEFT_ENCODER_ID, + FRONT_LEFT_DRIVE_PORT, + FRONT_LEFT_STEER_PORT, + FRONT_LEFT_ENCODER_PORT, kP, kI, kD, @@ -96,9 +94,9 @@ public Drive() { .withSize(2, 4).withPosition(2, 0), GearRatio.L2, canbus, - FRONT_RIGHT_DRIVE_ID, - FRONT_RIGHT_STEER_ID, - FRONT_RIGHT_ENCODER_ID, + FRONT_RIGHT_DRIVE_PORT, + FRONT_RIGHT_STEER_PORT, + FRONT_RIGHT_ENCODER_PORT, kP, kI, kD, @@ -110,9 +108,9 @@ public Drive() { .withSize(2, 4).withPosition(4, 0), GearRatio.L2, canbus, - BACK_LEFT_DRIVE_ID, - BACK_LEFT_STEER_ID, - BACK_LEFT_ENCODER_ID, + BACK_LEFT_DRIVE_PORT, + BACK_LEFT_STEER_PORT, + BACK_LEFT_ENCODER_PORT, kP, kI, kD, @@ -124,9 +122,9 @@ public Drive() { .withSize(2, 4).withPosition(6, 0), GearRatio.L2, canbus, - BACK_RIGHT_DRIVE_ID, - BACK_RIGHT_STEER_ID, - BACK_RIGHT_ENCODER_ID, + BACK_RIGHT_DRIVE_PORT, + BACK_RIGHT_STEER_PORT, + BACK_RIGHT_ENCODER_PORT, kP, kI, kD,