Skip to content

Commit

Permalink
v2
Browse files Browse the repository at this point in the history
  • Loading branch information
@Aidshot
Aidshot committed May 7, 2021
1 parent a7f0b6e commit 52ae555
Show file tree
Hide file tree
Showing 7 changed files with 614 additions and 115 deletions.
28 changes: 14 additions & 14 deletions ...va/org/firstinspires/ftc/teamcode/drive/opmode/BERSERK/Competition/BLUE_FULL_AUTO_V3.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,20 +103,6 @@ public void runOpMode() throws InterruptedException {

State state = State.ZERO;

while (!isStarted()) {
switch (pipeline.getHeight()) {
case ZERO:
state = State.ZERO;
break;
case ONE:
state = State.ONE;
break;
case FOUR:
state = State.FOUR;
break;
}
}

Pose2d startPose = new Pose2d(-63.0,50, Math.toRadians(0.0));
drive.setPoseEstimate(startPose);

Expand Down Expand Up @@ -321,6 +307,20 @@ public void runOpMode() throws InterruptedException {
), new ProfileAccelerationConstraint(60))
.build();

while (!isStarted()) {
switch (pipeline.getHeight()) {
case ZERO:
state = State.ZERO;
break;
case ONE:
state = State.ONE;
break;
case FOUR:
state = State.FOUR;
break;
}
}

waitForStart();
if (isStopRequested()) return;

