Driver can now hold trigger rather than hitting it twice

FRCTeam3255 · Mar 17, 2024 · c5e75be · c5e75be
1 parent 6e1abbf
commit c5e75be
Show file tree

Hide file tree

Showing 3 changed files with 27 additions and 5 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -151,8 +151,13 @@ private void configureDriverBindings(SN_XboxController controller) {
             Commands.either(
                 new PrepAmp(subIntake, subPitch, subTransfer, subTurret, subShooter),
                 Commands.run(() -> subClimber.setPercentOutput(prefClimber.climberUpSpeed.getValue())),
-                () -> getLockedLocation() == LockedLocation.AMP))
-        .onFalse(Commands.run(() -> subClimber.setPercentOutput(0)));
+                () -> getLockedLocation() != LockedLocation.AMP).repeatedly())
+        // .whileTrue(
+        // Commands.run(() -> subIntake.setPivotBrake(false))
+        // .alongWith(Commands.runOnce(() -> subIntake.setPivotNeutralOutput()))
+        // .unless(() -> (subClimber.getPosition() < 2.5)).repeatedly())
+
+        .onFalse(Commands.runOnce(() -> subClimber.setPercentOutput(0)));
 
     controller.btn_RightTrigger
         .onTrue(Commands.runOnce(() -> subClimber.setCurrentLimiting(false)))

diff --git a/src/main/java/frc/robot/RobotPreferences.java b/src/main/java/frc/robot/RobotPreferences.java
@@ -53,7 +53,7 @@ public static final class prefClimber {
     /**
      * <b> Units: </b> Meters
      */
-    public static final SN_DoublePreference climberMaxPos = new SN_DoublePreference("climberMaxPos", 2.9);
+    public static final SN_DoublePreference climberMaxPos = new SN_DoublePreference("climberMaxPos", 2.99);
     public static final SN_BooleanPreference climberForwardLimitEnable = new SN_BooleanPreference(
         "climberForwardLimitEnable", true);
     /**
@@ -120,7 +120,7 @@ public static final class prefClimber {
     /**
      * <b> Units: </b> Percent Output
      */
-    public static final SN_DoublePreference climberDownSpeed = new SN_DoublePreference("climberDownSpeed", -0.2);
+    public static final SN_DoublePreference climberDownSpeed = new SN_DoublePreference("climberDownSpeed", -0.3);
 
     // - Positions -
     /**
@@ -292,7 +292,8 @@ public static final class prefIntake {
      */
     public static final SN_DoublePreference rollerSpitSpeed = new SN_DoublePreference("rollerSpitSpeed", -1);
 
-    public static final SN_DoublePreference rollerPlaceAmpSpeed = new SN_DoublePreference("rollerPlaceAmpSpeed", -0.2);
+    public static final SN_DoublePreference rollerPlaceAmpSpeed = new SN_DoublePreference("rollerPlaceAmpSpeed", -0.5);
+    // TODO: AMP IS -0.2. MAKE A PREFERENCE AND LOGIC FOR TRAP
 
     /**
      * <b> Units: </b> Percent Output
@@ -351,6 +352,9 @@ public static final class prefIntake {
     public static final SN_DoublePreference pivotPlaceAmpAngle = new SN_DoublePreference("pivotPlaceAmpAngle",
         11.4);
 
+    // TODO: SEPERATE TRAP PRESET : AMP IS 11.4
+    // TRAP IS 33.2
+
   }
 
   public static final class prefPitch {

diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java
@@ -9,6 +9,7 @@
 import com.ctre.phoenix6.controls.NeutralOut;
 import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DutyCycleEncoder;
@@ -89,10 +90,22 @@ public void configure() {
 
     rollerMotor.getConfigurator().apply(rollerConfig);
     pivotMotor.getConfigurator().apply(pivotConfig);
+    // TODO: MOVE THESE TO THE CONFIG
     rollerMotor.setInverted(prefIntake.rollerInverted.getValue());
     pivotMotor.setInverted(prefIntake.pivotInverted.getValue());
   }
 
+  public void setPivotBrake(boolean enabled) {
+    if (enabled) {
+      pivotConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    } else {
+      pivotConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast;
+    }
+
+    pivotMotor.getConfigurator().apply(pivotConfig);
+    pivotMotor.setInverted(prefIntake.pivotInverted.getValue());
+  }
+
   // - Get -
   /**
    * Get the raw position of the absolute encoder (without offset)