-
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.
- Loading branch information
1 parent
926c21f
commit 1a198c3
Showing
12 changed files
with
211 additions
and
6 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
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
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
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
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
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
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,51 @@ | ||
| package frc.robot.subsystems.amp; | ||
|
|
||
| import edu.wpi.first.math.controller.ProfiledPIDController; | ||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
| import edu.wpi.first.math.util.Units; | ||
| import edu.wpi.first.wpilibj2.command.SubsystemBase; | ||
| import frc.lib.utils.LoggedTunableNumber; | ||
| import frc.robot.Constants.AmpConstants; | ||
| import org.littletonrobotics.junction.Logger; | ||
|
|
||
| public class Amp extends SubsystemBase { | ||
| public AmpIO m_io; | ||
| public AmpInputsAutoLogged m_inputs; | ||
| private ProfiledPIDController m_controller; | ||
|
|
||
| public Amp(AmpIO io, ProfiledPIDController controller) { | ||
| m_io = io; | ||
| m_inputs = new AmpInputsAutoLogged(); | ||
| m_controller = controller; | ||
| } | ||
|
|
||
| @Override | ||
| public void periodic() { | ||
| LoggedTunableNumber.ifChanged( | ||
| hashCode(), | ||
| () -> { | ||
| m_controller.setP(AmpConstants.kP.get()); | ||
| m_controller.setI(AmpConstants.kI.get()); | ||
| m_controller.setD(AmpConstants.kD.get()); | ||
| }, | ||
| AmpConstants.kP, | ||
| AmpConstants.kI, | ||
| AmpConstants.kD); | ||
|
|
||
| m_io.updateInputs(m_inputs); | ||
|
|
||
| double pidVoltage = m_controller.calculate(m_inputs.curAngle); | ||
| m_io.setVoltage(pidVoltage); | ||
|
|
||
| Logger.processInputs("Amp", m_inputs); | ||
|
|
||
| Logger.recordOutput("Amp/PIDVoltage", pidVoltage); | ||
| Logger.recordOutput("Amp/CurAngle", Units.rotationsToDegrees(m_inputs.curAngle)); | ||
| Logger.recordOutput( | ||
| "Amp/DesiredAngle", Units.rotationsToDegrees(m_controller.getGoal().position)); | ||
| } | ||
|
|
||
| public void setAmpAngle(Rotation2d angle) { | ||
| m_controller.setGoal(angle.getRotations()); | ||
| } | ||
| } |
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,16 @@ | ||
| package frc.robot.subsystems.amp; | ||
|
|
||
| import frc.lib.advantagekit.LoggedIO; | ||
| import org.littletonrobotics.junction.AutoLog; | ||
|
|
||
| public interface AmpIO extends LoggedIO<AmpIO.AmpInputs> { | ||
| @AutoLog | ||
| public static class AmpInputs { | ||
| public double curVoltage; | ||
| public double curAmps; | ||
| public double curVelocity; | ||
| public double curAngle; | ||
| } | ||
|
|
||
| public void setVoltage(double voltage); | ||
| } |
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,37 @@ | ||
| package frc.robot.subsystems.amp; | ||
|
|
||
| import com.ctre.phoenix6.BaseStatusSignal; | ||
| import com.ctre.phoenix6.StatusSignal; | ||
| import com.ctre.phoenix6.hardware.TalonFX; | ||
|
|
||
| public class AmpIOFalcon implements AmpIO { | ||
| private TalonFX m_motor; | ||
| private StatusSignal<Double> m_velocitySignal; | ||
| private StatusSignal<Double> m_currentSignal; | ||
| private StatusSignal<Double> m_voltageSignal; | ||
| private StatusSignal<Double> m_angleSignal; | ||
|
|
||
| public AmpIOFalcon(int port) { | ||
| m_motor = new TalonFX(port); | ||
|
|
||
| m_velocitySignal = m_motor.getVelocity(); | ||
| m_currentSignal = m_motor.getSupplyCurrent(); | ||
| m_voltageSignal = m_motor.getMotorVoltage(); | ||
| m_angleSignal = m_motor.getPosition(); | ||
| } | ||
|
|
||
| @Override | ||
| public void updateInputs(AmpInputs inputs) { | ||
| BaseStatusSignal.refreshAll(m_velocitySignal, m_currentSignal, m_voltageSignal); | ||
|
|
||
| inputs.curVelocity = m_velocitySignal.getValueAsDouble(); | ||
| inputs.curAmps = m_currentSignal.getValueAsDouble(); | ||
| inputs.curVoltage = m_voltageSignal.getValueAsDouble(); | ||
| inputs.curAngle = m_angleSignal.getValueAsDouble(); | ||
| } | ||
|
|
||
| @Override | ||
| public void setVoltage(double voltage) { | ||
| m_motor.setVoltage(voltage); | ||
| } | ||
| } |
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,51 @@ | ||
| package frc.robot.subsystems.amp; | ||
|
|
||
| import edu.wpi.first.math.system.plant.DCMotor; | ||
| import edu.wpi.first.math.util.Units; | ||
| import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; | ||
| import frc.robot.Constants.AmpConstants; | ||
|
|
||
| public class AmpIOSim implements AmpIO { | ||
| private SingleJointedArmSim m_sim; | ||
| private double m_voltage; | ||
|
|
||
| public AmpIOSim() { | ||
| DCMotor gearbox = DCMotor.getFalcon500(1); | ||
| double gearing = 20; | ||
| double armLength = 0.4572; | ||
| double jKgMetersSquared = SingleJointedArmSim.estimateMOI(armLength, 0.91); | ||
|
|
||
| double minAngle = AmpConstants.kMinAngle.getRadians(); | ||
| double maxAngle = AmpConstants.kMaxAngle.getRadians(); | ||
| double startingAngle = AmpConstants.kHomeAngle.getRadians(); | ||
|
|
||
| m_sim = | ||
| new SingleJointedArmSim( | ||
| gearbox, | ||
| gearing, | ||
| jKgMetersSquared, | ||
| armLength, | ||
| minAngle, | ||
| maxAngle, | ||
| false, | ||
| startingAngle); | ||
|
|
||
| m_voltage = 0.0; | ||
| } | ||
|
|
||
| @Override | ||
| public void updateInputs(AmpInputs inputs) { | ||
| m_sim.update(0.02); | ||
|
|
||
| inputs.curAngle = Units.radiansToRotations(m_sim.getAngleRads()); | ||
| inputs.curVoltage = m_voltage; | ||
| inputs.curVelocity = Units.radiansToRotations(m_sim.getVelocityRadPerSec()); | ||
| inputs.curAmps = m_sim.getCurrentDrawAmps(); | ||
| } | ||
|
|
||
| @Override | ||
| public void setVoltage(double voltage) { | ||
| m_sim.setInputVoltage(voltage); | ||
| m_voltage = voltage; | ||
| } | ||
| } |
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
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