acmerobotics / road-runner-quickstart

FTC quickstart for https://github.com/acmerobotics/road-runner
BSD 3-Clause Clear License
171 stars 857 forks source link

RoadRunner Markers Causing Crashing of Code #140

Closed timmyjr11 closed 3 years ago

timmyjr11 commented 3 years ago

We use Roadrunner for our Autonomous program, and the markers are causing the code to end/crash

For a better understanding of what is going on, I'm going to give a high-level overview of the autonomous, then more information on the problem at hand, and what I did to try to fix it:

Overview of autonomous:

THIS IS THE PART WHERE IT MESSES UP:

Things to know:

  1. All the markers were called and used properly until it tries to shoot in the goal, that's when it ends

  2. It always ends on the same marker, after the second marker in the shooting Trajectory, no matter how much I change the overall time and the time between, it still ends on the same part and the same marker.

  3. I tried to separate the number of markers in half, having the second trajectory move 0.5 inches to the right to satisfy the "EmptyList" Exception, but it still ends on the same point and marker AT THE SAME EXACT TIME EVERY TIME, NO MATTER WHAT TIME IS LISTED ON THE TEMPORAL MARKERS.

  4. All the Trajectorys uses ".followTrejectory()" DOESN'T USE ASYNC, meaning it should follow through all the markers regardless if the robot is moving or not, right?

  5. I tried to isolate the code into parts to see what's wrong, every Trajectory works perfectly, markers and all, only the shooting Trajectory is the one that's crashing, meaning there is no outside code that is causing this.

  6. I restarted the phone, the robot, and reran the code, but nothing fixed it

  7. I even wrote a new .Java file to see if it fixes the problem, it didn't

  8. I tried to switch to displacement markers to see if it would fix the problem, it did not, it still ended at the same exact point the temporal markers did

  9. Instead of using markers, I tried to create my own shooting method using an FTC timer Instead of Roadrunners, like this:

tapper.setPosition(1);

timer.reset;

while(timer.miliseconds < 150) {

}

Which does actually shoot the rings without ending the code and crashing it, but since it's not from Roadrunner, it cannot calculate and correct its angle, meaning it's off-angle and misses its shots.

I don't know how to fix this. I even asked the FTC discord for advice, but it was to no avail. I'm in a major roadblock until I fix the problem. so if ANYONE has an idea or solution or a resource to help me with my problem, that would be most appreciated!

Code of my Auto: (Trajectory ShootIntoGoal is where it crashes)

package RoadRunnerStuff;

import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint;
import com.acmerobotics.roadrunner.trajectory.constraints.MecanumVelocityConstraint;
import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint;
import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.util.ElapsedTime;

import org.opencv.core.Core;
import org.opencv.core.Mat;
import org.opencv.core.Rect;
import org.opencv.core.Scalar;
import org.opencv.imgproc.Imgproc;
import org.openftc.easyopencv.OpenCvCamera;
import org.openftc.easyopencv.OpenCvCameraFactory;
import org.openftc.easyopencv.OpenCvCameraRotation;
import org.openftc.easyopencv.OpenCvInternalCamera;
import org.openftc.easyopencv.OpenCvPipeline;

import java.util.Arrays;

@Autonomous
@Config
public class TestingRRTest extends LinearOpMode {
    //Creates the phone camera as an object
    OpenCvCamera phoneCam;

    //creates ring count as an object
    static double ringCount;

    //Creates the rectangles
    int rect1x = 179; //May need to be adjusted //80
    int rect1y = 107; // May need to be adjust

    double frontVelocity;
    double backVelocity;

    @Override
    public void runOpMode() {

        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);

        drive.frontShooter.setDirection(DcMotorSimple.Direction.REVERSE);
        drive.backShooter.setDirection(DcMotorSimple.Direction.REVERSE);
        drive.intake.setDirection(DcMotorSimple.Direction.REVERSE);

        frontVelocity = 1140;
        backVelocity = 1710;

        ElapsedTime timer = new ElapsedTime();

        drive.tapper.setPosition(0);
        drive.stopper.setPosition(1);
        drive.leftPivot.setPosition(1);
        drive.rightPivot.setPosition(0);
        drive.armPivot.setPosition(1);
        drive.Gripper.setPosition(0);

        drive.frontShooter.setVelocityPIDFCoefficients(20, 0, 10, 13.4);
        drive.backShooter.setVelocityPIDFCoefficients(7, 0, 4, 12);

        //Allows the camera to be used
        int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
        phoneCam = OpenCvCameraFactory.getInstance().createInternalCamera(OpenCvInternalCamera.CameraDirection.BACK, cameraMonitorViewId);

        //Detector
        phoneCam.setPipeline(new TestingRRTest.RingDetectingPipeline());

        //Allows the camera to turn on
        phoneCam.openCameraDeviceAsync(() -> phoneCam.startStreaming(320, 240, OpenCvCameraRotation.UPRIGHT));

        waitForStart();

        phoneCam.closeCameraDevice();

        if (isStopRequested()) return;

        Pose2d start = new Pose2d(-71, -35);

        drive.setPoseEstimate(start);

        Trajectory moveToShootPower = drive.trajectoryBuilder(start)
                .splineToConstantHeading(new Vector2d(-20, -6), Math.toRadians(0))

                .addTemporalMarker(0.01, () -> {
                    drive.frontShooter.setVelocity(frontVelocity);
                    drive.backShooter.setVelocity(backVelocity);
                    drive.stopper.setPosition(0);
                })

                .addTemporalMarker(3, () -> {
                    drive.tapper.setPosition(1);
                    drive.frontShooter.setVelocity(frontVelocity);
                    drive.backShooter.setVelocity(backVelocity);
                })

                .build();

        Trajectory shootPowerTwo = drive.trajectoryBuilder(moveToShootPower.end())
                .strafeRight(8)

                .addTemporalMarker(0.1, () -> {
                    drive.tapper.setPosition(0);
                    drive.frontShooter.setVelocity(frontVelocity);
                    drive.backShooter.setVelocity(backVelocity);
                })

                .addDisplacementMarker(() -> {
                    drive.tapper.setPosition(1);
                    drive.frontShooter.setVelocity(frontVelocity);
                    drive.backShooter.setVelocity(backVelocity);
                })

                .build();

        Trajectory shootPowerThree = drive.trajectoryBuilder(shootPowerTwo.end())
                .strafeRight(8)

                .addTemporalMarker(0.1, () -> {
                    drive.tapper.setPosition(0);
                    drive.frontShooter.setVelocity(1150);
                    drive.backShooter.setVelocity(1720);

                })

                .addDisplacementMarker(() -> {
                    drive.tapper.setPosition(1);
                    drive.frontShooter.setVelocity(1150);
                    drive.backShooter.setVelocity(1720);
                })

                .build();

        Trajectory movingToRing = drive.trajectoryBuilder(shootPowerThree.end())
                .splineToLinearHeading(new Pose2d(-11, -34, Math.toRadians(182)), Math.toRadians(0))
                .addDisplacementMarker(1, () -> {
                    drive.convey.setPower(1);
                    drive.intake.setPower(1);

                    drive.leftPivot.setPosition(0);
                    drive.rightPivot.setPosition(1);

                    drive.tapper.setPosition(0);
                    drive.stopper.setPosition(1);
                    drive.frontShooter.setVelocity(0);
                    drive.backShooter.setVelocity(0);
                })

                .build();

        Trajectory grabRing1 = drive.trajectoryBuilder(movingToRing.end())
                .forward(15,
                        new MinVelocityConstraint(
                                Arrays.asList(
                                        new AngularVelocityConstraint(221.6),
                                        new MecanumVelocityConstraint(40.6125, DriveConstants.TRACK_WIDTH)
                                )), new ProfileAccelerationConstraint(DriveConstants.MAX_ACCEL)
                )

                .addDisplacementMarker(() -> {
                    timer.reset();
                    while (timer.milliseconds() < 250) {

                    }
                })
                .build();

        Trajectory grabRing2 = drive.trajectoryBuilder(grabRing1.end())
                .forward(8,
                        new MinVelocityConstraint(
                                Arrays.asList(
                                        new AngularVelocityConstraint(221.6),
                                        new MecanumVelocityConstraint(40.6125, DriveConstants.TRACK_WIDTH)
                                )), new ProfileAccelerationConstraint(DriveConstants.MAX_ACCEL)
                )

                .addDisplacementMarker(() -> {
                    timer.reset();
                    while (timer.milliseconds() < 250) {

                    }
                })
                .build();

