From ca1caf49083602637ba5d1d6d1aff1437a27fd3d Mon Sep 17 00:00:00 2001
From: Kaya <95276965+kytpbs@users.noreply.github.com>
Date: Sat, 27 Jan 2024 23:01:32 +0300
Subject: [PATCH 01/20] add an LED System

---
 src/main/java/frc/robot/Constants.java        |   5 +
 .../java/frc/robot/subsystems/LEDSystem.java  |  14 ++
 src/main/java/frc/utils/BetterLED.java        | 137 ++++++++++++++++++
 3 files changed, 156 insertions(+)
 create mode 100644 src/main/java/frc/robot/subsystems/LEDSystem.java
 create mode 100644 src/main/java/frc/utils/BetterLED.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index ddfd43f..e7c001e 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -184,6 +184,11 @@ public static final class PiCamera extends CameraDetails {
     }
   }
 
+  public static final class LEDConstants {
+    public static final int kLedPin = 9;
+    public static final int kLedCount = 50;
+  }
+
   public static final class AutoConstants {
     public static final double kMaxSpeedMetersPerSecond = 3;
     public static final double kMaxAccelerationMetersPerSecondSquared = 3;
diff --git a/src/main/java/frc/robot/subsystems/LEDSystem.java b/src/main/java/frc/robot/subsystems/LEDSystem.java
new file mode 100644
index 0000000..3795a6c
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/LEDSystem.java
@@ -0,0 +1,14 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.util.Color;
+import frc.robot.Constants.LEDConstants;
+import frc.utils.BetterLED;
+
+public final class LEDSystem {
+  private static final BetterLED strip =
+      new BetterLED(LEDConstants.kLedPin, LEDConstants.kLedCount);
+
+  public static void setLEDColorRGB(int r, int g, int b) {
+    strip.fill(new Color(r, g, b));
+  }
+}
diff --git a/src/main/java/frc/utils/BetterLED.java b/src/main/java/frc/utils/BetterLED.java
new file mode 100644
index 0000000..ccd23d6
--- /dev/null
+++ b/src/main/java/frc/utils/BetterLED.java
@@ -0,0 +1,137 @@
+package frc.utils;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.util.Color;
+import java.util.ArrayList;
+
+public class BetterLED extends AddressableLED {
+  public enum AnimationType { // Is already static.
+    LEFT_TO_RIGHT,
+    RIGHT_TO_LEFT
+  }
+
+  private AddressableLEDBuffer mainBuffer;
+  private int ledCount;
+  private int rainbowFirstPixelHue = 0;
+  private int currentAnimationIndex = 0;
+
+  // The call list will have all animation calls in it,
+  // This will run like a scheduler, just on a smaller scale.
+  // it will probably have only a single function, but is a an Arraylist just in case.
+  private ArrayList<Runnable> callList;
+
+  // The Thread will handle calling the callList and all other requirements for animations.
+  private Thread ledUpdateThread;
+
+  public BetterLED(int port, int ledCount) {
+    super(port);
+    mainBuffer = new AddressableLEDBuffer(ledCount);
+    this.setLength(mainBuffer.getLength());
+    this.start();
+    callList = new ArrayList<>(3); // 3 should be enough on most cases.
+    // Create an update thread that handles animations
+    ledUpdateThread =
+        new Thread(
+            () -> {
+              for (; ; ) { // Infinity call periodic,
+                periodic();
+                // Maybe add a delay?
+                // or should the delay be handled by the respective functions?
+              }
+            },
+            "LED Control Thread");
+    ledUpdateThread.start();
+  }
+
+  public void fill(Color color) {
+    for (int i = 0; i < ledCount; i++) {
+      mainBuffer.setLED(i, color);
+    }
+    this.setData(mainBuffer);
+  }
+
+  public void startRainbow() {
+    changeLoopTo(this::rainbowLoop);
+  }
+
+  public void animateColor(Color color, AnimationType animation) {
+    animateColor(color, animation, (ledCount * 2) / 3);
+  }
+
+  public void animateColor(Color color, AnimationType animation, int onLEDCount) {
+    switch (animation) {
+      case LEFT_TO_RIGHT:
+        changeLoopTo(createLeftToRightAnimationLoop(color, onLEDCount));
+        break;
+      case RIGHT_TO_LEFT:
+        changeLoopTo(createRightToLeftAnimationLoop(color, onLEDCount));
+        break;
+    }
+  }
+
+  public void stopAnimation() {
+    changeLoopTo(() -> {}); // Empty runnable
+  }
+
+  @Override
+  public void setLength(int length) {
+    mainBuffer = new AddressableLEDBuffer(length);
+    ledCount = length;
+    super.setLength(length);
+  }
+
+  private void changeLoopTo(Runnable func) {
+    callList.clear(); // Clear the list first
+    callList.add(func); // then add the runnable
+  }
+
+  private void periodic() {
+    // Call any functions in the callList
+    for (var callable : callList) {
+      callable.run();
+    }
+  }
+
+  private void rainbowLoop() {
+    for (int i = 0; i < ledCount; i++) {
+      // Calculate the hue - hue is easier for rainbows because the color
+      // shape is a circle so only one value needs to precess
+      final var hue = (rainbowFirstPixelHue + (i * 180 / ledCount)) % 80;
+      // Set the value
+      mainBuffer.setHSV(i, hue, 255, 255);
+    }
+    rainbowFirstPixelHue += 3;
+    rainbowFirstPixelHue %= 180;
+    this.setData(mainBuffer);
+  }
+
+  private Runnable createLeftToRightAnimationLoop(Color color, int onLEDCount) {
+    return () -> {
+      final int start = currentAnimationIndex > onLEDCount ? currentAnimationIndex - onLEDCount : 0;
+      for (int i = start; i < ledCount && i < currentAnimationIndex; i++) {
+        mainBuffer.setLED(i, color);
+      }
+      currentAnimationIndex++;
+      currentAnimationIndex %= ledCount;
+      this.setData(mainBuffer);
+      Timer.delay(0.1);
+    };
+  }
+
+  private Runnable createRightToLeftAnimationLoop(Color color, int onLEDCount) {
+    return () -> {
+      final int start =
+          currentAnimationIndex < onLEDCount ? currentAnimationIndex + onLEDCount : ledCount;
+      for (int i = start; i > 0 && i > currentAnimationIndex; i--) {
+        mainBuffer.setLED(i, color);
+      }
+      currentAnimationIndex--;
+      currentAnimationIndex =
+          Math.floorMod(currentAnimationIndex, ledCount); // ! DO NOT USE % WITH NEGATIVE NUMBERS.
+      this.setData(mainBuffer);
+      Timer.delay(0.1);
+    };
+  }
+}

From 93a7540c140aa4ad2f11bb414a656ad61ce1acaa Mon Sep 17 00:00:00 2001
From: Kaya <95276965+kytpbs@users.noreply.github.com>
Date: Sat, 27 Jan 2024 23:40:57 +0300
Subject: [PATCH 02/20] add blink function

---
 src/main/java/frc/utils/BetterLED.java | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/src/main/java/frc/utils/BetterLED.java b/src/main/java/frc/utils/BetterLED.java
index ccd23d6..74832ab 100644
--- a/src/main/java/frc/utils/BetterLED.java
+++ b/src/main/java/frc/utils/BetterLED.java
@@ -75,6 +75,18 @@ public void stopAnimation() {
     changeLoopTo(() -> {}); // Empty runnable
   }
 
+  public void blink(Color color, int blinkCount) {
+    changeLoopTo(
+        () -> {
+          for (int i = 0; i < blinkCount; i++) {
+            fill(color);
+            Timer.delay(0.5);
+            fill(Color.kBlack);
+            Timer.delay(0.5);
+          }
+        });
+  }
+
   @Override
   public void setLength(int length) {
     mainBuffer = new AddressableLEDBuffer(length);

From 165520f754411f1a7a324e0edbd49f2c52e11556 Mon Sep 17 00:00:00 2001
From: Kaya <95276965+kytpbs@users.noreply.github.com>
Date: Sat, 27 Jan 2024 23:44:06 +0300
Subject: [PATCH 03/20] add an extra "add runnable" function

This will probably be never used, but is there incase we need it. or we extend the class.
---
 src/main/java/frc/utils/BetterLED.java | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/src/main/java/frc/utils/BetterLED.java b/src/main/java/frc/utils/BetterLED.java
index 74832ab..3635be0 100644
--- a/src/main/java/frc/utils/BetterLED.java
+++ b/src/main/java/frc/utils/BetterLED.java
@@ -87,6 +87,16 @@ public void blink(Color color, int blinkCount) {
         });
   }
 
