today work

svhsrobotics · Dec 11, 2023 · f1d2ec7 · f1d2ec7
1 parent 4b3fe78
commit f1d2ec7
Show file tree

Hide file tree

Showing 4 changed files with 32 additions and 14 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TeleOp.java
@@ -25,7 +25,7 @@ public void runOpMode() throws InterruptedException {
         cameras[2] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Right"), 8, Math.toRadians(-70), Math.toRadians(45));
 
         waitForStart();
-
+//ryan is a silly goofy fella
 
         while (!isStopRequested()) {
             drive.update(); // MUST be called every loop cycle so that RoadRunner calculates the pose correctly

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestAuto.java
@@ -13,10 +13,12 @@
 import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
 import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.WaitSegment;
 import org.firstinspires.ftc.teamcode.util.GlobalOpMode;
+import org.firstinspires.ftc.teamcode.util.Timeout;
 import org.firstinspires.ftc.teamcode.vision.AprilTagCamera;
 import org.firstinspires.ftc.teamcode.vision.AprilTagLocalizer;
 import org.firstinspires.ftc.teamcode.vision.PropellerDetection1;
 import org.firstinspires.ftc.teamcode.vision.TensorFlowDetection;
+import org.tensorflow.lite.Tensor;
 
 @Autonomous
 @Config
@@ -37,41 +39,50 @@ private Pose2d estimateWithAllCameras(AprilTagCamera[] cameras, AprilTagLocalize
         return pose;
     }
 
+    public static double LEFTFORTYFIVE = -45;
+    public static double LEFTSEVENTY = 70;
+    public static double RIGHTFORTYFIVE = 45;
+    public static double RIGHTSEVENTY = -70;
+
     @Override
     public void runOpMode() throws InterruptedException {
         GlobalOpMode.opMode = this;
         TestBotDrive drive = new TestBotDrive(hardwareMap);
 
         AprilTagCamera[] cameras = new AprilTagCamera[3];
-        cameras[0] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Left"), 8, Math.toRadians(70), Math.toRadians(-45));
+        cameras[0] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Left"), 8, Math.toRadians(LEFTSEVENTY), Math.toRadians(LEFTFORTYFIVE));
         cameras[1] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Center"), 7, Math.toRadians(90), Math.toRadians(0));
-        cameras[2] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Right"), 8, Math.toRadians(-70), Math.toRadians(45));
+        cameras[2] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Right"), 8, Math.toRadians(RIGHTSEVENTY), Math.toRadians(RIGHTFORTYFIVE));
 
 
-        TensorFlowDetection tensor = new TensorFlowDetection(cameras[1].webcamName);
+        //TensorFlowDetection tensor = new TensorFlowDetection(cameras[1].webcamName);
+        TensorFlowDetection tensor = null;
         Log.i("PROGRESS", "I did this!");
 //        while (!isStopRequested()) {
 //            tensor.getPropPosition();
 //          android.util.Log.w("LOCATION", "tensor result " + tensor.getPropPosition());
 //        }
 
 
-        TensorFlowDetection.PropPosition tensorPos = tensor.getPropPosition();
+
 
 //        TensorFlowDetection.PropPosition tensorPos = TensorFlowDetection.PropPosition.LEFT;
-        Log.i("PROGRESS", "Made it here");
-        Log.i("PROGRESS", tensorPos + "");
-        Log.i("PROGRESS", tensor.getPropPosition() + "");
+
 
 
         AprilTagLocalizer aprilTag = new AprilTagLocalizer(cameras);
         Pose2d startPose = null;
         Log.i("PROGRESS", "here?");
-        while(opModeInInit() && !isStopRequested() && startPose == null) {
+        while(opModeInInit() && !isStopRequested() ) {
             Log.i("PROGRESS", "before");
             startPose = estimateWithAllCameras(cameras, aprilTag);
             Log.i("PROGRESS", "after");
+            if (startPose != null) {
+                drive.setPoseEstimate(startPose);
+                drive.update();
+            }
         }
+        waitForStart();
         Log.i("PROGRESS", "Maybe?");
         if (startPose == null) {
             // Check one more time
@@ -90,6 +101,11 @@ public void runOpMode() throws InterruptedException {
         drive.setPoseEstimate(startPose);
         Log.i("PROGRESS", "Im here too!");
 
+        TensorFlowDetection.PropPosition tensorPos = tensor.getPropPosition();
+        if (tensorPos == null) {
+            tensorPos = TensorFlowDetection.PropPosition.CENTER;
+        }
+
 
         switch (AprilTagLocalizer.whichQuadrant(startPose)) {
             case RED_BOARD:
@@ -285,7 +301,6 @@ public void runOpMode() throws InterruptedException {
             break;
         }
         Log.i("PROGRESS", "I got here???");
-        waitForStart();
 
         if (!isStopRequested())
             drive.followTrajectorySequence(traj);

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/AprilTagCamera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/AprilTagCamera.java
@@ -21,8 +21,8 @@ public AprilTagCamera(WebcamName webcamName, double offset, double orientation,
         this.angle = angle;
     }
 
-    public Pose2d translatePoseOrig(Pose2d inputPose) {
-        double thetaNeed = -orientation + (inputPose.getHeading() - Math.toRadians(90)) + angle;
+    public Pose2d translatePose(Pose2d inputPose) {
+        double thetaNeed = Math.toRadians(180) - angle - inputPose.getHeading() + orientation;
         android.util.Log.e("APRILTAG", "MAGIC ANGLE: " + Math.toDegrees(thetaNeed));
 
         double offsetX = offset * Math.cos(thetaNeed);
@@ -44,7 +44,7 @@ public Pose2d translatePoseOrig(Pose2d inputPose) {
      * @param inputPose Pose2d to translate
      * @return translated Pose2d
      */
-    public Pose2d translatePose(Pose2d inputPose) {
+    public Pose2d translatePoseFudged(Pose2d inputPose) {
         // 6.5: Distance from left camera to center of robot
         // -6.5: Distance from right camera to center of robot
         // 5: distance from center of robot to cameras (forward)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/TensorFlowDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/TensorFlowDetection.java
@@ -6,6 +6,7 @@
 import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
 import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
 import org.firstinspires.ftc.teamcode.util.GlobalOpMode;
+import org.firstinspires.ftc.teamcode.util.Timeout;
 import org.firstinspires.ftc.vision.VisionPortal;
 import org.firstinspires.ftc.vision.tfod.TfodProcessor;
 
@@ -105,6 +106,8 @@ public enum PropPosition{
         LEFT, RIGHT, CENTER
     }
 
+
+
     public PropPosition getPropPosition() {
         List<Recognition> currentRecognitions = tfod.getRecognitions();
         while(currentRecognitions.size() < 1 && !GlobalOpMode.opMode.isStopRequested()) {
@@ -122,7 +125,7 @@ public PropPosition getPropPosition() {
 
         // GET MORE CURRENT RECOGNITIONS?
 //        }
-        visionPortal.close();
+        //visionPortal.close();
 
         double centerX = (recognition.getLeft() + recognition.getRight()) / 2 ;
         if (centerX < 214) {