diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java index 6e2db74..469eab2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java @@ -39,13 +39,13 @@ public class PedroConstants { // Robot encoders // NOTE: Encoders are plugged into the same ports as motors hence the weird names - public static final String RIGHT_ENCODER = "back-right"; //0 + public static final String RIGHT_ENCODER = "front-left"; //2 public static final String BACK_ENCODER = "front-right"; //1 - public static final String LEFT_ENCODER = "front-left"; //2 + public static final String LEFT_ENCODER = "back-right"; //0 // Robot encoder direction public static final double LEFT_ENCODER_DIRECTION = Encoder.FORWARD; - public static final double RIGHT_ENCODER_DIRECTION = Encoder.FORWARD; + public static final double RIGHT_ENCODER_DIRECTION = Encoder.REVERSE; public static final double BACK_ENCODER_DIRECTION = Encoder.FORWARD; // Arm config diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AsherPathV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AsherPathV1.java deleted file mode 100644 index eadafc9..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AsherPathV1.java +++ /dev/null @@ -1,135 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots.projects; - -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 = "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/projects/AutoExample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExample.java deleted file mode 100644 index 763aa0e..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExample.java +++ /dev/null @@ -1,79 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots.projects; - -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.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 = "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/projects/AutoExampleFour.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleFour.java deleted file mode 100644 index 296d7c3..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleFour.java +++ /dev/null @@ -1,106 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots.projects; - -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 = "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/projects/AutoExampleSeason2025V1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleSeason2025V1.java deleted file mode 100644 index a1f6bfc..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleSeason2025V1.java +++ /dev/null @@ -1,142 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots.projects; - -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.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 = "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/projects/AutoExampleThree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleThree.java deleted file mode 100644 index 033783e..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleThree.java +++ /dev/null @@ -1,89 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots.projects; - -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 = "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/projects/AutoExampleTwo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleTwo.java deleted file mode 100644 index 038328b..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleTwo.java +++ /dev/null @@ -1,80 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots.projects; - -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.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 = "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/projects/AutoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoTest.java deleted file mode 100644 index 37d0916..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoTest.java +++ /dev/null @@ -1,91 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots.projects; - -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.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 = "Test", group = "Autonomous Pathing Tuning") -public class AutoTest extends OpMode { - private Telemetry telemetryA; - - - private Follower follower; - - private PathChain test; - - @Override - public void init() { - follower = new Follower(hardwareMap); - - test = follower.pathBuilder() - .addPath( - new BezierLine( - new Point(8.000, 60.000, Point.CARTESIAN), - new Point(18.000, 60.000, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 2 - new BezierCurve( - new Point(18.000, 60.000, Point.CARTESIAN), - new Point(18.000, 23.000, Point.CARTESIAN), - new Point(48.000, 23.000, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 3 - new BezierLine( - new Point(48.000, 23.000, Point.CARTESIAN), - new Point(60.000, 36.000, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 4 - new BezierLine( - new Point(60.000, 36.000, Point.CARTESIAN), - new Point(60.000, 49.000, Point.CARTESIAN) - ) - ).build(); - - - follower.followPath(test); - - telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); - telemetryA.update(); - } - - - @Override - public void loop() { - follower.update(); - if (follower.atParametricEnd()) { - follower.followPath(test); - } - - follower.telemetryDebug(telemetryA); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BlueAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BlueAuto.java deleted file mode 100644 index d9b1c47..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BlueAuto.java +++ /dev/null @@ -1,128 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots.projects; - - -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; -public class BlueAuto { - - - - public void GeneratedPath() { - PathBuilder builder = new PathBuilder(); - - builder - .addPath( - // Line 1 - new BezierLine( - new Point(9.757, 84.983, Point.CARTESIAN), - new Point(8.442, 129.227, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 2 - new BezierLine( - new Point(8.442, 129.227, Point.CARTESIAN), - new Point(52.762, 101.628, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 3 - new BezierLine( - new Point(52.762, 101.628, Point.CARTESIAN), - new Point(79.224, 116.564, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 4 - new BezierLine( - new Point(79.224, 116.564, Point.CARTESIAN), - new Point(54.548, 130.525, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 5 - new BezierLine( - new Point(54.548, 130.525, Point.CARTESIAN), - new Point(12.338, 133.772, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 6 - new BezierLine( - new Point(12.338, 133.772, Point.CARTESIAN), - new Point(52.437, 101.628, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 7 - new BezierLine( - new Point(52.437, 101.628, Point.CARTESIAN), - new Point(71.594, 120.947, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 8 - new BezierLine( - new Point(71.594, 120.947, Point.CARTESIAN), - new Point(52.275, 120.785, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 9 - new BezierLine( - new Point(52.275, 120.785, Point.CARTESIAN), - new Point(11.039, 131.012, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 10 - new BezierLine( - new Point(11.039, 131.012, Point.CARTESIAN), - new Point(70.782, 120.460, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 11 - new BezierLine( - new Point(70.782, 120.460, Point.CARTESIAN), - new Point(50.327, 142.377, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 12 - new BezierLine( - new Point(50.327, 142.377, Point.CARTESIAN), - new Point(13.799, 134.422, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 13 - new BezierLine( - new Point(13.799, 134.422, Point.CARTESIAN), - new Point(13.799, 134.422, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 14 - new BezierLine( - new Point(13.799, 134.422, Point.CARTESIAN), - new Point(71.919, 103.901, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation(); - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BlueBasketAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BlueBasketAuto.java deleted file mode 100644 index 782c451..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BlueBasketAuto.java +++ /dev/null @@ -1,161 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots.projects; - -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 -@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, 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 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, 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); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BluebAutoV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BluebAutoV1.java deleted file mode 100644 index 2275b5d..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BluebAutoV1.java +++ /dev/null @@ -1,246 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots.projects; - -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.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 = "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.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/projects/BluenbAutov1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BluenbAutov1.java deleted file mode 100644 index e5581b8..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BluenbAutov1.java +++ /dev/null @@ -1,4 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots.projects; - -public class BluenbAutov1 { -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/GeneratedPath.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/GeneratedPath.java deleted file mode 100644 index dcc3596..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/GeneratedPath.java +++ /dev/null @@ -1,109 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots.projects; - -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; - -public class GeneratedPath { - public GeneratedPath() { - PathBuilder builder = new PathBuilder(); - - builder - .addPath( - // Line 1 - new BezierLine( - new Point(9.757, 84.983, Point.CARTESIAN), - new Point(28.573, 76.302, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 2 - new BezierLine( - new Point(28.573, 76.302, Point.CARTESIAN), - new Point(36.203, 76.140, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 3 - new BezierLine( - new Point(36.203, 76.140, Point.CARTESIAN), - new Point(35.067, 35.716, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 4 - new BezierLine( - new Point(35.067, 35.716, Point.CARTESIAN), - new Point(73.705, 34.742, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 5 - new BezierLine( - new Point(73.705, 34.742, Point.CARTESIAN), - new Point(73.705, 24.839, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 6 - new BezierLine( - new Point(73.705, 24.839, Point.CARTESIAN), - new Point(7.630, 26.462, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 7 - new BezierLine( - new Point(7.630, 26.462, Point.CARTESIAN), - new Point(64.126, 22.728, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 8 - new BezierLine( - new Point(64.126, 22.728, Point.CARTESIAN), - new Point(63.964, 13.150, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 9 - new BezierLine( - new Point(63.964, 13.150, Point.CARTESIAN), - new Point(12.338, 15.260, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 10 - new BezierLine( - new Point(12.338, 15.260, Point.CARTESIAN), - new Point(63.802, 13.150, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 11 - new BezierLine( - new Point(63.802, 13.150, Point.CARTESIAN), - new Point(63.639, 11.689, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 12 - new BezierLine( - new Point(63.639, 11.689, Point.CARTESIAN), - new Point(12.014, 11.689, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation(); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/RedAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/RedAuto.java deleted file mode 100644 index 3c9e1c6..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/RedAuto.java +++ /dev/null @@ -1,99 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots.projects; - - - -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; -public class RedAuto { - - - public class GeneratedPath { - - public GeneratedPath() { - PathBuilder builder = new PathBuilder(); - - builder - .addPath( - // Line 1 - new BezierLine( - new Point(131.499, 58.931, Point.CARTESIAN), - new Point(131.986, 18.183, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 2 - new BezierLine( - new Point(131.986, 18.183, Point.CARTESIAN), - new Point(90.264, 40.911, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 3 - new BezierLine( - new Point(90.264, 40.911, Point.CARTESIAN), - new Point(83.445, 26.300, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 4 - new BezierLine( - new Point(83.445, 26.300, Point.CARTESIAN), - new Point(136.207, 14.286, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 5 - new BezierLine( - new Point(136.207, 14.286, Point.CARTESIAN), - new Point(81.497, 24.352, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 6 - new BezierLine( - new Point(81.497, 24.352, Point.CARTESIAN), - new Point(82.634, 12.988, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 7 - new BezierLine( - new Point(82.634, 12.988, Point.CARTESIAN), - new Point(133.935, 11.364, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 8 - new BezierLine( - new Point(133.935, 11.364, Point.CARTESIAN), - new Point(82.309, 11.689, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 9 - new BezierLine( - new Point(82.309, 11.689, Point.CARTESIAN), - new Point(83.445, 2.598, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 10 - new BezierLine( - new Point(83.445, 2.598, Point.CARTESIAN), - new Point(132.149, 10.390, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation(); - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/SensorBNO055IMU.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/SensorBNO055IMU.java deleted file mode 100644 index 259989e..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/SensorBNO055IMU.java +++ /dev/null @@ -1,185 +0,0 @@ -/* Copyright (c) 2017 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.projects; - -import com.qualcomm.hardware.bosch.BNO055IMU; -import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.firstinspires.ftc.robotcore.external.Func; -import org.firstinspires.ftc.robotcore.external.navigation.Acceleration; -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; -import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; -import org.firstinspires.ftc.robotcore.external.navigation.Orientation; -import org.firstinspires.ftc.robotcore.external.navigation.Position; -import org.firstinspires.ftc.robotcore.external.navigation.Velocity; - -import java.util.Locale; - -/* - * This OpMode gives a short demo on how to use the BNO055 Inertial Motion Unit (IMU) from AdaFruit. - * - * Note: this is a Legacy example that will not work with newer Control/Expansion Hubs that use a different IMU - * Please use the new SensorIMUOrthogonal or SensorIMUNonOrthogonal samples for a more universal IMU interface. - * - * 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 - * - * @see Adafruit IMU - */ -@TeleOp(name = "Sensor: BNO055 IMU", group = "Sensor") -public class SensorBNO055IMU extends LinearOpMode - { - //---------------------------------------------------------------------------------------------- - // State - //---------------------------------------------------------------------------------------------- - - // The IMU sensor object - BNO055IMU imu; - - // State used for updating telemetry - Orientation angles; - Acceleration gravity; - - //---------------------------------------------------------------------------------------------- - // Main logic - //---------------------------------------------------------------------------------------------- - - @Override public void runOpMode() { - - // Set up the parameters with which we will use our IMU. Note that integration - // algorithm here just reports accelerations to the logcat log; it doesn't actually - // provide positional information. - BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); - parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES; - parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC; - parameters.calibrationDataFile = "BNO055IMUCalibration.json"; // see the calibration sample OpMode - parameters.loggingEnabled = true; - parameters.loggingTag = "IMU"; - parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator(); - - // Retrieve and initialize the IMU. We expect the IMU to be attached to an I2C port - // on a Core Device Interface Module, configured to be a sensor of type "AdaFruit IMU", - // and named "imu". - imu = hardwareMap.get(BNO055IMU.class, "adafruit_imu"); - imu.initialize(parameters); - - // Set up our telemetry dashboard - composeTelemetry(); - - // Wait until we're told to go - waitForStart(); - - // Start the logging of measured acceleration - imu.startAccelerationIntegration(new Position(), new Velocity(), 1000); - - // Loop and update the dashboard - while (opModeIsActive()) { - telemetry.update(); - } - } - - //---------------------------------------------------------------------------------------------- - // Telemetry Configuration - //---------------------------------------------------------------------------------------------- - - void composeTelemetry() { - - // At the beginning of each telemetry update, grab a bunch of data - // from the IMU that we will then display in separate lines. - telemetry.addAction(new Runnable() { @Override public void run() - { - // Acquiring the angles is relatively expensive; we don't want - // to do that in each of the three items that need that info, as that's - // three times the necessary expense. - angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES); - gravity = imu.getGravity(); - } - }); - - telemetry.addLine() - .addData("status", new Func() { - @Override public String value() { - return imu.getSystemStatus().toShortString(); - } - }) - .addData("calib", new Func() { - @Override public String value() { - return imu.getCalibrationStatus().toString(); - } - }); - - telemetry.addLine() - .addData("heading", new Func() { - @Override public String value() { - return formatAngle(angles.angleUnit, angles.firstAngle); - } - }) - .addData("roll", new Func() { - @Override public String value() { - return formatAngle(angles.angleUnit, angles.secondAngle); - } - }) - .addData("pitch", new Func() { - @Override public String value() { - return formatAngle(angles.angleUnit, angles.thirdAngle); - } - }); - - telemetry.addLine() - .addData("grvty", new Func() { - @Override public String value() { - return gravity.toString(); - } - }) - .addData("mag", new Func() { - @Override public String value() { - return String.format(Locale.getDefault(), "%.3f", - Math.sqrt(gravity.xAccel*gravity.xAccel - + gravity.yAccel*gravity.yAccel - + gravity.zAccel*gravity.zAccel)); - } - }); - } - - //---------------------------------------------------------------------------------------------- - // Formatting - //---------------------------------------------------------------------------------------------- - - String formatAngle(AngleUnit angleUnit, double angle) { - return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle)); - } - - String formatDegrees(double degrees){ - return String.format(Locale.getDefault(), "%.1f", AngleUnit.DEGREES.normalize(degrees)); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/SensorBNO055IMUCalibration.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/SensorBNO055IMUCalibration.java deleted file mode 100644 index 4e1cef5..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/SensorBNO055IMUCalibration.java +++ /dev/null @@ -1,229 +0,0 @@ -/* Copyright (c) 2017 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.projects; - -import com.qualcomm.hardware.bosch.BNO055IMU; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.util.ReadWriteFile; - -import org.firstinspires.ftc.robotcore.external.Func; -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; -import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; -import org.firstinspires.ftc.robotcore.external.navigation.Orientation; -import org.firstinspires.ftc.robotcore.internal.system.AppUtil; - -import java.io.File; -import java.util.Locale; - -/* - * This OpMode calibrates a BNO055 IMU per - * "Section 3.11 Calibration" of the BNO055 specification. - * - * Note: this is a Legacy example that will not work with newer Control/Expansion Hubs that use a different IMU - * Please use the new SensorIMUOrthogonal or SensorIMUNonOrthogonal samples for a more universal IMU interface. - * - * Manual calibration of the IMU is definitely NOT necessary: except for the magnetometer (which is not used by the - * default "IMU" SensorMode), the BNO055 is internally self-calibrating and thus can be very successfully used without - * manual intervention. That said, performing a one-time calibration, saving the results persistently, then loading them - * again at each run can help reduce the time that automatic calibration requires. - * - * This summary of the calibration process from Intel is informative: - * http://iotdk.intel.com/docs/master/upm/classupm_1_1_b_n_o055.html - * - * "This device requires calibration in order to operate accurately. [...] Calibration data is - * lost on a power cycle. See one of the examples for a description of how to calibrate the device, - * but in essence: - * - * There is a calibration status register available [...] that returns the calibration status - * of the accelerometer (ACC), magnetometer (MAG), gyroscope (GYR), and overall system (SYS). - * Each of these values range from 0 (uncalibrated) to 3 (fully calibrated). Calibration [ideally] - * involves certain motions to get all 4 values at 3. The motions are as follows (though see the - * datasheet for more information): - * - * 1. GYR: Simply let the sensor sit flat for a few seconds. - * 2. ACC: Move the sensor in various positions. Start flat, then rotate slowly by 45 - * degrees, hold for a few seconds, then continue rotating another 45 degrees and - * hold, etc. 6 or more movements of this type may be required. You can move through - * any axis you desire, but make sure that the device is lying at least once - * perpendicular to the x, y, and z axis. - * 3. MAG: Move slowly in a figure 8 pattern in the air, until the calibration values reaches 3. - * 4. SYS: This will usually reach 3 when the other items have also reached 3. If not, continue - * slowly moving the device though various axes until it does." - * - * To calibrate the IMU, run this sample OpMode with a gamepad attached to the driver station. - * Once the IMU has reached sufficient calibration as reported on telemetry, press the 'A' - * button on the gamepad to write the calibration to a file. That file can then be indicated - * later when running an OpMode which uses the IMU. - * - * Note: if your intended uses of the IMU do not include use of all its sensors (for example, - * you might not use the magnetometer), then it makes little sense for you to wait for full - * calibration of the sensors you are not using before saving the calibration data. Indeed, - * it appears that in a SensorMode that doesn't use the magnetometer (for example), the - * magnetometer cannot actually be calibrated. - * - * References: - * The AdafruitBNO055IMU Javadoc - * The BNO055IMU.Parameters.calibrationDataFile Javadoc - * The BNO055 product page: https://www.bosch-sensortec.com/bst/products/all_products/bno055 - * The BNO055 datasheet: https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf - */ -@TeleOp(name = "Sensor: BNO055 IMU Calibration", group = "Sensor") -public class SensorBNO055IMUCalibration extends LinearOpMode - { - //---------------------------------------------------------------------------------------------- - // State - //---------------------------------------------------------------------------------------------- - - // Our sensors, motors, and other devices go here, along with other long term state - BNO055IMU imu; - - // State used for updating telemetry - Orientation angles; - - //---------------------------------------------------------------------------------------------- - // Main logic - //---------------------------------------------------------------------------------------------- - - @Override public void runOpMode() { - - telemetry.log().setCapacity(12); - telemetry.log().add(""); - telemetry.log().add("Please refer to the calibration instructions"); - telemetry.log().add("contained in the Adafruit IMU calibration"); - telemetry.log().add("sample OpMode."); - telemetry.log().add(""); - telemetry.log().add("When sufficient calibration has been reached,"); - telemetry.log().add("press the 'A' button to write the current"); - telemetry.log().add("calibration data to a file."); - telemetry.log().add(""); - - // We are expecting the IMU to be attached to an I2C port on a Core Device Interface Module and named "imu". - BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); - parameters.loggingEnabled = true; - parameters.loggingTag = "IMU"; - imu = hardwareMap.get(BNO055IMU.class, "adafruit_imu"); - imu.initialize(parameters); - - composeTelemetry(); - telemetry.log().add("Waiting for start..."); - - // Wait until we're told to go - while (!isStarted()) { - telemetry.update(); - idle(); - } - - telemetry.log().add("...started..."); - - while (opModeIsActive()) { - - if (gamepad1.a) { - - // Get the calibration data - BNO055IMU.CalibrationData calibrationData = imu.readCalibrationData(); - - // Save the calibration data to a file. You can choose whatever file - // name you wish here, but you'll want to indicate the same file name - // when you initialize the IMU in an OpMode in which it is used. If you - // have more than one IMU on your robot, you'll of course want to use - // different configuration file names for each. - String filename = "AdafruitIMUCalibration.json"; - File file = AppUtil.getInstance().getSettingsFile(filename); - ReadWriteFile.writeFile(file, calibrationData.serialize()); - telemetry.log().add("saved to '%s'", filename); - - // Wait for the button to be released - while (gamepad1.a) { - telemetry.update(); - idle(); - } - } - - telemetry.update(); - } - } - - void composeTelemetry() { - - // At the beginning of each telemetry update, grab a bunch of data - // from the IMU that we will then display in separate lines. - telemetry.addAction(new Runnable() { @Override public void run() - { - // Acquiring the angles is relatively expensive; we don't want - // to do that in each of the three items that need that info, as that's - // three times the necessary expense. - angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES); - } - }); - - telemetry.addLine() - .addData("status", new Func() { - @Override public String value() { - return imu.getSystemStatus().toShortString(); - } - }) - .addData("calib", new Func() { - @Override public String value() { - return imu.getCalibrationStatus().toString(); - } - }); - - telemetry.addLine() - .addData("heading", new Func() { - @Override public String value() { - return formatAngle(angles.angleUnit, angles.firstAngle); - } - }) - .addData("roll", new Func() { - @Override public String value() { - return formatAngle(angles.angleUnit, angles.secondAngle); - } - }) - .addData("pitch", new Func() { - @Override public String value() { - return formatAngle(angles.angleUnit, angles.thirdAngle); - } - }); - } - - //---------------------------------------------------------------------------------------------- - // Formatting - //---------------------------------------------------------------------------------------------- - - String formatAngle(AngleUnit angleUnit, double angle) { - return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle)); - } - - String formatDegrees(double degrees){ - return String.format(Locale.getDefault(), "%.1f", AngleUnit.DEGREES.normalize(degrees)); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/SensorIMUOrthogonal.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/SensorIMUOrthogonal.java deleted file mode 100644 index 908fdcc..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/SensorIMUOrthogonal.java +++ /dev/null @@ -1,172 +0,0 @@ -/* Copyright (c) 2022 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.projects; - -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.IMU_LOGO_FACING_DIRECTION; -import static org.firstinspires.ftc.teamcode.PedroConstants.IMU_USB_FACING_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.hardware.rev.RevHubOrientationOnRobot; -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.DcMotorEx; -import com.qualcomm.robotcore.hardware.IMU; - -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity; -import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; -import org.firstinspires.ftc.teamcode.PedroConstants; -import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; - -/* - * This OpMode shows how to use the new universal IMU interface. This - * interface may be used with the BNO055 IMU or the BHI260 IMU. It assumes that an IMU is configured - * on the robot with the name "imu". - * - * The sample will display the current Yaw, Pitch and Roll of the robot.
- * With the correct orientation parameters selected, pitch/roll/yaw should act as follows: - * Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X)
- * Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y)
- * Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z)
- * - * The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller) - * - * This specific sample assumes that the Hub is mounted on one of the three orthogonal planes - * (X/Y, X/Z or Y/Z) and that the Hub has only been rotated in a range of 90 degree increments. - * - * Note: if your Hub is mounted on a surface angled at some non-90 Degree multiple (like 30) look at - * the alternative SensorIMUNonOrthogonal sample in this folder. - * - * This "Orthogonal" requirement means that: - * - * 1) The Logo printed on the top of the Hub can ONLY be pointing in one of six directions: - * FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT. - * - * 2) The USB ports can only be pointing in one of the same six directions:
- * FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT. - * - * So, To fully define how your Hub is mounted to the robot, you must simply specify:
- * logoFacingDirection
- * usbFacingDirection - * - * 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. - * - * Finally, choose the two correct parameters to define how your Hub is mounted and edit this OpMode - * to use those parameters. - */ -@TeleOp(name = "Sensor: IMU Orthogonal", group = "Sensor") -@Disabled // Comment this out to add to the OpMode list -public class SensorIMUOrthogonal extends LinearOpMode { - // The IMU sensor object - IMU imu; - private Encoder leftEncoder; - private Encoder rightEncoder; - private Encoder strafeEncoder; - - //---------------------------------------------------------------------------------------------- - // Main logic - //---------------------------------------------------------------------------------------------- - - @Override - public void runOpMode() throws InterruptedException { - - // Retrieve and initialize the IMU. - // This sample expects the IMU to be in a REV Hub and named "imu". - imu = hardwareMap.get(IMU.class, PedroConstants.IMU); - - // TODO: replace these with your encoder ports - leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, LEFT_ENCODER)); - rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, RIGHT_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); - - /* Define how the hub is mounted on the robot to get the correct Yaw, Pitch and Roll values. - * - * Two input parameters are required to fully specify the Orientation. - * The first parameter specifies the direction the printed logo on the Hub is pointing. - * The second parameter specifies the direction the USB connector on the Hub is pointing. - * All directions are relative to the robot, and left/right is as-viewed from behind the robot. - * - * If you are using a REV 9-Axis IMU, you can use the Rev9AxisImuOrientationOnRobot class instead of the - * RevHubOrientationOnRobot class, which has an I2cPortFacingDirection instead of a UsbFacingDirection. - */ - - /* The next two lines define Hub orientation. - * The Default Orientation (shown) is when a hub is mounted horizontally with the printed logo pointing UP and the USB port pointing FORWARD. - * - * To Do: EDIT these two lines to match YOUR mounting configuration. - */ - RevHubOrientationOnRobot.LogoFacingDirection logoDirection = IMU_LOGO_FACING_DIRECTION; - RevHubOrientationOnRobot.UsbFacingDirection usbDirection = IMU_USB_FACING_DIRECTION; - - RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection); - - // Now initialize the IMU with this mounting orientation - // Note: if you choose two conflicting directions, this initialization will cause a code exception. - imu.initialize(new IMU.Parameters(orientationOnRobot)); - - // Loop and update the dashboard - while (!isStopRequested()) { - - telemetry.addData("Hub orientation", "Logo=%s USB=%s\n ", logoDirection, usbDirection); - - // Check to see if heading reset is requested - if (gamepad1.y) { - telemetry.addData("Yaw", "Resetting\n"); - imu.resetYaw(); - } else { - telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n"); - } - - // Retrieve Rotational Angles and Velocities - YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles(); - AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES); - - telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES)); - telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES)); - telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES)); - telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate); - telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate); - telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate); - telemetry.update(); - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/bBlueAutoV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/bBlueAutoV1.java deleted file mode 100644 index eb85bcf..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/bBlueAutoV1.java +++ /dev/null @@ -1,237 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots.projects; - -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.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; - -@Autonomous(name = "org.firstinspires.ftc.teamcode.cometbots.projects.bBlueAutoV1", group = "V1") -public class bBlueAutoV1 extends OpMode { - - public Telemetry telemetry; - public Follower robot; - public PathChain path; - - @Override - public void init() { - robot = new Follower(hardwareMap); - - PathBuilder builder = new PathBuilder(); - - path = builder - .addPath( - // Line 1 - new BezierLine( - new Point(9.757, 84.983, Point.CARTESIAN), - new Point(28.573, 76.302, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 2 - new BezierLine( - new Point(28.573, 76.302, Point.CARTESIAN), - new Point(36.203, 76.140, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 3 - new BezierLine( - new Point(36.203, 76.140, Point.CARTESIAN), - new Point(35.067, 35.716, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 4 - new BezierLine( - new Point(35.067, 35.716, Point.CARTESIAN), - new Point(73.705, 34.742, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 5 - new BezierLine( - new Point(73.705, 34.742, Point.CARTESIAN), - new Point(73.705, 24.839, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 6 - new BezierLine( - new Point(73.705, 24.839, Point.CARTESIAN), - new Point(7.630, 26.462, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 7 - new BezierLine( - new Point(7.630, 26.462, Point.CARTESIAN), - new Point(64.126, 22.728, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 8 - new BezierLine( - new Point(64.126, 22.728, Point.CARTESIAN), - new Point(63.964, 13.150, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 9 - new BezierLine( - new Point(63.964, 13.150, Point.CARTESIAN), - new Point(12.338, 15.260, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 10 - new BezierLine( - new Point(12.338, 15.260, Point.CARTESIAN), - new Point(63.802, 13.150, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 11 - new BezierLine( - new Point(63.802, 13.150, Point.CARTESIAN), - new Point(63.639, 11.689, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 12 - new BezierLine( - new Point(63.639, 11.689, Point.CARTESIAN), - new Point(12.014, 11.689, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 13 - new BezierLine( - new Point(12.014, 11.689, Point.CARTESIAN), - new Point(62.665, 30.196, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 14 - new BezierLine( - new Point(62.665, 30.196, Point.CARTESIAN), - new Point(13.312, 51.463, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 15 - new BezierLine( - new Point(13.312, 51.463, Point.CARTESIAN), - new Point(16.234, 103.738, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 16 - new BezierLine( - new Point(16.234, 103.738, Point.CARTESIAN), - new Point(68.023, 108.284, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 17 - new BezierLine( - new Point(68.023, 108.284, Point.CARTESIAN), - new Point(68.185, 121.109, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 18 - new BezierLine( - new Point(68.185, 121.109, Point.CARTESIAN), - new Point(21.754, 119.811, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 19 - new BezierLine( - new Point(21.754, 119.811, Point.CARTESIAN), - new Point(11.526, 129.227, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 20 - new BezierLine( - new Point(11.526, 129.227, Point.CARTESIAN), - new Point(72.568, 111.856, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 21 - new BezierLine( - new Point(72.568, 111.856, Point.CARTESIAN), - new Point(58.607, 128.902, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 22 - new BezierLine( - new Point(58.607, 128.902, Point.CARTESIAN), - new Point(11.364, 130.850, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 23 - new BezierLine( - new Point(11.364, 130.850, Point.CARTESIAN), - new Point(58.931, 128.577, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 24 - new BezierLine( - new Point(58.931, 128.577, Point.CARTESIAN), - new Point(58.769, 133.123, Point.CARTESIAN) - ) - ) - - .addPath( - // Line 25 - new BezierLine( - new Point(58.769, 133.123, Point.CARTESIAN), - new Point(13.475, 133.935, Point.CARTESIAN) - ) - ).build(); - ; - } - - - @Override - public void loop() { - robot.update(); - if (robot.atParametricEnd()) - robot.followPath(path); - robot.telemetryDebug(telemetry); - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/bRedAutoV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/bRedAutoV1.java deleted file mode 100644 index d785128..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/bRedAutoV1.java +++ /dev/null @@ -1,174 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots.projects; - -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; - -public class bRedAutoV1 { - - public bRedAutoV1() { - PathBuilder builder = new PathBuilder(); - - builder - .addPath( - // Line 1 - new BezierLine( - new Point(133.935, 83.770, Point.CARTESIAN), - new Point(79.874, 117.213, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 2 - new BezierLine( - new Point(79.874, 117.213, Point.CARTESIAN), - new Point(79.874, 120.785, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 3 - new BezierLine( - new Point(79.874, 120.785, Point.CARTESIAN), - new Point(131.824, 118.349, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 4 - new BezierLine( - new Point(131.824, 118.349, Point.CARTESIAN), - new Point(79.549, 120.460, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 5 - new BezierLine( - new Point(79.549, 120.460, Point.CARTESIAN), - new Point(79.549, 128.740, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 6 - new BezierLine( - new Point(79.549, 128.740, Point.CARTESIAN), - new Point(131.337, 128.090, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 7 - new BezierLine( - new Point(131.337, 128.090, Point.CARTESIAN), - new Point(79.549, 128.740, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 8 - new BezierLine( - new Point(79.549, 128.740, Point.CARTESIAN), - new Point(79.549, 133.610, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 9 - new BezierLine( - new Point(79.549, 133.610, Point.CARTESIAN), - new Point(130.850, 133.285, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 10 - new BezierLine( - new Point(130.850, 133.285, Point.CARTESIAN), - new Point(130.201, 36.528, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 11 - new BezierLine( - new Point(130.201, 36.528, Point.CARTESIAN), - new Point(84.095, 36.203, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 12 - new BezierLine( - new Point(84.095, 36.203, Point.CARTESIAN), - new Point(84.095, 23.378, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 13 - new BezierLine( - new Point(84.095, 23.378, Point.CARTESIAN), - new Point(119.811, 23.378, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 14 - new BezierLine( - new Point(119.811, 23.378, Point.CARTESIAN), - new Point(127.603, 13.475, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 15 - new BezierLine( - new Point(127.603, 13.475, Point.CARTESIAN), - new Point(88.640, 28.248, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 16 - new BezierLine( - new Point(88.640, 28.248, Point.CARTESIAN), - new Point(87.666, 15.910, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 17 - new BezierLine( - new Point(87.666, 15.910, Point.CARTESIAN), - new Point(127.603, 12.014, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 18 - new BezierLine( - new Point(127.603, 12.014, Point.CARTESIAN), - new Point(86.692, 12.825, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 19 - new BezierLine( - new Point(86.692, 12.825, Point.CARTESIAN), - new Point(86.692, 10.390, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation() - .addPath( - // Line 20 - new BezierLine( - new Point(86.692, 10.390, Point.CARTESIAN), - new Point(126.467, 9.903, Point.CARTESIAN) - ) - ) - .setTangentHeadingInterpolation(); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java index c554097..ea558a7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java @@ -80,8 +80,8 @@ public class ThreeWheelLocalizer extends Localizer { */ public ThreeWheelLocalizer(HardwareMap map, Pose setStartPose) { // TODO: replace these with your encoder positions - leftEncoderPose = new Pose(0, 6.19375, 0); - rightEncoderPose = new Pose(0, -6.19375, 0); + leftEncoderPose = new Pose(0, 6.25, 0); + rightEncoderPose = new Pose(0, -6.25, 0); strafeEncoderPose = new Pose(-7, 0, Math.toRadians(90)); hardwareMap = map; @@ -92,9 +92,9 @@ public class ThreeWheelLocalizer extends Localizer { strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, BACK_ENCODER)); // TODO: reverse any encoders necessary - //leftEncoder.setDirection(Encoder.REVERSE); - // rightEncoder.setDirection(Encoder.REVERSE); - //strafeEncoder.setDirection(Encoder.FORWARD); + leftEncoder.setDirection(LEFT_ENCODER_DIRECTION); + rightEncoder.setDirection(RIGHT_ENCODER_DIRECTION); + strafeEncoder.setDirection(BACK_ENCODER_DIRECTION); setStartPose(setStartPose); timer = new NanoTimer(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java index 32e0452..45348c2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LocalizationTest.java @@ -1,5 +1,9 @@ package org.firstinspires.ftc.teamcode.pedroPathing.localization.tuning; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftFrontMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.leftRearMotorName; import static org.firstinspires.ftc.teamcode.pedroPathing.tuning.FollowerConstants.rightFrontMotorName; @@ -58,8 +62,10 @@ public class LocalizationTest extends OpMode { rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName); rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName); - leftFront.setDirection(DcMotorSimple.Direction.REVERSE); - leftRear.setDirection(DcMotorSimple.Direction.REVERSE); + leftFront.setDirection(FRONT_LEFT_MOTOR_DIRECTION); + leftRear.setDirection(BACK_LEFT_MOTOR_DIRECTION); + rightFront.setDirection(FRONT_RIGHT_MOTOR_DIRECTION); + rightRear.setDirection(BACK_RIGHT_MOTOR_DIRECTION); motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear);