From 69f62aa0534407fb755de133fa385c09255281dc Mon Sep 17 00:00:00 2001
From: cooking-hambrops <ethan.soh360@gmail.com>
Date: Sat, 4 Mar 2023 15:01:50 -0500
Subject: [PATCH] (DO NOT MERGE) Code Review: Bling Strobe + Battery

---
 .../frc/robot/commands/SetBlingCommand.java   | 60 ++++++++++++++++++-
 .../frc/robot/subsystems/BlingSubsystem.java  |  5 ++
 2 files changed, 62 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/commands/SetBlingCommand.java b/src/main/java/frc/robot/commands/SetBlingCommand.java
index b1dda14..6fa3a0e 100644
--- a/src/main/java/frc/robot/commands/SetBlingCommand.java
+++ b/src/main/java/frc/robot/commands/SetBlingCommand.java
@@ -4,6 +4,8 @@
 
 package frc.robot.commands;
 
+import edu.wpi.first.wpilibj.PowerDistribution;
+import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.subsystems.BlingSubsystem;
 import com.ctre.phoenix.led.FireAnimation;
@@ -69,6 +71,21 @@ public void initialize() {
         case 9:
           setRgbFade();
           break;
+        case 10: //TODO: for testing purposes, may add to robot
+          bling.setOrange(); //Special set (only a certain amount of LEDs light up)
+          break; //remove break?
+        case 11:
+          setBlueStrobe();
+          break;
+        case 12: 
+          setYellowStrobe();
+          break;
+        case 13: 
+          setRedStrobe();
+          break;
+        case 14: 
+          setPurpleStrobe();
+          break;
         default:
           break;
       }
@@ -89,14 +106,35 @@ public void setRainbow() {
 
   public void setFire()
   {
-    FireAnimation fireAnimation = new FireAnimation(1, 0.000001, 64, 0.8, 0.4);
+    FireAnimation fireAnimation = new FireAnimation(1, 0.000001, 64, 3, 1);
 
     bling.candle.animate(fireAnimation);
   }
 
-  public void setStrobe()
+  public void setPurpleStrobe()
   {
-    StrobeAnimation strobeAnimation = new StrobeAnimation(255, 255, 255, 255, 0.8, 64);
+    StrobeAnimation strobeAnimation = new StrobeAnimation(138, 43, 226, 127, 0.001, 64); //TODO: test all of the rgbw bling values
+
+    bling.candle.animate(strobeAnimation);
+  }
+
+  public void setRedStrobe()
+  {
+    StrobeAnimation strobeAnimation = new StrobeAnimation(255, 0, 0, 127, 0.001, 64);
+
+    bling.candle.animate(strobeAnimation);
+  }
+
+  public void setBlueStrobe()
+  {
+    StrobeAnimation strobeAnimation = new StrobeAnimation(0, 0, 255, 127, 0.001, 64);
+
+    bling.candle.animate(strobeAnimation);
+  }
+
+  public void setYellowStrobe()
+  {
+    StrobeAnimation strobeAnimation = new StrobeAnimation(255, 255, 0, 127, 0.001, 64);
 
     bling.candle.animate(strobeAnimation);
   }
@@ -108,5 +146,21 @@ public void setRgbFade()
     bling.candle.animate(rgbFadeAnimation);
   }
 
+  PowerDistribution pd = new PowerDistribution(0, ModuleType.kCTRE);
+
+  boolean checkBatteryVoltageConstantly = true;
+
+  // Get the voltage going into the PDP, in Volts.
+  // The PDP returns the voltage in increments of 0.05 Volts.
+  double voltage = PowerDistribution.ModuleType.class.m_pdp.getVoltage();
+  SmartDashboard.putNumber("Voltage", voltage);
+
+  while (checkBatteryVoltageConstantly = true) {
+    if (voltage <= 12.50); { //TODO:  Using 12.50 Volts for now, may change later 
+      bling.setOrange(); //Test to see if this replaces a current bling colour or not
+      checkBatteryVoltageConstantly = false; //TODO: Check if its ok that: putting this basically means it will be low voltage once below or equal to 12.50 V all the way until the robot is turned off (because then the battery would be changed)
+    }
+  }
+
 }
 
diff --git a/src/main/java/frc/robot/subsystems/BlingSubsystem.java b/src/main/java/frc/robot/subsystems/BlingSubsystem.java
index b50f52b..c0d2159 100644
--- a/src/main/java/frc/robot/subsystems/BlingSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/BlingSubsystem.java
@@ -87,6 +87,11 @@ public void setYellow()
     candle.setLEDs(255, 255, 0);
   }
 
+  public void setOrange()
+  {
+    candle.setLEDs(255, 165, 0, 127, 1, 10); //test to see how many leds this is and if it works cuz the white is 127
+  }
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run