Merge branch 'bumblebee_dev' of https://github.com/IronEaglesRobotics/CenterStage into bumblebee_dev

TeamCode/src/main/java/opmodes/AutoBase.java

@@ -18,68 +18,62 @@ 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.teamcode.vision.Detection;
-import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
-import org.opencv.core.Point;
 
 @Config
 public abstract class AutoBase extends LinearOpMode {
     public static int DEPOSIT_HEIGHT = 100;
-
+    public static int SCORING_DURATION_MS =  5000;
     protected Robot robot;
     protected FtcDashboard dashboard;
     protected Telemetry dashboardTelemetry;
     protected CenterStageCommon.PropLocation propLocation;
+    protected final Pose2d initialPosition;
+    protected final CenterStageCommon.Alliance alliance;
+    protected final Vector2d rendezvous;
+
+    protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Vector2d rendezvous) {
+        this.alliance = alliance;
+        this.initialPosition = initialPosition;
+        this.rendezvous = rendezvous;
+    }
 
     @Override
     public void runOpMode() {
-        this.robot = new Robot(hardwareMap, telemetry);
+        // Initialize Robot and Dashboard
+        this.robot = new Robot(hardwareMap, telemetry, initialPosition, alliance);
         this.dashboard = FtcDashboard.getInstance();
         this.dashboardTelemetry = dashboard.getTelemetry();
 
-        this.robot.getCamera().setAlliance(CenterStageCommon.Alliance.Blue);
+        // Wait for match to start
-
-        this.robot.getDrive().setPoseEstimate(new Pose2d(-36, 63, Math.toRadians(-90)));
-
         while(!isStarted() && !isStopRequested()) {
             this.robot.update();
             this.sleep(20);
         }
 
-        setPropLocationIfVisible(Center, null);
+        // If the prop is visible at this point, then it must be in the center (2) position
+        determinePropLocation();
 
-        TrajectorySequenceBuilder builder = this.robot.getDrive()
+        switch (this.propLocation) {
-                .trajectorySequenceBuilder(new Pose2d(-36, 63, Math.toRadians(-90)));
+            case Left:
-        if (this.propLocation != CenterStageCommon.PropLocation.Center) {
+                // TODO Tommy: Place the pixel on the left tape and move to rendezvous position
-            builder.forward(5);
+                break;
-            builder.turn(Math.toRadians(-33));
+            case Unknown:
-            this.robot.getDrive().followTrajectorySequence(builder.build());
+            case Center:
-
+                dislodgePropAndPlacePixel();
-            setPropLocationIfVisible(Right, Left);
+                break;
-            return;
+            case Right:
-        } else {
+                // TODO Tommy: Place the pixel on the right tape and move to rendezvous position
-            // Center
+                break;
-            builder.lineToConstantHeading(new Vector2d(-36, 11));
-            builder.addDisplacementMarker(10, () -> {
-                this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
-            });
-            this.robot.getDrive().followTrajectorySequence(builder.build());
-
-            this.robot.getClaw().openSync();
-            this.sleep(100);
-            this.robot.getClaw().setArmPosition(PICKUP_ARM_MAX);
         }
 
-        builder = this.robot.getDrive().trajectorySequenceBuilder(new Pose2d(-36, 11));
+        moveToBackstage();
-        builder.lineToLinearHeading(new Pose2d(36, 11, 0));
+        prepareToScore();
-        builder.lineToLinearHeading(new Pose2d(36, 38, 0));
+        scorePreloadedPixel();
-        this.robot.getDrive().followTrajectorySequence(builder.build());
 
-        double distance = getDistanceToAprilTag();
+        // TODO Tommy: Park
-
+    }
-        builder = this.robot.getDrive().trajectorySequenceBuilder(new Pose2d(36, 38, 0));
-        builder.forward(distance - SCORING_DISTANCE_FROM_APRIL_TAG);
-        this.robot.getDrive().followTrajectorySequence(builder.build());
 
+    private void scorePreloadedPixel() {
         this.robot.getGantry().setSlideTarget(DEPOSIT_HEIGHT);
         this.robot.getGantry().armOut();
         while(this.robot.getGantry().isIn()) {
@@ -87,33 +81,50 @@ public abstract class AutoBase extends LinearOpMode {
             sleep(20);
         }
         this.robot.getGantry().deposit();
-
+        this.sleep(SCORING_DURATION_MS);
-        while(opModeIsActive()) {
+        this.robot.getGantry().stop();
-            this.robot.update();
-            AprilTagDetection aprilTagDetection = this.robot.getCamera().getAprilTag(2);
-            if (aprilTagDetection != null) {
-                Point center = aprilTagDetection.center;
-                this.dashboardTelemetry.addData("center", center);
-                this.dashboardTelemetry.addData("x", aprilTagDetection.ftcPose.x);
-                this.dashboardTelemetry.addData("y", aprilTagDetection.ftcPose.y);
-                this.dashboardTelemetry.addData("z", aprilTagDetection.ftcPose.z);
-                this.dashboardTelemetry.update();
     }
-            sleep(20);
+
+    private void prepareToScore() {
+        double distance = this.robot.getCamera().getDistanceToAprilTag(2, 25, 5);
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.forward(distance - SCORING_DISTANCE_FROM_APRIL_TAG);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    private void moveToBackstage() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(new Pose2d(36, 11, 0));
+        builder.lineToLinearHeading(new Pose2d(36, 38, 0));
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    private void dislodgePropAndPlacePixel() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToConstantHeading(rendezvous);
+        builder.addDisplacementMarker(10, () -> {
+            this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN);
+        });
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+        this.robot.getClaw().openSync();
+        this.sleep(100);
+        this.robot.getClaw().setArmPosition(PICKUP_ARM_MAX);
+    }
+
+    private void determinePropLocation() {
+        setPropLocationIfVisible(Center, null);
+        if (this.propLocation != Center) {
+            peekRight();
         }
     }
 
-    private double getDistanceToAprilTag() {
+    private void peekRight() {
-        double minDistance = Double.MAX_VALUE;
+        TrajectorySequenceBuilder builder = this.robot.getDrive()
-        for (int i = 0; i < 10; i++) {
+                .trajectorySequenceBuilder(initialPosition);
-            AprilTagDetection aprilTagDetection = this.robot.getCamera().getAprilTag(2);
+        builder.forward(5);
-            if (aprilTagDetection != null) {
+        builder.turn(Math.toRadians(-33));
-                if (aprilTagDetection.ftcPose.y < minDistance) {
+        this.robot.getDrive().followTrajectorySequence(builder.build());
-                    minDistance = aprilTagDetection.ftcPose.y;
+        setPropLocationIfVisible(Right, Left);
-                }
-            }
-        }
-        return minDistance;
     }
 
     protected static int getExpectedAprilTagId(CenterStageCommon.PropLocation propLocation) {

TeamCode/src/main/java/opmodes/LeftAuto.java

@@ -1,9 +1,20 @@
 package opmodes;
 
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 
+import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
+
 @Autonomous(name = "Left Auto", preselectTeleOp = "MainTeleOp")
 public class LeftAuto extends AutoBase {
+    public LeftAuto() {
+        super(
+                CenterStageCommon.Alliance.Red,
+                new Pose2d(-36, 63, Math.toRadians(-90)),
+                new Vector2d(-36, 11));
+    }
+
     @Override
     public void runOpMode() {
         super.runOpMode();

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

@@ -17,9 +17,6 @@ import org.firstinspires.ftc.vision.VisionPortal;
 import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
 import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
 
-import lombok.Getter;
-import lombok.Setter;
-
 public class Camera {
     private PropDetectionPipeline prop;
     private AprilTagProcessor aprilTag;
@@ -92,6 +89,19 @@ public class Camera {
                 .orElse(null);
     }
 
+    public double getDistanceToAprilTag(int id, double rejectAbove, double rejectBelow) {
+        for (int i = 0; i < 10; i++) {
+            AprilTagDetection aprilTagDetection = getAprilTag(id);
+            if (aprilTagDetection != null) {
+                if (aprilTagDetection.ftcPose.y < rejectAbove
+                        && aprilTagDetection.ftcPose.y > rejectBelow) {
+                    return aprilTagDetection.ftcPose.y;
+                }
+            }
+        }
+        return Double.MAX_VALUE;
+    }
+
     public void setAlliance(CenterStageCommon.Alliance alliance) {
         this.prop.setAlliance(alliance);
     }

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

@@ -1,13 +1,13 @@
 package org.firstinspires.ftc.teamcode.hardware;
 
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_ARM_LEFT_NAME;
-import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_KP;
-import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MAX;
-import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_ARM_RIGHT_NAME;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_KP;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_MAX;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_MIN;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_NAME;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MAX;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN;
 
 import com.arcrobotics.ftclib.controller.PController;
 import com.qualcomm.robotcore.hardware.HardwareMap;

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

@@ -5,12 +5,12 @@ import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_ARM_KP;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_ARM_MAX;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_ARM_MIN;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_ARM_NAME;
-import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SLIDE_POWER;
-import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_CENTER;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_SCREW_NAME;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_X_NAME;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.LEFT_SLIDE_MOTOR_NAME;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.RIGHT_SLIDE_MOTOR_NAME;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SLIDE_POWER;
+import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_CENTER;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_KP;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_MAX;
 import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_MIN;

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

@@ -4,9 +4,10 @@ import com.acmerobotics.roadrunner.geometry.Pose2d;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
-import org.checkerframework.checker.units.qual.C;
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
 
 import lombok.Getter;
 
@@ -27,6 +28,9 @@ public class Robot {
     @Getter
     private Camera camera;
 
+    @Getter
+    CenterStageCommon.Alliance alliance;
+
     private final Telemetry telemetry;
 
     public Robot(HardwareMap hardwareMap, Telemetry telemetry) {
@@ -34,6 +38,12 @@ public class Robot {
         this.init(hardwareMap);
     }
 
+    public Robot(HardwareMap hardwareMap, Telemetry telemetry, Pose2d initialPosition, CenterStageCommon.Alliance alliance) {
+        this(hardwareMap, telemetry);
+        this.getDrive().setPoseEstimate(initialPosition);
+        this.setAlliance(alliance);
+    }
+
     private void init(HardwareMap hardwareMap) {
         this.drive = new MecanumDrive(hardwareMap);
         this.drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
@@ -43,6 +53,15 @@ public class Robot {
         this.camera = new Camera(hardwareMap, telemetry);
     }
 
+    public TrajectorySequenceBuilder getTrajectorySequenceBuilder() {
+        return this.drive.trajectorySequenceBuilder(this.drive.getPoseEstimate());
+    }
+
+    public void setAlliance(CenterStageCommon.Alliance alliance) {
+        this.alliance = alliance;
+        this.camera.setAlliance(alliance);
+    }
+
     public void update() {
         this.gantry.update();
         this.lift.update();

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

@@ -17,6 +17,9 @@ import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveCons
 import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.RUN_USING_ENCODER;
 import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.TRACK_WIDTH;
 import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.encoderTicksToInches;
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kA;
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kStatic;
+import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kV;
 
 import androidx.annotation.NonNull;
 
@@ -51,10 +54,6 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.Tra
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceRunner;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.LynxModuleUtil;
 
-import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kV;
-import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kA;
-import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kStatic;
-
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.List;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/trajectorysequence/TrajectorySequenceRunner.java

@@ -25,7 +25,6 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.seq
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.DashboardUtil;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.LogFiles;
 
-
 import java.util.ArrayList;
 import java.util.Collections;
 import java.util.LinkedList;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/util/LogFiles.java

@@ -20,7 +20,6 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.StandardTrackingWheelLocalizer;
 
-
 import java.io.File;
 import java.io.FileInputStream;
 import java.io.IOException;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Colors.java

@@ -1,7 +1,8 @@
 package org.firstinspires.ftc.teamcode.util;
 
-import org.opencv.core.Scalar;
 import android.graphics.Color;
+
+import org.opencv.core.Scalar;
 public class Colors {
     // CV Color Threshold Constants
     public static Scalar FTC_RED_LOWER = new Scalar(165, 80, 80);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Detection.java

@@ -1,12 +1,5 @@
 package org.firstinspires.ftc.teamcode.vision;
 
-import org.opencv.core.Mat;
-import org.opencv.core.MatOfPoint;
-import org.opencv.core.Point;
-import org.opencv.core.Scalar;
-import org.opencv.core.Size;
-import org.opencv.imgproc.Imgproc;
-
 import static org.firstinspires.ftc.teamcode.util.Constants.GREEN;
 import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_AREA;
 import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_POINT;
@@ -17,6 +10,13 @@ import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getBottomLeftOfCont
 import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getBottomRightOfContour;
 import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getCenterOfContour;
 
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfPoint;
+import org.opencv.core.Point;
+import org.opencv.core.Scalar;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+
 // Class for a Detection
 public class Detection {
     private double minAreaPx;