+  /**
+   * Adds a runnable to the call list, this will be called every loop. You probably don't need to
+   * use this, but it's here if you need it.
+   *
+   * @param func The runnable to add to the call list.
+   */
+  public void addToLoop(Runnable func) {
+    callList.add(func);
+  }
+
   @Override
   public void setLength(int length) {
     mainBuffer = new AddressableLEDBuffer(length);

From ecb34233d8de1bf89a97a0eea8db375721901012 Mon Sep 17 00:00:00 2001
From: Kaya <95276965+kytpbs@users.noreply.github.com>
Date: Sat, 27 Jan 2024 23:45:06 +0300
Subject: [PATCH 04/20] rename add to loop Function

---
 src/main/java/frc/utils/BetterLED.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/utils/BetterLED.java b/src/main/java/frc/utils/BetterLED.java
index 3635be0..5ed66fc 100644
--- a/src/main/java/frc/utils/BetterLED.java
+++ b/src/main/java/frc/utils/BetterLED.java
@@ -93,7 +93,7 @@ public void blink(Color color, int blinkCount) {
    *
    * @param func The runnable to add to the call list.
    */
-  public void addToLoop(Runnable func) {
+  public void addRunnableToLoop(Runnable func) {
     callList.add(func);
   }
 

From 41e1540420337fb60b30ab4b506278a46e214a84 Mon Sep 17 00:00:00 2001
From: Kaya <95276965+kytpbs@users.noreply.github.com>
Date: Mon, 29 Jan 2024 16:45:57 +0300
Subject: [PATCH 05/20] add a breathe function

---
 src/main/java/frc/utils/BetterLED.java | 32 ++++++++++++++++++++++++++
 1 file changed, 32 insertions(+)

diff --git a/src/main/java/frc/utils/BetterLED.java b/src/main/java/frc/utils/BetterLED.java
index 5ed66fc..43bf9e8 100644
--- a/src/main/java/frc/utils/BetterLED.java
+++ b/src/main/java/frc/utils/BetterLED.java
@@ -87,6 +87,38 @@ public void blink(Color color, int blinkCount) {
         });
   }
 
+  /**
+   * Breathe the led, this will light up the led and then light it down. This will repeat forever.
+   *
+   * @param color The color to breathe. see: WPILIB {@link Color}
+   * @param delayTime The time delay between all the steps in seconds; NOT the total time!
+   * @param max The max value of the led, normally the maximum is 255.
+   * @param min The min value of the led, normally the minimum is 0.
+   */
+  public void breathe(Color color, double delayTime, int max, int min) {
+    changeLoopTo(
+        () -> {
+          // Light UP The led
+          for (int i = min; i < max; i++) {
+            fill(new Color(color.red * i, color.green * i, color.blue * i));
+            Timer.delay(delayTime);
+          }
+          // Light DOWN the led
+          for (int i = max; i > min; i--) {
+            fill(new Color(color.red * i, color.green * i, color.blue * i));
+            Timer.delay(delayTime);
+          }
+        });
+  }
+
+  public void breathe(Color color, double delayTime) {
+    breathe(color, delayTime, 255, 0);
+  }
+
+  public void breathe(Color color) {
+    breathe(color, 0.01);
+  }
+
   /**
    * Adds a runnable to the call list, this will be called every loop. You probably don't need to
    * use this, but it's here if you need it.

From 7260e5edc8ce5b7437a871ba95b925b93117cfbe Mon Sep 17 00:00:00 2001
From: MEFThunders7035 <157488669+MEFThunders7035@users.noreply.github.com>
Date: Tue, 27 Feb 2024 10:25:09 +0300
Subject: [PATCH 06/20] update color sensor wrap so it works in simulation

Because rev already supports simulation... BUT FOR SOME REASON DOESN'T SUPPORT UNIT TESTING. WHY REV WHY
---
 .../java/frc/utils/sim_utils/ColorSensorV3Wrapped.java | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/utils/sim_utils/ColorSensorV3Wrapped.java b/src/main/java/frc/utils/sim_utils/ColorSensorV3Wrapped.java
index 1e4c233..742b3e8 100644
--- a/src/main/java/frc/utils/sim_utils/ColorSensorV3Wrapped.java
+++ b/src/main/java/frc/utils/sim_utils/ColorSensorV3Wrapped.java
@@ -50,7 +50,9 @@ public static void setRGBD(int red, int green, int blue, int distance) {
 
   @Override
   public int getRed() {
-    if (RobotBase.isReal()) {
+    // we check if getRed() is 0 because the color sensor returns 0 when its not being "simulated"
+    // as this is only used for unit tests.
+    if (RobotBase.isReal() || colorSensor == null || colorSensor.getRed() != 0) {
       return super.getRed();
     }
     return rgbd[0];
@@ -58,7 +60,7 @@ public int getRed() {
 
   @Override
   public int getGreen() {
-    if (RobotBase.isReal()) {
+    if (RobotBase.isReal() || colorSensor == null || colorSensor.getGreen() != 0) {
       return super.getGreen();
     }
     return rgbd[1];
@@ -66,7 +68,7 @@ public int getGreen() {
 
   @Override
   public int getBlue() {
-    if (RobotBase.isReal()) {
+    if (RobotBase.isReal() || colorSensor == null || colorSensor.getBlue() != 0) {
       return super.getBlue();
     }
     return rgbd[2];
@@ -74,7 +76,7 @@ public int getBlue() {
 
   @Override
   public int getProximity() {
-    if (RobotBase.isReal()) {
+    if (RobotBase.isReal() || colorSensor == null || colorSensor.getProximity() != 0) {
       return super.getProximity();
     }
     return rgbd[3];

From a3c059026c0b28afae9d8f71517a994c40bbf5b4 Mon Sep 17 00:00:00 2001
From: MEFThunders7035 <157488669+MEFThunders7035@users.noreply.github.com>
Date: Tue, 27 Feb 2024 10:27:22 +0300
Subject: [PATCH 07/20] fix recursive call error

Idiot me...
---
 .../java/frc/utils/sim_utils/ColorSensorV3Wrapped.java    | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/utils/sim_utils/ColorSensorV3Wrapped.java b/src/main/java/frc/utils/sim_utils/ColorSensorV3Wrapped.java
index 742b3e8..9780606 100644
--- a/src/main/java/frc/utils/sim_utils/ColorSensorV3Wrapped.java
+++ b/src/main/java/frc/utils/sim_utils/ColorSensorV3Wrapped.java
@@ -52,7 +52,7 @@ public static void setRGBD(int red, int green, int blue, int distance) {
   public int getRed() {
     // we check if getRed() is 0 because the color sensor returns 0 when its not being "simulated"
     // as this is only used for unit tests.
-    if (RobotBase.isReal() || colorSensor == null || colorSensor.getRed() != 0) {
+    if (RobotBase.isReal() || colorSensor == null || super.getRed() != 0) {
       return super.getRed();
     }
     return rgbd[0];
@@ -60,7 +60,7 @@ public int getRed() {
 
   @Override
   public int getGreen() {
-    if (RobotBase.isReal() || colorSensor == null || colorSensor.getGreen() != 0) {
+    if (RobotBase.isReal() || colorSensor == null || super.getGreen() != 0) {
       return super.getGreen();
     }
     return rgbd[1];
@@ -68,7 +68,7 @@ public int getGreen() {
 
   @Override
   public int getBlue() {
-    if (RobotBase.isReal() || colorSensor == null || colorSensor.getBlue() != 0) {
+    if (RobotBase.isReal() || colorSensor == null || super.getBlue() != 0) {
       return super.getBlue();
     }
     return rgbd[2];
@@ -76,7 +76,7 @@ public int getBlue() {
 
   @Override
   public int getProximity() {
-    if (RobotBase.isReal() || colorSensor == null || colorSensor.getProximity() != 0) {
+    if (RobotBase.isReal() || colorSensor == null || super.getProximity() != 0) {
       return super.getProximity();
     }
     return rgbd[3];

From 430dc8c73311057c9c704982e37044f4abb48ea5 Mon Sep 17 00:00:00 2001
From: MEFThunders7035 <157488669+MEFThunders7035@users.noreply.github.com>
Date: Tue, 27 Feb 2024 10:40:00 +0300
Subject: [PATCH 08/20] Make the vibrate actually work

I had forgotten to add a waitUntil for the note... Finally it works
---
 src/main/java/frc/robot/subsystems/IntakeSubsystem.java | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
index bed8d6c..d0fdeb0 100644
--- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -4,8 +4,10 @@
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
 import frc.robot.Constants.IntakeConstants;
 import frc.robot.Constants.IntakeConstants.ColorSensorConstants;
 import frc.robot.commands.VibrateControllerCommand;
@@ -62,7 +64,9 @@ public Command loadToShooterCommand() {
   }
 
   public Command vibrateControllerOnNoteCommand(XboxController controller) {
-    return new VibrateControllerCommand(controller, 2, 1, 0.2);
+    return new VibrateControllerCommand(controller, 2, 1, 0.1)
+        .alongWith(new PrintCommand("Note!"))
+        .beforeStarting(new WaitUntilCommand(this::hasNote));
   }
 
   @Override

From c7332802728c8f7a503795d01d822e9968172553 Mon Sep 17 00:00:00 2001
From: MEFThunders7035 <157488669+MEFThunders7035@users.noreply.github.com>
Date: Tue, 27 Feb 2024 10:41:31 +0300
Subject: [PATCH 09/20] change name of set arm to zero -> stop arm

This should give easier understanding
---
 src/main/java/frc/robot/subsystems/ArmSubsystem.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java
index 87eeb73..1308aa1 100644
--- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java
@@ -67,7 +67,7 @@ public void setArmSpeed(double speed) {
    *
    * @apiNote YOU SHOULD NOT USE THIS METHOD UNLESS NECESSARY! USE THE PID CONTROLLER INSTEAD.
    */
-  public void setArmToZero() {
+  public void stopArm() {
     arm.set(0);
   }
 

From 0077efa88ff5caac9e5a5a7c3b106b901cfc52ab Mon Sep 17 00:00:00 2001
From: MEFThunders7035 <157488669+MEFThunders7035@users.noreply.github.com>
Date: Tue, 27 Feb 2024 10:43:04 +0300
Subject: [PATCH 10/20] add a is arm at position command

funny revlib doesn't support this either... but I don't care
---
 src/main/java/frc/robot/Constants.java               | 2 ++
 src/main/java/frc/robot/subsystems/ArmSubsystem.java | 5 +++++
 2 files changed, 7 insertions(+)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 82d5d99..91281e1 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -151,6 +151,8 @@ public static final class ArmPIDConstants {
       public static final double kI = 0.0;
       public static final double kD = 0.0;
       public static final double kFF = 0.0;
+
+      public static final double kAllowedError = 0.05;
     }
   }
 
diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java
index 1308aa1..a886ad8 100644
--- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java
@@ -9,6 +9,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.IntakeConstants;
+import frc.robot.Constants.IntakeConstants.ArmPIDConstants;
 import frc.utils.ExtraFunctions;
 import frc.utils.sim_utils.CANSparkMAXWrapped;
 
@@ -71,6 +72,10 @@ public void stopArm() {
     arm.set(0);
   }
 
+  public boolean isArmAtPosition(double position) {
+    return Math.abs(encoder.getPosition() - position) < ArmPIDConstants.kAllowedError;
+  }
+
   /**
    * Sets the arm to a given position.
    *

From ce962217ae1b27bb25a811689f566007f341b891 Mon Sep 17 00:00:00 2001
From: MEFThunders7035 <157488669+MEFThunders7035@users.noreply.github.com>
Date: Tue, 27 Feb 2024 10:43:31 +0300
Subject: [PATCH 11/20] create set arm to position command

---
 .../java/frc/robot/subsystems/ArmSubsystem.java  | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java
index a886ad8..468751a 100644
--- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java
@@ -7,6 +7,7 @@
 import com.revrobotics.SparkRelativeEncoder;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.IntakeConstants;
 import frc.robot.Constants.IntakeConstants.ArmPIDConstants;
@@ -96,6 +97,21 @@ public void setArmToPosition(int positionDegrees) {
     setArmToPosition(positionDegrees / 360.0);
   }
 
+  public Command setArmToPositionCommand(double position) {
+    return this.run(() -> this.setArmToPosition(position))
+        .onlyWhile(() -> !this.isArmAtPosition(position));
+  }
+
+  /**
+   * Sets the arm to a given position.
+   *
+   * @param positionDegrees The rotation the arm should be at. (from 0 to 360)
+   * @return A command that will set the arm to the given position.
+   */
+  public Command setArmToPositionCommand(int positionDegrees) {
+    return this.setArmToPositionCommand(positionDegrees / 360.0);
+  }
+
   public void setArmToAprilTag() {
     var currentAlliance = DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue);
     boolean isBlueAlliance = currentAlliance == DriverStation.Alliance.Blue;

From f388cd7e238cf89c5d2c2baa7833340d28bf298a Mon Sep 17 00:00:00 2001
From: MEFThunders7035 <157488669+MEFThunders7035@users.noreply.github.com>
Date: Tue, 27 Feb 2024 10:44:06 +0300
Subject: [PATCH 12/20] Update Constants.java

---
 src/main/java/frc/robot/Constants.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 91281e1..4af29c8 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -118,7 +118,7 @@ public static final class ModuleConstants {
     public static final IdleMode kTurningMotorIdleMode = IdleMode.kBrake;
 
     public static final int kDrivingMotorCurrentLimit = 50; // amps
-    public static final int kTurningMotorCurrentLimit = 20; // amps
+    public static final int kTurningMotorCurrentLimit = 30; // amps
   }
 
   public static final class OIConstants {

From 8e77fca963cee3faf443598ab60f7cb6c469148a Mon Sep 17 00:00:00 2001
From: MEFThunders7035 <157488669+MEFThunders7035@users.noreply.github.com>
Date: Tue, 27 Feb 2024 10:44:14 +0300
Subject: [PATCH 13/20] Create SmartIntakeCommand.java

---
 .../robot/commands/SmartIntakeCommand.java    | 23 +++++++++++++++++++
 1 file changed, 23 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/SmartIntakeCommand.java

diff --git a/src/main/java/frc/robot/commands/SmartIntakeCommand.java b/src/main/java/frc/robot/commands/SmartIntakeCommand.java
new file mode 100644
index 0000000..9991ac0
--- /dev/null
+++ b/src/main/java/frc/robot/commands/SmartIntakeCommand.java
@@ -0,0 +1,23 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.PrintCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.Constants.IntakeConstants;
+import frc.robot.subsystems.ArmSubsystem;
+import frc.robot.subsystems.IntakeSubsystem;
+
+public class SmartIntakeCommand extends SequentialCommandGroup {
+  public SmartIntakeCommand(
+      IntakeSubsystem intakeSubsystem, ArmSubsystem armSubsystem, XboxController controller) {
+    super(
+        armSubsystem
+            .setArmToPositionCommand(0)
+            .onlyIf(() -> !intakeSubsystem.hasNote())
+            .beforeStarting(new PrintCommand("Setting Arm to 0"))
+            .andThen(
+                intakeSubsystem.run(
+                    () -> intakeSubsystem.setIntakeSpeed(IntakeConstants.kIntakeSpeed)))
+            .alongWith(intakeSubsystem.vibrateControllerOnNoteCommand(controller)));
+  }
+}

From b17c4656225786947cd4efcaa9f0b17ba263e5a1 Mon Sep 17 00:00:00 2001
From: MEFThunders7035 <157488669+MEFThunders7035@users.noreply.github.com>
Date: Tue, 27 Feb 2024 10:44:34 +0300
Subject: [PATCH 14/20] use the new smartIntakeCommand

---
 src/main/java/frc/robot/RobotContainer.java | 7 ++-----
 1 file changed, 2 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 9df67f3..b36e6fb 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -8,6 +8,7 @@
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc.robot.Constants.IntakeConstants;
 import frc.robot.Constants.ShooterConstants;
+import frc.robot.commands.SmartIntakeCommand;
 import frc.robot.commands.SmartShootCommand;
 import frc.robot.commands.TestAutoCommand;
 import frc.robot.simulationSystems.PhotonSim;
@@ -52,11 +53,7 @@ private void configureBindings() {
         .whileTrue(new RunCommand(driveSubsystem::setX, driveSubsystem));
 
     new JoystickButton(controller, Button.kB.value) // Intake
-        .whileTrue(
-            new RunCommand(
-                    () -> intakeSubsystem.setIntakeSpeed(IntakeConstants.kIntakeSpeed),
-                    intakeSubsystem)
-                .alongWith(intakeSubsystem.vibrateControllerOnNoteCommand(controller)));
+        .whileTrue(new SmartIntakeCommand(intakeSubsystem, armSubsystem, controller));
 
     // TODO: DEPRECATED, REMOVE
     new JoystickButton(controller, Button.kX.value) // Shoot, basic (Run Shooter)

From 0dab34ac34f2d003ceab6d88dc01de574545bc5f Mon Sep 17 00:00:00 2001
From: MEFThunders7035 <157488669+MEFThunders7035@users.noreply.github.com>
Date: Tue, 27 Feb 2024 10:47:43 +0300
Subject: [PATCH 15/20] move sim functions to a sperate thread

The performance issues were too tiring to handle
---
 src/main/java/frc/robot/RobotContainer.java | 18 ++++++++++++++++--
 1 file changed, 16 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index b36e6fb..60826d7 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -1,5 +1,7 @@
 package frc.robot;
 
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.XboxController.Button;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -31,12 +33,24 @@ public RobotContainer() {
     configureBindings();
     setDefaultCommands();
     PhotonCameraSystem.getAprilTagWithID(0); // Load the class before enable.
+    if (RobotBase.isSimulation()) {
+      simInit();
+    }
   }
 
-  public void simPeriodic() {
-    PhotonSim.update(driveSubsystem.getPose());
+  public void simInit() {
+    new Thread(
+            () -> {
+              for (; ; ) {
+                PhotonSim.update(driveSubsystem.getPose());
+                Timer.delay(0.02);
+              }
+            })
+        .start();
   }
 
+  public void simPeriodic() {}
+
   private void setDefaultCommands() {
     driveSubsystem.setDefaultCommand(driveSubsystem.defaultDriveCommand(controller));
     intakeSubsystem.setDefaultCommand(

From 795f5847fcf059143678d826e6ec9a4649675e9e Mon Sep 17 00:00:00 2001
From: MEFThunders7035 <157488669+MEFThunders7035@users.noreply.github.com>
Date: Tue, 27 Feb 2024 14:17:44 +0300
Subject: [PATCH 16/20] clear RGB values on close() at color sensor

---
 src/main/java/frc/utils/sim_utils/ColorSensorV3Wrapped.java | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/main/java/frc/utils/sim_utils/ColorSensorV3Wrapped.java b/src/main/java/frc/utils/sim_utils/ColorSensorV3Wrapped.java
index 9780606..89480c7 100644
--- a/src/main/java/frc/utils/sim_utils/ColorSensorV3Wrapped.java
+++ b/src/main/java/frc/utils/sim_utils/ColorSensorV3Wrapped.java
@@ -23,6 +23,11 @@ public void close() {
       return;
     }
     colorSensor = null;
+
+    // Clear the RGBD values
+    for (int i = 0; i < rgbd.length; i++) {
+      rgbd[i] = 0;
+    }
   }
 
   /**

From b3c10681e7aa7c0e926de5b50ff7c7542d80a2c5 Mon Sep 17 00:00:00 2001
From: MEFThunders7035 <157488669+MEFThunders7035@users.noreply.github.com>
Date: Tue, 27 Feb 2024 14:18:26 +0300
Subject: [PATCH 17/20] close the color sensor on intake subsystem.

---
 src/main/java/frc/robot/subsystems/IntakeSubsystem.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
index d0fdeb0..2a68f06 100644
--- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -26,6 +26,7 @@ public IntakeSubsystem() {
   @Override
   public void close() {
     intake.close();
+    colorSensor.close();
   }
 
   public boolean hasNote() {

From 74f37247c9dee03ac0947d0b2af7a5f885ff9cb2 Mon Sep 17 00:00:00 2001
From: MEFThunders7035 <157488669+MEFThunders7035@users.noreply.github.com>
Date: Tue, 27 Feb 2024 14:18:54 +0300
Subject: [PATCH 18/20] add note detect tests for intake subsystem

We should add more using real world values.
---
 src/test/java/IntakeTests.java | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/src/test/java/IntakeTests.java b/src/test/java/IntakeTests.java
index 5e5164e..d9c7fd5 100644
--- a/src/test/java/IntakeTests.java
+++ b/src/test/java/IntakeTests.java
@@ -32,6 +32,15 @@ void testIntakeSubsystem() {
     assertEquals(0.5, intakeMotor.get(), 0.001);
   }
 
+  @Test
+  void testIntakeNoteDetected() {
+    ColorSensorV3Wrapped.setRGBD(2500, 0, 0, 900);
+    assertEquals(true, intakeSubsystem.hasNote(), "Intake should detect a note");
+    ColorSensorV3Wrapped.setRGBD(0, 0, 0, 0);
+    assertEquals(false, intakeSubsystem.hasNote(), "Intake should not detect a note");
+    // TODO: Add more tests for different RGBD values
+  }
+
   @Test
   void testIntakeSubsystemWithNote() {
     ColorSensorV3Wrapped.setRGBD(2500, 0, 0, 900);

From 28fda79bb40b258b8f99f7a43a3db88cf1e4e819 Mon Sep 17 00:00:00 2001
From: MEFThunders7035 <157488669+MEFThunders7035@users.noreply.github.com>
Date: Tue, 27 Feb 2024 14:20:02 +0300
Subject: [PATCH 19/20] start using feedfoward (NOT COMPLETE)

still not complete but is on progress. I have to train motion profiles and other couple of stuff.
---
 src/main/java/frc/robot/Constants.java               | 6 ++++++
 src/main/java/frc/robot/subsystems/ArmSubsystem.java | 6 +++++-
 2 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 4af29c8..424a023 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -152,6 +152,12 @@ public static final class ArmPIDConstants {
       public static final double kD = 0.0;
       public static final double kFF = 0.0;
 
+      // Feedforward gains
+      // ! TODO: TUNE
+      public static final double kS = 0.0;
+      public static final double kG = 0.0;
+      public static final double kV = 0.0;
+
       public static final double kAllowedError = 0.05;
     }
   }
diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java
index 468751a..4395425 100644
--- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java
@@ -5,6 +5,7 @@
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.SparkPIDController;
 import com.revrobotics.SparkRelativeEncoder;
+import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -20,6 +21,7 @@ public class ArmSubsystem extends SubsystemBase {
   private final CANSparkMAXWrapped armFollower;
   private final RelativeEncoder encoder;
   private final SparkPIDController pidController;
+  private final ArmFeedforward feedforward;
 
   public ArmSubsystem() {
     arm = new CANSparkMAXWrapped(IntakeConstants.kArmMotorCanID, MotorType.kBrushed);
@@ -28,6 +30,7 @@ public ArmSubsystem() {
     armFollower.follow(arm, true); // Inverted
     encoder = arm.getEncoder(SparkRelativeEncoder.Type.kQuadrature, IntakeConstants.kArmEncoderCPR);
     pidController = arm.getPIDController();
+    feedforward = new ArmFeedforward(ArmPIDConstants.kS, ArmPIDConstants.kG, ArmPIDConstants.kV);
 
     setupSparkMax();
   }
@@ -84,7 +87,8 @@ public boolean isArmAtPosition(double position) {
    * @see #setArmToPosition(int)
    */
   public void setArmToPosition(double position) {
-    pidController.setReference(position, ControlType.kPosition);
+    pidController.setReference(
+        position, ControlType.kPosition, 0, feedforward.calculate(position, encoder.getVelocity()));
   }
 
   /**

From 07c31ebd9687b9eb0ff7ec64d27455c2556db3b6 Mon Sep 17 00:00:00 2001
From: MEFThunders7035 <157488669+MEFThunders7035@users.noreply.github.com>
Date: Tue, 27 Feb 2024 14:20:28 +0300
Subject: [PATCH 20/20] Update settings.json

---
 .vscode/settings.json | 1 +
 1 file changed, 1 insertion(+)

diff --git a/.vscode/settings.json b/.vscode/settings.json
index f3d43d9..930a088 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -46,6 +46,7 @@
     "AHRS",
     "Brushless",
     "Deadband",
+    "Feedforward",
     "intaking",
     "Odometry",
     "Roborio",