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

Make AutoBase nicer

This commit is contained in:
Scott Barnes 2024-02-09 11:16:10 -06:00
parent 6cfd2876bd
commit c4bd3099d3
6 changed files with 192 additions and 174 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

197
TeamCode/src/main/java/opmodes/AutoBase.java
View File

@ -9,10 +9,14 @@ import static org.firstinspires.ftc.teamcode.util.CenterStageCommon.PropLocation
import static org.firstinspires.ftc.teamcode.util.CenterStageCommon.PropLocation.Right; import static org.firstinspires.ftc.teamcode.util.CenterStageCommon.PropLocation.Right;
import static org.firstinspires.ftc.teamcode.util.CenterStageCommon.PropLocation.Unknown; import static org.firstinspires.ftc.teamcode.util.CenterStageCommon.PropLocation.Unknown;
import android.annotation.SuppressLint;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d; import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.arcrobotics.ftclib.gamepad.GamepadKeys;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.Telemetry;
@ -23,82 +27,60 @@ import org.firstinspires.ftc.teamcode.vision.Detection;
@Config @Config
public abstract class AutoBase extends LinearOpMode { public abstract class AutoBase extends LinearOpMode {
public static int DEPOSIT_HEIGHT = 100; protected AutoConfig config;
public static double SCORING_DURATION_S = 2f; // for spin of axle
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 Robot robot;
protected FtcDashboard dashboard;
protected Telemetry dashboardTelemetry;
protected CenterStageCommon.PropLocation propLocation;
protected final Pose2d initialPosition;
protected final CenterStageCommon.Alliance alliance;
protected final Pose2d parkLeft;
protected final Pose2d parkRight;
protected Pose2d park; protected AutoBase(AutoConfig config) {
protected int delay = 0; this.config = config;
boolean leftWasPressed = false;
boolean rightWasPressed = false;
boolean upWasPressed = false;
boolean downWasPressed = false;
protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Pose2d parkLeft, Pose2d parkRight) {
this.alliance = alliance;
this.initialPosition = initialPosition;
this.parkLeft = parkLeft;
this.parkRight = parkRight;
} }
@SuppressLint("DefaultLocale")
@Override @Override
public void runOpMode() { public void runOpMode() {
// Initialize Robot and Dashboard // Initialize Robot and Dashboard
this.robot = new Robot(hardwareMap, telemetry, initialPosition, alliance); this.robot = new Robot(hardwareMap, telemetry, this.config.getInitialPosition(), this.config.getAlliance());
this.dashboard = FtcDashboard.getInstance(); GamepadEx controller1 = new GamepadEx(this.gamepad1);
this.dashboardTelemetry = dashboard.getTelemetry(); GamepadEx controller2 = new GamepadEx(this.gamepad2);
this.park = parkLeft;
// Wait for match to start // Wait for match to start
while(!isStarted() && !isStopRequested()) { while (!isStarted() && !isStopRequested()) {
this.robot.update(); this.robot.update();
controller1.readButtons();
controller2.readButtons();
boolean leftPressed = gamepad1.dpad_left; if (controller1.wasJustPressed(GamepadKeys.Button.DPAD_LEFT)
boolean rightPressed = gamepad1.dpad_right; || controller2.wasJustPressed(GamepadKeys.Button.DPAD_LEFT)) {
boolean upPressed = gamepad1.dpad_up; this.config.setParkLocation(AutoConfig.ParkLocation.Left);
boolean downPressed = gamepad1.dpad_down; }
this.telemetry.addData("To select parking location, use the dpad right or left. To add delay, use the dpad up to increase delay, and dpad down to decrease delay", ""); if (controller1.wasJustPressed(GamepadKeys.Button.DPAD_RIGHT)
if(leftPressed && !leftWasPressed) { || controller2.wasJustPressed(GamepadKeys.Button.DPAD_RIGHT)) {
this.park = parkLeft; this.config.setParkLocation(AutoConfig.ParkLocation.Right);
} else if(rightPressed && !rightWasPressed) {
this.park = parkRight;
} else if(upPressed && !upWasPressed) {
this.delay += 1000;
} else if(downPressed && !downWasPressed) {
this.delay -= 1000;
} }
this.leftWasPressed = leftPressed; if (controller1.wasJustPressed(GamepadKeys.Button.DPAD_UP)
this.rightWasPressed = rightPressed; || controller2.wasJustPressed(GamepadKeys.Button.DPAD_UP)) {
this.upWasPressed = upPressed; this.config.increaseDelay();
this.downWasPressed = downPressed; }
if (controller1.wasJustPressed(GamepadKeys.Button.DPAD_DOWN)
|| controller2.wasJustPressed(GamepadKeys.Button.DPAD_DOWN)) {
this.config.decreaseDelay();
}
this.telemetry.addData("Delay", this.delay); this.telemetry.addLine("Press DPAD_UP to increase delay");
this.telemetry.addData("Park set to", this.park); this.telemetry.addLine("Press DPAD_DOWN to decrease delay");
this.telemetry.addLine("Press DPAD_LEFT to park on the left");
this.telemetry.addLine("Press DPAD_RIGHT to park on the right");
this.telemetry.addData("Delay", String.format("%d second(s)", this.config.getDelay() / 1000));
this.telemetry.addData("Park set to", this.config.getParkLocation());
this.sleep(20); this.sleep(20);
} }
if (isStopRequested()) { // remove later if nessacary as recent addition might be interefering if (isStopRequested()) {
return; return;
} }
// If the prop is visible at this point, then it must be in the center (2) position CenterStageCommon.PropLocation propLocation = determinePropLocation();
determinePropLocation(); switch (propLocation) {
switch (this.propLocation) {
case Left: case Left:
propLeft(); propLeft();
break; break;
@ -110,9 +92,10 @@ public abstract class AutoBase extends LinearOpMode {
propRight(); propRight();
break; break;
} }
this.sleep(delay);
this.sleep(this.config.getDelay());
moveBackstage(); moveBackstage();
prepareToScore(); prepareToScore(propLocation);
scorePreloadedPixel(); scorePreloadedPixel();
park(); park();
} }
@ -123,6 +106,8 @@ public abstract class AutoBase extends LinearOpMode {
protected abstract void propRight(); protected abstract void propRight();
protected void moveBackstage() { }
protected void openAndLiftClaw() { protected void openAndLiftClaw() {
this.robot.getClaw().openSync(); this.robot.getClaw().openSync();
this.sleep(100); this.sleep(100);
@ -130,15 +115,15 @@ public abstract class AutoBase extends LinearOpMode {
} }
protected void scorePreloadedPixel() { protected void scorePreloadedPixel() {
this.robot.getGantry().setSlideTarget(DEPOSIT_HEIGHT); this.robot.getGantry().setSlideTarget(AutoConfig.DEPOSIT_HEIGHT);
this.robot.getGantry().armOut(); this.robot.getGantry().armOut();
while(this.robot.getGantry().isIn()) { while (this.robot.getGantry().isIn()) {
this.robot.update(); this.robot.update();
sleep(20); sleep(20);
} }
this.robot.getGantry().deposit(); this.robot.getGantry().deposit();
double startTime = this.getRuntime(); double startTime = this.getRuntime();
while (this.getRuntime() < (startTime + SCORING_DURATION_S)) { while (this.getRuntime() < (startTime + AutoConfig.SCORING_DURATION_S)) {
this.robot.update(); this.robot.update();
} }
this.robot.getGantry().stop(); this.robot.getGantry().stop();
@ -146,22 +131,18 @@ public abstract class AutoBase extends LinearOpMode {
this.robot.getGantry().armInSync(); this.robot.getGantry().armInSync();
} }
protected void prepareToScore() { protected void prepareToScore(CenterStageCommon.PropLocation propLocation) {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
this.robot.getGantry().armOut(); this.robot.getGantry().armOut();
this.robot.getGantry().setSlideTarget(DEPOSIT_HEIGHT); this.robot.getGantry().setSlideTarget(AutoConfig.DEPOSIT_HEIGHT);
if (this.alliance == CenterStageCommon.Alliance.Blue) { builder.lineToLinearHeading(new Pose2d(35, Math.copySign(36, this.config.getInitialPosition().getY()), 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().followTrajectorySequenceAsync(builder.build()); this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
Pose2d inferredPos = null; Pose2d inferredPos;
while(this.robot.getDrive().isBusy()) { while (this.robot.getDrive().isBusy()) {
this.robot.update(); this.robot.update();
inferredPos = this.robot.getCamera().estimatePoseFromAprilTag(); inferredPos = this.robot.getCamera().estimatePoseFromAprilTag();
@ -176,53 +157,34 @@ public abstract class AutoBase extends LinearOpMode {
inferredPos = this.robot.getDrive().getPoseEstimate(); inferredPos = this.robot.getDrive().getPoseEstimate();
} }
// At this point we know that Y = 38
// For 2 -> Ydelta = 0
// For 3 -> 3 5/8
// For 1 -> - 3 5/8
double delta = 0; double delta = 0;
switch (this.propLocation) { switch (propLocation) {
case Left: case Left:
delta = APRIL_TAG_LEFT_DELTA; delta = AutoConfig.APRIL_TAG_LEFT_DELTA;
break; break;
case Unknown: case Unknown:
case Center: case Center:
delta = 0; delta = 0;
break; break;
case Right: case Right:
delta = APRIL_TAG_RIGHT_DELTA; delta = AutoConfig.APRIL_TAG_RIGHT_DELTA;
break; break;
} }
builder = this.robot.getDrive().trajectorySequenceBuilder(inferredPos); builder = this.robot.getDrive().trajectorySequenceBuilder(inferredPos);
Pose2d target = new Pose2d( Pose2d target = new Pose2d(
60 - SCORING_DISTANCE_FROM_APRIL_TAG - CAMERA_FORWARD_OFFSET_IN, // 60 is the X position of the april tag 60 - SCORING_DISTANCE_FROM_APRIL_TAG - CAMERA_FORWARD_OFFSET_IN, // 60 is the X position of the april tag
36 + delta, Math.copySign(36, this.config.getInitialPosition().getY()) + delta,
0); 0);
builder.lineToLinearHeading(target); builder.lineToLinearHeading(target);
this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
while(this.robot.getDrive().isBusy()) { while (this.robot.getDrive().isBusy()) {
this.robot.update(); this.robot.update();
} }
} }
protected void moveBackstage() { protected CenterStageCommon.PropLocation determinePropLocation() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); this.robot.getClaw().setArmPositionAsync(PICKUP_ARM_MIN);
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));
}
this.robot.getDrive().followTrajectorySequence(builder.build());
}
}
protected void determinePropLocation() {
this.robot.getClaw().setArmPositionAsync(PICKUP_ARM_MIN); // changed from setArmPositionAsync
while (!this.robot.getClaw().isArmAtPosition() && !this.robot.getCamera().getProp().isValid()) { while (!this.robot.getClaw().isArmAtPosition() && !this.robot.getCamera().getProp().isValid()) {
this.robot.update(); this.robot.update();
@ -231,26 +193,24 @@ public abstract class AutoBase extends LinearOpMode {
sleep(250); sleep(250);
setPropLocationIfVisible(Center, Unknown); //only works if arm is async CenterStageCommon.PropLocation propLocation = getPropLocationIfVisible(Center, Unknown);
if (this.propLocation != Center) { if (propLocation != Center) {
peekRight(); 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);
propLocation = getPropLocationIfVisible(Right, Left);
} }
return propLocation;
} }
protected void peekRight() { private CenterStageCommon.PropLocation getPropLocationIfVisible(CenterStageCommon.PropLocation ifVisible, CenterStageCommon.PropLocation ifNotVisible) {
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);
}
protected void setPropLocationIfVisible(CenterStageCommon.PropLocation ifVisible, CenterStageCommon.PropLocation ifNotVisible) {
float seenCount = 0; float seenCount = 0;
float samples = 10; float samples = 10;
for (int i = 0; i < samples; i++) { for (int i = 0; i < samples; i++) {
@ -260,21 +220,18 @@ public abstract class AutoBase extends LinearOpMode {
} }
} }
if (seenCount / samples > 0.5) { if (seenCount / samples > 0.5) {
this.propLocation = ifVisible; return ifVisible;
} else { } else {
this.propLocation = ifNotVisible; return ifNotVisible;
} }
} }
public void park() { private void park() {
Pose2d park = this.config.getSelectionParkPosition();
double currentX = this.robot.getDrive().getPoseEstimate().getX(); double currentX = this.robot.getDrive().getPoseEstimate().getX();
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.strafeTo(new Vector2d(currentX, park.getY())); builder.strafeTo(new Vector2d(currentX, park.getY()));
builder.lineToLinearHeading(park); builder.lineToLinearHeading(park);
this.robot.getDrive().followTrajectorySequence(builder.build()); this.robot.getDrive().followTrajectorySequence(builder.build());
} }
protected boolean isBackstage() {
return this.initialPosition.getX() > 0;
}
} }

