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..1934679 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/mode 1.auto @@ -0,0 +1,75 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 5.76, + "y": 3.93 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "1" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "named", + "data": { + "name": "lancer" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "2" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "named", + "data": { + "name": "lancer" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "3" + } + } + ] + } + }, + "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/2.path similarity index 54% rename from src/main/deploy/pathplanner/paths/New Path.path rename to src/main/deploy/pathplanner/paths/2.path index c7cd203..867b117 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/2.path @@ -3,41 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.764349477682812, - "y": 3.9299145299145297 + "x": 10.4, + "y": 5.2 }, "prevControl": null, "nextControl": { - "x": 6.764349477682812, - "y": 3.9299145299145297 + "x": 9.705754985754986, + "y": 4.745773979107312 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.039563152896486, - "y": 4.872174738841406 + "x": 9.3, + "y": 4.0 }, "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 + "x": 9.936805143100598, + "y": 4.758914817072607 }, "nextControl": null, "isLocked": false, @@ -55,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 90.0, "rotateFast": false }, "reversed": false, "folder": null, "previewStartingState": { - "rotation": 0, + "rotation": 90.60460638654384, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/3.path b/src/main/deploy/pathplanner/paths/3.path new file mode 100644 index 0000000..5364c8c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3.path @@ -0,0 +1,95 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 9.3, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 9.062260208926876, + "y": 3.550712250712251 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.545166191832859, + "y": 3.2749287749287745 + }, + "prevControl": { + "x": 9.07375118708452, + "y": 3.2749287749287745 + }, + "nextControl": { + "x": 8.004601461786502, + "y": 3.2749287749287745 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.441748878923768, + "y": 4.964798206278027 + }, + "prevControl": { + "x": 7.957769732906826, + "y": 4.951218710120577 + }, + "nextControl": { + "x": 7.005091708933264, + "y": 4.976289184435672 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.76, + "y": 3.93 + }, + "prevControl": { + "x": 6.374633781763827, + "y": 4.277503736920778 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 30.0, + "rotateFast": false + }, + { + "waypointRelativePos": 2, + "rotationDegrees": 0.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.8151092400891717, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 89.62831416675282, + "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 47ee2bc..8176d38 100644 --- a/src/main/java/frc/robot/Commands/FollowAprilTag.java +++ b/src/main/java/frc/robot/Commands/FollowAprilTag.java @@ -27,16 +27,12 @@ public class FollowAprilTag extends Command { @Override public void execute() { - if (lanceur.limitswitch1()){ - - } - else if(lanceur.limitswitch2()){ - - } - else if (enlignement.getv()==1) - { - lanceur.tourelRotation(0,0, enlignement.getx()/30); - } + + 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 39f3b18..9126cb5 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(3,3) .withPosition(5,0); @@ -37,6 +44,9 @@ public class RobotContainer { drive.setDefaultCommand(new RunCommand(()->{ drive.drive(-MathUtil.applyDeadband(manette.getRightX(),0.2), MathUtil.applyDeadband(-manette.getRightY(),0.2), MathUtil.applyDeadband(-manette.getRightX(), 0.2)); },drive)); + dashboard.add("autochooser",autoChooser) + .withSize(1,1) + .withPosition(6,0); } private void configureBindings() { @@ -47,6 +57,6 @@ public class RobotContainer { } public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); + return autoChooser.getSelected(); } }