Auto Placement LEDs

src/main/java/frc/robot/Constants.java

@@ -142,8 +142,15 @@ public static class constLEDs {
 
     public static final int[] SHOOTER_UP_TO_SPEED_COLOR = { 36, 240, 83 };
     public static final int[] INTAKE_GAME_PIECE_COLLECTED = { 240, 186, 36 };
+    public static final int[] RED_COLOR = { 255, 0, 0 };
+    public static final int[] BLUE_COLOR = { 0, 0, 255 };
+    public static final int[] GREEN_COLOR = { 0, 255, 0 };
+    public static final int[] YELLOW_COLOR = { 255, 255, 0 };
+    public static final int[] PURPLE_COLOR = { 156, 5, 250 };
+    public static final int[] AUTO_ALIGNED_COLOR = { 207, 82, 4 };
     public static final int[] SPIT_OUT_GAME_PIECE = { 255, 60, 0 };
 
+
     public static final ColorFlowAnimation PANIC_ANIMATION = new ColorFlowAnimation(76, 22, 105, 0, 0.95, LED_NUMBER,
         Direction.Forward);
     public static final ColorFlowAnimation AMPLIFY_ANIMATION = new ColorFlowAnimation(160, 10, 247, 0, 0.95, LED_NUMBER,

src/main/java/frc/robot/Robot.java

@@ -58,6 +58,7 @@ public void disabledInit() {
   @Override
   public void disabledPeriodic() {
     FieldConstants.ALLIANCE = DriverStation.getAlliance();
+    m_robotContainer.setAutoPlacementLEDs();
   }
 
   @Override

src/main/java/frc/robot/RobotContainer.java

@@ -24,6 +24,7 @@
 import frc.robot.RobotMap.mapControllers;
 import frc.robot.RobotPreferences.prefClimber;
 import frc.robot.RobotPreferences.prefPitch;
+import frc.robot.RobotPreferences.prefVision;
 import frc.robot.RobotPreferences.prefShooter;
 import frc.robot.RobotPreferences.prefTurret;
 import frc.robot.commands.AddVisionMeasurement;
@@ -72,6 +73,15 @@ public class RobotContainer implements Logged {
   private final static Transfer subTransfer = new Transfer();
   private final static Vision subVision = new Vision();
 
+  // TODO: placeholders for now, find a way to get the actual starting auto
+  // position
+  double desiredStartingPositionX = 0;
+  double desiredStartingPositionY = 0;
+  double desiredStartingRotation = 70;
+  int[] rotationColor;
+  int[] XTranslationColor;
+  int[] YTranslationColor;
+
   public RobotContainer() {
     conDriver.setLeftDeadband(constControllers.DRIVER_LEFT_STICK_DEADBAND);
 
@@ -103,6 +113,7 @@ public RobotContainer() {
 
     subDrivetrain.resetModulesToAbsolute();
     subTurret.resetTurretToAbsolutePosition();
+    subLEDs.clearAnimation();
   }
 
   private void configureDriverBindings(SN_XboxController controller) {
@@ -244,4 +255,82 @@ public static LockedLocation getLockedLocation() {
   public Command zeroPitch() {
     return new ZeroPitch(subPitch).withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming).withTimeout(3);
   }
+
+  public void setAutoPlacementLEDs() {
+    boolean rotationCorrect = false;
+    boolean XCorrect = false;
+    boolean YCorrect = false;
+
+    // values only for testing
+    // SmartDashboard.putNumber("Current Drivetrain X",
+    // subDrivetrain.getPose().getX());
+    // SmartDashboard.putNumber("Current Drivetrain Y",
+    // subDrivetrain.getPose().getY());
+    // SmartDashboard.putNumber("Current Drivetrain Rotation",
+    // subDrivetrain.getPose().getRotation().getDegrees());
+
+    subLEDs.setLEDBrightness(0.4);
+    subLEDs.clearAnimation();
+
+    // Checking Rotation
+    if (Math.abs(desiredStartingRotation
+        - subDrivetrain.getPose().getRotation().getDegrees()) <= prefVision.rotationalAutoPlacementTolerance
+            .getValue()) {
+      rotationCorrect = true;
+      rotationColor = constLEDs.GREEN_COLOR;
+    } else if (desiredStartingRotation < 0) {
+      if (subDrivetrain.getPose().getRotation().getDegrees() > desiredStartingRotation &&
+          subDrivetrain.getPose().getRotation().getDegrees() < desiredStartingRotation + 180) {
+        rotationColor = constLEDs.BLUE_COLOR;
+      } else {
+        rotationColor = constLEDs.RED_COLOR;
+      }
+    } else if (desiredStartingRotation >= 0) {
+      if (subDrivetrain.getPose().getRotation().getDegrees() < desiredStartingRotation &&
+          subDrivetrain.getPose().getRotation().getDegrees() > desiredStartingRotation - 180) {
+        rotationColor = constLEDs.RED_COLOR;
+      } else {
+        rotationColor = constLEDs.BLUE_COLOR;
+      }
+    }
+
+    // Checking X Translation
+    if (Math.abs(desiredStartingPositionX
+        - subDrivetrain.getPose().getX()) <= prefVision.translationalAutoPlacementTolerance.getValue()) {
+      XCorrect = true;
+      XTranslationColor = constLEDs.GREEN_COLOR;
+      subLEDs.setIndividualLED(constLEDs.GREEN_COLOR, 2);
+    } else if (subDrivetrain.getPose().getX() > desiredStartingPositionX) {
+      XTranslationColor = constLEDs.BLUE_COLOR;
+    } else if (subDrivetrain.getPose().getX() < desiredStartingPositionX) {
+      XTranslationColor = constLEDs.RED_COLOR;
+    }
+
+    // Checking Y Translation
+    if (Math.abs(desiredStartingPositionY
+        - subDrivetrain.getPose().getY()) <= prefVision.translationalAutoPlacementTolerance.getValue()) {
+      YCorrect = true;
+      YTranslationColor = constLEDs.GREEN_COLOR;
+    } else if (subDrivetrain.getPose().getY() > desiredStartingPositionY) {
+      YTranslationColor = constLEDs.PURPLE_COLOR;
+    } else if (subDrivetrain.getPose().getY() < desiredStartingPositionY) {
+      YTranslationColor = constLEDs.YELLOW_COLOR;
+    }
+
+    // Light up in Shang Chi color if both translation and rotation are correct
+    if (rotationCorrect && XCorrect && YCorrect) {
+      subLEDs.setLEDs(constLEDs.AUTO_ALIGNED_COLOR);
+    } else {
+      subLEDs.setIndividualLED(rotationColor, 0);
+      subLEDs.setIndividualLED(rotationColor, 3);
+      subLEDs.setIndividualLED(rotationColor, 4);
+      subLEDs.setIndividualLED(rotationColor, 7);
+
+      subLEDs.setIndividualLED(XTranslationColor, 1);
+      subLEDs.setIndividualLED(XTranslationColor, 2);
+
+      subLEDs.setIndividualLED(YTranslationColor, 5);
+      subLEDs.setIndividualLED(YTranslationColor, 6);
+    }
+  }
 }

src/main/java/frc/robot/RobotPreferences.java

@@ -430,6 +430,22 @@ public static final class prefVision {
         "visionStdDevsHeading", Units.degreesToRadians(10));
 
     public static final SN_DoublePreference maxAmbiguity = new SN_DoublePreference("maxAmbiguity", 0.2);
+
+    /**
+     * The translational tolerance of how off we want to be to count as correct
+     * (when placing the robot on the starting position in auto)
+     * <b>Units:</b> Meters
+     */
+    public static final SN_DoublePreference translationalAutoPlacementTolerance = new SN_DoublePreference(
+        "translationalAutoPlacementTolerance", 0.15);
+
+    /**
+     * The rotational tolerance of how off we want to be to count as correct
+     * (when placing the robot on the starting position in auto)
+     * <b>Units:</b> Degrees
+     */
+    public static final SN_DoublePreference rotationalAutoPlacementTolerance = new SN_DoublePreference(
+        "translationalAutoPlacementTolerance", 2);
   }
 
 }

src/main/java/frc/robot/subsystems/LEDs.java

@@ -27,12 +27,27 @@ public void configure() {
     CANdle.configBrightnessScalar(constLEDs.LED_BRIGHTNESS);
   }
 
+  /**
+   * Set all the LEDs to a single color
+   * 
+   * @param rgb How much red, green, and blue the color has
+   */
   public void setLEDs(int[] rgb) {
     CANdle.setLEDs(rgb[0], rgb[1], rgb[2]);
   }
 
   /**
-   * Types of Animations available:
+   * Sets a certain LED to a desired color
+   * 
+   * @param rgb      How much red, green, and blue the color has
+   * @param LEDIndex The index number of the specific LED to control
+   */
+  public void setIndividualLED(int[] rgb, int LEDIndex) {
+    CANdle.setLEDs(rgb[0], rgb[1], rgb[2], 0, LEDIndex, 1);
+  }
+
+  /**
+   * <b>Types of Animations available:</b>
    * ColorFlowAnimation, FireAnimation, LarsonAnimation, RainbowAnimation,
    * RgbFadeAnimation, SingleFadeAnimation, StrobeAnimation, TwinkleAnimation,
    * TwinkleOffAnimation
@@ -43,6 +58,13 @@ public void setLEDsToAnimation(Animation animation) {
     CANdle.animate(animation, 0);
   }
 
+  public void setLEDBrightness(double brightness) {
+    CANdle.configBrightnessScalar(brightness);
+  }
+
+  /**
+   * Clears the LEDs of the current animation or color
+   */
   public void clearAnimation() {
     CANdle.clearAnimation(0);
     CANdle.setLEDs(0, 0, 0);