59
TeamCode/src/main/java/opmodes/AutoConfig.java Normal file
View File

@ -0,0 +1,59 @@
package opmodes;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
import lombok.AllArgsConstructor;
import lombok.Builder;
import lombok.Data;
import lombok.NoArgsConstructor;
@AllArgsConstructor
@NoArgsConstructor
@Builder
@Data
public class AutoConfig {
public static int DEPOSIT_HEIGHT = 100;
public static double SCORING_DURATION_S = 2f;
public static double APRIL_TAG_RIGHT_DELTA = -8.5;
public static double APRIL_TAG_LEFT_DELTA = 7.0;
private Pose2d initialPosition;
private Pose2d leftParkPosition;
private Pose2d rightParkPosition;
private ParkLocation parkLocation = ParkLocation.Left;
private long delay;
public Pose2d getSelectionParkPosition() {
return this.parkLocation == ParkLocation.Left
? this.leftParkPosition
: this.rightParkPosition;
}
protected CenterStageCommon.Alliance getAlliance() {
if (this.initialPosition == null) {
throw new RuntimeException("Thou fool! Thou must set the initial position!");
}
return this.getInitialPosition().getY() > 0
? CenterStageCommon.Alliance.Blue
: CenterStageCommon.Alliance.Red;
}
public void increaseDelay() {
this.delay += 1000;
}
public void decreaseDelay() {
this.delay -= 1000;
}
protected boolean isBackstage() {
return this.initialPosition.getX() > 0;
}
public enum ParkLocation {
Left,
Right
}
}

