From b503f4f75b4f89f913952e43dee36c7296997e3c Mon Sep 17 00:00:00 2001
From: Jade Turner <spacey-sooty@proton.me>
Date: Mon, 17 Feb 2025 10:37:43 +0800
Subject: [PATCH] Most of plumbing finished

Signed-off-by: Jade Turner <spacey-sooty@proton.me>
---
 .../java/org/curtinfrc/frc2025/Robot.java     | 47 ++++++++++++++++---
 .../frc2025/subsystems/elevator/Elevator.java |  3 +-
 2 files changed, 42 insertions(+), 8 deletions(-)

diff --git a/src/main/java/org/curtinfrc/frc2025/Robot.java b/src/main/java/org/curtinfrc/frc2025/Robot.java
index 6fea49c2..c1da6c60 100644
--- a/src/main/java/org/curtinfrc/frc2025/Robot.java
+++ b/src/main/java/org/curtinfrc/frc2025/Robot.java
@@ -14,7 +14,9 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import java.util.Map;
 import org.curtinfrc.frc2025.Constants.Mode;
 import org.curtinfrc.frc2025.generated.TunerConstants;
 import org.curtinfrc.frc2025.subsystems.drive.Drive;
@@ -26,15 +28,18 @@
 import org.curtinfrc.frc2025.subsystems.drive.ModuleIOSim;
 import org.curtinfrc.frc2025.subsystems.drive.ModuleIOTalonFX;
 import org.curtinfrc.frc2025.subsystems.ejector.Ejector;
+import org.curtinfrc.frc2025.subsystems.ejector.EjectorConstants;
 import org.curtinfrc.frc2025.subsystems.ejector.EjectorIO;
 import org.curtinfrc.frc2025.subsystems.ejector.EjectorIONEO;
 import org.curtinfrc.frc2025.subsystems.ejector.EjectorIOSim;
 import org.curtinfrc.frc2025.subsystems.elevator.Elevator;
+import org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants;
 import org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants.ElevatorSetpoints;
 import org.curtinfrc.frc2025.subsystems.elevator.ElevatorIO;
 import org.curtinfrc.frc2025.subsystems.elevator.ElevatorIONEO;
 import org.curtinfrc.frc2025.subsystems.elevator.ElevatorIOSim;
 import org.curtinfrc.frc2025.subsystems.intake.Intake;
+import org.curtinfrc.frc2025.subsystems.intake.IntakeConstants;
 import org.curtinfrc.frc2025.subsystems.intake.IntakeIO;
 import org.curtinfrc.frc2025.subsystems.intake.IntakeIONEO;
 import org.curtinfrc.frc2025.subsystems.intake.IntakeIOSim;
@@ -79,6 +84,23 @@ public class Robot extends LoggedRobot {
   private final AutoFactory autoFactory;
   private final Autos autos;
 
+  // Setpoints
+  public static record Setpoint(ElevatorSetpoints elevatorSetpoint, DriveSetpoints driveSetpoint) {
+    public static Setpoint NULL() {
+      return new Setpoint(ElevatorSetpoints.BASE, DriveSetpoints.NULL);
+    }
+  }
+
+  private Setpoint currentSetpoint = new Setpoint(ElevatorSetpoints.BASE, DriveSetpoints.NULL);
+  private Setpoint nextSetpoint = new Setpoint(ElevatorSetpoints.BASE, DriveSetpoints.NULL);
+
+  // Triggers
+  public final Trigger atSetpoint = elevator.atSetpoint.and(drive.atSetpoint);
+  // TODO end sensor triggered
+  public final Trigger setpointDone =
+      atSetpoint.or(new Trigger(() -> currentSetpoint.equals(Setpoint.NULL())));
+  public final Trigger hasSetpoint = new Trigger(() -> !currentSetpoint.equals(Setpoint.NULL()));
+
   public Robot() {
     // Record metadata
     Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
@@ -121,7 +143,15 @@ public Robot() {
     }
 
     SignalLogger.start();
-    Logger.registerURCL(URCL.startExternal());
+    Logger.registerURCL(
+        URCL.startExternal(
+            Map.of(
+                ElevatorConstants.motorPort,
+                "Elevator",
+                EjectorConstants.motorId,
+                "Ejector",
+                IntakeConstants.intakeMotorId,
+                "Intake")));
     // Start AdvantageKit logger
     Logger.start();
 
@@ -267,11 +297,7 @@ public Robot() {
             () -> -controller.getLeftX(),
             () -> -controller.getRightX()));
 
-    elevator
-        .isNotAtCollect
-        .and(elevator.atSetpoint)
-        .and(drive.atSetpoint)
-        .whileTrue(ejector.eject(500));
+    elevator.isNotAtCollect.and(atSetpoint).whileTrue(ejector.eject(500));
 
     intake.setDefaultCommand(intake.intake(intakeVolts));
     ejector.setDefaultCommand(
@@ -340,6 +366,15 @@ public Robot() {
     controller.rightStick().whileTrue(drive.autoAlign(DriveSetpoints.RIGHT_HP));
     controller.leftStick().whileTrue(drive.autoAlign(DriveSetpoints.LEFT_HP));
 
+    setpointDone.onTrue(
+        Commands.runOnce(
+            () -> {
+              currentSetpoint = nextSetpoint;
+              nextSetpoint = Setpoint.NULL();
+            }));
+
+    hasSetpoint.whileTrue(drive.autoAlign(currentSetpoint.driveSetpoint()));
+
     // board.leftHp().onTrue(drive.autoAlign(DriveSetpoints.LEFT_HP));
     // board.rightHp().onTrue(drive.autoAlign(DriveSetpoints.RIGHT_HP));
     // board.processor().onTrue(drive.autoAlign(DriveSetpoints.PROCESSOR));
diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java
index 7953352f..87af1b25 100644
--- a/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java
+++ b/src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java
@@ -19,6 +19,7 @@ public class Elevator extends SubsystemBase {
 
   public final Trigger isNotAtCollect = new Trigger(() -> setpoint != ElevatorSetpoints.BASE);
   public final Trigger toZero = new Trigger(() -> inputs.touchSensor);
+  public final Trigger atSetpoint = new Trigger(pid::atSetpoint);
 
   public Elevator(ElevatorIO io) {
     this.io = io;
@@ -35,8 +36,6 @@ public void periodic() {
     Logger.recordOutput("Elevator/ActualError", pid.getError());
   }
 
-  public Trigger atSetpoint = new Trigger(pid::atSetpoint);
-
   public Command goToSetpoint(ElevatorSetpoints point) {
     return run(
         () -> {