diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java index e9ce578..2c1beff 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java +++ b/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() { - Pose2d estimatedPose = null; - if (camera != null) { - estimatedPose = this.camera.estimatePoseFromAprilTag(); + public void refreshPoseEstimateFromAprilTag() { + this.drive.update(); + drivePose = this.drive.getPoseEstimate(); + 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; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java index eb4c1a6..b1dc65f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java +++ b/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 HEADING_PID = new PIDCoefficients(12, 0, .5); + public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(5, 0, 4); + 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; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java index 7628101..8506dc0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java +++ b/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 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java index 39e8544..ac4459c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java +++ b/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()); -// parkLocation(); - randomization = "CENTER"; + randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue()); + parkLocation(); +// randomization = "CENTER"; this.telemetry.addData("Park Position", parkLocation); this.telemetry.update(); } - robot.update(); scorePreloadOne(); - boardScore(); - sleep(250); + boardScore(); + this.robot.refreshPoseEstimateFromAprilTag(); + sleep(250); this.robot.getClaw().open(); sleep(250); park(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java index 8fc0383..73ebfb4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java +++ b/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(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java index 124af7d..2b0952f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java +++ b/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(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Camera.java index 6358e9f..966da71 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Camera.java +++ b/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(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/PropDetectionPipeline.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/PropDetectionPipeline.java index bc307f6..dbbc07f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/PropDetectionPipeline.java +++ b/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; } } \ No newline at end of file