diff --git a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt index 65645c9c..178cd4ce 100644 --- a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt @@ -6,10 +6,10 @@ package com.team4099.robot2023 const val MAVEN_GROUP = "" const val MAVEN_NAME = "Crescendo-2024" const val VERSION = "unspecified" -const val GIT_REVISION = 26 -const val GIT_SHA = "73f4f3ef5bab17de6b7bbc919ece786ceb7b52ea" -const val GIT_DATE = "2024-01-15T19:49:07Z" +const val GIT_REVISION = 27 +const val GIT_SHA = "e6dd2ad00da75da1686f2dc6e2688aab809838a9" +const val GIT_DATE = "2024-01-15T20:47:37Z" const val GIT_BRANCH = "intake" -const val BUILD_DATE = "2024-01-15T20:41:51Z" -const val BUILD_UNIX_TIME = 1705369311220L +const val BUILD_DATE = "2024-01-16T14:47:03Z" +const val BUILD_UNIX_TIME = 1705434423104L const val DIRTY = 1 diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index f648bf94..dc76fdd6 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -14,6 +14,7 @@ import com.team4099.robot2023.subsystems.intake.IntakeIONEO import com.team4099.robot2023.subsystems.intake.IntakeIOSim import com.team4099.robot2023.subsystems.limelight.LimelightVision import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import com.team4099.robot2023.subsystems.vision.Vision import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar import com.team4099.robot2023.util.driver.Ryan @@ -90,7 +91,7 @@ object RobotContainer { } fun zeroSensors() { - drivetrain.zeroSensors() + drivetrain.currentRequest = DrivetrainRequest.ZeroSensors() } fun zeroAngle(toAngle: Angle) { diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt index d7c4c6dd..97472978 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt @@ -1,6 +1,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import edu.wpi.first.wpilibj2.command.Command import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.radians @@ -12,7 +13,7 @@ class DriveBrakeModeCommand(val drivetrain: Drivetrain) : Command() { } override fun execute() { - drivetrain.setOpenLoop( + drivetrain.currentRequest = DrivetrainRequest.OpenLoop( 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) ) 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 index 51438b3b..9c6fea93 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt @@ -6,6 +6,7 @@ import com.team4099.lib.trajectory.CustomTrajectoryGenerator import com.team4099.lib.trajectory.Waypoint import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import com.team4099.robot2023.util.AllianceFlipUtil import com.team4099.robot2023.util.Velocity2d import edu.wpi.first.math.kinematics.ChassisSpeeds @@ -215,7 +216,7 @@ class DrivePathCommand( Pose2dWPILIB(desiredState.poseMeters.translation, desiredRotation.position) ) - drivetrain.setClosedLoop( + drivetrain.currentRequest = DrivetrainRequest.ClosedLoop( nextDriveState, ChassisAccels(xAccel, yAccel, 0.0.radians.perSecond.perSecond).chassisAccelsWPILIB ) @@ -280,16 +281,16 @@ class DrivePathCommand( Logger.recordOutput("ActiveCommands/DrivePathCommand", false) if (interrupted) { // Stop where we are if interrupted - drivetrain.setOpenLoop( - 0.degrees.perSecond, Pair(0.meters.perSecond, 0.meters.perSecond) - ) + drivetrain.currentRequest = DrivetrainRequest.OpenLoop( + 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + ) } else { // Execute one last time to end up in the final state of the trajectory // Since we weren't interrupted, we know curTime > endTime execute() - drivetrain.setOpenLoop( - 0.degrees.perSecond, Pair(0.meters.perSecond, 0.meters.perSecond) - ) + drivetrain.currentRequest = DrivetrainRequest.OpenLoop( + 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + ) } } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt index 0779ea8f..30d1a3f2 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt @@ -3,6 +3,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger @@ -81,7 +82,7 @@ class GoToAngle(val drivetrain: Drivetrain) : Command() { val thetaFeedback = thetaPID.calculate(drivetrain.odometryPose.rotation, desiredAngle) - drivetrain.setOpenLoop( + drivetrain.currentRequest = DrivetrainRequest.OpenLoop( thetaFeedback, Pair(0.0.meters.perSecond, 0.0.meters.perSecond), fieldOriented = true ) @@ -97,8 +98,8 @@ class GoToAngle(val drivetrain: Drivetrain) : Command() { } override fun end(interrupted: Boolean) { - drivetrain.setOpenLoop( - 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) - ) + drivetrain.currentRequest = DrivetrainRequest.OpenLoop( + 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + ) } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt index 5d22049e..e49f994f 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt @@ -4,6 +4,7 @@ import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.ProfiledPIDController @@ -91,7 +92,7 @@ class GyroAutoLevel(val drivetrain: Drivetrain) : Command() { override fun execute() { Logger.recordOutput("ActiveCommands/AutoLevelCommand", true) - drivetrain.setOpenLoop( + drivetrain.currentRequest = DrivetrainRequest.OpenLoop( 0.0.radians.perSecond, gyroFeedback, fieldOriented = true ) @@ -119,8 +120,8 @@ class GyroAutoLevel(val drivetrain: Drivetrain) : Command() { } override fun end(interrupted: Boolean) { - drivetrain.setOpenLoop( - 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) - ) + drivetrain.currentRequest = DrivetrainRequest.OpenLoop( + 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + ) } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt index c4847a4d..6f4fa148 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt @@ -1,6 +1,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import edu.wpi.first.wpilibj2.command.Command import org.team4099.lib.units.base.feet import org.team4099.lib.units.derived.degrees @@ -12,7 +13,7 @@ class OpenLoopReverseCommand(val drivetrain: Drivetrain) : Command() { } override fun execute() { - drivetrain.setOpenLoop( + drivetrain.currentRequest = DrivetrainRequest.OpenLoop( 0.degrees.perSecond, Pair(-5.0.feet.perSecond, 0.0.feet.perSecond), fieldOriented = false diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt new file mode 100644 index 00000000..9a4a7070 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt @@ -0,0 +1,120 @@ +package com.team4099.robot2023.commands.drivetrain + +import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.DrivetrainConstants +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Request +import edu.wpi.first.wpilibj.RobotBase +import edu.wpi.first.wpilibj2.command.Command +import org.littletonrobotics.junction.Logger +import org.team4099.lib.controller.ProfiledPIDController +import org.team4099.lib.controller.TrapezoidProfile +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.units.Velocity +import org.team4099.lib.units.base.inMeters +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegree +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds +import org.team4099.lib.units.derived.perDegree +import org.team4099.lib.units.derived.perDegreePerSecond +import org.team4099.lib.units.derived.perDegreeSeconds +import org.team4099.lib.units.derived.radians +import org.team4099.lib.units.inDegreesPerSecond +import org.team4099.lib.units.perSecond +import kotlin.math.PI +import kotlin.math.tan + +class TargetPoseCommand(val drivetrain: Drivetrain, val targetPose: Pose2d) : Command() { + private val thetaPID: ProfiledPIDController> + + val thetakP = + LoggedTunableValue( + "Pathfollow/thetakP", + Pair({ it.inDegreesPerSecondPerDegree }, { it.degrees.perSecond.perDegree }) + ) + val thetakI = + LoggedTunableValue( + "Pathfollow/thetakI", + Pair( + { it.inDegreesPerSecondPerDegreeSeconds }, { it.degrees.perSecond.perDegreeSeconds } + ) + ) + val thetakD = + LoggedTunableValue( + "Pathfollow/thetakD", + Pair( + { it.inDegreesPerSecondPerDegreePerSecond }, + { it.degrees.perSecond.perDegreePerSecond } + ) + ) + + val thetaMaxVel = + LoggedTunableValue("Pathfollow/thetaMaxVel", DrivetrainConstants.PID.MAX_AUTO_ANGULAR_VEL) + val thetaMaxAccel = + LoggedTunableValue("Pathfollow/thetaMaxAccel", DrivetrainConstants.PID.MAX_AUTO_ANGULAR_ACCEL) + var desiredAngle: Angle = 0.0.degrees + + init { + addRequirements(drivetrain) + + if (RobotBase.isReal()) { + thetakP.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KP) + thetakI.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KI) + thetakD.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KD) + } else { + thetakP.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP) + thetakI.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KI) + thetakD.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD) + } + + thetaPID = + ProfiledPIDController( + thetakP.get(), + thetakI.get(), + thetakD.get(), + TrapezoidProfile.Constraints(thetaMaxVel.get(), thetaMaxAccel.get()) + ) + thetaPID.enableContinuousInput(-PI.radians, PI.radians) + } + + override fun initialize() { + thetaPID.reset(drivetrain.odometryPose.rotation) + } + + override fun execute() { + Logger.recordOutput("ActiveCommands/TargetPoseCommand", true) + + val currentPose = drivetrain.odometryPose + desiredAngle = + tan((currentPose.y - targetPose.y).inMeters / (currentPose.x - targetPose.x).inMeters) + .radians + val thetaFeedback = thetaPID.calculate(currentPose.rotation, desiredAngle) + + drivetrain.currentRequest = + Request.DrivetrainRequest.OpenLoop( + thetaFeedback, + Pair(drivetrain.fieldVelocity.x, drivetrain.fieldVelocity.y), + fieldOriented = true + ) + + Logger.recordOutput("AutoLevel/CurrentYawDegrees", drivetrain.odometryPose.rotation.inDegrees) + Logger.recordOutput("AutoLevel/DesiredYawDegrees", desiredAngle.inDegrees) + Logger.recordOutput("AutoLevel/thetaFeedbackDPS", thetaFeedback.inDegreesPerSecond) + } + + override fun isFinished(): Boolean { + return (drivetrain.odometryPose.rotation - desiredAngle).absoluteValue < + DrivetrainConstants.PID.AUTO_THETA_ALLOWED_ERROR + } + + override fun end(interrupted: Boolean) { + drivetrain.currentRequest = + Request.DrivetrainRequest.OpenLoop( + 0.0.radians.perSecond, Pair(drivetrain.fieldVelocity.x, drivetrain.fieldVelocity.y) + ) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt index 849c0b28..18f554e4 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt @@ -1,6 +1,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import com.team4099.robot2023.util.driver.DriverProfile import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger @@ -23,7 +24,7 @@ class TeleopDriveCommand( override fun execute() { val speed = driver.driveSpeedClampedSupplier(driveX, driveY, slowMode) val rotation = driver.rotationSpeedClampedSupplier(turn, slowMode) - drivetrain.setOpenLoop(rotation, speed) + drivetrain.currentRequest = DrivetrainRequest.OpenLoop(rotation, speed) Logger.recordOutput("ActiveCommands/TeleopDriveCommand", true) } override fun isFinished(): Boolean { diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt index 8654eba7..d3b6d267 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt @@ -1,6 +1,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger @@ -11,7 +12,7 @@ class ZeroSensorsCommand(val drivetrain: Drivetrain) : Command() { } override fun initialize() { - drivetrain.zeroSensors() + drivetrain.currentRequest = DrivetrainRequest.ZeroSensors() } override fun isFinished(): Boolean { 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 eae04997..870ddbdc 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 @@ -5,6 +5,7 @@ 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.DrivetrainRequest as DrivetrainRequest import com.team4099.robot2023.util.Alert import com.team4099.robot2023.util.FMSData import com.team4099.robot2023.util.PoseEstimator @@ -168,6 +169,38 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB var lastGyroYaw = 0.0.radians + var velocityTarget = 0.degrees.perSecond + + var targetedDriveVector = Pair(0.meters.perSecond, 0.meters.perSecond) + + var fieldOrientation = true + + var targetedChassisSpeeds = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) + + var targetedChassisAccels = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) + + var currentState: DrivetrainState = DrivetrainState.UNINITIALIZED + var currentRequest: DrivetrainRequest = DrivetrainRequest.ZeroSensors() + set(value) { + when (value) { + is DrivetrainRequest.OpenLoop -> { + velocityTarget = value.angularVelocity + targetedDriveVector = value.driveVector + fieldOrientation = value.fieldOriented + } + is DrivetrainRequest.ClosedLoop -> { + targetedChassisSpeeds = value.chassisSpeeds + targetedChassisAccels = value.chassisAccels + } + else -> {} + } + field = value + } + + init { + zeroSteering() + } + override fun periodic() { val startTime = Clock.realTimestamp gyroNotConnectedAlert.set(!gyroInputs.gyroConnected) @@ -240,6 +273,44 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB "LoggedRobot/Subsystems/DrivetrainLoopTimeMS", (Clock.realTimestamp - startTime).inMilliseconds ) + + var nextState = currentState + + when (currentState) { + DrivetrainState.UNINITIALIZED -> { + // Transitions + nextState = DrivetrainState.ZEROING_SENSORS + } + DrivetrainState.ZEROING_SENSORS -> { + zeroSensors() + + // Transitions + currentRequest = DrivetrainRequest.Idle() + nextState = fromRequestToState(currentRequest) + } + DrivetrainState.OPEN_LOOP -> { + // Outputs + setOpenLoop(velocityTarget, targetedDriveVector, fieldOrientation) + + // Transitions + nextState = fromRequestToState(currentRequest) + } + DrivetrainState.CLOSED_LOOP -> { + // Outputs + setClosedLoop(targetedChassisSpeeds, targetedChassisAccels) + + // Transitions + nextState = fromRequestToState(currentRequest) + } + DrivetrainState.IDLE -> { + nextState = fromRequestToState(currentRequest) + } + } + + currentState = nextState + + // Log the current state + Logger.recordOutput("Drivetrain/requestedState", currentState.toString()) } private fun updateOdometry() { @@ -553,4 +624,32 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB fun addVisionData(visionData: List) { swerveDrivePoseEstimator.addVisionData(visionData) } + + companion object { + enum class DrivetrainState { + UNINITIALIZED, + IDLE, + ZEROING_SENSORS, + OPEN_LOOP, + CLOSED_LOOP; + + inline fun equivalentToRequest(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) + ) + } + } + + inline fun fromRequestToState(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 + } + } + } } \ No newline at end of file