diff --git a/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/GoToBoard.java b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/GoToBoard.java new file mode 100644 index 00000000..e4690005 --- /dev/null +++ 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 index 7d6adb3a..0f1ddfd7 100644 --- 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 new file mode 100644 index 00000000..b726f920 --- /dev/null +++ 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); + + } + + +}