From 247f355bd6314b3957f3945c7e52d370c70fe2d4 Mon Sep 17 00:00:00 2001 From: Jeffery Ji Date: Fri, 8 Dec 2023 17:40:34 -0500 Subject: [PATCH] added crappy dynamic profiler command that spits out a .wpilog file. --- .../team449/control/holonomic/SwerveDrive.kt | 8 ++- .../holonomic/sysid/SwerveDynamicProfiler.kt | 62 +++++++++++++++++++ .../robot2023/auto/routines/RoutineChooser.kt | 2 +- .../robot2023/constants/RobotConstants.kt | 15 +++-- 4 files changed, 76 insertions(+), 11 deletions(-) create mode 100644 src/main/kotlin/frc/team449/control/holonomic/sysid/SwerveDynamicProfiler.kt diff --git a/src/main/kotlin/frc/team449/control/holonomic/SwerveDrive.kt b/src/main/kotlin/frc/team449/control/holonomic/SwerveDrive.kt index 91300bd..36d71e1 100644 --- a/src/main/kotlin/frc/team449/control/holonomic/SwerveDrive.kt +++ b/src/main/kotlin/frc/team449/control/holonomic/SwerveDrive.kt @@ -138,15 +138,19 @@ open class SwerveDrive( } /** @return An array of [SwerveModulePosition] for each module, containing distance and angle. */ - private fun getPositions(): Array { + fun getPositions(): Array { return Array(modules.size) { i -> modules[i].position } } /** @return An array of [SwerveModuleState] for each module, containing speed and angle. */ - private fun getStates(): Array { + fun getStates(): Array { return Array(modules.size) { i -> modules[i].state } } + fun getVoltages(): Array { + return Array(modules.size) { i -> modules[i].lastDrivingVoltage() } + } + private fun setRobotPose() { this.field.robotPose = this.pose this.field.getObject("FL").pose = this.pose.plus(Transform2d(Translation2d(SwerveConstants.WHEELBASE / 2, SwerveConstants.TRACKWIDTH / 2), this.getPositions()[0].angle)) diff --git a/src/main/kotlin/frc/team449/control/holonomic/sysid/SwerveDynamicProfiler.kt b/src/main/kotlin/frc/team449/control/holonomic/sysid/SwerveDynamicProfiler.kt new file mode 100644 index 0000000..b5bc48e --- /dev/null +++ b/src/main/kotlin/frc/team449/control/holonomic/sysid/SwerveDynamicProfiler.kt @@ -0,0 +1,62 @@ +package frc.team449.control.holonomic.sysid + +import edu.wpi.first.math.kinematics.ChassisSpeeds +import edu.wpi.first.util.datalog.DoubleLogEntry +import edu.wpi.first.wpilibj.DataLogManager +import edu.wpi.first.wpilibj.Timer +import edu.wpi.first.wpilibj2.command.Command +import frc.team449.control.holonomic.SwerveDrive + +/** + * Before using this set kS, kA, kP, kI, kD = 0 and kV = 1 + */ +class SwerveDynamicProfiler( + private val drive: SwerveDrive +) : Command() { + val timer = Timer() + + val log = DataLogManager.getLog() + val time = DoubleLogEntry(log, "time") + val frontLeftVol = DoubleLogEntry(log, "front left voltage") + val frontLeftVel = DoubleLogEntry(log, "front left velocity") + val frontLeftPos = DoubleLogEntry(log, "front left position") + val frontRightVol = DoubleLogEntry(log, "front right voltage") + val frontRightVel = DoubleLogEntry(log, "front right velocity") + val frontRightPos = DoubleLogEntry(log, "front right position") + val backRightVol = DoubleLogEntry(log, "back right voltage") + val backRightVel = DoubleLogEntry(log, "back right velocity") + val backRightPos = DoubleLogEntry(log, "back right position") + val backLeftVol = DoubleLogEntry(log, "back left voltage") + val backLeftVel = DoubleLogEntry(log, "back left velocity") + val backLeftPos = DoubleLogEntry(log, "back left position") + + init { + addRequirements(drive) + timer.restart() + + DataLogManager.start() + } + + override fun execute() { + val v_goal = 2.0 * timer.get() / 5 + drive.set(ChassisSpeeds(v_goal, 0.0, 0.0)) + val positions = drive.getPositions() + val velocities = drive.getStates() + val voltages = drive.getVoltages() + if (v_goal > 0) { + time.append(timer.get()) + frontLeftVol.append(voltages[0]) + frontRightVol.append(voltages[1]) + backRightVol.append(voltages[2]) + backLeftVol.append(voltages[3]) + frontLeftPos.append(positions[0].distanceMeters) + frontRightPos.append(positions[1].distanceMeters) + backRightPos.append(positions[2].distanceMeters) + backLeftPos.append(positions[3].distanceMeters) + frontLeftVel.append(velocities[0].speedMetersPerSecond) + frontRightVel.append(velocities[1].speedMetersPerSecond) + backRightVel.append(velocities[2].speedMetersPerSecond) + backLeftVel.append(velocities[3].speedMetersPerSecond) + } + } +} diff --git a/src/main/kotlin/frc/team449/robot2023/auto/routines/RoutineChooser.kt b/src/main/kotlin/frc/team449/robot2023/auto/routines/RoutineChooser.kt index 7870daa..76c1cf1 100644 --- a/src/main/kotlin/frc/team449/robot2023/auto/routines/RoutineChooser.kt +++ b/src/main/kotlin/frc/team449/robot2023/auto/routines/RoutineChooser.kt @@ -34,7 +34,7 @@ class RoutineChooser(private val robot: Robot) : SendableChooser() { ) this.addOption( - "One Piece and Drop", + "One Piece and Drop, WIP, Don't use.", if (isRed) { "RedOnePieceDrop" } else { diff --git a/src/main/kotlin/frc/team449/robot2023/constants/RobotConstants.kt b/src/main/kotlin/frc/team449/robot2023/constants/RobotConstants.kt index c3fbf06..e5c3772 100644 --- a/src/main/kotlin/frc/team449/robot2023/constants/RobotConstants.kt +++ b/src/main/kotlin/frc/team449/robot2023/constants/RobotConstants.kt @@ -25,18 +25,17 @@ object RobotConstants { const val MAX_LINEAR_SPEED = SwerveConstants.MAX_ATTAINABLE_MK4I_SPEED // m/s const val MAX_ROT_SPEED = PI // rad/s val MAX_ACCEL = 4 * DCMotor( - MotorConstants.NOMINAL_VOLTAGE, - MotorConstants.STALL_TORQUE, - MotorConstants.STALL_CURRENT, - MotorConstants.FREE_CURRENT, - MotorConstants.FREE_SPEED, - 1 - ).getTorque(90.0)/ + MotorConstants.NOMINAL_VOLTAGE, + MotorConstants.STALL_TORQUE, + MotorConstants.STALL_CURRENT, + MotorConstants.FREE_CURRENT, + MotorConstants.FREE_SPEED, + 1 + ).getTorque(90.0) / (Units.inchesToMeters(2.0) * ROBOT_WEIGHT * SwerveConstants.DRIVE_GEARING) // m/s/s val INITIAL_POSE = Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0)) - init { println("Max Accel $MAX_ACCEL") }