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.Roller;
010
011/**
012 * Collection of roller TalonFX data
013 */
014public class RollerIOTalonFX implements RollerIO {
015  /**
016   * Signal for roller position in rotations
017   */
018  private final StatusSignal<Angle> _rollerPosition;
019  /**
020   * Signal for roller velocity in rotations per second
021   */
022  private final StatusSignal<AngularVelocity> _rollerVelocity;
023  /**
024   * Signal for roller applied (output) motor voltage
025   */
026  private final StatusSignal<Voltage> _rollerAppliedVolts;
027  /**
028   * Signal for roller current corresponding to the stator windings
029   */
030  private final StatusSignal<Current> _rollerCurrentAmps;
031
032  /**
033   * Constructs a layer with the below roller TalonFX data
034   *
035   * @param roller microsystem
036   */
037  public RollerIOTalonFX(Roller roller) {
038    _rollerPosition = roller.getPrimaryMotor().getPosition();
039    _rollerVelocity = roller.getPrimaryMotor().getVelocity();
040    _rollerAppliedVolts = roller.getPrimaryMotor().getMotorVoltage();
041    _rollerCurrentAmps = roller.getPrimaryMotor().getStatorCurrent();
042    BaseStatusSignal.setUpdateFrequencyForAll(
043        50.0, _rollerPosition, _rollerVelocity, _rollerAppliedVolts, _rollerCurrentAmps
044    );
045  }
046
047  @Override
048  public void updateInputs(RollerIOInputs inputs) {
049    BaseStatusSignal.refreshAll(
050        _rollerPosition, _rollerVelocity, _rollerAppliedVolts, _rollerCurrentAmps
051    );
052    inputs.rollerPositionRot = _rollerPosition.getValueAsDouble();
053    inputs.rollerVelocityRPS = _rollerVelocity.getValueAsDouble();
054    inputs.rollerAppliedVolts = _rollerAppliedVolts.getValueAsDouble();
055    inputs.rollerCurrentAmps = _rollerCurrentAmps.getValueAsDouble();
056  }
057}