        Trajectory grabRing3 = drive.trajectoryBuilder(grabRing2.end())
                .forward(4,
                        new MinVelocityConstraint(
                                Arrays.asList(
                                        new AngularVelocityConstraint(221.6),
                                        new MecanumVelocityConstraint(40.6125, DriveConstants.TRACK_WIDTH)
                                )), new ProfileAccelerationConstraint(DriveConstants.MAX_ACCEL)
                )

                .addDisplacementMarker(() -> {
                    timer.reset();
                    while (timer.milliseconds() < 250) {

                    }
                })
                .build();

        Trajectory ShootIntoGoal = drive.trajectoryBuilder(grabRing3.end())
                .splineToLinearHeading(new Pose2d(-20, -30, Math.toRadians(0)), Math.toRadians(0))

                .addTemporalMarker(1, () -> {
                    drive.intake.setPower(0);

                    drive.leftPivot.setPosition(1);
                    drive.rightPivot.setPosition(0);

                    drive.frontShooter.setVelocity(1300);
                    drive.backShooter.setVelocity(1950);
                })

                .addTemporalMarker(1.5, () -> {
                    drive.stopper.setPosition(0);
                    drive.frontShooter.setVelocity(1300);
                    drive.backShooter.setVelocity(1950);
                })

                .addTemporalMarker(3, () -> {
                    drive.tapper.setPosition(1);
                    drive.frontShooter.setVelocity(1300);
                    drive.backShooter.setVelocity(1950);
                })

                .addTemporalMarker(3.2, () -> {
                    drive.tapper.setPosition(0);
                    drive.frontShooter.setVelocity(1300);
                    drive.backShooter.setVelocity(1950);
                })

                .addTemporalMarker(3.4, () -> {
                    drive.tapper.setPosition(1);
                    drive.frontShooter.setVelocity(1300);
                    drive.backShooter.setVelocity(1950);
                })

                .addTemporalMarker(3.6, () -> {
                    drive.tapper.setPosition(0);
                    drive.frontShooter.setVelocity(1300);
                    drive.backShooter.setVelocity(1950);
                })

                .addTemporalMarker(3.8, () -> {
                    drive.tapper.setPosition(1);
                    drive.frontShooter.setVelocity(1300);
                    drive.backShooter.setVelocity(1950);
                })

                .addTemporalMarker(4, () -> {
                    drive.tapper.setPosition(0);
                    drive.stopper.setPosition(1);
                    drive.frontShooter.setVelocity(0);
                    drive.backShooter.setVelocity(0);
                })
                .build();

        Trajectory goalZeroRingsP1 = drive.trajectoryBuilder(shootPowerThree.end())
                .splineToConstantHeading(new Vector2d(-15, -60), Math.toRadians(0))

                .addTemporalMarker(1.25, () -> drive.armPivot.setPosition(0.1))

                .addTemporalMarker(1.75, () -> drive.Gripper.setPosition(0.7))

                .addTemporalMarker(2.25, () -> drive.armPivot.setPosition(1))

                .addTemporalMarker(2.75, () -> drive.Gripper.setPosition(0))
                .build();

        //Work later on  Trajectory goalZeroRingP2

        Trajectory goalOneRingP1 = drive.trajectoryBuilder(shootPowerThree.end())
                .splineToConstantHeading(new Vector2d(10, -32), Math.toRadians(0))

                .addTemporalMarker(1, () -> drive.armPivot.setPosition(0.1))

                .addTemporalMarker(1.5, () -> drive.Gripper.setPosition(0.7))

                .addTemporalMarker(2, () -> drive.armPivot.setPosition(1))

                .addTemporalMarker(2.5, () -> drive.Gripper.setPosition(0))

                .build();

        //Work later on Trajectory goalOneRingP2

        Trajectory goalFourRingP1 = drive.trajectoryBuilder(shootPowerThree.end())
                .splineToConstantHeading(new Vector2d(35, -55), Math.toRadians(0))

                .addTemporalMarker(1.5, () -> drive.armPivot.setPosition(0.1))

                .addTemporalMarker(2, () -> drive.Gripper.setPosition(0.7))

                .addTemporalMarker(2.5, () -> drive.armPivot.setPosition(1))

                .addTemporalMarker(3, () -> drive.Gripper.setPosition(0))

                .build();

