diff --git a/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/RedBottomMeepMeep.java b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/RedBottomMeepMeep.java index 86a9350..a33c8d9 100644 --- a/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/RedBottomMeepMeep.java +++ b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/RedBottomMeepMeep.java @@ -12,7 +12,7 @@ public class RedBottomMeepMeep { public static void main(String[] args) { - MeepMeep meepMeep = new MeepMeep(800); + MeepMeep meepMeep = new MeepMeep(600); RoadRunnerBotEntity myBot = null; myBot = new DefaultBotBuilder(meepMeep) diff --git a/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/RedTopMeepMeep.java b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/RedTopMeepMeep.java new file mode 100644 index 0000000..8b26764 --- /dev/null +++ b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/RedTopMeepMeep.java @@ -0,0 +1,76 @@ +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.roadrunner.DefaultBotBuilder; +import com.noahbres.meepmeep.roadrunner.entity.RoadRunnerBotEntity; + +public class RedTopMeepMeep { + public static void main(String[] args) { + MeepMeep meepMeep = new MeepMeep(600); + RoadRunnerBotEntity myBot = null; + + myBot = new DefaultBotBuilder(meepMeep) + // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width + .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15) + .setColorScheme(new ColorSchemeBlueDark()) + .followTrajectorySequence(drive -> + drive.trajectorySequenceBuilder(new Pose2d(12, -60, Math.toRadians(-90))) + //Red Top Right Case +// .lineToLinearHeading(new Pose2d(23,-34, Math.toRadians(-90))) +// .forward(15) +// .turn(Math.toRadians(-90)) +// +// .lineToSplineHeading(new Pose2d(48, -30, Math.toRadians(180))) +// +// .back(4) +// .forward(4) +// +// .lineTo(new Vector2d(48, -60)) +// .lineTo(new Vector2d(60, -60)) + + //Red Top Left Case +// .lineToLinearHeading(new Pose2d(0,-34, Math.toRadians(-90))) +// .forward(15) +// .turn(Math.toRadians(-90)) +// +// .lineToSplineHeading(new Pose2d(48, -30, Math.toRadians(180))) +// +// .back(4) +// .forward(4) +// +// .lineTo(new Vector2d(48, -60)) +// .lineTo(new Vector2d(60, -60)) +// +// +// +// .build()); + + //Red Top Straight Case + .lineToLinearHeading(new Pose2d(12,-32, Math.toRadians(-90))) + .forward(7) + .turn(Math.toRadians(-90)) + + .lineToSplineHeading(new Pose2d(48, -30, Math.toRadians(180))) + + .back(4) + .forward(4) + + .lineTo(new Vector2d(48, -60)) + .lineTo(new Vector2d(60, -60)) + + + + .build()); + + + meepMeep.setBackground(MeepMeep.Background.FIELD_CENTERSTAGE_JUICE_DARK) + .setDarkMode(true) + .setBackgroundAlpha(0.95f) + .addEntity(myBot) + .start(); +} + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EK10582/auton/modes/RedBottom.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EK10582/auton/modes/RedBottom.java index be11f83..6265ef4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EK10582/auton/modes/RedBottom.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EK10582/auton/modes/RedBottom.java @@ -43,7 +43,7 @@ public void runOpMode() { case LEFT: traj_pushPixel = robot.roadRunner.trajectorySequenceBuilder(startPos) .lineToLinearHeading(new Pose2d(-47,-30, Math.toRadians(-90))) - .forward(4) + .back(4) .strafeLeft(8) .build(); traj_toBackboard = robot.roadRunner.trajectorySequenceBuilder(traj_pushPixel.end()) @@ -61,7 +61,7 @@ public void runOpMode() { break; case RIGHT: traj_pushPixel = robot.roadRunner.trajectorySequenceBuilder(startPos) - .lineToLinearHeading(new Pose2d(-24,-30, Math.toRadians(-90))) + .splineToLinearHeading(new Pose2d(20, -30, Math.toRadians(-90)), Math.toRadians(0)) .forward(4) .strafeRight(8) .build(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EK10582/auton/modes/RedTop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EK10582/auton/modes/RedTop.java index b876205..d27fc85 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EK10582/auton/modes/RedTop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EK10582/auton/modes/RedTop.java @@ -21,8 +21,8 @@ public class RedTop extends AutonBase { @Override public void runOpMode() { - double distFromAprilTagX, distFromAprilTagForward; - Pose2d startPos = new Pose2d(12,-60, Math.toRadians(0)); +// double distFromAprilTagX, distFromAprilTagForward; + Pose2d startPos = new Pose2d(16, -60, Math.toRadians(-90)); waitForStart(); @@ -36,198 +36,248 @@ public void runOpMode() { robot.roadRunner.setPoseEstimate(startPos); - switch (pos) { - case RIGHT: - robot.aprilTags.targetAprilTag = 6; - break; - - case LEFT: - robot.aprilTags.targetAprilTag = 4; - break; - - default: //case middle - robot.aprilTags.targetAprilTag = 5; - break; - } - robot.aprilTags.init(true); - sleep(1000); + TrajectorySequence traj_pushPixel = robot.roadRunner.trajectorySequenceBuilder(startPos).build(); + TrajectorySequence traj_toBackboard = robot.roadRunner.trajectorySequenceBuilder(startPos).build(); + TrajectorySequence traj_placePixel = robot.roadRunner.trajectorySequenceBuilder(startPos).build(); + TrajectorySequence traj_park = robot.roadRunner.trajectorySequenceBuilder(startPos).build(); switch (pos) { - case LEFT: - Trajectory pushPixelL = robot.roadRunner.trajectoryBuilder(startPos) - .lineToLinearHeading(new Pose2d(12,-36,Math.toRadians(0))) - .build(); - Trajectory toBoard = robot.roadRunner.trajectoryBuilder(pushPixelL.end()) - .lineToLinearHeading(new Pose2d(45, -36, Math.toRadians(-180))) - .build(); - - robot.roadRunner.followTrajectory(pushPixelL); - sleep(200); - robot.roadRunner.followTrajectory(toBoard); - sleep(200); - robot.aprilTags.update(true); - distFromAprilTagX = robot.aprilTags.tagX; - distFromAprilTagForward = robot.aprilTags.tagDistance; - - //failsafe - if(distFromAprilTagForward == -1){ - distFromAprilTagForward = 27; - distFromAprilTagX = 0; - } - - telemetry.addData("seetag for " + robot.aprilTags.targetAprilTag + ": ", robot.aprilTags.seeTag); - telemetry.addData("tagx ", robot.aprilTags.tagX); - telemetry.addData("tagdistance ", robot.aprilTags.tagDistance); - telemetry.update(); - - - sleep(2000); - TrajectorySequence turnToSlides = robot.roadRunner.trajectorySequenceBuilder(toBoard.end()) - .turn(Math.toRadians(-95)) + case RIGHT: + traj_pushPixel = robot.roadRunner.trajectorySequenceBuilder(startPos) + .lineToLinearHeading(new Pose2d(23,-34, Math.toRadians(-90))) + .forward(15) + .turn(Math.toRadians(-90)) .build(); - - Trajectory alignAprilTagL = robot.roadRunner.trajectoryBuilder(turnToSlides.end()) - .strafeTo(new Vector2d(20 + distFromAprilTagForward - 2, 28 - distFromAprilTagX - 4)) + traj_toBackboard = robot.roadRunner.trajectorySequenceBuilder(traj_pushPixel.end()) + .lineToSplineHeading(new Pose2d(48, -30, Math.toRadians(180))) .build(); - robot.roadRunner.followTrajectorySequence(turnToSlides); - sleep(1000); - robot.roadRunner.followTrajectory(alignAprilTagL); - - //TODO: fix dumper position - //robot.dumper.setPosition(SubsystemConstants.dumperTop); - - Trajectory awayL = robot.roadRunner.trajectoryBuilder(alignAprilTagL.end()) + traj_placePixel = robot.roadRunner.trajectorySequenceBuilder(traj_toBackboard.end()) + .back(4) .forward(4) .build(); - - Trajectory parkL = robot.roadRunner.trajectoryBuilder(awayL.end()) - .strafeLeft(32) + traj_park = robot.roadRunner.trajectorySequenceBuilder(traj_placePixel.end()) + .lineTo(new Vector2d(52, -60)) + .lineTo(new Vector2d(60, -60)) .build(); - - sleep(3000); - - robot.dumper.setPosition(0.5); - - robot.roadRunner.followTrajectory(awayL); - robot.roadRunner.followTrajectory(parkL); break; - - case RIGHT: - Trajectory toSpike = robot.roadRunner.trajectoryBuilder(startPos) - .lineToLinearHeading(new Pose2d(12,-36,Math.toRadians(180))) - .build(); - Trajectory strafeRightR = robot.roadRunner.trajectoryBuilder(toSpike.end()) - .lineToLinearHeading(new Pose2d(45, -36, Math.toRadians(-180))) + case LEFT: + traj_pushPixel = robot.roadRunner.trajectorySequenceBuilder(startPos) + .lineToLinearHeading(new Pose2d(0,-34, Math.toRadians(-90))) + .forward(15) + .turn(Math.toRadians(-90)) .build(); - - robot.roadRunner.followTrajectory(toSpike); - sleep(200); - robot.roadRunner.followTrajectory(strafeRightR); - sleep(200); - //get apriltag values - robot.aprilTags.update(true); - distFromAprilTagX = robot.aprilTags.tagX; - distFromAprilTagForward = robot.aprilTags.tagDistance; - - if(distFromAprilTagForward == -1){ - distFromAprilTagForward = 32; - distFromAprilTagX = -15; - } - - telemetry.addData("seetag for " + robot.aprilTags.targetAprilTag + ": ", robot.aprilTags.seeTag); - telemetry.addData("tagx ", robot.aprilTags.tagX); - telemetry.addData("tagdistance ", robot.aprilTags.tagDistance); - telemetry.update(); - - - sleep(5000); - robot.roadRunner.followTrajectory(strafeRightR); - - - Trajectory alignAprilTagR = robot.roadRunner.trajectoryBuilder(strafeRightR.end()) - .strafeTo(new Vector2d(20 + distFromAprilTagForward + 5, 28 - distFromAprilTagX - 4)) + traj_toBackboard = robot.roadRunner.trajectorySequenceBuilder(traj_pushPixel.end()) + .lineToSplineHeading(new Pose2d(48, -30, Math.toRadians(180))) .build(); - robot.roadRunner.followTrajectory(alignAprilTagR); - //robot.dumper.setPosition(SubsystemConstants.dumperTop); - - Trajectory awayR = robot.roadRunner.trajectoryBuilder(alignAprilTagR.end()) + traj_placePixel = robot.roadRunner.trajectorySequenceBuilder(traj_toBackboard.end()) + .back(4) .forward(4) .build(); - - Trajectory parkR = robot.roadRunner.trajectoryBuilder(awayR.end()) - .strafeLeft(16) + traj_park = robot.roadRunner.trajectorySequenceBuilder(traj_placePixel.end()) + .lineTo(new Vector2d(52, -60)) + .lineTo(new Vector2d(60, -60)) .build(); - - sleep(3000); - - robot.dumper.setPosition(0.5); - robot.roadRunner.followTrajectory(awayR); - robot.roadRunner.followTrajectory(parkR); break; default: //case middle - - //declare trajectories - Trajectory pushPixel = robot.roadRunner.trajectoryBuilder(startPos) - .back(24) + traj_pushPixel = robot.roadRunner.trajectorySequenceBuilder(startPos) + .lineToLinearHeading(new Pose2d(12,-32, Math.toRadians(-90))) + .forward(7) + .turn(Math.toRadians(-90)) .build(); - - Trajectory back = robot.roadRunner.trajectoryBuilder(pushPixel.end()) - .lineToLinearHeading(new Pose2d(45, -36, Math.toRadians(-180))) + traj_toBackboard = robot.roadRunner.trajectorySequenceBuilder(traj_pushPixel.end()) + .lineToSplineHeading(new Pose2d(48, -30, Math.toRadians(180))) .build(); - - robot.roadRunner.followTrajectory(pushPixel); - sleep(200); - - robot.roadRunner.followTrajectory(back); - sleep(200); - - robot.aprilTags.update(true); - distFromAprilTagX = robot.aprilTags.tagX; - distFromAprilTagForward = robot.aprilTags.tagDistance; - - if(distFromAprilTagForward == -1){ - distFromAprilTagForward = 27; - distFromAprilTagX = 0; - } - - telemetry.addData("seetag for " + robot.aprilTags.targetAprilTag + ": ", robot.aprilTags.seeTag); - telemetry.addData("tagx ", robot.aprilTags.tagX); - telemetry.addData("tagdistance ", robot.aprilTags.tagDistance); - telemetry.update(); - - sleep(500); - robot.roadRunner.followTrajectory(back); - - - Trajectory alignAprilTag = robot.roadRunner.trajectoryBuilder(back.end()) - .strafeTo(new Vector2d(20 + distFromAprilTagForward - 0, 28 - distFromAprilTagX - 2)) - .build(); - robot.roadRunner.followTrajectory(alignAprilTag); - //robot.dumper.setPosition(SubsystemConstants.dumperTop); - - Trajectory away = robot.roadRunner.trajectoryBuilder(alignAprilTag.end()) + traj_placePixel = robot.roadRunner.trajectorySequenceBuilder(traj_toBackboard.end()) + .back(4) .forward(4) .build(); - - Trajectory park = robot.roadRunner.trajectoryBuilder(away.end()) - .strafeLeft(24) + traj_park = robot.roadRunner.trajectorySequenceBuilder(traj_placePixel.end()) + .lineTo(new Vector2d(52, -60)) + .lineTo(new Vector2d(60, -60)) .build(); - - sleep(3000); - - robot.dumper.setPosition(0.5); - - robot.roadRunner.followTrajectory(away); - robot.roadRunner.followTrajectory(park); break; - - //end case middle - - } + robot.aprilTags.init(true); + sleep(1000); - - +// switch (pos) { +// case LEFT: +// Trajectory pushPixelL = robot.roadRunner.trajectoryBuilder(startPos) +// .lineToLinearHeading(new Pose2d(12,-36,Math.toRadians(0))) +// .build(); +// Trajectory toBoard = robot.roadRunner.trajectoryBuilder(pushPixelL.end()) +// .lineToLinearHeading(new Pose2d(45, -36, Math.toRadians(-180))) +// .build(); +// +// robot.roadRunner.followTrajectory(pushPixelL); +// sleep(200); +// robot.roadRunner.followTrajectory(toBoard); +// sleep(200); +// robot.aprilTags.update(true); +// distFromAprilTagX = robot.aprilTags.tagX; +// distFromAprilTagForward = robot.aprilTags.tagDistance; +// +// //failsafe +// if(distFromAprilTagForward == -1){ +// distFromAprilTagForward = 27; +// distFromAprilTagX = 0; +// } +// +// telemetry.addData("seetag for " + robot.aprilTags.targetAprilTag + ": ", robot.aprilTags.seeTag); +// telemetry.addData("tagx ", robot.aprilTags.tagX); +// telemetry.addData("tagdistance ", robot.aprilTags.tagDistance); +// telemetry.update(); +// +// +// sleep(2000); +// TrajectorySequence turnToSlides = robot.roadRunner.trajectorySequenceBuilder(toBoard.end()) +// .turn(Math.toRadians(-95)) +// .build(); +// +// Trajectory alignAprilTagL = robot.roadRunner.trajectoryBuilder(turnToSlides.end()) +// .strafeTo(new Vector2d(20 + distFromAprilTagForward - 2, 28 - distFromAprilTagX - 4)) +// .build(); +// robot.roadRunner.followTrajectorySequence(turnToSlides); +// sleep(1000); +// robot.roadRunner.followTrajectory(alignAprilTagL); +// +// //TODO: fix dumper position +// //robot.dumper.setPosition(SubsystemConstants.dumperTop); +// +// Trajectory awayL = robot.roadRunner.trajectoryBuilder(alignAprilTagL.end()) +// .forward(4) +// .build(); +// +// Trajectory parkL = robot.roadRunner.trajectoryBuilder(awayL.end()) +// .strafeLeft(32) +// .build(); +// +// sleep(3000); +// +// robot.dumper.setPosition(0.5); +// +// robot.roadRunner.followTrajectory(awayL); +// robot.roadRunner.followTrajectory(parkL); +// break; +// +// case RIGHT: +// Trajectory toSpike = robot.roadRunner.trajectoryBuilder(startPos) +// .lineToLinearHeading(new Pose2d(12,-36,Math.toRadians(180))) +// .build(); +// Trajectory strafeRightR = robot.roadRunner.trajectoryBuilder(toSpike.end()) +// .lineToLinearHeading(new Pose2d(45, -36, Math.toRadians(-180))) +// .build(); +// +// robot.roadRunner.followTrajectory(toSpike); +// sleep(200); +// robot.roadRunner.followTrajectory(strafeRightR); +// sleep(200); +// //get apriltag values +// robot.aprilTags.update(true); +// distFromAprilTagX = robot.aprilTags.tagX; +// distFromAprilTagForward = robot.aprilTags.tagDistance; +// +// if(distFromAprilTagForward == -1){ +// distFromAprilTagForward = 32; +// distFromAprilTagX = -15; +// } +// +// telemetry.addData("seetag for " + robot.aprilTags.targetAprilTag + ": ", robot.aprilTags.seeTag); +// telemetry.addData("tagx ", robot.aprilTags.tagX); +// telemetry.addData("tagdistance ", robot.aprilTags.tagDistance); +// telemetry.update(); +// +// +// sleep(5000); +// robot.roadRunner.followTrajectory(strafeRightR); +// +// +// Trajectory alignAprilTagR = robot.roadRunner.trajectoryBuilder(strafeRightR.end()) +// .strafeTo(new Vector2d(20 + distFromAprilTagForward + 5, 28 - distFromAprilTagX - 4)) +// .build(); +// robot.roadRunner.followTrajectory(alignAprilTagR); +// //robot.dumper.setPosition(SubsystemConstants.dumperTop); +// +// Trajectory awayR = robot.roadRunner.trajectoryBuilder(alignAprilTagR.end()) +// .forward(4) +// .build(); +// +// Trajectory parkR = robot.roadRunner.trajectoryBuilder(awayR.end()) +// .strafeLeft(16) +// .build(); +// +// sleep(3000); +// +// robot.dumper.setPosition(0.5); +// robot.roadRunner.followTrajectory(awayR); +// robot.roadRunner.followTrajectory(parkR); +// break; +// +// default: //case middle +// +// //declare trajectories +// Trajectory pushPixel = robot.roadRunner.trajectoryBuilder(startPos) +// .back(24) +// .build(); +// +// Trajectory back = robot.roadRunner.trajectoryBuilder(pushPixel.end()) +// .lineToLinearHeading(new Pose2d(45, -36, Math.toRadians(-180))) +// .build(); +// +// robot.roadRunner.followTrajectory(pushPixel); +// sleep(200); +// +// robot.roadRunner.followTrajectory(back); +// sleep(200); +// +// robot.aprilTags.update(true); +// distFromAprilTagX = robot.aprilTags.tagX; +// distFromAprilTagForward = robot.aprilTags.tagDistance; +// +// if(distFromAprilTagForward == -1){ +// distFromAprilTagForward = 27; +// distFromAprilTagX = 0; +// } +// +// telemetry.addData("seetag for " + robot.aprilTags.targetAprilTag + ": ", robot.aprilTags.seeTag); +// telemetry.addData("tagx ", robot.aprilTags.tagX); +// telemetry.addData("tagdistance ", robot.aprilTags.tagDistance); +// telemetry.update(); +// +// sleep(500); +// robot.roadRunner.followTrajectory(back); +// +// +// Trajectory alignAprilTag = robot.roadRunner.trajectoryBuilder(back.end()) +// .strafeTo(new Vector2d(20 + distFromAprilTagForward - 0, 28 - distFromAprilTagX - 2)) +// .build(); +// robot.roadRunner.followTrajectory(alignAprilTag); +// //robot.dumper.setPosition(SubsystemConstants.dumperTop); +// +// Trajectory away = robot.roadRunner.trajectoryBuilder(alignAprilTag.end()) +// .forward(4) +// .build(); +// +// Trajectory park = robot.roadRunner.trajectoryBuilder(away.end()) +// .strafeLeft(24) +// .build(); +// +// sleep(3000); +// +// robot.dumper.setPosition(0.5); +// +// robot.roadRunner.followTrajectory(away); +// robot.roadRunner.followTrajectory(park); +// break; +// +// //end case middle +// +// +// } +// +// +// +// } } } \ No newline at end of file