Climber devel

src/main/java/frc/robot/commands/ClimberCommand.java

@@ -0,0 +1,27 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.climber.ClimberSubsystem;
+
+public class ClimberCommand extends Command{
+   private final ClimberSubsystem climberSubsystem;
+   private final double climberTarget;
+
+   public ClimberCommand(ClimberSubsystem climberSubsystem, double climberPosition) {
+       this.climberSubsystem = climberSubsystem;
+       this.climberTarget = climberPosition;
+   }
+   public void initialize() {
+
+        climberSubsystem.setClimberTargetPosition(climberTarget);
+   }
+    public void execute() {
+          climberSubsystem.runClimber();
+          SmartDashboard.putNumber("climberMotor", climberSubsystem.getClimberPosition());
+          SmartDashboard.putNumber("climberTarget", climberTarget);
+    }
+    public void end(boolean interrupted) {
+          climberSubsystem.resetClimberPosition();
+    }
+}

src/main/java/frc/robot/subsystems/climber/ClimberConstants.java

@@ -0,0 +1,13 @@
+package frc.robot.subsystems.climber;
+
+import com.ctre.phoenix6.CANBus;
+
+public class ClimberConstants {
+    public static final CANBus climberBus = new CANBus("GTX7130");
+    public static final int leftMotorID = 20;
+    public static final int rightMotorID = 21;
+
+    public static final double climberKP = 0.1;
+    public static final double climberKI = 0.0; 
+    public static final double climberKD = 0.0; 
+}

src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java

@@ -0,0 +1,51 @@
+package frc.robot.subsystems.climber;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.configs.FeedbackConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.MotorAlignmentValue;
+
+import edu.wpi.first.math.controller.PIDController;
+import frc.robot.subsystems.climber.ClimberConstants;
+
+public class ClimberSubsystem {
+    private TalonFX leftMotor = new TalonFX(ClimberConstants.leftMotorID,ClimberConstants.climberBus);
+    private TalonFX rightMotor = new TalonFX(ClimberConstants.rightMotorID,ClimberConstants.climberBus);
+
+    private PIDController climberPID = new PIDController(ClimberConstants.climberKP,ClimberConstants.climberKI, ClimberConstants.climberKD);
+
+    public ClimberSubsystem() {
+        TalonFXConfiguration leftConfiguration = new TalonFXConfiguration();
+        TalonFXConfiguration rightConfiguration = new TalonFXConfiguration();
+        leftConfiguration.Feedback.withSensorToMechanismRatio(12);
+        rightConfiguration.Feedback.withSensorToMechanismRatio(12);
+
+        MotorOutputConfigs leftConfigs = new MotorOutputConfigs();
+        MotorOutputConfigs rightConfigs = new MotorOutputConfigs();
+        leftConfigs.Inverted = InvertedValue.CounterClockwise_Positive;
+        rightConfigs.Inverted =  InvertedValue.Clockwise_Positive;
+        leftMotor.setControl(new Follower(ClimberConstants.rightMotorID , MotorAlignmentValue.Aligned));
+        rightMotor.setPosition(0);
+    }
+
+    public double getClimberPosition() {
+        return (leftMotor.getPosition().getValueAsDouble()+rightMotor.getPosition().getValueAsDouble())/2;
+    }
+
+    public void resetClimberPosition() {
+        rightMotor.setPosition(0);
+    }
+
+    public void setClimberTargetPosition(double position) {
+        climberPID.setSetpoint(position);
+    }
+
+    public void runClimber() {
+        leftMotor.setVoltage(climberPID.calculate((leftMotor.getPosition().getValueAsDouble()
+        + rightMotor.getPosition().getValueAsDouble())/2));
+    }
+}