From fb570d6be81da3ffbbddddcf2a0d5776ba75c60a Mon Sep 17 00:00:00 2001 From: Carlos Rivas Date: Fri, 13 Sep 2024 13:36:51 -0700 Subject: [PATCH] Static-ization of variables that are modified constantly --- .../ftc/teamcode/MecanumDrive.java | 48 +++++++++++-------- .../ftc/teamcode/MecanumDriveParameters.java | 22 +++++++++ 2 files changed, 49 insertions(+), 21 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDriveParameters.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index d3cb8be..8675eb2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode; +import static org.firstinspires.ftc.teamcode.MecanumDriveParameters.*; + import androidx.annotation.NonNull; import com.acmerobotics.dashboard.canvas.Canvas; @@ -63,14 +65,14 @@ public final class MecanumDrive { public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; // drive model parameters - public double inPerTick = 1; - public double lateralInPerTick = inPerTick; - public double trackWidthTicks = 0; + public double inPerTick = INCHES_PER_TICK_VALUE; + public double lateralInPerTick = LATERAL_INCHES_PER_TICK_VALUE; + public double trackWidthTicks = TRACK_WIDTH_IN_TICKS_VALUE; // feedforward parameters (in tick units) - public double kS = 0; - public double kV = 0; - public double kA = 0; + public double kS = KS_VALUE; + public double kV = KV_VALUE; + public double kA = KA_VALUE; // path profile parameters (in inches) public double maxWheelVel = 50; @@ -94,15 +96,18 @@ public final class MecanumDrive { public static Params PARAMS = new Params(); public final MecanumKinematics kinematics = new MecanumKinematics( - PARAMS.inPerTick * PARAMS.trackWidthTicks, PARAMS.inPerTick / PARAMS.lateralInPerTick); + PARAMS.inPerTick * PARAMS.trackWidthTicks, + PARAMS.inPerTick / PARAMS.lateralInPerTick); public final TurnConstraints defaultTurnConstraints = new TurnConstraints( PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel); + public final VelConstraint defaultVelConstraint = new MinVelConstraint(Arrays.asList( kinematics.new WheelVelConstraint(PARAMS.maxWheelVel), new AngularVelConstraint(PARAMS.maxAngVel) )); + public final AccelConstraint defaultAccelConstraint = new ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel); @@ -135,11 +140,7 @@ public final class MecanumDrive { leftBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftBack)); rightBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightBack)); rightFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightFront)); - imu = lazyImu.get(); - - // TODO: reverse encoders if needed - // leftFront.setDirection(DcMotorSimple.Direction.REVERSE); } @Override @@ -215,23 +216,28 @@ public final class MecanumDrive { module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); } - // TODO: make sure your config has motors with these names (or change them) - // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html - leftFront = hardwareMap.get(DcMotorEx.class, "Drive front lt"); - leftBack = hardwareMap.get(DcMotorEx.class, "Drive back lt"); - rightBack = hardwareMap.get(DcMotorEx.class, "Drive back rt"); - rightFront = hardwareMap.get(DcMotorEx.class, "Drive front rt"); + /* + Configure motors definitions + */ + leftFront = hardwareMap.get(DcMotorEx.class, FRONT_LEFT_MOTOR); + leftBack = hardwareMap.get(DcMotorEx.class, BACK_LEFT_MOTOR); + rightFront = hardwareMap.get(DcMotorEx.class, FRONT_RIGHT_MOTOR); + rightBack = hardwareMap.get(DcMotorEx.class, BACK_RIGHT_MOTOR); leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - // TODO: reverse motor directions if needed - // leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + /* + Configure motors direction + */ + //leftFront.setDirection(DcMotorSimple.Direction.REVERSE); - // TODO: make sure your config has an IMU with this name (can be BNO or BHI) - // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html + /* + IMU Orientation Application + See https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html + */ lazyImu = new LazyImu(hardwareMap, "imu", new RevHubOrientationOnRobot( PARAMS.logoFacingDirection, PARAMS.usbFacingDirection)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDriveParameters.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDriveParameters.java new file mode 100644 index 0000000..cf20752 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDriveParameters.java @@ -0,0 +1,22 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.hardware.DcMotorEx; + +public class MecanumDriveParameters { + + // Roadrunner tuning parameters + public static final double INCHES_PER_TICK_VALUE = 125.5/42126.8333333; + public static final double LATERAL_INCHES_PER_TICK_VALUE = 0.0026542073080477006; + public static final double TRACK_WIDTH_IN_TICKS_VALUE = 4012.7667474312616; + + // FeedForward parameters (in tick units) + public static final double KS_VALUE = 0.7579547290234911; + public static final double KV_VALUE = 0.0005724562271687877; + public static final double KA_VALUE = 0; + + // 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"; +}