april tag for cameras 1 and 2 progress

svhsrobotics · Nov 2, 2023 · 373c120 · 373c120
1 parent d6a269f
commit 373c120
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 24 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ApriltagTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ApriltagTest.java
@@ -53,6 +53,12 @@ private double quadrantTransform(double angle) {
      * @param cameraIndex the index of the camera to switch to
      */
     private void switchCamera(int cameraIndex) {
+        // Wait for the visionPortal status to be STREAMING before we attempt to switch cameras
+        android.util.Log.i("AprilTag Test", "Waiting for camera to start STREAMING before switching cameras");
+        while (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING) {
+            sleep(10);
+        }
+        android.util.Log.i("AprilTag Test", "Camera is STREAMING, switching cameras...");
         // Get the current camera
         CameraName currentCamera = visionPortal.getActiveCamera();
         // Find the index of the current camera in the array
@@ -134,27 +140,26 @@ public void runOpMode() throws InterruptedException {
                 .build();
 
         android.util.Log.i("AprilTag Test", "Waiting for start...");
+        switchCamera(1);
         waitForStart();
 
         while(opModeIsActive()) {
-            Pose2d cameraPose = estimateCameraPoseFromAprilTags(0);
+            Pose2d cameraPose = estimateCameraPoseFromAprilTags(1);
 
             if (cameraPose != null) {
                 android.util.Log.i("AprilTag Test", "Camera pose: " + cameraPose);
 
-                double CAMERA_OFFSET = 6; // Center camera
-                // TODO: Use camera angle, 0 for center camera
-                //double theta = Math.toRadians(quadrantTransform(Math.toDegrees(cameraPose.getHeading())));
-                double theta = cameraPose.getHeading();
+                double CAMERA_OFFSET = 10; // Center camera
+                double CAMERA_ANGLE = Math.toRadians(50);
 
-                double offsetX = CAMERA_OFFSET * Math.cos(theta);
-                double offsetY = CAMERA_OFFSET * Math.sin(theta);
+                double offsetX = CAMERA_OFFSET * Math.cos(cameraPose.getHeading());
+                double offsetY = CAMERA_OFFSET * Math.sin(cameraPose.getHeading());
 
                 android.util.Log.i("AprilTag Test", "Camera offset: " + offsetX + ", " + offsetY);
 
                 double robotX = cameraPose.getX() - offsetX;
-                double robotY = cameraPose.getY() + offsetY;
-                double robotHeading = cameraPose.getHeading();
+                double robotY = cameraPose.getY() - offsetY;
+                double robotHeading = cameraPose.getHeading() - CAMERA_ANGLE;
 
                 Pose2d robotPose = new Pose2d(robotX, robotY, robotHeading);
 

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/RedLeftAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/RedLeftAuto.java
@@ -21,30 +21,30 @@ public void runOpMode() throws InterruptedException {
         TrajectorySequence autoDrive = drive.trajectorySequenceBuilder(startPose)
 
                 //relativistic driving that also includes turning to simulate april tags recognition
-                .forward(24)
-                .waitSeconds(1)
-                .turn(Math.toRadians(90))
-                .waitSeconds(3)
-                .turn(Math.toRadians(-180))
-                .forward(80)
-                        .build();
+//                .forward(24)
+//                .waitSeconds(1)
+//                .turn(Math.toRadians(90))
+//                .waitSeconds(3)
+//                .turn(Math.toRadians(-180))
+//                .forward(80)
+//                .build();
+
 
 
 
 
-           /*
            //coordinate based driving
                 .lineTo(new Vector2d(-34, -36))
                 .strafeTo(new Vector2d(48, -36))
                 .build();
-           */
-           /*
+
+
            // also coordinate but spline and goes to corner instead of right in front of board
-              .splineTo(new Vector2d(-34, -30), Math.toRadians(90))
-                .splineTo(new Vector2d(34, -30), Math.toRadians(-90))
-                .splineTo(new Vector2d(56, -56),Math.toRadians(0) )
-                .build();
-           */
+//              .splineTo(new Vector2d(-34, -30), Math.toRadians(90))
+//                .splineTo(new Vector2d(34, -30), Math.toRadians(-90))
+//                .splineTo(new Vector2d(56, -56),Math.toRadians(0) )
+//                .build();
+//
 
         waitForStart();