API episode 3: revenge of the sith
This commit is contained in:
parent
d489794dc7
commit
2dcd1aa28e
|
@ -0,0 +1,40 @@
|
|||
package org.firstinspires.ftc.teamcode.createdcode.enhancedautos;
|
||||
|
||||
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
import com.acmerobotics.roadrunner.geometry.Vector2d;
|
||||
import com.acmerobotics.roadrunner.trajectory.Trajectory;
|
||||
import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||
|
||||
public class API {
|
||||
OpMode opMode;
|
||||
|
||||
public API(OpMode opMode) {
|
||||
this.opMode = opMode;
|
||||
}
|
||||
|
||||
public void pause(double seconds) {
|
||||
double time = opMode.getRuntime() + seconds;
|
||||
while (true) {
|
||||
if (!(opMode.getRuntime() < time)) break; // stop android studio from yelling
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
public Trajectory makeTrajectories(SampleMecanumDrive mecanumDrive, Pose2d startPos, int[] x, int[] y) {
|
||||
assert x.length == y.length;
|
||||
TrajectoryBuilder builder = mecanumDrive.trajectoryBuilder(startPos);
|
||||
|
||||
for (int i = 0; i < x.length; i++) {
|
||||
builder.splineTo(new Vector2d(x[i], y[i]), 0);
|
||||
}
|
||||
|
||||
return builder.build();
|
||||
}
|
||||
|
||||
public int getLargest(int x, int y, int z) {
|
||||
return Math.max(z, Math.max(x, y));
|
||||
}
|
||||
}
|
|
@ -18,11 +18,12 @@ public class AutoBlueColorSensor extends LinearOpMode {
|
|||
public void runOpMode() throws InterruptedException {
|
||||
ColorSensor sensor = hardwareMap.get(ColorSensor.class, "color");
|
||||
mecanumDrive = new SampleMecanumDrive(hardwareMap);
|
||||
|
||||
API api = new API(this);
|
||||
|
||||
waitForStart();
|
||||
|
||||
Trajectory firstMove = makeTrajectories(
|
||||
Trajectory firstMove = api.makeTrajectories(
|
||||
mecanumDrive,
|
||||
new Pose2d(new Vector2d(-34, 60), 0),
|
||||
new int[]{-34},
|
||||
new int[]{40}
|
||||
|
@ -30,39 +31,43 @@ public class AutoBlueColorSensor extends LinearOpMode {
|
|||
|
||||
mecanumDrive.followTrajectory(firstMove);
|
||||
|
||||
double time = getRuntime() + 5;
|
||||
/* wait for 5 seconds to allow color sensor to settle */
|
||||
while (true) {
|
||||
if (!(getRuntime() < time)) break; // needed to stop android studio from yelling
|
||||
}
|
||||
api.pause(5);
|
||||
int r = sensor.red();
|
||||
int g = sensor.green();
|
||||
int b = sensor.blue();
|
||||
|
||||
int largest = getLargest(r, g, b);
|
||||
int largest = api.getLargest(r, g, b);
|
||||
|
||||
Trajectory endpos;
|
||||
if (largest == r) {
|
||||
// move to position 1
|
||||
endpos = makeTrajectories(firstMove.end(),
|
||||
endpos = api.makeTrajectories(
|
||||
mecanumDrive,
|
||||
firstMove.end(),
|
||||
new int[]{-34, -12, -12},
|
||||
new int[]{60, 60, 12}
|
||||
);
|
||||
} else if (largest == g) {
|
||||
// move to position 2
|
||||
endpos = makeTrajectories(firstMove.end(),
|
||||
endpos = api.makeTrajectories(
|
||||
mecanumDrive,
|
||||
firstMove.end(),
|
||||
new int[]{-34, -12, -12, -34},
|
||||
new int[]{60, 60, 12, 12}
|
||||
);
|
||||
} else if (largest == b) {
|
||||
// move to position 3
|
||||
endpos = makeTrajectories(firstMove.end(),
|
||||
endpos = api.makeTrajectories(
|
||||
mecanumDrive,
|
||||
firstMove.end(),
|
||||
new int[]{-34, -70, -60},
|
||||
new int[]{60, 60, 30}
|
||||
);
|
||||
} else {
|
||||
// Color sensor broke, move to backup position
|
||||
endpos = makeTrajectories(firstMove.end(),
|
||||
endpos = api.makeTrajectories(
|
||||
mecanumDrive,
|
||||
firstMove.end(),
|
||||
new int[]{-34, -70},
|
||||
new int[]{60, 60}
|
||||
);
|
||||
|
@ -70,19 +75,4 @@ public class AutoBlueColorSensor extends LinearOpMode {
|
|||
|
||||
mecanumDrive.followTrajectory(endpos);
|
||||
}
|
||||
|
||||
private Trajectory makeTrajectories(Pose2d startPos, int[] x, int[] y) {
|
||||
assert x.length == y.length;
|
||||
TrajectoryBuilder builder = mecanumDrive.trajectoryBuilder(startPos);
|
||||
|
||||
for (int i = 0; i < x.length; i++) {
|
||||
builder.splineTo(new Vector2d(x[i], y[i]), 0);
|
||||
}
|
||||
|
||||
return builder.build();
|
||||
}
|
||||
|
||||
private int getLargest(int x, int y, int z) {
|
||||
return Math.max(z, (Math.max(x, y)));
|
||||
}
|
||||
}
|
||||
|
|
|
@ -16,12 +16,14 @@ public class AutoBlueSubstationParkingBehavior extends LinearOpMode {
|
|||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
mecanumDrive = new SampleMecanumDrive(hardwareMap);
|
||||
API api = new API(this);
|
||||
|
||||
|
||||
waitForStart();
|
||||
|
||||
// init move to default terminal
|
||||
Trajectory endpos = makeTrajectories(
|
||||
Trajectory endpos = api.makeTrajectories(
|
||||
mecanumDrive,
|
||||
new Pose2d(new Vector2d(-34, 60), 0),
|
||||
new int[]{-34, 1},
|
||||
new int[]{60, 60}
|
||||
|
@ -29,15 +31,4 @@ public class AutoBlueSubstationParkingBehavior extends LinearOpMode {
|
|||
|
||||
mecanumDrive.followTrajectory(endpos);
|
||||
}
|
||||
|
||||
private Trajectory makeTrajectories(Pose2d startPos, int[] x, int[] y) {
|
||||
assert x.length == y.length;
|
||||
TrajectoryBuilder builder = mecanumDrive.trajectoryBuilder(startPos);
|
||||
|
||||
for (int i = 0; i < x.length; i++) {
|
||||
builder.splineTo(new Vector2d(x[i], y[i]), 0);
|
||||
}
|
||||
|
||||
return builder.build();
|
||||
}
|
||||
}
|
||||
|
|
|
@ -16,12 +16,14 @@ public class AutoBlueTerminalParkingBehavior extends LinearOpMode {
|
|||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
mecanumDrive = new SampleMecanumDrive(hardwareMap);
|
||||
API api = new API(this);
|
||||
|
||||
|
||||
waitForStart();
|
||||
|
||||
// init move to default terminal
|
||||
Trajectory endpos = makeTrajectories(
|
||||
Trajectory endpos = api.makeTrajectories(
|
||||
mecanumDrive,
|
||||
new Pose2d(new Vector2d(-34, 60), 0),
|
||||
new int[]{-34, -70},
|
||||
new int[]{60, 60}
|
||||
|
@ -29,15 +31,4 @@ public class AutoBlueTerminalParkingBehavior extends LinearOpMode {
|
|||
|
||||
mecanumDrive.followTrajectory(endpos);
|
||||
}
|
||||
|
||||
private Trajectory makeTrajectories(Pose2d startPos, int[] x, int[] y) {
|
||||
assert x.length == y.length;
|
||||
TrajectoryBuilder builder = mecanumDrive.trajectoryBuilder(startPos);
|
||||
|
||||
for (int i = 0; i < x.length; i++) {
|
||||
builder.splineTo(new Vector2d(x[i], y[i]), 0);
|
||||
}
|
||||
|
||||
return builder.build();
|
||||
}
|
||||
}
|
||||
|
|
|
@ -18,11 +18,13 @@ public class AutoRedColorSensor extends LinearOpMode {
|
|||
public void runOpMode() throws InterruptedException {
|
||||
ColorSensor sensor = hardwareMap.get(ColorSensor.class, "color");
|
||||
mecanumDrive = new SampleMecanumDrive(hardwareMap);
|
||||
API api = new API(this);
|
||||
|
||||
|
||||
waitForStart();
|
||||
|
||||
Trajectory firstMove = makeTrajectories(
|
||||
Trajectory firstMove = api.makeTrajectories(
|
||||
mecanumDrive,
|
||||
new Pose2d(new Vector2d(-34, -60), 0),
|
||||
new int[]{-34},
|
||||
new int[]{-40}
|
||||
|
@ -30,39 +32,39 @@ public class AutoRedColorSensor extends LinearOpMode {
|
|||
|
||||
mecanumDrive.followTrajectory(firstMove);
|
||||
|
||||
double time = getRuntime() + 5;
|
||||
/* wait for 5 seconds to allow color sensor to settle */
|
||||
while (true) {
|
||||
if (!(getRuntime() < time)) break; // needed to stop android studio from yelling
|
||||
}
|
||||
api.pause(5);
|
||||
int r = sensor.red();
|
||||
int g = sensor.green();
|
||||
int b = sensor.blue();
|
||||
|
||||
int largest = getLargest(r, g, b);
|
||||
int largest = api.getLargest(r, g, b);
|
||||
|
||||
Trajectory endpos;
|
||||
if (largest == r) {
|
||||
// move to position 1
|
||||
endpos = makeTrajectories(firstMove.end(),
|
||||
endpos = api.makeTrajectories(
|
||||
mecanumDrive,
|
||||
firstMove.end(),
|
||||
new int[]{-34, -12, -12},
|
||||
new int[]{-60, -60, -12}
|
||||
);
|
||||
} else if (largest == g) {
|
||||
// move to position 2
|
||||
endpos = makeTrajectories(firstMove.end(),
|
||||
endpos = api.makeTrajectories(
|
||||
mecanumDrive,
|
||||
firstMove.end(),
|
||||
new int[]{-34, -12, -12, -34},
|
||||
new int[]{-60, -60, -12, -12}
|
||||
);
|
||||
} else if (largest == b) {
|
||||
// move to position 3
|
||||
endpos = makeTrajectories(firstMove.end(),
|
||||
endpos = api.makeTrajectories(mecanumDrive, firstMove.end(),
|
||||
new int[]{-34, -70, -60},
|
||||
new int[]{-60, -60, -30}
|
||||
);
|
||||
} else {
|
||||
// Color sensor broke, move to backup position
|
||||
endpos = makeTrajectories(firstMove.end(),
|
||||
endpos = api.makeTrajectories(mecanumDrive, firstMove.end(),
|
||||
new int[]{-34, -70},
|
||||
new int[]{-60, -60}
|
||||
);
|
||||
|
@ -70,19 +72,4 @@ public class AutoRedColorSensor extends LinearOpMode {
|
|||
|
||||
mecanumDrive.followTrajectory(endpos);
|
||||
}
|
||||
|
||||
private Trajectory makeTrajectories(Pose2d startPos, int[] x, int[] y) {
|
||||
assert x.length == y.length;
|
||||
TrajectoryBuilder builder = mecanumDrive.trajectoryBuilder(startPos);
|
||||
|
||||
for (int i = 0; i < x.length; i++) {
|
||||
builder.splineTo(new Vector2d(x[i], y[i]), 0);
|
||||
}
|
||||
|
||||
return builder.build();
|
||||
}
|
||||
|
||||
private int getLargest(int x, int y, int z) {
|
||||
return Math.max(z, (Math.max(x, y)));
|
||||
}
|
||||
}
|
||||
|
|
|
@ -16,12 +16,14 @@ public class AutoRedSubstationParkingBehavior extends LinearOpMode {
|
|||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
mecanumDrive = new SampleMecanumDrive(hardwareMap);
|
||||
API api = new API(this);
|
||||
|
||||
|
||||
waitForStart();
|
||||
|
||||
// init move to default terminal
|
||||
Trajectory endpos = makeTrajectories(
|
||||
Trajectory endpos = api.makeTrajectories(
|
||||
mecanumDrive,
|
||||
new Pose2d(new Vector2d(-34, -60), 0),
|
||||
new int[]{-34, 1},
|
||||
new int[]{-60, -60}
|
||||
|
@ -29,15 +31,4 @@ public class AutoRedSubstationParkingBehavior extends LinearOpMode {
|
|||
|
||||
mecanumDrive.followTrajectory(endpos);
|
||||
}
|
||||
|
||||
private Trajectory makeTrajectories(Pose2d startPos, int[] x, int[] y) {
|
||||
assert x.length == y.length;
|
||||
TrajectoryBuilder builder = mecanumDrive.trajectoryBuilder(startPos);
|
||||
|
||||
for (int i = 0; i < x.length; i++) {
|
||||
builder.splineTo(new Vector2d(x[i], y[i]), 0);
|
||||
}
|
||||
|
||||
return builder.build();
|
||||
}
|
||||
}
|
||||
|
|
|
@ -16,12 +16,13 @@ public class AutoRedTerminalParkingBehavior extends LinearOpMode {
|
|||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
mecanumDrive = new SampleMecanumDrive(hardwareMap);
|
||||
|
||||
API api = new API(this);
|
||||
|
||||
waitForStart();
|
||||
|
||||
// init move to default terminal
|
||||
Trajectory endpos = makeTrajectories(
|
||||
Trajectory endpos = api.makeTrajectories(
|
||||
mecanumDrive,
|
||||
new Pose2d(new Vector2d(-34, -60), 0),
|
||||
new int[]{-34, -70},
|
||||
new int[]{-60, -60}
|
||||
|
@ -30,14 +31,4 @@ public class AutoRedTerminalParkingBehavior extends LinearOpMode {
|
|||
mecanumDrive.followTrajectory(endpos);
|
||||
}
|
||||
|
||||
private Trajectory makeTrajectories(Pose2d startPos, int[] x, int[] y) {
|
||||
assert x.length == y.length;
|
||||
TrajectoryBuilder builder = mecanumDrive.trajectoryBuilder(startPos);
|
||||
|
||||
for (int i = 0; i < x.length; i++) {
|
||||
builder.splineTo(new Vector2d(x[i], y[i]), 0);
|
||||
}
|
||||
|
||||
return builder.build();
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in a new issue