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/PreLoadedBlueBasketAuto.java

@@ -0,0 +1,122 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+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.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+@Autonomous(name = "Pre Loaded Blue Basket Auto", group = "Competition")
+public class PreLoadedBlueBasketAuto extends OpMode {
+    private Telemetry telemetryA;
+
+    private Follower follower;
+
+    private PathChain path;
+
+    private final Pose startPose = new Pose(7.875, 89.357);
+
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+
+        follower.setMaxPower(.45);
+
+        follower.setStartingPose(startPose);
+
+        path = follower.pathBuilder()
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(8.036, 89.196, Point.CARTESIAN),
+                                new Point(10.125, 126.804, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 2
+                        new BezierCurve(
+                                new Point(10.125, 126.804, Point.CARTESIAN),
+                                new Point(37.607, 90.000, Point.CARTESIAN),
+                                new Point(62.357, 119.893, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 3
+                        new BezierCurve(
+                                new Point(62.357, 119.893, Point.CARTESIAN),
+                                new Point(33.750, 112.500, Point.CARTESIAN),
+                                new Point(15.107, 130.661, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 4
+                        new BezierCurve(
+                                new Point(15.107, 130.661, Point.CARTESIAN),
+                                new Point(58.821, 103.018, Point.CARTESIAN),
+                                new Point(59.625, 126.964, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 5
+                        new BezierLine(
+                                new Point(59.625, 126.964, Point.CARTESIAN),
+                                new Point(15.107, 130.339, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 6
+                        new BezierLine(
+                                new Point(15.107, 130.339, Point.CARTESIAN),
+                                new Point(59.625, 126.964, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 7
+                        new BezierLine(
+                                new Point(59.625, 126.964, Point.CARTESIAN),
+                                new Point(57.857, 133.071, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 8
+                        new BezierLine(
+                                new Point(57.857, 133.071, Point.CARTESIAN),
+                                new Point(18.964, 134.679, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 9
+                        new BezierCurve(
+                                new Point(18.964, 134.679, Point.CARTESIAN),
+                                new Point(84.536, 131.786, Point.CARTESIAN),
+                                new Point(80.036, 96.429, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build();
+        follower.followPath(path);
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.update();
+    }
+
+    @Override
+    public void loop() {
+        follower.update();
+        follower.telemetryDebug(telemetryA);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BlueNonBasketAuto.java

@@ -0,0 +1,151 @@
+package org.firstinspires.ftc.teamcode.cometbots;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+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.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+/**
+ * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
+ * a circle, but some Bezier curves that have control points set essentially in a square. However,
+ * it turns enough to tune your centripetal force correction and some of your heading. Some lag in
+ * heading is to be expected.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @version 1.0, 3/12/2024
+ */
+@Config
+@Autonomous(name = "BlueNonBasketAuto", group = "Autonomous Pathing Tuning")
+public class BlueNonBasketAuto extends OpMode {
+    private Telemetry telemetryA;
+
+    private Follower follower;
+
+    private PathChain path;
+
+    private final Pose startPose = new Pose(10.929, 55.446, 0);
+
+    /**
+     * This initializes the Follower and creates the PathChain for the "circle". Additionally, this
+     * initializes the FTC Dashboard telemetry.
+     */
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+
+        follower.setMaxPower(.45);
+
+        follower.setStartingPose(startPose);
+
+        path = follower.pathBuilder()
+                .addPath(
+                // Line 1
+                new BezierCurve(
+                        new Point(10.929, 55.446, Point.CARTESIAN),
+                        new Point(42.429, 46.446, Point.CARTESIAN),
+                        new Point(36.321, 38.089, Point.CARTESIAN)
+                )
+        )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 2
+                        new BezierLine(
+                                new Point(36.321, 38.089, Point.CARTESIAN),
+                                new Point(59.786, 36.643, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 3
+                        new BezierLine(
+                                new Point(59.786, 36.643, Point.CARTESIAN),
+                                new Point(59.304, 24.750, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 4
+                        new BezierLine(
+                                new Point(59.304, 24.750, Point.CARTESIAN),
+                                new Point(13.982, 23.946, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 5
+                        new BezierLine(
+                                new Point(13.982, 23.946, Point.CARTESIAN),
+                                new Point(59.464, 24.429, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 6
+                        new BezierLine(
+                                new Point(59.464, 24.429, Point.CARTESIAN),
+                                new Point(58.982, 15.268, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 7
+                        new BezierLine(
+                                new Point(58.982, 15.268, Point.CARTESIAN),
+                                new Point(13.821, 14.464, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 8
+                        new BezierLine(
+                                new Point(13.821, 14.464, Point.CARTESIAN),
+                                new Point(58.661, 13.500, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 9
+                        new BezierLine(
+                                new Point(58.661, 13.500, Point.CARTESIAN),
+                                new Point(58.339, 8.679, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0))
+                .addPath(
+                        // Line 10
+                        new BezierLine(
+                                new Point(58.339, 8.679, Point.CARTESIAN),
+                                new Point(14.625, 8.518, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(0)).build();
+        follower.followPath(path, true);
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.update();
+    }
+
+    /**
+     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
+     * the Telemetry, as well as the FTC Dashboard.
+     */
+    @Override
+    public void loop() {
+        follower.update();
+        if (follower.atParametricEnd()) {
+            follower.followPath(path, true);
+        }
+        follower.telemetryDebug(telemetryA);
+    }
+}

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();
+    }
+}
+