3 1 changes (#100)

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

* 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

* ran spotless

Author: DylanTaylor29

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

@@ -66,7 +66,7 @@ public int getBinarySwitchPosition() {
     int sum = 0;
     for (int i = 0; i < switchPositions.length; i++) {
       if (!switchPositions[i].get()) {
-        sum += 2 ^ i;
+        sum += (1 << (i));
       }
     }
     return sum;
@@ -113,6 +113,10 @@ public void disabledPeriodic() {
     eventLoop.poll();
     SmartDashboard.putNumber("AutoSelectorSwitchPosition", getBinarySwitchPosition());
 
+    SmartDashboard.putBoolean("AutoSelectorDIOPort0", !switchPositions[0].get());
+    SmartDashboard.putBoolean("AutoSelectorDIOPort1", !switchPositions[1].get());
+    SmartDashboard.putBoolean("AutoSelectorDIOPort2", !switchPositions[2].get());
+
     currentAutoOption.ifPresentOrElse(
         ao -> {
           SmartDashboard.putString("SelectedAutonomousMode", ao.getName());

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

@@ -234,10 +234,10 @@ public static final class DpadDirection {
   }
 
   public static final class AutoConstants {
-    public static final int kAllianceColorSelectorPort = 10;
+    public static final int kAllianceColorSelectorPort = 3;
 
     // max length is 8
-    public static final int[] kAutonomousModeSelectorPorts = {11, 12, 13, 18, 19};
+    public static final int[] kAutonomousModeSelectorPorts = {0, 1, 2};
 
     // public static final double kMaxSpeedMetersPerSecond = 3.0;
     // public static final double kMaxAccelerationMetersPerSecondSquared = 3.0;
@@ -266,7 +266,7 @@ public static final class ClimberConstants {
     public static final double kEngagedPosition = 600.0 / 1024.0;
     public static final double kDisengedPosition = 475.0 / 1024.0;
 
-    public static final int kCageSensorPort = 1;
+    public static final int kCageSensorPort = 6;
 
     public static final double kP = 0.1;
     public static final double kI = 0;

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

@@ -30,8 +30,10 @@
 import frc.robot.LEDs.LEDs;
 import frc.robot.auto.BlueL4AlgaeAuto;
 import frc.robot.auto.BlueNoProcess3PieceAuto;
-import frc.robot.auto.ExampleAuto;
+import frc.robot.auto.BlueProcess3PieceAuto;
 import frc.robot.auto.RedL4AlgaeAuto;
+import frc.robot.auto.RedNoProcess3PieceAuto;
+import frc.robot.auto.RedProcess3PieceAuto;
 import frc.robot.climber.Climber;
 import frc.robot.drivetrain.Drivetrain;
 import frc.robot.drivetrain.commands.DriveToPoseCommand;
@@ -88,6 +90,8 @@ public Robot() {
     SmartDashboard.putData(
         "Align Encoders",
         new InstantCommand(() -> swerve.zeroAbsTurningEncoderOffsets()).ignoringDisable(true));
+
+    addPeriodic(() -> swerve.refreshRelativeTurningEncoder(), 0.1);
   }
 
   @Override
@@ -283,9 +287,14 @@ private void configureEventBindings() {
   private void configureAutoOptions() {
     autoSelector.addAuto(new AutoOption(Alliance.Red, 1, new RedL4AlgaeAuto(swerve, elevator)));
     autoSelector.addAuto(new AutoOption(Alliance.Blue, 1, new BlueL4AlgaeAuto(swerve, elevator)));
+    autoSelector.addAuto(
+        new AutoOption(Alliance.Red, 2, new RedNoProcess3PieceAuto(swerve, elevator)));
     autoSelector.addAuto(
         new AutoOption(Alliance.Blue, 2, new BlueNoProcess3PieceAuto(swerve, elevator)));
-    autoSelector.addAuto(new AutoOption(Alliance.Blue, 3, new ExampleAuto(swerve)));
+    autoSelector.addAuto(
+        new AutoOption(Alliance.Red, 3, new RedProcess3PieceAuto(swerve, elevator)));
+    autoSelector.addAuto(
+        new AutoOption(Alliance.Blue, 3, new BlueProcess3PieceAuto(swerve, elevator)));
   }
 
   /**

src/main/java/frc/robot/drivetrain/Drivetrain.java

@@ -89,8 +89,6 @@ public void periodic() {
     m_visionPosePublisher.set(poseEstimator.getEstimatedPosition());
 
     for (SwerveModule module : SwerveModule.values()) {
-      module.refreshRelativeTurningEncoder();
-
       SmartDashboard.putNumber(
           module.getName() + "/RelativeTurningPosition",
           module.getRelativeTurningPosition().getDegrees());
@@ -142,6 +140,12 @@ public void zeroAbsTurningEncoderOffsets() {
     }
   }
 
+  public void refreshRelativeTurningEncoder() {
+    for (SwerveModule module : SwerveModule.values()) {
+      module.refreshRelativeTurningEncoder();
+    }
+  }
+
   public Rotation2d getHeading() {
     return poseEstimator.getEstimatedPosition().getRotation();
   }

src/main/java/frc/robot/elevator/ElevatorConstants.java

@@ -70,7 +70,7 @@ private LifterState(double inches) {
 
   public static final class CoralRollerConstants {
     public static final int kMotorPort = 23;
-    public static final int kCoralSensorPort = 3;
+    public static final int kCoralSensorPort = 5;
 
     public static final double kGearRatio = 5.0;
     public static final double kRollerDiameter = 2.0; // inches