From bb8a4d5bc11aa18d3c66dac7af6d7826f640b474 Mon Sep 17 00:00:00 2001
From: Khosraw Azizi <khosraw.azizi@gmail.com>
Date: Sat, 5 Nov 2022 15:18:56 -0500
Subject: [PATCH] added blue default parking behavior

---
 .../AutoBlueDefaultParkingBehaviorBottom.java | 55 +++++++++++++++++++
 .../AutoBlueDefaultParkingBehaviorTop.java    | 55 +++++++++++++++++++
 2 files changed, 110 insertions(+)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueDefaultParkingBehaviorBottom.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueDefaultParkingBehaviorTop.java

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueDefaultParkingBehaviorBottom.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueDefaultParkingBehaviorBottom.java
new file mode 100644
index 0000000..ad8a193
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueDefaultParkingBehaviorBottom.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 AutoBlueDefaultParkingBehaviorBottom 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, -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();
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueDefaultParkingBehaviorTop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueDefaultParkingBehaviorTop.java
new file mode 100644
index 0000000..603dbde
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueDefaultParkingBehaviorTop.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 AutoBlueDefaultParkingBehaviorTop 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();
+    }
+}