24
TeamCode/src/main/java/opmodes/BlueBackStage.java
View File

@ -1,29 +1,25 @@
package opmodes; package opmodes;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN;
import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d; import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Autonomous(name = "BlueBackStage", preselectTeleOp = "MainTeleOp") @Autonomous(name = "BlueBackStage", preselectTeleOp = "MainTeleOp")
public class BlueBackStage extends AutoBase { public class BlueBackStage extends AutoBase {
public BlueBackStage() { public BlueBackStage() {
super( super(AutoConfig.builder()
CenterStageCommon.Alliance.Blue, .initialPosition(new Pose2d(12, 63, Math.toRadians(90)))
new Pose2d(12, 63, Math.toRadians(90)), .leftParkPosition(new Pose2d(62, 62))
new Pose2d(62, 62), .rightParkPosition(new Pose2d(62, 12, Math.toRadians(0)))
new Pose2d(62, 12, Math.toRadians(0))); .parkLocation(AutoConfig.ParkLocation.Right)
.build());
} }
@Override
protected void propLeft() { protected void propLeft() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(17.3, 41.6, Math.toRadians(108.25))); builder.lineToLinearHeading(new Pose2d(17.3, 41.6, Math.toRadians(108.25)));
this.robot.getDrive().followTrajectorySequence(builder.build()); this.robot.getDrive().followTrajectorySequence(builder.build());
@ -35,17 +31,17 @@ public class BlueBackStage extends AutoBase {
this.robot.getDrive().followTrajectorySequence(builder.build()); this.robot.getDrive().followTrajectorySequence(builder.build());
} }
@Override
protected void propCenter() { protected void propCenter() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(12, 42, initialPosition.getHeading())); builder.lineToLinearHeading(new Pose2d(12, 42, this.config.getInitialPosition().getHeading()));
this.robot.getDrive().followTrajectorySequence(builder.build()); this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw(); openAndLiftClaw();
} }
@Override
protected void propRight() { protected void propRight() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(18.25, 32, Math.toRadians(0))); builder.lineToLinearHeading(new Pose2d(18.25, 32, Math.toRadians(0)));
this.robot.getDrive().followTrajectorySequence(builder.build()); this.robot.getDrive().followTrajectorySequence(builder.build());

