Ollie's work committed

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/NetAuto.java

@@ -0,0 +1,64 @@
+package org.firstinspires.ftc.teamcode;
+import com.acmerobotics.roadrunner.Action;
+import com.acmerobotics.roadrunner.SleepAction;
+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.pedroPathing.localization.Pose;
+import org.firstinspires.ftc.teamcode.subsystem.AutoLine1;
+import org.firstinspires.ftc.teamcode.subsystem.AutoLine2;
+import org.firstinspires.ftc.teamcode.subsystem.AutoLine3;
+
+@Autonomous(name = "BlueNetAuto", group = "Dev")
+public class NetAuto extends OpMode {
+
+    public Follower follower;
+
+    public AutoLine1 myFirstPath = new AutoLine1();
+    public AutoLine2 mySecondPath = new AutoLine2();
+    public int pathState = 0;
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+        follower.setMaxPower(0.65);
+        myFirstPath.moveToAutoLine1(follower);
+    }
+
+    @Override
+    public void loop() {
+        follower.update();
+        switch(pathState) {
+            case 0:
+                if (!follower.isBusy()) {
+                    pathState = 1;
+                    mySecondPath.moveToAutoLine2(follower);
+                }
+            case 1:
+                if (!follower.isBusy()) {
+                    System.out.println("Finished");
+                }
+        }
+//        switch(pathState) {
+//            case 0:
+//                if (!follower.isBusy()) {
+//                    mySecondPath.moveToAutoLine2(follower);
+//                    pathState = 1;
+//                }
+//            case 1:
+//                if (!follower.isBusy()) {
+//                    pathState = 2;
+//                }
+//            case 2:
+//                // set path 3
+//                // as if busy, if not, set path 4 and so on.
+//                System.out.print("we're at the end");
+//
+//        }
+
+
+
+        follower.telemetryDebug(telemetry);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java

@@ -38,9 +38,10 @@ public class PedroConstants {
             = RevHubOrientationOnRobot.UsbFacingDirection.UP;
 
     // Robot encoders
-    public static final String LEFT_ENCODER = "encoder-left";
+    // NOTE:  Encoders are plugged into the same ports as motors hence the weird names
-    public static final String RIGHT_ENCODER = "encoder-right";
+    public static final String RIGHT_ENCODER = "back-right"; //0
-    public static final String BACK_ENCODER = "encoder-back";
+    public static final String BACK_ENCODER = "front-right"; //1
+    public static final String LEFT_ENCODER = "front-left"; //2
 
     // Robot encoder direction
     public static final double LEFT_ENCODER_DIRECTION = Encoder.FORWARD;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine1.java

@@ -0,0 +1,44 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+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;
+
+
+
+/*
+    AutoLine# - This file does something of a path......
+
+ */
+public class AutoLine1 {
+
+    private PathChain pathChain;
+    private Pose autoLin1StartPose = new Pose(8,65);
+
+    public void moveToAutoLine1(Follower robot) {
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(8.000, 65.000, Point.CARTESIAN),
+                                new Point(30.000, 72.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0));
+        pathChain = builder.build();
+        robot.setStartingPose(autoLin1StartPose);
+        robot.followPath(pathChain);
+    }
+
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine2.java

@@ -0,0 +1,40 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+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;
+
+
+/*
+    AutoLine# - This file does something of a path......
+
+ */
+public class AutoLine2 {
+    private PathChain pathChain;
+    public void moveToAutoLine2(Follower robot) {
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 2
+                        new BezierCurve(
+                                new Point(36.000, 72.000, Point.CARTESIAN),
+                                new Point(24.000, 24.000, Point.CARTESIAN),
+                                new Point(72.000, 36.000, Point.CARTESIAN),
+                                new Point(56.000, 24.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+}
+

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/AutoLine3.java

@@ -0,0 +1,73 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+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;
+
+
+/*
+    AutoLine# - This file does something of a path......
+
+ */
+public class AutoLine3 implements Action {
+
+    private Follower actionRobot;
+    private PathChain pathChain;
+
+    private Pose startPose = new Pose(56,24);
+
+    public AutoLine3(Follower robot) {
+        this.actionRobot = robot;
+        this.actionRobot.setStartingPose(startPose);
+
+        PathBuilder builder = new PathBuilder();
+        builder
+         /*       .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(8.000, 65.000, Point.CARTESIAN),
+                                new Point(30.000, 72.000, Point.CARTESIAN)
+//           This is the origional                     new Point(36.000, 72.000, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 2
+                        new BezierCurve(
+                                new Point(36.000, 72.000, Point.CARTESIAN),
+                                new Point(24.000, 24.000, Point.CARTESIAN),
+                                new Point(72.000, 36.000, Point.CARTESIAN),
+                                new Point(56.000, 24.000, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180))          */
+                .addPath(
+                   // Line 3
+                   new BezierLine(
+                            new Point(56.000, 24.000, Point.CARTESIAN),
+                             new Point(18.000, 24.000, Point.CARTESIAN)
+                        )
+                )
+                .setTangentHeadingInterpolation();
+
+        pathChain = builder.build();
+
+        this.actionRobot.followPath(this.pathChain);
+    }
+
+    @Override
+    public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+        this.actionRobot.update();
+        return this.actionRobot.isBusy();
+    }
+}
+