diff --git a/src/main/deploy/pathplanner/autos/mode 1.auto b/src/main/deploy/pathplanner/autos/mode 1.auto new file mode 100644 index 0000000..c70435c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/mode 1.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 5.76, + "y": 3.93 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "1" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/1.path b/src/main/deploy/pathplanner/paths/1.path new file mode 100644 index 0000000..9a7ab9d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/1.path @@ -0,0 +1,137 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.76, + "y": 3.93 + }, + "prevControl": null, + "nextControl": { + "x": 5.801431310952625, + "y": 3.93 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.3, + "y": 3.2979107312440648 + }, + "prevControl": { + "x": 6.660542519857817, + "y": 3.2979107312440648 + }, + "nextControl": { + "x": 7.948552157357058, + "y": 3.2979107312440648 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.005090218423552, + "y": 4.1022792022792025 + }, + "prevControl": { + "x": 7.89972103271822, + "y": 3.7203159040973657 + }, + "nextControl": { + "x": 8.09701804368471, + "y": 4.435517568850902 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.774985754985755, + "y": 4.9411206077872745 + }, + "prevControl": { + "x": 8.087653449552494, + "y": 5.261875683656129 + }, + "nextControl": { + "x": 9.292079772079772, + "y": 4.699810066476734 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 10.41, + "y": 3.07 + }, + "prevControl": { + "x": 9.74022792022792, + "y": 2.8152896486229815 + }, + "nextControl": { + "x": 10.747612735931797, + "y": 3.19839212203024 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 10.4, + "y": 5.2 + }, + "prevControl": { + "x": 10.388509021842355, + "y": 5.2 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 0.0, + "rotateFast": false + }, + { + "waypointRelativePos": 2, + "rotationDegrees": 76.0, + "rotateFast": false + }, + { + "waypointRelativePos": 3, + "rotationDegrees": 90.0, + "rotateFast": false + }, + { + "waypointRelativePos": 4, + "rotationDegrees": 90.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.55, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path deleted file mode 100644 index c7cd203..0000000 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ /dev/null @@ -1,68 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 5.764349477682812, - "y": 3.9299145299145297 - }, - "prevControl": null, - "nextControl": { - "x": 6.764349477682812, - "y": 3.9299145299145297 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.039563152896486, - "y": 4.872174738841406 - }, - "prevControl": { - "x": 7.039563152896486, - "y": 4.872174738841406 - }, - "nextControl": { - "x": 9.039563152896486, - "y": 4.872174738841406 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.36131054131054, - "y": 6.009781576448243 - }, - "prevControl": { - "x": 9.056657169990501, - "y": 6.61880341880342 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "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": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/java/frc/robot/Commands/FollowAprilTag.java b/src/main/java/frc/robot/Commands/FollowAprilTag.java index 22a27e8..e05cb82 100644 --- a/src/main/java/frc/robot/Commands/FollowAprilTag.java +++ b/src/main/java/frc/robot/Commands/FollowAprilTag.java @@ -27,13 +27,13 @@ public class FollowAprilTag extends Command { @Override public void execute() { - - if (enlignement.getv()==1) - { - lanceur.tourelRotation(0,0, enlignement.getx()/30); - } if (lanceur.limitswitch1() || lanceur.limitswitch2()){ lanceur.lance(0);} + else if (enlignement.getv()==1) + { + lanceur.tourelRotation(0,0, enlignement.getx()/30); + } + else { lanceur.tourelRotation(0, 0, 0); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 20a4557..5aa2fea 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,9 +4,13 @@ package frc.robot; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.RunCommand; @@ -20,6 +24,7 @@ import frc.robot.Subsystems.Lanceur; import frc.robot.Subsystems.Limelight3G; public class RobotContainer { + private final SendableChooser autoChooser; Lanceur lanceur= new Lanceur(); Accumulateur accumulateur = new Accumulateur(); Limelight3G limelight3G = new Limelight3G(); @@ -27,6 +32,8 @@ public class RobotContainer { ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard"); CommandXboxController manette = new CommandXboxController(0); public RobotContainer() { + NamedCommands.registerCommand("lancer", new Lancer(lanceur, accumulateur)); + autoChooser = AutoBuilder.buildAutoChooser(); dashboard.addCamera("limelight3G", "limelight3G","limelight.local:5800") .withSize(4,3) .withPosition(0,0); @@ -47,6 +54,6 @@ public class RobotContainer { } public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); + return autoChooser.getSelected(); } }