Merge pull request #49 from svhsrobotics/robotica-split

MeepMeepTesting/build.gradle

@@ -8,7 +8,7 @@ java {
 }
 
 dependencies {
-    implementation files('/Users/Shared/MeepMeep-1.0-SNAPSHOT.jar')
+    implementation files('C:\\Users\\mcweb\\Downloads\\MeepMeep-1.0-SNAPSHOT.jar')
     implementation 'org.apache.commons:commons-math3:3.6.1'
     implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7'
     implementation 'com.acmerobotics.roadrunner:core:0.5.6'

MeepMeepTesting/src/main/java/com/example/meepmeeptesting/Component.java

@@ -0,0 +1,24 @@
+package com.example.meepmeeptesting;
+
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+
+public abstract class Component {
+    static Vector2d BLUE_BOARD_CENTER_LINE = new Vector2d(12, 24.5);
+    static Vector2d BLUE_BOARD_LEFT_LINE = new Vector2d(23, 30);
+    static Vector2d BLUE_BOARD_RIGHT_LINE = new Vector2d(1, 30);
+    static Vector2d RED_BOARD_CENTER_LINE = new Vector2d(12, -24.5);
+    // LEFT MISSING
+    static Vector2d RED_BOARD_RIGHT_LINE = new Vector2d(23, -30);
+    //static Vector2d RED_AUDIENCE_CENTER_LINE = new Vector2d(12, -24.5);
+
+    private final Robot robot;
+    protected Component(Robot robot) {
+        this.robot = robot;
+    }
+
+    protected Robot getRobot() {
+        return robot;
+    }
+
+    public abstract void drive();
+}

MeepMeepTesting/src/main/java/com/example/meepmeeptesting/DriveShimShim.java

@@ -0,0 +1,51 @@
+package com.example.meepmeeptesting;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.noahbres.meepmeep.roadrunner.DriveShim;
+import com.noahbres.meepmeep.roadrunner.trajectorysequence.TrajectorySequence;
+import com.noahbres.meepmeep.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+
+public class DriveShimShim {
+    private final DriveShim driveShim;
+    public TrajectorySequence currentTrajectorySequence;
+    public DriveShimShim(DriveShim driveShim) {
+        this.driveShim = driveShim;
+    }
+
+    public void followTrajectorySequence(TrajectorySequence trajectorySequence) {
+        currentTrajectorySequence = trajectorySequence;
+    }
+
+    public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) {
+        return driveShim.trajectorySequenceBuilder(startPose);
+    }
+
+    public Pose2d getPoseEstimate() {
+        return driveShim.getPoseEstimate();
+    }
+
+    public void setPoseEstimate(Pose2d poseEstimate) {
+        driveShim.setPoseEstimate(poseEstimate);
+    }
+
+    public static TrajectoryDrive.Quadrant whichQuadrant(Pose2d pose) {
+        if (pose.getX() < 0) {
+            if (pose.getY() < 0) {
+                return TrajectoryDrive.Quadrant.RED_AUDIENCE;
+            } else {
+                return TrajectoryDrive.Quadrant.BLUE_AUDIENCE;
+            }
+        } else {
+            if (pose.getY() < 0) {
+                return TrajectoryDrive.Quadrant.RED_BOARD;
+            } else {
+                return TrajectoryDrive.Quadrant.BLUE_BOARD;
+            }
+        }
+    }
+
+    public TrajectoryDrive.Quadrant currentQuadrant() {
+        return whichQuadrant(getPoseEstimate());
+    }
+
+}

MeepMeepTesting/src/main/java/com/example/meepmeeptesting/GoToBoard.java

@@ -0,0 +1,77 @@
+package com.example.meepmeeptesting;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.noahbres.meepmeep.roadrunner.trajectorysequence.TrajectorySequence;
+import com.noahbres.meepmeep.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+
+public class GoToBoard extends Component{
+
+    protected GoToBoard(Robot robot) {
+        super(robot);
+    }
+
+    @Override
+    public void drive() {
+        //getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_BOARD
+
+        Pose2d startPose = getRobot().getDrive().getPoseEstimate();
+
+        TrajectorySequenceBuilder trajB = getRobot().getDrive().trajectorySequenceBuilder(startPose);
+
+        if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_AUDIENCE) {
+            trajB = trajB.lineTo(new Vector2d(startPose.getX(), -36))
+                    .lineTo(new Vector2d(50, -36))
+
+            .turn(Math.toRadians(-90-startPose.getHeading()))
+            //TODO:fix error with temporal marker because ryan is a dum dum
+           .addTemporalMarker(() -> {
+    //            android.util.Log.i("PLACE PIXEL", "Placed pixel at Red Board");
+            });
+
+        } else if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_BOARD) {
+            trajB = trajB.lineTo(new Vector2d(startPose.getX(), -36))
+                    .lineTo(new Vector2d(50, -36))
+
+                   .turn(Math.toRadians(-90-startPose.getHeading()))
+
+            //TODO:fix error with temporal marker because ryan is a dum dum
+            .addTemporalMarker(() -> {
+    //            android.util.Log.i("PLACE PIXEL", "Placed pixel at Red Board");
+            });
+
+        } else if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_BOARD) {
+            trajB = trajB.lineTo(new Vector2d(startPose.getX(), 36))
+                    .lineTo(new Vector2d(50, 36))
+
+            .turn(Math.toRadians(90-startPose.getHeading()))
+
+            //TODO:fix error with temporal marker because ryan is a dum dum
+            .addTemporalMarker(() -> {
+     //           android.util.Log.i("PLACE PIXEL", "Placed pixel at Blue Board");
+            });
+
+        }else {
+            trajB = trajB.lineTo(new Vector2d(startPose.getX(), 36))
+                    .lineTo(new Vector2d(50, 36))
+
+            .turn(Math.toRadians(90-startPose.getHeading()))
+
+
+            //TODO:fix error with temporal marker because ryan is a dum dum
+            .addTemporalMarker(() -> {
+     //           android.util.Log.i("PLACE PIXEL", "Placed pixel at Blue Board");
+            });
+                    //.turn
+
+        }
+
+        TrajectorySequence traj = trajB.build();
+        getRobot().getDrive().followTrajectorySequence(traj);
+
+    }
+
+
+
+
+}

MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java

@@ -1,33 +1,49 @@
 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.roadrunner.DefaultBotBuilder;
 import com.noahbres.meepmeep.roadrunner.entity.RoadRunnerBotEntity;
 
+
+
 public class MeepMeepTesting {
     public static void main(String[] args) {
         MeepMeep meepMeep = new MeepMeep(800);
-        Pose2d startPose = new Pose2d(-34,-62, Math.toRadians(90));
+        Pose2d startPose = new Pose2d(12,-62, Math.toRadians(270)); // RED_BOARD
+        //Pose2d startPose = new Pose2d(-36,-62, Math.toRadians(270)); // RED_AUDIENCE
+        //Pose2d startPose = new Pose2d(12,62, Math.toRadians(90)); // BLUE_BOARD
+       // Pose2d startPose = new Pose2d(-36,62, Math.toRadians(90)); // BLUE_AUDIENCE
+        //Pose2d startPose = new Pose2d(-60,-12, Math.toRadians(90)); //random spot
 
         RoadRunnerBotEntity myBot = new DefaultBotBuilder(meepMeep)
                 // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width
                 .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15)
-                        .followTrajectorySequence(drive ->
-                                drive.trajectorySequenceBuilder(startPose)
-                                        .lineTo(new Vector2d(startPose.getX(), 3*12-2))
-                                .turn(Math.toRadians(90))
-                                .addDisplacementMarker(()->{
-                                    //PIXEL DROP
-                                    System.out.println("DROPPED PIXEL");
-                                })
-
-                                .lineToSplineHeading(new Pose2d(2 * 12, 3 * 12 + 2, Math.toRadians(90)))
-                                .lineToSplineHeading(new Pose2d(3 * 12 + 5, 3 * 12 + 2, 0))
-                                        .build());
-
-        meepMeep.setBackground(MeepMeep.Background.FIELD_CENTERSTAGE_JUICE_LIGHT)
+                .followTrajectorySequence(drive -> {
+                    Robot robot = new Robot(drive);
+                    robot.getDrive().setPoseEstimate(startPose);
+
+                    // CHANGE THESE LINES TO RUN ANOTHER COMPONENT
+//                    SampleComponent sampleComponent = new SampleComponent(robot);
+//                    sampleComponent.drive();
+                    //PurplePixelComponent purplePixelComponent = new PurplePixelComponent(robot, TensorFlowDetection.PropPosition.CENTER);
+
+                    ParkingOut parking = new ParkingOut(robot);
+                    parking.drive();
+//                    GoToBoard placePixel = new GoToBoard(robot);
+                    //purplePixelComponent.drive();
+//                    placePixel.drive();
+
+                    // END CHANGE LINES
+
+                    return robot.getCurrentTrajectorySequence(); // This is a dirty hack, and assumes the component only calls followTrajectorySequence once
+                });
+
+
+
+
+
+                    meepMeep.setBackground(MeepMeep.Background.FIELD_CENTERSTAGE_JUICE_LIGHT)
                 .setDarkMode(true)
                 .setBackgroundAlpha(0.95f)
                 .addEntity(myBot)

MeepMeepTesting/src/main/java/com/example/meepmeeptesting/ParkingOut.java

@@ -0,0 +1,54 @@
+package com.example.meepmeeptesting;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.noahbres.meepmeep.roadrunner.trajectorysequence.TrajectorySequence;
+import com.noahbres.meepmeep.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+
+public class ParkingOut extends Component{
+
+    protected ParkingOut(Robot robot) {
+        super(robot);
+    }
+
+    @Override
+    public void drive() {
+
+        //getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_BOARD
+
+        Pose2d currentPose = getRobot().getDrive().getPoseEstimate();
+        TrajectorySequenceBuilder trajB = getRobot().getDrive().trajectorySequenceBuilder(currentPose);
+
+
+
+        if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_AUDIENCE) {
+            trajB =trajB.lineTo(new Vector2d(currentPose.getX(), -(4*12+9)))
+                    .lineTo(new Vector2d(0, -0));
+        } else if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_BOARD) {
+            trajB = trajB.lineTo(new Vector2d(currentPose.getX(), -56))
+                    .lineTo(new Vector2d(36, -56))
+                    .turn(Math.toRadians(180))
+                    .forward(90)
+                    .turn(Math.toRadians(90))
+                    .forward(90)
+                    .turn(Math.toRadians(90))
+                    .forward(45)
+                    .turn(Math.toRadians(90))
+                    .forward(90);
+
+
+
+
+        } else if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_BOARD) {
+            trajB = trajB.lineTo(new Vector2d(currentPose.getX(), 57))
+                    .lineTo(new Vector2d(59, 57));
+        } else {
+            trajB = trajB.lineTo(new Vector2d(currentPose.getX(), 57))
+                    .lineTo(new Vector2d(59, 57));
+        }
+        TrajectorySequence traj = trajB.build();
+        getRobot().getDrive().followTrajectorySequence(traj);
+    }
+//ryan i had to fix ur code it didn't copy ParkingIn correctly - not the values just formatting
+}
+