From afa043d3349bd15980732a807b44e5afba92f69a Mon Sep 17 00:00:00 2001 From: Brandon Date: Tue, 10 Mar 2026 19:40:38 -0400 Subject: [PATCH 1/2] Citris Auton --- .../autos/Basic Comp Auto - CENTER.auto | 39 +++++----- .../deploy/pathplanner/autos/Citris copy.auto | 73 +++++++++++++++++++ .../deploy/pathplanner/autos/HP Shoot.auto | 32 ++++++++ ...ediate HP Auto (Around Tower) - RIGHT.auto | 6 -- .../autos/Immediate Shoot Auto - LEFT.auto | 6 -- .../autos/Immediate Shoot Auto - RIGHT.auto | 6 -- .../pathplanner/paths/Hub to pouch.path | 14 ++-- .../pathplanner/paths/cubby to shot.path | 54 ++++++++++++++ .../deploy/pathplanner/paths/mid to 2nd.path | 59 +++++++++++++++ .../pathplanner/paths/pouch to hub.path | 43 ++++------- .../deploy/pathplanner/paths/run mid.path | 54 ++++++++++++++ .../deploy/pathplanner/paths/shot to mid.path | 63 ++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 3 +- .../subsystems/intake/IntakeSubsystem.java | 1 + 14 files changed, 380 insertions(+), 73 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Citris copy.auto create mode 100644 src/main/deploy/pathplanner/paths/cubby to shot.path create mode 100644 src/main/deploy/pathplanner/paths/mid to 2nd.path create mode 100644 src/main/deploy/pathplanner/paths/run mid.path create mode 100644 src/main/deploy/pathplanner/paths/shot to mid.path diff --git a/src/main/deploy/pathplanner/autos/Basic Comp Auto - CENTER.auto b/src/main/deploy/pathplanner/autos/Basic Comp Auto - CENTER.auto index b446284..93af73f 100644 --- a/src/main/deploy/pathplanner/autos/Basic Comp Auto - CENTER.auto +++ b/src/main/deploy/pathplanner/autos/Basic Comp Auto - CENTER.auto @@ -10,29 +10,10 @@ "pathName": "Start to Shoot - CENTER" } }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AutonShoot" - } - }, - { - "type": "wait", - "data": { - "waitTime": 5.0 - } - } - ] - } - }, { "type": "named", "data": { - "name": "StopShot" + "name": "AutonShoot" } }, { @@ -53,11 +34,29 @@ "name": "HP Reload" } }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, { "type": "path", "data": { "pathName": "HP to Shoot - CENTER" } + }, + { + "type": "named", + "data": { + "name": "AutonShoot" + } + }, + { + "type": "named", + "data": { + "name": "Stow Intake" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/Citris copy.auto b/src/main/deploy/pathplanner/autos/Citris copy.auto new file mode 100644 index 0000000..0d0f0f7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Citris copy.auto @@ -0,0 +1,73 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "pouch to hub" + } + }, + { + "type": "path", + "data": { + "pathName": "Hub to pouch" + } + }, + { + "type": "named", + "data": { + "name": "Collect Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "cubby to shot" + } + }, + { + "type": "named", + "data": { + "name": "AutonShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "shot to mid" + } + }, + { + "type": "named", + "data": { + "name": "Collect Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "run mid" + } + }, + { + "type": "path", + "data": { + "pathName": "mid to 2nd" + } + }, + { + "type": "named", + "data": { + "name": "AutonShoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Test Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/HP Shoot.auto b/src/main/deploy/pathplanner/autos/HP Shoot.auto index 212149a..2b7862a 100644 --- a/src/main/deploy/pathplanner/autos/HP Shoot.auto +++ b/src/main/deploy/pathplanner/autos/HP Shoot.auto @@ -41,6 +41,38 @@ "pathName": "HP to Shoot - HP SHOOT" } }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AutonShoot" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "named", + "data": { + "name": "Stow Intake" + } + } + ] + } + } + ] + } + }, { "type": "parallel", "data": { diff --git a/src/main/deploy/pathplanner/autos/Immediate HP Auto (Around Tower) - RIGHT.auto b/src/main/deploy/pathplanner/autos/Immediate HP Auto (Around Tower) - RIGHT.auto index 1b54b17..766de5e 100644 --- a/src/main/deploy/pathplanner/autos/Immediate HP Auto (Around Tower) - RIGHT.auto +++ b/src/main/deploy/pathplanner/autos/Immediate HP Auto (Around Tower) - RIGHT.auto @@ -22,12 +22,6 @@ "pathName": "HP to Right Tower - HP AUTO" } }, - { - "type": "named", - "data": { - "name": "Collect Intake" - } - }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/autos/Immediate Shoot Auto - LEFT.auto b/src/main/deploy/pathplanner/autos/Immediate Shoot Auto - LEFT.auto index d2c2ba4..c0c4f48 100644 --- a/src/main/deploy/pathplanner/autos/Immediate Shoot Auto - LEFT.auto +++ b/src/main/deploy/pathplanner/autos/Immediate Shoot Auto - LEFT.auto @@ -22,12 +22,6 @@ "pathName": "Shoot to Overbump - SHOOT LEFT" } }, - { - "type": "named", - "data": { - "name": "Collect Intake" - } - }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/autos/Immediate Shoot Auto - RIGHT.auto b/src/main/deploy/pathplanner/autos/Immediate Shoot Auto - RIGHT.auto index 299a625..e5860c2 100644 --- a/src/main/deploy/pathplanner/autos/Immediate Shoot Auto - RIGHT.auto +++ b/src/main/deploy/pathplanner/autos/Immediate Shoot Auto - RIGHT.auto @@ -22,12 +22,6 @@ "pathName": "Shoot to Overbump - SHOOT RIGHT" } }, - { - "type": "named", - "data": { - "name": "Collect Intake" - } - }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/paths/Hub to pouch.path b/src/main/deploy/pathplanner/paths/Hub to pouch.path index 02225c9..c6d35d1 100644 --- a/src/main/deploy/pathplanner/paths/Hub to pouch.path +++ b/src/main/deploy/pathplanner/paths/Hub to pouch.path @@ -3,21 +3,21 @@ "waypoints": [ { "anchor": { - "x": 3.534450784593438, - "y": 4.1191012838801715 + "x": 0.509, + "y": 4.933 }, "prevControl": null, "nextControl": { - "x": 2.4087874465049923, - "y": 4.520199714693296 + "x": 0.5284570353904334, + "y": 6.183946200319118 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.7618544935805986, - "y": 4.960114122681883 + "x": 0.509, + "y": 7.082054208275134 }, "prevControl": { "x": 2.041483433139685, @@ -64,7 +64,7 @@ "folder": "Around the World", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 90.70975035955198 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/cubby to shot.path b/src/main/deploy/pathplanner/paths/cubby to shot.path new file mode 100644 index 0000000..b5c0188 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/cubby to shot.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.509, + "y": 7.082 + }, + "prevControl": null, + "nextControl": { + "x": 1.509, + "y": 7.082 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.0815977175463622, + "y": 4.9083594864479325 + }, + "prevControl": { + "x": 2.4734807417974314, + "y": 5.503537803138374 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -37.18470645323322 + }, + "reversed": false, + "folder": "Around the World", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/mid to 2nd.path b/src/main/deploy/pathplanner/paths/mid to 2nd.path new file mode 100644 index 0000000..7df721f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/mid to 2nd.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.675, + "y": 1.169 + }, + "prevControl": null, + "nextControl": { + "x": 8.675, + "y": 1.1690000000000005 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5511126961483592, + "y": 2.721726105563481 + }, + "prevControl": { + "x": 3.6767760342368043, + "y": 2.838174037089873 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6814345991561169, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 35.8376529542783 + }, + "reversed": false, + "folder": "Around the World", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/pouch to hub.path b/src/main/deploy/pathplanner/paths/pouch to hub.path index 6cfc547..420e2d7 100644 --- a/src/main/deploy/pathplanner/paths/pouch to hub.path +++ b/src/main/deploy/pathplanner/paths/pouch to hub.path @@ -3,48 +3,37 @@ "waypoints": [ { "anchor": { - "x": 0.5844365192582024, - "y": 4.895420827389444 + "x": 3.543971466460352, + "y": 3.996570284112104 }, "prevControl": null, "nextControl": { - "x": 0.5585592011412266, - "y": 5.839942938659058 + "x": 3.427697181024026, + "y": 5.269773709639866 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 0.4550499286733236, - "y": 7.0691155492154065 + "x": 0.5092126165722639, + "y": 4.932578281874523 }, "prevControl": { - "x": 0.45504992867332367, - "y": 6.3406334235631885 - }, - "nextControl": { - "x": 0.45504992867332367, - "y": 7.754864479315264 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.913395149786019, - "y": 5.076562054208274 - }, - "prevControl": { - "x": 2.162952924393723, - "y": 5.775249643366619 + "x": 1.0091920439484625, + "y": 4.950019424689972 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.4430379746835446, + "rotationDegrees": 91.23093843575762 + } + ], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -58,13 +47,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -49.23639479905885 + "rotation": 90.65854317756363 }, "reversed": false, "folder": "Around the World", "idealStartingState": { "velocity": 0, - "rotation": 90.9548412538722 + "rotation": 0.6902771978651236 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/run mid.path b/src/main/deploy/pathplanner/paths/run mid.path new file mode 100644 index 0000000..28baa90 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/run mid.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.675, + "y": 7.329 + }, + "prevControl": null, + "nextControl": { + "x": 7.652137361528301, + "y": 6.898425905918782 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.675, + "y": 1.1690870185449365 + }, + "prevControl": { + "x": 7.685206203526455, + "y": 2.233796687236829 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.01645068964164 + }, + "reversed": false, + "folder": "Around the World", + "idealStartingState": { + "velocity": 0, + "rotation": -90.01791124422833 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/shot to mid.path b/src/main/deploy/pathplanner/paths/shot to mid.path new file mode 100644 index 0000000..0f596d9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/shot to mid.path @@ -0,0 +1,63 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.082, + "y": 4.908 + }, + "prevControl": null, + "nextControl": { + "x": 3.7414693295364643, + "y": 6.150470756059642 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.674821683316775, + "y": 7.327888730382036 + }, + "prevControl": { + "x": 3.4568188302569487, + "y": 3.9250213979965998 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.07805907172995864, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.7130801687763653, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Around the World", + "idealStartingState": { + "velocity": 0, + "rotation": -42.357974387689254 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 164808d..b98f83f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -97,6 +97,7 @@ public RobotContainer() { NamedCommands.registerCommand("Stow Intake", intake.stowNoPIDCommand()); NamedCommands.registerCommand( "HP Reload", new WaitCommand(IntakePreferences.outpostReloadWait.getValue())); + NamedCommands.registerCommand("Mowing", intake.collectNoPIDCommand())//make a parameter that runs this action for a set amount of time when at mid-field autoChooser = AutoBuilder.buildAutoChooser("Auto Chooser"); autoChooser.addOption("Auton Shoot", autonShootCommand); SmartDashboard.putData("Auto Mode", autoChooser); @@ -191,7 +192,7 @@ public void configureTeleopBindings() { driverJoystick .b() - .whileTrue(intake.outtakeRollerNoPID().alongWith(indexer.startIndexerReverseNoPID())) + .whileTrue(intake.outtakeRollerNoPID().alongWith(indexer.startIndexerReverseNoPID()))/* .alongWith(shooter.spinFlywheelCommand))*/ .onFalse(intake.stopRollerNoPID().andThen(indexer.stopIndexerNoPID())); operatorJoystick diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index dbd544b..0293ad7 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -124,6 +124,7 @@ public Command setExtendNoPID() { .withName("Extend Intake Percent"); } + public Command setRetractNoPID() { return run(() -> extensionMotor.set(IntakePreferences.retractPercent.getValue())) .withName("Retract Intake Percent"); From 6df725044da53183f805c266e026e06a841e2933 Mon Sep 17 00:00:00 2001 From: William Crouch Date: Tue, 17 Mar 2026 20:27:52 -0400 Subject: [PATCH 2/2] Buildable --- src/main/java/frc/robot/RobotContainer.java | 12 ++++++++++-- .../frc/robot/subsystems/intake/IntakeSubsystem.java | 1 - 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b98f83f..dc9429e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -97,7 +97,10 @@ public RobotContainer() { NamedCommands.registerCommand("Stow Intake", intake.stowNoPIDCommand()); NamedCommands.registerCommand( "HP Reload", new WaitCommand(IntakePreferences.outpostReloadWait.getValue())); - NamedCommands.registerCommand("Mowing", intake.collectNoPIDCommand())//make a parameter that runs this action for a set amount of time when at mid-field + NamedCommands.registerCommand( + "Mowing", + intake.collectNoPIDCommand()); // make a parameter that runs this action for a set amount of + // time when at mid-field autoChooser = AutoBuilder.buildAutoChooser("Auto Chooser"); autoChooser.addOption("Auton Shoot", autonShootCommand); SmartDashboard.putData("Auto Mode", autoChooser); @@ -192,7 +195,12 @@ public void configureTeleopBindings() { driverJoystick .b() - .whileTrue(intake.outtakeRollerNoPID().alongWith(indexer.startIndexerReverseNoPID()))/* .alongWith(shooter.spinFlywheelCommand))*/ + .whileTrue( + intake + .outtakeRollerNoPID() + .alongWith( + indexer + .startIndexerReverseNoPID())) /* .alongWith(shooter.spinFlywheelCommand))*/ .onFalse(intake.stopRollerNoPID().andThen(indexer.stopIndexerNoPID())); operatorJoystick diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index 0293ad7..dbd544b 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -124,7 +124,6 @@ public Command setExtendNoPID() { .withName("Extend Intake Percent"); } - public Command setRetractNoPID() { return run(() -> extensionMotor.set(IntakePreferences.retractPercent.getValue())) .withName("Retract Intake Percent");