From 20e9b3bf6ccd389bd20540493a3484534fd77d56 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Thu, 25 Jan 2024 22:33:55 -0500 Subject: [PATCH] [#64] More Pos logging --- .../pathplanner/autos/1MeterSquareLL.auto | 25 +++++ src/main/deploy/pathplanner/autos/SM-3.auto | 2 +- src/main/deploy/pathplanner/autos/Spin.auto | 2 +- .../pathplanner/autos/Through_Pieces.auto | 24 +++++ .../pathplanner/paths/1MeterSquareLL.path | 97 +++++++++++++++++++ .../pathplanner/paths/Run_Through_Pieces.path | 12 +-- .../paths/{Spin.path => Sipnslow.path} | 0 .../deploy/pathplanner/paths/StraightOut.path | 65 +++++++++++++ src/main/java/frc/robot/RobotContainer.java | 11 ++- .../java/frc/robot/subsystems/Swerve.java | 22 ++++- vendordeps/PathplannerLib.json | 6 +- 11 files changed, 247 insertions(+), 19 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/1MeterSquareLL.auto create mode 100644 src/main/deploy/pathplanner/paths/1MeterSquareLL.path rename src/main/deploy/pathplanner/paths/{Spin.path => Sipnslow.path} (100%) create mode 100644 src/main/deploy/pathplanner/paths/StraightOut.path diff --git a/src/main/deploy/pathplanner/autos/1MeterSquareLL.auto b/src/main/deploy/pathplanner/autos/1MeterSquareLL.auto new file mode 100644 index 00000000..48410087 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/1MeterSquareLL.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "test" + } + }, + { + "type": "path", + "data": { + "pathName": "1MeterSquareLL" + } + } + ] + } + }, + "folder": "Test Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SM-3.auto b/src/main/deploy/pathplanner/autos/SM-3.auto index 1414e5f7..97fb9a92 100644 --- a/src/main/deploy/pathplanner/autos/SM-3.auto +++ b/src/main/deploy/pathplanner/autos/SM-3.auto @@ -8,7 +8,7 @@ { "type": "path", "data": { - "pathName": "SM-3" + "pathName": "Sipnslow" } }, { diff --git a/src/main/deploy/pathplanner/autos/Spin.auto b/src/main/deploy/pathplanner/autos/Spin.auto index 5987af6a..66d3398b 100644 --- a/src/main/deploy/pathplanner/autos/Spin.auto +++ b/src/main/deploy/pathplanner/autos/Spin.auto @@ -14,7 +14,7 @@ { "type": "path", "data": { - "pathName": "Spin" + "pathName": "Sipnslow" } } ] diff --git a/src/main/deploy/pathplanner/autos/Through_Pieces.auto b/src/main/deploy/pathplanner/autos/Through_Pieces.auto index 9cbe39a5..dac2c604 100644 --- a/src/main/deploy/pathplanner/autos/Through_Pieces.auto +++ b/src/main/deploy/pathplanner/autos/Through_Pieces.auto @@ -11,11 +11,35 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "disable-Vision" + } + }, { "type": "path", "data": { "pathName": "Run_Through_Pieces" } + }, + { + "type": "named", + "data": { + "name": "test" + } + }, + { + "type": "path", + "data": { + "pathName": "StraightOut" + } + }, + { + "type": "named", + "data": { + "name": "test" + } } ] } diff --git a/src/main/deploy/pathplanner/paths/1MeterSquareLL.path b/src/main/deploy/pathplanner/paths/1MeterSquareLL.path new file mode 100644 index 00000000..8473de26 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/1MeterSquareLL.path @@ -0,0 +1,97 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.32, + "y": 5.525487974052871 + }, + "prevControl": null, + "nextControl": { + "x": 1.82, + "y": 5.525487974052871 + }, + "isLocked": false, + "linkedName": "SubwooferFront" + }, + { + "anchor": { + "x": 2.32, + "y": 5.53 + }, + "prevControl": { + "x": 1.8199999999999998, + "y": 5.53 + }, + "nextControl": { + "x": 2.38, + "y": 5.53 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.32, + "y": 6.53 + }, + "prevControl": { + "x": 2.32, + "y": 6.03 + }, + "nextControl": { + "x": 2.32, + "y": 6.59 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.32, + "y": 6.53 + }, + "prevControl": { + "x": 1.82, + "y": 6.53 + }, + "nextControl": { + "x": 1.26, + "y": 6.53 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.32, + "y": 5.525487974052871 + }, + "prevControl": { + "x": 1.32, + "y": 6.025487974052871 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "SubwooferFront" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Test paths", + "previewStartingState": null, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Run_Through_Pieces.path b/src/main/deploy/pathplanner/paths/Run_Through_Pieces.path index bef3c2aa..e8c3abdb 100644 --- a/src/main/deploy/pathplanner/paths/Run_Through_Pieces.path +++ b/src/main/deploy/pathplanner/paths/Run_Through_Pieces.path @@ -64,16 +64,16 @@ }, { "anchor": { - "x": 0.7319122330552369, - "y": 4.459858150485532 + "x": 1.32, + "y": 5.525487974052871 }, "prevControl": { - "x": 2.616498309919698, - "y": 4.09478126648561 + "x": 3.204586076864461, + "y": 5.160411090052949 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "SubwooferFront" } ], "rotationTargets": [ @@ -103,7 +103,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 118.35496178532406, + "rotation": 180.0, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Spin.path b/src/main/deploy/pathplanner/paths/Sipnslow.path similarity index 100% rename from src/main/deploy/pathplanner/paths/Spin.path rename to src/main/deploy/pathplanner/paths/Sipnslow.path diff --git a/src/main/deploy/pathplanner/paths/StraightOut.path b/src/main/deploy/pathplanner/paths/StraightOut.path new file mode 100644 index 00000000..980c607b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/StraightOut.path @@ -0,0 +1,65 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.32, + "y": 5.525487974052871 + }, + "prevControl": null, + "nextControl": { + "x": 2.4380339887498943, + "y": 5.525487974052871 + }, + "isLocked": false, + "linkedName": "SubwooferFront" + }, + { + "anchor": { + "x": 2.8631718801899155, + "y": 5.525487974052871 + }, + "prevControl": { + "x": 1.4489583178168204, + "y": 5.525487974052871 + }, + "nextControl": { + "x": 4.27738544256301, + "y": 5.525487974052871 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.458177839432603, + "y": 5.525487974052871 + }, + "prevControl": { + "x": 3.937487206858049, + "y": 5.525487974052871 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 180.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 ce5d2eab..c789dc1b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -52,7 +52,7 @@ public class RobotContainer extends LightningContainer { // Shooter shooter; // Collision collision; // Indexer indexer; - Climber climber; + // Climber climber; private SendableChooser autoChooser; // TODO I want field-centric driving in open loop WE NEED TO FIGURE OUT WHAT @@ -79,7 +79,7 @@ protected void initializeSubsystems() { // pivot = new Pivot(); // shooter = new Shooter(pivot, flywheel, indexer); // collision = new Collision(drivetrain); - climber = new Climber(); + // climber = new Climber(); drive = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage);//.withDeadband(DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SPEED_DB).withRotationalDeadband(DrivetrAinConstants.MaxAngularRate * DrivetrAinConstants.ROT_DB); // I want field-centric driving in closed loop slow = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage);//.withDeadband(DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SPEED_DB).withRotationalDeadband(DrivetrAinConstants.MaxAngularRate * DrivetrAinConstants.ROT_DB); // I want field-centric driving in closed loop @@ -93,6 +93,7 @@ protected void initializeSubsystems() { @Override protected void initializeNamedCommands() { NamedCommands.registerCommand("test", new InstantCommand(() -> System.out.println("Hello World!"))); + NamedCommands.registerCommand("disable-Vision", new InstantCommand(() -> drivetrain.disableVision())); } @Override @@ -109,9 +110,9 @@ protected void configureButtonBindings() { new Trigger(driver::getRightBumper).onTrue(new InstantCommand(() -> drivetrain.setSlowMode(true))).onFalse(new InstantCommand(() -> drivetrain.setSlowMode(false))); new Trigger(driver::getXButton).whileTrue(new PointAtTag(drivetrain, driver, "limelight-front", false)); new Trigger(driver::getBackButton).whileTrue(new TipDetection(drivetrain)); - new Trigger(driver::getXButton).whileTrue(new PointAtTag(drivetrain, driver, "limelight-front", false)); - new Trigger(driver::getYButton).whileTrue(new Climb(climber, ClimbConstants.CLIMB_PID_SETPOINT_EXTENDED)); + new Trigger(driver::getYButton).onTrue(new InstantCommand(() -> drivetrain.disableVision()).alongWith(new InstantCommand(() -> System.out.println("Vision Disabled")))); + // new Trigger(driver::getYButton).whileTrue(new Climb(climber, ClimbConstants.CLIMB_PID_SETPOINT_EXTENDED)); } @Override @@ -124,7 +125,7 @@ protected void configureDefaultCommands() { .withRotationalRate(-MathUtil.applyDeadband(driver.getRightX(), ControllerConstants.DEADBAND) * DrivetrAinConstants.MaxAngularRate * DrivetrAinConstants.ROT_MULT) // Drive counterclockwise with negative X (left) )); // climber.setDefaultCommand(new ManualClimb(() -> (coPilot.getRightTriggerAxis() - coPilot.getLeftTriggerAxis()), climber)); - climber.setDefaultCommand(new Climb(climber, ClimbConstants.CLIMB_PID_SETPOINT_RETRACTED)); + // climber.setDefaultCommand(new Climb(climber, ClimbConstants.CLIMB_PID_SETPOINT_RETRACTED)); // shooter.setDefaultCommand(new Shoot(shooter, indexer, drivetrain, () -> coPilot.getAButton())); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index f202139a..d2133ef3 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -33,6 +33,7 @@ public class Swerve extends SwerveDrivetrain implements Subsystem { private final SwerveRequest.ApplyChassisSpeeds autoRequest = new SwerveRequest.ApplyChassisSpeeds(); private Limelight[] limelights; private boolean slowMode = false; + private boolean disableVision = false; public Swerve(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, SwerveModuleConstants... modules) { @@ -66,9 +67,16 @@ public void simulationPeriodic() { public void periodic() { for (Limelight limelight : Limelight.filterLimelights(limelights)) { Pose4d pose = limelight.getAlliancePose(); - addVisionMeasurement(pose.toPose2d(), - Timer.getFPGATimestamp() - Units.millisecondsToSeconds(pose.getLatency()) - - VisionConstants.PROCESS_LATENCY); + // LightningShuffleboard.set("Swerve", "Vision pose", pose.toPose2d()); + LightningShuffleboard.setDouble("Swerve", "Vision pose X", pose.toPose2d().getX()); + LightningShuffleboard.setDouble("Swerve", "Vision pose Y", pose.toPose2d().getY()); + LightningShuffleboard.setDouble("Swerve", "Vision time offset", Timer.getFPGATimestamp() - Units.millisecondsToSeconds(pose.getLatency()) + - VisionConstants.PROCESS_LATENCY); + if(!disableVision) { + addVisionMeasurement(pose.toPose2d(), + Timer.getFPGATimestamp() - Units.millisecondsToSeconds(pose.getLatency()) + - VisionConstants.PROCESS_LATENCY); + } } LightningShuffleboard.setDouble("Swerve", "yaw", m_yawGetter.getValueAsDouble()); @@ -98,6 +106,14 @@ AutonomousConstants.DRIVE_BASE_RADIUS, new ReplanningConfig(), }, this); // Subsystem for requirements } + public void disableVision() { + disableVision = true; + } + + public void enableVision() { + disableVision = false; + } + public Supplier getPose() { return () -> getState().Pose; } diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 0bf11fbf..6ddd312f 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.1.4", + "version": "2024.1.5", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.1.4" + "version": "2024.1.5" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.1.4", + "version": "2024.1.5", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false,