Leds (#101)

* added feedforward to the elevator

* changed most things to units updated the kV

* added feedforward values to constants

* Changed internal elevator logic to meters

* added kg and changed ks

* Updates for new robot

* Merge and fix

* Remove unused imports

* added method that refreshes the relative turning encoder every 0.1 seconds

* ran spotless

* added DIO ports and leds for auto selector

* fixed issues with overlapping DIO ports

* fixed auto selector to display correct value

* added the rest of the auto routines

* changed climber deploy position to 14 inches and created branch for practice testing

* tweakd elevator heights and intake voltages

* Added algaemode indicator LEDs

* fix auto selector

* reanabled default command

* Remove unused imports

* more robot tweaks

* Mostly re-wrote the LED code.

* Added some comments, did a little cleanup

* added command for retracting the climber to a position

* fixed elevator feed forward, tested and worked

* more tuning

* Remove unused imports

* Added intake/outtake annimation, ran spotless.

* cancelled all running led commands on init (yet to be tested)

* led commands change with game state

* Animate scrolling blocks of appropriate color on intake/outtake

* Remove teleop periodic call to LED subsystem

* Added simple implementation for displayPoseSeek()

* Ran spotless

* Fix swapped rotation colors

* Make sure to clear bottom segment that is not lit.

* tweaked units for better accuracy

* Remove timeout from roller animation command.  Add some debug code.

* Add method to immediately replace default command on LED subsystem

* Added method to LEDs to immediately replace default command

* Change some pose-seek display colors

* spotless

* Removed some unused variables

* removed unused imports

* Increase middle segment size, reduce animation speed, use RPM threshold to determine roller motion.  Animations still only going one direction.  Believe somehow the logic for determining whether it's intake or outtake is incorrect in a subtle way.

* Fix logic to use dynamic values in animation command instead of fixing the ones at command creation time.

* spotless

* Initialize color and roller direction on Command.initialize() so it uses the state when the command is first scheduled.

* Overlay auto selection on top of pose-seek info.

* Added stub to create effect on auto selection

* Fix null object in Optional

* added the andymark april tag layout

* Fixed hasAlgae typo

* spotless

---------

Co-authored-by: DylanTaylor29 <[email protected]>
Co-authored-by: nlaverdure <[email protected]>
Co-authored-by: Christopher Larrieu <[email protected]>

Author: 4 people

src/main/java/frc/lib/AutoOption.java

@@ -2,8 +2,10 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.auto.AutoMode;
+import frc.util.Util;
 import java.util.Optional;
 
 public class AutoOption {
@@ -37,14 +39,21 @@ public AutoOption(Alliance color, int option) {
   /**
    * @return Alliance for which the option is valid
    */
-  public Alliance getColor() {
+  public Alliance getAlliance() {
     return this.allianceColor;
   }
 
+  /**
+   * @return Color of the associated alliance
+   */
+  public Color getAllianceColor() {
+    return Util.allianceToColor(getAlliance());
+  }
+
   /**
    * @return Selector switch index for which the option is valid
    */
-  public int getOption() {
+  public int getOptionNumber() {
     return this.switchNumber;
   }
 

src/main/java/frc/lib/AutoSelector.java

@@ -15,7 +15,7 @@
 import java.util.Optional;
 import java.util.function.Supplier;
 
-public class AutoSelector {
+public class AutoSelector implements Supplier<Optional<AutoOption>> {
 
   private Optional<AutoOption> currentAutoOption;
   private DigitalInput[] switchPositions;
@@ -72,14 +72,14 @@ public int getBinarySwitchPosition() {
     return sum;
   }
 
-  private Alliance getAllianceColor() {
+  private Alliance getAlliance() {
     return allianceColorSupplier.get();
   }
 
   private Optional<AutoOption> findMatchingOption() {
     return autoOptions.stream()
-        .filter(o -> o.getColor() == getAllianceColor())
-        .filter(o -> o.getOption() == getBinarySwitchPosition())
+        .filter(o -> o.getAlliance() == getAlliance())
+        .filter(o -> o.getOptionNumber() == getBinarySwitchPosition())
         .findFirst();
   }
 
@@ -92,6 +92,11 @@ private boolean updateAuto() {
     return true;
   }
 
+  @Override
+  public Optional<AutoOption> get() {
+    return currentAutoOption;
+  }
+
   /**
    * @return Object for binding a command to a change in autonomous mode selection
    */

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

@@ -17,6 +17,7 @@
 import edu.wpi.first.units.measure.Time;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.util.Color;
 
 public final class Constants {
 
@@ -294,9 +295,12 @@ public static final class ClimberConstants {
 
   public static final class LedConstants {
     public static final int kLedPort = 9;
-    public static final int kLedBufferLength = 40;
+    public static final int kLedPixelCount = 40;
 
     public static final int kLEDsPerBlock = 2;
     public static final int kLEDsBetweenBlocks = 1;
+
+    public static final Color algaeColor = Color.kGreen;
+    public static final Color coralColor = Color.kCoral;
   }
 }