Made instantiation of the 0 chassis speed static and moved static zer…

…oes to DriveTrainConstants
team4099 · Jul 4, 2024 · ccef242 · ccef242
1 parent 39abfdd
commit ccef242
Show file tree

Hide file tree

Showing 12 changed files with 21 additions and 19 deletions.
diff --git a/.gradle/8.4/executionHistory/executionHistory.bin b/.gradle/8.4/executionHistory/executionHistory.bin
diff --git a/.gradle/8.4/executionHistory/executionHistory.lock b/.gradle/8.4/executionHistory/executionHistory.lock
diff --git a/.gradle/8.4/fileHashes/fileHashes.bin b/.gradle/8.4/fileHashes/fileHashes.bin
diff --git a/.gradle/8.4/fileHashes/fileHashes.lock b/.gradle/8.4/fileHashes/fileHashes.lock
diff --git a/.gradle/8.4/fileHashes/resourceHashesCache.bin b/.gradle/8.4/fileHashes/resourceHashesCache.bin
diff --git a/.gradle/buildOutputCleanup/buildOutputCleanup.lock b/.gradle/buildOutputCleanup/buildOutputCleanup.lock
diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt
@@ -1,7 +1,7 @@
 package com.team4099.robot2023.commands.drivetrain
 
+import com.team4099.robot2023.config.constants.DrivetrainConstants
 import com.team4099.robot2023.subsystems.drivetrain.Drivetrain
-import com.team4099.robot2023.util.Velocity2d
 import edu.wpi.first.wpilibj2.command.Command
 import org.team4099.lib.units.derived.radians
 import org.team4099.lib.units.perSecond