32
TeamCode/src/main/java/opmodes/BlueFrontStage.java
View File

@ -1,28 +1,24 @@
package opmodes; package opmodes;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN;
import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Autonomous(name = "BlueFrontStage", preselectTeleOp = "MainTeleOp") @Autonomous(name = "BlueFrontStage", preselectTeleOp = "MainTeleOp")
public class BlueFrontStage extends AutoBase { public class BlueFrontStage extends AutoBase {
public BlueFrontStage() { public BlueFrontStage() {
super( super(AutoConfig.builder()
CenterStageCommon.Alliance.Blue, .initialPosition(new Pose2d(-36, 63, Math.toRadians(90)))
new Pose2d(-36, 63, Math.toRadians(90)), .leftParkPosition(new Pose2d(62, 62))
new Pose2d(62, 62), .rightParkPosition(new Pose2d(62, 12, Math.toRadians(0)))
new Pose2d(62, 12, Math.toRadians(0))); .parkLocation(AutoConfig.ParkLocation.Left)
.build());
} }
@Override
protected void propLeft() { protected void propLeft() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-43.5, 31.5, Math.toRadians(180))); builder.lineToLinearHeading(new Pose2d(-43.5, 31.5, Math.toRadians(180)));
this.robot.getDrive().followTrajectorySequence(builder.build()); this.robot.getDrive().followTrajectorySequence(builder.build());
@ -30,21 +26,29 @@ public class BlueFrontStage extends AutoBase {
openAndLiftClaw(); openAndLiftClaw();
} }
@Override
protected void propCenter() { protected void propCenter() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-36, 42, initialPosition.getHeading())); builder.lineToLinearHeading(new Pose2d(-36, 42, this.config.getInitialPosition().getHeading()));
this.robot.getDrive().followTrajectorySequence(builder.build()); this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw(); openAndLiftClaw();
} }
@Override
protected void propRight() { protected void propRight() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-39.34, 40.45, Math.toRadians(64.3))); builder.lineToLinearHeading(new Pose2d(-39.34, 40.45, Math.toRadians(64.3)));
this.robot.getDrive().followTrajectorySequence(builder.build()); this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw(); openAndLiftClaw();
} }
@Override
protected void moveBackstage() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToSplineHeading(new Pose2d(-40, 60, Math.PI));
builder.lineToLinearHeading(new Pose2d(12, 60, Math.PI));
this.robot.getDrive().followTrajectorySequence(builder.build());
}
} }

