diff --git a/simgui-ds.json b/simgui-ds.json index 179e064a4..5658abea0 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -4,6 +4,11 @@ "visible": true } }, + "System Joysticks": { + "window": { + "enabled": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/simgui-window.json b/simgui-window.json index eccfe2d0f..b30e73ed7 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -6,65 +6,65 @@ "GLOBAL": { "font": "Proggy Dotted", "fps": "120", - "height": "720", - "maximized": "0", + "height": "1984", + "maximized": "1", "style": "0", "userScale": "2", - "width": "1280", - "xpos": "193", - "ypos": "106" + "width": "3840", + "xpos": "0", + "ypos": "56" } }, "Window": { "###/FMSInfo": { "Collapsed": "0", - "Pos": "60,60", - "Size": "181,167" + "Pos": "819,899", + "Size": "368,329" }, "###/SmartDashboard/Field": { "Collapsed": "0", - "Pos": "166,24", - "Size": "740,373" + "Pos": "1478,31", + "Size": "1920,1157" }, "###FMS": { "Collapsed": "0", - "Pos": "4,414", - "Size": "169,184" + "Pos": "342,656", + "Size": "383,364" }, "###Joysticks": { "Collapsed": "0", - "Pos": "250,465", - "Size": "796,189" + "Pos": "1205,1515", + "Size": "1876,462" }, "###Keyboard 0 Settings": { "Collapsed": "0", - "Pos": "369,59", - "Size": "300,560" + "Pos": "368,1359", + "Size": "367,560" }, "###NetworkTables": { "Collapsed": "0", - "Pos": "399,253", + "Pos": "670,1253", "Size": "750,236" }, "###NetworkTables Info": { - "Collapsed": "1", - "Pos": "250,129", - "Size": "750,145" + "Collapsed": "0", + "Pos": "80,63", + "Size": "750,320" }, "###Other Devices": { "Collapsed": "0", - "Pos": "1025,20", - "Size": "250,695" + "Pos": "1006,51", + "Size": "278,695" }, "###System Joysticks": { "Collapsed": "0", - "Pos": "205,308", - "Size": "192,218" + "Pos": "900,977", + "Size": "435,434" }, "###Timing": { "Collapsed": "0", - "Pos": "5,150", - "Size": "135,219" + "Pos": "42,507", + "Size": "294,299" }, "Debug##Default": { "Collapsed": "0", @@ -73,8 +73,8 @@ }, "Robot State": { "Collapsed": "0", - "Pos": "7,20", - "Size": "92,116" + "Pos": "37,101", + "Size": "213,224" } } } diff --git a/simgui.json b/simgui.json index 18af23dda..ae3ce1d7f 100644 --- a/simgui.json +++ b/simgui.json @@ -60,11 +60,14 @@ "/LiveWindow/Ungrouped/PIDController[1]": "PIDController", "/LiveWindow/Ungrouped/Pigeon 2 [13]": "Gyro", "/LiveWindow/Ungrouped/Scheduler": "Scheduler", + "/SmartDashboard/Alerts": "Alerts", + "/SmartDashboard/Auto Chooser": "String Chooser", "/SmartDashboard/Encoders": "Alerts", "/SmartDashboard/Field": "Field2d", "/SmartDashboard/IMU": "Alerts", "/SmartDashboard/JSON": "Alerts", "/SmartDashboard/Motors": "Alerts", + "/SmartDashboard/PathPlanner": "Alerts", "/SmartDashboard/Pigeon 2 (v6) [13]": "Gyro", "/SmartDashboard/Pigeon 2 [13]": "Gyro", "/SmartDashboard/SendableChooser[0]": "String Chooser", @@ -105,12 +108,13 @@ "arrowWeight": 3.0, "style": "Hidden" }, - "bottom": 1476, - "height": 8.210550308227539, - "left": 150, - "right": 2961, - "top": 79, - "width": 16.541748046875, + "bottom": 1638, + "builtin": "2025 Reefscape", + "height": 8.051901817321777, + "left": 534, + "right": 3466, + "top": 291, + "width": 17.54825210571289, "window": { "visible": true } diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/Drive Forward.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/New Auto.auto rename to src/main/deploy/pathplanner/autos/Drive Forward.auto diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index 679a12e17..54aee03f3 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -3,41 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 7.225, + "y": 4.026 }, "prevControl": null, "nextControl": { - "x": 3.0, - "y": 6.5 + "x": 7.491840257883163, + "y": 4.013194162719998 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.958476027397261, - "y": 6.012192024384049 + "x": 6.725, + "y": 4.0259 }, "prevControl": { - "x": 3.9584760273972606, - "y": 7.012192024384048 - }, - "nextControl": { - "x": 5.958476027397261, - "y": 5.012192024384049 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.225280307703473, - "y": 6.012192024384049 - }, - "prevControl": { - "x": 6.975280307703473, - "y": 7.512192024384059 + "x": 7.003952143618287, + "y": 4.042917427000163 }, "nextControl": null, "isLocked": false, @@ -58,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 79a0f8c1e..8175a585b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,7 @@ package frc.robot; +import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; @@ -14,6 +15,8 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.RobotBase; +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.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -37,6 +40,14 @@ public class RobotContainer private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve/neo")); + // Establishing the Auto Chooser that will appear on the SmartDashboard + private final SendableChooser autoChooser; + + /** + * Enable using PathPlanner for autos + */ + private final boolean usingPathPlanner = false; //default is set to not use PathPlanner + /** * Converts driver input into a field-relative ChassisSpeeds that is controlled by angular velocity. */ @@ -98,7 +109,20 @@ public RobotContainer() // Configure the trigger bindings configureBindings(); DriverStation.silenceJoystickConnectionWarning(true); + + + // Create the NamedCommands that will be used in PathPlanner NamedCommands.registerCommand("test", Commands.print("I EXIST")); + + + //Set the default auto and put the autoChoser on the SmartDashboard + autoChooser = AutoBuilder.buildAutoChooser("Drive Forward"); // A default auto will be run in autonomous if a different auto is not selected + // This auto has only a path that will drive forward 0.5M from the start line + // The usingPathPlanner boolean must be set to true for this auto to run + + SmartDashboard.putData("Auto Chooser", autoChooser); + + } /** @@ -184,10 +208,19 @@ private void configureBindings() */ public Command getAutonomousCommand() { - // An example command will be run in autonomous - return drivebase.getAutonomousCommand("New Auto"); + if (usingPathPlanner) { + + return autoChooser.getSelected(); + } + else { + + return Commands.runOnce(drivebase::zeroGyroWithAlliance); + + } + } + public void setMotorBrake(boolean brake) { drivebase.setMotorBrake(brake); diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 1dad5fd9f..10332b3c3 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -30,7 +30,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import frc.robot.Constants; import frc.robot.subsystems.swervedrive.Vision.Cameras; @@ -109,7 +108,8 @@ public SwerveSubsystem(File directory) swerveDrive.stopOdometryThread(); } setupPathPlanner(); - RobotModeTriggers.autonomous().onTrue(Commands.runOnce(this::zeroGyroWithAlliance)); + + } /** diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java index b90ecdb84..f6a4be62a 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.swervedrive; import static edu.wpi.first.units.Units.Microseconds; -import static edu.wpi.first.units.Units.Milliseconds; import static edu.wpi.first.units.Units.Seconds; import edu.wpi.first.apriltag.AprilTagFieldLayout; @@ -525,7 +524,6 @@ private void updateUnreadResults() { double mostRecentTimestamp = resultsList.isEmpty() ? 0.0 : resultsList.get(0).getTimestampSeconds(); double currentTimestamp = Microseconds.of(NetworkTablesJNI.now()).in(Seconds); - double debounceTime = Milliseconds.of(15).in(Seconds); for (PhotonPipelineResult result : resultsList) { mostRecentTimestamp = Math.max(mostRecentTimestamp, result.getTimestampSeconds());