From 7607f51f9576ecdd290043bb2a940137617cef02 Mon Sep 17 00:00:00 2001 From: Yama Date: Sat, 22 Jun 2024 16:50:57 -0400 Subject: [PATCH] create test drive velocity command (needs to be tested) --- .../com/team4099/robot2023/RobotContainer.kt | 1 + .../team4099/robot2023/config/ControlBoard.kt | 5 +-- .../config/constants/DrivetrainConstants.kt | 4 +++ .../subsystems/drivetrain/drive/Drivetrain.kt | 35 ++++++++++++------- .../superstructure/Superstructure.kt | 22 ++++++++++-- 5 files changed, 50 insertions(+), 17 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index b7a212c4..44ef4f67 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -240,6 +240,7 @@ object RobotContainer { ControlBoard.passingShot.whileTrue(superstructure.passingShotCommand()) ControlBoard.underStagePassingShot.whileTrue(superstructure.underStageCommand()) ControlBoard.testWrist.whileTrue(superstructure.testWristCommand()) + ControlBoard.testDriveVelocity.whileTrue(superstructure.testDriveVelocityCommand()) /* ControlBoard.targetAmp.whileTrue( diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index a4e3779e..e36d6a94 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -55,8 +55,7 @@ object ControlBoard { // val score = Trigger {driver.leftTriggerAxis > 0.5} // val intake = Trigger { driver.rightShoulderButton} - // TODO: uncomment - //val targetAmp = Trigger { driver.aButton } + val targetAmp = Trigger { driver.aButton } val prepAmp = Trigger { operator.aButton } val prepLow = Trigger { operator.xButton } val prepHighProtected = Trigger { operator.bButton } @@ -72,6 +71,8 @@ object ControlBoard { val testWrist = Trigger { driver.aButton } + val testDriveVelocity = Trigger { driver.aButton } + val characterizeWrist = Trigger { driver.rightShoulderButton } val climbAlignFar = Trigger { driver.dPadUp } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt index 4598ced7..655b5e4a 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -58,6 +58,10 @@ object DrivetrainConstants { const val GYRO_RATE_COEFFICIENT = 0.0 // TODO: Change this value + val testXVelocity = 1.0.meters.perSecond + val testYVelocity = 1.0.meters.perSecond + val testOmega = 1.0.radians.perSecond + val SLOW_AUTO_VEL = 2.meters.perSecond val SLOW_AUTO_ACCEL = 2.0.meters.perSecond.perSecond 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 9819c6c7..50fbfc4d 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 @@ -31,27 +31,38 @@ import org.team4099.lib.geometry.Rotation3d import org.team4099.lib.geometry.Transform2d import org.team4099.lib.geometry.Translation2d import org.team4099.lib.kinematics.ChassisSpeeds -import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.* 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 org.team4099.lib.units.derived.* import java.util.concurrent.locks.Lock import java.util.concurrent.locks.ReentrantLock import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class Drivetrain(private val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemBase() { + object TunableDriveStates { + val testXVelocity = + LoggedTunableValue( + "Drivetrain/testXVelocity", + DrivetrainConstants.testXVelocity, + Pair( { it.inMetersPerSecond }, { it.meters.perSecond }) + ) + val testYVelocity = + LoggedTunableValue( + "Drivetrain/testYVelocity", + DrivetrainConstants.testYVelocity, + Pair( { it.inMetersPerSecond }, { it.meters.perSecond }) + ) + val testOmega = + LoggedTunableValue( + "Drivetrain/testOmega", + DrivetrainConstants.testOmega, + Pair( { it.inRadiansPerSecond }, { it.radians.perSecond }) + ) + } + private val gyroNotConnectedAlert = Alert( "Gyro is not connected, field relative driving will be significantly worse.", diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt index 2f758847..da49c56f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt @@ -1,6 +1,7 @@ package com.team4099.robot2023.subsystems.superstructure import com.team4099.lib.hal.Clock +import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.config.constants.FieldConstants import com.team4099.robot2023.config.constants.FlywheelConstants import com.team4099.robot2023.config.constants.WristConstants @@ -24,7 +25,8 @@ import org.team4099.lib.geometry.Pose3d import org.team4099.lib.geometry.Rotation3d import org.team4099.lib.geometry.Transform3d import org.team4099.lib.geometry.Translation3d -import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.kinematics.ChassisSpeeds +import org.team4099.lib.units.* import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.base.inMilliseconds import org.team4099.lib.units.base.inSeconds @@ -35,8 +37,6 @@ import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inDegrees 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 class Superstructure( private val intake: Intake, @@ -1167,6 +1167,22 @@ class Superstructure( return returnCommand } + fun testDriveVelocityCommand(): Command { + val returnCommand = runOnce { + currentRequest= Request.SuperstructureRequest.Tuning() + drivetrain.currentRequest = + Request.DrivetrainRequest.ClosedLoop(edu.wpi.first.math.kinematics.ChassisSpeeds( + DrivetrainConstants.testXVelocity.inMetersPerSecond, + DrivetrainConstants.testYVelocity.inMetersPerSecond, + DrivetrainConstants.testOmega.inRadiansPerSecond + )) + } + returnCommand.name = "TestDriveVelocityCommand" + return returnCommand + } + + // TODO: Test Drivetrain Steer Command + companion object { enum class SuperstructureStates { UNINITIALIZED,