diff --git a/src/main/deploy/pathplanner/paths/Blue1AutoPickup.path b/src/main/deploy/pathplanner/paths/Blue1AutoPickup.path new file mode 100644 index 00000000..2924902a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue1AutoPickup.path @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 6.837327548528971, + "y": 7.488209571013737 + }, + "prevControl": null, + "nextControl": { + "x": 7.042370324056887, + "y": 7.495952997604001 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.311335876233006, + "y": 7.488209571013737 + }, + "prevControl": { + "x": 7.124229405153969, + "y": 7.4842588431615615 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CollectNote1Blue" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "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": "Collect Notes", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue1Pickup.path b/src/main/deploy/pathplanner/paths/Blue1Pickup.path index 98dee82e..3eae59eb 100644 --- a/src/main/deploy/pathplanner/paths/Blue1Pickup.path +++ b/src/main/deploy/pathplanner/paths/Blue1Pickup.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 7.826510788156711, + "x": 7.311335876233006, "y": 7.488209571013737 }, "prevControl": { - "x": 7.37352033262625, + "x": 6.858345420702545, "y": 7.4697201646655556 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path b/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path new file mode 100644 index 00000000..c14f82a2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path @@ -0,0 +1,60 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 6.652433485047149, + "y": 6.240174642511447 + }, + "prevControl": null, + "nextControl": { + "x": 6.9020404707476075, + "y": 6.162819391165848 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.428277420657405, + "y": 6.034183692299009 + }, + "prevControl": { + "x": 7.100841096269087, + "y": 6.104348618953648 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CollectNote2Blue" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": -12.380756928807017, + "rotateFast": true + }, + { + "waypointRelativePos": 0, + "rotationDegrees": -13.172553423326905, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -16.460014812039997, + "rotateFast": false + }, + "reversed": false, + "folder": "Collect Notes", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue2Pickup.path b/src/main/deploy/pathplanner/paths/Blue2Pickup.path index 3fdd567f..801fd358 100644 --- a/src/main/deploy/pathplanner/paths/Blue2Pickup.path +++ b/src/main/deploy/pathplanner/paths/Blue2Pickup.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 7.068445127881247, - "y": 6.101504094900082 + "x": 7.323030030675445, + "y": 6.0692661556263285 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.808021381808529, - "y": 5.898120625070079 + "x": 7.428277420657405, + "y": 6.034183692299009 }, "prevControl": { - "x": 7.410499145322615, - "y": 6.018301766333263 + "x": 6.890346316305168, + "y": 6.139431082280968 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue3AutoPickupAmpSide.path b/src/main/deploy/pathplanner/paths/Blue3AutoPickupAmpSide.path new file mode 100644 index 00000000..c825b96f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue3AutoPickupAmpSide.path @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.276544769305755, + "y": 6.378845190122813 + }, + "prevControl": null, + "nextControl": { + "x": 7.6323720215007995, + "y": 6.5729839567787245 + }, + "isLocked": false, + "linkedName": "SourceSideShoot" + }, + { + "anchor": { + "x": 7.627078046178885, + "y": 5.005098101364294 + }, + "prevControl": { + "x": 7.118619371603878, + "y": 5.800142574336119 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": -52.028396238949526, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -54.688786560367, + "rotateFast": false + }, + "reversed": false, + "folder": "Collect Notes", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageAmp.path b/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageAmp.path new file mode 100644 index 00000000..553e130c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageAmp.path @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.276544769305755, + "y": 6.378845190122813 + }, + "prevControl": null, + "nextControl": { + "x": 3.9896948365325304, + "y": 3.2543996516564464 + }, + "isLocked": false, + "linkedName": "SourceSideShoot" + }, + { + "anchor": { + "x": 7.369806648445205, + "y": 4.057871591526659 + }, + "prevControl": { + "x": 6.255687848779034, + "y": 3.9259364705135598 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 0.0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 4.1058976263956195, + "rotateFast": false + }, + "reversed": false, + "folder": "Collect Notes", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageSource.path b/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageSource.path new file mode 100644 index 00000000..4c120035 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageSource.path @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.9896948365325304, + "y": 2.433470009797163 + }, + "prevControl": null, + "nextControl": { + "x": 4.165608331216663, + "y": 4.661707609129505 + }, + "isLocked": false, + "linkedName": "SourceSideBlueShoot" + }, + { + "anchor": { + "x": 7.311335876233006, + "y": 4.1046482092964185 + }, + "prevControl": { + "x": 6.138579245005459, + "y": 4.133967125077106 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 0.0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 1.5911402711946643, + "rotateFast": false + }, + "reversed": false, + "folder": "Collect Notes", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue4AutoPickupSourceSide.path b/src/main/deploy/pathplanner/paths/Blue4AutoPickupSourceSide.path new file mode 100644 index 00000000..ba80268f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue4AutoPickupSourceSide.path @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.9896948365325304, + "y": 2.433470009797163 + }, + "prevControl": null, + "nextControl": { + "x": 5.690191951812475, + "y": 1.0115025944337612 + }, + "isLocked": false, + "linkedName": "SourceSideBlueShoot" + }, + { + "anchor": { + "x": 7.393194957330086, + "y": 1.9646179463299098 + }, + "prevControl": { + "x": 6.191119410321849, + "y": 1.4955152938388905 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 22.988716802080663, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 20.136303428248127, + "rotateFast": false + }, + "reversed": false, + "folder": "Collect Notes", + "previewStartingState": null, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue4AutoPickupUnderStage.path b/src/main/deploy/pathplanner/paths/Blue4AutoPickupUnderStage.path new file mode 100644 index 00000000..02db20e2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue4AutoPickupUnderStage.path @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.276544769305755, + "y": 6.378845190122813 + }, + "prevControl": null, + "nextControl": { + "x": 2.934213868427737, + "y": 3.899415798831598 + }, + "isLocked": false, + "linkedName": "SourceSideShoot" + }, + { + "anchor": { + "x": 7.311335876233006, + "y": 3.0521743094768246 + }, + "prevControl": { + "x": 6.48105091081977, + "y": 4.02278912819934 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": -34.69515353123401, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -29.98163936884927, + "rotateFast": false + }, + "reversed": false, + "folder": "Collect Notes", + "previewStartingState": null, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue5AutoPickupSourceSide.path b/src/main/deploy/pathplanner/paths/Blue5AutoPickupSourceSide.path new file mode 100644 index 00000000..6e2b68e5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue5AutoPickupSourceSide.path @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.9896948365325304, + "y": 2.433470009797163 + }, + "prevControl": null, + "nextControl": { + "x": 5.674154254291414, + "y": 1.4968517686323124 + }, + "isLocked": false, + "linkedName": "SourceSideBlueShoot" + }, + { + "anchor": { + "x": 7.1593118684812875, + "y": 1.0641680542620358 + }, + "prevControl": { + "x": 5.814484107600694, + "y": 1.3682160697654735 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": -19.290046219188806, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -14.322719978203544, + "rotateFast": false + }, + "reversed": false, + "folder": "Collect Notes", + "previewStartingState": null, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path b/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path index 2551e29c..6c35530d 100644 --- a/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path +++ b/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 7.826510788156711, + "x": 7.311335876233006, "y": 7.488209571013737 }, "prevControl": null, "nextControl": { - "x": 7.918305018503843, + "x": 7.403130106580138, "y": 7.448538179873802 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BlueAmpShoot2.path b/src/main/deploy/pathplanner/paths/BlueAmpShoot2.path index 8ef427f2..3b288596 100644 --- a/src/main/deploy/pathplanner/paths/BlueAmpShoot2.path +++ b/src/main/deploy/pathplanner/paths/BlueAmpShoot2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.808021381808529, - "y": 5.898120625070079 + "x": 7.428277420657405, + "y": 6.034183692299009 }, "prevControl": null, "nextControl": { - "x": 7.899111279042554, - "y": 5.856857680169195 + "x": 7.51936731789143, + "y": 5.992920747398125 }, "isLocked": false, "linkedName": "CollectNote2Blue" diff --git a/src/main/deploy/pathplanner/paths/BlueSourceShoot4.path b/src/main/deploy/pathplanner/paths/BlueSourceShoot4.path index 7b1b24d2..f5b8732a 100644 --- a/src/main/deploy/pathplanner/paths/BlueSourceShoot4.path +++ b/src/main/deploy/pathplanner/paths/BlueSourceShoot4.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 14.783900564952129, - "y": 7.87016593976208 + "x": 3.9896948365325304, + "y": 2.433470009797163 }, "prevControl": { - "x": 10.293387337107893, - "y": 3.2109618297385225 + "x": -0.5008183913117055, + "y": -2.2257341002263944 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0f8395ee..206d729d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,6 +24,7 @@ import frc.robot.commands.AimRobotMoving; import frc.robot.commands.IntakeCommand; import frc.robot.commands.Autos; +import frc.robot.commands.CollectNote; import frc.robot.commands.Drive; import frc.robot.commands.ExampleCommand; @@ -298,6 +299,7 @@ private void registerNamedCommands() { NamedCommands.registerCommand("feedShooter", new InstantCommand(()->indexer.feederFeed(0.5), indexer)); NamedCommands.registerCommand("stopFeedingShooter", new InstantCommand(indexer::feederOff, indexer)); NamedCommands.registerCommand("intakeOn", new InstantCommand(()-> intake.intakeYes(1))); + if(intakeExists) {NamedCommands.registerCommand("autoPickup", new CollectNote(driveTrain, intake));} } public void teleopPeriodic() {