Actions Run Blocking

[Dynamic295]

RR FTC Version

1.0

Observed Behavior

Robot will drive and raise arm but will not run other commands in Sequential Action. We don't know if its the sequence or commands

Tuning Files

package org.firstinspires.ftc.teamcode.tuning;

// RR-specific imports
import androidx.annotation.NonNull;

import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.SequentialAction;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.ftc.Actions;

// Non-RR imports
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
//import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.MecanumDrive;

@config
@autonomous(name = "AutoTest2", group = "Autonomous")
public class AutoTest2 extends LinearOpMode {

// rotate arm class

final double ARM_TICKS_PER_DEGREE = 19.79;
final double EXTEND_TICKS_PER_DEGREE = 1.068;
final double ARM_UP = -250 * ARM_TICKS_PER_DEGREE;
final double ARM_DN = 230 * ARM_TICKS_PER_DEGREE;
final double EXTEND_OUT = -2000 * EXTEND_TICKS_PER_DEGREE;
final double EXTEND_IN = 160 * EXTEND_TICKS_PER_DEGREE;
final double INTAKE_COLLECT = -1.0;
final double INTAKE_DEPOSIT = 1.0;


// Declare hardware components
DcMotor ArmP2, ExtendP1;
CRServo Intake;

// Method to move the arm to a target position (blocking)
public static class RunnableAction implements Action {
    private final Runnable runnable;
    private boolean isFinished = false;

    public RunnableAction(Runnable runnable) {
        this.runnable = runnable;
    }


    public boolean isFinished() {
        // Return true after the action is completed
        return isFinished;
    }


    public boolean run(@NonNull TelemetryPacket packet) {
        // Run the action (this should match the RoadRunner interface exactly)
        runnable.run();
        // Mark the action as finished after running
        isFinished = true;
        return false;
    }


    public void start() {
        // Reset the finished flag when starting
        isFinished = false;
    }


    public void cancel() {
        // Mark as finished when cancelled
        isFinished = true;
    }
}

@Override
public void runOpMode() {
    // Initialize motors and servo
    ArmP2 = hardwareMap.get(DcMotor.class, "ArmP2");
    ExtendP1 = hardwareMap.get(DcMotor.class, "ExtendP1");
    Intake = hardwareMap.get(CRServo.class, "Intake");

    // Set arm and extension motors to use encoders
    ArmP2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    ExtendP1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

    ArmP2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
    ExtendP1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

    Pose2d initialPose = new Pose2d(-35.31, -63.13, Math.toRadians(90.00));
    MecanumDrive drive = new MecanumDrive(hardwareMap, initialPose);

//Arm Extend and Take methods???

    //moveArmToPosition(ARM_UP)
    //moveArmToPosition(ARM_DN)
    //extendArmToPosition(EXTEND_OUT)
    //extendArmToPosition(EXTEND_IN)
    //runIntake(INTAKE_COLLECT)
    //runBlocking(() -> runIntake(INTAKE_DEPOSIT)

    TrajectoryActionBuilder tab1 = drive.actionBuilder(initialPose)
            .splineTo(new Vector2d(-39.53, -42.17), Math.toRadians(157.02))
            .splineTo(new Vector2d(-51.41, -52.33), Math.toRadians(220.33)) //new (-48.41,55.33)?
            .waitSeconds(2);


    //.splineTo(new Vector2d(30, 30), Math.PI / 2)
                /*.lineToYSplineHeading(33, Math.toRadians(0))
                .waitSeconds(2)
                .setTangent(Math.toRadians(90))
                .lineToY(48)
                .setTangent(Math.toRadians(0))
                .lineToX(32)
                .strafeTo(new Vector2d(44.5, 30))
                .turn(Math.toRadians(180))
                .lineToX(47.5)
                */
    //.waitSeconds(3);
    // TrajectoryActionBuilder tab2 = drive.actionBuilder(initialPose)
    //.turn(90)
    //.waitSeconds(2);

    //TrajectoryActionBuilder tab3 = drive.actionBuilder(initialPose)
    //.turn(15);
    // Action trajectoryActionCloseOut = tab1.fresh()
    //.turn(-15)
    //.build();

    waitForStart();


    // Use Actions.runBlocking to run multiple tasks in sequence
    if (opModeIsActive()) {
        // Use Actions.runBlocking to run multiple tasks in sequence
        Actions.runBlocking(
                new SequentialAction(
                        tab1.build(),  // First trajectory

                        // Move arm up
                        new RunnableAction(() -> moveArmToPosition(ARM_UP)),

                        // Extend arm out
                        new RunnableAction(() -> extendArmToPosition(EXTEND_OUT)),

                        tab1.build(),  // First trajectory

                        // Run intake to deposit
                        new RunnableAction(() -> runIntake(INTAKE_DEPOSIT)),

                        // Retract arm
                        new RunnableAction(() -> extendArmToPosition(EXTEND_IN)),

                        // Move arm down
                        new RunnableAction(() -> moveArmToPosition(ARM_DN))

                        // Run intake to collect
                       // new RunnableAction(() -> runIntake(INTAKE_COLLECT)),

                        // Run intake to deposit
                        //new RunnableAction(() -> runIntake(INTAKE_DEPOSIT))
                )
        );
    }
}

// Method to move the arm to a target position (blocking)


public void moveArmToPosition(double targetPosition) {
    if (!ArmP2.isBusy()) {
        ArmP2.setTargetPosition((int) targetPosition);
        ArmP2.setMode(DcMotor.RunMode.RUN_TO_POSITION);

        if (targetPosition > 0) {
            ArmP2.setPower(0.5);  // Moving up
        } else {
            ArmP2.setPower(-0.5); // Moving down
        }

        while (opModeIsActive() && ArmP2.isBusy()) {
            telemetry.addData("Arm Position", ArmP2.getCurrentPosition());
            telemetry.update();
        }

        ArmP2.setPower(0);
        ArmP2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
    }
}

public boolean extendArmToPosition(double targetPosition) {
    if (!ExtendP1.isBusy()) {
        ExtendP1.setTargetPosition((int) targetPosition);
        ExtendP1.setMode(DcMotor.RunMode.RUN_TO_POSITION);

        if (targetPosition > 0) {
            ExtendP1.setPower(0.5);  // Extending
        } else {
            ExtendP1.setPower(-0.5); // Retracting
        }

        while (opModeIsActive() && ExtendP1.isBusy()) {
            telemetry.addData("Extension Position", ExtendP1.getCurrentPosition());
            telemetry.update();
        }

        ExtendP1.setPower(0);
        ExtendP1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
    }
    return true;  // Return true to indicate the action is complete
}

public void runIntake(double power) {
    Intake.setPower(power);
    sleep(2000); // Run intake for 2 seconds
    Intake.setPower(0);
}

}

