MeepMeepTwoBots.java

package com.example.meepmeeptesting;

import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.noahbres.meepmeep.MeepMeep;
import com.noahbres.meepmeep.core.colorscheme.scheme.ColorSchemeBlueDark;
import com.noahbres.meepmeep.core.colorscheme.scheme.ColorSchemeRedDark;
import com.noahbres.meepmeep.roadrunner.Constraints;
import com.noahbres.meepmeep.roadrunner.entity.RoadRunnerBotEntity;
import com.noahbres.meepmeep.roadrunner.trajectorysequence.TrajectorySequence;

public class MeepMeepTesting {
    public static void main(String[] args) {
        MeepMeep mm = new MeepMeep(650)
                // Set field image
                .setBackground(MeepMeep.Background.FIELD_ULTIMATE_GOAL_DARK)
                // Set theme
                .setTheme(new ColorSchemeRedDark())
                // Background opacity from 0-1
                .setBackgroundAlpha(1.0f)
                // Set constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width
                .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15)
                .followTrajectorySequence(drive ->
                        drive.trajectorySequenceBuilder(new Pose2d(-63.25, -15, 0))
                                .addDisplacementMarker(() -> {
                                })
                                .lineTo(new Vector2d(-6, -15))
                                .waitSeconds(3)
                                .splineTo(new Vector2d(24, -25), -Math.PI / 2)
                                .lineTo(new Vector2d(24, -52))
                                .splineTo(new Vector2d(32, -57), 0)
                                .splineTo(new Vector2d(40, -52), Math.PI / 2)
                                .lineTo(new Vector2d(40, -25))
                                .splineTo(new Vector2d(55, -15), 0)
                                .turn(Math.PI / 2)
                                .lineTo(new Vector2d(58, -41))
                                .UNSTABLE_addTemporalMarkerOffset(0, () -> {
                                })
                                .UNSTABLE_addTemporalMarkerOffset(0.4, () -> {
                                })
                                .UNSTABLE_addTemporalMarkerOffset(0.6, () -> {
                                })
                                .waitSeconds(1)
                                .lineTo(new Vector2d(55, -10))
                                .lineToLinearHeading(new Pose2d(-6, -15, 0))
                                .waitSeconds(3)
                                .lineTo(new Vector2d(-6, -36))
                                .lineTo(new Vector2d(-12, -36))
                                .waitSeconds(1)
                                .lineTo(new Vector2d(-34, -36))
                                .lineTo(new Vector2d(-6, -15))
                                .waitSeconds(2)
                                .forward(10)
                                .build()

                );

        Pose2d secondBotStartPose = new Pose2d(-63, -47, Math.PI);

        Constraints secondBotConstraints = new Constraints(50, 50, Math.toRadians(180), Math.toRadians(180), 15);
        RoadRunnerBotEntity secondBot = new RoadRunnerBotEntity(mm, secondBotConstraints, 15, 15, secondBotStartPose, new ColorSchemeBlueDark(), 1.0);

        TrajectorySequence secondBotTs = secondBot.getDrive().trajectorySequenceBuilder(secondBotStartPose)
                .setReversed(true)
                .addTemporalMarker(0.1, () -> {
                })
                .addTemporalMarker(1, () -> {
                }) //time to wait before dropping intake
                .splineTo(new Vector2d(-1, -59), 0) //wobble
                .UNSTABLE_addTemporalMarkerOffset(0.1, () -> {
                })
                .UNSTABLE_addTemporalMarkerOffset(1.25, () -> {
                })
                .UNSTABLE_addTemporalMarkerOffset(2.49, () -> {
                })
                .waitSeconds(2.5) //total time for wobble sequence
                .lineTo(new Vector2d(-6, -52)) //shooting
                .UNSTABLE_addTemporalMarkerOffset(0.1, () -> {
                })
                .UNSTABLE_addTemporalMarkerOffset(3.25, () -> {
                })
                .UNSTABLE_addTemporalMarkerOffset(4.9, () -> {
                })
                .waitSeconds(5) //total time for shooting sequence
                .forward(40) //get out of the way for partner
                .waitSeconds(13) //time to wait before park
                .splineToLinearHeading(new Pose2d(14, -36, Math.toRadians(1)), 0) //park
                .addTemporalMarker(0.9, 0.1, () -> {
                })
                .build();

        secondBot.followTrajectorySequence(secondBotTs);
        secondBot.start();

        mm.requestToAddEntity(secondBot);

        mm.start();
    }
}