add choreo, move auton and alliance switches to util subsystems

src/main/java/frc/lib/AllianceSelector.java

@@ -0,0 +1,30 @@
+package frc.lib;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.AutoConstants.AllianceColor;
+
+import java.util.function.BooleanSupplier;
+
+public class AllianceSelector extends SubsystemBase{
+
+  private DigitalInput m_allianceSelectionSwitch;
+
+  public AllianceSelector(int port) {
+    this.m_allianceSelectionSwitch = new DigitalInput(port);
+  }
+
+  public BooleanSupplier fieldRotatedSupplier() {
+    return () -> m_allianceSelectionSwitch.get();
+  }
+
+  public AllianceColor getAllianceColor() {
+    return m_allianceSelectionSwitch.get() ? AllianceColor.Red : AllianceColor.Blue;
+  }
+
+  @Override
+  public void periodic() {
+    SmartDashboard.putString("Alliance Color", getAllianceColor().name());
+  }
+}

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

@@ -0,0 +1,73 @@
+package frc.lib;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.AutoConstants.AllianceColor;
+import frc.robot.Robot.AutoOption;
+import frc.robot.autos.ChoreoAuto;
+
+import java.util.List;
+
+public class AutoSelector {
+
+  private DigitalInput[] m_switchPositions;
+  private AllianceSelector m_allianceSelector;
+  private List<AutoOption> m_autoOptions;
+
+  private ChoreoAuto m_autonomous;
+
+  public AutoSelector(int[] ports, AllianceSelector allianceSelector, List<AutoOption> autoOptions) {
+
+    m_switchPositions = new DigitalInput[ports.length];
+    for (int i = 0; i < ports.length; i++) {
+      m_switchPositions[i] = new DigitalInput(ports[i]);
+    }
+
+    this.m_allianceSelector = allianceSelector;
+    this.m_autoOptions = autoOptions;
+  }
+
+  /**
+   * @return Index in array of Digital Inputs corresponding to selected auto mode
+   */
+  private int getSwitchPosition() {
+    for (int i = 0; i < m_switchPositions.length; i++) {
+      if (!m_switchPositions[i].get()) {
+        return i + 1;
+      }
+    }
+    return 0; // failure of the physical switch
+  }
+
+  public ChoreoAuto getSelectedAutonomous() {
+    int switchPosition = getSwitchPosition();
+    AllianceColor color = m_allianceSelector.getAllianceColor();
+
+    for (int i = 0; i < m_autoOptions.size(); i++) {
+      if (m_autoOptions.get(i).getColor() == color)
+        if (m_autoOptions.get(i).getOption() == switchPosition) {
+        return m_autoOptions.get(i).getChoreoAuto();
+     }
+    }
+    return null; 
+  }
+
+  /**
+   * @return The Command that runs the selected autonomous mode
+   */
+  public Command getAutonomousCommand() {
+    getSelectedAutonomous();
+    if (m_autonomous != null) return m_autonomous;
+    else return null;
+  }
+
+  public void periodic() {
+
+    if (m_autonomous != null) {
+      SmartDashboard.putString("Auto", m_autonomous.getName());
+    } else {
+      SmartDashboard.putString("Auto", "Null");
+    }
+  }
+}

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

@@ -4,7 +4,6 @@
 
 import static edu.wpi.first.units.Units.*;
 
