refining auto

svhsrobotics · Mar 4, 2024 · 04e44ae · 04e44ae
1 parent c04a4a1
commit 04e44ae
Show file tree

Hide file tree

Showing 8 changed files with 38 additions and 28 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/RoboticaBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/RoboticaBot.java
@@ -215,7 +215,7 @@ public AprilTagCamera getPrimaryCamera() {
     @Override
     public void dropPurplePixel(boolean state) {
         if (state) {
-            purpleServo.setPosition(1);
+            purpleServo.setPosition(.8);
         } else {
             purpleServo.setPosition(0.5);
         }
@@ -245,7 +245,7 @@ public TrajectoryDrive getDrive() {
 
     public void recalibrateShoulder() {
         // Raise the arm up
-        shoulderMotor.setTargetPosition(700);
+        shoulderMotor.setTargetPosition(1200);
         shoulderMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
         shoulderMotor.setPower(1);
         while (shoulderMotor.isBusy() && !GlobalOpMode.opMode.isStopRequested()) {}

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
@@ -1,5 +1,6 @@
 package org.firstinspires.ftc.teamcode.opmode;
 
+import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@@ -24,6 +25,7 @@
 import java.util.Objects;
 
 @Autonomous
+@Config
 public class TestAuto extends LinearOpMode {
     private Pose2d estimateWithAllCameras(AprilTagCamera[] cameras, AprilTagLocalizer aprilTag) {
         Pose2d pose = null;
@@ -35,7 +37,7 @@ private Pose2d estimateWithAllCameras(AprilTagCamera[] cameras, AprilTagLocalize
         }
         return pose;
     }
-
+    public static TensorFlowDetection.PropPosition TENSORFLOWDEF = TensorFlowDetection.PropPosition.RIGHT;
     private TensorFlowDetection.PropPosition detectProp(Robot robot, int timeout) {
         if (robot.getClass() == PsiBot.class) {
             ((PsiBot) robot).armMotor.setTargetPosition(-109);
@@ -46,7 +48,7 @@ private TensorFlowDetection.PropPosition detectProp(Robot robot, int timeout) {
         TensorFlowDetection.PropPosition position = tensor.getPropPosition(new Timeout(timeout));
         if (position == null) {
             telemetry.log().add("Unable to detect prop, using CENTER");
-            position = TensorFlowDetection.PropPosition.CENTER;
+            position = TENSORFLOWDEF;
         }
         return position;
     }

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestTeleOp.java
@@ -217,7 +217,7 @@ public void runOpMode() throws InterruptedException {
 
     public static int RAISED_ARM = 1855;
     public static double RAISED_ELBOW = 0.272;
-    public static double RAISED_WRIST = 0.456;
+    public static double RAISED_WRIST = 0.544;
 
     public static int NEUTRAL_ARM = 113;
     public static double NEUTRAL_ELBOW = 0.248;

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/GoToBoard.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/GoToBoard.java
@@ -32,18 +32,20 @@ public void drive() {
 
         trajB.lineTo(new Vector2d(startPose.getX(), y))
             .lineTo(new Vector2d(fi(3, 10), y))
-                .turnTo(Math.toRadians(0))
+                .turnTo(Math.toRadians(180))
             //.turn(Math.toRadians(90))
             .lineTo(new Vector2d(fi(3,8), fi(-2,11)));
 
         TrajectorySequence traj = trajB.build();
 
         getRobot().getDrive().followTrajectorySequence(traj);
 
-        if (getRobot().getClass() == RoboticaBot.class) {
-            ((RoboticaBot) getRobot()).recalibrateShoulder();
-            ((RoboticaBot) getRobot()).setShoulderTargetPosition(TestTeleOp.RAISED_ARM);
-        }
+//        if (getRobot().getClass() == RoboticaBot.class) {
+//            ((RoboticaBot) getRobot()).recalibrateShoulder();
+//            ((RoboticaBot) getRobot()).setShoulderTargetPosition(TestTeleOp.RAISED_ARM);
+//            ((RoboticaBot) getRobot()).elbowServo.setPosition(TestTeleOp.RAISED_ELBOW);
+//            ((RoboticaBot) getRobot()).wristLiftServo.setPosition(TestTeleOp.RAISED_WRIST);
+//        }
 
         //GlobalOpMode.opMode.sleep(2000);
 

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/ParkingIn.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/ParkingIn.java
@@ -24,7 +24,8 @@ public void drive() {
         int y = (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_BOARD) || (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_AUDIENCE) ? fi(0,10) : fi(0,-10);
 
         trajB.lineTo(new Vector2d(currentPose.getX(), y));
-        trajB.lineTo(new Vector2d(fi(4,6), y));
+        trajB.lineTo(new Vector2d(fi(4,0), y));
+        trajB.turnTo(Math.toRadians(180));
 
         TrajectorySequence traj = trajB.build();
         getRobot().getDrive().followTrajectorySequence(traj);

diff --git a/.../src/main/java/org/firstinspires/ftc/teamcode/opmode/components/PurplePixelComponent.java b/.../src/main/java/org/firstinspires/ftc/teamcode/opmode/components/PurplePixelComponent.java
@@ -54,20 +54,21 @@ public void drive() {
 
         if (propPosition == TensorFlowDetection.PropPosition.LEFT) {
             // Special hack since our dropper is off-center
-            if (getRobot().getClass() == RoboticaBot.class && (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_BOARD || getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_AUDIENCE)) {
-                trajB.turn(Math.toRadians(45))
-                        .forward(12 * driveBackwards);
-            } if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_AUDIENCE) {
-                trajB.forward(9 * driveBackwards)
-                        .turn(Math.toRadians(90))
-                        .forward(8 * driveBackwards);
-            } else {
-                trajB.turn(Math.toRadians(90))
-                        .forward(8 * driveBackwards); // orig 13
-            }
+            //if (getRobot().getClass() == RoboticaBot.class && (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_BOARD || getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_AUDIENCE)) {
+            trajB.back(6 * driveBackwards)
+                    .turn(Math.toRadians(45))
+                    .forward(14 * driveBackwards);
+//            } if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_AUDIENCE) {
+//                trajB.forward(9 * driveBackwards)
+//                        .turn(Math.toRadians(90))
+//                        .forward(8 * driveBackwards);
+//            } else {
+//                trajB.turn(Math.toRadians(90))
+//                        .forward(8 * driveBackwards); // orig 13
+//            }
         } else if (propPosition == TensorFlowDetection.PropPosition.RIGHT) {
             trajB.turn(Math.toRadians(-90))
-                    .forward(10 * driveBackwards);
+                    .forward(7 * driveBackwards);
         } else {
             if (moveTowardsCenter) {
                 trajB.turn(Math.toRadians(180))
@@ -83,11 +84,12 @@ public void drive() {
                 .waitSeconds(0.5);
 
         //if (propPosition == TensorFlowDetection.PropPosition.CENTER && (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_AUDIENCE || getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_AUDIENCE )) {
-        if (propPosition == TensorFlowDetection.PropPosition.LEFT && getRobot().getClass() == RoboticaBot.class && getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_BOARD) {
-            trajB.forward(-14 * driveBackwards)
+        if (propPosition == TensorFlowDetection.PropPosition.LEFT/* && getRobot().getClass() == RoboticaBot.class && getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_BOARD*/) {
+            trajB.forward(-16 * driveBackwards)
                     .turn(Math.toRadians(-45));
         }
         if (moveTowardsCenter && propPosition == TensorFlowDetection.PropPosition.CENTER) {
+            // be careful with relative forwards, this wouldn't work for RIGHT etc. since we never un-turn
             trajB.forward(-14 * driveBackwards);
         } else {
             trajB.lineTo(currentTapeMarks());

diff --git a/...ain/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceBuilder.java b/...ain/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceBuilder.java
@@ -462,9 +462,12 @@ public TrajectorySequenceBuilder turnTo(double angle) {
     }
 
     public TrajectorySequenceBuilder turnTo(double angle, double maxAngVel, double maxAngAccel) {
-        return turn(angle - lastPose.getHeading(), maxAngVel, maxAngAccel);
+        if (lastPose.getHeading() > Math.toRadians(-180) && lastPose.getHeading() < Math.toRadians(180)) {
+            return turn(angle - lastPose.getHeading(), maxAngVel, maxAngAccel);
+        } else {
+            return turn(angle + (2*Math.PI) - lastPose.getHeading(), maxAngVel, maxAngAccel);
+        }
     }
-
     public TrajectorySequenceBuilder waitSeconds(double seconds) {
         pushPath();
         sequenceSegments.add(new WaitSegment(lastPose, seconds, Collections.emptyList()));

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/AprilTagLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/AprilTagLocalizer.java
@@ -81,7 +81,7 @@ private Pose2d estimateCameraPoseFromAprilTags(AprilTagCamera camera) {
         List<AprilTagDetection> detections = aprilTag.getFreshDetections();
         // Wait for detections to not be null
         // TODO: Timeout
-        while (detections == null) {
+        while (detections == null && !GlobalOpMode.opMode.isStopRequested()) {
             //android.util.Log.i("APRILTAG", "Waiting for non null return");
             detections = aprilTag.getFreshDetections();
         }