before LM3 all subsystems working, auto red 2+0, 2+2 in progress. Learnt better way of auto, implemented in 2+2, not tested.
This commit is contained in:
sihan
parent
6d076a16cf
commit
dbf8423ae3
|
|
@ -76,7 +76,7 @@ public class Robot {
|
|||
|
||||
public static class Slides {
|
||||
private DcMotor slidesRight = null;
|
||||
private DcMotor slidesLeft = null;
|
||||
public DcMotor slidesLeft = null;
|
||||
|
||||
public Slides init(HardwareMap hardwareMap) {
|
||||
this.slidesLeft = hardwareMap.get(DcMotor.class, SLIDELEFT);
|
||||
|
|
|
|||
|
|
@ -4,50 +4,52 @@ import com.acmerobotics.dashboard.FtcDashboard;
|
|||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
import com.acmerobotics.roadrunner.trajectory.Trajectory;
|
||||
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;
|
||||
|
||||
@Config
|
||||
@Autonomous(name = "autoRed2+2New")
|
||||
public class AutoRedTwoPlusTwoNew extends LinearOpMode {
|
||||
@Autonomous(name = "autoBLue2+2")
|
||||
public class AutoBlueTwoPlusTwo extends LinearOpMode {
|
||||
protected Pose2d initialPosition;
|
||||
|
||||
protected Trajectory park1;
|
||||
protected Trajectory park2;
|
||||
|
||||
private Robot robot;
|
||||
private String randomization;
|
||||
|
||||
//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, -29, Math.toRadians(360));
|
||||
final static Pose2d CENTER_PRELOAD = new Pose2d(33, -28, Math.toRadians(270));
|
||||
final static Pose2d RIGHT_PRELOAD = new Pose2d(46, -35, Math.toRadians(270));
|
||||
final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -37.5, Math.toRadians(270));
|
||||
final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -29, Math.toRadians(180));
|
||||
final static Pose2d CENTER_PRELOAD = new Pose2d(-33, -28, Math.toRadians(270));
|
||||
final static Pose2d LEFT_PRELOAD = new Pose2d(-43, -35, Math.toRadians(270));
|
||||
//Board Scores
|
||||
final static Pose2d LEFT_BOARD = new Pose2d(76, -26.5, Math.toRadians(360));
|
||||
final static Pose2d CENTER_BOARD = new Pose2d(75.7, -36.3, Math.toRadians(360));
|
||||
final static Pose2d RIGHT_BOARD = new Pose2d(75, -42, Math.toRadians(360));
|
||||
final static Pose2d RIGHT_BOARD = new Pose2d(-75.3, -24.5, Math.toRadians(185));
|
||||
final static Pose2d CENTER_BOARD = new Pose2d(-75.3, -35, Math.toRadians(185));
|
||||
final static Pose2d LEFT_BOARD = new Pose2d(-75.3, -42, Math.toRadians(185));
|
||||
//Stack Cycle
|
||||
final static Pose2d LEAVE_BOARD = new Pose2d(65, -10, Math.toRadians(360));
|
||||
final static Pose2d TO_STACK = new Pose2d(-41, -8, Math.toRadians(360));
|
||||
final static Pose2d BACK_THROUGH_GATE = new Pose2d(50, -10, Math.toRadians(360));
|
||||
final static Pose2d APPROACHING_BOARD = new Pose2d(73.5, -28, Math.toRadians(360));
|
||||
final static Pose2d SCORE_STACK = new Pose2d(72.5, -28, Math.toRadians(360));
|
||||
final static Pose2d LEAVE_BOARD = new Pose2d(-65, -10, Math.toRadians(180));
|
||||
final static Pose2d TO_STACK = new Pose2d(40.75, -7.25, 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(-74.5, -28, Math.toRadians(180));
|
||||
//Park
|
||||
final static Pose2d BACK_OFF = new Pose2d(-60,-58,Math.toRadians(180));
|
||||
final static Pose2d PARK = new Pose2d(-80, -60, Math.toRadians(180));
|
||||
|
||||
protected void scorePreloadOne() {
|
||||
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
|
||||
switch (randomization) {
|
||||
case "LEFT":
|
||||
builder.lineToLinearHeading(LEFT_PRELOAD_ONE);
|
||||
builder.lineToLinearHeading(LEFT_PRELOAD_TWO);
|
||||
case "RIGHT":
|
||||
builder.lineToLinearHeading(RIGHT_PRELOAD_ONE);
|
||||
builder.lineToLinearHeading(RIGHT_PRELOAD_TWO);
|
||||
break;
|
||||
case "CENTER":
|
||||
builder.lineToLinearHeading(CENTER_PRELOAD);
|
||||
case "RIGHT":
|
||||
builder.lineToLinearHeading(RIGHT_PRELOAD);
|
||||
break;
|
||||
case "LEFT":
|
||||
builder.lineToLinearHeading(LEFT_PRELOAD);
|
||||
break;
|
||||
}
|
||||
this.robot.getDrive().followTrajectorySequence(builder.build());
|
||||
}
|
||||
|
|
@ -55,12 +57,15 @@ public class AutoRedTwoPlusTwoNew extends LinearOpMode {
|
|||
protected void boardScore() {
|
||||
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
|
||||
switch (randomization) {
|
||||
case "LEFT":
|
||||
builder.lineToLinearHeading(LEFT_BOARD);
|
||||
case "CENTER":
|
||||
builder.lineToLinearHeading(CENTER_BOARD);
|
||||
case "RIGHT":
|
||||
builder.lineToLinearHeading(RIGHT_BOARD);
|
||||
break;
|
||||
case "CENTER":
|
||||
builder.lineToLinearHeading(CENTER_BOARD);
|
||||
break;
|
||||
case "LEFT":
|
||||
builder.lineToLinearHeading(LEFT_BOARD);
|
||||
break;
|
||||
}
|
||||
builder.addTemporalMarker(.2, robot.getArm()::armScore);
|
||||
builder.addTemporalMarker(.2, robot.getSlides()::slideFirstLayer);
|
||||
|
|
@ -71,9 +76,10 @@ public class AutoRedTwoPlusTwoNew extends LinearOpMode {
|
|||
protected void toStack() {
|
||||
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
|
||||
builder.lineToLinearHeading(LEAVE_BOARD);
|
||||
builder.addTemporalMarker(.3, robot.getArm()::armRest);
|
||||
builder.addTemporalMarker(.3, robot.getArm()::armPickupStack);
|
||||
builder.addTemporalMarker(.3, robot.getWrist()::wristPickup);
|
||||
builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
|
||||
builder.addTemporalMarker(1.5,robot.getClaw()::openStack);
|
||||
builder.lineToLinearHeading(TO_STACK);
|
||||
this.robot.getDrive().followTrajectorySequence(builder.build());
|
||||
}
|
||||
|
|
@ -83,22 +89,35 @@ public class AutoRedTwoPlusTwoNew extends LinearOpMode {
|
|||
builder.lineToLinearHeading(BACK_THROUGH_GATE);
|
||||
builder.lineToLinearHeading(APPROACHING_BOARD);
|
||||
builder.lineToLinearHeading(SCORE_STACK);
|
||||
builder.addTemporalMarker(2, robot.getArm()::armSecondaryScore);
|
||||
builder.addTemporalMarker(2, robot.getWrist()::wristScore);
|
||||
builder.addTemporalMarker(2, robot.getSlides()::slideAutoStacks);
|
||||
builder.addTemporalMarker(2.5, robot.getArm()::armSecondaryScore);
|
||||
builder.addTemporalMarker(2.5, robot.getWrist()::wristScore);
|
||||
builder.addTemporalMarker(2.5, robot.getSlides()::slideAutoStacks);
|
||||
this.robot.getDrive().followTrajectorySequence(builder.build());
|
||||
}
|
||||
|
||||
protected void clawSlowOpen() {
|
||||
double currentPos = .86;
|
||||
double targetPos = .78;
|
||||
double targetPos = .8;
|
||||
double delta = (targetPos - currentPos) / 100;
|
||||
for (int i = 0; i < 100; i++) {
|
||||
// int Position = this.robot.getSlides().slidesLeft.getCurrentPosition();
|
||||
this.robot.getClaw().setPos(currentPos + (delta * i));
|
||||
sleep(12);
|
||||
// this.robot.getSlides().slideTo(Position + 14,1);
|
||||
sleep(35);
|
||||
}
|
||||
}
|
||||
|
||||
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());
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
|
|
@ -106,17 +125,17 @@ public class AutoRedTwoPlusTwoNew extends LinearOpMode {
|
|||
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.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270));
|
||||
this.robot.getDrive().setPoseEstimate(initialPosition);
|
||||
|
||||
this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end())
|
||||
.lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360)))
|
||||
.build();
|
||||
// this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end())
|
||||
// .lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360)))
|
||||
// .build();
|
||||
|
||||
// Do super fancy chinese shit
|
||||
while (!this.isStarted()) {
|
||||
this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition());
|
||||
randomization = String.valueOf(this.robot.getCamera().getStartingPosition());
|
||||
this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPositionBlue());
|
||||
randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue());
|
||||
this.telemetry.update();
|
||||
}
|
||||
|
||||
|
|
@ -130,13 +149,15 @@ public class AutoRedTwoPlusTwoNew extends LinearOpMode {
|
|||
toStack();
|
||||
|
||||
sleep(500);
|
||||
this.robot.getClaw().setPos(.86);
|
||||
sleep(250);
|
||||
this.robot.getClaw().close();
|
||||
sleep(500);
|
||||
this.robot.getArm().armRest();
|
||||
scoreStack();
|
||||
|
||||
sleep(200);
|
||||
this.robot.getClaw().setPos(.86);
|
||||
sleep(150);
|
||||
clawSlowOpen();
|
||||
sleep(300);
|
||||
park();
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -1,142 +1,132 @@
|
|||
package org.firstinspires.ftc.teamcode.opmodes;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
import com.acmerobotics.roadrunner.trajectory.Trajectory;
|
||||
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;
|
||||
@Config
|
||||
@Autonomous(name = "autoRed2+2")
|
||||
public class AutoRedTwoPlusTwo extends LinearOpMode {
|
||||
protected Pose2d initialPosition;
|
||||
|
||||
protected Trajectory preloadOne;
|
||||
protected Trajectory scoreOne;
|
||||
protected Trajectory boardOne;
|
||||
protected Trajectory backOffOne;
|
||||
|
||||
|
||||
protected Trajectory preloadTwo;
|
||||
protected Trajectory scoreTwo;
|
||||
protected Trajectory backOffTwo;
|
||||
|
||||
|
||||
protected Trajectory preloadThree;
|
||||
protected Trajectory scoreThree;
|
||||
protected Trajectory boardThree;
|
||||
protected Trajectory backOffThree;
|
||||
|
||||
|
||||
protected Trajectory park1;
|
||||
protected Trajectory park2;
|
||||
|
||||
protected Trajectory goGate;
|
||||
protected Trajectory goStack;
|
||||
protected Trajectory backGate;
|
||||
protected Trajectory approachBoard;
|
||||
protected Trajectory scoreStack;
|
||||
|
||||
|
||||
private Robot robot;
|
||||
private String randomization;
|
||||
private int random;
|
||||
|
||||
//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, -29, Math.toRadians(360));
|
||||
final static Pose2d CENTER_PRELOAD = new Pose2d(33, -28, Math.toRadians(270));
|
||||
final static Pose2d RIGHT_PRELOAD = new Pose2d(43, -35, Math.toRadians(270));
|
||||
//Board Scores
|
||||
final static Pose2d LEFT_BOARD = new Pose2d(74.3, -26.5, Math.toRadians(355));
|
||||
final static Pose2d CENTER_BOARD = new Pose2d(74.7, -36.3, Math.toRadians(355));
|
||||
final static Pose2d RIGHT_BOARD = new Pose2d(74.3, -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(-40, -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, -28, Math.toRadians(360));
|
||||
final static Pose2d SCORE_STACK = new Pose2d(74.45, -28, Math.toRadians(360));
|
||||
//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);
|
||||
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);
|
||||
break;
|
||||
case "RIGHT":
|
||||
builder.lineToLinearHeading(RIGHT_BOARD);
|
||||
break;
|
||||
}
|
||||
builder.addTemporalMarker(.2, robot.getArm()::armScore);
|
||||
builder.addTemporalMarker(.2, robot.getSlides()::slideFirstLayer);
|
||||
builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
|
||||
this.robot.getDrive().followTrajectorySequence(builder.build());
|
||||
}
|
||||
|
||||
protected void toStack() {
|
||||
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
|
||||
builder.lineToLinearHeading(LEAVE_BOARD);
|
||||
builder.addTemporalMarker(.3, robot.getArm()::armPickupStack);
|
||||
builder.addTemporalMarker(.3, robot.getWrist()::wristPickup);
|
||||
builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
|
||||
builder.addTemporalMarker(1.5,robot.getClaw()::openStack);
|
||||
builder.lineToLinearHeading(TO_STACK);
|
||||
this.robot.getDrive().followTrajectorySequence(builder.build());
|
||||
}
|
||||
|
||||
protected void scoreStack() {
|
||||
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.addTemporalMarker(2.5, robot.getWrist()::wristScore);
|
||||
builder.addTemporalMarker(2.5, robot.getSlides()::slideAutoStacks);
|
||||
this.robot.getDrive().followTrajectorySequence(builder.build());
|
||||
}
|
||||
|
||||
protected void clawSlowOpen() {
|
||||
double currentPos = .86;
|
||||
double targetPos = .78;
|
||||
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();
|
||||
builder.lineToLinearHeading(BACK_OFF);
|
||||
builder.lineToLinearHeading(PARK);
|
||||
builder.addTemporalMarker(.1, robot.getArm()::armRest);
|
||||
builder.addTemporalMarker(.1, robot.getWrist()::wristPickup);
|
||||
builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
|
||||
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.robot.getCamera().initTargetingCamera();
|
||||
|
||||
//Trajectories
|
||||
this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270));
|
||||
this.robot.getDrive().setPoseEstimate(initialPosition);
|
||||
|
||||
//Randomization One
|
||||
this.preloadOne = this.robot.getDrive().trajectoryBuilder(initialPosition)
|
||||
.lineToLinearHeading(new Pose2d(40, -37.5, Math.toRadians(270)))
|
||||
.build();
|
||||
|
||||
this.scoreOne = this.robot.getDrive().trajectoryBuilder(preloadOne.end())
|
||||
.lineToLinearHeading(new Pose2d(29.5, -29, Math.toRadians(360)))
|
||||
.build();
|
||||
|
||||
this.boardOne = this.robot.getDrive().trajectoryBuilder(scoreOne.end())
|
||||
.lineToLinearHeading(new Pose2d(76, -26.5, Math.toRadians(360)))
|
||||
.addTemporalMarker(.2, robot.getArm()::armScore)
|
||||
.addTemporalMarker(.2,robot.getSlides()::slideFirstLayer)
|
||||
.addTemporalMarker(.2, robot.getWrist()::wristScore)
|
||||
.build();
|
||||
|
||||
this.backOffOne = this.robot.getDrive().trajectoryBuilder(boardOne.end())
|
||||
.lineToLinearHeading(new Pose2d(60, -15, Math.toRadians(360)))
|
||||
.build();
|
||||
|
||||
|
||||
//Randomization Two
|
||||
this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition)
|
||||
.lineToLinearHeading(new Pose2d(33, -28, Math.toRadians(270)))
|
||||
.build();
|
||||
|
||||
this.scoreTwo = this.robot.getDrive().trajectoryBuilder(preloadTwo.end())
|
||||
.lineToLinearHeading(new Pose2d(75.7, -36.3, Math.toRadians(360)))
|
||||
.addTemporalMarker(.2, robot.getArm()::armScore)
|
||||
.addTemporalMarker(.2,robot.getSlides()::slideFirstLayer)
|
||||
.addTemporalMarker(.2, robot.getWrist()::wristScore)
|
||||
.build();
|
||||
|
||||
this.backOffTwo = this.robot.getDrive().trajectoryBuilder(scoreTwo.end())
|
||||
.lineToLinearHeading(new Pose2d(60, -35, Math.toRadians(360)))
|
||||
.build();
|
||||
|
||||
//Randomization Three
|
||||
this.preloadThree = this.robot.getDrive().trajectoryBuilder(initialPosition)
|
||||
.lineToLinearHeading(new Pose2d(46, -35, Math.toRadians(270)))
|
||||
.build();
|
||||
|
||||
this.scoreThree = this.robot.getDrive().trajectoryBuilder(preloadThree.end())
|
||||
.lineToLinearHeading(new Pose2d(75, -42, Math.toRadians(360)))
|
||||
.addTemporalMarker(.2, robot.getArm()::armScore)
|
||||
.addTemporalMarker(.2,robot.getSlides()::slideFirstLayer)
|
||||
.addTemporalMarker(.2, robot.getWrist()::wristScore)
|
||||
.build();
|
||||
|
||||
this.backOffThree = this.robot.getDrive().trajectoryBuilder(scoreThree.end())
|
||||
.lineToLinearHeading(new Pose2d(60, -45, Math.toRadians(360)))
|
||||
.build();
|
||||
|
||||
//Park
|
||||
this.park1 = this.robot.getDrive().trajectoryBuilder(backOffTwo.end())
|
||||
.lineToLinearHeading(new Pose2d(65, -10, Math.toRadians(360)))
|
||||
.addTemporalMarker(.3, robot.getArm()::armRest)
|
||||
.addTemporalMarker(.3, robot.getWrist()::wristPickup)
|
||||
.addTemporalMarker(.1,robot.getSlides()::slideDown)
|
||||
.build();
|
||||
this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end())
|
||||
.lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360)))
|
||||
.build();
|
||||
|
||||
// //Cycle
|
||||
this.goGate = this.robot.getDrive().trajectoryBuilder(park1.end())
|
||||
.lineToLinearHeading(new Pose2d(-41,-8, Math.toRadians(360)))
|
||||
.addTemporalMarker(.5,robot.getClaw()::openStack)
|
||||
.addTemporalMarker(.2,robot.getArm()::armPickupStack)
|
||||
.addTemporalMarker(.1,robot.getSlides()::slideDown)
|
||||
.build();
|
||||
this.backGate = this.robot.getDrive().trajectoryBuilder(goGate.end())
|
||||
.lineToLinearHeading(new Pose2d(50,-10, Math.toRadians(360)))
|
||||
.build();
|
||||
this.approachBoard = this.robot.getDrive().trajectoryBuilder(backGate.end())
|
||||
.lineToLinearHeading(new Pose2d(73.5, -28, Math.toRadians(360)))
|
||||
.addTemporalMarker(.2, robot.getArm()::armSecondaryScore)
|
||||
.addTemporalMarker(.2, robot.getWrist()::wristScore)
|
||||
.addTemporalMarker(.2,robot.getSlides()::slideAutoStacks)
|
||||
.build();
|
||||
this.scoreStack = this.robot.getDrive().trajectoryBuilder(approachBoard.end())
|
||||
.lineToLinearHeading(new Pose2d(72.5, -28, Math.toRadians(360)))
|
||||
.build();
|
||||
// this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end())
|
||||
// .lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360)))
|
||||
// .build();
|
||||
|
||||
// Do super fancy chinese shit
|
||||
while (!this.isStarted()) {
|
||||
|
|
@ -145,60 +135,25 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
|
|||
this.telemetry.update();
|
||||
}
|
||||
|
||||
switch (randomization) {
|
||||
case "LEFT":
|
||||
this.robot.getDrive().followTrajectory(preloadOne);
|
||||
this.robot.getDrive().followTrajectory(scoreOne);
|
||||
this.robot.getDrive().followTrajectory(boardOne);
|
||||
sleep(500);
|
||||
this.robot.getClaw().open();
|
||||
sleep(500);
|
||||
this.robot.getDrive().followTrajectory(backOffOne);
|
||||
this.robot.getSlides().slideDown();
|
||||
sleep(300);
|
||||
break;
|
||||
case "CENTER":
|
||||
this.robot.getDrive().followTrajectory(preloadTwo);
|
||||
this.robot.getDrive().followTrajectory(scoreTwo);
|
||||
sleep(500);
|
||||
this.robot.getClaw().open();
|
||||
sleep(500);
|
||||
this.robot.getDrive().followTrajectory(backOffTwo);
|
||||
this.robot.getSlides().slideDown();
|
||||
sleep(300);
|
||||
break;
|
||||
case "RIGHT":
|
||||
this.robot.getDrive().followTrajectory(preloadThree);
|
||||
this.robot.getDrive().followTrajectory(scoreThree);
|
||||
sleep(500);
|
||||
this.robot.getClaw().open();
|
||||
sleep(500);
|
||||
this.robot.getDrive().followTrajectory(backOffThree);
|
||||
this.robot.getSlides().slideDown();
|
||||
sleep(300);
|
||||
break;
|
||||
}
|
||||
//Cycle
|
||||
this.robot.getDrive().followTrajectory(park1);
|
||||
this.robot.getDrive().followTrajectory(goGate);
|
||||
scorePreloadOne();
|
||||
boardScore();
|
||||
|
||||
sleep(250);
|
||||
this.robot.getClaw().open();
|
||||
sleep(250);
|
||||
|
||||
toStack();
|
||||
|
||||
sleep(500);
|
||||
this.robot.getClaw().close();
|
||||
sleep(250);
|
||||
this.robot.getDrive().followTrajectory(backGate);
|
||||
this.robot.getDrive().followTrajectory(approachBoard);
|
||||
sleep(500);
|
||||
this.robot.getArm().armRest();
|
||||
scoreStack();
|
||||
this.robot.getClaw().setPos(.86);
|
||||
sleep(200);
|
||||
|
||||
double currentPos = .86;
|
||||
double targetPos = .78;
|
||||
double delta = (targetPos - currentPos) / 100;
|
||||
for (int i = 0 ; i < 100; i++) {
|
||||
this.robot.getClaw().setPos(currentPos + (delta * i));
|
||||
sleep(12);
|
||||
}
|
||||
|
||||
sleep(500);
|
||||
clawSlowOpen();
|
||||
sleep(300);
|
||||
this.robot.getDrive().followTrajectory(park1);
|
||||
park();
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -16,13 +16,13 @@ public class Configurables {
|
|||
//Servo Positions
|
||||
public static double ARMREST = 0.8;
|
||||
public static double ARMSCORE = 0.39;
|
||||
public static double ARMACCSCORE = .39;
|
||||
public static double ARMPICKUPSTACK = 0.825;
|
||||
public static double ARMACCSCORE = .38;
|
||||
public static double ARMPICKUPSTACK = 0.843;
|
||||
public static double PICKUP = 0.84;
|
||||
public static double WRISTPICKUP = 0.28;
|
||||
public static double WRISTSCORE = .96;
|
||||
public static double OPEN = 0.85;
|
||||
public static double BIGOPEN = 0.73;
|
||||
public static double BIGOPEN = 0.67;
|
||||
public static double CLOSE = 0.95;
|
||||
public static double PLANELOCK = 0.1;
|
||||
public static double PLANELAUNCH = 0.9;
|
||||
|
|
@ -38,7 +38,7 @@ public class Configurables {
|
|||
public static double SLIDE_POWER_UP = 1;
|
||||
public static double SLIDE_POWER_DOWN = .7;
|
||||
public static int SLIDELAYERONE = 150;
|
||||
public static int SLIDEAUTOSTACKS = 300;
|
||||
public static int SLIDEAUTOSTACKS = 225;
|
||||
public static int SLIDEUP = 1150;
|
||||
public static int HANGRELEASE = 2500;
|
||||
public static int HANG = 0;
|
||||
|
|
|
|||
Loading…
Reference in New Issue