From cd079c56013482fa6ca0c6c7a9ff0ad47b6094fc Mon Sep 17 00:00:00 2001 From: SirBeans Date: Fri, 21 Jun 2024 20:20:03 -0400 Subject: [PATCH] Fix errors in SwerveModule code and start the cleanup process for drivetrain logic file --- .../subsystems/drivetrain/drive/Drivetrain.kt | 679 ++++++++++++++++-- .../drivetrain/swervemodule/SwerveModule.kt | 27 +- .../drivetrain/swervemodule/SwerveModuleIO.kt | 2 +- .../swervemodule/SwerveModuleIOTalon.kt | 4 +- .../subsystems/superstructure/Request.kt | 1 - 5 files changed, 635 insertions(+), 78 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 09093467..7a2d82ea 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 @@ -1,128 +1,261 @@ + package com.team4099.robot2023.subsystems.drivetrain.drive import com.team4099.lib.hal.Clock +import com.team4099.lib.logging.LoggedTunableValue import com.team4099.lib.math.asPose2d import com.team4099.lib.math.asTransform2d +import com.team4099.lib.vision.TimestampedTrigVisionUpdate +import com.team4099.lib.vision.TimestampedVisionUpdate +import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.DrivetrainConstants +import com.team4099.robot2023.config.constants.VisionConstants import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO +import com.team4099.robot2023.subsystems.superstructure.Request import com.team4099.robot2023.util.Alert import com.team4099.robot2023.util.CustomLogger +import com.team4099.robot2023.util.FMSData import com.team4099.robot2023.util.FieldFrameEstimator +import com.team4099.robot2023.util.Velocity2d import edu.wpi.first.math.VecBuilder import edu.wpi.first.math.kinematics.SwerveDriveKinematics import edu.wpi.first.math.kinematics.SwerveDriveOdometry -import org.team4099.lib.geometry.Translation2d -import edu.wpi.first.wpilibj2.command.SubsystemBase -import org.team4099.lib.units.base.meters -import org.team4099.lib.units.derived.inRotation2ds -import org.team4099.lib.units.derived.radians -import org.team4099.lib.units.perSecond +import edu.wpi.first.math.kinematics.SwerveModulePosition import edu.wpi.first.math.kinematics.SwerveModuleState +import edu.wpi.first.wpilibj.DriverStation +import edu.wpi.first.wpilibj.RobotBase +import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.Commands +import edu.wpi.first.wpilibj2.command.SubsystemBase +import org.littletonrobotics.junction.Logger import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Pose3d +import org.team4099.lib.geometry.Rotation3d import org.team4099.lib.geometry.Transform2d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.geometry.Twist2d import org.team4099.lib.kinematics.ChassisSpeeds +import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.LinearVelocity +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 com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemBase() { - // TODO: Add default values - private val gyroNotConnectedAlert = Alert("Failed to connect gyro", Alert.AlertType.ERROR) + private val gyroNotConnectedAlert = + Alert( + "Gyro is not connected, field relative driving will be significantly worse.", + Alert.AlertType.ERROR + ) + val swerveModules = swerveModuleIOs.getSwerveModules() // FL, FR, BL, BR - private val swerveModules = swerveModuleIOs.getSwerveModules() + val gyroInputs = GyroIO.GyroIOInputs() - private val gyroYawOffset = 0.0.radians + private var gyroYawOffset = 0.0.radians - private val gyroInputs = GyroIO.GyroIOInputs() + // TODO: tune these + private var fieldFrameEstimator = FieldFrameEstimator(VecBuilder.fill(0.003, 0.003, 0.0001)) - private val fieldFrameEstimator = FieldFrameEstimator(VecBuilder.fill(0.003, 0.003, 0.0001)) + private var angularVelocityTarget = 0.degrees.perSecond - private val angularVelocityTarget = 0.0.radians.perSecond + private var targetedDriveVector = Velocity2d() - private val targetedDriveVector = Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + private var isFieldOriented = true - private val isFieldOriented = true + var targetedChassisSpeeds = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) + private set - var isInAuto = false + var targetedChassisAccels = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) + private set - private val frontLeftLocation = - Translation2d(DrivetrainConstants.DRIVETRAIN_LENGTH / 2, DrivetrainConstants.DRIVETRAIN_WIDTH / 2) + var isInAutonomous = false + private set - private val frontRightLocation = - Translation2d(DrivetrainConstants.DRIVETRAIN_LENGTH / 2, -DrivetrainConstants.DRIVETRAIN_WIDTH / 2) + var targetPose: Pose2d = Pose2d(0.0.meters, 0.0.meters, 0.0.radians) - private val backLeftLocation = - Translation2d(-DrivetrainConstants.DRIVETRAIN_LENGTH / 2, DrivetrainConstants.DRIVETRAIN_WIDTH) + private var drift: Transform2d = Transform2d(Translation2d(), 0.0.radians) - private val backRightLocation = - Translation2d(-DrivetrainConstants.DRIVETRAIN_LENGTH / 2, -DrivetrainConstants.DRIVETRAIN_WIDTH/2) + var fieldVelocity = Velocity2d(0.0.meters.perSecond, 0.0.meters.perSecond) + private set - var targetedChassisSpeeds = edu.wpi.first.math.kinematics.ChassisSpeeds( - 0.0, 0.0, 0.0 - ) + var robotVelocity = Velocity2d(0.0.meters.perSecond, 0.0.meters.perSecond) - var targetedChassisAccels = edu.wpi.first.math.kinematics.ChassisSpeeds( - 0.0, 0.0, 0.0 - ) + private var omegaVelocity = 0.0.radians.perSecond - private val swerveDriveKinematics = SwerveDriveKinematics( - frontLeftLocation.translation2d, - frontRightLocation.translation2d, - backLeftLocation.translation2d, - backRightLocation.translation2d - ) + private var characterizationInput = 0.0.volts - private var swerveDriveOdometry: SwerveDriveOdometry = SwerveDriveOdometry ( - swerveDriveKinematics, - gyroInputs.gyroYaw.inRotation2ds, - swerveModules.map{it.modulePosition}.toTypedArray() - ) + var lastGyroYaw = { gyroInputs.gyroYaw } - private var undriftedSwerveDriveOdometry: SwerveDriveOdometry = SwerveDriveOdometry ( - swerveDriveKinematics, - gyroInputs.gyroYaw.inRotation2ds, - swerveModules.map{it.modulePosition}.toTypedArray() - ) - private var setpointStates = mutableListOf(SwerveModuleState(), SwerveModuleState(), SwerveModuleState(), SwerveModuleState()) + var currentState: DrivetrainState = DrivetrainState.UNINITIALIZED + private set - val odomTRobot: Pose2d get() = Pose2d(swerveDriveOdometry.poseMeters) + private val testAngle = + LoggedTunableValue("Drivetrain/testAngle", 0.degrees, Pair({ it.inDegrees }, { it.degrees })) - val fieldTRobot: Pose2d get() = - (fieldFrameEstimator.getLatestOdometryTField().inverse() + odomTRobot.asTransform2d()) - .asPose2d() + private val swerveModuleID = + LoggedTunableValue("Drivetrain/testModule", 0.degrees, Pair({ it.inDegrees }, { it.degrees })) - val odomTField: Transform2d get() = fieldFrameEstimator.getLatestOdometryTField() + val closestAlignmentAngle: Angle + get() { + for (angle in -180..90 step 90) { + if ((odomTRobot.rotation - angle.degrees).absoluteValue <= 45.degrees) { + return angle.degrees + } + } + return 0.0.degrees + } - val odomTSpeaker: Transform2d get() = fieldFrameEstimator.getLatestOdometryTSpeaker() + var currentRequest: DrivetrainRequest = DrivetrainRequest.ZeroSensors() + set(value) { + when (value) { + is DrivetrainRequest.OpenLoop -> { + angularVelocityTarget = value.angularVelocity + targetedDriveVector = value.driveVector + isFieldOriented = value.fieldOriented + } + is DrivetrainRequest.ClosedLoop -> { + targetedChassisSpeeds = value.chassisSpeeds + targetedChassisAccels = value.chassisAccels + } + is DrivetrainRequest.ZeroSensors -> { + isInAutonomous = value.isInAutonomous + } + is DrivetrainRequest.Characterize -> { + characterizationInput = value.voltage + } + else -> {} + } + field = value + } - private val lastModulePositions = arrayOf( - SwerveModuleState(), - SwerveModuleState(), - SwerveModuleState(), SwerveModuleState()) + private val frontLeftWheelLocation = + Translation2d( + DrivetrainConstants.DRIVETRAIN_LENGTH / 2, DrivetrainConstants.DRIVETRAIN_WIDTH / 2 + ) + private val frontRightWheelLocation = + Translation2d( + DrivetrainConstants.DRIVETRAIN_LENGTH / 2, -DrivetrainConstants.DRIVETRAIN_WIDTH / 2 + ) + private val backLeftWheelLocation = + Translation2d( + -DrivetrainConstants.DRIVETRAIN_LENGTH / 2, DrivetrainConstants.DRIVETRAIN_WIDTH / 2 + ) + private val backRightWheelLocation = + Translation2d( + -DrivetrainConstants.DRIVETRAIN_LENGTH / 2, -DrivetrainConstants.DRIVETRAIN_WIDTH / 2 + ) + + val moduleTranslations = + listOf( + frontLeftWheelLocation, + frontRightWheelLocation, + backLeftWheelLocation, + backRightWheelLocation + ) + + private val swerveDriveKinematics = + SwerveDriveKinematics( + frontLeftWheelLocation.translation2d, + frontRightWheelLocation.translation2d, + backLeftWheelLocation.translation2d, + backRightWheelLocation.translation2d + ) + + private var swerveDriveOdometry = + SwerveDriveOdometry( + swerveDriveKinematics, + gyroInputs.gyroYaw.inRotation2ds, + swerveModules.map { it.modulePosition }.toTypedArray() + ) + + private var undriftedSwerveDriveOdometry = + SwerveDriveOdometry( + swerveDriveKinematics, + gyroInputs.gyroYaw.inRotation2ds, + swerveModules.map { it.modulePosition }.toTypedArray() + ) + + private var setPointStates = + mutableListOf( + SwerveModuleState(), SwerveModuleState(), SwerveModuleState(), SwerveModuleState() + ) + + val odomTRobot: Pose2d + get() = Pose2d(swerveDriveOdometry.poseMeters) + + // NOTE(parth): This should be expected to be noisy. Avoid using this directly if possible. + val fieldTRobot: Pose2d + get() = + (fieldFrameEstimator.getLatestOdometryTField().inverse() + odomTRobot.asTransform2d()) + .asPose2d() + + val odomTField: Transform2d + get() = fieldFrameEstimator.getLatestOdometryTField() + + val odomTSpeaker: Transform2d + get() = fieldFrameEstimator.getLatestOdometryTSpeaker() + + private var undriftedPose: Pose2d + get() = Pose2d(undriftedSwerveDriveOdometry.poseMeters) + set(value) { + undriftedSwerveDriveOdometry.resetPosition( + gyroInputs.gyroYaw.inRotation2ds, + swerveModules.map { it.modulePosition }.toTypedArray(), + value.pose2d + ) + } - private var rawGryoAngle = odomTRobot.rotation - init { - zeroSteer() - } - - fun zeroSteer(isAuto: Boolean = false) { - swerveModules.forEach { it.zeroSteer(isAuto) } - } + private var rawGyroAngle = odomTRobot.rotation + private val lastModulePositions = + arrayOf( + SwerveModulePosition(), + SwerveModulePosition(), + SwerveModulePosition(), + SwerveModulePosition() + ) + init { + // Wheel speeds + zeroSteering() + } override fun periodic() { - CustomLogger.processInputs("Drivetrain", gyroInputs) - + val startTime = Clock.realTimestamp gyroIO.updateInputs(gyroInputs) - val timeProfiledGeneratedAt = Clock.realTimestamp + // Read new gyro and wheel data from sensors + // odometryLock.lock() // Prevents odometry updates while reading data + + swerveModules.forEach { it.updateInputs() } + // odometryLock.unlock() - swerveModules.forEach {it.updateInputs()} + Logger.processInputs("Gyro", gyroInputs) gyroNotConnectedAlert.set(!gyroInputs.gyroConnected) swerveModules.forEach { it.periodic() } - val measuredStates = arrayOfNulls(4) + // Update field velocity + val measuredStates = mutableListOf( + SwerveModuleState(), + SwerveModuleState(), + SwerveModuleState(), + SwerveModuleState() + ) for (i in 0..3) { measuredStates[i] = SwerveModuleState( @@ -130,10 +263,412 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB swerveModules[i].inputs.steerPosition.inRotation2ds ) } - val chassisState: ChassisSpeeds = ChassisSpeeds(swerveDriveKinematics.toChassisSpeeds(*measuredStates)) + val chassisState: ChassisSpeeds = + ChassisSpeeds(swerveDriveKinematics.toChassisSpeeds(*measuredStates.toTypedArray())) val fieldVelCalculated = Translation2d( chassisState.vx.inMetersPerSecond.meters, chassisState.vy.inMetersPerSecond.meters ) + .rotateBy(odomTRobot.rotation) // we don't use this but it's there if you want it ig + + robotVelocity = Velocity2d(chassisState.vx, chassisState.vy) + fieldVelocity = Velocity2d(fieldVelCalculated.x.perSecond, fieldVelCalculated.y.perSecond) + + omegaVelocity = chassisState.omega + if (!gyroInputs.gyroConnected) { + gyroInputs.gyroYawRate = omegaVelocity + rawGyroAngle += Constants.Universal.LOOP_PERIOD_TIME * gyroInputs.gyroYawRate + gyroInputs.gyroYaw = rawGyroAngle + gyroYawOffset + } + + // updating odometry every loop cycle + updateOdometry() + + CustomLogger.recordOutput( + "FieldFrameEstimator/odomTSpeaker", + fieldFrameEstimator.getLatestOdometryTSpeaker().transform2d + ) + + CustomLogger.recordOutput("Drivetrain/OdometryGyroRotationValue", odomTRobot.rotation.inDegrees) + + CustomLogger.recordOutput( + "Drivetrain/xRobotVelocityMetersPerSecond", robotVelocity.x.inMetersPerSecond + ) + CustomLogger.recordOutput( + "Drivetrain/xRobotVelocityMetersPerSecond", robotVelocity.x.inMetersPerSecond + ) + CustomLogger.recordOutput( + "Drivetrain/xFieldVelocityMetersPerSecond", fieldVelocity.x.inMetersPerSecond + ) + CustomLogger.recordOutput( + "Drivetrain/yFieldVelocityMetersPerSecond", fieldVelocity.y.inMetersPerSecond + ) + + CustomLogger.processInputs("Drivetrain/Gyro", gyroInputs) + CustomLogger.recordOutput( + VisionConstants.POSE_TOPIC_NAME, + doubleArrayOf(odomTRobot.x.inMeters, odomTRobot.y.inMeters, odomTRobot.rotation.inRadians) + ) + CustomLogger.recordOutput("FieldRelativePose/robotPose", fieldTRobot.pose2d) + + CustomLogger.recordOutput("Drivetrain/ModuleStates", *measuredStates.toTypedArray()) + CustomLogger.recordOutput("Drivetrain/setPointStates", *setPointStates.toTypedArray()) + + CustomLogger.recordOutput( + "Odometry/pose", + odomTRobot.pose2d + ) + CustomLogger.recordOutput( + "Odometry/pose3d", + Pose3d( + odomTRobot.x, + odomTRobot.y, + 0.0.meters, + Rotation3d(gyroInputs.gyroRoll, gyroInputs.gyroPitch, gyroInputs.gyroYaw) + ) + .pose3d + ) + + CustomLogger.recordOutput("FieldFrameEstimator/odomTField", odomTField.transform2d) + + CustomLogger.recordOutput( + "Odometry/targetPose", + targetPose.pose2d + ) + + CustomLogger.recordOutput( + "LoggedRobot/Subsystems/DrivetrainLoopTimeMS", + (Clock.realTimestamp - startTime).inMilliseconds + ) + + // Log the current state + CustomLogger.recordOutput("Drivetrain/currentState", currentState.toString()) + // Log the current state + CustomLogger.recordOutput("Drivetrain/currentRequest", currentRequest.javaClass.toString()) + + var nextState = currentState + + when (currentState) { + DrivetrainState.UNINITIALIZED -> { + // Transitions + nextState = DrivetrainState.ZEROING_SENSORS + } + DrivetrainState.ZEROING_SENSORS -> { + zeroSensors(isInAutonomous) + // Transitions + currentRequest = DrivetrainRequest.Idle() + nextState = fromRequestToState(currentRequest) + } + DrivetrainState.IDLE -> { + nextState = fromRequestToState(currentRequest) + } + DrivetrainState.OPEN_LOOP -> { + // Outputs + setOpenLoop(angularVelocityTarget, targetedDriveVector., isFieldOriented) + CustomLogger.recordOutput( + "Drivetrain/TargetVelocityXInMPS", targetedDriveVector.first.inMetersPerSecond + ) + CustomLogger.recordOutput( + "Drivetrain/TargetVelocityYInMPS", targetedDriveVector.second.inMetersPerSecond + ) + // Transitions + nextState = fromRequestToState(currentRequest) + } + DrivetrainState.CLOSED_LOOP -> { + // Outputs + setClosedLoop(targetedChassisSpeeds, targetedChassisAccels) + + CustomLogger.recordOutput("Drivetrain/targetChassisSpeeds", targetedChassisSpeeds) + CustomLogger.recordOutput("Drivetrain/targetChassisAccels", targetedChassisAccels) + + // Transitions + nextState = fromRequestToState(currentRequest) + } + DrivetrainState.CHARACTERIZE -> { + swerveModules.forEach { it.runCharacterization(characterizationInput) } + + // Transitions + nextState = fromRequestToState(currentRequest) + } + } + + currentState = nextState + } + + private fun updateOdometry() { + for (i in 0 until 4) { + lastModulePositions[i] = + SwerveModulePosition( + swerveModules[i].inputs.drivePosition.inMeters, + swerveModules[i].inputs.steerPosition.inRotation2ds + ) + } + + swerveDriveOdometry.update(gyroInputs.gyroYaw.inRotation2ds, lastModulePositions) + fieldFrameEstimator.addDriveData(Clock.fpgaTime, odomTRobot) + } + + /** + * @param angularVelocity Represents the angular velocity of the chassis + * @param driveVector Pair of linear velocities: First is X vel, second is Y vel + * @param fieldOriented Are the chassis speeds driving relative to field (aka use gyro or not) + */ + fun setOpenLoop( + angularVelocity: AngularVelocity, + driveVector: Velocity2d, + fieldOriented: Boolean = true + ) { + + CustomLogger.recordOutput("Drivetrain/isFieldOriented", fieldOriented) + // flip the direction base don alliance color + val driveVectorRespectiveToAlliance = if (FMSData.allianceColor == DriverStation.Alliance.Blue) driveVector else driveVector.unaryMinus() + + CustomLogger.recordOutput( + "Drivetrain/driveVectorRespectiveToAllianceXInMPS", driveVectorRespectiveToAlliance.x.inMetersPerSecond + ) + CustomLogger.recordOutput( + "Drivetrain/driveVectorRespectiveToAllianceYInMPS", driveVectorRespectiveToAlliance.y.inMetersPerSecond + ) + + val swerveModuleStates: Array + var desiredChassisSpeeds: ChassisSpeeds + + // calculated chasis speeds, apply field oriented transformation + if (fieldOriented) { + desiredChassisSpeeds = + ChassisSpeeds.fromFieldRelativeSpeeds( + driveVectorRespectiveToAlliance.x, + driveVectorRespectiveToAlliance.y, + angularVelocity + odomTRobot.rotation + ) + } else { + desiredChassisSpeeds = + ChassisSpeeds( + driveVectorRespectiveToAlliance.x, + driveVectorRespectiveToAlliance.y, + angularVelocity, + ) + } + + + + + if (DrivetrainConstants.MINIMIZE_SKEW) { + desiredChassisSpeeds = + ChassisSpeeds(edu.wpi.first.math.kinematics.ChassisSpeeds.discretize( + desiredChassisSpeeds.vx.inMetersPerSecond, + desiredChassisSpeeds.vy.inMetersPerSecond, + desiredChassisSpeeds.omega.inRadiansPerSecond, + Constants.Universal.LOOP_PERIOD_TIME.inSeconds + )) + } + CustomLogger.recordDebugOutput( + "Drivetrain/desiredChassisSpeedsVXInMPS", desiredChassisSpeeds.vx.inMetersPerSecond + ) + CustomLogger.recordDebugOutput( + "Drivetrain/desiredChassisSpeedsVYInMPS", desiredChassisSpeeds.vx.inMetersPerSecond + ) + CustomLogger.recordDebugOutput( + "Drivetrain/omegaDegreesPerSecond", desiredChassisSpeeds.omega.inDegreesPerSecond + ) + + // convert target chassis speeds to individual module setpoint states + swerveModuleStates = + swerveDriveKinematics.toSwerveModuleStates(desiredChassisSpeeds.chassisSpeedsWPILIB) + SwerveDriveKinematics.desaturateWheelSpeeds( + swerveModuleStates, DrivetrainConstants.DRIVE_SETPOINT_MAX.inMetersPerSecond + ) + setPointStates = swerveModuleStates.toMutableList() + + // set each module openloop based on corresponding states + for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { + swerveModules[moduleIndex].setPositionOpenLoop(swerveModuleStates[moduleIndex]) + } + } + + + + /** + * Sets the drivetrain to the specified angular and X & Y velocities based on the current angular + * and linear acceleration. Calculates both angular and linear velocities and acceleration and + * calls setPositionClosedLoop for each SwerveModule object. + * + * @param angularVelocity The angular velocity of a specified drive + * @param driveVector.first The linear velocity on the X axis + * @param driveVector.second The linear velocity on the Y axis + * @param angularAcceleration The angular acceleration of a specified drive + * @param driveAcceleration.first The linear acceleration on the X axis + * @param driveAcceleration.second The linear acceleration on the Y axis + * @param fieldOriented Defines whether module states are calculated relative to field + */ + 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) + ) { + var chassisSpeeds = chassisSpeeds + + if (DrivetrainConstants.MINIMIZE_SKEW) { + chassisSpeeds = + edu.wpi.first.math.kinematics.ChassisSpeeds.discretize( + chassisSpeeds.vxMetersPerSecond, + chassisSpeeds.vyMetersPerSecond, + chassisSpeeds.omegaRadiansPerSecond, + Constants.Universal.LOOP_PERIOD_TIME.inSeconds + ) + } + + val velSwerveModuleStates: Array = + swerveDriveKinematics.toSwerveModuleStates(chassisSpeeds) + val accelSwerveModuleStates: Array = + swerveDriveKinematics.toSwerveModuleStates(chassisAccels) + + SwerveDriveKinematics.desaturateWheelSpeeds( + velSwerveModuleStates, DrivetrainConstants.MAX_AUTO_VEL.inMetersPerSecond + ) + + setPointStates = velSwerveModuleStates.toMutableList() + + // Once we have all of our states obtained for both velocity and acceleration, apply these + // states to each swerve module + for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { + swerveModules[moduleIndex].closedLoop( + velSwerveModuleStates[moduleIndex], accelSwerveModuleStates[moduleIndex] + ) + } + } + + fun resetModuleZero() { + swerveModules.forEach { it.resetModuleZero() } + } + + /** Zeros all the sensors on the drivetrain. */ + fun zeroSensors(isInAutonomous: Boolean) { + zeroGyroPitch() + zeroGyroRoll() + zeroSteering(isInAutonomous) + + if (!isInAutonomous) { + zeroDrive() + } + } + + /** Resets the field frame estimator given some current pose of the robot. */ + fun resetFieldFrameEstimator(fieldTRobot: Pose2d) { + fieldFrameEstimator.resetFieldFrameFilter( + odomTRobot.asTransform2d() + fieldTRobot.asTransform2d().inverse() + ) } -} \ No newline at end of file + + fun tempZeroGyroYaw(toAngle: Angle = 0.0.degrees) { + swerveDriveOdometry.resetPosition( + gyroInputs.gyroYaw.inRotation2ds, + lastModulePositions, + Pose2d(odomTRobot.x, odomTRobot.y, toAngle).pose2d + ) + } + + /** + * Sets the gyroOffset in such a way that when added to the gyro angle it gives back toAngle. + * + * @param toAngle Zeros the gyro to the value + */ + fun zeroGyroYaw(toAngle: Angle = 0.degrees) { + // TODO(parth): This feels incorrect -- I think the first arg should be the gyro angle and the + // undrifted pose should be updated to toAngle + if (RobotBase.isSimulation()) { + // NOTE(parth): The gyro itself should never need to be reset in-match on a real robot, the + // odometry can be updated directly + gyroIO.zeroGyroYaw(toAngle) + undriftedSwerveDriveOdometry.resetPosition( + toAngle.inRotation2ds, + swerveModules.map { it.modulePosition }.toTypedArray(), + undriftedPose.pose2d + ) + } + + // TODO(parth): Update the field frame estimator's transform here too, otherwise it will need to + // re-converge + swerveDriveOdometry.resetPosition( + gyroInputs.gyroYaw.inRotation2ds, + lastModulePositions, + Pose2d(odomTRobot.x, odomTRobot.y, toAngle).pose2d + ) + + fieldFrameEstimator.resetFieldFrameFilter( + Transform2d(odomTField.translation, gyroInputs.gyroYaw) + ) + + if (!(gyroInputs.gyroConnected)) { + gyroYawOffset = toAngle - rawGyroAngle + } + } + + fun zeroGyroPitch(toAngle: Angle = 0.0.degrees) { + gyroIO.zeroGyroPitch(toAngle) + } + + fun zeroGyroRoll(toAngle: Angle = 0.0.degrees) { + gyroIO.zeroGyroRoll(toAngle) + } + + /** Zeros the steering motors for each swerve module. */ + fun zeroSteering(isInAutonomous: Boolean = false) { + swerveModules.forEach { it.zeroSteer(isInAutonomous) } + } + + /** Zeros the drive motors for each swerve module. */ + private fun zeroDrive() { + swerveModules.forEach { it.zeroDrive() } + } + + fun driveSetpointTestCommand(): Command { + return runOnce { + swerveModules[swerveModuleID.get().inDegrees.toInt()].setOpenLoop( + testAngle.get(), 0.meters.perSecond, false + ) + } + } + + fun addVisionData(visionData: List) { + fieldFrameEstimator.addVisionData(visionData) + } + + fun addSpeakerVisionData(visionData: TimestampedTrigVisionUpdate) { + fieldFrameEstimator.addSpeakerVisionData(visionData) + } + + + + // Drivetrain states for state machine. + enum class DrivetrainState { + UNINITIALIZED, + IDLE, + ZEROING_SENSORS, + OPEN_LOOP, + CLOSED_LOOP, + CHARACTERIZE; + + inline fun equivalentToRequest(request: Request.DrivetrainRequest): Boolean { + return ( + (request is DrivetrainRequest.ZeroSensors && this == ZEROING_SENSORS) || + (request is DrivetrainRequest.OpenLoop && this == OPEN_LOOP) || + (request is DrivetrainRequest.ClosedLoop && this == CLOSED_LOOP) || + (request is DrivetrainRequest.Idle && this == IDLE) || + (request is DrivetrainRequest.Characterize && this == CHARACTERIZE) + ) + } + } + + inline fun fromRequestToState(request: Request.DrivetrainRequest): DrivetrainState { + return when (request) { + is DrivetrainRequest.OpenLoop -> DrivetrainState.OPEN_LOOP + is DrivetrainRequest.ClosedLoop -> DrivetrainState.CLOSED_LOOP + is DrivetrainRequest.ZeroSensors -> DrivetrainState.ZEROING_SENSORS + is DrivetrainRequest.Idle -> DrivetrainState.IDLE + is DrivetrainRequest.Characterize -> DrivetrainState.CHARACTERIZE + } + } + } + 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 2ed9d962..c8d22bc5 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 @@ -30,12 +30,15 @@ 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.perSecond +import kotlin.math.IEEErem import kotlin.math.abs import kotlin.math.cos +import kotlin.math.withSign class SwerveModule(val io: SwerveModuleIO) { @@ -53,6 +56,8 @@ class SwerveModule(val io: SwerveModuleIO) { private var lastDrivePos = 0.0.meters + private var shouldInvert = false + private val steerkP = LoggedTunableValue( "Drivetrain/modulesteerkP", Pair({ it.inVoltsPerDegree }, { it.volts.perDegree }) @@ -173,7 +178,25 @@ class SwerveModule(val io: SwerveModuleIO) { io.configDrivePID(drivekP.get(), drivekI.get(), drivekD.get(), drivekV.get(), drivekA.get()) } } - fun openLoop(desiredState: SwerveModuleState, optimize: Boolean) { + + fun setOpenLoop(steering: Angle, speed: LinearVelocity, optimize: Boolean = true) { + var steeringDifference = + (steering - inputs.steerPosition).inRadians.IEEErem(2 * Math.PI).radians + shouldInvert = steeringDifference.absoluteValue > (Math.PI / 2).radians && optimize + if (shouldInvert) { + steeringDifference -= Math.PI.withSign(steeringDifference.inRadians).radians + } + val outputSpeed = + if (shouldInvert) { + speed * -1 + } else { + speed + } + steeringSetpoint = inputs.steerPosition + steeringDifference + io.setOpenLoop(steeringSetpoint, outputSpeed) + } + + fun setPositionOpenLoop(desiredState: SwerveModuleState, optimize: Boolean = true) { if (optimize) { val optimizedState = SwerveModuleState.optimize(desiredState, inputs.steerPosition.inRotation2ds) @@ -193,7 +216,7 @@ class SwerveModule(val io: SwerveModuleIO) { fun closedLoop( desiredAccelState: SwerveModuleState, desiredVeloState: SwerveModuleState, - optimize: Boolean + optimize: Boolean = true ) { if (optimize) { val optmizedVeloState = 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 5ff73251..6e79de7e 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 @@ -30,7 +30,7 @@ import org.team4099.lib.units.inMetersPerSecond import org.team4099.lib.units.inRadiansPerSecond import org.team4099.lib.units.perSecond -interface SwerveModuleIO { +interface SwerveModuleIO{ class SwerveModuleIOInputs : LoggableInputs { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt index e695222e..3f2f2168 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt @@ -359,7 +359,7 @@ class SwerveModuleIOTalon( steeringPositionQueue.clear() } - override fun configureDrivePID( + override fun configDrivePID( kP: ProportionalGain, Volt>, kI: IntegralGain, Volt>, kD: DerivativeGain, Volt>, @@ -377,7 +377,7 @@ class SwerveModuleIOTalon( driveFalcon.configurator.apply(PIDConfig) } - override fun configureSteeringPID( + override fun configSteeringPID( kP: ProportionalGain, kI: IntegralGain, kD: DerivativeGain diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index 649a07ec..d9dc72c3 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -71,7 +71,6 @@ sealed interface Request { class ZeroSensors(val isInAutonomous: Boolean = false) : DrivetrainRequest class Idle : DrivetrainRequest - class LockWheels : DrivetrainRequest class Characterize(val voltage: ElectricalPotential) : DrivetrainRequest }