Merge branch 'vision' of https://github.com/robototes/Reefscape2025 i…

…nto vision

src/main/java/frc/robot/Controls.java

@@ -46,6 +46,7 @@ public Controls(Subsystems s) {
     configureDrivebaseBindings();
     configureElevatorBindings();
     configureArmPivotBindings();
+    configureSpinnyClawBindings();
   }
 
   private void configureDrivebaseBindings() {
@@ -101,4 +102,17 @@ private void configureArmPivotBindings() {
     s.armPivotSubsystem.setDefaultCommand(
         s.armPivotSubsystem.startMovingVoltage(() -> Volts.of(6 * operatorController.getLeftY())));
   }
+
+  private void configureSpinnyClawBindings() {
+    if (s.spinnyClawSubsytem == null) {
+      return;
+    }
+    // Claw controls bindings go here
+    operatorController
+        .rightBumper()
+        .whileTrue(s.spinnyClawSubsytem.movingVoltage(() -> Volts.of(3)));
+    operatorController
+        .leftBumper()
+        .whileTrue(s.spinnyClawSubsytem.movingVoltage(() -> Volts.of(-3)));
+  }
 }

src/main/java/frc/robot/Sensors.java

@@ -24,6 +24,7 @@ public Sensors() {
     } else {
       armSensor = null;
     }
+
     if (BRANCHSENSORS_ENABLED) {
       branchSensors = new BranchSensors();
     } else {

src/main/java/frc/robot/Subsystems.java

@@ -6,6 +6,7 @@
 import frc.robot.subsystems.ArmPivot;
 import frc.robot.subsystems.DrivebaseWrapper;
 import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.SpinnyClaw;
 import frc.robot.subsystems.VisionSubsystem;
 import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain;
 
@@ -19,6 +20,7 @@ public static class SubsystemConstants {
 
     public static final boolean ELEVATOR_ENABLED = true;
     public static final boolean ARMPIVOT_ENABLED = true;
+    public static final boolean SPINNYCLAW_ENABLED = true;
   }
 
   // Subsystems go here
@@ -27,6 +29,7 @@ public static class SubsystemConstants {
   public final VisionSubsystem visionSubsystem;
   public final ElevatorSubsystem elevatorSubsystem;
   public final ArmPivot armPivotSubsystem;
+  public final SpinnyClaw spinnyClawSubsytem;
 
   public Subsystems() {
     // Initialize subsystems here (don't forget to check if they're enabled!)
@@ -54,5 +57,10 @@ public Subsystems() {
     } else {
       armPivotSubsystem = null;
     }
+    if (SPINNYCLAW_ENABLED) {
+      spinnyClawSubsytem = new SpinnyClaw();
+    } else {
+      spinnyClawSubsytem = null;
+    }
   }
 }

src/main/java/frc/robot/subsystems/SpinnyClaw.java

@@ -0,0 +1,48 @@
+package frc.robot.subsystems;
+
+import static edu.wpi.first.units.Units.Volts;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfigurator;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Hardware;
+import java.util.function.Supplier;
+
+public class SpinnyClaw extends SubsystemBase {
+  // Remove once we implement PID speed
+  public static int placeholderPIDSpeed;
+
+  // TalonFX
+  private final TalonFX motor;
+
+  public SpinnyClaw() {
+    motor = new TalonFX(Hardware.SPINNY_CLAW_MOTOR_ID);
+    factoryDefaults();
+  }
+
+  // (+) is to move arm up, and (-) is down
+  public Command movingVoltage(Supplier<Voltage> speedControl) {
+    return run(() -> motor.setVoltage(speedControl.get().in(Volts)))
+        .finallyDo(() -> motor.setVoltage(0));
+  }
+
+  // TalonFX config
+  public void factoryDefaults() {
+    TalonFXConfiguration configuration = new TalonFXConfiguration();
+    TalonFXConfigurator cfg = motor.getConfigurator();
+    var currentLimits = new CurrentLimitsConfigs();
+    configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    cfg.apply(configuration);
+    // enabling stator current limits
+    currentLimits.StatorCurrentLimit = 5; // starting low for testing
+    currentLimits.StatorCurrentLimitEnable = true;
+    currentLimits.SupplyCurrentLimit = 5; // starting low for testing
+    currentLimits.SupplyCurrentLimitEnable = true;
+    cfg.apply(currentLimits);
+  }
+}