From 818588b172876d31eaf604131d2421546cd8dd6b Mon Sep 17 00:00:00 2001 From: Jonathan Date: Mon, 10 Jul 2023 08:10:12 -0600 Subject: [PATCH 1/2] switching to logged chooser --- simgui-ds.json | 5 ++- simgui-window.json | 2 +- simgui.json | 1 + src/main/java/team3176/robot/Robot.java | 2 +- .../java/team3176/robot/RobotContainer.java | 32 ++++++++++++++----- .../subsystems/drivetrain/Drivetrain.java | 4 +-- 6 files changed, 31 insertions(+), 15 deletions(-) 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..73b0ae0 100644 --- a/src/main/java/team3176/robot/RobotContainer.java +++ b/src/main/java/team3176/robot/RobotContainer.java @@ -6,6 +6,8 @@ import java.io.File; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +import org.littletonrobotics.junction.networktables.LoggedDashboardInput; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.Joystick; @@ -15,6 +17,7 @@ import edu.wpi.first.wpilibj.PowerDistribution; 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 +60,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 +84,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 +94,7 @@ public RobotContainer() { } - SmartDashboard.putData("Auton Choice", autonChooser); + SmartDashboard.putData("Auton Choice", autonChooser.getSendableChooser()); configureBindings(); } @@ -219,7 +224,13 @@ public void clearCanFaults(){ public void printCanFaults(){ pdh.getStickyFaults(); } - + public void checkAutonomousSelection() { + if(autonChooser.get() != null && !choosenAutonomousString.equals(autonChooser.get())) { + choosenAutonomousString = autonChooser.get(); + choosenAutonomousCommand = new PathPlannerAuto(choosenAutonomousString).getauto(); + + } + } /** * Use this to pass the autonomous command to the main {@link Robot} class. * @@ -227,10 +238,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) { + //String chosen = autonChooser.getSelected(); + String chosen = "wall_3nSteal_4"; - 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); From 868afa27a8392f6d76175619fd3c12430b748441 Mon Sep 17 00:00:00 2001 From: Jonathan Heidegger Date: Mon, 10 Jul 2023 11:03:27 -0400 Subject: [PATCH 2/2] verifying in simulation --- .../java/team3176/robot/RobotContainer.java | 21 ++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/src/main/java/team3176/robot/RobotContainer.java b/src/main/java/team3176/robot/RobotContainer.java index 73b0ae0..befb853 100644 --- a/src/main/java/team3176/robot/RobotContainer.java +++ b/src/main/java/team3176/robot/RobotContainer.java @@ -6,6 +6,7 @@ import java.io.File; +import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedDashboardInput; @@ -15,6 +16,7 @@ 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; @@ -84,7 +86,7 @@ public RobotContainer() { controller::getStrafe, controller::getSpin)); arm.setDefaultCommand(arm.armFineTune( () -> controller.operator.getLeftY())); - autonChooser.addDefaultOption("wall_3_cube_poop_4_steal", "wall_3_cube_poop_4_steal"); + //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()) { @@ -226,9 +228,18 @@ public void printCanFaults(){ } public void checkAutonomousSelection() { if(autonChooser.get() != null && !choosenAutonomousString.equals(autonChooser.get())) { + Long start = System.nanoTime(); choosenAutonomousString = autonChooser.get(); - choosenAutonomousCommand = new PathPlannerAuto(choosenAutonomousString).getauto(); - + 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"); } } /** @@ -240,8 +251,8 @@ public Command getAutonomousCommand() { // An example command will be run in autonomous if(choosenAutonomousCommand == null) { - //String chosen = autonChooser.getSelected(); - String chosen = "wall_3nSteal_4"; + //this is if for some reason checkAutonomousSelection is never called + String chosen = autonChooser.get(); PathPlannerAuto ppSwerveAuto = new PathPlannerAuto(chosen); return ppSwerveAuto.getauto();