Expand Down
30 changes: 15 additions & 15 deletions ...va/org/firstinspires/ftc/teamcode/drive/opmode/BERSERK/Competition/BLUE_HALF_AUTO_V3.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,20 +103,6 @@ public void runOpMode() throws InterruptedException {

State state = State.ZERO;

while (!isStarted()) {
switch (pipeline.getHeight()) {
case ZERO:
state = State.ZERO;
break;
case ONE:
state = State.ONE;
break;
case FOUR:
state = State.FOUR;
break;
}
}

Pose2d startPose = new Pose2d(-63.0,50, Math.toRadians(0.0));
drive.setPoseEstimate(startPose);

Expand Down Expand Up @@ -239,7 +225,7 @@ public void runOpMode() throws InterruptedException {

.build();

//WOBBLE C POSITION
//PARK
Trajectory C6 = drive.trajectoryBuilder(C5.end(),true)
.splineToLinearHeading( new Pose2d(15.0,34.0, Math.toRadians(5.0)), Math.toRadians(180.0),
new MinVelocityConstraint(Arrays.asList(
Expand All @@ -249,6 +235,20 @@ public void runOpMode() throws InterruptedException {
), new ProfileAccelerationConstraint(60))
.build();

while (!isStarted()) {
switch (pipeline.getHeight()) {
case ZERO:
state = State.ZERO;
break;
case ONE:
state = State.ONE;
break;
case FOUR:
state = State.FOUR;
break;
}
}

waitForStart();
if (isStopRequested()) return;

Expand Down
35 changes: 17 additions & 18 deletions .../org/firstinspires/ftc/teamcode/drive/opmode/BERSERK/Competition/BLUE_POWERSHOT_AUTO.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,28 +109,10 @@ public void runOpMode() throws InterruptedException {

State state = State.ZERO;

while (!isStarted()) {
switch (pipeline.getHeight()) {
case ZERO:
state = State.ZERO;
break;
case ONE:
state = State.ONE;
break;
case FOUR:
state = State.FOUR;
break;
}
}

waitForStart();

robot.kicker.setPosition(kicker_out);
Pose2d startPose = new Pose2d(-63.0,26, Math.toRadians(0.0));
drive.setPoseEstimate(startPose);

if (isStopRequested()) return;

// A AUTO TRAJECTORIES //
//SHOOT POSITION
Trajectory A1 = drive.trajectoryBuilder(startPose)
Expand Down Expand Up @@ -224,6 +206,23 @@ public void runOpMode() throws InterruptedException {
.splineToSplineHeading(new Pose2d(10.0, 10.0, Math.toRadians(5.0)), Math.toRadians(180.0))
.build();

while (!isStarted()) {
switch (pipeline.getHeight()) {
case ZERO:
state = State.ZERO;
break;
case ONE:
state = State.ONE;
break;
case FOUR:
state = State.FOUR;
break;
}
}

waitForStart();
if (isStopRequested()) return;

// AUTO CASE STATEMENT
switch (state) {
case ZERO:
Expand Down
35 changes: 17 additions & 18 deletions ...g/firstinspires/ftc/teamcode/drive/opmode/BERSERK/Competition/BLUE_POWERSHOT_BB_AUTO.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -99,28 +99,10 @@ public void runOpMode() throws InterruptedException {

State state = State.ZERO;

while (!isStarted()) {
switch (pipeline.getHeight()) {
case ZERO:
state = State.ZERO;
break;
case ONE:
state = State.ONE;
break;
case FOUR:
state = State.FOUR;
break;
}
}

waitForStart();

robot.kicker.setPosition(kicker_out);
Pose2d startPose = new Pose2d(-63.0,26, Math.toRadians(0.0));
drive.setPoseEstimate(startPose);

if (isStopRequested()) return;

// A AUTO TRAJECTORIES //
//SHOOT POSITION
Trajectory A1 = drive.trajectoryBuilder(startPose)
Expand Down Expand Up @@ -238,6 +220,23 @@ public void runOpMode() throws InterruptedException {
.splineToSplineHeading(new Pose2d(10.0, 10.0, Math.toRadians(5.0)), Math.toRadians(180.0))
.build();

while (!isStarted()) {
switch (pipeline.getHeight()) {
case ZERO:
state = State.ZERO;
break;
case ONE:
state = State.ONE;
break;
case FOUR:
state = State.FOUR;
break;
}
}

waitForStart();
if (isStopRequested()) return;

// AUTO CASE STATEMENT
switch (state) {
case ZERO:
Expand Down
73 changes: 40 additions & 33 deletions ...ava/org/firstinspires/ftc/teamcode/drive/opmode/BERSERK/Competition/RED_FULL_AUTO_V3.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,20 +103,6 @@ public void runOpMode() throws InterruptedException {

State state = State.ZERO;

while (!isStarted()) {
switch (pipeline.getHeight()) {
case ZERO:
state = State.ZERO;
break;
case ONE:
state = State.ONE;
break;
case FOUR:
state = State.FOUR;
break;
}
}

Pose2d startPose = new Pose2d(-63.0,-50, Math.toRadians(0.0));
drive.setPoseEstimate(startPose);

Expand Down Expand Up @@ -159,30 +145,31 @@ public void runOpMode() throws InterruptedException {

//WOBBLE A POSITION
Trajectory A4 = drive.trajectoryBuilder(A3.end())
.splineToLinearHeading(new Pose2d(10.0, -46.0, Math.toRadians(180.0)), Math.toRadians(0.0))
.splineToLinearHeading(new Pose2d(10.0, -49.0, Math.toRadians(180.0)), Math.toRadians(0.0))
.build();

//PARK
Trajectory A5 = drive.trajectoryBuilder(A4.end())
.strafeRight(5)
.strafeRight(8)
.build();

// B AUTO TRAJECTORIES //
//SHOOT POSITION
Trajectory B1 = drive.trajectoryBuilder(startPose)
.splineTo(new Vector2d(-25.0, 55.0), Math.toRadians(0.0))
.splineTo(new Vector2d(-25.0, -55.0), Math.toRadians(0.0))
.addTemporalMarker(0.1, () -> {
robot.foldout_lift.setPower(foldout);
})
.addTemporalMarker(1.8, () -> {
robot.foldout_lift.setPower(0);
})
.splineTo(new Vector2d(-5.0, 41.0), Math.toRadians(2.0))
//.splineTo(new Vector2d(-5.0, -41.0), Math.toRadians(2.0))
.splineToSplineHeading(new Pose2d(-5.0, -41.0, Math.toRadians(2.0)), Math.toRadians(90.0))
.build();

//WOBBLE B POSITION
Trajectory B2 = drive.trajectoryBuilder(B1.end())
.splineToLinearHeading(new Pose2d(29.0, 36.0, Math.toRadians(-90.0)), Math.toRadians(0.0))
.splineToLinearHeading(new Pose2d(29.0, -36.0, Math.toRadians(-90.0)), Math.toRadians(0.0))
.build();

//MOVE TOWARDS STACK
Expand All @@ -193,9 +180,9 @@ public void runOpMode() throws InterruptedException {
//PICKUP WOBBLE
Trajectory B4 = drive.trajectoryBuilder(B2.end(),true)
.strafeRight(10)
.splineToSplineHeading(new Pose2d(-19.0, 39.0, Math.toRadians(180.0)), Math.toRadians(180.0))
.splineToSplineHeading( new Pose2d(-35.0, 25.0, Math.toRadians(115.0)), Math.toRadians(180)) //135
.splineToConstantHeading( new Vector2d(-40.0, 24.0), Math.toRadians(100),
.splineToSplineHeading(new Pose2d(-19.0, -39.0, Math.toRadians(180.0)), Math.toRadians(180.0))
.splineToSplineHeading( new Pose2d(-35.0, -25.0, Math.toRadians(115.0)), Math.toRadians(180)) //135
.splineToConstantHeading( new Vector2d(-39.0, -24.0), Math.toRadians(100),
new MinVelocityConstraint(Arrays.asList(
new AngularVelocityConstraint(DriveConstants.MAX_ANG_VEL),
new MecanumVelocityConstraint(8, DriveConstants.TRACK_WIDTH)
Expand All @@ -205,23 +192,23 @@ public void runOpMode() throws InterruptedException {

//SHOOT
Trajectory B5 = drive.trajectoryBuilder(B4.end())
.splineToLinearHeading( new Pose2d(-5.0,39.0, Math.toRadians(2.0)), Math.toRadians(0.0))
.splineToLinearHeading( new Pose2d(-5.0,-39.0, Math.toRadians(2.0)), Math.toRadians(0.0))
.build();

//DROP WOBBLE
Trajectory B6 = drive.trajectoryBuilder(B5.end())
.splineToLinearHeading( new Pose2d(20.0, 40.0, Math.toRadians(-90.0)), Math.toRadians(0.0))
.splineToLinearHeading( new Pose2d(20.0, -40.0, Math.toRadians(-90.0)), Math.toRadians(0.0))
.build();

//PARK
Trajectory B7 = drive.trajectoryBuilder(B6.end())
.splineToLinearHeading( new Pose2d(6.0,34.0, Math.toRadians(0.0)), Math.toRadians(-90.0))
.splineToLinearHeading( new Pose2d(6.0,-45.0, Math.toRadians(0.0)), Math.toRadians(-90.0))
.build();

// C AUTO TRAJECTORIES //
//SHOOT POSITIONadb connect 192.168.43.1
Trajectory C1 = drive.trajectoryBuilder(startPose)
.lineToSplineHeading(new Pose2d(-42,35, Math.toRadians(3.0)))
.lineToSplineHeading(new Pose2d(-42,-39, Math.toRadians(3.0)))
//.strafeTo(new Vector2d(-42.0, 35.0))
.addTemporalMarker(0.1, () -> {
robot.intake.setPower(0.2);
Expand Down Expand Up @@ -251,15 +238,15 @@ public void runOpMode() throws InterruptedException {
Trajectory C5 = drive.trajectoryBuilder(C3.end())

//Approach Zone C
.splineToSplineHeading( new Pose2d(38.0, 48.0, Math.toRadians(0)), Math.toRadians(0),
.splineToSplineHeading( new Pose2d(60.0, -60.0, Math.toRadians(-135)), Math.toRadians(0), //-90
new MinVelocityConstraint(Arrays.asList(
new AngularVelocityConstraint(DriveConstants.MAX_ANG_VEL),
new MecanumVelocityConstraint(70, DriveConstants.TRACK_WIDTH)
)
), new ProfileAccelerationConstraint(60))

//Lower Wobble Arm
.addSpatialMarker(new Vector2d(30, 50), () -> {
.addSpatialMarker(new Vector2d(30, -50), () -> {
robot.wobble_lift.setPosition(wobble_down);
})

Expand All @@ -272,20 +259,26 @@ public void runOpMode() throws InterruptedException {
// ), new ProfileAccelerationConstraint(10))

//Open Wobble Claw
.addSpatialMarker(new Vector2d(55, 48), () -> {
.addSpatialMarker(new Vector2d(52, -60), () -> {
robot.wobble_claw.setPosition(wobble_open);
robot.wobble_lift.setPosition(wobble_up);
})


//Approach Wobble 2
.splineToSplineHeading( new Pose2d(20.0, 12, Math.toRadians(90)), Math.toRadians(180),
.splineToSplineHeading( new Pose2d(20.0, -42, Math.toRadians(90)), Math.toRadians(180),
new MinVelocityConstraint(Arrays.asList(
new AngularVelocityConstraint(DriveConstants.MAX_ANG_VEL),
new MecanumVelocityConstraint(70, DriveConstants.TRACK_WIDTH)
)
), new ProfileAccelerationConstraint(60))

.addDisplacementMarker(() -> {
robot.wobble_lift.setPosition(wobble_down);
})

//Continue to Approach Wobble 2
.splineToSplineHeading( new Pose2d(-35.0, 12, Math.toRadians(90)), Math.toRadians(180),
.splineToSplineHeading( new Pose2d(-35.0, -42, Math.toRadians(90)), Math.toRadians(180),
new MinVelocityConstraint(Arrays.asList(
new AngularVelocityConstraint(DriveConstants.MAX_ANG_VEL),
new MecanumVelocityConstraint(70, DriveConstants.TRACK_WIDTH)
Expand All @@ -303,7 +296,7 @@ public void runOpMode() throws InterruptedException {

//WOBBLE C POSITION
Trajectory C6 = drive.trajectoryBuilder(C5.end())
.splineToSplineHeading( new Pose2d(42.0, 56.0, Math.toRadians(-45.0)), Math.toRadians(0),
.splineToSplineHeading( new Pose2d(48.0, -56.0, Math.toRadians(-135.0)), Math.toRadians(0),
new MinVelocityConstraint(Arrays.asList(
new AngularVelocityConstraint(DriveConstants.MAX_ANG_VEL),
new MecanumVelocityConstraint(70, DriveConstants.TRACK_WIDTH)
Expand All @@ -313,14 +306,28 @@ public void runOpMode() throws InterruptedException {

//PARK
Trajectory C7 = drive.trajectoryBuilder(C6.end(),true)
.splineToLinearHeading( new Pose2d(15.0,34.0, Math.toRadians(5.0)), Math.toRadians(180.0),
.splineToLinearHeading( new Pose2d(15.0,-47.0, Math.toRadians(5.0)), Math.toRadians(180.0),
new MinVelocityConstraint(Arrays.asList(
new AngularVelocityConstraint(DriveConstants.MAX_ANG_VEL),
new MecanumVelocityConstraint(70, DriveConstants.TRACK_WIDTH)
)
), new ProfileAccelerationConstraint(60))
.build();

while (!isStarted()) {
switch (pipeline.getHeight()) {
case ZERO:
state = State.ZERO;
break;
case ONE:
state = State.ONE;
break;
case FOUR:
state = State.FOUR;
break;
}
}

waitForStart();
if (isStopRequested()) return;

Expand Down
Loading

0 comments on commit 52ae555

Please sign in to comment.