diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp index 407746c0ae..cbd916d298 100644 --- a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp @@ -129,7 +129,7 @@ void DifferentialDrivetrainSim::SetPose(const frc::Pose2d& pose) { Vectord<7> DifferentialDrivetrainSim::Dynamics(const Vectord<7>& x, const Vectord<2>& u) { - // Because G^2 can be factored out of A, we can divide by the old ratio + // Because G² can be factored out of A, we can divide by the old ratio // squared and multiply by the new ratio squared to get a new drivetrain // model. Matrixd<4, 2> B; diff --git a/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h index 420ff4eea1..c1cc1d7c59 100644 --- a/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h @@ -255,7 +255,7 @@ class DifferentialDrivetrainSim { static DifferentialDrivetrainSim CreateKitbotSim( frc::DCMotor motor, double gearing, units::meter_t wheelSize, const std::array& measurementStdDevs = {}) { - // MOI estimation -- note that I = m r^2 for point masses + // MOI estimation -- note that I = mr² for point masses units::kilogram_square_meter_t batteryMoi = 12.5_lb * 10_in * 10_in; units::kilogram_square_meter_t gearboxMoi = (2.8_lb + 2.0_lb) * 2 // CIM plus toughbox per side diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java index 1be4872b0c..0bdb76f63e 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java @@ -323,7 +323,7 @@ public class DifferentialDrivetrainSim { var B = new Matrix<>(Nat.N4(), Nat.N2()); B.assignBlock(0, 0, m_plant.getB().times(this.m_currentGearing / this.m_originalGearing)); - // Because G^2 can be factored out of A, we can divide by the old ratio squared and multiply + // Because G² can be factored out of A, we can divide by the old ratio squared and multiply // by the new ratio squared to get a new drivetrain model. var A = new Matrix<>(Nat.N4(), Nat.N4()); A.assignBlock( @@ -455,7 +455,7 @@ public class DifferentialDrivetrainSim { KitbotGearing gearing, KitbotWheelSize wheelSize, Matrix measurementStdDevs) { - // MOI estimation -- note that I = m r^2 for point masses + // MOI estimation -- note that I = mr² for point masses var batteryMoi = 12.5 / 2.2 * Math.pow(Units.inchesToMeters(10), 2); var gearboxMoi = (2.8 /* CIM motor */ * 2 / 2.2 + 2.0 /* Toughbox Mini- ish */) diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java b/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java index 9bbeaf6032..9471422bd6 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java +++ b/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java @@ -81,25 +81,25 @@ public class CubicHermiteSpline extends Spline { private SimpleMatrix makeHermiteBasis() { if (hermiteBasis == null) { // Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find - // the coefficients of the spline P(t) = a3 * t^3 + a2 * t^2 + a1 * t + a0. + // the coefficients of the spline P(t) = a₃t³ + a₂t² + a₁t + a₀. // - // P(i) = P(0) = a0 - // P'(i) = P'(0) = a1 - // P(i+1) = P(1) = a3 + a2 + a1 + a0 - // P'(i+1) = P'(1) = 3 * a3 + 2 * a2 + a1 + // P(i) = P(0) = a₀ + // P'(i) = P'(0) = a₁ + // P(i+1) = P(1) = a₃ + a₂ + a₁ + a₀ + // P'(i+1) = P'(1) = 3a₃ + 2a₂ + a₁ // - // [ P(i) ] = [ 0 0 0 1 ][ a3 ] - // [ P'(i) ] = [ 0 0 1 0 ][ a2 ] - // [ P(i+1) ] = [ 1 1 1 1 ][ a1 ] - // [ P'(i+1) ] = [ 3 2 1 0 ][ a0 ] + // [P(i) ] = [0 0 0 1][a₃] + // [P'(i) ] = [0 0 1 0][a₂] + // [P(i+1) ] = [1 1 1 1][a₁] + // [P'(i+1)] = [3 2 1 0][a₀] // // To solve for the coefficients, we can invert the 4x4 matrix and move it // to the other side of the equation. // - // [ a3 ] = [ 2 1 -2 1 ][ P(i) ] - // [ a2 ] = [ -3 -2 3 -1 ][ P'(i) ] - // [ a1 ] = [ 0 1 0 0 ][ P(i+1) ] - // [ a0 ] = [ 1 0 0 0 ][ P'(i+1) ] + // [a₃] = [ 2 1 -2 1][P(i) ] + // [a₂] = [-3 -2 3 -1][P'(i) ] + // [a₁] = [ 0 1 0 0][P(i+1) ] + // [a₀] = [ 1 0 0 0][P'(i+1)] hermiteBasis = new SimpleMatrix( 4, diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java b/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java index 40170443f6..7dbdef4eea 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java +++ b/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java @@ -80,33 +80,33 @@ public class QuinticHermiteSpline extends Spline { */ private SimpleMatrix makeHermiteBasis() { if (hermiteBasis == null) { - // Given P(i), P'(i), P''(i), P(i+1), P'(i+1), P''(i+1), the control - // vectors, we want to find the coefficients of the spline - // P(t) = a5 * t^5 + a4 * t^4 + a3 * t^3 + a2 * t^2 + a1 * t + a0. + // Given P(i), P'(i), P"(i), P(i+1), P'(i+1), P"(i+1), the control vectors, + // we want to find the coefficients of the spline + // P(t) = a₅t⁵ + a₄t⁴ + a₃t³ + a₂t² + a₁t + a₀. // - // P(i) = P(0) = a0 - // P'(i) = P'(0) = a1 - // P''(i) = P''(0) = 2 * a2 - // P(i+1) = P(1) = a5 + a4 + a3 + a2 + a1 + a0 - // P'(i+1) = P'(1) = 5 * a5 + 4 * a4 + 3 * a3 + 2 * a2 + a1 - // P''(i+1) = P''(1) = 20 * a5 + 12 * a4 + 6 * a3 + 2 * a2 + // P(i) = P(0) = a₀ + // P'(i) = P'(0) = a₁ + // P''(i) = P"(0) = 2a₂ + // P(i+1) = P(1) = a₅ + a₄ + a₃ + a₂ + a₁ + a₀ + // P'(i+1) = P'(1) = 5a₅ + 4a₄ + 3a₃ + 2a₂ + a₁ + // P"(i+1) = P"(1) = 20a₅ + 12a₄ + 6a₃ + 2a₂ // - // [ P(i) ] = [ 0 0 0 0 0 1 ][ a5 ] - // [ P'(i) ] = [ 0 0 0 0 1 0 ][ a4 ] - // [ P''(i) ] = [ 0 0 0 2 0 0 ][ a3 ] - // [ P(i+1) ] = [ 1 1 1 1 1 1 ][ a2 ] - // [ P'(i+1) ] = [ 5 4 3 2 1 0 ][ a1 ] - // [ P''(i+1) ] = [ 20 12 6 2 0 0 ][ a0 ] + // [P(i) ] = [ 0 0 0 0 0 1][a₅] + // [P'(i) ] = [ 0 0 0 0 1 0][a₄] + // [P"(i) ] = [ 0 0 0 2 0 0][a₃] + // [P(i+1) ] = [ 1 1 1 1 1 1][a₂] + // [P'(i+1)] = [ 5 4 3 2 1 0][a₁] + // [P"(i+1)] = [20 12 6 2 0 0][a₀] // // To solve for the coefficients, we can invert the 6x6 matrix and move it // to the other side of the equation. // - // [ a5 ] = [ -6.0 -3.0 -0.5 6.0 -3.0 0.5 ][ P(i) ] - // [ a4 ] = [ 15.0 8.0 1.5 -15.0 7.0 -1.0 ][ P'(i) ] - // [ a3 ] = [ -10.0 -6.0 -1.5 10.0 -4.0 0.5 ][ P''(i) ] - // [ a2 ] = [ 0.0 0.0 0.5 0.0 0.0 0.0 ][ P(i+1) ] - // [ a1 ] = [ 0.0 1.0 0.0 0.0 0.0 0.0 ][ P'(i+1) ] - // [ a0 ] = [ 1.0 0.0 0.0 0.0 0.0 0.0 ][ P''(i+1) ] + // [a₅] = [ -6.0 -3.0 -0.5 6.0 -3.0 0.5][P(i) ] + // [a₄] = [ 15.0 8.0 1.5 -15.0 7.0 -1.0][P'(i) ] + // [a₃] = [-10.0 -6.0 -1.5 10.0 -4.0 0.5][P"(i) ] + // [a₂] = [ 0.0 0.0 0.5 0.0 0.0 0.0][P(i+1) ] + // [a₁] = [ 0.0 1.0 0.0 0.0 0.0 0.0][P'(i+1)] + // [a₀] = [ 1.0 0.0 0.0 0.0 0.0 0.0][P"(i+1)] hermiteBasis = new SimpleMatrix( 6, diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java index 1a05eb6104..75390c21ae 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java @@ -211,7 +211,7 @@ public final class LinearSystemId { } /** - * Identify a velocity system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec^2). These + * Identify a velocity system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec²). These * constants cam be found using SysId. The states of the system are [velocity], inputs are * [voltage], and outputs are [velocity]. * @@ -241,7 +241,7 @@ public final class LinearSystemId { } /** - * Identify a position system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec^2). These + * Identify a position system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec²). These * constants cam be found using SysId. The states of the system are [position, velocity]ᵀ, inputs * are [voltage], and outputs are [position]. * @@ -272,8 +272,8 @@ public final class LinearSystemId { /** * Identify a standard differential drive drivetrain, given the drivetrain's kV and kA in both - * linear (volts/(meter/sec) and volts/(meter/sec^2)) and angular (volts/(meter/sec) and - * volts/(meter/sec^2)) cases. This can be found using SysId. The states of the system are [left + * linear (volts/(meter/sec) and volts/(meter/sec²)) and angular (volts/(meter/sec) and + * volts/(meter/sec²)) cases. This can be found using SysId. The states of the system are [left * velocity, right velocity]ᵀ, inputs are [left voltage, right voltage]ᵀ, and outputs are [left * velocity, right velocity]ᵀ. * @@ -316,8 +316,8 @@ public final class LinearSystemId { /** * Identify a standard differential drive drivetrain, given the drivetrain's kV and kA in both - * linear (volts/(meter/sec) and volts/(meter/sec^2)) and angular (volts/(radian/sec) and - * volts/(radian/sec^2)) cases. This can be found using SysId. The states of the system are [left + * linear (volts/(meter/sec) and volts/(meter/sec²)) and angular (volts/(radian/sec) and + * volts/(radian/sec²)) cases. This can be found using SysId. The states of the system are [left * velocity, right velocity]ᵀ, inputs are [left voltage, right voltage]ᵀ, and outputs are [left * velocity, right velocity]ᵀ. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java index 2ec42441c2..d74b9f72af 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java @@ -332,7 +332,7 @@ public class Trajectory { final double newV = velocityMetersPerSecond + (accelerationMetersPerSecondSq * deltaT); // Calculate the change in position. - // delta_s = v_0 t + 0.5 at^2 + // delta_s = v_0 t + 0.5at² final double newS = (velocityMetersPerSecond * deltaT + 0.5 * accelerationMetersPerSecondSq * Math.pow(deltaT, 2)) diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java index d7fbf599d8..28c4153211 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java @@ -90,7 +90,7 @@ public final class TrajectoryParameterizer { // acceleration, since acceleration limits may be a function of velocity. while (true) { // Enforce global max velocity and max reachable velocity by global - // acceleration limit. vf = std::sqrt(vi^2 + 2*a*d). + // acceleration limit. v_f = √(v_i² + 2ad). constrainedState.maxVelocityMetersPerSecond = Math.min( maxVelocityMetersPerSecond, @@ -164,7 +164,7 @@ public final class TrajectoryParameterizer { while (true) { // Enforce max velocity limit (reverse) - // vf = std::sqrt(vi^2 + 2*a*d), where vi = successor. + // v_f = √(v_i² + 2ad), where v_i = successor. double newMaxVelocity = Math.sqrt( successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/CentripetalAccelerationConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/CentripetalAccelerationConstraint.java index 13138e84ce..7a47fcac4c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/CentripetalAccelerationConstraint.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/CentripetalAccelerationConstraint.java @@ -38,12 +38,12 @@ public class CentripetalAccelerationConstraint implements TrajectoryConstraint { @Override public double getMaxVelocityMetersPerSecond( Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) { - // ac = v^2 / r - // k (curvature) = 1 / r + // ac = v²/r + // k (curvature) = 1/r - // therefore, ac = v^2 * k - // ac / k = v^2 - // v = std::sqrt(ac / k) + // therefore, ac = v²k + // ac/k = v² + // v = √(ac/k) return Math.sqrt( m_maxCentripetalAccelerationMetersPerSecondSq / Math.abs(curvatureRadPerMeter)); diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java index c3bd226c2f..856671ea40 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java @@ -65,11 +65,16 @@ public class EllipticalRegionConstraint implements TrajectoryConstraint { * @return Whether the robot pose is within the constraint region. */ public boolean isPoseInRegion(Pose2d robotPose) { - // The region (disk) bounded by the ellipse is given by the equation: - // ((x-h)^2)/Rx^2) + ((y-k)^2)/Ry^2) <= 1 + // The region bounded by the ellipse is given by the equation: + // + // (x−h)²/Rx² + (y−k)²/Ry² ≤ 1 + // + // Multiply by Rx²Ry² for efficiency reasons: + // + // (x−h)²Ry² + (y−k)²Rx² ≤ Rx²Ry² + // // If the inequality is satisfied, then it is inside the ellipse; otherwise // it is outside the ellipse. - // Both sides have been multiplied by Rx^2 * Ry^2 for efficiency reasons. return Math.pow(robotPose.getX() - m_center.getX(), 2) * Math.pow(m_radii.getY(), 2) + Math.pow(robotPose.getY() - m_center.getY(), 2) * Math.pow(m_radii.getX(), 2) <= Math.pow(m_radii.getX(), 2) * Math.pow(m_radii.getY(), 2); diff --git a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp index ecb36c2b80..097edce726 100644 --- a/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp +++ b/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp @@ -46,7 +46,7 @@ Trajectory::State Trajectory::State::Interpolate(State endValue, const units::meters_per_second_t newV = velocity + (acceleration * deltaT); // Calculate the change in position. - // delta_s = v_0 t + 0.5 at^2 + // delta_s = v_0 t + 0.5at² const units::meter_t newS = (velocity * deltaT + 0.5 * acceleration * deltaT * deltaT) * (reversing ? -1.0 : 1.0); diff --git a/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp index d397d0ce64..92d52ed2ce 100644 --- a/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp +++ b/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp @@ -62,7 +62,7 @@ Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory( // acceleration, since acceleration limits may be a function of velocity. while (true) { // Enforce global max velocity and max reachable velocity by global - // acceleration limit. vf = std::sqrt(vi^2 + 2*a*d). + // acceleration limit. v_f = √(v_i² + 2ad). constrainedState.maxVelocity = units::math::min( maxVelocity, @@ -126,7 +126,7 @@ Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory( while (true) { // Enforce max velocity limit (reverse) - // vf = std::sqrt(vi^2 + 2*a*d), where vi = successor. + // v_f = √(v_i² + 2ad), where v_i = successor. units::meters_per_second_t newMaxVelocity = units::math::sqrt(successor.maxVelocity * successor.maxVelocity + successor.minAcceleration * ds * 2.0); @@ -187,10 +187,10 @@ Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory( if (i > 0) { states.at(i - 1).acceleration = reversed ? -accel : accel; if (units::math::abs(accel) > 1E-6_mps_sq) { - // v_f = v_0 + a * t + // v_f = v_0 + at dt = (state.maxVelocity - v) / accel; } else if (units::math::abs(v) > 1E-6_mps) { - // delta_x = v * t + // delta_x = vt dt = ds / v; } else { throw std::runtime_error(fmt::format( diff --git a/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp b/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp index 738d2439b0..40913a806b 100644 --- a/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp +++ b/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp @@ -15,12 +15,12 @@ CentripetalAccelerationConstraint::CentripetalAccelerationConstraint( units::meters_per_second_t CentripetalAccelerationConstraint::MaxVelocity( const Pose2d& pose, units::curvature_t curvature, units::meters_per_second_t velocity) const { - // ac = v^2 / r - // k (curvature) = 1 / r + // ac = v²/r + // k (curvature) = 1/r - // therefore, ac = v^2 * k - // ac / k = v^2 - // v = std::sqrt(ac / k) + // therefore, ac = v²k + // ac/k = v² + // v = √(ac/k) // We have to multiply by 1_rad here to get the units to cancel out nicely. // The units library defines a unit for radians although it is technically diff --git a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h index 1f0e4077a3..2741668132 100644 --- a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h @@ -37,7 +37,7 @@ class WPILIB_DLLEXPORT ArmFeedforward { * @param kS The static gain, in volts. * @param kG The gravity gain, in volts. * @param kV The velocity gain, in volt seconds per radian. - * @param kA The acceleration gain, in volt seconds^2 per radian. + * @param kA The acceleration gain, in volt seconds² per radian. */ constexpr ArmFeedforward( units::volt_t kS, units::volt_t kG, units::unit_t kV, @@ -49,7 +49,7 @@ class WPILIB_DLLEXPORT ArmFeedforward { * * @param angle The angle setpoint, in radians. * @param velocity The velocity setpoint, in radians per second. - * @param acceleration The acceleration setpoint, in radians per second^2. + * @param acceleration The acceleration setpoint, in radians per second². * @return The computed feedforward, in volts. */ units::volt_t Calculate(units::unit_t angle, diff --git a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h index 269a7e635a..e07c26b21a 100644 --- a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h @@ -33,7 +33,7 @@ class ElevatorFeedforward { * @param kS The static gain, in volts. * @param kG The gravity gain, in volts. * @param kV The velocity gain, in volt seconds per distance. - * @param kA The acceleration gain, in volt seconds^2 per distance. + * @param kA The acceleration gain, in volt seconds² per distance. */ constexpr ElevatorFeedforward( units::volt_t kS, units::volt_t kG, units::unit_t kV, @@ -44,7 +44,7 @@ class ElevatorFeedforward { * Calculates the feedforward from the gains and setpoints. * * @param velocity The velocity setpoint, in distance per second. - * @param acceleration The acceleration setpoint, in distance per second^2. + * @param acceleration The acceleration setpoint, in distance per second². * @return The computed feedforward, in volts. */ constexpr units::volt_t Calculate(units::unit_t velocity, diff --git a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h index 27852f7499..86b40eab06 100644 --- a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h @@ -35,7 +35,7 @@ class SimpleMotorFeedforward { * * @param kS The static gain, in volts. * @param kV The velocity gain, in volt seconds per distance. - * @param kA The acceleration gain, in volt seconds^2 per distance. + * @param kA The acceleration gain, in volt seconds² per distance. */ constexpr SimpleMotorFeedforward( units::volt_t kS, units::unit_t kV, @@ -46,7 +46,7 @@ class SimpleMotorFeedforward { * Calculates the feedforward from the gains and setpoints. * * @param velocity The velocity setpoint, in distance per second. - * @param acceleration The acceleration setpoint, in distance per second^2. + * @param acceleration The acceleration setpoint, in distance per second². * @return The computed feedforward, in volts. */ constexpr units::volt_t Calculate(units::unit_t velocity, diff --git a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h index 771f7ab1fe..0636707958 100644 --- a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h +++ b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h @@ -51,25 +51,25 @@ class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> { */ static Matrixd<4, 4> MakeHermiteBasis() { // Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find - // the coefficients of the spline P(t) = a3 * t^3 + a2 * t^2 + a1 * t + a0. + // the coefficients of the spline P(t) = a₃t³ + a₂t² + a₁t + a₀. // - // P(i) = P(0) = a0 - // P'(i) = P'(0) = a1 - // P(i+1) = P(1) = a3 + a2 + a1 + a0 - // P'(i+1) = P'(1) = 3 * a3 + 2 * a2 + a1 + // P(i) = P(0) = a₀ + // P'(i) = P'(0) = a₁ + // P(i+1) = P(1) = a₃ + a₂ + a₁ + a₀ + // P'(i+1) = P'(1) = 3a₃ + 2a₂ + a₁ // - // [ P(i) ] = [ 0 0 0 1 ][ a3 ] - // [ P'(i) ] = [ 0 0 1 0 ][ a2 ] - // [ P(i+1) ] = [ 1 1 1 1 ][ a1 ] - // [ P'(i+1) ] = [ 3 2 1 0 ][ a0 ] + // [P(i) ] = [0 0 0 1][a₃] + // [P'(i) ] = [0 0 1 0][a₂] + // [P(i+1) ] = [1 1 1 1][a₁] + // [P'(i+1)] = [3 2 1 0][a₀] // // To solve for the coefficients, we can invert the 4x4 matrix and move it // to the other side of the equation. // - // [ a3 ] = [ 2 1 -2 1 ][ P(i) ] - // [ a2 ] = [ -3 -2 3 -1 ][ P'(i) ] - // [ a1 ] = [ 0 1 0 0 ][ P(i+1) ] - // [ a0 ] = [ 1 0 0 0 ][ P'(i+1) ] + // [a₃] = [ 2 1 -2 1][P(i) ] + // [a₂] = [-3 -2 3 -1][P'(i) ] + // [a₁] = [ 0 1 0 0][P(i+1) ] + // [a₀] = [ 1 0 0 0][P'(i+1)] static const Matrixd<4, 4> basis{{+2.0, +1.0, -2.0, +1.0}, {-3.0, -2.0, +3.0, -1.0}, diff --git a/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h index 5743ee9069..ad03a23919 100644 --- a/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h +++ b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h @@ -50,33 +50,33 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> { * @return The hermite basis matrix for quintic hermite spline interpolation. */ static Matrixd<6, 6> MakeHermiteBasis() { - // Given P(i), P'(i), P''(i), P(i+1), P'(i+1), P''(i+1), the control - // vectors, we want to find the coefficients of the spline - // P(t) = a5 * t^5 + a4 * t^4 + a3 * t^3 + a2 * t^2 + a1 * t + a0. + // Given P(i), P'(i), P"(i), P(i+1), P'(i+1), P"(i+1), the control vectors, + // we want to find the coefficients of the spline + // P(t) = a₅t⁵ + a₄t⁴ + a₃t³ + a₂t² + a₁t + a₀. // - // P(i) = P(0) = a0 - // P'(i) = P'(0) = a1 - // P''(i) = P''(0) = 2 * a2 - // P(i+1) = P(1) = a5 + a4 + a3 + a2 + a1 + a0 - // P'(i+1) = P'(1) = 5 * a5 + 4 * a4 + 3 * a3 + 2 * a2 + a1 - // P''(i+1) = P''(1) = 20 * a5 + 12 * a4 + 6 * a3 + 2 * a2 + // P(i) = P(0) = a₀ + // P'(i) = P'(0) = a₁ + // P''(i) = P"(0) = 2a₂ + // P(i+1) = P(1) = a₅ + a₄ + a₃ + a₂ + a₁ + a₀ + // P'(i+1) = P'(1) = 5a₅ + 4a₄ + 3a₃ + 2a₂ + a₁ + // P"(i+1) = P"(1) = 20a₅ + 12a₄ + 6a₃ + 2a₂ // - // [ P(i) ] = [ 0 0 0 0 0 1 ][ a5 ] - // [ P'(i) ] = [ 0 0 0 0 1 0 ][ a4 ] - // [ P''(i) ] = [ 0 0 0 2 0 0 ][ a3 ] - // [ P(i+1) ] = [ 1 1 1 1 1 1 ][ a2 ] - // [ P'(i+1) ] = [ 5 4 3 2 1 0 ][ a1 ] - // [ P''(i+1) ] = [ 20 12 6 2 0 0 ][ a0 ] + // [P(i) ] = [ 0 0 0 0 0 1][a₅] + // [P'(i) ] = [ 0 0 0 0 1 0][a₄] + // [P"(i) ] = [ 0 0 0 2 0 0][a₃] + // [P(i+1) ] = [ 1 1 1 1 1 1][a₂] + // [P'(i+1)] = [ 5 4 3 2 1 0][a₁] + // [P"(i+1)] = [20 12 6 2 0 0][a₀] // // To solve for the coefficients, we can invert the 6x6 matrix and move it // to the other side of the equation. // - // [ a5 ] = [ -6.0 -3.0 -0.5 6.0 -3.0 0.5 ][ P(i) ] - // [ a4 ] = [ 15.0 8.0 1.5 -15.0 7.0 -1.0 ][ P'(i) ] - // [ a3 ] = [ -10.0 -6.0 -1.5 10.0 -4.0 0.5 ][ P''(i) ] - // [ a2 ] = [ 0.0 0.0 0.5 0.0 0.0 0.0 ][ P(i+1) ] - // [ a1 ] = [ 0.0 1.0 0.0 0.0 0.0 0.0 ][ P'(i+1) ] - // [ a0 ] = [ 1.0 0.0 0.0 0.0 0.0 0.0 ][ P''(i+1) ] + // [a₅] = [ -6.0 -3.0 -0.5 6.0 -3.0 0.5][P(i) ] + // [a₄] = [ 15.0 8.0 1.5 -15.0 7.0 -1.0][P'(i) ] + // [a₃] = [-10.0 -6.0 -1.5 10.0 -4.0 0.5][P"(i) ] + // [a₂] = [ 0.0 0.0 0.5 0.0 0.0 0.0][P(i+1) ] + // [a₁] = [ 0.0 1.0 0.0 0.0 0.0 0.0][P'(i+1)] + // [a₀] = [ 1.0 0.0 0.0 0.0 0.0 0.0][P"(i+1)] static const Matrixd<6, 6> basis{ {-06.0, -03.0, -00.5, +06.0, -03.0, +00.5}, diff --git a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h index 9c9768b570..d2a57b3dce 100644 --- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h +++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h @@ -116,7 +116,7 @@ class WPILIB_DLLEXPORT LinearSystemId { * u = K_v v + K_a a * * @param kV The velocity gain, in volt seconds per distance. - * @param kA The acceleration gain, in volt seconds^2 per distance. + * @param kA The acceleration gain, in volt seconds² per distance. * @throws std::domain_error if kV <= 0 or kA <= 0. */ template (pose.X() - m_center.X()) * units::math::pow<2>(m_radii.Y()) + units::math::pow<2>(pose.Y() - m_center.Y()) * diff --git a/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java index f0b39c6b91..5e6d5bbb69 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java @@ -141,7 +141,7 @@ class LinearFilterTest { 3, // f(x) = ln(x) (double x) -> Math.log(x), - // df/dx = 1 / x + // df/dx = 1/x (double x) -> 1.0 / x, h, 1.0, @@ -174,7 +174,7 @@ class LinearFilterTest { 5, // f(x) = ln(x) (double x) -> Math.log(x), - // d²f/dx² = -1 / x² + // d²f/dx² = -1/x² (double x) -> -1.0 / (x * x), h, 1.0, @@ -213,7 +213,7 @@ class LinearFilterTest { 2, // f(x) = ln(x) (double x) -> Math.log(x), - // df/dx = 1 / x + // df/dx = 1/x (double x) -> 1.0 / x, h, 1.0, @@ -246,7 +246,7 @@ class LinearFilterTest { 4, // f(x) = ln(x) (double x) -> Math.log(x), - // d²f/dx² = -1 / x² + // d²f/dx² = -1/x² (double x) -> -1.0 / (x * x), h, 1.0, diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java index db4b76b474..64abab4652 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java @@ -102,8 +102,8 @@ class SwerveDriveKinematicsTest { var moduleStates = m_kinematics.toSwerveModuleStates(speeds); /* - The circumference of the wheels about the COR is pi * diameter, or 2 * pi * radius - the radius is the sqrt(12^2in + 12^2in), or 16.9706in, so the circumference the wheels + The circumference of the wheels about the COR is π * diameter, or 2π * radius + the radius is the √(12²in + 12²in), or 16.9706in, so the circumference the wheels trace out is 106.629190516in. since we want our robot to rotate at 1 rotation per second, our wheels must trace out 1 rotation (or 106.63 inches) per second. */ @@ -143,7 +143,7 @@ class SwerveDriveKinematicsTest { This one is a bit trickier. Because we are rotating about the front-left wheel, it should be parked at 0 degrees and 0 speed. The front-right and back-left wheels both travel an arc with radius 24 (and circumference 150.796), and the back-right wheel travels an arc with - radius sqrt(24^2 + 24^2) and circumference 213.2584. As for angles, the front-right wheel + radius √(24² + 24²) and circumference 213.2584. As for angles, the front-right wheel should be pointing straight forward, the back-left wheel should be pointing straight right, and the back-right wheel should be at a -45 degree angle */ @@ -169,12 +169,12 @@ class SwerveDriveKinematicsTest { var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState); /* - We already know that our omega should be 2pi from the previous test. Next, we need to determine + We already know that our omega should be 2π from the previous test. Next, we need to determine the vx and vy of our chassis center. Because our COR is at a 45 degree angle from the center, we know that vx and vy must be the same. Furthermore, we know that the center of mass makes a full revolution about the center of revolution once every second. Therefore, the center of mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 triagle are - 1:sqrt(2)/2:sqrt(2)/2, we find that the COM vx is -75.398, and vy is 75.398. + 1:√(2)/2:√(2)/2, we find that the COM vx is -75.398, and vy is 75.398. */ assertAll( diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java index e4df93f975..3db878e8e3 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java @@ -15,16 +15,16 @@ import org.junit.jupiter.api.Test; class RungeKuttaTimeVaryingTest { private static Matrix rungeKuttaTimeVaryingSolution(double t) { return new MatBuilder<>(Nat.N1(), Nat.N1()) - .fill(12.0 * Math.exp(t) / (Math.pow(Math.exp(t) + 1.0, 2.0))); + .fill(12.0 * Math.exp(t) / Math.pow(Math.exp(t) + 1.0, 2.0)); } // Tests RK4 with a time varying solution. From // http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html: - // x' = x (2 / (e^t + 1) - 1) + // x' = x (2/(eᵗ + 1) - 1) // // The true (analytical) solution is: // - // x(t) = 12 * e^t / ((e^t + 1)^2) + // x(t) = 12eᵗ/(eᵗ + 1)² @Test void rungeKuttaTimeVaryingTest() { final var y0 = rungeKuttaTimeVaryingSolution(5.0); diff --git a/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp b/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp index 56121faf02..4f6c7d16de 100644 --- a/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp +++ b/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp @@ -207,14 +207,14 @@ TEST(LinearFilterOutputTest, CentralFiniteDifference) { return std::log(x); }, [](double x) { - // df/dx = 1 / x + // df/dx = 1/x return 1.0 / x; }, h, 1.0, 20.0); AssertCentralResults<2, 5>( [](double x) { - // f(x) = x^2 + // f(x) = x² return x * x; }, [](double x) { @@ -240,7 +240,7 @@ TEST(LinearFilterOutputTest, CentralFiniteDifference) { return std::log(x); }, [](double x) { - // d²f/dx² = -1 / x² + // d²f/dx² = -1/x² return -1.0 / (x * x); }, h, 1.0, 20.0); @@ -280,14 +280,14 @@ TEST(LinearFilterOutputTest, BackwardFiniteDifference) { return std::log(x); }, [](double x) { - // df/dx = 1 / x + // df/dx = 1/x return 1.0 / x; }, h, 1.0, 20.0); AssertBackwardResults<2, 4>( [](double x) { - // f(x) = x^2 + // f(x) = x² return x * x; }, [](double x) { @@ -313,7 +313,7 @@ TEST(LinearFilterOutputTest, BackwardFiniteDifference) { return std::log(x); }, [](double x) { - // d²f/dx² = -1 / x² + // d²f/dx² = -1/x² return -1.0 / (x * x); }, h, 1.0, 20.0); diff --git a/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp b/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp index d5311f85ab..70f193892f 100644 --- a/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp +++ b/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp @@ -10,18 +10,17 @@ namespace { frc::Vectord<1> RungeKuttaTimeVaryingSolution(double t) { - return frc::Vectord<1>{12.0 * std::exp(t) / - (std::pow(std::exp(t) + 1.0, 2.0))}; + return frc::Vectord<1>{12.0 * std::exp(t) / std::pow(std::exp(t) + 1.0, 2.0)}; } } // namespace // Tests RK4 with a time varying solution. From // http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html: -// x' = x (2 / (e^t + 1) - 1) +// x' = x (2/(eᵗ + 1) - 1) // // The true (analytical) solution is: // -// x(t) = 12 * e^t / ((e^t + 1)^2) +// x(t) = 12eᵗ/((eᵗ + 1)²) TEST(RungeKuttaTimeVaryingTest, RungeKuttaTimeVarying) { frc::Vectord<1> y0 = RungeKuttaTimeVaryingSolution(5.0);