fix superstructure test command to call tunabledrivestates

team4099 · Jun 23, 2024 · 301a332 · 301a332
1 parent 8f61162
commit 301a332
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 5 deletions.
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt
@@ -53,8 +53,8 @@ object Constants {
   }
 
   object Tuning {
-    const val TUNING_MODE = false
-    const val DEBUGING_MODE = false
+    const val TUNING_MODE = true
+    const val DEBUGING_MODE = true
     const val SIMULATE_DRIFT = false
     const val DRIFT_CONSTANT = 0.001
 

diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt
@@ -31,7 +31,7 @@ import kotlin.math.sqrt
 
 object DrivetrainConstants {
   const val FOC_ENABLED = true
-  const val MINIMIZE_SKEW = false
+  const val MINIMIZE_SKEW = true
 
   const val TELEOP_TURNING_SPEED_PERCENT = 0.6
 

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
@@ -1173,8 +1173,11 @@ class Superstructure(
       currentRequest= Request.SuperstructureRequest.Tuning()
       drivetrain.currentRequest =
         Request.DrivetrainRequest.OpenLoop(
-          DrivetrainConstants.testOmega,
-          Velocity2d(DrivetrainConstants.testXVelocity, DrivetrainConstants.testYVelocity)
+          Drivetrain.TunableDriveStates.testOmega.get(),
+          Velocity2d(
+            Drivetrain.TunableDriveStates.testXVelocity.get(),
+            Drivetrain.TunableDriveStates.testYVelocity.get()
+          )
         )
     }
     returnCommand.name = "TestDriveVelocityCommand"