From b4b123237bd03f5fb7e8d8f54c9432d0733ec04b Mon Sep 17 00:00:00 2001
From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com>
Date: Mon, 17 Feb 2025 11:48:43 -0700
Subject: [PATCH] Limit to beam break

Changed limit switch basin to beam break basin
---
 src/main/java/frc4388/robot/Constants.java    |  9 ++---
 .../frc4388/robot/subsystems/Elevator.java    | 33 +++++++++++++++----
 2 files changed, 31 insertions(+), 11 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index e775c85..26f8df1 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -398,13 +398,14 @@ public final class Constants {
         public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15);
         public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16);
 
-        public static final int BASIN_LIMIT_SWITCH = 0; // TODO: FIND
-        public static final int ENDEFFECTOR_LIMIT_SWITCH = 1; // TODO: FIND
+        public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND
+        public static final int ENDEFFECTOR_LIMIT_SWITCH = 9; // TODO: FIND
         
-        public static final double GEAR_RATIO_ELEVATOR = 9.0;
+        public static final double GEAR_RATIO_ELEVATOR = -9.0;
 
         public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR;
-        public static final double WAITING_POSITION_ELEVATOR = 2 * GEAR_RATIO_ELEVATOR; // TODO: find 4-6 off the ground
+        public static final double WAITING_POSITION_ELEVATOR = 2 * GEAR_RATIO_ELEVATOR; // TODO: find 2-4 in off the pipe
+        public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = 2 * GEAR_RATIO_ELEVATOR; // TODO: find on the pipe
         public static final double MAX_POSITION_ELEVATOR = 4.5 * GEAR_RATIO_ELEVATOR; // TODO: find MAX position
 
         public static final double GEAR_RATIO_ENDEFECTOR = 100.0;
diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java
index 442349c..7559f03 100644
--- a/src/main/java/frc4388/robot/subsystems/Elevator.java
+++ b/src/main/java/frc4388/robot/subsystems/Elevator.java
@@ -9,7 +9,10 @@ import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 
+import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.ElevatorConstants;
 
@@ -18,12 +21,13 @@ public class Elevator extends SubsystemBase {
   private TalonFX elevatorMotor;
   private TalonFX endefectorMotor;
 
-  private DigitalInput basinLimitSwitch;
+  private DigitalInput basinBeamBreak;
   private DigitalInput endefectorLimitSwitch;
 
   public enum CoordinationState {
-    Waiting, // for coral into the though 
-    Ready, // Has coral in enefector
+    Waiting, // for coral into the though
+    WatingBeamTriped, //once the beam break trips
+    Ready, // Has coral in endefector
     PrimedThree, // Arm and elevator Waiting to score in the level 3 position
     ScoringThree, // Arm and elevator in the level three position
     PrimedFour, // Arm and elevator Waiting to score in the level 4 position
@@ -40,7 +44,7 @@ public class Elevator extends SubsystemBase {
     elevatorMotor = elevatorTalonFX;
     endefectorMotor = endefectorTalonFX;
 
-    this.basinLimitSwitch = basinLimitSwitch;
+    this.basinBeamBreak = basinLimitSwitch;
     this.endefectorLimitSwitch = endefectorLimitSwitch;
 
     elevatorMotor.setNeutralMode(NeutralModeValue.Brake);
@@ -75,6 +79,12 @@ public class Elevator extends SubsystemBase {
         break;
       }
 
+      case WatingBeamTriped: {
+        PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR);
+        PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
+        break;
+      }
+
       case Ready: {
         PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR);
         PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR);
@@ -97,7 +107,11 @@ public class Elevator extends SubsystemBase {
   }
 
   private void periodicWaiting() {
-    if (basinLimitSwitch.get()) transitionState(CoordinationState.Ready);
+    if (basinBeamBreak.get()) transitionState(CoordinationState.WatingBeamTriped);
+  }
+
+  private void periodicWaitingTripped() {
+    if (basinBeamBreak.get()) transitionState(CoordinationState.Ready);
   }
   
   private void periodicScoring() {
@@ -107,8 +121,13 @@ public class Elevator extends SubsystemBase {
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
-    // if (currentState == CoordinationState.Waiting) {
-    //   periodicWaiting();
+    SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0);
+    SmartDashboard.putNumber("endefector", basinBeamBreak.get() ? 1 : 0);
+    if (currentState == CoordinationState.Waiting) {
+      periodicWaiting();
+    } else if (currentState == CoordinationState.WatingBeamTriped) {
+      periodicWaitingTripped();
+    }
     // } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) {
     //   periodicScoring();
     // }