diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExample.java new file mode 100644 index 0000000..b9108c5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExample.java @@ -0,0 +1,122 @@ +package org.firstinspires.ftc.teamcode; + +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR; + +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 com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +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", group = "Autonomous Pathing Tuning") +public class AutoExample extends OpMode { + private Telemetry telemetryA; + + private Follower follower; + + private PathChain path; + + private final Pose startPose = new Pose(12.0, 11, 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(.6); + + follower.setStartingPose(startPose); + + path = follower.pathBuilder() + .addPath( + // Line 1 + new BezierLine( + new Point(12.804, 11.223, Point.CARTESIAN), + new Point(12.804, 42.362, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 2 + new BezierCurve( + new Point(12.804, 42.362, Point.CARTESIAN), + new Point(11.381, 57.379, Point.CARTESIAN), + new Point(31.614, 56.588, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 3 + new BezierLine( + new Point(31.614, 56.588, Point.CARTESIAN), + new Point(51.214, 56.746, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 4 + new BezierCurve( + new Point(51.214, 56.746, Point.CARTESIAN), + new Point(64.334, 58.643, Point.CARTESIAN), + new Point(61.172, 45.524, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 5 + new BezierCurve( + new Point(61.172, 45.524, Point.CARTESIAN), + new Point(36.198, 26.239, Point.CARTESIAN), + new Point(19.759, 11.065, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .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/AutoExampleTwo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleTwo.java new file mode 100644 index 0000000..b905cd3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoExampleTwo.java @@ -0,0 +1,95 @@ +package org.firstinspires.ftc.teamcode; + +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 = "AutoExampleTwo", 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, 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() + .addPath( + // Line 1 + new BezierLine( + new Point(10.000, 40.000, Point.CARTESIAN), + new Point(60.000, 40.000, Point.CARTESIAN) + ) + ) + .setConstantHeadingInterpolation(Math.toRadians(90)) + .addPath( + // Line 2 + new BezierLine( + new Point(60.000, 40.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(10.000, 25.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/BasicOmniOpMode_Linear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOmniOpMode_Linear.java index 835606a..18aa94e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOmniOpMode_Linear.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOmniOpMode_Linear.java @@ -85,29 +85,24 @@ import org.firstinspires.ftc.teamcode.pedroPathing.localization.Encoder; public class BasicOmniOpMode_Linear extends LinearOpMode { // Declare OpMode members for each of the 4 motors. - private ElapsedTime runtime = new ElapsedTime(); - private DcMotor leftFrontDrive = null; - private DcMotor leftBackDrive = null; - private DcMotor rightFrontDrive = null; - private DcMotor rightBackDrive = null; - private Encoder leftEncoder; - private Encoder rightEncoder; - private Encoder strafeEncoder; + 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. - leftFrontDrive = hardwareMap.get(DcMotor.class, FRONT_LEFT_MOTOR); - leftBackDrive = hardwareMap.get(DcMotor.class, BACK_LEFT_MOTOR); - rightFrontDrive = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR); - rightBackDrive = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR); + 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 - 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)); + 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); 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 9b8ebb8..2d1c4da 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java @@ -58,15 +58,15 @@ public class PedroConstants { public static final double ROBOT_SPEED_LATERAL = 28.7119; // Rate of deceleration when power is cut-off when the robot is moving forward - public static final double FORWARD_ZERO_POWER_ACCEL = -61.125; + public static final double FORWARD_ZERO_POWER_ACCEL = -57.805; // Rate of deceleration when power is cut-off when the robot is moving to the right - public static final double LATERAL_ZERO_POWER_ACCEL = -82.579633333; + public static final double LATERAL_ZERO_POWER_ACCEL = -99.672; // Determines how fast your robot will decelerate as a factor of how fast your robot will coast to a stop - public static final double ZERO_POWER_ACCEL_MULT = 4; + public static final double ZERO_POWER_ACCEL_MULT = 3.5; /* Centripetal force correction - increase if robot is correcting into the path - decrease if robot is correcting away from the path */ - public static final double CENTRIPETAL_SCALING = 0.0005; + public static final double CENTRIPETAL_SCALING = 0.0004; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java index 3576f9d..fe5b223 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java @@ -71,6 +71,7 @@ public class PoseUpdater { public PoseUpdater(HardwareMap hardwareMap) { // TODO: replace the second argument with your preferred localizer this(hardwareMap, new ThreeWheelLocalizer(hardwareMap)); +// this(hardwareMap, new ThreeWheelIMULocalizer(hardwareMap)); } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java index f3d42f3..d82de3f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/tuning/LateralTuner.java @@ -63,6 +63,7 @@ public class LateralTuner extends OpMode { telemetryA.addData("distance moved", poseUpdater.getPose().getY()); telemetryA.addLine("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); telemetryA.addData("multiplier", DISTANCE / (poseUpdater.getPose().getY() / poseUpdater.getLocalizer().getLateralMultiplier())); + telemetryA.update(); Drawing.drawPoseHistory(dashboardPoseTracker, "#4CAF50"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java index 3cafa4a..766d629 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java @@ -58,10 +58,10 @@ public class FollowerConstants { // Heading error PIDF coefficients public static CustomPIDFCoefficients headingPIDFCoefficients = new CustomPIDFCoefficients( - 1, + 2, 0, - 0.02, - 0); + .075, + -.03125); // Feed forward constant added on to the heading PIDF public static double headingPIDFFeedForward = 0.01; @@ -69,10 +69,10 @@ public class FollowerConstants { // Drive PIDF coefficients public static CustomFilteredPIDFCoefficients drivePIDFCoefficients = new CustomFilteredPIDFCoefficients( - 0.025, + 0.006, 0, 0.00001, - 0.6, + 0.8, 0); // Feed forward constant added on to the drive PIDF diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java index 1fab8ae..2c27a1d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/LateralZeroPowerAccelerationTuner.java @@ -119,6 +119,13 @@ public class LateralZeroPowerAccelerationTuner extends OpMode { */ @Override public void loop() { + + telemetry.addData("x",poseUpdater.getPose().getX()); + telemetry.addData("y",poseUpdater.getPose().getY()); + telemetry.addData("heading",poseUpdater.getPose().getHeading()); + telemetry.addData("velo mag", poseUpdater.getVelocity().getMagnitude()); + telemetry.addData("velo theta", poseUpdater.getVelocity().getTheta()); + if (gamepad1.cross || gamepad1.a) { requestOpModeStop(); }