diff --git a/TeamCode/src/main/java/opmodes/AutoBase.java b/TeamCode/src/main/java/opmodes/AutoBase.java index 6bbd0f9..de9b536 100644 --- a/TeamCode/src/main/java/opmodes/AutoBase.java +++ b/TeamCode/src/main/java/opmodes/AutoBase.java @@ -26,6 +26,7 @@ public abstract class AutoBase extends LinearOpMode { public static double SCORING_DURATION_S = 5f; public static double APRIL_TAG_RIGHT_DELTA = -8.5; public static double APRIL_TAG_LEFT_DELTA = 7.0; + protected static double Delay = 5000; protected Robot robot; protected FtcDashboard dashboard; @@ -35,6 +36,7 @@ public abstract class AutoBase extends LinearOpMode { protected final CenterStageCommon.Alliance alliance; protected final Pose2d park; + protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Pose2d park) { this.alliance = alliance; this.initialPosition = initialPosition; @@ -53,7 +55,6 @@ public abstract class AutoBase extends LinearOpMode { this.robot.update(); this.sleep(20); } - this.sleep(5000); if (isStopRequested()) { return; } diff --git a/TeamCode/src/main/java/opmodes/BlueFrontStage.java b/TeamCode/src/main/java/opmodes/BlueFrontStage.java index b49ff00..23c4f97 100644 --- a/TeamCode/src/main/java/opmodes/BlueFrontStage.java +++ b/TeamCode/src/main/java/opmodes/BlueFrontStage.java @@ -21,6 +21,7 @@ public class BlueFrontStage extends AutoBase { } protected void propLeft() { + this.sleep(5000); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToLinearHeading(new Pose2d(-52, 31, Math.toRadians(-180))); builder.lineToConstantHeading(new Vector2d(-42, 31)); @@ -36,6 +37,7 @@ public class BlueFrontStage extends AutoBase { this.robot.getDrive().followTrajectorySequence(builder.build()); } protected void propCenter() { + this.sleep(5000); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToConstantHeading(rendezvous.vec()); builder.addDisplacementMarker(10, () -> { @@ -50,6 +52,7 @@ public class BlueFrontStage extends AutoBase { this.robot.getDrive().followTrajectorySequence(builder.build()); } protected void propRight() { + this.sleep(5000); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToLinearHeading(new Pose2d(-54, 17, Math.toRadians(-123))); builder.addDisplacementMarker(10, () -> { diff --git a/TeamCode/src/main/java/opmodes/RedBackStage.java b/TeamCode/src/main/java/opmodes/RedBackStage.java index f544091..c729c66 100644 --- a/TeamCode/src/main/java/opmodes/RedBackStage.java +++ b/TeamCode/src/main/java/opmodes/RedBackStage.java @@ -48,8 +48,9 @@ public class RedBackStage extends AutoBase { } protected void propRight() { + // red back side TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(new Pose2d(36, 25, Math.toRadians(-33))); + builder.lineToLinearHeading(new Pose2d(36, -25, Math.toRadians(-33))); builder.addDisplacementMarker(10, () -> { this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); }); diff --git a/TeamCode/src/main/java/opmodes/RedFrontStage.java b/TeamCode/src/main/java/opmodes/RedFrontStage.java index 508ec43..4764f12 100644 --- a/TeamCode/src/main/java/opmodes/RedFrontStage.java +++ b/TeamCode/src/main/java/opmodes/RedFrontStage.java @@ -22,6 +22,7 @@ public class RedFrontStage extends AutoBase { // propLeft will be a reverse of BlueFrontpropRight protected void propLeft() { + this.sleep(5000); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToLinearHeading(new Pose2d(-58, -17, Math.toRadians(123))); builder.addDisplacementMarker(10, () -> { @@ -37,6 +38,7 @@ public class RedFrontStage extends AutoBase { } protected void propCenter() { + this.sleep(5000); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToConstantHeading(rendezvous.vec()); builder.addDisplacementMarker(10, () -> { @@ -53,6 +55,7 @@ public class RedFrontStage extends AutoBase { // propRight will be the reverse of BlueFrontpropRight protected void propRight() { + this.sleep(5000); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToLinearHeading(new Pose2d(-52, -31, Math.toRadians(-180))); builder.lineToConstantHeading(new Vector2d(-42, -31)); 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 886eb54..aac79e1 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 @@ -18,8 +18,8 @@ import org.opencv.core.Point; @Config public class Camera { - public static float PROP_REJECTION_VERTICAL_UPPER = 250; - public static float PROP_REJECTION_VERTICAL_LOWER = 370; + public static float PROP_REJECTION_VERTICAL_UPPER = 150; + public static float PROP_REJECTION_VERTICAL_LOWER = 250; private PropDetectionPipeline prop; private AprilTagProcessor aprilTag; private VisionPortal visionPortal;