Autonselectionfix

simgui-ds.json

@@ -90,12 +90,11 @@
     }
   ],
   "robotJoysticks": [
-    {},
     {
-      "guid": "Keyboard1"
+      "guid": "Keyboard0"
     },
     {
-      "guid": "Keyboard0"
+      "guid": "Keyboard1"
     }
   ]
 }

simgui-window.json

@@ -27,7 +27,7 @@
     },
     "###NetworkTables": {
       "Collapsed": "0",
-      "Pos": "250,277",
+      "Pos": "244,55",
       "Size": "750,185"
     },
     "###Other Devices": {

simgui.json

@@ -29,6 +29,7 @@
       "/LiveWindow/Ungrouped/PIDController[8]": "PIDController",
       "/LiveWindow/Ungrouped/PIDController[9]": "PIDController",
       "/LiveWindow/Ungrouped/navX-Sensor[4]": "Gyro",
+      "/SmartDashboard/AutoSelector": "String Chooser",
       "/SmartDashboard/Auton Choice": "String Chooser",
       "/SmartDashboard/Field": "Field2d",
       "/SmartDashboard/Scheduler": "Scheduler"

src/main/java/team3176/robot/Robot.java

@@ -131,7 +131,7 @@ public void disabledInit() {
 
   @Override
   public void disabledPeriodic() {
-    //nan
+    robotContainer.checkAutonomousSelection();
   }
 
   /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */

src/main/java/team3176/robot/RobotContainer.java

@@ -6,15 +6,20 @@
 
 import java.io.File;
 
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
+import org.littletonrobotics.junction.networktables.LoggedDashboardInput;
 
 import edu.wpi.first.wpilibj.Filesystem;
 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.wpilibj.PowerDistribution;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
 import team3176.robot.commands.*;
 import team3176.robot.commands.drivetrain.*;
@@ -57,7 +62,9 @@ public class RobotContainer {
   private final Drivetrain drivetrain;
   private final VisionCubeChase vision;
   private final Superstructure superstructure;
-  private SendableChooser<String> autonChooser;
+  private LoggedDashboardChooser<String> autonChooser = new LoggedDashboardChooser<>("AutoSelector");
+  private String choosenAutonomousString = "";
+  private Command choosenAutonomousCommand;
 
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -79,7 +86,7 @@ public RobotContainer() {
         controller::getStrafe,
         controller::getSpin));
     arm.setDefaultCommand(arm.armFineTune( () -> controller.operator.getLeftY()));
-    autonChooser = new SendableChooser<>();
+    //autonChooser.addDefaultOption("wall_3_cube_poop_4_steal", "wall_3_cube_poop_4_steal");
     File paths = new File(Filesystem.getDeployDirectory(), "pathplanner");
     for (File f : paths.listFiles()) {
       if (!f.isDirectory()) {
@@ -89,7 +96,7 @@ public RobotContainer() {
     }
 
 
-    SmartDashboard.putData("Auton Choice", autonChooser);
+    SmartDashboard.putData("Auton Choice", autonChooser.getSendableChooser());
 
     configureBindings();
   }
@@ -219,18 +226,38 @@ public void clearCanFaults(){
   public void printCanFaults(){
     pdh.getStickyFaults();
   }
-
+  public void checkAutonomousSelection() {
+    if(autonChooser.get() != null && !choosenAutonomousString.equals(autonChooser.get())) {
+      Long start = System.nanoTime();
+      choosenAutonomousString = autonChooser.get();
+      try {
+        choosenAutonomousCommand = new PathPlannerAuto(choosenAutonomousString).getauto();
+      }
+      catch(Exception e){
+        System.out.println("[ERROR] could not find" + choosenAutonomousString);
+        System.out.println(e.toString());
+      }
+
+      Long totalTime =   System.nanoTime() - start;
+      System.out.println("Autonomous Selected: [" + choosenAutonomousString + "] generated in " + (totalTime / 1000000.0) + "ms");
+    }
+  }
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
     // An example command will be run in autonomous
-    String chosen = autonChooser.getSelected();
-    //String chosen = "wall_cone_exit_balance";
+
+    if(choosenAutonomousCommand == null) {
+      //this is if for some reason checkAutonomousSelection is never called
+      String chosen = autonChooser.get();
 
-    PathPlannerAuto ppSwerveAuto = new PathPlannerAuto(chosen);
-    return ppSwerveAuto.getauto();
+      PathPlannerAuto ppSwerveAuto = new PathPlannerAuto(chosen);
+      return ppSwerveAuto.getauto();
+    }
+    return choosenAutonomousCommand;
+
   }
 }

src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java

@@ -267,8 +267,8 @@ private void calculateNSetPodPositions() {
         realStates[idx] = new SwerveModuleState(pods.get(idx).getVelocity(),Rotation2d.fromDegrees(pods.get(idx).getAzimuth()));
       }
       //Logger.getInstance().recordOutput("SwerveStates/Setpoints", podStates);
-      //Logger.getInstance().recordOutput("SwerveStates/real", realStates);
-      //Logger.getInstance().recordOutput("SwerveStates/SetpointsOptimized", optimizedStates);
+      Logger.getInstance().recordOutput("SwerveStates/real", realStates);
+      Logger.getInstance().recordOutput("SwerveStates/SetpointsOptimized", optimizedStates);
       //Logger.getInstance().recordOutput("Drive/SpinCommand", spinCommand);
       SmartDashboard.putNumber("spinCommand", spinCommand);
       SmartDashboard.putNumber("pod0 m/s", podStates[0].speedMetersPerSecond);