added rainbow speed effect and spike blinking effect

Valor6800 · Mar 21, 2023 · 3c672f2 · 3c672f2
1 parent fbadedb
commit 3c672f2
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 2 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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;
 };