diff --git a/TeamCode/src/main/java/opmodes/AutoBase.java b/TeamCode/src/main/java/opmodes/AutoBase.java index de9b536..485317c 100644 --- a/TeamCode/src/main/java/opmodes/AutoBase.java +++ b/TeamCode/src/main/java/opmodes/AutoBase.java @@ -140,11 +140,11 @@ public abstract class AutoBase extends LinearOpMode { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); if (this.alliance == CenterStageCommon.Alliance.Blue) { - builder.lineToLinearHeading(new Pose2d(36, 11, 0)); - builder.lineToLinearHeading(new Pose2d(36, 38, 0)); + builder.lineToLinearHeading(new Pose2d(35, 11, 0)); + builder.lineToLinearHeading(new Pose2d(35, 38, 0)); } else { - builder.lineToLinearHeading(new Pose2d(36, -11, 0)); - builder.lineToLinearHeading(new Pose2d(36, -38, 0)); + builder.lineToLinearHeading(new Pose2d(35, -11, 0)); + builder.lineToLinearHeading(new Pose2d(35, -36, 0)); } this.robot.getDrive().followTrajectorySequence(builder.build()); diff --git a/TeamCode/src/main/java/opmodes/MainTeleOp.java b/TeamCode/src/main/java/opmodes/MainTeleOp.java index 1a4dc11..19af9fb 100644 --- a/TeamCode/src/main/java/opmodes/MainTeleOp.java +++ b/TeamCode/src/main/java/opmodes/MainTeleOp.java @@ -8,10 +8,12 @@ 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.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.hardware.Robot; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; @TeleOp(name = "MainTeleOp", group = "Main") public class MainTeleOp extends OpMode { @@ -50,7 +52,7 @@ public class MainTeleOp extends OpMode { boolean clawDown = gamepad2.a || clawDownSafe; // A // Robot Drone Launch - boolean robotDroneLaunch = gamepad1.left_bumper; // Change if Merck wants (LT) + boolean robotDroneLaunch = gamepad1.left_bumper && gamepad1.right_bumper; // Change if Merck wants (LT) // Robot Lift boolean robotLiftRotation = gamepad2.right_trigger > 0.05; // RT diff --git a/TeamCode/src/main/java/opmodes/RedFrontStage.java b/TeamCode/src/main/java/opmodes/RedFrontStage.java index 4764f12..8de80a6 100644 --- a/TeamCode/src/main/java/opmodes/RedFrontStage.java +++ b/TeamCode/src/main/java/opmodes/RedFrontStage.java @@ -17,7 +17,7 @@ public class RedFrontStage extends AutoBase { super( CenterStageCommon.Alliance.Red, new Pose2d(-36, -63, Math.toRadians(90)), - new Pose2d(62, -12)); + new Pose2d(61, -12)); } // propLeft will be a reverse of BlueFrontpropRight 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 aac79e1..fa29457 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 @@ -19,7 +19,7 @@ import org.opencv.core.Point; @Config public class Camera { public static float PROP_REJECTION_VERTICAL_UPPER = 150; - public static float PROP_REJECTION_VERTICAL_LOWER = 250; + public static float PROP_REJECTION_VERTICAL_LOWER = 275; private PropDetectionPipeline prop; private AprilTagProcessor aprilTag; private VisionPortal visionPortal; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java index ec695a2..218b56d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java @@ -101,7 +101,7 @@ public class Gantry { } public void intake() { - this.screwServo.setPower(-1); + this.screwServo.setPower(1); } public void deposit() {