Lebob-Robotics · Eason-F · Sep 27, 2026
@@ -114,3 +114,32 @@ run the `TeamCode` configuration. Android Studio does the same `adb install` and
 launch shown above. If the Hub does not appear in the device dropdown, run
 `adb connect 192.168.43.1:5555` in a terminal first; Android Studio picks up
 devices from the same adb server.
+
+## TeleOp hardware and controls
+
+Before selecting **BioBuzz TeleOp**, configure the Control Hub with these exact
+names: `front_left_drive`, `front_right_drive`, `back_left_drive`,
+`back_right_drive`, `pinpoint`, `intake`, `indexer`, `shooter_left`, and
+`shooter_right`. The intake and shooter motors must support `DcMotorEx` encoder
+control. Motor directions, shooter speed and PIDF, and Pinpoint offsets are in
+`TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Constants.java`; confirm
+them on the robot before a match.
+
+During INIT, D-pad left selects red and D-pad right selects blue. The selected
+alliance appears in telemetry. The current code does not use it for camera
+aiming yet.
+
+| Gamepad 1 input | Action |
+| --- | --- |
+| Left stick | Field-relative translation |
+| Right stick X | Rotation |
+| Left bumper (hold) | Robot-relative translation |
+| A | Zero heading |
+| Right trigger (hold) | Run intake and indexer |
+| Left trigger (hold) | Reverse intake and indexer |
+| Right bumper | Toggle shooter spin-up |
+| X (hold) | Feed with the indexer when both flywheels are at speed |
+
+Left trigger takes priority over right trigger, and either trigger takes
+priority over X. Test motor directions, Pinpoint heading, and shooter RPM on
+the robot before firing.
@@ -0,0 +1,6 @@
+package org.firstinspires.ftc.teamcode;
+
+public enum Alliance {
+    RED,
+    BLUE
+}
@@ -0,0 +1,57 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+
+/** Hardware names, directions and tuning values. One home so drivers and Pedro Pathing can find them. */
+public final class Constants {
+    private Constants() {}
+
+    // Robot Controller configuration names.
+    public static final String FRONT_LEFT_DRIVE = "front_left_drive";
+    public static final String FRONT_RIGHT_DRIVE = "front_right_drive";
+    public static final String BACK_LEFT_DRIVE = "back_left_drive";
+    public static final String BACK_RIGHT_DRIVE = "back_right_drive";
+    public static final String INTAKE = "intake";
+    public static final String INDEXER = "indexer";
+    public static final String SHOOTER_LEFT = "shooter_left";
+    public static final String SHOOTER_RIGHT = "shooter_right";
+    public static final String PINPOINT = "pinpoint";
+    public static final String WEBCAM = "Webcam 1";
+
+    // Motor directions. Left side reversed so positive power on every drive motor goes forward.
+    public static final DcMotorSimple.Direction FRONT_LEFT_DIRECTION = DcMotorSimple.Direction.REVERSE;
+    public static final DcMotorSimple.Direction FRONT_RIGHT_DIRECTION = DcMotorSimple.Direction.FORWARD;
+    public static final DcMotorSimple.Direction BACK_LEFT_DIRECTION = DcMotorSimple.Direction.REVERSE;
+    public static final DcMotorSimple.Direction BACK_RIGHT_DIRECTION = DcMotorSimple.Direction.FORWARD;
+    public static final DcMotorSimple.Direction INTAKE_DIRECTION = DcMotorSimple.Direction.FORWARD;
+    public static final DcMotorSimple.Direction INDEXER_DIRECTION = DcMotorSimple.Direction.FORWARD;
+    public static final DcMotorSimple.Direction SHOOTER_LEFT_DIRECTION = DcMotorSimple.Direction.FORWARD;
+    public static final DcMotorSimple.Direction SHOOTER_RIGHT_DIRECTION = DcMotorSimple.Direction.REVERSE;
+
+    // Pinpoint pod offsets from the tracking point, mm. X pod: left of centre positive.
+    // Y pod: forward of centre positive. Measure on the robot per the goBILDA setup guide.
+    public static final double PINPOINT_X_OFFSET_MM = 0.0;
+    public static final double PINPOINT_Y_OFFSET_MM = -105.0;
+
+    // Intake and indexer open-loop powers.
+    public static final double INTAKE_POWER = 1.0;
+    public static final double INDEXER_POWER = 1.0;
+
+    // Shooter. 1:1 Yellow Jacket, 28 ticks per rev, 6000 RPM free speed.
+    public static final double SHOOTER_TICKS_PER_REV = 28.0;
+    public static final double SHOOTER_SETPOINT_RPM = 3500.0;   // tune on the robot
+    public static final double SHOOTER_TOLERANCE_RPM = 100.0;
+    // Velocity PIDF starting point: F = 32767 / max ticks per second, P = 0.1 F, I = 0.1 P, D = 0.
+    public static final double SHOOTER_P = 1.17;
+    public static final double SHOOTER_I = 0.117;
+    public static final double SHOOTER_D = 0.0;
+    public static final double SHOOTER_F = 11.7;
+
+    // Driver controls.
+    public static final double TRIGGER_THRESHOLD = 0.2;
+
+    // Aim assist: rotation = -AIM_KP * bearingDeg, clamped. Bearing is positive to the left.
+    public static final double AIM_KP = 0.02;
+    public static final double AIM_MAX_ROTATE = 0.5;
+    public static final double AIM_DEADBAND_DEG = 1.0;
+}
@@ -13,6 +13,11 @@ public void init() {
         robot.init();
     }
 
