Created
June 29, 2021 17:35
-
-
Save NoahBres/cf85848ef627753fb1bcc22446668356 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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(); | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment