Skip to content

Commit

Permalink
Extract Port class
Browse files Browse the repository at this point in the history
This improves type safety and ensures that can IDs are unique.
  • Loading branch information
kcooney committed Jan 24, 2024
1 parent cfc8683 commit bb61500
Show file tree
Hide file tree
Showing 18 changed files with 604 additions and 483 deletions.
1 change: 0 additions & 1 deletion Robot2024/.gitignore
Original file line number Diff line number Diff line change
Expand Up @@ -162,7 +162,6 @@ bin/
*.iml
*.ipr
*.iws
.idea/
out/

# Fleet
Expand Down
9 changes: 9 additions & 0 deletions Robot2024/build.gradle
Original file line number Diff line number Diff line change
Expand Up @@ -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'
}

Expand Down Expand Up @@ -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'
Expand Down
7 changes: 6 additions & 1 deletion Robot2024/lib/build.gradle
Original file line number Diff line number Diff line change
Expand Up @@ -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'
Expand Down Expand Up @@ -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 :)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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 {

Expand All @@ -13,23 +14,23 @@ 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()));
}

/**
* 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()));
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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();

Expand All @@ -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);
Expand All @@ -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();

Expand All @@ -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);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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()));
Expand Down
Original file line number Diff line number Diff line change
@@ -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);
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand Down
Original file line number Diff line number Diff line change
@@ -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;
}

Expand Down
Loading

0 comments on commit bb61500

Please sign in to comment.