From 8799bd048b70adf34870155781443bb945650889 Mon Sep 17 00:00:00 2001 From: arunaassija Date: Sun, 17 Nov 2024 10:42:07 -0500 Subject: [PATCH] roadrunner finished --- .../utils/roadrunner/drive/DriveConstants.kt | 14 +++++++++----- .../drive/StandardTrackingWheelLocalizer.kt | 4 +++- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/DriveConstants.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/DriveConstants.kt index 0629efb..1783a88 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/DriveConstants.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/DriveConstants.kt @@ -47,7 +47,7 @@ object DriveConstants { var WHEEL_RADIUS = 1.88976 // in` var GEAR_RATIO = 1.0 // output (wheel) speed / input (motor) speed @JvmField - var TRACK_WIDTH = 15.5 // in + var TRACK_WIDTH = 13.94 // in /* * These are the feedforward parameters used to model the drive motor behavior. If you are using @@ -56,11 +56,11 @@ object DriveConstants { * empirically tuned. */ @JvmField - var kV = 0.0123 + var kV = 0.0144 @JvmField - var kA = 0.003 + var kA = 0.0015 @JvmField - var kStatic = 0.01 + var kStatic = 0.012 /* * These values are used to generate the trajectories for you robot. To ensure proper operation, @@ -90,9 +90,13 @@ object DriveConstants { * You are free to raise this on your own if you would like. It is best determined through experimentation. */ + @JvmField var MAX_VEL = 30.0 + @JvmField var MAX_ACCEL = 30.0 - var MAX_ANG_VEL = Math.toRadians(180.0) + @JvmField + var MAX_ANG_VEL = Math.toRadians(170.0) + @JvmField var MAX_ANG_ACCEL = Math.toRadians(170.0) fun encoderTicksToInches(ticks: Double): Double { return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/StandardTrackingWheelLocalizer.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/StandardTrackingWheelLocalizer.kt index 54b99fb..5d4cb68 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/StandardTrackingWheelLocalizer.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/StandardTrackingWheelLocalizer.kt @@ -73,12 +73,14 @@ class StandardTrackingWheelLocalizer(hardwareMap: HardwareMap) : ThreeTrackingWh val GEAR_RATIO = 1.0 // output (wheel) speed / input (encoder) speed @JvmField - var LATERAL_DISTANCE = 15.153260857569071 // in; distance between the left and right wheels + var LATERAL_DISTANCE = 15.45 // in; distance between the left and right wheels @JvmField var FORWARD_OFFSET = -2.1 // in; offset of the lateral wheel + @JvmField var X_MULTIPLIER = 0.9944039016245 // Multiplier in the X direction + @JvmField var Y_MULTIPLIER = 0.99960646480333// Multiplier in the Y direction fun encoderTicksToInches(ticks: Double): Double { return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV