From 5a981fc88dfbbf3aa9a8d434802882dd595cd939 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 5 Sep 2024 22:30:49 +0300 Subject: [PATCH 1/2] Redid wheel radius characterization --- .../commands/WheelRadiusCharacterization.java | 116 ----------------- .../WheelRadiusCharacterizationCommand.java | 120 ++++++++++++++++++ 2 files changed, 120 insertions(+), 116 deletions(-) delete mode 100644 src/main/java/org/trigon/commands/WheelRadiusCharacterization.java create mode 100644 src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java diff --git a/src/main/java/org/trigon/commands/WheelRadiusCharacterization.java b/src/main/java/org/trigon/commands/WheelRadiusCharacterization.java deleted file mode 100644 index e52ba754..00000000 --- a/src/main/java/org/trigon/commands/WheelRadiusCharacterization.java +++ /dev/null @@ -1,116 +0,0 @@ -// Copyright (c) 2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file at -// the root directory of this project. - -package org.trigon.commands; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; -import org.trigon.utilities.Conversions; - -import java.util.Arrays; -import java.util.function.Consumer; -import java.util.function.DoubleSupplier; -import java.util.function.Supplier; - -public class WheelRadiusCharacterization extends Command { - private static final LoggedDashboardNumber CHARACTERIZATION_SPEED = - new LoggedDashboardNumber("WheelRadiusCharacterization/SpeedRadsPerSec", 0.1); - private final double driveRadius; - private final DoubleSupplier gyroYawRadsSupplier; - private final Supplier getWheelPositions; - private final Consumer runWheelRadiusCharacterization; - - public enum Direction { - CLOCKWISE(-1), - COUNTER_CLOCKWISE(1); - - Direction(int value) { - this.value = value; - } - - private final int value; - } - - private final Direction omegaDirection; - private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(1.0); - - private double lastGyroYawRads = 0.0; - private double accumGyroYawRads = 0.0; - - private double[] startWheelPositions; - - private double currentEffectiveWheelRadius = 0.0; - - public WheelRadiusCharacterization(Direction omegaDirection, double driveRadius, - DoubleSupplier gyroYawRadsSupplier, SubsystemBase requirement, - Consumer runWheelRadiusCharacterization, Supplier getWheelPositions - ) { - this.omegaDirection = omegaDirection; - this.driveRadius = driveRadius; - this.gyroYawRadsSupplier = gyroYawRadsSupplier; - this.getWheelPositions = getWheelPositions; - this.runWheelRadiusCharacterization = runWheelRadiusCharacterization; - addRequirements(requirement); - } - - @Override - public void initialize() { - // Reset - lastGyroYawRads = gyroYawRadsSupplier.getAsDouble(); - accumGyroYawRads = 0.0; - - startWheelPositions = getWheelRadiusCharacterizationPosition(); - - omegaLimiter.reset(0); - } - - @Override - public void execute() { - // Run drive at velocity - runWheelRadiusCharacterization.accept( - omegaLimiter.calculate(omegaDirection.value * CHARACTERIZATION_SPEED.get())); - - // Get yaw and wheel positions - accumGyroYawRads += MathUtil.angleModulus(gyroYawRadsSupplier.getAsDouble() - lastGyroYawRads); - lastGyroYawRads = gyroYawRadsSupplier.getAsDouble(); - double averageWheelPosition = 0.0; - double[] wheelPositions = getWheelRadiusCharacterizationPosition(); - for (int i = 0; i < 4; i++) { - averageWheelPosition += Math.abs(wheelPositions[i] - startWheelPositions[i]); - } - averageWheelPosition /= 4.0; - - currentEffectiveWheelRadius = (accumGyroYawRads * driveRadius) / averageWheelPosition; - Logger.recordOutput("Drive/RadiusCharacterization/DrivePosition", averageWheelPosition); - Logger.recordOutput("Drive/RadiusCharacterization/AccumGyroYawRads", accumGyroYawRads); - Logger.recordOutput( - "Drive/RadiusCharacterization/CurrentWheelRadiusMeters", - currentEffectiveWheelRadius); - } - - @Override - public void end(boolean interrupted) { - if (accumGyroYawRads <= Math.PI * 2.0) { - System.out.println("Not enough data for characterization"); - } else { - System.out.println( - "Effective Wheel Radius: " - + currentEffectiveWheelRadius - + " meters"); - } - } - - private double[] getWheelRadiusCharacterizationPosition() { - return Arrays.stream(getWheelPositions.get()).mapToDouble((pos) -> Units.rotationsToRadians(Conversions.distanceToRotations(pos.distanceMeters, 0.1016))).toArray(); - } -} \ No newline at end of file diff --git a/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java b/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java new file mode 100644 index 00000000..89077526 --- /dev/null +++ b/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java @@ -0,0 +1,120 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.trigon.robot.commands; + +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; + +import java.util.Arrays; +import java.util.function.DoubleConsumer; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class WheelRadiusCharacterizationCommand extends Command { + private static final LoggedDashboardNumber CHARACTERIZATION_SPEED = new LoggedDashboardNumber("WheelRadiusCharacterization/SpeedRadiansPerSecond", 1); + private static final LoggedDashboardNumber ROTATION_RATE_LIMIT = new LoggedDashboardNumber("WheelRadiusCharacterization/RotationRateLimit", 1); + private static final LoggedDashboardBoolean SHOULD_MOVE_CLOCKWISE = new LoggedDashboardBoolean("WheelRadiusCharacterization/ShouldMoveClockwise", false); + + private final double[] wheelDistancesFromCenterMeters; + private final Supplier wheelPositionsRadiansSupplier; + private final DoubleSupplier gyroYawRadiansSupplier; + private final DoubleConsumer runWheelRadiusCharacterization; + + private SlewRateLimiter rotationSlewRateLimiter = new SlewRateLimiter(ROTATION_RATE_LIMIT.get()); + private double startingGyroYawRadians; + private double accumulatedGyroYawRadians; + private double[] startingWheelPositions; + private double driveWheelsRadius; + + public WheelRadiusCharacterizationCommand(Translation2d[] wheelDistancesFromCenterMeters, Supplier wheelPositionsRadiansSupplier, + DoubleSupplier gyroYawRadiansSupplier, DoubleConsumer runWheelRadiusCharacterization, SubsystemBase requirement + ) { + this.wheelDistancesFromCenterMeters = Arrays.stream(wheelDistancesFromCenterMeters).mapToDouble(Translation2d::getNorm).toArray(); + this.wheelPositionsRadiansSupplier = wheelPositionsRadiansSupplier; + this.gyroYawRadiansSupplier = gyroYawRadiansSupplier; + this.runWheelRadiusCharacterization = runWheelRadiusCharacterization; + addRequirements(requirement); + } + + public WheelRadiusCharacterizationCommand(double[] wheelDistancesFromCenterMeters, Supplier wheelPositionsRadiansSupplier, + DoubleSupplier gyroYawRadiansSupplier, DoubleConsumer runWheelRadiusCharacterization, SubsystemBase requirement + ) { + this.wheelDistancesFromCenterMeters = wheelDistancesFromCenterMeters; + this.wheelPositionsRadiansSupplier = wheelPositionsRadiansSupplier; + this.gyroYawRadiansSupplier = gyroYawRadiansSupplier; + this.runWheelRadiusCharacterization = runWheelRadiusCharacterization; + addRequirements(requirement); + } + + @Override + public void initialize() { + configureStartingStats(); + } + + @Override + public void execute() { + driveMotors(); + + accumulatedGyroYawRadians = getAccumulatedGyroYaw(); + driveWheelsRadius = calculateDriveWheelRadius(); + + Logger.recordOutput("WheelRadiusCharacterization/AccumulatedGyroYawRadians", accumulatedGyroYawRadians); + Logger.recordOutput("RadiusCharacterization/DriveWheelRadius", driveWheelsRadius); + } + + @Override + public void end(boolean interrupted) { + printResults(); + } + + private void configureStartingStats() { + startingGyroYawRadians = gyroYawRadiansSupplier.getAsDouble(); + startingWheelPositions = wheelPositionsRadiansSupplier.get(); + accumulatedGyroYawRadians = 0.0; + + rotationSlewRateLimiter = new SlewRateLimiter(ROTATION_RATE_LIMIT.get()); + rotationSlewRateLimiter.reset(0); + } + + private void driveMotors() { + runWheelRadiusCharacterization.accept(rotationSlewRateLimiter.calculate(getRotationDirection() * CHARACTERIZATION_SPEED.get())); + } + + private double getAccumulatedGyroYaw() { + return Math.abs(startingGyroYawRadians - gyroYawRadiansSupplier.getAsDouble()); + } + + private double calculateDriveWheelRadius() { + driveWheelsRadius = 0; + final double[] wheelPositionsRadians = wheelPositionsRadiansSupplier.get(); + + for (int i = 0; i < 4; i++) { + final double accumulatedWheelRadians = Math.abs(wheelPositionsRadians[i] - startingWheelPositions[i]); + driveWheelsRadius += (accumulatedGyroYawRadians * wheelDistancesFromCenterMeters[i]) / accumulatedWheelRadians; + Logger.recordOutput("RadiusCharacterization/AccumulatedWheelRadians" + i, accumulatedWheelRadians); + } + + return driveWheelsRadius /= 4; + } + + private void printResults() { + if (accumulatedGyroYawRadians <= Math.PI * 2.0) + System.out.println("Not enough data for characterization"); + else + System.out.println("Drive Wheel Radius: " + driveWheelsRadius + " meters"); + } + + private int getRotationDirection() { + return SHOULD_MOVE_CLOCKWISE.get() ? -1 : 1; + } +} \ No newline at end of file From 919f8dfa03228896c51e2139c0f8022a1cdd77a1 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 5 Sep 2024 22:44:31 +0300 Subject: [PATCH 2/2] Fixed bug --- .../org/trigon/commands/WheelRadiusCharacterizationCommand.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java b/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java index 89077526..481a7d32 100644 --- a/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java +++ b/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java @@ -5,7 +5,7 @@ // license that can be found in the LICENSE file at // the root directory of this project. -package frc.trigon.robot.commands; +package org.trigon.commands; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Translation2d;