From bfab6dc7d08eb5ba7dfa86610a2b11e7e9c687b8 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Fri, 17 May 2024 10:04:28 -0400 Subject: [PATCH] more fixes --- .../localization/DriveEncoderLocalizer.java | 1 - .../localization/tuning/LateralTuner.java | 2 +- .../tuning/FollowerConstants.java | 48 +++++++++---------- .../LateralZeroPowerAccelerationTuner.java | 15 ++++++ 4 files changed, 39 insertions(+), 27 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java index 15501cc..44e04a5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/DriveEncoderLocalizer.java @@ -53,7 +53,6 @@ public class DriveEncoderLocalizer extends Localizer { // todo: make drive encod * @param setStartPose the Pose to start from */ public DriveEncoderLocalizer(HardwareMap map, Pose setStartPose) { - hardwareMap = map; // TODO: replace these with your encoder ports 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 282577d..f3d42f3 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 @@ -62,7 +62,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().getX() / poseUpdater.getLocalizer().getLateralMultiplier())); + 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 a236b59..b6aada6 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 @@ -27,29 +27,6 @@ public class FollowerConstants { private static double[] convertToPolar = Point.cartesianToPolar(xMovement, -yMovement); public static Vector frontLeftVector = MathFunctions.normalizeVector(new Vector(convertToPolar[0],convertToPolar[1])); - // Large heading error PIDF coefficients - public static CustomPIDFCoefficients largeHeadingPIDFCoefficients = new CustomPIDFCoefficients( - 1, - 0, - 0, - 0); - - // Feed forward constant added on to the large heading PIDF - public static double largeHeadingPIDFFeedForward = 0.01; - - // the limit at which the heading PIDF switches between the large and small heading PIDFs - public static double headingPIDFSwitch = Math.PI/20; - - // Small heading error PIDF coefficients - public static CustomPIDFCoefficients smallHeadingPIDFCoefficients = new CustomPIDFCoefficients( - 5, - 0, - 0.08, - 0); - - // Feed forward constant added on to the small heading PIDF - public static double smallHeadingPIDFFeedForward = 0.01; - // Large translational PIDF coefficients public static CustomPIDFCoefficients largeTranslationalPIDFCoefficients = new CustomPIDFCoefficients( 0.1, @@ -87,6 +64,29 @@ public class FollowerConstants { // Feed forward constant added on to the small translational PIDF public static double smallTranslationalPIDFFeedForward = 0.015; + // Large heading error PIDF coefficients + public static CustomPIDFCoefficients largeHeadingPIDFCoefficients = new CustomPIDFCoefficients( + 1, + 0, + 0, + 0); + + // Feed forward constant added on to the large heading PIDF + public static double largeHeadingPIDFFeedForward = 0.01; + + // the limit at which the heading PIDF switches between the large and small heading PIDFs + public static double headingPIDFSwitch = Math.PI/20; + + // Small heading error PIDF coefficients + public static CustomPIDFCoefficients smallHeadingPIDFCoefficients = new CustomPIDFCoefficients( + 5, + 0, + 0.08, + 0); + + // Feed forward constant added on to the small heading PIDF + public static double smallHeadingPIDFFeedForward = 0.01; + // Large drive PIDF coefficients public static CustomPIDFCoefficients largeDrivePIDFCoefficients = new CustomPIDFCoefficients( 0.025, @@ -118,12 +118,10 @@ public class FollowerConstants { // Acceleration of the drivetrain when power is cut in inches/second^2 (should be negative) // if not negative, then the robot thinks that its going to go faster under 0 power - // this is for curves public static double forwardZeroPowerAcceleration = -34.62719; // Acceleration of the drivetrain when power is cut in inches/second^2 (should be negative) // if not negative, then the robot thinks that its going to go faster under 0 power - // this is for curves public static double lateralZeroPowerAcceleration = -78.15554; // A multiplier for the zero power acceleration to change the speed the robot decelerates at 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 8692e1b..0c22b9b 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 @@ -19,6 +19,21 @@ import java.util.ArrayList; import java.util.Arrays; import java.util.List; +/** + * This is the LateralZeroPowerAccelerationTuner autonomous tuning OpMode. This runs the robot + * to the right until a specified velocity is achieved. Then, the robot cuts power to the motors, setting + * them to zero power. The deceleration, or negative acceleration, is then measured until the robot + * stops. The accelerations across the entire time the robot is slowing down is then averaged and + * that number is then printed. This is used to determine how the robot will decelerate in the + * forward direction when power is cut, making the estimations used in the calculations for the + * drive Vector more accurate and giving better braking at the end of Paths. + * You can adjust the max velocity the robot will hit on FTC Dashboard: 192/168/43/1:8080/dash + * + * @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/13/2024 + */ @Config @Autonomous (name = "Lateral Zero Power Acceleration Tuner", group = "Autonomous Pathing Tuning") public class LateralZeroPowerAccelerationTuner extends OpMode {