Skip to content

Commit

Permalink
Add TalonSRX open-loop control through voltage, setVelocity not imple…
Browse files Browse the repository at this point in the history 
…mented
  • Loading branch information
@harnwalN
harnwalN committed Jan 21, 2024
1 parent cc9a2ed commit 0e12843
Showing 1 changed file with 78 additions and 78 deletions.
156 changes: 78 additions & 78 deletions src/main/java/frc/robot/subsystems/drive/DriveIOTalonSRX.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<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 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<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();
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<Double> 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,
Expand All @@ -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,
Expand All @@ -149,6 +149,6 @@ public void setVelocity(
0,
false,
false,
false));
false));*/
}
}
}

0 comments on commit 0e12843

Please sign in to comment.