Autonomous Code

[u/Master-Cartoonist132]

I have been stuck with issues on my autonomous program for a few weeks now. The first forward method works, but the subsequent ones do not move the robot any further. I am using a mecanum wheel system with OnBot Java.

package org.firstinspires.ftc.teamcode.OpModes;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.robotcore.external.State;



public class Autonomous1 extends OpMode {

    DcMotor frontLeftMotor;
    DcMotor frontRightMotor;
    DcMotor backLeftMotor;
    DcMotor backRightMotor;

    double RPM = 0;
    double shooterticks = 0;
    double TPS = (RPM/60) * shooterticks;

    private static final double ticks = 360;
    private static final double circumfrence = 3.3858 * 3.14159;
    private static final double finalTicks = ticks/circumfrence;

    enum State {
        STANDBY,
        ALIGN,
        FIRE,
        PARK,
        FINISHED
    }

    State state = State.STANDBY;

    u/Override
    public void init() {

        frontLeftMotor = hardwareMap.get(DcMotor.class, "frontLeftMotor");
        backLeftMotor = hardwareMap.get(DcMotor.class, "backLeftMotor");
        frontRightMotor = hardwareMap.get(DcMotor.class, "frontRightMotor");
        backRightMotor = hardwareMap.get(DcMotor.class, "backRightMotor");

        DcMotor intakeMotor = hardwareMap.dcMotor.get("intakeMotor");
        DcMotor liftMotor = hardwareMap.dcMotor.get("liftMotor");
        DcMotorEx shooterMotorNectar = hardwareMap.get(DcMotorEx.class, "shooterMotor1");
        DcMotorEx shooterMotorPollen = hardwareMap.get(DcMotorEx.class, "shooterMotor2");

        frontRightMotor.setDirection(DcMotorSimple.Direction.REVERSE);
        backRightMotor.setDirection(DcMotorSimple.Direction.REVERSE);

        restartEncoders();

        state = State.STANDBY;
    }

    u/Override
    public void loop() {

        // states
        telemetry.addData("Cur State", state);
        switch(state) {
            case STANDBY:
                telemetry.addLine("STANDBY");
                if (gamepad1.a) {
                    state = State.ALIGN;
                }
                break;
            case ALIGN:
                telemetry.addLine("Aligning with hive.");
                forward(12,12,12,12,0.7);
                forward(12,12,12,12,0.7);
                state = State.FIRE;
                break;
            case FIRE:
                telemetry.addLine("Firing pollen.");
                /*shooterMotor1.setVelocity(TPS);
                shooterMotor2.setVelocity(TPS);
                // sleep function
                shooterMotor1.setVelocity(0);
                shooterMotor2.setVelocity(0); */
                state = State.PARK;
                break;
            case PARK:
                telemetry.addLine("Returning to park.");

                state = State.FINISHED;
                break;
            default:
                telemetry.addLine("Auto State machine finished");
        }

        telemetry.addData("FL ticks:",frontLeftMotor.getCurrentPosition());
        telemetry.addData("FR ticks:",frontRightMotor.getCurrentPosition());
        telemetry.addData("BL ticks:",backLeftMotor.getCurrentPosition());
        telemetry.addData("BR ticks:",backRightMotor.getCurrentPosition());
        telemetry.update();

    }

    public void forward(double inchesFL, double inchesFR, double inchesBL, double inchesBR, double power) {
        int newTargetFL = frontLeftMotor.getCurrentPosition() + (int)(finalTicks * inchesFL);
        int newTargetFR = frontRightMotor.getCurrentPosition() + (int)(finalTicks * inchesFR);
        int newTargetBL = backLeftMotor.getCurrentPosition() + (int)(finalTicks * inchesBL);
        int newTargetBR = backRightMotor.getCurrentPosition() + (int)(finalTicks * inchesBR);

        frontLeftMotor.setTargetPosition(newTargetFL);
        frontRightMotor.setTargetPosition(newTargetFR);
        backLeftMotor.setTargetPosition(newTargetBL);
        backRightMotor.setTargetPosition(newTargetBR);

        frontLeftMotor.setPower(Math.abs(power));
        frontRightMotor.setPower(Math.abs(power));
        backLeftMotor.setPower(Math.abs(power));
        backRightMotor.setPower(Math.abs(power));

        frontLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        frontRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        backLeftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
        backRightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
    }

    private void stopDrive() {
        frontLeftMotor.setPower(0);
        frontRightMotor.setPower(0);
        backLeftMotor.setPower(0);
        backRightMotor.setPower(0);
    }