        Trajectory park = drive.trajectoryBuilder(ShootIntoGoal.end())
                .lineTo(new Vector2d(-10, -29))
                .build();

        drive.followTrajectory(moveToShootPower);
        drive.followTrajectory(shootPowerTwo);
        drive.followTrajectory(shootPowerThree);

        if (ringCount == 4) {
            drive.followTrajectory(goalFourRingP1);
        } else if (ringCount == 1) {
            drive.followTrajectory(goalOneRingP1);
        } else {
            drive.followTrajectory(goalZeroRingsP1);
        }

        if (ringCount == 4) {
            drive.followTrajectory(movingToRing);
            drive.followTrajectory(grabRing1);
            drive.followTrajectory(grabRing2);
            drive.followTrajectory(grabRing3);
            drive.followTrajectory(ShootIntoGoal);

        } else if (ringCount == 1) {
            drive.followTrajectory(movingToRing);
            drive.followTrajectory(grabRing1);
            drive.followTrajectory(ShootIntoGoal);
        }

        drive.followTrajectory(park);
    }

    //Class used to create the pipeline
    class RingDetectingPipeline extends OpenCvPipeline {

        //Creates the YCbCr color space as a mat
        Mat YCbCr = new Mat();

        //Creates output as a mat
        Mat outPut = new Mat();

        // Creates the lower part of the rectangle as a mat
        Mat lowerCrop = new Mat();

        //Used to process the frame from the camera
        @Override
        public Mat processFrame(Mat input) {
            //Converts the input that is RGB into YCbCr
            Imgproc.cvtColor(input, YCbCr, Imgproc.COLOR_RGB2YCrCb);

            //Copies the conversion into output
            input.copyTo(outPut);

            //Creating the longer rectangle
            Rect rect1 = new Rect(rect1x, rect1y, 47, 40); //May need to be adjusted

            Scalar rectangleColor = new Scalar(0, 0, 255);

            //Drawing the rectangles on the screen
            Imgproc.rectangle(outPut, rect1, rectangleColor, 2);

            //Cropping the image for stack height

            //cropping YCbCr, putting it on loweCrop mat
            lowerCrop = YCbCr.submat(rect1); //May need to be rect1

            //Extracting the orange color, placing it on a mat
            Core.extractChannel(lowerCrop, lowerCrop, 2);

            //Creates an average using raw data, puts data on a Scalar variable
            Scalar lowerAverageOrange = Core.mean(lowerCrop);

            //Taking the first value of the average and putting it in a variable
            double finalAverage = lowerAverageOrange.val[0];

            //Comparing average values to calculate ring count
            //Needs to be adjusted through test runs
            if (finalAverage > 107) {
                ringCount = 0;
            } else if (finalAverage > 97) {
                ringCount = 1;
            } else {
                ringCount = 4;
            }

            //Outputs the average onto the driver station
            telemetry.addData("Average", finalAverage);
            telemetry.addLine("There are " + ringCount + " rings.");
            telemetry.update();

            //Returns the output into the main code
            return outPut;
        }
    }
}
rbrott commented 3 years ago
  • but only two of them are used and then the code ends/crashes when it is not supposed to.

Do you have the stack trace/error message for the crash?

                .addDisplacementMarker(() -> {
                    timer.reset();
                    while (timer.milliseconds() < 250) {

                    }
                })
                .build();

Marker callbacks should never block for extended periods.

2. It always ends on the same marker, after the second marker in the shooting Trajectory, no matter how much I change the overall time and the time between, it still ends on the same part and the same marker.

3. I tried to separate the number of markers in half, having the second trajectory move 0.5 inches to the right to satisfy the "EmptyList" Exception, but it still ends on the same point and marker AT THE SAME EXACT TIME EVERY TIME, NO MATTER WHAT TIME IS LISTED ON THE TEMPORAL MARKERS.

4. All the Trajectorys uses ".followTrejectory()" DOESN'T USE ASYNC, meaning it should follow through all the markers regardless if the robot is moving or not, right?

Markers are designed to execute while the trajectory is running. When the follower has finished the trajectory, it immediately schedules all remaining markers to execute. It does not stall to respect their given times. For example, if I have a 2.5-second trajectory with markers at 1, 2, 3, and 4 seconds, the first two markers will execute as schedule, while the last two will both execute at 2.5 seconds when the follower finishes (this is complicated slightly by admissible error).

