From 0e12843aabf126d23b91755c638d2e7304a31c4c Mon Sep 17 00:00:00 2001 From: harnwalN Date: Sat, 20 Jan 2024 19:10:52 -0500 Subject: [PATCH] Add TalonSRX open-loop control through voltage, setVelocity not implemented --- .../subsystems/drive/DriveIOTalonSRX.java | 156 +++++++++--------- 1 file changed, 78 insertions(+), 78 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveIOTalonSRX.java b/src/main/java/frc/robot/subsystems/drive/DriveIOTalonSRX.java index 2d1717a3..b4b03099 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveIOTalonSRX.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveIOTalonSRX.java @@ -13,61 +13,69 @@ package frc.robot.subsystems.drive; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import static com.ctre.phoenix.motorcontrol.FeedbackDevice.QuadEncoder; +import static com.ctre.phoenix.motorcontrol.NeutralMode.Brake; +import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; +import com.ctre.phoenix.motorcontrol.can.SlotConfiguration; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; public class DriveIOTalonSRX implements DriveIO { private static final double GEAR_RATIO = 10.0; + + private static final double MAX_VOLTAGE = 12.0; private static final double KP = 1.0; // TODO: MUST BE TUNED, consider using Phoenix Tuner X private static final double KD = 0.0; // TODO: MUST BE TUNED, consider using Phoenix Tuner X - private final TalonSRX leftLeader = new TalonSRX(0); - private final TalonSRX leftFollower = new TalonSRX(1); - private final TalonSRX rightLeader = new TalonSRX(2); - private final TalonSRX rightFollower = new TalonSRX(3); + private final TalonSRX leftLeader = new TalonSRX(2); + private final TalonSRX leftFollower = new TalonSRX(0); + private final TalonSRX rightLeader = new TalonSRX(3); + private final TalonSRX rightFollower = new TalonSRX(1); - 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 double leftPosition = leftLeader.getSelectedSensorPosition(); + private double leftVelocity = leftLeader.getSelectedSensorVelocity(); + private double leftAppliedVolts = leftLeader.getMotorOutputVoltage(); + private double leftLeaderCurrent = leftLeader.getStatorCurrent(); + private double leftFollowerCurrent = leftFollower.getStatorCurrent(); - private final StatusSignal rightVelocity = rightLeader.getVelocity(); - private final StatusSignal rightAppliedVolts = rightLeader.getMotorVoltage(); - private final StatusSignal rightLeaderCurrent = rightLeader.getStatorCurrent(); - private final StatusSignal rightFollowerCurrent = rightFollower.getStatorCurrent(); + private double rightPosition = rightLeader.getSelectedSensorPosition(); + private double rightVelocity = rightLeader.getSelectedSensorVelocity(); + private double rightAppliedVolts = rightLeader.getMotorOutputVoltage(); + private double rightLeaderCurrent = rightLeader.getStatorCurrent(); + private double rightFollowerCurrent = rightFollower.getStatorCurrent(); - private final Pigeon2 pigeon = new Pigeon2(20); - private final StatusSignal yaw = pigeon.getYaw(); + // private final Pigeon2 pigeon = new Pigeon2(20); public DriveIOTalonSRX() { var config = new TalonSRXConfiguration(); - config.CurrentLimits.StatorCurrentLimit = 60.0; - config.CurrentLimits.StatorCurrentLimitEnable = true; - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.Slot0.kP = KP; - config.Slot0.kD = KD; - 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)); - - leftLeader.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 10); //new Talon SRX code - rightLeader.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 10); //new Talon SRX code + config.peakCurrentLimit = 80; + config.peakCurrentDuration = 100; + config.continuousCurrentLimit = 60; - BaseStatusSignal.setUpdateFrequencyForAll( + leftLeader.setNeutralMode(Brake); + leftFollower.setNeutralMode(Brake); + rightLeader.setNeutralMode(Brake); + rightFollower.setNeutralMode(Brake); + + SlotConfiguration slot = new SlotConfiguration(); + slot.kP = KP; + slot.kD = KD; + config.slot0 = slot; + + leftLeader.configAllSettings(config); + leftFollower.configAllSettings(config); + rightLeader.configAllSettings(config); + rightFollower.configAllSettings(config); + + leftFollower.follow(leftLeader); + rightFollower.follow(rightLeader); + + leftLeader.configSelectedFeedbackSensor(QuadEncoder); + rightLeader.configSelectedFeedbackSensor(QuadEncoder); + + /*BaseStatusSignal.setUpdateFrequencyForAll( 100.0, leftPosition, rightPosition, yaw); // Required for odometry, use faster rate BaseStatusSignal.setUpdateFrequencyForAll( 50.0, @@ -78,59 +86,51 @@ public DriveIOTalonSRX() { rightVelocity, rightAppliedVolts, rightLeaderCurrent, - rightFollowerCurrent); - leftLeader.optimizeBusUtilization(); + rightFollowerCurrent);*/ + /*leftLeader.optimizeBusUtilization(); leftFollower.optimizeBusUtilization(); rightLeader.optimizeBusUtilization(); - rightFollower.optimizeBusUtilization(); - pigeon.optimizeBusUtilization(); + rightFollower.optimizeBusUtilization();*/ + // pigeon.optimizeBusUtilization(); } @Override public void updateInputs(DriveIOInputs inputs) { - BaseStatusSignal.refreshAll( - leftPosition, - leftVelocity, - leftAppliedVolts, - leftLeaderCurrent, - leftFollowerCurrent, - rightPosition, - rightVelocity, - rightAppliedVolts, - rightLeaderCurrent, - rightFollowerCurrent, - yaw); - - inputs.leftPositionRad = Units.rotationsToRadians(leftPosition.getValueAsDouble()) / GEAR_RATIO; - inputs.leftVelocityRadPerSec = - Units.rotationsToRadians(leftVelocity.getValueAsDouble()) / GEAR_RATIO; - inputs.leftAppliedVolts = leftAppliedVolts.getValueAsDouble(); - inputs.leftCurrentAmps = - new double[] {leftLeaderCurrent.getValueAsDouble(), leftFollowerCurrent.getValueAsDouble()}; - - inputs.rightPositionRad = - Units.rotationsToRadians(rightPosition.getValueAsDouble()) / GEAR_RATIO; - inputs.rightVelocityRadPerSec = - Units.rotationsToRadians(rightVelocity.getValueAsDouble()) / GEAR_RATIO; - inputs.rightAppliedVolts = rightAppliedVolts.getValueAsDouble(); - inputs.rightCurrentAmps = - new double[] { - rightLeaderCurrent.getValueAsDouble(), rightFollowerCurrent.getValueAsDouble() - }; - - inputs.gyroYaw = Rotation2d.fromDegrees(yaw.getValueAsDouble()); + leftPosition = leftLeader.getSelectedSensorPosition(); + leftVelocity = leftLeader.getSelectedSensorVelocity(); + leftAppliedVolts = leftLeader.getMotorOutputVoltage(); + leftLeaderCurrent = leftLeader.getStatorCurrent(); + leftFollowerCurrent = leftFollower.getStatorCurrent(); + + rightPosition = rightLeader.getSelectedSensorPosition(); + rightVelocity = rightLeader.getSelectedSensorVelocity(); + rightAppliedVolts = rightLeader.getMotorOutputVoltage(); + rightLeaderCurrent = rightLeader.getStatorCurrent(); + rightFollowerCurrent = rightFollower.getStatorCurrent(); + + inputs.leftPositionRad = Units.rotationsToRadians(leftPosition) / GEAR_RATIO; + inputs.leftVelocityRadPerSec = Units.rotationsToRadians(leftVelocity) / GEAR_RATIO; + inputs.leftAppliedVolts = leftAppliedVolts; + inputs.leftCurrentAmps = new double[] {leftLeaderCurrent, leftFollowerCurrent}; + + inputs.rightPositionRad = Units.rotationsToRadians(rightPosition) / GEAR_RATIO; + inputs.rightVelocityRadPerSec = Units.rotationsToRadians(rightVelocity) / GEAR_RATIO; + inputs.rightAppliedVolts = rightAppliedVolts; + inputs.rightCurrentAmps = new double[] {rightLeaderCurrent, rightFollowerCurrent}; } @Override public void setVoltage(double leftVolts, double rightVolts) { - leftLeader.setControl(new VoltageOut(leftVolts)); - rightLeader.setControl(new VoltageOut(rightVolts)); + leftLeader.set(TalonSRXControlMode.PercentOutput, leftVolts / MAX_VOLTAGE); + rightLeader.set(TalonSRXControlMode.PercentOutput, -1 * rightVolts / MAX_VOLTAGE); } @Override public void setVelocity( double leftRadPerSec, double rightRadPerSec, double leftFFVolts, double rightFFVolts) { - leftLeader.setControl( + return; + + /*leftLeader.setControl( new VelocityVoltage( Units.radiansToRotations(leftRadPerSec * GEAR_RATIO), 0.0, @@ -149,6 +149,6 @@ public void setVelocity( 0, false, false, - false)); + false));*/ } -} +} \ No newline at end of file