create test drive velocity command (needs to be tested)

team4099 · Jun 22, 2024 · 7607f51 · 7607f51
1 parent 4521d6c
commit 7607f51
Show file tree

Hide file tree

Showing 5 changed files with 50 additions and 17 deletions.
diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -240,6 +240,7 @@ object RobotContainer {
     ControlBoard.passingShot.whileTrue(superstructure.passingShotCommand())
     ControlBoard.underStagePassingShot.whileTrue(superstructure.underStageCommand())
     ControlBoard.testWrist.whileTrue(superstructure.testWristCommand())
+    ControlBoard.testDriveVelocity.whileTrue(superstructure.testDriveVelocityCommand())
 
     /*
     ControlBoard.targetAmp.whileTrue(

diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt
@@ -55,8 +55,7 @@ object ControlBoard {
   // val score = Trigger {driver.leftTriggerAxis > 0.5}
   // val intake = Trigger { driver.rightShoulderButton}
 
-  // TODO: uncomment
-  //val targetAmp = Trigger { driver.aButton }
+  val targetAmp = Trigger { driver.aButton }
   val prepAmp = Trigger { operator.aButton }
   val prepLow = Trigger { operator.xButton }
   val prepHighProtected = Trigger { operator.bButton }
@@ -72,6 +71,8 @@ object ControlBoard {
 
   val testWrist = Trigger { driver.aButton }
 
+  val testDriveVelocity = Trigger { driver.aButton }
+
   val characterizeWrist = Trigger { driver.rightShoulderButton }
 
   val climbAlignFar = Trigger { driver.dPadUp }

diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt
@@ -58,6 +58,10 @@ object DrivetrainConstants {
 
   const val GYRO_RATE_COEFFICIENT = 0.0 // TODO: Change this value
 
+  val testXVelocity = 1.0.meters.perSecond
+  val testYVelocity = 1.0.meters.perSecond
+  val testOmega = 1.0.radians.perSecond
+
   val SLOW_AUTO_VEL = 2.meters.perSecond
   val SLOW_AUTO_ACCEL = 2.0.meters.perSecond.perSecond
 

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt
@@ -31,27 +31,38 @@ import org.team4099.lib.geometry.Rotation3d
 import org.team4099.lib.geometry.Transform2d
 import org.team4099.lib.geometry.Translation2d
 import org.team4099.lib.kinematics.ChassisSpeeds
-import org.team4099.lib.units.AngularVelocity
+import org.team4099.lib.units.*
 import org.team4099.lib.units.base.inMeters
 import org.team4099.lib.units.base.inMilliseconds
 import org.team4099.lib.units.base.inSeconds
 import org.team4099.lib.units.base.meters
-import org.team4099.lib.units.derived.Angle
-import org.team4099.lib.units.derived.degrees
-import org.team4099.lib.units.derived.inDegrees
-import org.team4099.lib.units.derived.inRadians
-import org.team4099.lib.units.derived.inRotation2ds
-import org.team4099.lib.units.derived.radians
-import org.team4099.lib.units.derived.volts
-import org.team4099.lib.units.inDegreesPerSecond
-import org.team4099.lib.units.inMetersPerSecond
-import org.team4099.lib.units.inRadiansPerSecond
-import org.team4099.lib.units.perSecond
+import org.team4099.lib.units.derived.*
 import java.util.concurrent.locks.Lock
 import java.util.concurrent.locks.ReentrantLock
 import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 
 class Drivetrain(private val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemBase() {
+  object TunableDriveStates {
+    val testXVelocity =
+      LoggedTunableValue(
+        "Drivetrain/testXVelocity",
+        DrivetrainConstants.testXVelocity,
+        Pair( { it.inMetersPerSecond }, { it.meters.perSecond })
+      )
+    val testYVelocity =
+      LoggedTunableValue(
+        "Drivetrain/testYVelocity",
+        DrivetrainConstants.testYVelocity,
+        Pair( { it.inMetersPerSecond }, { it.meters.perSecond })
+      )
+    val testOmega =
+      LoggedTunableValue(
+        "Drivetrain/testOmega",
+        DrivetrainConstants.testOmega,
+        Pair( { it.inRadiansPerSecond }, { it.radians.perSecond })
+      )
+  }
+
   private val gyroNotConnectedAlert =
     Alert(
       "Gyro is not connected, field relative driving will be significantly worse.",

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
@@ -1,6 +1,7 @@
 package com.team4099.robot2023.subsystems.superstructure
 
 import com.team4099.lib.hal.Clock
+import com.team4099.robot2023.config.constants.DrivetrainConstants
 import com.team4099.robot2023.config.constants.FieldConstants
 import com.team4099.robot2023.config.constants.FlywheelConstants
 import com.team4099.robot2023.config.constants.WristConstants
@@ -24,7 +25,8 @@ import org.team4099.lib.geometry.Pose3d
 import org.team4099.lib.geometry.Rotation3d
 import org.team4099.lib.geometry.Transform3d
 import org.team4099.lib.geometry.Translation3d
-import org.team4099.lib.units.AngularVelocity
+import org.team4099.lib.kinematics.ChassisSpeeds
+import org.team4099.lib.units.*
 import org.team4099.lib.units.base.inMeters
 import org.team4099.lib.units.base.inMilliseconds
 import org.team4099.lib.units.base.inSeconds
@@ -35,8 +37,6 @@ import org.team4099.lib.units.derived.degrees
 import org.team4099.lib.units.derived.inDegrees
 import org.team4099.lib.units.derived.rotations
 import org.team4099.lib.units.derived.volts
-import org.team4099.lib.units.inRotationsPerMinute
-import org.team4099.lib.units.perMinute
 
 class Superstructure(
   private val intake: Intake,
@@ -1167,6 +1167,22 @@ class Superstructure(
     return returnCommand
   }
 
+  fun testDriveVelocityCommand(): Command {
+    val returnCommand = runOnce {
+      currentRequest= Request.SuperstructureRequest.Tuning()
+      drivetrain.currentRequest =
+        Request.DrivetrainRequest.ClosedLoop(edu.wpi.first.math.kinematics.ChassisSpeeds(
+          DrivetrainConstants.testXVelocity.inMetersPerSecond,
+          DrivetrainConstants.testYVelocity.inMetersPerSecond,
+          DrivetrainConstants.testOmega.inRadiansPerSecond
+        ))
+    }
+    returnCommand.name = "TestDriveVelocityCommand"
+    return returnCommand
+  }
+
+  // TODO: Test Drivetrain Steer Command
+
   companion object {
     enum class SuperstructureStates {
       UNINITIALIZED,