auto Drive stuff and such and such

TeamCode/src/main/java/opmodes/MainTeleOp.java

@@ -1,7 +1,10 @@
 package opmodes;
 
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_ARM_DELTA;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.FORWARD_OFFSET_IN;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MAX;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SCORING_DISTANCE_FROM_APRIL_TAG;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SIDE_OFFSET_IN;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SLIDE_UP;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_CENTER;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_MAX;
@@ -9,9 +12,11 @@ import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_MIN;
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
+import org.firstinspires.ftc.teamcode.hardware.Camera;
 import org.firstinspires.ftc.teamcode.hardware.Robot;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
 
@@ -28,7 +33,9 @@ public class MainTeleOp extends OpMode {
     private boolean liftArmShouldBeUp = false;
     private boolean screwArmIsMoving = false;
     private FtcDashboard dashboard;
-
+    private boolean leftWasPressed;
+    private boolean rightWasPressed;
+    private boolean isGantryFlipped = false;
     @Override
     public void init() {
         this.dashboard = FtcDashboard.getInstance();
@@ -45,12 +52,14 @@ public class MainTeleOp extends OpMode {
         this.robot.getDrive().setInput(gamepad1, gamepad2, slowmode);
 
         // Button Mappings
+        // Drive
+        boolean leftPressed = gamepad1.dpad_left;
+        boolean rightPressed = gamepad1.dpad_right;
         // Claw / Pickup
         boolean openClaw = gamepad2.b; // B
         boolean clawUp = gamepad2.y; // Y
         boolean clawDownSafe = gamepad2.dpad_down; // dpad-down
         boolean clawDown = gamepad2.a || clawDownSafe; // A
-
         // Robot Drone Launch
         boolean robotDroneLaunch = gamepad1.left_bumper && gamepad1.right_bumper; // Change if Merck wants (LT)
 
@@ -71,7 +80,7 @@ public class MainTeleOp extends OpMode {
         if (openClaw) {
             this.robot.getClaw().open();
             this.screwArmIsMoving = false;
-        } else if (!clawUp && !clawDown && !this.screwArmIsMoving){
+        } else if (!clawUp && !clawDown && !this.screwArmIsMoving) {
             this.robot.getClaw().close();
         }
         if (clawUp) {
@@ -88,13 +97,15 @@ public class MainTeleOp extends OpMode {
         }
 
         // Gantry
-        if (!previousScrewArmToggle && screwArmToggle) {
+        if (!previousScrewArmToggle && screwArmToggle  ) {
             this.screwArmIsMoving = true;
             if (screwArmPos) {
                 this.robot.getGantry().armIn();
+                this.isGantryFlipped = false;
             } else {
                 this.robot.getClaw().open();
                 this.robot.getGantry().armOut();
+                this.isGantryFlipped = true;
             }
             this.robot.getClaw().open();
             screwArmPos = !screwArmPos;
@@ -107,17 +118,22 @@ public class MainTeleOp extends OpMode {
             this.robot.getGantry().stop();
         }
 
-        if (slideUp) {
+        if (slideUp && isGantryFlipped) {
             this.robot.getGantry().setSlideTarget(SLIDE_UP);
         } else if (slideDown) {
             this.robot.getGantry().setSlideTarget(0);
-        } else if (previousSlideUp || previousSlideDown){
+        } else if (previousSlideUp || previousSlideDown) {
             this.robot.getGantry().setSlideTarget(this.robot.getGantry().getSlidePosition());
-        }
+        } else if (!isGantryFlipped)
+            this.telemetry.addData("GantryNotFlipped!", "Null");
 
+        if (isGantryFlipped) {
             double gantryXPosition = X_CENTER + (gantryXInput * 0.5);
             gantryXPosition = Math.max(X_MIN, Math.min(gantryXPosition, X_MAX));
             this.robot.getGantry().setX(gantryXPosition);
+        } else {
+            this.telemetry.addData("GantryNotFlippedGoober", "Null");
+        }
 
         // Robot Drone
 
@@ -151,6 +167,22 @@ public class MainTeleOp extends OpMode {
             this.robot.getLift().stopReset();
         }
 
+        Vector2d poseFromAprilTag = this.robot.getCamera().getPoseFromAprilTag(2, 5);
+
+        if (poseFromAprilTag != null) {
+            if (leftPressed && !leftWasPressed) {
+                macroToScore(poseFromAprilTag, true);
+            } else if (rightPressed && !rightWasPressed) {
+                macroToScore(poseFromAprilTag, false);
+            }
+        }
+
+        if (!leftPressed && !rightPressed) { // breaks drive as makes it so it continuously brakes, added !this.robot.getDrive().isBusy just for placement
+            this.robot.getDrive().breakFollowing();
+        }
+
+        this.leftWasPressed = leftPressed;
+        this.rightWasPressed = rightPressed;
 
         this.previousSlideUp = slideUp;
         this.previousScrewArmToggle = screwArmToggle;
@@ -159,4 +191,18 @@ public class MainTeleOp extends OpMode {
 
         this.robot.update();
     }
+
+    private void macroToScore(Vector2d poseFromAprilTag, boolean left) {
+        Pose2d target;  // defines a new pose2d named target, position not yet given
+        Pose2d poseEstimate = new Pose2d(poseFromAprilTag.getX(), poseFromAprilTag.getY(), this.robot.getDrive().getPoseEstimate().getHeading());
+        double y = poseEstimate.getY() > 0
+                ? left ? 40 : 30
+                : left ? -30 : -40;
+        this.robot.getDrive().setPoseEstimate(poseEstimate);
+        target = new Pose2d(Camera.tag2Pose.getX() - SCORING_DISTANCE_FROM_APRIL_TAG - FORWARD_OFFSET_IN, y - SIDE_OFFSET_IN, 0);
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(target);
+        this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
+    }
+
 }

TeamCode/src/main/java/opmodes/Test.java

@@ -0,0 +1,97 @@
+package opmodes;
+
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.FORWARD_OFFSET_IN;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SCORING_DISTANCE_FROM_APRIL_TAG;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SIDE_OFFSET_IN;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.hardware.Camera;
+import org.firstinspires.ftc.teamcode.hardware.Robot;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
+import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
+
+@TeleOp(name = "Test", group = "Main")
+public class Test extends OpMode {
+    private Robot robot;
+
+    private FtcDashboard dashboard;
+    private boolean leftWasPressed;
+    private boolean rightWasPressed;
+    private int targetTagId;
+
+    @Override
+    public void init() {
+        this.dashboard = FtcDashboard.getInstance();
+
+        this.robot = new Robot(this.hardwareMap, telemetry);
+        telemetry.addData("Status", "Initialized");
+    }
+
+    @Override
+    public void loop() {
+        robot.update();
+
+        Vector2d poseFromAprilTag = this.robot.getCamera().getPoseFromAprilTag(2, 5);
+        dashboard.getTelemetry().addData("Inferred Position", poseFromAprilTag);
+        dashboard.getTelemetry().update();
+
+        boolean leftPressed = gamepad1.dpad_left;
+        boolean rightPressed = gamepad1.dpad_right;
+        if (poseFromAprilTag != null) {
+            if (leftPressed && !leftWasPressed) {
+                macroToScore(poseFromAprilTag, true);
+            } else if (rightPressed && !rightWasPressed) {
+                macroToScore(poseFromAprilTag, false);
+            }
+        } else {
+            if (leftPressed && !leftWasPressed || rightPressed && !rightWasPressed) {
+                moveToStartSquare();
+            }
+        }
+
+        if (!leftPressed && !rightPressed) {
+            this.robot.getDrive().breakFollowing();
+        }
+
+        leftWasPressed = leftPressed;
+        rightWasPressed = rightPressed;
+    }
+
+    private void moveToStartSquare() {
+        if (this.robot.getDrive().isBusy()) {
+            return;
+        }
+
+        Pose2d currentPoseEstimate = this.robot.getDrive().getPoseEstimate();
+        if (currentPoseEstimate.getX() < 0) {
+            return;
+        }
+
+        this.targetTagId = currentPoseEstimate.getY() >= 0 ? 2 : 5;
+
+        double y = targetTagId == 2 ? 36f : -36f;
+
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(new Pose2d(36, y, 0));
+        this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
+    }
+
+    private void macroToScore(Vector2d poseFromAprilTag, boolean left) {
+        Pose2d target;
+        Pose2d poseEstimate = new Pose2d(poseFromAprilTag.getX(), poseFromAprilTag.getY(), this.robot.getDrive().getPoseEstimate().getHeading());
+        double y = poseEstimate.getY() > 0
+                ? left ? 40 : 30
+                : left ? -30 : -40;
+        this.robot.getDrive().setPoseEstimate(poseEstimate);
+        target = new Pose2d(Camera.tag2Pose.getX() - SCORING_DISTANCE_FROM_APRIL_TAG - FORWARD_OFFSET_IN, y - SIDE_OFFSET_IN, 0);
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(target);
+        this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java

@@ -1,20 +1,32 @@
 package org.firstinspires.ftc.teamcode.hardware;
 
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.FORWARD_OFFSET_IN;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SIDE_OFFSET_IN;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.WEBCAM_NAME;
 import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_DETECTION;
 
+import android.hardware.GeomagneticField;
+
+import androidx.annotation.NonNull;
+
 import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.StandardTrackingWheelLocalizer;
 import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
 import org.firstinspires.ftc.teamcode.vision.Detection;
 import org.firstinspires.ftc.teamcode.vision.PropDetectionPipeline;
 import org.firstinspires.ftc.vision.VisionPortal;
 import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
+import org.firstinspires.ftc.vision.apriltag.AprilTagPoseFtc;
 import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
-import org.opencv.core.Point;
+import org.jetbrains.annotations.NotNull;
+
+import java.util.Arrays;
 
 @Config
 public class Camera {
@@ -24,7 +36,8 @@ public class Camera {
     private AprilTagProcessor aprilTag;
     private VisionPortal visionPortal;
     private Telemetry telemetry;
-
+    public static final Vector2d tag2Pose = new Vector2d(60, 36);
+    public static final Vector2d tag5Pose = new Vector2d(60, -36);
     private boolean initialized;
 
     public Camera(HardwareMap hardwareMap, Telemetry telemetry) {
@@ -62,10 +75,10 @@ public class Camera {
         return INVALID_DETECTION;
     }
 
-    public AprilTagDetection getAprilTag(int id) {
+    public AprilTagDetection getAprilTag(int ... ids) {
         return this.aprilTag.getDetections()
                 .stream()
-                .filter(x -> x.id == id)
+                .filter(x -> Arrays.stream(ids).filter(id -> x.id == id).count() > 0)
                 .findFirst()
                 .orElse(null);
     }
@@ -90,4 +103,37 @@ public class Camera {
     public CenterStageCommon.Alliance getAlliance() {
         return this.prop != null ? this.prop.getAlliance() : null;
     }
+
+    public Vector2d getPoseFromAprilTag(int ... ids) {
+        if (ids == null || ids.length == 0) {
+            ids = new int[]{2, 5};
+        }
+
+        AprilTagDetection aprilTagDetection = getAprilTag(ids);
+
+        if (aprilTagDetection == null) {
+            return null;
+        }
+
+        AprilTagPoseFtc ftcPose = aprilTagDetection.ftcPose;
+
+        double ourPoseX;
+        double ourPoseY;
+        switch (aprilTagDetection.id) {
+            case 2:
+                ourPoseX = tag2Pose.getX() - ftcPose.y - FORWARD_OFFSET_IN;
+                ourPoseY = tag2Pose.getY() - ftcPose.x - SIDE_OFFSET_IN;
+                break;
+            case 5:
+                ourPoseX = tag5Pose.getX() - ftcPose.y - FORWARD_OFFSET_IN;
+                ourPoseY = tag5Pose.getY() - ftcPose.x - SIDE_OFFSET_IN;
+                break;
+            default:
+                ourPoseX = 0;
+                ourPoseY = 0;
+                break;
+        }
+
+        return new Vector2d(ourPoseX, ourPoseY);
+    }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java

@@ -101,11 +101,11 @@ public class Gantry {
     }
 
     public void intake() {
-        this.screwServo.setPower(1);
+        this.screwServo.setPower(-1);
     }
 
     public void deposit() {
-        this.screwServo.setPower(-1);
+        this.screwServo.setPower(1);
     }
 
     public void stop() {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java

@@ -72,4 +72,6 @@ public class RobotConfig {
     public static double DETECTION_CENTER_X = .5;
     public static double DETECTION_RIGHT_X = 1;
     public static double SCORING_DISTANCE_FROM_APRIL_TAG = 6f;
+    public static final double FORWARD_OFFSET_IN = 7.75;
+    public static final double SIDE_OFFSET_IN = 0.5;
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java

@@ -209,7 +209,11 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive
 
     public void breakFollowing() {
         trajectorySequenceRunner.breakFollowing();
+        if (trajectorySequenceRunner.isBusy()) {
+            setWeightedDrivePower(new Pose2d());
         }
+    }
+
     public Pose2d getLastError() {
         return trajectorySequenceRunner.getLastPoseError();
     }