diff --git a/src/main/java/frc/robot/subsystems/drive/DriveIOTalon.java b/src/main/java/frc/robot/subsystems/drive/DriveIOTalon.java index 18702e0..f64274c 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveIOTalon.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveIOTalon.java @@ -13,94 +13,38 @@ package frc.robot.subsystems.drive; -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.*; -public class DriveIOTalon implements DriveIO { - private static final double GEAR_RATIO = 6.0; - - private final TalonFX leftLeader = new TalonFX(4); - private final TalonFX leftFollower = new TalonFX(5); - private final TalonFX rightLeader = new TalonFX(6); - private final TalonFX rightFollower = new TalonFX(8); - - private final StatusSignal leftPosition = leftLeader.getPosition(); - private final StatusSignal leftVelocity = leftLeader.getVelocity(); - private final StatusSignal leftAppliedVolts = leftLeader.getMotorVoltage(); - private final StatusSignal leftLeaderCurrent = leftLeader.getStatorCurrent(); - private final StatusSignal leftFollowerCurrent = leftFollower.getStatorCurrent(); - - private final StatusSignal rightPosition = rightLeader.getPosition(); - private final StatusSignal rightVelocity = rightLeader.getVelocity(); - private final StatusSignal rightAppliedVolts = rightLeader.getMotorVoltage(); - private final StatusSignal rightLeaderCurrent = rightLeader.getStatorCurrent(); - private final StatusSignal rightFollowerCurrent = rightFollower.getStatorCurrent(); +import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; +public class DriveIOTalon implements DriveIO { + private final WPI_VictorSPX leftLeader = new WPI_VictorSPX(4); + private final WPI_VictorSPX leftFollower = new WPI_VictorSPX(5); + private final WPI_VictorSPX rightLeader = new WPI_VictorSPX(6); + private final WPI_VictorSPX rightFollower = new WPI_VictorSPX(8); private GyroIOReal gyro; - public DriveIOTalon() { - var config = new TalonFXConfiguration(); - config.CurrentLimits.StatorCurrentLimit = 30.0; - config.CurrentLimits.StatorCurrentLimitEnable = true; - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - leftLeader.getConfigurator().apply(config); - leftFollower.getConfigurator().apply(config); - rightLeader.getConfigurator().apply(config); - rightFollower.getConfigurator().apply(config); - leftFollower.setControl(new Follower(leftLeader.getDeviceID(), false)); - rightFollower.setControl(new Follower(rightLeader.getDeviceID(), false)); + leftFollower.follow(leftLeader); + rightFollower.follow(rightLeader); - BaseStatusSignal.setUpdateFrequencyForAll( - 100.0, leftPosition, rightPosition); // Required for odometry, use faster rate - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - leftVelocity, - leftAppliedVolts, - leftLeaderCurrent, - leftFollowerCurrent, - rightVelocity, - rightAppliedVolts, - rightLeaderCurrent, - rightFollowerCurrent); - leftLeader.optimizeBusUtilization(); - leftFollower.optimizeBusUtilization(); - rightLeader.optimizeBusUtilization(); - rightFollower.optimizeBusUtilization(); + leftFollower.setNeutralMode(NeutralMode.Brake); + leftFollower.setNeutralMode(NeutralMode.Coast); + rightLeader.setNeutralMode(NeutralMode.Brake); + rightFollower.setNeutralMode(NeutralMode.Coast); gyro = GyroIOReal.getInstance(); } @Override public void updateInputs(DriveIOInputs inputs) { - BaseStatusSignal.refreshAll( - leftPosition, - leftVelocity, - leftAppliedVolts, - leftLeaderCurrent, - leftFollowerCurrent, - rightPosition, - rightVelocity, - rightAppliedVolts, - rightLeaderCurrent, - rightFollowerCurrent - ); - - inputs.leftPositionRad = Units.rotationsToRadians(leftPosition.getValueAsDouble()) / GEAR_RATIO; - inputs.leftVelocityRadPerSec = Units.rotationsToRadians(leftVelocity.getValueAsDouble()) / GEAR_RATIO; - inputs.rightPositionRad = Units.rotationsToRadians(rightPosition.getValueAsDouble()) / GEAR_RATIO; - inputs.rightVelocityRadPerSec = Units.rotationsToRadians(rightVelocity.getValueAsDouble()) / GEAR_RATIO; - inputs.gyroYawRad = gyro.getYawAngle(); + inputs.leftPositionRad = 0; + inputs.rightPositionRad = 0; + inputs.leftVelocityRadPerSec = leftLeader.getBusVoltage(); + inputs.rightVelocityRadPerSec = rightLeader.getBusVoltage(); inputs.gyroRollPitchYawRad[0] = gyro.getRollAngle(); inputs.gyroRollPitchYawRad[1] = gyro.getPitchAngle(); inputs.gyroRollPitchYawRad[2] = gyro.getYawAngle(); @@ -108,7 +52,27 @@ public void updateInputs(DriveIOInputs inputs) { @Override public void setVoltage(double leftVolts, double rightVolts) { - leftLeader.setControl(new VoltageOut(leftVolts)); - rightLeader.setControl(new VoltageOut(rightVolts)); + leftLeader.set(leftVolts); + rightLeader.set(rightVolts); + } + + /* No encoders so no position or velocity control works */ + @Override + public double getLeftPositionMeters() { + return 0; + } + + @Override + public double getRightPositionMeters() { + return 0; + } + + @Override + public void setVelocity(DifferentialDriveWheelSpeeds wheelSpeeds) { + } + + @Override + public void zero() { + GyroIOReal.getInstance().zeroAll(); } } \ No newline at end of file diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json new file mode 100644 index 0000000..88a68dd --- /dev/null +++ b/vendordeps/Phoenix5.json @@ -0,0 +1,151 @@ +{ + "fileName": "Phoenix5.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.33.0", + "frcYear": 2024, + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json", + "requires": [ + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", + "offlineFileName": "Phoenix6.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.33.0" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.33.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.33.0", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.33.0", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.0", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.33.0", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.33.0", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.0", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json deleted file mode 100644 index 7020206..0000000 --- a/vendordeps/Phoenix6.json +++ /dev/null @@ -1,339 +0,0 @@ -{ - "fileName": "Phoenix6.json", - "name": "CTRE-Phoenix (v6)", - "version": "24.0.0-beta-8", - "frcYear": 2024, - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-beta-latest.json", - "conflictsWith": [ - { - "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", - "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", - "offlineFileName": "Phoenix6And5.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-java", - "version": "24.0.0-beta-8" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "24.0.0-beta-8", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "24.0.0-beta-8", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "24.0.0-beta-8", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.0.0-beta-8", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "24.0.0-beta-8", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "24.0.0-beta-8", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "24.0.0-beta-8", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "24.0.0-beta-8", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "24.0.0-beta-8", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "24.0.0-beta-8", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-cpp", - "version": "24.0.0-beta-8", - "libName": "CTRE_Phoenix6_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "24.0.0-beta-8", - "libName": "CTRE_PhoenixTools", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "24.0.0-beta-8", - "libName": "CTRE_Phoenix6_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "24.0.0-beta-8", - "libName": "CTRE_PhoenixTools_Sim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "24.0.0-beta-8", - "libName": "CTRE_SimTalonSRX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.0.0-beta-8", - "libName": "CTRE_SimTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "24.0.0-beta-8", - "libName": "CTRE_SimVictorSPX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "24.0.0-beta-8", - "libName": "CTRE_SimPigeonIMU", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "24.0.0-beta-8", - "libName": "CTRE_SimCANCoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "24.0.0-beta-8", - "libName": "CTRE_SimProTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "24.0.0-beta-8", - "libName": "CTRE_SimProCANcoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "24.0.0-beta-8", - "libName": "CTRE_SimProPigeon2", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] -} \ No newline at end of file