From fe39ffda11d0285ae5404d9e4dfea4003ece0abb Mon Sep 17 00:00:00 2001 From: robotics1 Date: Wed, 11 Dec 2024 10:41:04 -0800 Subject: [PATCH] Auto code --- .../ftc/teamcode/BlueBasketAutoWithDrop1.java | 56 ++++++++++++++++++ .../ftc/teamcode/BlueBasketAutoWithDrop2.java | 57 ++++++++++++++++++ .../ftc/teamcode/BlueBasketAutoWithDrop3.java | 57 ++++++++++++++++++ .../ftc/teamcode/BlueBasketAutoWithDrop4.java | 57 ++++++++++++++++++ .../ftc/teamcode/BlueBasketAutoWithDrop5.java | 58 +++++++++++++++++++ .../ftc/teamcode/BlueBasketAutoWithDrop6.java | 56 ++++++++++++++++++ .../pathGeneration/PathBuilder.java | 1 + 7 files changed, 342 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop1.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop2.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop3.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop4.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop5.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop6.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop1.java new file mode 100644 index 0000000..40dd318 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop1.java @@ -0,0 +1,56 @@ +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.PathChain; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; + +@Autonomous(name = "Auto Test", group = "Dev") +public class BlueBasketAutoWithDrop1 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 BezierCurve( + new Point(8.000, 89.000, Point.CARTESIAN), + new Point(24.000, 96.000, Point.CARTESIAN), + new Point(16.000, 128.000, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)).build(); + + follower.followPath(path); + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.update(); + } + + + @Override + public void loop() { + follower.update(); + follower.telemetryDebug(telemetryA); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop2.java new file mode 100644 index 0000000..bb35afa --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop2.java @@ -0,0 +1,57 @@ +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.PathChain; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; + +@Autonomous(name = "Auto Test", group = "Dev") +public class BlueBasketAutoWithDrop2 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 2 + new BezierCurve( + new Point(16.000, 128.000, Point.CARTESIAN), + new Point(16.000, 120.000, Point.CARTESIAN), + new Point(32.000, 123.000, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0)).build(); + + follower.followPath(path); + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.update(); + } + + + @Override + public void loop() { + follower.update(); + follower.telemetryDebug(telemetryA); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop3.java new file mode 100644 index 0000000..04ad369 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop3.java @@ -0,0 +1,57 @@ +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.PathChain; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; + +@Autonomous(name = "Auto Test", group = "Dev") +public class BlueBasketAutoWithDrop3 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() + .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0)) + .addPath( + // Line 3 + new BezierCurve( + new Point(32.000, 123.000, Point.CARTESIAN), + new Point(16.000, 120.000, Point.CARTESIAN), + new Point(16.000, 128.000, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)).build(); + + follower.followPath(path); + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.update(); + } + + + @Override + public void loop() { + follower.update(); + follower.telemetryDebug(telemetryA); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop4.java new file mode 100644 index 0000000..a68d354 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop4.java @@ -0,0 +1,57 @@ +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 BlueBasketAutoWithDrop4 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() + .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0)) + .addPath( + // Line 4 + new BezierLine( + new Point(16.000, 128.000, Point.CARTESIAN), + new Point(32.000, 133.500, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0)).build(); + + follower.followPath(path); + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.update(); + } + + + @Override + public void loop() { + follower.update(); + follower.telemetryDebug(telemetryA); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop5.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop5.java new file mode 100644 index 0000000..f8edce6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop5.java @@ -0,0 +1,58 @@ +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 BlueBasketAutoWithDrop5 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() + .setLinearHeadingInterpolation(Math.toRadians(135), Math.toRadians(0)) + .addPath( + // Line 5 + new BezierLine( + new Point(32.000, 133.500, Point.CARTESIAN), + new Point(16.000, 128.000, Point.CARTESIAN) + ) + ) + .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(135)).build(); + + + follower.followPath(path); + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.update(); + } + + + @Override + public void loop() { + follower.update(); + follower.telemetryDebug(telemetryA); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop6.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop6.java new file mode 100644 index 0000000..90bc3c3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBasketAutoWithDrop6.java @@ -0,0 +1,56 @@ +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.PathChain; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; + +@Autonomous(name = "Auto Test", group = "Dev") +public class BlueBasketAutoWithDrop6 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 6 + new BezierCurve( + new Point(16.000, 128.000, Point.CARTESIAN), + new Point(85.000, 132.750, Point.CARTESIAN), + new Point(84.000, 97.000, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation.build(); + + follower.followPath(path); + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.update(); + } + + + @Override + public void loop() { + follower.update(); + follower.telemetryDebug(telemetryA); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java index 5bfff7d..3585b83 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/PathBuilder.java @@ -14,6 +14,7 @@ import java.util.ArrayList; * @version 1.0, 3/11/2024 */ public class PathBuilder { + public PathBuilder setTangentHeadingInterpolation; private ArrayList paths = new ArrayList<>(); private ArrayList callbacks = new ArrayList<>();