feat: epic Led effects

simgui-ds.json

@@ -98,6 +98,9 @@
     {
       "guid": "78696e70757401000000000000000000",
       "useGamepad": true
+    },
+    {
+      "useGamepad": true
     }
   ]
 }

simgui.json

@@ -1,6 +1,10 @@
 {
   "HALProvider": {
     "Addressable LEDs": {
+      "0": {
+        "columns": 1,
+        "start": 1
+      },
       "window": {
         "visible": true
       }
@@ -9,6 +13,37 @@
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",
+      "/LiveWindow/DriveSubsystem": "Subsystem",
+      "/LiveWindow/Ungrouped/DigitalInput[8]": "Digital Input",
+      "/LiveWindow/Ungrouped/DigitalInput[9]": "Digital Input",
+      "/LiveWindow/Ungrouped/PIDController[10]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[11]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[12]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[13]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[14]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[15]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[16]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[17]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[18]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[19]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[1]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[20]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[21]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[22]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[23]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[24]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[25]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[26]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[2]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[3]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[4]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[5]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[6]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[7]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[8]": "PIDController",
+      "/LiveWindow/Ungrouped/PIDController[9]": "PIDController",
+      "/LiveWindow/Ungrouped/Scheduler": "Scheduler",
+      "/LiveWindow/Ungrouped/navX-Sensor[4]": "Gyro",
       "/SmartDashboard/Field": "Field2d",
       "/SmartDashboard/SendableChooser[0]": "String Chooser",
       "/SmartDashboard/SendableChooser[1]": "String Chooser"
@@ -52,8 +87,5 @@
         "open": true
       }
     }
-  },
-  "NetworkTables View": {
-    "visible": false
   }
 }

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

