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

wed before states, 2 backstage mostly working 2+4s

This commit is contained in:
sihan 2024-03-20 17:05:28 -05:00
parent 834b5c234f
commit da3701612b
9 changed files with 529 additions and 541 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java
View File

@ -461,7 +461,7 @@ public class Robot {
public static class Claw {
//Values
public static double OPEN = 0.65;
public static double BIGOPEN = .48;
public static double BIGOPEN = .42;
public static double CLOSE = 0.75;
public static double CLAW_MIN = 0;
public static double CLAW_MAX = 1;

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java
View File

@ -336,8 +336,8 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive
}
public void setInput(Gamepad gamepad1, Gamepad gamepad2) {
double speedScale = gamepad1.y ? SLOWMODE_SPEED : SPEED;
double turnScale = gamepad1.y ? SLOWMODE_TURN : TURN;
double speedScale = gamepad1.y || gamepad1.right_bumper ? SLOWMODE_SPEED : SPEED;
double turnScale = gamepad1.y || gamepad1.right_bumper ? SLOWMODE_TURN : TURN;
this.setWeightedDrivePower(
new Pose2d(

44
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBase.java
View File

@ -1,44 +0,0 @@
package org.firstinspires.ftc.teamcode.opmodes;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.opmodes.AutoRedFarTwoPlusTwo.autoState;
public abstract class AutoBase extends LinearOpMode {
protected Pose2d initialPosition;
Robot robot;
String randomization;
String parkLocation;
double runtime;
autoState state = autoState.PURPLE;
int delay;
public abstract void followTrajectories();
@Override
public void runOpMode() {
this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
this.robot = new Robot().init(hardwareMap);
this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270));
this.robot.getDrive().setPoseEstimate(initialPosition);
while (!this.isStarted()) {
parkLocation = "RIGHT";
this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition());
randomization = String.valueOf(this.robot.getCamera().getStartingPosition());
this.telemetry.addData("Park Position", parkLocation);
this.telemetry.addData("Delay", delay);
this.telemetry.update();
}
while (state != autoState.STOP) {
followTrajectories();
robot.update();
}
}
}

284
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueTwoPlusTwo.java
View File

@ -1,8 +1,11 @@
package org.firstinspires.ftc.teamcode.opmodes;
import static org.firstinspires.ftc.teamcode.hardware.Robot.Slides.SLIDELAYERONE;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
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.LinearOpMode;
@ -15,36 +18,42 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.Tra
public class AutoBlueTwoPlusTwo extends LinearOpMode {
protected Pose2d initialPosition;
private Robot robot;
private String randomization;
private String randomization = "CENTER";
private String parkLocation = "LEFT";
//Pose2ds
//Preloads
final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -33.5, Math.toRadians(210));
final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-30.5, -31, Math.toRadians(210));
final static Pose2d CENTER_PRELOAD = new Pose2d(-33, -26.8, Math.toRadians(270));
final static Pose2d LEFT_PRELOAD = new Pose2d(-47, -35, Math.toRadians(270));
final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-32, -30, Math.toRadians(180));
final static Pose2d CENTER_PRELOAD = new Pose2d(-33, -26, Math.toRadians(270));
final static Pose2d LEFT_PRELOAD = new Pose2d(-45, -27, Math.toRadians(270));
//Board Scores
final static Pose2d RIGHT_BOARD = new Pose2d(-75.5, -26.5, Math.toRadians(182));
final static Pose2d CENTER_BOARD = new Pose2d(-77, -33.5, Math.toRadians(185));
final static Pose2d LEFT_BOARD = new Pose2d(-76, -41, Math.toRadians(185));
//Stack Cycle
final static Pose2d LEAVE_BOARD = new Pose2d(-65, -10, Math.toRadians(180));
final static Pose2d TO_STACK = new Pose2d(35, -6.5, Math.toRadians(180));
final static Pose2d TO_STACK_SLOW = new Pose2d(40, -7.5, Math.toRadians(180));
final static Pose2d TO_STACK_SLOW2 = new Pose2d(40, -8.5, Math.toRadians(183));
final static Pose2d RIGHT_BOARD = new Pose2d(-76.5, -24, Math.toRadians(180));
final static Pose2d CENTER_BOARD = new Pose2d(-78, -35, Math.toRadians(184));
final static Pose2d LEFT_BOARD = new Pose2d(-78, -41, Math.toRadians(182));
//Parka
final static Pose2d PARK = new Pose2d(-60, -58, Math.toRadians(180));
final static Pose2d PARK2 = new Pose2d(-80, -60, Math.toRadians(180));
final static Pose2d PARKLEFT = new Pose2d(-70, -15, Math.toRadians(180));
final static Pose2d PARKLEFT2 = new Pose2d(-75, -12, Math.toRadians(180));
//Cycles
final static Vector2d LEAVE_BOARD = new Vector2d(-50, -10);
final static Vector2d TO_STACK = new Vector2d(38, -10);
final static Vector2d TO_STACK_SLOW = new Vector2d(47, -9.5);
final static Pose2d TO_STACK_SLOW2 = new Pose2d(38.5, -8, Math.toRadians(180));
final static Pose2d BACK_THROUGH_GATE = new Pose2d(-50, -10, Math.toRadians(180));
final static Pose2d APPROACHING_BOARD = new Pose2d(-70, -28, Math.toRadians(180));
final static Pose2d SCORE_STACK = new Pose2d(-73.5, -29, Math.toRadians(180));
//Park
final static Pose2d BACK_OFF = new Pose2d(-68, -55,Math.toRadians(180));
final static Pose2d PARK = new Pose2d(-80, -64, Math.toRadians(180));
final static Pose2d APPROACHING_BOARD = new Pose2d(-70, -31, Math.toRadians(180));
final static Vector2d SCORE_STACK = new Vector2d(-72, -30);
final static Vector2d SCORE_STACK_SLOW = new Vector2d(-73, -30);
protected void scorePreloadOne() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
switch (randomization) {
case "RIGHT":
builder.lineToLinearHeading(RIGHT_PRELOAD_ONE);
builder.lineToLinearHeading(RIGHT_PRELOAD_TWO);
builder.setReversed(true);
builder.splineToSplineHeading(RIGHT_PRELOAD_TWO, Math.toRadians(360),
MecanumDrive.getVelocityConstraint(70, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(50));
builder.setReversed(false);
break;
case "CENTER":
builder.lineToLinearHeading(CENTER_PRELOAD);
@ -53,148 +62,223 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode {
builder.lineToLinearHeading(LEFT_PRELOAD);
break;
}
builder.addTemporalMarker(.5, robot.getArm()::armScore);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void boardScore() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
switch (randomization) {
case "LEFT":
builder.lineToLinearHeading(LEFT_BOARD,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));;
case "RIGHT":
builder.lineToLinearHeading(RIGHT_BOARD);
break;
case "CENTER":
builder.lineToLinearHeading(CENTER_BOARD,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
MecanumDrive.getVelocityConstraint(50, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(50));
break;
case "RIGHT":
builder.lineToLinearHeading(RIGHT_BOARD,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
case "LEFT":
builder.lineToLinearHeading(LEFT_BOARD,
MecanumDrive.getVelocityConstraint(50, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(30));
break;
}
builder.addTemporalMarker(.2, robot.getArm()::armScore);
builder.addTemporalMarker(.2, robot.getSlides()::slideFirstLayer);
builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore);
builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
this.robot.getDrive().followTrajectorySequence(builder.build());
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
while (this.robot.getDrive().isBusy()) {
robot.update();
}
}
protected void toStack() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(LEAVE_BOARD);
builder.addTemporalMarker(.3, robot.getArm()::armRest);
builder.setReversed(true);
builder.splineToConstantHeading(LEAVE_BOARD, Math.toRadians(360));
builder.lineToConstantHeading(TO_STACK,
MecanumDrive.getVelocityConstraint(80, Math.toRadians(360), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(80));
builder.setReversed(false);
switch (randomization) {
case "RIGHT":
builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 2)),
MecanumDrive.getVelocityConstraint(15, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(15));
break;
case "CENTER":
builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 1)),
MecanumDrive.getVelocityConstraint(15, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(15));
break;
case "LEFT":
builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 1)),
MecanumDrive.getVelocityConstraint(15, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(15));
break;
}
builder.addTemporalMarker(.3, robot.getArm()::armPickupStack);
builder.addTemporalMarker(.3, robot.getClaw()::close);
builder.addTemporalMarker(.3, robot.getWrist()::wristPickup);
builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
builder.addTemporalMarker(1.5,robot.getClaw()::openStack);
builder.addTemporalMarker(1.5, robot.getArm()::pickup);
builder.lineToLinearHeading(TO_STACK);
builder.lineToLinearHeading(TO_STACK_SLOW,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
this.robot.getDrive().followTrajectorySequence(builder.build());
builder.addTemporalMarker(2, robot.getClaw()::openStack);
builder.addTemporalMarker(.2, robot.getSlides()::slideDown);
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
while (this.robot.getDrive().isBusy()) {
robot.update();
}
}
protected void toStackNoDrift() {
protected void toStackLower() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(LEAVE_BOARD);
builder.addTemporalMarker(.3, robot.getArm()::armRest);
builder.setReversed(true);
builder.splineToConstantHeading(LEAVE_BOARD.plus(new Vector2d(0, -1)), Math.toRadians(360));
builder.lineToConstantHeading(TO_STACK.plus(new Vector2d(0, -1)),
MecanumDrive.getVelocityConstraint(80, Math.toRadians(360), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(80));
builder.setReversed(false);
switch (randomization) {
case "RIGHT":
builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 1)),
MecanumDrive.getVelocityConstraint(15, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(15));
break;
case "CENTER":
builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 1)),
MecanumDrive.getVelocityConstraint(15, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(15));
break;
case "LEFT":
builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 1)),
MecanumDrive.getVelocityConstraint(15, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(15));
break;
}
builder.addTemporalMarker(.3, robot.getArm()::armPickupStackLow);
builder.addTemporalMarker(.3, robot.getClaw()::close);
builder.addTemporalMarker(.3, robot.getWrist()::wristPickup);
builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
builder.addTemporalMarker(1.5,robot.getClaw()::openStack);
builder.addTemporalMarker(1.5, robot.getArm()::pickup);
builder.lineToLinearHeading(TO_STACK);
builder.lineToLinearHeading(TO_STACK_SLOW2,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
this.robot.getDrive().followTrajectorySequence(builder.build());
builder.addTemporalMarker(.7, robot.getClaw()::openStack);
builder.addTemporalMarker(.2, robot.getSlides()::slideDown);
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
while (this.robot.getDrive().isBusy()) {
robot.update();
}
}
protected void scoreStack() {
protected void toBoard() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(BACK_THROUGH_GATE);
builder.lineToLinearHeading(APPROACHING_BOARD);
builder.lineToLinearHeading(SCORE_STACK);
builder.addTemporalMarker(2.5, robot.getArm()::armSecondaryScore);
builder.lineToConstantHeading(LEAVE_BOARD);
builder.splineToConstantHeading(SCORE_STACK, Math.toRadians(180),
MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(40));
builder.addTemporalMarker(.01, robot.getArm()::armPickdaUpy);
builder.addTemporalMarker(2.5, robot.getArm()::armScoreStack);
builder.addTemporalMarker(2.5, robot.getWrist()::wristScore);
builder.addTemporalMarker(2.5, robot.getSlides()::slideScoreStack);
this.robot.getDrive().followTrajectorySequence(builder.build());
builder.addTemporalMarker(2.5, robot.getSlides()::slideFirstLayer);
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
while (this.robot.getDrive().isBusy()) {
robot.update();
}
}
protected void toStage() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(PARKLEFT2.vec());
builder.addTemporalMarker(.01, robot.getArm()::armPickdaUpy);
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
while (this.robot.getDrive().isBusy()) {
robot.update();
}
}
protected void scoreTest() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(-77.5, -31, Math.toRadians(183)),
MecanumDrive.getVelocityConstraint(20, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
builder.addTemporalMarker(.2,this::clawSlowOpen);
builder.lineToConstantHeading(SCORE_STACK_SLOW,
MecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(30));
builder.addTemporalMarker(.3, this::clawSlowOpen);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void clawSlowOpen() {
double currentPos = .8;
double targetPos = .73;
double currentPos = .61;
double targetPos = .58;
int slidesPos = SLIDELAYERONE;
int slideDelta = 2;
double delta = (targetPos - currentPos) / 100;
for (int i = 0; i < 100; i++) {
this.robot.getClaw().setPos(currentPos + (delta * i));
sleep(30);
this.robot.getSlides().slideTo(slidesPos + (slideDelta * i), 1);
sleep(7);
}
}
protected void park() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(BACK_OFF);
builder.lineToLinearHeading(PARK);
builder.addTemporalMarker(.1, robot.getArm()::armRest);
builder.addTemporalMarker(.1, robot.getClaw()::close);
builder.addTemporalMarker(.1, robot.getWrist()::wristPickup);
builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
this.robot.getDrive().followTrajectorySequence(builder.build());
builder.lineToLinearHeading(PARKLEFT);
builder.addTemporalMarker(.01, robot.getArm()::armRest);
builder.addTemporalMarker(.01, robot.getWrist()::wristPickup);
builder.addTemporalMarker(.01, robot.getSlides()::slideDown);
builder.addTemporalMarker(.01, robot.getClaw()::open);
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
while (this.robot.getDrive().isBusy()) {
robot.update();
}
}
// protected void parkLocation() {
// if (gamepad2.dpad_left) {
// parkLocation = "LEFT";
// } else if (gamepad2.dpad_right) {
// parkLocation = "RIGHT";
// }
// }
protected void startLocation() {
if (gamepad2.x) {
randomization = "LEFT";
} else if (gamepad2.y) {
randomization = "CENTER";
} else if (gamepad2.b) {
randomization = "RIGHT";
}
}
@Override
public void runOpMode() throws InterruptedException {
this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
this.robot = new Robot().init(hardwareMap);
// this.robot.getCamera().setAlliance(Alliance.Blue);
// this.robot.getCamera().initTargetingCamera();
this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270));
this.robot.getDrive().setPoseEstimate(initialPosition);
// Do super fancy chinese shit
while (!this.isStarted()) {
this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPositionBlue());
randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue());
// randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue());
// parkLocation();
startLocation();
this.telemetry.addData("Starting Position", randomization);
this.telemetry.addData("Park Position", parkLocation);
this.telemetry.update();
}
robot.update();
this.robot.getClaw().close();
scorePreloadOne();
boardScore();
sleep(100);
// sleep(250);
this.robot.getClaw().open();
switch (randomization) {
case "RIGHT":
toStackNoDrift();
break;
case "CENTER":
toStack();
break;
case "LEFT":
toStack();
break;
}
sleep(500);
// sleep(250);
toStack();
this.robot.getClaw().close();
sleep(250);
this.robot.getArm().armRest();
scoreStack();
this.robot.getClaw().setPos(.83);
scoreTest();
sleep(300);
toBoard();
clawSlowOpen();
// sleep(100);
toStackLower();
this.robot.getClaw().close();
sleep(300);
toBoard();
clawSlowOpen();
park();
// park();
}
}

