Merge remote-tracking branch 'origin/work_april_3'

Author: sjcbulldog

src/main/deploy/pathplanner/paths/a7CollectLoop.path

@@ -33,47 +33,47 @@
     {
       "anchor": {
         "x": 6.708995782632744,
-        "y": 2.9864684043141585
+        "y": 3.7499931899889374
       },
       "prevControl": {
         "x": 7.697838255014892,
-        "y": 2.580282702370341
+        "y": 3.34380748804512
       },
       "nextControl": {
         "x": 5.825374028251257,
-        "y": 3.349432711148226
+        "y": 4.112957496823005
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 6.129280835176992,
-        "y": 4.461494607300884
+        "x": 6.008044558904868,
+        "y": 4.801829300331858
       },
       "prevControl": {
-        "x": 6.156139549466578,
-        "y": 4.0372859451716305
+        "x": 6.034903273194454,
+        "y": 4.377620638202604
       },
       "nextControl": {
-        "x": 6.1024221208874065,
-        "y": 4.885703269430138
+        "x": 5.981185844615283,
+        "y": 5.2260379624611115
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 4.654466848727876,
-        "y": 5.592790652654867
+        "x": 4.634185011061947,
+        "y": 5.56311065403761
       },
       "prevControl": {
-        "x": 5.071713567647901,
-        "y": 5.52944676656619
+        "x": 5.051431729981973,
+        "y": 5.499766767948933
       },
       "nextControl": {
-        "x": 4.067828395426444,
-        "y": 5.681850572544342
+        "x": 4.047546557760516,
+        "y": 5.652170573927084
       },
       "isLocked": false,
       "linkedName": null
@@ -102,8 +102,12 @@
       "rotationDegrees": -56.98406371610071
     },
     {
-      "waypointRelativePos": 1.7967115097160609,
-      "rotationDegrees": 165.52779753636696
+      "waypointRelativePos": 1.375409915123457,
+      "rotationDegrees": -78.72123223305731
+    },
+    {
+      "waypointRelativePos": 1.9728009259259265,
+      "rotationDegrees": -177.36077850349736
     },
     {
       "waypointRelativePos": 3.0,
@@ -136,6 +140,19 @@
         "nominalVoltage": 12.0,
         "unlimited": false
       }
+    },
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.8977962551299594,
+      "maxWaypointRelativePos": 3.0263284456224335,
+      "constraints": {
+        "maxVelocity": 2.5,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 300.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
     }
   ],
   "pointTowardsZones": [],

src/main/deploy/pathplanner/paths/a7DepotCollectFast.path

@@ -56,8 +56,8 @@
       "minWaypointRelativePos": 0,
       "maxWaypointRelativePos": 0.800786593707251,
       "constraints": {
-        "maxVelocity": 0.8,
-        "maxAcceleration": 1.5,
+        "maxVelocity": 2.0,
+        "maxAcceleration": 2.0,
         "maxAngularVelocity": 540.0,
         "maxAngularAcceleration": 720.0,
         "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/a7StartToCollect.path

@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 1.5,
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,

src/main/deploy/tuning/arc.json

@@ -113,7 +113,7 @@
                 },
                 {
                     "distance": 5.3,
-                    "velocity": 68.5
+                    "velocity": 70.0
                 }
             ]
         }

src/main/java/frc/robot/RobotContainer.java