22
TeamCode/src/main/java/opmodes/RedBackStage.java
View File

@ -1,27 +1,25 @@
package opmodes; package opmodes;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN;
import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d; import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Autonomous(name = "RedBackStage", preselectTeleOp = "MainTeleOp") @Autonomous(name = "RedBackStage", preselectTeleOp = "MainTeleOp")
public class RedBackStage extends AutoBase { public class RedBackStage extends AutoBase {
public RedBackStage() { public RedBackStage() {
super( super(AutoConfig.builder()
CenterStageCommon.Alliance.Red, .initialPosition(new Pose2d(12, -63, Math.toRadians(-90)))
new Pose2d(12, -63, Math.toRadians(-90)), .leftParkPosition(new Pose2d(61, -12))
new Pose2d(61, -12), .rightParkPosition(new Pose2d(62, -62))
new Pose2d(62, -62)); .parkLocation(AutoConfig.ParkLocation.Left)
.build());
} }
@Override
protected void propLeft() { protected void propLeft() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(18.25, -33.5, Math.toRadians(0))); builder.lineToLinearHeading(new Pose2d(18.25, -33.5, Math.toRadians(0)));
this.robot.getDrive().followTrajectorySequence(builder.build()); this.robot.getDrive().followTrajectorySequence(builder.build());
@ -29,17 +27,17 @@ public class RedBackStage extends AutoBase {
openAndLiftClaw(); openAndLiftClaw();
} }
@Override
protected void propCenter() { protected void propCenter() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(12, -42, initialPosition.getHeading())); builder.lineToLinearHeading(new Pose2d(12, -42, this.config.getInitialPosition().getHeading()));
this.robot.getDrive().followTrajectorySequence(builder.build()); this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw(); openAndLiftClaw();
} }
@Override
protected void propRight() { protected void propRight() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(15.9, -41.35, Math.toRadians(244.15))); builder.lineToLinearHeading(new Pose2d(15.9, -41.35, Math.toRadians(244.15)));
this.robot.getDrive().followTrajectorySequence(builder.build()); this.robot.getDrive().followTrajectorySequence(builder.build());

