start the periodic for Drivetrain

team4099 · Jun 21, 2024 · 8dcb30d · 8dcb30d
1 parent de9f858
commit 8dcb30d
Show file tree

Hide file tree

Showing 3 changed files with 60 additions and 43 deletions.
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
@@ -20,27 +20,28 @@ import org.team4099.lib.units.perSecond
 import edu.wpi.first.math.kinematics.SwerveModuleState
 import org.team4099.lib.geometry.Pose2d
 import org.team4099.lib.geometry.Transform2d
-import edu.wpi.first.math.kinematics.ChassisSpeeds
+import org.team4099.lib.kinematics.ChassisSpeeds
+import org.team4099.lib.units.inMetersPerSecond
 
 class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemBase() {
   // TODO: Add default values
-  private val gyroNotConnected = Alert("Failed to connect gyro", Alert.AlertType.ERROR)
+  private val gyroNotConnectedAlert = Alert("Failed to connect gyro", Alert.AlertType.ERROR)
 
-  private val swerveModule = swerveModuleIOs.getSwerveModules()
+  private val swerveModules = swerveModuleIOs.getSwerveModules()
 
   private val gyroYawOffset = 0.0.radians
 
   private val gyroInputs = GyroIO.GyroIOInputs()
 
   private val fieldFrameEstimator = FieldFrameEstimator(VecBuilder.fill(0.003, 0.003, 0.0001))
 
-  private val angularVelocity = 0.0.radians.perSecond
+  private val angularVelocityTarget = 0.0.radians.perSecond
 
   private val targetedDriveVector = Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
 
   private val isFieldOriented = true
 
-  private val isInAuto = false
+  var isInAuto = false
 
   private val frontLeftLocation =
     Translation2d(DrivetrainConstants.DRIVETRAIN_LENGTH / 2, DrivetrainConstants.DRIVETRAIN_WIDTH / 2)
@@ -54,11 +55,11 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
   private val backRightLocation =
     Translation2d(-DrivetrainConstants.DRIVETRAIN_LENGTH / 2, -DrivetrainConstants.DRIVETRAIN_WIDTH/2)
 
-  private val targetedChassisVelocity = ChassisSpeeds(
+  var targetedChassisSpeeds = edu.wpi.first.math.kinematics.ChassisSpeeds(
     0.0, 0.0, 0.0
   )
 
-  private val targetedChassisAcceleration = ChassisSpeeds(
+  var targetedChassisAccels = edu.wpi.first.math.kinematics.ChassisSpeeds(
     0.0, 0.0, 0.0
   )
 
@@ -72,44 +73,67 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
   private var swerveDriveOdometry: SwerveDriveOdometry = SwerveDriveOdometry (
     swerveDriveKinematics,
     gyroInputs.gyroYaw.inRotation2ds,
-    swerveModule.map{it.modulePosition}.toTypedArray()
+    swerveModules.map{it.modulePosition}.toTypedArray()
   )
 
   private var undriftedSwerveDriveOdometry: SwerveDriveOdometry = SwerveDriveOdometry (
     swerveDriveKinematics,
     gyroInputs.gyroYaw.inRotation2ds,
-    swerveModule.map{it.modulePosition}.toTypedArray()
+    swerveModules.map{it.modulePosition}.toTypedArray()
   )
   private var setpointStates = mutableListOf(SwerveModuleState(), SwerveModuleState(), SwerveModuleState(), SwerveModuleState())
 
-  val odometryToRobot: Pose2d get() = Pose2d(swerveDriveOdometry.poseMeters)
+  val odomTRobot: Pose2d get() = Pose2d(swerveDriveOdometry.poseMeters)
 
-  val odometryFieldToRobot: Pose2d get() =
-    (fieldFrameEstimator.getLatestOdometryTField().inverse() + odometryToRobot.asTransform2d())
+  val fieldTRobot: Pose2d get() =
+    (fieldFrameEstimator.getLatestOdometryTField().inverse() + odomTRobot.asTransform2d())
       .asPose2d()
 
-  val odometryToField: Transform2d get() = fieldFrameEstimator.getLatestOdometryTField()
+  val odomTField: Transform2d get() = fieldFrameEstimator.getLatestOdometryTField()
 
-  val odometryToSpeaker: Transform2d get() = fieldFrameEstimator.getLatestOdometryTSpeaker()
+  val odomTSpeaker: Transform2d get() = fieldFrameEstimator.getLatestOdometryTSpeaker()
 
-  private val lastModulePositions = arrayOf(SwerveModuleState(), SwerveModuleState(), SwerveModuleState(), SwerveModuleState(), )
+  private val lastModulePositions = arrayOf(
+    SwerveModuleState(),
+    SwerveModuleState(),
+    SwerveModuleState(), SwerveModuleState())
 
+  private var rawGryoAngle = odomTRobot.rotation
   init {
     zeroSteer()
   }
 
   fun zeroSteer(isAuto: Boolean = false) {
-    swerveModule.forEach { it.zeroSteer(isAuto) }
+    swerveModules.forEach { it.zeroSteer(isAuto) }
   }
 
-  fun updateInputs() {
-    gyroIO.updateInputs(gyroInputs)
-  }
+
 
   override fun periodic() {
     CustomLogger.processInputs("Drivetrain", gyroInputs)
 
+    gyroIO.updateInputs(gyroInputs)
 
     val timeProfiledGeneratedAt = Clock.realTimestamp
+
+    swerveModules.forEach {it.updateInputs()}
+
+    gyroNotConnectedAlert.set(!gyroInputs.gyroConnected)
+
+    swerveModules.forEach { it.periodic() }
+
+    val measuredStates = arrayOfNulls<SwerveModuleState>(4)
+    for (i in 0..3) {
+      measuredStates[i] =
+        SwerveModuleState(
+          swerveModules[i].inputs.driveVelocity.inMetersPerSecond,
+          swerveModules[i].inputs.steerPosition.inRotation2ds
+        )
+    }
+    val chassisState: ChassisSpeeds = ChassisSpeeds(swerveDriveKinematics.toChassisSpeeds(*measuredStates))
+    val fieldVelCalculated =
+      Translation2d(
+        chassisState.vx.inMetersPerSecond.meters, chassisState.vy.inMetersPerSecond.meters
+      )
   }
 }
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt
@@ -2,20 +2,17 @@ package com.team4099.robot2023.subsystems.drivetrain.swervemodule
 
 import com.team4099.lib.logging.LoggedTunableValue
 import com.team4099.robot2023.config.constants.DrivetrainConstants
-import com.team4099.robot2023.util.DebugLogger
+import com.team4099.robot2023.util.CustomLogger
 import edu.wpi.first.math.kinematics.SwerveModulePosition
 import edu.wpi.first.math.kinematics.SwerveModuleState
 import edu.wpi.first.wpilibj.RobotBase.isReal
 import org.littletonrobotics.junction.Logger
 import org.team4099.lib.units.LinearAcceleration
 import org.team4099.lib.units.LinearVelocity
-import org.team4099.lib.units.base.feet
-import org.team4099.lib.units.base.inCelsius
 import org.team4099.lib.units.base.inMeters
 import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.base.seconds
 import org.team4099.lib.units.derived.Angle
-import org.team4099.lib.units.derived.ElectricalPotential
 import org.team4099.lib.units.derived.angle
 import org.team4099.lib.units.derived.degrees
 import org.team4099.lib.units.derived.inDegrees
@@ -33,22 +30,18 @@ import org.team4099.lib.units.derived.perDegreePerSecond
 import org.team4099.lib.units.derived.perDegreeSeconds
 import org.team4099.lib.units.derived.perMeterPerSecond
 import org.team4099.lib.units.derived.perMeterPerSecondPerSecond
-import org.team4099.lib.units.derived.radians
 import org.team4099.lib.units.derived.volts
 import org.team4099.lib.units.inMetersPerSecond
 import org.team4099.lib.units.inMetersPerSecondPerSecond
-import org.team4099.lib.units.inRadiansPerSecond
 import org.team4099.lib.units.perSecond
-import kotlin.math.IEEErem
 import kotlin.math.abs
 import kotlin.math.cos
-import kotlin.math.withSign
 
 class SwerveModule(val io: SwerveModuleIO) {
 
-  private val inputs = SwerveModuleIO.SwerveModuleIOInputs()
+  val inputs = SwerveModuleIO.SwerveModuleIOInputs()
 
-  private var modulePosition = SwerveModulePosition()
+  var modulePosition = SwerveModulePosition()
 
   private var posDeltas = mutableListOf<SwerveModulePosition>()
 
@@ -108,7 +101,7 @@ class SwerveModule(val io: SwerveModuleIO) {
   private val steerMaxVelo =
     LoggedTunableValue("Drivetrain/steerMaxVelocity", DrivetrainConstants.STEERING_VEL_MAX)
 
-  init{
+  init {
     if (isReal()) {
       steerkP.initDefault(DrivetrainConstants.PID.STEERING_KP)
       steerkI.initDefault(DrivetrainConstants.PID.STEERING_KI)
@@ -141,12 +134,16 @@ class SwerveModule(val io: SwerveModuleIO) {
     )
 
     Logger.processInputs(io.label, inputs)
-    DebugLogger.recordDebugOutput("${io.label}/lastDrivePos", lastDrivePos.inMeters)
+    CustomLogger.recordDebugOutput("${io.label}/lastDrivePos", lastDrivePos.inMeters)
 
-    DebugLogger.recordDebugOutput("${io.label}/velocitySetpoint", velocitySetpoint.inMetersPerSecond)
-    DebugLogger.recordDebugOutput("${io.label}/accelerationSetpoint", accelerationSetpoint.inMetersPerSecondPerSecond)
-    DebugLogger.recordDebugOutput("${io.label}/steeringSetpoint", steeringSetpoint.inDegrees)
-    DebugLogger.recordDebugOutput("${io.label}/lastDrivePos", lastDrivePos.inMeters)
+    CustomLogger.recordDebugOutput(
+      "${io.label}/velocitySetpoint", velocitySetpoint.inMetersPerSecond
+    )
+    CustomLogger.recordDebugOutput(
+      "${io.label}/accelerationSetpoint", accelerationSetpoint.inMetersPerSecondPerSecond
+    )
+    CustomLogger.recordDebugOutput("${io.label}/steeringSetpoint", steeringSetpoint.inDegrees)
+    CustomLogger.recordDebugOutput("${io.label}/lastDrivePos", lastDrivePos.inMeters)
 
     Logger.recordOutput("${io.label}/driveAppliedVoltage", inputs.driveAppliedVoltage.inVolts)
     Logger.recordOutput("${io.label}/swerveAppliedVoltage", inputs.swerveAppliedVoltage.inVolts)
@@ -166,6 +163,7 @@ class SwerveModule(val io: SwerveModuleIO) {
       io.configSteerPID(steerkP.get(), steerkI.get(), steerkD.get())
     }
 
+
     if (drivekD.hasChanged() ||
       drivekP.hasChanged() ||
       drivekI.hasChanged() ||
@@ -182,13 +180,13 @@ class SwerveModule(val io: SwerveModuleIO) {
       io.setOpenLoop(
         optimizedState.angle.angle,
         optimizedState.speedMetersPerSecond.meters.perSecond *
-                cos(abs((optimizedState.angle.angle - inputs.steerPosition).inRadians))
+          cos(abs((optimizedState.angle.angle - inputs.steerPosition).inRadians))
       )
     } else {
       io.setOpenLoop(
         desiredState.angle.angle,
         desiredState.speedMetersPerSecond.meters.perSecond *
-                cos(abs((desiredState.angle.angle - inputs.steerPosition).inRadians))
+          cos(abs((desiredState.angle.angle - inputs.steerPosition).inRadians))
       )
     }
   }

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt
@@ -2,8 +2,6 @@ package com.team4099.robot2023.subsystems.drivetrain.swervemodule
 
 import org.littletonrobotics.junction.LogTable
 import org.littletonrobotics.junction.inputs.LoggableInputs
-import org.team4099.lib.units.AngularAcceleration
-import org.team4099.lib.units.AngularVelocity
 import org.team4099.lib.units.Fraction
 import org.team4099.lib.units.LinearAcceleration
 import org.team4099.lib.units.LinearVelocity
@@ -19,12 +17,10 @@ import org.team4099.lib.units.base.inMeters
 import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.DerivativeGain
-import org.team4099.lib.units.derived.ElectricalPotential
 import org.team4099.lib.units.derived.IntegralGain
 import org.team4099.lib.units.derived.ProportionalGain
 import org.team4099.lib.units.derived.Radian
 import org.team4099.lib.units.derived.Volt
-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.inVolts
@@ -151,5 +147,4 @@ interface SwerveModuleIO {
     kV: Value<Fraction<Volt, Velocity<Meter>>>,
     kA: Value<Fraction<Volt, Velocity<Velocity<Meter>>>>
   )
-
-}
+}