Merge remote-tracking branch 'origin/abstractingndparenting' into abs…

…tractingndparenting
svhsrobotics · Jan 11, 2024 · 94adee0 · 94adee0
2 parents 082cd1b + 27ed18b
commit 94adee0
Show file tree

Hide file tree

Showing 3 changed files with 160 additions and 5 deletions.
diff --git a/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/GoToBoard.java b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/GoToBoard.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.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))
+            //TODO: fix this turning it hurts :(
+            .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))
+                  //TODO: fix this turning it hurts :(
+                   .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))
+            //TODO: fix this turning it hurts :(
+            .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))
+            //TODO: fix this turning it hurts :(
+            .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");
+//            });
+
+        }
+
+        TrajectorySequence traj = trajB.build();
+        getRobot().getDrive().followTrajectorySequence(traj);
+
+    }
+
+
+
+
+}
diff --git a/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java
@@ -26,17 +26,23 @@ public static void main(String[] args) {
                     // CHANGE THESE LINES TO RUN ANOTHER COMPONENT
 //                    SampleComponent sampleComponent = new SampleComponent(robot);
 //                    sampleComponent.drive();
-                    //PurplePixelComponent purplePixelComponent = new PurplePixelComponent(robot, TensorFlowDetection.PropPosition.LEFT);
-                    //purplePixelComponent.drive();
-                    ParkingOut parking = new ParkingOut(robot);
-                    parking.drive();
+                    PurplePixelComponent purplePixelComponent = new PurplePixelComponent(robot, TensorFlowDetection.PropPosition.LEFT);
+
+                    //ParkingOut parking = new ParkingOut(robot);
+//                    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)
+
+
+
+                    meepMeep.setBackground(MeepMeep.Background.FIELD_CENTERSTAGE_JUICE_LIGHT)
                 .setDarkMode(true)
                 .setBackgroundAlpha(0.95f)
                 .addEntity(myBot)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/GoToBoard.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/GoToBoard.java
@@ -0,0 +1,73 @@
+package org.firstinspires.ftc.teamcode.opmode.components;
+
+import android.util.Log;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+
+import org.firstinspires.ftc.teamcode.drive.Robot;
+import org.firstinspires.ftc.teamcode.drive.TrajectoryDrive;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
+import org.firstinspires.ftc.teamcode.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");
+//            });
+
+        }
+
+        TrajectorySequence traj = trajB.build();
+        getRobot().getDrive().followTrajectorySequence(traj);
+
+    }
+
+
+}