changed the way subsystems are passed into Auto and changed any Chore…

…oAuto to AutoRoutine to reflect the methods we are using

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

@@ -1,13 +1,15 @@
 package frc.lib;
 
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import frc.robot.autos.ChoreoAuto;
+import choreo.auto.AutoRoutine;
+// import frc.robot.autos.ChoreoAuto;
+// import frc.robot.Auto;
 import java.util.Optional;
 
 public class AutoOption {
   private Alliance m_color;
   private int m_option;
-  private Optional<ChoreoAuto> m_auto;
+  private Optional<AutoRoutine> m_auto;
 
   /**
    * Constructs a selectable autonomous mode option
@@ -16,7 +18,7 @@ public class AutoOption {
    * @param option Selector switch index for which the option is valid
    * @param auto Command which runs the autonomous mode
    */
-  public AutoOption(Alliance color, int option, ChoreoAuto auto) {
+  public AutoOption(Alliance color, int option, AutoRoutine auto) {
     this.m_color = color;
     this.m_option = option;
     this.m_auto = Optional.of(auto);
@@ -51,7 +53,7 @@ public int getOption() {
   /**
    * @return The command which runs the selected autonomous mode
    */
-  public Optional<ChoreoAuto> getChoreoAuto() {
+  public Optional<AutoRoutine> getChoreoAuto() {
     return this.m_auto;
   }
 }

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

@@ -6,14 +6,15 @@
 import edu.wpi.first.wpilibj.event.EventLoop;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.autos.ChoreoAuto;
+// import frc.robot.autos.ChoreoAuto;
+import choreo.auto.AutoRoutine;
 import java.util.List;
 import java.util.Optional;
 import java.util.function.Supplier;
 
 public class AutoSelector {
 
-  private Optional<ChoreoAuto> m_currentAuto;
+  private Optional<AutoRoutine> m_currentAuto;
   private DigitalInput[] m_switchPositions;
   private Supplier<Alliance> m_allianceColorSupplier;
   private List<AutoOption> m_autoOptions;
@@ -52,7 +53,7 @@ public int getSwitchPosition() {
     return 0; // failure of the physical switch
   }
 
-  private Optional<ChoreoAuto> findMatchingOption() {
+  private Optional<AutoRoutine> findMatchingOption() {
     int switchPosition = getSwitchPosition();
     Alliance color = m_allianceColorSupplier.get();
 
@@ -66,7 +67,7 @@ private Optional<ChoreoAuto> findMatchingOption() {
   }
 
   private boolean updateAuto() {
-    Optional<ChoreoAuto> m_newAuto = findMatchingOption();
+    Optional<AutoRoutine> m_newAuto = findMatchingOption();
     if (m_newAuto.equals(m_currentAuto)) return false;
     else {
       m_currentAuto = m_newAuto;
@@ -83,12 +84,12 @@ public Trigger getChangedAutoSelection() {
 
   /** Schedules the command corresponding to the selected autonomous mode */
   public void scheduleAuto() {
-    if (m_currentAuto.isPresent()) m_currentAuto.get().schedule();
+    if (m_currentAuto.isPresent()) m_currentAuto.get();
   }
 
   /** Deschedules the command corresponding to the selected autonomous mode */
   public void cancelAuto() {
-    if (m_currentAuto.isPresent()) m_currentAuto.get().cancel();
+    if (m_currentAuto.isPresent()) m_currentAuto.get().kill();
   }
 
   public void disabledPeriodic() {
@@ -97,7 +98,7 @@ public void disabledPeriodic() {
     SmartDashboard.putNumber("Auto Selector Switch Position", getSwitchPosition());
 
     if (m_currentAuto.isPresent()) {
-      SmartDashboard.putString("Auto", m_currentAuto.get().getName());
+      SmartDashboard.putString("Auto", m_currentAuto.get().toString());
     } else {
       SmartDashboard.putString("Auto", "Null");
     }

src/main/java/frc/robot/Auto.java

@@ -6,7 +6,6 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.lib.AllianceSelector;
-import frc.robot.Constants.AutoConstants;
 import frc.robot.drivetrain.Drivetrain;
 
 public class Auto {
@@ -15,10 +14,10 @@ public class Auto {
 
     private final AutoFactory m_autoFactory;
 
-    public Auto() {
-        m_allianceSelector = new AllianceSelector(AutoConstants.kAllianceColorSelectorPort);
+    public Auto(AllianceSelector allianceSelector, Drivetrain drivetrain) {
+        this.m_allianceSelector = allianceSelector;
 
-        m_swerve = new Drivetrain(m_allianceSelector::fieldRotated);
+        this.m_swerve = drivetrain;
 
         m_autoFactory = new AutoFactory(m_swerve::getPose, m_swerve::setPose, m_swerve::followTrajectory, false, m_swerve);
     }

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

@@ -7,9 +7,9 @@
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
+// import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import edu.wpi.first.wpilibj2.command.Commands;
+// import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.lib.AllianceSelector;
@@ -21,13 +21,13 @@
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.OIConstants;
 import frc.robot.LEDs.LEDs;
-import frc.robot.autos.ChoreoAuto;
+// import frc.robot.autos.ChoreoAuto;
 import frc.robot.drivetrain.Drivetrain;
 import frc.robot.drivetrain.commands.ZorroDriveCommand;
 import java.util.ArrayList;
 import java.util.List;
 
-import choreo.auto.AutoFactory;
+// import choreo.auto.AutoFactory;
 
 public class Robot extends TimedRobot {
 
@@ -38,6 +38,7 @@ public class Robot extends TimedRobot {
   private final AutoSelector m_autoSelector;
   private final Drivetrain m_swerve;
   private final LEDs m_LEDs;
+  private final Auto m_auto;
 
   // private final AutoFactory m_autoFactory;
 
@@ -67,6 +68,8 @@ public Robot() {
         "Align Encoders",
         new InstantCommand(() -> m_swerve.zeroAbsTurningEncoderOffsets()).ignoringDisable(true));
 
+    m_auto = new Auto(m_allianceSelector, m_swerve);
+
     // m_autoFactory = new AutoFactory(m_swerve::getPose, m_swerve::setPose, m_swerve::followTrajectory, false, m_swerve);
   }
 
@@ -180,7 +183,7 @@ private void configureEventBindings() {
 
   private void configureAutoOptions() {
     m_autoOptions.add(new AutoOption(Alliance.Red, 4));
-    m_autoOptions.add(new AutoOption(Alliance.Blue, 1));
+    m_autoOptions.add(new AutoOption(Alliance.Blue, 1, m_auto.exampleRoutine()));
   }
 
   /**