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.Elevator;
010
011/**
012 * Collection of elevator TalonFX data
013 */
014public class ElevatorIOTalonFX implements ElevatorIO {
015  /**
016   * Elevator microsystem
017   */
018  private final Elevator _elevator;
019  /**
020   * Signal for elevator position in meters
021   */
022  private final StatusSignal<Angle> _elevatorHeight;
023  /**
024   * Signal for elevator velocity in meters per second
025   */
026  private final StatusSignal<AngularVelocity> _elevatorVelocity;
027  /**
028   * Signal for elevator applied (output) motor voltage
029   */
030  private final StatusSignal<Voltage> _elevatorAppliedVolts;
031  /**
032   * Signal for elevator current corresponding to the stator windings
033   */
034  private final StatusSignal<Current> _elevatorCurrentAmps;
035
036  /**
037   * Constructs a layer with the below elevator TalonFX data
038   *
039   * @param elevator microsystem
040   */
041  public ElevatorIOTalonFX(Elevator elevator) {
042    _elevator = elevator;
043    _elevatorHeight = elevator.getPrimaryMotor().getPosition();
044    _elevatorVelocity = elevator.getPrimaryMotor().getVelocity();
045    _elevatorAppliedVolts = elevator.getPrimaryMotor().getMotorVoltage();
046    _elevatorCurrentAmps = elevator.getPrimaryMotor().getStatorCurrent();
047    BaseStatusSignal.setUpdateFrequencyForAll(
048        50.0, _elevatorHeight, _elevatorVelocity, _elevatorAppliedVolts, _elevatorCurrentAmps
049    );
050  }
051
052  @Override
053  public void updateInputs(ElevatorIOInputs inputs) {
054    BaseStatusSignal.refreshAll(
055        _elevatorHeight, _elevatorVelocity, _elevatorAppliedVolts, _elevatorCurrentAmps
056    );
057    inputs.elevatorHeightM = _elevator.motorToMeters(_elevatorHeight.getValueAsDouble());
058    inputs.elevatorVelocityMPS = _elevator.motorToMeters(_elevatorVelocity.getValueAsDouble());
059    inputs.elevatorAppliedVolts = _elevatorAppliedVolts.getValueAsDouble();
060    inputs.elevatorCurrentAmps = _elevatorCurrentAmps.getValueAsDouble();
061  }
062}