-import com.pathplanner.lib.util.PIDConstants;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.units.measure.AngularVelocity;
@@ -74,14 +73,18 @@ public static final class ModuleConstants {
     public static final int kDriveMotorCurrentLimit = 80;
     public static final int kTurningMotorCurrentLimit = 80;
 
-    public static final double kDriveP = 0.1; // 2023 Competition Robot
-    public static final double kDriveI = 0.0; // 2023 Competition Robot
-    public static final double kDriveD = 0.0; // 2023 Competition Robot
-    public static final double kDriveFF = 0.255; // 2023 Competition Robot
+    public static final class DriveControllerGains {
+      public static final double kP = 0.1; // 2023 Competition Robot
+      public static final double kI = 0.0; // 2023 Competition Robot
+      public static final double kD = 0.0; // 2023 Competition Robot
+      public static final double kFF = 0.255; // 2023 Competition Robot
+    }
 
-    public static final double kTurningP = 10.0; // 1.5;
-    public static final double kTurningI = 0.0; // 2023 Competition Robot
-    public static final double kTurningD = 0.0; // 2023 Competition Robot
+    public static final class TurningControllerGains {
+      public static final double kP = 10.0; // 1.5;
+      public static final double kI = 0.0; // 2023 Competition Robot
+      public static final double kD = 0.0; // 2023 Competition Robot
+    }
 
     // Not adjusted
     // public static final double kMaxModuleAngularSpeedRadiansPerSecond = 0.05 * Math.PI;
@@ -160,18 +163,33 @@ public static final class XBox {
   }
 
   public static final class AutoConstants {
+
+    public static enum AllianceColor {
+      Red,
+      Blue
+    }
+
     public static final int kAllianceColorSelectorPort = 10;
 
-    // length is 8
+    // max length is 8
     public static final int[] kAutonomousModeSelectorPorts = {11, 12, 13, 18, 19};
 
     // public static final double kMaxSpeedMetersPerSecond = 3.0;
     // public static final double kMaxAccelerationMetersPerSecondSquared = 3.0;
     // public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
     // public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
 
-    public static final PIDConstants kTranslationControllerGains = new PIDConstants(1.0, 0.0, 0.0);
-    public static final PIDConstants kRotationControllerGains = new PIDConstants(7.0, 0.0, 0.0);
+    public static final class TranslationControllerGains {
+      public static final double kP = 1.0;
+      public static final double kI = 0.0;
+      public static final double kD = 0.0;
+    }
+
+    public static final class RotationControllerGains {
+      public static final double kP = 7.0;
+      public static final double kI = 0.0;
+      public static final double kD = 0.0;
+    }
 
     // Constraint for the motion profilied robot angle controller
     // public static final TrapezoidProfile.Constraints kThetaControllerConstraints =

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

@@ -2,28 +2,65 @@
 
 package frc.robot;
 
-import static frc.robot.RobotContainer.getRobotContainer;
+import java.util.List;
 
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.PowerDistribution;
+import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import frc.lib.AllianceSelector;
+import frc.lib.AutoSelector;
 import frc.lib.ControllerPatroller;
+import frc.lib.SendableZorroController;
+import frc.robot.Constants.AutoConstants.AllianceColor;
+import frc.robot.Constants.AutoConstants;
+import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.OIConstants;
+import frc.robot.Constants.OIConstants.Zorro;
+import frc.robot.autos.ChoreoAuto;
+import frc.robot.drivetrain.Drivetrain;
+import frc.robot.drivetrain.commands.ZorroDriveCommand;
 
 public class Robot extends TimedRobot {
 
   private Command m_autonomousCommand;
+  private List<AutoOption> m_autoOptions;
+
+  private final PowerDistribution m_powerDistribution = new PowerDistribution(1, ModuleType.kRev);
+  private final AllianceSelector m_allianceSelector;
+  private final AutoSelector m_autoSelector;
+  private final Drivetrain m_swerve;
+
+  private SendableZorroController m_driver;
+  private XboxController m_operator;
 
   private int m_usb_check_delay = OIConstants.kUSBCheckNumLoops;
 
+  public Robot() {
+    configureButtonBindings();
+    configureDefaultCommands();
+    configureAutoOptions();
+
+    m_allianceSelector = new AllianceSelector(AutoConstants.kAllianceColorSelectorPort);
+    m_autoSelector = new AutoSelector(AutoConstants.kAutonomousModeSelectorPorts, m_allianceSelector, m_autoOptions);
+
+    m_swerve = new Drivetrain(m_allianceSelector.fieldRotatedSupplier());
+
+    // Create a button on Smart Dashboard to reset the encoders.
+    SmartDashboard.putData(
+        "Align Encoders",
+        new InstantCommand(() -> m_swerve.zeroAbsTurningEncoderOffsets()).ignoringDisable(true));
+  }
+
   @Override
   public void robotInit() {
-    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
-    // autonomous chooser on the dashboard.
-    getRobotContainer();
-
     // Starts recording to data log
     // https://docs.wpilib.org/en/stable/docs/software/telemetry/datalog.html#logging-joystick-data
     DataLogManager.start();
@@ -33,13 +70,13 @@ public void robotInit() {
   @Override
   public void robotPeriodic() {
     CommandScheduler.getInstance().run();
-    getRobotContainer().periodic();
+    SmartDashboard.putData(m_driver);
+    SmartDashboard.putData(m_operator);
+    SmartDashboard.putData(m_powerDistribution);
   }
 
   @Override
-  public void disabledInit() {
-    getRobotContainer().disabledInit();
-  }
+  public void disabledInit() {}
 
   @Override
   public void disabledPeriodic() {
@@ -49,21 +86,19 @@ public void disabledPeriodic() {
      * Only check if controllers changed every kUSBCheckNumLoops loops of disablePeriodic().
      * This prevents us from hammering on some routines that cause the RIO to lock up.
      */
-    RobotContainer rc = getRobotContainer();
     m_usb_check_delay--;
     if (0 >= m_usb_check_delay) {
       m_usb_check_delay = OIConstants.kUSBCheckNumLoops;
       if (ControllerPatroller.getInstance().controllersChanged()) {
         // Reset the joysticks & button mappings.
-        rc.configureButtonBindings();
+        configureButtonBindings();
       }
     }
   }
 
   @Override
   public void autonomousInit() {
-    getRobotContainer().autonomousInit();
-    m_autonomousCommand = getRobotContainer().getAutonomousCommand();
+    m_autonomousCommand = m_autoSelector.getAutonomousCommand();
 
     if (m_autonomousCommand != null) {
       m_autonomousCommand.schedule();
@@ -78,8 +113,6 @@ public void teleopInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
-
-    getRobotContainer().teleopInit();
   }
 
   @Override
@@ -92,4 +125,84 @@ public void testInit() {
 
   @Override
   public void testPeriodic() {}
+
+  private void configureDefaultCommands() {
+    m_swerve.setDefaultCommand(
+        new ZorroDriveCommand(m_swerve, DriveConstants.kDriveKinematics, m_driver));
+  }
+
+  public void configureButtonBindings() {
+
+    // Clear any active buttons.
+    CommandScheduler.getInstance().getActiveButtonLoop().clear();
+
+    ControllerPatroller cp = ControllerPatroller.getInstance();
+
+    // We use two different types of controllers - Joystick & XboxController.
+    // Create objects of the specific types.
+    m_driver = new SendableZorroController(cp.findDriverPort());
+    m_operator = new XboxController(cp.findOperatorPort());
+
+    configureDriverButtonBindings();
+    configureOperatorButtonBindings();
+  }
+
+  // spotless:off
+  private void configureDriverButtonBindings() {
+
+    // Reset heading
+    new JoystickButton(m_driver, Zorro.kHIn)
+        .onTrue(new InstantCommand(() -> m_swerve.resetHeading())
+        .ignoringDisable(true));
+
+  }
+  // spotless:on
+
+  private void configureOperatorButtonBindings() {}
+
+  private void configureAutoOptions() {
+    m_autoOptions.add(new AutoOption(AllianceColor.Red, 1));
+    m_autoOptions.add(new AutoOption(AllianceColor.Blue, 1));
+  }
+
+  public class AutoOption {
+    private AllianceColor m_color;
+    private int m_option;
+    private ChoreoAuto m_auto;
+
+    public AutoOption(AllianceColor color, int option, ChoreoAuto auto) {
+      this.m_color = color;
+      this.m_option = option;
+      this.m_auto = auto;
+    }
+
+    public AutoOption(AllianceColor color, int option) {
+      this.m_color = color;
+      this.m_option = option;
+      this.m_auto = null;
+    }
+
+    public AllianceColor getColor() {
+      return this.m_color;
+    }
+
+    public int getOption() {
+      return this.m_option;
+    }
+
+    public ChoreoAuto getChoreoAuto() {
+      return this.m_auto;
+    }
+  }
+
+  /**
+   * Gets the current drawn from the Power Distribution Hub by a CAN motor controller, assuming that
+   * (PDH port number + 10) = CAN ID
+   *
+   * @param CANBusPort The CAN ID of the motor controller
+   * @return Current in Amps on the PDH channel corresponding to the motor channel
+   */
+  public double getPDHCurrent(int CANBusPort) {
+    return m_powerDistribution.getCurrent(CANBusPort - 10);
+  }
 }