Skip to content

Commit

Permalink
fixing testauto because im better -ryan ;)
Browse files Browse the repository at this point in the history
  • Loading branch information
ExultantExalted committed Dec 11, 2023
1 parent c9578f9 commit 4b3fe78
Show file tree
Hide file tree
Showing 2 changed files with 36 additions and 32 deletions.
4 changes: 2 additions & 2 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOp.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@ public void runOpMode() throws InterruptedException {


while (!isStopRequested()) {
drive.update(); // MUST be calld every loop cycle so that RoadRunner calculates the pose correctly
drive.update(); // MUST be called every loop cycle so that RoadRunner calculates the pose correctly
// Read pose
Pose2d poseEstimate = drive.getPoseEstimate();

Expand Down Expand Up @@ -57,7 +57,6 @@ public void runOpMode() throws InterruptedException {
} else {
// Close pixel hand servo
}

if (gamepad1.right_bumper && gamepad1.left_bumper && getRuntime() >= 85) { // technically endgame is 90sec, we let them launch a little early just in case
// Launch the airplane
}
Expand All @@ -69,4 +68,5 @@ public void runOpMode() throws InterruptedException {


}
//deus pater omnipotens domine fili unigenite jesu christe domini domine deus agnus agnus dei filius agnus dei filius patris quitolis pecca tamundi misere renobis agunus quitolis pecca tamundi misere renobis suscipe deprecationemnostram quise des ad teram patris misere misere misere renobis
}
64 changes: 34 additions & 30 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestAuto.java
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
package org.firstinspires.ftc.teamcode.opmode;

import android.util.Log;

import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
Expand Down Expand Up @@ -47,7 +49,7 @@ public void runOpMode() throws InterruptedException {


TensorFlowDetection tensor = new TensorFlowDetection(cameras[1].webcamName);
android.util.Log.i("PROGRESS", "I did this!");
Log.i("PROGRESS", "I did this!");
// while (!isStopRequested()) {
// tensor.getPropPosition();
// android.util.Log.w("LOCATION", "tensor result " + tensor.getPropPosition());
Expand All @@ -57,23 +59,23 @@ public void runOpMode() throws InterruptedException {
TensorFlowDetection.PropPosition tensorPos = tensor.getPropPosition();

// TensorFlowDetection.PropPosition tensorPos = TensorFlowDetection.PropPosition.LEFT;
android.util.Log.i("PROGRESS", "Made it here");
android.util.Log.i("PROGRESS", tensorPos + "");
android.util.Log.i("PROGRESS", tensor.getPropPosition() + "");
Log.i("PROGRESS", "Made it here");
Log.i("PROGRESS", tensorPos + "");
Log.i("PROGRESS", tensor.getPropPosition() + "");


AprilTagLocalizer aprilTag = new AprilTagLocalizer(cameras);
Pose2d startPose = null;
android.util.Log.i("PROGRESS", "here?");
Log.i("PROGRESS", "here?");
while(opModeInInit() && !isStopRequested() && startPose == null) {
android.util.Log.i("PROGRESS", "before");
Log.i("PROGRESS", "before");
startPose = estimateWithAllCameras(cameras, aprilTag);
android.util.Log.i("PROGRESS", "after");
Log.i("PROGRESS", "after");
}
android.util.Log.i("PROGRESS", "Maybe?");
Log.i("PROGRESS", "Maybe?");
if (startPose == null) {
// Check one more time
android.util.Log.w("APRILTAG", "Did not find AprilTag in init, trying again");
Log.w("APRILTAG", "Did not find AprilTag in init, trying again");
startPose = estimateWithAllCameras(cameras, aprilTag);
}

Expand All @@ -82,11 +84,11 @@ public void runOpMode() throws InterruptedException {
//startPose = new Pose2d(0, 0, 0);
throw new RuntimeException("Could not find AprilTag to determine start pose");
}
android.util.Log.i("PROGRESS", "may as well check");
Log.i("PROGRESS", "may as well check");
TrajectorySequence traj = null;

drive.setPoseEstimate(startPose);
android.util.Log.i("PROGRESS", "Im here too!");
Log.i("PROGRESS", "Im here too!");


switch (AprilTagLocalizer.whichQuadrant(startPose)) {
Expand Down Expand Up @@ -118,7 +120,7 @@ public void runOpMode() throws InterruptedException {
.turn(Math.toRadians(-90))
.addTemporalMarker(() ->{
//ARM DROPS PIXEL
android.util.Log.i("DROP", "drop");
Log.i("DROP", "drop");
})
.lineTo(new Vector2d(startPose.getX(), startPose.getY()))
.splineToSplineHeading(new Pose2d(3 * 12 + 5, -3 * 12, 0), 0)
Expand All @@ -130,7 +132,7 @@ public void runOpMode() throws InterruptedException {
.lineTo(new Vector2d(16, -36))
.addTemporalMarker(() -> {
//ARM DROPS PIXEL
android.util.Log.i("DROP", "drop");
Log.i("DROP", "drop");

})
.turn(Math.toRadians(90))
Expand All @@ -150,7 +152,7 @@ public void runOpMode() throws InterruptedException {
.addTemporalMarker(() -> {
//PIXEL DROP

android.util.Log.i("DROP", "drop");
Log.i("DROP", "drop");

})
.waitSeconds(1)
Expand All @@ -168,7 +170,7 @@ public void runOpMode() throws InterruptedException {
.turn(Math.toRadians(-90))
.addDisplacementMarker(() -> {
//PIXEL DROP
android.util.Log.i("DROP", "drop");
Log.i("DROP", "drop");
})
.lineTo(new Vector2d(startPose.getX(), startPose.getY()))
//.splineToSplineHeading(new Pose2d(3*12 + 5, -3*12, 0), 0)
Expand All @@ -181,7 +183,7 @@ public void runOpMode() throws InterruptedException {
.lineTo(new Vector2d(startPose.getX(), -3 * 12 + 2)) // drive forward to prevent the spline from cutting through the poles
.addTemporalMarker(() -> {
//PIXEL DROP
android.util.Log.i("DROP", "drop");
Log.i("DROP", "drop");
})
.turn(Math.toRadians(90))
//.splineToSplineHeading(new Pose2d(3*12 + 5, -3*12, 0), 0)
Expand All @@ -197,9 +199,9 @@ public void runOpMode() throws InterruptedException {
traj = drive.trajectorySequenceBuilder(startPose)
.lineTo(new Vector2d(startPose.getX(), 3*12-2))
.turn(Math.toRadians(90))
.addDisplacementMarker(()->{
.addTemporalMarker(()->{
//PIXEL DROP
android.util.Log.i("ARM", "drop");
Log.i("ARM", "drop");
})
.lineToSplineHeading(new Pose2d(2 * 12, 3 * 12 + 2, Math.toRadians(90)))
.lineToSplineHeading(new Pose2d(3 * 12 + 5, 3 * 12 + 2, 0))
Expand All @@ -211,10 +213,11 @@ public void runOpMode() throws InterruptedException {
traj = drive.trajectorySequenceBuilder(startPose)
.lineTo(new Vector2d(startPose.getX(), 3*12-2))
.turn(Math.toRadians(-90))
.addDisplacementMarker(()->{
.addTemporalMarker(()->{
//PIXEL DROP
android.util.Log.i("DROP", "drop");
Log.i("DROP", "drop");
})
.lineTo(new Vector2d(startPose.getX(),startPose.getY()))
.turn(Math.toRadians(180))
.lineToSplineHeading(new Pose2d(2 * 12, 3 * 12 + 2, Math.toRadians(90)))
.lineToSplineHeading(new Pose2d(3 * 12 + 5, 3 * 12 + 2, 0))
Expand All @@ -223,10 +226,10 @@ public void runOpMode() throws InterruptedException {
case CENTER:
traj = drive.trajectorySequenceBuilder(startPose)
.lineTo(new Vector2d(startPose.getX(), 3*12-2))
.addDisplacementMarker(()->{
.addTemporalMarker(()->{
//PIXEL DROP
//wait(200);
android.util.Log.i("DROP", "drop");
Log.i("DROP", "drop");
})
.turn(Math.toRadians(90))
.lineToSplineHeading(new Pose2d(2 * 12, 3 * 12 + 2, Math.toRadians(90)))
Expand All @@ -245,9 +248,9 @@ public void runOpMode() throws InterruptedException {
traj = drive.trajectorySequenceBuilder(startPose)
.lineTo(new Vector2d(startPose.getX(), 36))
.turn(Math.toRadians(90))
.addDisplacementMarker(()->{
.addTemporalMarker(()->{
//PIXEL DROP
android.util.Log.i("DROP", "drop");
Log.i("DROP", "drop");
})

.splineToSplineHeading(new Pose2d(3 * 12 + 5, 3 * 12, 0), 0)
Expand All @@ -257,10 +260,11 @@ public void runOpMode() throws InterruptedException {
traj = drive.trajectorySequenceBuilder(startPose)
.lineTo(new Vector2d(startPose.getX(), 36))
.turn(Math.toRadians(-90))
.addDisplacementMarker(()->{
.addTemporalMarker(()->{
//PIXEL DROP
android.util.Log.i("DROP", "drop");
Log.i("DROP", "drop");
})
.lineTo(new Vector2d(startPose.getX(),startPose.getY()))
.turn(Math.toRadians(180))
.splineToSplineHeading(new Pose2d(3 * 12 + 5, 3 * 12, 0), 0)
.build();
Expand All @@ -269,9 +273,9 @@ public void runOpMode() throws InterruptedException {
traj = drive.trajectorySequenceBuilder(startPose)
.lineTo(new Vector2d(startPose.getX(), 36))

.addDisplacementMarker(()->{
.addTemporalMarker(()->{
//PIXEL DROP
android.util.Log.i("DROP", "drop");
Log.i("DROP", "drop");
})
.turn(Math.toRadians(90))
.splineToSplineHeading(new Pose2d(3 * 12 + 5, 3 * 12, 0), 0)
Expand All @@ -280,15 +284,15 @@ public void runOpMode() throws InterruptedException {
}
break;
}
android.util.Log.i("PROGRESS", "I got here???");
Log.i("PROGRESS", "I got here???");
waitForStart();

if (!isStopRequested())
drive.followTrajectorySequence(traj);

//aprilTag.centerOnTag(cameras[1], drive, 5);

android.util.Log.i("PROGRESS", "whatttttt");
Log.i("PROGRESS", "whatttttt");
}
}

Expand Down

0 comments on commit 4b3fe78

Please sign in to comment.