From 3c672f27bfef3a3bcbe134d325ae1f05cf0b42af Mon Sep 17 00:00:00 2001
From: mau <maudiaz0109@gmail.com>
Date: Wed, 15 Mar 2023 16:23:02 -0500
Subject: [PATCH] added rainbow speed effect and spike blinking effect

---
 src/main/cpp/subsystems/Drivetrain.cpp   | 7 ++++++-
 src/main/cpp/subsystems/Intake.cpp       | 5 ++++-
 src/main/include/subsystems/Drivetrain.h | 3 +++
 src/main/include/subsystems/Intake.h     | 3 +++
 4 files changed, 16 insertions(+), 2 deletions(-)

diff --git a/src/main/cpp/subsystems/Drivetrain.cpp b/src/main/cpp/subsystems/Drivetrain.cpp
index 633f4e1a..bcd094c1 100644
--- a/src/main/cpp/subsystems/Drivetrain.cpp
+++ b/src/main/cpp/subsystems/Drivetrain.cpp
@@ -77,7 +77,8 @@ Drivetrain::Drivetrain(frc::TimedRobot *_robot) : ValorSubsystem(_robot, "Drivet
                         estimator(NULL),
                         config(NULL),
                         thetaController{KPT, KIT, KDT, frc::ProfiledPIDController<units::radians>::Constraints(units::angular_velocity::radians_per_second_t{rotMaxSpeed}, units::angular_acceleration::radians_per_second_squared_t{rotMaxAccel})},
-                        swerveNoError(true)
+                        swerveNoError(true),
+                        candle(robot, 286, 60, "baseCAN")
 {
     frc2::CommandScheduler::GetInstance().RegisterSubsystem(this);
     init();
@@ -316,6 +317,10 @@ void Drivetrain::assignOutputs()
         limeTable->PutNumber("pipeline", LimelightPipes::APRIL_TAGS);    
         drive(state.xSpeedMPS, state.ySpeedMPS, state.rotRPS, true);
     }
+
+    if((fabs(state.xSpeed) + fabs(state.ySpeed))/2.0 > 0.8){
+        candle.setAnimation(candle.Rainbow);
+    }
 }
 
 void Drivetrain::pullSwerveModuleZeroReference(){
diff --git a/src/main/cpp/subsystems/Intake.cpp b/src/main/cpp/subsystems/Intake.cpp
index 1f233f57..af051d17 100644
--- a/src/main/cpp/subsystems/Intake.cpp
+++ b/src/main/cpp/subsystems/Intake.cpp
@@ -22,7 +22,8 @@
 
 Intake::Intake(frc::TimedRobot *_robot) : ValorSubsystem(_robot, "Intake"),  
                             intakeMotor(CANIDs::INTAKE_LEAD_CAN, ValorNeutralMode::Coast, false, "baseCAN"),
-                            currentSensor(_robot, subsystemName)
+                            currentSensor(_robot, subsystemName),
+                            candle(robot, 286, CANIDs::CANDLE, "baseCAN")
 
 {  
     frc2::CommandScheduler::GetInstance().RegisterSubsystem(this);
@@ -180,6 +181,8 @@ void Intake::assignOutputs()
             state.isCubeStall = false;
         }
     }
+
+    state.intakeState == SPIKED?candle.setAnimation(candle.Strobe):candle.clearAnimation();
     prevState = state;
 }
 
diff --git a/src/main/include/subsystems/Drivetrain.h b/src/main/include/subsystems/Drivetrain.h
index c13d17b9..63c680d2 100644
--- a/src/main/include/subsystems/Drivetrain.h
+++ b/src/main/include/subsystems/Drivetrain.h
@@ -14,6 +14,7 @@
 #include "controllers/ValorFalconController.h"
 #include "controllers/ValorNeoController.h"
 #include "controllers/ValorPIDF.h"
+#include "sensors/ValorCANdleSensor.h"
 
 #include "AHRS.h"
 #include "ctre/phoenix/sensors/WPI_Pigeon2.h"
@@ -285,4 +286,6 @@ class Drivetrain : public ValorSubsystem
      std::shared_ptr<nt::NetworkTable> limeTable;
 
      bool swerveNoError;
+
+     ValorCANdleSensor candle;
 };
diff --git a/src/main/include/subsystems/Intake.h b/src/main/include/subsystems/Intake.h
index f7d496d6..6038f470 100644
--- a/src/main/include/subsystems/Intake.h
+++ b/src/main/include/subsystems/Intake.h
@@ -16,6 +16,7 @@
 #include "subsystems/Direction.h"
 #include "subsystems/Position.h"
 #include "subsystems/Piece.h"
+#include "sensors/ValorCANdleSensor.h"
 
 #include <frc/smartdashboard/SendableChooser.h>
 #include <frc/smartdashboard/SmartDashboard.h>
@@ -128,4 +129,6 @@ class Intake : public ValorSubsystem
      ValorFalconController intakeMotor;
 
      ValorCurrentSensor currentSensor;
+
+     ValorCANdleSensor candle;
 };
\ No newline at end of file