diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedDefaultParkingBehaviorBottom.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedDefaultParkingBehaviorBottom.java index 441e8ea..c607140 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedDefaultParkingBehaviorBottom.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedDefaultParkingBehaviorBottom.java @@ -36,7 +36,7 @@ public class AutoRedDefaultParkingBehaviorBottom extends LinearOpMode { // init move to default terminal Trajectory endpos = makeTrajectories(firstMove.end(), new int[]{-34, -70}, - new int[]{60, -60} + new int[]{-60, -60} ); mecanumDrive.followTrajectory(endpos); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedDefaultParkingBehaviorTop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedDefaultParkingBehaviorTop.java new file mode 100644 index 0000000..9601b97 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedDefaultParkingBehaviorTop.java @@ -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 AutoRedDefaultParkingBehaviorTop 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, -70}, + 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(); + } +}