diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index 88b1b93..9e579b1 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -10,19 +10,19 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.AutoLogOutput; public class ClimberIO { @AutoLog public static class ClimberIOInputs { - public Rotation2d motorPositionRotations = new Rotation2d(); + public double motorPositionMeters = 0.0; public double motorVelocityMetersPerSec = 0.0; public double motorStatorCurrentAmps = 0.0; public double motorSupplyCurrentAmps = 0.0; @@ -32,8 +32,9 @@ public static class ClimberIOInputs { protected final TalonFX climberMotor; - private final StatusSignal motorPositionRotations; - private final StatusSignal angularVelocityRotsPerSec; + // Rotation -> linear conversion happens in sensor to mech ratio + private final StatusSignal motorPositionMeters; + private final StatusSignal velocityMetersPerSec; private final StatusSignal statorCurrentAmps; private final StatusSignal supplyCurrentAmps; private final StatusSignal motorVoltage; @@ -50,21 +51,21 @@ public ClimberIO(CANBus canBus) { climberMotor = new TalonFX(30, canBus); climberMotor.getConfigurator().apply(ClimberIO.getClimberConfiguration()); - angularVelocityRotsPerSec = climberMotor.getVelocity(); + velocityMetersPerSec = climberMotor.getVelocity(); supplyCurrentAmps = climberMotor.getSupplyCurrent(); motorVoltage = climberMotor.getMotorVoltage(); statorCurrentAmps = climberMotor.getStatorCurrent(); motorTemp = climberMotor.getDeviceTemp(); - motorPositionRotations = climberMotor.getPosition(); + motorPositionMeters = climberMotor.getPosition(); BaseStatusSignal.setUpdateFrequencyForAll( 50.0, - angularVelocityRotsPerSec, + velocityMetersPerSec, supplyCurrentAmps, motorVoltage, statorCurrentAmps, motorTemp, - motorPositionRotations); + motorPositionMeters); climberMotor.optimizeBusUtilization(); } @@ -76,7 +77,8 @@ public static TalonFXConfiguration getClimberConfiguration() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; // todo: find and make climber gear ratio variable - config.Feedback.SensorToMechanismRatio = ClimberSubsystem.GEAR_RATIO; + config.Feedback.SensorToMechanismRatio = + ClimberSubsystem.GEAR_RATIO * (Math.PI * ClimberSubsystem.SPOOL_DIAMETER_METERS); // todo: tune config.Slot0.kS = 0.0; @@ -108,19 +110,23 @@ public void setClimberVelocity(double climberVelocity) { public void updateInputs(ClimberIOInputs inputs) { BaseStatusSignal.refreshAll( - motorPositionRotations, - angularVelocityRotsPerSec, + motorPositionMeters, + velocityMetersPerSec, statorCurrentAmps, supplyCurrentAmps, motorVoltage, motorTemp); - inputs.motorPositionRotations = - Rotation2d.fromRotations(motorPositionRotations.getValueAsDouble()); + inputs.motorPositionMeters = motorPositionMeters.getValueAsDouble(); inputs.motorVoltage = motorVoltage.getValueAsDouble(); inputs.motorTempC = motorTemp.getValueAsDouble(); inputs.motorSupplyCurrentAmps = supplyCurrentAmps.getValueAsDouble(); inputs.motorStatorCurrentAmps = statorCurrentAmps.getValueAsDouble(); - inputs.motorVelocityMetersPerSec = angularVelocityRotsPerSec.getValueAsDouble(); + inputs.motorVelocityMetersPerSec = velocityMetersPerSec.getValueAsDouble(); + } + + @AutoLogOutput(key = "Climber/Setpoint Meters") + public double getClimberSetpointMeters() { + return climberSetpoint; } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index 57c1c46..3224de8 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -1,10 +1,13 @@ package frc.robot.subsystems.climber; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; public class ClimberSubsystem extends SubsystemBase { + // From CAD + public static final double SPOOL_DIAMETER_METERS = Units.inchesToMeters(0.668898); // todo: find actual constants public static double GEAR_RATIO = (45.0 / 1.0); public static double MAX_EXTENSION_METERS = 0.2413;