Shuffle board poc

.gitignore

@@ -105,3 +105,7 @@ gradle-app.setting
 # End of https://www.gitignore.io/api/gradle,intellij+all
 
 imgui.ini
+.settings/org.eclipse.buildship.core.prefs
+.settings/org.eclipse.jdt.core.prefs
+.project
+.classpath

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

@@ -1,7 +1,11 @@
 package frc.robot;
 
+import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.commands.Test;
+import frc.robot.led.LED;
+import frc.robot.led.LEDComponentsImpl;
 import frc.robot.robotControl.DeputyOi;
 import frc.robot.robotControl.DriverOi;
 import frc.robot.subsystems.driveTrain.DriveTrain;
@@ -10,6 +14,7 @@
 import frc.robot.subsystems.driveTrain.features.KeepAngleController;
 import frc.robot.subsystems.driveTrain.features.PoseEstimator;
 import frc.robot.subsystems.driveTrain.features.SwerveModule;
+import sensors.Switch.Switch;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -20,6 +25,9 @@
 public class Robot extends TimedRobot {
     private DriverOi driverOi;
     private DeputyOi deputyOi;
+    private DigitalInput toplimitSwitch;
+
+
 
     /**
      * This function is run when the robot is first started up and should be used for any
@@ -28,10 +36,12 @@ public class Robot extends TimedRobot {
     @Override
     public void robotInit() {
         DriveTrain.init(new DriveTrainComponentsImpl());
+        LED.init(new LEDComponentsImpl());
         PoseEstimator.init();
         KeepAngleController.init();
         driverOi = new DriverOi();
         deputyOi = new DeputyOi();
+        toplimitSwitch = new DigitalInput(9);
     }
 
     /**
@@ -59,6 +69,11 @@ public void disabledInit() {
 
     @Override
     public void disabledPeriodic() {
+        if(! toplimitSwitch.get()){
+            LED.getInstance().setStrip(0,255,0);
+        }else {
+            LED.getInstance().setStrip(255,0,0);
+        }
     }
 
     /**

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

@@ -0,0 +1,22 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.led.LED;
+
+public class Test extends CommandBase {
+    private DigitalInput toplimitSwitch;
+
+    public Test() {
+        toplimitSwitch = new DigitalInput(9);
+    }
+
+    @Override
+    public void execute() {
+        if(! toplimitSwitch.get()){
+            LED.getInstance().setStrip(0,255,0);
+        }else {
+            LED.getInstance().setStrip(255,0,0);
+        }
+    }
+}

src/main/java/frc/robot/commands/led/Blink.java

@@ -0,0 +1,38 @@
+package frc.robot.commands.led;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.robot.led.LED;
+
+import static frc.robot.led.LEDConstants.BLACK;
+
+public class Blink extends SequentialCommandGroup {
+    private int ranTimes = 0;
+
+    public Blink(int red, int green, int blue, double blinkDelays, int timesToBlink) {
+        addCommands
+                (
+                        new SequentialCommandGroup(
+                                new BlinkOnce(red, green, blue, blinkDelays),
+                                new InstantCommand(() -> ranTimes++)
+                        ).repeatedly().until(() -> ranTimes == timesToBlink)
+                );
+    }
+
+    private class BlinkOnce extends SequentialCommandGroup {
+
+        private LED led;
+
+        public BlinkOnce(int red, int green, int blue, double blinkDelays) {
+            led = LED.getInstance();
+            addRequirements(led);
+            addCommands(
+                    new InstantCommand(() -> led.setStrip(red, green, blue)),
+                    new WaitCommand(blinkDelays),
+                    new InstantCommand(() -> led.setStrip(BLACK.getRed(), BLACK.getGreen(), BLACK.getBlue())),
+                    new WaitCommand(blinkDelays)
+            );
+        }
+    }
+}

src/main/java/frc/robot/commands/led/Gaming.java

@@ -0,0 +1,26 @@
+package frc.robot.commands.led;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.led.LED;
+
+public class Gaming extends CommandBase {
+
+    private LED led;
+    private int jumps;
+
+    public Gaming(int jumps) {
+        this.led = LED.getInstance();
+        this.jumps = jumps;
+        addRequirements(led);
+    }
+
+    @Override
+    public void execute() {
+        led.setGaming(jumps);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        led.setStripOff();
+    }
+}

src/main/java/frc/robot/led/LED.java

@@ -0,0 +1,81 @@
+package frc.robot.led;
+
+import static frc.robot.led.LEDConstants.BLACK;
+import static frc.robot.led.LEDConstants.MAX_HUE;
+import static frc.robot.led.LEDConstants.RAINBOW_INDICATOR;
+import static frc.robot.led.LEDConstants.RAINBOW_SATURATION;
+import static frc.robot.led.LEDConstants.RAINBOW_VALUE;
+import static frc.robot.led.LEDConstants.STRIP_LENGTH;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class LED extends SubsystemBase {
+
+    private int red;
+    private int green;
+    private int blue;
+
+    private final LEDComponents components;
+
+    private LED(LEDComponents components) {
+        this.components = components;
+    }
+
+    public String getCurrentColor() {
+        return "Red: " + red +
+                " Green: " + green +
+                " Blue: " + blue;
+    }
+
+    private void setOneLed(int ledIndex, int red, int green, int blue) {
+        components.getBuffer().setRGB(ledIndex, red, green, blue);
+    }
+
+    public void setStrip(int red, int green, int blue) {
+        for (int ledIndex = 0; ledIndex < STRIP_LENGTH; ++ledIndex)
+            setOneLed(ledIndex, red, green, blue);
+
+        this.red = red;
+        this.blue = blue;
+        this.green = green;
+
+        Update();
+    }
+
+    public void setStripOff() {
+        setStrip(BLACK.getRed(), BLACK.getGreen(), BLACK.getBlue());
+    }
+
+    public void setGaming(int ledJumps) {
+        int hue;
+        int ledOffset = 0;
+        for (int ledIndex = 0; ledIndex < STRIP_LENGTH; ++ledIndex) {
+            hue = (ledOffset + (ledIndex * MAX_HUE / STRIP_LENGTH)) % MAX_HUE;
+
+            components.getBuffer().setHSV(ledIndex, hue, RAINBOW_SATURATION, RAINBOW_VALUE);
+        }
+
+        ledOffset += ledJumps;
+        ledOffset %= MAX_HUE;
+
+        this.red = RAINBOW_INDICATOR.getRed();
+        this.green = RAINBOW_INDICATOR.getGreen();
+        this.blue = RAINBOW_INDICATOR.getBlue();
+
+        Update();
+    }
+
+    public void Update() {
+        components.getStrip().setData(components.getBuffer());
+    }
+
+    private static LED instance;
+
+    public static void init(LEDComponents components) {
+        instance = new LED(components);
+    }
+
+    public static LED getInstance() {
+        return instance;
+    }
+}

src/main/java/frc/robot/led/LEDComponents.java

@@ -0,0 +1,11 @@
+package frc.robot.led;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+
+public interface LEDComponents {
+
+    AddressableLED getStrip();
+
+    AddressableLEDBuffer getBuffer();
+}

src/main/java/frc/robot/led/LEDComponentsImpl.java

@@ -0,0 +1,34 @@
+package frc.robot.led;
+
+import static frc.robot.led.LEDConstants.PORT;
+import static frc.robot.led.LEDConstants.STRIP_LENGTH;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+
+public class LEDComponentsImpl implements LEDComponents {
+
+    private final AddressableLED Strip;
+    private final AddressableLEDBuffer Buffer;
+
+    public LEDComponentsImpl() {
+        Strip = new AddressableLED(PORT);
+        Buffer = new AddressableLEDBuffer(STRIP_LENGTH);
+
+
+        Strip.setLength(STRIP_LENGTH);
+
+        Strip.setData(Buffer);
+        Strip.start();
+    }
+
+    @Override
+    public AddressableLED getStrip() {
+        return Strip;
+    }
+
+    @Override
+    public AddressableLEDBuffer getBuffer() {
+        return Buffer;
+    }
+}

src/main/java/frc/robot/led/LEDConstants.java

@@ -0,0 +1,15 @@
+package frc.robot.led;
+
+import java.awt.Color;
+
+public class LEDConstants {
+
+    public static Color BLACK = new Color(0, 0, 0);
+    public static Color RAINBOW_INDICATOR = new Color(210, 52, 235);
+
+    public static final int STRIP_LENGTH = 144;
+    public static final int PORT = 9;
+    public static final int RAINBOW_SATURATION = 255;
+    public static final int RAINBOW_VALUE = 128;
+    public static final int MAX_HUE = 180;
+}

src/main/java/frc/robot/logging/DriveTrainShuffleBoard.java

@@ -1,20 +1,65 @@
 package frc.robot.logging;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.PrintCommand;
+import frc.robot.commands.driveTrain.MoveByDistance;
+import frc.robot.commands.led.Blink;
+import frc.robot.commands.led.Gaming;
 import frc.robot.subsystems.driveTrain.DriveTrain;
 import frc.robot.subsystems.driveTrain.DriveTrainComponents;
 import frc.robot.subsystems.driveTrain.features.PoseEstimator;
 import frc.robot.subsystems.driveTrain.features.SwerveModule;
+import frc.robot.led.LED;
+
+import java.util.function.DoubleSupplier;
+import java.util.function.IntSupplier;
+import java.util.function.LongSupplier;
 
 public class DriveTrainShuffleBoard {
 
-    public DriveTrainShuffleBoard(DriveTrain driveTrain, DriveTrainComponents components) {
+    private DriveTrain driveTrain;
+    private ShuffleboardTab tab;
+
+    private int red;
 
-        ShuffleboardTab tab = Shuffleboard.getTab("Swerve");
+    public DriveTrainShuffleBoard(DriveTrainComponents components) {
+        driveTrain = DriveTrain.getInstance();
         SwerveModule[] swerveModules = components.getSwerveModules();
-        tab.addString("Pose",()-> PoseEstimator.getInstance().getPose2d().toString());
-        tab.addNumber("CurrentModuleAngle", () -> swerveModules[0].getCurrentAbsoluteDeg());
-        tab.addNumber("cancoderAng", () -> swerveModules[0].getAbsEncDeg() - swerveModules[0].getAngleOffset());}
 
-}
+        tab = Shuffleboard.getTab("DriveTrain");
+        tab.addNumber("FL encoderUnits ", () -> swerveModules[0].getAbsEncDeg());
+//        tab.addNumber("FR encoderUnits ", () -> swerveModules[1].getAbsEncDeg());
+//        tab.addNumber("BL encoderUnits ", () -> swerveModules[2].getAbsEncDeg());
+//        tab.addNumber("BR encoderUnits ", () -> swerveModules[3].getAbsEncDeg());
+
+        tab.addNumber("FL CurrentModuleAngle", () -> swerveModules[0].getCurrentAbsoluteDeg());
+//        tab.addNumber("FR CurrentModuleAngle", () -> swerveModules[1].getCurrentAbsoluteDeg());
+//        tab.addNumber("BL CurrentModuleAngle", () -> swerveModules[2].getCurrentAbsoluteDeg());
+//        tab.addNumber("BR CurrentModuleAngle", () -> swerveModules[3].getCurrentAbsoluteDeg());
+
+        tab.addNumber("Robot's angle", () -> PoseEstimator.getInstance().getHeading().getDegrees());
+
+        //tab.add("Turning 90 ",new MoveByDistance(new Pose2d(0, 0, Rotation2d.fromDegrees(90))));
+//        rotate 90d, (not working)
+
+
+           tab.add("change color Red", new InstantCommand(()-> LED.getInstance().setStrip(255, 0, 0)));
+//change color to red
+           tab.add("change color Rainbow", new InstantCommand(()-> LED.getInstance().setGaming(3)));
+           //change to rainbow (not working)
+
+        tab.addString("get current color", ()-> LED.getInstance().getCurrentColor());
+        // Get current color
+
+        tab.add("change color null", new InstantCommand(()-> LED.getInstance().setStrip(0, 0, 0)));
+
+
+        tab.add("change color Blink", new InstantCommand(()-> new Blink(183,30,190,1.5,5)));
+        tab.add("change color Gaming", new InstantCommand(()-> new Gaming(3)));
+    }
+}