Think all 4 autos work, thursday night before LM2
This commit is contained in:
sihan
parent
61f589b000
commit
1814413011
|
|
@ -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(25, 0, 2.5);
|
||||
public static PIDCoefficients HEADING_PID = new PIDCoefficients(15, 0, 1);
|
||||
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(20, 0, 1.5);
|
||||
public static PIDCoefficients HEADING_PID = new PIDCoefficients(10, 0, 2);
|
||||
|
||||
public static double LATERAL_MULTIPLIER = 1;
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,203 @@
|
|||
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.trajectory.Trajectory;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.hardware.Robot;
|
||||
|
||||
@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;
|
||||
|
||||
@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(-49.25, -28, Math.toRadians(180)))
|
||||
.addTemporalMarker(.2, robot.getArm()::armAccurateScore)
|
||||
.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()::armAccurateScore)
|
||||
.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()::armAccurateScore)
|
||||
.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());
|
||||
randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue());
|
||||
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);
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -9,8 +9,8 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
|||
|
||||
import org.firstinspires.ftc.teamcode.hardware.Robot;
|
||||
|
||||
@Autonomous(name = "ThisIsTheLongestFlippingOpModeNameEverLolIamSeahorse!")
|
||||
public class Auto extends LinearOpMode {
|
||||
@Autonomous(name = "autoRed")
|
||||
public class AutoRed extends LinearOpMode {
|
||||
protected Pose2d initialPosition;
|
||||
|
||||
protected Trajectory preloadOne;
|
||||
|
|
@ -33,6 +33,12 @@ public class Auto extends LinearOpMode {
|
|||
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;
|
||||
|
|
@ -58,13 +64,13 @@ public class Auto extends LinearOpMode {
|
|||
.build();
|
||||
|
||||
this.boardOne = this.robot.getDrive().trajectoryBuilder(scoreOne.end())
|
||||
.lineToLinearHeading(new Pose2d(73, -28, Math.toRadians(360)))
|
||||
.lineToLinearHeading(new Pose2d(72, -28, Math.toRadians(360)))
|
||||
.addTemporalMarker(.2, robot.getArm()::armAccurateScore)
|
||||
.addTemporalMarker(.2, robot.getWrist()::wristScore)
|
||||
.build();
|
||||
|
||||
this.backOffOne = this.robot.getDrive().trajectoryBuilder(boardOne.end())
|
||||
.lineToLinearHeading(new Pose2d(60, -26, Math.toRadians(360)))
|
||||
.lineToLinearHeading(new Pose2d(60, -45, Math.toRadians(360)))
|
||||
.build();
|
||||
|
||||
|
||||
|
|
@ -74,13 +80,13 @@ public class Auto extends LinearOpMode {
|
|||
.build();
|
||||
|
||||
this.scoreTwo = this.robot.getDrive().trajectoryBuilder(preloadTwo.end())
|
||||
.lineToLinearHeading(new Pose2d(73, -34.5, Math.toRadians(360)))
|
||||
.lineToLinearHeading(new Pose2d(72, -33.3, Math.toRadians(360)))
|
||||
.addTemporalMarker(.2, robot.getArm()::armAccurateScore)
|
||||
.addTemporalMarker(.2, robot.getWrist()::wristScore)
|
||||
.build();
|
||||
|
||||
this.backOffTwo = this.robot.getDrive().trajectoryBuilder(scoreTwo.end())
|
||||
.lineToLinearHeading(new Pose2d(60, -34, Math.toRadians(360)))
|
||||
.lineToLinearHeading(new Pose2d(60, -35, Math.toRadians(360)))
|
||||
.build();
|
||||
|
||||
//Randomization Three
|
||||
|
|
@ -95,19 +101,37 @@ public class Auto extends LinearOpMode {
|
|||
.build();
|
||||
|
||||
this.backOffThree = this.robot.getDrive().trajectoryBuilder(scoreThree.end())
|
||||
.lineToLinearHeading(new Pose2d(60, -40, Math.toRadians(360)))
|
||||
.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)))
|
||||
.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, -10, Math.toRadians(360)))
|
||||
.lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360)))
|
||||
.build();
|
||||
|
||||
// //Cycle
|
||||
// this.goGate = this.robot.getDrive().trajectoryBuilder(park1.end())
|
||||
// .lineToLinearHeading(new Pose2d(-37,-7, Math.toRadians(360)))
|
||||
// .addTemporalMarker(.3, robot.getArm()::armRest)
|
||||
// .addTemporalMarker(.3, robot.getWrist()::wristPickup)
|
||||
// .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(68, -28, Math.toRadians(360)))
|
||||
// .addTemporalMarker(.2, robot.getArm()::armAccurateScore)
|
||||
// .addTemporalMarker(.2, robot.getWrist()::wristScore)
|
||||
// .build();
|
||||
// this.scoreStack = this.robot.getDrive().trajectoryBuilder(approachBoard.end())
|
||||
// .lineToLinearHeading(new Pose2d(72.5, -28, Math.toRadians(360)))
|
||||
// .build();
|
||||
|
||||
// Do super fancy chinese shit
|
||||
while (!this.isStarted()) {
|
||||
this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition());
|
||||
|
|
@ -120,34 +144,58 @@ public class Auto extends LinearOpMode {
|
|||
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);
|
||||
sleep(500);
|
||||
this.robot.getDrive().followTrajectory(park1);
|
||||
this.robot.getDrive().followTrajectory(park2);
|
||||
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);
|
||||
sleep(500);
|
||||
this.robot.getDrive().followTrajectory(park1);
|
||||
this.robot.getDrive().followTrajectory(park2);
|
||||
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);
|
||||
sleep(500);
|
||||
this.robot.getDrive().followTrajectory(park1);
|
||||
this.robot.getDrive().followTrajectory(park2);
|
||||
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);
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,204 @@
|
|||
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.trajectory.Trajectory;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.hardware.Robot;
|
||||
|
||||
@Autonomous(name = "autoRedFar")
|
||||
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;
|
||||
|
||||
@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()::armAccurateScore)
|
||||
.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()::armAccurateScore)
|
||||
.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, -37, Math.toRadians(360)))
|
||||
.addTemporalMarker(.02, robot.getArm()::armAccurateScore)
|
||||
.addTemporalMarker(.02, robot.getWrist()::wristScore)
|
||||
.build();
|
||||
|
||||
this.backOffThree = this.robot.getDrive().trajectoryBuilder(boardThree.end())
|
||||
.lineToLinearHeading(new Pose2d(40, -47, Math.toRadians(360)))
|
||||
.build();
|
||||
|
||||
|
||||
//Park
|
||||
this.park1 = this.robot.getDrive().trajectoryBuilder(backOffOne.end())
|
||||
.lineToLinearHeading(new Pose2d(40, -10, 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(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.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);
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -18,10 +18,10 @@ public class Configurables {
|
|||
public static double LOCKSPEED = .25;
|
||||
public static double UNLOCK = .6;
|
||||
public static double LOCK = 0.4;
|
||||
public static double ARMREST = .88;
|
||||
public static double ARMSCORE = 0.15;
|
||||
public static double ARMACCSCORE = 0.02;
|
||||
public static double PICKUP = .935;
|
||||
public static double ARMREST = .91;
|
||||
public static double ARMSCORE = 0.14 ;
|
||||
public static double ARMACCSCORE = 0.04;
|
||||
public static double PICKUP = 0.97;
|
||||
public static double WRISTPICKUP = 0.28;
|
||||
public static double WRISTSCORE = .96;
|
||||
public static double OPEN = 0.53;
|
||||
|
|
@ -34,6 +34,6 @@ public class Configurables {
|
|||
public static double SPEED = 1;
|
||||
public static double SLOWMODE_SPEED = 0.5;
|
||||
public static double TURN = 1;
|
||||
public static double SLOWMODE_TURN = 0.5;
|
||||
public static double SLOWMODE_TURN = 0.3;
|
||||
|
||||
}
|
||||
Loading…
Reference in New Issue