diff --git a/src/main/deploy/pathplanner/autos/HubCenter-2Score (side).auto b/src/main/deploy/pathplanner/autos/HubCenter-2Score (side).auto index cd42020..63b80c8 100644 --- a/src/main/deploy/pathplanner/autos/HubCenter-2Score (side).auto +++ b/src/main/deploy/pathplanner/autos/HubCenter-2Score (side).auto @@ -46,12 +46,6 @@ "pathName": "Depot-Intake-Shoot (side)" } }, - { - "type": "named", - "data": { - "name": "Intake Retracted" - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/HubCenter-2Score (straight).auto b/src/main/deploy/pathplanner/autos/HubCenter-2Score (straight).auto index 89837a2..f0941d9 100644 --- a/src/main/deploy/pathplanner/autos/HubCenter-2Score (straight).auto +++ b/src/main/deploy/pathplanner/autos/HubCenter-2Score (straight).auto @@ -7,13 +7,19 @@ { "type": "named", "data": { - "name": "Robot Rev Up" + "name": "Intake Retracted" } }, { "type": "named", "data": { - "name": "Intake Retracted" + "name": "Robot Roller Off" + } + }, + { + "type": "named", + "data": { + "name": "Robot Rev Up" } }, { @@ -49,7 +55,7 @@ { "type": "named", "data": { - "name": "Intake Retracted" + "name": "Robot Roller Off" } }, { diff --git a/src/main/deploy/pathplanner/paths/Center-Hub-Shoot.path b/src/main/deploy/pathplanner/paths/Center-Hub-Shoot.path index 30b94a0..109ecd1 100644 --- a/src/main/deploy/pathplanner/paths/Center-Hub-Shoot.path +++ b/src/main/deploy/pathplanner/paths/Center-Hub-Shoot.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.7, - "maxAcceleration": 2.0, + "maxVelocity": 1.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 500.0, "maxAngularAcceleration": 700.0, "nominalVoltage": 10.0, diff --git a/src/main/deploy/pathplanner/paths/Center-ToDepot.path b/src/main/deploy/pathplanner/paths/Center-ToDepot.path index cf2aee7..56ba7ae 100644 --- a/src/main/deploy/pathplanner/paths/Center-ToDepot.path +++ b/src/main/deploy/pathplanner/paths/Center-ToDepot.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.7, - "maxAcceleration": 2.0, + "maxVelocity": 1.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 500.0, "maxAngularAcceleration": 700.0, "nominalVoltage": 10.0, diff --git a/src/main/deploy/pathplanner/paths/Depot-Intake-Shoot (side).path b/src/main/deploy/pathplanner/paths/Depot-Intake-Shoot (side).path index 583ca74..bdff2af 100644 --- a/src/main/deploy/pathplanner/paths/Depot-Intake-Shoot (side).path +++ b/src/main/deploy/pathplanner/paths/Depot-Intake-Shoot (side).path @@ -16,15 +16,15 @@ }, { "anchor": { - "x": 0.485, + "x": 0.52, "y": 6.972 }, "prevControl": { - "x": 0.4543797151601425, + "x": 0.48937971516014256, "y": 7.437592646883011 }, "nextControl": { - "x": 0.5156202848398574, + "x": 0.5506202848398575, "y": 6.50640735311699 }, "isLocked": false, @@ -36,8 +36,8 @@ "y": 4.985190476190476 }, "prevControl": { - "x": 0.5545595169162219, - "y": 4.745062440329646 + "x": 0.554559516916223, + "y": 4.745062440329642 }, "nextControl": { "x": 0.36614285714285816, @@ -76,11 +76,11 @@ ], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.7, - "maxAcceleration": 2.0, + "maxVelocity": 0.4, + "maxAcceleration": 1.0, "maxAngularVelocity": 500.0, "maxAngularAcceleration": 700.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { @@ -93,5 +93,5 @@ "velocity": 0, "rotation": 180.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot-Intake-Shoot (straight).path b/src/main/deploy/pathplanner/paths/Depot-Intake-Shoot (straight).path index 9ca773e..3044e8a 100644 --- a/src/main/deploy/pathplanner/paths/Depot-Intake-Shoot (straight).path +++ b/src/main/deploy/pathplanner/paths/Depot-Intake-Shoot (straight).path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 1.8650000000000004, + "x": 1.8650000000000002, "y": 6.0 }, "isLocked": false, @@ -16,15 +16,15 @@ }, { "anchor": { - "x": 0.7, + "x": 0.4, "y": 5.95697619047619 }, "prevControl": { - "x": 0.9480694691784167, + "x": 0.648069469178417, "y": 5.987984874123494 }, "nextControl": { - "x": 0.4519305308215832, + "x": 0.1519305308215833, "y": 5.925967506828887 }, "isLocked": false, @@ -60,11 +60,11 @@ ], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.7, - "maxAcceleration": 2.0, + "maxVelocity": 0.4, + "maxAcceleration": 1.0, "maxAngularVelocity": 500.0, "maxAngularAcceleration": 700.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { @@ -77,5 +77,5 @@ "velocity": 0, "rotation": 180.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HubFarLeft-Shoot.path b/src/main/deploy/pathplanner/paths/HubFarLeft-Shoot.path index f3ea130..08780e9 100644 --- a/src/main/deploy/pathplanner/paths/HubFarLeft-Shoot.path +++ b/src/main/deploy/pathplanner/paths/HubFarLeft-Shoot.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.7, - "maxAcceleration": 2.0, + "maxVelocity": 1.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 500.0, "maxAngularAcceleration": 700.0, "nominalVoltage": 10.0, diff --git a/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone.path b/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone.path index 2815c96..e47dc82 100644 --- a/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone.path +++ b/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone.path @@ -96,8 +96,8 @@ ], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.7, - "maxAcceleration": 2.0, + "maxVelocity": 1.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 500.0, "maxAngularAcceleration": 700.0, "nominalVoltage": 10.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 303ae14..78d7f05 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -6,8 +6,8 @@ "New Folder" ], "autoFolders": [], - "defaultMaxVel": 1.7, - "defaultMaxAccel": 2.0, + "defaultMaxVel": 1.0, + "defaultMaxAccel": 1.0, "defaultMaxAngVel": 500.0, "defaultMaxAngAccel": 700.0, "defaultNominalVoltage": 10.0, diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b6b511f..fc4ec85 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -129,12 +129,18 @@ public class RobotContainer { new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake) ); + private Command RobotRollerOff = new SequentialCommandGroup( + new InstantCommand(() -> m_robotIntake.rollerStop(), m_robotIntake) + ); + private Command RobotShoot = new SequentialCommandGroup( // TEST NEW AUTO ALIGN //new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false), new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed), new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter), - new WaitCommand(5), + new WaitCommand(3), + RobotIntakeRetracted, + new WaitCommand(2), new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter), new InstantCommand(()->m_robotShooter.spinUpIdle(), m_robotShooter) ); @@ -161,6 +167,7 @@ public class RobotContainer { NamedCommands.registerCommand("Robot Rev Up", RobotRev); NamedCommands.registerCommand("Robot Intake Retracted", RobotIntakeRetracted); NamedCommands.registerCommand("Robot Shoot", RobotShoot); + NamedCommands.registerCommand("Robot Roller Off", RobotRollerOff); // NamedCommands.registerCommand("Lidar Intake", LidarIntake); NamedCommands.registerCommand("Intake Extended", IntakeExtended); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 873d59c..40411bc 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 122; - public static final String GIT_SHA = "249751108410352b9e80c7a12316efaca273ae7a"; - public static final String GIT_DATE = "2026-02-28 10:45:49 MST"; + public static final int GIT_REVISION = 125; + public static final String GIT_SHA = "86387f5f088f59fbb43dc22ef0253b87c70ff574"; + public static final String GIT_DATE = "2026-02-28 12:07:15 MST"; public static final String GIT_BRANCH = "shikhar-op-controls"; - public static final String BUILD_DATE = "2026-02-28 11:14:49 MST"; - public static final long BUILD_UNIX_TIME = 1772302489449L; + public static final String BUILD_DATE = "2026-02-28 12:48:24 MST"; + public static final long BUILD_UNIX_TIME = 1772308104087L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index c966714..c20be74 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -45,15 +45,15 @@ public class IntakeConstants { public static final Slot0Configs ARM_PID = new Slot0Configs() - .withKP(0.03) + .withKP(0.08) .withKI(0.0) - .withKD(0.0); + .withKD(0.06); public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.08); public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KI", 0); - public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0.1); + public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0.06);