Merge branch 'master' into autos-and-intake-fixes

src/main/java/frc4388/robot/subsystems/Elevator.java

@@ -6,6 +6,8 @@ package frc4388.robot.subsystems;
 
 import java.time.Instant;
 
+import org.opencv.ml.RTrees;
+
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.controls.PositionDutyCycle;
 import com.ctre.phoenix6.controls.PositionVoltage;
@@ -43,6 +45,8 @@ public class Elevator extends Subsystem {
   private boolean elevatorManualStop = true;
   private boolean endefectorManualStop = true;
 
+  private boolean disableAutoIntake = false;
+
   private DigitalInput basinBeamBreak;
   private DigitalInput endeffectorLimitSwitch;
 
@@ -200,6 +204,10 @@ public class Elevator extends Subsystem {
 
   }
 
+  public void togggleAutoIntaking() {
+    disableAutoIntake = !disableAutoIntake;
+  }
+
   public boolean elevatorAtReference() {
     // double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble();
     double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble();
@@ -256,6 +264,8 @@ public class Elevator extends Subsystem {
   public void manualElevatorVel(double velocity) {
     if (Math.abs(velocity) > 0.1) {
       elevatorMotor.set(velocity);
+      elevatorManualStop = false;
+      return;
     }
     if (!elevatorManualStop) {
       elevatorManualStop = true;
@@ -266,6 +276,8 @@ public class Elevator extends Subsystem {
   public void manualEndeffectorVel(double velocity) {
     if (Math.abs(velocity) > 0.1) {
       endeffectorMotor.set(velocity);
+      endefectorManualStop = false;
+      return;
     }
     if (!endefectorManualStop) {
       endefectorManualStop = true;
@@ -280,6 +292,8 @@ public class Elevator extends Subsystem {
     SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0);
     SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0);
     SmartDashboard.putString("State", currentState.toString());
+    
+    if (disableAutoIntake) return;
 
     if (currentState == CoordinationState.Waiting) {
       periodicWaiting();