001package tagalong.logging;
002
003import com.ctre.phoenix6.BaseStatusSignal;
004import com.ctre.phoenix6.StatusSignal;
005import edu.wpi.first.units.measure.Angle;
006import edu.wpi.first.units.measure.AngularVelocity;
007import edu.wpi.first.units.measure.Current;
008import edu.wpi.first.units.measure.Voltage;
009import tagalong.subsystems.micro.Pivot;
010
011/**
012 * Collection of pivot TalonFX data
013 */
014public class PivotIOTalonFX implements PivotIO {
015  /**
016   * Signal for pivot position in rotations
017   */
018  private final StatusSignal<Angle> _pivotPosition;
019  /**
020   * Signal for pivot velocity in rotations per second
021   */
022  private final StatusSignal<AngularVelocity> _pivotVelocity;
023  /**
024   * Signal for pivot applied (output) motor voltage
025   */
026  private final StatusSignal<Voltage> _pivotAppliedVolts;
027  /**
028   * Signal for pivot current corresponding to the stator windings
029   */
030  private final StatusSignal<Current> _pivotCurrentAmps;
031
032  /**
033   * Constructs a layer with the below pivot TalonFX data
034   *
035   * @param pivot microsystem
036   */
037  public PivotIOTalonFX(Pivot pivot) {
038    _pivotPosition = pivot.getPrimaryMotor().getPosition();
039    _pivotVelocity = pivot.getPrimaryMotor().getVelocity();
040    _pivotAppliedVolts = pivot.getPrimaryMotor().getMotorVoltage();
041    _pivotCurrentAmps = pivot.getPrimaryMotor().getStatorCurrent();
042    BaseStatusSignal.setUpdateFrequencyForAll(
043        50.0, _pivotPosition, _pivotVelocity, _pivotAppliedVolts, _pivotCurrentAmps
044    );
045  }
046  @Override
047  public void updateInputs(PivotIOInputs inputs) {
048    BaseStatusSignal.refreshAll(
049        _pivotPosition, _pivotVelocity, _pivotAppliedVolts, _pivotCurrentAmps
050    );
051    inputs.pivotPositionRot = _pivotPosition.getValueAsDouble();
052    inputs.pivotVelocityRPS = _pivotVelocity.getValueAsDouble();
053    inputs.pivotAppliedVolts = _pivotAppliedVolts.getValueAsDouble();
054    inputs.pivotCurrentAmps = _pivotCurrentAmps.getValueAsDouble();
055  }
056}