diff --git a/src/main/deploy/pathplanner/cube_2p0_top_P1.path b/src/main/deploy/pathplanner/cube_2p0_top_P1.path new file mode 100644 index 0000000..4d6b294 --- /dev/null +++ b/src/main/deploy/pathplanner/cube_2p0_top_P1.path @@ -0,0 +1,197 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 2.8683931004225647, + "y": 4.673112891680329 + }, + "prevControl": null, + "nextControl": { + "x": 2.2580645161290325, + "y": 4.664003509825203 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "Gripper2PickCube", + "Arm2CubeTop" + ], + "executionBehavior": "sequential", + "waitBehavior": "after", + "waitTime": 0.2 + } + }, + { + "anchorPoint": { + "x": 1.7934860415175378, + "y": 4.445378345302146 + }, + "prevControl": { + "x": 2.531345971782853, + "y": 4.4635971090124 + }, + "nextControl": { + "x": 2.531345971782853, + "y": 4.4635971090124 + }, + "holonomicAngle": 179.50608310138125, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.622439790334126, + "y": 4.654894127970074 + }, + "prevControl": { + "x": 2.385595862100815, + "y": 4.5820190731290555 + }, + "nextControl": { + "x": 2.9543183547695113, + "y": 4.757010609334808 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.8066594315006816, + "y": 4.73687856466622 + }, + "prevControl": { + "x": 3.4149560117302062, + "y": 4.718659800955966 + }, + "nextControl": { + "x": 4.198362851271154, + "y": 4.755097328376475 + }, + "holonomicAngle": 90.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [ + "Arm2Pickup" + ], + "executionBehavior": "parallel", + "waitBehavior": "after", + "waitTime": 0.0 + } + }, + { + "anchorPoint": { + "x": 5.027038451026925, + "y": 4.752640344981246 + }, + "prevControl": { + "x": 4.726428849807722, + "y": 4.784523181474191 + }, + "nextControl": { + "x": 5.327648052246128, + "y": 4.7207575084883 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "Arm2Pickup" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.689960387554363, + "y": 4.589459560598511 + }, + "prevControl": { + "x": 5.485984407083318, + "y": 4.589459560592613 + }, + "nextControl": { + "x": 5.893936368028865, + "y": 4.58945956060441 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.148906343614214, + "y": 4.589459560598511 + }, + "prevControl": { + "x": 5.98036739252744, + "y": 4.589459560598511 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": 1.0, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 2.8, + "maxAcceleration": 3.5, + "isReversed": null, + "markers": [ + { + "position": 0.92, + "names": [ + "Gripper2Open" + ] + }, + { + "position": 1.08, + "names": [ + "Arm2Pickup" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/cube_2p0_top_P2.path b/src/main/deploy/pathplanner/cube_2p0_top_P2.path new file mode 100644 index 0000000..f5f0a5a --- /dev/null +++ b/src/main/deploy/pathplanner/cube_2p0_top_P2.path @@ -0,0 +1,115 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 7.268224536449073, + "y": 4.609347218694438 + }, + "prevControl": null, + "nextControl": { + "x": 6.421052023922229, + "y": 4.63667536425982 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.154847946059528, + "y": 4.745987946521348 + }, + "prevControl": { + "x": 5.910926640035097, + "y": 4.745987946521348 + }, + "nextControl": { + "x": 4.508081834345487, + "y": 4.745987946521348 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.6770960814648905, + "y": 4.709550419100839 + }, + "prevControl": { + "x": 3.0023938576741496, + "y": 4.797766765191485 + }, + "nextControl": { + "x": 2.1396425520123774, + "y": 4.563800309418801 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 2.2034082249982685, + "y": 3.97 + }, + "prevControl": { + "x": 2.5768928810584892, + "y": 4.071893689241924 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 2.8, + "maxAcceleration": 3.5, + "isReversed": null, + "markers": [ + { + "position": 0.3454545454545475, + "names": [ + "SetStateBaseCone" + ] + }, + { + "position": 1.0272727272727296, + "names": [ + "Arm2ConeTop" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/cube_2p0_top_P3.path b/src/main/deploy/pathplanner/cube_2p0_top_P3.path new file mode 100644 index 0000000..b67d0e1 --- /dev/null +++ b/src/main/deploy/pathplanner/cube_2p0_top_P3.path @@ -0,0 +1,109 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.7843766596624102, + "y": 3.88059667028425 + }, + "prevControl": null, + "nextControl": { + "x": 2.2307363705636503, + "y": 3.8897060521393776 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.041471355669984, + "y": 4.700441037245711 + }, + "prevControl": { + "x": 2.003001824185467, + "y": 4.691331655390584 + }, + "nextControl": { + "x": 3.5354631235836305, + "y": 4.70477429836776 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.100191654928764, + "y": 4.682222273535457 + }, + "prevControl": { + "x": 4.325840614497801, + "y": 4.682222273535457 + }, + "nextControl": { + "x": 6.0482678305920325, + "y": 4.682222273535457 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 7.259115154593945, + "y": 4.390722054171381 + }, + "prevControl": { + "x": 6.259115154593945, + "y": 4.390722054171381 + }, + "nextControl": null, + "holonomicAngle": -164.3577535427914, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 2.8, + "maxAcceleration": 3.5, + "isReversed": null, + "markers": [ + { + "position": 0.5, + "names": [ + "Arm2Pickup" + ] + } + ] +} \ 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 21d7636..fce07ac 100644 --- a/src/main/java/frc/robot/auto/AutoRoutines.java +++ b/src/main/java/frc/robot/auto/AutoRoutines.java @@ -73,6 +73,10 @@ public class AutoRoutines { List cone_2p0_bot_P1; List cone_2p0_bot_P2; + List cube_2p0_top_P1; + List cube_2p0_top_P2; + List cube_2p0_top_P3; + // List place_pick_place_pick_place_bottom2; // List cube_0p5_top_charge_good; @@ -211,6 +215,10 @@ public AutoRoutines() { cone_2p0_bot_P1 = PathPlanner.loadPathGroup("cone_2p0_bot_P1", 2.7, 3.8); cone_2p0_bot_P2 = PathPlanner.loadPathGroup("cone_2p0_bot_P2", 2.7, 3.8); + cube_2p0_top_P1 = PathPlanner.loadPathGroup("cube_2p0_top_P1", 2.7, 3.8); + cube_2p0_top_P2 = PathPlanner.loadPathGroup("cube_2p0_top_P2", 2.7, 3.8); + cube_2p0_top_P3 = PathPlanner.loadPathGroup("cube_2p0_top_P3", 2.7, 3.8); + forward = PathPlanner.loadPathGroup("TuningDriveForward", 2.5, 3); curve = PathPlanner.loadPathGroup("TuningDriveCurve", 2.5, 3); @@ -290,7 +298,30 @@ public Command getAutonomousCommand(int selectAuto) { return (autoBuilder.fullAuto(cube_1p0_top)); case 5: - return (autoBuilder.fullAuto(cube_0p5_bottom)); + // 1 cube 1 cone auto with vision. Starts top and starts with a cube. + DoubleSubscriber yawSub3 = NetworkTableInstance.getDefault().getTable("MergeVisionPipelineIntake22").getDoubleTopic("Yaw").subscribe(-99); + return new SequentialCommandGroup( + autoBuilder.fullAuto(cube_2p0_top_P1), // Run P1 PathPlanner. Will reset odometry to where vision aligned the robot. + new AlignToGamePiece(yawSub3, 1.25), // Use vision to align to game piece. Increase value here (currently 1.2 meters) if the robot doesn't drive far enough to grab the cone. + schedule(new SequentialCommandGroup( + new GripperCommand(GRIPPER_INSTRUCTION.PICK_UP_CONE, CompRobotContainer.setState), // Pick up the cone + new WaitCommand(0.3), // Give time for the cone is be grabbed aka pistons to extend + new ArmCommand(ArmSetpoint.HOME_AFTER_PICKUP))), // Lift arm to home position + new WaitCommand(0.5), // Give time for cone to be grabbed and arm lifted + autoBuilder.followPathGroupWithEvents(cube_2p0_top_P2), // Run P2 PathPlanner. Will not reset odometry. + new AlignToTargetVision(true, 1.0, 0.05, 0, Math.PI, 2.5, 2.5, true).withTimeout(2.5), // Use vision to align to tape node target. + schedule(new ParallelCommandGroup( + new ArmCommand(ArmSetpoint.TOP_CONE_RELEASE), // Lower the arm + new WaitCommand(0.2).andThen(new GripperCommand(GRIPPER_INSTRUCTION.OPEN, CompRobotContainer.setState)) // Then release the gripper + )).withTimeout(0.1), + new WaitCommand(0.35), // Give time to lower and release cone + autoBuilder.followPathGroupWithEvents(cube_2p0_top_P3) // Run P3 PathPlanner. Will not reset odometry. + ).withTimeout(14.95).andThen(brakes(true)); // Force auto to end at 14.95 seconds and ensure the brakes are on + + + + + // return (autoBuilder.fullAuto(cube_0p5_bottom)); case 6: return null;