add control for shooter, intake and indexer

.gitignore

@@ -87,3 +87,5 @@ lint/tmp/
 # lint/reports/
 # Linked git worktrees for parallel sessions
 .worktrees/
+# Local Android SDK install
+/android-sdk/

README.md

@@ -4,7 +4,7 @@ Robot code for Lebob Robotics, team 29550, competing in FIRST Tech Challenge
 BIOBUZZ 2026/27 at the Western Australia Qualifier.
 
 This repository is an unmodified import of the FIRST Tech Challenge SDK with
-our own code in `TeamCode`. 
+our own code in `TeamCode`.
 
 ## Requirements
 
@@ -14,11 +14,11 @@ our own code in `TeamCode`.
 
 The required SDK packages:
 
-| Package | Why |
-| --- | --- |
+| Package                | Why                                               |
+| ---------------------- | ------------------------------------------------- |
 | `platforms;android-30` | `build.common.gradle` sets `compileSdkVersion 30` |
-| `build-tools;35.0.0` | the revision AGP 8.13.2 resolves to |
-| `platform-tools` | provides `adb`, used to deploy to the Control Hub |
+| `build-tools;35.0.0`   | the revision AGP 8.13.2 resolves to               |
+| `platform-tools`       | provides `adb`, used to deploy to the Control Hub |
 
 ### Getting the SDK without Android Studio
 
@@ -98,14 +98,12 @@ above restarts it; `adb reboot` also works and takes longer.
 A one-line version of build-and-deploy, once the connection is up:
 
 ```
-./gradlew :TeamCode:assembleDebug && \
-  adb install -r TeamCode/build/outputs/apk/debug/TeamCode-debug.apk && \
-  adb shell am start -n com.qualcomm.ftcrobotcontroller/org.firstinspires.ftc.robotcontroller.internal.PermissionValidatorWrapper
+./gradlew :TeamCode:assembleDebug && adb install -r TeamCode/build/outputs/apk/debug/TeamCode-debug.apk && adb shell am start -n com.qualcomm.ftcrobotcontroller/org.firstinspires.ftc.robotcontroller.internal.PermissionValidatorWrapper
 ```
 
 ### Using Android Studio instead
 
-Android Studio download: 
+Android Studio download:
 
 <https://developer.android.com/studio>
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Main.java

