diff --git a/simgui-ds.json b/simgui-ds.json index 17e0f72..4726e2d 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -90,12 +90,11 @@ } ], "robotJoysticks": [ - {}, { - "guid": "Keyboard1" + "guid": "Keyboard0" }, { - "guid": "Keyboard0" + "guid": "Keyboard1" } ] } diff --git a/simgui-window.json b/simgui-window.json index 81ae590..55f1055 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -27,7 +27,7 @@ }, "###NetworkTables": { "Collapsed": "0", - "Pos": "250,277", + "Pos": "244,55", "Size": "750,185" }, "###Other Devices": { diff --git a/simgui.json b/simgui.json index 5543b54..08ac180 100644 --- a/simgui.json +++ b/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" diff --git a/src/main/java/team3176/robot/Robot.java b/src/main/java/team3176/robot/Robot.java index 9ecc806..8c2c229 100644 --- a/src/main/java/team3176/robot/Robot.java +++ b/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. */ diff --git a/src/main/java/team3176/robot/RobotContainer.java b/src/main/java/team3176/robot/RobotContainer.java index 069839f..befb853 100644 --- a/src/main/java/team3176/robot/RobotContainer.java +++ b/src/main/java/team3176/robot/RobotContainer.java @@ -6,6 +6,9 @@ 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; @@ -13,8 +16,10 @@ 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 autonChooser; + private LoggedDashboardChooser 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,7 +226,22 @@ 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. * @@ -227,10 +249,15 @@ public void printCanFaults(){ */ 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; + } } diff --git a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java index 656ad51..c0f5efc 100644 --- a/src/main/java/team3176/robot/subsystems/drivetrain/Drivetrain.java +++ b/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);