From 8dcb30d51233c3d592dcc6ce31024b60bad73668 Mon Sep 17 00:00:00 2001 From: SirBeans Date: Fri, 21 Jun 2024 18:08:09 -0400 Subject: [PATCH] start the periodic for Drivetrain --- .../subsystems/drivetrain/drive/Drivetrain.kt | 62 +++++++++++++------ .../drivetrain/swervemodule/SwerveModule.kt | 34 +++++----- .../drivetrain/swervemodule/SwerveModuleIO.kt | 7 +-- 3 files changed, 60 insertions(+), 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 index 3a31ee0b..09093467 100644 --- 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,13 +20,14 @@ 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 @@ -34,13 +35,13 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB 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(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 + ) } } \ No newline at end of file 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 index 1a650ff2..2ed9d962 100644 --- 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() @@ -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 index 30d1a32c..5ff73251 100644 --- 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>>, kA: Value>>> ) - -} \ No newline at end of file +}