Road runner help

Hi,

We are trying to score a cone to a small junction during autonomous and then park.

For thr road runner we have the following sequence to build the trajectory:

               //We have already raised the height to medium junction however currently at starting position
                .strafeLeft(14)
                .forward(14)
                .addDisplacementMarker(() -> {
                    sleep(1000);
                    // code to open Servo claw
                    sleep(1000)
                })
                .back(12)
                .strafeRight(12)

I added the sleep(1000) before and after the claw open action to slow down a bit. However it completly skips the .back(12) and goes straight to StrafeRight step which messes up the autonomous path.

if there any other alternative so that the code is not dropped prematurely?

Thanks!

The trajectory planned doesn’t really “see” the sleep commands, so it won’t adjust the trajectory based on them. A simple approach is to just add some waits into the trajectory itself. Here’s one example of what we do if we just need to pause while we are not moving.

trjReadSignal = drive.trajectorySequenceBuilder(startPosition)
       .waitSeconds(0.1)
       // Move forward and raise lift.  Start looking for signal image
       .addDisplacementMarker(0.1, () -> {
             elevator.setLiftTargetPosition(Elevator.ELEVATOR_LOW);
             lookForSignalImage = true;
         })
        .lineTo(new Vector2d(-TILEx2_0 - 6, sideFlip(TILEx1_5)))
        .waitSeconds(0.5)
        .build();
1 Like