From f6b8c6e3f08607ef9a35f7dc9642e868a08f3452 Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Sat, 25 Feb 2023 12:51:23 -0700
Subject: [PATCH] autoChooser + playback filename

---
 .../java/frc4388/robot/RobotContainer.java    | 33 ++++++++++++++++---
 .../robot/commands/JoystickPlayback.java      | 18 +++++++---
 2 files changed, 42 insertions(+), 9 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 8412738..d2b28fe 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -23,6 +23,7 @@ import edu.wpi.first.math.trajectory.TrajectoryConfig;
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
 import edu.wpi.first.math.trajectory.Trajectory.State;
 import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -62,6 +63,20 @@ public class RobotContainer {
     private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
     private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
 
+    /* Autos */
+    private SendableChooser<Command> chooser = new SendableChooser<>();
+    
+    private Command noAuto = new InstantCommand();
+    
+    private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive);
+    
+    private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "BlueNearDriveToChargeStation.txt");
+    private Command blue1PathWithBalance = blue1Path.andThen(balance);
+    
+    private Command red1Path = new JoystickPlayback(m_robotSwerveDrive, "BlueNearDriveToChargeStation.txt", -1);
+    private Command red1PathWithBalance = red1Path.andThen(balance);
+
+
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
@@ -75,6 +90,17 @@ public class RobotContainer {
                                                   true);
             }, m_robotSwerveDrive)
             .withName("SwerveDrive DefaultCommand"));
+        
+        // * Auto Commands
+        chooser.setDefaultOption("NoAuto", noAuto);
+
+        chooser.addOption("Blue1Path", blue1Path);
+        chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance);
+
+        chooser.addOption("Red1Path", red1Path);
+        chooser.addOption("Red1PathWithBalance", red1PathWithBalance);
+
+        SmartDashboard.putData(chooser);
     }
 
 
@@ -104,8 +130,8 @@ public class RobotContainer {
                                             () -> getDeadbandedDriverController().getRightY()))
             .onFalse(new InstantCommand());
 
-        new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
-            .onTrue(new JoystickPlayback(m_robotSwerveDrive, 1));
+        // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
+        //     .onTrue(new JoystickPlayback(m_robotSwerveDrive, 1));
 
         // * Operator Buttons
     }
@@ -117,8 +143,7 @@ public class RobotContainer {
      */
     public Command getAutonomousCommand() {
 
-        return new JoystickPlayback(m_robotSwerveDrive, 1)
-            .andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
+        return chooser.getSelected();
         // return new InstantCommand();
     }
 
diff --git a/src/main/java/frc4388/robot/commands/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/JoystickPlayback.java
index 0d7facc..c844abd 100644
--- a/src/main/java/frc4388/robot/commands/JoystickPlayback.java
+++ b/src/main/java/frc4388/robot/commands/JoystickPlayback.java
@@ -15,6 +15,8 @@ import frc4388.utility.UtilityStructs.TimedOutput;
 
 public class JoystickPlayback extends CommandBase {
   private final SwerveDrive            swerve;
+  private       String                 filename;
+  private       int                    mult       = 1;
   private       Scanner                input;
   private final ArrayList<TimedOutput> outputs      = new ArrayList<>();
   private       int                    counter      = 0;
@@ -22,15 +24,21 @@ public class JoystickPlayback extends CommandBase {
   private       long                   playbackTime = 0;
   private       int                    lastIndex;
   private       boolean                m_finished   = false; // ! find a better way
-  private       int                    m_mult       = 1;
 
   /** Creates a new JoystickPlayback. */
-  public JoystickPlayback(SwerveDrive swerve, int mult) {
+  public JoystickPlayback(SwerveDrive swerve, String filename, int mult) {
     this.swerve = swerve;
-    m_mult = mult;
+    this.filename = filename;
+    this.mult = mult;
+
     addRequirements(this.swerve);
   }
 
+  /** Creates a new JoystickPlayback. */
+  public JoystickPlayback(SwerveDrive swerve, String filename) {
+    this(swerve, filename, 1);
+  }
+
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
@@ -38,7 +46,7 @@ public class JoystickPlayback extends CommandBase {
     playbackTime = 0;
     lastIndex    = 0;
     try {
-      input = new Scanner(new File("/home/lvuser/BlueNearDriveToChargeStation.txt"));
+      input = new Scanner(new File("/home/lvuser/" + filename));
 
       String line = "";
       while (input.hasNextLine()) {
@@ -52,7 +60,7 @@ public class JoystickPlayback extends CommandBase {
         System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]);
 
         var out = new TimedOutput();
-        out.leftX  = Double.parseDouble(values[0]) * m_mult;
+        out.leftX  = Double.parseDouble(values[0]) * mult;
         out.leftY  = Double.parseDouble(values[1]);
         out.rightX = Double.parseDouble(values[2]);
         out.rightY = Double.parseDouble(values[3]);