To be tested

FtcRobotController/build/generated/source/buildConfig/debug/com/qualcomm/ftcrobotcontroller/BuildConfig.java

@@ -8,5 +8,5 @@ public final class BuildConfig {
   public static final String LIBRARY_PACKAGE_NAME = "com.qualcomm.ftcrobotcontroller";
   public static final String BUILD_TYPE = "debug";
   // Field from default config.
-  public static final String APP_BUILD_TIME = "2023-11-06T13:40:01.464-0600";
+  public static final String APP_BUILD_TIME = "2023-11-15T14:04:54.790-0600";
 }

TeamCode/build/intermediates/apk/debug/output-metadata.json

@@ -0,0 +1,20 @@
+{
+  "version": 3,
+  "artifactType": {
+    "type": "APK",
+    "kind": "Directory"
+  },
+  "applicationId": "com.qualcomm.ftcrobotcontroller",
+  "variantName": "debug",
+  "elements": [
+    {
+      "type": "SINGLE",
+      "filters": [],
+      "attributes": [],
+      "versionCode": 51,
+      "versionName": "9.0",
+      "outputFile": "TeamCode-debug.apk"
+    }
+  ],
+  "elementType": "File"
+}

TeamCode/build/intermediates/apk_ide_redirect_file/debug/redirect.txt

@@ -1,2 +1,2 @@
 #- File Locator -
-listingFile=../../../outputs/apk/debug/output-metadata.json
+listingFile=../../apk/debug/output-metadata.json

TeamCode/build/intermediates/incremental/packageDebug/tmp/debug/dex-renamer-state.txt

@@ -1,4 +1,4 @@
-#Mon Nov 06 13:40:44 CST 2023
+#Wed Nov 15 14:04:57 CST 2023
 base.0=/Users/pandeys01/Developer/2867-CenterStage/TeamCode/build/intermediates/dex/debug/mergeExtDexDebug/classes.dex
 base.1=/Users/pandeys01/Developer/2867-CenterStage/TeamCode/build/intermediates/dex/debug/mergeLibDexDebug/10/classes.dex
 base.2=/Users/pandeys01/Developer/2867-CenterStage/TeamCode/build/intermediates/dex/debug/mergeLibDexDebug/11/classes.dex

TeamCode/build/intermediates/merged_manifest/debug/AndroidManifest.xml

@@ -106,6 +106,7 @@
         android:icon="@drawable/ic_launcher"
         android:label="@string/app_name"
         android:largeHeap="true"
+        android:testOnly="true"
         android:theme="@style/AppThemeRedRC"
         android:usesCleartextTraffic="true" > <!-- Indicates to the ControlHubUpdater what the latest version of the Control Hub is that this app supports -->
         <meta-data

TeamCode/build/intermediates/merged_manifests/debug/AndroidManifest.xml

@@ -106,6 +106,7 @@
         android:icon="@drawable/ic_launcher"
         android:label="@string/app_name"
         android:largeHeap="true"
+        android:testOnly="true"
         android:theme="@style/AppThemeRedRC"
         android:usesCleartextTraffic="true" > <!-- Indicates to the ControlHubUpdater what the latest version of the Control Hub is that this app supports -->
         <meta-data

TeamCode/build/intermediates/packaged_manifests/debug/AndroidManifest.xml

@@ -106,6 +106,7 @@
         android:icon="@drawable/ic_launcher"
         android:label="@string/app_name"
         android:largeHeap="true"
+        android:testOnly="true"
         android:theme="@style/AppThemeRedRC"
         android:usesCleartextTraffic="true" > <!-- Indicates to the ControlHubUpdater what the latest version of the Control Hub is that this app supports -->
         <meta-data

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/BlueBackAutonomous.java

@@ -0,0 +1,17 @@
+package org.firstinspires.ftc.teamcode.autonomous;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.enums.StartPosition;
+
+
+@Autonomous(name="Blue Backstage Auto", group="Autonomous")
+
+public class BlueBackAutonomous extends LinearOpMode{
+    @Override
+    public void runOpMode() throws InterruptedException{
+        waitForStart();
+        FirstCenterStage.run(this, StartPosition.BACKSTAGE_BLUE);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/FirstCenterStage.java

@@ -4,29 +4,56 @@
 import com.qualcomm.hardware.bosch.BNO055IMU;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.IMU;
 import com.qualcomm.robotcore.hardware.Servo;
 
 //import org.firstinspires.ftc.teamcode.enums.AllianceSide;
 //import org.firstinspires.ftc.teamcode.enums.ColorSide;
+import org.firstinspires.ftc.teamcode.dependencies.MecanumEncoder;
+import org.firstinspires.ftc.teamcode.dependencies.PositionVector;
 import org.firstinspires.ftc.teamcode.enums.Direction;
+import org.firstinspires.ftc.teamcode.enums.StartPosition;
 import org.firstinspires.ftc.teamcode.dependencies.RobotParameters;
 //import org.firstinspires.ftc.teamcode.enums.Parking;
 
 public class FirstCenterStage {
     private static DcMotor motorFrontLeft, motorBackLeft, motorFrontRight, motorBackRight, linearSlide;
     private static Servo claw;
 
-    private static BNO055IMU imu;
-
-    public static void run(LinearOpMode linearOpMode) {
-        motorFrontLeft = linearOpMode.hardwareMap.dcMotor.get("frontLeft");
-        motorBackLeft = linearOpMode.hardwareMap.dcMotor.get("backLeft");
-        motorFrontRight = linearOpMode.hardwareMap.dcMotor.get("frontRight");
-        motorBackRight = linearOpMode.hardwareMap.dcMotor.get("backRight");
-        imu = linearOpMode.hardwareMap.get(BNO055IMU.class, "imu");
+    private static IMU imu;
 
+    public static void run(LinearOpMode linearOpMode, StartPosition startPosition) {
+        motorFrontLeft = linearOpMode.hardwareMap.dcMotor.get("frontRight");
+        motorBackLeft = linearOpMode.hardwareMap.dcMotor.get("backRight");
+        motorFrontRight = linearOpMode.hardwareMap.dcMotor.get("frontLeft");
+        motorBackRight = linearOpMode.hardwareMap.dcMotor.get("backLeft");
+        IMU imu = linearOpMode.hardwareMap.get(IMU.class, "imu");
         motorFrontLeft.setDirection(DcMotor.Direction.REVERSE);
-        motorBackLeft.setDirection(DcMotor.Direction.REVERSE);
+        motorBackRight.setDirection(DcMotor.Direction.REVERSE);
         RobotParameters rP = new RobotParameters(motorFrontLeft, motorFrontRight, motorBackLeft, motorBackRight, 28, 7.5, 60, imu, 10.5);
+        MecanumEncoder autoEncoder = new MecanumEncoder(rP, linearOpMode);
+//        autoEncoder.newRotateDegrees(40,0.5, Direction.CW);
+//        autoEncoder.rotateDegrees(Direction.CW,40, 0.5);
+//        autoEncoder.travelTo(new PositionVector(24, 24, 60), 0.6, 0.8);
+
+        // The code below is commented for now so that the code above can be tested without complications
+
+        switch (startPosition) { // Remember that the collection side (no crossbars) is the "back"
+            case FRONT:
+                autoEncoder.moveInches(Direction.BACKWARD, 31, 1);
+                autoEncoder.moveInches(Direction.FORWARD, 8, 1);
+                break;
+            case BACKSTAGE_RED: // temporarily set to push 2 pixels to the score zone and park
+                autoEncoder.moveInches(Direction.BACKWARD, 41, 1);
+                autoEncoder.moveInches(Direction.FORWARD, 8, 1);
+                break;
+            case BACKSTAGE_BLUE: // same as above just to mitigate confusion
+                autoEncoder.moveInches(Direction.BACKWARD, 41, 1);
+                autoEncoder.moveInches(Direction.FORWARD, 8, 1);
+                break;
+            default:
+                throw new IllegalArgumentException("StartPosition must be FRONT, BACKSTAGE_BLUE, or BACKSTAGE_RED");
+        }
+
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/TestAutonomous.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/FrontAutonomous.java

@@ -4,12 +4,15 @@
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
+import org.firstinspires.ftc.teamcode.enums.StartPosition;
 
-@Autonomous(name="TestAutonomous", group="Autonomous")
 
-public class TestAutonomous extends LinearOpMode{
+@Autonomous(name="Front Auto", group="Autonomous")
+
+public class FrontAutonomous extends LinearOpMode{
     @Override
     public void runOpMode() throws InterruptedException{
-        FirstCenterStage.run(this);
+        waitForStart();
+        FirstCenterStage.run(this, StartPosition.FRONT);
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/RedBackAutonomous.java

@@ -0,0 +1,17 @@
+package org.firstinspires.ftc.teamcode.autonomous;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.enums.StartPosition;
+
+
+@Autonomous(name="Red Backstage Auto", group="Autonomous")
+
+public class RedBackAutonomous extends LinearOpMode{
+    @Override
+    public void runOpMode() throws InterruptedException{
+        waitForStart();
+        FirstCenterStage.run(this, StartPosition.BACKSTAGE_RED);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/dependencies/AprilTagDetectionPipeline.java

@@ -33,11 +33,12 @@
 import org.opencv.imgproc.Imgproc;
 import org.openftc.apriltag.AprilTagDetection;
 import org.openftc.apriltag.AprilTagDetectorJNI;
+import org.openftc.apriltag.AprilTagPose;
 import org.openftc.easyopencv.OpenCvPipeline;
 
 import java.util.ArrayList;
 
-class AprilTagDetectionPipeline extends OpenCvPipeline
+public class AprilTagDetectionPipeline extends OpenCvPipeline
 {
     private long nativeApriltagPtr;
     private Mat grey = new Mat();
@@ -122,11 +123,11 @@ public Mat processFrame(Mat input)
             detectionsUpdate = detections;
         }
 
-        // For fun, use OpenCV to draw 6DOF markers on the image. We actually recompute the pose using
-        // OpenCV because I haven't yet figured out how to re-use AprilTag's pose in OpenCV.
+        // For fun, use OpenCV to draw 6DOF markers on the image.
         for(AprilTagDetection detection : detections)
         {
-            Pose pose = poseFromTrapezoid(detection.corners, cameraMatrix, tagsizeX, tagsizeY);
+            Pose pose = aprilTagPoseToOpenCvPose(detection.pose);
+            //Pose pose = poseFromTrapezoid(detection.corners, cameraMatrix, tagsizeX, tagsizeY);
             drawAxisMarker(input, tagsizeY/2.0, 6, pose.rvec, pose.tvec, cameraMatrix);
             draw3dCubeMarker(input, tagsizeX, tagsizeX, tagsizeY, 5, pose.rvec, pose.tvec, cameraMatrix);
         }
@@ -258,6 +259,28 @@ void draw3dCubeMarker(Mat buf, double length, double tagWidth, double tagHeight,
         Imgproc.line(buf, projectedPoints[4], projectedPoints[7], green, thickness);
     }
 
+    Pose aprilTagPoseToOpenCvPose(AprilTagPose aprilTagPose)
+    {
+        Pose pose = new Pose();
+        pose.tvec.put(0,0, aprilTagPose.x);
+        pose.tvec.put(1,0, aprilTagPose.y);
+        pose.tvec.put(2,0, aprilTagPose.z);
+
+        Mat R = new Mat(3, 3, CvType.CV_32F);
+
+        for (int i = 0; i < 3; i++)
+        {
+            for (int j = 0; j < 3; j++)
+            {
+                R.put(i,j, aprilTagPose.R.get(i,j));
+            }
+        }
+
+        Calib3d.Rodrigues(R, pose.rvec);
+
+        return pose;
+    }
+
     /**
      * Extracts 6DOF pose from a trapezoid, using a camera intrinsics matrix and the
      * original size of the tag.
@@ -299,8 +322,8 @@ class Pose
 
         public Pose()
         {
-            rvec = new Mat();
-            tvec = new Mat();
+            rvec = new Mat(3, 1, CvType.CV_32F);
+            tvec = new Mat(3, 1, CvType.CV_32F);
         }
 
         public Pose(Mat rvec, Mat tvec)