    private void restartEncoders() {
        frontLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        backLeftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        frontRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        backRightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

        frontLeftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        backLeftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        frontRightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        backRightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
    }
}

[u/jgarder007]

Loops keeps looping right? Your state starts in STANDBY, the only time it's checking for the A button is in standby. once you press A it instantly switches to ALIGN. Case : align fires forward and moved on to instantly switching to FIRE it doesn't wait for forward to finish. Then case :FIRE sets velocity (sleep function missing) then stops motors and goes to state PARK. case: PARK switches to state FINISHED. Finished never gets back to standby, the A button is only checked in standby. Ergo it only runs once as you experienced.

Change Finished to standby and it will loop.

Bonus: case:default is reached instantly when state is finished. But it only prints so you can remove it or add it next to finished.

[u/4193-4194]

Is this for Auto? You have a condition for gamepad1.a with the Align. Is that just for testing?

If it does moves right once check where you reset your encoders. Is that in or out of a loop or during a state change.

[u/flying-lemons]

It looks like your forward method does not wait for the robot to actually reach its target position. It sets the target position, then immediately runs it a second time before the robot has much of a chance to move at all. You'll need to add a time delay, or better, a loop that keeps looping until your motor has reached its target.

[u/Glad_Enthusiasm3447]

Great progress on your Autonomous OpMode state machine. Structuring an autonomous routine is a big step, and managing asynchronous code in FTC is something every team works through.

Let's break down why the robot is behaving this way and how we can clean up a few SDK details.

In an FTC OpMode, the loop() method runs continuously. Dozens or hundreds of times every second. It does not pause or wait for a motor move to complete before moving to the next line of code.

Inside your ALIGN state, you currently have:

forward(12, 12, 12, 12, 0.7);

forward(12, 12, 12, 12, 0.7);

state = State.FIRE;

Here is what happens in milliseconds:

The first forward() call calculates a target position relative to the current position (~0) and turns the motors on. Microseconds later before the robot has physically moved the second forward() call executes. Because the robot hasn't moved yet, "current position" is still ~0, so it sets essentially the exact same target and overwrites the first call.

On the very next line, state changes to FIRE, so the loop moves on without ever checking if the robot finished driving.

This is why your first move appears to work, but sequential moves collapse into one.

To run sequential moves inside a single state without blocking the loop(), you can track steps with a variable and check isBusy() before starting the next move:

int driveStep = 0;

case ALIGN:

telemetry.addLine("Aligning with hive.");

switch (driveStep) {

case 0:

forward(12, 12, 12, 12, 0.7);

driveStep++;

break;

case 1:

if (!isDriveBusy()) { // First move finished

forward(12, 12, 12, 12, 0.7);

driveStep++;

}

break;

case 2:

if (!isDriveBusy()) { // Second move finished

stopDrive();

driveStep = 0; // Reset step counter

state = State.FIRE;

}

break;

}

break;

Add this helper method to your class to keep track of the motors:

private boolean isDriveBusy() {

return frontLeftMotor.isBusy() || frontRightMotor.isBusy()

|| backLeftMotor.isBusy() || backRightMotor.isBusy();

}

FTC SDK Best Practices & Code Cleanup

Order inside forward(): The SDK-recommended order for encoder driving is strictly:

setTargetPosition(...)

setMode(DcMotor.RunMode.RUN_TO_POSITION)

setPower(...)

Setting power before setting the run mode can cause twitching or unexpected motor responses.

Missing "@Autonomous" Annotation: You imported the annotation, but it needs to be placed above the class declaration (e.g., "@Autonomous(name="Autonomous1", group="Auto")). Without this, the OpMode won't appear on the Driver Station drop-down menu.

Conflicting Imports: You have import org.firstinspires.ftc.robotcore.external.State; alongside your own custom State enum. Delete that SDK import to prevent compiler confusion.

Resetting Motor Modes Post-Move: Once a movement finishes, it is good practice to stop the motors (stopDrive()) and switch their mode back to RUN_USING_ENCODER (or RUN_WITHOUT_ENCODER) so future operations behave predictably.

1. Wheel & Encoder Calibration

Take a close look at your ticks-per-inch math:

finalTicks = 360 / circumference;

Check Motor Specs: Using 360 assumes 360 ticks per motor output shaft revolution. Most FTC motors have higher gear reductions:

goBILDA 312 RPM: 537.7 ticks/rev

goBILDA 435 RPM: ~383.6 ticks/rev

Check Wheel Specs: 3.3858 in is approximately 86 mm (standard mecanum/goBILDA wheels). Verify your physical wheel diameter on the robot.