-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat: shooter IO interfaces + all hardware implementations
- Loading branch information
1 parent
e11715f
commit afe2d26
Showing
5 changed files
with
331 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
39 changes: 39 additions & 0 deletions
39
src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIO.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,39 @@ | ||
| package org.team1540.robot2024.subsystems.shooter; | ||
|
|
||
| import org.littletonrobotics.junction.AutoLog; | ||
|
|
||
| public interface FlywheelsIO { | ||
| @AutoLog | ||
| class FlywheelsIOInputs { | ||
| public double leftAppliedVolts = 0.0; | ||
| public double leftCurrentAmps = 0.0; | ||
| public double leftVelocityRPM = 0.0; | ||
| public double leftSetpointRPM = 0.0; | ||
|
|
||
| public double rightAppliedVolts = 0.0; | ||
| public double rightCurrentAmps = 0.0; | ||
| public double rightVelocityRPM = 0.0; | ||
| public double rightSetpointRPM = 0.0; | ||
| } | ||
|
|
||
| /** | ||
| * Updates the set of loggable inputs | ||
| */ | ||
| default void updateInputs(FlywheelsIOInputs inputs) {} | ||
|
|
||
| /** | ||
| * Runs open loop at the specified voltages | ||
| */ | ||
| default void setVoltage(double leftVolts, double rightVolts) {} | ||
|
|
||
|
|
||
| /** | ||
| * Runs closed loop at the specified RPMs | ||
| */ | ||
| default void setVelocity(double leftRPM, double rightRPM) {} | ||
|
|
||
| /** | ||
| * Configures the PID controller | ||
| */ | ||
| default void configPID(double kP, double kI, double kD) {} | ||
| } |
116 changes: 116 additions & 0 deletions
116
src/main/java/org/team1540/robot2024/subsystems/shooter/FlywheelsIOTalonFX.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,116 @@ | ||
| package org.team1540.robot2024.subsystems.shooter; | ||
|
|
||
| import com.ctre.phoenix6.BaseStatusSignal; | ||
| import com.ctre.phoenix6.StatusSignal; | ||
| import com.ctre.phoenix6.configs.Slot0Configs; | ||
| import com.ctre.phoenix6.configs.TalonFXConfiguration; | ||
| import com.ctre.phoenix6.controls.VelocityVoltage; | ||
| import com.ctre.phoenix6.controls.VoltageOut; | ||
| import com.ctre.phoenix6.hardware.TalonFX; | ||
| import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; | ||
| import com.ctre.phoenix6.signals.NeutralModeValue; | ||
|
|
||
| import static org.team1540.robot2024.Constants.Shooter.Flywheels.*; | ||
|
|
||
| public class FlywheelsIOTalonFX implements FlywheelsIO { | ||
| private final TalonFX leftMotor = new TalonFX(LEFT_ID); | ||
| private final TalonFX rightMotor = new TalonFX(RIGHT_ID); | ||
|
|
||
| private final StatusSignal<Double> leftVelocity = leftMotor.getVelocity(); | ||
| private final StatusSignal<Double> leftAppliedVolts = leftMotor.getMotorVoltage(); | ||
| private final StatusSignal<Double> leftCurrent = leftMotor.getSupplyCurrent(); | ||
|
|
||
| private final StatusSignal<Double> rightVelocity = rightMotor.getVelocity(); | ||
| private final StatusSignal<Double> rightAppliedVolts = rightMotor.getMotorVoltage(); | ||
| private final StatusSignal<Double> rightCurrent = rightMotor.getSupplyCurrent(); | ||
|
|
||
| private final VelocityVoltage leftVelocityCtrlReq = | ||
| new VelocityVoltage(0).withEnableFOC(false).withSlot(0); | ||
| private final VoltageOut leftVoltageCtrlReq = | ||
| new VoltageOut(0).withEnableFOC(false); | ||
|
|
||
| private final VelocityVoltage rightVelocityCtrlReq = | ||
| new VelocityVoltage(0).withEnableFOC(false).withSlot(0); | ||
| private final VoltageOut rightVoltageCtrlReq = | ||
| new VoltageOut(0).withEnableFOC(false); | ||
|
|
||
| public FlywheelsIOTalonFX() { | ||
| TalonFXConfiguration config = new TalonFXConfiguration(); | ||
| config.MotorOutput.NeutralMode = NeutralModeValue.Coast; | ||
|
|
||
| // Shooter current limits are banned | ||
| config.CurrentLimits.SupplyCurrentLimitEnable = false; | ||
| config.CurrentLimits.StatorCurrentLimitEnable = false; | ||
|
|
||
| config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor; | ||
| config.Feedback.SensorToMechanismRatio = GEAR_RATIO; | ||
| config.Feedback.RotorToSensorRatio = 1.0; | ||
|
|
||
| config.Slot0.kP = KP; | ||
| config.Slot0.kI = KI; | ||
| config.Slot0.kD = KD; | ||
| config.Slot0.kS = KS; | ||
| config.Slot0.kV = KV; | ||
|
|
||
| leftMotor.getConfigurator().apply(config); | ||
| rightMotor.getConfigurator().apply(config); | ||
| leftMotor.setInverted(false); | ||
| rightMotor.setInverted(true); | ||
|
|
||
| BaseStatusSignal.setUpdateFrequencyForAll( | ||
| 50, | ||
| leftVelocity, | ||
| leftAppliedVolts, | ||
| leftCurrent, | ||
| rightVelocity, | ||
| rightAppliedVolts, | ||
| rightCurrent); | ||
|
|
||
| leftMotor.optimizeBusUtilization(); | ||
| rightMotor.optimizeBusUtilization(); | ||
| } | ||
|
|
||
| @Override | ||
| public void updateInputs(FlywheelsIOInputs inputs) { | ||
| BaseStatusSignal.refreshAll( | ||
| leftVelocity, | ||
| leftAppliedVolts, | ||
| leftCurrent, | ||
| rightVelocity, | ||
| rightAppliedVolts, | ||
| rightCurrent); | ||
|
|
||
| inputs.leftVelocityRPM = leftVelocity.getValueAsDouble() * 60; | ||
| inputs.leftSetpointRPM = leftVelocityCtrlReq.Velocity * 60; | ||
| inputs.leftAppliedVolts = leftAppliedVolts.getValueAsDouble(); | ||
| inputs.leftCurrentAmps = leftCurrent.getValueAsDouble(); | ||
|
|
||
| inputs.rightVelocityRPM = rightVelocity.getValueAsDouble() * 60; | ||
| inputs.rightSetpointRPM = rightVelocityCtrlReq.Velocity * 60; | ||
| inputs.rightAppliedVolts = rightAppliedVolts.getValueAsDouble(); | ||
| inputs.rightCurrentAmps = rightCurrent.getValueAsDouble(); | ||
| } | ||
|
|
||
| @Override | ||
| public void setVelocity(double leftRPM, double rightRPM) { | ||
| leftMotor.setControl(leftVelocityCtrlReq.withVelocity(leftRPM / 60)); | ||
| rightMotor.setControl(rightVelocityCtrlReq.withVelocity(rightRPM / 60)); | ||
| } | ||
|
|
||
| @Override | ||
| public void setVoltage(double leftVolts, double rightVolts) { | ||
| leftMotor.setControl(leftVoltageCtrlReq.withOutput(leftVolts)); | ||
| rightMotor.setControl(rightVoltageCtrlReq.withOutput(rightVolts)); | ||
| } | ||
|
|
||
| @Override | ||
| public void configPID(double kP, double kI, double kD) { | ||
| Slot0Configs pidConfigs = new Slot0Configs(); | ||
| leftMotor.getConfigurator().refresh(pidConfigs); | ||
| pidConfigs.kP = kP; | ||
| pidConfigs.kI = kI; | ||
| pidConfigs.kD = kD; | ||
| leftMotor.getConfigurator().apply(pidConfigs); | ||
| rightMotor.getConfigurator().apply(pidConfigs); | ||
| } | ||
| } |
38 changes: 38 additions & 0 deletions
38
src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIO.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,38 @@ | ||
| package org.team1540.robot2024.subsystems.shooter; | ||
|
|
||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
| import org.littletonrobotics.junction.AutoLog; | ||
|
|
||
| public interface ShooterPivotIO { | ||
| @AutoLog | ||
| class ShooterPivotIOInputs { | ||
| public Rotation2d position = new Rotation2d(); | ||
| public Rotation2d absolutePosition = new Rotation2d(); | ||
| public Rotation2d setpoint = new Rotation2d(); | ||
| public double velocityRPS = 0.0; | ||
| public double appliedVolts = 0.0; | ||
| public double currentAmps = 0.0; | ||
| } | ||
|
|
||
| /** | ||
| * Updates the set of loggable inputs | ||
| */ | ||
| default void updateInputs(ShooterPivotIOInputs inputs) {} | ||
|
|
||
| /** | ||
| * Runs closed loop to the specified position | ||
| */ | ||
| default void setPosition(Rotation2d position) {} | ||
|
|
||
| /** | ||
| * Runs open loop at the specified voltage | ||
| */ | ||
| default void setVoltage(double volts) {} | ||
|
|
||
| default void configBrakeMode(boolean isBrakeMode) {} | ||
|
|
||
| /** | ||
| * Configures the PID controller | ||
| */ | ||
| default void configPID(double kP, double kI, double kD) {} | ||
| } |
92 changes: 92 additions & 0 deletions
92
src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOTalonFX.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,92 @@ | ||
| package org.team1540.robot2024.subsystems.shooter; | ||
|
|
||
| import com.ctre.phoenix6.BaseStatusSignal; | ||
| import com.ctre.phoenix6.StatusSignal; | ||
| import com.ctre.phoenix6.configs.CANcoderConfiguration; | ||
| import com.ctre.phoenix6.configs.TalonFXConfiguration; | ||
| import com.ctre.phoenix6.controls.MotionMagicVoltage; | ||
| import com.ctre.phoenix6.controls.VoltageOut; | ||
| import com.ctre.phoenix6.hardware.CANcoder; | ||
| import com.ctre.phoenix6.hardware.TalonFX; | ||
| import com.ctre.phoenix6.signals.*; | ||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
|
|
||
| import static org.team1540.robot2024.Constants.Shooter.Pivot.*; | ||
|
|
||
| public class ShooterPivotIOTalonFX implements ShooterPivotIO { | ||
| private final TalonFX motor = new TalonFX(MOTOR_ID); | ||
| private final CANcoder cancoder = new CANcoder(CANCODER_ID); | ||
|
|
||
| private final StatusSignal<Double> position = motor.getPosition(); | ||
| private final StatusSignal<Double> absolutePosition = cancoder.getAbsolutePosition(); | ||
| private final StatusSignal<Double> velocity = motor.getVelocity(); | ||
| private final StatusSignal<Double> appliedVoltage = motor.getMotorVoltage(); | ||
| private final StatusSignal<Double> current = motor.getSupplyCurrent(); | ||
|
|
||
| private final MotionMagicVoltage positionCtrlReq = new MotionMagicVoltage(0).withSlot(0); | ||
| private final VoltageOut voltageCtrlReq = new VoltageOut(0); | ||
|
|
||
| public ShooterPivotIOTalonFX() { | ||
| TalonFXConfiguration motorConfig = new TalonFXConfiguration(); | ||
|
|
||
| // TODO: find invert | ||
| motorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; | ||
| motorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; | ||
|
|
||
| motorConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; | ||
| motorConfig.Feedback.FeedbackRemoteSensorID = CANCODER_ID; | ||
| motorConfig.Feedback.SensorToMechanismRatio = CANCODER_TO_PIVOT; | ||
| motorConfig.Feedback.RotorToSensorRatio = MOTOR_TO_CANCODER; | ||
|
|
||
| motorConfig.Slot0.kP = KP; | ||
| motorConfig.Slot0.kI = KI; | ||
| motorConfig.Slot0.kD = KD; | ||
| motorConfig.Slot0.kS = KS; | ||
| motorConfig.Slot0.kG = KG; | ||
| motorConfig.Slot0.kV = KV; | ||
|
|
||
| motorConfig.MotionMagic.MotionMagicCruiseVelocity = CRUISE_VELOCITY_RPS; | ||
| motorConfig.MotionMagic.MotionMagicAcceleration = MAX_ACCEL_RPS2; | ||
| motorConfig.MotionMagic.MotionMagicJerk = JERK_RPS3; | ||
|
|
||
| CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); | ||
| cancoderConfig.MagnetSensor.MagnetOffset = CANCODER_OFFSET_ROTS; | ||
| // TODO: find invert | ||
| cancoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; | ||
| cancoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Unsigned_0To1; | ||
|
|
||
| cancoder.getConfigurator().apply(cancoderConfig); | ||
| motor.getConfigurator().apply(motorConfig); | ||
|
|
||
| BaseStatusSignal.setUpdateFrequencyForAll( | ||
| 50, | ||
| position, | ||
| absolutePosition, | ||
| velocity, | ||
| appliedVoltage, | ||
| current); | ||
|
|
||
| motor.optimizeBusUtilization(); | ||
| cancoder.optimizeBusUtilization(); | ||
| } | ||
|
|
||
| @Override | ||
| public void updateInputs(ShooterPivotIOInputs inputs) { | ||
| inputs.position = Rotation2d.fromRotations(position.getValueAsDouble()); | ||
| inputs.absolutePosition = Rotation2d.fromRotations(absolutePosition.getValueAsDouble() / CANCODER_TO_PIVOT); | ||
| inputs.setpoint = Rotation2d.fromRotations(positionCtrlReq.Position); | ||
| inputs.velocityRPS = velocity.getValueAsDouble(); | ||
| inputs.appliedVolts = appliedVoltage.getValueAsDouble(); | ||
| inputs.currentAmps = current.getValueAsDouble(); | ||
| } | ||
|
|
||
| @Override | ||
| public void setPosition(Rotation2d position) { | ||
| motor.setControl(positionCtrlReq.withPosition(position.getRotations())); | ||
| } | ||
|
|
||
| @Override | ||
| public void setVoltage(double volts) { | ||
| motor.setControl(voltageCtrlReq.withOutput(volts)); | ||
| } | ||
| } |