Merge pull request #3 from team5675/GVSULeds

Gvsu leds

Author: JacobMazelin

src/main/deploy/pathplanner/autos/LB 3 Coral.auto

@@ -168,6 +168,12 @@
             ]
           }
         },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.25
+          }
+        },
         {
           "type": "parallel",
           "data": {

src/main/deploy/pathplanner/autos/RB 3 Coral.auto

@@ -168,6 +168,12 @@
             ]
           }
         },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.25
+          }
+        },
         {
           "type": "parallel",
           "data": {

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

@@ -10,6 +10,8 @@
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
@@ -20,7 +22,7 @@ public class Robot extends TimedRobot {
 
   public Robot() {
     m_robotContainer = new RobotContainer();
-    CameraServer.startAutomaticCapture();
+    // CameraServer.startAutomaticCapture();
     PathfindingCommand.warmupCommand().schedule();
 
     //SignalLogger.start();
@@ -35,10 +37,13 @@ public Robot() {
   @Override
   public void robotPeriodic() {
     CommandScheduler.getInstance().run(); 
+    SmartDashboard.putNumber("Game Timer", Timer.getFPGATimestamp());
   }
 
   @Override
-  public void disabledInit() {}
+  public void disabledInit() {
+
+  }
 
   @Override
   public void disabledPeriodic() {}
@@ -53,13 +58,17 @@ public void autonomousInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.schedule();
     }
+
+    
   }
 
   @Override
   public void autonomousPeriodic() {}
 
   @Override
-  public void autonomousExit() {}
+  public void autonomousExit() {
+
+  }
 
   @Override
   public void teleopInit() {

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

@@ -1,7 +1,3 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
 package frc.robot;
 
 import static edu.wpi.first.units.Units.*;
@@ -21,6 +17,9 @@
 import frc.robot.subsystems.Algae.AlgaeInCommand;
 import frc.robot.subsystems.Algae.AlgaeOutCommand;
 import frc.robot.subsystems.Elevator.RunElevatorCommand;
+import frc.robot.subsystems.LED.LEDStateManager;
+import frc.robot.subsystems.LED.SetLEDAnimationCommand;
+import frc.robot.subsystems.LED.CustomAnimations.RainbowShootingLines;
 import frc.robot.subsystems.Climber.SetClimbCommand;
 import frc.robot.subsystems.Coral.Coral;
 import frc.robot.subsystems.Coral.IntakeCommand;
@@ -91,6 +90,7 @@ public static CommandXboxController getDriverController() {
     public Command pathfindingCommand;
 
     public RobotContainer() {
+        // LEDStateManager.getInstance().setDefault();
 
         NamedCommands.registerCommand("IntakeCommand", new AutoIntakeCommand());
         NamedCommands.registerCommand("PlaceCommand", new PlaceCommand());
@@ -197,6 +197,20 @@ private void configureBindings() {
         level4.onTrue(new RunElevatorCommand(ElevatorLevel.L4_HEIGHT));
         ElevatorReset.onTrue(new RunElevatorCommand(ElevatorLevel.RESET_HEIGHT));
 
+        // getDriverController().a().onTrue(new SetLEDAnimationCommand(
+        //     new RainbowShootingLines(
+        //         RainbowShootingLines.RainbowType.PASTEL_RAINBOW, 
+        //         RainbowShootingLines.ColorDistribution.PER_LINE, 
+        //         RainbowShootingLines.DirectionType.FORWARD,
+        //         10, 
+        //         0, 
+        //         0, 
+        //         1, 
+        //         0, 
+        //         true
+        //     )
+        // ));
+
         drivetrain.registerTelemetry(logger::telemeterize);
     }
 

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

@@ -116,7 +116,7 @@ public void setAxisPosition(double position) {
 
   //spins the wheels based on input speed
   public void setFlywheelSpeed(double speed) {
-    System.out.println("Spinning Wheel!");
+    // System.out.println("Spinning Wheel!");
     wheelsMotor.set(speed);
   }
 

src/main/java/frc/robot/subsystems/Algae/AlgaeInCommand.java

@@ -4,7 +4,12 @@
 
 package frc.robot.subsystems.Algae;
 
+import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.LED.RGB;
+import frc.robot.subsystems.LED.SetLEDAnimationCommand;
+import frc.robot.subsystems.LED.CustomAnimations.Pulse;
+import frc.robot.subsystems.LED.CustomAnimations.SolidColor;
 
 /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
 public class AlgaeInCommand extends Command {
@@ -21,12 +26,20 @@ public AlgaeInCommand() {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
+    new SetLEDAnimationCommand(
+      new Pulse(
+        new RGB(Color.kSeaGreen), 
+        0.5, 
+        1, 
+        0.2
+      )
+    ).schedule();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    System.out.println("Algae in");
+    // System.out.println("Algae in");
     algae.setIntake(true);
     if (-algae.axisTicks.getPosition() < AlgaeConstants.AxisOutTicks) {
       algae.setAxisPosition(AlgaeConstants.AxisOutTicks);
@@ -43,6 +56,12 @@ public void execute() {
   public void end(boolean interrupted) {
     algae.setFlywheelSpeed(0);
     algae.setIntake(false);
+
+    new SetLEDAnimationCommand(
+      new SolidColor(
+          new RGB(Color.kSeaGreen)
+      )
+    ).schedule();
   }
 
   // Returns true when the command should end.

src/main/java/frc/robot/subsystems/Algae/AlgaeOutCommand.java

@@ -4,7 +4,14 @@
 
 package frc.robot.subsystems.Algae;
 
+import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.LED.LEDStateManager;
+import frc.robot.subsystems.LED.RGB;
+import frc.robot.subsystems.LED.SetLEDAnimationCommand;
+import frc.robot.subsystems.LED.CustomAnimations.Pulse;
+import frc.robot.subsystems.LED.CustomAnimations.ShootingLines;
+import frc.robot.subsystems.LED.CustomAnimations.SolidColor;
 
 /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
 public class AlgaeOutCommand extends Command {
@@ -21,12 +28,21 @@ public AlgaeOutCommand() {
   @Override
   public void initialize() {
     algae.setIntake(true);
+
+    new SetLEDAnimationCommand(
+      new Pulse(
+        new RGB(Color.kSeaGreen), 
+        0.5, 
+        1, 
+        0.2
+      )
+    ).schedule();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    System.out.println("Algae out");
+    // System.out.println("Algae out");
     algae.setFlywheelSpeed(-1);
   }
 
@@ -35,6 +51,8 @@ public void execute() {
   public void end(boolean interrupted) {
     algae.setIntake(false);
     algae.setFlywheelSpeed(0);
+
+    LEDStateManager.getInstance().setDefault();
   }
 
   // Returns true when the command should end.

src/main/java/frc/robot/subsystems/Climber/ClimbCommand.java

@@ -1,6 +1,12 @@
 package frc.robot.subsystems.Climber;
 
+import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.LED.LEDStateManager;
+import frc.robot.subsystems.LED.RGB;
+import frc.robot.subsystems.LED.SetLEDAnimationCommand;
+import frc.robot.subsystems.LED.CustomAnimations.Glitch;
+import frc.robot.subsystems.LED.CustomAnimations.RainbowPulse;
 
 public class ClimbCommand extends Command {
     private final Climber climber;
@@ -12,7 +18,7 @@ public ClimbCommand(Climber climber) {
 
     @Override
     public void initialize() {
-        System.out.println("Climbing Up...");
+        // System.out.println("Climbing Up...");
         climber.climberMotor.set(0.6);
     }
 
@@ -31,7 +37,12 @@ public boolean isFinished() {
     @Override
     public void end(boolean interrupted) {
         climber.climberMotor.set(0);
-        System.out.println("Climb Complete.");
+        // System.out.println("Climb Complete.");
+
+        new SetLEDAnimationCommand(
+            LEDStateManager.getInstance().STARTING_SHOOTING_LINES
+        ).schedule();
+
         //To Stay up in the air if needed
         //climber.climberMotor.set(0.1);
     }

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

@@ -51,7 +51,7 @@ public Climber() {
 
         ClimberForwardlimit = new DigitalInput(ClimberConstants.ClimbLimitChannel);
         SetLimitSwitch = new DigitalInput(ClimberConstants.SetClimbLimitChannel);
-        isClimbLimitSwitchTripped = new Trigger(SetLimitSwitch::get);
+        isClimbLimitSwitchTripped = new Trigger(SetLimitSwitch::get); 
         isSetLimitSwitchTripped = new Trigger(ClimberForwardlimit::get);
     }
 

src/main/java/frc/robot/subsystems/Climber/CloseClawCommand.java

@@ -11,7 +11,7 @@ public CloseClawCommand(Climber climber) {
 
     @Override
     public void initialize() {
-        System.out.println("Closing Claw...");
+        // System.out.println("Closing Claw...");
         climber.clawMotor.setVoltage(ClimberConstants.closeClaw);
     }
 
@@ -22,6 +22,6 @@ public boolean isFinished() {
 
     @Override
     public void end(boolean interrupted) {
-        System.out.println("Claw Closed.");
+        // System.out.println("Claw Closed.");
     }
 }