@@ -5,21 +5,24 @@
 
 @TeleOp(name = "BioBuzz TeleOp", group = "Robot")
 public class Main extends OpMode {
-    private Robot robot;
+  private Robot robot;
 
-    @Override
-    public void init() {
-        robot = new Robot(hardwareMap, telemetry, gamepad1);
-        robot.init();
-    }
+  @Override
+  public void init() {
+    robot = new Robot(hardwareMap, telemetry, gamepad1);
+    robot.init();
+  }
 
-    @Override
-    public void loop() {
-        robot.periodic();
-    }
+  @Override
+  public void loop() {
+    robot.periodic();
+  }
 
-    @Override
-    public void stop() {
-        robot.stop();
+  @Override
+  public void stop() {
+    // robot is null if init() threw (e.g. a device name missing from the config).
+    if (robot != null) {
+      robot.stop();
     }
+  }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java

@@ -6,57 +6,92 @@
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.teamcode.subsystems.IntakeSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.IntakeSubsystem.IntakeState;
 import org.firstinspires.ftc.teamcode.subsystems.MecanumDriveSubsystem;
 import org.firstinspires.ftc.teamcode.subsystems.OdometrySubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ShooterSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ShooterSubsystem.ShooterState;
 
 public class Robot {
-    private final Telemetry telemetry;
-    private final Gamepad driver;
-
-    public final MecanumDriveSubsystem drive;
-    public final OdometrySubsystem odometry;
-
-    public Robot(
-            HardwareMap hardwareMap,
-            Telemetry telemetry,
-            Gamepad driver
-    ) {
-        this.telemetry = telemetry;
-        this.driver = driver;
-
-        CommandScheduler.getInstance().reset();
-
-        // Construct subsystems.
-        drive = new MecanumDriveSubsystem(hardwareMap);
-        odometry = new OdometrySubsystem(hardwareMap);
-        // intake = new IntakeSubsystem(hardwareMap);
-        // outtake = new OuttakeSubsystem(hardwareMap);
-    }
+  private final Telemetry telemetry;
+  private final Gamepad driver;
 
-    /** Called once when the OpMode enters INIT. */
-    public void init() {
-        odometry.init();
-    }
+  public final IntakeSubsystem intake;
+  public final MecanumDriveSubsystem drive;
+  public final OdometrySubsystem odometry;
+  public final ShooterSubsystem shooter;
+
+  public static final double triggerLimit = 0.5;
+
+  public Robot(
+      HardwareMap hardwareMap,
+      Telemetry telemetry,
+      Gamepad driver) {
+    this.telemetry = telemetry;
+    this.driver = driver;
+
+    CommandScheduler.getInstance().reset();
+
+    // Construct subsystems.
+    intake = new IntakeSubsystem(hardwareMap);
+    drive = new MecanumDriveSubsystem(hardwareMap);
+    odometry = new OdometrySubsystem(hardwareMap);
+    shooter = new ShooterSubsystem(hardwareMap);
 
-    /** Called repeatedly while the OpMode is running. */
-    public void periodic() {
-        CommandScheduler.getInstance().run();
+  }
 
-        if (driver.a) {
-            odometry.resetHeading();
-        }
+  /** Called once when the OpMode enters INIT. */
+  public void init() {
+    odometry.init();
+    shooter.setShooterState(ShooterState.IDLE);
+  }
 
-        // Hold left bumper to drive robot-relative; otherwise drive field-relative.
-        drive.drive(-driver.left_stick_y, driver.left_stick_x, driver.right_stick_x,
-                !driver.left_bumper, odometry.getPose().getHeading(AngleUnit.RADIANS));
+  /** Called repeatedly while the OpMode is running. */
+  public void periodic() {
+    CommandScheduler.getInstance().run();
 
-        telemetry.addData("Pose", odometry.getPose());
-        telemetry.update();
+    if (driver.a) {
+      odometry.resetHeading();
     }
 
-    /** Called once when the OpMode stops. */
-    public void stop() {
-        CommandScheduler.getInstance().cancelAll();
-        CommandScheduler.getInstance().reset();
+    if (driver.right_trigger > triggerLimit) {
+      shooter.setShooterState(ShooterState.SHOOT);
+    } else if (driver.b) {
+      shooter.setShooterState(ShooterState.EJECT);
+    } else if (driver.x) {
+      shooter.setShooterState(ShooterState.STOP);
+    } else {
+      shooter.setShooterState(ShooterState.IDLE);
     }
+
+    // The intake also runs while shooting: the indexer can't pull in balls waiting
+    // where the intake meets it, so with the intake stopped they sit there. It
+    // pauses while the indexer backs off a jam, so it doesn't pack the jam tighter.
+    boolean feeding = driver.right_trigger > triggerLimit && shooter.isIndexerFeeding();
+    if (driver.b) {
+      intake.setIntakeState(IntakeState.EJECT);
+    } else if (driver.left_trigger > triggerLimit || feeding) {
+      intake.setIntakeState(IntakeState.INTAKE);
+    } else {
+      intake.setIntakeState(IntakeState.STOP);
+    }
+
+    // Hold left bumper to drive robot-relative; otherwise drive field-relative.
+    drive.drive(-driver.left_stick_y, driver.left_stick_x, driver.right_stick_x,
+        !driver.left_bumper, odometry.getPose().getHeading(AngleUnit.RADIANS));
+
+    telemetry.addData("Pose", odometry.getPose());
+    telemetry.addData("Indexer", shooter.getIndexerStatus());
+    if (shooter.isIndexerJammed()) {
+      telemetry.addLine("INDEXER JAMMED: release the right trigger and clear the balls");
+    }
+    telemetry.update();
+  }
+
+  /** Called once when the OpMode stops. */
+  public void stop() {
+    CommandScheduler.getInstance().cancelAll();
+    CommandScheduler.getInstance().reset();
+  }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/GobildaMotor.java

@@ -0,0 +1,113 @@
+package org.firstinspires.ftc.teamcode.hardware;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
+
+/**
+ * Thin wrapper around a {@link DcMotorEx}, shared by every subsystem that
+ * drives a motor (drivetrain, intake, shooter, indexer, ...).
+ *
+ * <pre>{@code
+ * GobildaMotor shooter = new GobildaMotor(
+ *     hardwareMap, "shooter", true, DcMotor.RunMode.RUN_USING_ENCODER, false);
+ * }</pre>
+ */
+public class GobildaMotor {
+  /** Power changes smaller than this are not re-sent to the hub. */
+  private static final double POWER_MIN = 1e-3;
+
+  private final String name;
+  private final DcMotorEx motor;
+  private double lastPower = Double.NaN;
+
+  /**
+   * @param hardwareMap the OpMode's hardware map
+   * @param name        the device name in the robot configuration
+   * @param reversed    true to flip the motor's positive direction
+   * @param mode        run mode to configure the motor with
+   * @param brake       true to brake at zero power, false to float
+   */
+  public GobildaMotor(HardwareMap hardwareMap, String name, boolean reversed, DcMotor.RunMode mode, boolean brake) {
+    this.name = name;
+    this.motor = hardwareMap.get(DcMotorEx.class, name);
+    motor.setDirection(reversed ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
+    motor.setMode(mode);
+    motor.setZeroPowerBehavior(brake ? DcMotor.ZeroPowerBehavior.BRAKE : DcMotor.ZeroPowerBehavior.FLOAT);
+  }
+
+  /** Zeroes the encoder, then restores the previous run mode. */
+  public GobildaMotor resetEncoder() {
+    DcMotor.RunMode previous = motor.getMode();
+    motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+    motor.setMode(previous);
+    return this;
+  }
+
+  /**
+   * Sets motor power, clamped to [-1, 1]. Skips the hardware write if unchanged.
+   */
+  public void setPower(double power) {
+    power = Math.max(-1.0, Math.min(1.0, power));
+    if (!Double.isNaN(lastPower) && Math.abs(power - lastPower) < POWER_MIN) {
+      return;
+    }
+    lastPower = power;
+    motor.setPower(power);
+  }
+
+  /**
+   * Changes the run mode if it differs, and forgets the cached power so the
+   * next setPower reaches the motor in the new mode.
+   */
+  public void setMode(DcMotor.RunMode mode) {
+    if (motor.getMode() == mode) {
+      return;
+    }
+    motor.setMode(mode);
+    lastPower = Double.NaN;
+  }
+
+  /** Sets a target velocity in ticks/second (requires RUN_USING_ENCODER). */
+  public void setVelocity(double ticksPerSecond) {
+    lastPower = Double.NaN; // power cache is no longer meaningful
+    motor.setVelocity(ticksPerSecond);
+  }
+
+  public void stop() {
+    setPower(0.0);
+  }
+
+  // ---------------------------------------------------------------- state
+
+  public double getPower() {
+    return motor.getPower();
+  }
+
+  /** Encoder position in ticks. */
+  public int getPosition() {
+    return motor.getCurrentPosition();
+  }
+
+  /** Velocity in ticks/second. */
+  public double getVelocity() {
+    return motor.getVelocity();
+  }
+
+  /** Current draw in amps. Each call is a separate hub read. */
+  public double getCurrentAmps() {
+    return motor.getCurrent(CurrentUnit.AMPS);
+  }
+
+  public String getName() {
+    return name;
+  }
+
+  /** Escape hatch for anything not wrapped here. */
+  public DcMotorEx getRaw() {
+    return motor;
+  }
+}