diff --git a/TeamCode/src/main/java/opmodes/AutoBase.java b/TeamCode/src/main/java/opmodes/AutoBase.java index d37845b..a2d28dd 100644 --- a/TeamCode/src/main/java/opmodes/AutoBase.java +++ b/TeamCode/src/main/java/opmodes/AutoBase.java @@ -1,5 +1,6 @@ package opmodes; +import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CAMERA_FORWARD_OFFSET_IN; 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.SCORING_DISTANCE_FROM_APRIL_TAG; @@ -62,7 +63,6 @@ public abstract class AutoBase extends LinearOpMode { // If the prop is visible at this point, then it must be in the center (2) position determinePropLocation(); - TrajectorySequenceBuilder builder; switch (this.propLocation) { case Left: propLeft(); @@ -75,7 +75,7 @@ public abstract class AutoBase extends LinearOpMode { propRight(); break; } - moveToBackstage(); + moveToEasel(); prepareToScore(); scorePreloadedPixel(); park(); @@ -108,7 +108,6 @@ public abstract class AutoBase extends LinearOpMode { this.robot.getGantry().stop(); this.robot.getGantry().setSlideTarget(0); this.robot.getGantry().armInSync(); - } protected void prepareToScore() { @@ -129,30 +128,51 @@ public abstract class AutoBase extends LinearOpMode { delta = APRIL_TAG_RIGHT_DELTA; break; } - double distance = this.robot.getCamera().getDistanceToAprilTag(this.alliance == CenterStageCommon.Alliance.Blue ? 2:5, 25, 5); - Vector2d target = new Vector2d(this.robot.getDrive().getPoseEstimate().getX() + - (distance - SCORING_DISTANCE_FROM_APRIL_TAG), - this.robot.getDrive().getPoseEstimate().getY() + delta); + + Pose2d inferredPos = this.robot.getCamera().estimatePoseFromAprilTag(); + this.robot.getDrive().setPoseEstimate(inferredPos); + Pose2d target = new Pose2d( + 60 - SCORING_DISTANCE_FROM_APRIL_TAG - CAMERA_FORWARD_OFFSET_IN, // 60 is the X position of the april tag + inferredPos.getY() + delta, + 0); + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToConstantHeading(target); + builder.lineToLinearHeading(target); this.robot.getDrive().followTrajectorySequence(builder.build()); } - protected void moveToBackstage() { + protected void moveToEasel() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + if (!this.isBackstage()) { + if (this.alliance == CenterStageCommon.Alliance.Blue) { + builder.lineToSplineHeading(new Pose2d(-40, 60, Math.PI)); + builder.lineToLinearHeading(new Pose2d(12, 60, Math.PI)); + } else if (this.alliance == CenterStageCommon.Alliance.Red) { + builder.lineToSplineHeading(new Pose2d(-40, -60, Math.PI)); + builder.lineToLinearHeading(new Pose2d(12, -60, Math.PI)); + } + } + if (this.alliance == CenterStageCommon.Alliance.Blue) { - builder.lineToLinearHeading(new Pose2d(35, 11, 0)); - builder.lineToLinearHeading(new Pose2d(35, 38, 0)); - } else { - builder.lineToLinearHeading(new Pose2d(35, -11, 0)); - builder.lineToLinearHeading(new Pose2d(35, -36, 0)); + builder.lineToLinearHeading(new Pose2d(35, 36, 0)); + } else if (this.alliance == CenterStageCommon.Alliance.Red) { + builder.lineToLinearHeading(new Pose2d(35, -35, 0)); } this.robot.getDrive().followTrajectorySequence(builder.build()); } protected void determinePropLocation() { + this.robot.getClaw().setArmPositionAsync(PICKUP_ARM_MIN); + + while (!this.robot.getClaw().isArmAtPosition()) { + this.robot.update(); + sleep(20); + } + + sleep(250); + setPropLocationIfVisible(Center, Unknown); if (this.propLocation != Center) { peekRight(); @@ -160,11 +180,15 @@ public abstract class AutoBase extends LinearOpMode { } protected void peekRight() { - TrajectorySequenceBuilder builder = this.robot.getDrive() - .trajectorySequenceBuilder(initialPosition); - builder.forward(5); - builder.turn(Math.toRadians(-33)); + Pose2d currentPose = this.robot.getDrive().getPoseEstimate(); + final double y = currentPose.getY() > 0 ? -5 : 5; + final double z = Math.toRadians(-25); + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(currentPose.plus(new Pose2d(0, y, z))); this.robot.getDrive().followTrajectorySequence(builder.build()); + + this.sleep(250); + setPropLocationIfVisible(Right, Left); } @@ -188,7 +212,11 @@ public abstract class AutoBase extends LinearOpMode { double currentX = this.robot.getDrive().getPoseEstimate().getX(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.strafeTo(new Vector2d(currentX, park.getY())); - builder.lineToConstantHeading(park.vec()); + builder.lineToLinearHeading(park); this.robot.getDrive().followTrajectorySequence(builder.build()); } + + protected boolean isBackstage() { + return this.initialPosition.getX() > 0; + } } diff --git a/TeamCode/src/main/java/opmodes/BlueBackStage.java b/TeamCode/src/main/java/opmodes/BlueBackStage.java index f45dbeb..8470e63 100644 --- a/TeamCode/src/main/java/opmodes/BlueBackStage.java +++ b/TeamCode/src/main/java/opmodes/BlueBackStage.java @@ -11,48 +11,44 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon; @Autonomous(name = "BlueBackStage", preselectTeleOp = "MainTeleOp") public class BlueBackStage extends AutoBase { - private final Pose2d rendezvous = new Pose2d(12, 10); + private static final int delay = 0; public BlueBackStage() { super( CenterStageCommon.Alliance.Blue, - new Pose2d(12, 63, Math.toRadians(-90)), + new Pose2d(12, 63, Math.toRadians(90)), new Pose2d(62, 62)); } protected void propLeft() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(new Pose2d(36, 25, Math.toRadians(-33))); - builder.addDisplacementMarker(10, () -> { - this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); - }); - this.robot.getDrive().followTrajectorySequence(builder.build()); + this.sleep(delay); - openAndLiftClaw(); - } - - protected void propCenter() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToConstantHeading(rendezvous.vec()); - builder.addDisplacementMarker(10, () -> { - this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); - }); + builder.lineToLinearHeading(new Pose2d(17.3, 41.6, Math.toRadians(108.25))); this.robot.getDrive().followTrajectorySequence(builder.build()); openAndLiftClaw(); builder = this.robot.getTrajectorySequenceBuilder(); - builder.turn(Math.toRadians(90)); + builder.lineToConstantHeading(new Vector2d(24, 50)); this.robot.getDrive().followTrajectorySequence(builder.build()); } - protected void propRight() { + protected void propCenter() { + this.sleep(delay); + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(new Pose2d(32, 34, Math.toRadians(0))); - builder.lineToConstantHeading(new Vector2d(19, 34)); - builder.addTemporalMarker(0.5, () -> { - this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); - }); + builder.lineToLinearHeading(new Pose2d(12, 42, initialPosition.getHeading())); + this.robot.getDrive().followTrajectorySequence(builder.build()); + + openAndLiftClaw(); + } + + protected void propRight() { + this.sleep(delay); + + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(new Pose2d(18.25, 32, Math.toRadians(0))); this.robot.getDrive().followTrajectorySequence(builder.build()); openAndLiftClaw(); diff --git a/TeamCode/src/main/java/opmodes/BlueFrontStage.java b/TeamCode/src/main/java/opmodes/BlueFrontStage.java index 2aab26b..9f5cf70 100644 --- a/TeamCode/src/main/java/opmodes/BlueFrontStage.java +++ b/TeamCode/src/main/java/opmodes/BlueFrontStage.java @@ -11,59 +11,42 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon; @Autonomous(name = "BlueFrontStage", preselectTeleOp = "MainTeleOp") public class BlueFrontStage extends AutoBase { - private final Pose2d rendezvous = new Pose2d(-36, 11); + private static final int delay = 0; public BlueFrontStage() { super( CenterStageCommon.Alliance.Blue, - new Pose2d(-36, 63, Math.toRadians(-90)), - new Pose2d(62, 12)); + new Pose2d(-36, 63, Math.toRadians(90)), + new Pose2d(62, 12, Math.toRadians(0))); } protected void propLeft() { - this.sleep(5000); + this.sleep(delay); + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(new Pose2d(-52, 31, Math.toRadians(-180))); - builder.lineToConstantHeading(new Vector2d(-40, 31)); - builder.addTemporalMarker(0.2, () -> { - this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); - }); + builder.lineToLinearHeading(new Pose2d(-43.5, 31.5, Math.toRadians(180))); this.robot.getDrive().followTrajectorySequence(builder.build()); openAndLiftClaw(); - - builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(rendezvous); - this.robot.getDrive().followTrajectorySequence(builder.build()); } + protected void propCenter() { - this.sleep(5000); + this.sleep(delay); + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToConstantHeading(rendezvous.vec()); - builder.addDisplacementMarker(10, () -> { - this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); - }); + builder.lineToLinearHeading(new Pose2d(-36, 42, initialPosition.getHeading())); this.robot.getDrive().followTrajectorySequence(builder.build()); openAndLiftClaw(); - - builder = this.robot.getTrajectorySequenceBuilder(); - builder.turn(Math.toRadians(90)); - this.robot.getDrive().followTrajectorySequence(builder.build()); } + protected void propRight() { - this.sleep(5000); + this.sleep(delay); + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(new Pose2d(-54, 17, Math.toRadians(-123))); - builder.addDisplacementMarker(10, () -> { - this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); - }); + builder.lineToLinearHeading(new Pose2d(-39.34, 40.45, Math.toRadians(64.3))); this.robot.getDrive().followTrajectorySequence(builder.build()); openAndLiftClaw(); - - builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(this.rendezvous); - this.robot.getDrive().followTrajectorySequence(builder.build()); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/opmodes/MainTeleOp.java b/TeamCode/src/main/java/opmodes/MainTeleOp.java index 73e1ed8..bb7f245 100644 --- a/TeamCode/src/main/java/opmodes/MainTeleOp.java +++ b/TeamCode/src/main/java/opmodes/MainTeleOp.java @@ -1,18 +1,18 @@ 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.CAMERA_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.CAMERA_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; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_MIN; import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; 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; @@ -40,10 +40,12 @@ public class MainTeleOp extends OpMode { @Override public void init() { this.dashboard = FtcDashboard.getInstance(); + this.telemetry = new MultipleTelemetry(telemetry, dashboard.getTelemetry()); this.clawArmPosition = PICKUP_ARM_MAX; this.robot = new Robot(this.hardwareMap, telemetry); this.robot.getCamera().setAlliance(CenterStageCommon.Alliance.Blue); + this.robot.getDrive().setPoseEstimate(new Pose2d(12, 63, Math.toRadians(90))); telemetry.addData("Status", "Initialized"); } @@ -170,7 +172,7 @@ public class MainTeleOp extends OpMode { this.robot.getLift().stopReset(); } - Pose2d poseFromAprilTag = this.robot.getCamera().getPoseFromAprilTag(2, 5); + Pose2d poseFromAprilTag = this.robot.getCamera().estimatePoseFromAprilTag(); dashboard.getTelemetry().addData("Inferred Position", poseFromAprilTag); dashboard.getTelemetry().update(); @@ -223,12 +225,11 @@ public class MainTeleOp extends OpMode { } Pose2d target; // defines a new pose2d named target, position not yet given - Pose2d poseEstimate = new Pose2d(poseFromAprilTag.getX(), poseFromAprilTag.getY(), -Math.toRadians(poseFromAprilTag.getHeading())); - double y = poseEstimate.getY() > 0 + double y = poseFromAprilTag.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); + this.robot.getDrive().setPoseEstimate(poseFromAprilTag); + target = new Pose2d(Camera.tag2Pose.getX() - SCORING_DISTANCE_FROM_APRIL_TAG - CAMERA_FORWARD_OFFSET_IN, y - CAMERA_SIDE_OFFSET_IN, 0); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToLinearHeading(target); this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); diff --git a/TeamCode/src/main/java/opmodes/RedBackStage.java b/TeamCode/src/main/java/opmodes/RedBackStage.java index d208a72..12a3f20 100644 --- a/TeamCode/src/main/java/opmodes/RedBackStage.java +++ b/TeamCode/src/main/java/opmodes/RedBackStage.java @@ -11,52 +11,47 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon; @Autonomous(name = "RedBackStage", preselectTeleOp = "MainTeleOp") public class RedBackStage extends AutoBase { - private final Pose2d rendezvous = new Pose2d(12, -11); + private static final int delay = 0; public RedBackStage() { super( CenterStageCommon.Alliance.Red, - new Pose2d(12, -63, Math.toRadians(90)), + new Pose2d(12, -63, Math.toRadians(-90)), new Pose2d(62, -62)); } protected void propLeft() { + this.sleep(delay); + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(new Pose2d(32, -34, Math.toRadians(0))); - builder.lineToConstantHeading(new Vector2d(19, -34)); - builder.addTemporalMarker(0.5, () -> { - this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); - }); + builder.lineToLinearHeading(new Pose2d(18.25, -33.5, Math.toRadians(0))); this.robot.getDrive().followTrajectorySequence(builder.build()); openAndLiftClaw(); } protected void propCenter() { + this.sleep(delay); + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToConstantHeading(rendezvous.vec()); - builder.addDisplacementMarker(10, () -> { - this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); - }); + builder.lineToLinearHeading(new Pose2d(12, -42, initialPosition.getHeading())); + this.robot.getDrive().followTrajectorySequence(builder.build()); + + openAndLiftClaw(); + } + + protected void propRight() { + this.sleep(delay); + + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(new Pose2d(15.9, -41.35, Math.toRadians(244.15))); this.robot.getDrive().followTrajectorySequence(builder.build()); openAndLiftClaw(); builder = this.robot.getTrajectorySequenceBuilder(); - builder.turn(Math.toRadians(-90)); + builder.lineToConstantHeading(new Vector2d(24, -50)); this.robot.getDrive().followTrajectorySequence(builder.build()); } - - protected void propRight() { - // red back side - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(new Pose2d(36, -25, Math.toRadians(33))); - builder.addDisplacementMarker(10, () -> { - this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); - }); - this.robot.getDrive().followTrajectorySequence(builder.build()); - - openAndLiftClaw(); - } } diff --git a/TeamCode/src/main/java/opmodes/RedFrontStage.java b/TeamCode/src/main/java/opmodes/RedFrontStage.java index 8de80a6..6584acb 100644 --- a/TeamCode/src/main/java/opmodes/RedFrontStage.java +++ b/TeamCode/src/main/java/opmodes/RedFrontStage.java @@ -11,63 +11,42 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon; @Autonomous(name = "RedFrontStage", preselectTeleOp = "MainTeleOp") public class RedFrontStage extends AutoBase { - private final Pose2d rendezvous = new Pose2d(-36, -10); + private static final int delay = 0; public RedFrontStage() { super( CenterStageCommon.Alliance.Red, - new Pose2d(-36, -63, Math.toRadians(90)), + new Pose2d(-36, -63, Math.toRadians(-90)), new Pose2d(61, -12)); } - // propLeft will be a reverse of BlueFrontpropRight protected void propLeft() { - this.sleep(5000); + this.sleep(delay); + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(new Pose2d(-58, -17, Math.toRadians(123))); - builder.addDisplacementMarker(10, () -> { - this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); - }); + builder.lineToLinearHeading(new Pose2d(-40.46, -41.93, Math.toRadians(291.8))); this.robot.getDrive().followTrajectorySequence(builder.build()); openAndLiftClaw(); - - builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(this.rendezvous); - this.robot.getDrive().followTrajectorySequence(builder.build()); } protected void propCenter() { - this.sleep(5000); + this.sleep(delay); + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToConstantHeading(rendezvous.vec()); - builder.addDisplacementMarker(10, () -> { - this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); - }); + builder.lineToLinearHeading(new Pose2d(-36, -42, initialPosition.getHeading())); this.robot.getDrive().followTrajectorySequence(builder.build()); openAndLiftClaw(); - - builder = this.robot.getTrajectorySequenceBuilder(); - builder.turn(Math.toRadians(-90)); - this.robot.getDrive().followTrajectorySequence(builder.build()); } - // propRight will be the reverse of BlueFrontpropRight protected void propRight() { - this.sleep(5000); + this.sleep(delay); + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(new Pose2d(-52, -31, Math.toRadians(-180))); - builder.lineToConstantHeading(new Vector2d(-42, -31)); - builder.addTemporalMarker(0.5, () -> { - this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); - }); + builder.lineToLinearHeading(new Pose2d(-41.82, -33.68, Math.toRadians(180))); this.robot.getDrive().followTrajectorySequence(builder.build()); openAndLiftClaw(); - - builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(this.rendezvous); - this.robot.getDrive().followTrajectorySequence(builder.build()); } } diff --git a/TeamCode/src/main/java/opmodes/Test.java b/TeamCode/src/main/java/opmodes/Test.java index a9fe231..eae7ae6 100644 --- a/TeamCode/src/main/java/opmodes/Test.java +++ b/TeamCode/src/main/java/opmodes/Test.java @@ -1,20 +1,17 @@ package opmodes; -import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.FORWARD_OFFSET_IN; +import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CAMERA_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 static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CAMERA_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 { @@ -41,7 +38,7 @@ public class Test extends OpMode { boolean slowmode = gamepad1.right_bumper || gamepad1.y; this.robot.getDrive().setInput(gamepad1, gamepad2, slowmode); - Pose2d poseFromAprilTag = this.robot.getCamera().getPoseFromAprilTag(2, 5); + Pose2d poseFromAprilTag = this.robot.getCamera().estimatePoseFromAprilTag(); dashboard.getTelemetry().addData("Inferred Position", poseFromAprilTag); dashboard.getTelemetry().update(); @@ -96,7 +93,7 @@ public class Test extends OpMode { ? 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); + target = new Pose2d(Camera.tag2Pose.getX() - SCORING_DISTANCE_FROM_APRIL_TAG - CAMERA_FORWARD_OFFSET_IN, y - CAMERA_SIDE_OFFSET_IN, 0); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToLinearHeading(target); this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java index dc57d1b..eb4e043 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java @@ -1,19 +1,24 @@ 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.CAMERA_FORWARD_OFFSET_IN; +import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CAMERA_ROTATION_DEG; +import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CAMERA_SIDE_OFFSET_IN; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.WEBCAM_MINI_NAME; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.WEBCAM_NAME; import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_DETECTION; +import static java.lang.Math.cos; +import static java.lang.Math.sin; +import static java.lang.Math.tan; + 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.apache.commons.math3.ml.neuralnet.twod.util.TopographicErrorHistogram; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.external.matrices.VectorF; import org.firstinspires.ftc.teamcode.util.CenterStageCommon; import org.firstinspires.ftc.teamcode.vision.Detection; import org.firstinspires.ftc.teamcode.vision.PropDetectionPipeline; @@ -22,12 +27,14 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagPoseFtc; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; -import java.util.Arrays; +import java.util.List; + +import lombok.NonNull; @Config public class Camera { - public static float PROP_REJECTION_VERTICAL_UPPER = 10; - public static float PROP_REJECTION_VERTICAL_LOWER = 470; + public static float PROP_REJECTION_VERTICAL_UPPER = 480f * 0.33f; + public static float PROP_REJECTION_VERTICAL_LOWER = 440; public static float PROP_REJECTION_HORIZONTAL_LEFT = 10; public static float PROP_REJECTION_HORIZONTAL_RIGHT = 630; private PropDetectionPipeline prop; @@ -91,27 +98,13 @@ public class Camera { return INVALID_DETECTION; } - public AprilTagDetection getAprilTag(int ... ids) { + public AprilTagDetection getAprilTag() { return this.aprilTag.getDetections() .stream() - .filter(x -> Arrays.stream(ids).filter(id -> x.id == id).count() > 0) .findFirst() .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); } @@ -120,36 +113,71 @@ public class Camera { return this.prop != null ? this.prop.getAlliance() : null; } - public Pose2d getPoseFromAprilTag(int ... ids) { - if (ids == null || ids.length == 0) { - ids = new int[]{2, 5}; - } + public Pose2d estimatePoseFromAprilTag() { + List aprilTagDetections = aprilTag.getDetections(); - AprilTagDetection aprilTagDetection = getAprilTag(ids); - - if (aprilTagDetection == null) { + if (aprilTagDetections == null || aprilTagDetections.isEmpty()) { return null; } - AprilTagPoseFtc ftcPose = aprilTagDetection.ftcPose; +// return estimatePoseFromAprilTag(aprilTagDetections.get(0)); - 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; + int numDetections = aprilTagDetections.size(); + Pose2d acc = new Pose2d(0, 0, 0); + for (AprilTagDetection aprilTagDetection : aprilTagDetections) { + acc = acc.plus(estimatePoseFromAprilTag(aprilTagDetection)); } - return new Pose2d(ourPoseX, ourPoseY, ftcPose.bearing); + return acc.div(numDetections); + } + + private Pose2d estimatePoseFromAprilTag(@NonNull AprilTagDetection aprilTagDetection) { + VectorF reference = aprilTagDetection.metadata.fieldPosition; + AprilTagPoseFtc ftcPose = aprilTagDetection.ftcPose; + + double ax = reference.get(0); + double ay = reference.get(1); + double t = -Math.toRadians(ftcPose.yaw); + double r = -ftcPose.range; + double b = Math.toRadians(ftcPose.bearing); + + double x1 = r * cos(b); + double y1 = r * sin(b); + + double x2 = x1 * cos(t) - y1 * sin(t); + double y2 = x1 * sin(t) + y1 * cos(t); + + double cx = ax + x2; + double cy = ay + y2; + + double t1 = t + Math.toRadians(CAMERA_ROTATION_DEG); + double h = tan(t1); + + double rx = -CAMERA_FORWARD_OFFSET_IN * cos(t1) + CAMERA_SIDE_OFFSET_IN * sin(t1); + double ry = -CAMERA_FORWARD_OFFSET_IN * sin(t1) - CAMERA_SIDE_OFFSET_IN * cos(t1); + + rx += cx; + ry += cy; + +// AprilTagDetection foo = aprilTagDetection; +// telemetry.addData("id", foo.id); +// telemetry.addData("ax", foo.metadata.fieldPosition.get(0)); +// telemetry.addData("ay", foo.metadata.fieldPosition.get(1)); +// telemetry.addData("yaw", foo.ftcPose.yaw); +// telemetry.addData("range", foo.ftcPose.range); +// telemetry.addData("bearing", foo.ftcPose.bearing); +// telemetry.addData("x1", x1); +// telemetry.addData("y1", y1); +// telemetry.addData("x2", x2); +// telemetry.addData("y2", y2); +// telemetry.addData("cx", cx); +// telemetry.addData("cy", cy); +// telemetry.addData("t1", t1); +// telemetry.addData("h", h); +// telemetry.addData("rx", rx); +// telemetry.addData("ry", ry); +// telemetry.update(); + + return new Pose2d(rx, ry, h); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java index ba54bc3..63e57e7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.hardware; +import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_ARM_KP; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_ARM_LEFT_NAME; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_ARM_RIGHT_NAME; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_KP; @@ -22,6 +23,8 @@ public class Claw implements Updatable { private Telemetry telemetry; PController clawController = new PController(CLAW_KP); private double clawControllerTarget; + PController armController = new PController(CLAW_ARM_KP); + private double armControllerTarget = -1; public Claw(HardwareMap hardwareMap) { this.claw = hardwareMap.get(Servo.class, CLAW_NAME); @@ -58,15 +61,32 @@ public class Claw implements Updatable { this.armRight.setPosition(target); } + public void setArmPositionAsync(double armControllerTarget) { + this.armControllerTarget = armControllerTarget; + } + + public boolean isArmAtPosition() { + return this.armController.atSetPoint(); + } public void update() { this.clawController.setSetPoint(this.clawControllerTarget); this.clawController.setTolerance(0.001); this.clawController.setP(CLAW_KP); - double output = 0; + this.armController.setSetPoint(this.armControllerTarget); + this.armController.setP(CLAW_ARM_KP); + if (!this.clawController.atSetPoint()) { + double output = 0; output = Math.max(-1 * CLAW_MAX, Math.min(CLAW_MAX, this.clawController.calculate(claw.getPosition()))); this.claw.setPosition(this.claw.getPosition() + output); } + + if (this.armControllerTarget > 0 && !this.armController.atSetPoint()) { + double output = 0; + output = Math.max(-1 * PICKUP_ARM_MAX, Math.min(PICKUP_ARM_MAX, this.armController.calculate(armLeft.getPosition()))); + this.armLeft.setPosition(this.armLeft.getPosition() + output); + this.armRight.setPosition(this.armRight.getPosition() + output); + } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java index 2677689..9c56d8c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java @@ -32,12 +32,13 @@ public class RobotConfig { public static double AUTO_STRAFE_SLOWDOWN = 4; // Arm - public static double PICKUP_ARM_MIN = 0.185; + public static double PICKUP_ARM_MIN = 0.175; public static double PICKUP_ARM_MAX = 0.760; public static double CLAW_MIN = 0.89; public static double CLAW_MAX = 0.65; public static double CLAW_ARM_DELTA = 0.03; public static double CLAW_KP = 0.3; + public static double CLAW_ARM_KP = 0.15; // Gantry public static double GANTRY_ARM_MIN = 0.42; @@ -68,7 +69,8 @@ public class RobotConfig { // Vision public static double DETECTION_AREA_MIN = 0.05f; public static double DETECTION_AREA_MAX = 0.8f; - public static double SCORING_DISTANCE_FROM_APRIL_TAG = 7f; - public static final double FORWARD_OFFSET_IN = 7.75; - public static final double SIDE_OFFSET_IN = 0.5; + public static double SCORING_DISTANCE_FROM_APRIL_TAG = 6.25f; + public static final double CAMERA_FORWARD_OFFSET_IN = 7.77; + public static final double CAMERA_SIDE_OFFSET_IN = 0.707; + public static final double CAMERA_ROTATION_DEG = 0; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java index bf936f5..95419ac 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java @@ -74,9 +74,9 @@ public class DriveConstants { * Adjust the orientations here to match your robot. See the FTC SDK documentation for details. */ public static RevHubOrientationOnRobot.LogoFacingDirection LOGO_FACING_DIR = - RevHubOrientationOnRobot.LogoFacingDirection.UP; + RevHubOrientationOnRobot.LogoFacingDirection.LEFT; public static RevHubOrientationOnRobot.UsbFacingDirection USB_FACING_DIR = - RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; + RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD; public static double encoderTicksToInches(double ticks) {