From cf21452f4bb6f6dc59331c3a41b46b6f9df48ebf Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Thu, 11 Jan 2024 15:39:25 -0500 Subject: [PATCH] feat(auto): Add three meter calibration path. --- .../deploy/pathplanner/autos/Calibration.auto | 25 ++++++++++ src/main/deploy/pathplanner/navgrid.json | 1 + .../pathplanner/paths/3m Backwards.path | 49 +++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 4 +- .../frc/robot/odometry/GyroscopeIOSim.java | 4 +- .../java/frc/robot/odometry/Odometry.java | 5 +- .../frc/robot/odometry/OdometryFactory.java | 6 +-- 7 files changed, 87 insertions(+), 7 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Calibration.auto create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/paths/3m Backwards.path diff --git a/src/main/deploy/pathplanner/autos/Calibration.auto b/src/main/deploy/pathplanner/autos/Calibration.auto new file mode 100644 index 0000000..993c91b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Calibration.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2.0, + "y": 7.0 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3m Backwards" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..bab0da9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"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 diff --git a/src/main/deploy/pathplanner/paths/3m Backwards.path b/src/main/deploy/pathplanner/paths/3m Backwards.path new file mode 100644 index 0000000..f03256c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3m Backwards.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.118033988749895, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.0, + "y": 7.0 + }, + "prevControl": { + "x": 3.479309367425449, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "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 41631c5..c98c4ff 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,6 +7,7 @@ import frc.robot.auto.Auto; import frc.robot.climber.Climber; import frc.robot.intake.Intake; +import frc.robot.odometry.Odometry; import frc.robot.shooter.Shooter; import frc.robot.swerve.Swerve; import frc.robot.vision.Vision; @@ -20,6 +21,7 @@ public class RobotContainer { private final Auto auto = Auto.getInstance(); private final Climber climber = Climber.getInstance(); private final Intake intake = Intake.getInstance(); + private final Odometry odometry = Odometry.getInstance(); private final Shooter shooter = Shooter.getInstance(); private final Swerve swerve = Swerve.getInstance(); private final Vision vision = Vision.getInstance(); @@ -48,7 +50,7 @@ public static RobotContainer getInstance() { /** Initializes subsystem telemetry. */ private void initializeTelemetry() { - Telemetry.initializeShuffleboards(arm, auto, climber, intake, shooter, swerve, vision); + Telemetry.initializeShuffleboards(arm, auto, climber, intake, odometry, shooter, swerve, vision); } /** Configures operator controller bindings. */ diff --git a/src/main/java/frc/robot/odometry/GyroscopeIOSim.java b/src/main/java/frc/robot/odometry/GyroscopeIOSim.java index b00f094..e594b44 100644 --- a/src/main/java/frc/robot/odometry/GyroscopeIOSim.java +++ b/src/main/java/frc/robot/odometry/GyroscopeIOSim.java @@ -12,11 +12,11 @@ public class GyroscopeIOSim implements GyroscopeIO { private final DoubleSupplier yawVelocityRotationsPerSecondSupplier; /** Creates a new simulated gyroscope. */ - public GyroscopeIOSim() { + public GyroscopeIOSim(Odometry odometry) { yawRotations = 0.0; yawVelocityRotationsPerSecondSupplier = - () -> Odometry.getInstance().getVelocity().getRotation().getRotations(); + () -> odometry.getVelocity().getRotation().getRotations(); } @Override diff --git a/src/main/java/frc/robot/odometry/Odometry.java b/src/main/java/frc/robot/odometry/Odometry.java index de03471..f466ac1 100644 --- a/src/main/java/frc/robot/odometry/Odometry.java +++ b/src/main/java/frc/robot/odometry/Odometry.java @@ -38,7 +38,7 @@ public class Odometry extends Subsystem { /** Creates a new instance of the odometry subsystem. */ private Odometry() { - gyroscope = OdometryFactory.createGyroscope(); + gyroscope = OdometryFactory.createGyroscope(this); gyroscope.configure(); swerveModulePositionsSupplier = () -> Swerve.getInstance().getModulePositions(); @@ -125,6 +125,9 @@ public void setPosition(Pose2d position) { * @return the velocity of the robot on the field. */ public Transform2d getVelocity() { + // TODO Guards against simulated odometry hardware calling before the swerve pose estimator is intialized + if (swervePoseEstimator == null) return new Transform2d(); + ChassisSpeeds chassisSpeeds = swerveChassisSpeedsSupplier.get(); Rotation2d rotation = getPosition().getRotation(); diff --git a/src/main/java/frc/robot/odometry/OdometryFactory.java b/src/main/java/frc/robot/odometry/OdometryFactory.java index 89931b8..0a79c4a 100644 --- a/src/main/java/frc/robot/odometry/OdometryFactory.java +++ b/src/main/java/frc/robot/odometry/OdometryFactory.java @@ -10,9 +10,9 @@ public class OdometryFactory { * * @return a gyroscope. */ - public static GyroscopeIO createGyroscope() { - if (Robot.isReal()) return new GyroscopeIOSim(); + public static GyroscopeIO createGyroscope(Odometry odometry) { + if (Robot.isReal()) return new GyroscopeIOSim(odometry); - return new GyroscopeIOSim(); + return new GyroscopeIOSim(odometry); } }