From ff1506700391ee7ae3c91ae2cc21dbab7380b801 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Tue, 9 Apr 2024 13:08:36 -0400 Subject: [PATCH] refactor: Remove dead code. --- simgui.json | 10 +- .../java/frc/lib/AccelerationCalculator.java | 20 --- .../frc/lib/DoubleJointedArmFeedforward.java | 154 ------------------ src/main/java/frc/lib/JointConstants.java | 73 --------- src/main/java/frc/lib/LinearServo.java | 69 -------- .../frc/lib/SingleJointedArmFeedforward.java | 79 --------- src/main/java/frc/lib/SnapRotation.java | 26 --- .../frc/lib/TrapezoidProfileTelemetry.java | 53 ------ 8 files changed, 5 insertions(+), 479 deletions(-) delete mode 100644 src/main/java/frc/lib/AccelerationCalculator.java delete mode 100644 src/main/java/frc/lib/DoubleJointedArmFeedforward.java delete mode 100644 src/main/java/frc/lib/JointConstants.java delete mode 100644 src/main/java/frc/lib/LinearServo.java delete mode 100644 src/main/java/frc/lib/SingleJointedArmFeedforward.java delete mode 100644 src/main/java/frc/lib/SnapRotation.java delete mode 100644 src/main/java/frc/lib/TrapezoidProfileTelemetry.java diff --git a/simgui.json b/simgui.json index de51bfc..9ad41a7 100644 --- a/simgui.json +++ b/simgui.json @@ -161,9 +161,6 @@ } } }, - "NetworkTables Info": { - "visible": true - }, "Plot": { "Plot <0>": { "plots": [ @@ -195,7 +192,7 @@ 0.0, 0.8500000238418579 ], - "height": 338, + "height": 0, "series": [ { "color": [ @@ -217,7 +214,10 @@ } ] } - ] + ], + "window": { + "visible": false + } } } } diff --git a/src/main/java/frc/lib/AccelerationCalculator.java b/src/main/java/frc/lib/AccelerationCalculator.java deleted file mode 100644 index 2a74b0c..0000000 --- a/src/main/java/frc/lib/AccelerationCalculator.java +++ /dev/null @@ -1,20 +0,0 @@ -package frc.lib; - -import edu.wpi.first.wpilibj.Timer; - -public class AccelerationCalculator { - - private double previousVelocity; - private double previousTime; - - public double calculate(double velocity) { - double time = Timer.getFPGATimestamp(); - - double acceleration = (velocity - previousVelocity) / (time - previousTime); - - previousVelocity = velocity; - previousTime = time; - - return acceleration; - } -} diff --git a/src/main/java/frc/lib/DoubleJointedArmFeedforward.java b/src/main/java/frc/lib/DoubleJointedArmFeedforward.java deleted file mode 100644 index 340296a..0000000 --- a/src/main/java/frc/lib/DoubleJointedArmFeedforward.java +++ /dev/null @@ -1,154 +0,0 @@ -package frc.lib; - -import edu.wpi.first.math.geometry.Rotation2d; -import org.ejml.data.MatrixType; -import org.ejml.simple.SimpleMatrix; - -public class DoubleJointedArmFeedforward { - - private final double m1; - private final double m2; - private final double l1; - private final double r1; - private final double r2; - private final double I1; - private final double I2; - private final double g1; - private final double g2; - - private final SimpleMatrix M_MATRIX = new SimpleMatrix(2, 2, MatrixType.DDRM); - private final SimpleMatrix C_MATRIX = new SimpleMatrix(2, 2, MatrixType.DDRM); - private final SimpleMatrix Tg_VECTOR = new SimpleMatrix(2, 1, MatrixType.DDRM); - private final SimpleMatrix Kb_MATRIX = new SimpleMatrix(2, 2, MatrixType.DDRM); - private final SimpleMatrix B_MATRIX = new SimpleMatrix(2, 2, MatrixType.DDRM); - - public DoubleJointedArmFeedforward(JointConstants shoulder, JointConstants elbow) { - this.m1 = shoulder.massKg(); - this.m2 = elbow.massKg(); - this.l1 = shoulder.lengthMeters(); - this.r1 = shoulder.radiusMeters(); - this.r2 = elbow.radiusMeters(); - this.I1 = shoulder.moiKgMetersSquared(); - this.I2 = elbow.moiKgMetersSquared(); - this.g1 = shoulder.gearing(); - this.g2 = elbow.gearing(); - - B_MATRIX.set(0, 0, shoulder.torqueNm()); - B_MATRIX.set(1, 1, elbow.torqueNm()); - B_MATRIX.set(1, 0, 0); - B_MATRIX.set(0, 1, 0); - - Kb_MATRIX.set(0, 0, shoulder.torqueLossNm()); - Kb_MATRIX.set(1, 1, elbow.torqueLossNm()); - Kb_MATRIX.set(1, 0, 0); - Kb_MATRIX.set(0, 1, 0); - } - - /** - * Calculates the M matrix. - * - * @param elbowPosition the position of the elbow joint. - */ - public void updateMMatrix(Rotation2d elbowPosition) { - double c2 = elbowPosition.getCos(); - double diagonal = m2 * (r2 * r2 + l1 * r2 * c2) + I2; - - M_MATRIX.set(0, 0, m1 * r1 * r1 + m2 * (l1 * l1 + r2 * r2 + 2 * l1 * r2 * c2) + I1 + I2); - M_MATRIX.set(0, 1, diagonal); - M_MATRIX.set(1, 0, diagonal); - M_MATRIX.set(1, 1, m2 * r2 * r2 + I2); - } - - /** - * Calculates the C matrix. - * - * @param elbowPosition the position of the elbow joint. - * @param shoulderVelocity the velocity of the shoulder joint. - * @param elbowVelocity the velocity of the elbow joint. - */ - public void updateCMatrix( - Rotation2d elbowPosition, Rotation2d shoulderVelocity, Rotation2d elbowVelocity) { - double s2 = elbowPosition.getSin(); - - C_MATRIX.set(0, 0, -m2 * l1 * r2 * s2 * elbowVelocity.getRadians()); - C_MATRIX.set( - 0, 1, -m2 * l1 * r2 * s2 * (shoulderVelocity.getRadians() + elbowVelocity.getRadians())); - C_MATRIX.set(1, 0, m2 * l1 * r2 * s2 * shoulderVelocity.getRadians()); - C_MATRIX.set(1, 1, 0); - } - - /** - * Calculates the Tg vector. - * - * @param shoulderPosition the position of the shoulder joint. - * @param elbowPosition the position of the elbow joint. - */ - public void updateTgVector(Rotation2d shoulderPosition, Rotation2d elbowPosition) { - double c1 = shoulderPosition.getCos(); - double c12 = shoulderPosition.plus(elbowPosition).getCos(); - - Tg_VECTOR.set(0, 0, (m1 * r1 + m2 * l1) * g1 * c1 + m2 * r2 * g2 * c12); - Tg_VECTOR.set(1, 0, m2 * r2 * g2 * c12); - } - - /** - * Calculates the voltage feedforward required to move the arm in a certain state. - * - * @param shoulderPosition the angle of the shoulder joint. - * @param elbowPosition the angle of the elbow joint. - * @param shoulderVelocity the angular velocity of the shoulder joint. - * @param elbowVelocity the angular velocity of the elbow joint. - * @param shoulderAcceleration the angular acceleration of the shoulder joint. - * @param elbowAcceleration the angular acceleration of the elbow joint. - * @return the voltage feedforward required to move the arm in the given state. - */ - public TwoJointedArmFeedforwardResult calculateFeedforward( - Rotation2d shoulderPosition, - Rotation2d elbowPosition, - Rotation2d shoulderVelocity, - Rotation2d elbowVelocity, - Rotation2d shoulderAcceleration, - Rotation2d elbowAcceleration) { - updateMMatrix(elbowPosition); - updateCMatrix(elbowPosition, shoulderVelocity, elbowVelocity); - updateTgVector(shoulderPosition, elbowPosition); - - var thetaDotVector = new SimpleMatrix(2, 1, MatrixType.DDRM); - thetaDotVector.set(0, 0, shoulderVelocity.getRadians()); - thetaDotVector.set(1, 0, elbowVelocity.getRadians()); - - var thetaDotDotVector = new SimpleMatrix(2, 1, MatrixType.DDRM); - thetaDotDotVector.set(0, 0, shoulderAcceleration.getRadians()); - thetaDotDotVector.set(1, 0, elbowAcceleration.getRadians()); - - var M = M_MATRIX.mult(thetaDotDotVector); - var C = C_MATRIX.mult(thetaDotVector); - var Kb = Kb_MATRIX.mult(thetaDotVector); - var B_INV = B_MATRIX.invert(); - - var u = B_INV.mult(M.plus(C).plus(Kb).plus(Tg_VECTOR)); - - return new TwoJointedArmFeedforwardResult(u.get(0, 0), u.get(1, 0)); - } - - /** - * Calculates the voltage feedforward required to move the arm in a certain (static) state. - * - * @param shoulderPosition the angle of the shoulder joint. - * @param elbowPosition the angle of the elbow joint. - * @return the voltage feedforward required to move the arm in the given (static) state. - */ - public TwoJointedArmFeedforwardResult calculateFeedforward( - Rotation2d shoulderPosition, Rotation2d elbowPosition) { - return calculateFeedforward( - shoulderPosition, - elbowPosition, - new Rotation2d(), - new Rotation2d(), - new Rotation2d(), - new Rotation2d()); - } - - /** Calculates the voltage feedforward required to move the arm in a certain state. */ - public static record TwoJointedArmFeedforwardResult(double shoulderVolts, double elbowVolts) {} -} diff --git a/src/main/java/frc/lib/JointConstants.java b/src/main/java/frc/lib/JointConstants.java deleted file mode 100644 index 005b4a8..0000000 --- a/src/main/java/frc/lib/JointConstants.java +++ /dev/null @@ -1,73 +0,0 @@ -package frc.lib; - -import edu.wpi.first.math.system.plant.DCMotor; -import java.util.Objects; - -public record JointConstants( - double massKg, - double lengthMeters, - double radiusMeters, - double moiKgMetersSquared, - double gearing, - DCMotor motor, - int motorCount) { - - /** - * Creates joint constants for the joint of an arm. - * - * @param massKg the joint's mass in kilograms. - * @param lengthMeters the joint's length in meters. - * @param radius the distance between the joint's pivot and the joint's center of mass in meters. - * @param moiKgMetersSquared the joint's moment of inertia in kilograms meters squared. - * @param gearing the gearing between the joint's motor and the joint's pivot. - * @param motor the type of the joint's driving motor. - * @param motorCount the number of the driving motors. - */ - public JointConstants { - Objects.requireNonNull(massKg); - Objects.requireNonNull(lengthMeters); - Objects.requireNonNull(radiusMeters); - Objects.requireNonNull(moiKgMetersSquared); - Objects.requireNonNull(gearing); - Objects.requireNonNull(motor); - Objects.requireNonNull(motorCount); - } - - /** - * Creates joint constants for the joint of an arm. - * - * @param massKg the joint's mass in kilograms. - * @param length the joint's length in meters. - * @param radius the distance between the joint's pivot and the joint's center of mass in meters. - * @param moi the joint's moment of inertia in kilograms meters squared. - * @param gearing the gearing between the joint's motor and the joint's pivot. - * @param motor the type of the joint's driving motor. - */ - public JointConstants( - double massKg, double length, double radius, double moi, double gearing, DCMotor motor) { - this(massKg, length, radius, moi, gearing, motor, 1); - } - - /** - * Calculates the torque generated by the joint's gearbox element for the feedforward matrix. - * - * @return the torque generated by the joint's gearbox element for the feedforward matrix. - */ - public double torqueNm() { - return gearing * motorCount * motor.KtNMPerAmp / motor.rOhms; - } - - /** - * Calculates the torque loss due to back-emf for the feedforward matrix. - * - * @return the torque loss due to back-emf for the feedforward matrix. - */ - public double torqueLossNm() { - return gearing - * gearing - * motorCount - * motor.KtNMPerAmp - / motor.rOhms - / motor.KvRadPerSecPerVolt; - } -} diff --git a/src/main/java/frc/lib/LinearServo.java b/src/main/java/frc/lib/LinearServo.java deleted file mode 100644 index 1dc42a5..0000000 --- a/src/main/java/frc/lib/LinearServo.java +++ /dev/null @@ -1,69 +0,0 @@ -package frc.lib; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj.Servo; -import edu.wpi.first.wpilibj.Timer; - -public class LinearServo extends Servo { - - private final double speedMetersPerSecond; - private final double maximumLengthMeters; - - private double position; - private double setpointMeters; - private double previousTimeSeconds = 0; - - public LinearServo( - int channel, double maxiumumSpeedMetersPerSecond, double maximumLengthMetersPerSecond) { - super(channel); - - this.speedMetersPerSecond = maxiumumSpeedMetersPerSecond; - this.maximumLengthMeters = maximumLengthMetersPerSecond; - - setBounds(2.0, 1.8, 1.5, 1.2, 1.0); - } - - private void setBounds( - double maxMilliseconds, - double deadbandMaxMilliseconds, - double centerMilliseconds, - double deadbandMinMilliseconds, - double minMilliseconds) { - int maxMicroseconds = (int) (1000 * maxMilliseconds); - int deadbandMaxMicroseconds = (int) (1000 * deadbandMaxMilliseconds); - int centerMicroseconds = (int) (1000 * centerMilliseconds); - int deadbandMinMicroseconds = (int) (1000 * deadbandMinMilliseconds); - int minMicroseconds = (int) (1000 * minMilliseconds); - - setBoundsMicroseconds( - maxMicroseconds, - deadbandMaxMicroseconds, - centerMicroseconds, - deadbandMinMicroseconds, - minMicroseconds); - } - - public void setSetpoint(double setpointMeters) { - setpointMeters = MathUtil.clamp(setpointMeters, 0, maximumLengthMeters); - - this.setpointMeters = setpointMeters; - - double percentage = setpointMeters / maximumLengthMeters; - - double speed = 2 * percentage - 1; - - setSpeed(speed); - } - - public void updateCurPos() { - double currentTimeSeconds = Timer.getFPGATimestamp(); - double dt = currentTimeSeconds - previousTimeSeconds; - if (position > setpointMeters + speedMetersPerSecond * dt) { - position -= speedMetersPerSecond * dt; - } else if (position < setpointMeters - speedMetersPerSecond * dt) { - position += speedMetersPerSecond * dt; - } else { - position = setpointMeters; - } - } -} diff --git a/src/main/java/frc/lib/SingleJointedArmFeedforward.java b/src/main/java/frc/lib/SingleJointedArmFeedforward.java deleted file mode 100644 index 2d3798a..0000000 --- a/src/main/java/frc/lib/SingleJointedArmFeedforward.java +++ /dev/null @@ -1,79 +0,0 @@ -package frc.lib; - -import edu.wpi.first.math.geometry.Rotation2d; - -public class SingleJointedArmFeedforward { - - public static class SingleJointedArmFeedforwardConstants { - public double kS; - public double kG; - public double kV; - public double kA; - - public SingleJointedArmFeedforwardConstants() { - this.kS = 0.0; - this.kG = 0.0; - this.kV = 0.0; - this.kA = 0.0; - } - - public SingleJointedArmFeedforwardConstants withKs(double kS) { - this.kS = kS; - return this; - } - - public SingleJointedArmFeedforwardConstants withKg(double kG) { - this.kG = kG; - return this; - } - - /** - * Calculates the gravity compensation constant for an arm given the voltage applied at an - * equilibrium position. - * - * @param angle the equilibrium position of the arm. - * @param volts the voltage applied to the arm to hold it the equilibrium position. - * @return the gravity compensation constant for the arm. - */ - public SingleJointedArmFeedforwardConstants withKg(Rotation2d angle, double volts) { - double kG = volts / angle.getCos(); - - return this.withKg(kG); - } - - public SingleJointedArmFeedforwardConstants withKv(double kV) { - this.kV = kV; - return this; - } - - public SingleJointedArmFeedforwardConstants withKa(double kA) { - this.kA = kA; - return this; - } - } - - private final SingleJointedArmFeedforwardConstants constants; - - public SingleJointedArmFeedforward() { - this.constants = new SingleJointedArmFeedforwardConstants(); - } - - public SingleJointedArmFeedforward(SingleJointedArmFeedforwardConstants constants) { - this.constants = constants; - } - - public double calculate(Rotation2d position) { - return calculate(position, new Rotation2d()); - } - - public double calculate(Rotation2d position, Rotation2d velocity) { - return calculate(position, new Rotation2d(), new Rotation2d()); - } - - public double calculate(Rotation2d position, Rotation2d velocity, Rotation2d acceleration) { - return constants.kS * Math.signum(velocity.getRotations()) - + constants.kG * position.getCos() - + constants.kV * velocity.getRotations() - + constants.kA * acceleration.getRotations(); - } -} diff --git a/src/main/java/frc/lib/SnapRotation.java b/src/main/java/frc/lib/SnapRotation.java deleted file mode 100644 index db2373d..0000000 --- a/src/main/java/frc/lib/SnapRotation.java +++ /dev/null @@ -1,26 +0,0 @@ -package frc.lib; - -import edu.wpi.first.math.geometry.Rotation2d; - -public class SnapRotation { - - private final Rotation2d multiple; - - private SnapRotation(Rotation2d multiple) { - this.multiple = multiple; - } - - public static SnapRotation to(Rotation2d multiple) { - return new SnapRotation(multiple); - } - - private double snapToNearest(double n, double multiple) { - return Math.round(n / multiple) * multiple; - } - - public Rotation2d snap(Rotation2d angle) { - double snapped = snapToNearest(angle.getRotations(), multiple.getRotations()); - - return Rotation2d.fromRotations(snapped); - } -} diff --git a/src/main/java/frc/lib/TrapezoidProfileTelemetry.java b/src/main/java/frc/lib/TrapezoidProfileTelemetry.java deleted file mode 100644 index 8691e7f..0000000 --- a/src/main/java/frc/lib/TrapezoidProfileTelemetry.java +++ /dev/null @@ -1,53 +0,0 @@ -package frc.lib; - -import edu.wpi.first.math.trajectory.TrapezoidProfile.State; -import edu.wpi.first.networktables.DoubleEntry; - -public class TrapezoidProfileTelemetry { - - private final DoubleEntry measuredPosition, measuredVelocity; - private final DoubleEntry setpointPosition, setpointVelocity; - private final DoubleEntry goalPosition, goalVelocity; - - public TrapezoidProfileTelemetry(String table) { - measuredPosition = Telemetry.addDoubleEntry(table, "measuredPosition"); - measuredVelocity = Telemetry.addDoubleEntry(table, "measuredVelocity"); - setpointPosition = Telemetry.addDoubleEntry(table, "setpointPosition"); - setpointVelocity = Telemetry.addDoubleEntry(table, "setpointVelocity"); - goalPosition = Telemetry.addDoubleEntry(table, "goalPosition"); - goalVelocity = Telemetry.addDoubleEntry(table, "goalVelocity"); - } - - public void updateMeasurement(double position, double velocity) { - measuredPosition.set(position); - measuredVelocity.set(velocity); - } - - public void updateMeasurement(State state) { - updateMeasurement(state.position, state.velocity); - } - - public void updateSetpoint(double position, double velocity) { - setpointPosition.set(position); - setpointVelocity.set(velocity); - } - - public void updateSetpoint(State state) { - updateSetpoint(state.position, state.velocity); - } - - public void updateGoal(double position, double velocity) { - goalPosition.set(position); - goalVelocity.set(velocity); - } - - public void updateGoal(State state) { - updateGoal(state.position, state.velocity); - } - - public void update(State measurement, State setpoint, State goal) { - updateMeasurement(measurement); - updateSetpoint(setpoint); - updateGoal(goal); - } -}