From 6a5728d8777d0fbfc5df63e777aedb0d19ce509c Mon Sep 17 00:00:00 2001 From: bobel-lebob Date: Mon, 28 Sep 2026 12:21:57 +0800 Subject: [PATCH 01/14] chore(main): make robot only stop if it is null due to init error --- .../src/main/java/org/firstinspires/ftc/teamcode/Main.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Main.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Main.java index 3a9752b..f503112 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Main.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Main.java @@ -20,6 +20,9 @@ public void loop() { @Override public void stop() { - robot.stop(); + // robot is null if init() threw (e.g. a device name missing from the config). + if (robot != null) { + robot.stop(); + } } } From ecfd5009603108dc19cecac24c14aea04d5477e6 Mon Sep 17 00:00:00 2001 From: bobel-lebob Date: Mon, 28 Sep 2026 12:23:34 +0800 Subject: [PATCH 02/14] refactor(docs): format readme --- README.md | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index 6fd496f..2b752ba 100644 --- a/README.md +++ b/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: From 1f443fe10cdf1b4f4db3899f77e7f7004d51aa25 Mon Sep 17 00:00:00 2001 From: bobel-lebob Date: Mon, 28 Sep 2026 12:23:49 +0800 Subject: [PATCH 03/14] chore(gitignore): add build files to gitignore --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.gitignore b/.gitignore index 2adcaa6..e0be891 100644 --- a/.gitignore +++ b/.gitignore @@ -87,3 +87,5 @@ lint/tmp/ # lint/reports/ # Linked git worktrees for parallel sessions .worktrees/ +# Local Android SDK install +/android-sdk/ From 7b98867c0940209236cb4b17aa545ddd729b1bf3 Mon Sep 17 00:00:00 2001 From: bobel-lebob Date: Mon, 28 Sep 2026 12:25:44 +0800 Subject: [PATCH 04/14] refactor(code): refactor all code to follow java standard --- .../org/firstinspires/ftc/teamcode/Main.java | 32 +++--- .../org/firstinspires/ftc/teamcode/Robot.java | 89 ++++++++------- .../org/firstinspires/ftc/teamcode/readme.md | 83 +++++++------- .../subsystems/MecanumDriveSubsystem.java | 106 +++++++++--------- .../subsystems/OdometrySubsystem.java | 71 ++++++------ 5 files changed, 193 insertions(+), 188 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Main.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Main.java index f503112..d18dbf0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Main.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Main.java @@ -5,24 +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 is null if init() threw (e.g. a device name missing from the config). - if (robot != null) { - 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(); } + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index 3b08345..9bd44dc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -10,53 +10,52 @@ import org.firstinspires.ftc.teamcode.subsystems.OdometrySubsystem; 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; + + 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); + } + + /** Called once when the OpMode enters INIT. */ + public void init() { + odometry.init(); + } + + /** 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(); - } - - /** Called repeatedly while the OpMode is running. */ - public void periodic() { - CommandScheduler.getInstance().run(); - - if (driver.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)); - // 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.update(); + } - telemetry.addData("Pose", odometry.getPose()); - telemetry.update(); - } - - /** Called once when the OpMode stops. */ - public void stop() { - CommandScheduler.getInstance().cancelAll(); - CommandScheduler.getInstance().reset(); - } + /** Called once when the OpMode stops. */ + public void stop() { + CommandScheduler.getInstance().cancelAll(); + CommandScheduler.getInstance().reset(); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md index 4d1da42..cc5675a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md @@ -14,7 +14,7 @@ Sample opmodes exist in the FtcRobotController module. To locate these samples, find the FtcRobotController module in the "Project/Android" tab. Expand the following tree elements: - FtcRobotController/java/org.firstinspires.ftc.robotcontroller/external/samples +FtcRobotController/java/org.firstinspires.ftc.robotcontroller/external/samples ### Naming of Samples @@ -27,44 +27,44 @@ To summarize: A range of different samples classes will reside in the java/exter The class names will follow a naming convention which indicates the purpose of each class. The prefix of the name will be one of the following: -Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure - of a particular style of OpMode. These are bare bones examples. +Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure +of a particular style of OpMode. These are bare bones examples. -Sensor: This is a Sample OpMode that shows how to use a specific sensor. - It is not intended to drive a functioning robot, it is simply showing the minimal code - required to read and display the sensor values. +Sensor: This is a Sample OpMode that shows how to use a specific sensor. +It is not intended to drive a functioning robot, it is simply showing the minimal code +required to read and display the sensor values. -Robot: This is a Sample OpMode that assumes a simple two-motor (differential) drive base. - It may be used to provide a common baseline driving OpMode, or - to demonstrate how a particular sensor or concept can be used to navigate. +Robot: This is a Sample OpMode that assumes a simple two-motor (differential) drive base. +It may be used to provide a common baseline driving OpMode, or +to demonstrate how a particular sensor or concept can be used to navigate. -Concept: This is a sample OpMode that illustrates performing a specific function or concept. - These may be complex, but their operation should be explained clearly in the comments, - or the comments should reference an external doc, guide or tutorial. - Each OpMode should try to only demonstrate a single concept so they are easy to - locate based on their name. These OpModes may not produce a drivable robot. +Concept: This is a sample OpMode that illustrates performing a specific function or concept. +These may be complex, but their operation should be explained clearly in the comments, +or the comments should reference an external doc, guide or tutorial. +Each OpMode should try to only demonstrate a single concept so they are easy to +locate based on their name. These OpModes may not produce a drivable robot. After the prefix, other conventions will apply: -* Sensor class names are constructed as: Sensor - Company - Type -* Robot class names are constructed as: Robot - Mode - Action - OpModetype -* Concept class names are constructed as: Concept - Topic - OpModetype +- Sensor class names are constructed as: Sensor - Company - Type +- Robot class names are constructed as: Robot - Mode - Action - OpModetype +- Concept class names are constructed as: Concept - Topic - OpModetype Once you are familiar with the range of samples available, you can choose one to be the -basis for your own robot. In all cases, the desired sample(s) needs to be copied into +basis for your own robot. In all cases, the desired sample(s) needs to be copied into your TeamCode module to be used. This is done inside Android Studio directly, using the following steps: - 1) Locate the desired sample class in the Project/Android tree. +1. Locate the desired sample class in the Project/Android tree. - 2) Right click on the sample class and select "Copy" +2. Right click on the sample class and select "Copy" - 3) Expand the TeamCode/java folder +3. Expand the TeamCode/java folder - 4) Right click on the org.firstinspires.ftc.teamcode folder and select "Paste" +4. Right click on the org.firstinspires.ftc.teamcode folder and select "Paste" - 5) You will be prompted for a class name for the copy. +5. You will be prompted for a class name for the copy. Choose something meaningful based on the purpose of this class. Start with a capital letter, and remember that there may be more similar classes later. @@ -80,20 +80,18 @@ Each OpMode sample class begins with several lines of code like the ones shown b ``` The name that will appear on the driver station's "opmode list" is defined by the code: - ``name="Template: Linear OpMode"`` +`name="Template: Linear OpMode"` You can change what appears between the quotes to better describe your opmode. The "group=" portion of the code can be used to help organize your list of OpModes. As shown, the current OpMode will NOT appear on the driver station's OpMode list because of the - ``@Disabled`` annotation which has been included. +`@Disabled` annotation which has been included. This line can simply be deleted , or commented out, to make the OpMode visible. - - -## ADVANCED Multi-Team App management: Cloning the TeamCode Module +## ADVANCED Multi-Team App management: Cloning the TeamCode Module In some situations, you have multiple teams in your club and you want them to all share -a common code organization, with each being able to *see* the others code but each having +a common code organization, with each being able to _see_ the others code but each having their own team module with their own code that they maintain themselves. In this situation, you might wish to clone the TeamCode module, once for each of these teams. @@ -103,29 +101,30 @@ together with the FtcRobotController module (and the original TeamCode module). Selective Team phones can then be programmed by selecting the desired Module from the pulldown list prior to clicking to the green Run arrow. -Warning: This is not for the inexperienced Software developer. +Warning: This is not for the inexperienced Software developer. You will need to be comfortable with File manipulations and managing Android Studio Modules. These changes are performed OUTSIDE of Android Studios, so close Android Studios before you do this. - + Also.. Make a full project backup before you start this :) To clone TeamCode, do the following: -Note: Some names start with "Team" and others start with "team". This is intentional. +Note: Some names start with "Team" and others start with "team". This is intentional. -1) Using your operating system file management tools, copy the whole "TeamCode" +1. Using your operating system file management tools, copy the whole "TeamCode" folder to a sibling folder with a corresponding new name, eg: "Team0417". -2) In the new Team0417 folder, delete the TeamCode.iml file. +2. In the new Team0417 folder, delete the TeamCode.iml file. -3) the new Team0417 folder, rename the "src/main/java/org/firstinspires/ftc/teamcode" folder - to a matching name with a lowercase 'team' eg: "team0417". +3. the new Team0417 folder, rename the "src/main/java/org/firstinspires/ftc/teamcode" folder + to a matching name with a lowercase 'team' eg: "team0417". -4) In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, change the line that contains - package="org.firstinspires.ftc.teamcode" +4. In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, change the line that contains + package="org.firstinspires.ftc.teamcode" to be - package="org.firstinspires.ftc.team0417" + package="org.firstinspires.ftc.team0417" + +5. Add: include ':Team0417' to the "/settings.gradle" file. + +6. Open up Android Studios and clean out any old files by using the menu to "Build/Clean Project"" -5) Add: include ':Team0417' to the "/settings.gradle" file. - -6) Open up Android Studios and clean out any old files by using the menu to "Build/Clean Project"" \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java index 2f14fe6..facab1f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java @@ -7,67 +7,71 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; /** - * Drivetrain subsystem for a four-motor mecanum base, with optional field-centric + * Drivetrain subsystem for a four-motor mecanum base, with optional + * field-centric * driving using the Pinpoint's heading. */ public class MecanumDriveSubsystem extends SubsystemBase { - private final DcMotor frontLeftDrive; - private final DcMotor frontRightDrive; - private final DcMotor backLeftDrive; - private final DcMotor backRightDrive; + private final DcMotor frontLeftDrive; + private final DcMotor frontRightDrive; + private final DcMotor backLeftDrive; + 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"); + 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"); - // 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); + // 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.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - } + frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + } - /** - * Drives the robot given forward/right/rotate joystick components. - * - * @param forward forward power, [-1, 1] - * @param right strafe-right power, [-1, 1] - * @param rotate clockwise rotation power, [-1, 1] - * @param fieldCentric if true, forward/right are relative to the field instead of the robot - * @param headingRadians the robot's current field heading; only used when fieldCentric is true - */ - public void drive(double forward, double right, double rotate, boolean fieldCentric, double headingRadians) { - if (fieldCentric) { - double theta = Math.atan2(forward, right); - double r = Math.hypot(right, forward); + /** + * Drives the robot given forward/right/rotate joystick components. + * + * @param forward forward power, [-1, 1] + * @param right strafe-right power, [-1, 1] + * @param rotate clockwise rotation power, [-1, 1] + * @param fieldCentric if true, forward/right are relative to the field + * instead of the robot + * @param headingRadians the robot's current field heading; only used when + * fieldCentric is true + */ + public void drive(double forward, double right, double rotate, boolean fieldCentric, double headingRadians) { + if (fieldCentric) { + double theta = Math.atan2(forward, right); + double r = Math.hypot(right, forward); - theta = AngleUnit.normalizeRadians(theta - headingRadians); + theta = AngleUnit.normalizeRadians(theta - headingRadians); - forward = r * Math.sin(theta); - right = r * Math.cos(theta); - } + forward = r * Math.sin(theta); + right = r * Math.cos(theta); + } - double frontLeftPower = forward + right + rotate; - double frontRightPower = forward - right - rotate; - double backLeftPower = forward - right + rotate; - double backRightPower = forward + right - rotate; + double frontLeftPower = forward - right + rotate; + double frontRightPower = forward - right - rotate; + double backLeftPower = forward + right + rotate; + double backRightPower = forward + right - rotate; - double maxPower = 1.0; - maxPower = Math.max(maxPower, Math.abs(frontLeftPower)); - maxPower = Math.max(maxPower, Math.abs(frontRightPower)); - maxPower = Math.max(maxPower, Math.abs(backLeftPower)); - maxPower = Math.max(maxPower, Math.abs(backRightPower)); + double maxPower = 1.0; + maxPower = Math.max(maxPower, Math.abs(frontLeftPower)); + maxPower = Math.max(maxPower, Math.abs(frontRightPower)); + maxPower = Math.max(maxPower, Math.abs(backLeftPower)); + maxPower = Math.max(maxPower, Math.abs(backRightPower)); - frontLeftDrive.setPower(frontLeftPower / maxPower); - frontRightDrive.setPower(frontRightPower / maxPower); - backLeftDrive.setPower(backLeftPower / maxPower); - backRightDrive.setPower(backRightPower / maxPower); - } + frontLeftDrive.setPower(frontLeftPower / maxPower); + frontRightDrive.setPower(frontRightPower / maxPower); + backLeftDrive.setPower(backLeftPower / maxPower); + backRightDrive.setPower(backRightPower / maxPower); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java index 26f8ff5..6164537 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OdometrySubsystem.java @@ -13,38 +13,41 @@ * reports the robot's field pose. */ public class OdometrySubsystem extends SubsystemBase { - private final GoBildaPinpointDriver pinpoint; - - public OdometrySubsystem(HardwareMap hardwareMap) { - pinpoint = hardwareMap.get(GoBildaPinpointDriver.class, "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.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); - pinpoint.setEncoderDirections( - GoBildaPinpointDriver.EncoderDirection.FORWARD, - GoBildaPinpointDriver.EncoderDirection.FORWARD); - - // Robot must be stationary during this call. - pinpoint.resetPosAndIMU(); - } - - @Override - public void periodic() { - pinpoint.update(); - } - - public Pose2D getPose() { - return pinpoint.getPosition(); - } - - /** Zeroes the reported heading in place, keeping the current position. */ - public void resetHeading() { - Pose2D current = pinpoint.getPosition(); - pinpoint.setPosition(new Pose2D( - DistanceUnit.MM, current.getX(DistanceUnit.MM), current.getY(DistanceUnit.MM), - AngleUnit.DEGREES, 0)); - } + private final GoBildaPinpointDriver pinpoint; + + public OdometrySubsystem(HardwareMap hardwareMap) { + pinpoint = hardwareMap.get(GoBildaPinpointDriver.class, "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.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); + pinpoint.setEncoderDirections( + GoBildaPinpointDriver.EncoderDirection.FORWARD, + GoBildaPinpointDriver.EncoderDirection.FORWARD); + + // Robot must be stationary during this call. + pinpoint.resetPosAndIMU(); + } + + @Override + public void periodic() { + pinpoint.update(); + } + + public Pose2D getPose() { + return pinpoint.getPosition(); + } + + /** Zeroes the reported heading in place, keeping the current position. */ + public void resetHeading() { + Pose2D current = pinpoint.getPosition(); + pinpoint.setPosition(new Pose2D( + DistanceUnit.MM, current.getX(DistanceUnit.MM), current.getY(DistanceUnit.MM), + AngleUnit.DEGREES, 0)); + } } From eb9a7ff042c1e4edd673d5e1b7759dc661bbc107 Mon Sep 17 00:00:00 2001 From: bobel-lebob Date: Mon, 28 Sep 2026 14:00:30 +0800 Subject: [PATCH 05/14] refactor(motor): create new motor class, and use it in drive subsystem --- .../ftc/teamcode/hardware/GobildaMotor.java | 97 +++++++++++++++++++ .../subsystems/MecanumDriveSubsystem.java | 58 +++++------ 2 files changed, 127 insertions(+), 28 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/GobildaMotor.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/GobildaMotor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/GobildaMotor.java new file mode 100644 index 0000000..51b504a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/GobildaMotor.java @@ -0,0 +1,97 @@ +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; + +/** + * Thin wrapper around a {@link DcMotorEx}, shared by every subsystem that + * drives a motor (drivetrain, intake, shooter, indexer, ...). + * + *

+ * Configuration methods return {@code this} so a motor can be set up in one + * expression: + * + * 

{@code
+ * GobildaMotor shooter = new GobildaMotor(hardwareMap, "shooter")
+ *     .setReversed(true)
+ *     .setRunMode(DcMotor.RunMode.RUN_USING_ENCODER)
+ *     .setBrakeOnZeroPower(false);
+ * }
+ */ +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 + */ + public GobildaMotor(HardwareMap hardwareMap, String name, boolean reversed, DcMotor.RunMode mode, boolean brake) { + this.name = name; + this.motor = hardwareMap.get(DcMotorEx.class, name); + this.setDirection(reversed ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD); + this.setMode(mode); + this.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_EPSILON) { + return; + } + lastPower = power; + motor.setPower(power); + } + + /** 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(); + } + + public String getName() { + return name; + } + + /** Escape hatch for anything not wrapped here. */ + public DcMotorEx getRaw() { + return motor; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java index facab1f..edd9d74 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/MecanumDriveSubsystem.java @@ -5,35 +5,29 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.hardware.GobildaMotor; /** * Drivetrain subsystem for a four-motor mecanum base, with optional - * field-centric - * driving using the Pinpoint's heading. + * field-centric driving using the Pinpoint's heading. */ public class MecanumDriveSubsystem extends SubsystemBase { - private final DcMotor frontLeftDrive; - private final DcMotor frontRightDrive; - private final DcMotor backLeftDrive; - private final DcMotor backRightDrive; + private final GobildaMotor frontLeft; + private final GobildaMotor frontRight; + private final GobildaMotor backLeft; + private final GobildaMotor backRight; 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"); - // 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); + frontLeft = createDriveMotor(hardwareMap, "DriveFL", true); + frontRight = createDriveMotor(hardwareMap, "DriveFR", false); + backLeft = createDriveMotor(hardwareMap, "DriveBL", true); + backRight = createDriveMotor(hardwareMap, "DriveBR", false); + } - frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + private static GobildaMotor createDriveMotor(HardwareMap hardwareMap, String name, boolean reversed) { + return new GobildaMotor(hardwareMap, name, reversed, DcMotor.RunMode.RUN_USING_ENCODER, false); } /** @@ -63,15 +57,23 @@ public void drive(double forward, double right, double rotate, boolean fieldCent double backLeftPower = forward + right + rotate; double backRightPower = forward + right - rotate; - double maxPower = 1.0; - maxPower = Math.max(maxPower, Math.abs(frontLeftPower)); - maxPower = Math.max(maxPower, Math.abs(frontRightPower)); - maxPower = Math.max(maxPower, Math.abs(backLeftPower)); - maxPower = Math.max(maxPower, Math.abs(backRightPower)); + // Scale all wheels down together so no power exceeds 1 and the direction + // of travel is preserved. + double maxPower = Math.max(1.0, + Math.max(Math.max(Math.abs(frontLeftPower), Math.abs(frontRightPower)), + Math.max(Math.abs(backLeftPower), Math.abs(backRightPower)))); + + frontLeft.setPower(frontLeftPower / maxPower); + frontRight.setPower(frontRightPower / maxPower); + backLeft.setPower(backLeftPower / maxPower); + backRight.setPower(backRightPower / maxPower); + } - frontLeftDrive.setPower(frontLeftPower / maxPower); - frontRightDrive.setPower(frontRightPower / maxPower); - backLeftDrive.setPower(backLeftPower / maxPower); - backRightDrive.setPower(backRightPower / maxPower); + /** Cuts power to all four wheels. */ + public void stop() { + frontLeft.stop(); + frontRight.stop(); + backLeft.stop(); + backRight.stop(); } } From ab8f2cd8e308d9efbc13869e7f16217d1206956a Mon Sep 17 00:00:00 2001 From: bobel-lebob Date: Mon, 28 Sep 2026 15:59:22 +0800 Subject: [PATCH 06/14] fix(motor): update variable names --- .../ftc/teamcode/hardware/GobildaMotor.java | 23 ++++++++----------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/GobildaMotor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/GobildaMotor.java index 51b504a..fd6b3b1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/GobildaMotor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/GobildaMotor.java @@ -9,15 +9,9 @@ * Thin wrapper around a {@link DcMotorEx}, shared by every subsystem that * drives a motor (drivetrain, intake, shooter, indexer, ...). * - *

- * Configuration methods return {@code this} so a motor can be set up in one - * expression: - * * 

{@code
- * GobildaMotor shooter = new GobildaMotor(hardwareMap, "shooter")
- *     .setReversed(true)
- *     .setRunMode(DcMotor.RunMode.RUN_USING_ENCODER)
- *     .setBrakeOnZeroPower(false);
+ * GobildaMotor shooter = new GobildaMotor(
+ *     hardwareMap, "shooter", true, DcMotor.RunMode.RUN_USING_ENCODER, false);
  * }
*/ public class GobildaMotor { @@ -31,13 +25,16 @@ public class GobildaMotor { /** * @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); - this.setDirection(reversed ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD); - this.setMode(mode); - this.setZeroPowerBehavior(brake ? DcMotor.ZeroPowerBehavior.BRAKE : DcMotor.ZeroPowerBehavior.FLOAT); + 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. */ @@ -48,12 +45,12 @@ public GobildaMotor resetEncoder() { 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_EPSILON) { + if (!Double.isNaN(lastPower) && Math.abs(power - lastPower) < POWER_MIN) { return; } lastPower = power; From 2ff95463dde6b05b87af650b7b27b8f0f56d7571 Mon Sep 17 00:00:00 2001 From: bobel-lebob Date: Mon, 28 Sep 2026 16:00:27 +0800 Subject: [PATCH 07/14] feat(intake): implement intake state system and use it to control motors --- .../teamcode/subsystems/IntakeSubsystem.java | 51 +++++++++++++++++++ 1 file changed, 51 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IntakeSubsystem.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IntakeSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IntakeSubsystem.java new file mode 100644 index 0000000..948c25f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IntakeSubsystem.java @@ -0,0 +1,51 @@ +package org.firstinspires.ftc.teamcode.subsystems; + +import com.arcrobotics.ftclib.command.SubsystemBase; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.hardware.GobildaMotor; + +/** + * Subsystem to control intake and indexer + */ +public class IntakeSubsystem extends SubsystemBase { + private final GobildaMotor intakeMotor; + + public enum IntakeState { + STOP, INTAKE, EJECT + } + + private IntakeState intakeState; + private IntakeState previousIntakeState = IntakeState.STOP; + + public IntakeSubsystem(HardwareMap hardwareMap) { + intakeMotor = new GobildaMotor(hardwareMap, "Intake", true, DcMotor.RunMode.RUN_USING_ENCODER, false); + } + + private void doIntakeState() { + if (previousIntakeState == intakeState) { + return; + } + + switch (intakeState) { + case STOP: + intakeMotor.stop(); + break; + case INTAKE: + intakeMotor.setPower(1.0); + break; + case EJECT: + intakeMotor.setPower(-1.0); + break; + } + } + + public void setIntakeState(IntakeState newIntakeState) { + previousIntakeState = intakeState; + intakeState = newIntakeState; + doIntakeState(); + } + +} From 737578a500c9f2bd3306f5356def3b649e7a06cb Mon Sep 17 00:00:00 2001 From: bobel-lebob Date: Mon, 28 Sep 2026 16:01:03 +0800 Subject: [PATCH 08/14] feat(shooter): implement state system for shooter and use it to control motors --- .../teamcode/subsystems/ShooterSubsystem.java | 98 +++++++++++++++++++ 1 file changed, 98 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java new file mode 100644 index 0000000..940ba77 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java @@ -0,0 +1,98 @@ +package org.firstinspires.ftc.teamcode.subsystems; + +import com.arcrobotics.ftclib.command.SubsystemBase; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.hardware.GobildaMotor; + +/** + * Controls the two-wheel shooter and the indexer that feeds balls into it. + * + *
    + *
  • {@link #init()} spins the shooter until {@link #stop()} or + * {@link #eject()}.
    • + *
  • {@link #shoot()} runs the indexer once the shooter is at speed + * (spinning it up first if needed).
    • + *
  • {@link #eject()} runs shooter and indexer backward.
    • + *
  • {@link #stop()} stops everything.
    • + *
+ * + * The "wait until at speed" step is non-blocking: it is handled in + * {@link #periodic()}, which the CommandScheduler calls every loop, so it never + * stalls the OpMode loop. + */ +public class ShooterSubsystem extends SubsystemBase { + // ---- Tuning ---------------------------------------------------------- + private static final double SHOOTER_VELOCITY = 4000; + private static final double INDEXER_POWER = 1.0; + private static final double EJECT_INDEXER_POWER = 0.6; + + // ---- Hardware -------------------------------------------------------- + private final GobildaMotor indexerMotor; + private final GobildaMotor shooterFrontMotor; + private final GobildaMotor shooterBackMotor; + + public enum ShooterState { + STOP, IDLE, SHOOT, EJECT + } + + private ShooterState shooterState; + private ShooterState previousShooterState = ShooterState.STOP; + + public ShooterSubsystem(HardwareMap hardwareMap) { + indexerMotor = new GobildaMotor(hardwareMap, "Indexer", false, DcMotor.RunMode.RUN_USING_ENCODER, true); + shooterFrontMotor = new GobildaMotor(hardwareMap, "ShooterF", false, DcMotor.RunMode.RUN_USING_ENCODER, false); + shooterBackMotor = new GobildaMotor(hardwareMap, "ShooterB", false, DcMotor.RunMode.RUN_USING_ENCODER, false); + + setShooterState(ShooterState.IDLE); + } + + private void doShooterState() { + if (previousShooterState == shooterState) { + return; + } + + switch (shooterState) { + case STOP: + indexerMotor.stop(); + setShooterPower(0.0); + break; + case IDLE: + indexerMotor.stop(); + setShooterPower(1.0); + break; + case SHOOT: + indexerMotor.setPower(1.0); + setShooterPower(1.0); + break; + case EJECT: + indexerMotor.setPower(-1.0); + setShooterPower(-1.0); + break; + } + } + + public void setShooterState(ShooterState newShooterState) { + previousShooterState = shooterState; + shooterState = newShooterState; + doShooterState(); + } + + // + // @Override + // public void periodic() { + // doShooterState(); + // } + // + public boolean isAtSpeed() { + double minSpeed = SHOOTER_VELOCITY; + return Math.abs(shooterFrontMotor.getVelocity()) >= minSpeed + && Math.abs(shooterBackMotor.getVelocity()) >= minSpeed; + } + + private void setShooterPower(double ticksPerSecond) { + shooterFrontMotor.setPower(ticksPerSecond); + shooterBackMotor.setPower(ticksPerSecond); + } +} From 3fe6a3d724114dc82475f5cc6b173edbc0f51f8f Mon Sep 17 00:00:00 2001 From: bobel-lebob Date: Mon, 28 Sep 2026 16:01:43 +0800 Subject: [PATCH 09/14] feat(control): bind console to control intake and shooter by setting their states --- .../org/firstinspires/ftc/teamcode/Robot.java | 30 +++++++++++++++++-- 1 file changed, 28 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index 9bd44dc..39e695a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -6,15 +6,23 @@ 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 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, @@ -26,15 +34,17 @@ public Robot( CommandScheduler.getInstance().reset(); // Construct subsystems. + intake = new IntakeSubsystem(hardwareMap); drive = new MecanumDriveSubsystem(hardwareMap); odometry = new OdometrySubsystem(hardwareMap); - // intake = new IntakeSubsystem(hardwareMap); - // outtake = new OuttakeSubsystem(hardwareMap); + shooter = new ShooterSubsystem(hardwareMap); + } /** Called once when the OpMode enters INIT. */ public void init() { odometry.init(); + shooter.setShooterState(ShooterState.IDLE); } /** Called repeatedly while the OpMode is running. */ @@ -45,6 +55,22 @@ public void periodic() { odometry.resetHeading(); } + if (driver.right_trigger > triggerLimit) { + shooter.setShooterState(ShooterState.SHOOT); + } else if (driver.b) { + shooter.setShooterState(ShooterState.EJECT); + } else { + shooter.setShooterState(ShooterState.IDLE); + } + + if (driver.b) { + intake.setIntakeState(IntakeState.EJECT); + } else if (driver.left_trigger > triggerLimit) { + 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)); From dfd45b26cc43b32f453920b4040d172a2c036b6d Mon Sep 17 00:00:00 2001 From: bobel-lebob Date: Wed, 30 Sep 2026 12:16:05 +0800 Subject: [PATCH 10/14] feat(control): bind south button to stop shooter motor --- .../src/main/java/org/firstinspires/ftc/teamcode/Robot.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index 39e695a..6d7d61c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -59,8 +59,10 @@ public void periodic() { 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); + shooter.setshooterstate(ShooterState.IDLE); } if (driver.b) { From 054f510d29cda7f4dc59180669f088cdfb39fffe Mon Sep 17 00:00:00 2001 From: Andre Nijman Date: Wed, 30 Sep 2026 12:31:53 +0800 Subject: [PATCH 11/14] fix(control): call setShooterState with the right case The stop binding called setshooterstate, which does not exist, so the build failed. --- .../src/main/java/org/firstinspires/ftc/teamcode/Robot.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index 6d7d61c..1f6be6c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -60,9 +60,9 @@ public void periodic() { } else if (driver.b) { shooter.setShooterState(ShooterState.EJECT); } else if (driver.x) { - shooter.setshooterstate(ShooterState.STOP); + shooter.setShooterState(ShooterState.STOP); } else { - shooter.setshooterstate(ShooterState.IDLE); + shooter.setShooterState(ShooterState.IDLE); } if (driver.b) { From 3b811542d9bdd71084f8003dfe2770c77e3ca3ad Mon Sep 17 00:00:00 2001 From: Andre Nijman Date: Wed, 30 Sep 2026 12:32:00 +0800 Subject: [PATCH 12/14] fix(control): run the intake while shooting Right trigger ran the indexer but stopped the intake, so balls waiting where the intake meets the indexer never got in. Tested on the prototype on 30 Sep: balls now pass the handoff. --- .../src/main/java/org/firstinspires/ftc/teamcode/Robot.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index 1f6be6c..a42e81f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -65,9 +65,11 @@ public void periodic() { 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. if (driver.b) { intake.setIntakeState(IntakeState.EJECT); - } else if (driver.left_trigger > triggerLimit) { + } else if (driver.left_trigger > triggerLimit || driver.right_trigger > triggerLimit) { intake.setIntakeState(IntakeState.INTAKE); } else { intake.setIntakeState(IntakeState.STOP); From 5140e4ac6f67b5212b34b5793a03a80c3ba36e95 Mon Sep 17 00:00:00 2001 From: Andre Nijman Date: Wed, 30 Sep 2026 12:33:26 +0800 Subject: [PATCH 13/14] feat(shooter): add stall protection for the indexer A jammed indexer drew 8.4 to 11.2 A at 0 ticks/s on the prototype, and held that for as long as the trigger was down. While shooting, the indexer's current and velocity are now sampled every 50 ms; above 7 A and under 150 ticks/s for 0.3 s counts as a jam, and the indexer backs off at -0.6 for 0.2 s before feeding again. After 3 back-offs in one press, the next jam leaves it off until the trigger is released and pressed again. The intake pauses while the indexer backs off, so it doesn't pack the jam tighter. The decision logic is IndexerStallGuard, pure and unit tested against the measured jam. Telemetry shows the indexer's feed state, current and speed, and each jam is logged to logcat under the tag BioBuzz. --- .../org/firstinspires/ftc/teamcode/Robot.java | 10 +- .../ftc/teamcode/hardware/GobildaMotor.java | 7 ++ .../subsystems/IndexerStallGuard.java | 111 ++++++++++++++++++ .../teamcode/subsystems/ShooterSubsystem.java | 68 +++++++++-- .../subsystems/IndexerStallGuardTest.java | 80 +++++++++++++ 5 files changed, 267 insertions(+), 9 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IndexerStallGuard.java create mode 100644 TeamCode/src/test/java/org/firstinspires/ftc/teamcode/subsystems/IndexerStallGuardTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index a42e81f..ac57cba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -66,10 +66,12 @@ public void periodic() { } // 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. + // 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 || driver.right_trigger > triggerLimit) { + } else if (driver.left_trigger > triggerLimit || feeding) { intake.setIntakeState(IntakeState.INTAKE); } else { intake.setIntakeState(IntakeState.STOP); @@ -80,6 +82,10 @@ public void periodic() { !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(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/GobildaMotor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/GobildaMotor.java index fd6b3b1..91caf23 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/GobildaMotor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/GobildaMotor.java @@ -5,6 +5,8 @@ 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, ...). @@ -83,6 +85,11 @@ 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; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IndexerStallGuard.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IndexerStallGuard.java new file mode 100644 index 0000000..b67a75b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IndexerStallGuard.java @@ -0,0 +1,111 @@ +package org.firstinspires.ftc.teamcode.subsystems; + +/** + * Stall protection for the indexer while it feeds the shooter. Pure logic with + * no hardware, so it can be unit tested. + * + *

+ * Measured on the prototype on 30 Sep: jammed on two balls, the indexer drew + * 8.4 to 11.2 A at 0 ticks/s, while heavily loaded but still moving it turned + * at 460 ticks/s or more. So high current at near-zero speed for + * {@link #STALL_TIME_S} means jammed. Each jam backs the indexer off for + * {@link #UNJAM_TIME_S} and then feeding resumes. After + * {@link #MAX_UNJAM_ATTEMPTS} back-offs in one feed, the next jam leaves the + * indexer off until the next feed starts, so a hard jam does not cook the + * motor. + */ +public class IndexerStallGuard { + public static final double STALL_CURRENT_AMPS = 7.0; + /** Ticks per second. */ + public static final double STALL_VELOCITY = 150; + public static final double STALL_TIME_S = 0.3; + public static final double UNJAM_TIME_S = 0.2; + public static final int MAX_UNJAM_ATTEMPTS = 3; + + public enum Phase { + FEEDING, UNJAMMING, JAMMED + } + + private final double feedPower; + private final double unjamPower; + + private Phase phase = Phase.FEEDING; + private int unjamAttempts; + private double phaseStartS; + private double stalledSinceS = Double.NaN; + + /** + * @param feedPower indexer power while feeding + * @param unjamPower indexer power while backing off a jam (negative) + */ + public IndexerStallGuard(double feedPower, double unjamPower) { + this.feedPower = feedPower; + this.unjamPower = unjamPower; + } + + /** Starts a new feed, clearing any jam and the attempt count. */ + public void start(double nowS) { + unjamAttempts = 0; + enter(Phase.FEEDING, nowS); + } + + /** + * @param nowS time in seconds + * @param currentAmps indexer current draw + * @param velocity indexer velocity in ticks/second + * @return the power to apply to the indexer + */ + public double update(double nowS, double currentAmps, double velocity) { + switch (phase) { + case FEEDING: + boolean stalled = currentAmps > STALL_CURRENT_AMPS && Math.abs(velocity) < STALL_VELOCITY; + if (!stalled) { + stalledSinceS = Double.NaN; + } else if (Double.isNaN(stalledSinceS)) { + stalledSinceS = nowS; + } else if (nowS - stalledSinceS >= STALL_TIME_S) { + if (unjamAttempts < MAX_UNJAM_ATTEMPTS) { + unjamAttempts++; + enter(Phase.UNJAMMING, nowS); + } else { + enter(Phase.JAMMED, nowS); + } + } + break; + case UNJAMMING: + if (nowS - phaseStartS >= UNJAM_TIME_S) { + enter(Phase.FEEDING, nowS); + } + break; + case JAMMED: + break; + } + return getPower(); + } + + private void enter(Phase newPhase, double nowS) { + phase = newPhase; + phaseStartS = nowS; + stalledSinceS = Double.NaN; + } + + /** The power the indexer should be at in the current phase. */ + public double getPower() { + switch (phase) { + case FEEDING: + return feedPower; + case UNJAMMING: + return unjamPower; + default: + return 0.0; + } + } + + public Phase getPhase() { + return phase; + } + + public int getUnjamAttempts() { + return unjamAttempts; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java index 940ba77..f3232ab 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java @@ -3,8 +3,11 @@ import com.arcrobotics.ftclib.command.SubsystemBase; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.RobotLog; import org.firstinspires.ftc.teamcode.hardware.GobildaMotor; +import org.firstinspires.ftc.teamcode.subsystems.IndexerStallGuard.Phase; /** * Controls the two-wheel shooter and the indexer that feeds balls into it. @@ -27,6 +30,9 @@ public class ShooterSubsystem extends SubsystemBase { private static final double SHOOTER_VELOCITY = 4000; private static final double INDEXER_POWER = 1.0; private static final double EJECT_INDEXER_POWER = 0.6; + private static final double UNJAM_INDEXER_POWER = 0.6; + /** Current and velocity are separate hub reads, so sample them this often. */ + private static final double STALL_SAMPLE_PERIOD_S = 0.05; // ---- Hardware -------------------------------------------------------- private final GobildaMotor indexerMotor; @@ -40,6 +46,13 @@ public enum ShooterState { private ShooterState shooterState; private ShooterState previousShooterState = ShooterState.STOP; + // ---- Stall protection ------------------------------------------------ + private final IndexerStallGuard indexerStallGuard = new IndexerStallGuard(INDEXER_POWER, -UNJAM_INDEXER_POWER); + private final ElapsedTime clock = new ElapsedTime(); + private double lastStallSampleS = Double.NEGATIVE_INFINITY; + private double indexerCurrentAmps; + private double indexerVelocity; + public ShooterSubsystem(HardwareMap hardwareMap) { indexerMotor = new GobildaMotor(hardwareMap, "Indexer", false, DcMotor.RunMode.RUN_USING_ENCODER, true); shooterFrontMotor = new GobildaMotor(hardwareMap, "ShooterF", false, DcMotor.RunMode.RUN_USING_ENCODER, false); @@ -63,7 +76,8 @@ private void doShooterState() { setShooterPower(1.0); break; case SHOOT: - indexerMotor.setPower(1.0); + indexerStallGuard.start(clock.seconds()); + indexerMotor.setPower(indexerStallGuard.getPower()); setShooterPower(1.0); break; case EJECT: @@ -79,12 +93,52 @@ public void setShooterState(ShooterState newShooterState) { doShooterState(); } - // - // @Override - // public void periodic() { - // doShooterState(); - // } - // + /** + * While shooting, watches the indexer for a jam and backs it off. See + * {@link IndexerStallGuard}. + */ + @Override + public void periodic() { + if (shooterState != ShooterState.SHOOT) { + return; + } + double now = clock.seconds(); + if (now - lastStallSampleS < STALL_SAMPLE_PERIOD_S) { + return; + } + lastStallSampleS = now; + + indexerCurrentAmps = indexerMotor.getCurrentAmps(); + indexerVelocity = indexerMotor.getVelocity(); + Phase before = indexerStallGuard.getPhase(); + indexerMotor.setPower(indexerStallGuard.update(now, indexerCurrentAmps, indexerVelocity)); + if (indexerStallGuard.getPhase() != before) { + RobotLog.dd("BioBuzz", "indexer %s -> %s at %.1f A, %.0f ticks/s, unjam %d/%d", + before, indexerStallGuard.getPhase(), indexerCurrentAmps, indexerVelocity, + indexerStallGuard.getUnjamAttempts(), IndexerStallGuard.MAX_UNJAM_ATTEMPTS); + } + } + + /** True while shooting with the indexer feeding, not backing off a jam. */ + public boolean isIndexerFeeding() { + return shooterState == ShooterState.SHOOT && indexerStallGuard.getPhase() == Phase.FEEDING; + } + + /** True once the indexer has given up on a jam; clears when shooting restarts. */ + public boolean isIndexerJammed() { + return shooterState == ShooterState.SHOOT && indexerStallGuard.getPhase() == Phase.JAMMED; + } + + /** Indexer feed state and the last current and velocity reading, for telemetry. */ + public String getIndexerStatus() { + if (shooterState != ShooterState.SHOOT) { + return shooterState.toString(); + } + return String.format("%s %.1f A %.0f ticks/s unjam %d/%d", + indexerStallGuard.getPhase(), indexerCurrentAmps, indexerVelocity, + indexerStallGuard.getUnjamAttempts(), IndexerStallGuard.MAX_UNJAM_ATTEMPTS); + } + public boolean isAtSpeed() { double minSpeed = SHOOTER_VELOCITY; return Math.abs(shooterFrontMotor.getVelocity()) >= minSpeed diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/subsystems/IndexerStallGuardTest.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/subsystems/IndexerStallGuardTest.java new file mode 100644 index 0000000..4f87189 --- /dev/null +++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/subsystems/IndexerStallGuardTest.java @@ -0,0 +1,80 @@ +package org.firstinspires.ftc.teamcode.subsystems; + +import static org.junit.Assert.assertEquals; + +import org.firstinspires.ftc.teamcode.subsystems.IndexerStallGuard.Phase; +import org.junit.Before; +import org.junit.Test; + +/** Currents and speeds are from the prototype's two-ball jam on 30 Sep. */ +public class IndexerStallGuardTest { + private static final double FEED = 1.0; + private static final double UNJAM = -0.6; + private static final double STEP_S = 0.05; + + private IndexerStallGuard guard; + private double now; + + @Before + public void setUp() { + guard = new IndexerStallGuard(FEED, UNJAM); + now = 0; + guard.start(now); + } + + /** Feeds the same reading for a duration, returning the last power. */ + private double run(double seconds, double amps, double velocity) { + double power = guard.getPower(); + for (double end = now + seconds; now < end - 1e-9;) { + now += STEP_S; + power = guard.update(now, amps, velocity); + } + return power; + } + + @Test + public void feedsWhileRunningFreely() { + assertEquals(FEED, run(2.0, 1.7, 2200), 0); + assertEquals(Phase.FEEDING, guard.getPhase()); + } + + @Test + public void heavyLoadThatStillMovesIsNotAStall() { + assertEquals(FEED, run(2.0, 8.7, 460), 0); + assertEquals(Phase.FEEDING, guard.getPhase()); + } + + @Test + public void briefStallIsIgnored() { + run(0.2, 10.5, 0); + assertEquals(FEED, run(1.0, 7.5, 800), 0); + assertEquals(0, guard.getUnjamAttempts()); + } + + @Test + public void sustainedStallBacksOffThenResumes() { + assertEquals(UNJAM, run(0.4, 10.5, 0), 0); + assertEquals(Phase.UNJAMMING, guard.getPhase()); + assertEquals(1, guard.getUnjamAttempts()); + + assertEquals(FEED, run(IndexerStallGuard.UNJAM_TIME_S, 3.0, -300), 0); + assertEquals(Phase.FEEDING, guard.getPhase()); + } + + @Test + public void givesUpAfterMaxAttemptsAndStaysOff() { + for (int i = 0; i < IndexerStallGuard.MAX_UNJAM_ATTEMPTS; i++) { + run(0.4, 10.5, 0); + run(IndexerStallGuard.UNJAM_TIME_S, 3.0, -300); + } + assertEquals(0.0, run(0.4, 10.5, 0), 0); + assertEquals(Phase.JAMMED, guard.getPhase()); + + // Stays off even once the motor goes quiet, until a new feed starts. + assertEquals(0.0, run(1.0, 0.0, 0), 0); + guard.start(now); + assertEquals(Phase.FEEDING, guard.getPhase()); + assertEquals(0, guard.getUnjamAttempts()); + assertEquals(FEED, guard.getPower(), 0); + } +} From 84770aae7e3d88f80551dd481a79031a7e3900be Mon Sep 17 00:00:00 2001 From: Andre Nijman Date: Wed, 30 Sep 2026 12:42:33 +0800 Subject: [PATCH 14/14] feat(shooter): run the shooter at full power while shooting In RUN_USING_ENCODER the hub turns power 1.0 into a velocity target of 85% of the motor's rated speed, about 5100 RPM on the 6000 RPM goBILDA motors. SHOOT now switches the shooter to RUN_WITHOUT_ENCODER so power 1.0 is full battery voltage; IDLE and EJECT switch back to closed loop as before. GobildaMotor.setMode clears the cached power on a mode change, since IDLE and SHOOT both use power 1.0 and the cache would otherwise skip the write. --- .../ftc/teamcode/hardware/GobildaMotor.java | 12 ++++++++++++ .../ftc/teamcode/subsystems/ShooterSubsystem.java | 10 ++++++++++ 2 files changed, 22 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/GobildaMotor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/GobildaMotor.java index 91caf23..bc093e8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/GobildaMotor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/GobildaMotor.java @@ -59,6 +59,18 @@ public void setPower(double 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 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java index f3232ab..9e4a693 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/ShooterSubsystem.java @@ -73,15 +73,20 @@ private void doShooterState() { break; case IDLE: indexerMotor.stop(); + setShooterMode(DcMotor.RunMode.RUN_USING_ENCODER); setShooterPower(1.0); break; case SHOOT: indexerStallGuard.start(clock.seconds()); indexerMotor.setPower(indexerStallGuard.getPower()); + // RUN_USING_ENCODER holds power 1.0 to 85% of the motor's rated speed + // (about 5100 RPM on the 6000 RPM motors); open loop gives full voltage. + setShooterMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); setShooterPower(1.0); break; case EJECT: indexerMotor.setPower(-1.0); + setShooterMode(DcMotor.RunMode.RUN_USING_ENCODER); setShooterPower(-1.0); break; } @@ -145,6 +150,11 @@ public boolean isAtSpeed() { && Math.abs(shooterBackMotor.getVelocity()) >= minSpeed; } + private void setShooterMode(DcMotor.RunMode mode) { + shooterFrontMotor.setMode(mode); + shooterBackMotor.setMode(mode); + } + private void setShooterPower(double ticksPerSecond) { shooterFrontMotor.setPower(ticksPerSecond); shooterBackMotor.setPower(ticksPerSecond);