sync fork again

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/BaseAutoOpMode.java

@@ -3,6 +3,7 @@
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.teamcode.RobotContainer;
+import org.firstinspires.ftc.teamcode.constants.AutonomousConstants;
 
 public abstract class BaseAutoOpMode extends LinearOpMode {
     protected final RobotContainer robot = new RobotContainer();
@@ -18,6 +19,12 @@ public void runOpMode() throws InterruptedException {
             return;
         }
 
+        // Start the intake forward immediately at autonomous start
+        robot.intake.runFromTrigger(AutonomousConstants.INTAKE_FORWARD_SPEED);
+
+        // Wait 800 ms to let the Pinpoint calibrate on reset
+        sleep(1200);
+
         runAutonomous();
         robot.stopAll();
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Controller.java

@@ -2,19 +2,23 @@
 
 import com.pedropathing.follower.Follower;
 import com.pedropathing.geometry.Pose;
-import com.pedropathing.pathgen.BezierLine;
-import com.pedropathing.pathgen.Path;
-import com.pedropathing.pathgen.Point;
+import com.pedropathing.geometry.BezierLine;
+import com.pedropathing.paths.PathChain;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.util.ElapsedTime;
+import com.qualcomm.hardware.rev.RevColorSensorV3;
 
 import org.firstinspires.ftc.teamcode.RobotContainer;
 import org.firstinspires.ftc.teamcode.constants.AutonomousConstants;
+import org.firstinspires.ftc.teamcode.constants.HardwareConstants;
+import org.firstinspires.ftc.teamcode.constants.ShooterConstants;
 
 public class Controller {
     private final LinearOpMode opMode;
     private final RobotContainer robot;
     private Follower follower;
+    private RevColorSensorV3 colorShootSensor;
+    private boolean isShooting = false;
 
     public Controller(LinearOpMode opMode, RobotContainer robot) {
         this.opMode = opMode;
@@ -24,6 +28,14 @@ public Controller(LinearOpMode opMode, RobotContainer robot) {
     public void init() {
         // Assume robot.init has already been called in BaseAutoOpMode
         follower = robot.getFollower();
+        try {
+            colorShootSensor = opMode.hardwareMap.get(RevColorSensorV3.class, HardwareConstants.COLOR_SHOOT_SENSOR);
+        } catch (Exception ignored) {
+            colorShootSensor = null;
+        }
+
+        // Start the intake forward at configurable speed immediately
+        robot.intake.runFromTrigger(AutonomousConstants.INTAKE_FORWARD_SPEED);
     }
 
     public void setStartingPose(Pose startPose) {
@@ -42,12 +54,9 @@ public void startShooter(double rpm) {
      * Turns on the intake to the default forward speed.
      */
     public void enableIntake() {
-        robot.intake.reverse(-AutonomousConstants.INTAKE_FORWARD_SPEED); // Assuming forward is positive in reverse? Wait, IntakeSubsystem uses runFromTrigger.
-        // Actually, IntakeSubsystem has reverse(power) where positive power reverses it.
-        // Let's look at IntakeSubsystem. runFromTrigger(value) runs forward. reverse(value) runs backward.
+        robot.intake.runFromTrigger(AutonomousConstants.INTAKE_FORWARD_SPEED);
     }
 
-    // I need to correct IntakeSubsystem calls later. Let's make helper methods for this:
     public void runIntakeForward() {
         robot.intake.runFromTrigger(AutonomousConstants.INTAKE_FORWARD_SPEED);
     }
@@ -66,17 +75,38 @@ public void disableIntake() {
     public void pathTo(Pose targetPose) {
         if (!opMode.opModeIsActive()) return;
 
-        Path path = new Path(new BezierLine(
-                new Point(follower.getPose()),
-                new Point(targetPose)
-        ));
-        path.setLinearHeadingInterpolation(follower.getPose().getHeading(), targetPose.getHeading());
+        com.pedropathing.paths.PathBuilder builder = follower.pathBuilder();
+        builder.addPath(new BezierLine(follower.getPose(), targetPose))
+                .setLinearHeadingInterpolation(follower.getPose().getHeading(), targetPose.getHeading());
+        PathChain path = builder.build();
 
         follower.followPath(path);
 
         while (opMode.opModeIsActive() && follower.isBusy()) {
             update();
         }
+
+        delayBetweenCommands();
+    }
+
+    /**
+     * Drives to a point with a custom maximum speed scaling and blocks until the robot is near the target or opmode stops.
+     */
+    public void pathTo(Pose targetPose, double speedPercent) {
+        if (!opMode.opModeIsActive()) return;
+
+        com.pedropathing.paths.PathBuilder builder = follower.pathBuilder();
+        builder.addPath(new BezierLine(follower.getPose(), targetPose))
+                .setLinearHeadingInterpolation(follower.getPose().getHeading(), targetPose.getHeading());
+        PathChain path = builder.build();
+
+        follower.followPath(path, speedPercent, true);
+
+        while (opMode.opModeIsActive() && follower.isBusy()) {
+            update();
+        }
+
+        delayBetweenCommands();
     }
 
     /**
@@ -86,43 +116,73 @@ public void pathTo(Pose targetPose) {
     public void shoot(long durationMs) {
         if (!opMode.opModeIsActive()) return;
 
+        // Wait to allow the flywheel to spin up before feeding the balls
+        if (AutonomousConstants.SPIN_UP_DELAY_MS > 0) {
+            ElapsedTime spinUpTimer = new ElapsedTime();
+            while (opMode.opModeIsActive() && spinUpTimer.milliseconds() < AutonomousConstants.SPIN_UP_DELAY_MS) {
+                update();
+            }
+        }
+
+        isShooting = true;
+
         ElapsedTime timer = new ElapsedTime();
         timer.reset();
-
-        ElapsedTime cycleTimer = new ElapsedTime();
-        cycleTimer.reset();
 
         robot.shooter.setFeederPower(AutonomousConstants.FEEDER_SPEED);
+        robot.intake.runFromTrigger(1.0);
 
         while (opMode.opModeIsActive() && timer.milliseconds() < durationMs) {
-            long cycleTime = (long) cycleTimer.milliseconds();
-            long totalCycleMs = AutonomousConstants.SHOOT_FORWARD_CYCLE_MS + AutonomousConstants.SHOOT_REVERSE_CYCLE_MS;
-
-            if (cycleTime > totalCycleMs) {
-                cycleTimer.reset();
-                cycleTime = 0;
-            }
-
-            if (cycleTime < AutonomousConstants.SHOOT_FORWARD_CYCLE_MS) {
-                runIntakeForward();
-            } else {
-                runIntakeReverse();
-            }
-
             update();
         }
 
         robot.shooter.setFeederPower(0.0);
-        // Turn intake back to normal forward spinning after shooting finishes,
-        // since the user states "intake should be running the entire time"
-        runIntakeForward();
+        isShooting = false;
+
+        delayBetweenCommands();
     }
 
     /**
      * Updates all background processes like the path follower and shooter PID.
      */
     public void update() {
         follower.update();
+
+        // Always ensure the intake is running forward during autonomous
+        robot.intake.runFromTrigger(AutonomousConstants.INTAKE_FORWARD_SPEED);
+
+        if (!isShooting) {
+            double feederPower;
+            int alpha = -1;
+            if (colorShootSensor != null) {
+                alpha = colorShootSensor.alpha();
+            }
+            boolean shouldStopIndexer = alpha > ShooterConstants.FEEDER_ALPHA_STOP_THRESHOLD;
+            feederPower = shouldStopIndexer ? 0.0 : ShooterConstants.FEEDER_INDEX_POWER;
+            robot.shooter.setFeederPower(feederPower);
+        }
+
         robot.shooter.update();
     }
+
+    /**
+     * Sleeps for the specified duration in milliseconds while updating background subsystems.
+     */
+    public void sleep(long durationMs) {
+        if (!opMode.opModeIsActive()) return;
+
+        ElapsedTime timer = new ElapsedTime();
+        while (opMode.opModeIsActive() && timer.milliseconds() < durationMs) {
+            update();
+        }
+    }
+
+    private void delayBetweenCommands() {
+        if (AutonomousConstants.COMMAND_DELAY_MS > 0 && opMode.opModeIsActive()) {
+            ElapsedTime delayTimer = new ElapsedTime();
+            while (opMode.opModeIsActive() && delayTimer.milliseconds() < AutonomousConstants.COMMAND_DELAY_MS) {
+                update();
+            }
+        }
+    }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueBack12.java

@@ -19,34 +19,40 @@ protected void runAutonomous() {
         // 1. move to shoot position
         controller.pathTo(AutonomousPoints.Blue.Back.SHOOT);
 
+        // 1.5 wait for spin up delay
+        controller.sleep(AutonomousConstants.SPIN_UP_DELAY_MS);
+
+
         // 2. shoot balls (preloads)
         controller.shoot(AutonomousConstants.PRELOAD_SHOOT_TIME);
 
         // Engage intake (for rest of auton)
         controller.runIntakeForward();
 
-        // 3. move to human player ball intake position
-        controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_HUMAN);
-
-        // 4. shoot balls
-        controller.pathTo(AutonomousPoints.Blue.Back.SHOOT);
-        controller.shoot(AutonomousConstants.CYCLE_SHOOT_TIME);
+        if (!AutonomousConstants.SKIP_HUMAN_INTAKE) {
+            // 3. move to human player ball intake position
+            controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_HUMAN);
+
+            // 4. shoot balls
+            controller.pathTo(AutonomousPoints.Blue.Back.SHOOT);
+            controller.shoot(AutonomousConstants.CYCLE_SHOOT_TIME);
+        }
 
         // 5. move to intake position 1 for back row intake
-        controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_CLOSE_1);
+        controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_CLOSE_1, AutonomousConstants.INTAKE_PATH_SPEED_SCALING);
 
         // 6. move to intake position 2 for back row intake
-        controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_CLOSE_2);
+        controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_CLOSE_2, AutonomousConstants.INTAKE_PATH_SPEED_SCALING);
 
         // 7. shoot balls
         controller.pathTo(AutonomousPoints.Blue.Back.SHOOT);
         controller.shoot(AutonomousConstants.CYCLE_SHOOT_TIME);
 
         // 8. move to intake position 1 for middle row intake
-        controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_MIDDLE_1);
+        controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_MIDDLE_1, AutonomousConstants.INTAKE_PATH_SPEED_SCALING);
 
         // 9. move to intake position 2 for middle row intake
