From 5f87912475b485d520579b181fa66e2787a0d445 Mon Sep 17 00:00:00 2001 From: suryatho Date: Mon, 15 Jan 2024 17:24:36 -0500 Subject: [PATCH] Added a few example trajectories for autos --- src/main/deploy/pathplanner/navgrid.json | 1634 ++++++++++++++++- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/FieldConstants.java | 48 +- src/main/java/frc/robot/RobotContainer.java | 30 + .../frc/robot/commands/DriveCommands.java | 7 +- .../frc/robot/commands/DriveTrajectory.java | 124 ++ .../commands/FeedForwardCharacterization.java | 2 +- .../frc/robot/subsystems/drive/Drive.java | 64 +- .../subsystems/drive/DriveConstants.java | 29 +- .../frc/robot/util/LoggedTunableNumber.java | 87 + .../trajectory/ChoreoTrajectoryReader.java | 30 +- .../trajectory/HolonomicDriveController.java | 10 +- .../frc/robot/util/trajectory/Trajectory.java | 13 + 13 files changed, 2018 insertions(+), 61 deletions(-) create mode 100644 src/main/java/frc/robot/commands/DriveTrajectory.java create mode 100644 src/main/java/frc/robot/util/LoggedTunableNumber.java create mode 100644 src/main/java/frc/robot/util/trajectory/Trajectory.java diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json index bab0da93..690f5db2 100644 --- a/src/main/deploy/pathplanner/navgrid.json +++ b/src/main/deploy/pathplanner/navgrid.json @@ -1 +1,1633 @@ -{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file +{ + "field_size": { + "x": 16.54, + "y": 8.21 + }, + "nodeSizeMeters": 0.3, + "grid": [ + [ + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true + ], + [ + true, + true, + true, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true + ] + ] +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bf4e599a..81d18ca6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -24,6 +24,7 @@ public final class Constants { public static final int loopPeriodMs = 20; public static final Mode currentMode = Mode.SIM; + public static final boolean tuningMode = true; public static final boolean characterizationMode = false; public static enum Mode { diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index 05bedbc1..bb640444 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -1,7 +1,11 @@ package frc.robot; -import edu.wpi.first.math.geometry.Translation2d; +import static edu.wpi.first.apriltag.AprilTagFields.k2024Crescendo; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.util.Units; +import java.io.IOException; /** * Contains various field dimensions and useful reference points. Dimensions are in meters, and sets @@ -18,14 +22,16 @@ public class FieldConstants { public static double fieldLength = Units.inchesToMeters(651.223); public static double fieldWidth = Units.inchesToMeters(323.277); public static double wingX = Units.inchesToMeters(229.201); - public static double podiumX = Units.inchesToMeters(126.75); public static double startingLineX = Units.inchesToMeters(74.111); + public static Translation2d ampCenter = + new Translation2d(Units.inchesToMeters(72.455), Units.inchesToMeters(322.996)); + + /** Staging locations for each note */ public static final class StagingLocations { public static double centerlineX = Units.inchesToMeters(fieldLength / 2); - // need to update public static double centerlineFirstY = Units.inchesToMeters(29.638); public static double centerlineSeparationY = Units.inchesToMeters(66); @@ -50,4 +56,40 @@ public static final class StagingLocations { } } } + + /** Each corner of the speaker * */ + public static final class Speaker { + + /** Center of the speaker opening (blue alliance) */ + public static Pose2d centerSpeakerOpening = + new Pose2d(0.0, fieldWidth - Units.inchesToMeters(104.0), new Rotation2d()); + } + + // corners (blue alliance origin) + public static Translation3d topRightSpeaker = + new Translation3d( + Units.inchesToMeters(18.055), + Units.inchesToMeters(238.815), + Units.inchesToMeters(13.091)); + + public static Translation3d topLeftSpeaker = + new Translation3d( + Units.inchesToMeters(18.055), + Units.inchesToMeters(197.765), + Units.inchesToMeters(83.091)); + + public static Translation3d bottomRightSpeaker = + new Translation3d(0.0, Units.inchesToMeters(238.815), Units.inchesToMeters(78.324)); + public static Translation3d bottomLeftSpeaker = + new Translation3d(0.0, Units.inchesToMeters(197.765), Units.inchesToMeters(78.324)); + + public static AprilTagFieldLayout fieldLayout; + + static { + try { + fieldLayout = AprilTagFieldLayout.loadFromResource(k2024Crescendo.m_resourceFile); + } catch (IOException e) { + throw new RuntimeException(e); + } + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8c9fc807..2920236c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,15 +13,19 @@ package frc.robot; +import com.pathplanner.lib.path.*; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.DriveCommands; +import frc.robot.commands.DriveTrajectory; import frc.robot.commands.FeedForwardCharacterization; +import frc.robot.commands.autos.TestAutos; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.GyroIO; import frc.robot.subsystems.drive.GyroIOPigeon2; @@ -29,6 +33,12 @@ import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOSparkMax; import frc.robot.subsystems.kitbotshooter.*; +import frc.robot.util.trajectory.ChoreoTrajectoryReader; +import frc.robot.util.trajectory.Trajectory; +import java.io.File; +import java.util.Objects; +import java.util.Optional; +import java.util.function.Function; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; @@ -113,6 +123,26 @@ public RobotContainer() { shooter, shooter::runFlywheelVolts, shooter::getFlywheelVelocityRadPerSec), Commands.runOnce(() -> shooter.setCurrentMode(null)))); + // Testing autos paths + Function> trajectoryCommandFactory = + trajectoryFile -> { + Optional trajectory = ChoreoTrajectoryReader.generate(trajectoryFile); + return trajectory.map( + traj -> + Commands.sequence( + Commands.runOnce(() -> drive.setPose(traj.startPose()), drive), + new DriveTrajectory(drive, traj))); + }; + final File rootTrajectoryDir = new File(Filesystem.getDeployDirectory(), "choreo"); + for (File trajectoryFile : Objects.requireNonNull(rootTrajectoryDir.listFiles())) { + trajectoryCommandFactory + .apply(trajectoryFile) + .ifPresent( + command -> { + autoChooser.addOption(trajectoryFile.getName(), command); + }); + } + // Configure the button bindings configureButtonBindings(); } diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index e8d4faa5..636c3327 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -22,6 +22,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.DriveConstants; import java.util.function.DoubleSupplier; public class DriveCommands { @@ -60,9 +61,9 @@ public static Command joystickDrive( // Convert to field relative speeds & send command drive.runVelocity( ChassisSpeeds.fromFieldRelativeSpeeds( - linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), - linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), - omega * drive.getMaxAngularSpeedRadPerSec(), + linearVelocity.getX() * DriveConstants.maxLinearSpeed, + linearVelocity.getY() * DriveConstants.maxLinearSpeed, + omega * DriveConstants.maxAngularSpeed, drive.getRotation())); }, drive); diff --git a/src/main/java/frc/robot/commands/DriveTrajectory.java b/src/main/java/frc/robot/commands/DriveTrajectory.java new file mode 100644 index 00000000..1d22c559 --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveTrajectory.java @@ -0,0 +1,124 @@ +package frc.robot.commands; + +import static frc.robot.util.trajectory.HolonomicDriveController.HolonomicDriveState; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.DriveConstants; +import frc.robot.util.AllianceFlipUtil; +import frc.robot.util.LoggedTunableNumber; +import frc.robot.util.trajectory.HolonomicDriveController; +import frc.robot.util.trajectory.Trajectory; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; + +public class DriveTrajectory extends Command { + private static final LoggedTunableNumber driveKp = + new LoggedTunableNumber("DriveTrajectory/DriveKp"); + private static final LoggedTunableNumber driveKd = + new LoggedTunableNumber("DriveTrajectory/DriveKd"); + private static final LoggedTunableNumber turnKp = + new LoggedTunableNumber("DriveTrajectory/TurnKp"); + private static final LoggedTunableNumber turnKd = + new LoggedTunableNumber("DriveTrajectory/TurnKd"); + + // initialize tunable numbers to constants + static { + driveKp.initDefault(DriveConstants.trajectoryConstants().drivekp()); + driveKd.initDefault(DriveConstants.trajectoryConstants().drivekd()); + turnKp.initDefault(DriveConstants.trajectoryConstants().turnkp()); + turnKd.initDefault(DriveConstants.trajectoryConstants().turnkd()); + } + + public static final PIDController linearController = new PIDController(0.0, 0.0, 0.0); + public static final PIDController thetaController = new PIDController(0.0, 0.0, 0.0); + public static final HolonomicDriveController controller = + new HolonomicDriveController(linearController, thetaController); + + private final Drive drive; + private final Supplier trajectorySupplier; + private Trajectory trajectory; + private final Timer timer = new Timer(); + + public DriveTrajectory(Drive drive, Supplier trajectorySupplier) { + this.drive = drive; + this.trajectorySupplier = trajectorySupplier; + + addRequirements(drive); + } + + public DriveTrajectory(Drive drive, Trajectory trajectory) { + this(drive, () -> trajectory); + } + + @Override + public void initialize() { + // Log trajectory + trajectory = trajectorySupplier.get(); + Logger.recordOutput("Odometry/Trajectory", trajectory.getTrajectoryPoses()); + + // Reset all controllers + timer.reset(); + timer.start(); + linearController.reset(); + thetaController.reset(); + + // Reset PID gains + linearController.setP(driveKp.get()); + linearController.setD(driveKd.get()); + thetaController.setP(turnKp.get()); + thetaController.setD(turnKd.get()); + } + + @Override + public void execute() { + if (driveKp.hasChanged(hashCode()) + || driveKd.hasChanged(hashCode()) + || turnKp.hasChanged(hashCode()) + || turnKd.hasChanged(hashCode())) { + linearController.setP(driveKp.get()); + linearController.setD(driveKd.get()); + thetaController.setP(turnKp.get()); + thetaController.setD(turnKd.get()); + } + + // sample and maybe flip trajectory state + HolonomicDriveState referenceState = trajectory.sample(timer.get()); + referenceState = AllianceFlipUtil.apply(referenceState); + + Pose2d referencePose = referenceState.pose(); + Logger.recordOutput("Odometry/TrajectorySetpoint", referencePose); + + ChassisSpeeds outputSpeeds = controller.calculate(drive.getPose(), referenceState); + drive.runVelocity(outputSpeeds); + + Logger.recordOutput( + "Odometry/TrajectoryTranslationErrorMeters", + controller.getPoseError().getTranslation().getNorm()); + Logger.recordOutput( + "Odometry/TrajectoryRotationErrorRadians", controller.getRotationError().getRadians()); + Logger.recordOutput( + "Drive/autoSpeeds", + new double[] { + outputSpeeds.vxMetersPerSecond, + outputSpeeds.vyMetersPerSecond, + outputSpeeds.omegaRadiansPerSecond + }); + } + + @Override + public boolean isFinished() { + return timer.hasElapsed(trajectory.getDuration()); + } + + @Override + public void end(boolean interrupted) { + drive.stop(); + Logger.recordOutput("Odometry/Trajectory", new Pose2d[] {}); + Logger.recordOutput("Odometry/TrajectorySetpoint", new double[] {}); + } +} diff --git a/src/main/java/frc/robot/commands/FeedForwardCharacterization.java b/src/main/java/frc/robot/commands/FeedForwardCharacterization.java index 391a3d31..d7ac7a7a 100644 --- a/src/main/java/frc/robot/commands/FeedForwardCharacterization.java +++ b/src/main/java/frc/robot/commands/FeedForwardCharacterization.java @@ -24,7 +24,7 @@ public class FeedForwardCharacterization extends Command { private static final double START_DELAY_SECS = 2.0; - private static final double RAMP_VOLTS_PER_SEC = 0.4; + private static final double RAMP_VOLTS_PER_SEC = 0.1; private FeedForwardCharacterizationData data; private final Consumer voltageConsumer; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index ac2f489c..54426345 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -21,30 +21,25 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.Arrays; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Drive extends SubsystemBase { - private static final double MAX_LINEAR_SPEED = Units.inchesToMeters(140.0); - private static final double TRACK_WIDTH_X = Units.inchesToMeters(26.0); - private static final double TRACK_WIDTH_Y = Units.inchesToMeters(26.0); - private static final double DRIVE_BASE_RADIUS = - Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); - private static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; - public static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[4]; // FL, FR, BL, BR - - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); + private final SwerveDriveKinematics kinematics = + new SwerveDriveKinematics(DriveConstants.moduleTranslations); private Pose2d pose = new Pose2d(); private Rotation2d lastGyroRotation = new Rotation2d(); + private ChassisSpeeds robotVelocity = new ChassisSpeeds(); + private ChassisSpeeds fieldVelocity = new ChassisSpeeds(); public Drive( GyroIO gyroIO, @@ -110,6 +105,21 @@ public void periodic() { // Apply the twist (change since last sample) to the current pose pose = pose.exp(twist); } + + // update current velocities + ChassisSpeeds robotRelativeVelocity = kinematics.toChassisSpeeds(getModuleStates()); + robotVelocity = robotRelativeVelocity; + Translation2d linearFieldVel = + new Translation2d( + robotRelativeVelocity.vxMetersPerSecond, robotRelativeVelocity.vyMetersPerSecond) + .rotateBy(getRotation()); + fieldVelocity = + new ChassisSpeeds( + linearFieldVel.getX(), + linearFieldVel.getY(), + gyroInputs.connected + ? gyroInputs.yawVelocityRadPerSec + : robotRelativeVelocity.omegaRadiansPerSecond); } /** @@ -121,7 +131,7 @@ public void runVelocity(ChassisSpeeds speeds) { // Calculate module setpoints ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); - SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, MAX_LINEAR_SPEED); + SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, DriveConstants.maxLinearSpeed); // Send setpoints to modules SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; @@ -147,7 +157,7 @@ public void stop() { public void stopWithX() { Rotation2d[] headings = new Rotation2d[4]; for (int i = 0; i < 4; i++) { - headings[i] = getModuleTranslations()[i].getAngle(); + headings[i] = DriveConstants.moduleTranslations[i].getAngle(); } kinematics.resetHeadings(headings); stop(); @@ -172,11 +182,7 @@ public double getCharacterizationVelocity() { /** Returns the module states (turn angles and drive velocities) for all of the modules. */ @AutoLogOutput(key = "SwerveStates/Measured") private SwerveModuleState[] getModuleStates() { - SwerveModuleState[] states = new SwerveModuleState[4]; - for (int i = 0; i < 4; i++) { - states[i] = modules[i].getState(); - } - return states; + return Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new); } /** Returns the current odometry pose. */ @@ -195,23 +201,15 @@ public void setPose(Pose2d pose) { this.pose = pose; } - /** Returns the maximum linear speed in meters per sec. */ - public double getMaxLinearSpeedMetersPerSec() { - return MAX_LINEAR_SPEED; - } - - /** Returns the maximum angular speed in radians per sec. */ - public double getMaxAngularSpeedRadPerSec() { - return MAX_ANGULAR_SPEED; + /** Get current robot relative velocity of robot */ + @AutoLogOutput(key = "Odometry/RobotVelocity") + public ChassisSpeeds getRobotVelocity() { + return robotVelocity; } - /** Returns an array of module translations. */ - public static Translation2d[] getModuleTranslations() { - return new Translation2d[] { - new Translation2d(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0), - new Translation2d(TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0), - new Translation2d(-TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0), - new Translation2d(-TRACK_WIDTH_X / 2.0, -TRACK_WIDTH_Y / 2.0) - }; + /** Get current field velocity of robot */ + @AutoLogOutput(key = "Odometry/FieldVelocity") + public ChassisSpeeds getFieldVelocity() { + return fieldVelocity; } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 70e571ef..52ac3a54 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -1,3 +1,30 @@ package frc.robot.subsystems.drive; -public class DriveConstants {} +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import frc.robot.Constants; + +public final class DriveConstants { + public static final double maxLinearSpeed = Units.feetToMeters(12.16); + public static final double trackwidthX = Units.inchesToMeters(26.0); + public static final double trackwidthY = Units.inchesToMeters(26.0); + public static final double driveBaseRadius = Math.hypot(trackwidthX / 2.0, trackwidthY / 2.0); + public static final double maxAngularSpeed = maxLinearSpeed / driveBaseRadius; + public static final Translation2d[] moduleTranslations = + new Translation2d[] { + new Translation2d(trackwidthX / 2.0, trackwidthY / 2.0), + new Translation2d(trackwidthX / 2.0, -trackwidthY / 2.0), + new Translation2d(-trackwidthX / 2.0, trackwidthY / 2.0), + new Translation2d(-trackwidthX / 2.0, -trackwidthY / 2.0) + }; + + // Replace with robots + public static TrajectoryConstants trajectoryConstants() { + return switch (Constants.currentMode) { + case REPLAY, REAL -> new TrajectoryConstants(6.0, 0.0, 8.0, 0.0); + case SIM -> new TrajectoryConstants(2.5, 0.0, 7.5, 0.0); + }; + } + + public record TrajectoryConstants(double drivekp, double drivekd, double turnkp, double turnkd) {} +} diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java new file mode 100644 index 00000000..20958122 --- /dev/null +++ b/src/main/java/frc/robot/util/LoggedTunableNumber.java @@ -0,0 +1,87 @@ +package frc.robot.util; + +import frc.robot.Constants; +import java.util.HashMap; +import java.util.Map; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; + +/** + * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or + * value not in dashboard. + */ +public class LoggedTunableNumber { + private static final String tableKey = "TunableNumbers"; + + private final String key; + private boolean hasDefault = false; + private double defaultValue; + private LoggedDashboardNumber dashboardNumber; + private Map lastHasChangedValues = new HashMap<>(); + + /** + * Create a new LoggedTunableNumber + * + * @param dashboardKey Key on dashboard + */ + public LoggedTunableNumber(String dashboardKey) { + this.key = tableKey + "/" + dashboardKey; + } + + /** + * Create a new LoggedTunableNumber with the default value + * + * @param dashboardKey Key on dashboard + * @param defaultValue Default value + */ + public LoggedTunableNumber(String dashboardKey, double defaultValue) { + this(dashboardKey); + initDefault(defaultValue); + } + + /** + * Set the default value of the number. The default value can only be set once. + * + * @param defaultValue The default value + */ + public void initDefault(double defaultValue) { + if (!hasDefault) { + hasDefault = true; + this.defaultValue = defaultValue; + if (Constants.tuningMode) { + dashboardNumber = new LoggedDashboardNumber(key, defaultValue); + } + } + } + + /** + * Get the current value, from dashboard if available and in tuning mode. + * + * @return The current value + */ + public double get() { + if (!hasDefault) { + return 0.0; + } else { + return Constants.tuningMode ? dashboardNumber.get() : defaultValue; + } + } + + /** + * Checks whether the number has changed since our last check + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple + * objects. Recommended approach is to pass the result of "hashCode()" + * @return True if the number has changed since the last time this method was called, false + * otherwise. + */ + public boolean hasChanged(int id) { + double currentValue = get(); + Double lastValue = lastHasChangedValues.get(id); + if (lastValue == null || currentValue != lastValue) { + lastHasChangedValues.put(id, currentValue); + return true; + } + + return false; + } +} diff --git a/src/main/java/frc/robot/util/trajectory/ChoreoTrajectoryReader.java b/src/main/java/frc/robot/util/trajectory/ChoreoTrajectoryReader.java index 59917753..76543436 100644 --- a/src/main/java/frc/robot/util/trajectory/ChoreoTrajectoryReader.java +++ b/src/main/java/frc/robot/util/trajectory/ChoreoTrajectoryReader.java @@ -17,14 +17,12 @@ public final class ChoreoTrajectoryReader { private static ObjectMapper objectMapper = new ObjectMapper(); /** Load Trajectory file (.traj) */ - public Optional generate(File file) { - String type = file.getName().substring(file.getName().lastIndexOf('.')); - // Not generated from code or choreo - if (!type.equals(".traj") || !type.equals(".json")) return Optional.empty(); - + public static Optional generate(File file) { List choreoStates = new ArrayList<>(); try { - choreoStates = objectMapper.readValue(file, new TypeReference<>() {}); + choreoStates = + objectMapper.convertValue( + objectMapper.readTree(file).get("samples"), new TypeReference<>() {}); } catch (IOException e) { System.out.println("Failed to read states from file"); return Optional.empty(); @@ -38,10 +36,16 @@ private static Trajectory generateTrajectoryImplementation( final List states) { return new Trajectory() { @Override - public double getTotatTimeSeconds() { + public double getDuration() { return states.get(states.size() - 1).timestamp; } + @Override + public Pose2d startPose() { + ChoreoTrajectoryState start = states.get(0); + return new Pose2d(start.x, start.y, new Rotation2d(start.angularVelocity)); + } + @Override public Pose2d[] getTrajectoryPoses() { return states.stream() @@ -105,20 +109,12 @@ private static HolonomicDriveState fromChoreoState(ChoreoTrajectoryState state) state.angularVelocity()); } - interface Trajectory { - double getTotatTimeSeconds(); - - Pose2d[] getTrajectoryPoses(); - - HolonomicDriveState sample(double timeSeconds); - } - record ChoreoTrajectoryState( - double timestamp, double x, double y, double heading, + double angularVelocity, double velocityX, double velocityY, - double angularVelocity) {} + double timestamp) {} } diff --git a/src/main/java/frc/robot/util/trajectory/HolonomicDriveController.java b/src/main/java/frc/robot/util/trajectory/HolonomicDriveController.java index 9c1ca8e0..22176c3b 100644 --- a/src/main/java/frc/robot/util/trajectory/HolonomicDriveController.java +++ b/src/main/java/frc/robot/util/trajectory/HolonomicDriveController.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; public class HolonomicDriveController { @@ -25,8 +26,13 @@ public ChassisSpeeds calculate(Pose2d currentPose, HolonomicDriveState state) { m_rotationError = poseRef.getRotation().minus(currentPose.getRotation()); // Calculate feedback velocities (based on position error). - double xFeedback = linearController.calculate(currentPose.getX(), poseRef.getX()); - double yFeedback = linearController.calculate(currentPose.getY(), poseRef.getY()); + Translation2d currToStateTranslation = + poseRef.getTranslation().minus(currentPose.getTranslation()); + double linearFeedback = + linearController.calculate( + 0, currentPose.getTranslation().getDistance(poseRef.getTranslation())); + double xFeedback = linearFeedback * (currToStateTranslation.getAngle().getCos()); + double yFeedback = linearFeedback * (currToStateTranslation.getAngle().getSin()); double thetaFeedback = thetaController.calculate( currentPose.getRotation().getRadians(), poseRef.getRotation().getRadians()); diff --git a/src/main/java/frc/robot/util/trajectory/Trajectory.java b/src/main/java/frc/robot/util/trajectory/Trajectory.java new file mode 100644 index 00000000..ccaedf59 --- /dev/null +++ b/src/main/java/frc/robot/util/trajectory/Trajectory.java @@ -0,0 +1,13 @@ +package frc.robot.util.trajectory; + +import edu.wpi.first.math.geometry.Pose2d; + +public interface Trajectory { + double getDuration(); + + Pose2d startPose(); + + Pose2d[] getTrajectoryPoses(); + + HolonomicDriveController.HolonomicDriveState sample(double timeSeconds); +}