Skip to content

Commit

Permalink
Fix PurplePixel component
Browse files Browse the repository at this point in the history
  • Loading branch information
@JJTech0130
JJTech0130 committed Jan 8, 2024
1 parent df5efd5 commit 2ce2776
Show file tree
Hide file tree
Showing 2 changed files with 159 additions and 18 deletions.
141 changes: 141 additions & 0 deletions MeepMeepTesting/src/main/java/com/example/meepmeeptesting/PurplePixelComponent.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,141 @@
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 PurplePixelComponent extends Component {
private TensorFlowDetection.PropPosition propPosition;
public PurplePixelComponent(Robot robot, TensorFlowDetection.PropPosition propPosition) {
super(robot);
this.propPosition = propPosition;
}

@Override
public void drive() {
//Pose2d getRobot().getDrive().getPoseEstimate() = getRobot().getDrive().getPoseEstimate();
Pose2d RED_AUDIENCE_TAPE_MARKS = new Pose2d(-3*12, -2*12 - 8);
Pose2d BLUE_AUDIENCE_TAPE_MARKS = new Pose2d(-3*12, 2*12 + 8);
Pose2d RED_BOARD_TAPE_MARKS = new Pose2d(12, -2*12 - 8);
Pose2d BLUE_BOARD_TAPE_MARKS = new Pose2d(12, 2*12 + 8);

TrajectorySequenceBuilder trajB = getRobot().getDrive().trajectorySequenceBuilder(getRobot().getDrive().getPoseEstimate());

if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_BOARD) {
if (propPosition == TensorFlowDetection.PropPosition.LEFT) {
trajB = trajB.lineTo(new Vector2d(BLUE_BOARD_TAPE_MARKS.getX(), BLUE_BOARD_TAPE_MARKS.getY()))
.strafeLeft(-12)
.addTemporalMarker(() -> {
getRobot().dropPurplePixel(true);
});

} else if (propPosition == TensorFlowDetection.PropPosition.RIGHT) {
trajB = trajB.lineTo(new Vector2d(BLUE_BOARD_TAPE_MARKS.getX(), BLUE_BOARD_TAPE_MARKS.getY()))
.strafeRight(-12)
.addTemporalMarker(() -> {
getRobot().dropPurplePixel(true);
});

} else {
trajB = trajB.lineTo(new Vector2d(BLUE_BOARD_TAPE_MARKS.getX(), BLUE_BOARD_TAPE_MARKS.getY()))
.forward(-8)
.addTemporalMarker(() -> {
getRobot().dropPurplePixel(true);
});


}
} else if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_AUDIENCE) {
if (propPosition == TensorFlowDetection.PropPosition.LEFT) {
trajB = trajB.lineTo(new Vector2d(BLUE_AUDIENCE_TAPE_MARKS.getX(), BLUE_AUDIENCE_TAPE_MARKS.getY()))
.strafeLeft(-12)
.addTemporalMarker(() -> {
getRobot().dropPurplePixel(true);
});

} else if (propPosition == TensorFlowDetection.PropPosition.RIGHT) {
trajB = trajB.lineTo(new Vector2d(BLUE_AUDIENCE_TAPE_MARKS.getX(), BLUE_AUDIENCE_TAPE_MARKS.getY()))
.strafeRight(-12)
.addTemporalMarker(() -> {
getRobot().dropPurplePixel(true);
});

} else {
trajB = trajB.lineTo(new Vector2d(BLUE_AUDIENCE_TAPE_MARKS.getX(), BLUE_AUDIENCE_TAPE_MARKS.getY()))
.forward(-8)
.addTemporalMarker(() -> {
getRobot().dropPurplePixel(true);
});


}
} else if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_AUDIENCE) {
if (propPosition == TensorFlowDetection.PropPosition.LEFT) {
trajB = trajB.lineTo(new Vector2d(RED_AUDIENCE_TAPE_MARKS.getX(), RED_AUDIENCE_TAPE_MARKS.getY()))
.strafeLeft(-12)
.addTemporalMarker(() -> {
getRobot().dropPurplePixel(true);
});

} else if (propPosition == TensorFlowDetection.PropPosition.RIGHT) {
trajB = trajB.lineTo(new Vector2d(RED_AUDIENCE_TAPE_MARKS.getX(), RED_AUDIENCE_TAPE_MARKS.getY()))
.strafeRight(-12)
.addTemporalMarker(() -> {
getRobot().dropPurplePixel(true);
});

} else {
trajB = trajB.lineTo(new Vector2d(RED_AUDIENCE_TAPE_MARKS.getX(), RED_AUDIENCE_TAPE_MARKS.getY()))
.forward(-8)
.addTemporalMarker(() -> {
getRobot().dropPurplePixel(true);
});


}
} else if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_BOARD) {
if (propPosition == TensorFlowDetection.PropPosition.LEFT) {
trajB = trajB.lineTo(new Vector2d(RED_BOARD_TAPE_MARKS.getX(), RED_BOARD_TAPE_MARKS.getY()))
.strafeLeft(-12)
.addTemporalMarker(() -> {
getRobot().dropPurplePixel(true);
});

} else if (propPosition == TensorFlowDetection.PropPosition.RIGHT) {
trajB = trajB.lineTo(new Vector2d(RED_BOARD_TAPE_MARKS.getX(), RED_BOARD_TAPE_MARKS.getY()))
.strafeRight(-12)
.addTemporalMarker(() -> {
getRobot().dropPurplePixel(true);
});

} else {
trajB = trajB.lineTo(new Vector2d(RED_BOARD_TAPE_MARKS.getX(), RED_BOARD_TAPE_MARKS.getY()))
.forward(-8)
.addTemporalMarker(() -> {
getRobot().dropPurplePixel(true);
});


}
}


