From 47c8d11cdab56e4d116e6d4f27884d86dbe3d659 Mon Sep 17 00:00:00 2001 From: Matt Lui <92664603+superyuuki@users.noreply.github.com> Date: Wed, 21 Feb 2024 19:51:49 -0700 Subject: [PATCH] fix: optimize settings fix: unit tests for fast kinemaitcs add: swerve generator from 254, prepare to mattlib and make fast/good add: more logging for limit switches add: start work on a faster polynomial regression (broked) --- {mattlib2-auto => mattlib2-controls}/pom.xml | 9 +- .../readme.md | 0 .../auriium/mattlib2/auto/RoutineTesting.java | 0 .../auto/dynamics/FastSwerveKinematics.java | 31 +- .../auto/dynamics/UngodlyAbomination.java | 376 ++++++++++++++++++ .../mattlib2/auto/ff/BaseFFGenRoutine.java | 0 .../auto/ff/FastPolynomialRegression.java | 83 ++++ .../mattlib2/auto/ff/GenerateFFComponent.java | 0 .../mattlib2/auto/ff/LinearFFGenRoutine.java | 0 .../auto/ff/PolynomialRegression.java | 0 .../auto/ff/RotationFFGenRoutine.java | 0 .../mattlib2/auto/pid/PIDGenComponent.java | 0 .../auto/PolynomialRegressionTest.java | 11 + .../dynamics/FastSwerveKinematicsTest.java | 55 +++ .../auto/ff/BaseFFGenRoutineTest.java | 0 mattlib2-core/pom.xml | 2 +- .../xyz/auriium/mattlib2/MattlibLooper.java | 8 + .../xyz/auriium/mattlib2/MattlibSettings.java | 1 + .../auriium/mattlib2/loop/IMattlibHooked.java | 5 + .../xyz/auriium/mattlib2/utils/AngleUtil.java | 3 + mattlib2-ctre/pom.xml | 6 +- .../mattlib/ctre/BaseTalonFXMotor.java | 38 +- mattlib2-foxe-ext/package-lock.json | 28 +- mattlib2-hardware/pom.xml | 4 +- .../hardware/config/CommonPIDComponent.java | 3 +- .../config/CommonProfiledPIDComponent.java | 4 + .../config/IndividualMotorComponent.java | 4 + mattlib2-log-foxe/pom.xml | 4 +- mattlib2-log-nt/pom.xml | 4 +- mattlib2-rev/pom.xml | 8 +- .../auriium/mattlib2/rev/BaseSparkMotor.java | 2 + .../xyz/auriium/mattlib2/rev/HardwareREV.java | 22 + .../mattlib2/rev/OnboardLinearController.java | 63 +++ .../rev/OnboardRotationController.java | 64 +++ mattlib2-sim/pom.xml | 4 +- .../auriium/mattlib2/sim/DCSimController.java | 37 +- pom.xml | 4 +- 37 files changed, 813 insertions(+), 70 deletions(-) rename {mattlib2-auto => mattlib2-controls}/pom.xml (87%) rename {mattlib2-auto => mattlib2-controls}/readme.md (100%) rename {mattlib2-auto => mattlib2-controls}/src/main/java/xyz/auriium/mattlib2/auto/RoutineTesting.java (100%) rename {mattlib2-auto => mattlib2-controls}/src/main/java/xyz/auriium/mattlib2/auto/dynamics/FastSwerveKinematics.java (78%) create mode 100644 mattlib2-controls/src/main/java/xyz/auriium/mattlib2/auto/dynamics/UngodlyAbomination.java rename {mattlib2-auto => mattlib2-controls}/src/main/java/xyz/auriium/mattlib2/auto/ff/BaseFFGenRoutine.java (100%) create mode 100644 mattlib2-controls/src/main/java/xyz/auriium/mattlib2/auto/ff/FastPolynomialRegression.java rename {mattlib2-auto => mattlib2-controls}/src/main/java/xyz/auriium/mattlib2/auto/ff/GenerateFFComponent.java (100%) rename {mattlib2-auto => mattlib2-controls}/src/main/java/xyz/auriium/mattlib2/auto/ff/LinearFFGenRoutine.java (100%) rename {mattlib2-auto => mattlib2-controls}/src/main/java/xyz/auriium/mattlib2/auto/ff/PolynomialRegression.java (100%) rename {mattlib2-auto => mattlib2-controls}/src/main/java/xyz/auriium/mattlib2/auto/ff/RotationFFGenRoutine.java (100%) rename {mattlib2-auto => mattlib2-controls}/src/main/java/xyz/auriium/mattlib2/auto/pid/PIDGenComponent.java (100%) rename {mattlib2-auto => mattlib2-controls}/src/test/java/xyz/auriium/mattlib2/auto/PolynomialRegressionTest.java (56%) create mode 100644 mattlib2-controls/src/test/java/xyz/auriium/mattlib2/auto/dynamics/FastSwerveKinematicsTest.java rename {mattlib2-auto => mattlib2-controls}/src/test/java/xyz/auriium/mattlib2/auto/ff/BaseFFGenRoutineTest.java (100%) create mode 100644 mattlib2-hardware/src/main/java/xyz/auriium/mattlib2/hardware/config/CommonProfiledPIDComponent.java create mode 100644 mattlib2-rev/src/main/java/xyz/auriium/mattlib2/rev/OnboardLinearController.java create mode 100644 mattlib2-rev/src/main/java/xyz/auriium/mattlib2/rev/OnboardRotationController.java diff --git a/mattlib2-auto/pom.xml b/mattlib2-controls/pom.xml similarity index 87% rename from mattlib2-auto/pom.xml rename to mattlib2-controls/pom.xml index 5d1e282..432f439 100644 --- a/mattlib2-auto/pom.xml +++ b/mattlib2-controls/pom.xml @@ -5,11 +5,11 @@ mattlib2 xyz.auriium - 1.6.4-SNAPSHOT + 1.7.1-SNAPSHOT 4.0.0 - mattlib2-auto + mattlib2-controls 17 @@ -26,12 +26,12 @@ xyz.auriium mattlib2-core - 1.6.4-SNAPSHOT + 1.7.1-SNAPSHOT xyz.auriium mattlib2-hardware - 1.6.4-SNAPSHOT + 1.7.1-SNAPSHOT org.ojalgo @@ -40,7 +40,6 @@ - \ No newline at end of file diff --git a/mattlib2-auto/readme.md b/mattlib2-controls/readme.md similarity index 100% rename from mattlib2-auto/readme.md rename to mattlib2-controls/readme.md diff --git a/mattlib2-auto/src/main/java/xyz/auriium/mattlib2/auto/RoutineTesting.java b/mattlib2-controls/src/main/java/xyz/auriium/mattlib2/auto/RoutineTesting.java similarity index 100% rename from mattlib2-auto/src/main/java/xyz/auriium/mattlib2/auto/RoutineTesting.java rename to mattlib2-controls/src/main/java/xyz/auriium/mattlib2/auto/RoutineTesting.java diff --git a/mattlib2-auto/src/main/java/xyz/auriium/mattlib2/auto/dynamics/FastSwerveKinematics.java b/mattlib2-controls/src/main/java/xyz/auriium/mattlib2/auto/dynamics/FastSwerveKinematics.java similarity index 78% rename from mattlib2-auto/src/main/java/xyz/auriium/mattlib2/auto/dynamics/FastSwerveKinematics.java rename to mattlib2-controls/src/main/java/xyz/auriium/mattlib2/auto/dynamics/FastSwerveKinematics.java index 8351b1b..fb6fe6b 100644 --- a/mattlib2-auto/src/main/java/xyz/auriium/mattlib2/auto/dynamics/FastSwerveKinematics.java +++ b/mattlib2-controls/src/main/java/xyz/auriium/mattlib2/auto/dynamics/FastSwerveKinematics.java @@ -3,21 +3,26 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; import org.ojalgo.matrix.MatrixR064; -import org.ojalgo.matrix.store.Primitive64Store; + +import java.util.Arrays; public class FastSwerveKinematics { final MatrixR064 inverseKinematics; final MatrixR064 forwardKinematics_pseudo; + final Rotation2d[] rotationVector = new Rotation2d[4]; + public FastSwerveKinematics(MatrixR064 inverseKinematics, MatrixR064 forwardKinematics_pseudo) { this.inverseKinematics = inverseKinematics; this.forwardKinematics_pseudo = forwardKinematics_pseudo; + Arrays.fill(rotationVector, new Rotation2d()); } - public FastSwerveKinematics load(Translation2d[] swerveModulePositionOffsets_four) { + public static FastSwerveKinematics load(Translation2d[] swerveModulePositionOffsets_four) { //Generate 1st order inverse kinematic matrix var inverseKinematicBuilder = MatrixR064.FACTORY.makeDense(8,3); @@ -35,26 +40,41 @@ public FastSwerveKinematics load(Translation2d[] swerveModulePositionOffsets_fou } MatrixR064 inverseKinematics = inverseKinematicBuilder.get(); - MatrixR064 forwardKinematics_pseudo = inverseKinematics.invert(); + MatrixR064 forwardKinematics_pseudo = (inverseKinematics.transpose().multiply(inverseKinematics)).invert().multiply(inverseKinematics.transpose()); + + //FUCKING GIVE ME THE MOORE PERSIJISNINFOSNFOSNFAISNFO + //DUMB SHIT return new FastSwerveKinematics(inverseKinematics, forwardKinematics_pseudo); } public SwerveModuleState[] convertCentroidStateToModuleStateSafe(ChassisSpeeds speeds) { + if (speeds.vxMetersPerSecond == 0.0 + && speeds.vyMetersPerSecond == 0.0 + && speeds.omegaRadiansPerSecond == 0.0) { + SwerveModuleState[] moduleStates = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + moduleStates[i] = new SwerveModuleState(0.0, rotationVector[i]); + } + + return moduleStates; + } + MatrixR064 centroidStateVector = MatrixR064.FACTORY.column(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond); MatrixR064 moduleStateVector = convertCentroidStateToModuleState(centroidStateVector); //compiler inlines this SwerveModuleState[] locallyAllocatedStates = new SwerveModuleState[4]; //TODO reduce object allocations for (int i = 0; i < 4; i++) { - double x = moduleStateVector.doubleValue(2 * i); - double y = moduleStateVector.doubleValue(2 * i + 1); + double x = moduleStateVector.doubleValue(i * 2); + double y = moduleStateVector.doubleValue(i * 2 + 1); double velocity = Math.hypot(x,y); Rotation2d angle = new Rotation2d(x,y); locallyAllocatedStates[i] = new SwerveModuleState(velocity, angle); + rotationVector[i] = angle; } return locallyAllocatedStates; @@ -73,6 +93,7 @@ public ChassisSpeeds convertModuleStateToCentroidState(SwerveModuleState[] state states[3].speedMetersPerSecond * states[3].angle.getCos() ); + MatrixR064 centroidStateVector = convertModuleStateToCentroidState(moduleStateVector); return new ChassisSpeeds( diff --git a/mattlib2-controls/src/main/java/xyz/auriium/mattlib2/auto/dynamics/UngodlyAbomination.java b/mattlib2-controls/src/main/java/xyz/auriium/mattlib2/auto/dynamics/UngodlyAbomination.java new file mode 100644 index 0000000..04e03da --- /dev/null +++ b/mattlib2-controls/src/main/java/xyz/auriium/mattlib2/auto/dynamics/UngodlyAbomination.java @@ -0,0 +1,376 @@ +package xyz.auriium.mattlib2.auto.dynamics; + + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModuleState; + +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; + +//254 code +public class UngodlyAbomination { + + public static Twist2d toTwist2d(ChassisSpeeds speeds) { + return new Twist2d( + speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond); + } + public static boolean epsilonEquals(Twist2d twist, Twist2d other) { + return epsilonEquals(twist.dx, other.dx) + && epsilonEquals(twist.dy, other.dy) + && epsilonEquals(twist.dtheta, other.dtheta); + } + + + public record SwerveSetpoint(ChassisSpeeds chassisSpeeds, SwerveModuleState[] moduleStates) {} + public record ModuleLimits(double maxDriveVelocity, double maxDriveAcceleration, double maxSteeringVelocity) {} + + private final SwerveDriveKinematics kinematics; + private final Translation2d[] moduleLocations; + + public UngodlyAbomination(SwerveDriveKinematics kinematics, Translation2d[] moduleLocations) { + this.kinematics = kinematics; + this.moduleLocations = moduleLocations; + } + + public static boolean epsilonEquals(double a, double b, double epsilon) { + return (a - epsilon <= b) && (a + epsilon >= b); + } + + public static boolean epsilonEquals(double a, double b) { + return epsilonEquals(a, b, 1e-9); + } + + + private boolean flipHeading(Rotation2d prevToGoal) { + return Math.abs(prevToGoal.getRadians()) > Math.PI / 2.0; + } + + private double unwrapAngle(double ref, double angle) { + double diff = angle - ref; + if (diff > Math.PI) { + return angle - 2.0 * Math.PI; + } else if (diff < -Math.PI) { + return angle + 2.0 * Math.PI; + } else { + return angle; + } + } + + @FunctionalInterface + private interface Function2d { + double f(double x, double y); + } + + /** + * Find the root of the generic 2D parametric function 'func' using the regula falsi technique. + * This is a pretty naive way to do root finding, but it's usually faster than simple bisection + * while being robust in ways that e.g. the Newton-Raphson method isn't. + * + * @param func The Function2d to take the root of. + * @param x_0 x value of the lower bracket. + * @param y_0 y value of the lower bracket. + * @param f_0 value of 'func' at x_0, y_0 (passed in by caller to save a call to 'func' during + * recursion) + * @param x_1 x value of the upper bracket. + * @param y_1 y value of the upper bracket. + * @param f_1 value of 'func' at x_1, y_1 (passed in by caller to save a call to 'func' during + * recursion) + * @param iterations_left Number of iterations of root finding left. + * @return The parameter value 's' that interpolating between 0 and 1 that corresponds to the + * (approximate) root. + */ + private double findRoot( + Function2d func, + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + int iterations_left) { + if (iterations_left < 0 || epsilonEquals(f_0, f_1)) { + return 1.0; + } + var s_guess = Math.max(0.0, Math.min(1.0, -f_0 / (f_1 - f_0))); + var x_guess = (x_1 - x_0) * s_guess + x_0; + var y_guess = (y_1 - y_0) * s_guess + y_0; + var f_guess = func.f(x_guess, y_guess); + if (Math.signum(f_0) == Math.signum(f_guess)) { + // 0 and guess on same side of root, so use upper bracket. + return s_guess + + (1.0 - s_guess) + * findRoot(func, x_guess, y_guess, f_guess, x_1, y_1, f_1, iterations_left - 1); + } else { + // Use lower bracket. + return s_guess + * findRoot(func, x_0, y_0, f_0, x_guess, y_guess, f_guess, iterations_left - 1); + } + } + + protected double findSteeringMaxS( + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + double max_deviation, + int max_iterations) { + f_1 = unwrapAngle(f_0, f_1); + double diff = f_1 - f_0; + if (Math.abs(diff) <= max_deviation) { + // Can go all the way to s=1. + return 1.0; + } + double offset = f_0 + Math.signum(diff) * max_deviation; + Function2d func = + (x, y) -> unwrapAngle(f_0, Math.atan2(y, x)) - offset; + return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, max_iterations); + } + + protected double findDriveMaxS( + double x_0, + double y_0, + double f_0, + double x_1, + double y_1, + double f_1, + double max_vel_step, + int max_iterations) { + double diff = f_1 - f_0; + if (Math.abs(diff) <= max_vel_step) { + // Can go all the way to s=1. + return 1.0; + } + double offset = f_0 + Math.signum(diff) * max_vel_step; + Function2d func = + (x, y) -> { + return Math.hypot(x, y) - offset; + }; + return findRoot(func, x_0, y_0, f_0 - offset, x_1, y_1, f_1 - offset, max_iterations); + } + + + /** + * Generate a new setpoint. + * + * @param limits The kinematic limits to respect for this setpoint. + * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous + * iteration setpoint instead of the actual measured/estimated kinematic state. + * @param desiredState The desired state of motion, such as from the driver sticks or a path + * following algorithm. + * @param dt The loop time. + * @return A Setpoint object that satisfies all of the KinematicLimits while converging to + * desiredState quickly. + */ + public SwerveSetpoint generateSetpoint( + final ModuleLimits limits, + final SwerveSetpoint prevSetpoint, + ChassisSpeeds desiredState, + double dt) { + final Translation2d[] modules = moduleLocations; + + SwerveModuleState[] desiredModuleState = kinematics.toSwerveModuleStates(desiredState); + // Make sure desiredState respects velocity limits. + if (limits.maxDriveVelocity() > 0.0) { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleState, limits.maxDriveVelocity()); + desiredState = kinematics.toChassisSpeeds(desiredModuleState); + } + + // Special case: desiredState is a complete stop. In this case, module angle is arbitrary, so + // just use the previous angle. + boolean need_to_steer = true; + if (epsilonEquals(toTwist2d(desiredState),(new Twist2d()))) { + need_to_steer = false; + for (int i = 0; i < modules.length; ++i) { + desiredModuleState[i].angle = prevSetpoint.moduleStates()[i].angle; + desiredModuleState[i].speedMetersPerSecond = 0.0; + } + } + + // For each module, compute local Vx and Vy vectors. + double[] prev_vx = new double[modules.length]; + double[] prev_vy = new double[modules.length]; + Rotation2d[] prev_heading = new Rotation2d[modules.length]; + double[] desired_vx = new double[modules.length]; + double[] desired_vy = new double[modules.length]; + Rotation2d[] desired_heading = new Rotation2d[modules.length]; + boolean all_modules_should_flip = true; + for (int i = 0; i < modules.length; ++i) { + prev_vx[i] = + prevSetpoint.moduleStates()[i].angle.getCos() + * prevSetpoint.moduleStates()[i].speedMetersPerSecond; + prev_vy[i] = + prevSetpoint.moduleStates()[i].angle.getSin() + * prevSetpoint.moduleStates()[i].speedMetersPerSecond; + prev_heading[i] = prevSetpoint.moduleStates()[i].angle; + if (prevSetpoint.moduleStates()[i].speedMetersPerSecond < 0.0) { + prev_heading[i] = prev_heading[i].rotateBy(Rotation2d.fromRadians(Math.PI)); + } + desired_vx[i] = + desiredModuleState[i].angle.getCos() * desiredModuleState[i].speedMetersPerSecond; + desired_vy[i] = + desiredModuleState[i].angle.getSin() * desiredModuleState[i].speedMetersPerSecond; + desired_heading[i] = desiredModuleState[i].angle; + if (desiredModuleState[i].speedMetersPerSecond < 0.0) { + desired_heading[i] = desired_heading[i].rotateBy(Rotation2d.fromRadians(Math.PI)); + } + if (all_modules_should_flip) { + double required_rotation_rad = + Math.abs(prev_heading[i].unaryMinus().rotateBy(desired_heading[i]).getRadians()); + if (required_rotation_rad < Math.PI / 2.0) { + all_modules_should_flip = false; + } + } + } + if (all_modules_should_flip + && !epsilonEquals(toTwist2d(prevSetpoint.chassisSpeeds()),new Twist2d()) + && !epsilonEquals(toTwist2d(desiredState), new Twist2d())) { + // It will (likely) be faster to stop the robot, rotate the modules in place to the complement + // of the desired + // angle, and accelerate again. + return generateSetpoint(limits, prevSetpoint, new ChassisSpeeds(), dt); + } + + // Compute the deltas between start and goal. We can then interpolate from the start state to + // the goal state; then + // find the amount we can move from start towards goal in this cycle such that no kinematic + // limit is exceeded. + double dx = desiredState.vxMetersPerSecond - prevSetpoint.chassisSpeeds().vxMetersPerSecond; + double dy = desiredState.vyMetersPerSecond - prevSetpoint.chassisSpeeds().vyMetersPerSecond; + double dtheta = + desiredState.omegaRadiansPerSecond - prevSetpoint.chassisSpeeds().omegaRadiansPerSecond; + + // 's' interpolates between start and goal. At 0, we are at prevState and at 1, we are at + // desiredState. + double min_s = 1.0; + + // In cases where an individual module is stopped, we want to remember the right steering angle + // to command (since + // inverse kinematics doesn't care about angle, we can be opportunistically lazy). + List> overrideSteering = new ArrayList<>(modules.length); + // Enforce steering velocity limits. We do this by taking the derivative of steering angle at + // the current angle, + // and then backing out the maximum interpolant between start and goal states. We remember the + // minimum across all modules, since + // that is the active constraint. + final double max_theta_step = dt * limits.maxSteeringVelocity(); + for (int i = 0; i < modules.length; ++i) { + if (!need_to_steer) { + overrideSteering.add(Optional.of(prevSetpoint.moduleStates()[i].angle)); + continue; + } + overrideSteering.add(Optional.empty()); + if (epsilonEquals(prevSetpoint.moduleStates()[i].speedMetersPerSecond, 0.0)) { + // If module is stopped, we know that we will need to move straight to the final steering + // angle, so limit based + // purely on rotation in place. + if (epsilonEquals(desiredModuleState[i].speedMetersPerSecond, 0.0)) { + // Goal angle doesn't matter. Just leave module at its current angle. + overrideSteering.set(i, Optional.of(prevSetpoint.moduleStates()[i].angle)); + continue; + } + + var necessaryRotation = + prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(desiredModuleState[i].angle); + if (flipHeading(necessaryRotation)) { + necessaryRotation = necessaryRotation.rotateBy(Rotation2d.fromRadians(Math.PI)); + } + // getRadians() bounds to +/- Pi. + final double numStepsNeeded = Math.abs(necessaryRotation.getRadians()) / max_theta_step; + + if (numStepsNeeded <= 1.0) { + // Steer directly to goal angle. + overrideSteering.set(i, Optional.of(desiredModuleState[i].angle)); + // Don't limit the global min_s; + continue; + } else { + // Adjust steering by max_theta_step. + overrideSteering.set( + i, + Optional.of( + prevSetpoint.moduleStates()[i].angle.rotateBy( + Rotation2d.fromRadians( + Math.signum(necessaryRotation.getRadians()) * max_theta_step)))); + min_s = 0.0; + continue; + } + } + if (min_s == 0.0) { + // s can't get any lower. Save some CPU. + continue; + } + + final int kMaxIterations = 8; + double s = + findSteeringMaxS( + prev_vx[i], + prev_vy[i], + prev_heading[i].getRadians(), + desired_vx[i], + desired_vy[i], + desired_heading[i].getRadians(), + max_theta_step, + kMaxIterations); + min_s = Math.min(min_s, s); + } + + // Enforce drive wheel acceleration limits. + final double max_vel_step = dt * limits.maxDriveAcceleration(); + for (int i = 0; i < modules.length; ++i) { + if (min_s == 0.0) { + // No need to carry on. + break; + } + double vx_min_s = + min_s == 1.0 ? desired_vx[i] : (desired_vx[i] - prev_vx[i]) * min_s + prev_vx[i]; + double vy_min_s = + min_s == 1.0 ? desired_vy[i] : (desired_vy[i] - prev_vy[i]) * min_s + prev_vy[i]; + // Find the max s for this drive wheel. Search on the interval between 0 and min_s, because we + // already know we can't go faster + // than that. + final int kMaxIterations = 10; + double s = + min_s + * findDriveMaxS( + prev_vx[i], + prev_vy[i], + Math.hypot(prev_vx[i], prev_vy[i]), + vx_min_s, + vy_min_s, + Math.hypot(vx_min_s, vy_min_s), + max_vel_step, + kMaxIterations); + min_s = Math.min(min_s, s); + } + + ChassisSpeeds retSpeeds = + new ChassisSpeeds( + prevSetpoint.chassisSpeeds().vxMetersPerSecond + min_s * dx, + prevSetpoint.chassisSpeeds().vyMetersPerSecond + min_s * dy, + prevSetpoint.chassisSpeeds().omegaRadiansPerSecond + min_s * dtheta); + var retStates = kinematics.toSwerveModuleStates(retSpeeds); + for (int i = 0; i < modules.length; ++i) { + final var maybeOverride = overrideSteering.get(i); + if (maybeOverride.isPresent()) { + var override = maybeOverride.get(); + if (flipHeading(retStates[i].angle.unaryMinus().rotateBy(override))) { + retStates[i].speedMetersPerSecond *= -1.0; + } + retStates[i].angle = override; + } + final var deltaRotation = + prevSetpoint.moduleStates()[i].angle.unaryMinus().rotateBy(retStates[i].angle); + if (flipHeading(deltaRotation)) { + retStates[i].angle = retStates[i].angle.rotateBy(Rotation2d.fromRadians(Math.PI)); + retStates[i].speedMetersPerSecond *= -1.0; + } + } + return new SwerveSetpoint(retSpeeds, retStates); + } +} diff --git a/mattlib2-auto/src/main/java/xyz/auriium/mattlib2/auto/ff/BaseFFGenRoutine.java b/mattlib2-controls/src/main/java/xyz/auriium/mattlib2/auto/ff/BaseFFGenRoutine.java similarity index 100% rename from mattlib2-auto/src/main/java/xyz/auriium/mattlib2/auto/ff/BaseFFGenRoutine.java rename to mattlib2-controls/src/main/java/xyz/auriium/mattlib2/auto/ff/BaseFFGenRoutine.java diff --git a/mattlib2-controls/src/main/java/xyz/auriium/mattlib2/auto/ff/FastPolynomialRegression.java b/mattlib2-controls/src/main/java/xyz/auriium/mattlib2/auto/ff/FastPolynomialRegression.java new file mode 100644 index 0000000..b6dbe93 --- /dev/null +++ b/mattlib2-controls/src/main/java/xyz/auriium/mattlib2/auto/ff/FastPolynomialRegression.java @@ -0,0 +1,83 @@ +package xyz.auriium.mattlib2.auto.ff; + +import org.ojalgo.matrix.MatrixR064; +import org.ojalgo.matrix.decomposition.QR; +import xyz.auriium.yuukonstants.exception.ExplainedException; + +//faster but not fast lol +public class FastPolynomialRegression { + + final MatrixR064 polyCoefficientsBeta; //n by 1 vector matrix + final int polynomialDegree; + + public FastPolynomialRegression(MatrixR064 polyCoefficientsBeta, int polynomialDegree) { + this.polyCoefficientsBeta = polyCoefficientsBeta; + this.polynomialDegree = polynomialDegree; + } + + public static FastPolynomialRegression loadFullRank(double[] x, double[] y, int polynomialDegree) throws ExplainedException { + + double[][] vandermondeMatrixComposition = new double[x.length][polynomialDegree + 1]; + for (int i = 0; i < x.length; i++) { + for (int j = 0; j <= polynomialDegree; j++) { + vandermondeMatrixComposition[i][j] = Math.pow(x[i], j); + } + } + + + MatrixR064 vandermondeMatrix = MatrixR064.FACTORY.rows(vandermondeMatrixComposition); + MatrixR064 qrSolution = MatrixR064.FACTORY.make(QR.R064.decompose(vandermondeMatrix).reconstruct()); + + + MatrixR064 yMatrix = MatrixR064.FACTORY.rows(y); + MatrixR064 polyCoefficientsBeta = qrSolution.solve(yMatrix); + + return new FastPolynomialRegression(polyCoefficientsBeta, polynomialDegree); + + } + + public static FastPolynomialRegression loadRankDeficient_iterative(double[] x, double[] y, int degree) throws ExplainedException { + + int polynomialDegree = degree; + MatrixR064 qrSolution; + + while (true) { + double[][] vandermondeMatrixComposition = new double[x.length][polynomialDegree + 1]; + for (int i = 0; i < x.length; i++) { + for (int j = 0; j <= polynomialDegree; j++) { + vandermondeMatrixComposition[i][j] = Math.pow(x[i], j); + } + } + + MatrixR064 vandermondeMatrix = MatrixR064.FACTORY.rows(vandermondeMatrixComposition); + + System.out.println(vandermondeMatrix.countRows() + " " + vandermondeMatrix.countColumns()); + + qrSolution = MatrixR064.FACTORY.make(QR.R064.decompose(vandermondeMatrix).reconstruct()); + if (qrSolution.getRank() == degree + 1) { + break; + } + + degree--; + + } + MatrixR064 yMatrix = MatrixR064.FACTORY.rows(y); + MatrixR064 polyCoefficientsBeta = qrSolution.solve(yMatrix); + + return new FastPolynomialRegression(polyCoefficientsBeta, polynomialDegree); + } + + + public double beta(int j) { + var out = polyCoefficientsBeta.get(j, 0); + + if (Math.abs(out) < 1E-4) return 0.0; + return out; + } + + public double predict(double x) { + double y = 0.0; + for (int j = polynomialDegree; j >= 0; j--) y = beta(j) + (x * y); + return y; + } +} diff --git a/mattlib2-auto/src/main/java/xyz/auriium/mattlib2/auto/ff/GenerateFFComponent.java b/mattlib2-controls/src/main/java/xyz/auriium/mattlib2/auto/ff/GenerateFFComponent.java similarity index 100% rename from mattlib2-auto/src/main/java/xyz/auriium/mattlib2/auto/ff/GenerateFFComponent.java rename to mattlib2-controls/src/main/java/xyz/auriium/mattlib2/auto/ff/GenerateFFComponent.java diff --git a/mattlib2-auto/src/main/java/xyz/auriium/mattlib2/auto/ff/LinearFFGenRoutine.java b/mattlib2-controls/src/main/java/xyz/auriium/mattlib2/auto/ff/LinearFFGenRoutine.java similarity index 100% rename from mattlib2-auto/src/main/java/xyz/auriium/mattlib2/auto/ff/LinearFFGenRoutine.java rename to mattlib2-controls/src/main/java/xyz/auriium/mattlib2/auto/ff/LinearFFGenRoutine.java diff --git a/mattlib2-auto/src/main/java/xyz/auriium/mattlib2/auto/ff/PolynomialRegression.java b/mattlib2-controls/src/main/java/xyz/auriium/mattlib2/auto/ff/PolynomialRegression.java similarity index 100% rename from mattlib2-auto/src/main/java/xyz/auriium/mattlib2/auto/ff/PolynomialRegression.java rename to mattlib2-controls/src/main/java/xyz/auriium/mattlib2/auto/ff/PolynomialRegression.java diff --git a/mattlib2-auto/src/main/java/xyz/auriium/mattlib2/auto/ff/RotationFFGenRoutine.java b/mattlib2-controls/src/main/java/xyz/auriium/mattlib2/auto/ff/RotationFFGenRoutine.java similarity index 100% rename from mattlib2-auto/src/main/java/xyz/auriium/mattlib2/auto/ff/RotationFFGenRoutine.java rename to mattlib2-controls/src/main/java/xyz/auriium/mattlib2/auto/ff/RotationFFGenRoutine.java diff --git a/mattlib2-auto/src/main/java/xyz/auriium/mattlib2/auto/pid/PIDGenComponent.java b/mattlib2-controls/src/main/java/xyz/auriium/mattlib2/auto/pid/PIDGenComponent.java similarity index 100% rename from mattlib2-auto/src/main/java/xyz/auriium/mattlib2/auto/pid/PIDGenComponent.java rename to mattlib2-controls/src/main/java/xyz/auriium/mattlib2/auto/pid/PIDGenComponent.java diff --git a/mattlib2-auto/src/test/java/xyz/auriium/mattlib2/auto/PolynomialRegressionTest.java b/mattlib2-controls/src/test/java/xyz/auriium/mattlib2/auto/PolynomialRegressionTest.java similarity index 56% rename from mattlib2-auto/src/test/java/xyz/auriium/mattlib2/auto/PolynomialRegressionTest.java rename to mattlib2-controls/src/test/java/xyz/auriium/mattlib2/auto/PolynomialRegressionTest.java index bb0b9df..ca3403d 100644 --- a/mattlib2-auto/src/test/java/xyz/auriium/mattlib2/auto/PolynomialRegressionTest.java +++ b/mattlib2-controls/src/test/java/xyz/auriium/mattlib2/auto/PolynomialRegressionTest.java @@ -2,6 +2,7 @@ import org.junit.jupiter.api.Assertions; import org.junit.jupiter.api.Test; +import xyz.auriium.mattlib2.auto.ff.FastPolynomialRegression; import xyz.auriium.mattlib2.auto.ff.PolynomialRegression; class PolynomialRegressionTest { @@ -17,5 +18,15 @@ public void testPolynomialRegressionIsSane() { Assertions.assertEquals(100, regression.predict(10), 1); Assertions.assertEquals(0.997, regression.R2(), 0.01); } +/* + @Test + public void testFastPolyReg() { + double[] x = {10, 20, 40, 80, 160, 200}; + double[] y = {100, 350, 1500, 6700, 20160, 40000}; + FastPolynomialRegression regression = FastPolynomialRegression.loadFullRank(x, y, 40); + + Assertions.assertEquals(100, regression.predict(10), 1); + //Assertions.assertEquals(0.997, regression.R2(), 0.01); + }*/ } \ No newline at end of file diff --git a/mattlib2-controls/src/test/java/xyz/auriium/mattlib2/auto/dynamics/FastSwerveKinematicsTest.java b/mattlib2-controls/src/test/java/xyz/auriium/mattlib2/auto/dynamics/FastSwerveKinematicsTest.java new file mode 100644 index 0000000..f219eda --- /dev/null +++ b/mattlib2-controls/src/test/java/xyz/auriium/mattlib2/auto/dynamics/FastSwerveKinematicsTest.java @@ -0,0 +1,55 @@ +package xyz.auriium.mattlib2.auto.dynamics; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import org.junit.jupiter.api.Assertions; +import org.junit.jupiter.api.Disabled; +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.*; + +class FastSwerveKinematicsTest { + + + @Disabled + @Test + void convertCentroidStateToModuleStateSafe() { + + Translation2d[] td = new Translation2d[]{ + new Translation2d(0.2286, 0.3429), + new Translation2d(0.2286, -0.3429), + new Translation2d(-0.3556, 0.3429), + new Translation2d(-0.3556, -0.3429) + }; + + SwerveDriveKinematics swerveDriveKinematics = new SwerveDriveKinematics( + td + ); + + var original = swerveDriveKinematics.toSwerveModuleStates(new ChassisSpeeds(3,2,0)); + + FastSwerveKinematics fastSwerveKinematics = FastSwerveKinematics.load( + td + ); + + var converted = fastSwerveKinematics.convertCentroidStateToModuleStateSafe(new ChassisSpeeds(3,2,0)); + + for (int i = 0; i < original.length; i++) { + Assertions.assertEquals(original[i].speedMetersPerSecond, converted[i].speedMetersPerSecond, 0.01); + Assertions.assertEquals(original[i].angle.getRadians(), converted[i].angle.getRadians(), 0.01); + } + } + + @Test + void convertModuleStateToCentroidState() { + } + + @Test + void convertCentroidStateToModuleState() { + } + + @Test + void testConvertModuleStateToCentroidState() { + } +} \ No newline at end of file diff --git a/mattlib2-auto/src/test/java/xyz/auriium/mattlib2/auto/ff/BaseFFGenRoutineTest.java b/mattlib2-controls/src/test/java/xyz/auriium/mattlib2/auto/ff/BaseFFGenRoutineTest.java similarity index 100% rename from mattlib2-auto/src/test/java/xyz/auriium/mattlib2/auto/ff/BaseFFGenRoutineTest.java rename to mattlib2-controls/src/test/java/xyz/auriium/mattlib2/auto/ff/BaseFFGenRoutineTest.java diff --git a/mattlib2-core/pom.xml b/mattlib2-core/pom.xml index 907e3f4..c780816 100644 --- a/mattlib2-core/pom.xml +++ b/mattlib2-core/pom.xml @@ -5,7 +5,7 @@ mattlib2 xyz.auriium - 1.6.4-SNAPSHOT + 1.7.1-SNAPSHOT 4.0.0 diff --git a/mattlib2-core/src/main/java/xyz/auriium/mattlib2/MattlibLooper.java b/mattlib2-core/src/main/java/xyz/auriium/mattlib2/MattlibLooper.java index 475e6d3..a1bc88c 100644 --- a/mattlib2-core/src/main/java/xyz/auriium/mattlib2/MattlibLooper.java +++ b/mattlib2-core/src/main/java/xyz/auriium/mattlib2/MattlibLooper.java @@ -43,11 +43,19 @@ public void runPeriodicLoop() { runnable.logicPeriodic(); } + for (IMattlibHooked runnable : orderedThingsToBeLooped) { + runnable.alwaysLogPeriodic(); + } + + + if (MattlibSettings.USE_LOGGING) { for (IMattlibHooked runnable : orderedThingsToBeLooped) { runnable.logPeriodic(); } + } + if (MattlibSettings.USE_TUNING) { for (IMattlibHooked runnable : orderedThingsToBeLooped) { runnable.tunePeriodic(); } diff --git a/mattlib2-core/src/main/java/xyz/auriium/mattlib2/MattlibSettings.java b/mattlib2-core/src/main/java/xyz/auriium/mattlib2/MattlibSettings.java index 06a65ea..9980484 100644 --- a/mattlib2-core/src/main/java/xyz/auriium/mattlib2/MattlibSettings.java +++ b/mattlib2-core/src/main/java/xyz/auriium/mattlib2/MattlibSettings.java @@ -12,6 +12,7 @@ public enum Robot { * Set this to false to kill logging */ public static boolean USE_LOGGING = true; + public static boolean USE_TUNING = true; public static boolean USE_TELEMETRY_TOO = true; public static Robot ROBOT = Robot.CARY; diff --git a/mattlib2-core/src/main/java/xyz/auriium/mattlib2/loop/IMattlibHooked.java b/mattlib2-core/src/main/java/xyz/auriium/mattlib2/loop/IMattlibHooked.java index 71269da..5ee4353 100644 --- a/mattlib2-core/src/main/java/xyz/auriium/mattlib2/loop/IMattlibHooked.java +++ b/mattlib2-core/src/main/java/xyz/auriium/mattlib2/loop/IMattlibHooked.java @@ -63,6 +63,11 @@ default void logicPeriodic() { } + /** + * This function will be logged even when logging is disabled + */ + default void alwaysLogPeriodic() {} + /** * This function should be run in robot periodic, optionally */ diff --git a/mattlib2-core/src/main/java/xyz/auriium/mattlib2/utils/AngleUtil.java b/mattlib2-core/src/main/java/xyz/auriium/mattlib2/utils/AngleUtil.java index bd2b69c..fccd094 100644 --- a/mattlib2-core/src/main/java/xyz/auriium/mattlib2/utils/AngleUtil.java +++ b/mattlib2-core/src/main/java/xyz/auriium/mattlib2/utils/AngleUtil.java @@ -1,12 +1,15 @@ package xyz.auriium.mattlib2.utils; + public class AngleUtil { + //TODO this doesn't conform to the reference frame stuff public static double normalizeRotations(double numberOnInfiniteSet_rotations) { double huge = numberOnInfiniteSet_rotations % 1; if (huge < 0) { huge +=1; } + return huge; } diff --git a/mattlib2-ctre/pom.xml b/mattlib2-ctre/pom.xml index d0607a3..971b5c9 100644 --- a/mattlib2-ctre/pom.xml +++ b/mattlib2-ctre/pom.xml @@ -6,7 +6,7 @@ xyz.auriium mattlib2 - 1.6.4-SNAPSHOT + 1.7.1-SNAPSHOT mattlib2-ctre @@ -28,12 +28,12 @@ xyz.auriium mattlib2-core - 1.6.4-SNAPSHOT + 1.7.1-SNAPSHOT xyz.auriium mattlib2-hardware - 1.6.4-SNAPSHOT + 1.7.1-SNAPSHOT com.ctre.phoenix6 diff --git a/mattlib2-ctre/src/main/java/xyz/auriium/mattlib/ctre/BaseTalonFXMotor.java b/mattlib2-ctre/src/main/java/xyz/auriium/mattlib/ctre/BaseTalonFXMotor.java index 73b003b..6702380 100644 --- a/mattlib2-ctre/src/main/java/xyz/auriium/mattlib/ctre/BaseTalonFXMotor.java +++ b/mattlib2-ctre/src/main/java/xyz/auriium/mattlib/ctre/BaseTalonFXMotor.java @@ -109,7 +109,7 @@ public ExplainedException[] verifyInit() { reverseLimitSwitchHit = talonFX.getReverseLimit(); if (motorComponent.fwReset_mechanismRot().isPresent() || motorComponent.rvReset_mechanismRot().isPresent()) { - BaseStatusSignal.setUpdateFrequencyForAll(50, + BaseStatusSignal.setUpdateFrequencyForAll(25, forwardLimitSwitchHit, reverseLimitSwitchHit ); @@ -131,21 +131,20 @@ public ExplainedException[] verifyInit() { BaseStatusSignal.setUpdateFrequencyForAll( - 25, + 12.5, currentNow, voltageOutput, - temperature, - velocity_mechanismRotationsPerSecond + temperature + ); BaseStatusSignal.setUpdateFrequencyForAll( 50, + velocity_mechanismRotationsPerSecond, position_mechanismRotations ); - System.out.println("DONE AND SYS: " + motorComponent.encoderToMechanismCoefficient()); - return exceptions; } @@ -194,16 +193,18 @@ public void logicPeriodic() { } if (botherRunningRvLimitLoop) { - var forwardNormally = motorComponent.reverseLimit().get(); + var reverseNormally = motorComponent.reverseLimit().get(); - boolean suddenlyClosed = forwardNormally == CommonMotorComponent.Normally.CLOSED && reverseLimitSwitchHit.getValue() == ReverseLimitValue.Open; - boolean suddenlyOpen = forwardNormally == CommonMotorComponent.Normally.OPEN && reverseLimitSwitchHit.getValue() == ReverseLimitValue.ClosedToGround; + boolean suddenlyClosed = reverseNormally == CommonMotorComponent.Normally.CLOSED && reverseLimitSwitchHit.getValue() == ReverseLimitValue.Open; + boolean suddenlyOpen = reverseNormally == CommonMotorComponent.Normally.OPEN && reverseLimitSwitchHit.getValue() == ReverseLimitValue.ClosedToGround; if (suddenlyOpen || suddenlyClosed) { talonFX.setPosition(motorComponent.rvReset_mechanismRot().get()); } } + talonFX.optimizeBusUtilization(); + } @@ -214,6 +215,25 @@ public void logPeriodic() { motorComponent.reportVoltageGiven(voltageOutput.getValue()); motorComponent.reportMechanismRotations(position_mechanismRotations.getValue()); motorComponent.reportMechanismVelocity(velocity_mechanismRotationsPerSecond.getValue()); + + if (botherRunningFwLimitLoop) { + var forwardNormally = motorComponent.forwardLimit().get(); + + boolean suddenlyClosed = forwardNormally == CommonMotorComponent.Normally.CLOSED && reverseLimitSwitchHit.getValue() == ReverseLimitValue.Open; + boolean suddenlyOpen = forwardNormally == CommonMotorComponent.Normally.OPEN && reverseLimitSwitchHit.getValue() == ReverseLimitValue.ClosedToGround; + + motorComponent.reportFwLimitTriggered(suddenlyClosed || suddenlyOpen); + + } + + if (botherRunningRvLimitLoop) { + var reverseNormally = motorComponent.reverseLimit().get(); + + boolean suddenlyClosed = reverseNormally == CommonMotorComponent.Normally.CLOSED && reverseLimitSwitchHit.getValue() == ReverseLimitValue.Open; + boolean suddenlyOpen = reverseNormally == CommonMotorComponent.Normally.OPEN && reverseLimitSwitchHit.getValue() == ReverseLimitValue.ClosedToGround; + + motorComponent.reportRvLimitTriggered(suddenlyClosed || suddenlyOpen); + } } diff --git a/mattlib2-foxe-ext/package-lock.json b/mattlib2-foxe-ext/package-lock.json index f7583d1..975fe7b 100644 --- a/mattlib2-foxe-ext/package-lock.json +++ b/mattlib2-foxe-ext/package-lock.json @@ -339,7 +339,7 @@ "@nodelib/fs.stat": "^2.0.2", "@nodelib/fs.walk": "^1.2.3", "glob-parent": "^5.1.2", - "merge2": "^1.6.4-SNAPSHOT", + "merge2": "^1.7.1-SNAPSHOT", "micromatch": "^4.0.4" }, "engines": { @@ -691,7 +691,7 @@ "@nodelib/fs.stat": "^2.0.2", "@nodelib/fs.walk": "^1.2.3", "glob-parent": "^5.1.2", - "merge2": "^1.6.4-SNAPSHOT", + "merge2": "^1.7.1-SNAPSHOT", "micromatch": "^4.0.4" }, "engines": { @@ -1136,7 +1136,7 @@ "@nodelib/fs.stat": "^2.0.2", "@nodelib/fs.walk": "^1.2.3", "glob-parent": "^5.1.2", - "merge2": "^1.6.4-SNAPSHOT", + "merge2": "^1.7.1-SNAPSHOT", "micromatch": "^4.0.4" }, "engines": { @@ -1558,7 +1558,7 @@ "@nodelib/fs.stat": "^2.0.2", "@nodelib/fs.walk": "^1.2.3", "glob-parent": "^5.1.2", - "merge2": "^1.6.4-SNAPSHOT", + "merge2": "^1.7.1-SNAPSHOT", "micromatch": "^4.0.4" }, "engines": { @@ -4391,7 +4391,7 @@ "inflight": "^1.0.4", "inherits": "2", "minimatch": "^3.1.1", - "once": "^1.6.4-SNAPSHOT", + "once": "^1.7.1-SNAPSHOT", "path-is-absolute": "^1.0.0" }, "engines": { @@ -4432,7 +4432,7 @@ "integrity": "sha512-k92I/b08q4wvFscXCLvqfsHCrjrF7yiXsQuIVvVE7N82W3+aqpzuUdBbfhWcy/FZR3/4IgflMgKLOsvPDrGCJA==", "dev": true, "dependencies": { - "once": "^1.6.4-SNAPSHOT", + "once": "^1.7.1-SNAPSHOT", "wrappy": "1" } }, @@ -5729,8 +5729,8 @@ } }, "node_modules/eslint-plugin-es/node_modules/eslint-visitor-keys": { - "version": "1.6.4-SNAPSHOT", - "resolved": "https://registry.npmjs.org/eslint-visitor-keys/-/eslint-visitor-keys-1.6.4-SNAPSHOT.tgz", + "version": "1.7.1-SNAPSHOT", + "resolved": "https://registry.npmjs.org/eslint-visitor-keys/-/eslint-visitor-keys-1.7.1-SNAPSHOT.tgz", "integrity": "sha512-6J72N8UNa462wa/KFODt/PJ3IU60SDpC3QXC1Hjc1BXXpfL2C9R5+AU7jhe0F6GREqVMh4Juu+NY7xn+6dipUQ==", "dev": true, "engines": { @@ -7162,7 +7162,7 @@ "@nodelib/fs.stat": "^2.0.2", "@nodelib/fs.walk": "^1.2.3", "glob-parent": "^5.1.2", - "merge2": "^1.6.4-SNAPSHOT", + "merge2": "^1.7.1-SNAPSHOT", "micromatch": "^4.0.4" }, "engines": { @@ -7585,8 +7585,8 @@ } }, "node_modules/eslint-plugin-prettier/node_modules/fast-diff": { - "version": "1.6.4-SNAPSHOT", - "resolved": "https://registry.npmjs.org/fast-diff/-/fast-diff-1.6.4-SNAPSHOT.tgz", + "version": "1.7.1-SNAPSHOT", + "resolved": "https://registry.npmjs.org/fast-diff/-/fast-diff-1.7.1-SNAPSHOT.tgz", "integrity": "sha512-VxPP4NqbUjj6MaAOafWeUn2cXWLcCtljklUtZf0Ind4XQ+QPtmA0b18zZy0jIQx+ExRVCR/ZQpBmik5lXshNsw==", "dev": true }, @@ -7599,7 +7599,7 @@ "@nodelib/fs.stat": "^2.0.2", "@nodelib/fs.walk": "^1.2.3", "glob-parent": "^5.1.2", - "merge2": "^1.6.4-SNAPSHOT", + "merge2": "^1.7.1-SNAPSHOT", "micromatch": "^4.0.4" }, "engines": { @@ -8739,7 +8739,7 @@ "inflight": "^1.0.4", "inherits": "2", "minimatch": "^3.1.1", - "once": "^1.6.4-SNAPSHOT", + "once": "^1.7.1-SNAPSHOT", "path-is-absolute": "^1.0.0" }, "engines": { @@ -8816,7 +8816,7 @@ "integrity": "sha512-k92I/b08q4wvFscXCLvqfsHCrjrF7yiXsQuIVvVE7N82W3+aqpzuUdBbfhWcy/FZR3/4IgflMgKLOsvPDrGCJA==", "dev": true, "dependencies": { - "once": "^1.6.4-SNAPSHOT", + "once": "^1.7.1-SNAPSHOT", "wrappy": "1" } }, diff --git a/mattlib2-hardware/pom.xml b/mattlib2-hardware/pom.xml index 89fee35..fdee2b3 100644 --- a/mattlib2-hardware/pom.xml +++ b/mattlib2-hardware/pom.xml @@ -5,7 +5,7 @@ mattlib2 xyz.auriium - 1.6.4-SNAPSHOT + 1.7.1-SNAPSHOT 4.0.0 @@ -21,7 +21,7 @@ xyz.auriium mattlib2-core - 1.6.4-SNAPSHOT + 1.7.1-SNAPSHOT xyz.auriium diff --git a/mattlib2-hardware/src/main/java/xyz/auriium/mattlib2/hardware/config/CommonPIDComponent.java b/mattlib2-hardware/src/main/java/xyz/auriium/mattlib2/hardware/config/CommonPIDComponent.java index 399b4ac..c6d955c 100644 --- a/mattlib2-hardware/src/main/java/xyz/auriium/mattlib2/hardware/config/CommonPIDComponent.java +++ b/mattlib2-hardware/src/main/java/xyz/auriium/mattlib2/hardware/config/CommonPIDComponent.java @@ -3,7 +3,6 @@ import xyz.auriium.mattlib2.log.INetworkedComponent; import xyz.auriium.mattlib2.log.annote.HasUpdated; import xyz.auriium.mattlib2.log.annote.Tune; -import yuukonfig.core.annotate.Key; /** * Commonly reused details of a PID controller @@ -13,7 +12,7 @@ public interface CommonPIDComponent extends INetworkedComponent { @Tune("p") double pConstant(); @Tune("i") double iConstant(); @Tune("d") double dConstant(); - + @Tune("tolerance") double tolerance_pidUnits(); @HasUpdated(keysToCheck = {"p", "i", "d"}) boolean hasUpdated(); diff --git a/mattlib2-hardware/src/main/java/xyz/auriium/mattlib2/hardware/config/CommonProfiledPIDComponent.java b/mattlib2-hardware/src/main/java/xyz/auriium/mattlib2/hardware/config/CommonProfiledPIDComponent.java new file mode 100644 index 0000000..b888376 --- /dev/null +++ b/mattlib2-hardware/src/main/java/xyz/auriium/mattlib2/hardware/config/CommonProfiledPIDComponent.java @@ -0,0 +1,4 @@ +package xyz.auriium.mattlib2.hardware.config; + +public interface CommonProfiledPIDComponent extends CommonPIDComponent { +} diff --git a/mattlib2-hardware/src/main/java/xyz/auriium/mattlib2/hardware/config/IndividualMotorComponent.java b/mattlib2-hardware/src/main/java/xyz/auriium/mattlib2/hardware/config/IndividualMotorComponent.java index e684d0f..61f78ef 100644 --- a/mattlib2-hardware/src/main/java/xyz/auriium/mattlib2/hardware/config/IndividualMotorComponent.java +++ b/mattlib2-hardware/src/main/java/xyz/auriium/mattlib2/hardware/config/IndividualMotorComponent.java @@ -35,4 +35,8 @@ public interface IndividualMotorComponent extends INetworkedComponent { @Log("position_mechanismRotations") void reportMechanismRotations(double mechanismRotations); @Log("velocity_mechanismRotationsPerSecond") void reportMechanismVelocity(double mechanismRotationsPerSecond); + @Log("forwardLimit_triggered") void reportFwLimitTriggered(boolean triggered); + @Log("reverseLimit_triggered") void reportRvLimitTriggered(boolean triggered); + + } diff --git a/mattlib2-log-foxe/pom.xml b/mattlib2-log-foxe/pom.xml index 5814430..a6d0d3f 100644 --- a/mattlib2-log-foxe/pom.xml +++ b/mattlib2-log-foxe/pom.xml @@ -5,7 +5,7 @@ mattlib2 xyz.auriium - 1.6.4-SNAPSHOT + 1.7.1-SNAPSHOT 4.0.0 @@ -41,7 +41,7 @@ xyz.auriium mattlib2-core - 1.6.4-SNAPSHOT + 1.7.1-SNAPSHOT diff --git a/mattlib2-log-nt/pom.xml b/mattlib2-log-nt/pom.xml index 2ca7118..484ecc1 100644 --- a/mattlib2-log-nt/pom.xml +++ b/mattlib2-log-nt/pom.xml @@ -5,7 +5,7 @@ mattlib2 xyz.auriium - 1.6.4-SNAPSHOT + 1.7.1-SNAPSHOT 4.0.0 @@ -21,7 +21,7 @@ xyz.auriium mattlib2-core - 1.6.4-SNAPSHOT + 1.7.1-SNAPSHOT edu.wpi.first.ntcore diff --git a/mattlib2-rev/pom.xml b/mattlib2-rev/pom.xml index 5470dd2..1b6f17a 100644 --- a/mattlib2-rev/pom.xml +++ b/mattlib2-rev/pom.xml @@ -5,7 +5,7 @@ mattlib2 xyz.auriium - 1.6.4-SNAPSHOT + 1.7.1-SNAPSHOT 4.0.0 @@ -28,18 +28,18 @@ xyz.auriium mattlib2-core - 1.6.4-SNAPSHOT + 1.7.1-SNAPSHOT xyz.auriium mattlib2-log-nt - 1.6.4-SNAPSHOT + 1.7.1-SNAPSHOT test xyz.auriium mattlib2-hardware - 1.6.4-SNAPSHOT + 1.7.1-SNAPSHOT com.revrobotics.frc diff --git a/mattlib2-rev/src/main/java/xyz/auriium/mattlib2/rev/BaseSparkMotor.java b/mattlib2-rev/src/main/java/xyz/auriium/mattlib2/rev/BaseSparkMotor.java index 30f0b7c..65667bd 100644 --- a/mattlib2-rev/src/main/java/xyz/auriium/mattlib2/rev/BaseSparkMotor.java +++ b/mattlib2-rev/src/main/java/xyz/auriium/mattlib2/rev/BaseSparkMotor.java @@ -43,6 +43,8 @@ class BaseSparkMotor implements ILinearMotor, IRotationalMotor, IMattlibHooked { double temperature = 0; double loadLinearCoef() { + + if (!linearCoefSet) { linearCoefSet = true; Optional coefOptional = motorComponent.rotationToMeterCoefficient(); diff --git a/mattlib2-rev/src/main/java/xyz/auriium/mattlib2/rev/HardwareREV.java b/mattlib2-rev/src/main/java/xyz/auriium/mattlib2/rev/HardwareREV.java index 1609e58..5c8f2e8 100644 --- a/mattlib2-rev/src/main/java/xyz/auriium/mattlib2/rev/HardwareREV.java +++ b/mattlib2-rev/src/main/java/xyz/auriium/mattlib2/rev/HardwareREV.java @@ -131,6 +131,28 @@ public static IRotationalController rotationalSpark_builtInPID(MotorComponent mo } + /** + * + * @param motorComponent + * @param pdConfig + * @param stateObserver + * @return use onboard pid controler + */ + public static IRotationalController rotationalSpark_onboardPID(MotorComponent motorComponent, PIDComponent pdConfig, IRotationEncoder stateObserver) { + int canId = motorComponent.id(); + GenericPath possiblyNullPath = IDS_ALREADY_SEEN.get(canId); + GenericPath pathOfComponent = motorComponent.selfPath(); + + if (possiblyNullPath != null) { + Exceptions.DUPLICATE_IDS_FOUND(pathOfComponent, canId, possiblyNullPath); + } + + + CANSparkMax sparkMax = createSparkmax(motorComponent); + RelativeEncoder relativeEncoder = sparkMax.getEncoder(); + + return new OnboardRotationController(sparkMax, motorComponent, relativeEncoder, stateObserver, pdConfig); + } /** * Sets up a mattlib SparkMax device that is tasked with controlling linear motion; diff --git a/mattlib2-rev/src/main/java/xyz/auriium/mattlib2/rev/OnboardLinearController.java b/mattlib2-rev/src/main/java/xyz/auriium/mattlib2/rev/OnboardLinearController.java new file mode 100644 index 0000000..f05c2dc --- /dev/null +++ b/mattlib2-rev/src/main/java/xyz/auriium/mattlib2/rev/OnboardLinearController.java @@ -0,0 +1,63 @@ +package xyz.auriium.mattlib2.rev; + + +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.math.controller.PIDController; + +import xyz.auriium.mattlib2.hardware.ILinearController; +import xyz.auriium.mattlib2.hardware.ILinearEncoder; +import xyz.auriium.mattlib2.hardware.ILinearMotor; +import xyz.auriium.mattlib2.hardware.config.MotorComponent; +import xyz.auriium.mattlib2.hardware.config.PIDComponent; +import xyz.auriium.yuukonstants.exception.ExplainedException; + +public class OnboardLinearController extends BaseSparkMotor implements ILinearController{ + + final ILinearEncoder stateObserver; + final PIDComponent pidComponent; + + public OnboardLinearController(CANSparkMax sparkMax, MotorComponent motorComponent, RelativeEncoder encoder, ILinearEncoder stateObserver, PIDComponent pidComponent) { + super(sparkMax, motorComponent, encoder); + this.stateObserver = stateObserver; + this.pidComponent = pidComponent; + } + + final PIDController pidController = new PIDController(0,0,0); + + + @Override + public ExplainedException[] verifyInit() { + var ee = super.verifyInit(); + + pidController.setPID( + pidComponent.pConstant(), + pidComponent.iConstant(), + pidComponent.dConstant() + ); + + pidController.setTolerance(pidComponent.tolerance_pidUnits()); + + return ee; + } + + @Override public void tunePeriodic() { + super.tunePeriodic(); + + if (pidComponent.hasUpdated()) { + pidController.setP(pidComponent.pConstant()); + pidController.setI(pidComponent.iConstant()); + pidController.setD(pidComponent.dConstant()); + pidController.setTolerance(pidComponent.tolerance_pidUnits()); + } + + } + + @Override public void controlToLinearReferenceArbitrary(double setpointMechanism_meters, double arbitraryFF_volts) { + double measurement_mechanismMeters = stateObserver.linearPosition_mechanismMeters(); + double feedbackVoltage = pidController.calculate(measurement_mechanismMeters, setpointMechanism_meters); + + setToVoltage(feedbackVoltage + arbitraryFF_volts); + } + +} diff --git a/mattlib2-rev/src/main/java/xyz/auriium/mattlib2/rev/OnboardRotationController.java b/mattlib2-rev/src/main/java/xyz/auriium/mattlib2/rev/OnboardRotationController.java new file mode 100644 index 0000000..bdc96ae --- /dev/null +++ b/mattlib2-rev/src/main/java/xyz/auriium/mattlib2/rev/OnboardRotationController.java @@ -0,0 +1,64 @@ +package xyz.auriium.mattlib2.rev; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import edu.wpi.first.math.controller.PIDController; +import xyz.auriium.mattlib2.hardware.IRotationEncoder; +import xyz.auriium.mattlib2.hardware.IRotationalController; +import xyz.auriium.mattlib2.hardware.config.MotorComponent; +import xyz.auriium.mattlib2.hardware.config.PIDComponent; + +public class OnboardRotationController extends BaseSparkMotor implements IRotationalController { + + final IRotationEncoder stateObserver; + final PIDComponent pidComponent; + + public OnboardRotationController(CANSparkMax sparkMax, MotorComponent motorComponent, RelativeEncoder encoder, IRotationEncoder stateObserver, PIDComponent pidComponent) { + super(sparkMax, motorComponent, encoder); + this.stateObserver = stateObserver; + this.pidComponent = pidComponent; + } + + final PIDController pidController = new PIDController(0,0,0); + boolean normalizedMode; + + double setpoint_primeUnits = 0; + double observation_primeUnits = 0; + + @Override public void logPeriodic() { + pidComponent.reportState(observation_primeUnits); + pidComponent.reportReference(setpoint_primeUnits); + } + + @Override + public void controlToNormalizedReferenceArbitrary(double setpoint_mechanismNormalizedRotations, double arbitraryFF_volts) { + if (!normalizedMode) { + normalizedMode = true; + pidController.enableContinuousInput(0,1); + } + + double measurement_mechanismNormalizedRotations = stateObserver.angularPosition_normalizedMechanismRotations(); + double feedbackVoltage = pidController.calculate(measurement_mechanismNormalizedRotations, setpoint_mechanismNormalizedRotations); + + setpoint_primeUnits = setpoint_mechanismNormalizedRotations; + observation_primeUnits = measurement_mechanismNormalizedRotations; + + setToVoltage(feedbackVoltage + arbitraryFF_volts); + } + + @Override + public void controlToInfiniteReferenceArbitrary(double setpoint_mechanismRotations, double arbitraryFF_volts) { + if (normalizedMode) { + normalizedMode = false; + pidController.disableContinuousInput(); + } + + double measurement_mechanismArbitraryRotations = stateObserver.angularPosition_mechanismRotations(); + double feedbackVoltage = pidController.calculate(measurement_mechanismArbitraryRotations, setpoint_mechanismRotations); + + setpoint_primeUnits = setpoint_mechanismRotations; + observation_primeUnits = measurement_mechanismArbitraryRotations; + + setToVoltage(feedbackVoltage + arbitraryFF_volts); + } +} diff --git a/mattlib2-sim/pom.xml b/mattlib2-sim/pom.xml index 54f3b48..20003b2 100644 --- a/mattlib2-sim/pom.xml +++ b/mattlib2-sim/pom.xml @@ -5,7 +5,7 @@ mattlib2 xyz.auriium - 1.6.4-SNAPSHOT + 1.7.1-SNAPSHOT 4.0.0 @@ -21,7 +21,7 @@ xyz.auriium mattlib2-hardware - 1.6.4-SNAPSHOT + 1.7.1-SNAPSHOT edu.wpi.first.wpilibj diff --git a/mattlib2-sim/src/main/java/xyz/auriium/mattlib2/sim/DCSimController.java b/mattlib2-sim/src/main/java/xyz/auriium/mattlib2/sim/DCSimController.java index 39b46fb..b683379 100644 --- a/mattlib2-sim/src/main/java/xyz/auriium/mattlib2/sim/DCSimController.java +++ b/mattlib2-sim/src/main/java/xyz/auriium/mattlib2/sim/DCSimController.java @@ -35,8 +35,15 @@ public void tunePeriodic() { } } + boolean isContinuous = false; + @Override public void controlToLinearReferenceArbitrary(double setpointMechanism_meters, double arbFF) { + if (isContinuous) { + isContinuous = false; + pidController.disableContinuousInput(); + } + double coef = motorComponent.rotationToMeterCoefficient().orElseThrow(() -> Exceptions.MOTOR_NOT_LINEAR(motorComponent.selfPath())); double controlEffort = pidController.calculate( @@ -52,33 +59,29 @@ public void controlToLinearReferenceArbitrary(double setpointMechanism_meters, d @Override public void controlToNormalizedReferenceArbitrary(double setpoint_mechanismNormalizedRotations, double arbFF) { - - - double measurement_mechanismNormalizedRotations = angularPosition_normalizedMechanismRotations(); - double currentAngle_mechanismNormalizedRotations = measurement_mechanismNormalizedRotations % 1d; - if (currentAngle_mechanismNormalizedRotations < 0d) { - currentAngle_mechanismNormalizedRotations += 1d; //no idea why this works + if (!isContinuous) { + isContinuous = true; + pidController.enableContinuousInput(0, 1); } - // take (infinite - normalized) for (current offset) then add (setpoint normalized) for (setpoint infinite) - double reference_mechanismInfiniteRotations = setpoint_mechanismNormalizedRotations - + measurement_mechanismNormalizedRotations - - currentAngle_mechanismNormalizedRotations; + double controlEffort = pidController.calculate( + this.angularPosition_encoderRotations(), + setpoint_mechanismNormalizedRotations + ); - // more modulus code i don't understand - if (setpoint_mechanismNormalizedRotations - currentAngle_mechanismNormalizedRotations > 0.5) { - reference_mechanismInfiniteRotations -= 1d; - } else if (setpoint_mechanismNormalizedRotations - currentAngle_mechanismNormalizedRotations < -0.5) { - reference_mechanismInfiniteRotations += 1d; - } + this.setToVoltage(controlEffort + arbFF); - controlToInfiniteReferenceArbitrary(reference_mechanismInfiniteRotations, arbFF); } @Override public void controlToInfiniteReferenceArbitrary(double setpoint_mechanismRotations, double arbFF) { + if (isContinuous) { + isContinuous = false; + pidController.disableContinuousInput(); + } + double controlEffort = pidController.calculate( this.angularPosition_encoderRotations(), setpoint_mechanismRotations diff --git a/pom.xml b/pom.xml index a0db5c7..ebc54de 100644 --- a/pom.xml +++ b/pom.xml @@ -7,13 +7,13 @@ xyz.auriium mattlib2 pom - 1.6.4-SNAPSHOT + 1.7.2bad-SNAPSHOT mattlib2-core mattlib2-rev mattlib2-log-foxe mattlib2-log-nt - mattlib2-auto + mattlib2-controls mattlib2-hardware mattlib2-sim mattlib2-ctre