Merge pull request #1 from ftc13100/roadrunner

Complete Roadrunner Tuning

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/AutomaticFeedforwardTuner.kt

@@ -26,7 +26,7 @@ import org.firstinspires.ftc.teamcode.utils.roadrunner.util.RegressionUtil
  *   4. Adjust the encoder data based on the velocity tuning data and find kA with another linear
  *      regression.
  */
-@Disabled
+//@Disabled
 @Config
 @Autonomous(group = "drive")
 class AutomaticFeedforwardTuner : LinearOpMode() {

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/BackAndForth.kt

@@ -23,7 +23,7 @@ import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
  * This opmode is designed as a convenient, coarse tuning for the follower PID coefficients. It
  * is recommended that you use the FollowerPIDTuner opmode for further fine tuning.
  */
-@Disabled
+//@Disabled
 @Config
 @Autonomous(group = "drive")
 class BackAndForth : LinearOpMode() {

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/DriveVelocityPIDTuner.kt

@@ -40,7 +40,7 @@ import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants
  * user to reset the position of the bot in the event that it drifts off the path.
  * Pressing B/O (Xbox/PS4) will cede control back to the tuning process.
  */
-@Disabled
+//@Disabled
 @Config
 @Autonomous(group = "drive")
 class DriveVelocityPIDTuner : LinearOpMode() {

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/FollowerPIDTuner.kt

@@ -20,7 +20,7 @@ import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
  * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID.
  * These coefficients can be tuned live in dashboard.
  */
-@Disabled
+//@Disabled
 @Config
 @Autonomous(group = "drive")
 class FollowerPIDTuner : LinearOpMode() {

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/LocalizationTest.kt

@@ -14,7 +14,7 @@ import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
  * exercise is to ascertain whether the localizer has been configured properly (note: the pure
  * encoder localizer heading may be significantly off if the track width has not been tuned).
  */
-@Disabled
+//@Disabled
 @TeleOp(group = "drive")
 class LocalizationTest : LinearOpMode() {
     @Throws(InterruptedException::class)

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/ManualFeedforwardTuner.kt

@@ -36,7 +36,7 @@ import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.kV
  * user to reset the position of the bot in the event that it drifts off the path.
  * Pressing B/O (Xbox/PS4) will cede control back to the tuning process.
  */
-@Disabled
+//@Disabled
 @Config
 @Autonomous(group = "drive")
 class ManualFeedforwardTuner : LinearOpMode() {
@@ -121,7 +121,7 @@ class ManualFeedforwardTuner : LinearOpMode() {
     }
 
     companion object {
-        private var DISTANCE = 72.0 // in
+        private var DISTANCE = 110.0 // in
         private fun generateProfile(movingForward: Boolean): MotionProfile {
             val start = MotionState(if (movingForward) 0.0 else DISTANCE, 0.0, 0.0, 0.0)
             val goal = MotionState(if (movingForward) DISTANCE else 0.0, 0.0, 0.0, 0.0)

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxAngularVeloTuner.kt

@@ -20,7 +20,7 @@ import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
  *
  * Further fine tuning of MAX_ANG_VEL may be desired.
  */
-@Disabled
+//@Disabled
 @Config
 @Autonomous(group = "drive")
 class MaxAngularVeloTuner : LinearOpMode() {

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxVelocityTuner.kt

@@ -23,7 +23,7 @@ import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.getM
  *
  * Further fine tuning of kF may be desired.
  */
-@Disabled //@Config
+//@Disabled //@Config
 @Autonomous(group = "drive")
 class MaxVelocityTuner : LinearOpMode() {
     private lateinit var timer: ElapsedTime

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MotorDirectionDebugger.kt

@@ -30,7 +30,7 @@ import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
  * \________/
  *
  */
-@Disabled
+//@Disabled
 @Config
 @TeleOp(group = "drive")
 class MotorDirectionDebugger : LinearOpMode() {

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/SplineTest.kt

@@ -10,7 +10,7 @@ import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
 /*
  * This is an example of a more complex path to really test the tuning.
  */
-@Disabled
+//@Disabled
 @Autonomous(group = "drive")
 class SplineTest : LinearOpMode() {
     @Throws(InterruptedException::class)

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/StrafeTest.kt

@@ -12,7 +12,7 @@ import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
 /*
  * This is a simple routine to test translational drive capabilities.
  */
-@Disabled
+//@Disabled
 @Config
 @Autonomous(group = "drive")
 class StrafeTest : LinearOpMode() {

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/StraightTest.kt

@@ -12,7 +12,7 @@ import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
 /*
  * This is a simple routine to test translational drive capabilities.
  */
-@Disabled
+//@Disabled
 @Config
 @Autonomous(group = "drive")
 class StraightTest : LinearOpMode() {

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackWidthTuner.kt

@@ -22,7 +22,7 @@ import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants
  * this procedure a few times and averages the values for additional accuracy. Note: a relatively
  * accurate track width estimate is important or else the angular constraints will be thrown off.
  */
-@Disabled
+//@Disabled
 @Config
 @Autonomous(group = "drive")
 class TrackWidthTuner : LinearOpMode() {

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackingWheelForwardOffsetTuner.kt

@@ -34,7 +34,7 @@ import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.StandardTrackingWhe
  * for the forward offset. You can run this procedure as many times as necessary until a
  * satisfactory result is produced.
  */
-@Disabled
+//@Disabled
 @Config
 @Autonomous(group = "drive")
 class TrackingWheelForwardOffsetTuner : LinearOpMode() {

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackingWheelLateralDistanceTuner.kt

@@ -73,7 +73,7 @@ import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.StandardTrackingWhe
  * slightly but your heading will still be fine. This does not affect your overall tracking
  * precision. The heading should still line up.
  */
-@Disabled
+//@Disabled
 @Config
 @TeleOp(group = "drive")
 class TrackingWheelLateralDistanceTuner : LinearOpMode() {

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TurnTest.kt

@@ -9,7 +9,7 @@ import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
 /*
  * This is a simple routine to test turning capabilities.
  */
-@Disabled
+//@Disabled
 @Config
 @Autonomous(group = "drive")
 class TurnTest : LinearOpMode() {

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/drive/DriveSubsystem.kt

@@ -275,7 +275,7 @@ class DriveSubsystem @JvmOverloads constructor(
     companion object {
         var TRANSLATIONAL_PID = PIDCoefficients(8.0, 0.0, 0.0)
         var HEADING_PID = PIDCoefficients(8.0, 0.0, 0.0)
-        var LATERAL_MULTIPLIER = 1.0
+        var LATERAL_MULTIPLIER = 1.0126582278481012658227848101266
         var VX_WEIGHT = 1.0
         var VY_WEIGHT = 1.0
         var OMEGA_WEIGHT = 1.0

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/DriveConstants.kt

@@ -47,18 +47,22 @@ object DriveConstants {
      * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO.
      */
     var WHEEL_RADIUS = 1.88976 // in`
-    var GEAR_RATIO = 0.5 // output (wheel) speed / input (motor) speed
-    var TRACK_WIDTH = 13.1 // in
+    var GEAR_RATIO = 1.0 // output (wheel) speed / input (motor) speed
+    @JvmField
+    var TRACK_WIDTH = 13.94 // in
 
     /*
      * These are the feedforward parameters used to model the drive motor behavior. If you are using
      * the built-in velocity PID, *these values are fine as is*. However, if you do not have drive
      * motor encoders or have elected not to use them for velocity control, these values should be
      * empirically tuned.
      */
-    var kV = 0.024
-    var kA = 0.003
-    var kStatic = 0.01
+    @JvmField
+    var kV = 0.0144
+    @JvmField
+    var kA = 0.0015
+    @JvmField
+    var kStatic = 0.012
 
     /*
      * These values are used to generate the trajectories for you robot. To ensure proper operation,
@@ -73,25 +77,29 @@ object DriveConstants {
      * ((MAX_RPM / 60) * GEAR_RATIO * WHEEL_RADIUS * 2 * Math.PI) * 0.85
      * Resulting in 73.17330064499293 in/s.
      * This is only 85% of the theoretical maximum velocity of the bot, following the recommendation above.
-     * This is capped at 85% because there are a number of variables that will prevent your bot from actually
+     * This is capped at 85% because there are a number of variables that will preve    nt your bot from actually
      * reaching this maximum velocity: voltage dropping over the game, bot weight, general mechanical inefficiencies, etc.
-     * However, you can push this higher yourself if you'd like. Perhaps raise it to 90-95% of the theoretically 
+     * However, you can push this higher yourself if you'd like. Perhaps raise it to 90-95% of the theoretically
      * max velocity. The theoretically maximum velocity is 86.08623605293286 in/s.
      * Just make sure that your bot can actually reach this maximum velocity. Path following will be detrimentally
      * affected if it is aiming for a velocity not actually possible.
-     * 
+     *
      * The maximum acceleration is somewhat arbitrary and it is recommended that you tweak this yourself based on
      * actual testing. Just set it at a reasonable value and keep increasing until your path following starts
      * to degrade. As of now, it simply mirrors the velocity, resulting in 73.17330064499293 in/s/s
      *
      * Maximum Angular Velocity is calculated as: maximum velocity / trackWidth * (180 / Math.PI) but capped at 360°/s.
      * You are free to raise this on your own if you would like. It is best determined through experimentation.
-     
+
      */
-    var MAX_VEL = 38.110287416570166
-    var MAX_ACCEL = 38.110287416570166
-    var MAX_ANG_VEL = Math.toRadians(355.94452299662714)
-    var MAX_ANG_ACCEL = Math.toRadians(172.2983035714285)
+    @JvmField
+    var MAX_VEL = 30.0
+    @JvmField
+    var MAX_ACCEL = 30.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
     }

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/StandardTrackingWheelLocalizer.kt

@@ -10,6 +10,7 @@ import org.firstinspires.ftc.teamcode.utils.roadrunner.util.Encoder
 
 /*
  * Sample tracking wheel localizer implementation assuming the standard configuration:
+
  *
  *    /--------------\
  *    |     ____     |
@@ -38,7 +39,9 @@ class StandardTrackingWheelLocalizer(hardwareMap: HardwareMap) : ThreeTrackingWh
             hardwareMap.get(
                 DcMotorEx::class.java, ControlBoard.ODO_LEFT_ENCODER.deviceName
             )
-        )
+        )   
+
+
         rightEncoder = Encoder(
             hardwareMap.get(
                 DcMotorEx::class.java, ControlBoard.ODO_RIGHT_ENCODER.deviceName
@@ -49,6 +52,7 @@ class StandardTrackingWheelLocalizer(hardwareMap: HardwareMap) : ThreeTrackingWh
                 DcMotorEx::class.java, ControlBoard.ODO_STRAFE_ENCODER.deviceName
             )
         )
+//        strafeEncoder.direction = Encoder.Direction.REVERSE
     }
 
     override fun getWheelPositions() = listOf(
@@ -64,18 +68,20 @@ class StandardTrackingWheelLocalizer(hardwareMap: HardwareMap) : ThreeTrackingWh
     )
 
     companion object {
-        val TICKS_PER_REV = 8192.0
-        val WHEEL_RADIUS = 0.6889764 // in
+        val TICKS_PER_REV = 2000.0
+        val WHEEL_RADIUS = 0.944882 // in
         val GEAR_RATIO = 1.0 // output (wheel) speed / input (encoder) speed
 
         @JvmField
-        var LATERAL_DISTANCE = 12.3206682876705018 // 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 = 1.28125 // in; offset of the lateral wheel
+        var FORWARD_OFFSET = -2.1 // in; offset of the lateral wheel
 
-        var X_MULTIPLIER = 1.10506990189 // Multiplier in the X direction
-        var Y_MULTIPLIER = 1.09955932763 // Multiplier in the Y direction
+        @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
         }

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/Encoder.kt

@@ -19,7 +19,7 @@ class Encoder @JvmOverloads constructor(
      *
      * @param direction either reverse or forward depending on if encoder counts should be negated
      */
-    private var direction: Direction
+    var direction: Direction
     private var lastPosition = 0
     private var velocityEstimate = 0.0
     private var lastUpdateTime: Double