32
TeamCode/src/main/java/opmodes/RedFrontStage.java
View File

@ -1,28 +1,24 @@
package opmodes; package opmodes;
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN;
import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Autonomous(name = "RedFrontStage", preselectTeleOp = "MainTeleOp") @Autonomous(name = "RedFrontStage", preselectTeleOp = "MainTeleOp")
public class RedFrontStage extends AutoBase { public class RedFrontStage extends AutoBase {
public RedFrontStage() { public RedFrontStage() {
super( super(AutoConfig.builder()
CenterStageCommon.Alliance.Red, .initialPosition(new Pose2d(-36, -63, Math.toRadians(-90)))
new Pose2d(-36, -63, Math.toRadians(-90)), .leftParkPosition(new Pose2d(61, -12))
new Pose2d(61, -12), .rightParkPosition(new Pose2d(62, -62))
new Pose2d(62, -62)); .parkLocation(AutoConfig.ParkLocation.Right)
.build());
} }
@Override
protected void propLeft() { protected void propLeft() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-40.46, -41.93, Math.toRadians(291.8))); builder.lineToLinearHeading(new Pose2d(-40.46, -41.93, Math.toRadians(291.8)));
this.robot.getDrive().followTrajectorySequence(builder.build()); this.robot.getDrive().followTrajectorySequence(builder.build());
@ -30,21 +26,29 @@ public class RedFrontStage extends AutoBase {
openAndLiftClaw(); openAndLiftClaw();
} }
@Override
protected void propCenter() { protected void propCenter() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-36, -42, initialPosition.getHeading())); builder.lineToLinearHeading(new Pose2d(-36, -42, this.config.getInitialPosition().getHeading()));
this.robot.getDrive().followTrajectorySequence(builder.build()); this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw(); openAndLiftClaw();
} }
@Override
protected void propRight() { protected void propRight() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-41.82, -33.68, Math.toRadians(180))); builder.lineToLinearHeading(new Pose2d(-41.82, -33.68, Math.toRadians(180)));
this.robot.getDrive().followTrajectorySequence(builder.build()); this.robot.getDrive().followTrajectorySequence(builder.build());
openAndLiftClaw(); openAndLiftClaw();
} }
@Override
protected void moveBackstage() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToSplineHeading(new Pose2d(-40, -60, Math.PI));
builder.lineToLinearHeading(new Pose2d(12, -60, Math.PI));
this.robot.getDrive().followTrajectorySequence(builder.build());
}
} }