From 3e41a861fd26361fd89fecb2111d6133df328443 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 8 Feb 2026 15:27:03 -0800 Subject: [PATCH 1/4] Make sure all climber stuff uses correct units --- src/main/java/frc/robot/Robot.java | 31 ++++++++++++++++++ .../robot/subsystems/climber/ClimberIO.java | 32 +++++++++++-------- 2 files changed, 50 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1d7f329..66b71ee 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -572,6 +572,37 @@ public void robotPeriodic() { // TODO Log mechanism poses + // TODO: ACTUAL SETPOINT + Pose3d turretSetpoint = new Pose3d( + new Translation3d(-0.177413, -0.111702, 0.350341), + new Rotation3d(0, 0, turretAngle.getAsDouble())); + // TODO: ACTUAL SETPOINTS + Logger.recordOutput( + "Robot/Mechanism Setpoints", + new Pose3d[] { + // Turret + turretSetpoint, + // Hood + turretSetpoint + // First transform the hood out to the hood pivot, and rotate by the amount needed + .transformBy( + new Transform3d( + new Translation3d(-0.095638, 0, 0.095123), + new Rotation3d(0, hoodAngle.getAsDouble() * -1, 0))) + // Then, transform the hood back to the correct location relative to the turret + .transformBy( + new Transform3d( + new Translation3d(-0.095638, 0, 0.095123).times(-1), Rotation3d.kZero)), + // Intake + new Pose3d( + intakeExtension.get() * LintakeSubsystem.INTAKE_ROTATION.getCos(), + 0, + -(intakeExtension.get() * LintakeSubsystem.INTAKE_ROTATION.getSin()), + Rotation3d.kZero), + // Climber + new Pose3d(0, 0, climber.getClimberExtensionMeters(), Rotation3d.kZero) + }); + updateAlerts(); } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index 88b1b93..bc85932 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -17,12 +17,13 @@ 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 +33,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 +52,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 +78,7 @@ 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; } } From 23fca40a0bc6707e416a60bef284ae6a3deb07ee Mon Sep 17 00:00:00 2001 From: SCool62 Date: Wed, 11 Feb 2026 10:23:43 -0800 Subject: [PATCH 2/4] Remove mechanism viz stuff --- src/main/java/frc/robot/Robot.java | 31 ------------------------------ 1 file changed, 31 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 66b71ee..1d7f329 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -572,37 +572,6 @@ public void robotPeriodic() { // TODO Log mechanism poses - // TODO: ACTUAL SETPOINT - Pose3d turretSetpoint = new Pose3d( - new Translation3d(-0.177413, -0.111702, 0.350341), - new Rotation3d(0, 0, turretAngle.getAsDouble())); - // TODO: ACTUAL SETPOINTS - Logger.recordOutput( - "Robot/Mechanism Setpoints", - new Pose3d[] { - // Turret - turretSetpoint, - // Hood - turretSetpoint - // First transform the hood out to the hood pivot, and rotate by the amount needed - .transformBy( - new Transform3d( - new Translation3d(-0.095638, 0, 0.095123), - new Rotation3d(0, hoodAngle.getAsDouble() * -1, 0))) - // Then, transform the hood back to the correct location relative to the turret - .transformBy( - new Transform3d( - new Translation3d(-0.095638, 0, 0.095123).times(-1), Rotation3d.kZero)), - // Intake - new Pose3d( - intakeExtension.get() * LintakeSubsystem.INTAKE_ROTATION.getCos(), - 0, - -(intakeExtension.get() * LintakeSubsystem.INTAKE_ROTATION.getSin()), - Rotation3d.kZero), - // Climber - new Pose3d(0, 0, climber.getClimberExtensionMeters(), Rotation3d.kZero) - }); - updateAlerts(); } From 08d06d6dad78a0f3923f95b2e44e464192990486 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Wed, 11 Feb 2026 10:24:52 -0800 Subject: [PATCH 3/4] Add spool diameter constant --- .../java/frc/robot/subsystems/climber/ClimberSubsystem.java | 3 +++ 1 file changed, 3 insertions(+) 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; From 6df76e9d83b809d3d6b61978d74b0b3a22b6491a Mon Sep 17 00:00:00 2001 From: SCool62 Date: Wed, 11 Feb 2026 10:25:05 -0800 Subject: [PATCH 4/4] Spotless --- src/main/java/frc/robot/subsystems/climber/ClimberIO.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index bc85932..9e579b1 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -10,7 +10,6 @@ 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; @@ -78,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 * (Math.PI * ClimberSubsystem.SPOOL_DIAMETER_METERS); + config.Feedback.SensorToMechanismRatio = + ClimberSubsystem.GEAR_RATIO * (Math.PI * ClimberSubsystem.SPOOL_DIAMETER_METERS); // todo: tune config.Slot0.kS = 0.0;