From acf2d9d32c865420101d899e27be2431b652d5c0 Mon Sep 17 00:00:00 2001
From: Sachet Korada <sachet.korada@gmail.com>
Date: Thu, 19 Dec 2024 20:08:31 -0500
Subject: [PATCH] Update to no tune version

---
 .../teamcode/commands/drive/DriveCommand.kt   |  2 +-
 .../roadrunner/ThreeDeadWheelLocalizer.kt     |  6 +-
 .../messages/DriveCommandMessage.kt           | 14 +--
 .../messages/MecanumCommandMessage.kt         | 12 +--
 .../messages/MecanumLocalizerInputsMessage.kt | 24 ++---
 .../roadrunner/messages/PoseMessage.kt        |  8 +-
 .../roadrunner/messages/TankCommandMessage.kt |  8 +-
 .../messages/TankLocalizerInputsMessage.kt    |  6 +-
 .../messages/ThreeDeadWheelInputsMessage.kt   |  8 +-
 .../messages/TwoDeadWheelInputsMessage.kt     | 31 ++----
 .../subsystems/drive/DriveSubsystem.kt        | 94 +++++++------------
 11 files changed, 83 insertions(+), 130 deletions(-)

diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/DriveCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/DriveCommand.kt
index e3f53ef..3b5915c 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/DriveCommand.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/DriveCommand.kt
@@ -17,7 +17,7 @@ class DriveCommand(
 
     override fun execute() {
         subsystem.drive(
-            leftY = zonedDrive(-leftY.invoke() * 0.9, zoneVal).pow(3),
+            leftY = zonedDrive(leftY.invoke() * 0.9, zoneVal).pow(3),
             leftX = zonedDrive(leftX.invoke() * 0.9, zoneVal).pow(3),
             rightX = zonedDrive(rightX.invoke() * 0.9, zoneVal).pow(3),
         )
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/ThreeDeadWheelLocalizer.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/ThreeDeadWheelLocalizer.kt
index 2cc13d1..e08700b 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/ThreeDeadWheelLocalizer.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/ThreeDeadWheelLocalizer.kt
@@ -17,9 +17,9 @@ import org.firstinspires.ftc.teamcode.roadrunner.messages.ThreeDeadWheelInputsMe
 @Config
 class ThreeDeadWheelLocalizer(hardwareMap: HardwareMap, val inPerTick: Double) : Localizer {
     class Params {
-        var par0YTicks: Double = 0.0 // y position of the first parallel encoder (in tick units)
-        var par1YTicks: Double = 1.0 // y position of the second parallel encoder (in tick units)
-        var perpXTicks: Double = 0.0 // x position of the perpendicular encoder (in tick units)
+        @JvmField var par0YTicks: Double = 0.0 // y position of the first parallel encoder (in tick units)
+        @JvmField var par1YTicks: Double = 1.0 // y position of the second parallel encoder (in tick units)
+        @JvmField var perpXTicks: Double = 0.0 // x position of the perpendicular encoder (in tick units)
     }
 
     // TODO: make sure your config has **motors** with these names (or change them)
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/DriveCommandMessage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/DriveCommandMessage.kt
index fabde81..73596b0 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/DriveCommandMessage.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/DriveCommandMessage.kt
@@ -4,11 +4,11 @@ import com.acmerobotics.roadrunner.PoseVelocity2dDual
 import com.acmerobotics.roadrunner.Time
 
 class DriveCommandMessage(poseVelocity: PoseVelocity2dDual<Time>) {
-    var timestamp: Long = System.nanoTime()
-    var forwardVelocity: Double = poseVelocity.linearVel.x[0]
-    var forwardAcceleration: Double = poseVelocity.linearVel.x[1]
-    var lateralVelocity: Double = poseVelocity.linearVel.y[0]
-    var lateralAcceleration: Double = poseVelocity.linearVel.y[1]
-    var angularVelocity: Double = poseVelocity.angVel[0]
-    var angularAcceleration: Double = poseVelocity.angVel[1]
+    @JvmField var timestamp: Long = System.nanoTime()
+    @JvmField var forwardVelocity: Double = poseVelocity.linearVel.x[0]
+    @JvmField var forwardAcceleration: Double = poseVelocity.linearVel.x[1]
+    @JvmField var lateralVelocity: Double = poseVelocity.linearVel.y[0]
+    @JvmField var lateralAcceleration: Double = poseVelocity.linearVel.y[1]
+    @JvmField var angularVelocity: Double = poseVelocity.angVel[0]
+    @JvmField var angularAcceleration: Double = poseVelocity.angVel[1]
 }
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumCommandMessage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumCommandMessage.kt
index edd0e42..1c1edda 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumCommandMessage.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumCommandMessage.kt
@@ -1,11 +1,11 @@
 package org.firstinspires.ftc.teamcode.roadrunner.messages
 
 class MecanumCommandMessage(
-    var voltage: Double,
-    var leftFrontPower: Double,
-    var leftBackPower: Double,
-    var rightBackPower: Double,
-    var rightFrontPower: Double
+    @JvmField var voltage: Double,
+    @JvmField var leftFrontPower: Double,
+    @JvmField var leftBackPower: Double,
+    @JvmField var rightBackPower: Double,
+    @JvmField var rightFrontPower: Double
 ) {
-    var timestamp: Long = System.nanoTime()
+    @JvmField var timestamp: Long = System.nanoTime()
 }
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumLocalizerInputsMessage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumLocalizerInputsMessage.kt
index f93e312..0ae858d 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumLocalizerInputsMessage.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/MecanumLocalizerInputsMessage.kt
@@ -5,22 +5,14 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit
 import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles
 
 class MecanumLocalizerInputsMessage(
-    var leftFront: PositionVelocityPair,
-    var leftBack: PositionVelocityPair,
-    var rightBack: PositionVelocityPair,
-    var rightFront: PositionVelocityPair,
+    @JvmField var leftFront: PositionVelocityPair,
+    @JvmField var leftBack: PositionVelocityPair,
+    @JvmField var rightBack: PositionVelocityPair,
+    @JvmField var rightFront: PositionVelocityPair,
     angles: YawPitchRollAngles
 ) {
-    var timestamp: Long = System.nanoTime()
-    var yaw: Double = 0.0
-    var pitch: Double = 0.0
-    var roll: Double = 0.0
-
-    init {
-        run {
-            this.yaw = angles.getYaw(AngleUnit.RADIANS)
-            this.pitch = angles.getPitch(AngleUnit.RADIANS)
-            this.roll = angles.getRoll(AngleUnit.RADIANS)
-        }
-    }
+    @JvmField var timestamp: Long = System.nanoTime()
+    @JvmField var yaw: Double = angles.getYaw(AngleUnit.RADIANS)
+    @JvmField var pitch: Double = angles.getPitch(AngleUnit.RADIANS)
+    @JvmField var roll: Double = angles.getRoll(AngleUnit.RADIANS)
 }
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/PoseMessage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/PoseMessage.kt
index 81eb5c0..224149c 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/PoseMessage.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/PoseMessage.kt
@@ -3,9 +3,9 @@ package org.firstinspires.ftc.teamcode.roadrunner.messages
 import com.acmerobotics.roadrunner.Pose2d
 
 class PoseMessage(pose: Pose2d) {
-    var timestamp: Long = System.nanoTime()
-    var x: Double = pose.position.x
-    var y: Double = pose.position.y
-    var heading: Double = pose.heading.toDouble()
+    @JvmField var timestamp: Long = System.nanoTime()
+    @JvmField var x: Double = pose.position.x
+    @JvmField var y: Double = pose.position.y
+    @JvmField var heading: Double = pose.heading.toDouble()
 }
 
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TankCommandMessage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TankCommandMessage.kt
index de1571e..be8a6e4 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TankCommandMessage.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TankCommandMessage.kt
@@ -1,5 +1,9 @@
 package org.firstinspires.ftc.teamcode.roadrunner.messages
 
-class TankCommandMessage(var voltage: Double, var leftPower: Double, var rightPower: Double) {
-    var timestamp: Long = System.nanoTime()
+class TankCommandMessage(
+    @JvmField var voltage: Double,
+    @JvmField var leftPower: Double,
+    @JvmField var rightPower: Double
+) {
+    @JvmField var timestamp: Long = System.nanoTime()
 }
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TankLocalizerInputsMessage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TankLocalizerInputsMessage.kt
index 5ed861a..ecfd03d 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TankLocalizerInputsMessage.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TankLocalizerInputsMessage.kt
@@ -6,7 +6,7 @@ class TankLocalizerInputsMessage(
     left: List<PositionVelocityPair>,
     right: List<PositionVelocityPair>
 ) {
-    var timestamp: Long = System.nanoTime()
-    var left: Array<PositionVelocityPair> = left.toTypedArray<PositionVelocityPair>()
-    var right: Array<PositionVelocityPair> = right.toTypedArray<PositionVelocityPair>()
+    @JvmField  var timestamp: Long = System.nanoTime()
+    @JvmField var left: Array<PositionVelocityPair> = left.toTypedArray<PositionVelocityPair>()
+    @JvmField var right: Array<PositionVelocityPair> = right.toTypedArray<PositionVelocityPair>()
 }
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/ThreeDeadWheelInputsMessage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/ThreeDeadWheelInputsMessage.kt
index 58262f0..ccb93b6 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/ThreeDeadWheelInputsMessage.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/ThreeDeadWheelInputsMessage.kt
@@ -3,9 +3,9 @@ package org.firstinspires.ftc.teamcode.roadrunner.messages
 import com.acmerobotics.roadrunner.ftc.PositionVelocityPair
 
 class ThreeDeadWheelInputsMessage(
-    var par0: PositionVelocityPair,
-    var par1: PositionVelocityPair,
-    var perp: PositionVelocityPair
+    @JvmField var par0: PositionVelocityPair,
+    @JvmField var par1: PositionVelocityPair,
+    @JvmField var perp: PositionVelocityPair
 ) {
-    var timestamp: Long = System.nanoTime()
+    @JvmField  var timestamp: Long = System.nanoTime()
 }
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TwoDeadWheelInputsMessage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TwoDeadWheelInputsMessage.kt
index b565732..02859e9 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TwoDeadWheelInputsMessage.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/messages/TwoDeadWheelInputsMessage.kt
@@ -6,29 +6,16 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity
 import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles
 
 class TwoDeadWheelInputsMessage(
-    var par: PositionVelocityPair,
-    var perp: PositionVelocityPair,
+    @JvmField var par: PositionVelocityPair,
+    @JvmField var perp: PositionVelocityPair,
     angles: YawPitchRollAngles,
     angularVelocity: AngularVelocity
 ) {
-    var timestamp: Long = System.nanoTime()
-    var yaw: Double = 0.0
-    var pitch: Double = 0.0
-    var roll: Double = 0.0
-    var xRotationRate: Double = 0.0
-    var yRotationRate: Double = 0.0
-    var zRotationRate: Double = 0.0
-
-    init {
-        run {
-            this.yaw = angles.getYaw(AngleUnit.RADIANS)
-            this.pitch = angles.getPitch(AngleUnit.RADIANS)
-            this.roll = angles.getRoll(AngleUnit.RADIANS)
-        }
-        run {
-            this.xRotationRate = angularVelocity.xRotationRate.toDouble()
-            this.yRotationRate = angularVelocity.yRotationRate.toDouble()
-            this.zRotationRate = angularVelocity.zRotationRate.toDouble()
-        }
-    }
+    @JvmField var timestamp: Long = System.nanoTime()
+    @JvmField var yaw: Double = angles.getYaw(AngleUnit.RADIANS)
+    @JvmField var pitch: Double = angles.getPitch(AngleUnit.RADIANS)
+    @JvmField var roll: Double =  angles.getRoll(AngleUnit.RADIANS)
+    @JvmField var xRotationRate: Double = angularVelocity.xRotationRate.toDouble()
+    @JvmField var yRotationRate: Double = angularVelocity.yRotationRate.toDouble()
+    @JvmField var zRotationRate: Double = angularVelocity.zRotationRate.toDouble()
 }
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/drive/DriveSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/drive/DriveSubsystem.kt
index 8931d2d..3049dbd 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/drive/DriveSubsystem.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/drive/DriveSubsystem.kt
@@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.subsystems.drive
 import com.acmerobotics.dashboard.canvas.Canvas
 import com.acmerobotics.dashboard.config.Config
 import com.acmerobotics.dashboard.telemetry.TelemetryPacket
-import com.acmerobotics.roadrunner.AccelConstraint
 import com.acmerobotics.roadrunner.Action
 import com.acmerobotics.roadrunner.AngularVelConstraint
 import com.acmerobotics.roadrunner.DualNum
@@ -27,7 +26,6 @@ import com.acmerobotics.roadrunner.TurnConstraints
 import com.acmerobotics.roadrunner.Twist2dDual
 import com.acmerobotics.roadrunner.Vector2d
 import com.acmerobotics.roadrunner.Vector2dDual
-import com.acmerobotics.roadrunner.VelConstraint
 import com.acmerobotics.roadrunner.ftc.DownsampledWriter
 import com.acmerobotics.roadrunner.ftc.Encoder
 import com.acmerobotics.roadrunner.ftc.FlightRecorder.write
@@ -63,8 +61,7 @@ object DriveSubsystem : SubsystemBase() {
     private lateinit var hardwareMap: HardwareMap
     var pose: Pose2d = Pose2d(0.0, 0.0, 0.0)
 
-    @JvmField
-    var PARAMS = Params()
+    @JvmField var PARAMS = Params()
 
     class Params {
         // IMU orientation
@@ -75,76 +72,48 @@ object DriveSubsystem : SubsystemBase() {
         var usbFacingDirection: UsbFacingDirection = UsbFacingDirection.RIGHT
 
         // drive model parameters
-        @JvmField
-        var inPerTick: Double = 1.0
-
-        @JvmField
-        var lateralInPerTick: Double = inPerTick
-
-        @JvmField
-        var trackWidthTicks: Double = 0.0
+        @JvmField var inPerTick: Double = 1.0
+        @JvmField var lateralInPerTick: Double = inPerTick
+        @JvmField var trackWidthTicks: Double = 0.0
 
         // feedforward parameters (in tick units)
-        @JvmField
-        var kS: Double = 0.0
-
-        @JvmField
-        var kV: Double = 0.0
-
-        @JvmField
-        var kA: Double = 0.0
+        @JvmField var kS: Double = 0.0
+        @JvmField var kV: Double = 0.0
+        @JvmField var kA: Double = 0.0
 
         // path profile parameters (in inches)
-        @JvmField
-        var maxWheelVel: Double = 50.0
-
-        @JvmField
-        var minProfileAccel: Double = -30.0
-
-        @JvmField
-        var maxProfileAccel: Double = 50.0
+        @JvmField var maxWheelVel: Double = 50.0
+        @JvmField var minProfileAccel: Double = -30.0
+        @JvmField var maxProfileAccel: Double = 50.0
 
         // turn profile parameters (in radians)
-        @JvmField
-        var maxAngVel: Double = Math.PI // shared with path
-
-        @JvmField
-        var maxAngAccel: Double = Math.PI
+        @JvmField var maxAngVel: Double = Math.PI // shared with path
+        @JvmField var maxAngAccel: Double = Math.PI
 
         // path controller gains
-        @JvmField
-        var axialGain: Double = 0.0
-
-        @JvmField
-        var lateralGain: Double = 0.0
+        @JvmField var axialGain: Double = 0.0
+        @JvmField var lateralGain: Double = 0.0
+        @JvmField var headingGain: Double = 0.0 // shared with turn
 
-        @JvmField
-        var headingGain: Double = 0.0 // shared with turn
-
-        @JvmField
-        var axialVelGain: Double = 0.0
-
-        @JvmField
-        var lateralVelGain: Double = 0.0
-
-        @JvmField
-        var headingVelGain: Double = 0.0 // shared with turn
+        @JvmField var axialVelGain: Double = 0.0
+        @JvmField var lateralVelGain: Double = 0.0
+        @JvmField var headingVelGain: Double = 0.0 // shared with turn
     }
 
-    val kinematics: MecanumKinematics = MecanumKinematics(
+    val kinematics = MecanumKinematics(
         PARAMS.inPerTick * PARAMS.trackWidthTicks, PARAMS.inPerTick / PARAMS.lateralInPerTick
     )
 
-    val defaultTurnConstraints: TurnConstraints = TurnConstraints(
+    val defaultTurnConstraints = TurnConstraints(
         PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel
     )
-    val defaultVelConstraint: VelConstraint = MinVelConstraint(
+    val defaultVelConstraint = MinVelConstraint(
         listOf(
             kinematics.WheelVelConstraint(PARAMS.maxWheelVel),
             AngularVelConstraint(PARAMS.maxAngVel)
         )
     )
-    val defaultAccelConstraint: AccelConstraint =
+    val defaultAccelConstraint =
         ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel)
 
     lateinit var leftFront: DcMotorEx
@@ -183,10 +152,10 @@ object DriveSubsystem : SubsystemBase() {
         private var lastHeading: Rotation2d = Rotation2d.exp(imu.robotYawPitchRollAngles.getYaw(AngleUnit.RADIANS))
         private var initialized = false
 
-//        init {
-            // TODO: reverse encoders if needed
-            //   leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
-//        }
+        init {
+//             TODO: reverse encoders if needed
+//               leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
+        }
 
         override fun update(): Twist2dDual<Time> {
             val leftFrontPosVel = leftFront.getPositionAndVelocity()
@@ -264,11 +233,12 @@ object DriveSubsystem : SubsystemBase() {
         }
     }
 
-    fun initialize(hardwareMap: HardwareMap,
-                   pose2d: Pose2d = Pose2d(0.0, 0.0, 0.0)
-    ) : DriveSubsystem {
+    fun initialize(hardwareMap: HardwareMap, reset: Boolean = true) : DriveSubsystem {
         this.hardwareMap = hardwareMap
-        this.pose = pose2d
+
+        if (reset) {
+            pose = Pose2d(0.0, 0.0, 0.0)
+        }
 
         throwIfModulesAreOutdated(hardwareMap)
 
@@ -557,7 +527,7 @@ object DriveSubsystem : SubsystemBase() {
     fun drive(leftY: Double, leftX: Double, rightX: Double) {
         setDrivePowers(
             PoseVelocity2d(
-                linearVel = Vector2d(leftX, leftY),
+                linearVel = Vector2d(leftY, leftX),
                 angVel = rightX
             )
         )