timmyjr11 commented 3 years ago
  • but only two of them are used and then the code ends/crashes when it is not supposed to.

Do you have the stack trace/error message for the crash?

                .addDisplacementMarker(() -> {
                    timer.reset();
                    while (timer.milliseconds() < 250) {

                    }
                })
                .build();

Marker callbacks should never block for extended periods.

  1. It always ends on the same marker, after the second marker in the shooting Trajectory, no matter how much I change the overall time and the time between, it still ends on the same part and the same marker.

  2. I tried to separate the number of markers in half, having the second trajectory move 0.5 inches to the right to satisfy the "EmptyList" Exception, but it still ends on the same point and marker AT THE SAME EXACT TIME EVERY TIME, NO MATTER WHAT TIME IS LISTED ON THE TEMPORAL MARKERS.

  3. All the Trajectorys uses ".followTrejectory()" DOESN'T USE ASYNC, meaning it should follow through all the markers regardless if the robot is moving or not, right?

Markers are designed to execute while the trajectory is running. When the follower has finished the trajectory, it immediately schedules all remaining markers to execute. It does not stall to respect their given times. For example, if I have a 2.5-second trajectory with markers at 1, 2, 3, and 4 seconds, the first two markers will execute as schedule, while the last two will both execute at 2.5 seconds when the follower finishes (this is complicated slightly by admissible error).

  • but only two of them are used and then the code ends/crashes when it is not supposed to.

Do you have the stack trace/error message for the crash?

                .addDisplacementMarker(() -> {
                    timer.reset();
                    while (timer.milliseconds() < 250) {

                    }
                })
                .build();

Marker callbacks should never block for extended periods.

  1. It always ends on the same marker, after the second marker in the shooting Trajectory, no matter how much I change the overall time and the time between, it still ends on the same part and the same marker.

  2. I tried to separate the number of markers in half, having the second trajectory move 0.5 inches to the right to satisfy the "EmptyList" Exception, but it still ends on the same point and marker AT THE SAME EXACT TIME EVERY TIME, NO MATTER WHAT TIME IS LISTED ON THE TEMPORAL MARKERS.

  3. All the Trajectorys uses ".followTrejectory()" DOESN'T USE ASYNC, meaning it should follow through all the markers regardless if the robot is moving or not, right?

Markers are designed to execute while the trajectory is running. When the follower has finished the trajectory, it immediately schedules all remaining markers to execute. It does not stall to respect their given times. For example, if I have a 2.5-second trajectory with markers at 1, 2, 3, and 4 seconds, the first two markers will execute as schedule, while the last two will both execute at 2.5 seconds when the follower finishes (this is complicated slightly by admissible error).

I get no error message or anything like that. The code kinda stops as if I pressed on the stop button.

If that's the case, then that's what's wrong with my code, it just runs the rest of the markers instantly and at the same time at the end. Is there any way to have it stop moving and follow through with the times then? Because moving while shooting is a problem. And I understand the problem with blocking the next trajectory with the while statement, the reason I had to do that is because the rings get stuck inside the intake if rings are picked up so closely together in terms of time, which is why I made it stop. I can't use a sleep method outside of the trajectory because it stops the intake motor as well.

rbrott commented 3 years ago

Is there any way to have it stop moving and follow through with the times then?

The easiest solution is find the duration of the trajectory and separate out the late markers into normal op mode code.

timmyjr11 commented 3 years ago

Is there any way to have it stop moving and follow through with the times then?

The easiest solution is find the duration of the trajectory and separate out the late markers into normal op mode code.

Wouldn't that causes my 9th point? Where if it doesn't check it's Trajectory it becomes off angle?

rbrott commented 3 years ago

My proposed solution works provided you don't need feedback after the robot stops.

If you need feedback after the follower has finished, you can just continue calling TrajectoryFollower#update(). This will require some changes to the drive class (perhaps a new mode?).

timmyjr11 commented 3 years ago

My proposed solution works provided you don't need feedback after the robot stops.

If you need feedback after the follower has finished, you can just continue calling TrajectoryFollower#update(). This will require some changes to the drive class (perhaps a new mode?).

Okay cooollllll, I'll work with that and learn about it further. I had made a solution by having the robot move little distance REALLY SLOWLY in different Trejectory to have all the markers work properly, thanks for the help!