diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AsherOrientBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AsherOrientBlue.java deleted file mode 100644 index 4ff899a..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AsherOrientBlue.java +++ /dev/null @@ -1,95 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots.poc; - -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.Disabled; -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 -@Disabled -@Autonomous(name = "AsherOrientBlue", group = "Autonomous Pathing Tuning") -public class AsherOrientBlue extends OpMode { - private Telemetry telemetryA; - - private Follower follower; - - private PathChain path; - - private final Pose startPose = new Pose(9.757, 84.983, 90); - - /** - * 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(.4); - - follower.setStartingPose(startPose); - - path = follower.pathBuilder() - /* - * Only update this path - */ - .addPath( - // Line 1 - new BezierLine( - new Point(20.500, 7.800, Point.CARTESIAN), - new Point(20.500, 87.500, Point.CARTESIAN) - ) - ) - .addPath( - // Line 2 - new BezierLine( - new Point(20.500, 87.500, Point.CARTESIAN), - new Point(7.800, 87.500, Point.CARTESIAN) - ) - ) - - .setConstantHeadingInterpolation(Math.toRadians(90)).build(); - /* - * End of only update this path - */ - - follower.followPath(path); - - 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); - } - follower.telemetryDebug(telemetryA); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AsherPathBlueV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AsherPathBlueV1.java deleted file mode 100644 index d95f40e..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AsherPathBlueV1.java +++ /dev/null @@ -1,135 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots.poc; - -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.Disabled; -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 -@Disabled -@Autonomous(name = "AsherPathBlueV1", group = "Autonomous Pathing Tuning") -public class AsherPathBlueV1 extends OpMode { - private Telemetry telemetryA; - - private Follower follower; - - private PathChain path; - - private final Pose startPose = new Pose(9.757, 84.983, 90); - - /** - * 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(.4); - - follower.setStartingPose(startPose); - - path = follower.pathBuilder() - /* - * Only update this path - */ - .addPath( - // Line 1 - new BezierCurve( - new Point(7.800, 87.5, Point.CARTESIAN), - new Point(19.000, 116.000, Point.CARTESIAN), - new Point(93.000, 118.000, Point.CARTESIAN), - new Point(45.000, 115.000, Point.CARTESIAN) - ) - ) - .addPath( - // Line 2 - new BezierLine( - new Point(45.000, 115.000, Point.CARTESIAN), - new Point(14.000, 126.000, Point.CARTESIAN) - ) - ) - .addPath( - // Line 3 - new BezierCurve( - new Point(14.000, 126.000, Point.CARTESIAN), - new Point(43.000, 112.500, Point.CARTESIAN), - new Point(64.000, 92.000, Point.CARTESIAN), - new Point(77.000, 117.000, Point.CARTESIAN) - ) - ) - .addPath( - // Line 4 - new BezierLine( - new Point(77.000, 117.000, Point.CARTESIAN), - new Point(20.000, 135.000, Point.CARTESIAN) - ) - ) - .addPath( - // Line 5 - new BezierCurve( - new Point(20.000, 135.000, Point.CARTESIAN), - new Point(113.000, 95.000, Point.CARTESIAN), - new Point(69.000, 135.000, Point.CARTESIAN) - ) - ) - .addPath( - // Line 6 - new BezierLine( - new Point(69.000, 135.000, Point.CARTESIAN), - new Point(20.500, 135.000, Point.CARTESIAN) - ) - ) - .addPath( - // Line 7 - new BezierCurve( - new Point(20.500, 135.000, Point.CARTESIAN), - new Point(101.500, 95.500, Point.CARTESIAN), - new Point(72.500, 95.500, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)).build(); - /* - * End of only update this path - */ - - follower.followPath(path); - - 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); - } - follower.telemetryDebug(telemetryA); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AsherPathV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AsherPathV1.java deleted file mode 100644 index b13d9b5..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AsherPathV1.java +++ /dev/null @@ -1,137 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots.poc; - -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.Disabled; -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; -import com.acmerobotics.roadrunner.SleepAction; -/** - * 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 -@Disabled -@Autonomous(name = "AsherPathV1", group = "Autonomous Pathing Tuning") -public class AsherPathV1 extends OpMode { - private Telemetry telemetryA; - - private Follower follower; - - private PathChain path; - - private final Pose startPose = new Pose(10.0, 40, 90); - - /** - * 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(.4); - - follower.setStartingPose(startPose); - - path = follower.pathBuilder() - /* - * Only update this path - */ - .addPath( - // Line 1 - new BezierCurve( - new Point(9.757, 84.983, Point.CARTESIAN), - new Point(33.000, 105.000, Point.CARTESIAN), - new Point(80.000, 118.000, Point.CARTESIAN), - new Point(55.000, 120.000, Point.CARTESIAN) - ) - ) - .addPath( - // Line 2 - new BezierCurve( - new Point(55.000, 120.000, Point.CARTESIAN), - new Point(22.000, 106.000, Point.CARTESIAN), - new Point(11.000, 131.000, Point.CARTESIAN) - ) - ) - .addPath( - // Line 3 - new BezierCurve( - new Point(11.000, 131.000, Point.CARTESIAN), - new Point(75.000, 95.000, Point.CARTESIAN), - new Point(112.000, 132.000, Point.CARTESIAN), - new Point(61.000, 131.000, Point.CARTESIAN) - ) - ) - .addPath( - // Line 4 - new BezierLine( - new Point(61.000, 131.000, Point.CARTESIAN), - new Point(11.000, 131.000, Point.CARTESIAN) - ) - ) - .addPath( - // Line 5 - new BezierCurve( - new Point(11.000, 131.000, Point.CARTESIAN), - new Point(100.000, 118.000, Point.CARTESIAN), - new Point(103.000, 135.000, Point.CARTESIAN), - new Point(61.000, 135.000, Point.CARTESIAN) - ) - ) - .addPath( - // Line 6 - new BezierLine( - new Point(61.000, 135.000, Point.CARTESIAN), - new Point(11.000, 131.000, Point.CARTESIAN) - ) - ) - .addPath( - // Line 7 - new BezierCurve( - new Point(11.000, 131.000, Point.CARTESIAN), - new Point(113.000, 95.000, Point.CARTESIAN), - new Point(67.000, 95.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)).build(); - /* - * End of only update this path - */ - - follower.followPath(path); - - 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); - } - follower.telemetryDebug(telemetryA); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExample.java deleted file mode 100644 index ec47d35..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExample.java +++ /dev/null @@ -1,81 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots.poc; - -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.Disabled; -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.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 -@Disabled -@Autonomous(name = "AutoExample - Straight Path", group = "Autonomous Pathing Tuning") -public class AutoExample extends OpMode { - private Telemetry telemetryA; - - private Follower follower; - - private PathChain path; - - private final Pose startPose = new Pose(0.0, 20.0, 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(.6); - - follower.setStartingPose(startPose); - - path = follower.pathBuilder() - .addPath( - // Line 1 - new BezierLine( - new Point(0.000, 20.000, Point.CARTESIAN), - new Point(50.000, 20.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .build(); - - follower.followPath(path); - - 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); - } - follower.telemetryDebug(telemetryA); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExampleFour.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExampleFour.java deleted file mode 100644 index 71501db..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExampleFour.java +++ /dev/null @@ -1,108 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots.poc; - -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.Disabled; -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 -@Disabled -@Autonomous(name = "AutoExample - 2 Curves/2 Lines", group = "Autonomous Pathing Tuning") -public class AutoExampleFour extends OpMode { - private Telemetry telemetryA; - - private Follower follower; - - private PathChain path; - - private final Pose startPose = new Pose(12,60, 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(12.000, 60.000, Point.CARTESIAN), - new Point(60.000, 60.000, Point.CARTESIAN), - new Point(60.000, 12.000, Point.CARTESIAN) - ) - ) - .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-90)) - .addPath( - // Line 2 - new BezierLine( - new Point(60.000, 12.000, Point.CARTESIAN), - new Point(40.000, 12.000, Point.CARTESIAN) - ) - ) - .setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(-90)) - .addPath( - // Line 3 - new BezierCurve( - new Point(40.000, 12.000, Point.CARTESIAN), - new Point(35.000, 35.000, Point.CARTESIAN), - new Point(12.000, 35.000, Point.CARTESIAN) - ) - ) - .setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(-90)) - .addPath( - // Line 4 - new BezierLine( - new Point(12.000, 35.000, Point.CARTESIAN), - new Point(12.000, 60.000, Point.CARTESIAN) - ) - ) - .setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(0)) - .build(); - - follower.followPath(path); - - 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); - } - follower.telemetryDebug(telemetryA); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExampleSeason2025V1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExampleSeason2025V1.java deleted file mode 100644 index 51096ea..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExampleSeason2025V1.java +++ /dev/null @@ -1,144 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots.poc; - -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.Disabled; -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.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 -@Disabled -@Autonomous(name = "AutoExampleSeason2025V1", group = "Autonomous Pathing Tuning") -public class AutoExampleSeason2025V1 extends OpMode { - private Telemetry telemetryA; - - private Follower follower; - - private PathChain path; - - private final Pose startPose = new Pose(15.0, 35, 90); - - /** - * 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(.375); - - follower.setStartingPose(startPose); - - path = follower.pathBuilder() - .addPath( - // Line 1 - new BezierLine( - new Point(15.000, 35.000, Point.CARTESIAN), - new Point(60.000, 35.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 2 - new BezierLine( - new Point(60.000, 35.000, Point.CARTESIAN), - new Point(60.000, 25.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 3 - new BezierLine( - new Point(60.000, 25.000, Point.CARTESIAN), - new Point(15.000, 25.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 4 - new BezierLine( - new Point(15.000, 25.000, Point.CARTESIAN), - new Point(60.000, 25.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 5 - new BezierLine( - new Point(60.000, 25.000, Point.CARTESIAN), - new Point(60.000, 15.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 6 - new BezierLine( - new Point(60.000, 15.000, Point.CARTESIAN), - new Point(15.000, 15.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 7 - new BezierLine( - new Point(15.000, 15.000, Point.CARTESIAN), - new Point(60.000, 15.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 8 - new BezierLine( - new Point(60.000, 15.000, Point.CARTESIAN), - new Point(60.000, 8.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 9 - new BezierLine( - new Point(60.000, 8.000, Point.CARTESIAN), - new Point(15.000, 8.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)).build(); - - follower.followPath(path); - - 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); - } - follower.telemetryDebug(telemetryA); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExampleThree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExampleThree.java deleted file mode 100644 index b762758..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExampleThree.java +++ /dev/null @@ -1,91 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots.poc; - -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.Disabled; -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 -@Disabled -@Autonomous(name = "AutoExample - Curve and Line", group = "Autonomous Pathing Tuning") -public class AutoExampleThree extends OpMode { - private Telemetry telemetryA; - - private Follower follower; - - private PathChain path; - - private final Pose startPose = new Pose(10,45, 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(.4); - - follower.setStartingPose(startPose); - - path = follower.pathBuilder() - .addPath( - // Line 1 - new BezierCurve( - new Point(10.000, 45.000, Point.CARTESIAN), - new Point(45.000, 45.000, Point.CARTESIAN), - new Point(50.000, 20.000, Point.CARTESIAN) - ) - ) - .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-90)) - .addPath( - // Line 2 - new BezierLine( - new Point(50.000, 20.000, Point.CARTESIAN), - new Point(10.000, 20.000, Point.CARTESIAN) - ) - ) - .setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(-90)) - .build(); - - follower.followPath(path); - - 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); - } - follower.telemetryDebug(telemetryA); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExampleTwo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExampleTwo.java deleted file mode 100644 index edcbe30..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/AutoExampleTwo.java +++ /dev/null @@ -1,82 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots.poc; - -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.Disabled; -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; - -/** - * 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 -@Disabled -@Autonomous(name = "AutoExample - Simple Curve", group = "Autonomous Pathing Tuning") -public class AutoExampleTwo extends OpMode { - private Telemetry telemetryA; - - private Follower follower; - - private PathChain path; - - private final Pose startPose = new Pose(10.0, 45, 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(.4); - - follower.setStartingPose(startPose); - - path = follower.pathBuilder() - .addPath( - // Line 1 - new BezierCurve( - new Point(10.000, 45.000, Point.CARTESIAN), - new Point(45.000, 45.000, Point.CARTESIAN), - new Point(50.000, 20.000, Point.CARTESIAN) - ) - ) - .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(-90)) - .build(); - - follower.followPath(path); - - 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); - } - follower.telemetryDebug(telemetryA); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/BasicOmniOpMode_Linear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/BasicOmniOpMode_Linear.java deleted file mode 100644 index 5ee9e5f..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/BasicOmniOpMode_Linear.java +++ /dev/null @@ -1,199 +0,0 @@ -/* Copyright (c) 2021 FIRST. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, - * are permitted (subject to the limitations in the disclaimer below) provided that - * the following conditions are met: - * - * Redistributions of source code must retain the above copyright notice, this list - * of conditions and the following disclaimer. - * - * Redistributions in binary form must reproduce the above copyright notice, this - * list of conditions and the following disclaimer in the documentation and/or - * other materials provided with the distribution. - * - * Neither the name of FIRST nor the names of its contributors may be used to endorse or - * promote products derived from this software without specific prior written permission. - * - * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS - * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -package org.firstinspires.ftc.teamcode.cometbots.poc; - -import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER; -import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION; -import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR; -import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION; -import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR; -import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION; -import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR; -import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION; -import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR; -import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION; -import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER; -import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTION; -import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER; -import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION; - -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.util.ElapsedTime; - -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; - -/* - * This file contains an example of a Linear "OpMode". - * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. - * The names of OpModes appear on the menu of the FTC Driver Station. - * When a selection is made from the menu, the corresponding OpMode is executed. - * - * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. - * This code will work with either a Mecanum-Drive or an X-Drive train. - * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html - * Note that a Mecanum drive must display an X roller-pattern when viewed from above. - * - * Also note that it is critical to set the correct rotation direction for each motor. See details below. - * - * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. - * Each motion axis is controlled by one Joystick axis. - * - * 1) Axial: Driving forward and backward Left-joystick Forward/Backward - * 2) Lateral: Strafing right and left Left-joystick Right and Left - * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left - * - * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. - * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip - * the direction of all 4 motors (see code below). - * - * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. - * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list - */ - -@TeleOp(name="Basic: Omni Linear OpMode", group="Linear OpMode") -@Disabled -public class BasicOmniOpMode_Linear extends LinearOpMode { - - // Declare OpMode members for each of the 4 motors. - private final ElapsedTime runtime = new ElapsedTime(); - - @Override - public void runOpMode() { - - // Initialize the hardware variables. Note that the strings used here must correspond - // to the names assigned during the robot configuration step on the DS or RC devices. - DcMotor leftFrontDrive = hardwareMap.get(DcMotor.class, FRONT_LEFT_MOTOR); - DcMotor leftBackDrive = hardwareMap.get(DcMotor.class, BACK_LEFT_MOTOR); - DcMotor rightFrontDrive = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR); - DcMotor rightBackDrive = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR); - - - - // TODO: replace these with your encoder ports - Encoder leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER)); - Encoder rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, RIGHT_ENCODER)); - Encoder strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER)); - - // TODO: reverse any encoders necessary - leftEncoder.setDirection(LEFT_ENCODER_DIRECTION); - rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION); - strafeEncoder.setDirection(BACK_ENCODER_DIRECTION); - - // ######################################################################################## - // !!! IMPORTANT Drive Information. Test your motor directions. !!!!! - // ######################################################################################## - // Most robots need the motors on one side to be reversed to drive forward. - // The motor reversals shown here are for a "direct drive" robot (the wheels turn the same direction as the motor shaft) - // If your robot has additional gear reductions or uses a right-angled drive, it's important to ensure - // that your motors are turning in the correct direction. So, start out with the reversals here, BUT - // when you first test your robot, push the left joystick forward and observe the direction the wheels turn. - // Reverse the direction (flip FORWARD <-> REVERSE ) of any wheel that runs backward - // Keep testing until ALL the wheels move the robot forward when you push the left joystick forward. - leftFrontDrive.setDirection(FRONT_LEFT_MOTOR_DIRECTION); - leftBackDrive.setDirection(BACK_LEFT_MOTOR_DIRECTION); - rightFrontDrive.setDirection(FRONT_RIGHT_MOTOR_DIRECTION); - rightBackDrive.setDirection(BACK_RIGHT_MOTOR_DIRECTION); - - // Wait for the game to start (driver presses START) - telemetry.addData("Status", "Initialized"); - telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition()); - telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition()); - telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition()); - telemetry.update(); - - waitForStart(); - runtime.reset(); - - // run until the end of the match (driver presses STOP) - while (opModeIsActive()) { - double max; - - // POV Mode uses left joystick to go forward & strafe, and right joystick to rotate. - double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value - double lateral = gamepad1.left_stick_x; - double yaw = gamepad1.right_stick_x; - - // Combine the joystick requests for each axis-motion to determine each wheel's power. - // Set up a variable for each drive wheel to save the power level for telemetry. - double leftFrontPower = axial + lateral + yaw; - double rightFrontPower = axial - lateral - yaw; - double leftBackPower = axial - lateral + yaw; - double rightBackPower = axial + lateral - yaw; - - // Normalize the values so no wheel power exceeds 100% - // This ensures that the robot maintains the desired motion. - max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower)); - max = Math.max(max, Math.abs(leftBackPower)); - max = Math.max(max, Math.abs(rightBackPower)); - - if (max > 1.0) { - leftFrontPower /= max; - rightFrontPower /= max; - leftBackPower /= max; - rightBackPower /= max; - } - - // This is test code: - // - // Uncomment the following code to test your motor directions. - // Each button should make the corresponding motor run FORWARD. - // 1) First get all the motors to take to correct positions on the robot - // by adjusting your Robot Configuration if necessary. - // 2) Then make sure they run in the correct direction by modifying the - // the setDirection() calls above. - // Once the correct motors move in the correct direction re-comment this code. - - /* - leftFrontPower = gamepad1.x ? 1.0 : 0.0; // X gamepad - leftBackPower = gamepad1.a ? 1.0 : 0.0; // A gamepad - rightFrontPower = gamepad1.y ? 1.0 : 0.0; // Y gamepad - rightBackPower = gamepad1.b ? 1.0 : 0.0; // B gamepad - */ - - // Send calculated power to wheels - leftFrontDrive.setPower(leftFrontPower); - rightFrontDrive.setPower(rightFrontPower); - leftBackDrive.setPower(leftBackPower); - rightBackDrive.setPower(rightBackPower); - - // Show the elapsed game time and wheel power. - telemetry.addData("Status", "Run Time: " + runtime.toString()); - telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower); - telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower); - telemetry.addData("Left Encoder Value", leftEncoder.getDeltaPosition()); - telemetry.addData("Right Encoder Value", rightEncoder.getDeltaPosition()); - telemetry.addData("Strafe Encoder Value", strafeEncoder.getDeltaPosition()); - telemetry.update(); - } - }} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/BlueBasketAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/BlueBasketAuto.java deleted file mode 100644 index 3bff880..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/BlueBasketAuto.java +++ /dev/null @@ -1,163 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots.poc; - -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.OpMode; -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 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 -@Disabled -@Autonomous(name = "BlueBasketAuto", group = "Autonomous Pathing Tuning") -public class BlueBasketAuto extends OpMode { - private Telemetry telemetryA; - - private Follower follower; - - private PathChain path; - - private final Pose startPose = new Pose(11.25, 95.75); - - /** - * 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 BezierLine( - new Point(11.250, 95.750, Point.CARTESIAN), - new Point(37.000, 108.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 2 - new BezierCurve( - new Point(37.000, 108.000, Point.CARTESIAN), - new Point(73.286, 111.536, Point.CARTESIAN), - new Point(67.821, 120.536, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 3 - new BezierLine( - new Point(67.821, 120.536, Point.CARTESIAN), - new Point(28.000, 121.500, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 4 - new BezierLine( - new Point(28.000, 121.500, Point.CARTESIAN), - new Point(18.000, 130.179, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 5 - new BezierCurve( - new Point(18.000, 130.179, Point.CARTESIAN), - new Point(59.000, 102.500, Point.CARTESIAN), - new Point(68.700, 130.500, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 6 - new BezierLine( - new Point(68.700, 130.500, Point.CARTESIAN), - new Point(18.000, 130.339, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 7 - new BezierCurve( - new Point(18.000, 130.339, Point.CARTESIAN), - new Point(49.018, 121.179, Point.CARTESIAN), - new Point(63.804, 135.321, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 8 - new BezierLine( - new Point(63.804, 135.321, Point.CARTESIAN), - new Point(53.036, 135.161, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 9 - new BezierLine( - new Point(53.036, 135.161, Point.CARTESIAN), - new Point(18.643, 135.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 10 - new BezierLine( - new Point(18.643, 135.000, Point.CARTESIAN), - new Point(72.300, 97.400, Point.CARTESIAN) - ) - ) - .addPath( - // Line 9 - new BezierLine( - new Point(18.643, 135.000, Point.CARTESIAN), - new Point(83.250, 95.464, Point.CARTESIAN) - ) - ) - .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270)).build(); - follower.followPath(path); - - 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); - } - follower.telemetryDebug(telemetryA); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/BluebAutoV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/BluebAutoV1.java deleted file mode 100644 index a3ba0ad..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/poc/BluebAutoV1.java +++ /dev/null @@ -1,257 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots.poc; - -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.Disabled; -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.BezierLine; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierPoint; -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 -@Disabled -@Autonomous(name = "BluebAutoV1", group = "Autonomous Pathing Tuning") -public class BluebAutoV1 extends OpMode { - private Telemetry telemetryA; - - private Follower follower; - - private PathChain path; - - private final Pose startPose = new Pose(7.5, 72, 90); - - /** - * 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(.4); - - follower.setStartingPose(startPose); - - path = follower.pathBuilder() - .addPath( - // Line 1 - new BezierLine( - new Point(7.5, 72, Point.CARTESIAN), - new Point(29.893, 38.250, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 2 - new BezierLine( - new Point(29.893, 38.250, Point.CARTESIAN), - new Point(65.250, 32.143, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 3 - new BezierLine( - new Point(65.250, 32.143, Point.CARTESIAN), - new Point(61.714, 24.429, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 4 - new BezierLine( - new Point(61.714, 24.429, Point.CARTESIAN), - new Point(13.821, 22.821, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 5 - new BezierLine( - new Point(13.821, 22.821, Point.CARTESIAN), - new Point(61.714, 24.429, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 6 - new BezierLine( - new Point(61.714, 24.429, Point.CARTESIAN), - new Point(60.750, 12.696, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 7 - new BezierLine( - new Point(60.750, 12.696, Point.CARTESIAN), - new Point(12.375, 13.179, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 8 - new BezierLine( - new Point(12.375, 13.179, Point.CARTESIAN), - new Point(60.750, 12.536, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 9 - new BezierLine( - new Point(60.750, 12.536, Point.CARTESIAN), - new Point(60.589, 9.321, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 10 - new BezierLine( - new Point(60.589, 9.321, Point.CARTESIAN), - new Point(12.536, 8.357, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 11 - new BezierLine( - new Point(12.536, 8.357, Point.CARTESIAN), - new Point(26.679, 8.679, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 12 - new BezierLine( - new Point(26.679, 8.679, Point.CARTESIAN), - new Point(22.821, 109.446, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 13 - new BezierLine( - new Point(22.821, 109.446, Point.CARTESIAN), - new Point(70.714, 109.446, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 14 - new BezierLine( - new Point(70.714, 109.446, Point.CARTESIAN), - new Point(71.036, 120.214, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 15 - new BezierLine( - new Point(71.036, 120.214, Point.CARTESIAN), - new Point(22.179, 120.214, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 16 - new BezierLine( - new Point(22.179, 120.214, Point.CARTESIAN), - new Point(11.089, 130.821, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 17 - new BezierLine( - new Point(11.089, 130.821, Point.CARTESIAN), - new Point(70.714, 112.018, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 18 - new BezierLine( - new Point(70.714, 112.018, Point.CARTESIAN), - new Point(70.714, 128.250, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 19 - new BezierLine( - new Point(70.714, 128.250, Point.CARTESIAN), - new Point(9.964, 130.018, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 20 - new BezierLine( - new Point(9.964, 130.018, Point.CARTESIAN), - new Point(70.554, 130.500, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 21 - new BezierLine( - new Point(70.554, 130.500, Point.CARTESIAN), - new Point(70.393, 135.000, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)) - .addPath( - // Line 22 - new BezierLine( - new Point(70.393, 135.000, Point.CARTESIAN), - new Point(13.821, 134.839, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(90)).build(); - - - follower.holdPoint( - new BezierPoint( - new Point(13.821, 134.839, Point.CARTESIAN) - ), - 90 - ); - - follower.followPath(path); - 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); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java deleted file mode 100644 index bee167c..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java +++ /dev/null @@ -1,87 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystem; - -import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armBucket; -import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armFloor; -import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark; - -import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.ServoImplEx; - -import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.util.action.Actions; -import org.firstinspires.ftc.teamcode.util.action.RunAction; - -public class ArmSubsystem { - - public enum ArmState { - PARK, FLOOR, BUCKET - } - - private ServoImplEx arm; - private ArmState state; - public RunAction toFloorPosition, toParkPosition, toBucketPosition; - private Telemetry telemetry; - - public ArmSubsystem(HardwareMap hardwareMap, Telemetry telemetry) { - this.arm = hardwareMap.get(ServoImplEx.class, "arm-servo"); - this.toParkPosition = new RunAction(this::toParkPosition); - this.toFloorPosition = new RunAction(this::toFloorPosition); - this.toBucketPosition = new RunAction(this::toBucketPosition); - this.telemetry = telemetry; - } - - public void setState(ArmState armState) { - if (armState == ArmState.FLOOR) { - this.arm.setPosition(armFloor); - } else if (armState == ArmState.PARK) { - this.arm.setPosition(armPark); - } else if (armState == ArmState.BUCKET) { - this.arm.setPosition(armBucket); - } - this.state = armState; - } - - public void toFloorPosition() { - this.setState(ArmState.FLOOR); - } - - public void toParkPosition() { - this.setState(ArmState.PARK); - } - - public void toBucketPosition() { - this.setState(ArmState.BUCKET); - } - - public void switchState() { - if (this.state == ArmState.FLOOR) { - this.setState(ArmState.PARK); - } else if (this.state == ArmState.PARK) { - this.setState(ArmState.BUCKET); - } else if (this.state == ArmState.BUCKET) { - this.setState(ArmState.FLOOR); - } - } - - public ArmState getState() { - return this.state; - } - - public void init() { - this.arm.resetDeviceConfigurationForOpMode(); - Actions.runBlocking(this.toParkPosition); - } - - public void start() { - Actions.runBlocking(this.toParkPosition); - } - - public double getPosition() { - return this.arm.getPosition(); - } - - public void setPosition(double position) { - this.arm.setPosition(position); - } - -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java deleted file mode 100644 index 52acc57..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java +++ /dev/null @@ -1,72 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystem; - -import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawClose; -import static org.firstinspires.ftc.teamcode.configs.RobotConstants.clawOpen; - -import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.Servo; - -import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.util.action.Actions; -import org.firstinspires.ftc.teamcode.util.action.RunAction; - -public class ClawSubsystem { - - public enum ClawState { - CLOSED, OPEN - } - - private Servo claw; - private ClawState state; - public RunAction openClaw, closeClaw; - public Telemetry telemetry; - - public ClawSubsystem(HardwareMap hardwareMap, Telemetry telemetry) { - this.claw = hardwareMap.get(Servo.class, "claw-servo"); - this.openClaw = new RunAction(this::openClaw); - this.closeClaw = new RunAction(this::closeClaw); - this.telemetry = telemetry; - } - - public void setState(ClawState clawState) { - if (clawState == ClawState.CLOSED) { - claw.setPosition(clawClose); - } else if (clawState == ClawState.OPEN) { - claw.setPosition(clawOpen); - } - this.state = clawState; - } - - public ClawState getState() { - return this.state; - } - - public void switchState() { - if (state == ClawState.CLOSED) { - setState(ClawState.OPEN); - } else if (state == ClawState.OPEN) { - setState(ClawState.CLOSED); - } - } - - public void openClaw() { - setState(ClawState.OPEN); - } - - public void closeClaw() { - setState(ClawState.CLOSED); - } - - public void init() { - Actions.runBlocking(closeClaw); - } - - public void start() { - Actions.runBlocking(closeClaw); - } - - public double getPosition() { - return this.claw.getPosition(); - } - -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java deleted file mode 100644 index 3469a7e..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java +++ /dev/null @@ -1,95 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystem; - -import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftPower; -import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToSubmarinePos; -import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToFloorPos; -import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos; -import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos; - -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.hardware.HardwareMap; - -import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.util.action.Actions; -import org.firstinspires.ftc.teamcode.util.action.RunAction; - -public class LiftSubsystem { - - public DcMotor lift; - public RunAction toFloor, toLowBucket, toHighBucket, toFloat; - - public enum LiftState { - FLOOR, LOW_BUCKET, HIGH_BUCKET, FLOAT - } - - private LiftState liftState; - private Telemetry telemetry; - - public LiftSubsystem(HardwareMap hardwareMap, Telemetry telemetry) { - lift = hardwareMap.get(DcMotor.class, "lift-motor"); - toFloor = new RunAction(this::toFloor); - toLowBucket = new RunAction(this::toLowBucket); - toHighBucket = new RunAction(this::toHighBucket); - toFloat = new RunAction(this::toFloat); - this.telemetry = telemetry; - } - - public void setTarget(int b) { - lift.setTargetPosition(b); - lift.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } - - public void switchState() { - if (this.liftState == LiftState.FLOOR) { - this.toFloat(); - } else if (this.liftState == LiftState.FLOAT) { - this.toLowBucket(); - } else if (this.liftState == LiftState.LOW_BUCKET) { - this.toHighBucket(); - } else if (this.liftState == LiftState.HIGH_BUCKET) { - this.toFloor(); - } - } - - public void toFloor() { - this.setState(LiftState.FLOOR); - this.setTarget(liftToFloorPos); - } - - public void toFloat() { - this.setState(LiftState.FLOAT); - this.setTarget(liftToSubmarinePos); - } - - public void toLowBucket() { - this.setState(LiftState.LOW_BUCKET); - this.setTarget(liftToLowBucketPos); - } - - public void toHighBucket() { - this.setState(LiftState.HIGH_BUCKET); - this.setTarget(liftToHighBucketPos); - } - - public void init() { - lift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - lift.setDirection(DcMotorSimple.Direction.REVERSE); - lift.setPower(liftPower); - } - - private void setState(LiftState liftState) { - this.liftState = liftState; - } - - public LiftState getState() { return this.liftState; } - - public int getPosition() { - return lift.getCurrentPosition(); - } - - public void start() { - Actions.runBlocking(toFloor); - } - -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java deleted file mode 100644 index 9c64e03..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java +++ /dev/null @@ -1,79 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystem; - -import static org.firstinspires.ftc.teamcode.PedroConstants.WRIST_NAME; -import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristBucket; -import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristFloor; - -import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.ServoImplEx; - -import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.util.action.Actions; -import org.firstinspires.ftc.teamcode.util.action.RunAction; - -public class WristSubsystem { - - public enum WristState { - FLOOR, BUCKET - } - - public ServoImplEx wrist; - public WristState state; - public RunAction toFloorPosition, toBucketPosition; - public Telemetry telemetry; - - public WristSubsystem(HardwareMap hardwareMap, Telemetry telemetry) { - this.wrist = hardwareMap.get(ServoImplEx.class, WRIST_NAME); - this.telemetry = telemetry; - toBucketPosition = new RunAction(this::toBucketPosition); - toFloorPosition = new RunAction(this::toFloorPosition); - } - - public void setState(WristState wristState) { - if (wristState == WristState.FLOOR) { - wrist.setPosition(wristFloor); - } else if (wristState == WristState.BUCKET) { - wrist.setPosition(wristBucket); - } - this.state = wristState; - } - - public void toFloorPosition() { - setState(WristState.FLOOR); - } - - public void toBucketPosition() { - setState(WristState.BUCKET); - } - - public void switchState() { - if (state == WristState.FLOOR) { - setState(WristState.BUCKET); - } else if (state == WristState.BUCKET) { - setState(WristState.FLOOR); - } - } - - public WristState getState() { - return this.state; - } - - public void init() { - wrist.resetDeviceConfigurationForOpMode(); - Actions.runBlocking(toFloorPosition); - } - - public void start() { - Actions.runBlocking(toFloorPosition); - } - - public double getPosition() { - return this.wrist.getPosition(); - } - - public void setPosition(double position) { - this.wrist.setPosition(position); - } - - -}