-        controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_MIDDLE_2);
+        controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_MIDDLE_2, AutonomousConstants.INTAKE_PATH_SPEED_SCALING);
 
         // 10. shoot balls
         controller.pathTo(AutonomousPoints.Blue.Back.SHOOT);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueBack6.java

@@ -19,18 +19,24 @@ protected void runAutonomous() {
         // 1. move to shoot position
         controller.pathTo(AutonomousPoints.Blue.Back.SHOOT);
 
+        // 1.5 wait for spin up delay
+        controller.sleep(AutonomousConstants.SPIN_UP_DELAY_MS);
+
+
         // 2. shoot balls (preloads)
         controller.shoot(AutonomousConstants.PRELOAD_SHOOT_TIME);
 
         // Engage intake (for rest of auton)
         controller.runIntakeForward();
 
-        // 3. move to human player ball intake position
-        controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_HUMAN);
-
-        // 4. shoot balls
-        controller.pathTo(AutonomousPoints.Blue.Back.SHOOT);
-        controller.shoot(AutonomousConstants.CYCLE_SHOOT_TIME);
+        if (!AutonomousConstants.SKIP_HUMAN_INTAKE) {
+            // 3. move to human player ball intake position
+            controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_HUMAN);
+
+            // 4. shoot balls
+            controller.pathTo(AutonomousPoints.Blue.Back.SHOOT);
+            controller.shoot(AutonomousConstants.CYCLE_SHOOT_TIME);
+        }
 
         // 5. park back position
         controller.pathTo(AutonomousPoints.Blue.Back.CLOSE_PARK);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueBack9.java

@@ -19,24 +19,30 @@ protected void runAutonomous() {
         // 1. move to shoot position
         controller.pathTo(AutonomousPoints.Blue.Back.SHOOT);
 
+        // 1.5 wait for spin up delay
+        controller.sleep(AutonomousConstants.SPIN_UP_DELAY_MS);
+
+
         // 2. shoot balls (preloads)
         controller.shoot(AutonomousConstants.PRELOAD_SHOOT_TIME);
 
         // Engage intake (for rest of auton)
         controller.runIntakeForward();
 
-        // 3. move to human player ball intake position
-        controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_HUMAN);
-
-        // 4. shoot balls
-        controller.pathTo(AutonomousPoints.Blue.Back.SHOOT);
-        controller.shoot(AutonomousConstants.CYCLE_SHOOT_TIME);
+        if (!AutonomousConstants.SKIP_HUMAN_INTAKE) {
+            // 3. move to human player ball intake position
+            controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_HUMAN);
+
+            // 4. shoot balls
+            controller.pathTo(AutonomousPoints.Blue.Back.SHOOT);
+            controller.shoot(AutonomousConstants.CYCLE_SHOOT_TIME);
+        }
 
         // 5. move to intake position 1 for back row intake
-        controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_CLOSE_1);
+        controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_CLOSE_1, AutonomousConstants.INTAKE_PATH_SPEED_SCALING);
 
         // 6. move to intake position 2 for back row intake