TrajectorySequence traj = trajB.build();
getRobot().getDrive().followTrajectorySequence(traj);


// trajBuilder = trajBuilder.lineTo(new Vector2d(startPose.getX(), -2*12 - 3));
// .addTemporalMarker(() -> {
// Log.i("DROP", "dropping purple");
// purpleServo.setPosition(1);
// })
// .waitSeconds(1)
// .lineTo(new Vector2d(startPose.getX(), -5 * 12+3))
// .turn(Math.toRadians(90))
// .lineTo(new Vector2d(3*12+7, -5*12+4))
// .build();

}
}
36 changes: 18 additions & 18 deletions .../src/main/java/org/firstinspires/ftc/teamcode/opmode/components/PurplePixelComponent.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,32 +18,32 @@ public PurplePixelComponent(Robot robot, TensorFlowDetection.PropPosition propPo

@Override
public void drive() {
Pose2d currentPose = getRobot().getDrive().getPoseEstimate();
Pose2d RED_AUDIENCE_TAPE_MARKS = new Pose2d(-3*12, -4*12 + 6);
Pose2d BLUE_AUDIENCE_TAPE_MARKS = new Pose2d(-3*12, 4*12 + 6);
Pose2d RED_BOARD_TAPE_MARKS = new Pose2d(12, -4*12 + 6);
Pose2d BLUE_BOARD_TAPE_MARKS = new Pose2d(12, 4*12 + 6);
//Pose2d getRobot().getDrive().getPoseEstimate() = getRobot().getDrive().getPoseEstimate();
Pose2d RED_AUDIENCE_TAPE_MARKS = new Pose2d(-3*12, -2*12 - 8);
Pose2d BLUE_AUDIENCE_TAPE_MARKS = new Pose2d(-3*12, 2*12 + 8);
Pose2d RED_BOARD_TAPE_MARKS = new Pose2d(12, -2*12 - 8);
Pose2d BLUE_BOARD_TAPE_MARKS = new Pose2d(12, 2*12 + 8);

TrajectorySequenceBuilder trajB = getRobot().getDrive().trajectorySequenceBuilder(currentPose);
TrajectorySequenceBuilder trajB = getRobot().getDrive().trajectorySequenceBuilder(getRobot().getDrive().getPoseEstimate());

if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_BOARD) {
if (propPosition == TensorFlowDetection.PropPosition.LEFT) {
trajB = trajB.lineTo(new Vector2d(BLUE_BOARD_TAPE_MARKS.getX(), BLUE_BOARD_TAPE_MARKS.getY()))
.lineTo(new Vector2d(currentPose.getX() + 12, currentPose.getY()))
.strafeLeft(-12)
.addTemporalMarker(() -> {
getRobot().dropPurplePixel(true);
});

} else if (propPosition == TensorFlowDetection.PropPosition.RIGHT) {
trajB = trajB.lineTo(new Vector2d(BLUE_BOARD_TAPE_MARKS.getX(), BLUE_BOARD_TAPE_MARKS.getY()))
.lineTo(new Vector2d(currentPose.getX() - 12, currentPose.getY()))
.strafeRight(-12)
.addTemporalMarker(() -> {
getRobot().dropPurplePixel(true);
});

} else {
trajB = trajB.lineTo(new Vector2d(BLUE_BOARD_TAPE_MARKS.getX(), BLUE_BOARD_TAPE_MARKS.getY()))
.lineTo(new Vector2d(currentPose.getX(), currentPose.getY() - 8))
.forward(-8)
.addTemporalMarker(() -> {
getRobot().dropPurplePixel(true);
});
Expand All @@ -53,21 +53,21 @@ public void drive() {
} else if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_AUDIENCE) {
if (propPosition == TensorFlowDetection.PropPosition.LEFT) {
trajB = trajB.lineTo(new Vector2d(BLUE_AUDIENCE_TAPE_MARKS.getX(), BLUE_AUDIENCE_TAPE_MARKS.getY()))
.lineTo(new Vector2d(currentPose.getX() + 12, currentPose.getY()))
.strafeLeft(-12)
.addTemporalMarker(() -> {
getRobot().dropPurplePixel(true);
});

} else if (propPosition == TensorFlowDetection.PropPosition.RIGHT) {
trajB = trajB.lineTo(new Vector2d(BLUE_AUDIENCE_TAPE_MARKS.getX(), BLUE_AUDIENCE_TAPE_MARKS.getY()))
.lineTo(new Vector2d(currentPose.getX() - 12, currentPose.getY()))
.strafeRight(-12)
.addTemporalMarker(() -> {
getRobot().dropPurplePixel(true);
});

} else {
trajB = trajB.lineTo(new Vector2d(BLUE_AUDIENCE_TAPE_MARKS.getX(), BLUE_AUDIENCE_TAPE_MARKS.getY()))
.lineTo(new Vector2d(currentPose.getX(), currentPose.getY() - 8))
.forward(-8)
.addTemporalMarker(() -> {
getRobot().dropPurplePixel(true);
});
Expand All @@ -77,21 +77,21 @@ public void drive() {
} else if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_AUDIENCE) {
if (propPosition == TensorFlowDetection.PropPosition.LEFT) {
trajB = trajB.lineTo(new Vector2d(RED_AUDIENCE_TAPE_MARKS.getX(), RED_AUDIENCE_TAPE_MARKS.getY()))
.lineTo(new Vector2d(currentPose.getX() - 12, currentPose.getY()))
.strafeLeft(-12)
.addTemporalMarker(() -> {
getRobot().dropPurplePixel(true);
});

} else if (propPosition == TensorFlowDetection.PropPosition.RIGHT) {
trajB = trajB.lineTo(new Vector2d(RED_AUDIENCE_TAPE_MARKS.getX(), RED_AUDIENCE_TAPE_MARKS.getY()))
.lineTo(new Vector2d(currentPose.getX() + 12, currentPose.getY()))
.strafeRight(-12)
.addTemporalMarker(() -> {
getRobot().dropPurplePixel(true);
});

} else {
trajB = trajB.lineTo(new Vector2d(RED_AUDIENCE_TAPE_MARKS.getX(), RED_AUDIENCE_TAPE_MARKS.getY()))
.lineTo(new Vector2d(currentPose.getX(), currentPose.getY() + 8))
.forward(-8)
.addTemporalMarker(() -> {
getRobot().dropPurplePixel(true);
});
Expand All @@ -101,21 +101,21 @@ public void drive() {
} else if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_BOARD) {
if (propPosition == TensorFlowDetection.PropPosition.LEFT) {
trajB = trajB.lineTo(new Vector2d(RED_BOARD_TAPE_MARKS.getX(), RED_BOARD_TAPE_MARKS.getY()))
.lineTo(new Vector2d(currentPose.getX() - 12, currentPose.getY()))
.strafeLeft(-12)
.addTemporalMarker(() -> {
getRobot().dropPurplePixel(true);
});

} else if (propPosition == TensorFlowDetection.PropPosition.RIGHT) {
trajB = trajB.lineTo(new Vector2d(RED_BOARD_TAPE_MARKS.getX(), RED_BOARD_TAPE_MARKS.getY()))
.lineTo(new Vector2d(currentPose.getX() + 12, currentPose.getY()))
.strafeRight(-12)
.addTemporalMarker(() -> {
getRobot().dropPurplePixel(true);
});

} else {
trajB = trajB.lineTo(new Vector2d(RED_BOARD_TAPE_MARKS.getX(), RED_BOARD_TAPE_MARKS.getY()))
.lineTo(new Vector2d(currentPose.getX(), currentPose.getY() + 8))
.forward(-8)
.addTemporalMarker(() -> {
getRobot().dropPurplePixel(true);
});
Expand Down

0 comments on commit 2ce2776

Please sign in to comment.