+    @Override
+    public void init_loop() {
+        robot.initLoop();
+    }
+
     @Override
     public void loop() {
         robot.periodic();

@@ -1,61 +1,116 @@
 package org.firstinspires.ftc.teamcode;
 
 import com.arcrobotics.ftclib.command.CommandScheduler;
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+import com.qualcomm.hardware.lynx.LynxModule;
 import com.qualcomm.robotcore.hardware.Gamepad;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.teamcode.subsystems.IndexerSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.IntakeSubsystem;
 import org.firstinspires.ftc.teamcode.subsystems.MecanumDriveSubsystem;
 import org.firstinspires.ftc.teamcode.subsystems.OdometrySubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.ShooterSubsystem;
 
 public class Robot {
     private final Telemetry telemetry;
-    private final Gamepad driver;
+    private final GamepadEx driver;
+    private Alliance alliance = Alliance.RED;
 
     public final MecanumDriveSubsystem drive;
     public final OdometrySubsystem odometry;
+    public final IntakeSubsystem intake;
+    public final IndexerSubsystem indexer;
+    public final ShooterSubsystem shooter;
 
-    public Robot(
-            HardwareMap hardwareMap,
-            Telemetry telemetry,
-            Gamepad driver
-    ) {
+    public Robot(HardwareMap hardwareMap, Telemetry telemetry, Gamepad driverGamepad) {
         this.telemetry = telemetry;
-        this.driver = driver;
+        this.driver = new GamepadEx(driverGamepad);
+
+        for (LynxModule hub : hardwareMap.getAll(LynxModule.class)) {
+            hub.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
+        }
 
         CommandScheduler.getInstance().reset();
 
-        // Construct subsystems.
         drive = new MecanumDriveSubsystem(hardwareMap);
         odometry = new OdometrySubsystem(hardwareMap);
-        // intake = new IntakeSubsystem(hardwareMap);
-        // outtake = new OuttakeSubsystem(hardwareMap);
+        intake = new IntakeSubsystem(hardwareMap);
+        indexer = new IndexerSubsystem(hardwareMap);
+        shooter = new ShooterSubsystem(hardwareMap);
     }
 
     /** Called once when the OpMode enters INIT. */
     public void init() {
         odometry.init();
     }
 
+    /** Select the alliance before START with D-pad left or right. */
+    public void initLoop() {
+        driver.readButtons();
+        CommandScheduler.getInstance().run();
+
+        if (driver.wasJustPressed(GamepadKeys.Button.DPAD_LEFT)) {
+            alliance = Alliance.RED;
+        } else if (driver.wasJustPressed(GamepadKeys.Button.DPAD_RIGHT)) {
+            alliance = Alliance.BLUE;
+        }
+
+        telemetry.addData("Alliance (D-pad left/right)", alliance);
+        telemetry.update();
+    }
+
     /** Called repeatedly while the OpMode is running. */
     public void periodic() {
+        driver.readButtons();
         CommandScheduler.getInstance().run();
 
-        if (driver.a) {
+        if (driver.wasJustPressed(GamepadKeys.Button.A)) {
             odometry.resetHeading();
         }
 
         // 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));
+        drive.drive(driver.getLeftY(), driver.getLeftX(), driver.getRightX(),
+                !driver.isDown(GamepadKeys.Button.LEFT_BUMPER),
+                odometry.getPose().getHeading(AngleUnit.RADIANS));
+
+        if (driver.wasJustPressed(GamepadKeys.Button.RIGHT_BUMPER)) {
+            shooter.toggle();
+        }
+
+        double leftTrigger = driver.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER);
+        double rightTrigger = driver.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER);
+        if (leftTrigger > Constants.TRIGGER_THRESHOLD) {
+            intake.reverse();
+            indexer.reverse();
+        } else if (rightTrigger > Constants.TRIGGER_THRESHOLD) {
+            intake.run();
+            indexer.feed();
+        } else if (driver.isDown(GamepadKeys.Button.X) && shooter.atSpeed()) {
+            intake.stop();
+            indexer.feed();
+        } else {
+            intake.stop();
+            indexer.stop();
+        }
 
+        telemetry.addData("Alliance", alliance);
         telemetry.addData("Pose", odometry.getPose());
+        telemetry.addData("Shooter", "%s target %.0f RPM  L %.0f  R %.0f  %s",
+                shooter.isRunning() ? "ON" : "off", Constants.SHOOTER_SETPOINT_RPM,
+                shooter.getLeftRpm(), shooter.getRightRpm(), shooter.atSpeed() ? "AT SPEED" : "");
         telemetry.update();
     }
 
     /** Called once when the OpMode stops. */
     public void stop() {
+        drive.stop();
+        intake.stop();
+        indexer.stop();
+        shooter.idle();
         CommandScheduler.getInstance().cancelAll();
         CommandScheduler.getInstance().reset();
     }

@@ -0,0 +1,14 @@
+package org.firstinspires.ftc.teamcode;
+
+/** Encoder velocity conversions for the shooter flywheels. */
+public final class ShooterMath {
+    private ShooterMath() {}
+
+    public static double rpmToTicksPerSecond(double rpm, double ticksPerRev) {
+        return rpm * ticksPerRev / 60.0;
+    }
+
+    public static double ticksPerSecondToRpm(double ticksPerSecond, double ticksPerRev) {
+        return ticksPerSecond * 60.0 / ticksPerRev;
+    }
+}
@@ -0,0 +1,32 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import com.arcrobotics.ftclib.command.SubsystemBase;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.Constants;
+
+/** Feeds balls to the shooter and brakes when stopped. */
+public class IndexerSubsystem extends SubsystemBase {
+    private final DcMotorEx motor;
+
+    public IndexerSubsystem(HardwareMap hardwareMap) {
+        motor = hardwareMap.get(DcMotorEx.class, Constants.INDEXER);
+        motor.setDirection(Constants.INDEXER_DIRECTION);
+        motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+    }
+
+    public void feed() {
+        motor.setPower(Constants.INDEXER_POWER);
+    }
+
+    public void reverse() {
+        motor.setPower(-Constants.INDEXER_POWER);
+    }
+
+    public void stop() {
+        motor.setPower(0);
+    }
+}
@@ -0,0 +1,36 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import com.arcrobotics.ftclib.command.SubsystemBase;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.Constants;
+
+/** Front intake roller, driven open loop. */
+public class IntakeSubsystem extends SubsystemBase {
+    private final DcMotorEx motor;
+
+    public IntakeSubsystem(HardwareMap hardwareMap) {
+        motor = hardwareMap.get(DcMotorEx.class, Constants.INTAKE);
+        motor.setDirection(Constants.INTAKE_DIRECTION);
+        motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
+    }
+
+    public void intake(double power) {
+        motor.setPower(power);
+    }
+
+    public void run() {
+        intake(Constants.INTAKE_POWER);
+    }
+
+    public void reverse() {
+        intake(-Constants.INTAKE_POWER);
+    }
+
+    public void stop() {
+        intake(0);
+    }
+}
@@ -5,6 +5,7 @@
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.teamcode.Constants;
 
 /**
  * Drivetrain subsystem for a four-motor mecanum base, with optional field-centric
@@ -17,16 +18,16 @@ public class MecanumDriveSubsystem extends SubsystemBase {
     private final DcMotor backRightDrive;
 
     public MecanumDriveSubsystem(HardwareMap hardwareMap) {
-        frontLeftDrive = hardwareMap.get(DcMotor.class, "front_left_drive");
-        frontRightDrive = hardwareMap.get(DcMotor.class, "front_right_drive");
-        backLeftDrive = hardwareMap.get(DcMotor.class, "back_left_drive");
-        backRightDrive = hardwareMap.get(DcMotor.class, "back_right_drive");
+        frontLeftDrive = hardwareMap.get(DcMotor.class, Constants.FRONT_LEFT_DRIVE);
+        frontRightDrive = hardwareMap.get(DcMotor.class, Constants.FRONT_RIGHT_DRIVE);
+        backLeftDrive = hardwareMap.get(DcMotor.class, Constants.BACK_LEFT_DRIVE);
+        backRightDrive = hardwareMap.get(DcMotor.class, Constants.BACK_RIGHT_DRIVE);
 
         // Left motors are flipped so that a positive power on every motor drives straight.
-        frontLeftDrive.setDirection(DcMotor.Direction.REVERSE);
-        frontRightDrive.setDirection(DcMotor.Direction.FORWARD);
-        backLeftDrive.setDirection(DcMotor.Direction.REVERSE);
-        backRightDrive.setDirection(DcMotor.Direction.FORWARD);
+        frontLeftDrive.setDirection(Constants.FRONT_LEFT_DIRECTION);
+        frontRightDrive.setDirection(Constants.FRONT_RIGHT_DIRECTION);
+        backLeftDrive.setDirection(Constants.BACK_LEFT_DIRECTION);
+        backRightDrive.setDirection(Constants.BACK_RIGHT_DIRECTION);
 
         frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
         frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
@@ -70,4 +71,12 @@ public void drive(double forward, double right, double rotate, boolean fieldCent
         backLeftDrive.setPower(backLeftPower / maxPower);
         backRightDrive.setPower(backRightPower / maxPower);
     }
+
+    /** Stops all drive motors when the OpMode ends. */
+    public void stop() {
+        frontLeftDrive.setPower(0);
+        frontRightDrive.setPower(0);
+        backLeftDrive.setPower(0);
+        backRightDrive.setPower(0);
+    }
 }
@@ -7,6 +7,7 @@
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.firstinspires.ftc.robotcore.external.navigation.Pose2D;
+import org.firstinspires.ftc.teamcode.Constants;
 
 /**
  * Wraps a goBILDA Pinpoint Odometry Computer reading two dead-wheel pods, and
@@ -16,12 +17,12 @@ public class OdometrySubsystem extends SubsystemBase {
     private final GoBildaPinpointDriver pinpoint;
 
     public OdometrySubsystem(HardwareMap hardwareMap) {
-        pinpoint = hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint");
+        pinpoint = hardwareMap.get(GoBildaPinpointDriver.class, Constants.PINPOINT);
     }
 
     /** Called once from the OpMode's init(). Configures the Pinpoint and zeroes its pose. */
     public void init() {
-        pinpoint.setOffsets(0.0, -105.0, DistanceUnit.MM);
+        pinpoint.setOffsets(Constants.PINPOINT_X_OFFSET_MM, Constants.PINPOINT_Y_OFFSET_MM, DistanceUnit.MM);
         pinpoint.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD);
         pinpoint.setEncoderDirections(
                 GoBildaPinpointDriver.EncoderDirection.FORWARD,