READY CODE LM3
This commit is contained in:
sihan
parent
dbf8423ae3
commit
f8c5ba3296
|
|
@ -76,7 +76,7 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr
|
|||
|
||||
private TrajectoryFollower follower;
|
||||
|
||||
private DcMotorEx leftFront, leftRear, rightRear, rightFront;
|
||||
public DcMotorEx leftFront, leftRear, rightRear, rightFront;
|
||||
private List<DcMotorEx> motors;
|
||||
|
||||
private IMU imu;
|
||||
|
|
|
|||
|
|
@ -1,111 +1,96 @@
|
|||
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 = "autoBlue")
|
||||
public class AutoBlue 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 backOffThree;
|
||||
|
||||
|
||||
protected Trajectory park1;
|
||||
protected Trajectory park2;
|
||||
|
||||
|
||||
private Robot robot;
|
||||
private String randomization;
|
||||
private int random;
|
||||
|
||||
//Pose2ds
|
||||
//Preloads
|
||||
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 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));
|
||||
//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 "RIGHT":
|
||||
builder.lineToLinearHeading(RIGHT_PRELOAD_ONE);
|
||||
builder.lineToLinearHeading(RIGHT_PRELOAD_TWO);
|
||||
break;
|
||||
case "CENTER":
|
||||
builder.lineToLinearHeading(CENTER_PRELOAD);
|
||||
break;
|
||||
case "LEFT":
|
||||
builder.lineToLinearHeading(LEFT_PRELOAD);
|
||||
break;
|
||||
}
|
||||
this.robot.getDrive().followTrajectorySequence(builder.build());
|
||||
}
|
||||
|
||||
protected void boardScore() {
|
||||
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
|
||||
switch (randomization) {
|
||||
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);
|
||||
builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
|
||||
this.robot.getDrive().followTrajectorySequence(builder.build());
|
||||
}
|
||||
|
||||
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 {
|
||||
|
||||
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, -32, Math.toRadians(180)))
|
||||
.build();
|
||||
|
||||
this.boardOne = this.robot.getDrive().trajectoryBuilder(scoreOne.end())
|
||||
.lineToLinearHeading(new Pose2d(-72, -26.3, Math.toRadians(180)))
|
||||
.addTemporalMarker(.2, robot.getArm()::armSecondaryScore)
|
||||
.addTemporalMarker(.2, robot.getWrist()::wristScore)
|
||||
.build();
|
||||
|
||||
this.backOffOne = this.robot.getDrive().trajectoryBuilder(boardOne.end())
|
||||
.lineToLinearHeading(new Pose2d(-60, -26, Math.toRadians(180)))
|
||||
.build();
|
||||
|
||||
|
||||
//Randomization Two
|
||||
this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition)
|
||||
.lineToLinearHeading(new Pose2d(-35, -28, Math.toRadians(270)))
|
||||
.build();
|
||||
|
||||
this.scoreTwo = this.robot.getDrive().trajectoryBuilder(preloadTwo.end())
|
||||
.lineToLinearHeading(new Pose2d(-70, -34, Math.toRadians(180)))
|
||||
.addTemporalMarker(.2, robot.getArm()::armSecondaryScore)
|
||||
.addTemporalMarker(.2, robot.getWrist()::wristScore)
|
||||
.build();
|
||||
|
||||
this.backOffTwo = this.robot.getDrive().trajectoryBuilder(scoreTwo.end())
|
||||
.lineToLinearHeading(new Pose2d(-60, -34, Math.toRadians(180)))
|
||||
.build();
|
||||
|
||||
//Randomization Three
|
||||
this.preloadThree = this.robot.getDrive().trajectoryBuilder(initialPosition)
|
||||
.lineToLinearHeading(new Pose2d(-42, -35, Math.toRadians(270)))
|
||||
.build();
|
||||
|
||||
this.scoreThree = this.robot.getDrive().trajectoryBuilder(preloadThree.end())
|
||||
.lineToLinearHeading(new Pose2d(-71.5, -41.3, Math.toRadians(180)))
|
||||
.addTemporalMarker(.2, robot.getArm()::armSecondaryScore)
|
||||
.addTemporalMarker(.2, robot.getWrist()::wristScore)
|
||||
.build();
|
||||
|
||||
this.backOffThree = this.robot.getDrive().trajectoryBuilder(scoreThree.end())
|
||||
.lineToLinearHeading(new Pose2d(-60, -40, Math.toRadians(180)))
|
||||
.build();
|
||||
|
||||
//Park
|
||||
this.park1 = this.robot.getDrive().trajectoryBuilder(backOffTwo.end())
|
||||
.lineToLinearHeading(new Pose2d(-65, -55, Math.toRadians(180)))
|
||||
.addTemporalMarker(.3, robot.getArm()::armRest)
|
||||
.addTemporalMarker(.3, robot.getWrist()::wristPickup)
|
||||
.build();
|
||||
this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end())
|
||||
.lineToLinearHeading(new Pose2d(-80, -60, Math.toRadians(180)))
|
||||
.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()) {
|
||||
|
|
@ -114,41 +99,12 @@ public class AutoBlue extends LinearOpMode {
|
|||
this.telemetry.update();
|
||||
}
|
||||
|
||||
switch (randomization) {
|
||||
case "RIGHT":
|
||||
this.robot.getDrive().followTrajectory(preloadOne);
|
||||
this.robot.getDrive().followTrajectory(scoreOne);
|
||||
this.robot.getDrive().followTrajectory(boardOne);
|
||||
this.robot.getClaw().open();
|
||||
sleep(500);
|
||||
this.robot.getDrive().followTrajectory(backOffOne);
|
||||
sleep(500);
|
||||
this.robot.getDrive().followTrajectory(park1);
|
||||
this.robot.getDrive().followTrajectory(park2);
|
||||
break;
|
||||
case "CENTER":
|
||||
this.robot.getDrive().followTrajectory(preloadTwo);
|
||||
this.robot.getDrive().followTrajectory(scoreTwo);
|
||||
this.robot.getClaw().open();
|
||||
sleep(500);
|
||||
this.robot.getDrive().followTrajectory(backOffTwo);
|
||||
sleep(500);
|
||||
this.robot.getDrive().followTrajectory(park1);
|
||||
this.robot.getDrive().followTrajectory(park2);
|
||||
break;
|
||||
case "LEFT":
|
||||
this.robot.getDrive().followTrajectory(preloadThree);
|
||||
this.robot.getDrive().followTrajectory(scoreThree);
|
||||
this.robot.getClaw().open();
|
||||
sleep(500);
|
||||
this.robot.getDrive().followTrajectory(backOffThree);
|
||||
sleep(500);
|
||||
this.robot.getDrive().followTrajectory(park1);
|
||||
this.robot.getDrive().followTrajectory(park2);
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
scorePreloadOne();
|
||||
boardScore();
|
||||
sleep(250);
|
||||
this.robot.getClaw().open();
|
||||
sleep(250);
|
||||
park();
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,150 +1,60 @@
|
|||
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 = "autoBlueFar")
|
||||
public class AutoBlueFar extends LinearOpMode {
|
||||
protected Pose2d initialPosition;
|
||||
|
||||
protected Trajectory preloadOne;
|
||||
protected Trajectory scoreOne;
|
||||
protected Trajectory boardOne;
|
||||
protected Trajectory backOffOne;
|
||||
protected Trajectory goGate1;
|
||||
protected Trajectory passGate;
|
||||
|
||||
protected Trajectory preloadTwo;
|
||||
protected Trajectory scoreTwo;
|
||||
protected Trajectory backOffTwo;
|
||||
protected Trajectory tokyoDrift;
|
||||
protected Trajectory tokyoDrift2;
|
||||
protected Trajectory tokyoDrift3;
|
||||
|
||||
|
||||
protected Trajectory preloadThree;
|
||||
protected Trajectory boardThree;
|
||||
protected Trajectory scoreThree;
|
||||
protected Trajectory backOffThree;
|
||||
protected Trajectory goGate3;
|
||||
protected Trajectory goGate3Again;
|
||||
|
||||
|
||||
protected Trajectory park1;
|
||||
protected Trajectory park2;
|
||||
|
||||
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));
|
||||
|
||||
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 goBackToWhereYouCameFrom() {
|
||||
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
|
||||
builder.lineToLinearHeading(initialPosition);
|
||||
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(35, -20, Math.toRadians(150)))
|
||||
.build();
|
||||
|
||||
this.goGate1 = this.robot.getDrive().trajectoryBuilder(scoreOne.end())
|
||||
.lineToLinearHeading(new Pose2d(31, -10, Math.toRadians(180)))
|
||||
.build();
|
||||
|
||||
this.passGate = this.robot.getDrive().trajectoryBuilder(goGate1.end())
|
||||
.lineToLinearHeading(new Pose2d(-40, -12, Math.toRadians(180)))
|
||||
.build();
|
||||
|
||||
this.boardOne = this.robot.getDrive().trajectoryBuilder(passGate.end())
|
||||
.lineToLinearHeading(new Pose2d(-50, -28, Math.toRadians(180)))
|
||||
.addTemporalMarker(.2, robot.getArm()::armSecondaryScore)
|
||||
.addTemporalMarker(.2, robot.getWrist()::wristScore)
|
||||
.build();
|
||||
|
||||
this.backOffOne = this.robot.getDrive().trajectoryBuilder(boardOne.end())
|
||||
.lineToLinearHeading(new Pose2d(-40, -25, Math.toRadians(180)))
|
||||
.build();
|
||||
|
||||
//Randomization Two
|
||||
this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition)
|
||||
.lineToLinearHeading(new Pose2d(36, -28, Math.toRadians(290)))
|
||||
.build();
|
||||
|
||||
this.tokyoDrift = this.robot.getDrive().trajectoryBuilder(preloadTwo.end())
|
||||
.lineToLinearHeading(new Pose2d(50, -38, Math.toRadians(270)))
|
||||
.build();
|
||||
|
||||
this.tokyoDrift2 = this.robot.getDrive().trajectoryBuilder(tokyoDrift.end())
|
||||
.lineToLinearHeading(new Pose2d(50, -9, Math.toRadians(180)))
|
||||
.build();
|
||||
|
||||
this.tokyoDrift3 = this.robot.getDrive().trajectoryBuilder(tokyoDrift2.end())
|
||||
.lineToLinearHeading(new Pose2d(35, -9, Math.toRadians(180)))
|
||||
.build();
|
||||
|
||||
this.scoreTwo = this.robot.getDrive().trajectoryBuilder(passGate.end())
|
||||
.lineToLinearHeading(new Pose2d(-50, -33, Math.toRadians(180)))
|
||||
.addTemporalMarker(.02, robot.getArm()::armSecondaryScore)
|
||||
.addTemporalMarker(.02, robot.getWrist()::wristScore)
|
||||
.build();
|
||||
|
||||
this.backOffTwo = this.robot.getDrive().trajectoryBuilder(scoreTwo.end())
|
||||
.lineToLinearHeading(new Pose2d(-40, -33, Math.toRadians(180)))
|
||||
.build();
|
||||
|
||||
//Randomization Three
|
||||
this.preloadThree = this.robot.getDrive().trajectoryBuilder(initialPosition)
|
||||
.lineToLinearHeading(new Pose2d(43, -37.5, Math.toRadians(270)))
|
||||
.build();
|
||||
|
||||
this.scoreThree = this.robot.getDrive().trajectoryBuilder(preloadThree.end())
|
||||
.lineToLinearHeading(new Pose2d(29, -32, Math.toRadians(335)))
|
||||
.build();
|
||||
|
||||
this.goGate3 = this.robot.getDrive().trajectoryBuilder(scoreThree.end())
|
||||
.lineToLinearHeading(new Pose2d(40, -32, Math.toRadians(335)))
|
||||
.build();
|
||||
|
||||
this.goGate3Again = this.robot.getDrive().trajectoryBuilder(goGate3.end())
|
||||
.lineToLinearHeading(new Pose2d(35, -10, Math.toRadians(180)))
|
||||
.build();
|
||||
|
||||
this.boardThree = this.robot.getDrive().trajectoryBuilder(passGate.end())
|
||||
.lineToLinearHeading(new Pose2d(-50.5, -39, Math.toRadians(180)))
|
||||
.addTemporalMarker(.2, robot.getArm()::armSecondaryScore)
|
||||
.addTemporalMarker(.2, robot.getWrist()::wristScore)
|
||||
.build();
|
||||
|
||||
this.backOffThree = this.robot.getDrive().trajectoryBuilder(boardThree.end())
|
||||
.lineToLinearHeading(new Pose2d(-40, -40, Math.toRadians(180)))
|
||||
.build();
|
||||
|
||||
|
||||
//Park
|
||||
this.park1 = this.robot.getDrive().trajectoryBuilder(backOffOne.end())
|
||||
.lineToLinearHeading(new Pose2d(-40, -10, Math.toRadians(180)))
|
||||
.addTemporalMarker(.3, robot.getArm()::armRest)
|
||||
.addTemporalMarker(.3, robot.getWrist()::wristPickup)
|
||||
.build();
|
||||
this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end())
|
||||
.lineToLinearHeading(new Pose2d(-60, -10, Math.toRadians(170)))
|
||||
.build();
|
||||
|
||||
// Do super fancy chinese shit
|
||||
while (!this.isStarted()) {
|
||||
this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPositionBlue());
|
||||
|
|
@ -152,52 +62,8 @@ public class AutoBlueFar extends LinearOpMode {
|
|||
this.telemetry.update();
|
||||
}
|
||||
|
||||
sleep(5000);
|
||||
|
||||
switch (randomization) {
|
||||
case "RIGHT":
|
||||
this.robot.getDrive().followTrajectory(preloadOne);
|
||||
this.robot.getDrive().followTrajectory(scoreOne);
|
||||
this.robot.getDrive().followTrajectory(goGate1);
|
||||
this.robot.getDrive().followTrajectory(passGate);
|
||||
this.robot.getDrive().followTrajectory(boardOne);
|
||||
sleep(500);
|
||||
this.robot.getClaw().open();
|
||||
sleep(500);
|
||||
this.robot.getDrive().followTrajectory(backOffOne);
|
||||
sleep(300);
|
||||
break;
|
||||
case "CENTER":
|
||||
this.robot.getDrive().followTrajectory(preloadTwo);
|
||||
this.robot.getDrive().followTrajectory(tokyoDrift);
|
||||
this.robot.getDrive().followTrajectory(tokyoDrift2);
|
||||
this.robot.getDrive().followTrajectory(passGate);
|
||||
this.robot.getDrive().followTrajectory(scoreTwo);
|
||||
sleep(500);
|
||||
this.robot.getClaw().open();
|
||||
sleep(500);
|
||||
this.robot.getDrive().followTrajectory(backOffTwo);
|
||||
sleep(300);
|
||||
break;
|
||||
case "LEFT":
|
||||
this.robot.getDrive().followTrajectory(preloadThree);
|
||||
this.robot.getDrive().followTrajectory(scoreThree);
|
||||
this.robot.getDrive().followTrajectory(goGate3);
|
||||
this.robot.getDrive().followTrajectory(goGate3Again);
|
||||
this.robot.getDrive().followTrajectory(passGate);
|
||||
this.robot.getDrive().followTrajectory(boardThree);
|
||||
sleep(500);
|
||||
this.robot.getClaw().open();
|
||||
sleep(500);
|
||||
this.robot.getDrive().followTrajectory(backOffThree);
|
||||
sleep(300);
|
||||
break;
|
||||
}
|
||||
//Cycle
|
||||
this.robot.getDrive().followTrajectory(park1);
|
||||
this.robot.getDrive().followTrajectory(park2);
|
||||
|
||||
|
||||
scorePreloadOne();
|
||||
goBackToWhereYouCameFrom();
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -8,10 +8,12 @@ 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;
|
||||
|
||||
@Config
|
||||
@Autonomous(name = "autoBLue2+2")
|
||||
@Autonomous(name = "autoBlue2+2")
|
||||
public class AutoBlueTwoPlusTwo extends LinearOpMode {
|
||||
protected Pose2d initialPosition;
|
||||
private Robot robot;
|
||||
|
|
@ -20,19 +22,19 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode {
|
|||
//Pose2ds
|
||||
//Preloads
|
||||
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 RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -31, 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 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));
|
||||
final static Pose2d RIGHT_BOARD = new Pose2d(-75.7, -25.7, Math.toRadians(185));
|
||||
final static Pose2d CENTER_BOARD = new Pose2d(-75.7, -35, Math.toRadians(185));
|
||||
final static Pose2d LEFT_BOARD = new Pose2d(-75.7, -42, Math.toRadians(185));
|
||||
//Stack Cycle
|
||||
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));
|
||||
final static Pose2d SCORE_STACK = new Pose2d(-73, -31, 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));
|
||||
|
|
@ -76,10 +78,11 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode {
|
|||
protected void toStack() {
|
||||
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
|
||||
builder.lineToLinearHeading(LEAVE_BOARD);
|
||||
builder.addTemporalMarker(.3, robot.getArm()::armPickupStack);
|
||||
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()::pickup);
|
||||
builder.lineToLinearHeading(TO_STACK);
|
||||
this.robot.getDrive().followTrajectorySequence(builder.build());
|
||||
}
|
||||
|
|
@ -95,6 +98,15 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode {
|
|||
this.robot.getDrive().followTrajectorySequence(builder.build());
|
||||
}
|
||||
|
||||
protected void scoreTest() {
|
||||
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
|
||||
builder.lineToLinearHeading(new Pose2d(-77, -31, Math.toRadians(180)),
|
||||
MecanumDrive.getVelocityConstraint(20, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
|
||||
MecanumDrive.getAccelerationConstraint(20));
|
||||
builder.addTemporalMarker(.2,this::clawSlowOpen);
|
||||
this.robot.getDrive().followTrajectorySequence(builder.build());
|
||||
}
|
||||
|
||||
protected void clawSlowOpen() {
|
||||
double currentPos = .86;
|
||||
double targetPos = .8;
|
||||
|
|
@ -138,25 +150,21 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode {
|
|||
randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue());
|
||||
this.telemetry.update();
|
||||
}
|
||||
|
||||
scorePreloadOne();
|
||||
boardScore();
|
||||
|
||||
sleep(250);
|
||||
sleep(100);
|
||||
this.robot.getClaw().open();
|
||||
sleep(250);
|
||||
|
||||
toStack();
|
||||
|
||||
sleep(500);
|
||||
this.robot.getClaw().close();
|
||||
sleep(500);
|
||||
sleep(250);
|
||||
this.robot.getArm().armRest();
|
||||
scoreStack();
|
||||
this.robot.getClaw().setPos(.86);
|
||||
sleep(150);
|
||||
clawSlowOpen();
|
||||
sleep(300);
|
||||
scoreTest();
|
||||
park();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,121 +1,96 @@
|
|||
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 = "autoRed")
|
||||
public class AutoRed 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));
|
||||
|
||||
//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 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, -45, 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, -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, -55, Math.toRadians(360)))
|
||||
.addTemporalMarker(.3, robot.getArm()::armRest)
|
||||
.addTemporalMarker(.3, robot.getWrist()::wristPickup)
|
||||
.build();
|
||||
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()) {
|
||||
|
|
@ -124,68 +99,12 @@ public class AutoRed 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(park2);
|
||||
// this.robot.getDrive().followTrajectory(goGate);
|
||||
// sleep(120);
|
||||
// this.robot.getClaw().close();
|
||||
// sleep(120);
|
||||
// this.robot.getDrive().followTrajectory(backGate);
|
||||
// this.robot.getDrive().followTrajectory(approachBoard);
|
||||
// sleep(120);
|
||||
// this.robot.getClaw().open();
|
||||
// sleep(120);
|
||||
//
|
||||
// this.robot.getDrive().followTrajectory(park1);
|
||||
// this.robot.getDrive().followTrajectory(goGate);
|
||||
// sleep(120);
|
||||
// this.robot.getClaw().close();
|
||||
// sleep(120);
|
||||
// this.robot.getDrive().followTrajectory(backGate);
|
||||
// this.robot.getDrive().followTrajectory(approachBoard);
|
||||
// sleep(120);
|
||||
// this.robot.getClaw().open();
|
||||
// sleep(120);
|
||||
// this.robot.getDrive().followTrajectory(park1);
|
||||
// this.robot.getDrive().followTrajectory(park2);
|
||||
|
||||
|
||||
|
||||
|
||||
scorePreloadOne();
|
||||
boardScore();
|
||||
sleep(250);
|
||||
this.robot.getClaw().open();
|
||||
sleep(250);
|
||||
park();
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,151 +1,64 @@
|
|||
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;
|
||||
|
||||
@Autonomous(name = "autoRedFar")
|
||||
@Config
|
||||
@Autonomous(name = "autRedFar")
|
||||
public class AutoRedFar extends LinearOpMode {
|
||||
protected Pose2d initialPosition;
|
||||
|
||||
protected Trajectory preloadOne;
|
||||
protected Trajectory scoreOne;
|
||||
protected Trajectory boardOne;
|
||||
protected Trajectory backOffOne;
|
||||
protected Trajectory goGate1;
|
||||
protected Trajectory passGate;
|
||||
|
||||
protected Trajectory preloadTwo;
|
||||
protected Trajectory scoreTwo;
|
||||
protected Trajectory backOffTwo;
|
||||
protected Trajectory tokyoDrift;
|
||||
protected Trajectory tokyoDrift2;
|
||||
protected Trajectory tokyoDrift3;
|
||||
|
||||
protected Trajectory preloadThree;
|
||||
protected Trajectory boardThree;
|
||||
protected Trajectory scoreThree;
|
||||
protected Trajectory backOffThree;
|
||||
protected Trajectory goGate3;
|
||||
protected Trajectory goGate3Again;
|
||||
|
||||
|
||||
protected Trajectory park1;
|
||||
protected Trajectory park2;
|
||||
|
||||
private Robot robot;
|
||||
private String randomization;
|
||||
private int random;
|
||||
|
||||
//Pose2ds
|
||||
//Preloads
|
||||
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));
|
||||
|
||||
protected void scorePreloadOne() {
|
||||
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
|
||||
switch (randomization) {
|
||||
case "RIGHT":
|
||||
builder.lineToLinearHeading(RIGHT_PRELOAD_ONE);
|
||||
builder.lineToLinearHeading(RIGHT_PRELOAD_TWO);
|
||||
break;
|
||||
case "CENTER":
|
||||
builder.lineToLinearHeading(CENTER_PRELOAD);
|
||||
break;
|
||||
case "LEFT":
|
||||
builder.lineToLinearHeading(LEFT_PRELOAD);
|
||||
break;
|
||||
}
|
||||
this.robot.getDrive().followTrajectorySequence(builder.build());
|
||||
}
|
||||
|
||||
protected void goBackToWhereYouCameFrom() {
|
||||
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
|
||||
builder.lineToLinearHeading(initialPosition);
|
||||
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);
|
||||
//niggler
|
||||
//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(-35, -25, Math.toRadians(30)))
|
||||
.build();
|
||||
|
||||
this.goGate1 = this.robot.getDrive().trajectoryBuilder(scoreOne.end())
|
||||
.lineToLinearHeading(new Pose2d(-32, -10, Math.toRadians(360)))
|
||||
.build();
|
||||
|
||||
this.passGate = this.robot.getDrive().trajectoryBuilder(goGate1.end())
|
||||
.lineToLinearHeading(new Pose2d(40, -11, Math.toRadians(360)))
|
||||
.addTemporalMarker(2, robot.getArm()::armScore)
|
||||
.addTemporalMarker(2, robot.getWrist()::wristScore)
|
||||
.build();
|
||||
|
||||
this.boardOne = this.robot.getDrive().trajectoryBuilder(passGate.end())
|
||||
.lineToLinearHeading(new Pose2d(50, -28, Math.toRadians(360)))
|
||||
.addTemporalMarker(.02, robot.getArm()::armSecondaryScore)
|
||||
.addTemporalMarker(.02, robot.getWrist()::wristScore)
|
||||
.build();
|
||||
|
||||
this.backOffOne = this.robot.getDrive().trajectoryBuilder(boardOne.end())
|
||||
.lineToLinearHeading(new Pose2d(40, -25, Math.toRadians(360)))
|
||||
.build();
|
||||
|
||||
|
||||
//Randomization Two
|
||||
this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition)
|
||||
.lineToLinearHeading(new Pose2d(-36, -28, Math.toRadians(240)))
|
||||
.build();
|
||||
|
||||
this.tokyoDrift = this.robot.getDrive().trajectoryBuilder(preloadTwo.end())
|
||||
.lineToLinearHeading(new Pose2d(-50, -38, Math.toRadians(270)))
|
||||
.build();
|
||||
|
||||
this.tokyoDrift2 = this.robot.getDrive().trajectoryBuilder(tokyoDrift.end())
|
||||
.lineToLinearHeading(new Pose2d(-50, -9, Math.toRadians(360)))
|
||||
.build();
|
||||
|
||||
this.tokyoDrift3 = this.robot.getDrive().trajectoryBuilder(tokyoDrift2.end())
|
||||
.lineToLinearHeading(new Pose2d(-35, -9, Math.toRadians(360)))
|
||||
.build();
|
||||
|
||||
this.scoreTwo = this.robot.getDrive().trajectoryBuilder(passGate.end())
|
||||
.lineToLinearHeading(new Pose2d(50, -34, Math.toRadians(360)))
|
||||
.addTemporalMarker(.02, robot.getArm()::armSecondaryScore)
|
||||
.addTemporalMarker(.02, robot.getWrist()::wristScore)
|
||||
.build();
|
||||
|
||||
this.backOffTwo = this.robot.getDrive().trajectoryBuilder(scoreTwo.end())
|
||||
.lineToLinearHeading(new Pose2d(40, -33, Math.toRadians(360)))
|
||||
.build();
|
||||
|
||||
//Randomization Three
|
||||
this.preloadThree = this.robot.getDrive().trajectoryBuilder(initialPosition)
|
||||
.lineToLinearHeading(new Pose2d(-43, -37.5, Math.toRadians(270)))
|
||||
.build();
|
||||
|
||||
this.scoreThree = this.robot.getDrive().trajectoryBuilder(preloadThree.end())
|
||||
.lineToLinearHeading(new Pose2d(-28, -32, Math.toRadians(180)))
|
||||
.build();
|
||||
|
||||
this.goGate3 = this.robot.getDrive().trajectoryBuilder(scoreThree.end())
|
||||
.lineToSplineHeading(new Pose2d(-40, -32, Math.toRadians(180)))
|
||||
.build();
|
||||
|
||||
this.goGate3Again = this.robot.getDrive().trajectoryBuilder(goGate3.end())
|
||||
.lineToSplineHeading(new Pose2d(-33, -10, Math.toRadians(360)))
|
||||
.build();
|
||||
|
||||
this.boardThree = this.robot.getDrive().trajectoryBuilder(passGate.end())
|
||||
.lineToLinearHeading(new Pose2d(50, -39, Math.toRadians(360)))
|
||||
.addTemporalMarker(.02, robot.getArm()::armSecondaryScore)
|
||||
.addTemporalMarker(.02, robot.getWrist()::wristScore)
|
||||
.build();
|
||||
|
||||
this.backOffThree = this.robot.getDrive().trajectoryBuilder(boardThree.end())
|
||||
.lineToLinearHeading(new Pose2d(40, -42, Math.toRadians(360)))
|
||||
.build();
|
||||
|
||||
|
||||
//Park
|
||||
this.park1 = this.robot.getDrive().trajectoryBuilder(backOffOne.end())
|
||||
.lineToLinearHeading(new Pose2d(40, -12, Math.toRadians(360)))
|
||||
.addTemporalMarker(.3, robot.getArm()::armRest)
|
||||
.addTemporalMarker(.3, robot.getWrist()::wristPickup)
|
||||
.build();
|
||||
this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end())
|
||||
.lineToLinearHeading(new Pose2d(60, -10, Math.toRadians(10)))
|
||||
.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()) {
|
||||
|
|
@ -153,52 +66,9 @@ public class AutoRedFar extends LinearOpMode {
|
|||
randomization = String.valueOf(this.robot.getCamera().getStartingPosition());
|
||||
this.telemetry.update();
|
||||
}
|
||||
sleep(5000);
|
||||
switch (randomization) {
|
||||
case "LEFT":
|
||||
this.robot.getDrive().followTrajectory(preloadOne);
|
||||
this.robot.getDrive().followTrajectory(scoreOne);
|
||||
this.robot.getDrive().followTrajectory(goGate1);
|
||||
this.robot.getDrive().followTrajectory(passGate);
|
||||
this.robot.getDrive().followTrajectory(boardOne);
|
||||
sleep(500);
|
||||
this.robot.getClaw().open();
|
||||
sleep(500);
|
||||
this.robot.getDrive().followTrajectory(backOffOne);
|
||||
sleep(300);
|
||||
break;
|
||||
case "CENTER":
|
||||
this.robot.getDrive().followTrajectory(preloadTwo);
|
||||
this.robot.getDrive().followTrajectory(tokyoDrift);
|
||||
this.robot.getDrive().followTrajectory(tokyoDrift2);
|
||||
this.robot.getDrive().followTrajectory(tokyoDrift3);
|
||||
this.robot.getDrive().followTrajectory(passGate);
|
||||
this.robot.getDrive().followTrajectory(scoreTwo);
|
||||
sleep(500);
|
||||
this.robot.getClaw().open();
|
||||
sleep(500);
|
||||
this.robot.getDrive().followTrajectory(backOffTwo);
|
||||
sleep(300);
|
||||
break;
|
||||
case "RIGHT":
|
||||
this.robot.getDrive().followTrajectory(preloadThree);
|
||||
this.robot.getDrive().followTrajectory(scoreThree);
|
||||
this.robot.getDrive().followTrajectory(goGate3);
|
||||
this.robot.getDrive().followTrajectory(goGate3Again);
|
||||
this.robot.getDrive().followTrajectory(passGate);
|
||||
this.robot.getDrive().followTrajectory(boardThree);
|
||||
sleep(500);
|
||||
this.robot.getClaw().open();
|
||||
sleep(500);
|
||||
this.robot.getDrive().followTrajectory(backOffThree);
|
||||
sleep(300);
|
||||
break;
|
||||
}
|
||||
//Cycle
|
||||
this.robot.getDrive().followTrajectory(park1);
|
||||
this.robot.getDrive().followTrajectory(park2);
|
||||
|
||||
|
||||
scorePreloadOne();
|
||||
goBackToWhereYouCameFrom();
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -8,6 +8,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;
|
||||
@Config
|
||||
@Autonomous(name = "autoRed2+2")
|
||||
|
|
@ -19,19 +21,19 @@ public class AutoRedTwoPlusTwo 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, -29, Math.toRadians(360));
|
||||
final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(29.5, -32, 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));
|
||||
final static Pose2d LEFT_BOARD = new Pose2d(75.3, -26.5, Math.toRadians(360));
|
||||
final static Pose2d CENTER_BOARD = new Pose2d(75.3, -36.3, Math.toRadians(360));
|
||||
final static Pose2d RIGHT_BOARD = new Pose2d(75.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 TO_STACK = new Pose2d(-40, -8.4, 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));
|
||||
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));
|
||||
//Park
|
||||
final static Pose2d BACK_OFF = new Pose2d(60,-58,Math.toRadians(360));
|
||||
final static Pose2d PARK = new Pose2d(80, -60, Math.toRadians(360));
|
||||
|
|
@ -75,10 +77,11 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
|
|||
protected void toStack() {
|
||||
TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
|
||||
builder.lineToLinearHeading(LEAVE_BOARD);
|
||||
builder.addTemporalMarker(.3, robot.getArm()::armPickupStack);
|
||||
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()::pickup);
|
||||
builder.lineToLinearHeading(TO_STACK);
|
||||
this.robot.getDrive().followTrajectorySequence(builder.build());
|
||||
}
|
||||
|
|
@ -94,6 +97,15 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
|
|||
this.robot.getDrive().followTrajectorySequence(builder.build());
|
||||
}
|
||||
|
||||
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);
|
||||
this.robot.getDrive().followTrajectorySequence(builder.build());
|
||||
}
|
||||
|
||||
protected void clawSlowOpen() {
|
||||
double currentPos = .86;
|
||||
double targetPos = .78;
|
||||
|
|
@ -138,21 +150,18 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
|
|||
scorePreloadOne();
|
||||
boardScore();
|
||||
|
||||
sleep(250);
|
||||
sleep(100);
|
||||
this.robot.getClaw().open();
|
||||
sleep(250);
|
||||
|
||||
toStack();
|
||||
|
||||
sleep(500);
|
||||
this.robot.getClaw().close();
|
||||
sleep(500);
|
||||
sleep(250);
|
||||
this.robot.getArm().armRest();
|
||||
scoreStack();
|
||||
this.robot.getClaw().setPos(.86);
|
||||
sleep(500);
|
||||
clawSlowOpen();
|
||||
sleep(300);
|
||||
scoreTest();
|
||||
park();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -17,7 +17,7 @@ public class Configurables {
|
|||
public static double ARMREST = 0.8;
|
||||
public static double ARMSCORE = 0.39;
|
||||
public static double ARMACCSCORE = .38;
|
||||
public static double ARMPICKUPSTACK = 0.843;
|
||||
public static double ARMPICKUPSTACK = 0.8415;
|
||||
public static double PICKUP = 0.84;
|
||||
public static double WRISTPICKUP = 0.28;
|
||||
public static double WRISTSCORE = .96;
|
||||
|
|
@ -30,15 +30,15 @@ public class Configurables {
|
|||
|
||||
//Drive Speed
|
||||
public static double SPEED = 1;
|
||||
public static double SLOWMODE_SPEED = 0.5;
|
||||
public static double TURN = 1;
|
||||
public static double SLOWMODE_SPEED = 0.3;
|
||||
public static double TURN = .75;
|
||||
public static double SLOWMODE_TURN = 0.3;
|
||||
|
||||
//Motor Positions
|
||||
public static double SLIDE_POWER_UP = 1;
|
||||
public static double SLIDE_POWER_DOWN = .7;
|
||||
public static int SLIDELAYERONE = 150;
|
||||
public static int SLIDEAUTOSTACKS = 225;
|
||||
public static int SLIDEAUTOSTACKS = 350;
|
||||
public static int SLIDEUP = 1150;
|
||||
public static int HANGRELEASE = 2500;
|
||||
public static int HANG = 0;
|
||||
|
|
|
|||
Loading…
Reference in New Issue