Goog

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

@@ -33,11 +33,16 @@ import lombok.Getter;
 public class Robot {
     public static boolean clawIsOpen;
     public static double WRISTDELAY = .08;
-    double delay;
+    public static int MIN_ERROR = 1;
+    public static int MAX_ERROR = 6;
+    public double distance = Double.MAX_VALUE;
+    public Pose2d estimatedPose;
+    public Pose2d drivePose;
     public pickupMacroStates pickupMacroState = pickupMacroStates.IDLE;
     public armMacroStates armMacroState = armMacroStates.IDLE;
     @Getter
     public Arm arm;
+    double delay;
     @Getter
     private MecanumDrive drive;
     @Getter
@@ -130,15 +135,23 @@ public class Robot {
         return this.drive.trajectorySequenceBuilder(this.drive.getPoseEstimate());
     }
 
-    public void update() {
+    public void refreshPoseEstimateFromAprilTag() {
-        Pose2d estimatedPose = null;
+        this.drive.update();
-        if (camera != null) {
+        drivePose = this.drive.getPoseEstimate();
-            estimatedPose = this.camera.estimatePoseFromAprilTag();
+        estimatedPose = this.camera.estimatePoseFromAprilTag();
+
+        if (estimatedPose != null) {
+            distance = Math.sqrt(Math.pow(drivePose.getX() - estimatedPose.getX(), 2) + Math.pow(drivePose.getY() - estimatedPose.getY(), 2));
         }
+        if (distance < MAX_ERROR && distance > MIN_ERROR) {
+            this.drive.update(estimatedPose);
+        }
+    }
+
+    public void update() {
+        this.drive.update();
         this.arm.update();
         this.wrist.update();
-        this.drive.update(estimatedPose);
-
     }
 
     public enum pickupMacroStates {
@@ -392,10 +405,10 @@ public class Robot {
         public static double KP = 0.2;
         public static double TOL = 0.005;
         public static double MAX_DELTA = 0.04;
-        private PIDFController wristPController;
         //Values
         public static double WRISTPICKUP = 0.3;
         public static double WRISTSCORE = .98;
+        private PIDFController wristPController;
         //Servo
         private Servo wrist;
 

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

@@ -60,12 +60,13 @@ import java.util.List;
  */
 @Config
 public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive {
-    public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(3, 0, 2.5);
+    public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(5, 0, 4);
-    public static PIDCoefficients HEADING_PID = new PIDCoefficients(12, 0, .5);
+    public static PIDCoefficients HEADING_PID = new PIDCoefficients(15, 0, 1);
 
     public static double LATERAL_MULTIPLIER = 1;
 
     public static double VX_WEIGHT = 1;
+
     public static double VY_WEIGHT = 1;
     public static double OMEGA_WEIGHT = 1;
 

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

@@ -42,7 +42,7 @@ public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer {
     public static double PARALLEL_Y = 3.94; // Y is the strafe direction
 
     public static double PERPENDICULAR_X = 5.10;
-    public static double PERPENDICULAR_Y = 0.20;
+    public static double PERPENDICULAR_Y = 0.51;
 
     public static double X_MULTIPLIER = 1; // Multiplier in the X direction
     public static double Y_MULTIPLIER = 1; // Multiplier in the Y direction

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java

@@ -5,6 +5,7 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.tearabite.ielib.common.Alliance;
 
 import org.firstinspires.ftc.teamcode.hardware.Robot;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
@@ -105,6 +106,7 @@ public class AutoRed extends LinearOpMode {
 
         this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
         this.robot = new Robot().init(hardwareMap);
+        this.robot.getCamera().setAlliance(Alliance.Blue);
 //        this.robot.getCamera().initTargetingCamera();
         this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270));
         this.robot.getDrive().setPoseEstimate(initialPosition);
@@ -112,16 +114,16 @@ public class AutoRed extends LinearOpMode {
         // Do super fancy chinese shit
         while (!this.isStarted()) {
             this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition());
-//            randomization = String.valueOf(this.robot.getCamera().getStartingPosition());
+            randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue());
-//            parkLocation();
+            parkLocation();
-            randomization = "CENTER";
+//            randomization = "CENTER";
             this.telemetry.addData("Park Position", parkLocation);
             this.telemetry.update();
         }
-            robot.update();
             scorePreloadOne();
-            boardScore();
+        boardScore();
-            sleep(250);
+        this.robot.refreshPoseEstimateFromAprilTag();
+        sleep(250);
             this.robot.getClaw().open();
             sleep(250);
             park();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java

@@ -169,7 +169,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
             randomization = String.valueOf(this.robot.getCamera().getStartingPosition());
             this.telemetry.update();
         }
-
+        this.robot.update();
         scorePreloadOne();
         boardScore();
 
@@ -188,12 +188,12 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
                 toStack();
                 break;
         }
-
         sleep(500);
         this.robot.getClaw().close();
         sleep(250);
         this.robot.getArm().armRest();
         scoreStack();
+        this.robot.refreshPoseEstimateFromAprilTag();
         this.robot.getClaw().setPos(.83);
         scoreTest();
         park();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java

@@ -89,13 +89,16 @@ public class MainTeleOp extends OpMode {
         } else {
             this.robot.getPlane().planeLock();
         }
+        this.robot.update();
 //Telemetry
         int PositionLeft = this.robot.getSlides().slidesLeft.getCurrentPosition();
         telemetry.addData("positionLeft", (PositionLeft));
         int PositionRight = this.robot.getSlides().slidesRight.getCurrentPosition();
         telemetry.addData("positionRight", (PositionRight));
+        telemetry.addData("Camera Pos Estimate", (robot.estimatedPose));
+        telemetry.addData("Pos Estimate", (robot.drivePose));
+        telemetry.addData("Difference", (robot.distance));
         telemetry.update();
 //Update Robot
-        this.robot.update();
     }
 }

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

@@ -6,6 +6,7 @@ import static org.firstinspires.ftc.teamcode.util.Constants.TARGETING_WEBCAM;
 
 import com.acmerobotics.roadrunner.geometry.Pose2d;
 import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.tearabite.ielib.common.Alliance;
 import com.tearabite.ielib.localization.AprilTagPoseEstimator;
 
 import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
@@ -23,9 +24,9 @@ public class Camera {
     private VisionPortal aprilTagPortal;
     private VisionPortal propPortal;
     private boolean initialized;
-    private static final Pose2d TAG_CAMERA_OFFSETS = new Pose2d(8.27, 0, 0);
+    private static final Pose2d TAG_CAMERA_OFFSETS = new Pose2d(-8.27, -0.95, 0);
     private AprilTagPoseEstimator aprilTagPoseEstimator;
-    public static float PROP_REJECTION_VERTICAL_UPPER = 480f * 0.33f;
+    public static float PROP_REJECTION_VERTICAL_UPPER = 40;
     public static float PROP_REJECTION_VERTICAL_LOWER = 440;
 
 
@@ -78,9 +79,13 @@ public class Camera {
         return targetingCameraInitialized ? prop.getRed() : INVALID_DETECTION;
     }
 
+    public void setAlliance(Alliance alliance) {
+        this.prop.setAlliance(alliance);
+    }
+
     public StarterPosition getStartingPosition() {
         if (!targetingCameraInitialized) {
-           return StarterPosition.UNKNOWN;
+            return StarterPosition.UNKNOWN;
         }
 
         Detection detection = this.getRed();

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

@@ -18,10 +18,10 @@ import static org.firstinspires.ftc.teamcode.vision.Camera.PROP_REJECTION_VERTIC
 
 import android.graphics.Canvas;
 
+import com.tearabite.ielib.common.Alliance;
 import com.tearabite.ielib.vision.ScalarRange;
 
 import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration;
-import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
 import org.firstinspires.ftc.vision.VisionProcessor;
 import org.opencv.core.Core;
 import org.opencv.core.Mat;
@@ -34,7 +34,7 @@ import java.util.ArrayList;
 
 public class PropDetectionPipeline implements VisionProcessor {
 
-    CenterStageCommon.Alliance alliance;
+    Alliance alliance;
     private Mat blurred = new Mat();
     private Mat hsv = new Mat();
     private Mat mask = new Mat();
@@ -55,11 +55,11 @@ public class PropDetectionPipeline implements VisionProcessor {
         Imgproc.GaussianBlur(input, blurred, BLUR_SIZE, 0);
         Imgproc.cvtColor(blurred, hsv, Imgproc.COLOR_RGB2HSV);
 
-        if (alliance == CenterStageCommon.Alliance.Red) {
+        if (alliance == Alliance.Red) {
             red.setContour(detect(FTC_RED_RANGE_1, FTC_RED_RANGE_2));
         }
 
-        if (alliance == CenterStageCommon.Alliance.Blue) {
+        if (alliance == Alliance.Blue) {
             blue.setContour(detect(FTC_BLUE_RANGE));
         }
 
@@ -132,11 +132,11 @@ public class PropDetectionPipeline implements VisionProcessor {
         return this.blue;
     }
 
-    public void setAlliance(CenterStageCommon.Alliance alliance) {
+    public void setAlliance(Alliance alliance) {
         this.alliance = alliance;
     }
 
-    public CenterStageCommon.Alliance getAlliance() {
+    public Alliance getAlliance() {
         return this.alliance;
     }
 }