From 9c086fdecea65e866bbdc4fae263515e0b61890a Mon Sep 17 00:00:00 2001 From: David <98991028+ActualSkinwalker@users.noreply.github.com> Date: Tue, 7 Nov 2023 19:22:38 -0500 Subject: [PATCH 1/5] Various acceleration related changes Added the applyCircularDeadband() method to ControlsUtilities Modified DriveCommand to declare nextXSpeed, nextYSpeed, and nextThetaSpeed using ternary expressions. --- .../java/frc/robot/commands/DriveCommand.java | 23 +++---------------- .../frc/robot/util/ControlsUtilities.java | 12 ++++++++++ 2 files changed, 15 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java index 1c23374..3d3538a 100644 --- a/src/main/java/frc/robot/commands/DriveCommand.java +++ b/src/main/java/frc/robot/commands/DriveCommand.java @@ -94,26 +94,9 @@ public void execute() { double absoluteNextThetaSpeed = Math.abs(nextThetaSpeed); // Apply global deadband. - if ( - absoluteNextXSpeed < DriveCommand.X_DEADBAND && - absoluteNextYSpeed < DriveCommand.Y_DEADBAND && - absoluteNextThetaSpeed < DriveCommand.THETA_DEADBAND - ) { - - nextXSpeed = 0; - nextYSpeed = 0; - nextThetaSpeed = 0; - - } else { - - // Enforce maximum change in acceleration. - nextXSpeed = ControlsUtilities.enforceMaximumPositiveDelta(currentXSpeed, nextXSpeed, X_MAX_ACCELERATION); - nextYSpeed = ControlsUtilities.enforceMaximumPositiveDelta(currentYSpeed, nextYSpeed, Y_MAX_ACCELERATION); - nextThetaSpeed = ControlsUtilities.enforceMaximumPositiveDelta(currentThetaSpeed, nextThetaSpeed, THETA_MAX_ACCELERATION); - - } - - + nextXSpeed = absoluteNextXSpeed < DriveCommand.X_DEADBAND ? 0 : ControlsUtilities.enforceMaximumPositiveDelta(currentXSpeed, nextXSpeed, X_MAX_ACCELERATION); + nextYSpeed = absoluteNextYSpeed < DriveCommand.Y_DEADBAND ? 0 : ControlsUtilities.enforceMaximumPositiveDelta(currentYSpeed, nextYSpeed, Y_MAX_ACCELERATION); + nextThetaSpeed = absoluteNextThetaSpeed < DriveCommand.THETA_DEADBAND ? 0 : ControlsUtilities.enforceMaximumPositiveDelta(currentThetaSpeed, nextThetaSpeed, THETA_MAX_ACCELERATION); this.speedMultiplier = slowMode.getAsBoolean() ? 0.25 : 1; diff --git a/src/main/java/frc/robot/util/ControlsUtilities.java b/src/main/java/frc/robot/util/ControlsUtilities.java index 70814c5..f2843f0 100644 --- a/src/main/java/frc/robot/util/ControlsUtilities.java +++ b/src/main/java/frc/robot/util/ControlsUtilities.java @@ -24,6 +24,18 @@ public static double applyDeadband(double input, double deadband) { return Math.abs(input) < deadband ? 0 : input; } + + public static double applyCircularDeadband (double xInput, double yInput, double deadband, boolean valueToRetrieve) { + + double hypotenuse = Math.sqrt(Math.pow(xInput, 2) + Math.pow(yInput, 2)); + + if (Math.abs(hypotenuse) < deadband) { + if (valueToRetrieve) return xInput; + else return yInput; + } + + else return 0; + } /** * Returns the new value so long as its delta from the old value does not From 7b080906e5f2c2908d9b1d29514dbd5dd51f0f61 Mon Sep 17 00:00:00 2001 From: David <98991028+ActualSkinwalker@users.noreply.github.com> Date: Tue, 21 Nov 2023 11:01:27 -0500 Subject: [PATCH 2/5] Started work on Kinematics --- .../frc/robot/subsystems/SwerveModule.java | 1 - src/main/java/frc/robot/util/Kinematics.java | 68 ++++ src/main/java/frc/robot/util/Transform.java | 86 +++++ src/main/java/frc/robot/util/Vector.java | 299 ++++++++++++++++++ 4 files changed, 453 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/util/Kinematics.java create mode 100644 src/main/java/frc/robot/util/Transform.java create mode 100644 src/main/java/frc/robot/util/Vector.java diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 3028f63..426c53b 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -15,7 +15,6 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.util.AccelerationCurve; public class SwerveModule extends SubsystemBase { diff --git a/src/main/java/frc/robot/util/Kinematics.java b/src/main/java/frc/robot/util/Kinematics.java new file mode 100644 index 0000000..083a822 --- /dev/null +++ b/src/main/java/frc/robot/util/Kinematics.java @@ -0,0 +1,68 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.util; + +import com.kauailabs.navx.frc.AHRS; + +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.wpilibj.Timer; + +/** Add your docs here. */ +public class Kinematics { + + AHRS gyro; + + Timer xAccelTimer, yAccelTimer; + + public Kinematics (AHRS gyro) { + this.gyro = gyro; + xAccelTimer = new Timer(); + yAccelTimer = new Timer(); + xAccelTimer.start(); + yAccelTimer.start(); + } + + public boolean directionHasChanged (Vector vectorOne, Vector vectorTwo) { + return getDirectionChange(vectorOne, vectorTwo) >= 1; + } + + public double getRobotAccelerationX () { + return gyro.getWorldLinearAccelX(); + } + + public double getRobotAccelerationY () { + return gyro.getWorldLinearAccelY(); + } + + private double totalDisplacementY; + public double getRobotDisplacementY () { + return 0; + } + + private double totalDisplacementX; + public double getRobotDisplacementX () { + return 0; + } + + private void resetTimerX () { + xAccelTimer.reset(); + } + + public Vector getAccelerationVector() { + return new Vector<>( + Nat.N1(), + getRobotAccelerationX(), + getRobotAccelerationY()); + } + + public double getDirectionChange (Vector vectorOne, Vector vectorTwo) { + return Vector.getAngle(vectorTwo) - Vector.getAngle(vectorOne); + } + + public Transform2d getRobotRelativePosition () { + return null; + } +} diff --git a/src/main/java/frc/robot/util/Transform.java b/src/main/java/frc/robot/util/Transform.java new file mode 100644 index 0000000..9dd6da6 --- /dev/null +++ b/src/main/java/frc/robot/util/Transform.java @@ -0,0 +1,86 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.util; + +/** + * Represents a functional mapping of a real number onto another real number. + */ +@FunctionalInterface +public interface Transform { + /** + * f(x) = x + */ + public static final Transform NONE = x -> x; + + /** + * f(x) = -x + */ + public static final Transform NEGATE = x -> -x; + + /** + * The mathematical sign (sgn) function. + */ + public static final Transform SIGN = x -> x == 0 ? 0 : (x > 0 ? 1 : -1); + + /** + * Gets a {@link Transform} which clamps a value to a closed interval. + * @param low The minimum output value. + * @param high The maximum output value. + * @return The described clamp transform. + */ + public static Transform clamp (double low, double high) { + return input -> Math.min(Math.max(input, low), high); + } + + /** + * Gets a linear {@link Transform} which applies the equation {@code y = mx + b} + * to the parameter for the given {@code m} and {@code b}. + * @param m The slope of the line represented by this linear transform. + * @param b The y-intercept of the line represented by this linear transform. + * @return The linear transform. + */ + public static Transform linear (double m, double b) { + return x -> m*x + b; + } + + /** + * Turns a transform into an even function by reflecting all x values less than zero across the y-axis. + * @param transform The base {@link Transform} to turn into an even function. + * @return The symmetrical (even function) version of the given transform. + */ + public static Transform toEven (Transform transform) { + return x -> transform.apply(x >= 0 ? x : -x); + } + + /** + * Turns a transform into an odd function by reflecting all x values less than zero across the y-axis + * and negating the output for those reflected values. Note that {@code Transform.toOdd(f).apply(0)} will always + * yield 0 for any transform function {@code f}. + * @param transform The base {@link Transform} to turn into an odd function. + * @return The symmetrical (odd function) version of the given transform. + */ + public static Transform toOdd (Transform transform) { + return x -> toEven(transform).apply(x) * SIGN.apply(x); + } + + /** + * Composes this {@link Transform} with another. That is, + * {@code g.then(f)} is equivalent to the mathematical notation + * "f(g(x))". + * @param transform The transform to be applied after this one finishes. + * @return The composition of the two transforms. + */ + public default Transform then (Transform transform) { + return x -> transform.apply(this.apply(x)); + } + + /** + * Apply the functional mapping described by this transformation. + * @param x The {@code double} input to the function. + * @return The {@code double} output from the function. + */ + public double apply (double x); + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/Vector.java b/src/main/java/frc/robot/util/Vector.java new file mode 100644 index 0000000..744e204 --- /dev/null +++ b/src/main/java/frc/robot/util/Vector.java @@ -0,0 +1,299 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.util; +import java.util.Optional; + +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.Num; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.numbers.N4; + +/** + * A vector class which can be used for calculations. + */ + + //This file was originally written in CLAW +public class Vector { + + private Optional magnitude = Optional.empty(); + private Optional angle = Optional.empty(); + private final Nat dimensionality; + private final double[] components; + + /** + * Gets a {@link Vector} from a list of components and the dimensionality of the vector. + * @param dim The dimensionality of the vector as an instance of {@link Nat}. If this dimensionality + * does not match the length of the {@code components} array, an exception will be thrown. + * @param components An array containing the vector's components. + */ + public Vector (Nat dim, double... components) { + if (dim.getNum() != components.length) + throw new IllegalArgumentException("The number of components given for this Vector does not match the dimensionality of the Vector"); + dimensionality = dim; + this.components = components.clone(); + } + + /** + * Create a new one-dimensional vector {@code } from the given x component. + * @param x The x component of the vector. + * @return The vector {@code }. + */ + public static Vector from (double x) { + return new Vector<>(Nat.N1(), x); + } + + /** + * Create a new two-dimensional vector {@code } from the given components. + * @param x The x component of the vector. + * @param y The y component of the vector. + * @return The vector {@code }. + */ + public static Vector from (double x, double y) { + return new Vector<>(Nat.N2(), x, y); + } + + /** + * Create a new three-dimensional vector {@code } from the given components. + * @param x The x component of the vector. + * @param y The y component of the vector. + * @param z The z component of the vector. + * @return The vector {@code }. + */ + public static Vector from (double x, double y, double z) { + return new Vector<>(Nat.N3(), x, y, z); + } + + /** + * Create a new four-dimensional vector {@code } from the given components. + * @param x The x component of the vector. + * @param y The y component of the vector. + * @param z The z component of the vector. + * @param w The w component of the vector. + * @return The vector {@code }. + */ + public static Vector from (double x, double y, double z, double w) { + return new Vector<>(Nat.N4(), x, y, z, w); + } + + /** + * Get the dimensionality of this vector (i.e. the number of components it has), as an integer. + * @return The dimensionality of this vector. + */ + public int getDimensionality () { + return dimensionality.getNum(); + } + + /** + * Get the {@code i+1}th component of this vector. For example, + * {@code getComponent(2)} would return the third component. + * @param i The index of the component to retrieve. + * @return The {@code i+1}th component of this vector. + * @throws IllegalArgumentException If the given component index {@code i} is invalid for this vector + * (i.e. no {@code i+1}th component exists for this vector). + */ + public double getComponent (int i) throws IllegalArgumentException { + if (i < 0) { + throw new IllegalArgumentException("Cannot retrieve a component with index less than zero"); + } else if (i >= getDimensionality()) { + throw new IllegalArgumentException( + "Cannot retrieve component with index "+i+" from a " + + getDimensionality()+"-dimensional vector" + ); + } else { + return components[i]; + } + } + + /** + * Get the x (1st) component of this vector. + * If this vector's dimensionality is less than one, an exception will be thrown. + * @return The x component of this vector. + */ + public double getX () { + return getComponent(0); + } + + /** + * Get the y (2nd) component of this vector. + * If this vector's dimensionality is less than two, an exception will be thrown. + * @return The y component of this vector. + */ + public double getY () { + return getComponent(1); + } + + /** + * Get the z (3rd) component of this vector. + * If this vector's dimensionality is less than three, an exception will be thrown. + * @return The z component of this vector. + */ + public double getZ () { + return getComponent(2); + } + + /** + * Get the w (4th) component of this vector. + * If this vector's dimensionality is less than four, an exception will be thrown. + * @return The w component of this vector. + */ + public double getW () { + return getComponent(3); + } + + /** + * Gets the magnitude of the vector. + * @return The magnitude. + */ + public double getMagnitude () { + if (magnitude.isEmpty()) { + double squaredComponents = 0; + for (double component : components) + squaredComponents += component * component; + + magnitude = Optional.of(Math.sqrt(squaredComponents)); + } + + return magnitude.get(); + } + + /** + * Applies a {@link Transform} to every component of this vector, returning a vector + * with the same dimensionality but with all components being the application of the + * transform on this vector's corresponding component. + * @param transform The transform to apply to each component. + * @return The result of the transformation applied to each component of this vector. + */ + public Vector apply (Transform transform) { + double[] newComponents = new double[components.length]; + for (int i = 0; i < components.length; i ++) + newComponents[i] = transform.apply(components[i]); + return new Vector(dimensionality, newComponents); + } + + /** + * Returns this vector scaled by a given coefficient. + * @param k The coefficient to scale this vector by. + * @return The scaled vector result. + */ + public Vector scale (double k) { + return this.apply(a -> a*k); + } + + /** + * Returns this vector scaled so that the new magnitude is equal to the given magnitude. If this vector's magnitude + * is zero, a zero vector will be returned. + * @param magnitude The magnitude of the vector after scaling. + * @return This vector, scale such that the magnitude equals the provided magnitude. + */ + public Vector scaleToMagnitude (double magnitude) { + if (getMagnitude() == 0) + return this.scale(0); + else + return this.scale(magnitude / getMagnitude()); + } + + /** + * Negates this vector, resulting in a vector of equal magnitude and opposite direction. + * @return This vector, but negated. + */ + public Vector negate () { + return this.apply(a -> -a); + } + + /** + * Apply an operation to every component of the vector + */ + private Vector apply (Vector other, Operation operation) { + double[] newComponents = new double[components.length]; + for (int i = 0; i < components.length; i ++) + newComponents[i] = operation.apply(components[i], other.components[i]); + return new Vector(dimensionality, newComponents); + } + + private static interface Operation { + public double apply (double a, double b); + } + + /** + * Calculate the dot product of this vector and other. + * @param other The vector to calculate the dot product with. + * @return The dot product. + */ + public double dotProduct (Vector other) { + // Compute a vector containing the products of this vector and the other vector's components + Vector productVector = this.apply(other, (a, b) -> a * b); + + // Sum the product components + double sum = 0; + for (double component : productVector.components) + sum += component; + + // Return the dot product + return sum; + } + + /** + * Add this vector and another together. + * @param other The vector to add. + * @return The sum of the two vectors. + */ + public Vector add (Vector other) { + return this.apply(other, (a, b) -> a + b); + } + + /** + * Subtract another vector from this one. + * @param other The vector to subtract. + * @return The difference of the two vectors. + */ + public Vector subtract (Vector other) { + return this.apply(other, (a, b) -> a - b); + } + + /** + * Returns the angle formed between a two-dimensional vector and the x-axis, in radians. This angle + * increases counterclockwise. For example, a vector facing in the +y direction will return {@code pi/2}. + * @param vector The two-dimensional vector to retrieve the direction angle of. + * @return The angle of the vector, in radians. + */ + public static double getAngle (Vector vector) { + if (vector.angle.isEmpty()) { + + // Set vector.angle as a cache + + if (vector.getMagnitude() == 0) { + + // Return 0 if the magnitude is 0 to prevent dividing by zero (or yielding a nonsensical answer) + vector.angle = Optional.of(0.); + + } else { + + // Get the X component of the vector if it were on the unit circle so we can use arccos + double unitX = vector.components[0] / vector.getMagnitude(); + + // Get the angle if the vector were above the x-axis + double angle = Math.acos(unitX); + + // Use the angle directly if the vector is above the x-axis, or modify it to flip across the x-axis otherwise + vector.angle = Optional.of(vector.components[1] >= 0 ? (angle) : (2*Math.PI - angle)); + + } + + } + + return vector.angle.get(); + } + + @Override + public String toString () { + String[] componentsStrings = new String[components.length]; + for (int i = 0; i < components.length; i ++) + componentsStrings[i] = Double.toString(components[i]); + return "<" + String.join(", ", componentsStrings) + ">"; + } + +} \ No newline at end of file From 3890d8b8e29b31842a5eaa697968438c8a3ffcfa Mon Sep 17 00:00:00 2001 From: David <98991028+ActualSkinwalker@users.noreply.github.com> Date: Tue, 21 Nov 2023 11:02:43 -0500 Subject: [PATCH 3/5] Revert "Started work on Kinematics" This reverts commit 7b080906e5f2c2908d9b1d29514dbd5dd51f0f61. --- .../frc/robot/subsystems/SwerveModule.java | 1 + src/main/java/frc/robot/util/Kinematics.java | 68 ---- src/main/java/frc/robot/util/Transform.java | 86 ----- src/main/java/frc/robot/util/Vector.java | 299 ------------------ 4 files changed, 1 insertion(+), 453 deletions(-) delete mode 100644 src/main/java/frc/robot/util/Kinematics.java delete mode 100644 src/main/java/frc/robot/util/Transform.java delete mode 100644 src/main/java/frc/robot/util/Vector.java diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 426c53b..3028f63 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -15,6 +15,7 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.util.AccelerationCurve; public class SwerveModule extends SubsystemBase { diff --git a/src/main/java/frc/robot/util/Kinematics.java b/src/main/java/frc/robot/util/Kinematics.java deleted file mode 100644 index 083a822..0000000 --- a/src/main/java/frc/robot/util/Kinematics.java +++ /dev/null @@ -1,68 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.util; - -import com.kauailabs.navx.frc.AHRS; - -import edu.wpi.first.math.Nat; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.wpilibj.Timer; - -/** Add your docs here. */ -public class Kinematics { - - AHRS gyro; - - Timer xAccelTimer, yAccelTimer; - - public Kinematics (AHRS gyro) { - this.gyro = gyro; - xAccelTimer = new Timer(); - yAccelTimer = new Timer(); - xAccelTimer.start(); - yAccelTimer.start(); - } - - public boolean directionHasChanged (Vector vectorOne, Vector vectorTwo) { - return getDirectionChange(vectorOne, vectorTwo) >= 1; - } - - public double getRobotAccelerationX () { - return gyro.getWorldLinearAccelX(); - } - - public double getRobotAccelerationY () { - return gyro.getWorldLinearAccelY(); - } - - private double totalDisplacementY; - public double getRobotDisplacementY () { - return 0; - } - - private double totalDisplacementX; - public double getRobotDisplacementX () { - return 0; - } - - private void resetTimerX () { - xAccelTimer.reset(); - } - - public Vector getAccelerationVector() { - return new Vector<>( - Nat.N1(), - getRobotAccelerationX(), - getRobotAccelerationY()); - } - - public double getDirectionChange (Vector vectorOne, Vector vectorTwo) { - return Vector.getAngle(vectorTwo) - Vector.getAngle(vectorOne); - } - - public Transform2d getRobotRelativePosition () { - return null; - } -} diff --git a/src/main/java/frc/robot/util/Transform.java b/src/main/java/frc/robot/util/Transform.java deleted file mode 100644 index 9dd6da6..0000000 --- a/src/main/java/frc/robot/util/Transform.java +++ /dev/null @@ -1,86 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.util; - -/** - * Represents a functional mapping of a real number onto another real number. - */ -@FunctionalInterface -public interface Transform { - /** - * f(x) = x - */ - public static final Transform NONE = x -> x; - - /** - * f(x) = -x - */ - public static final Transform NEGATE = x -> -x; - - /** - * The mathematical sign (sgn) function. - */ - public static final Transform SIGN = x -> x == 0 ? 0 : (x > 0 ? 1 : -1); - - /** - * Gets a {@link Transform} which clamps a value to a closed interval. - * @param low The minimum output value. - * @param high The maximum output value. - * @return The described clamp transform. - */ - public static Transform clamp (double low, double high) { - return input -> Math.min(Math.max(input, low), high); - } - - /** - * Gets a linear {@link Transform} which applies the equation {@code y = mx + b} - * to the parameter for the given {@code m} and {@code b}. - * @param m The slope of the line represented by this linear transform. - * @param b The y-intercept of the line represented by this linear transform. - * @return The linear transform. - */ - public static Transform linear (double m, double b) { - return x -> m*x + b; - } - - /** - * Turns a transform into an even function by reflecting all x values less than zero across the y-axis. - * @param transform The base {@link Transform} to turn into an even function. - * @return The symmetrical (even function) version of the given transform. - */ - public static Transform toEven (Transform transform) { - return x -> transform.apply(x >= 0 ? x : -x); - } - - /** - * Turns a transform into an odd function by reflecting all x values less than zero across the y-axis - * and negating the output for those reflected values. Note that {@code Transform.toOdd(f).apply(0)} will always - * yield 0 for any transform function {@code f}. - * @param transform The base {@link Transform} to turn into an odd function. - * @return The symmetrical (odd function) version of the given transform. - */ - public static Transform toOdd (Transform transform) { - return x -> toEven(transform).apply(x) * SIGN.apply(x); - } - - /** - * Composes this {@link Transform} with another. That is, - * {@code g.then(f)} is equivalent to the mathematical notation - * "f(g(x))". - * @param transform The transform to be applied after this one finishes. - * @return The composition of the two transforms. - */ - public default Transform then (Transform transform) { - return x -> transform.apply(this.apply(x)); - } - - /** - * Apply the functional mapping described by this transformation. - * @param x The {@code double} input to the function. - * @return The {@code double} output from the function. - */ - public double apply (double x); - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/Vector.java b/src/main/java/frc/robot/util/Vector.java deleted file mode 100644 index 744e204..0000000 --- a/src/main/java/frc/robot/util/Vector.java +++ /dev/null @@ -1,299 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.util; -import java.util.Optional; - -import edu.wpi.first.math.Nat; -import edu.wpi.first.math.Num; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N2; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.numbers.N4; - -/** - * A vector class which can be used for calculations. - */ - - //This file was originally written in CLAW -public class Vector { - - private Optional magnitude = Optional.empty(); - private Optional angle = Optional.empty(); - private final Nat dimensionality; - private final double[] components; - - /** - * Gets a {@link Vector} from a list of components and the dimensionality of the vector. - * @param dim The dimensionality of the vector as an instance of {@link Nat}. If this dimensionality - * does not match the length of the {@code components} array, an exception will be thrown. - * @param components An array containing the vector's components. - */ - public Vector (Nat dim, double... components) { - if (dim.getNum() != components.length) - throw new IllegalArgumentException("The number of components given for this Vector does not match the dimensionality of the Vector"); - dimensionality = dim; - this.components = components.clone(); - } - - /** - * Create a new one-dimensional vector {@code } from the given x component. - * @param x The x component of the vector. - * @return The vector {@code }. - */ - public static Vector from (double x) { - return new Vector<>(Nat.N1(), x); - } - - /** - * Create a new two-dimensional vector {@code } from the given components. - * @param x The x component of the vector. - * @param y The y component of the vector. - * @return The vector {@code }. - */ - public static Vector from (double x, double y) { - return new Vector<>(Nat.N2(), x, y); - } - - /** - * Create a new three-dimensional vector {@code } from the given components. - * @param x The x component of the vector. - * @param y The y component of the vector. - * @param z The z component of the vector. - * @return The vector {@code }. - */ - public static Vector from (double x, double y, double z) { - return new Vector<>(Nat.N3(), x, y, z); - } - - /** - * Create a new four-dimensional vector {@code } from the given components. - * @param x The x component of the vector. - * @param y The y component of the vector. - * @param z The z component of the vector. - * @param w The w component of the vector. - * @return The vector {@code }. - */ - public static Vector from (double x, double y, double z, double w) { - return new Vector<>(Nat.N4(), x, y, z, w); - } - - /** - * Get the dimensionality of this vector (i.e. the number of components it has), as an integer. - * @return The dimensionality of this vector. - */ - public int getDimensionality () { - return dimensionality.getNum(); - } - - /** - * Get the {@code i+1}th component of this vector. For example, - * {@code getComponent(2)} would return the third component. - * @param i The index of the component to retrieve. - * @return The {@code i+1}th component of this vector. - * @throws IllegalArgumentException If the given component index {@code i} is invalid for this vector - * (i.e. no {@code i+1}th component exists for this vector). - */ - public double getComponent (int i) throws IllegalArgumentException { - if (i < 0) { - throw new IllegalArgumentException("Cannot retrieve a component with index less than zero"); - } else if (i >= getDimensionality()) { - throw new IllegalArgumentException( - "Cannot retrieve component with index "+i+" from a " + - getDimensionality()+"-dimensional vector" - ); - } else { - return components[i]; - } - } - - /** - * Get the x (1st) component of this vector. - * If this vector's dimensionality is less than one, an exception will be thrown. - * @return The x component of this vector. - */ - public double getX () { - return getComponent(0); - } - - /** - * Get the y (2nd) component of this vector. - * If this vector's dimensionality is less than two, an exception will be thrown. - * @return The y component of this vector. - */ - public double getY () { - return getComponent(1); - } - - /** - * Get the z (3rd) component of this vector. - * If this vector's dimensionality is less than three, an exception will be thrown. - * @return The z component of this vector. - */ - public double getZ () { - return getComponent(2); - } - - /** - * Get the w (4th) component of this vector. - * If this vector's dimensionality is less than four, an exception will be thrown. - * @return The w component of this vector. - */ - public double getW () { - return getComponent(3); - } - - /** - * Gets the magnitude of the vector. - * @return The magnitude. - */ - public double getMagnitude () { - if (magnitude.isEmpty()) { - double squaredComponents = 0; - for (double component : components) - squaredComponents += component * component; - - magnitude = Optional.of(Math.sqrt(squaredComponents)); - } - - return magnitude.get(); - } - - /** - * Applies a {@link Transform} to every component of this vector, returning a vector - * with the same dimensionality but with all components being the application of the - * transform on this vector's corresponding component. - * @param transform The transform to apply to each component. - * @return The result of the transformation applied to each component of this vector. - */ - public Vector apply (Transform transform) { - double[] newComponents = new double[components.length]; - for (int i = 0; i < components.length; i ++) - newComponents[i] = transform.apply(components[i]); - return new Vector(dimensionality, newComponents); - } - - /** - * Returns this vector scaled by a given coefficient. - * @param k The coefficient to scale this vector by. - * @return The scaled vector result. - */ - public Vector scale (double k) { - return this.apply(a -> a*k); - } - - /** - * Returns this vector scaled so that the new magnitude is equal to the given magnitude. If this vector's magnitude - * is zero, a zero vector will be returned. - * @param magnitude The magnitude of the vector after scaling. - * @return This vector, scale such that the magnitude equals the provided magnitude. - */ - public Vector scaleToMagnitude (double magnitude) { - if (getMagnitude() == 0) - return this.scale(0); - else - return this.scale(magnitude / getMagnitude()); - } - - /** - * Negates this vector, resulting in a vector of equal magnitude and opposite direction. - * @return This vector, but negated. - */ - public Vector negate () { - return this.apply(a -> -a); - } - - /** - * Apply an operation to every component of the vector - */ - private Vector apply (Vector other, Operation operation) { - double[] newComponents = new double[components.length]; - for (int i = 0; i < components.length; i ++) - newComponents[i] = operation.apply(components[i], other.components[i]); - return new Vector(dimensionality, newComponents); - } - - private static interface Operation { - public double apply (double a, double b); - } - - /** - * Calculate the dot product of this vector and other. - * @param other The vector to calculate the dot product with. - * @return The dot product. - */ - public double dotProduct (Vector other) { - // Compute a vector containing the products of this vector and the other vector's components - Vector productVector = this.apply(other, (a, b) -> a * b); - - // Sum the product components - double sum = 0; - for (double component : productVector.components) - sum += component; - - // Return the dot product - return sum; - } - - /** - * Add this vector and another together. - * @param other The vector to add. - * @return The sum of the two vectors. - */ - public Vector add (Vector other) { - return this.apply(other, (a, b) -> a + b); - } - - /** - * Subtract another vector from this one. - * @param other The vector to subtract. - * @return The difference of the two vectors. - */ - public Vector subtract (Vector other) { - return this.apply(other, (a, b) -> a - b); - } - - /** - * Returns the angle formed between a two-dimensional vector and the x-axis, in radians. This angle - * increases counterclockwise. For example, a vector facing in the +y direction will return {@code pi/2}. - * @param vector The two-dimensional vector to retrieve the direction angle of. - * @return The angle of the vector, in radians. - */ - public static double getAngle (Vector vector) { - if (vector.angle.isEmpty()) { - - // Set vector.angle as a cache - - if (vector.getMagnitude() == 0) { - - // Return 0 if the magnitude is 0 to prevent dividing by zero (or yielding a nonsensical answer) - vector.angle = Optional.of(0.); - - } else { - - // Get the X component of the vector if it were on the unit circle so we can use arccos - double unitX = vector.components[0] / vector.getMagnitude(); - - // Get the angle if the vector were above the x-axis - double angle = Math.acos(unitX); - - // Use the angle directly if the vector is above the x-axis, or modify it to flip across the x-axis otherwise - vector.angle = Optional.of(vector.components[1] >= 0 ? (angle) : (2*Math.PI - angle)); - - } - - } - - return vector.angle.get(); - } - - @Override - public String toString () { - String[] componentsStrings = new String[components.length]; - for (int i = 0; i < components.length; i ++) - componentsStrings[i] = Double.toString(components[i]); - return "<" + String.join(", ", componentsStrings) + ">"; - } - -} \ No newline at end of file From 1c74ce89da5fbae01cd15d2f13e4dfe54eac4ee0 Mon Sep 17 00:00:00 2001 From: David <98991028+ActualSkinwalker@users.noreply.github.com> Date: Thu, 7 Dec 2023 19:14:10 -0500 Subject: [PATCH 4/5] Addressed comments in PR Reverted changes to Drive command, changed circular deadband method to return a boolean --- .../java/frc/robot/commands/DriveCommand.java | 17 ++++++++++++++--- .../java/frc/robot/util/ControlsUtilities.java | 11 ++--------- 2 files changed, 16 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java index 3d3538a..71d2682 100644 --- a/src/main/java/frc/robot/commands/DriveCommand.java +++ b/src/main/java/frc/robot/commands/DriveCommand.java @@ -94,9 +94,20 @@ public void execute() { double absoluteNextThetaSpeed = Math.abs(nextThetaSpeed); // Apply global deadband. - nextXSpeed = absoluteNextXSpeed < DriveCommand.X_DEADBAND ? 0 : ControlsUtilities.enforceMaximumPositiveDelta(currentXSpeed, nextXSpeed, X_MAX_ACCELERATION); - nextYSpeed = absoluteNextYSpeed < DriveCommand.Y_DEADBAND ? 0 : ControlsUtilities.enforceMaximumPositiveDelta(currentYSpeed, nextYSpeed, Y_MAX_ACCELERATION); - nextThetaSpeed = absoluteNextThetaSpeed < DriveCommand.THETA_DEADBAND ? 0 : ControlsUtilities.enforceMaximumPositiveDelta(currentThetaSpeed, nextThetaSpeed, THETA_MAX_ACCELERATION); + if ( + absoluteNextXSpeed < DriveCommand.X_DEADBAND && + absoluteNextYSpeed < DriveCommand.Y_DEADBAND && + absoluteNextThetaSpeed < DriveCommand.THETA_DEADBAND + ) { + nextXSpeed = 0; + nextYSpeed = 0; + nextThetaSpeed = 0; + } else { + // Enforce maximum change in acceleration. + nextXSpeed = ControlsUtilities.enforceMaximumPositiveDelta(currentXSpeed, nextXSpeed, X_MAX_ACCELERATION); + nextYSpeed = ControlsUtilities.enforceMaximumPositiveDelta(currentYSpeed, nextYSpeed, Y_MAX_ACCELERATION); + nextThetaSpeed = ControlsUtilities.enforceMaximumPositiveDelta(currentThetaSpeed, nextThetaSpeed, THETA_MAX_ACCELERATION); + } this.speedMultiplier = slowMode.getAsBoolean() ? 0.25 : 1; diff --git a/src/main/java/frc/robot/util/ControlsUtilities.java b/src/main/java/frc/robot/util/ControlsUtilities.java index f2843f0..d873b1f 100644 --- a/src/main/java/frc/robot/util/ControlsUtilities.java +++ b/src/main/java/frc/robot/util/ControlsUtilities.java @@ -25,16 +25,9 @@ public static double applyDeadband(double input, double deadband) { } - public static double applyCircularDeadband (double xInput, double yInput, double deadband, boolean valueToRetrieve) { + public static boolean exceedsCircularDeadband (double xInput, double yInput, double deadband) { - double hypotenuse = Math.sqrt(Math.pow(xInput, 2) + Math.pow(yInput, 2)); - - if (Math.abs(hypotenuse) < deadband) { - if (valueToRetrieve) return xInput; - else return yInput; - } - - else return 0; + return Math.abs(Math.sqrt(Math.pow(xInput, 2) + Math.pow(yInput, 2))) < deadband; } /** From 0207096c6100e79a8e0a53ebc76cddc0c3934e65 Mon Sep 17 00:00:00 2001 From: David <98991028+ActualSkinwalker@users.noreply.github.com> Date: Mon, 11 Dec 2023 20:42:38 -0500 Subject: [PATCH 5/5] Updated drive command to use circular deadbands --- src/main/java/frc/robot/commands/DriveCommand.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java index 71d2682..5c9b778 100644 --- a/src/main/java/frc/robot/commands/DriveCommand.java +++ b/src/main/java/frc/robot/commands/DriveCommand.java @@ -19,6 +19,8 @@ public class DriveCommand extends CommandBase { static final double Y_DEADBAND = 0.10; + static final double LEFT_STICK_DEADBAND = 0.10; + static final double THETA_DEADBAND = 0.10; static final double X_MAX_ACCELERATION = 0.02; @@ -84,9 +86,9 @@ public void execute() { double nextYSpeed = ySpeed.getAsDouble(); double nextThetaSpeed = thetaSpeed.getAsDouble(); - // Apply individual deadbands. - nextXSpeed = ControlsUtilities.applyDeadband(nextXSpeed, DriveCommand.X_DEADBAND); - nextYSpeed = ControlsUtilities.applyDeadband(nextYSpeed, DriveCommand.Y_DEADBAND); + // Apply individual deadbands. + nextXSpeed = ControlsUtilities.exceedsCircularDeadband(nextXSpeed, nextYSpeed, DriveCommand.LEFT_STICK_DEADBAND) ? 0 : nextXSpeed; + nextYSpeed = ControlsUtilities.exceedsCircularDeadband(nextXSpeed, nextYSpeed, DriveCommand.LEFT_STICK_DEADBAND) ? 0 : nextYSpeed; nextThetaSpeed = ControlsUtilities.applyDeadband(nextThetaSpeed, DriveCommand.THETA_DEADBAND); double absoluteNextXSpeed = Math.abs(nextXSpeed);