-        controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_CLOSE_2);
+        controller.pathTo(AutonomousPoints.Blue.Back.INTAKE_CLOSE_2, AutonomousConstants.INTAKE_PATH_SPEED_SCALING);
 
         // 7. shoot balls
         controller.pathTo(AutonomousPoints.Blue.Back.SHOOT);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueFront3.java

@@ -19,6 +19,10 @@ protected void runAutonomous() {
         // 1. move to shoot position
         controller.pathTo(AutonomousPoints.Blue.Front.SHOOT);
 
+        // 1.5 wait for spin up delay
+        controller.sleep(AutonomousConstants.SPIN_UP_DELAY_MS);
+
+
         // 2. shoot balls (preloads)
         controller.shoot(AutonomousConstants.PRELOAD_SHOOT_TIME);
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/opmodes/BlueFront6.java

@@ -19,17 +19,21 @@ protected void runAutonomous() {
         // 1. move to shoot position
         controller.pathTo(AutonomousPoints.Blue.Front.SHOOT);
 
+        // 1.5 wait for spin up delay
+        controller.sleep(AutonomousConstants.SPIN_UP_DELAY_MS);
+
+
         // 2. shoot balls (preloads)
         controller.shoot(AutonomousConstants.PRELOAD_SHOOT_TIME);
 
         // Engage intake (for rest of auton)
         controller.runIntakeForward();
 
         // 3. move to intake position 1 for far row intake
-        controller.pathTo(AutonomousPoints.Blue.Front.INTAKE_FAR_1);
+        controller.pathTo(AutonomousPoints.Blue.Front.INTAKE_FAR_1, AutonomousConstants.INTAKE_PATH_SPEED_SCALING);
 
         // 4. move to intake position 2 for far row intake
-        controller.pathTo(AutonomousPoints.Blue.Front.INTAKE_FAR_2);
+        controller.pathTo(AutonomousPoints.Blue.Front.INTAKE_FAR_2, AutonomousConstants.INTAKE_PATH_SPEED_SCALING);
 
         // 5. move to shoot position
         controller.pathTo(AutonomousPoints.Blue.Front.SHOOT);