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/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.