diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop.java deleted file mode 100644 index 421c620..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop.java +++ /dev/null @@ -1,195 +0,0 @@ -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 = "Auto Test", group = "Dev") -public class BlueBasketAutoWithDrop extends OpMode { - private Telemetry telemetryA; - - private Follower follower; - - private PathChain path; - - private final Pose startPose = new Pose(8, 65); - - @Override - public void init() { - follower = new Follower(hardwareMap); - - follower.setMaxPower(.75); - - follower.setStartingPose(startPose); - - path = follower.pathBuilder() - .addPath( - // Line 1 - new BezierLine( - new Point(8.000, 65.000, Point.CARTESIAN), - new Point(33.000, 65.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 2 - new BezierLine( - new Point(33.000, 65.000, Point.CARTESIAN), - new Point(31.000, 65.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 3 - new BezierCurve( - new Point(31.000, 65.000, Point.CARTESIAN), - new Point(26.000, 32.000, Point.CARTESIAN), - new Point(60.000, 34.000, Point.CARTESIAN) - ) - ) - .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180)) - .addPath( - // Line 4 - new BezierLine( - new Point(60.000, 34.000, Point.CARTESIAN), - new Point(60.000, 24.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(180)) - .addPath( - // Line 5 - new BezierLine( - new Point(60.000, 24.000, Point.CARTESIAN), - new Point(14.500, 24.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(180)) - .addPath( - // Line 6 - new BezierLine( - new Point(14.500, 24.000, Point.CARTESIAN), - new Point(18.000, 24.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(180)) - .addPath( - // Line 1 - new BezierCurve( - new Point(18.000, 24.000, Point.CARTESIAN), - new Point(18.000, 67.000, Point.CARTESIAN), - new Point(31.000, 67.000, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 8 - new BezierLine( - new Point(31.000, 67.500, Point.CARTESIAN), - new Point(33.000, 67.500, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 9 - new BezierLine( - new Point(33.000, 67.500, Point.CARTESIAN), - new Point(31.000, 67.500, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 10 - new BezierCurve( - new Point(31.000, 67.500, Point.CARTESIAN), - new Point(26.000, 32.000, Point.CARTESIAN), - new Point(60.000, 34.000, Point.CARTESIAN) - ) - ) - .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180)) - .addPath( - // Line 11 - new BezierLine( - new Point(60.000, 34.000, Point.CARTESIAN), - new Point(60.000, 12.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(180)) - .addPath( - // Line 12 - new BezierLine( - new Point(60.000, 12.000, Point.CARTESIAN), - new Point(12.000, 12.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(180)) - .addPath( - // Line 13 - new BezierLine( - new Point(12.000, 12.000, Point.CARTESIAN), - new Point(20.000, 12.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(180)) - .addPath( - // Line 14 - new BezierLine( - new Point(20.000, 12.000, Point.CARTESIAN), - new Point(20.000, 24.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(180)) - .addPath( - // Line 15 - new BezierLine( - new Point(20.000, 24.000, Point.CARTESIAN), - new Point(14.000, 24.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(180)) - .addPath( - // Line 16 - new BezierCurve( - new Point(14.000, 24.000, Point.CARTESIAN), - new Point(14.000, 70.000, Point.CARTESIAN), - new Point(31.000, 70.000, Point.CARTESIAN) - ) - ) - .setLinearHeadingInterpolation(Math.toRadians(180), Math.toRadians(0)) - .addPath( - // Line 17 - new BezierLine( - new Point(31.000, 70.000, Point.CARTESIAN), - new Point(33.000, 70.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 18 - new BezierLine( - new Point(33.000, 70.000, Point.CARTESIAN), - new Point(31.000, 70.000, Point.CARTESIAN) - ) - ) - .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(180)).build(); - follower.followPath(path); - - telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); - telemetryA.update(); - } - - - @Override - public void loop() { - follower.update(); - follower.telemetryDebug(telemetryA); - } -}