From d41f368fe81d870ee5d332960158af1aa38bbb79 Mon Sep 17 00:00:00 2001 From: Carlos Rivas Date: Fri, 13 Sep 2024 15:44:05 -0700 Subject: [PATCH] Extract values so it's easier to manipulate --- .../ftc/teamcode/PedrosConstants.java | 36 +++++++++++++++++++ .../ftc/teamcode/USEFUL_LINKS.md | 5 +++ .../tuning/FollowerConstants.java | 26 +++++++------- 3 files changed, 55 insertions(+), 12 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedrosConstants.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/USEFUL_LINKS.md diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedrosConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedrosConstants.java new file mode 100644 index 0000000..ede9a66 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedrosConstants.java @@ -0,0 +1,36 @@ +package org.firstinspires.ftc.teamcode; + +public class PedrosConstants { + + // Robot motor configurations + public static final String FRONT_LEFT_MOTOR = "Drive front lt"; + public static final String BACK_LEFT_MOTOR = "Drive back lt"; + public static final String FRONT_RIGHT_MOTOR = "Drive front rt"; + public static final String BACK_RIGHT_MOTOR = "Drive back rt"; + + /* + Pedro's parameters + */ + + // The weight of the robot in Kilograms + public static final double ROBOT_WEIGHT_IN_KG = 10.65942; + + // Maximum velocity of the robot going forward + public static final double ROBOT_SPEED_FORWARD = 81.34056; + + // Maximum velocity of the robot going right + public static final double ROBOT_SPEED_LATERAL = 65.43028; + + // Rate of deceleration when power is cut-off when the robot is moving forward + public static final double FORWARD_ZERO_POWER_ACCEL = -34.62719; + + // Rate of deceleration when power is cut-off when the robot is moving to the right + public static final double LATERAL_ZERO_POWER_ACCEL = -78.15554; + + // 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; + + /* 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; +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/USEFUL_LINKS.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/USEFUL_LINKS.md new file mode 100644 index 0000000..c160e02 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/USEFUL_LINKS.md @@ -0,0 +1,5 @@ +# Useful Links + +## Pedro Pathing + +- `https://pedro-path-generator.vercel.app/` \ No newline at end of file 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 af72609..3290dab 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 @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.pedroPathing.tuning; +import static org.firstinspires.ftc.teamcode.PedrosConstants.*; + import com.acmerobotics.dashboard.config.Config; import org.firstinspires.ftc.teamcode.pedroPathing.util.CustomFilteredPIDFCoefficients; @@ -23,16 +25,16 @@ import org.firstinspires.ftc.teamcode.pedroPathing.util.KalmanFilterParameters; public class FollowerConstants { // This section is for configuring your motors - public static String leftFrontMotorName = "leftFront"; - public static String leftRearMotorName = "leftRear"; - public static String rightFrontMotorName = "rightFront"; - public static String rightRearMotorName = "rightRear"; + public static String leftFrontMotorName = FRONT_LEFT_MOTOR; + public static String leftRearMotorName = BACK_LEFT_MOTOR; + public static String rightFrontMotorName = FRONT_RIGHT_MOTOR; + public static String rightRearMotorName = BACK_RIGHT_MOTOR; // This section is for setting the actual drive vector for the front left wheel, if the robot // is facing a heading of 0 radians with the wheel centered at (0,0) - private static double xMovement = 81.34056; - private static double yMovement = 65.43028; - private static double[] convertToPolar = Point.cartesianToPolar(xMovement, -yMovement); + private static final double xMovement = ROBOT_SPEED_FORWARD; + private static final double yMovement = ROBOT_SPEED_LATERAL; + private static final double[] convertToPolar = Point.cartesianToPolar(xMovement, -yMovement); public static Vector frontLeftVector = MathFunctions.normalizeVector(new Vector(convertToPolar[0],convertToPolar[1])); @@ -83,19 +85,19 @@ public class FollowerConstants { // Mass of robot in kilograms - public static double mass = 10.65942; + public static double mass = ROBOT_WEIGHT_IN_KG; // Centripetal force to power scaling - public static double centripetalScaling = 0.0005; + public static double centripetalScaling = CENTRIPETAL_SCALING; // 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 - public static double forwardZeroPowerAcceleration = -34.62719; + public static double forwardZeroPowerAcceleration = FORWARD_ZERO_POWER_ACCEL; // 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 - public static double lateralZeroPowerAcceleration = -78.15554; + public static double lateralZeroPowerAcceleration = LATERAL_ZERO_POWER_ACCEL; // A multiplier for the zero power acceleration to change the speed the robot decelerates at // the end of paths. @@ -104,7 +106,7 @@ public class FollowerConstants { // Decreasing this will cause the deceleration at the end of the Path to be slower, making the // robot slower but reducing risk of end-of-path overshoots or localization slippage. // This can be set individually for each Path, but this is the default. - public static double zeroPowerAccelerationMultiplier = 4; + public static double zeroPowerAccelerationMultiplier = ZERO_POWER_ACCEL_MULT; // When the robot is at the end of its current Path or PathChain and the velocity goes below