@@ -14,7 +14,7 @@ class DriveBrakeModeCommand(val drivetrain: Drivetrain) : Command() {
 
   override fun execute() {
     drivetrain.currentRequest =
-      DrivetrainRequest.OpenLoop(0.0.radians.perSecond, Velocity2d.ZERO_VELOCITY_VECTOR)
+      DrivetrainRequest.OpenLoop(0.0.radians.perSecond, DrivetrainConstants.ZERO_VELOCITY_VECTOR)
     drivetrain.swerveModules.forEach() { it.setDriveBrakeMode(true) }
   }
 

diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
@@ -73,7 +73,7 @@ private constructor(
   val flipForAlliances: Boolean = true,
   val endPathOnceAtReference: Boolean = true,
   val leaveOutYAdjustment: Boolean = false,
-  val endVelocity: Velocity2d = Velocity2d.ZERO_VELOCITY_VECTOR,
+  val endVelocity: Velocity2d = DrivetrainConstants.ZERO_VELOCITY_VECTOR,
   var stateFrame: FrameType = FrameType.ODOMETRY,
   var pathFrame: FrameType = FrameType.FIELD,
 ) : Command() {
@@ -428,7 +428,9 @@ private constructor(
       // Since we weren't interrupted, we know curTime > endTime
       execute()
       drivetrain.currentRequest =
-        DrivetrainRequest.OpenLoop(0.0.radians.perSecond, Velocity2d.ZERO_VELOCITY_VECTOR)
+        DrivetrainRequest.OpenLoop(
+          0.0.radians.perSecond, DrivetrainConstants.ZERO_VELOCITY_VECTOR
+        )
     }
   }
 
@@ -444,7 +446,7 @@ private constructor(
       flipForAlliances: Boolean = true,
       endPathOnceAtReference: Boolean = true,
       leaveOutYAdjustment: Boolean = false,
-      endVelocity: Velocity2d = Velocity2d.ZERO_VELOCITY_VECTOR,
+      endVelocity: Velocity2d = DrivetrainConstants.ZERO_VELOCITY_VECTOR,
       stateFrame: FrameType = FrameType.ODOMETRY,
     ): DrivePathCommand<OdometryWaypoint> =
       DrivePathCommand(
@@ -468,7 +470,7 @@ private constructor(
       flipForAlliances: Boolean = true,
       endPathOnceAtReference: Boolean = true,
       leaveOutYAdjustment: Boolean = false,
-      endVelocity: Velocity2d = Velocity2d.ZERO_VELOCITY_VECTOR,
+      endVelocity: Velocity2d = DrivetrainConstants.ZERO_VELOCITY_VECTOR,
       stateFrame: FrameType = FrameType.ODOMETRY,
     ): DrivePathCommand<FieldWaypoint> =
       DrivePathCommand(

diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt
@@ -1,5 +1,7 @@
 package com.team4099.robot2023.config.constants
 
+import com.team4099.robot2023.util.Velocity2d
+import edu.wpi.first.math.kinematics.ChassisSpeeds
 import edu.wpi.first.wpilibj.RobotBase
 import org.team4099.lib.geometry.Translation2d
 import org.team4099.lib.units.Velocity
@@ -39,6 +41,9 @@ object DrivetrainConstants {
 
   val ODOMETRY_UPDATE_FREQUENCY = 250.0.hertz
 
+  val ZERO_VELOCITY_VECTOR = Velocity2d(0.0.meters.perSecond, 0.0.meters.perSecond)
+  val ZERO_CHASSIS_SPEED = ChassisSpeeds(0.0, 0.0, 0.0)
+
   const val WHEEL_COUNT = 4
   val WHEEL_DIAMETER = (2.083 * 2).inches
   val DRIVETRAIN_LENGTH = 22.750.inches

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/Drivetrain.kt
@@ -71,26 +71,22 @@ class Drivetrain(private val gyroIO: GyroIO, val swerveModules: List<SwerveModul
 
   private var angularVelocityTarget = 0.degrees.perSecond
 
-  private var targetedDriveVector = Velocity2d.ZERO_VELOCITY_VECTOR
+  private var targetedDriveVector = DrivetrainConstants.ZERO_VELOCITY_VECTOR
 
   private var isFieldOriented = true
 
-  private var targetedChassisSpeeds =
-    ChassisSpeeds(0.0.meters.perSecond, 0.0.meters.perSecond, 0.0.radians.perSecond)
-      .chassisSpeedsWPILIB
+  private var targetedChassisSpeeds = DrivetrainConstants.ZERO_CHASSIS_SPEED
 
-  private var targetedChassisAccels =
-    ChassisSpeeds(0.0.meters.perSecond, 0.0.meters.perSecond, 0.0.radians.perSecond)
-      .chassisSpeedsWPILIB
+  private var targetedChassisAccels = DrivetrainConstants.ZERO_CHASSIS_SPEED
 
   private var isInAutonomous = false
 
   var targetPose: Pose2d = Pose2d(0.0.meters, 0.0.meters, 0.0.radians)
 
-  var fieldVelocity = Velocity2d.ZERO_VELOCITY_VECTOR
+  var fieldVelocity = DrivetrainConstants.ZERO_VELOCITY_VECTOR
     private set
 
-  var robotVelocity = Velocity2d.ZERO_VELOCITY_VECTOR
+  var robotVelocity = DrivetrainConstants.ZERO_VELOCITY_VECTOR
     private set
 
   private var omegaVelocity = 0.0.radians.perSecond
@@ -444,7 +440,7 @@ class Drivetrain(private val gyroIO: GyroIO, val swerveModules: List<SwerveModul
   fun setClosedLoop(
     chassisSpeeds: edu.wpi.first.math.kinematics.ChassisSpeeds,
     chassisAccels: edu.wpi.first.math.kinematics.ChassisSpeeds =
-      edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0)
+      DrivetrainConstants.ZERO_CHASSIS_SPEED
   ) {
     // Allows parameter chassisSpeeds to have its value reassigned
     var chassisSpeeds = chassisSpeeds

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt
@@ -1,5 +1,6 @@
 package com.team4099.robot2023.subsystems.superstructure
 
+import com.team4099.robot2023.config.constants.DrivetrainConstants
 import com.team4099.robot2023.config.constants.WristConstants
 import com.team4099.robot2023.util.Velocity2d
 import edu.wpi.first.math.kinematics.ChassisSpeeds
@@ -64,7 +65,7 @@ sealed interface Request {
 
     class ClosedLoop(
       var chassisSpeeds: ChassisSpeeds,
-      val chassisAccels: ChassisSpeeds = ChassisSpeeds(0.0, 0.0, 0.0)
+      val chassisAccels: ChassisSpeeds = DrivetrainConstants.ZERO_CHASSIS_SPEED
     ) : DrivetrainRequest
 
     class ZeroSensors(val isInAutonomous: Boolean = false) : DrivetrainRequest

diff --git a/src/main/kotlin/com/team4099/robot2023/util/Velocity2d.kt b/src/main/kotlin/com/team4099/robot2023/util/Velocity2d.kt
@@ -19,8 +19,6 @@ data class Velocity2d(val x: LinearVelocity, val y: LinearVelocity) {
   val heading: Angle = velocity2dWPIlib.angle.angle
 
   companion object {
-    val ZERO_VELOCITY_VECTOR = Velocity2d(0.0.meters.perSecond, 0.0.meters.perSecond)
-
     fun fromVelocityVectorToVelocity2d(speed: LinearVelocity, heading: Angle): Velocity2d {
       return Velocity2d(speed * heading.cos, speed * heading.sin)
     }