MeepMeep Ring Entity.java

/*
 * Make sure to use MeepMeep 1.0.1
 *
 * This is a very rough POC and not polished at all. Velocity doesn't decelerate, no clear units, etc.
 * Also the shot in the marker should be moved to a reusable function
 */

/**** RingEntity.java *****/
package com.example.meepmeeptesting;

import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.noahbres.meepmeep.MeepMeep;
import com.noahbres.meepmeep.core.entity.Entity;
import com.noahbres.meepmeep.core.util.FieldUtil;

import org.jetbrains.annotations.NotNull;

import java.awt.BasicStroke;
import java.awt.Color;
import java.awt.Graphics2D;

public class RingEntity implements Entity {
    private String tag = "RING_ENTITY";

    private int zIndex = 10;

    private MeepMeep meepMeep;

    private double canvasWidth = FieldUtil.getCANVAS_WIDTH();
    private double canvasHeight = FieldUtil.getCANVAS_WIDTH();

    private double radius = 2.5;
    private double thickness = 1;

    private Vector2d position;
    private Vector2d velocity;

    public RingEntity(MeepMeep meepMeep, Vector2d intialPosition, Vector2d initialVelocity) {
        this.meepMeep = meepMeep;

        this.position = intialPosition;
        this.velocity = initialVelocity;
    }

    @Override
    public void update(long deltaTime) {
        position = position.plus(this.velocity.times(deltaTime / 1e9));

        if (position.getX() > FieldUtil.getFIELD_WIDTH() / 2.0 || position.getX() < -FieldUtil.getFIELD_WIDTH() / 2.0 || position.getY() > FieldUtil.getFIELD_HEIGHT() / 2.0 || position.getY() < -FieldUtil.getFIELD_HEIGHT() / 2.0) {
            meepMeep.requestToClearEntity(this);
        }
    }

    @Override
    public void render(Graphics2D gfx, int i, int i1) {
        Vector2d screenCoords = FieldUtil.fieldCoordsToScreenCoords(position);
        double radPixels = FieldUtil.scaleInchesToPixel(radius, canvasWidth, canvasHeight);

        gfx.setStroke(new BasicStroke((int) FieldUtil.scaleInchesToPixel(thickness, canvasWidth, canvasHeight)));
        gfx.setColor(new Color(255, 255, 0));
        gfx.drawOval(
                (int) (screenCoords.getX() - radPixels),
                (int) (screenCoords.getY() - radPixels),
                (int) (radPixels * 2),
                (int) (radPixels * 2)
        );
    }

    @NotNull
    @Override
    public MeepMeep getMeepMeep() {
        return null;
    }

    @NotNull
    @Override
    public String getTag() {
        return tag;
    }

    @Override
    public int getZIndex() {
        return this.zIndex;
    }

    @Override
    public void setZIndex(int i) {
        this.zIndex = i;
    }

    @Override
    public void setCanvasDimensions(double width, double height) {
        this.canvasWidth = width;
        this.canvasHeight = height;
    }
}

/***** MeepMeepTesting.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.ColorSchemeRedDark;


public class MeepMeepTesting {
    public static void main(String[] args) {
        MeepMeep mm = new MeepMeep(650);

        mm
                // 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, () -> {
                                    Vector2d botCenter = drive.getPoseEstimate().vec();
                                    Vector2d offset = new Vector2d(1, 3);
                                    Vector2d ringOrigin = botCenter.plus(offset.rotated(drive.getPoseEstimate().getHeading()));

                                    Vector2d headingVec = drive.getPoseEstimate().headingVec().times(90);

                                    mm.requestToAddEntity(new RingEntity(mm, ringOrigin, headingVec));
                                })
                                .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();
    }
}