created base superstructure system

FIRSTTeam6423 · Feb 13, 2025 · 9bdba47 · 9bdba47
1 parent a71d420
commit 9bdba47
Showing 1 changed file with 43 additions and 2 deletions.
diff --git a/src/main/java/org/frc6423/frc2025/subsystems/Superstructure.java b/src/main/java/org/frc6423/frc2025/subsystems/Superstructure.java
@@ -6,6 +6,47 @@
 
 package org.frc6423.frc2025.subsystems;
 
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import java.util.HashMap;
 
-public class Superstructure extends SubsystemBase {}
+import org.frc6423.frc2025.subsystems.elevator.Elevator;
+
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+
+public class Superstructure {
+    public static enum StructState {
+        IDLE,
+        INTAKE_CHUTE,
+        INTAKE_GROUND,
+        L1_SETUP,
+        L2_SETUP,
+        L3_SETUP,
+        L4_SETUP,
+        REEF_SCORE,
+        CORAL_OUTTAKE,
+        ALGAE_HIGHER,
+        ALGAE_LOWER,
+        PROCESSOR_SETUP,
+        PROCESSOR_SCORE,
+        ALGAE_OUTTAKE
+    }
+
+    private final Elevator m_elevator;
+
+    private StructState m_state;
+    private final HashMap<StructState, Trigger> m_stateTriggers;
+
+    public Superstructure(Elevator elevator) {
+        m_elevator = new Elevator();
+
+        m_state = StructState.IDLE;
+        m_stateTriggers = new HashMap<StructState, Trigger>();
+
+        for(StructState state : StructState.values()) {
+            m_stateTriggers.put(state, new Trigger(() -> m_state == state));
+        }
+
+        m_stateTriggers
+            .get(StructState.INTAKE_CHUTE)
+            .whileTrue(m_elevator.runPoseSetpoint(Elevator.kChuteIntakingPoseMeters));
+    }
+}