climber

注意测试climber的起始调零功能，它将在enable后以-0.07的duty cycle out进行0.5秒运动，然后清零它的编码器。
2026-02-24 14:43:28 +08:00
parent 27e177df3d
commit 0fe28c2653
4 changed files with 185 additions and 2 deletions
@@ -0,0 +1,96 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.controls.VelocityDutyCycle;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+
+public class ClimberSubsystem extends SubsystemBase {
+
+    private final TalonFX climberMotor;
+    private double targetPositionTicks;
+
+    private final PositionVoltage positionHoldRequest =
+            new PositionVoltage(0).withSlot(0);
+
+    private final VelocityVoltage velocityRequest = new VelocityVoltage(0).withSlot(0);
+
+    private double holdPositionTicks = 0.0;
+
+    private final NetworkTable climberTable;
+    
+
+    public ClimberSubsystem() {
+        climberMotor = new TalonFX(Constants.Climber.motorID);
+
+        climberMotor.getConfigurator().apply(Constants.Climber.slot0Configs);
+
+        var cfg = new MotorOutputConfigs();
+        cfg.Inverted = Constants.Climber.inverted
+                ? InvertedValue.Clockwise_Positive
+                : InvertedValue.CounterClockwise_Positive;
+        climberMotor.getConfigurator().apply(cfg);
+
+        climberTable = NetworkTableInstance.getDefault().getTable("Climber");
+
+        holdPositionTicks = getPositionTicks();
+    }
+    public void initialize() {
+        
+        climberMotor.set(-0.07);
+
+    }
+    public void zeroEncoder(){
+
+        climberMotor.setPosition(0);
+        stopMotor();
+    }
+
+    public double getPositionTicks() {
+        return climberMotor.getPosition().getValueAsDouble();
+    }
+
+    public double getVelocityRps() {
+        return climberMotor.getVelocity().getValueAsDouble();
+    }
+
+    public void climbUp() {
+        targetPositionTicks = Constants.Climber.climbUpTicks;  
+        climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
+    }
+
+    public void climbDown() {
+        targetPositionTicks = Constants.Climber.climbDownTicks;  
+        climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
+    }
+
+    public void captureHoldPosition() {
+        holdPositionTicks = getPositionTicks();
+    }
+
+    public void holdPosition() {
+        climberMotor.setControl(
+                positionHoldRequest.withPosition(holdPositionTicks)
+        );
+    }
+
+    public void stopMotor() {
+        climberMotor.set(0);
+    }
+
+    @Override
+    public void periodic() {
+        climberTable.getEntry("velocityRps").setDouble(getVelocityRps());
+        climberTable.getEntry("positionTicks").setDouble(getPositionTicks());
+        climberTable.getEntry("holdPositionTicks").setDouble(holdPositionTicks);
+    }
+}