Skip to content

Commit

Permalink
update talon code
Browse files Browse the repository at this point in the history
  • Loading branch information
@AkeBoss-tech
AkeBoss-tech committed Jan 12, 2024
1 parent 1ac726a commit 46dec4a
Show file tree
Hide file tree
Showing 3 changed files with 191 additions and 415 deletions.
116 changes: 40 additions & 76 deletions src/main/java/frc/robot/subsystems/drive/DriveIOTalon.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,102 +13,66 @@

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<Double> leftPosition = leftLeader.getPosition();
private final StatusSignal<Double> leftVelocity = leftLeader.getVelocity();
private final StatusSignal<Double> leftAppliedVolts = leftLeader.getMotorVoltage();
private final StatusSignal<Double> leftLeaderCurrent = leftLeader.getStatorCurrent();
private final StatusSignal<Double> leftFollowerCurrent = leftFollower.getStatorCurrent();

private final StatusSignal<Double> rightPosition = rightLeader.getPosition();
private final StatusSignal<Double> rightVelocity = rightLeader.getVelocity();
private final StatusSignal<Double> rightAppliedVolts = rightLeader.getMotorVoltage();
private final StatusSignal<Double> rightLeaderCurrent = rightLeader.getStatorCurrent();
private final StatusSignal<Double> 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();
}

@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();
}
}
151 changes: 151 additions & 0 deletions vendordeps/Phoenix5.json
View file
Original file line number Diff line number Diff line change
@@ -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"
}
]
}
Loading

0 comments on commit 46dec4a

Please sign in to comment.