From 118dcee1c7dca32d23fa40f275d9ba433046fcf4 Mon Sep 17 00:00:00 2001 From: mimigamin Date: Tue, 7 Apr 2026 20:37:26 -0600 Subject: [PATCH] swuidf --- .../deploy/pathplanner/paths/Bump test.path | 2 +- .../pathplanner/paths/BumpToCenter.path | 2 +- .../paths/CPlayerStation-CShoot.path | 2 +- .../pathplanner/paths/Center-NeutralL.path | 2 +- .../pathplanner/paths/Center-NeutralR.path | 2 +- .../pathplanner/paths/Center-ToDepot.path | 2 +- .../Copy of HubFarRight-NeutralZone 1-2.path | 2 +- .../paths/HubCenter-PlayerStation.path | 2 +- .../paths/HubFarLeft-NeutralZone 1-2.path | 2 +- .../paths/HubFarLeft-NeutralZone 2-2.path | 2 +- .../pathplanner/paths/HubFarLeft-Shoot.path | 2 +- .../paths/HubFarRight-NeutralZone 1-2.path | 2 +- .../paths/HubFarRight-NeutralZone 2-2.path | 2 +- .../pathplanner/paths/HubFarRight-Shoot.path | 2 +- .../pathplanner/paths/HubLeft-Shoot.path | 2 +- .../paths/HubRight-PlayerStation.path | 2 +- .../pathplanner/paths/HubRight-Shoot.path | 2 +- .../paths/LeftBump-Neutral-Shoot.path | 2 +- .../deploy/pathplanner/paths/LeftBump.path | 2 +- .../paths/LeftBump2-Neutral2-Shoot2.path | 2 +- .../deploy/pathplanner/paths/LeftBump2.path | 2 +- .../pathplanner/paths/LeftRamp-Shoot.path | 2 +- .../paths/PlayerStation-Shoot.path | 2 +- .../deploy/pathplanner/paths/ReadyPush.path | 2 +- .../paths/RightBump-Neutral-Shoot.path | 2 +- .../deploy/pathplanner/paths/RightBump.path | 2 +- .../paths/RightBump2-Neutral2-Shoot2.path | 2 +- .../deploy/pathplanner/paths/RightBump2.path | 2 +- .../paths/ShotRightR-PlayerStation.path | 4 +- .../paths/ShotRightT-PlayerStation.path | 2 +- .../deploy/pathplanner/paths/Stupidauto.path | 2 +- src/main/deploy/pathplanner/settings.json | 2 +- src/main/java/frc4388/robot/Main.java | 1 + src/main/java/frc4388/robot/Robot.java | 1 + .../java/frc4388/robot/RobotContainer.java | 15 +++--- .../robot/constants/BuildConstants.java | 10 ++-- .../frc4388/robot/constants/Constants.java | 2 +- .../robot/subsystems/intake/Intake.java | 7 ++- .../subsystems/intake/IntakeConstants.java | 10 ++-- .../robot/subsystems/intake/IntakeReal.java | 46 ++++++++++++++----- .../subsystems/shooter/ShooterConstants.java | 2 +- 41 files changed, 96 insertions(+), 64 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Bump test.path b/src/main/deploy/pathplanner/paths/Bump test.path index dfb810c..d6340e2 100644 --- a/src/main/deploy/pathplanner/paths/Bump test.path +++ b/src/main/deploy/pathplanner/paths/Bump test.path @@ -58,7 +58,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/BumpToCenter.path b/src/main/deploy/pathplanner/paths/BumpToCenter.path index 7051d4f..0721fbf 100644 --- a/src/main/deploy/pathplanner/paths/BumpToCenter.path +++ b/src/main/deploy/pathplanner/paths/BumpToCenter.path @@ -37,7 +37,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/CPlayerStation-CShoot.path b/src/main/deploy/pathplanner/paths/CPlayerStation-CShoot.path index dd06473..7325176 100644 --- a/src/main/deploy/pathplanner/paths/CPlayerStation-CShoot.path +++ b/src/main/deploy/pathplanner/paths/CPlayerStation-CShoot.path @@ -58,7 +58,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/Center-NeutralL.path b/src/main/deploy/pathplanner/paths/Center-NeutralL.path index 78cf2b4..a708e11 100644 --- a/src/main/deploy/pathplanner/paths/Center-NeutralL.path +++ b/src/main/deploy/pathplanner/paths/Center-NeutralL.path @@ -37,7 +37,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/Center-NeutralR.path b/src/main/deploy/pathplanner/paths/Center-NeutralR.path index 07295d3..8099573 100644 --- a/src/main/deploy/pathplanner/paths/Center-NeutralR.path +++ b/src/main/deploy/pathplanner/paths/Center-NeutralR.path @@ -37,7 +37,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/Center-ToDepot.path b/src/main/deploy/pathplanner/paths/Center-ToDepot.path index d24724a..a5205d5 100644 --- a/src/main/deploy/pathplanner/paths/Center-ToDepot.path +++ b/src/main/deploy/pathplanner/paths/Center-ToDepot.path @@ -37,7 +37,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/Copy of HubFarRight-NeutralZone 1-2.path b/src/main/deploy/pathplanner/paths/Copy of HubFarRight-NeutralZone 1-2.path index e65a3bf..9f98845 100644 --- a/src/main/deploy/pathplanner/paths/Copy of HubFarRight-NeutralZone 1-2.path +++ b/src/main/deploy/pathplanner/paths/Copy of HubFarRight-NeutralZone 1-2.path @@ -48,7 +48,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/HubCenter-PlayerStation.path b/src/main/deploy/pathplanner/paths/HubCenter-PlayerStation.path index 3307f61..cbd5c03 100644 --- a/src/main/deploy/pathplanner/paths/HubCenter-PlayerStation.path +++ b/src/main/deploy/pathplanner/paths/HubCenter-PlayerStation.path @@ -104,7 +104,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 1-2.path b/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 1-2.path index c2d5d1a..cf8bda0 100644 --- a/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 1-2.path +++ b/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 1-2.path @@ -48,7 +48,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 2-2.path b/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 2-2.path index ae1d06a..2cab886 100644 --- a/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 2-2.path +++ b/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 2-2.path @@ -88,7 +88,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/HubFarLeft-Shoot.path b/src/main/deploy/pathplanner/paths/HubFarLeft-Shoot.path index 900ac05..aa3eec5 100644 --- a/src/main/deploy/pathplanner/paths/HubFarLeft-Shoot.path +++ b/src/main/deploy/pathplanner/paths/HubFarLeft-Shoot.path @@ -58,7 +58,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 1-2.path b/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 1-2.path index 5252aec..4e9caae 100644 --- a/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 1-2.path +++ b/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 1-2.path @@ -48,7 +48,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 2-2.path b/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 2-2.path index 79a2251..ddc66c0 100644 --- a/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 2-2.path +++ b/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 2-2.path @@ -88,7 +88,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/HubFarRight-Shoot.path b/src/main/deploy/pathplanner/paths/HubFarRight-Shoot.path index a3834d8..068d796 100644 --- a/src/main/deploy/pathplanner/paths/HubFarRight-Shoot.path +++ b/src/main/deploy/pathplanner/paths/HubFarRight-Shoot.path @@ -58,7 +58,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/HubLeft-Shoot.path b/src/main/deploy/pathplanner/paths/HubLeft-Shoot.path index d9f96d5..b535ae3 100644 --- a/src/main/deploy/pathplanner/paths/HubLeft-Shoot.path +++ b/src/main/deploy/pathplanner/paths/HubLeft-Shoot.path @@ -48,7 +48,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/HubRight-PlayerStation.path b/src/main/deploy/pathplanner/paths/HubRight-PlayerStation.path index ab1ab91..0d931a0 100644 --- a/src/main/deploy/pathplanner/paths/HubRight-PlayerStation.path +++ b/src/main/deploy/pathplanner/paths/HubRight-PlayerStation.path @@ -64,7 +64,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/HubRight-Shoot.path b/src/main/deploy/pathplanner/paths/HubRight-Shoot.path index 9f25f6d..11b30f3 100644 --- a/src/main/deploy/pathplanner/paths/HubRight-Shoot.path +++ b/src/main/deploy/pathplanner/paths/HubRight-Shoot.path @@ -48,7 +48,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/LeftBump-Neutral-Shoot.path b/src/main/deploy/pathplanner/paths/LeftBump-Neutral-Shoot.path index 9091424..094ce2f 100644 --- a/src/main/deploy/pathplanner/paths/LeftBump-Neutral-Shoot.path +++ b/src/main/deploy/pathplanner/paths/LeftBump-Neutral-Shoot.path @@ -111,7 +111,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/LeftBump.path b/src/main/deploy/pathplanner/paths/LeftBump.path index 7bf33e3..9a95768 100644 --- a/src/main/deploy/pathplanner/paths/LeftBump.path +++ b/src/main/deploy/pathplanner/paths/LeftBump.path @@ -42,7 +42,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/LeftBump2-Neutral2-Shoot2.path b/src/main/deploy/pathplanner/paths/LeftBump2-Neutral2-Shoot2.path index 4eb860a..2cba6eb 100644 --- a/src/main/deploy/pathplanner/paths/LeftBump2-Neutral2-Shoot2.path +++ b/src/main/deploy/pathplanner/paths/LeftBump2-Neutral2-Shoot2.path @@ -131,7 +131,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/LeftBump2.path b/src/main/deploy/pathplanner/paths/LeftBump2.path index cc0f95c..bd9e0bb 100644 --- a/src/main/deploy/pathplanner/paths/LeftBump2.path +++ b/src/main/deploy/pathplanner/paths/LeftBump2.path @@ -42,7 +42,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/LeftRamp-Shoot.path b/src/main/deploy/pathplanner/paths/LeftRamp-Shoot.path index 213112a..7af0087 100644 --- a/src/main/deploy/pathplanner/paths/LeftRamp-Shoot.path +++ b/src/main/deploy/pathplanner/paths/LeftRamp-Shoot.path @@ -48,7 +48,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/PlayerStation-Shoot.path b/src/main/deploy/pathplanner/paths/PlayerStation-Shoot.path index 98ff3b0..f3f2e7c 100644 --- a/src/main/deploy/pathplanner/paths/PlayerStation-Shoot.path +++ b/src/main/deploy/pathplanner/paths/PlayerStation-Shoot.path @@ -58,7 +58,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/ReadyPush.path b/src/main/deploy/pathplanner/paths/ReadyPush.path index 4d01629..3c8bced 100644 --- a/src/main/deploy/pathplanner/paths/ReadyPush.path +++ b/src/main/deploy/pathplanner/paths/ReadyPush.path @@ -48,7 +48,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/RightBump-Neutral-Shoot.path b/src/main/deploy/pathplanner/paths/RightBump-Neutral-Shoot.path index abddcef..63d6dd2 100644 --- a/src/main/deploy/pathplanner/paths/RightBump-Neutral-Shoot.path +++ b/src/main/deploy/pathplanner/paths/RightBump-Neutral-Shoot.path @@ -111,7 +111,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/RightBump.path b/src/main/deploy/pathplanner/paths/RightBump.path index 1c00f4b..916396a 100644 --- a/src/main/deploy/pathplanner/paths/RightBump.path +++ b/src/main/deploy/pathplanner/paths/RightBump.path @@ -42,7 +42,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/RightBump2-Neutral2-Shoot2.path b/src/main/deploy/pathplanner/paths/RightBump2-Neutral2-Shoot2.path index 2f71f5d..712b6ab 100644 --- a/src/main/deploy/pathplanner/paths/RightBump2-Neutral2-Shoot2.path +++ b/src/main/deploy/pathplanner/paths/RightBump2-Neutral2-Shoot2.path @@ -125,7 +125,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/RightBump2.path b/src/main/deploy/pathplanner/paths/RightBump2.path index 561c39a..129f1b8 100644 --- a/src/main/deploy/pathplanner/paths/RightBump2.path +++ b/src/main/deploy/pathplanner/paths/RightBump2.path @@ -42,7 +42,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/ShotRightR-PlayerStation.path b/src/main/deploy/pathplanner/paths/ShotRightR-PlayerStation.path index 904a9bb..4fe709e 100644 --- a/src/main/deploy/pathplanner/paths/ShotRightR-PlayerStation.path +++ b/src/main/deploy/pathplanner/paths/ShotRightR-PlayerStation.path @@ -78,7 +78,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { @@ -89,7 +89,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -106.71386741074436 + "rotation": -106.71386741074437 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ShotRightT-PlayerStation.path b/src/main/deploy/pathplanner/paths/ShotRightT-PlayerStation.path index 7af0028..68761d1 100644 --- a/src/main/deploy/pathplanner/paths/ShotRightT-PlayerStation.path +++ b/src/main/deploy/pathplanner/paths/ShotRightT-PlayerStation.path @@ -78,7 +78,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/Stupidauto.path b/src/main/deploy/pathplanner/paths/Stupidauto.path index 2b34f3a..2a8ee7c 100644 --- a/src/main/deploy/pathplanner/paths/Stupidauto.path +++ b/src/main/deploy/pathplanner/paths/Stupidauto.path @@ -37,7 +37,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 10.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 6f1e894..ba45b6f 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -11,7 +11,7 @@ "defaultMaxAccel": 3.0, "defaultMaxAngVel": 600.0, "defaultMaxAngAccel": 750.0, - "defaultNominalVoltage": 10.0, + "defaultNominalVoltage": 12.0, "robotMass": 74.088, "robotMOI": 6.883, "robotTrackwidth": 0.546, diff --git a/src/main/java/frc4388/robot/Main.java b/src/main/java/frc4388/robot/Main.java index ad2d494..b2e3ea4 100644 --- a/src/main/java/frc4388/robot/Main.java +++ b/src/main/java/frc4388/robot/Main.java @@ -25,5 +25,6 @@ public final class Main { */ public static void main(String... args) { RobotBase.startRobot(Robot::new); + } } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 31054f1..a286a66 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -82,6 +82,7 @@ public class Robot extends LoggedRobot { // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); + } /** * This function is called once each time the robot enters Disabled mode. diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a45a5f8..0563669 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -139,7 +139,8 @@ public class RobotContainer { private Command RobotRev = new SequentialCommandGroup( new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter), IntakeExtended, - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Idle), m_robotIntake) + new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.ExpelBalls), m_robotIntake) + // new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Idle), m_robotIntake) ); private Command WaitIntakeReference = @@ -161,10 +162,11 @@ public class RobotContainer { // 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_robotIntake.setMode(IntakeMode.Idle), m_robotIntake), new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter), - new WaitCommand(4), + new WaitCommand(3), IntakeRetracted, - new WaitCommand(7), + new WaitCommand(4.5), new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter), new InstantCommand(()->m_robotShooter.spinUpIdle(), m_robotShooter) ); @@ -183,7 +185,8 @@ public class RobotContainer { DeferredBlock.addBlock(() -> { TimesNegativeOne.update(); FieldPositions.update(); - + m_robotIntake.setMode(IntakeMode.Idle); + m_robotShooter.spinUpIdle(); m_robotIntake.io.updateGains(); m_robotShooter.io.updateGains(); }, true); @@ -339,7 +342,6 @@ public class RobotContainer { new Trigger(() -> getDeadbandedOperatorController().getRightTriggerAxis() >= 0.5) .onTrue(new InstantCommand(() -> { m_robotShooter.allowShooting(); - m_robotIntake.rollerStop(); })).onFalse(new InstantCommand(() -> { m_robotShooter.denyShooting(); })); @@ -380,6 +382,7 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> { m_robotIntake.setMode(IntakeMode.ExtendingRolling); })); + new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> { m_robotIntake.setMode(IntakeMode.LabubuGrowl); @@ -392,7 +395,7 @@ public class RobotContainer { m_robotIntake.setMode(IntakeMode.Retracting); })) .onFalse(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.Idle); + m_robotIntake.setMode(IntakeMode.ArmIdleRollingNot); })); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 8d1b4ee..819936c 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 = 220; - public static final String GIT_SHA = "9564554c0708fd0b761f959685258d9c1a90d11e"; - public static final String GIT_DATE = "2026-04-06 19:50:34 MDT"; + public static final int GIT_REVISION = 221; + public static final String GIT_SHA = "1636a054ed1e688234fe329b76f567d6af08081f"; + public static final String GIT_DATE = "2026-04-06 22:44:17 MDT"; public static final String GIT_BRANCH = "New-Intake"; - public static final String BUILD_DATE = "2026-04-06 22:42:14 MDT"; - public static final long BUILD_UNIX_TIME = 1775536934067L; + public static final String BUILD_DATE = "2026-04-07 20:27:10 MDT"; + public static final long BUILD_UNIX_TIME = 1775615230294L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java index 3c54d38..b4fc464 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -97,7 +97,7 @@ public final class Constants { public static final class LEDConstants { - public static final int LED_SPARK_ID = 9; + public static final int LED_SPARK_ID = 8; public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_RAINBOW; diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index d024fb3..5436649 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -35,6 +35,7 @@ public class Intake extends SubsystemBase { ExtendingRolling, Retracting, + ArmIdleRollingNot, Idle, RectractTorque, @@ -79,7 +80,6 @@ public class Intake extends SubsystemBase { public double getRollerSpeed() { return state.rollerOutput; } - // public enum FieldZone { // // The robot should aim at the hub // InShootZone, @@ -136,7 +136,10 @@ public class Intake extends SubsystemBase { // io.setRollerOutput(state, 0); // } break; - + case ArmIdleRollingNot: + io.armOutput(0); + io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get()); + break; case Bouncing: // io.setRollerOutput(state, 0); diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index 40daee3..4840481 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -23,7 +23,7 @@ public class IntakeConstants { public static final double ROLLER_MOTOR_GEAR_RATIO = 3; public static final ConfigurableDouble ARM_ENCODER_OFFSET = new ConfigurableDouble("Arm Encoder Offset", 0); - public static final int ARM_LIMIT_SWITCH_CHANNEL = 9; + public static final int ARM_LIMIT_SWITCH_CHANNEL = 7; public static final ConfigurableDouble INTAKE_BOUNCE_HALF_PERIOD = new ConfigurableDouble("Bounce Half Period", 5.); public static final ConfigurableDouble INTAKE_BOUNCE_OUTPUT = new ConfigurableDouble("Bounce Output", 0.1); @@ -56,7 +56,7 @@ public class IntakeConstants { // public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90); public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", 0.); - public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 1.75); + public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 1.8); //new soft limt public static final ConfigurableDouble ARM_EXTEND_PERCENT_OUTPUT = new ConfigurableDouble("Arm extend % output", 0.4); public static final ConfigurableDouble ARM_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Arm retract % output", -0.2); @@ -108,9 +108,9 @@ public class IntakeConstants { .forwardLimitSwitchType(Type.kNormallyOpen) .forwardLimitSwitchTriggerBehavior(Behavior.kKeepMovingMotor) - .reverseLimitSwitchType(Type.kNormallyClosed) - .reverseLimitSwitchPosition(0) - .reverseLimitSwitchTriggerBehavior(Behavior.kStopMovingMotorAndSetPosition); + .reverseLimitSwitchType(Type.kNormallyOpen) + // .reverseLimitSwitchPosition(0) + .reverseLimitSwitchTriggerBehavior(Behavior.kKeepMovingMotor); ARM_MOTOR_CONFIG.idleMode(IdleMode.kBrake); diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java index 92b8ea0..c4ba79c 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -4,6 +4,8 @@ import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; +import org.littletonrobotics.junction.Logger; + import com.ctre.phoenix6.hardware.TalonFX; import com.revrobotics.PersistMode; import com.revrobotics.ResetMode; @@ -13,6 +15,8 @@ import com.revrobotics.spark.SparkMax; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Timer; +import frc4388.robot.constants.Constants; import frc4388.utility.compute.JankCoder; public class IntakeReal implements IntakeIO { @@ -23,6 +27,9 @@ public class IntakeReal implements IntakeIO { TalonFX m_rollerMotor; JankCoder m_encoder; DigitalInput m_armLimitSwitch; + boolean m_limitTRIGGER = false; + private final Timer m_limitTimer = new Timer(); + public IntakeReal( DigitalInput armLimitSwitch, @@ -78,9 +85,12 @@ public class IntakeReal implements IntakeIO { // m_rollerMotor.set(0); } - // private boolean retractedLimit() { - // return m_encoder.get() <= IntakeConstants.ARM_LIMIT_RETRACTED.get(); - // } + private boolean retractedLimit() { + return m_armLimitSwitch.get(); + } + private boolean retractedSoftLimit() { + return m_encoder.get() <= IntakeConstants.ARM_LIMIT_RETRACTED.get(); + } private boolean extendedLimit() { return m_encoder.get() >= IntakeConstants.ARM_LIMIT_EXTENDED.get(); } @@ -88,9 +98,9 @@ public class IntakeReal implements IntakeIO { @Override public void armOutput(double percentOutput){ - // if(retractedLimit()) { - // percentOutput = Math.max(percentOutput, 0); - // } + if(retractedSoftLimit()) { + percentOutput = Math.max(percentOutput, 0); + } if (extendedLimit()) { percentOutput = Math.min(percentOutput, 0); @@ -120,16 +130,30 @@ public class IntakeReal implements IntakeIO { state.intakeEncoder = m_encoder.getRotations(); state.encoderConnected = m_encoder.isConnected(); - state.retractedLimitSwitch = m_armLimitSwitch.get(); + state.retractedLimitSwitch = retractedLimit(); - if(state.retractedLimitSwitch) { - m_encoder.resetRotations(); - } + // if(state.retractedLimitSwitch && (state.armMotorVelocity.in(RotationsPerSecond) <0)) { + // if (!m_limitTRIGGER) { + // m_limitTRIGGER = true; + // m_limitTimer.restart(); + // } + // if (m_limitTimer.hasElapsed(1.0)) { + // m_encoder.resetRotations(); + // } + // } else { + // m_limitTRIGGER = false; + // m_limitTimer.reset(); + // } } @Override public void updateGains() { - m_encoder.loadRotations(); + // m_encoder.loadRotations(); + + + if(retractedLimit()) { + m_encoder.resetRotations(); + } // IntakeConstants.ARM_PID.kP = IntakeConstants.arm_kP.get(); // IntakeConstants.ARM_PID.kI = IntakeConstants.arm_kI.get(); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 72d2c34..63da997 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -26,7 +26,7 @@ public class ShooterConstants { // public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0); - public static final ConfigurableDouble SHOOTER_IDLE_PERCENT_OUTPUT = new ConfigurableDouble("Shooter idle % output", -0.15); + public static final ConfigurableDouble SHOOTER_IDLE_PERCENT_OUTPUT = new ConfigurableDouble("Shooter idle % output", 0.0);//-0.15); // public static final ConfigurableDouble SHOOTER_IDLE_TARGET_VEL = new ConfigurableDouble("Shooter idle target velocity", 20.); // public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10);