@@ -303,6 +303,9 @@ private void configureBindings() {
         gamepad_.rightTrigger().or(operatorGamepad_.rightTrigger())
             .whileTrue(RobotCommands.shoot(shooter_, hopper_, intake_, drivebase_));
 
+        gamepad_.rightBumper()
+            .whileTrue(intake_.hopperEjectSequence().alongWith(hopper_.reverseFeed()));
+
         // When the shooter isnt shooting, get it ready to shoot.
         shooter_.setDefaultCommand(shooter_.spinUpVelocityCommand(() -> {
             return RotationsPerSecond.zero();

src/main/java/frc/robot/RobotState.java

@@ -70,7 +70,11 @@ public static void periodic() {
         inAllianceZone = currentPose.getMeasureX().minus(allianceWall).abs(Meters) < ShooterConstants.Positions.spinUpZone.in(Meters);
 
         // Other logging
-        Logger.recordOutput("ShiftInfo", HubShiftUtil.getOfficialShiftInfo());
+        var shift = HubShiftUtil.getOfficialShiftInfo();
+        Logger.recordOutput("ShiftInfo", shift);
+        Logger.recordOutput("Shift/Active", shift.active());
+        Logger.recordOutput("Shift/Name", shift.currentShift());
+        Logger.recordOutput("Shift/SecondsRemaining", shift.remainingTime());
 
         LoggedTracer.record("RobotState");
     }

src/main/java/frc/robot/commands/AutoCommands.java

@@ -167,7 +167,7 @@ public static Command a7CoopDepot(Drive drive, Shooter shooter, IntakeSubsystem
                     .andThen(shooter.spinUpForDistanceHoodParked(() -> Meters.of(2)))
             ),
 
-            RobotCommands.shoot(shooter, hopper, intake, drive).withTimeout(Seconds.of(5)),
+            RobotCommands.shoot(shooter, hopper, intake, drive).withTimeout(Seconds.of(4.5)),
 
             DriveCommands.followPathCommand("a7CollectLoop").deadlineFor(
                 RobotCommands.intake(intake, hopper)

src/main/java/frc/robot/commands/robot/RobotCommands.java

@@ -70,10 +70,10 @@ public static Command shootHubNoAim(Shooter shooter, Hopper hopper, IntakeSubsys
             () -> !drive.rotationIsNear(RobotState.rotationToHub(), ShooterConstants.defenseTolerance);
 
         return Commands.repeatingSequence(
-            Commands.waitUntil(shouldShoot).deadlineFor(shooter.spinUpForDistance(RobotState::hubDistance)),
-            shooter.shootAtDistance(RobotState::hubDistance, hopper, intake)
+            Commands.waitUntil(shouldShoot),
+            hopper.feedForShooting(shouldShoot, intake)
                 .until(shouldStopShooting)
-        );
+        ).alongWith(shooter.shootAtDistance(RobotState::hubDistance));
     }
 
     /**
@@ -109,7 +109,7 @@ public static Command ferry(Shooter shooter, Hopper hopper, IntakeSubsystem inta
 
             return DriveCommands.joystickDriveAtAngle(targetingAngle)
                 .alongWith(
-                    shooter.shootAtDistance(targetDistance, hopper, intake),
+                    shooter.shootAtDistance(targetDistance),
                     Commands.runOnce(() -> {
                         Logger.recordOutput("Ferry/Target", target.get());
                         Logger.recordOutput("Ferry/IsFerrying", true);

src/main/java/frc/robot/subsystems/hopper/Hopper.java

@@ -6,6 +6,7 @@
 import static edu.wpi.first.units.Units.RotationsPerSecond;
 import static edu.wpi.first.units.Units.Volts;
 
+import java.util.function.BooleanSupplier;
 import java.util.function.Supplier;
 
 import org.littletonrobotics.junction.AutoLogOutput;
@@ -16,9 +17,11 @@
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.Constants.Mode;
+import frc.robot.subsystems.intake.IntakeSubsystem;
 import frc.robot.util.LoggedTracer;
 import frc.robot.util.MapleSimUtil;
 
@@ -110,6 +113,19 @@ private void stopAll() {
 
     // Commands
 
+    public Command feedForShooting(BooleanSupplier shouldFeed, IntakeSubsystem intake) {
+        return Commands.parallel(
+            preShoot().until(shouldFeed)
+                .andThen(forwardFeed()),
+
+            Commands.run(() -> {
+                Logger.recordOutput("Command/ReadyForShoot", shouldFeed);
+            }),
+
+            intake.shakeBalls()
+        );
+    }
+
     /**
      * Runs the scrambler at its idling speed until the command ends.
      * @return

src/main/java/frc/robot/subsystems/shooter/Shooter.java

@@ -38,7 +38,6 @@
 import frc.robot.Constants.Mode;
 import frc.robot.RobotState;
 import frc.robot.subsystems.hopper.Hopper;
-import frc.robot.subsystems.intake.IntakeSubsystem;
 import frc.robot.subsystems.shooter.ShooterTuning.ShooterParams;
 import frc.robot.util.LoggedTracer;
 import frc.robot.util.MapleSimUtil;
@@ -245,7 +244,7 @@ public Command hoodToPosCmd(Angle pos) {
      * 
      * 
      */
-    public Command shootAtDistance(Supplier<Distance> distance, Hopper hopper, IntakeSubsystem intake) {
+    public Command shootAtDistance(Supplier<Distance> distance) {
         Supplier<ShooterParams> shooterParams =
             () -> getTuning().getShooterParams(distance.get().in(Meters));
 
@@ -254,41 +253,10 @@ public Command shootAtDistance(Supplier<Distance> distance, Hopper hopper, Intak
             runDynamicSetpoints(
                 () -> RotationsPerSecond.of(shooterParams.get().velocity),
                 () -> Degrees.of(shooterParams.get().hood)
-            ),
-            
-            hopper.preShoot().until(this::isShooterReady).andThen(
-                hopper.forwardFeed().deadlineFor(Commands.run(() -> {
-                    var diff = shooterTarget.minus(shooterInputs.wheelVelocity).abs(RadiansPerSecond);
-                    // Ball counting?
-                    if (diff > 15.0) {
-                        if (!isCounted) {
-                            addBallToCount();
-                        }
-                        isCounted = true;
-                    } else {
-                        isCounted = false;
-                    }
-
-                    Logger.recordOutput("Stats/IsCounted", isCounted);
-                }).onlyIf(() -> Constants.getMode() == Mode.REPLAY))
-            ),
-
-            Commands.run(() -> {
-                Logger.recordOutput("Command/ReadyForShoot", isShooterReady());
-            }),
-
-            intake.shakeBalls()
+            )
         ).finallyDo(interrupted -> Logger.recordOutput("Command/ShootAtDistance", false));
     }
 
-    private int ballsShot = 0;
-    private boolean isCounted = false;
-
-    private void addBallToCount() {
-        ballsShot++;
-        Logger.recordOutput("Stats/BallsShot", ballsShot);
-    }
-
     /**
      * Spins the shooter up to shoot, without actually shooting.
      * @param distance