From 45434832ba0399c0491a6950cf2f2bb36cc839b2 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Fri, 9 Feb 2024 19:14:30 -0500 Subject: [PATCH] refactor(arm): Use motion profile calculator. --- ...Util.java => ArmFeedforwardCalculator.java} | 4 ++-- .../java/frc/lib/MotionProfileCalculator.java | 17 +++++++++++++++++ src/main/java/frc/robot/arm/ArmConstants.java | 4 +++- .../java/frc/robot/arm/ShoulderMotorIOSim.java | 4 ++-- .../frc/robot/arm/ShoulderMotorIOSparkMax.java | 5 +++-- .../java/frc/robot/swerve/SwerveConstants.java | 18 ++++-------------- 6 files changed, 31 insertions(+), 21 deletions(-) rename src/main/java/frc/lib/{FeedforwardUtil.java => ArmFeedforwardCalculator.java} (83%) create mode 100644 src/main/java/frc/lib/MotionProfileCalculator.java diff --git a/src/main/java/frc/lib/FeedforwardUtil.java b/src/main/java/frc/lib/ArmFeedforwardCalculator.java similarity index 83% rename from src/main/java/frc/lib/FeedforwardUtil.java rename to src/main/java/frc/lib/ArmFeedforwardCalculator.java index 770deec..e3b423f 100644 --- a/src/main/java/frc/lib/FeedforwardUtil.java +++ b/src/main/java/frc/lib/ArmFeedforwardCalculator.java @@ -2,8 +2,8 @@ import edu.wpi.first.math.geometry.Rotation2d; -/** Helper class to assist with feedforward calculations. */ -public class FeedforwardUtil { +/** Helper class to assist with arm feedforward calculations. */ +public class ArmFeedforwardCalculator { /** * Calculates the gravity compensation constant for an arm given the voltage applied at an diff --git a/src/main/java/frc/lib/MotionProfileCalculator.java b/src/main/java/frc/lib/MotionProfileCalculator.java new file mode 100644 index 0000000..7c37ac5 --- /dev/null +++ b/src/main/java/frc/lib/MotionProfileCalculator.java @@ -0,0 +1,17 @@ +package frc.lib; + +/** Helper class to assist with motion profile calculations. */ +public class MotionProfileCalculator { + + /** + * Calculates an acceleration using a ramp duration. + * + * @param maximumSpeed the maximum speed in units per second. + * @param desiredRampDurationSeconds the desired duration to ramp from no speed to full speed. + * @return the acceleration in units per second per second. + */ + public static double calculateAcceleration( + double maximumSpeed, double desiredRampDurationSeconds) { + return maximumSpeed / desiredRampDurationSeconds; + } +} diff --git a/src/main/java/frc/robot/arm/ArmConstants.java b/src/main/java/frc/robot/arm/ArmConstants.java index 37851e6..e7c0590 100644 --- a/src/main/java/frc/robot/arm/ArmConstants.java +++ b/src/main/java/frc/robot/arm/ArmConstants.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import frc.lib.CAN; +import frc.lib.MotionProfileCalculator; /** Constants for the arm subsystem. */ public class ArmConstants { @@ -33,7 +34,8 @@ public static class ShoulderMotorConstants { public static final double MAXIMUM_SPEED = 1.5; /** Maximum acceleration of the shoulder joint in rotations per second per second. */ - public static final double MAXIMUM_ACCELERATION = 5.0; + public static final double MAXIMUM_ACCELERATION = + MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.3); } /** Constants for the elbow motor. */ diff --git a/src/main/java/frc/robot/arm/ShoulderMotorIOSim.java b/src/main/java/frc/robot/arm/ShoulderMotorIOSim.java index e5c345f..eb81b94 100644 --- a/src/main/java/frc/robot/arm/ShoulderMotorIOSim.java +++ b/src/main/java/frc/robot/arm/ShoulderMotorIOSim.java @@ -7,7 +7,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; -import frc.lib.FeedforwardUtil; +import frc.lib.ArmFeedforwardCalculator; import frc.robot.RobotConstants; import frc.robot.arm.ArmConstants.ShoulderMotorConstants; @@ -44,7 +44,7 @@ public ShoulderMotorIOSim() { feedforward = new ArmFeedforward( 0, - FeedforwardUtil.calculateArmGravityCompensation( + ArmFeedforwardCalculator.calculateArmGravityCompensation( Rotation2d.fromDegrees(70.81), 0.101859), 0); } diff --git a/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java b/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java index d9ad2fc..2e9fa36 100644 --- a/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java +++ b/src/main/java/frc/robot/arm/ShoulderMotorIOSparkMax.java @@ -7,8 +7,8 @@ import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import frc.lib.ArmFeedforwardCalculator; import frc.lib.Configurator; -import frc.lib.FeedforwardUtil; import frc.robot.arm.ArmConstants.ShoulderMotorConstants; /** Shoulder motor using a Spark Max. */ @@ -38,7 +38,8 @@ public ShoulderMotorIOSparkMax() { feedforward = new ArmFeedforward( 0, - FeedforwardUtil.calculateArmGravityCompensation(Rotation2d.fromDegrees(18.0), 0.1222), + ArmFeedforwardCalculator.calculateArmGravityCompensation( + Rotation2d.fromDegrees(18.0), 0.1222), 0); } diff --git a/src/main/java/frc/robot/swerve/SwerveConstants.java b/src/main/java/frc/robot/swerve/SwerveConstants.java index aea6b86..56209cc 100644 --- a/src/main/java/frc/robot/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/swerve/SwerveConstants.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; +import frc.lib.MotionProfileCalculator; import frc.lib.PIDFConstants; /** Constants for the swerve subsystem. */ @@ -93,20 +94,9 @@ private static double calculateMaximumAttainableSpeed( /** Maximum speed in meters per second. */ public static final double MAXIMUM_SPEED = MAXIMUM_ATTAINABLE_SPEED; - /** - * Calculates an acceleration using a ramp duration. - * - * @param maximumSpeed the maximum speed in units per second. - * @param desiredRampDurationSeconds the desired duration to ramp from no speed to full speed. - * @return the acceleration in units per second per second. - */ - private static double calculateAcceleration( - double maximumSpeed, double desiredRampDurationSeconds) { - return maximumSpeed / desiredRampDurationSeconds; - } - /** Maximum acceleration in meters per second per second. */ - public static final double MAXIMUM_ACCELERATION = calculateAcceleration(MAXIMUM_SPEED, 0.1); + public static final double MAXIMUM_ACCELERATION = + MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.1); /** Maximum attainable rotational speed in rotations per second. */ public static final double MAXIMUM_ATTAINABLE_ROTATION_SPEED = @@ -117,7 +107,7 @@ private static double calculateAcceleration( /** Maximum acceleration in rotations per second per second. */ public static final double MAXIMUM_ROTATION_ACCELERATION = - calculateAcceleration(MAXIMUM_ROTATION_SPEED, 0.1); + MotionProfileCalculator.calculateAcceleration(MAXIMUM_ROTATION_SPEED, 0.1); public static final PIDFConstants DRIVE_PIDF_CONSTANTS = new PIDFConstants();