24
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java
View File

@ -24,11 +24,10 @@ public class AutoRed extends LinearOpMode {
final static Pose2d CENTER_PRELOAD = new Pose2d(33, -26, Math.toRadians(270));
final static Pose2d RIGHT_PRELOAD = new Pose2d(45, -27, Math.toRadians(270));
//Board Scores
final static Pose2d LEFT_BOARD = new Pose2d(78, -22, Math.toRadians(358));
final static Pose2d CENTER_BOARD = new Pose2d(80, -32, Math.toRadians(358));
final static Pose2d RIGHT_BOARD = new Pose2d(78, -39, Math.toRadians(358));
//Park
final static Pose2d LEFT_BOARD = new Pose2d(78, -22, Math.toRadians(360));
final static Pose2d CENTER_BOARD = new Pose2d(78, -33, Math.toRadians(360));
final static Pose2d RIGHT_BOARD = new Pose2d(76, -39, Math.toRadians(360));
//Parka
final static Pose2d PARK = new Pose2d(60, -58, Math.toRadians(360));
final static Pose2d PARK2 = new Pose2d(80, -60, Math.toRadians(360));
final static Pose2d PARKLEFT = new Pose2d(50, -15, Math.toRadians(360));
@ -41,7 +40,7 @@ public class AutoRed extends LinearOpMode {
builder.setReversed(true);
builder.splineToSplineHeading(LEFT_PRELOAD_TWO, Math.toRadians(180),
MecanumDrive.getVelocityConstraint(70, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(70));
MecanumDrive.getAccelerationConstraint(50));
builder.setReversed(false);
break;
case "CENTER":
@ -61,16 +60,17 @@ public class AutoRed extends LinearOpMode {
builder.lineToLinearHeading(LEFT_BOARD);
break;
case "CENTER":
builder.lineToLinearHeading(CENTER_BOARD);
builder.lineToLinearHeading(CENTER_BOARD,
MecanumDrive.getVelocityConstraint(50, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(50));
break;
case "RIGHT":
builder.lineToLinearHeading(RIGHT_BOARD,
MecanumDrive.getVelocityConstraint(50, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(50));
MecanumDrive.getAccelerationConstraint(30));
break;
}
builder.addTemporalMarker(.2, robot.getArm()::armScore);
// builder.addTemporalMarker(.2, robot.getSlides()::slideFirstLayer);
builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore);
builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
while (this.robot.getDrive().isBusy()) {
@ -122,14 +122,11 @@ public class AutoRed extends LinearOpMode {
this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
this.robot = new Robot().init(hardwareMap);
// this.robot.getCamera().setAlliance(Alliance.Blue);
// this.robot.getCamera().initTargetingCamera();
this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270));
this.robot.getDrive().setPoseEstimate(initialPosition);
// Do super fancy chinese shit
while (!this.isStarted()) {
// randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue());
parkLocation();
startLocation();
this.telemetry.addData("Starting Position", randomization);
@ -140,7 +137,6 @@ public class AutoRed extends LinearOpMode {
this.robot.getClaw().close();
scorePreloadOne();
boardScore();
// sleep(250);
this.robot.getClaw().open();
sleep(250);
park();

323
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFarTwoPlusTwo.java
View File

@ -1,323 +0,0 @@
package org.firstinspires.ftc.teamcode.opmodes;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
@Autonomous(name = "AutoRedFar2+2",preselectTeleOp = "Main TeleOp")
public class AutoRedFarTwoPlusTwo extends AutoBase {
//Pose2ds
//Preloads
final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -33.5, Math.toRadians(230));
final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -31, Math.toRadians(200));
final static Pose2d RIGHT_PRELOAD_GETOUT = new Pose2d(-43, -42, Math.toRadians(0));
final static Pose2d CENTER_PRELOAD = new Pose2d(-35, -28, Math.toRadians(270));
final static Pose2d LEFT_PRELOAD = new Pose2d(-45, -27, Math.toRadians(270));
//Ready Truss
final static Pose2d LEAVE = new Pose2d(-43, -40, Math.toRadians(270));
final static Pose2d READY_TRUSS = new Pose2d(-43, -56.5, Math.toRadians(0));
final static Pose2d TO_BOARD = new Pose2d(26, -56.5, Math.toRadians(0));
final static Pose2d READY_SCORE = new Pose2d(37, -34, Math.toRadians(0));
final static Pose2d SCORE_BOARD_LEFT = new Pose2d(55, -27, Math.toRadians(5));
final static Pose2d SCORE_BOARD_MID = new Pose2d(55.5, -32.2, Math.toRadians(5));
final static Pose2d SCORE_BOARD_RIGHT = new Pose2d(55, -39, Math.toRadians(5));
final static Pose2d GET_STACK = new Pose2d(-47, -30, Math.toRadians(0));
final static Pose2d PICKUP_STACK = new Pose2d(-61.5, -30.5, Math.toRadians(0));
final static Pose2d PICKUP_STACK_MID = new Pose2d(-61.5, -32.5, Math.toRadians(0));
final static Pose2d READY_SCORESTACK = new Pose2d(50, -41, Math.toRadians(0));
final static Pose2d PICKUP_STACK_LEFT = new Pose2d(-60.5, -33, Math.toRadians(0));
final static Pose2d PARK = new Pose2d(45, -60, Math.toRadians(0));
final static Pose2d PARK2 = new Pose2d(60, -63, Math.toRadians(0));
final static Pose2d PARKLEFT = new Pose2d(45, -12, Math.toRadians(0));
final static Pose2d PARKLEFT2 = new Pose2d(60, -12, Math.toRadians(0));
protected enum autoState{
STOP,
PURPLE,
YELLOW,
FLIPYELLOW,
SCOREYELLOW,
FIRSTCYCLE,
RETRACT,
SCORECYCLEONE,
}
protected void scorePreloadOne() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
switch (randomization) {
case "LEFT":
builder.lineToLinearHeading(LEFT_PRELOAD);
break;
case "CENTER":
builder.lineToLinearHeading(CENTER_PRELOAD);
break;
case "RIGHT":
builder.lineToLinearHeading(RIGHT_PRELOAD_ONE);
builder.lineToLinearHeading(RIGHT_PRELOAD_TWO);
break;
}
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void goBackToWhereYouCameFrom() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(LEAVE,
MecanumDrive.getVelocityConstraint(60, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(90));
builder.lineToLinearHeading(READY_TRUSS,
MecanumDrive.getVelocityConstraint(60, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(90));
builder.lineToLinearHeading(TO_BOARD,
MecanumDrive.getVelocityConstraint(90, Math.toRadians(90), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(90));
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void scoreBoard(){
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(READY_SCORE);
builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore);
builder.addTemporalMarker(.2, robot.getSlides()::slideAutoStacks);
builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void score(){
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
switch (randomization) {
case "LEFT":
builder.lineToLinearHeading(SCORE_BOARD_LEFT,
MecanumDrive.getVelocityConstraint(40, 40, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(40));
break;
case "CENTER":
builder.lineToLinearHeading(SCORE_BOARD_MID,
MecanumDrive.getVelocityConstraint(40, 40, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break;
case "RIGHT":
builder.lineToLinearHeading(SCORE_BOARD_RIGHT,
MecanumDrive.getVelocityConstraint(40, 40, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(40));
break;
}
builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore);
builder.addTemporalMarker(.2,robot.getSlides()::slideAutoStacks);
builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void backTruss(){
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(TO_BOARD);
builder.lineToLinearHeading(READY_TRUSS,
MecanumDrive.getVelocityConstraint(80, Math.toRadians(80), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(80));
builder.lineToLinearHeading(GET_STACK);
switch (randomization){
case "MID":
builder.lineToLinearHeading(PICKUP_STACK_MID,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break;
case "RIGHT":
builder.lineToLinearHeading(PICKUP_STACK,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break;
default:
builder.lineToLinearHeading(PICKUP_STACK_LEFT,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break;
}
builder.addTemporalMarker(.3, robot.getArm()::armRest);
builder.addTemporalMarker(.3, robot.getWrist()::wristPickup);
builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
builder.addTemporalMarker(1.5,robot.getClaw()::openStack);
builder.addTemporalMarker(1.5, robot.getArm()::armPickupStack);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void goBackstage() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(READY_TRUSS);
builder.lineToLinearHeading(TO_BOARD);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void scoreBoardStack(){
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(READY_SCORESTACK);
builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore);
builder.addTemporalMarker(.2, robot.getSlides()::slideAutoStacks);
builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void scoreTest() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(new Pose2d(55, -41, Math.toRadians(0)),
MecanumDrive.getVelocityConstraint(20, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
builder.addTemporalMarker(.3,this::clawSlowOpen);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void clawSlowOpen() {
double currentPos = .81;
double targetPos = .73;
double delta = (targetPos - currentPos) / 100;
for (int i = 0; i < 100; i++) {
this.robot.getClaw().setPos(currentPos + (delta * i));
sleep(30);
}
}
protected void park(){
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
switch(parkLocation) {
case "LEFT":
builder.lineToLinearHeading(PARKLEFT);
builder.lineToLinearHeading(PARKLEFT2);
break;
case "RIGHT":
builder.lineToLinearHeading(PARK);
builder.lineToLinearHeading(PARK2);
break;
}
builder.addTemporalMarker(.1, robot.getArm()::armRest);
builder.addTemporalMarker(.1, robot.getWrist()::wristPickup);
builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void parkLocation(){
if (gamepad2.dpad_left) {
parkLocation="LEFT";
} else if (gamepad2.dpad_right) {
parkLocation = "RIGHT";
}}
protected void delaySet(){
if (gamepad2.dpad_up && delay<13000) {
delay+=1000;
sleep(100);
} else if (gamepad2.dpad_down && delay>0) {
delay-=1000;
sleep(100);
}
}
@Override
public void followTrajectories(){
TrajectorySequenceBuilder builder;
switch (state){
case PURPLE:
builder = this.robot.getTrajectorySequenceBuilder();
switch (randomization) {
case "LEFT":
builder.lineToLinearHeading(LEFT_PRELOAD);
break;
case "CENTER":
builder.lineToLinearHeading(CENTER_PRELOAD);
break;
case "RIGHT":
builder.lineToLinearHeading(RIGHT_PRELOAD_ONE);
builder.lineToLinearHeading(RIGHT_PRELOAD_TWO);
break;
}
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
if (!robot.getDrive().isBusy()){
state = autoState.YELLOW;
}
break;
case YELLOW:
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(LEAVE);
builder.lineToLinearHeading(READY_TRUSS);
builder.lineToLinearHeading(TO_BOARD);
switch (randomization) {
case "LEFT":
builder.splineToConstantHeading(SCORE_BOARD_LEFT.vec(),Math.toRadians(0));
break;
case "CENTER":
builder.splineToConstantHeading(SCORE_BOARD_MID.vec(),Math.toRadians(0));
break;
case "RIGHT":
builder.splineToConstantHeading(SCORE_BOARD_RIGHT.vec(),Math.toRadians(0));
break;
}
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
runtime = getRuntime();
state = autoState.FLIPYELLOW;
break;
case FLIPYELLOW:
if (getRuntime() > runtime + 5) {
this.robot.getArm().armSecondaryScore();
this.robot.getWrist().wristScore();
state = autoState.SCOREYELLOW;
}
break;
case SCOREYELLOW:
if (!robot.getDrive().isBusy()){
this.robot.getClaw().open();
runtime = getRuntime();
state = autoState.FIRSTCYCLE;
}
break;
case FIRSTCYCLE:
builder = this.robot.getTrajectorySequenceBuilder();
builder.splineToConstantHeading(TO_BOARD.vec(),Math.toRadians(0));
builder.lineToLinearHeading(READY_TRUSS);
builder.splineToConstantHeading(GET_STACK.vec(),Math.toRadians(0));
builder.lineToLinearHeading(PICKUP_STACK,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
runtime = getRuntime();
state = autoState.RETRACT;
break;
case RETRACT:
if (getRuntime() > runtime + .3) {
this.robot.getClaw().close();
this.robot.getArm().armRest();
this.robot.getWrist().wristPickup();
this.robot.getSlides().slideDown();
}
if (getRuntime() > runtime + 2) {
this.robot.getClaw().openStack();
this.robot.getArm().armPickupStack();
}
if (!robot.getDrive().isBusy()) {
this.robot.getClaw().close();
state = autoState.SCORECYCLEONE;
}
break;
case SCORECYCLEONE:
break;
}
}
// scorePreloadOne();
// goBackToWhereYouCameFrom();
//// scoreBoard();
// score();
// this.robot.getClaw().open();
// backTruss();
// this.robot.getClaw().close();
// sleep(200);
// goBackstage();
// scoreBoardStack();
// scoreTest();
// park();
}

30
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusFour.java
View File

@ -12,20 +12,21 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
@Autonomous(name = "autoRed2+2")
public class AutoRedTwoPlusTwo extends LinearOpMode {
@Autonomous(name = "autoRed2+4")
public class AutoRedTwoPlusFour extends LinearOpMode {
protected Pose2d initialPosition;
private Robot robot;
private String randomization = "CENTER";
private String parkLocation = "LEFT";
//Pose2ds
//Preloads
final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(27, -26, Math.toRadians(360));
final static Pose2d CENTER_PRELOAD = new Pose2d(33, -26, Math.toRadians(270));
final static Pose2d RIGHT_PRELOAD = new Pose2d(45, -27, Math.toRadians(270));
//Board Scores
final static Pose2d LEFT_BOARD = new Pose2d(78, -22, Math.toRadians(360));
final static Pose2d LEFT_BOARD = new Pose2d(78, -23.5, Math.toRadians(360));
final static Pose2d CENTER_BOARD = new Pose2d(78, -33, Math.toRadians(360));
final static Pose2d RIGHT_BOARD = new Pose2d(76, -39, Math.toRadians(360));
//Parka
@ -36,7 +37,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
//Cycles
final static Vector2d LEAVE_BOARD = new Vector2d(50, -10);
final static Vector2d TO_STACK = new Vector2d(-35, -10);
final static Vector2d TO_STACK_SLOW = new Vector2d(-45, -10);
final static Vector2d TO_STACK_SLOW = new Vector2d(-47.5, -10);
final static Pose2d TO_STACK_SLOW2 = new Pose2d(-38.5, -8, Math.toRadians(360));
final static Pose2d BACK_THROUGH_GATE = new Pose2d(50, -10, Math.toRadians(360));
final static Pose2d APPROACHING_BOARD = new Pose2d(70, -31, Math.toRadians(360));
@ -83,7 +84,6 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
break;
}
builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore);
// builder.addTemporalMarker(.2, robot.getSlides()::slideFirstLayer);
builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
while (this.robot.getDrive().isBusy()) {
@ -106,12 +106,12 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
MecanumDrive.getAccelerationConstraint(40));
break;
case "CENTER":
builder.lineToConstantHeading(TO_STACK_SLOW,
builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 3.2)),
MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(40));
break;
case "RIGHT":
builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, -1)),
builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 0.25)),
MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(40));
break;
@ -142,12 +142,12 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
MecanumDrive.getAccelerationConstraint(40));
break;
case "CENTER":
builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, -.50)),
builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, -1)),
MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(40));
break;
case "RIGHT":
builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, -2)),
builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, -3)),
MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(40));
break;
@ -155,7 +155,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
builder.addTemporalMarker(.3, robot.getArm()::armPickupStackLow);
builder.addTemporalMarker(.3, robot.getClaw()::close);
builder.addTemporalMarker(.3, robot.getWrist()::wristPickup);
builder.addTemporalMarker(2, robot.getClaw()::openStack);
builder.addTemporalMarker(.7, robot.getClaw()::openStack);
builder.addTemporalMarker(.2, robot.getSlides()::slideDown);
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
while (this.robot.getDrive().isBusy()) {
@ -170,8 +170,8 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(40));
builder.addTemporalMarker(.01, robot.getArm()::armPickdaUpy);
builder.addTemporalMarker(2.5, robot.getArm()::armScoreStack);
builder.addTemporalMarker(2.5, robot.getWrist()::wristScore);
builder.addTemporalMarker(2.2, robot.getArm()::armScoreStack);
builder.addTemporalMarker(2.4, robot.getWrist()::wristScore);
builder.addTemporalMarker(2.5, robot.getSlides()::slideFirstLayer);
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
while (this.robot.getDrive().isBusy()) {
@ -276,11 +276,13 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
sleep(250);
toStack();
this.robot.getClaw().close();
sleep(300);
toBoard();
clawSlowOpen();
// sleep(100);
toStackLower();
this.robot.getClaw().close();
sleep(300);
switch (randomization) {
default:
toBoard();
@ -292,7 +294,9 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
}
while (!isStopRequested()) {
this.robot.getArm().armRest(true);
this.robot.getClaw().openStack();
this.robot.getWrist().wristPickup();
this.robot.getSlides().slideDown();
this.robot.getClaw().open();
robot.update();
}
// park();

315
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusFourTest.java Normal file
View File

@ -0,0 +1,315 @@
package org.firstinspires.ftc.teamcode.opmodes;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
@Autonomous(name = "autoRed2+4Test")
public class AutoRedTwoPlusFourTest extends LinearOpMode {
protected Pose2d initialPosition;
GamepadEx controller1;
GamepadEx controller2;
private Robot robot;
private String randomization = "CENTER";
private Boolean board1 = true;
private Boolean board2 = true;
private String parkLocation = "LEFT";
//Pose2ds
//Preloads
final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(27, -26, Math.toRadians(360));
final static Pose2d CENTER_PRELOAD = new Pose2d(33, -26, Math.toRadians(270));
final static Pose2d RIGHT_PRELOAD = new Pose2d(45, -27, Math.toRadians(270));
//Board Scores
final static Pose2d LEFT_BOARD = new Pose2d(78, -22, Math.toRadians(360));
final static Pose2d CENTER_BOARD = new Pose2d(78, -33, Math.toRadians(360));
final static Pose2d RIGHT_BOARD = new Pose2d(76, -39, Math.toRadians(360));
//Parka
final static Pose2d PARK = new Pose2d(60, -58, Math.toRadians(360));
final static Pose2d PARK2 = new Pose2d(80, -60, Math.toRadians(360));
final static Pose2d PARKLEFT = new Pose2d(50, -15, Math.toRadians(360));
final static Pose2d PARKLEFT2 = new Pose2d(75, -10, Math.toRadians(360));
//Cycles
final static Vector2d LEAVE_BOARD = new Vector2d(50, -10);
final static Vector2d TO_STACK = new Vector2d(-35, -10);
final static Vector2d TO_STACK_SLOW = new Vector2d(-45, -10);
final static Vector2d SCORE_STACK = new Vector2d(72.5, -35);
//Trajectory Builders
protected void scorePreloadOne() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
switch (randomization) {
case "LEFT":
builder.setReversed(true);
builder.splineToSplineHeading(LEFT_PRELOAD_TWO, Math.toRadians(180),
MecanumDrive.getVelocityConstraint(70, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(50));
builder.setReversed(false);
break;
case "CENTER":
builder.lineToLinearHeading(CENTER_PRELOAD);
break;
case "RIGHT":
builder.lineToLinearHeading(RIGHT_PRELOAD);
break;
}
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void boardScore() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
switch (randomization) {
case "LEFT":
builder.lineToLinearHeading(LEFT_BOARD);
break;
case "CENTER":
builder.lineToLinearHeading(CENTER_BOARD,
MecanumDrive.getVelocityConstraint(50, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(50));
break;
case "RIGHT":
builder.lineToLinearHeading(RIGHT_BOARD,
MecanumDrive.getVelocityConstraint(50, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(30));
break;
}
builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore);
builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
while (this.robot.getDrive().isBusy()) {
robot.update();
}
}
protected void toStack() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.setReversed(true);
builder.splineToConstantHeading(LEAVE_BOARD, Math.toRadians(180));
builder.lineToConstantHeading(TO_STACK,
MecanumDrive.getVelocityConstraint(80, Math.toRadians(360), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(80));
builder.setReversed(false);
switch (randomization) {
case "LEFT":
builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 1)),
MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(40));
break;
case "CENTER":
builder.lineToConstantHeading(TO_STACK_SLOW,
MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(40));
break;
case "RIGHT":
builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, -1)),
MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(40));
break;
}
builder.addTemporalMarker(.3, robot.getArm()::armPickupStack);
builder.addTemporalMarker(.3, robot.getClaw()::close);
builder.addTemporalMarker(.3, robot.getWrist()::wristPickup);
builder.addTemporalMarker(2, robot.getClaw()::openStack);
builder.addTemporalMarker(.2, robot.getSlides()::slideDown);
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
while (this.robot.getDrive().isBusy()) {
robot.update();
}
}
protected void toStackLower() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.setReversed(true);
builder.splineToConstantHeading(LEAVE_BOARD.plus(new Vector2d(0, -1)), Math.toRadians(180));
builder.lineToConstantHeading(TO_STACK.plus(new Vector2d(0, -1)),
MecanumDrive.getVelocityConstraint(80, Math.toRadians(360), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(80));
builder.setReversed(false);
switch (randomization) {
case "LEFT":
builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 0)),
MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(40));
break;
case "CENTER":
builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, -.50)),
MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(40));
break;
case "RIGHT":
builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, -2)),
MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(40));
break;
}
builder.addTemporalMarker(.3, robot.getArm()::armPickupStackLow);
builder.addTemporalMarker(.3, robot.getClaw()::close);
builder.addTemporalMarker(.3, robot.getWrist()::wristPickup);
builder.addTemporalMarker(.5, robot.getClaw()::openStack);
builder.addTemporalMarker(.2, robot.getSlides()::slideDown);
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
while (this.robot.getDrive().isBusy()) {
robot.update();
}
}
protected void toBoard() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(LEAVE_BOARD);
builder.splineToConstantHeading(SCORE_STACK, Math.toRadians(360),
MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(40));
builder.addTemporalMarker(.1, robot.getArm()::armPickdaUpy);
builder.addTemporalMarker(2.5, robot.getArm()::armScoreStack);
builder.addTemporalMarker(2.5, robot.getWrist()::wristScore);
builder.addTemporalMarker(2.5, robot.getSlides()::slideFirstLayer);
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
while (this.robot.getDrive().isBusy()) {
robot.update();
}
}
protected void toStage() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(PARKLEFT2.vec());
builder.addTemporalMarker(.01, robot.getArm()::armPickdaUpy);
builder.addTemporalMarker(2.5, robot.getArm()::armScoreStack);
builder.addTemporalMarker(2.5, robot.getWrist()::wristScore);
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
while (this.robot.getDrive().isBusy()) {
robot.update();
}
}
protected void clawSlowOpen() {
double currentPos = .62;
double targetPos = .6;
double delta = (targetPos - currentPos) / 100;
for (int i = 0; i < 100; i++) {
this.robot.getClaw().setPos(currentPos + (delta * i));
this.robot.getSlides().slideFirstLayer();
sleep(5);
}
}
protected void park() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
switch (parkLocation) {
case "LEFT":
builder.lineToLinearHeading(PARKLEFT);
builder.lineToLinearHeading(PARKLEFT2);
break;
case "RIGHT":
builder.lineToLinearHeading(PARK);
builder.lineToLinearHeading(PARK2);
break;
}
builder.addTemporalMarker(.1, robot.getArm()::armRest);
builder.addTemporalMarker(.1, robot.getWrist()::wristPickup);
builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
while (this.robot.getDrive().isBusy()) {
robot.update();
}
}
//Init Customization
protected void parkLocation() {
if (gamepad2.dpad_left) {
parkLocation = "LEFT";
} else if (gamepad2.dpad_right) {
parkLocation = "RIGHT";
}
}
protected void startLocation() {
if (gamepad2.x) {
randomization = "LEFT";
} else if (gamepad2.y) {
randomization = "CENTER";
} else if (gamepad2.b) {
randomization = "RIGHT";
}
}
protected void cycles() {
if (gamepad2.left_bumper) {
if (!board1) board1 = true;
else board1 = false;
sleep(500);
} else if (gamepad2.right_bumper) {
if (!board2) board2 = true;
else board2 = false;
sleep(500);
}
}
//Super fancy chinese shit
@Override
public void runOpMode() throws InterruptedException {
this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
this.robot = new Robot().init(hardwareMap);
// this.robot.getCamera().setAlliance(Alliance.Blue);
// this.robot.getCamera().initTargetingCamera();
this.controller2 = new GamepadEx(this.gamepad2);
this.controller1 = new GamepadEx(this.gamepad1);
this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270));
this.robot.getDrive().setPoseEstimate(initialPosition);
// Do super fancy chinese shit
while (!this.isStarted()) {
// randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue());
cycles();
parkLocation();
startLocation();
this.telemetry.addData("Randomization", randomization);
this.telemetry.addData("Park Position", parkLocation);
this.telemetry.addData("Board 1", board1);
this.telemetry.addData("Board 2", board2);
this.telemetry.update();
}
robot.update();
this.robot.getClaw().close();
scorePreloadOne();
boardScore();
this.robot.getClaw().open();
sleep(250);
toStack();
this.robot.getClaw().close();
if (board1) {
toBoard();
clawSlowOpen();
} else {
toStage();
this.robot.getClaw().openStack();
}
toStackLower();
this.robot.getClaw().close();
if (board2) {
toBoard();
clawSlowOpen();
} else {
toStage();
this.robot.getClaw().openStack();
}
while (!isStopRequested()) {
this.robot.getArm().armRest(true);
this.robot.getWrist().wristPickup();
this.robot.getSlides().slideDown();
this.robot.getClaw().open();
robot.update();
}
}
}

44
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoTest.java
View File

@ -1,44 +0,0 @@
package org.firstinspires.ftc.teamcode.opmodes;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
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.LinearOpMode;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
@Autonomous(name = "Test Stuff")
public class AutoTest extends LinearOpMode {
protected Pose2d initialPosition;
private Robot robot;
//Pose2ds
//Preloads
final static Pose2d FIRSTMOVE = new Pose2d(10, 10, Math.toRadians(0));
protected void sequenceOne() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.splineToConstantHeading(new Vector2d(10, 10), Math.toRadians(0));
this.robot.getDrive().followTrajectorySequence(builder.build());
}
@Override
public void runOpMode() throws InterruptedException {
this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
this.robot = new Robot().init(hardwareMap);
this.initialPosition = new Pose2d(0, 0, Math.toRadians(0));
this.robot.getDrive().setPoseEstimate(initialPosition);
// Do super fancy chinese shit
while (!this.isStarted()) {
}
sequenceOne();
}
}