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

phoenix red 2+4 weheeeeeeeee

This commit is contained in:
sihan 2024-03-19 19:14:22 -05:00
parent cfb25cd68d
commit 834b5c234f
6 changed files with 238 additions and 283 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -168,7 +168,7 @@ public class Robot {
//Values
public static double SLIDE_POWER_UP = .7;
public static double SLIDE_POWER_DOWN = .2;
public static int SLIDELAYERONE = 60;
public static int SLIDELAYERONE = 150;
public static int SLIDEAUTOSTACKS = 250;
public static int SLIDEUP = 630;
public static int SLIDELAYERTWO = 350;
@ -302,9 +302,10 @@ public class Robot {
public static double DIFFY_TOL = 0.01;
public static double ARM_MAX_DELTA = 0.02;
public static double ARMREST = 0.89;
public static double ARMSCORE = 0.4;
public static double ARMSCORESTACK = 0.53;
public static double ARMACCSCORE = .57;
public static double ARMPICKUPSTACK = 0.815;
public static double ARMPICKUPSTACK = 0.91;
public static double ARMPICKUPSTACKLOW = 0.93;
public static double PICKUP = 0.92;
//PControler
public PController armPController;
@ -332,6 +333,11 @@ public class Robot {
moveArm(PICKUP, now);
}
public void armPickdaUpy() {
moveArm(.83, false);
}
public void armScore() {
this.armSecondaryScore();
}
@ -340,10 +346,19 @@ public class Robot {
moveArm(ARMACCSCORE, false);
}
public void armScoreStack() {
moveArm(ARMSCORESTACK, false);
}
public void armPickupStack() {
moveArm(ARMPICKUPSTACK, false);
}
public void armPickupStackLow() {
moveArm(ARMPICKUPSTACKLOW, false);
}
public void armRest() {
armRest(false);
}
@ -446,8 +461,8 @@ public class Robot {
public static class Claw {
//Values
public static double OPEN = 0.65;
public static double BIGOPEN = 0f;
public static double CLOSE = 0.73;
public static double BIGOPEN = .48;
public static double CLOSE = 0.75;
public static double CLAW_MIN = 0;
public static double CLAW_MAX = 1;
//Servo

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

@ -68,8 +68,8 @@ public class DriveConstants {
* small and gradually increase them later after everything is working. All distance units are
* inches.
*/
public static double MAX_VEL = 80;
public static double MAX_ACCEL = 80;
public static double MAX_VEL = 60;
public static double MAX_ACCEL = 50;
public static double MAX_ANG_VEL = Math.toRadians(360);
public static double MAX_ANG_ACCEL = Math.toRadians(360);

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

@ -60,8 +60,8 @@ import java.util.List;
*/
@Config
public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive {
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(3, 0, 2.5);
public static PIDCoefficients HEADING_PID = new PIDCoefficients(10, 0, 1.5);
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(5, 0, 4);
public static PIDCoefficients HEADING_PID = new PIDCoefficients(10, 0, 2.5);
public static double LATERAL_MULTIPLIER = 1;

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

@ -7,6 +7,8 @@ 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 = "autoRed")
@ -18,26 +20,29 @@ public class AutoRed extends LinearOpMode {
//Pose2ds
//Preloads
final static Pose2d LEFT_PRELOAD_ONE = new Pose2d(40, -37.5, Math.toRadians(270));
final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(29.5, -32, Math.toRadians(360));
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, -35, Math.toRadians(270));
final static Pose2d RIGHT_PRELOAD = new Pose2d(45, -27, Math.toRadians(270));
//Board Scores
final static Pose2d LEFT_BOARD = new Pose2d(75.8, -26.5, Math.toRadians(358));
final static Pose2d CENTER_BOARD = new Pose2d(80, -29, Math.toRadians(358));
final static Pose2d RIGHT_BOARD = new Pose2d(75.8, -40, Math.toRadians(358));
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 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(60, -6, Math.toRadians(360));
final static Pose2d PARKLEFT2 = new Pose2d(80, -6, Math.toRadians(360));
final static Pose2d PARKLEFT = new Pose2d(50, -15, Math.toRadians(360));
final static Pose2d PARKLEFT2 = new Pose2d(75, -2, Math.toRadians(360));
protected void scorePreloadOne() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
switch (randomization) {
case "LEFT":
builder.splineToSplineHeading(LEFT_PRELOAD_TWO, Math.toRadians(360));
builder.setReversed(true);
builder.splineToSplineHeading(LEFT_PRELOAD_TWO, Math.toRadians(180),
MecanumDrive.getVelocityConstraint(70, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(70));
builder.setReversed(false);
break;
case "CENTER":
builder.lineToLinearHeading(CENTER_PRELOAD);
@ -59,11 +64,13 @@ public class AutoRed extends LinearOpMode {
builder.lineToLinearHeading(CENTER_BOARD);
break;
case "RIGHT":
builder.lineToLinearHeading(RIGHT_BOARD);
builder.lineToLinearHeading(RIGHT_BOARD,
MecanumDrive.getVelocityConstraint(50, Math.toRadians(60), DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(50));
break;
}
builder.addTemporalMarker(.2, robot.getArm()::armScore);
builder.addTemporalMarker(.2, robot.getSlides()::slideFirstLayer);
// builder.addTemporalMarker(.2, robot.getSlides()::slideFirstLayer);
builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
this.robot.getDrive().followTrajectorySequenceAsync(builder.build());
while (this.robot.getDrive().isBusy()) {
@ -130,12 +137,13 @@ public class AutoRed extends LinearOpMode {
this.telemetry.update();
}
robot.update();
this.robot.getClaw().close();
scorePreloadOne();
boardScore();
sleep(250);
// sleep(250);
this.robot.getClaw().open();
sleep(250);
// park();
park();
}
}

168
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFaronepluszero.java
View File

@ -1,168 +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.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 = "AutoRedFar1+0")
public class AutoRedFaronepluszero extends LinearOpMode {
protected Pose2d initialPosition;
private Robot robot;
private String randomization;
private String parkLocation;
private int delay = 10000;
//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(230));
final static Pose2d CENTER_PRELOAD = new Pose2d(-35, -28, Math.toRadians(270));
final static Pose2d LEFT_PRELOAD = new Pose2d(-47, -35, Math.toRadians(270));
//Ready Truss
final static Pose2d LEAVE = new Pose2d(-43, -40, Math.toRadians(270));
final static Pose2d READY_TRUSS = new Pose2d(-43, -57, Math.toRadians(0));
final static Pose2d TO_BOARD = new Pose2d(30, -57, Math.toRadians(0));
final static Pose2d READY_SCORE = new Pose2d(37, -34, Math.toRadians(0));
final static Pose2d SCORE_BOARD_LEFT = new Pose2d(54, -27, Math.toRadians(5));
final static Pose2d SCORE_BOARD_MID = new Pose2d(54, -32, Math.toRadians(0));
final static Pose2d SCORE_BOARD_RIGHT = new Pose2d(54, -41, 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 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);
builder.lineToLinearHeading(RIGHT_PRELOAD_GETOUT);
break;
}
builder.addTemporalMarker(.4,robot.getArm()::armScore);
// builder.addTemporalMarker(2,robot.getArm()::armRest);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
protected void goBackToWhereYouCameFrom() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(LEAVE);
// builder.lineToLinearHeading(READY_TRUSS);
// builder.lineToLinearHeading(TO_BOARD);
builder.addTemporalMarker(.3,robot.getArm()::armRest);
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(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break;
case "CENTER":
builder.lineToLinearHeading(SCORE_BOARD_MID,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break;
case "RIGHT":
builder.lineToLinearHeading(SCORE_BOARD_RIGHT,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
break;
}
builder.addTemporalMarker(1.8, robot.getClaw()::open);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
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 runOpMode() throws InterruptedException {
this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
this.robot = new Robot().init(hardwareMap);
// 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()) {
parkLocation();
delaySet();
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();
}
// sleep(delay);
scorePreloadOne();
goBackToWhereYouCameFrom();
// scoreBoard();
// score();
// sleep(500);
// park();
}
}

286
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java
View File

@ -3,6 +3,7 @@ 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;
@ -15,36 +16,44 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.Tra
public class AutoRedTwoPlusTwo extends LinearOpMode {
protected Pose2d initialPosition;
private Robot robot;
private String randomization;
private String randomization = "CENTER";
private String parkLocation = "LEFT";
//Pose2ds
//Preloads
final static Pose2d LEFT_PRELOAD_ONE = new Pose2d(40, -37.5, Math.toRadians(270));
final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(29.5, -32, Math.toRadians(360));
final static Pose2d CENTER_PRELOAD = new Pose2d(34, -28, Math.toRadians(270));
final static Pose2d RIGHT_PRELOAD = new Pose2d(43, -35, Math.toRadians(270));
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(76.7, -27, Math.toRadians(360));
final static Pose2d CENTER_BOARD = new Pose2d(76.5, -32, Math.toRadians(355));
final static Pose2d RIGHT_BOARD = new Pose2d(76.5, -40, Math.toRadians(355));
//Stack Cycle
final static Pose2d LEAVE_BOARD = new Pose2d(65, -10, Math.toRadians(360));
final static Pose2d TO_STACK = new Pose2d(-35, -8, Math.toRadians(360));
final static Pose2d TO_STACK_SLOW = new Pose2d(-38.5, -8.5, Math.toRadians(360));
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, -12, 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 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));
final static Pose2d SCORE_STACK = new Pose2d(73.5, -31, Math.toRadians(360));
final static Vector2d SCORE_STACK = new Vector2d(72.5, -35);
final static Vector2d SCORE_STACK_SLOW = new Vector2d(73, -35);
//Park
final static Pose2d BACK_OFF = new Pose2d(60, -58, Math.toRadians(360));
final static Pose2d PARK = new Pose2d(80, -60, Math.toRadians(360));
protected void scorePreloadOne() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
switch (randomization) {
case "LEFT":
builder.lineToLinearHeading(LEFT_PRELOAD_ONE);
builder.lineToLinearHeading(LEFT_PRELOAD_TWO);
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);
@ -53,7 +62,6 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
builder.lineToLinearHeading(RIGHT_PRELOAD);
break;
}
builder.addTemporalMarker(.5, robot.getArm()::armScore);
this.robot.getDrive().followTrajectorySequence(builder.build());
}
@ -61,142 +69,234 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
switch (randomization) {
case "LEFT":
builder.lineToLinearHeading(LEFT_BOARD,
MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH),
MecanumDrive.getAccelerationConstraint(20));
;
builder.lineToLinearHeading(LEFT_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));
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.getSlides()::slideFirstLayer);
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(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(.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(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(.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(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 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(360),
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(75, -31, Math.toRadians(360)),
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 = .7;
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));
sleep(30);
this.robot.getSlides().slideFirstLayer();
sleep(5);
}
}
protected void park() {
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(BACK_OFF);
builder.lineToLinearHeading(PARK);
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.getClaw()::close);
builder.addTemporalMarker(.1, robot.getWrist()::wristPickup);
builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
this.robot.getDrive().followTrajectorySequence(builder.build());
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().getStartingPosition());
randomization = String.valueOf(this.robot.getCamera().getStartingPosition());
// randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue());
parkLocation();
startLocation();
this.telemetry.addData("Starting Position", randomization);
this.telemetry.addData("Park Position", parkLocation);
this.telemetry.update();
}
this.robot.update();
robot.update();
this.robot.getClaw().close();
scorePreloadOne();
boardScore();
sleep(150);
// sleep(250);
this.robot.getClaw().open();
sleep(150);
switch (randomization) {
case "LEFT":
toStackNoDrift();
break;
case "CENTER":
toStack();
break;
case "RIGHT":
toStack();
break;
}
sleep(500);
this.robot.getClaw().close();
sleep(250);
this.robot.getArm().armRest();
scoreStack();
this.robot.refreshPoseEstimateFromAprilTag();
this.robot.getClaw().setPos(.83);
scoreTest();
park();
toStack();
this.robot.getClaw().close();
toBoard();
clawSlowOpen();
// sleep(100);
toStackLower();
this.robot.getClaw().close();
switch (randomization) {
default:
toBoard();
clawSlowOpen();
break;
// case "LEFT":
// toStage();
// break;
}
while (!isStopRequested()) {
this.robot.getArm().armRest(true);
this.robot.getClaw().openStack();
robot.update();
}
// park();
}
}