From deb1873595bcc6e3064ae36fe5789e31597fef47 Mon Sep 17 00:00:00 2001 From: Shayaan Wadkar <82843611+Shom770@users.noreply.github.com> Date: Tue, 9 Jan 2024 19:35:46 -0500 Subject: [PATCH 01/11] change version of wpilib in the workflow --- .github/workflows/gradle.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml index ff145033..543287a4 100644 --- a/.github/workflows/gradle.yml +++ b/.github/workflows/gradle.yml @@ -8,7 +8,7 @@ jobs: runs-on: ubuntu-latest # This grabs the WPILib docker container - container: wpilib/roborio-cross-ubuntu:2024.1.1-beta-4 + container: wpilib/roborio-cross-ubuntu:2024.1.1 steps: # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it - uses: actions/checkout@v2 From eca16395eb62f3d6d367479ab32e985e6472149b Mon Sep 17 00:00:00 2001 From: Shom770 Date: Tue, 16 Jan 2024 14:49:50 -0500 Subject: [PATCH 02/11] initial commit --- .../com/team4099/robot2023/BuildConstants.kt | 10 +- .../com/team4099/robot2023/RobotContainer.kt | 3 +- .../drivetrain/DriveBrakeModeCommand.kt | 3 +- .../commands/drivetrain/DrivePathCommand.kt | 15 ++- .../commands/drivetrain/GoToAngle.kt | 9 +- .../commands/drivetrain/GyroAutoLevel.kt | 9 +- .../drivetrain/OpenLoopReverseCommand.kt | 3 +- .../commands/drivetrain/TargetPoseCommand.kt | 120 ++++++++++++++++++ .../commands/drivetrain/TeleopDriveCommand.kt | 3 +- .../commands/drivetrain/ZeroSensorsCommand.kt | 3 +- .../subsystems/drivetrain/drive/Drivetrain.kt | 99 +++++++++++++++ 11 files changed, 252 insertions(+), 25 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt 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 From 7bc23edb5afc91c36f4e019483b12aabb97b6947 Mon Sep 17 00:00:00 2001 From: Shom770 Date: Tue, 16 Jan 2024 15:40:42 -0500 Subject: [PATCH 03/11] current changes so far --- .../kotlin/com/team4099/robot2023/BuildConstants.kt | 12 ++++++------ .../kotlin/com/team4099/robot2023/RobotContainer.kt | 5 ++++- .../commands/drivetrain/TargetPoseCommand.kt | 9 +++++++++ 3 files changed, 19 insertions(+), 7 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt index 178cd4ce..39f5c23d 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 = 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-16T14:47:03Z" -const val BUILD_UNIX_TIME = 1705434423104L +const val GIT_REVISION = 28 +const val GIT_SHA = "eca16395eb62f3d6d367479ab32e985e6472149b" +const val GIT_DATE = "2024-01-16T14:49:50Z" +const val GIT_BRANCH = "targeting-command-with-drivetrain" +const val BUILD_DATE = "2024-01-16T15:27:50Z" +const val BUILD_UNIX_TIME = 1705436870465L 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 dc76fdd6..f42e7050 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -2,6 +2,7 @@ package com.team4099.robot2023 import com.team4099.robot2023.auto.AutonomousSelector import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand +import com.team4099.robot2023.commands.drivetrain.TargetPoseCommand import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand import com.team4099.robot2023.config.ControlBoard import com.team4099.robot2023.config.constants.Constants @@ -19,7 +20,9 @@ import com.team4099.robot2023.subsystems.vision.Vision import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar import com.team4099.robot2023.util.driver.Ryan import edu.wpi.first.wpilibj.RobotBase +import org.team4099.lib.geometry.Pose2d import org.team4099.lib.smoothDeadband +import org.team4099.lib.units.base.feet import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees @@ -115,7 +118,7 @@ object RobotContainer { fun mapTeleopControls() { ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain, toAngle = 180.degrees)) - ControlBoard.groundIntakeTest.whileTrue(intake.generateIntakeTestCommand()) + ControlBoard.groundIntakeTest.whileTrue(TargetPoseCommand(drivetrain, Pose2d(27.feet, 13.5.feet, 0.0.degrees))) // ControlBoard.autoLevel.whileActiveContinuous( // GoToAngle(drivetrain).andThen(AutoLevel(drivetrain)) // ) diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt index 9a4a7070..c1221e2f 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt @@ -19,6 +19,7 @@ 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.inRadians import org.team4099.lib.units.derived.perDegree import org.team4099.lib.units.derived.perDegreePerSecond import org.team4099.lib.units.derived.perDegreeSeconds @@ -61,6 +62,8 @@ class TargetPoseCommand(val drivetrain: Drivetrain, val targetPose: Pose2d) : Co init { addRequirements(drivetrain) + Logger.recordOutput("Odometry/targetedPose", doubleArrayOf(targetPose.x.inMeters, targetPose.y.inMeters, targetPose.rotation.inRadians)) + if (RobotBase.isReal()) { thetakP.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KP) thetakI.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KI) @@ -92,6 +95,12 @@ class TargetPoseCommand(val drivetrain: Drivetrain, val targetPose: Pose2d) : Co desiredAngle = tan((currentPose.y - targetPose.y).inMeters / (currentPose.x - targetPose.x).inMeters) .radians + + // 30.meters, 27.meters is goal, 3 meters / 4 + // 17.5 meters 13.5 meters is goal + + + Logger.recordOutput("AutoLevel/desiredAngleTargetPoseCmd", desiredAngle.inDegrees) val thetaFeedback = thetaPID.calculate(currentPose.rotation, desiredAngle) drivetrain.currentRequest = From 41ca73147313c8194c41f425082cb00e80281177 Mon Sep 17 00:00:00 2001 From: Shom770 Date: Tue, 16 Jan 2024 15:58:16 -0500 Subject: [PATCH 04/11] working dt: --- .../com/team4099/robot2023/RobotContainer.kt | 2 - .../commands/drivetrain/TargetPoseCommand.kt | 129 ------------------ .../team4099/robot2023/config/ControlBoard.kt | 2 - 3 files changed, 133 deletions(-) delete mode 100644 src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index f42e7050..c04891d5 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -2,7 +2,6 @@ package com.team4099.robot2023 import com.team4099.robot2023.auto.AutonomousSelector import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand -import com.team4099.robot2023.commands.drivetrain.TargetPoseCommand import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand import com.team4099.robot2023.config.ControlBoard import com.team4099.robot2023.config.constants.Constants @@ -118,7 +117,6 @@ object RobotContainer { fun mapTeleopControls() { ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain, toAngle = 180.degrees)) - ControlBoard.groundIntakeTest.whileTrue(TargetPoseCommand(drivetrain, Pose2d(27.feet, 13.5.feet, 0.0.degrees))) // ControlBoard.autoLevel.whileActiveContinuous( // GoToAngle(drivetrain).andThen(AutoLevel(drivetrain)) // ) diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt deleted file mode 100644 index c1221e2f..00000000 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt +++ /dev/null @@ -1,129 +0,0 @@ -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.inRadians -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) - - Logger.recordOutput("Odometry/targetedPose", doubleArrayOf(targetPose.x.inMeters, targetPose.y.inMeters, targetPose.rotation.inRadians)) - - 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 - - // 30.meters, 27.meters is goal, 3 meters / 4 - // 17.5 meters 13.5 meters is goal - - - Logger.recordOutput("AutoLevel/desiredAngleTargetPoseCmd", desiredAngle.inDegrees) - 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/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index d1383fad..2a3446ac 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -97,6 +97,4 @@ object ControlBoard { // for testing val setArmCommand = Trigger { technician.yButton } - - val groundIntakeTest = Trigger { driver.aButton } } From 609dade0d35c6865f6e3be8844fbadac4f5eb7ad Mon Sep 17 00:00:00 2001 From: Saraansh Wadkar Date: Fri, 19 Jan 2024 19:31:12 -0500 Subject: [PATCH 05/11] add wrapper class on velocity and position voltage requests --- .../team4099/lib/phoenix6/PositionVoltage.kt | 61 ++++++++++++++++++ .../team4099/lib/phoenix6/VelocityVoltage.kt | 64 +++++++++++++++++++ 2 files changed, 125 insertions(+) create mode 100644 src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt create mode 100644 src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt diff --git a/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt b/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt new file mode 100644 index 00000000..51816cb9 --- /dev/null +++ b/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt @@ -0,0 +1,61 @@ +package com.team4099.lib.phoenix6 + +import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotations +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inRotationsPerSecond +import org.team4099.lib.units.perSecond +import com.ctre.phoenix6.controls.PositionVoltage as PositionVoltagePhoenix6 + +class PositionVoltage( + var position: Angle, // Assuming an AngularPosition type exists similar to AngularVelocity + var enableFOC: Boolean = true, + var feedforward: ElectricalPotential = 0.0.volts, + var slot: Int = 0, + var overrideBrakeDurNeutral: Boolean = false, + var limitForwardMotion: Boolean = false, + var limitReverseMotion: Boolean = false, + var velocity: AngularVelocity = 0.0.degrees.perSecond, +) { + + val positionVoltagePhoenix6 = PositionVoltagePhoenix6(position.inRotations, velocity.inRotationsPerSecond, enableFOC, feedforward.inVolts, slot, overrideBrakeDurNeutral, limitForwardMotion, limitReverseMotion) + + fun setPosition(new_position: Angle) { + position = new_position + positionVoltagePhoenix6.Position = new_position.inRotations + } + + fun setEnableFOC(new_enableFOC: Boolean) { + enableFOC = new_enableFOC + positionVoltagePhoenix6.EnableFOC = new_enableFOC + } + + fun setFeedforward(new_feedforward: ElectricalPotential) { + feedforward = new_feedforward + positionVoltagePhoenix6.FeedForward = new_feedforward.inVolts + } + + fun setSlot(new_slot: Int) { + slot = new_slot + positionVoltagePhoenix6.Slot = new_slot + } + + fun setOverrideBrakeDurNeutral(new_override: Boolean) { + overrideBrakeDurNeutral = new_override + positionVoltagePhoenix6.OverrideBrakeDurNeutral = new_override + } + + fun setLimitForwardMotion(new_limitForward: Boolean) { + limitForwardMotion = new_limitForward + positionVoltagePhoenix6.LimitForwardMotion = new_limitForward + } + + fun setLimitReverseMotion(new_limitReverse: Boolean) { + limitReverseMotion = new_limitReverse + positionVoltagePhoenix6.LimitReverseMotion = new_limitReverse + } +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt b/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt new file mode 100644 index 00000000..6efd986b --- /dev/null +++ b/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt @@ -0,0 +1,64 @@ +package com.team4099.lib.phoenix6 + +import com.ctre.phoenix6.controls.VelocityVoltage as VelocityVoltagePhoenix6 +import org.team4099.lib.units.AngularAcceleration +import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inRotationsPerSecond +import org.team4099.lib.units.inRotationsPerSecondPerSecond +import org.team4099.lib.units.perSecond + +class VelocityVoltage(var velocity: AngularVelocity, + var acceleration: AngularAcceleration = 0.0.degrees.perSecond.perSecond, + var enableFOC:Boolean = true, + var feedforward: ElectricalPotential = 0.0.volts, + var slot:Int = 0, + var overrideBrakeDurNeutral: Boolean = false, + var limitForwardMotion: Boolean = false, + var limitReverseMotion: Boolean = false){ + + val velocityVoltagePhoenix6 = VelocityVoltagePhoenix6(velocity.inRotationsPerSecond, acceleration.inRotationsPerSecondPerSecond, enableFOC, feedforward.inVolts, slot, overrideBrakeDurNeutral, limitForwardMotion, limitReverseMotion) + + fun setVelocity(new_velocity: AngularVelocity) { + velocity = new_velocity + velocityVoltagePhoenix6.Velocity = velocity.inRotationsPerSecond + } + + fun setAcceleration(new_accel: AngularAcceleration) { + acceleration = new_accel + velocityVoltagePhoenix6.Acceleration = acceleration.inRotationsPerSecondPerSecond + } + + fun setEnableFOC(new_enableFOC: Boolean) { + enableFOC = new_enableFOC + velocityVoltagePhoenix6.EnableFOC = new_enableFOC + } + + fun setFeedforward(new_feedforward: ElectricalPotential) { + feedforward = new_feedforward + velocityVoltagePhoenix6.FeedForward = new_feedforward.inVolts + } + + fun setSlot(new_slot: Int) { + slot = new_slot + velocityVoltagePhoenix6.Slot = new_slot + } + + fun setOverrideBrakeDurNeutral(new_override: Boolean) { + overrideBrakeDurNeutral = new_override + velocityVoltagePhoenix6.OverrideBrakeDurNeutral = new_override + } + + fun setLimitForwardMotion(new_limitForward: Boolean) { + limitForwardMotion = new_limitForward + velocityVoltagePhoenix6.LimitForwardMotion = new_limitForward + } + + fun setLimitReverseMotion(new_limitReverse: Boolean) { + limitReverseMotion = new_limitReverse + velocityVoltagePhoenix6.LimitReverseMotion = new_limitReverse + } +} \ No newline at end of file From f44998d0c18996625b1033c61fca8982a2c48774 Mon Sep 17 00:00:00 2001 From: Saraansh Wadkar Date: Fri, 19 Jan 2024 20:50:12 -0500 Subject: [PATCH 06/11] update falconutils to 1.1.28 --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index a21179a3..ba2238c0 100644 --- a/build.gradle +++ b/build.gradle @@ -83,7 +83,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'org.jetbrains.kotlin:kotlin-test-junit5' - implementation 'com.github.team4099:FalconUtils:1.1.26' + implementation 'com.github.team4099:FalconUtils:1.1.28' implementation 'org.apache.commons:commons-collections4:4.0' implementation 'com.google.code.gson:gson:2.10.1' implementation "io.javalin:javalin:5.3.2" From 5efa1ad33ff5d4e9066d29e97af9cbff478db035 Mon Sep 17 00:00:00 2001 From: Shom770 Date: Fri, 19 Jan 2024 23:15:02 -0500 Subject: [PATCH 07/11] delete intake stuff for now so we can create a pr --- .../robot2023/subsystems/intake/Intake.kt | 113 ------------------ .../robot2023/subsystems/intake/IntakeIO.kt | 63 ---------- .../subsystems/intake/IntakeIONEO.kt | 100 ---------------- .../subsystems/intake/IntakeIOSim.kt | 53 -------- 4 files changed, 329 deletions(-) delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt deleted file mode 100644 index e65f207e..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt +++ /dev/null @@ -1,113 +0,0 @@ -package com.team4099.robot2023.subsystems.intake - -import com.team4099.lib.hal.Clock -import com.team4099.robot2023.config.constants.Constants -import com.team4099.robot2023.config.constants.IntakeConstants -import com.team4099.robot2023.subsystems.superstructure.Request -import edu.wpi.first.wpilibj2.command.Command -import edu.wpi.first.wpilibj2.command.SubsystemBase -import org.littletonrobotics.junction.Logger -import org.team4099.lib.units.base.inSeconds -import org.team4099.lib.units.derived.ElectricalPotential -import org.team4099.lib.units.derived.inVolts -import org.team4099.lib.units.derived.volts - -class Intake(val io: IntakeIO) : SubsystemBase() { - val inputs = IntakeIO.IntakeIOInputs() - var rollerVoltageTarget: ElectricalPotential = 0.0.volts - var isZeroed = false - var currentState: IntakeState = IntakeState.UNINITIALIZED - - var currentRequest: Request.IntakeRequest = Request.IntakeRequest.Idle() - set(value) { - rollerVoltageTarget = when (value) { - is Request.IntakeRequest.OpenLoop -> { - value.rollerVoltage - } - - is Request.IntakeRequest.Idle -> { - IntakeConstants.IDLE_ROLLER_VOLTAGE - } - } - - field = value - } - - private var timeProfileGeneratedAt = Clock.fpgaTime - - override fun periodic() { - io.updateInputs(inputs) - - Logger.processInputs("GroundIntake", inputs) - Logger.recordOutput("GroundIntake/currentState", currentState.name) - Logger.recordOutput("GroundIntake/requestedState", currentRequest.javaClass.simpleName) - Logger.recordOutput("GroundIntake/isZeroed", isZeroed) - - if (Constants.Tuning.DEBUGING_MODE) { - Logger.recordOutput("GroundIntake/isAtCommandedState", currentState.equivalentToRequest(currentRequest)) - Logger.recordOutput("GroundIntake/timeProfileGeneratedAt", timeProfileGeneratedAt.inSeconds) - Logger.recordOutput("GroundIntake/rollerVoltageTarget", rollerVoltageTarget.inVolts) - } - - var nextState = currentState - when (currentState) { - IntakeState.UNINITIALIZED -> { - // Outputs - // No designated output functionality because targeting position will take care of it next - // loop cycle - - // Transitions - nextState = IntakeState.IDLE - } - IntakeState.IDLE -> { - setRollerVoltage(IntakeConstants.IDLE_ROLLER_VOLTAGE) - - // Transitions - nextState = fromRequestToState(currentRequest) - } - IntakeState.OPEN_LOOP -> { - setRollerVoltage(rollerVoltageTarget) - - // Transitions - nextState = fromRequestToState(currentRequest) - } - } - - // The next loop cycle, we want to run ground intake at the state that was requested. setting - // current state to the next state ensures that we run the logic for the state we want in the - // next loop cycle. - currentState = nextState - } - - /** @param appliedVoltage Represents the applied voltage of the roller motor */ - fun setRollerVoltage(appliedVoltage: ElectricalPotential) { - io.setRollerVoltage(appliedVoltage) - } - - fun generateIntakeTestCommand(): Command { - val returnCommand = runOnce({ currentRequest = Request.IntakeRequest.OpenLoop(12.volts) }) - return returnCommand - } - - companion object { - enum class IntakeState { - UNINITIALIZED, - IDLE, - OPEN_LOOP; - - fun equivalentToRequest(request: Request.IntakeRequest): Boolean { - return ( - (request is Request.IntakeRequest.OpenLoop && this == OPEN_LOOP) || - (request is Request.IntakeRequest.Idle && this == IDLE) - ) - } - } - - fun fromRequestToState(request: Request.IntakeRequest): IntakeState { - return when (request) { - is Request.IntakeRequest.OpenLoop -> IntakeState.OPEN_LOOP - is Request.IntakeRequest.Idle -> IntakeState.IDLE - } - } - } -} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt deleted file mode 100644 index 6ba96d7e..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt +++ /dev/null @@ -1,63 +0,0 @@ -package com.team4099.robot2023.subsystems.intake - -import org.littletonrobotics.junction.LogTable -import org.littletonrobotics.junction.inputs.LoggableInputs -import org.team4099.lib.units.base.amps -import org.team4099.lib.units.base.celsius -import org.team4099.lib.units.base.inAmperes -import org.team4099.lib.units.base.inCelsius -import org.team4099.lib.units.derived.ElectricalPotential -import org.team4099.lib.units.derived.inVolts -import org.team4099.lib.units.derived.rotations -import org.team4099.lib.units.derived.volts -import org.team4099.lib.units.inRotationsPerMinute -import org.team4099.lib.units.perMinute -import org.team4099.lib.units.perSecond - -interface IntakeIO { - class IntakeIOInputs : LoggableInputs { - var rollerVelocity = 0.0.rotations.perMinute - var rollerAppliedVoltage = 0.0.volts - - var rollerSupplyCurrent = 0.0.amps - var rollerStatorCurrent = 0.0.amps - - var rollerTemp = 0.0.celsius - - var isSimulated = false - - override fun toLog(table: LogTable?) { - table?.put("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute) - table?.put("rollerAppliedVoltage", rollerAppliedVoltage.inVolts) - table?.put("rollerSupplyCurrentAmps", rollerSupplyCurrent.inAmperes) - table?.put("rollerStatorCurrentAmps", rollerStatorCurrent.inAmperes) - table?.put("rollerTempCelsius", rollerTemp.inCelsius) - } - - override fun fromLog(table: LogTable?) { - table?.get("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute)?.let { - rollerVelocity = it.rotations.perSecond - } - - table?.get("rollerAppliedVoltage", rollerAppliedVoltage.inVolts)?.let { - rollerAppliedVoltage = it.volts - } - - table?.get("rollerSupplyCurrentAmps", rollerSupplyCurrent.inAmperes)?.let { - rollerSupplyCurrent = it.amps - } - - table?.get("rollerStatorCurrentAmps", rollerStatorCurrent.inAmperes)?.let { - rollerStatorCurrent = it.amps - } - - table?.get("rollerTempCelsius", rollerTemp.inCelsius)?.let { rollerTemp = it.celsius } - } - } - - fun updateInputs(io: IntakeIOInputs) {} - - fun setRollerVoltage(voltage: ElectricalPotential) {} - - fun setRollerBrakeMode(brake: Boolean) {} -} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt deleted file mode 100644 index dadcd0db..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt +++ /dev/null @@ -1,100 +0,0 @@ -package com.team4099.robot2023.subsystems.intake - -import com.revrobotics.CANSparkMax -import com.revrobotics.CANSparkMaxLowLevel -import com.team4099.lib.math.clamp -import com.team4099.robot2023.config.constants.Constants -import com.team4099.robot2023.config.constants.IntakeConstants -import com.team4099.robot2023.subsystems.falconspin.MotorChecker -import com.team4099.robot2023.subsystems.falconspin.MotorCollection -import com.team4099.robot2023.subsystems.falconspin.Neo -import org.littletonrobotics.junction.Logger -import org.team4099.lib.units.base.amps -import org.team4099.lib.units.base.celsius -import org.team4099.lib.units.base.inAmperes -import org.team4099.lib.units.derived.ElectricalPotential -import org.team4099.lib.units.derived.inVolts -import org.team4099.lib.units.derived.volts -import org.team4099.lib.units.sparkMaxAngularMechanismSensor -import kotlin.math.absoluteValue - -object IntakeIONEO : IntakeIO { - private val rollerSparkMax = - CANSparkMax(Constants.Intake.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) - - private val rollerSensor = - sparkMaxAngularMechanismSensor( - rollerSparkMax, IntakeConstants.ROLLER_GEAR_RATIO, IntakeConstants.VOLTAGE_COMPENSATION - ) - - init { - rollerSparkMax.restoreFactoryDefaults() - rollerSparkMax.clearFaults() - - rollerSparkMax.enableVoltageCompensation(IntakeConstants.VOLTAGE_COMPENSATION.inVolts) - rollerSparkMax.setSmartCurrentLimit(IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes.toInt()) - rollerSparkMax.inverted = IntakeConstants.ROLLER_MOTOR_INVERTED - - rollerSparkMax.idleMode = CANSparkMax.IdleMode.kCoast - - rollerSparkMax.openLoopRampRate = 0.0 - rollerSparkMax.burnFlash() - - MotorChecker.add( - "Ground Intake", - "Roller", - MotorCollection( - mutableListOf(Neo(rollerSparkMax, "Roller Motor")), - IntakeConstants.ROLLER_CURRENT_LIMIT, - 70.celsius, - IntakeConstants.ROLLER_CURRENT_LIMIT - 0.amps, - 90.celsius - ), - ) - } - - override fun updateInputs(inputs: IntakeIO.IntakeIOInputs) { - inputs.rollerVelocity = rollerSensor.velocity - inputs.rollerAppliedVoltage = rollerSparkMax.busVoltage.volts * rollerSparkMax.appliedOutput - inputs.rollerStatorCurrent = rollerSparkMax.outputCurrent.amps - - // BusVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent - // AppliedVoltage = percentOutput * BusVoltage - // SupplyCurrent = (percentOutput * BusVoltage / BusVoltage) * StatorCurrent = - // percentOutput * statorCurrent - inputs.rollerSupplyCurrent = - inputs.rollerStatorCurrent * rollerSparkMax.appliedOutput.absoluteValue - inputs.rollerTemp = rollerSparkMax.motorTemperature.celsius - - Logger.recordOutput( - "Intake/rollerMotorOvercurrentFault", - rollerSparkMax.getFault(CANSparkMax.FaultID.kOvercurrent) - ) - Logger.recordOutput("Intake/busVoltage", rollerSparkMax.busVoltage) - } - - /** - * Sets the roller motor voltage, ensures the voltage is limited to battery voltage compensation - * - * @param voltage the voltage to set the roller motor to - */ - override fun setRollerVoltage(voltage: ElectricalPotential) { - rollerSparkMax.setVoltage( - clamp(voltage, -IntakeConstants.VOLTAGE_COMPENSATION, IntakeConstants.VOLTAGE_COMPENSATION) - .inVolts - ) - } - - /** - * Sets the roller motor brake mode - * - * @param brake if it brakes - */ - override fun setRollerBrakeMode(brake: Boolean) { - if (brake) { - rollerSparkMax.idleMode = CANSparkMax.IdleMode.kBrake - } else { - rollerSparkMax.idleMode = CANSparkMax.IdleMode.kCoast - } - } -} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt deleted file mode 100644 index 801b2e84..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt +++ /dev/null @@ -1,53 +0,0 @@ -package com.team4099.robot2023.subsystems.intake - -import com.team4099.lib.math.clamp -import com.team4099.robot2023.config.constants.Constants -import com.team4099.robot2023.config.constants.IntakeConstants -import edu.wpi.first.math.system.plant.DCMotor -import edu.wpi.first.wpilibj.simulation.FlywheelSim -import org.team4099.lib.units.base.amps -import org.team4099.lib.units.base.celsius -import org.team4099.lib.units.base.inSeconds -import org.team4099.lib.units.derived.ElectricalPotential -import org.team4099.lib.units.derived.inVolts -import org.team4099.lib.units.derived.rotations -import org.team4099.lib.units.derived.volts -import org.team4099.lib.units.perMinute - -object IntakeIOSim : IntakeIO { - private val rollerSim: FlywheelSim = FlywheelSim( - DCMotor.getNEO(1), - IntakeConstants.ROLLER_GEAR_RATIO, - IntakeConstants.ROLLER_INERTIA - ) - - private var appliedVoltage = 0.volts; - init{} - - override fun updateInputs(inputs: IntakeIO.IntakeIOInputs) { - - rollerSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) - - inputs.rollerVelocity = rollerSim.getAngularVelocityRPM().rotations.perMinute - inputs.rollerAppliedVoltage = appliedVoltage - inputs.rollerSupplyCurrent = 0.amps - inputs.rollerStatorCurrent = rollerSim.currentDrawAmps.amps - inputs.rollerTemp = 0.0.celsius - - inputs.isSimulated = true - } - - override fun setRollerVoltage(voltage: ElectricalPotential) { - appliedVoltage = voltage - rollerSim.setInputVoltage( - clamp( - voltage, - -IntakeConstants.VOLTAGE_COMPENSATION, - IntakeConstants.VOLTAGE_COMPENSATION - ) - .inVolts - ) - } - - override fun setRollerBrakeMode(brake: Boolean) {} -} From 74cbc93f60a635347a49c4b2bdb1c28ed2d5ea93 Mon Sep 17 00:00:00 2001 From: Shom770 Date: Fri, 19 Jan 2024 23:16:32 -0500 Subject: [PATCH 08/11] bring back intake stuff --- .../kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt | 0 .../kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt | 0 .../com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt | 0 .../com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt | 0 4 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt new file mode 100644 index 00000000..e69de29b diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt new file mode 100644 index 00000000..e69de29b diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt new file mode 100644 index 00000000..e69de29b diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt new file mode 100644 index 00000000..e69de29b From af159c1c33de0333c6b94da944e524b947088fca Mon Sep 17 00:00:00 2001 From: Matthew Choulas Date: Fri, 19 Jan 2024 23:33:57 -0500 Subject: [PATCH 09/11] rebase --- .../config/constants/IntakeConstants.kt | 17 ------- .../robot2023/subsystems/intake/Intake.kt | 0 .../robot2023/subsystems/intake/IntakeIO.kt | 0 .../subsystems/intake/IntakeIONEO.kt | 0 .../subsystems/intake/IntakeIOSim.kt | 0 .../subsystems/superstructure/Request.kt | 46 ------------------- 6 files changed, 63 deletions(-) delete mode 100644 src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt deleted file mode 100644 index 00b49039..00000000 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt +++ /dev/null @@ -1,17 +0,0 @@ -package com.team4099.robot2023.config.constants - -import org.team4099.lib.units.base.amps -import org.team4099.lib.units.derived.volts - -object IntakeConstants { - val ROLLER_INERTIA = 0.002459315 // this one has been updated - val VOLTAGE_COMPENSATION = 12.0.volts - - // TODO: Change gear ratio according to robot - val ROLLER_CURRENT_LIMIT = 50.0.amps - const val ROLLER_MOTOR_INVERTED = true - const val ROLLER_GEAR_RATIO = 24.0 / 12.0 // this one has been updated - - // TODO: Update value - val IDLE_ROLLER_VOLTAGE = 2.0.volts -} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt deleted file mode 100644 index e69de29b..00000000 diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt deleted file mode 100644 index e69de29b..00000000 diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt deleted file mode 100644 index e69de29b..00000000 diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt deleted file mode 100644 index e69de29b..00000000 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 0f144610..200f125b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -17,52 +17,6 @@ import org.team4099.lib.units.perSecond sealed interface Request { - sealed interface SuperstructureRequest : Request { - class Idle() : SuperstructureRequest - class Home() : SuperstructureRequest - - class GroundIntakeCube() : SuperstructureRequest - class GroundIntakeCone() : SuperstructureRequest - - class EjectGamePiece() : SuperstructureRequest - - class DoubleSubstationIntakePrep(val gamePiece: GamePiece) : SuperstructureRequest - class SingleSubstationIntakePrep(val gamePiece: GamePiece) : SuperstructureRequest - - class DoubleSubstationIntake() : SuperstructureRequest - class SingleSubstationIntake(val gamePiece: GamePiece) : SuperstructureRequest - - class PrepScore(val gamePiece: GamePiece, val nodeTier: NodeTier) : SuperstructureRequest - - class Score() : SuperstructureRequest - - class Tuning() : SuperstructureRequest - } - - // Implements RequestStructure to ensure standardized structure - sealed interface ManipulatorRequest : Request { - class TargetingPosition(val position: Length, val rollerVoltage: ElectricalPotential) : - ManipulatorRequest - class OpenLoop(val voltage: ElectricalPotential, val rollerVoltage: ElectricalPotential) : - ManipulatorRequest - class Home() : ManipulatorRequest - } - - sealed interface ElevatorRequest : Request { - class TargetingPosition( - val position: Length, - val finalVelocity: LinearVelocity = 0.0.inches.perSecond, - val canContinueBuffer: Length = 5.0.inches - ) : ElevatorRequest - class OpenLoop(val voltage: ElectricalPotential) : ElevatorRequest - class Home : ElevatorRequest - } - - sealed interface IntakeRequest : Request { - class OpenLoop(val rollerVoltage: ElectricalPotential) : IntakeRequest - class Idle() : IntakeRequest - } - sealed interface DrivetrainRequest : Request { class OpenLoop( val angularVelocity: AngularVelocity, From 9b206f853120b850fbd130a397fb31ab6f0ee74d Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Sat, 20 Jan 2024 18:38:01 -0500 Subject: [PATCH 10/11] Update pathplanner version --- vendordeps/PathplannerLib.json | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index eb271997..f86862cc 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.0.0-beta-6", + "version": "2024.1.3", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.0.0-beta-6" + "version": "2024.1.3" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.0.0-beta-6", + "version": "2024.1.3", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, From 21269031576b4fd6c4a3978669bebdff4e7e094c Mon Sep 17 00:00:00 2001 From: Shayaan Wadkar <82843611+Shom770@users.noreply.github.com> Date: Sat, 20 Jan 2024 18:59:44 -0500 Subject: [PATCH 11/11] update .gitignore to ignore the build constants file --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.gitignore b/.gitignore index 3325f5c0..8842e0b4 100644 --- a/.gitignore +++ b/.gitignore @@ -138,6 +138,7 @@ $RECYCLE.BIN/ ### Gradle ### .gradle /build/ +BuildConstants.kt # Ignore Gradle GUI config gradle-app.setting @@ -168,5 +169,6 @@ out/ # Fleet .fleet + # Simulation GUI and other tools window save file *-window.json