created red and blue substation parking behavior
This commit is contained in:
Khosraw Azizi
parent
fa616ead11
commit
8336705662
|
|
@ -0,0 +1,55 @@
|
|||
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.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||
|
||||
@Autonomous
|
||||
public class AutoBlueSubstationParkingBehaviorBottom extends LinearOpMode {
|
||||
SampleMecanumDrive mecanumDrive;
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
mecanumDrive = new SampleMecanumDrive(hardwareMap);
|
||||
|
||||
|
||||
waitForStart();
|
||||
|
||||
Trajectory firstMove = makeTrajectories(
|
||||
new Pose2d(new Vector2d(-34, 60), 0),
|
||||
new int[]{-34},
|
||||
new int[]{40}
|
||||
);
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
// init move to default terminal
|
||||
Trajectory endpos = makeTrajectories(firstMove.end(),
|
||||
new int[]{-34, 1},
|
||||
new int[]{60, 60}
|
||||
);
|
||||
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,55 @@
|
|||
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.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||
|
||||
@Autonomous
|
||||
public class AutoBlueSubstationParkingBehaviorTop extends LinearOpMode {
|
||||
SampleMecanumDrive mecanumDrive;
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
mecanumDrive = new SampleMecanumDrive(hardwareMap);
|
||||
|
||||
|
||||
waitForStart();
|
||||
|
||||
Trajectory firstMove = makeTrajectories(
|
||||
new Pose2d(new Vector2d(36, 60), 0),
|
||||
new int[]{36},
|
||||
new int[]{40}
|
||||
);
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
// init move to default terminal
|
||||
Trajectory endpos = makeTrajectories(firstMove.end(),
|
||||
new int[]{36, 1},
|
||||
new int[]{60, 60}
|
||||
);
|
||||
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,55 @@
|
|||
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.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||
|
||||
@Autonomous
|
||||
public class AutoRedSubstationParkingBehaviorBottom extends LinearOpMode {
|
||||
SampleMecanumDrive mecanumDrive;
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
mecanumDrive = new SampleMecanumDrive(hardwareMap);
|
||||
|
||||
|
||||
waitForStart();
|
||||
|
||||
Trajectory firstMove = makeTrajectories(
|
||||
new Pose2d(new Vector2d(-34, -60), 0),
|
||||
new int[]{-34},
|
||||
new int[]{-40}
|
||||
);
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
// init move to default terminal
|
||||
Trajectory endpos = makeTrajectories(firstMove.end(),
|
||||
new int[]{-34, 1},
|
||||
new int[]{-60, -60}
|
||||
);
|
||||
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,55 @@
|
|||
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.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||
|
||||
@Autonomous
|
||||
public class AutoRedSubstationParkingBehaviorTop extends LinearOpMode {
|
||||
SampleMecanumDrive mecanumDrive;
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
mecanumDrive = new SampleMecanumDrive(hardwareMap);
|
||||
|
||||
|
||||
waitForStart();
|
||||
|
||||
Trajectory firstMove = makeTrajectories(
|
||||
new Pose2d(new Vector2d(36, -60), 0),
|
||||
new int[]{36},
|
||||
new int[]{-40}
|
||||
);
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
// init move to default terminal
|
||||
Trajectory endpos = makeTrajectories(firstMove.end(),
|
||||
new int[]{36, 1},
|
||||
new int[]{-60, -60}
|
||||
);
|
||||
|
||||
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