@@ -109,9 +109,9 @@ public static final class DriveConstants {
         new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
 
     public static final double kMaxSpeedMetersPerSecond = 3.6576;
-    public static final double kMaxAngularSpeedRadiansPerSecond = 15.24/3;
+    public static final double kMaxAngularSpeedRadiansPerSecond = 15.24 / 3;
 
-    public static final double kTurningStopTime = 0.2; // TODO: tune heading correction stop time
+    public static final double kTurningStopTime = 0.2;
     public static final double kSpeedIncreasePerPeriod = 0.15;
 
     public static final double kPSnapRotate = 6;
@@ -145,6 +145,15 @@ public static final class GrabberConstants {
   public static final class LEDConstants {
     public static final int kLEDPort = 0;
     public static final int kLEDLength = 28;
+
+    public static final double kSwipeTime = 0.02;
+    public static final double kLEDOnTime = 0.3;
+
+    public static final int kBlinkAmount = 3;
+    public static final double kBlinkTime = 0.2;
+
+    public static final double kTipMin = 5;
+    public static final double kTipMax = 55;
   }
 
   public static final class OIConstants {
@@ -168,7 +177,7 @@ public static final class VisionConstants {
     public static final Transform3d kCameraOffset = new Transform3d(new Translation3d(0.5, 0, 0.5),
         new Rotation3d(0, 0, 0));
     // Currently set as cam mounted facing forward, half a meter forward of center,
-    // half a meter up from center. // TODO adjust vision offset values
+    // half a meter up from center.
 
     public static final AprilTagFieldLayout kAprilTagFieldLayout = loadFieldLayout();
 

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

@@ -19,6 +19,8 @@
  */
 public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
+  private Command m_startupCommand;
+  private Command m_idleCommand;
 
   private RobotContainer m_robotContainer;
 
@@ -28,6 +30,13 @@ public class Robot extends TimedRobot {
   public void robotInit() {
     m_robotContainer = new RobotContainer();
     m_buttonTimer.start();
+
+    m_startupCommand = m_robotContainer.getStartupCommand();
+    m_idleCommand = m_robotContainer.getIdleCommand();
+
+    if (m_startupCommand != null) {
+      m_startupCommand.schedule();
+    }
   }
 
   @Override
@@ -37,6 +46,9 @@ public void robotPeriodic() {
 
   @Override
   public void disabledInit() {
+    if (!m_startupCommand.isScheduled() && m_idleCommand != null) {
+      m_idleCommand.schedule();
+    }
   }
 
   @Override
@@ -56,6 +68,14 @@ public void autonomousInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.schedule();
     }
+
+    if (m_startupCommand != null) {
+      m_startupCommand.cancel();
+    }
+
+    if (m_idleCommand != null) {
+      m_idleCommand.cancel();
+    }
   }
 
   @Override
@@ -67,6 +87,14 @@ public void teleopInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
+
+    if (m_startupCommand != null) {
+      m_startupCommand.cancel();
+    }
+
+    if (m_idleCommand != null) {
+      m_idleCommand.cancel();
+    }
   }
 
   @Override

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

@@ -12,6 +12,7 @@
 import com.pathplanner.lib.auto.SwerveAutoBuilder;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.XboxController.Button;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
@@ -23,17 +24,27 @@
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.POVButton;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.ArmConstants;
 import frc.robot.Constants.AutonConstants;
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.OIConstants;
 import frc.robot.commands.ArmCommand;
 import frc.robot.commands.BalanceCommand;
+import frc.robot.commands.LEDBlinkCommand;
+import frc.robot.commands.LEDBlinkCommand.BlinkType;
+import frc.robot.commands.LEDCountdownCommand;
+import frc.robot.commands.LEDDefaultCommand;
+import frc.robot.commands.LEDRainbowCommand;
+import frc.robot.commands.LEDSwipeCommand;
+import frc.robot.commands.LEDSwipeCommand.SwipeType;
+import frc.robot.commands.LEDTipCommand;
 import frc.robot.commands.SnapRotateCommand;
 import frc.robot.subsystems.ArmSubsystem;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.GrabberSubsystem;
 import frc.robot.subsystems.LEDSubsystem;
+import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
 
 /*
  * This class is where the bulk of the robot should be declared.  Since Command-based is a
@@ -51,6 +62,8 @@ public class RobotContainer {
   private final XboxController m_operatorController = new XboxController(OIConstants.kOperatorControllerPort);
   private final SendableChooser<String> m_chooser = new SendableChooser<>();
 
+  private boolean m_lockLED = false;
+
   private final HashMap<String, Command> m_eventMap = new HashMap<>();
   private final SwerveAutoBuilder m_autoBuilder = new SwerveAutoBuilder(
       m_robotDrive::getPose,
@@ -110,6 +123,8 @@ public RobotContainer() {
                 !m_driverController.getRightBumper()),
             m_robotDrive));
 
+    m_LEDSubsystem.setDefaultCommand(new LEDDefaultCommand(m_LEDSubsystem, () -> m_lockLED));
+
     m_chooser.addOption("Far", "Far");
     m_chooser.addOption("Charger", "Charger");
     m_chooser.addOption("Charger-comms", "Charger-comms");
@@ -132,7 +147,7 @@ public RobotContainer() {
             new InstantCommand(grabberSubsystem::toggle, grabberSubsystem),
             new WaitCommand(0.5)));
 
-    m_eventMap.put("Balance", new BalanceCommand(m_robotDrive));
+    m_eventMap.put("Balance", new BalanceCommand(m_robotDrive, m_LEDSubsystem));
   }
 
   /**
@@ -149,7 +164,7 @@ private void configureButtonBindings() {
         .onTrue(new InstantCommand(m_robotDrive::zeroHeading, m_robotDrive));
 
     new JoystickButton(m_driverController, Button.kY.value)
-        .whileTrue(new BalanceCommand(m_robotDrive));
+        .whileTrue(new BalanceCommand(m_robotDrive, m_LEDSubsystem));
     new JoystickButton(m_driverController, Button.kA.value)
         .onTrue(new SnapRotateCommand(m_robotDrive));
 
@@ -166,9 +181,15 @@ private void configureButtonBindings() {
         .onTrue(new ArmCommand(m_armSubsystem, 34, ArmConstants.kElevatorMinPosition));
 
     new JoystickButton(m_operatorController, Button.kLeftBumper.value)
-        .onTrue(new InstantCommand(() -> m_LEDSubsystem.setLED(50, 50, 0))); // Yellow
+        .whileTrue(new RunCommand(() -> m_LEDSubsystem.setCone(), m_LEDSubsystem))
+.onFalse(new SequentialCommandGroup(new InstantCommand(() -> {m_lockLED = true;}, m_LEDSubsystem), new WaitCommand(10), new InstantCommand(() -> {m_lockLED = false;}, m_LEDSubsystem)));
     new JoystickButton(m_operatorController, Button.kRightBumper.value)
-        .onTrue(new InstantCommand(() -> m_LEDSubsystem.setLED(27, 8, 44))); // Purple
+        .whileTrue(new RunCommand(() -> m_LEDSubsystem.setCube(), m_LEDSubsystem))
+.onFalse(new SequentialCommandGroup(new InstantCommand(() -> {m_lockLED = true;}, m_LEDSubsystem), new WaitCommand(10), new InstantCommand(() -> {m_lockLED = false;}, m_LEDSubsystem)));
+
+    new Trigger(() -> m_robotDrive.isTipped() && !m_lockLED).whileTrue(new LEDTipCommand(m_LEDSubsystem, m_robotDrive.getGyro()));
+    new Trigger(() -> DriverStation.getMatchTime() < 10 && DriverStation.isTeleop() && !m_lockLED)
+        .whileTrue(new LEDCountdownCommand(m_LEDSubsystem));
   }
 
   /**
@@ -190,4 +211,28 @@ public Command getAutonomousCommand() {
                 AutonConstants.maxVelocity - (path.equals("Charger-comms") ? 0.75 : 0),
                 AutonConstants.maxAcceleration)));
   }
+
+  /**
+   * Returns a command that does LED startup effects.
+   * 
+   * @return Startup command.
+   */
+  public Command getStartupCommand() {
+    if (m_lockLED) return new SequentialCommandGroup();
+    return new SequentialCommandGroup(
+        new LEDSwipeCommand(m_LEDSubsystem, SwipeType.kUp, 0, 0, 100, true),
+        new LEDBlinkCommand(m_LEDSubsystem, BlinkType.kBlink, 0, 0, 0),
+        new WaitCommand(5),
+        new LEDRainbowCommand(m_LEDSubsystem));
+  }
+
+  /**
+   * Returns RGB idle command.
+   * 
+   * @return RGB idle command.
+   */
+  public Command getIdleCommand() {
+    if (m_lockLED) return new SequentialCommandGroup();
+    return new LEDRainbowCommand(m_LEDSubsystem);
+  }
 }

src/main/java/frc/robot/commands/BalanceCommand.java

@@ -5,43 +5,68 @@
 package frc.robot.commands;
 
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.Constants.DriveConstants;
 import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.LEDSubsystem;
 
 /** Uses a PID and the gyroscope to balance the robot on the charger. */
 public class BalanceCommand extends CommandBase {
-  private final DriveSubsystem m_subsystem;
+  private final DriveSubsystem m_driveSubsystem;
+  private final LEDSubsystem m_LEDSubsystem;
+
   private final PIDController m_PID = new PIDController(0.025, 0, 0);
+  private final Timer m_timer = new Timer();
+
+  private boolean m_isRed;
+  private int m_fade;
 
   /**
    * Creates a new {@link BalanceCommand}.
    * 
-   * @param subsystem The required subsystem.
+   * @param driveSubsystem The required drive subsystem.
+   * @param LEDSubsystem   The required LED subsystem.
    */
-  public BalanceCommand(DriveSubsystem subsystem) {
-    m_subsystem = subsystem;
-    addRequirements(m_subsystem);
+  public BalanceCommand(DriveSubsystem driveSubsystem, LEDSubsystem LEDSubsystem) {
+    m_driveSubsystem = driveSubsystem;
+    m_LEDSubsystem = LEDSubsystem;
+    addRequirements(m_driveSubsystem, m_LEDSubsystem);
 
     m_PID.setTolerance(DriveConstants.kToleranceBalance);
   }
 
+  @Override
+  public void initialize() {
+    m_timer.restart();
+    m_fade = 100;
+    m_isRed = true;
+    m_LEDSubsystem.setLED(100, 0, 0);
+  }
+
   @Override
   public void execute() {
-    m_subsystem.drive(
-        m_PID.calculate(m_subsystem.getGyroPitch(), 0),
+    m_driveSubsystem.drive(
+        m_PID.calculate(m_driveSubsystem.getGyroPitch(), 0),
         0,
         0,
         false);
-  }
 
-  @Override
-  public void end(boolean interrupted) {
-    m_subsystem.drive(0, 0, 0, false);
+    if (m_PID.atSetpoint()) {
+      m_LEDSubsystem.setLED(0, m_fade, 0, false);
+      m_fade = m_fade <= 0 ? 100 : m_fade - 2;
+      m_isRed = false;
+    } else if (m_timer.hasElapsed(0.3)) {
+      m_fade = 100;
+      m_LEDSubsystem.setLED(m_isRed ? 0 : 100, 0, 0, false);
+      m_isRed = !m_isRed;
+      m_timer.reset();
+    }
   }
 
   @Override
-  public boolean isFinished() {
-    return false;
+  public void end(boolean interrupted) {
+    m_driveSubsystem.drive(0, 0, 0, false);
+    m_LEDSubsystem.unsetLED();
   }
 }