Robot Logs

No response

[rbrott]

Perhaps ArmP2.isBusy() never becomes false? I doubt there's a problem with SequentialAction itself, and RunnableAction seems to be implemented correctly even though it defeats most of the point of the whole action system.

[Dynamic295]

thank you for the reply. it helped. we rebuilt the code using your example. However, we cannot run more than one action in SequentialAction nor if we make a list after it. The code I pasted in has the original list in SA but remarked out and the list after it as an experiment.

package org.firstinspires.ftc.teamcode.tuning;

// RR-specific imports
import androidx.annotation.NonNull;

import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.SequentialAction;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.ftc.Actions;

// Non-RR imports
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
//import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.MecanumDrive;

@config
@autonomous(name = "AutoTest2", group = "Autonomous")
public class AutoTest2 extends LinearOpMode {

// rotate arm class

final double ARM_TICKS_PER_DEGREE = 19.79;
final double EXTEND_TICKS_PER_DEGREE = 1.068;
final double ARM_UP = 80 * ARM_TICKS_PER_DEGREE;
final double ARM_DN = -80 * ARM_TICKS_PER_DEGREE;
final double EXTEND_OUT = -1000 * EXTEND_TICKS_PER_DEGREE;
final double EXTEND_IN = 1000 * EXTEND_TICKS_PER_DEGREE;
final double INTAKE_COLLECT = -1.0;
final double INTAKE_DEPOSIT = 1.0;




// Method to move the arm to a target position (blocking)
public class Arm {
    private DcMotorEx Arm;

    public Arm(HardwareMap hardwareMap) {
        Arm = hardwareMap.get(DcMotorEx.class, "ArmP2");
        Arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        Arm.setDirection(DcMotorSimple.Direction.FORWARD);
    }

    public class ArmUP implements Action {
        private boolean initialized = false;

        @Override
        public boolean run(@NonNull TelemetryPacket packet) {
            if (!initialized) {
                Arm.setPower(-0.8);
                initialized = true;
            }

            double pos = Arm.getCurrentPosition();
            packet.put("liftPos", pos);
            if (pos < ARM_UP) {
                return true;
            } else {
                Arm.setPower(0);
                return false;
            }
        }
    }
    public Action ArmUP() {
        return new ArmUP();
    }

    public class ArmDN implements Action {
        private boolean initialized = false;

        @Override
        public boolean run(@NonNull TelemetryPacket packet) {
            if (!initialized) {
                Arm.setPower(0.8);
                initialized = true;
            }

            double pos = Arm.getCurrentPosition();
            packet.put("liftPos", pos);
            if (pos > ARM_DN) {
                return true;
            } else {
                Arm.setPower(0);
                return false;
            }
        }
    }
    public Action ArmDN(){
        return new ArmDN();
    }
}
public class Extend {
    private DcMotorEx Extend;

    public Extend(HardwareMap hardwareMap) {
        Extend = hardwareMap.get(DcMotorEx.class, "ExtendP1");
        Extend.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        Extend.setDirection(DcMotorSimple.Direction.FORWARD);
    }

