From 95b928210cbe15b008673a69f641926f694243c1 Mon Sep 17 00:00:00 2001 From: Matthew Choulas Date: Tue, 1 Aug 2023 21:41:13 -0400 Subject: [PATCH] swerve module steering tuning stuff --- .../drivetrain/SwerveModuleTuningCommand.kt | 28 ++++++++++++++ .../config/constants/FieldConstants.kt | 13 ++----- .../config/constants/VisionConstants.kt | 2 +- .../subsystems/drivetrain/drive/Drivetrain.kt | 17 ++++----- .../drivetrain/swervemodule/SwerveModule.kt | 13 +++++++ .../robot2023/subsystems/elevator/Elevator.kt | 6 +-- .../superstructure/Superstructure.kt | 37 ++++++++++++------- .../robot2023/subsystems/vision/Vision.kt | 8 +++- 8 files changed, 86 insertions(+), 38 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SwerveModuleTuningCommand.kt diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SwerveModuleTuningCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SwerveModuleTuningCommand.kt new file mode 100644 index 00000000..c82448d6 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SwerveModuleTuningCommand.kt @@ -0,0 +1,28 @@ +package com.team4099.robot2023.commands.drivetrain + +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import edu.wpi.first.math.kinematics.SwerveModuleState +import edu.wpi.first.wpilibj2.command.CommandBase +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.inRotation2ds + +class SwerveModuleTuningCommand(val drivetrain: Drivetrain, val steeringPosition: Angle) : + CommandBase() { + init { + addRequirements(drivetrain) + } + + override fun execute() { + for (module in drivetrain.swerveModules) { + module.setPositionClosedLoop( + SwerveModuleState(0.0, steeringPosition.inRotation2ds), + SwerveModuleState(0.0, steeringPosition.inRotation2ds), + false + ) + } + } + + override fun isFinished(): Boolean { + return false + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt index 469937cd..99f68f1f 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt @@ -46,15 +46,7 @@ object FieldConstants { Rotation3d(0.0.radians, 0.0.radians, 90.degrees) ) ), - AprilTag( - 5, - Pose3d( - (1).centi.meters, - (87.5).inches, - (89.3).centi.meters, - Rotation3d() - ) - ), + AprilTag(5, Pose3d((1).centi.meters, (87.5).inches, (89.3).centi.meters, Rotation3d())), AprilTag( 2, Pose3d( @@ -68,7 +60,8 @@ object FieldConstants { 6, Pose3d( (1).centi.meters, - (87.5).inches + 183.3.centi.meters, // FIRST's diagram has a typo (it says 147.19) // 91.8 + (87.5).inches + + 183.3.centi.meters, // FIRST's diagram has a typo (it says 147.19) // 91.8 (89.9).centi.meters, Rotation3d() ) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt index 89bf7658..bfce7c8c 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt @@ -39,7 +39,7 @@ object VisionConstants { val CAMERA_TRANSFORMS = listOf( Transform3d( - Translation3d(12.75.inches, 7.3125.inches, 28.75.inches), //18.69 + Translation3d(12.75.inches, 7.3125.inches, 28.75.inches), // 18.69 Rotation3d(180.degrees, 0.degrees, 0.degrees) ), // Transform3d( 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 d9e46f5f..f232f9b4 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 @@ -39,7 +39,6 @@ import org.team4099.lib.units.base.inMilliseconds import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.base.inches import org.team4099.lib.units.base.meters -import org.team4099.lib.units.centi import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inRadians @@ -122,7 +121,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB SwerveDriveOdometry( swerveDriveKinematics, gyroInputs.gyroYaw.inRotation2ds, - swerveModules.map { it. modulePosition }.toTypedArray() + swerveModules.map { it.modulePosition }.toTypedArray() ) var setPointStates = @@ -132,13 +131,13 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB var odometryPose: Pose2d get() = swerveDrivePoseEstimator.getLatestPose() -// get() { -// return Pose2d( -// 42.875.inches + 79.centi.meters + 14.inches + 2.75.inches, -// 113.25.inches + 1.inches, -// 180.degrees -// ) -// } + // get() { + // return Pose2d( + // 42.875.inches + 79.centi.meters + 14.inches + 2.75.inches, + // 113.25.inches + 1.inches, + // 180.degrees + // ) + // } set(value) { swerveDrivePoseEstimator.resetPose(value) 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 1ae148e1..6744241c 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,6 +30,8 @@ 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.withSign @@ -146,6 +148,17 @@ class SwerveModule(val io: SwerveModuleIO) { // "${io.label}/steeringValueDegreesWithMod", // inputs.steeringPosition.inDegrees.IEEErem(360.0) // ) + + Logger.getInstance().recordOutput("SwerveModule/SpeedSetPoint", speedSetPoint.inMetersPerSecond) + Logger.getInstance().recordOutput("SwerveModule/SteeringSetPoint", steeringSetPoint.inRadians) + Logger.getInstance() + .recordOutput( + "SwerveModule/AccelerationSetPoint", accelerationSetPoint.inMetersPerSecondPerSecond + ) + Logger.getInstance() + .recordOutput( + "SwerveModule/SteeringError", (steeringSetPoint - inputs.steeringPosition).inRadians + ) } /** diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt index e12f5d93..402277ab 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt @@ -337,9 +337,9 @@ class Elevator(val io: ElevatorIO) { get() = ( currentRequest is ElevatorRequest.TargetingPosition && - (inputs.elevatorPosition - elevatorPositionTarget).absoluteValue <= elevatorContinueBuffer - ) || - (TunableElevatorHeights.enableElevator.get() != 1.0) + (inputs.elevatorPosition - elevatorPositionTarget).absoluteValue <= + elevatorContinueBuffer + ) || (TunableElevatorHeights.enableElevator.get() != 1.0) init { TunableElevatorHeights 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 73e59d9a..1fc40a07 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt @@ -781,16 +781,23 @@ class Superstructure( } } - if ((manipulator.isAtTargetedPosition && - (elevator.isAtTargetedPosition || (NodeTier.HIGH == nodeTier && ( - (elevator.inputs.elevatorPosition - 48.inches).absoluteValue.inInches < 8 - ))) && - ( - groundIntake.isAtTargetedPosition || - groundIntake - .canContinueSafely - )) - ) { // because manipulator and elevator will move while we're + if (( + manipulator.isAtTargetedPosition && + ( + elevator.isAtTargetedPosition || + ( + NodeTier.HIGH == nodeTier && + ((elevator.inputs.elevatorPosition - 48.inches).absoluteValue.inInches < 8) + ) + ) && + ( + groundIntake.isAtTargetedPosition || + groundIntake + .canContinueSafely + ) + ) + ) { // because manipulator and elevator will move while + // we're // switching between different cone tiers val rollerCommandedVoltage = when (usingGamePiece) { @@ -846,10 +853,14 @@ class Superstructure( elevator.currentRequest = Request.ElevatorRequest.TargetingPosition(scoreHeight) - Logger.getInstance().recordOutput("Superstructure/PlsBruh", (elevator.inputs.elevatorPosition - scoreHeight).absoluteValue.inInches) - - if ((elevator.inputs.elevatorPosition - scoreHeight).absoluteValue.inInches < 8 || elevator.isAtTargetedPosition + Logger.getInstance() + .recordOutput( + "Superstructure/PlsBruh", + (elevator.inputs.elevatorPosition - scoreHeight).absoluteValue.inInches + ) + if ((elevator.inputs.elevatorPosition - scoreHeight).absoluteValue.inInches < 8 || + elevator.isAtTargetedPosition ) { val extension = diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt index 007014fa..b27c15cf 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt @@ -111,7 +111,9 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { // Logger.getInstance().recordOutput("Vision/${VisionConstants.CAMERA_NAMES[instance]}_transform", cameraPose.relativeTo(tuningPosition.toPose3d()).pose3d) robotPose = cameraPose.transformBy(cameraPoses[instance].inverse()).toPose2d() - println("CameraPoseX: ${cameraPose.x}, transformX: ${cameraPoses[instance].x}, robotPoseX: ${robotPose.x}") + println( + "CameraPoseX: ${cameraPose.x}, transformX: ${cameraPoses[instance].x}, robotPoseX: ${robotPose.x}" + ) } 2.0 -> { val error0 = values[1] @@ -192,7 +194,9 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { continue } - if ((robotPose.rotation - currentPose.rotation).absoluteValue > 7.degrees && DriverStation.isEnabled()){ + if ((robotPose.rotation - currentPose.rotation).absoluteValue > 7.degrees && + DriverStation.isEnabled() + ) { continue }