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 new file mode 100644 index 0000000..4a5089a --- /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 Yellow" + ], + "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 Red" + ], + "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..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; @@ -60,6 +60,12 @@ public class AutoRoutines { List cube_1p0_bottom; List cone_0p5_top_charge; List cube_0p5_bottom; + List emily_path; + List emily_path2; + + List Test_3m; + List Test_5m; + List cube_3p0_top; @@ -182,6 +188,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 +210,12 @@ 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); + + 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); @@ -243,11 +256,16 @@ public Command getAutonomousCommand(int selectAuto) { // return(autoBuilder.fullAuto(forward)); case 2: - return new ForceBrakesInAuto().alongWith(autoBuilder.fullAuto(cube_3p0_top)); + 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(cube_0p5_middle_charge)); + return (autoBuilder.fullAuto(Test_5m)); + // return (autoBuilder.fullAuto(emily_path2)); + + // return (autoBuilder.fullAuto(cube_0p5_middle_charge)); case 4: return (autoBuilder.fullAuto(cube_1p0_top)); @@ -286,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), 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.