From 696da42f5a153e6d1fb48ff716c52877b756da80 Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Wed, 22 Mar 2023 20:12:27 -0600
Subject: [PATCH] driver mode changes

---
 src/main/java/frc4388/robot/RobotContainer.java       | 4 +++-
 src/main/java/frc4388/robot/subsystems/Limelight.java | 2 +-
 2 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index c36b4f7..ea29bdb 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -94,6 +94,7 @@ public class RobotContainer {
     private SequentialCommandGroup alignToPole =
         new SequentialCommandGroup(
             new RotateToAngle(m_robotSwerveDrive, 0.0),
+            new InstantCommand(() -> m_robotLimeLight.setDriverMode(false), m_robotLimeLight),
             new InstantCommand(() -> m_robotLimeLight.setToLimePipeline(), m_robotLimeLight),
             new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getLowestTape().getYaw(), 0.04),
             new RotateToAngle(m_robotSwerveDrive, 0.0),
@@ -113,6 +114,7 @@ public class RobotContainer {
     private SequentialCommandGroup alignToShelf = 
         new SequentialCommandGroup(
             new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight),
+            new InstantCommand(() -> m_robotLimeLight.setDriverMode(false), m_robotLimeLight),
             new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight),
             new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04),
             new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight),
@@ -122,7 +124,7 @@ public class RobotContainer {
     public SequentialCommandGroup place = null;
 
     private Consumer<SequentialCommandGroup> queuePlacement = (scg) -> {
-        place = scg.andThen(new InstantCommand(() -> readyForPlacement = false), new InstantCommand(() -> isPole = null), new InstantCommand(() -> place = null));
+        place = scg.andThen(new InstantCommand(() -> m_robotLimeLight.setDriverMode(true), m_robotLimeLight), new InstantCommand(() -> readyForPlacement = false), new InstantCommand(() -> isPole = null), new InstantCommand(() -> place = null));
     };
 
     // TODO: find actual values
diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java
index e8aaed7..dff16ce 100644
--- a/src/main/java/frc4388/robot/subsystems/Limelight.java
+++ b/src/main/java/frc4388/robot/subsystems/Limelight.java
@@ -25,7 +25,7 @@ public class Limelight extends SubsystemBase {
   /** Creates a new Limelight. */
   public Limelight() {
     cam = new PhotonCamera(VisionConstants.NAME);
-    cam.setDriverMode(false);
+    cam.setDriverMode(true);
   }
 
   public void setLEDs(boolean on) {