    public class ExtendOUT implements Action {
        private boolean initialized = false;

        @Override
        public boolean run(@NonNull TelemetryPacket packet) {
            if (!initialized) {
                Extend.setPower(-0.8);
                initialized = true;

            }

            double pos = Extend.getCurrentPosition();
            packet.put("ExtendPos", pos);
            if (pos > EXTEND_OUT) {
                return true;
            } else {
                Extend.setPower(0);
                return false;
            }
        }
    }
    public Action ExtendOUT() {
        return new ExtendOUT();
    }

    public class ExtendIN implements Action {
        private boolean initialized = false;

        @Override
        public boolean run(@NonNull TelemetryPacket packet) {
            if (!initialized) {
                Extend.setPower(0.8);
                initialized = true;
            }

            double pos = Extend.getCurrentPosition();
            packet.put("ExtendPos", pos);
            if (pos < EXTEND_IN) {
                return true;
            } else {
                Extend.setPower(0);
                return false;
            }
        }
    }
    public Action ExtendIN() {
        return new ExtendIN();
    }

}
public class Spin {
    private CRServo Spin;

    public Spin(HardwareMap hardwareMap) {
        Spin = hardwareMap.get(CRServo.class, "Intake");
    }

    public class Deposit implements Action {
        @Override
        public boolean run(@NonNull TelemetryPacket packet) {
            Spin.setPower(INTAKE_DEPOSIT);
            return false;
        }
    }
    public Action Deposit() {
        return new Deposit();
    }

    public class Collect implements Action {
        @Override
        public boolean run(@NonNull TelemetryPacket packet) {
            Spin.setPower(INTAKE_COLLECT);
            return false;
        }
    }
    public Action Collect() {
        return new Collect();
    }
}



@Override
public void runOpMode() {


    // Initialize motors and servo
    //ArmP2 = hardwareMap.get(DcMotor.class, "ArmP2");
    //ExtendP1 = hardwareMap.get(DcMotor.class, "ExtendP1");
    //Intake = hardwareMap.get(CRServo.class, "Intake");

    // Set arm and extension motors to use encoders
    // ArmP2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    // ExtendP1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

    //ArmP2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
    // ExtendP1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);

    Pose2d initialPose = new Pose2d(-35.31, -63.13, Math.toRadians(90.00));
    MecanumDrive drive = new MecanumDrive(hardwareMap, initialPose);
    Spin Spin = new Spin(hardwareMap);
    Arm Arm = new Arm(hardwareMap);
    Extend Extend = new Extend(hardwareMap);

//Arm Extend and Take methods???

    //moveArmToPosition(ARM_UP)
    //moveArmToPosition(ARM_DN)
    //extendArmToPosition(EXTEND_OUT)
    //extendArmToPosition(EXTEND_IN)
    //runIntake(INTAKE_COLLECT)
    //runBlocking(() -> runIntake(INTAKE_DEPOSIT)

    TrajectoryActionBuilder tab1 = drive.actionBuilder(initialPose)
            .splineTo(new Vector2d(-39.53, -42.17), Math.toRadians(157.02))
            .splineTo(new Vector2d(-51.41, -52.33), Math.toRadians(220.33)); //new (-48.41,55.33)?
            //.turn(Math.toRadians(-20));
            //.waitSeconds(2);


    //.splineTo(new Vector2d(30, 30), Math.PI / 2)
                /*.lineToYSplineHeading(33, Math.toRadians(0))
                .waitSeconds(2)
                .setTangent(Math.toRadians(90))
                .lineToY(48)
                .setTangent(Math.toRadians(0))
                .lineToX(32)
                .strafeTo(new Vector2d(44.5, 30))
                .turn(Math.toRadians(180))
                .lineToX(47.5)
                */
    //.waitSeconds(3);
    // TrajectoryActionBuilder tab2 = drive.actionBuilder(initialPose)
    //.turn(90)
    //.waitSeconds(2);

    //TrajectoryActionBuilder tab3 = drive.actionBuilder(initialPose)
    //.turn(15);
    // Action trajectoryActionCloseOut = tab1.fresh()
    //.turn(-15)
    //.build();

    waitForStart();

    if (isStopRequested()) return;
    // Use Actions.runBlocking to run multiple tasks in sequence

    // Use Actions.runBlocking to run multiple tasks in sequence
    //Actions.runBlocking(Arm.ArmUP());
    Actions.runBlocking(
            new SequentialAction(
                    tab1.build()
                    //Arm.ArmUP(),
                    //Extend.ExtendOUT()
                    //Spin.Deposit()
                    //Arm.ArmDN()
                    //Extend.ExtendIN()
                    //Spin.Collect()


            )

    );
    Actions.runBlocking(Arm.ArmUP());
    Actions.runBlocking(Extend.ExtendOUT());
    Actions.runBlocking(Spin.Deposit());
    Actions.runBlocking(Extend.ExtendIN());
    Actions.runBlocking(Arm.ArmDN());
}

}