Started Climb Subsystem (#11)

Added climb with two motors Has a manual control break mode peak gaming --------- Co-authored-by: Joseph Eng <[email protected]>
robototes · Feb 2, 2025 · 25f0be9 · 25f0be9
1 parent 0c1210c
commit 25f0be9
Show file tree

Hide file tree

Showing 4 changed files with 97 additions and 0 deletions.
diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java
@@ -51,6 +51,7 @@ public Controls(Subsystems s) {
     configureDrivebaseBindings();
     configureElevatorBindings();
     configureArmPivotBindings();
+    configureClimbPivotBindings();
     configureSpinnyClawBindings();
   }
 
@@ -104,11 +105,27 @@ private void configureArmPivotBindings() {
     if (s.armPivotSubsystem == null) {
       return;
     }
+
     // Arm Controls binding goes here
     s.armPivotSubsystem.setDefaultCommand(
         s.armPivotSubsystem.startMovingVoltage(() -> Volts.of(6 * operatorController.getLeftY())));
   }
 
+  private void configureClimbPivotBindings() {
+    if (s.climbPivotSubsystem == null) {
+      return;
+    }
+
+    operatorController
+        .start()
+        .onTrue(
+            s.climbPivotSubsystem
+                .moveClimbMotor(0.1)
+                .withTimeout(0.5)
+                .andThen(s.climbPivotSubsystem.moveClimbMotor(-0.1))
+                .withTimeout(0.5));
+  }
+
   private void configureSpinnyClawBindings() {
     if (s.spinnyClawSubsytem == null) {
       return;

diff --git a/src/main/java/frc/robot/Hardware.java b/src/main/java/frc/robot/Hardware.java
@@ -16,6 +16,13 @@ public class Hardware {
   // arm pivot [30-34]
   public static final int ARM_PIVOT_MOTOR_ID = 30;
 
+  // climb [50-59]
+  public static final int CLIMB_PIVOT_MOTOR_ONE_ID = 50;
+  public static final int CLIMB_PIVOT_MOTOR_TWO_ID = 51;
+
+  // climb DIO
+  public static final int CLIMB_SENSOR = 1;
+
   // arm Sensors [35-39]
   public static final int MAIN_ARM_SENSOR = 35;
 

diff --git a/src/main/java/frc/robot/Subsystems.java b/src/main/java/frc/robot/Subsystems.java
@@ -5,6 +5,7 @@
 import frc.robot.generated.BonkTunerConstants;
 import frc.robot.generated.CompTunerConstants;
 import frc.robot.subsystems.ArmPivot;
+import frc.robot.subsystems.ClimbPivot;
 import frc.robot.subsystems.DrivebaseWrapper;
 import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.SpinnyClaw;
@@ -22,6 +23,7 @@ public static class SubsystemConstants {
 
     public static final boolean ELEVATOR_ENABLED = true;
     public static final boolean ARMPIVOT_ENABLED = true;
+    public static final boolean CLIMBPIVOT_ENABLED = false;
     public static final boolean SPINNYCLAW_ENABLED = true;
   }
 
@@ -31,6 +33,7 @@ public static class SubsystemConstants {
   public final VisionSubsystem visionSubsystem;
   public final ElevatorSubsystem elevatorSubsystem;
   public final ArmPivot armPivotSubsystem;
+  public final ClimbPivot climbPivotSubsystem;
   public final SpinnyClaw spinnyClawSubsytem;
 
   public Subsystems() {
@@ -53,16 +56,25 @@ public Subsystems() {
     } else {
       visionSubsystem = null;
     }
+
     if (ELEVATOR_ENABLED) {
       elevatorSubsystem = new ElevatorSubsystem();
     } else {
       elevatorSubsystem = null;
     }
+
     if (ARMPIVOT_ENABLED) {
       armPivotSubsystem = new ArmPivot();
     } else {
       armPivotSubsystem = null;
     }
+
+    if (CLIMBPIVOT_ENABLED) {
+      climbPivotSubsystem = new ClimbPivot();
+    } else {
+      climbPivotSubsystem = null;
+    }
+
     if (SPINNYCLAW_ENABLED) {
       spinnyClawSubsytem = new SpinnyClaw();
     } else {

diff --git a/src/main/java/frc/robot/subsystems/ClimbPivot.java b/src/main/java/frc/robot/subsystems/ClimbPivot.java
@@ -0,0 +1,61 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Hardware;
+
+public class ClimbPivot extends SubsystemBase {
+
+  private final TalonFX climbPivotMotorOne;
+  private final TalonFX climbPivotMotorTwo;
+  private final DigitalInput climbSensor;
+
+  public ClimbPivot() {
+    climbPivotMotorOne = new TalonFX(Hardware.CLIMB_PIVOT_MOTOR_ONE_ID);
+    climbPivotMotorTwo = new TalonFX(Hardware.CLIMB_PIVOT_MOTOR_TWO_ID);
+    climbSensor = new DigitalInput(Hardware.CLIMB_SENSOR);
+    configureMotors();
+  }
+
+  private void configureMotors() {
+    var talonFXConfigurator = climbPivotMotorOne.getConfigurator();
+    var talonFXConfigurator2 = climbPivotMotorTwo.getConfigurator();
+    // Set and enable current limit
+    var currentLimits = new CurrentLimitsConfigs();
+    currentLimits.StatorCurrentLimit = 5;
+    currentLimits.StatorCurrentLimitEnable = true;
+    currentLimits.SupplyCurrentLimit = 5;
+    currentLimits.SupplyCurrentLimitEnable = true;
+    talonFXConfigurator.apply(currentLimits);
+    talonFXConfigurator2.apply(currentLimits);
+
+    var talonFXMotorOutput = new MotorOutputConfigs();
+    // Enable brake mode
+    talonFXMotorOutput.NeutralMode = NeutralModeValue.Brake;
+    talonFXConfigurator.apply(talonFXMotorOutput);
+    talonFXConfigurator2.apply(talonFXMotorOutput);
+    // OpposeMasterDirection can be changed based on climb design, not yet sure if 2nd motor will be
+    // on opposite side
+    climbPivotMotorTwo.setControl(new Follower(climbPivotMotorOne.getDeviceID(), true));
+  }
+
+  public Command moveClimbMotor(double speed) {
+    return run(() -> {
+          climbPivotMotorOne.set(speed);
+        })
+        .finallyDo(
+            () -> {
+              climbPivotMotorOne.stopMotor();
+            });
+  }
+
+  public boolean checkClimbSensor() {
+    return climbSensor.get();
+  }
+}