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 d175a03..6733a94 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java @@ -63,19 +63,19 @@ public class PedroConstants { public static final double ROBOT_WEIGHT_IN_KG = 9; // Maximum velocity of the robot going forward - public static final double ROBOT_SPEED_FORWARD = 51.5; + public static final double ROBOT_SPEED_FORWARD = 53.223; // Maximum velocity of the robot going right - public static final double ROBOT_SPEED_LATERAL = 28.7; + public static final double ROBOT_SPEED_LATERAL = 41.4081; // Rate of deceleration when power is cut-off when the robot is moving forward - public static final double FORWARD_ZERO_POWER_ACCEL = -59.8; + public static final double FORWARD_ZERO_POWER_ACCEL = -76.8421; // Rate of deceleration when power is cut-off when the robot is moving to the right - public static final double LATERAL_ZERO_POWER_ACCEL = -99.7; + public static final double LATERAL_ZERO_POWER_ACCEL = -93.4183; // 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 = 3.5; + public static final double ZERO_POWER_ACCEL_MULT = 4.0; /* Centripetal force correction - increase if robot is correcting into the path - decrease if robot is correcting away from the path */ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BlueNonBasketAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BlueNonBasketAuto.java deleted file mode 100644 index 15fa80a..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BlueNonBasketAuto.java +++ /dev/null @@ -1,151 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots; - -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 = "BlueNonBasketAuto", group = "Autonomous Pathing Tuning") -public class BlueNonBasketAuto extends OpMode { - private Telemetry telemetryA; - - private Follower follower; - - private PathChain path; - - private final Pose startPose = new Pose(10.929, 55.446, 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(10.929, 55.446, Point.CARTESIAN), - new Point(42.429, 46.446, Point.CARTESIAN), - new Point(36.321, 38.089, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 2 - new BezierLine( - new Point(36.321, 38.089, Point.CARTESIAN), - new Point(59.786, 36.643, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 3 - new BezierLine( - new Point(59.786, 36.643, Point.CARTESIAN), - new Point(59.304, 24.750, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 4 - new BezierLine( - new Point(59.304, 24.750, Point.CARTESIAN), - new Point(13.982, 23.946, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 5 - new BezierLine( - new Point(13.982, 23.946, Point.CARTESIAN), - new Point(59.464, 24.429, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 6 - new BezierLine( - new Point(59.464, 24.429, Point.CARTESIAN), - new Point(58.982, 15.268, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 7 - new BezierLine( - new Point(58.982, 15.268, Point.CARTESIAN), - new Point(13.821, 14.464, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 8 - new BezierLine( - new Point(13.821, 14.464, Point.CARTESIAN), - new Point(58.661, 13.500, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 9 - new BezierLine( - new Point(58.661, 13.500, Point.CARTESIAN), - new Point(58.339, 8.679, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)) - .addPath( - // Line 10 - new BezierLine( - new Point(58.339, 8.679, Point.CARTESIAN), - new Point(14.625, 8.518, Point.CARTESIAN) - ) - ) - .setConstantHeadingInterpolation(Math.toRadians(0)).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/pedroPathing/localization/localizers/ThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java index ea558a7..4b149f7 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 @@ -57,9 +57,9 @@ public class ThreeWheelLocalizer extends Localizer { private Pose rightEncoderPose; private Pose strafeEncoderPose; private double totalHeading; - public static double FORWARD_TICKS_TO_INCHES = 0.0029;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963; - public static double STRAFE_TICKS_TO_INCHES = 0.0029;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659; - public static double TURN_TICKS_TO_RADIANS = 0.003;//8192 * 1.37795 * 2 * Math.PI * 0.5; + public static double FORWARD_TICKS_TO_INCHES = 0.003;//8192 * 1.37795 * 2 * Math.PI * 0.5008239963; + public static double STRAFE_TICKS_TO_INCHES = -0.003;//8192 * 1.37795 * 2 * Math.PI * 0.5018874659; + public static double TURN_TICKS_TO_RADIANS = 0.0029;//8192 * 1.37795 * 2 * Math.PI * 0.5; /** * This creates a new ThreeWheelLocalizer from a HardwareMap, with a starting Pose at (0,0) @@ -80,9 +80,9 @@ public class ThreeWheelLocalizer extends Localizer { */ public ThreeWheelLocalizer(HardwareMap map, Pose setStartPose) { // TODO: replace these with your encoder positions - leftEncoderPose = new Pose(0, 6.25, 0); - rightEncoderPose = new Pose(0, -6.25, 0); - strafeEncoderPose = new Pose(-7, 0, Math.toRadians(90)); + leftEncoderPose = new Pose(0.25, 6.25, 0); + rightEncoderPose = new Pose(0.25, -6.25, 0); + strafeEncoderPose = new Pose(-7, 0.25, Math.toRadians(90)); hardwareMap = map; 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 d0ab034..59a64f7 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 @@ -40,9 +40,9 @@ public class FollowerConstants { // Translational PIDF coefficients (don't use integral) public static CustomPIDFCoefficients translationalPIDFCoefficients = new CustomPIDFCoefficients( - 0.1, + .25, 0, - 0.01, + 0.0375, 0); // Translational Integral @@ -60,7 +60,7 @@ public class FollowerConstants { public static CustomPIDFCoefficients headingPIDFCoefficients = new CustomPIDFCoefficients( 2, 0, - 0.025, + 0.0375, 0); // Feed forward constant added on to the heading PIDF @@ -69,9 +69,9 @@ public class FollowerConstants { // Drive PIDF coefficients public static CustomFilteredPIDFCoefficients drivePIDFCoefficients = new CustomFilteredPIDFCoefficients( - 0.006, + 0.00375, 0, - 0.00001, + 0.00003, 0.8, 0);