eaglesfe
/
robotcode
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

[WIP] RedFrontStageAuto

This commit is contained in:
Scott Barnes 2023-11-18 17:02:04 -06:00
parent fbb6eeca9b
commit 296a382c56
1 changed files with 64 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
TeamCode/src/main/java/opmodes/RedFrontStage.java Normal file
View File

@ -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();
}
}