From 7f61c1d3f52cf9297d43367674d3cd3d7c88fc87 Mon Sep 17 00:00:00 2001
From: carlos <carlos@riv.as>
Date: Sun, 8 Dec 2024 08:44:15 -0800
Subject: [PATCH] Blue Net Auto files

---
 .../firstinspires/ftc/teamcode/NetAuto.java   | 31 +++++++++++++
 .../ftc/teamcode/subsystem/AutoLine1.java     | 46 +++++++++++++++++--
 2 files changed, 72 insertions(+), 5 deletions(-)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/NetAuto.java

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/NetAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/NetAuto.java
new file mode 100644
index 0000000..ad61f4e
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/NetAuto.java
@@ -0,0 +1,31 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.roadrunner.Action;
+import com.acmerobotics.roadrunner.ftc.Actions;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.subsystem.AutoLine1;
+
+@Autonomous(name = "BlueNetAuto", group = "Dev")
+public class NetAuto extends OpMode {
+
+    public Follower follower;
+
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+    }
+
+    public Action autoLine1() {return new AutoLine1(follower);}
+
+    @Override
+    public void loop() {
+        follower.update();
+        Actions.runBlocking(
+                autoLine1()
+        );
+        follower.telemetryDebug(telemetry);
+    }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine1.java
index c711315..13d22c3 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine1.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine1.java
@@ -7,7 +7,9 @@ import com.acmerobotics.roadrunner.Action;
 
 import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
 import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
 import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
 
@@ -27,12 +29,46 @@ public class AutoLine1 implements Action {
     public AutoLine1(Follower robot) {
         this.actionRobot = robot;
         this.actionRobot.setStartingPose(startPose);
-        this.pathChain = actionRobot.pathBuilder().addPath(
-                new BezierLine(
-                        new Point(8.000, 65.000, Point.CARTESIAN),
-                        new Point(33.000, 65.000, Point.CARTESIAN)
+
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(8.000, 65.000, Point.CARTESIAN),
+                                new Point(36.000, 72.000, Point.CARTESIAN)
+                        )
                 )
-        ).setConstantHeadingInterpolation(Math.toRadians(0)).build();
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 2
+                        new BezierCurve(
+                                new Point(36.000, 72.000, Point.CARTESIAN),
+                                new Point(36.000, 36.000, Point.CARTESIAN),
+                                new Point(48.000, 36.000, Point.CARTESIAN)
+                        )
+                )
+                .setTangentHeadingInterpolation()
+                .addPath(
+                        // Line 3
+                        new BezierCurve(
+                                new Point(48.000, 36.000, Point.CARTESIAN),
+                                new Point(72.000, 30.000, Point.CARTESIAN),
+                                new Point(57.000, 24.000, Point.CARTESIAN)
+                        )
+                )
+                .setTangentHeadingInterpolation()
+                .addPath(
+                        // Line 4
+                        new BezierLine(
+                                new Point(57.000, 24.000, Point.CARTESIAN),
+                                new Point(10.000, 24.000, Point.CARTESIAN)
+                        )
+                )
+                .setTangentHeadingInterpolation();
+
+        pathChain = builder.build();
+
         this.actionRobot.followPath(this.pathChain);
     }