From d2a04f0c88576b79d1f24e2a6522c5a5f81063b9 Mon Sep 17 00:00:00 2001 From: emilycxliu Date: Sat, 3 Jun 2023 15:05:26 -0400 Subject: [PATCH 1/2] test pathplanner stop events --- src/main/deploy/pathplanner/emily_path.path | 86 ++++++++++++++ src/main/deploy/pathplanner/emily_path2.path | 111 ++++++++++++++++++ .../java/frc/robot/auto/AutoRoutines.java | 14 ++- .../java/frc/robot/commands/ResetGyro.java | 2 + 4 files changed, 211 insertions(+), 2 deletions(-) create mode 100644 src/main/deploy/pathplanner/emily_path.path create mode 100644 src/main/deploy/pathplanner/emily_path2.path diff --git a/src/main/deploy/pathplanner/emily_path.path b/src/main/deploy/pathplanner/emily_path.path new file mode 100644 index 0000000..d56a4e9 --- /dev/null +++ b/src/main/deploy/pathplanner/emily_path.path @@ -0,0 +1,86 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.8009972862050994, + "y": 3.5058563350209417 + }, + "prevControl": null, + "nextControl": { + "x": 2.1726783303190675, + "y": 0.3013630087410566 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "Bling Red" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.32240112600526, + "y": 3.3350839634010105 + }, + "prevControl": { + "x": 4.272173957881751, + "y": 3.3250385297763083 + }, + "nextControl": { + "x": 4.372628294128768, + "y": 3.3451293970257128 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "moveX", + "Bling Purple" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.328614060071723, + "y": 4.691979241778389 + }, + "prevControl": { + "x": 4.338111993759534, + "y": 4.710975109154009 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 1.6848484848485286, + "names": [ + "Bling Red" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/emily_path2.path b/src/main/deploy/pathplanner/emily_path2.path new file mode 100644 index 0000000..e5ca4a9 --- /dev/null +++ b/src/main/deploy/pathplanner/emily_path2.path @@ -0,0 +1,111 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.8009972862050994, + "y": 3.5058563350209417 + }, + "prevControl": null, + "nextControl": { + "x": 2.1726783303190675, + "y": 0.3013630087410566 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "Bling Red" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.32240112600526, + "y": 3.3350839634010105 + }, + "prevControl": { + "x": 4.272173957881751, + "y": 3.3250385297763083 + }, + "nextControl": { + "x": 4.372628294128768, + "y": 3.3451293970257128 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "moveX", + "Bling Purple" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.82, + "y": 3.34 + }, + "prevControl": { + "x": 4.782370111772756, + "y": 3.501733887350626 + }, + "nextControl": { + "x": 4.874899975363861, + "y": 3.1040389900328687 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.81462737361565, + "y": 4.721353803609864 + }, + "prevControl": { + "x": 4.824125307303461, + "y": 4.740349670985483 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [ + { + "position": 1.6848484848485286, + "names": [ + "Bling Red" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/auto/AutoRoutines.java b/src/main/java/frc/robot/auto/AutoRoutines.java index 105bbda..4f2abf7 100644 --- a/src/main/java/frc/robot/auto/AutoRoutines.java +++ b/src/main/java/frc/robot/auto/AutoRoutines.java @@ -60,6 +60,9 @@ public class AutoRoutines { List cube_1p0_bottom; List cone_0p5_top_charge; List cube_0p5_bottom; + List emily_path; + List emily_path2; + List cube_3p0_top; @@ -182,6 +185,7 @@ public AutoRoutines() { CompRobotContainer.setState))); eventMap.put("wait", new WaitCommand(0.3)); + eventMap.put("moveX", new TranslationCommand(0.5,0)); autoBuilder = new SwerveAutoBuilder( SwerveSubsystem.getInstance()::getPose, @@ -203,6 +207,9 @@ public AutoRoutines() { cube_1p0_bottom = PathPlanner.loadPathGroup("cube_1p0_bottom", 2.5, 3); cube_0p5_bottom = PathPlanner.loadPathGroup("cube_0p5_bottom", 2.5, 3); cube_3p0_top = PathPlanner.loadPathGroup("cube_3p0_top", 2.6, 3.1); + emily_path = PathPlanner.loadPathGroup("emily_path", 2, 1); + emily_path2 = PathPlanner.loadPathGroup("emily_path2", 2, 1); + emily_path2.remove(1); cone_0p5_top_charge = PathPlanner.loadPathGroup("cone_0p5_top_charge", 2.5, 3); @@ -243,11 +250,14 @@ public Command getAutonomousCommand(int selectAuto) { // return(autoBuilder.fullAuto(forward)); case 2: - return new ForceBrakesInAuto().alongWith(autoBuilder.fullAuto(cube_3p0_top)); + return (autoBuilder.fullAuto(emily_path)); + //return new ForceBrakesInAuto().alongWith(autoBuilder.fullAuto(cube_3p0_top)); // return(autoBuilder.fullAuto(curve)); case 3: - return (autoBuilder.fullAuto(cube_0p5_middle_charge)); + return (autoBuilder.fullAuto(emily_path2)); + + // return (autoBuilder.fullAuto(cube_0p5_middle_charge)); case 4: return (autoBuilder.fullAuto(cube_1p0_top)); diff --git a/src/main/java/frc/robot/commands/ResetGyro.java b/src/main/java/frc/robot/commands/ResetGyro.java index abaa116..98496b2 100644 --- a/src/main/java/frc/robot/commands/ResetGyro.java +++ b/src/main/java/frc/robot/commands/ResetGyro.java @@ -27,6 +27,8 @@ public void initialize() { Pose2d pose = SwerveSubsystem.getInstance().getPose(); Pose2d newPose = new Pose2d(pose.getTranslation(), new Rotation2d(0)); SwerveSubsystem.getInstance().resetOdometry(newPose); + System.out.println("after resetting gyro, heading ="+SwerveSubsystem.getInstance().getHeading().getDegrees()); + } // Called every time the scheduler runs while the command is scheduled. From 4cf7b0cb8fb26c03a4388bf3665e80d40bc9307c Mon Sep 17 00:00:00 2001 From: emilycxliu Date: Sat, 17 Jun 2023 19:07:07 -0400 Subject: [PATCH 2/2] Added two test paths for odometry calibration --- src/main/deploy/pathplanner/Test_3m.path | 49 +++++++++++++++++++ src/main/deploy/pathplanner/Test_5m.path | 49 +++++++++++++++++++ src/main/deploy/pathplanner/emily_path.path | 4 +- .../java/frc/robot/auto/AutoRoutines.java | 48 +++++------------- 4 files changed, 111 insertions(+), 39 deletions(-) create mode 100644 src/main/deploy/pathplanner/Test_3m.path create mode 100644 src/main/deploy/pathplanner/Test_5m.path diff --git a/src/main/deploy/pathplanner/Test_3m.path b/src/main/deploy/pathplanner/Test_3m.path new file mode 100644 index 0000000..fd5f986 --- /dev/null +++ b/src/main/deploy/pathplanner/Test_3m.path @@ -0,0 +1,49 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.0, + "y": 3.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.0, + "y": 3.0 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.0, + "y": 3.0 + }, + "prevControl": { + "x": 3.0, + "y": 3.0 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Test_5m.path b/src/main/deploy/pathplanner/Test_5m.path new file mode 100644 index 0000000..81c1e20 --- /dev/null +++ b/src/main/deploy/pathplanner/Test_5m.path @@ -0,0 +1,49 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.0, + "y": 3.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.0, + "y": 3.0 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.0, + "y": 3.0 + }, + "prevControl": { + "x": 5.0, + "y": 3.0 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/emily_path.path b/src/main/deploy/pathplanner/emily_path.path index d56a4e9..4a5089a 100644 --- a/src/main/deploy/pathplanner/emily_path.path +++ b/src/main/deploy/pathplanner/emily_path.path @@ -17,7 +17,7 @@ "isStopPoint": false, "stopEvent": { "names": [ - "Bling Red" + "Bling Yellow" ], "executionBehavior": "parallel", "waitBehavior": "none", @@ -45,7 +45,7 @@ "stopEvent": { "names": [ "moveX", - "Bling Purple" + "Bling Red" ], "executionBehavior": "parallel", "waitBehavior": "none", diff --git a/src/main/java/frc/robot/auto/AutoRoutines.java b/src/main/java/frc/robot/auto/AutoRoutines.java index 4f2abf7..f549496 100644 --- a/src/main/java/frc/robot/auto/AutoRoutines.java +++ b/src/main/java/frc/robot/auto/AutoRoutines.java @@ -49,7 +49,7 @@ /** Add your docs here. */ public class AutoRoutines { - SwerveAutoBuilder autoBuilder; + public SwerveAutoBuilder autoBuilder; // HUMBER MAINS List cube_0p5_top_charge; @@ -63,6 +63,9 @@ public class AutoRoutines { List emily_path; List emily_path2; + List Test_3m; + List Test_5m; + List cube_3p0_top; @@ -211,6 +214,9 @@ public AutoRoutines() { emily_path2 = PathPlanner.loadPathGroup("emily_path2", 2, 1); emily_path2.remove(1); + Test_3m = PathPlanner.loadPathGroup("Test_3m", 2, 1); + Test_5m = PathPlanner.loadPathGroup("Test_5m", 2, 1); + cone_0p5_top_charge = PathPlanner.loadPathGroup("cone_0p5_top_charge", 2.5, 3); cone_2p0_bot_P1 = PathPlanner.loadPathGroup("cone_2p0_bot_P1", 2.5, 3); @@ -250,12 +256,14 @@ public Command getAutonomousCommand(int selectAuto) { // return(autoBuilder.fullAuto(forward)); case 2: - return (autoBuilder.fullAuto(emily_path)); + return (autoBuilder.fullAuto(Test_3m)); + //return (autoBuilder.fullAuto(emily_path)); //return new ForceBrakesInAuto().alongWith(autoBuilder.fullAuto(cube_3p0_top)); // return(autoBuilder.fullAuto(curve)); case 3: - return (autoBuilder.fullAuto(emily_path2)); + return (autoBuilder.fullAuto(Test_5m)); + // return (autoBuilder.fullAuto(emily_path2)); // return (autoBuilder.fullAuto(cube_0p5_middle_charge)); @@ -296,40 +304,6 @@ public Command getAutonomousCommand(int selectAuto) { )); // case 9: - // return (autoBuilder.fullAuto(cone_0p5_bottom_charge)); - - // case 10: - // return (autoBuilder.fullAuto(cone_1p0_top)); - - // case 11: - // return (autoBuilder.fullAuto(cone_1p0_bottom)); - - - // case 12: - // return (autoBuilder.fullAuto(BK_cube_2p0_bottom)); - - // case 13: - // return (autoBuilder.fullAuto(BK_cube_0p5_middle_charge)); - - // case 14: - // return (autoBuilder.fullAuto(BK_cone_0p5_charge)); - - // case 15: - // return (autoBuilder.fullAuto(place_pick_bottom2_charge_new)); - - // case 16: - // return (autoBuilder.fullAuto(place_pick_place_pick_place_bottom2)); - - // case 17: - // return (autoBuilder.fullAuto(place_pick_place_pick_place_bottom2_charge)); - - // case 18: - // return (autoBuilder.fullAuto(place_pick_place_pick_place_bottom_new)); - - // case 19: - // return (autoBuilder.fullAuto(cube_1p0_top_charge)); - - // case 20: // return new SequentialCommandGroup( // autoBuilder.fullAuto(cone_2p0_bot), // // new ArmCommand(ArmSetpoint.TOP_CONE_NO_WAYPOINT),