diff --git a/TeamCode/src/main/java/opmodes/RedFrontStage.java b/TeamCode/src/main/java/opmodes/RedFrontStage.java new file mode 100644 index 0000000..d3c4371 --- /dev/null +++ b/TeamCode/src/main/java/opmodes/RedFrontStage.java @@ -0,0 +1,64 @@ +package opmodes; + +import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; +import org.firstinspires.ftc.teamcode.util.CenterStageCommon; + +@Autonomous(name = "RedFrontStage", preselectTeleOp = "MainTeleOp") +public class RedFrontStage extends AutoBase { + private final Pose2d rendezvous = new Pose2d(-36, -11); + + public RedFrontStage() { + super( + CenterStageCommon.Alliance.Red, + new Pose2d(-36, -63, Math.toRadians(90)), + new Pose2d(-12, 62)); + } + + protected void propLeft() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(new Pose2d(-54, -17, Math.toRadians(123))); + builder.addDisplacementMarker(10, () -> { + this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); + }); + this.robot.getDrive().followTrajectorySequence(builder.build()); + + openAndLiftClaw(); + + builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(this.rendezvous); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void propCenter() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToConstantHeading(rendezvous.vec()); + builder.addDisplacementMarker(10, () -> { + this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); + }); + 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() { + 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); + }); + this.robot.getDrive().followTrajectorySequence(builder.build()); + + openAndLiftClaw(); + } +}