From 20d6f40b0a78b638db9b8f69c4d4a719e1b36c86 Mon Sep 17 00:00:00 2001
From: SR1899 <programming@saintsrobotics.com>
Date: Mon, 4 Mar 2024 17:30:28 -0800
Subject: [PATCH] Feat: Blue LED Lights when Robot is in an Intake Position

---
 src/main/java/frc/robot/Constants.java       | 26 ++++++++++
 src/main/java/frc/robot/commands/SetLED.java | 54 +++++++++++++++++---
 2 files changed, 73 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index f07e917..c8b9cc5 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -110,6 +110,32 @@ public static final class IntakeConstants {
 
     public static final double kIntakeSpeed = 3.0;
 
+
+    /** Intake Coordninates */
+    // TODO: MAKE THIS AN ACTUALY VALID VALUE + TUNE VALUES 
+    public static final double kIntakeDist = 999;
+    public static final double kIntakeError = 999;
+    public static final double KIntakeAngleTolerance = 1;
+    public static final double KIntakePositionTolerance = 1;
+
+    public static final double KLeftRingX = 2.9;
+    public static final double KTopLeftRingY = 7;
+
+    public static final double KMiddleLeftRingY = 5.5;
+
+    public static final double KBottomLeftRingY = 4;
+
+    public static final double KRightRingX = 8.3;
+
+    public static final double KTopRightRingY = 7.43;
+
+    public static final double KSecondTopRightRingY = 5.8;
+
+    public static final double KMiddleRightRingY = 4;
+
+    public static final double KSecondBottomRightRingY = 2.42;
+
+    public static final double KBottomRightRingY = 0.75;
     // TODO: Tune distance sensor threshold for detecting note
     public static final double kDistanceSensorThreshold = 10;
   }
diff --git a/src/main/java/frc/robot/commands/SetLED.java b/src/main/java/frc/robot/commands/SetLED.java
index 2fdf30b..f45d3a3 100644
--- a/src/main/java/frc/robot/commands/SetLED.java
+++ b/src/main/java/frc/robot/commands/SetLED.java
@@ -8,6 +8,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.LEDSubsystem;
+import frc.robot.Constants.IntakeConstants;
 import frc.robot.Constants.ShooterConstants;;
 
 public class SetLED extends Command {
@@ -18,9 +19,23 @@ public class SetLED extends Command {
   private Pose2d m_odometry = m_driveSubsystem.getOdometry();
 
   private double[] m_pointsX = { ShooterConstants.kTopShootX, ShooterConstants.kMiddleShootX,
-      ShooterConstants.kBottomShootX };
+      ShooterConstants.kBottomShootX, 
+      //Intake Positions
+      IntakeConstants.KLeftRingX,
+      IntakeConstants.KRightRingX,
+    };
   private double[] m_pointsY = { ShooterConstants.kTopShootY, ShooterConstants.kMiddleShootY,
-      ShooterConstants.kBottomShootY };
+      ShooterConstants.kBottomShootY, IntakeConstants.KTopLeftRingY,
+      //Intake Positions
+      IntakeConstants.KTopLeftRingY,
+      IntakeConstants.KMiddleLeftRingY,
+      IntakeConstants.KBottomLeftRingY,
+      IntakeConstants.KTopRightRingY,
+      IntakeConstants.KSecondTopRightRingY,
+      IntakeConstants.KMiddleRightRingY,
+      IntakeConstants.KSecondBottomRightRingY,
+      IntakeConstants.KBottomRightRingY
+    };
   private double[] m_angles = { ShooterConstants.kTopShootAngle, ShooterConstants.kMiddleShootAngle,
       ShooterConstants.kBottomShootAngle };
 
@@ -35,7 +50,8 @@ public SetLED(LEDSubsystem LED) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-
+    double robotX = m_odometry.getX();
+    double robotY = m_odometry.getY();
     for (int i = 0; i < 3; i++) {
       boolean validX = false;
       boolean validY = false;
@@ -43,12 +59,12 @@ public void initialize() {
       double X = m_pointsX[i];
       double Y = m_pointsY[i];
       double A = m_angles[i];
-      if (X + ShooterConstants.kPositionTolerance > m_odometry.getX()
-          && m_odometry.getX() > X - ShooterConstants.kPositionTolerance) {
+      if (X + ShooterConstants.kPositionTolerance > robotX
+          && robotX > X - ShooterConstants.kPositionTolerance) {
         validX = true;
       }
-      if (Y + ShooterConstants.kPositionTolerance > m_odometry.getY()
-          && m_odometry.getY() > Y - ShooterConstants.kPositionTolerance) {
+      if (Y + ShooterConstants.kPositionTolerance > robotY
+          && robotY > Y - ShooterConstants.kPositionTolerance) {
         validY = true;
         break;
       }
@@ -65,6 +81,30 @@ public void initialize() {
       }
     }
 
+    //Lighting for Intake Positions
+    //Run through the next 8 positions on the list
+    for(int i = 3; i < 11; i++){
+      //Figures out the rings x value (there is only 2 independent values per side)
+      double X;
+      if(i <= 5){
+        X = m_pointsX[3];
+      }else{
+        X = m_pointsX[4];
+      }
+      //Calculate distance to point
+      double dist = Math.hypot(robotX -X,robotY - m_pointsY[i]);
+      //Check if within an acceptable distance
+      if(dist >= IntakeConstants.kIntakeDist - IntakeConstants.KIntakePositionTolerance &&
+        dist <= IntakeConstants.kIntakeDist + IntakeConstants.KIntakePositionTolerance ){
+        //Estimates the proper angle
+        double estimateAngle = Math.toDegrees(Math.atan2(robotY - m_pointsY[i],robotX - X));
+        //Checks if the angle is within an acceptable measure
+        if(Math.abs(m_odometry.getRotation().getDegrees() - estimateAngle) <= IntakeConstants.KIntakeAngleTolerance){
+          m_ledSubsystem.setLED(0,0, 255);
+        }
+      }
+    }
+    
   }
 
   // Called every time the scheduler runs while the command is scheduled.