From 3a6103f98c7c661017b21e33f9f978205afbe51b Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Sun, 1 Oct 2023 22:18:18 -0400 Subject: [PATCH 01/76] added nextV calc methods to ArmFF & Elevator FF --- .../first/math/controller/ArmFeedforward.java | 143 +++++++++++++----- .../math/controller/ElevatorFeedforward.java | 21 +++ 2 files changed, 124 insertions(+), 40 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java index 2e9a8d84348..74193948e22 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java @@ -4,8 +4,14 @@ package edu.wpi.first.math.controller; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.system.plant.LinearSystemId; + /** - * A helper class that computes feedforward outputs for a simple arm (modeled as a motor acting + * A helper class that computes feedforward outputs for a simple arm (modeled as + * a motor acting * against the force of gravity on a beam suspended at an angle). */ public class ArmFeedforward { @@ -15,7 +21,8 @@ public class ArmFeedforward { public final double ka; /** - * Creates a new ArmFeedforward with the specified gains. Units of the gain values will dictate + * Creates a new ArmFeedforward with the specified gains. Units of the gain + * values will dictate * units of the computed feedforward. * * @param ks The static gain. @@ -31,7 +38,8 @@ public ArmFeedforward(double ks, double kg, double kv, double ka) { } /** - * Creates a new ArmFeedforward with the specified gains. Acceleration gain is defaulted to zero. + * Creates a new ArmFeedforward with the specified gains. Acceleration gain is + * defaulted to zero. * Units of the gain values will dictate units of the computed feedforward. * * @param ks The static gain. @@ -45,10 +53,13 @@ public ArmFeedforward(double ks, double kg, double kv) { /** * Calculates the feedforward from the gains and setpoints. * - * @param positionRadians The position (angle) setpoint. This angle should be measured from the - * horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If - * your encoder does not follow this convention, an offset should be added. - * @param velocityRadPerSec The velocity setpoint. + * @param positionRadians The position (angle) setpoint. This angle should + * be measured from the + * horizontal (i.e. if the provided angle is 0, the + * arm should be parallel with the floor). If + * your encoder does not follow this convention, an + * offset should be added. + * @param velocityRadPerSec The velocity setpoint. * @param accelRadPerSecSquared The acceleration setpoint. * @return The computed feedforward. */ @@ -61,13 +72,45 @@ public double calculate( } /** - * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be + * Calculates the feedforward from the gains and setpoints. + * + * @param positionRadians The position (angle) setpoint. This angle + * should be + * measured from the + * horizontal (i.e. if the provided angle is 0, + * the arm + * should be parallel with the floor). If + * your encoder does not follow this convention, + * an + * offset should be added. + * @param currentVelocityRadPerSec The current velocity setpoint. + * @param nextVelocityRadPerSec The next velocity setpoint. + * @param dtSeconds Time between velocity setpoints in seconds. + * @return The computed feedforward. + */ + public double calculate(double positionRadians, double currentVelocityRadPerSec, double nextVelocityRadPerSec, double dtSeconds) { + var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka); + var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds); + + var r = Matrix.mat(Nat.N1(), Nat.N1()).fill(currentVelocityRadPerSec); + var nextR = Matrix.mat(Nat.N1(), Nat.N1()).fill(nextVelocityRadPerSec); + + return ks * Math.signum(currentVelocityRadPerSec) + kg * Math.cos(positionRadians) + + feedforward.calculate(r, nextR).get(0, 0); + } + + /** + * Calculates the feedforward from the gains and velocity setpoint (acceleration + * is assumed to be * zero). * - * @param positionRadians The position (angle) setpoint. This angle should be measured from the - * horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If - * your encoder does not follow this convention, an offset should be added. - * @param velocity The velocity setpoint. + * @param positionRadians The position (angle) setpoint. This angle should be + * measured from the + * horizontal (i.e. if the provided angle is 0, the arm + * should be parallel with the floor). If + * your encoder does not follow this convention, an + * offset should be added. + * @param velocity The velocity setpoint. * @return The computed feedforward. */ public double calculate(double positionRadians, double velocity) { @@ -78,15 +121,20 @@ public double calculate(double positionRadians, double velocity) { // formulas for the methods below: /** - * Calculates the maximum achievable velocity given a maximum voltage supply, a position, and an - * acceleration. Useful for ensuring that velocity and acceleration constraints for a trapezoidal - * profile are simultaneously achievable - enter the acceleration constraint, and this will give + * Calculates the maximum achievable velocity given a maximum voltage supply, a + * position, and an + * acceleration. Useful for ensuring that velocity and acceleration constraints + * for a trapezoidal + * profile are simultaneously achievable - enter the acceleration constraint, + * and this will give * you a simultaneously-achievable velocity constraint. * - * @param maxVoltage The maximum voltage that can be supplied to the arm. - * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if - * the provided angle is 0, the arm should be parallel with the floor). If your encoder does - * not follow this convention, an offset should be added. + * @param maxVoltage The maximum voltage that can be supplied to the arm. + * @param angle The angle of the arm. This angle should be measured from + * the horizontal (i.e. if + * the provided angle is 0, the arm should be parallel with + * the floor). If your encoder does + * not follow this convention, an offset should be added. * @param acceleration The acceleration of the arm. * @return The maximum possible velocity at the given acceleration and angle. */ @@ -96,15 +144,20 @@ public double maxAchievableVelocity(double maxVoltage, double angle, double acce } /** - * Calculates the minimum achievable velocity given a maximum voltage supply, a position, and an - * acceleration. Useful for ensuring that velocity and acceleration constraints for a trapezoidal - * profile are simultaneously achievable - enter the acceleration constraint, and this will give + * Calculates the minimum achievable velocity given a maximum voltage supply, a + * position, and an + * acceleration. Useful for ensuring that velocity and acceleration constraints + * for a trapezoidal + * profile are simultaneously achievable - enter the acceleration constraint, + * and this will give * you a simultaneously-achievable velocity constraint. * - * @param maxVoltage The maximum voltage that can be supplied to the arm. - * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if - * the provided angle is 0, the arm should be parallel with the floor). If your encoder does - * not follow this convention, an offset should be added. + * @param maxVoltage The maximum voltage that can be supplied to the arm. + * @param angle The angle of the arm. This angle should be measured from + * the horizontal (i.e. if + * the provided angle is 0, the arm should be parallel with + * the floor). If your encoder does + * not follow this convention, an offset should be added. * @param acceleration The acceleration of the arm. * @return The minimum possible velocity at the given acceleration and angle. */ @@ -114,16 +167,21 @@ public double minAchievableVelocity(double maxVoltage, double angle, double acce } /** - * Calculates the maximum achievable acceleration given a maximum voltage supply, a position, and - * a velocity. Useful for ensuring that velocity and acceleration constraints for a trapezoidal - * profile are simultaneously achievable - enter the velocity constraint, and this will give you a + * Calculates the maximum achievable acceleration given a maximum voltage + * supply, a position, and + * a velocity. Useful for ensuring that velocity and acceleration constraints + * for a trapezoidal + * profile are simultaneously achievable - enter the velocity constraint, and + * this will give you a * simultaneously-achievable acceleration constraint. * * @param maxVoltage The maximum voltage that can be supplied to the arm. - * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if - * the provided angle is 0, the arm should be parallel with the floor). If your encoder does - * not follow this convention, an offset should be added. - * @param velocity The velocity of the arm. + * @param angle The angle of the arm. This angle should be measured from + * the horizontal (i.e. if + * the provided angle is 0, the arm should be parallel with + * the floor). If your encoder does + * not follow this convention, an offset should be added. + * @param velocity The velocity of the arm. * @return The maximum possible acceleration at the given velocity. */ public double maxAchievableAcceleration(double maxVoltage, double angle, double velocity) { @@ -131,16 +189,21 @@ public double maxAchievableAcceleration(double maxVoltage, double angle, double } /** - * Calculates the minimum achievable acceleration given a maximum voltage supply, a position, and - * a velocity. Useful for ensuring that velocity and acceleration constraints for a trapezoidal - * profile are simultaneously achievable - enter the velocity constraint, and this will give you a + * Calculates the minimum achievable acceleration given a maximum voltage + * supply, a position, and + * a velocity. Useful for ensuring that velocity and acceleration constraints + * for a trapezoidal + * profile are simultaneously achievable - enter the velocity constraint, and + * this will give you a * simultaneously-achievable acceleration constraint. * * @param maxVoltage The maximum voltage that can be supplied to the arm. - * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if - * the provided angle is 0, the arm should be parallel with the floor). If your encoder does - * not follow this convention, an offset should be added. - * @param velocity The velocity of the arm. + * @param angle The angle of the arm. This angle should be measured from + * the horizontal (i.e. if + * the provided angle is 0, the arm should be parallel with + * the floor). If your encoder does + * not follow this convention, an offset should be added. + * @param velocity The velocity of the arm. * @return The minimum possible acceleration at the given velocity. */ public double minAchievableAcceleration(double maxVoltage, double angle, double velocity) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java index 9f4dd863ff3..2b1683374d1 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java @@ -3,6 +3,9 @@ // the WPILib BSD license file in the root directory of this project. package edu.wpi.first.math.controller; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.system.plant.LinearSystemId; /** * A helper class that computes feedforward outputs for a simple elevator (modeled as a motor acting @@ -53,6 +56,24 @@ public double calculate(double velocity, double acceleration) { return ks * Math.signum(velocity) + kg + kv * velocity + ka * acceleration; } + /** + * Calculates the feedforward from the gains and setpoints. + * + * @param currentVelocity The current velocity setpoint. + * @param nextVelocity The next velocity setpoint. + * @param dtSeconds Time between velocity setpoints in seconds. + * @return The computed feedforward. + */ + public double calculate(double currentVelocity, double nextVelocity, double dtSeconds) { + var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka); + var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds); + + var r = Matrix.mat(Nat.N1(), Nat.N1()).fill(currentVelocity); + var nextR = Matrix.mat(Nat.N1(), Nat.N1()).fill(nextVelocity); + + return kg + ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0); + } + /** * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be * zero). From 57867c7bb074eadbce6fe96a1e8eeb86bc48dc60 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Sun, 1 Oct 2023 23:11:10 -0400 Subject: [PATCH 02/76] revert ArmFeedForward back --- .../first/math/controller/ArmFeedforward.java | 145 +++++------------- 1 file changed, 41 insertions(+), 104 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java index 74193948e22..84e4149060f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java @@ -4,14 +4,8 @@ package edu.wpi.first.math.controller; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Nat; -import edu.wpi.first.math.system.plant.LinearSystemId; - /** - * A helper class that computes feedforward outputs for a simple arm (modeled as - * a motor acting + * A helper class that computes feedforward outputs for a simple arm (modeled as a motor acting * against the force of gravity on a beam suspended at an angle). */ public class ArmFeedforward { @@ -21,8 +15,7 @@ public class ArmFeedforward { public final double ka; /** - * Creates a new ArmFeedforward with the specified gains. Units of the gain - * values will dictate + * Creates a new ArmFeedforward with the specified gains. Units of the gain values will dictate * units of the computed feedforward. * * @param ks The static gain. @@ -38,8 +31,7 @@ public ArmFeedforward(double ks, double kg, double kv, double ka) { } /** - * Creates a new ArmFeedforward with the specified gains. Acceleration gain is - * defaulted to zero. + * Creates a new ArmFeedforward with the specified gains. Acceleration gain is defaulted to zero. * Units of the gain values will dictate units of the computed feedforward. * * @param ks The static gain. @@ -53,13 +45,10 @@ public ArmFeedforward(double ks, double kg, double kv) { /** * Calculates the feedforward from the gains and setpoints. * - * @param positionRadians The position (angle) setpoint. This angle should - * be measured from the - * horizontal (i.e. if the provided angle is 0, the - * arm should be parallel with the floor). If - * your encoder does not follow this convention, an - * offset should be added. - * @param velocityRadPerSec The velocity setpoint. + * @param positionRadians The position (angle) setpoint. This angle should be measured from the + * horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If + * your encoder does not follow this convention, an offset should be added. + * @param velocityRadPerSec The velocity setpoint. * @param accelRadPerSecSquared The acceleration setpoint. * @return The computed feedforward. */ @@ -72,45 +61,13 @@ public double calculate( } /** - * Calculates the feedforward from the gains and setpoints. - * - * @param positionRadians The position (angle) setpoint. This angle - * should be - * measured from the - * horizontal (i.e. if the provided angle is 0, - * the arm - * should be parallel with the floor). If - * your encoder does not follow this convention, - * an - * offset should be added. - * @param currentVelocityRadPerSec The current velocity setpoint. - * @param nextVelocityRadPerSec The next velocity setpoint. - * @param dtSeconds Time between velocity setpoints in seconds. - * @return The computed feedforward. - */ - public double calculate(double positionRadians, double currentVelocityRadPerSec, double nextVelocityRadPerSec, double dtSeconds) { - var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka); - var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds); - - var r = Matrix.mat(Nat.N1(), Nat.N1()).fill(currentVelocityRadPerSec); - var nextR = Matrix.mat(Nat.N1(), Nat.N1()).fill(nextVelocityRadPerSec); - - return ks * Math.signum(currentVelocityRadPerSec) + kg * Math.cos(positionRadians) - + feedforward.calculate(r, nextR).get(0, 0); - } - - /** - * Calculates the feedforward from the gains and velocity setpoint (acceleration - * is assumed to be + * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be * zero). * - * @param positionRadians The position (angle) setpoint. This angle should be - * measured from the - * horizontal (i.e. if the provided angle is 0, the arm - * should be parallel with the floor). If - * your encoder does not follow this convention, an - * offset should be added. - * @param velocity The velocity setpoint. + * @param positionRadians The position (angle) setpoint. This angle should be measured from the + * horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If + * your encoder does not follow this convention, an offset should be added. + * @param velocity The velocity setpoint. * @return The computed feedforward. */ public double calculate(double positionRadians, double velocity) { @@ -121,20 +78,15 @@ public double calculate(double positionRadians, double velocity) { // formulas for the methods below: /** - * Calculates the maximum achievable velocity given a maximum voltage supply, a - * position, and an - * acceleration. Useful for ensuring that velocity and acceleration constraints - * for a trapezoidal - * profile are simultaneously achievable - enter the acceleration constraint, - * and this will give + * Calculates the maximum achievable velocity given a maximum voltage supply, a position, and an + * acceleration. Useful for ensuring that velocity and acceleration constraints for a trapezoidal + * profile are simultaneously achievable - enter the acceleration constraint, and this will give * you a simultaneously-achievable velocity constraint. * - * @param maxVoltage The maximum voltage that can be supplied to the arm. - * @param angle The angle of the arm. This angle should be measured from - * the horizontal (i.e. if - * the provided angle is 0, the arm should be parallel with - * the floor). If your encoder does - * not follow this convention, an offset should be added. + * @param maxVoltage The maximum voltage that can be supplied to the arm. + * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if + * the provided angle is 0, the arm should be parallel with the floor). If your encoder does + * not follow this convention, an offset should be added. * @param acceleration The acceleration of the arm. * @return The maximum possible velocity at the given acceleration and angle. */ @@ -144,20 +96,15 @@ public double maxAchievableVelocity(double maxVoltage, double angle, double acce } /** - * Calculates the minimum achievable velocity given a maximum voltage supply, a - * position, and an - * acceleration. Useful for ensuring that velocity and acceleration constraints - * for a trapezoidal - * profile are simultaneously achievable - enter the acceleration constraint, - * and this will give + * Calculates the minimum achievable velocity given a maximum voltage supply, a position, and an + * acceleration. Useful for ensuring that velocity and acceleration constraints for a trapezoidal + * profile are simultaneously achievable - enter the acceleration constraint, and this will give * you a simultaneously-achievable velocity constraint. * - * @param maxVoltage The maximum voltage that can be supplied to the arm. - * @param angle The angle of the arm. This angle should be measured from - * the horizontal (i.e. if - * the provided angle is 0, the arm should be parallel with - * the floor). If your encoder does - * not follow this convention, an offset should be added. + * @param maxVoltage The maximum voltage that can be supplied to the arm. + * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if + * the provided angle is 0, the arm should be parallel with the floor). If your encoder does + * not follow this convention, an offset should be added. * @param acceleration The acceleration of the arm. * @return The minimum possible velocity at the given acceleration and angle. */ @@ -167,21 +114,16 @@ public double minAchievableVelocity(double maxVoltage, double angle, double acce } /** - * Calculates the maximum achievable acceleration given a maximum voltage - * supply, a position, and - * a velocity. Useful for ensuring that velocity and acceleration constraints - * for a trapezoidal - * profile are simultaneously achievable - enter the velocity constraint, and - * this will give you a + * Calculates the maximum achievable acceleration given a maximum voltage supply, a position, and + * a velocity. Useful for ensuring that velocity and acceleration constraints for a trapezoidal + * profile are simultaneously achievable - enter the velocity constraint, and this will give you a * simultaneously-achievable acceleration constraint. * * @param maxVoltage The maximum voltage that can be supplied to the arm. - * @param angle The angle of the arm. This angle should be measured from - * the horizontal (i.e. if - * the provided angle is 0, the arm should be parallel with - * the floor). If your encoder does - * not follow this convention, an offset should be added. - * @param velocity The velocity of the arm. + * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if + * the provided angle is 0, the arm should be parallel with the floor). If your encoder does + * not follow this convention, an offset should be added. + * @param velocity The velocity of the arm. * @return The maximum possible acceleration at the given velocity. */ public double maxAchievableAcceleration(double maxVoltage, double angle, double velocity) { @@ -189,24 +131,19 @@ public double maxAchievableAcceleration(double maxVoltage, double angle, double } /** - * Calculates the minimum achievable acceleration given a maximum voltage - * supply, a position, and - * a velocity. Useful for ensuring that velocity and acceleration constraints - * for a trapezoidal - * profile are simultaneously achievable - enter the velocity constraint, and - * this will give you a + * Calculates the minimum achievable acceleration given a maximum voltage supply, a position, and + * a velocity. Useful for ensuring that velocity and acceleration constraints for a trapezoidal + * profile are simultaneously achievable - enter the velocity constraint, and this will give you a * simultaneously-achievable acceleration constraint. * * @param maxVoltage The maximum voltage that can be supplied to the arm. - * @param angle The angle of the arm. This angle should be measured from - * the horizontal (i.e. if - * the provided angle is 0, the arm should be parallel with - * the floor). If your encoder does - * not follow this convention, an offset should be added. - * @param velocity The velocity of the arm. + * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if + * the provided angle is 0, the arm should be parallel with the floor). If your encoder does + * not follow this convention, an offset should be added. + * @param velocity The velocity of the arm. * @return The minimum possible acceleration at the given velocity. */ public double minAchievableAcceleration(double maxVoltage, double angle, double velocity) { return maxAchievableAcceleration(-maxVoltage, angle, velocity); } -} +} \ No newline at end of file From 181ec2f78237521033186f2d580f560327b4568b Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Sun, 1 Oct 2023 23:20:02 -0400 Subject: [PATCH 03/76] ArmFF newline at EOF, ElevatorFF note about 0 --- .../main/java/edu/wpi/first/math/controller/ArmFeedforward.java | 2 +- .../java/edu/wpi/first/math/controller/ElevatorFeedforward.java | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java index 84e4149060f..2e9a8d84348 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java @@ -146,4 +146,4 @@ public double maxAchievableAcceleration(double maxVoltage, double angle, double public double minAchievableAcceleration(double maxVoltage, double angle, double velocity) { return maxAchievableAcceleration(-maxVoltage, angle, velocity); } -} \ No newline at end of file +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java index 2b1683374d1..b0093adf968 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java @@ -58,6 +58,7 @@ public double calculate(double velocity, double acceleration) { /** * Calculates the feedforward from the gains and setpoints. + * Note this method ignores the error that occurs when the velocity crosses 0. * * @param currentVelocity The current velocity setpoint. * @param nextVelocity The next velocity setpoint. From 3583ac354ed047ed53956a38cb3acf9b40775ebd Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Sun, 1 Oct 2023 23:22:56 -0400 Subject: [PATCH 04/76] Added Derivation to Elevator FF nextV method --- .../math/controller/ElevatorFeedforward.java | 20 ++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java index b0093adf968..a616be7e5f5 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java @@ -56,7 +56,7 @@ public double calculate(double velocity, double acceleration) { return ks * Math.signum(velocity) + kg + kv * velocity + ka * acceleration; } - /** + /** * Calculates the feedforward from the gains and setpoints. * Note this method ignores the error that occurs when the velocity crosses 0. * @@ -65,6 +65,24 @@ public double calculate(double velocity, double acceleration) { * @param dtSeconds Time between velocity setpoints in seconds. * @return The computed feedforward. */ + // dx/dt = Ax + Bu + c + // dx/dt = Ax + B(u + B⁺c) + // xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ) + // xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ) + // xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ + + // Solve for uₖ. + + // B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ) + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ + + // For an elevator with the model dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x), + // A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka). + + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka)) + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka) + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks public double calculate(double currentVelocity, double nextVelocity, double dtSeconds) { var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka); var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds); From 88b60f4e92c3313c475b2f4891f6ad0388dae031 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Sun, 1 Oct 2023 23:25:37 -0400 Subject: [PATCH 05/76] Derivation repositioned --- .../java/edu/wpi/first/math/controller/ElevatorFeedforward.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java index a616be7e5f5..fec56b1e286 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java @@ -65,6 +65,7 @@ public double calculate(double velocity, double acceleration) { * @param dtSeconds Time between velocity setpoints in seconds. * @return The computed feedforward. */ + public double calculate(double currentVelocity, double nextVelocity, double dtSeconds) { // dx/dt = Ax + Bu + c // dx/dt = Ax + B(u + B⁺c) // xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ) @@ -83,7 +84,6 @@ public double calculate(double velocity, double acceleration) { // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka)) // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka) // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks - public double calculate(double currentVelocity, double nextVelocity, double dtSeconds) { var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka); var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds); From 70dad83920169e92c7f1e4c1658663d41aca7b35 Mon Sep 17 00:00:00 2001 From: narmstro2020 <70530535+narmstro2020@users.noreply.github.com> Date: Sun, 1 Oct 2023 23:29:06 -0400 Subject: [PATCH 06/76] Update wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java Co-authored-by: Tyler Veness --- .../edu/wpi/first/math/controller/ElevatorFeedforward.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java index fec56b1e286..fae2239927f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java @@ -58,7 +58,8 @@ public double calculate(double velocity, double acceleration) { /** * Calculates the feedforward from the gains and setpoints. - * Note this method ignores the error that occurs when the velocity crosses 0. + * + * Note this method is inaccurate when the velocity crosses 0. * * @param currentVelocity The current velocity setpoint. * @param nextVelocity The next velocity setpoint. From 533759ed533c2516ed3e786dbe2251beb080c743 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Sun, 1 Oct 2023 23:37:35 -0400 Subject: [PATCH 07/76] fixing comments --- .../math/controller/ElevatorFeedforward.java | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java index fae2239927f..fa37a22717c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java @@ -67,24 +67,24 @@ public double calculate(double velocity, double acceleration) { * @return The computed feedforward. */ public double calculate(double currentVelocity, double nextVelocity, double dtSeconds) { - // dx/dt = Ax + Bu + c - // dx/dt = Ax + B(u + B⁺c) - // xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ) - // xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ) - // xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ + // dx/dt = Ax + Bu + c + // dx/dt = Ax + B(u + B⁺c) + // xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ) + // xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ) + // xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ - // Solve for uₖ. + // Solve for uₖ. - // B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ - // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ) - // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ + // B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ) + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ - // For an elevator with the model dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x), - // A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka). + // For an elevator with the model dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x), + // A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka). - // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka)) - // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka) - // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka)) + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka) + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka); var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds); From 4bee24d64c3add287ac323a36ea9f7fe65516db3 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Sun, 1 Oct 2023 23:45:48 -0400 Subject: [PATCH 08/76] more comment fixing --- .../edu/wpi/first/math/controller/ElevatorFeedforward.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java index fa37a22717c..b92ee95ba78 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java @@ -3,6 +3,7 @@ // the WPILib BSD license file in the root directory of this project. package edu.wpi.first.math.controller; + import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.system.plant.LinearSystemId; @@ -58,7 +59,7 @@ public double calculate(double velocity, double acceleration) { /** * Calculates the feedforward from the gains and setpoints. - * + *

* Note this method is inaccurate when the velocity crosses 0. * * @param currentVelocity The current velocity setpoint. From 09dabe25c534f21d08edcca2b53dda3351294235 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Sun, 1 Oct 2023 23:52:49 -0400 Subject: [PATCH 09/76] commenting fixing again --- .../edu/wpi/first/math/controller/ElevatorFeedforward.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java index b92ee95ba78..924e7a82f0c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java @@ -59,8 +59,8 @@ public double calculate(double velocity, double acceleration) { /** * Calculates the feedforward from the gains and setpoints. - *

- * Note this method is inaccurate when the velocity crosses 0. + * + *

Note this method is inaccurate when the velocity crosses 0. * * @param currentVelocity The current velocity setpoint. * @param nextVelocity The next velocity setpoint. From b4ad348b6a50015730d5a2b66074fe037d7b011e Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Mon, 2 Oct 2023 00:01:45 -0400 Subject: [PATCH 10/76] p tag issue --- .../edu/wpi/first/math/controller/ElevatorFeedforward.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java index 924e7a82f0c..b92ee95ba78 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java @@ -59,8 +59,8 @@ public double calculate(double velocity, double acceleration) { /** * Calculates the feedforward from the gains and setpoints. - * - *

Note this method is inaccurate when the velocity crosses 0. + *

+ * Note this method is inaccurate when the velocity crosses 0. * * @param currentVelocity The current velocity setpoint. * @param nextVelocity The next velocity setpoint. From 9834f99cfd17ff2f02aa5b90d42afad63451e9b3 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Mon, 2 Oct 2023 00:08:46 -0400 Subject: [PATCH 11/76] p tag commenting fix again --- .../edu/wpi/first/math/controller/ElevatorFeedforward.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java index b92ee95ba78..924e7a82f0c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java @@ -59,8 +59,8 @@ public double calculate(double velocity, double acceleration) { /** * Calculates the feedforward from the gains and setpoints. - *

- * Note this method is inaccurate when the velocity crosses 0. + * + *

Note this method is inaccurate when the velocity crosses 0. * * @param currentVelocity The current velocity setpoint. * @param nextVelocity The next velocity setpoint. From 7fedd142ff6a113dabc4e411416e34a9c644efac Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Mon, 2 Oct 2023 00:14:32 -0400 Subject: [PATCH 12/76] commenting fixes --- .../edu/wpi/first/math/controller/ElevatorFeedforward.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java index 924e7a82f0c..1e2ac177a25 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java @@ -59,8 +59,7 @@ public double calculate(double velocity, double acceleration) { /** * Calculates the feedforward from the gains and setpoints. - * - *

Note this method is inaccurate when the velocity crosses 0. + * Note this method is inaccurate when the velocity crosses 0. * * @param currentVelocity The current velocity setpoint. * @param nextVelocity The next velocity setpoint. From a8d04087976f2f326f70247a30db6dcd8540eb68 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Mon, 2 Oct 2023 09:23:14 -0700 Subject: [PATCH 13/76] Clean up comments --- .../math/controller/ElevatorFeedforward.java | 41 +++++++++++-------- 1 file changed, 23 insertions(+), 18 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java index 1e2ac177a25..cfe7c2b8926 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java @@ -59,7 +59,8 @@ public double calculate(double velocity, double acceleration) { /** * Calculates the feedforward from the gains and setpoints. - * Note this method is inaccurate when the velocity crosses 0. + * + *

Note this method is inaccurate when the velocity crosses 0. * * @param currentVelocity The current velocity setpoint. * @param nextVelocity The next velocity setpoint. @@ -67,24 +68,28 @@ public double calculate(double velocity, double acceleration) { * @return The computed feedforward. */ public double calculate(double currentVelocity, double nextVelocity, double dtSeconds) { - // dx/dt = Ax + Bu + c - // dx/dt = Ax + B(u + B⁺c) - // xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ) - // xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ) - // xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ - + // Discretize the affine model. + // + // dx/dt = Ax + Bu + c + // dx/dt = Ax + B(u + B⁺c) + // xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ) + // xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ) + // xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ + // // Solve for uₖ. - - // B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ - // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ) - // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ - - // For an elevator with the model dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x), - // A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka). - - // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka)) - // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka) - // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks + // + // B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ) + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ + // + // For an elevator with the model + // dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x), + // A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka sgn(x)). Substitute in B + // assuming sgn(x) is a constant for the duration of the step. + // + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka sgn(x))) + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka sgn(x)) + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks sgn(x) var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka); var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds); From fadd134738d838a0ed47fc974cd93c8d3574377c Mon Sep 17 00:00:00 2001 From: Nick Armstrong Date: Mon, 2 Oct 2023 12:57:38 -0400 Subject: [PATCH 14/76] comment fixes From ef1a14d4df1e19fb4f24d3ef61dcaa28a778de39 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Mon, 2 Oct 2023 19:39:16 -0700 Subject: [PATCH 15/76] Add C++ version --- .../frc/controller/ElevatorFeedforward.h | 47 +++++++++++++++++++ 1 file changed, 47 insertions(+) diff --git a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h index bbe7720686d..62a7bad8676 100644 --- a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h @@ -6,6 +6,9 @@ #include +#include "frc/EigenCore.h" +#include "frc/controller/LinearPlantInversionFeedforward.h" +#include "frc/system/plant/LinearSystemId.h" #include "units/length.h" #include "units/time.h" #include "units/voltage.h" @@ -54,6 +57,50 @@ class ElevatorFeedforward { return kS * wpi::sgn(velocity) + kG + kV * velocity + kA * acceleration; } + /** + * Calculates the feedforward from the gains and setpoints. + * + * @param currentVelocity The current velocity setpoint, in distance per + * second. + * @param nextVelocity The next velocity setpoint, in distance per second. + * @param dt Time between velocity setpoints in seconds. + * @return The computed feedforward, in volts. + */ + units::volt_t Calculate(units::unit_t currentVelocity, + units::unit_t nextVelocity, + units::second_t dt) const { + // Discretize the affine model. + // + // dx/dt = Ax + Bu + c + // dx/dt = Ax + B(u + B⁺c) + // xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ) + // xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ) + // xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ + // + // Solve for uₖ. + // + // B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ) + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ + // + // For an elevator with the model + // dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x), + // A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka sgn(x)). Substitute in B + // assuming sgn(x) is a constant for the duration of the step. + // + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka sgn(x))) + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka sgn(x)) + // uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks sgn(x) + auto plant = LinearSystemId::IdentifyVelocitySystem(kV, kA); + LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt}; + + Vectord<1> r{currentVelocity.value()}; + Vectord<1> nextR{nextVelocity.value()}; + + return kG + kS * wpi::sgn(currentVelocity.value()) + + units::volt_t{feedforward.Calculate(r, nextR)(0)}; + } + // Rearranging the main equation from the calculate() method yields the // formulas for the methods below: From cd9c3d04cab8f0c5915adf34b9bf7509a9157c7f Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Sat, 7 Oct 2023 21:45:57 -0400 Subject: [PATCH 16/76] Tests for new ElevatorFeedForward calculate method --- .../controller/ElevatorFeedforwardTest.java | 20 +++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java index c701f38d1b1..7fef0e84efa 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java @@ -8,6 +8,12 @@ import org.junit.jupiter.api.Test; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.numbers.N1; + + class ElevatorFeedforwardTest { private static final double ks = 0.5; private static final double kg = 1; @@ -22,6 +28,20 @@ void testCalculate() { assertEquals(4.5, m_elevatorFF.calculate(2), 0.002); assertEquals(6.5, m_elevatorFF.calculate(2, 1), 0.002); assertEquals(-0.5, m_elevatorFF.calculate(-2, 1), 0.002); + var r = VecBuilder.fill(2.0); + var nextR = VecBuilder.fill(3.0); + var A = Matrix.mat(Nat.N1(), Nat.N1()).fill(-kv / ka); + var B = Matrix.mat(Nat.N1(), Nat.N1()).fill(1.0 / ka); + + var plantInversion = new LinearPlantInversionFeedforward(A, B, 0.020); + var elevatorMotor = new ElevatorFeedforward(ks, kg, kv, ka); + + assertEquals( + plantInversion.calculate(r, nextR).get(0, 0) + ks + kg, + elevatorMotor.calculate(2.0, 3.0, 0.020), + 0.002); + + } @Test From b86ca3632c76cabbdd92a3f5e839b9f48c10f158 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Sat, 7 Oct 2023 21:55:43 -0400 Subject: [PATCH 17/76] Elevator Test import cleanup --- .../first/math/controller/ElevatorFeedforwardTest.java | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java index 7fef0e84efa..690f16bfced 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java @@ -6,13 +6,11 @@ import static org.junit.jupiter.api.Assertions.assertEquals; -import org.junit.jupiter.api.Test; - -import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.numbers.N1; - +import org.junit.jupiter.api.Test; class ElevatorFeedforwardTest { private static final double ks = 0.5; @@ -40,8 +38,6 @@ void testCalculate() { plantInversion.calculate(r, nextR).get(0, 0) + ks + kg, elevatorMotor.calculate(2.0, 3.0, 0.020), 0.002); - - } @Test From bc2eab5d2976b263b7f15027ff27cf29f5fab6b4 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Sun, 8 Oct 2023 22:08:48 -0400 Subject: [PATCH 18/76] ElevatorFeedforwardTest c++ V nextV calculate --- .../cpp/controller/ElevatorFeedforwardTest.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp index bf2c1ba3b04..2e6d06c3412 100644 --- a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp @@ -6,6 +6,8 @@ #include +#include "frc/EigenCore.h" +#include "frc/controller/LinearPlantInversionFeedforward.h" #include "frc/controller/ElevatorFeedforward.h" #include "units/acceleration.h" #include "units/length.h" @@ -17,6 +19,13 @@ static constexpr auto Ka = 2_V * 1_s * 1_s / 1_m; static constexpr auto Kg = 1_V; TEST(ElevatorFeedforwardTest, Calculate) { + + Vectord<1> r{2.0}; + Vectord<1> nextR{3.0}; + + Matrixd<1, 1> A{-Kv / Ka}; + Matrixd<1, 1> B{1.0 / Ka}; + frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt}; frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka}; EXPECT_NEAR(elevatorFF.Calculate(0_m / 1_s).value(), Kg.value(), 0.002); EXPECT_NEAR(elevatorFF.Calculate(2_m / 1_s).value(), 4.5, 0.002); @@ -24,6 +33,9 @@ TEST(ElevatorFeedforwardTest, Calculate) { 0.002); EXPECT_NEAR(elevatorFF.Calculate(-2_m / 1_s, 1_m / 1_s / 1_s).value(), -0.5, 0.002); + EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value() + Kg.value(), + elevatorFF.Calculate(2_mps, 3_mps, dt).value(), 0.002); + } TEST(ElevatorFeedforwardTest, AchievableVelocity) { From 227794f18cf3bd7d3d5dcc270f41686f06e58bcf Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Sun, 8 Oct 2023 22:14:44 -0400 Subject: [PATCH 19/76] Cleanup From 73834475a4e18fe6cfab5d86c949400d170a40e3 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Sun, 8 Oct 2023 22:16:00 -0400 Subject: [PATCH 20/76] moved asserts --- .../cpp/controller/ElevatorFeedforwardTest.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp index 2e6d06c3412..828ac265527 100644 --- a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp @@ -20,19 +20,20 @@ static constexpr auto Kg = 1_V; TEST(ElevatorFeedforwardTest, Calculate) { - Vectord<1> r{2.0}; - Vectord<1> nextR{3.0}; - Matrixd<1, 1> A{-Kv / Ka}; - Matrixd<1, 1> B{1.0 / Ka}; - frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt}; - frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka}; EXPECT_NEAR(elevatorFF.Calculate(0_m / 1_s).value(), Kg.value(), 0.002); EXPECT_NEAR(elevatorFF.Calculate(2_m / 1_s).value(), 4.5, 0.002); EXPECT_NEAR(elevatorFF.Calculate(2_m / 1_s, 1_m / 1_s / 1_s).value(), 6.5, 0.002); EXPECT_NEAR(elevatorFF.Calculate(-2_m / 1_s, 1_m / 1_s / 1_s).value(), -0.5, 0.002); + Vectord<1> r{2.0}; + Vectord<1> nextR{3.0}; + + Matrixd<1, 1> A{-Kv / Ka}; + Matrixd<1, 1> B{1.0 / Ka}; + frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt}; + frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka}; EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value() + Kg.value(), elevatorFF.Calculate(2_mps, 3_mps, dt).value(), 0.002); From 8f955cba4be63271d51e888d79c3c74e3f955692 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Sun, 8 Oct 2023 22:22:53 -0400 Subject: [PATCH 21/76] fixed dt --- .../src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp index 828ac265527..87ac14bc66d 100644 --- a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp @@ -35,8 +35,7 @@ TEST(ElevatorFeedforwardTest, Calculate) { frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt}; frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka}; EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value() + Kg.value(), - elevatorFF.Calculate(2_mps, 3_mps, dt).value(), 0.002); - + elevatorFF.Calculate(2_mps, 3_mps, 0.020).value(), 0.002); } TEST(ElevatorFeedforwardTest, AchievableVelocity) { From 90fcc60ebc0a7b76df15de0bb7bf6272ca1c36d2 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Sun, 8 Oct 2023 22:26:25 -0400 Subject: [PATCH 22/76] added newlines --- .../src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp index 87ac14bc66d..d0fc88ff197 100644 --- a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp @@ -29,11 +29,12 @@ TEST(ElevatorFeedforwardTest, Calculate) { 0.002); Vectord<1> r{2.0}; Vectord<1> nextR{3.0}; - Matrixd<1, 1> A{-Kv / Ka}; Matrixd<1, 1> B{1.0 / Ka}; + frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt}; frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka}; + EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value() + Kg.value(), elevatorFF.Calculate(2_mps, 3_mps, 0.020).value(), 0.002); } From 2d812e39cf8ea2678f5449a23e104a26c9720999 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Sun, 8 Oct 2023 22:29:22 -0400 Subject: [PATCH 23/76] added another newline --- .../src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp index d0fc88ff197..8f7e0e149f1 100644 --- a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp @@ -27,6 +27,7 @@ TEST(ElevatorFeedforwardTest, Calculate) { 0.002); EXPECT_NEAR(elevatorFF.Calculate(-2_m / 1_s, 1_m / 1_s / 1_s).value(), -0.5, 0.002); + Vectord<1> r{2.0}; Vectord<1> nextR{3.0}; Matrixd<1, 1> A{-Kv / Ka}; From 4dc0c3e1dc684327646d5cbb92a54d4099a02105 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Sun, 8 Oct 2023 22:31:17 -0400 Subject: [PATCH 24/76] fixed dt --- .../src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp index 8f7e0e149f1..51a42da994f 100644 --- a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp @@ -33,7 +33,7 @@ TEST(ElevatorFeedforwardTest, Calculate) { Matrixd<1, 1> A{-Kv / Ka}; Matrixd<1, 1> B{1.0 / Ka}; - frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt}; + frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, 0.020}; frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka}; EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value() + Kg.value(), From 532194fc7daf5e937087d18c4b5752d429e79d68 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Sun, 8 Oct 2023 22:42:18 -0400 Subject: [PATCH 25/76] units on 0.020 s --- .../test/native/cpp/controller/ElevatorFeedforwardTest.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp index 51a42da994f..6747433d01a 100644 --- a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp @@ -33,11 +33,11 @@ TEST(ElevatorFeedforwardTest, Calculate) { Matrixd<1, 1> A{-Kv / Ka}; Matrixd<1, 1> B{1.0 / Ka}; - frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, 0.020}; + frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, 0.020_s}; frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka}; EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value() + Kg.value(), - elevatorFF.Calculate(2_mps, 3_mps, 0.020).value(), 0.002); + elevatorFF.Calculate(2_mps, 3_mps, 0.020_s).value(), 0.002); } TEST(ElevatorFeedforwardTest, AchievableVelocity) { From 41525dbfc67b93171a2027c8aaee4c7e27021ca7 Mon Sep 17 00:00:00 2001 From: Gold856 <117957790+Gold856@users.noreply.github.com> Date: Sun, 1 Oct 2023 23:08:09 -0400 Subject: [PATCH 26/76] [build] Remove unnecessary CMake config installs (#5714) --- romiVendordep/CMakeLists.txt | 2 -- wpilibNewCommands/CMakeLists.txt | 2 -- xrpVendordep/CMakeLists.txt | 2 -- 3 files changed, 6 deletions(-) diff --git a/romiVendordep/CMakeLists.txt b/romiVendordep/CMakeLists.txt index 6ec0a2b36c8..c1157e937e1 100644 --- a/romiVendordep/CMakeLists.txt +++ b/romiVendordep/CMakeLists.txt @@ -22,8 +22,6 @@ if (WITH_JAVA) else() set (romiVendordep_config_dir share/romiVendordep) endif() - - install(FILES romiVendordep-config.cmake DESTINATION ${romiVendordep_config_dir}) endif() file(GLOB_RECURSE romiVendordep_native_src src/main/native/cpp/*.cpp) diff --git a/wpilibNewCommands/CMakeLists.txt b/wpilibNewCommands/CMakeLists.txt index d3aa541aa36..5803468c715 100644 --- a/wpilibNewCommands/CMakeLists.txt +++ b/wpilibNewCommands/CMakeLists.txt @@ -22,8 +22,6 @@ if (WITH_JAVA) else() set (wpilibNewCommands_config_dir share/wpilibNewCommands) endif() - - install(FILES ${WPILIB_BINARY_DIR}/wpilibNewCommands-config.cmake DESTINATION ${wpilibNewCommands_config_dir}) endif() file(GLOB_RECURSE wpilibNewCommands_native_src src/main/native/cpp/*.cpp) diff --git a/xrpVendordep/CMakeLists.txt b/xrpVendordep/CMakeLists.txt index 9ba681b29b5..2b7556d156d 100644 --- a/xrpVendordep/CMakeLists.txt +++ b/xrpVendordep/CMakeLists.txt @@ -22,8 +22,6 @@ if (WITH_JAVA) else() set (xrpVendordep_config_dir share/xrpVendordep) endif() - - install(FILES xrpVendordep-config.cmake DESTINATION ${xrpVendordep_config_dir}) endif() file(GLOB_RECURSE xrpVendordep_native_src src/main/native/cpp/*.cpp) From 9e05ab857635cd74d07d5eea91c4eb39d6af4340 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Sun, 1 Oct 2023 22:44:12 -0700 Subject: [PATCH 27/76] [ci] Fix -dirty version (#5716) --- .github/workflows/gradle.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml index 38d1c95311b..18403b46972 100644 --- a/.github/workflows/gradle.yml +++ b/.github/workflows/gradle.yml @@ -48,7 +48,7 @@ jobs: with: image: ${{ matrix.container }} options: -v ${{ github.workspace }}:/work -w /work -e ARTIFACTORY_PUBLISH_USERNAME=${{ secrets.ARTIFACTORY_USERNAME }} -e ARTIFACTORY_PUBLISH_PASSWORD=${{ secrets.ARTIFACTORY_PASSWORD }} - run: df . && ./gradlew build --build-cache -PbuildServer -PskipJavaFormat ${{ matrix.build-options }} ${{ env.EXTRA_GRADLE_ARGS }} + run: df . && rm -f semicolon_delimited_script && ./gradlew build --build-cache -PbuildServer -PskipJavaFormat ${{ matrix.build-options }} ${{ env.EXTRA_GRADLE_ARGS }} - name: Check free disk space run: df . - uses: actions/upload-artifact@v3 From eabf2f1a749a4a5c4b85c0a70c12f0b92c7323c0 Mon Sep 17 00:00:00 2001 From: Starlight220 <53231611+Starlight220@users.noreply.github.com> Date: Mon, 2 Oct 2023 18:23:11 +0300 Subject: [PATCH 28/76] [wpiutil] SendableBuilder: Add PublishConst methods (#5158) --- .../smartdashboard/SendableBuilderImpl.cpp | 67 ++++++++++++ .../frc/smartdashboard/SendableBuilderImpl.h | 32 ++++++ .../smartdashboard/SendableBuilderImpl.java | 100 ++++++++++++++++++ .../first/util/sendable/SendableBuilder.java | 89 ++++++++++++++++ .../include/wpi/sendable/SendableBuilder.h | 97 +++++++++++++++++ 5 files changed, 385 insertions(+) diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.cpp index 79b0e6e8969..c0638504119 100644 --- a/wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.cpp +++ b/wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.cpp @@ -141,6 +141,14 @@ void SendableBuilderImpl::AddPropertyImpl(Topic topic, Getter getter, m_properties.emplace_back(std::move(prop)); } +template +void SendableBuilderImpl::PublishConstImpl(Topic topic, Value value) { + auto prop = std::make_unique>(); + prop->pub = topic.Publish(); + prop->pub.Set(value); + m_properties.emplace_back(std::move(prop)); +} + void SendableBuilderImpl::AddBooleanProperty(std::string_view key, std::function getter, std::function setter) { @@ -148,6 +156,11 @@ void SendableBuilderImpl::AddBooleanProperty(std::string_view key, std::move(setter)); } +void SendableBuilderImpl::PublishConstBoolean(std::string_view key, + bool value) { + PublishConstImpl(m_table->GetBooleanTopic(key), value); +} + void SendableBuilderImpl::AddIntegerProperty( std::string_view key, std::function getter, std::function setter) { @@ -155,6 +168,11 @@ void SendableBuilderImpl::AddIntegerProperty( std::move(setter)); } +void SendableBuilderImpl::PublishConstInteger(std::string_view key, + int64_t value) { + PublishConstImpl(m_table->GetIntegerTopic(key), value); +} + void SendableBuilderImpl::AddFloatProperty(std::string_view key, std::function getter, std::function setter) { @@ -162,6 +180,10 @@ void SendableBuilderImpl::AddFloatProperty(std::string_view key, std::move(setter)); } +void SendableBuilderImpl::PublishConstFloat(std::string_view key, float value) { + PublishConstImpl(m_table->GetFloatTopic(key), value); +} + void SendableBuilderImpl::AddDoubleProperty( std::string_view key, std::function getter, std::function setter) { @@ -169,6 +191,11 @@ void SendableBuilderImpl::AddDoubleProperty( std::move(setter)); } +void SendableBuilderImpl::PublishConstDouble(std::string_view key, + double value) { + PublishConstImpl(m_table->GetDoubleTopic(key), value); +} + void SendableBuilderImpl::AddStringProperty( std::string_view key, std::function getter, std::function setter) { @@ -176,6 +203,11 @@ void SendableBuilderImpl::AddStringProperty( std::move(setter)); } +void SendableBuilderImpl::PublishConstString(std::string_view key, + std::string_view value) { + PublishConstImpl(m_table->GetStringTopic(key), value); +} + void SendableBuilderImpl::AddBooleanArrayProperty( std::string_view key, std::function()> getter, std::function)> setter) { @@ -183,6 +215,11 @@ void SendableBuilderImpl::AddBooleanArrayProperty( std::move(setter)); } +void SendableBuilderImpl::PublishConstBooleanArray(std::string_view key, + std::span value) { + PublishConstImpl(m_table->GetBooleanArrayTopic(key), value); +} + void SendableBuilderImpl::AddIntegerArrayProperty( std::string_view key, std::function()> getter, std::function)> setter) { @@ -190,6 +227,11 @@ void SendableBuilderImpl::AddIntegerArrayProperty( std::move(setter)); } +void SendableBuilderImpl::PublishConstIntegerArray( + std::string_view key, std::span value) { + PublishConstImpl(m_table->GetIntegerArrayTopic(key), value); +} + void SendableBuilderImpl::AddFloatArrayProperty( std::string_view key, std::function()> getter, std::function)> setter) { @@ -197,6 +239,11 @@ void SendableBuilderImpl::AddFloatArrayProperty( std::move(setter)); } +void SendableBuilderImpl::PublishConstFloatArray(std::string_view key, + std::span value) { + PublishConstImpl(m_table->GetFloatArrayTopic(key), value); +} + void SendableBuilderImpl::AddDoubleArrayProperty( std::string_view key, std::function()> getter, std::function)> setter) { @@ -204,6 +251,11 @@ void SendableBuilderImpl::AddDoubleArrayProperty( std::move(setter)); } +void SendableBuilderImpl::PublishConstDoubleArray( + std::string_view key, std::span value) { + PublishConstImpl(m_table->GetDoubleArrayTopic(key), value); +} + void SendableBuilderImpl::AddStringArrayProperty( std::string_view key, std::function()> getter, std::function)> setter) { @@ -211,6 +263,11 @@ void SendableBuilderImpl::AddStringArrayProperty( std::move(setter)); } +void SendableBuilderImpl::PublishConstStringArray( + std::string_view key, std::span value) { + PublishConstImpl(m_table->GetStringArrayTopic(key), value); +} + void SendableBuilderImpl::AddRawProperty( std::string_view key, std::string_view typeString, std::function()> getter, @@ -235,6 +292,16 @@ void SendableBuilderImpl::AddRawProperty( m_properties.emplace_back(std::move(prop)); } +void SendableBuilderImpl::PublishConstRaw(std::string_view key, + std::string_view typeString, + std::span value) { + auto topic = m_table->GetRawTopic(key); + auto prop = std::make_unique>(); + prop->pub = topic.Publish(typeString); + prop->pub.Set(value); + m_properties.emplace_back(std::move(prop)); +} + template void SendableBuilderImpl::AddSmallPropertyImpl(Topic topic, Getter getter, diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h b/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h index 4bd42896e2a..baee6ffaa45 100644 --- a/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h +++ b/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h @@ -96,44 +96,73 @@ class SendableBuilderImpl : public nt::NTSendableBuilder { void AddBooleanProperty(std::string_view key, std::function getter, std::function setter) override; + void PublishConstBoolean(std::string_view key, bool value) override; + void AddIntegerProperty(std::string_view key, std::function getter, std::function setter) override; + void PublishConstInteger(std::string_view key, int64_t value) override; + void AddFloatProperty(std::string_view key, std::function getter, std::function setter) override; + void PublishConstFloat(std::string_view key, float value) override; + void AddDoubleProperty(std::string_view key, std::function getter, std::function setter) override; + void PublishConstDouble(std::string_view key, double value) override; + void AddStringProperty(std::string_view key, std::function getter, std::function setter) override; + void PublishConstString(std::string_view key, + std::string_view value) override; + void AddBooleanArrayProperty( std::string_view key, std::function()> getter, std::function)> setter) override; + void PublishConstBooleanArray(std::string_view key, + std::span value) override; + void AddIntegerArrayProperty( std::string_view key, std::function()> getter, std::function)> setter) override; + void PublishConstIntegerArray(std::string_view key, + std::span value) override; + void AddFloatArrayProperty( std::string_view key, std::function()> getter, std::function)> setter) override; + void PublishConstFloatArray(std::string_view key, + std::span value) override; + void AddDoubleArrayProperty( std::string_view key, std::function()> getter, std::function)> setter) override; + void PublishConstDoubleArray(std::string_view key, + std::span value) override; + void AddStringArrayProperty( std::string_view key, std::function()> getter, std::function)> setter) override; + void PublishConstStringArray(std::string_view key, + std::span value) override; + void AddRawProperty( std::string_view key, std::string_view typeString, std::function()> getter, std::function)> setter) override; + void PublishConstRaw(std::string_view key, std::string_view typeString, + std::span value) override; + void AddSmallStringProperty( std::string_view key, std::function& buf)> getter, @@ -198,6 +227,9 @@ class SendableBuilderImpl : public nt::NTSendableBuilder { template void AddPropertyImpl(Topic topic, Getter getter, Setter setter); + template + void PublishConstImpl(Topic topic, Value value); + template void AddSmallPropertyImpl(Topic topic, Getter getter, Setter setter); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java index 7bb266e658c..2c409e8314c 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java @@ -57,6 +57,7 @@ import java.util.function.LongSupplier; import java.util.function.Supplier; +@SuppressWarnings("PMD.CompareObjectsWithEquals") public class SendableBuilderImpl implements NTSendableBuilder { @FunctionalInterface private interface TimedConsumer { @@ -328,6 +329,15 @@ public void addBooleanProperty(String key, BooleanSupplier getter, BooleanConsum m_properties.add(property); } + @Override + public void publishConstBoolean(String key, boolean value) { + Property property = new Property<>(); + BooleanTopic topic = m_table.getBooleanTopic(key); + property.m_pub = topic.publish(); + property.m_pub.set(value); + m_properties.add(property); + } + /** * Add an integer property. * @@ -355,6 +365,15 @@ public void addIntegerProperty(String key, LongSupplier getter, LongConsumer set m_properties.add(property); } + @Override + public void publishConstInteger(String key, long value) { + Property property = new Property<>(); + IntegerTopic topic = m_table.getIntegerTopic(key); + property.m_pub = topic.publish(); + property.m_pub.set(value); + m_properties.add(property); + } + /** * Add a float property. * @@ -382,6 +401,15 @@ public void addFloatProperty(String key, FloatSupplier getter, FloatConsumer set m_properties.add(property); } + @Override + public void publishConstFloat(String key, float value) { + Property property = new Property<>(); + FloatTopic topic = m_table.getFloatTopic(key); + property.m_pub = topic.publish(); + property.m_pub.set(value); + m_properties.add(property); + } + /** * Add a double property. * @@ -409,6 +437,15 @@ public void addDoubleProperty(String key, DoubleSupplier getter, DoubleConsumer m_properties.add(property); } + @Override + public void publishConstDouble(String key, double value) { + Property property = new Property<>(); + DoubleTopic topic = m_table.getDoubleTopic(key); + property.m_pub = topic.publish(); + property.m_pub.set(value); + m_properties.add(property); + } + /** * Add a string property. * @@ -436,6 +473,15 @@ public void addStringProperty(String key, Supplier getter, Consumer property = new Property<>(); + StringTopic topic = m_table.getStringTopic(key); + property.m_pub = topic.publish(); + property.m_pub.set(value); + m_properties.add(property); + } + /** * Add a boolean array property. * @@ -465,6 +511,15 @@ public void addBooleanArrayProperty( m_properties.add(property); } + @Override + public void publishConstBooleanArray(String key, boolean[] value) { + Property property = new Property<>(); + BooleanArrayTopic topic = m_table.getBooleanArrayTopic(key); + property.m_pub = topic.publish(); + property.m_pub.set(value); + m_properties.add(property); + } + /** * Add an integer array property. * @@ -494,6 +549,15 @@ public void addIntegerArrayProperty( m_properties.add(property); } + @Override + public void publishConstIntegerArray(String key, long[] value) { + Property property = new Property<>(); + IntegerArrayTopic topic = m_table.getIntegerArrayTopic(key); + property.m_pub = topic.publish(); + property.m_pub.set(value); + m_properties.add(property); + } + /** * Add a float array property. * @@ -523,6 +587,15 @@ public void addFloatArrayProperty( m_properties.add(property); } + @Override + public void publishConstFloatArray(String key, float[] value) { + Property property = new Property<>(); + FloatArrayTopic topic = m_table.getFloatArrayTopic(key); + property.m_pub = topic.publish(); + property.m_pub.set(value); + m_properties.add(property); + } + /** * Add a double array property. * @@ -552,6 +625,15 @@ public void addDoubleArrayProperty( m_properties.add(property); } + @Override + public void publishConstDoubleArray(String key, double[] value) { + Property property = new Property<>(); + DoubleArrayTopic topic = m_table.getDoubleArrayTopic(key); + property.m_pub = topic.publish(); + property.m_pub.set(value); + m_properties.add(property); + } + /** * Add a string array property. * @@ -581,6 +663,15 @@ public void addStringArrayProperty( m_properties.add(property); } + @Override + public void publishConstStringArray(String key, String[] value) { + Property property = new Property<>(); + StringArrayTopic topic = m_table.getStringArrayTopic(key); + property.m_pub = topic.publish(); + property.m_pub.set(value); + m_properties.add(property); + } + /** * Add a raw property. * @@ -610,4 +701,13 @@ public void addRawProperty( } m_properties.add(property); } + + @Override + public void publishConstRaw(String key, String typestring, byte[] value) { + Property property = new Property<>(); + RawTopic topic = m_table.getRawTopic(key); + property.m_pub = topic.publish(typestring); + property.m_pub.set(value); + m_properties.add(property); + } } diff --git a/wpiutil/src/main/java/edu/wpi/first/util/sendable/SendableBuilder.java b/wpiutil/src/main/java/edu/wpi/first/util/sendable/SendableBuilder.java index a58c3a30c28..db822ce441b 100644 --- a/wpiutil/src/main/java/edu/wpi/first/util/sendable/SendableBuilder.java +++ b/wpiutil/src/main/java/edu/wpi/first/util/sendable/SendableBuilder.java @@ -55,6 +55,14 @@ enum BackendKind { */ void addBooleanProperty(String key, BooleanSupplier getter, BooleanConsumer setter); + /** + * Add a constant boolean property. + * + * @param key property name + * @param value the value + */ + void publishConstBoolean(String key, boolean value); + /** * Add an integer property. * @@ -64,6 +72,14 @@ enum BackendKind { */ void addIntegerProperty(String key, LongSupplier getter, LongConsumer setter); + /** + * Add a constant integer property. + * + * @param key property name + * @param value the value + */ + void publishConstInteger(String key, long value); + /** * Add a float property. * @@ -73,6 +89,14 @@ enum BackendKind { */ void addFloatProperty(String key, FloatSupplier getter, FloatConsumer setter); + /** + * Add a constant float property. + * + * @param key property name + * @param value the value + */ + void publishConstFloat(String key, float value); + /** * Add a double property. * @@ -82,6 +106,14 @@ enum BackendKind { */ void addDoubleProperty(String key, DoubleSupplier getter, DoubleConsumer setter); + /** + * Add a constant double property. + * + * @param key property name + * @param value the value + */ + void publishConstDouble(String key, double value); + /** * Add a string property. * @@ -91,6 +123,14 @@ enum BackendKind { */ void addStringProperty(String key, Supplier getter, Consumer setter); + /** + * Add a constant string property. + * + * @param key property name + * @param value the value + */ + void publishConstString(String key, String value); + /** * Add a boolean array property. * @@ -100,6 +140,14 @@ enum BackendKind { */ void addBooleanArrayProperty(String key, Supplier getter, Consumer setter); + /** + * Add a constant boolean array property. + * + * @param key property name + * @param value the value + */ + void publishConstBooleanArray(String key, boolean[] value); + /** * Add an integer array property. * @@ -109,6 +157,14 @@ enum BackendKind { */ void addIntegerArrayProperty(String key, Supplier getter, Consumer setter); + /** + * Add a constant integer property. + * + * @param key property name + * @param value the value + */ + void publishConstIntegerArray(String key, long[] value); + /** * Add a float array property. * @@ -118,6 +174,14 @@ enum BackendKind { */ void addFloatArrayProperty(String key, Supplier getter, Consumer setter); + /** + * Add a constant float array property. + * + * @param key property name + * @param value the value + */ + void publishConstFloatArray(String key, float[] value); + /** * Add a double array property. * @@ -127,6 +191,14 @@ enum BackendKind { */ void addDoubleArrayProperty(String key, Supplier getter, Consumer setter); + /** + * Add a constant double array property. + * + * @param key property name + * @param value the value + */ + void publishConstDoubleArray(String key, double[] value); + /** * Add a string array property. * @@ -136,6 +208,14 @@ enum BackendKind { */ void addStringArrayProperty(String key, Supplier getter, Consumer setter); + /** + * Add a constant string array property. + * + * @param key property name + * @param value the value + */ + void publishConstStringArray(String key, String[] value); + /** * Add a raw property. * @@ -147,6 +227,15 @@ enum BackendKind { void addRawProperty( String key, String typeString, Supplier getter, Consumer setter); + /** + * Add a constant raw property. + * + * @param key property name + * @param typeString type string + * @param value the value + */ + void publishConstRaw(String key, String typeString, byte[] value); + /** * Gets the kind of backend being used. * diff --git a/wpiutil/src/main/native/include/wpi/sendable/SendableBuilder.h b/wpiutil/src/main/native/include/wpi/sendable/SendableBuilder.h index 2287b731cc0..4115420c79f 100644 --- a/wpiutil/src/main/native/include/wpi/sendable/SendableBuilder.h +++ b/wpiutil/src/main/native/include/wpi/sendable/SendableBuilder.h @@ -59,6 +59,14 @@ class SendableBuilder { std::function getter, std::function setter) = 0; + /** + * Add a constant boolean property. + * + * @param key property name + * @param value the value + */ + virtual void PublishConstBoolean(std::string_view key, bool value) = 0; + /** * Add an integer property. * @@ -70,6 +78,14 @@ class SendableBuilder { std::function getter, std::function setter) = 0; + /** + * Add a constant integer property. + * + * @param key property name + * @param value the value + */ + virtual void PublishConstInteger(std::string_view key, int64_t value) = 0; + /** * Add a float property. * @@ -81,6 +97,14 @@ class SendableBuilder { std::function getter, std::function setter) = 0; + /** + * Add a constant float property. + * + * @param key property name + * @param value the value + */ + virtual void PublishConstFloat(std::string_view key, float value) = 0; + /** * Add a double property. * @@ -92,6 +116,14 @@ class SendableBuilder { std::function getter, std::function setter) = 0; + /** + * Add a constant double property. + * + * @param key property name + * @param value the value + */ + virtual void PublishConstDouble(std::string_view key, double value) = 0; + /** * Add a string property. * @@ -103,6 +135,15 @@ class SendableBuilder { std::string_view key, std::function getter, std::function setter) = 0; + /** + * Add a constant string property. + * + * @param key property name + * @param value the value + */ + virtual void PublishConstString(std::string_view key, + std::string_view value) = 0; + /** * Add a boolean array property. * @@ -114,6 +155,15 @@ class SendableBuilder { std::string_view key, std::function()> getter, std::function)> setter) = 0; + /** + * Add a constant boolean array property. + * + * @param key property name + * @param value the value + */ + virtual void PublishConstBooleanArray(std::string_view key, + std::span value) = 0; + /** * Add an integer array property. * @@ -125,6 +175,15 @@ class SendableBuilder { std::string_view key, std::function()> getter, std::function)> setter) = 0; + /** + * Add a constant integer array property. + * + * @param key property name + * @param value the value + */ + virtual void PublishConstIntegerArray(std::string_view key, + std::span value) = 0; + /** * Add a float array property. * @@ -136,6 +195,15 @@ class SendableBuilder { std::string_view key, std::function()> getter, std::function)> setter) = 0; + /** + * Add a constant float array property. + * + * @param key property name + * @param value the value + */ + virtual void PublishConstFloatArray(std::string_view key, + std::span value) = 0; + /** * Add a double array property. * @@ -147,6 +215,15 @@ class SendableBuilder { std::string_view key, std::function()> getter, std::function)> setter) = 0; + /** + * Add a constant double array property. + * + * @param key property name + * @param value the value + */ + virtual void PublishConstDoubleArray(std::string_view key, + std::span value) = 0; + /** * Add a string array property. * @@ -158,6 +235,15 @@ class SendableBuilder { std::string_view key, std::function()> getter, std::function)> setter) = 0; + /** + * Add a constant string array property. + * + * @param key property name + * @param value the value + */ + virtual void PublishConstStringArray(std::string_view key, + std::span value) = 0; + /** * Add a raw property. * @@ -171,6 +257,17 @@ class SendableBuilder { std::function()> getter, std::function)> setter) = 0; + /** + * Add a constant raw property. + * + * @param key property name + * @param typeString type string + * @param value the value + */ + virtual void PublishConstRaw(std::string_view key, + std::string_view typeString, + std::span value) = 0; + /** * Add a string property (SmallString form). * From 95afbaa3861b848d2d20b61d69fed1104f0a6b04 Mon Sep 17 00:00:00 2001 From: Oliver Date: Tue, 3 Oct 2023 12:50:28 -0600 Subject: [PATCH 29/76] [wpigui] Fix loading a maximized window on second monitor (#5721) --- wpigui/src/main/native/cpp/wpigui.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/wpigui/src/main/native/cpp/wpigui.cpp b/wpigui/src/main/native/cpp/wpigui.cpp index 6fd39ae7e3f..e2e05ce0674 100644 --- a/wpigui/src/main/native/cpp/wpigui.cpp +++ b/wpigui/src/main/native/cpp/wpigui.cpp @@ -227,8 +227,6 @@ bool gui::Initialize(const char* title, int width, int height, } // Set initial window settings - glfwWindowHint(GLFW_MAXIMIZED, gContext->maximized ? GLFW_TRUE : GLFW_FALSE); - if (gContext->width == 0 || gContext->height == 0) { gContext->width = gContext->defaultWidth; gContext->height = gContext->defaultHeight; @@ -303,6 +301,10 @@ bool gui::Initialize(const char* title, int width, int height, glfwShowWindow(gContext->window); } + if (gContext->maximized) { + glfwMaximizeWindow(gContext->window); + } + // Set window callbacks glfwGetWindowSize(gContext->window, &gContext->width, &gContext->height); glfwSetWindowSizeCallback(gContext->window, WindowSizeCallback); From 871b73db20877469338a57c75f60932e2d1b63cb Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Tue, 3 Oct 2023 20:38:48 -0700 Subject: [PATCH 30/76] [wpiutil] Get more precise system time on Windows (#5722) Windows 8 added GetSystemTimePreciseAsFileTime(). --- wpiutil/src/main/native/cpp/timestamp.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpiutil/src/main/native/cpp/timestamp.cpp b/wpiutil/src/main/native/cpp/timestamp.cpp index f5f82bec66a..c7e2fa92d10 100644 --- a/wpiutil/src/main/native/cpp/timestamp.cpp +++ b/wpiutil/src/main/native/cpp/timestamp.cpp @@ -120,7 +120,7 @@ static uint64_t time_since_epoch() noexcept { uint64_t tmpres = 0; // 100-nanosecond intervals since January 1, 1601 (UTC) // which means 0.1 us - GetSystemTimeAsFileTime(&ft); + GetSystemTimePreciseAsFileTime(&ft); tmpres |= ft.dwHighDateTime; tmpres <<= 32; tmpres |= ft.dwLowDateTime; From c5b549593d13c1d14a99aa840f1461547ba8ef7e Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 3 Oct 2023 20:39:09 -0700 Subject: [PATCH 31/76] [wpinet] Revert removal of uv_clock_gettime() (#5723) GetSystemTimePreciseAsFileTime() is supposed to be available, and wpiutil already uses it. --- ...missing-libraries-and-set-_WIN32_WIN.patch | 42 ++++++++++ ...gettime-and-add-pragmas-for-missing-.patch | 79 ------------------- upstream_utils/update_libuv.py | 2 +- .../main/native/thirdparty/libuv/include/uv.h | 1 + .../native/thirdparty/libuv/include/uv/win.h | 2 +- .../native/thirdparty/libuv/src/win/util.cpp | 31 ++++++++ 6 files changed, 76 insertions(+), 81 deletions(-) create mode 100644 upstream_utils/libuv_patches/0010-Add-pragmas-for-missing-libraries-and-set-_WIN32_WIN.patch delete mode 100644 upstream_utils/libuv_patches/0010-Remove-uv_clock_gettime-and-add-pragmas-for-missing-.patch diff --git a/upstream_utils/libuv_patches/0010-Add-pragmas-for-missing-libraries-and-set-_WIN32_WIN.patch b/upstream_utils/libuv_patches/0010-Add-pragmas-for-missing-libraries-and-set-_WIN32_WIN.patch new file mode 100644 index 00000000000..07d02f5b95e --- /dev/null +++ b/upstream_utils/libuv_patches/0010-Add-pragmas-for-missing-libraries-and-set-_WIN32_WIN.patch @@ -0,0 +1,42 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Tyler Veness +Date: Fri, 14 Jul 2023 16:40:18 -0700 +Subject: [PATCH 10/10] Add pragmas for missing libraries and set _WIN32_WINNT + to Windows 10 + +This makes GetSystemTimePreciseAsFileTime() available. + +The #define value is from +https://learn.microsoft.com/en-us/cpp/porting/modifying-winver-and-win32-winnt. +--- + include/uv/win.h | 2 +- + src/win/util.c | 2 ++ + 2 files changed, 3 insertions(+), 1 deletion(-) + +diff --git a/include/uv/win.h b/include/uv/win.h +index 6d0afe69e7dd4caf4c9459e548fe75cf0c51b501..613065df435d813cd517efbc138b13ee46f01f2d 100644 +--- a/include/uv/win.h ++++ b/include/uv/win.h +@@ -20,7 +20,7 @@ + */ + + #ifndef _WIN32_WINNT +-# define _WIN32_WINNT 0x0600 ++# define _WIN32_WINNT 0x0A00 + #endif + + #if !defined(_SSIZE_T_) && !defined(_SSIZE_T_DEFINED) +diff --git a/src/win/util.c b/src/win/util.c +index 9324992ec521cc3496e3e9304e600963a3f20897..4b76417fcbac2480725471740c037deb859e17ca 100644 +--- a/src/win/util.c ++++ b/src/win/util.c +@@ -73,7 +73,9 @@ static char *process_title; + static CRITICAL_SECTION process_title_lock; + + #pragma comment(lib, "Advapi32.lib") ++#pragma comment(lib, "Dbghelp.lib") + #pragma comment(lib, "IPHLPAPI.lib") ++#pragma comment(lib, "Ole32.lib") + #pragma comment(lib, "Psapi.lib") + #pragma comment(lib, "Userenv.lib") + #pragma comment(lib, "kernel32.lib") diff --git a/upstream_utils/libuv_patches/0010-Remove-uv_clock_gettime-and-add-pragmas-for-missing-.patch b/upstream_utils/libuv_patches/0010-Remove-uv_clock_gettime-and-add-pragmas-for-missing-.patch deleted file mode 100644 index 50b610f8e72..00000000000 --- a/upstream_utils/libuv_patches/0010-Remove-uv_clock_gettime-and-add-pragmas-for-missing-.patch +++ /dev/null @@ -1,79 +0,0 @@ -From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 -From: Tyler Veness -Date: Fri, 14 Jul 2023 16:40:18 -0700 -Subject: [PATCH 10/10] Remove uv_clock_gettime() and add pragmas for missing - libraries - -The Win32 function GetSystemTimePreciseAsFileTime() is missing, which -causes compilation errors in uv_clock_gettime(). However, neither WPILib -nor libuv use uv_clock_gettime(), so removing it works around the -problem. ---- - include/uv.h | 1 - - src/win/util.c | 33 ++------------------------------- - 2 files changed, 2 insertions(+), 32 deletions(-) - -diff --git a/include/uv.h b/include/uv.h -index d5342b0d52232bbf83825948cc6bc09e5d74a4c7..9adcf955e143bb323363c9d96da51ccdb618261a 100644 ---- a/include/uv.h -+++ b/include/uv.h -@@ -1755,7 +1755,6 @@ UV_EXTERN uint64_t uv_get_total_memory(void); - UV_EXTERN uint64_t uv_get_constrained_memory(void); - UV_EXTERN uint64_t uv_get_available_memory(void); - --UV_EXTERN int uv_clock_gettime(uv_clock_id clock_id, uv_timespec64_t* ts); - UV_EXTERN uint64_t uv_hrtime(void); - UV_EXTERN void uv_sleep(unsigned int msec); - -diff --git a/src/win/util.c b/src/win/util.c -index 9324992ec521cc3496e3e9304e600963a3f20897..4ea2eada07f8d7e664bee22e74b20c705a0eb9a3 100644 ---- a/src/win/util.c -+++ b/src/win/util.c -@@ -73,7 +73,9 @@ static char *process_title; - static CRITICAL_SECTION process_title_lock; - - #pragma comment(lib, "Advapi32.lib") -+#pragma comment(lib, "Dbghelp.lib") - #pragma comment(lib, "IPHLPAPI.lib") -+#pragma comment(lib, "Ole32.lib") - #pragma comment(lib, "Psapi.lib") - #pragma comment(lib, "Userenv.lib") - #pragma comment(lib, "kernel32.lib") -@@ -513,37 +515,6 @@ int uv_get_process_title(char* buffer, size_t size) { - } - - --/* https://github.com/libuv/libuv/issues/1674 */ --int uv_clock_gettime(uv_clock_id clock_id, uv_timespec64_t* ts) { -- FILETIME ft; -- int64_t t; -- -- if (ts == NULL) -- return UV_EFAULT; -- -- switch (clock_id) { -- case UV_CLOCK_MONOTONIC: -- uv__once_init(); -- t = uv__hrtime(UV__NANOSEC); -- ts->tv_sec = t / 1000000000; -- ts->tv_nsec = t % 1000000000; -- return 0; -- case UV_CLOCK_REALTIME: -- GetSystemTimePreciseAsFileTime(&ft); -- /* In 100-nanosecond increments from 1601-01-01 UTC because why not? */ -- t = (int64_t) ft.dwHighDateTime << 32 | ft.dwLowDateTime; -- /* Convert to UNIX epoch, 1970-01-01. Still in 100 ns increments. */ -- t -= 116444736000000000ll; -- /* Now convert to seconds and nanoseconds. */ -- ts->tv_sec = t / 10000000; -- ts->tv_nsec = t % 10000000 * 100; -- return 0; -- } -- -- return UV_EINVAL; --} -- -- - uint64_t uv_hrtime(void) { - uv__once_init(); - return uv__hrtime(UV__NANOSEC); diff --git a/upstream_utils/update_libuv.py b/upstream_utils/update_libuv.py index ddbb539ecc1..179632f0aed 100755 --- a/upstream_utils/update_libuv.py +++ b/upstream_utils/update_libuv.py @@ -29,7 +29,7 @@ def main(): "0007-Fix-Win32-warning-suppression-pragma.patch", "0008-Use-C-atomics.patch", "0009-Remove-static-from-array-indices.patch", - "0010-Remove-uv_clock_gettime-and-add-pragmas-for-missing-.patch", + "0010-Add-pragmas-for-missing-libraries-and-set-_WIN32_WIN.patch", ]: git_am(os.path.join(wpilib_root, "upstream_utils/libuv_patches", f)) diff --git a/wpinet/src/main/native/thirdparty/libuv/include/uv.h b/wpinet/src/main/native/thirdparty/libuv/include/uv.h index 9adcf955e14..d5342b0d522 100644 --- a/wpinet/src/main/native/thirdparty/libuv/include/uv.h +++ b/wpinet/src/main/native/thirdparty/libuv/include/uv.h @@ -1755,6 +1755,7 @@ UV_EXTERN uint64_t uv_get_total_memory(void); UV_EXTERN uint64_t uv_get_constrained_memory(void); UV_EXTERN uint64_t uv_get_available_memory(void); +UV_EXTERN int uv_clock_gettime(uv_clock_id clock_id, uv_timespec64_t* ts); UV_EXTERN uint64_t uv_hrtime(void); UV_EXTERN void uv_sleep(unsigned int msec); diff --git a/wpinet/src/main/native/thirdparty/libuv/include/uv/win.h b/wpinet/src/main/native/thirdparty/libuv/include/uv/win.h index 6d0afe69e7d..613065df435 100644 --- a/wpinet/src/main/native/thirdparty/libuv/include/uv/win.h +++ b/wpinet/src/main/native/thirdparty/libuv/include/uv/win.h @@ -20,7 +20,7 @@ */ #ifndef _WIN32_WINNT -# define _WIN32_WINNT 0x0600 +# define _WIN32_WINNT 0x0A00 #endif #if !defined(_SSIZE_T_) && !defined(_SSIZE_T_DEFINED) diff --git a/wpinet/src/main/native/thirdparty/libuv/src/win/util.cpp b/wpinet/src/main/native/thirdparty/libuv/src/win/util.cpp index 4ea2eada07f..4b76417fcba 100644 --- a/wpinet/src/main/native/thirdparty/libuv/src/win/util.cpp +++ b/wpinet/src/main/native/thirdparty/libuv/src/win/util.cpp @@ -515,6 +515,37 @@ int uv_get_process_title(char* buffer, size_t size) { } +/* https://github.com/libuv/libuv/issues/1674 */ +int uv_clock_gettime(uv_clock_id clock_id, uv_timespec64_t* ts) { + FILETIME ft; + int64_t t; + + if (ts == NULL) + return UV_EFAULT; + + switch (clock_id) { + case UV_CLOCK_MONOTONIC: + uv__once_init(); + t = uv__hrtime(UV__NANOSEC); + ts->tv_sec = t / 1000000000; + ts->tv_nsec = t % 1000000000; + return 0; + case UV_CLOCK_REALTIME: + GetSystemTimePreciseAsFileTime(&ft); + /* In 100-nanosecond increments from 1601-01-01 UTC because why not? */ + t = (int64_t) ft.dwHighDateTime << 32 | ft.dwLowDateTime; + /* Convert to UNIX epoch, 1970-01-01. Still in 100 ns increments. */ + t -= 116444736000000000ll; + /* Now convert to seconds and nanoseconds. */ + ts->tv_sec = t / 10000000; + ts->tv_nsec = t % 10000000 * 100; + return 0; + } + + return UV_EINVAL; +} + + uint64_t uv_hrtime(void) { uv__once_init(); return uv__hrtime(UV__NANOSEC); From 58a619383cbc31c4dc5f0e730d67ce80b6b43a4d Mon Sep 17 00:00:00 2001 From: Thad House Date: Wed, 4 Oct 2023 19:31:25 -0700 Subject: [PATCH 32/76] [build] Update to gradle 8.4, enable win arm builds (#5727) --- .github/workflows/gradle.yml | 26 +++++++++++--------- build.gradle | 8 +++--- cameraserver/multiCameraServer/build.gradle | 4 ++- gradle/wrapper/gradle-wrapper.jar | Bin 62076 -> 63721 bytes gradle/wrapper/gradle-wrapper.properties | 3 ++- gradlew | 22 ++++++++++------- myRobot/build.gradle | 4 ++- wpilibjIntegrationTests/build.gradle | 4 ++- 8 files changed, 43 insertions(+), 28 deletions(-) diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml index 18403b46972..817cf36d206 100644 --- a/.github/workflows/gradle.yml +++ b/.github/workflows/gradle.yml @@ -77,18 +77,20 @@ jobs: task: "copyAllOutputs" outputs: "build/allOutputs" build-dir: "c:\\work" - # - os: windows-2022 - # artifact-name: WinArm64Debug - # architecture: x64 - # task: "build" - # build-options: "-PciDebugOnly -Pbuildwinarm64 -Ponlywindowsarm64 --max-workers 1" - # outputs: "build/allOutputs" - # - os: windows-2022 - # artifact-name: WinArm64Release - # architecture: x64 - # build-options: "-PciReleaseOnly -Pbuildwinarm64 -Ponlywindowsarm64 --max-workers 1" - # task: "copyAllOutputs" - # outputs: "build/allOutputs" + - os: windows-2022 + artifact-name: WinArm64Debug + architecture: x64 + task: "build" + build-options: "-PciDebugOnly -Pbuildwinarm64 -Ponlywindowsarm64 --max-workers 1" + outputs: "build/allOutputs" + build-dir: "c:\\work" + - os: windows-2022 + artifact-name: WinArm64Release + architecture: x64 + build-options: "-PciReleaseOnly -Pbuildwinarm64 -Ponlywindowsarm64 --max-workers 1" + task: "copyAllOutputs" + outputs: "build/allOutputs" + build-dir: "c:\\work" - os: macOS-12 artifact-name: macOS architecture: x64 diff --git a/build.gradle b/build.gradle index 7c5204f4118..f4b2ec50e06 100644 --- a/build.gradle +++ b/build.gradle @@ -109,8 +109,10 @@ subprojects { } plugins.withType(JavaPlugin) { - sourceCompatibility = 11 - targetCompatibility = 11 + java { + sourceCompatibility = 11 + targetCompatibility = 11 + } } apply from: "${rootDir}/shared/java/javastyle.gradle" @@ -168,5 +170,5 @@ ext.getCurrentArch = { } wrapper { - gradleVersion = '8.1' + gradleVersion = '8.4' } diff --git a/cameraserver/multiCameraServer/build.gradle b/cameraserver/multiCameraServer/build.gradle index a7cfa423f3e..d13637c7110 100644 --- a/cameraserver/multiCameraServer/build.gradle +++ b/cameraserver/multiCameraServer/build.gradle @@ -18,7 +18,9 @@ ext { apply from: "${rootDir}/shared/opencv.gradle" -mainClassName = 'edu.wpi.Main' +application { + mainClass = 'edu.wpi.Main' +} apply plugin: 'com.github.johnrengelman.shadow' diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar index c1962a79e29d3e0ab67b14947c167a862655af9b..7f93135c49b765f8051ef9d0a6055ff8e46073d8 100644 GIT binary patch delta 37652 zcmZ6SQ*jNdnQBE-m!q1z)J^6!8liD~E|8k;d@!RKqW+P+c{{A_w4h-Fct^jI*3f}}> z2Q39vaxe&dYajQhot|R|okxP_$~ju*X0I0#4uyvp5Y5h!UbielGCB{+S&Y%+upGDb zq|BVDT9Ed2QC(eCsVrrfln`c3G!v|}sr1Y02i z%&LlPps4#Ty_mb$1n|@5Qfpv_+YV$Jdc936HIb{37?{S?l#NH+(Uw<@p6J%2p)un; z8fSGPL>@VtAl4yv;YO5e z$ce51CS;`NGd!WVoXeA9vfJC?1>OLi=8DCWBC=^_)V|)E5|B~`jRg01sgJZg#H@DN z(%3v>_-$+>k5p8l?YQWO0Xnm+Qg}U9W+}Al#c_RurG{H6IF}%vlMobp!nmIFL5{I# zoF z4ytIT@lBphb!xg@+~Hd9$f>Hh zUWt4fdi9Gtx|Z%Qfqw2|q5|Nnxh|mer1*VKpI}@@YPdN?TtU6jE;@uhxp8=l?#DTW z3?}F=_muS@5OK7^63G_i&I}DlJCSXGU*&Kq^(hgNE-=%%`BAo0 zBU#vb^C+2dcfe0`MDBTc%;9sY8a+%WNboJPY~n<&z)unXq5*0aZ&|aYVl1Am$Xp_c zU6TBDJ)I1Czr9Fusl92Pkm{EaI=QRi&nIo%&vvPM$PW7gOATu2+6A9&#{E|R8_vZD zo=}nNASfxDaaoMiy1+Z0+XD9hN4VaK<7I$rOt z5^|1qXwt%WJ5}+eQ#RFYSZ*(`YcT-098L^_8q29iO=XfmXO;Z9NHp+;FxUbI$Fg; zi510A`7H3>G6C##jBjc~Ixv7Rty}TthLu-u<1akLY7djP%xObB2KP!vAp?%YSbD^% zu=YcbKXUUhzgC;^%P&GvnnDJ&9=Xg%dauiSajot%RIn@(gf);fn@&Ru4)KS47(OdJ z$h)5lhgOh?n~P1R&)RcABS_Qia>NzjcvP`~C&VU6N2E8OL&X&1=1U2b&N`9o??Yn> zF<;;DseXn1&2-S!d-L&Z@p7C>>z>}0fA`19kNzf@X6+?iRv;E4ptwF7UwR@K58#?IR?)HVT8 zl~Dm+bfAIu3_Uc6J6a+zC+(~hEa^(RtRb#jVZn#5;_Fi`yR0K0?3LpaJTu+@7UsX& z#qUh`Nb;vJ0R=JB!leZl^YGMQ=p^l!6|^I_CMO(I)y+$u>K3zK#wVX08}j>x3CZwp zlk*ylL1!pfyq)Mh{n_|@TFPDddYx131Jmjk#j{Kh5*L*ig|AGXsfKOg#A9=C+CntSIZTb-d{G)j<>I+x8(cr40Xc1%<2LuzauvEDVt6i97SpA6 zsxGPO)MV;#UbwBSPiP{2*4l8o(o6o*tddwUFwx3;(g3LspjtuwUQvC*_4iMDCj+7uNe z>HNYl12vbCMsk!BRX&lF@neUQF46p|G{+&{RA1VANjF~C@9I6Br_$YAdX+rqwy7+| zPf=TFt(2f#W6Zb>-7(K%c~P$-E5B%z+?{oOh@b%O6VJEKH^@I;y!78V5vYfx#vL|J zte^#>+1NkFzOBEu6N-m!uO({kkWTY=oOtt5gF-!78Cb;LJH|+GW=czxXTyUDFBdbg zw&;1{SfPq|#+>6wJ;@YCj^E*1Z{Wtt;APe=!aZ&)_P~Wq$346{9sl6}#we1s$o+9H zH2@_Ct7gbH9Oqtdr=IDyUGFHc@}NPiXO$7%44}{^?+MTHPpFs}U1ktHWzj}Bmh7}} z0r`~t6xa4x#>EyC{l!C;zpw){$b=O||F?$c0b<;(<3p_FLE)z)5kvMz%M$s$!kQ_@ zn7YaOX%*Syd%2nV(t`wfW^U1#TSeTnz~P(CuN9rh$N(BdqHmQpSlbru>&Qzp$!Wk% z@i17nZv$pOU|V^^=Zs*wcArd+Ig@jr0zuo%Wd)iEO1x#u)m37$r7*KFW9)89oswQ# zSYKZ^R5ka^d-_*@na|Ow8zNyJ708zX4N6j&jykXV7%hZ|j*C~=m!BN;4KHywBL@+J zFMVY_D2@vrI@t{z&|1*KsUw>d1SRZ?V>}z7O@%r#Y@yFi4d#!`PKfi>SE6(y7$7?o zh^&V1d)~1F!w62_{X|LVW2E~`cd+u_koSGZOL**qSQj;OFHOrag&04h*(pJdFN6hx zh<`idoM?HedX~KoGce-)-;g^Xb;;7#SY~TY0~yH&G~!Kdm$7U4=b5|mk@Ktm{rke$ zRd_nDsKt3|h;WU(v78jFvhvoGaG=F!ZU7;=mve%3PVm+Zsz!^ELnE&b8=*|m;?b*BQe}|1AK&i+{?MLRhV+uBX*Du$tfT}EnHNpBthR}_xDzZ#PB_ElYd?REZ#@GIbt4a63@b<^e z0Roi}Zr-Q-sD~v`HAvj{K=fpGi}!iUTfwsL^W_7opUM5+Nom4Vf|-l>{5T=VEoa9` z$wdiRKM}u~6cGK4Hyv}17PNx+9%x+42m!jaas7pL9uM@LO#WpY_b#a??K_*O@u4As zNH0$up@AAflGq@Ck)t(XG>@nlrgzJuhUh>K8*K9?5DAIZZ53v-hlF|kK6vrENdAWw z<*oCApq8wFPL+lLQGuCv0r!I762os)Fb@WTS)7ZCeFb|Zct|UBAa<1<9M|wVu@TfO zAY@^rrg}Qu{e0z*!oHB!*>jZ}Zm^X;t)`1iOubj30>uC2dHBgCdTcn4*hIt&>mjgs z@chLwLzCM3Jk`)6J@77;ave;*g27yps*!8eRuZLmf z+~W>kS#<_W3dbNz0z1PI5<%@gMRiLvo9RlIcyf{gTTjZp>n zCA6CO0>+*AiqzO8qo3-eITXeI1N^_bvwWZ^K!gDU^FT|w=A=#{^cmmW%f^#;Yr)G(EHZ=8TYj> zSU%DrTk1YIp0WUqaalA-#p+mWV?;DN3=)M8r7Oej=b#Z}Xs{p~wrO27JcTDGW`H(0 z!qD_Xd^F$s$C;GWMER%{I%p#(W`>Mg=YV%ztG2Bf&VQByR5*<=W;(~&w450Sw- z&v)+bPcx|8L2x+5rc-uwKl**(w@A)E_^BHgze1&B1!a?Kcro8Vf7s-=ujFiEi}=4W zvQ80O;nlZ@sW?VZ$D}IQT1l~EunsL>ui8nrr5#Py;lRFQLppSXmNScPVcjw`_=j7P zC6G&zna5UjbOxVD{Q?%G!F`(<@txVX)Rb&Ci&WIc+boK)Vx(P@Y8^%#E9tp2FzsL7 zN|ujIll!%^2cqT#x#Uyw0QsvnjnYFmnVc&9Ld&rvD|uMh`9B(k0+h;9@|U*z83Zc| z^gDgyTIr>eE7P&o5`8o6Z-74$JA$Bv)q6&oCFFOj1RmC~f%)|`q|~|=VS@4ai}IRA zrk`paX)_$nXpBX5HkEt<+QYcJn>9!r{#OpG*?**E zF4DG7h+-+ilK6_$ewPrM*B&FEKdt7gB^xtmpUu&pu~YsM){ycr7!-yBp}ssn|2T*4%vhs9ZX;FE0WM5iEo7Jrgyj(au+Q_^8*7aN%nC2v9BpOz6E;@Ae z6`jsk$$MUJAA<`gSa8*9$LWW)G=q*z?}1lGb2_RIg8vFk4Kb@u0;H9#xQjVQLVD3rgP%9YxIfY>cZQp1Um8nZhx30;BqgqHI=dBJ- zdDdvni6NaU&Ju2^7K*hiXC33bnfox+8vbL>w;of20_c&+q)y&FWUtoFa-yRj_~F%* z=t;#(7UlA4%Fm}#R5c575CsnOc(YVYm$s!TAdo@;(UJrBnhU)PuuD)E^o@HJN32XF zYRqj+d$AM1tACioZZ8YvrXci@ELZr9ACNU$1_KXS?$MRCcwM*ZcE)&wi_#NLH;2%V268UW?OVFSIJ;C5d zKnqu91}(Z4e^!Ki`q{xJp?Jd2guS*fpuaD+t{iW;&|>9^MF4nuNuEk zeolrCT^Ek-YNOs`eZ&)69=31j{z1%<32I;=$`ub8Vi%T_1cDAB{f3dJi$)l~eK&Si z6kXy;&3=8NH(oC@C8nADzKW@aD|L^|q~s^QYooSr7bhXw! zuUyO%6(tOngxFePj>!*q@_o!6ypM;f-s^+xlK1=+ujdy244_Jo>v1f6(Pe6ez09HD z5S+aeYZ&4cxB^+feStV~!Wj9^s=zT|6sU-^I-Plyy5(MeJAz~QV0bHxP85Oi1^%Tx>axi;rp2a} z>Uy%3d(Zo0^Xv8fg4LQYpu`q5$rNQs;=XF?#5J!C7T|wJ4`yx zCf;EWH`O&&AAbQ8Z)h1_!=pZFDTPzM{C98nxWH6h4zf^Z@qOQRnH!=_=GxW=Z?srv7J=%JCXF*? zw;&5KD3-^6{WS3O+hyH5tzQ_ev{ zuOquYA(x%naj=Y8C+^9@Pn`mxO-Ws8gKa<|CKwHljJXoe146CN&DfGd+S&KK&6K1k zv?FDRELtxCRu~W?6;#dFMD2<~Oc=PWPC=v!(tOfriOePfkh^dga&#=mxYxmc4pXcf zfmFJ@7EZikj4xi{g@lHmj(N3P8#ol}n%^xUL&2GlG6z#o@BA5xgomE`-T4y}?6Cw| zx$OoWyAx{_EmPiM zEi%=fEgF+Zd2S7=j&s_l#rQZ6u%Fqo@*|xxH2irHz`i6nPt^V-Ou8_YYVQfeCAJ9K zAGqsa3u-)Hrr8K~wQJ7AQWZE%f%b%sR7l~T)YDpg%88Uq1Cc(OZ8i~ln};D7)*Ly< z9lUkgXPLAN=&w<1i5R73?8rUTPEdh#StrnUghGvJbbUq)?|p(cAAKe;QuPfd1ubD+ zl+)mVP!*K1J^Sl0khkO$JJ;ek*|!TE@7Ai@Uej%#@Ya-Nl$F0TDPz>u&S)#j$peaG zm(rIO;#Bz@Kqguv-Lbk_N)6?va8rmb0U6cZH*yUYaBK7}bbjf^^=Z15+ZO2p#3z0| zo%K((lY-D_&bNsp$;_h2W=6i{$k14a1 zu8Pj(iv4aKPJM26ZuvHk2i#{Bg+HsHj=r&)8LzZopotENKxdgup)@{UDN)?ydnAe^ zz`+DYsE8;BSSY(0793hBr*-soAl@H(kB9spa9UUr>`_qP?&q162GTWMKkmdc%~F?0OQvPBw%M3DjAH$mP_0 zn;RX&9lJ$sP|i!6&4StDdL>Oz8svAEg<5wtY-|z(uu#pLh&n?=w*%|EQ=aHVisIDh z3}DGGi|h6YYoJTe%1*Q?#aJOUF<<|(vPg&H)+|u~iu9vS9sg50!Jh21FtQ-Pz@-0q zwA}x1tYtZcPJ%x{1*NEO1C}H(zgAPp#c4)(B19LzlLYI?m}EoBSY?;O{hq6FwvrbW z)lHA7VJ(b2N-!(!IVHIH<{P-D%)mF9p z_v?`xOtzi+5CRLMJ^!E`ceH`wurLx)LoK<1?vNbHmJZX00c5H_f(EWqPZ}y~qOI(t zJxI~%HIt;jAwNf8r?TMW6-K7}r$h>HgwU2AF zYg%ruK{p0=fR@mW9RPFOJsCkllZXIzJ>`7cH&SG>sXL=!Wy(AU9z(NqV!IpoUa^)d zok2QH@BZ(1i8DFw6=)u*OH7j9ka*UR-LIEOI}w|z^Und?K;rb7{H;3HO15)S52HBj zse@>hT}GDaZn#Y2cHx1h(NJLFi+^t46z{2GOpo4}Cpx=4V76uK&CfJ`ly;RIQ_b zhK1n^bnX3=S1ZWRULjo^?^Ech$&!N^3VmQy?d(I{oRCK*{r}(mJ zPik|X+)CrZob_ZsN;}R=Tg{%3_|m&$wR0G;(5CCJZ$DAK_aF@U0mtHaS!*?8ifx64 z`H7aSSuvA*o+?b<;tSB*|K8ZkDZ1)Q-K3)yfg+*2`r?9&6MHexRSxdv&xv$Wq}UQO zHUx`7rPA=%i#!y`fADsSIb%$ngkI)zrE5Xzxm|Z zh|~QJ^;QB6S5Wgb_P{Xe#Xa0;ph&uC<9qQuVHBJAszfF%v9hT=2(u?G!i!Ht&=ieG zgDS!r#*!8Js!5pvrgN;5Uq1srr4>gEUjlkyZTY?*6RlBLSl;+)oseT%r4G{ch9L*} zU>TXDTA=^70wFFUESu9j=$7?02#dN0b+UbLbIq_@q>!{Y$u;rG{SrL-{(bRR0!<9V za2E#uYrGkqP@39Z#}Rpd6+WA5Izn^aD2GY7;b4bS?ig+2Qu1HO%iLlTaqu}hvjLiU zOy8q3(};?+|Gws4jkLa`FMd}DOkbQPH-SKKDA@ej_R6FW!JnW@1q@|WLEwACWn;1m zq?j^VRI}`q%CI78G$)k=BnD>CU#81a1_xl)_Q+|`3*=Xb7|H)Y7Z*ny$X}3FiyiDP zmb2Lz9hZ51KR^)aBTXD$##R)i9A--B7Q7+WNZiJi=?nRV6k_7x8<%3SfY652A z&V2*%x;wu?c^zj?ZN{}By_a0S@e&Q_n+4O7p*CBF#6u@UEcMFD+GkPgyxgJ+95>u+ zQgVKm9`_w)#ZuCFa$Z%t>|(ngMThCS_vhD52HNAY8FthjYZ4JdVsB?oN8q>O{kVV!IjZE)hnTcUc&~{Vyg!7tQ4nFp z;i?p@^=jOv?>~mT3FR4z&q}QJR+F+Uelw~!jt6@rsFY+vf_S|&ZB}hXL4fh(<+e+kGjS07#P=N zWJZg$-!MkOAGQy#eo1{&$D`X9SD${kCwI%Z9e&$Lry~;C;7_U@cP%0U2%useF8ovz z-%5Z$(;>zPH&<`m*Y=2 zmAK5EHz>RQ8Lt7_c*ZB`pTm3 zO?<8$R^ztmO9dtdOemZT_AH)su9yuW{WF|`s z`E$HVAoe3gCz`9|&hF1C(V*Dj%oUV7=2tit&}H5CNmSW9VZNn%g+e-7&J}w{2LJj3 zdxYxxSqPFkHOq>mQ9guwv-2-w8HY(Y7ERx`K6+)5@qwK3VIXTp=e|Tu+>zgklyW%a z^2{D*G$jO9SSjtn|A+9D6`a` zY_t#Jzv}gvVn%@cr{4B|kt>6IWBtj^V|&YoAD)LXR0b~)AIhWmt#*yVfgILzl6m*pC)sVEpC>2G zU@%r2Qbji8K{nWm_RIC=#$zHm@t$YW%wFPBD+FVZO&Ey!gEnhPSNkLF*OhUF*C3bD zWhCgqAJ~&iw-nYAWd>5?zNmDr>dfe9)c4mVuIghr#;12v8r(|cmc_&Kz?^_<-W($V zY(P0bg*XU_>HRy$z!emZ&0g>QLq*+;k&aiU0D~Ev#;4o*x+5ne$NjqK!l00`W5$L@ zGia0dJg*}t+^PQK7u?FokiKmyA=DfT_QIYTs3%1n(INy?gZN-RFi#J*55ks2)-}o6 z`2;^C;D@&Jvv5tE9B;@|1hdlwPfE$h#YkDFqOh-J<8W(AenY;$K+1efw_psQ;AjBC z0EOkWMnBU%hzPQ&1=>~CqD^}p={B=fB;d@2RfRG!dyQ=6Ml)%d6wjm$&!i7obBE1S zaQh-Q?YQF)xHq*}?Q7RZ@daB^IJ@IN5&o-}Ypvn#BtD5?xE=yS1a60|Q<$bPiHdJX zs84+OG3a1mbaY@~RR2du&`J5yupnzA-IbKDSjMx7Ip!=3YBV!6?eI$vxPbIw?HnkU zVTFFu0d3gGPdj=I3i1hx(E8w?8?>?o@>*HgDm2Xu1JX`#Ean+1@aFldgU#mY8Emps za>k3`BB`%ezKIMQ@LZn-!0WE(Y?nE~Dd3#1*Wvm-447Qnr>E6W+4*gT7wDrd!i$jY zMiaw% zG?#L)sKISRO49P7*$AtIAZU~h{4jaz_IzK{%cfWL?zT}*35C_HFhVB7Y}^ck{a8)3 z6j#N}q!lx(JP}=-VY@(J)p6_9#HLxP>SnyGXUE14?PQ*zo&C*H^3=tR?`dT8m7MCz*5lBy6p zq>TO{HFsBK8q}x_)`4;J%UdG~z3*|*LyS>mS-&6_ehQ#-77MfZDU(>N1)I9_U`N9+ zH+f^gh4O8k`BXs_ftV57Lddg*W{>WEa#%=S90s)8kK@;R?7;nAg%35yGoYraMjAEI z`;}1>+j>fSRnp1pAepm}PKtvdahlK+xS-YDYYOrB3lo-GxnHD<7rn(hhM-Z%-2Z$g zpggDHiZbvcIsgnut}WH*rSX{FCUvEzuBukQ(a-ZS5=)k;9E9VT++U49x4BZ{Tm zHL|19Ab?t?vA>~a<}B~~I9MXPO3jmISbtQF?^V*j4+k~Kh!yLKj-oScKLWA;GWoN7 z=xGvqAU?clBP2(fD73gngTRVf*TA=)k}w=7W?ev;(d6>R)Wm^qUttviohjljZc3w- zP(QP1wC>Ku5Ar59M@9%1NtkIFV02d<+>&$Y^lB%byWzGBRa9BPT5*gDYUmG*m#6ml z4LLOMA|ULbd@B=Rt6V&x@#a#}87oil=M-MN+z!neF<1k-Q1~$y*L6fUC|O|NcG)dk z+^eYd8FqDY-UqB%g@Xf7Sv^uEX# zdD(a}u^AN$OnvT4nihKguQ1Wx*L-(B|6z2jXt+CD)E5 zlfr~j14MK+5hE?`3uzvuri!35s%A@U)oy{oUflp(^z$vHK%k=C&bGv-C8t~JImU%0HUKZse(qO>{99Bvsl zib(}khqWh+7ZGQbGABDko8dOM@<)OQY{P^PA-faqW^(h4dcP5gfL2U6D>u5tXVDw! z4Mbs4R*60r8vEPgID5etTc_M|88B0cJuXn~4LM7zoSKp6D`^Ap&w3lB&6$*ApI^5c zGfA?L%c4rxTmAu$dCxJs!B!LIQhFfZOOowN7hW8$EfWkx-pCHxtd4UPBhZ$h6(in| zROv`G-FMhB-{;zL*jHHTf_X+S@Ji*O2BF#>vxP!3ZqV3cUyU&Z^!-@BBoDGSm6qai zhJve-6jR!`c1~(RRohZKRgo=3Z=zr#O4XyvilFJqv7EprbvjB;(FSzrkHtbybpR=P_7j|qGl{n5`~^i;e$_m}tZm)Hi5Ev+;t!0nAcuGY zxHvBZ`6_K67+`~ubaYA$J+tvv8MtO6sxEqrL}BVyaWe4=H)CJ{RSN5%?>0l57NBa& zV&ZZVbvN}gb&C|J14!Gln%Hh%OS~QzOx>yydwkN((`r5Hx)WSg(l$~V8J%PQ=p?h* ze5l%M2G{s0$crU z#!eygiTwrF*K|bMArB@?oO+F*nkO0lWAV@KPusDnKx5Fs1LJdEP0H=X zBJJ-uH@onSH20f&74iUiE_NL zQnlb>Bx9k4EXiWVg_N>0SW+AP)=lZ{=j{!hO#MtEEAPS6ZW;7 zSf;k9&Ilhol+gZTemQv^)H)jQ9^rYe z#tYKj@&l`HdyGwthiYX2ztuvHy`V;9YB zDwd^XE48}(sIlFwD@RtoO0iYxX?(npiDcZMf45rpD@q;t4D^ctz4a{3oofz9)c)I= ztNxP)8hCK@JH~_E%G(JtE_XH>JFn6?5QGp-T5MsbzrE znukDnlPT``K~uzJew$MRJxj6_&&SiGBu^%bBGu@A4{0*HbrfAmqkM$*%(x@iX-9o> zT6lo5;@gX%mUB)FVx@bJ$!52Qpox0xgM9*Z2+G%K%xfZ~st+X3NLtu2pCPyj+9C~~ z|6z3goCto*p|3WSz{IkoPYiQ_cXd$WzP1wZgkxZsRPn3T$b)CP+$&g)A~}OYUw&Yn z-|h7cD)Tk1x--q?+dxOt)ly4pF(WPxpR?4Ys)eVVcHG^DdNez~&QgFQbP zT{fIjOL%rOszhK21=6f{PT2 zyd5R4m~vOvSb=FB?7WrRKaI%|%8wlE0Gp&=Punl6yX#@uJ{VA&2xr zYo`-aamROVpiD^_p72LBu9@(!;v!M~XlB;lhG{4MNZBblPloOD*vaSE%x-s7zs4um z)Ff3aKS_{CCI5*cI&RfyI#9ly+*wlrdA%3BFn+qcc3C%Z#_*S853{*|*dKltn zC7y9@#b#L~m4Q|2fw@IJ`EId0^7Q_(9jC7biWYI%4J3HQJUo{$5apf@O%xp8i1QgR z(DG(2ZzTvKkdZNG4qcYtjw|TaZ1<`C#HCs%b*wZ9*rPEkwt=00>Fz<03# zU_#wZ)q+fj^xJfa_v-5qs4x4aiyu0qeE>M4YMws1Owp7B8tBnWkjFyL^BwxQhG)(o z8U*Qm&F0X#o7)+;h~I)Ca+XQfffjt?OPyPADv^&Jg0!8tb4CXWn2BEK6+p5+f~2!Z zRYMAdh)MyQO`$nIxrqWaNjmM^;Yc0+?zDJ)b1NBg;f|VW0&z?=J*CBvibxL|92s@~ z(#eZ^_X0Z@c%Pjk_X>CijiF<=tI2NApn!Q}q<;E@{;mAwl%csrBnJlBO!D|$=f$1b z^R1@4sgPTOs~g6B7i-6l9?XOaeXbgZ=LTzYeV&>JS|U=q++1PWyhq#^tn_dM<(L#6 zoT?Xhv~N~Mjnxv=t9v%p<~G%){f5z!^~Byza0XN(bq(NsqU1ti7(!t&hgPW|VXFjX ztCR-V$nOLtxTL%oS;fT0+CkxV!zGKc<$4k6ThZ+Tk;tBb*K-A`exdY7oOUT~&M_Zw zn@6g8%wbMJJ|S60xDFG_aFr&1;Sh@qh(Ex79NiN~mubW`KEsBdvIb>p&oa0Q%_31(B_(a3FgQFW(=#Ordovk@Ytc1s3W z&^6x@RiSs9Yj8{}|NH2S*G!NcrmEJ3{pzn$=XZ8UH*;iIV>Rt>L3CJbDen8z+haeN z&LWQC9?-1}nU$RgFWF;2_LR5RK3+~(zU`R{1rLHjnQ@}RgIOo{&jOvaL0+Zxu8e-A z4a-w<9^f$Ths7v42{^okK0Ii(hlt{F0bCHwcpe#w1-!le#pE`wbH>r6OS}6gvC;s; zV?eMm?|MuIlIpVwwsTvghd@`r4X-8h@70tNf6pJk7qGX}6*n0{<$x4x7d5mGbZAf2 zM|A949+S$H^bpJ<(qyFu8d@{f5C&2T+}LCRLj#dXnH5>1u8R4x!ABOVm+p;z>mRd) z_1n0+?E34#x0fOz$AOJ^CuGe6cutu=w&QD!z(E?GGzccc+_|l|djQraM_yHay-~&e z!M z-nTV`a>sFX40^~%{r32*EcMK-O&N!(_68aDs-9ys$H=I=Irk%Q>H`&l_Byybc^^n{d=(;1`NqW8|Ai8KXWjSUZ zrH6lPKR5MASwyP!=Ki;v6#YAnHNpzW-tqxydW#_6mYpdun|Fed@XEPE_4{`}HS<1EZ9>#pBf;OFNP5dJP~Ec4ZWjzHuP0V_1~N&z zsE65DUkRqM(KxDXezH-Oc3o&eaZO%;#!FuacDF$yv&?{(Zb*w=IEa+azX4QyfgQuk zLp&LZVV51-S~K<9 zsu!8uk8U3Dv-&!X-))yJXyg=@mDR5r_!BfI<8|69)pBNVstm5Wx5q$JxH`K**2nM+ zH$tDTN_D*HRmg|dx{)BNUSBbvcTI-=K4a3a@lR0pV4I3YSl`(9WxSF54^b7-XQ9QC z+O&tiAQ6QYlo4OeH@uRwzvCL(J{)?ItkeBAyx&9#0wk*bCVKId&5jMfkKJCwb)zf- zC(&U_S5t}8({#`1Tw}IFW=cY8&(s}|?ykgmk1s|kk)Q&^-a0OxjfV_48l_a7mXfpE zyyt!dS(w+PGBsbx%|m)G>75*GIID8g5vVM>L~v$pzly(0yZBL2+f>EZ=J0 zlAT@L<7dg;CJCi-*kI7hrY|2#CfklOObCNCzf(vm4S*4Wa54J)-)Z38IM^wuksl9! zfNt_4k~#xx0NHHLR~S84@a&7TR@`5*HFCdy?9XYZyLcILG_r#d-OTa&C!@RnD(Gim zpW^jv&aZ}`qCl@Xv;*=+h6Cl_QT?!Ie6JNm&k`+L+6ip~oNhoI6NdA%Pk>cFG|G57 zjV3@(vSt^}Chq2j-Ju=-x`Bjq)`o*I%jU!rAT5G^-QoD1rd6}CC-QP7Ss?wA)2^+d zXEi10(yosD^UgdPcA{41rncq)CR00O7nc+@T}=XY%&$;L3s_NR)dna!39kUTO*}7Q*@EVDm6}po zuAe31`e9C)+3su@bJ_j^uLpS~p#C(WauizGw707`K*tKz zYs0@_PEfmM^Knyn(T9@Rc28oa{JRXOj zg^@{fL*plU8ET4l{cQ34b1X|uB^lQq4w?2XeWE?gmLm9n7#x5dKSM5p$|7?L;{szWu!Z1$zyJm z0{~5BsM?DI**zFYscpUNQJ&gIfA5u5#O=nEI~mC%3#OgAVr-egpgDp(msqkjCBddk zU8tQS9M^dN>msPe60~p$yJGzQ?984+J7=(x%!z+ri}@%@|=37bX~rU2q4#DI8EGXi=o=idpUdfX$FX z$+2cH^!&pziAMg(f7R{npVYUfhEOz%TVTUcRF&o^%opw9>vE9%uL7R$X>p2_ST;~XaIINz`a%7AW$T} ztPKCdeobpS26iR~l-w@tbJOfi?A|~8d_SR$kQ4#q#ycXcVIWBCXsu?a-BTFe;@kP~ z#E`}i%Fu!n73t4FQf<05JQV_ARhH=0Vszb{q0sQ1`%uMPAI6(@!;=IK_qmM4_r{r< zYHTsaGOXKD=Iq$iUh)*|goECD(gS0f!nDR3@(mIOCH{myv~u!);eZt5$qW275nK(~ z76`v#qP(iqLlAnY&PuH$^sMb!lud^%T|rLHCHFAruWp6Jzga<~O_Cd%!ufa-wQP$5 zzl5pp#J+cse0S%37IL_&2fl1onJNaCs%#FjZ8&6Gd*EXKb-sxtwM^f+qG3c4*Kegv zsHMlUB35Oa*2|?sDQUtguZg{`3v0AFgtmiz2SkmwnSc(_=s^BE6?Q!3xUMUsrq!$h zpSy0X(fZN%_J=<`I0iGO zQciT|1_PP4OY=nujM7e0fF$6h7e`zu+#^UjIslQ&!00^ko-VmvQOkOT1YT|4f^xIz z>@q^52#?f=hQMzchjbxK7*s5HZQ8?_4$8+2rOsJ9kXP~C5KkCTQPp^jD#5!Y*BkBE z-su-^24H^wAEoQ7U##c^2Wuj7i`$1BnF=~{{AL$(ygx3(gQ ziHcSP2U@LYCvMhXHb!M3Jvg2QDf*s83Gw>gmavnlSw6^HzDe@tdcy@MfR~xFbv*yh z^`3q9J<0BQf6Lqb0=p6FT}kL4V?6C|#-PVKOH@c};I}3^zCG$V47pZz56&mh39+@! zL=SyVf0l^2`x#g*PRocx8in^-TZAX;hXuZgU#Wc}P5u!G^25~=i$)cBy$$SGQOd^D z1LX{IMP?Imeje6L5018e|XOA#>q(-A?493IPjgl*{AqOpD~In*jRq&xyG zk%@j-CcK9&pM2wue&1>L4?e8ObLE2D*0? z0%@1U?62gC^aI+?!5g_j>7VExQEzq{TIGT()jVvka^%V>mJKV42#L$%loz1eRkEl1 zL;8NI03$y6J9JOtwYEYEzT;-|h0iUix{x~0m4}mmHaayFd2Gd21&{t%1*4+}=qi>2 z)_Q?_D3CT&WP>9woR|(%423oeJEi6%I@>tjVF)su8FN^CZ2l1kM_$zB=L6D=aN~1f z+^FAMo5DN%OvD4RmX{q)z{3kua&u$Up6nUtPg80&e<(CFI-UOol|X90SO`(3p@W49 z5A>7%7{ai;ZW9uh$(2A3(3*O)f%g+a^aX!r23wx}fcEq+Q2vIV9_$S6L8bB8b3|w} z5D)zdZB>~6LQG6!WPF8i2!fR&S@lCBRuM#46baUj9u~(4OJbaLVw!bHc4^W}XiauA zxQvu!H-k~K2IOi?o*SpN3MCQiply1-8kAo*DCc8(dSGY|Eiv8Rm{ODKb6g^3!K8os zBl-mAq`D8CXvaogp*4WjbW)`(zChcI`a2?P-Rd5qf4-F9Q<#R)kZ}QFlF>^^?L#l? z$0QrT6uU?ghLB|!Fvo_al&eH8O5`(CMip6luTA1TQ5fW#^72v?lPe)gk)py-rfzF6 zT1gk(5Di^Rq)K=vVijfR>A+Jrfwnxy-|wS+AMu}?r4NZ{?D8q4zS=-b;6sTPAZ5by zBV3ekUb=ixB!&9FP)h>@6aWAS2mk;8K>!wxRf3+A>U%+d`)?CR5dQXTa`t6Sj2lQ( z8c2%^wv*Tnr4JHb!6}s1d5~906DXVW$~k(ybI<37{6qbjR^YTns`!aY{Z}d>`arEz z33c}3M79$-G;(%lcE6dO`DS+S*Ox#24B#wE299AgO2b(LeRx-?=c0HI?$sug6NWB--Kr+@ z39iO@!}Ur{dzR}koJysO_ry0M=SV-dKZrcUD$4K9wn`$fv4vC4&HJ9^ zlnE3eknftV%@7Uni&aVS$L4)uemNy7L9RMJWw_j#zm6G>2J~w8^J*AnIC%h?!I*bz zo++A1zQjL#YR+B3ge zv+R=eI99Mqhh=wD=eVs5?{Iv9yA1JmLx#iIHeNyb98e7ofi)Ga$#DuvhV1|A2Zm$2 zC$w!0bYzktlv32kshj5H*ELxsqlL|iBDGC_Pc=7H%OS}YBo!z5DmaEivvV`ImKjdJ zs^6w4iR#63Lb@zOCr>SBsPN`~?6cN|#aAxhEH2oHbjV0p1cMI!( z!kh3su}Ke8D!o#mrr#%=l|p(6gY*vf(Ob>padnGG3PDqsiaPmC($0~l(QIUf9zn}& zA@m(-8U|?WA`I{wPSD5$*}zG>O>6*fKc3%U|VrXM4*JUmjzYg_1jK*1h; z5G166JxyN};2DMZoIW7G(>Lf3oX4M7r2y~Z1x);n3jPg}$xy(n=*2r^6(aN1-3tbgWHIPQzZ>PQ#Dv1 zjUXFTAs1NY@fMW#5LIrB>@*6O{^Ah|uMg8#`u_t^O9KQH000OG0000%0MY{>(K-|W z05mKB03nlMcOHK(V{Bn_bIn=_d{oudKPQ>Ydzrj!14IS^M+FR7l`2bu5fXw4BmpxC zG@#N)@{){9X3|+$Y}ML|cC%uoi(0p~mM+p_D-#ea+C^Kt*?qT*TbHla?yJrBKli=a zk}=Zn`+mQEKxK!pYEXq&zIhU5<12UH9kXTX3Lp=Y0i}9ERE0hP!%td z!D4Ba2!nHUu9jU(b*|C4);emm4_Db`Lc3>&dcR< zh0Ls!W|e<5P0}=LyjtT6J=Dmvb#9T*i=UQ5bbhtzVd<&D&84g>~wvZW%Suv(F)>*@5A{1X2*%J;$%%RQE$Vk+R#kzvA zxCKHcFQ)eHTbqcFTH$zb(2PegS>E5Xv1ilPo*i4-djp-DdO+57g}K{o44L7P#y~t8 z439K3m9|B~vA7wIZ!tp&OXq`3i`KQTU)z7*)wiRky>IKL-iRA_H;?6>%b1IlhTKm_pZ|~g^=-k$hscK>>+uXb9;@2#Dk&6ZgU(&#ev{R*o-Hl5a5E`)z#B2IDMuCXOxAl z_?}2~S6^_>`)+wT$HW&%-hH(SaEPYOOmN7F6%}b|wpa?LI z?!#xh{W&L>Vv(8#oo77j_^SM;!}I_F!bd1_jI(b%WuWGK=V$wR)6Ofb!FcoZ8S!;# zAZ`xs!ajAH#_!Vj-5S4#sr%FvK4nl{J)?vEok%zpj#IoMzWv~TP=Hgkl8AqK&41EP zDhTfV|8FQI=X?a~aBu{vZfXTl8Mm-nh{|JDc&NiNhkC8oCaf3&snP*9l3ZhdZ>OD- za1^%8&#ZLBgxN|*b1>4_xv72cpf&ES6(*uVWX{~9Q4M0|u+<+8 zOhKHp<6>M+C(c#2cuO(uX#3OMt)MbT7 z;-gt7SwpEQ-T-^2>RekSAuO2Y<|v+H&@(ejfym%4EACXB9K)&#RF!{LWK$wOo`?ep zmN|yyf?znuDdEhb#?>0xdW0{*7T~En5tk@$gNcwCxBAnTI4i%Oa@AIr3#%)aK8{0ib%8eCoZ}R} znPyk#J;5V$TaYN^>RDnBoO@ekW+^@Aj>PO6UU4LrJ-IeII4XbFzQIA@f6;m8p3Bsb zHR|B4_@f5j$87Ln>3y6(flKcU zY8Z4I-EPqP=zxDgchCV`NoF8k^a>9G2+T(ex|8lQ=!107phxIYU}_ZkxM5uKytvcg z`}vbdHZmK_OvBzYai0Fr5N4m!_yL2Da?+q*&@T<1;9~|K=LZoyFJBE%GCJDVt~2-q z!}hqUY@zDtJ=;$tc{TNA;M ziku4J=8xJn%pZ^V4gMT|UYf^{Vg17-=#$@%pQgaK~bb{tE_wk)JU5OF#>Ki=ISzOl@yf1;QH2&czTTyVhhc$!TAf<|_t& zRm|}N;W0U`46%GC))9Ga)n$ z{>>rFR4@t0f&iF5k!BcZK*|wzk!bKrr>MDYAq@I0y=d?+`Bw)2ngRI=(Yis>M?Qi_$yF>_XHRv4g&&Fvz_2O8w z`nNQzYw%zBZ^#9C5^d+Y^p$nNOnLY`q$E_i(wyQ0?K9&}xWN7*Xm-9wXl{gdrG|e_ z>Pc;ya#@5W^WVj?^I|xQJPSH~qtVD7`?)Je=IiQ9 zgMg*pC)re(YR)m0qS1qC16Adarwk`gk5Mz$W9^Nrm(Vs8tgss7UV+lLJ2zzAXaVDT zJRNpA=A1V{;kewwS5{BoI(;VZ`5S-#&mNU>b1obaD=f()PG060&3p@+X>rkcieUyi zQ@*J5#H_e;qe0wcT~~AH)EPzbhyrUxbKiSBdQo>-3j_u4?uA)|`@q1T!K$GH+Q0xK2PyEE#`>aP_D3 zfN}0R%~R;}_;o7%d`L9Ia?Q-@rej-atYX%T!gF>iNjod^hIWtb8VW{ZnQs!ZU*f*Z zT+T~X*2YX5(Ya0K*Hw~YX5Xd>1&inHaESySF_8#bsQ*b_zW0(>BK zrvj4iW%B}n6pA1Z6%B?WF?nvm27$p*ODdO!en&*U&yn6{R8yyCifJTsU6Qb*W|yG5 zK5CAPsR!WrDM3Hax5NLlZK9tW;b(?oQ(`m)>ut8Ms+5S$vK^!*n{9uX4aNwDcSm-?f2;BsWBbfupHAmu zu-1KX^}TsM4drXA`PFSRpd*w~7KJRco@hn!Kchfzff4`#t z0LFMJqhF4>d+9@H4`H;O+~ktknp&=_KSl+|sBnT@_p41GM(e>R(fL$H7tlx0tFg)H zqx3QLTg!4K2CJS3QlNSwN}*zOpTlT`G?L$QR%S7ptNsjPoiQU$G2tj@PLq*+y_ zSyiT4RXVJsC;GW?%3=CA*1(htu_EGbK0)q*3DUZ1lB6G}Vy5o82x72rWV>r7f}zb zQS$r2eK9SePtbq;O2W#4(64{U5$n@RtcM-3p1`~&|J9&o zg1j}gM`>0~{ZX1-<8vLQIW@kbqr^2QsA{0LZh}rbN^@)GxQ~(##Pc$eFQHnC=1~`&L)}yl|C~pglvW)!x3pHv(poJ`Yqcz`)v~l!%N(twC#Z90>9;IL zzmxcRgdTr&g22LVp;=n<0I~P<<21hjD63SX1#0vdm7k!612sHBXB;DcMy)a>LNCpy zKB}fIN_?B)Qb+s=1a?t+%OB%S>TEoyT4T;9b= zT2fQ%Lm-}mQ8i4tG)b^GMDisG3rVVzroLrCC4GP4E~-004Fe~r5z%z6_q-%6!(p%T zo{!FgBwgTLj!u$ROwh`chiG+^Yi8;ubZkZ!c$@8=BFXBL_d^8_jZ+Mnz*fHnxFH$< zoVQ`+Qkq4V!K0TWqwb(OdJU43Nlmm9kv9n64`J^pb`K-ljvxgE(-@Y0pQF#iC<$5t zYd?Rk{CPNyfW!0!`XadNK;;wkB^c2IZ+;m*E>s4F8(yMujlROI8m(GMUsXnDx)MKM zqbD64_fw%A2hjJzB(-d<5yW1UNp-e2Ltrz8emI>jazpIvN)+jRgT9HK8D<6YWt+{c za4*!7|9r59d$_5{adVUV1g(MT*A9Sj>jZzb_4wRydy~s?wm7;;6PNq6B&|z1ygk)f zFHXO>si?A=9@3k18Fei86t5^LUQy~R^65$H99Ujla2H*6j5Z``PBf>sT9yC$gn zWL3$W;{E1|lB!bmSz1*(n|j8I58gormOT3p-bVA(oVB79?B>>DuBzlXZFW<=PcMI* zQ=Ftr4o%*UrCHwIBn5m$kCE;xN>X3_W3;_KN&SbYuSpYzDR6BCd_==nd0(9eRN4d$ zoNOx3f1oA@`pQpAk}iW)UqE!>lh1&)U*I#pTqS_p3}rq=_6 zS0VJTre?YZY4Z(8G}j_h--rTx9bkXCAEAFeAbA7rrZ zu@|Wk!SR%&M_!XcBzg`a(X$a*z%BF>`Y9Dc26pxqaWnmlehv$j@iG-eZWTIDQpqG( zm1)abg$3aT1|06BR3}v#TZ{yq1>^x1UMqn6pUE5^J;t z0sQAn| zT_C?{aPrV$I6?ATOAUAo_0%KV-S4$(AybluZzDqm#0UbS&O4flq@aJqPyGa4VaE=V zL#7BVRQ2+c;Pok(_W?+fq|?CNPsefpc`)mO*pg0TE$KAYLcak(3b1=6Abr5es4&() zsRW*#ownyH5d9VyRQBYMb62?$X4{pdPp?ycN^L4WG^|?EJF3v}7S2OQbc8P+B zI&O5A!1=uh`)lxN+o%SXA^1fH^ydQn4X7Tg0GCUkUN3@R6!y3V;d3nlNbGefEHD=o zzoXydga$gB{(znfGjr*W^e1?56gIZ!u0_fJGyMg>Kmj83C=&Wn&5K5M&@iQf4MSJ;YkJhMql_O_u#S0B zhU*O&j)F}^%rO!<&#VqW`9fC))gb2b9ClY&^}~4>1f)~Q>Kj0 zI(jxMo#Ci5LLEWj_R`(-7x$LU#qz|fMs;celUZ&h9<3-)2tfWlK=+2CEf+koW_w?kb4aA`UWR}|U1LV6HIg~Uk(L+jCnwm- zvGVXvuupM2=Oks|Q!%zKW}~E^vXZ9l8h=)LSbEcTO2vx;4qSl_bP7CzM+NpV2%}vf zg8c$r@JLOm6@eTcSFml_)i^b_l|Gp>%#?Hlu3=W-I&LVa?y_eDUShlpFAKban*y&g zc#UbV;|+l~@s_~bct^#%0`K8{fe-MZijM?7#wP-wGWTcrT*VgxU*anjw*3?tV zt-yDmH5 zr$Qzjse5vO=7xgaidVJrC0p6@)nUGO>(kO3)j5EmHB`b!^o(5Dm_adlOt5Y%rJyr> z@A177h4PbNoo5Fm1+C#qbE8ZVxqr6KaA{6b#%y9jd%Lx^Wf?Q6pSM)%6e@6SN`IQtlgr*5 zVsF;|{46~<#zER)beUm&ee(1x1EMxN3Dt@{cq&1!$8aqX`(%ISluntok~ zl2kYC&Z7#ow6;X{&qIlH%zvXQ(m9XnNONc&p-6MhJZd5fsQsCEs?bBQmL!1z`Y;2w z5{+bW5QhPO$2JuDr@2aJWI?%%8sFyKJ5Z-0zo9CRx;v+%py>j~tsVS%21 zqE_e8IEN$q^Vm3t9wI1A3=WzWv1zzt5u4{wN6VJmzhEn^+wypb_a5FDf&KTTha zr!j;W;y8l~7)AmkNaGy6XK~!341bSt{D=wsgh~8L9E-T*XD@;f$?d=q9QE^fw~)s$ ze!wxRp+eH#h126i-%X6_e{n&Ds-pLAZ1@MGv>{JGdK5g_*hg7^D#*HDU#?S4B#(!0 zS1g_g7z#$0)DZ0R`A?$XUk7l?KO3Y_57DlPXnPU-^%C_7X#WGV0i@Fc3N83v*!&p) z08TcO-liyj2Y6f66+Y)_JXwAjw&NtqR6(qlxlR6EN{#at(kdU-T>shv0Ie7b}9Ea=x&t9CV8A8k0viY&&^(L z;muxpl()#^Or2Z3G@09E(N>+e$(-#v@9TlFZJ+cLgc+3exH|Wtp3aM=@)!OKK+uf zl*d&be!p~I?cr-=?tTwno6pzr_409pJZ|*x2fXwjzDef~dZ|VDc#UtCo?E09m)3{8 zd@Fz0OA)?J#QKQralpg3>wJfFe$-1J<&Q~!=bawDOWt>T`5wO4!ylKCPY45_l!>46 z@Ifzsnm;2Oe^%%Fywt-R^*NdNfQKK{`H;?sJ^XnOe?dmS=%qe>NFGC8p3cKM zACdRNUK-#p={(}4ePVz|st9kr1KO_8q zJnP}F$@@9kUy|1SGW**)zpV3jy!0Vi za0`D|R(($dc*V<$MQ!`>K^xt6M$A}UI2ezcaVB4V!-m>z zO z2TTwDkV$XbSbNUW6)VwdZ2*ymHYRR#AY2?w?r^lH$BZ$}Y>LKus(WI=uCQ6XHx}&g zH)GXJY7k^SUD3Ufa5UJ(G$+@@#(H~PSm+NXdTYUdUq@Id&(F1BOXeIbnqlsL>kJRX zLwn2(p|Dxo*=fe(&A~`e@m8ISLc>uPfSh|xC=yDnWjed`7;+t3l6Pl&(RLuTVeRfv&p<4g2t^|`i!6_S2t}(!Ct`}u%yFhg$4v?nbz%EhsAE9Bx5dIt6D{%) zGf};*wGmSa!XjdQ#yp*Wgzl!X-Av2hRhtXOt-=nvFi{_hr8ZB?W~j|~h5F?iI)gu$ z{jv=DE$B8AoxRx{Y%k5GkS)xZvE$Y_JYYh^G`r&UsQ}?!_%p@GiYB&y4_99p>aPZ? zDIURpai)ITdV`42wt+slZg&tYfQ}wBF)k=Dp)C>YJij^Eue?a-AM5-Rgv>Z0GpL+# z{9cnS`l4K@;!X7Rr!?(`b9PBsPEV~|KhWK6#>}o(H6p@gP{|b9=*n`IpMvy21jpsxY?k(AT!G4+@nkm}~ib!0PzM^zI8}G^{%$ygG4!|uGs^**f`pwRS*`>^w z7wk+71jDMW_gQL(M#B~if-!$?J7;>WODu`0lXhoM)!Bm$+Cn{%U}7K!vWwq^);O<5 z%*V|{!#?;$LIPon8S4wh;}+;@;uG$8qANO(NI8e1wILdR>kB3l3K^VXBuY%~?*M{j zXlF|-Dk(ha67b1>rlRo^YEr?cdK)7k8yo0{{xacUf@QwCXkTA20*5v*DH^lgSm#%v zhfsV+D1x#EOgl;!0khrFcuP>soo8W*N;~7w0~7W5fGRg&m(E_W8#CcIMZ3qFT4z73 zp#TnVGm?mZ4W>{tD=o-~s~1v&gho}DO6RGn3h4eAu`ZsrZTxh z?e7%gSa4wy$ES^F#7?bCbCX(Ael*qv?|!B8ui(aR;A8ulJ+Dfp8Envx4EhRv)u1=%O@kif-+=xJ72mSxw+4NV9x&)2 zecGVU&}R+0kM1}4cl>*u{~+%_8vG~zv%!DiKch}MhDnwPxxX6xH~u?#%@oDpfABv6 zAxB?-Z1BH$hC#2=uMGMjH`5l8tp*xM#6pcY8cNvC4AyZ+0RwtH-i67K7Lvw(F<`feb<*3$>uZ8HDNum7!5Emm@Wnq;F=FcpF{iqZJ{*rh}BX@U|Zdv}CEdE`cy?s#>VvbcSuw}r)t{OvIqn&DKYsH0qIjRI3v$S>EX)?byi_cV5 z3D^)FNKoKJGw0aFp{~^#TD{g_Xd49i%F;lD$`;DpE>FMv{iPLIZ`A}A4c z?Q}!is5R=^CPODqQf+o3fY+D_5`tg%49IjcyVo{40cL!!HO(c&(Hm+(?U+pW!HT6mnL5UT0qumlN8 z&7~)Pmy^uib{Uj=_gs~KPgZi;+8c}RwNBs@vyUptWT!eB6H>88B33Sy zL+u!`Gp<#pl;*rhafjlT@Dmcz+P1pJ#w5Da@E!nt2bB#1c7cqyB9%_W?MZ5%tP;j+8sOt^0$coNUta%`H9F(NKB0 zxz9pe=uQ&P)+kdxcvry{>BJUGj&9GR-rhOI5CTuT*DnHp61xZbyMn^5jt&d4++8+8 zI!hPHEnx;EN={X3%}+!(rZ4x3OB-_s3Qq7nqF_KG_L@~%cPyvYL-B^b{=}f%f~)MV ze*PFocK7jwN=^Ehyi$(Ir{>hu@m~ix?%1U>2 z(Qp{u))il#Db}&`ZE23H$2_^H++bZl7QlDLijW_Q*C&q^&}Fa-emEH#tqVq?5mfzQ zD;lSk=D1B$DK#$o6UH;upS~K@_Xb0W4HCr@RhVRd~eY#=Y&2B1h&{m6Q+}oE7zp>u?!~ZUoK*| zwWWSa&KRgs0p1kdi{u*=s7~&YIVa~Hp5%!W@Se$6U2ibf2FEjjTgu6tVdY1~DL2Wc zo)Xge&*T>I5ue>6;nGA>Exrk=x$=V2VWZ9i|>zT ze3#<;6ZFZ{_ot{(Zq(2&luI@BzK`x#@6XYH19%r+pkLqR<58abP9M_O>-zfCs7T3 z0V8D=P5L4|M5J266RVbRrKy(i-$Qwd@`~~yGMdZ2Ncm_?XsH~ci2)~n zo|6JDbb5TQ5t`gy=5zU+73ITJFhqqD#azKoWOp28X@<}bs{uh3U5#`!bo z%fracKIafk3Ah|9-R_k-n4fxpJjL#R1Ef0-lGCx$Q|vham6lfw)3mb6VVYhh3ydN1 zmHS-7G^4B>oinleAk_yv#k%_*D)70UAp`Ru>Fj{Zxzc^5K3c4Qj7}P%Iqf4fw|$uW zh4Y4JKDIllZ~+=aR5DB_KVIy$YUPE68JvX?#ioO9y z*Xf&>xtcuh&;*?#%=wPf_$@Mjc$DUnN2g+)igfyxdOoiv5bHGSEg6{g20Sy5h`l07@{(J3Yz5^Q--MmJ}RCG3y)AG z3{=%FrmY^P#R0d^Jw!_ay1bV9^g{uU)$%+ZaKf?klGa=fVwFc|g&1^yWpeLTnKMp7 zuei?Y)F>Zkg}TQV5wzj?^SQh0|M}IEA%gihhKrnxQd$SYOLOm_19s| zeyq5T60q-Hx`77iM!JJO0NdQ8EZqvL&ZF(hf=;YnMlY$@I2%ClZF(8j8briBOW#p2 zFp{$Vh#hOv5MGJkv9YeK_qXsSP_0 zjy`i(?URQ0yYRdlcD@IZd@pT4+G#|pNeVL$c=2O}jo=|A%qArQk|Vt0C-hTr{4?|# zsh*$P;!Py&ZHeE1U+DD9HLY@M8Zrb zwvLp%9rSD4cpWyL%}37pjlwgL(?h_f-Eh?m*zw3sww*VB?o?<;bVFg&5o&H4p_Xls)V2jHxw`lp<95=Jsm+k8)3Zw3MhpNmLYYnf*S4QiP??d0!aAi^LS@8J+sPFdxc?YP@q(9Ifp`KoV%Aeqf zZrTdkf5xb|{SEXNC|Tn0YWgev4T_yam(t(qAK-m|BU0BtvDSe-*U-P{-=HGKSVH|6`XhA}mjBlOh!ek4OIbKK3$xIe+(3^Jbje-SXqFcqD1lHB z#Ha&n=h8bWmebKHV?VdOcoI3@B0r*ahSE$C7LPL7;rbFb61b?Ve1@Ed5qupe)x?iF z56HJfTVa>36mJ_SJ>{NAz4Y{tj zXc8=+X?FRU(GFHjP%zOdNOC*rN60*cW_NSNGuFol^&t3qTPnn)kFAs{u-IMfx|imE z`JBb>rO5mG5QPqqQR&kkrt>t~aitqk_mj%BiL1aK(Q720nGc7X3}a1!DW*dfKZIHh zA!@X13Ylz|jm(Mjsi37C3Dx3z|<$KRC?NznY2rIIDMnFr4MI!ax6mcFWA4J?f*`&Q;XOQoig+Rw^JH4a1r*>y=(z}BFon+LsdPS1 zqs!PwSFxY2;hA(T&!U^qzJ+JgtvrYB;JI`U&!bQBeEKpkP`2!c)pt-8D8H>yksgxf)rNWyLxre~l zlS;Y=z}?-p)f>p;8O6S-`WgQsI`!#19hH}kLLY882YrHk&dfrtj`(&>^3et3hA zXV@5d1~!qXD=NJF2wm}cx^jrFYAP>${}5d*r%~&m=9MYD5Y>CBl76axwZ!JzfR<;f zFxKQJe4FqSkX4|qrd-9+QoOEdu6UXjIo8guK)#z-ru?pA_EI<=N}H9=V(0DTa@>EV z1F`l~N&ok!a7H02S74(`Fi}O5xf-Fim=^I87-1m^lZ@%ZcDvppuaT zY?kp{m{nM>NvXU>RY$CU)H{J3Z&RVp^LX~_Afn0t^k8Gklj@K|bo~hJZ$~z{R%)8- z#B(2}>m{j}(z-!Pxf=y3arTg)_`ngmNsbzB`S>6ZMPlM+c>i-FbPGb~L+w8IFx@&# z9}ehcl``ozpFT_Yay! z-sN~-PFJe8GkuigQz?(v(Il=VAFqeU*5feJKYV6ml))(CwD?mCie8VnwB+7%+6l!O=g# z8CwB72hxS8D!q9Jxp@~A@NSyLX9M@op#^+yMp`dPNnFBz%a96LwU$FOlGf*{%E^Hu zdm68Ri&~Nzq`gIMx}-Df2c)t9$r@f`>)|ZBR`P;mrJOai!#Sy1f_YO^y(y|*P_=Ffyl^$^rohW<)lESv zQDe__e3~sTM!?1%x725xdp`?m+^PNC_I?`NSaREXx>K3MfdgDItm#D(wf=h)4*VG9 z{TH*@z@7vNSE58q=)-)HFs5if@D0~UW6!b>5%9Kw%S|HmR; z5%9o_UXaU^s%aT&-nLX-6MrCOHBB)xW!W?pQ^4Vi^AnRZQ>#l0Q}e6SbGfP2g~j>o z>_q{Qnd|aRIaQXmQfh%5Xr*xhof%y-Em^ac<+7~^=(;>VcWElK*s$s<8FI0#ESZWi ztyfsXb))L3$JMezE_$kleqAY8ld3_ZZrm0SJg;i1bwR+f*lz9Jvwx9g0sf3$B(L2w zs;11^mAqms%K5Uwa5>jy*-&}zE&8o>m69Bq(T!5dMV7i{$knQ1q%O#)(sTNUM8h2I{)09if zq*_u;OTeJ3WGV&QP_5gk+|J*mAIRUfxNzI9rUeL;o)#E2b1sO`GOy$k6@-HP4El_m~V9blWH>yhw$Ef*C-!Y^3om-rPilalaj zo{i%-5`N3l?|<-`fHNPy^4Z6E5xD8=FRB=?b5fyl zO`MBT-9=S1YHK$%{gy+~J7nENK9}dNxoc^`t8ib8TjTHtJjCQ8HnO)$5A5lFX{Ydd zV=b$FuQI1hd2lGLC}8vhoqeywxRY7>b|xoUUI4osExQMadYOySo46Q`wBTTp=q&3p z0TWGmO@CQ3Q~^g@AL=F_(#|>c5KEs}$YitIIFJ0FCgnDetaDEm2;k}c>Daf+g}7C? zjm{q%;Z_&4t3}x&cY)Z|G?Nf4deMThth;hBmTkFR@m3wbxw5!!=(o5vI^1^9%YeWa zm5sSIcG&_u@zHMD`R)FCD3)y6U~o4 zJdCpt@G+XTAwlzx@0gDw!rhPL2sc3biu8|~42_S{Y=v}u^zDwKUEN8&(jB&U(_!n}(B1qSZK6E*nj2;}0) zI)8$*@zF#b;yM2oLM!~My^in}I#%kCXx3RnSEQSUK0ggL^wjadxxlt=WS8!NUAm5x zY#If((7VzX=nK|yaI=wHKY}z4Q(iGbJ%YnTYKAD>K+?%^+Qr<+@eU?2MH#izoAq%b zxs9xBTqMaywiVJpOFU&rJ4*}%$d80eB!2}-ldc($3zKHd>2RC?`tRXT4TtM^aJHF? z3xCvw--H{cFYpirJ?+4YyKWlrh8-w^BQa2h_aJ5*cx`-#cmQ4}JGLB)^xZ>$jshN; zO;WUhEex*s3DnU#j`f_ZA-b8{!q7_O1nt$y`;O=1^n^Z6{+jeXM&krM@YCp_)PIL4 z@(GH~_#P$-f*8OYE>rvtqUckYC)*PwFJRHhW~_mJ3`-9BWs-vs@*>4)<15-jeVr`1 zwQS16Jf z_dn>QCltRAytvPiCHw3ro?^KqZ-33H3xgCqxtSdFU#nrH8T}At49YP;`AL*v59Ji0 z9Gbh;-$2lhr|}HM2;d-Aonn&ca4{C2gQXq9e-ROJjp5K+#DnuZxnbhYCn5wW`3geu zH_^74h>SL7zD?dWubd)dR7OrsrM8d5L-+RpzDmKKqTo-{7Cl27cFh5N$R3T;0DK;W z#s(3Fu1@-2bc$2KN1gJdYmw4CgZBRcv$4z`1r0!7MVMs+003DD0Bv1oI9yv7X7oNr zi&3IS^ftOEi5k&`3?anmT^NZnL^t|TBHCagL`y^qA$lizZuAl@2$RGmL40$4x%WQ4 z=R4=eIcx2At-b&3=Q(@tv)-3L6kl}94E%|Mq7u_ROefU9y@wJh^`y?>nUn(&7*UeA zq9r17&|0BMTVa^=CN+bzNO+oru8?%7fbC`ihg0w}+5UBfF9PZVK0d*(gWk-aeGzZS zI{A6JdWAs0O^bR(Lb%hK*haIEV;x}`+r}gM%^|Z-Wbma%Xoi0Hkeig76eGgY36oCK zi>gbEc;5K+JK>Q7_STl40~dddmvl|&rL@EGfZBL(x}icnuArP zOjg1c{s(i@BO?#2Lbi`J2T$&qxz=ml#nH?PH|4zZwZ!d^qpWvn3)1^+hPkH=s?t%= zyDV!}`R>no^$Z-VH{$hBS6JwHeq9Igvdy6) zeca_&BmGo(5LDlVR0L;enh8sLltx!icr^#U7-w7dzxWvQvqs(T9Y6God>$5r#3Z+e zEoqD@4Jjps##{9EysCB|-ay|xR(kfV@^otWfS*LMkjjpD{P9*JoXHL@9?dJ)PT_rw`oLGs(}<4by9i0709tX6c-;@Z+{iK*}?UTaOH?D zPL095%bcO*@dglXNYbjbztzTz-k>K70bR;tn+Xk8Y!8VJq_h)0HDdgxXU15o7ZX`lAaz+QK z-)B<$?7^XZ9 z*a*+0KRAx8pIqHnLI{)^m|{Gc5EVAMUy6GIzOk-u#@$CG89NlAtThaPQyd7S#E4y1 z)$=LU%_Mc$=%f;#W`k4A2vA>*$RW$>>ycc^U0n2>4)m}e;FK=}4jSZ;HTBzgZ#S1Q zrvnYF8=Ufh;485J374FzA6Je>%2jq&x^c9vG@XgYZ~!^^sd_0+I#7(jI54F_BZWmi zgfO-v;_da}V=(w#lv)oL4tMVxsvLpCU>aqy z7O{;J$rgHo9pyLP!Zj#RNjhL}7E>GEmAcTk23_0y>^*FJ>C1@_=B3zJIbF+GUIgDm zIn=^XLGfE0=dZU>$VK7h%0RZkmic7l{*l4@eD7=I51c3GVrRkOPoIR|L&@V)<>Ro+ zmp|b`e+Bm?(|tRl|HXc|N}PNd@i7^f@YgXz=3@#o?it zBR_Z-D@D$}u7IkD9i(7Ir66;k{2K4FaW2C4z3!0+=jz7|zFI zp3K|_B}MsBl^NC14~sA`2Qzqcp?t?;2BPO%Bx(6M0Cp z15Jz$YWhi9EOvX>Cx~S_de~@XZ+cgX zz4P0E*qw&rEPL$$`LI)xq)csn{`zWdU4>)423OtTIe~kK@Qj5*Hz8mcQ+V2w7g8D0krlL8 zOj#!cyy$WK^tL6XpWJVvk0?p}G+?43GnV*$aOS&4*=ED_?cow-RyQ;rUJ=H;!JX+6 z8P{23C3aorq!+oxu@WqHJj?ytqyB*YZ4(vB zdfX%-Lqumh5BFRsqdqdvmKmnLrxFYvu`~W!?VRhCyTLtZpv$>qehE?B+d8NjU%sj* z;8R#U704Bp(~;h5=e4dgK`yz4nm~M%SaFHO{}n%EL>EgX`0;&o?2!bN90h zxj)zD5X`V>@3B~t{~#N7h4*o3!nN;%fwZI!r6_kdY9H3ccBE#oVGsT|G2z=0p-VzD zR4pd$HsU0OpKe8fRn@-kA>=e(;p%Fy2#&$=c5`;BZj{&?9Y?($LyrV2#7RQ;r|WPb z4eg>CXz0kC?I%h1C0nUgi=k2stxP4`aS@}T%5|S3SZHUg+~ARDsI~?Fvs7ja{i*r3 zQk2KQkqW0%yOqNUAqpG1H1>4MTJb=i$Fn1J%FOVv2@?eWmI)+B&t`(fq!3^@Zg;l29oI8aZrMJc{HXqJCze)>g=;?*so zjnMK-uH&_C*O0}-Dvq*EpZkP7MRgOE%J{_0Ox5*_eReH&-7o@<@2U8RD_WiXP`N=H zjMf!Y6HyIbi2JS7$EN1q+J1!euw7lzl(nZ%#obJ^82i>;vn;pJHGDI$qG#^@CNnKB z6=WMbU$JC#Z4C=M3e#^kmFd6VID&TW1aNpjzgBtMmx#B2hb2j+01SunTi z{!nNS34c7c`2%Yomu9Kq^kG}<7Yc(h>e5+|ozOcP43QfigjavrJ0Re09#<}QdcNW3 zmS0>nW&5+|O-V<7-E9SJD$_Fkmk^YmDX_YGf z*y)xke_>E?%eZozTm@`A@8*4y(@2gkuSK!2taMQ;x(W1`GqGZ)acl)%Dh=T_UYGx1<{ls8=W?^L6Ov? zFC^UPo30rQfNA5r{RTytV>r1Cn5S-(XLmD6TiP3g>Xe5tVrZu!%tDE1or?v$)@h~| zbE`QXROe1=G22C&(>MpQwwt&;Q>%rZc9_tRtyDl~vR2f%RLWMOMA1{yfzy(c2XN!& zo+P-mwud>h+xuKDWJDmb)2p7i4>S)d+F+kfC`G#9BJ~gt6|z1n28iQ1ObX;H4KDYDRlzVMr= z!qyT#G}US~H+o21s`kRAqWD$*j^nQSV9_&!KXC~yPT1~C&r$Bk?!u?5ZR%jjexFgj zS6PAA(iXJOzE}D3)I@9j+3!_wTo(na?_FzH#9669nqI#f{%G5AOeL56MmFn{>~rs8 zhH_#BvyM|t_jIerYcF%ZN-xq6d41d;dnF7c^VRqjI%C~-@4ks7Z&RBYhhQcLMh)_J zdu4Vr`gVvIC2Ub*1a7g0UdFxQl{bWIK%*i@rSX&@e>k~Ryh8XwPXkk@mM?7ykhzd? zGtKIV+UPWGgEx3_JKZwEH4W#gX)paoqQT({tTTh=V8HVFRiI#9 zfbD`f?cY)OCpLT;SX$R3K9{=`+h7KbDWAu9ZE&-n3tr+otH;+$NneN=xpoc`yG9Kl zSHbN6iV6}Cs9XT{tN#Z6B{K*H^f$pI=NfK+-6j*L>Bjkxb2hn&&xN|$Hkm=R+ULIG zO)>ThB3&1<>geG?Jb1k>Qov(N2*i39;IrPE5gg14j*qb^`)!f~`+Gd>{}zl85O7_HC5a~bd&&})BL{{a~b{e%Dj delta 36122 zcmY(qQ*@wR6Rn$$ZL4G3wr$(C^~UL#9otUFwrzE6+v#9`dz>-$8UKA=FUx=)eZ;1Or zd~{}NCcVW==2~aNQ2E z`CuaUA4}uu727iJ0V!-;H~aMz1lDHz%8n7{{yDokd(C1tmag30=jUCr9=ds!n{yYkX;R zPI2rG=1~{&b=dnRr>4*vu5rRTE1oe;EEQ3Y*7S{MD4s{600@@fZBfQS1yXYQe)_X9 zWF!2Qg61I8rPN`2OT}5|UzoQ!4e6snbv?A9W9mc#f+_Vt$C~0_8W~0&pvu-5ju15v z%*EJ{Ulk*jk>k%?Ewp$t)^fkm{Vf@BC;U(7c-Ud=NGDjib2RSXc9j^-tpSX%dHb}p zxQb&FHTA*)Kn7fGMtj)olHETj#8OzC_j4}mbanP4V7}JsC|uT!aZWMOW3kC|@e(dfZ~y~V z@_8>n(HBd{$_|}}BN~?jicz-kw^>awsjuDuMxTx{utSV9=zece)Ki2N8gV>72h}Ff zkMLQwu2KSk+Ay`5vuwI<4k-X`T zC3FKuw9J@?izpw9&^bqq9=a2_@FsD#Lefgmr$|E43L{e|teI*t2G?d(E`g*W zZT^~^P9kl*MJPDiCLrDc^KR2)Ag9EcT2FSIT6dkBp8$m8TSk z1*6X3q)^8tbec))-fX&3+Q1#KuiEEXA!WexaCcs{%q4bTM2Q2UjlI~m{i~-E^d2k0 zXQ>D8E&Qibix0q&5$xRqy}|gDp*ai+DJp zq8Kud1-4I?3o8x%yV{@)luLxxop@9I@|&;m7mgyG@4g~?MlXxjshX}|mlYX78cr&r z)9BsWSw2!z4K=&cDguESTuA-C{X~@itg`?7&M|8R%@j#UHEylhJc8(`dU(6mJ6b() zmuKPNGQwqRvB}hVTPiT@KE*7DU-<)P1j+Roo;AV|;oa{*@wajDmBdSDok;f2!3c%e zub;RSe5?GeMHYtl=#I}u-KL@&CZCg1gvqV zagwiuSfdRS)+p2mQE=mlgn9FdqPqk84M-$gzDjxTxgf!l23Uai|2TurDbe<}j*O?* zh_W-{8<^99kENpo9RX2@h=FPfDI|R%7`GIEU`3@FtX1?4y!i2F&dvUZE3uNarPP9y zK=cE#4{`On($c`!HF*gV8|hxU(lDHe@SyP1=;F7qXW zx?Ce#9GClVDCF{Y?gad8{44nVc7_Gw>P2=yw@_xKmBJj#CaDn~N{)l0hhT!U%2gXZ z4Le$?)JZHl!ZSJz;^4fQ>J0UB0=o}VQb7Vc3*Q@v^M(I>UX|eI8DvVW(mqmKSMj9r zk*UJ2Xx3@2%;e=BT)L^y&~I%h?lwyg@1An9UC{k>N097VEKJM!Ym%^H!^<;>L%e3E zCfng|NUtu1I5tBPbUOUnw=iap%-C!7oh(*XVeBMcOaP#l_=5ZZ%&-Bic7K^Jn;02NadkRB9_= z*iAA`t}BeEa6Te#g!b3zm<#Ji@V|SoOXf+rU|s*Pf3V+BtBXT|M`}^}?fA~ckflc; zj9sweaZ{<79Y3jTwB}FheMB%&o$T9V$!Y?mV+`VpHl_K(yA-VaVVl57Oc*5KSq%1t zIAJa{!am`;W+hV`s%ZNV>co36u{scvV@P$%_U6lw79BRCug1g>0Z1G zIs#tFh_f%rt5rV{Tj}ukVwSBt0}3mXf^-^RT0@F)pTfw@q)UK#8kt9K#qX@Xb{!uu zq*hW!C1ekWm`&h_gSKk>7xYJV*yO7hBf|-W$2Nwi+uSleLxhd;;&SX?19KGll^QGd;n6irTXNp+t?F*S zjFgG6Zy@?Ppzd(&OkXw}s=I;~7IpprzDI12Jg77$DVgfOt+X$LANKC#2vV*K7(Byz z1-qnezu~;n{(VPx3`tj$h;%O^H|x=%qYh_j1iXsTLk(^;b;|n+PRr1Jk^0qpnIL`L zSlVNL~27L|5I{gd~B_fTL>NQ=#$a_cJ^B)`LvJYWi(9FFt&Bqp?|BPW32O4fc3;5x` zI^vy}xkgXuI> zo9>CF1S_yn&0Ntr6NcE>OQo1X{Z;1bW0B-GXQDs3vAVF@xK|myujWGz417#qS!KfL5 zPpxv@3W&-=XcC!TvjWDEChH{%3i)$Mm4Sav1n0XA8&eLE!0`7RmLbz!|LdhA$!X4( zJOXA-BvKBq>&d3;4R_9Gz}*pTAg&Eg`r3?MHi zXrUI5nN&+xkdfB8lx7!U-eV}wE`J2T@)oyxGDEDXl6PRn;zj8n9*g-RzVQ@xF)5TA z<&a;@Yv)Z#TI;n-4Ow;7A<~S0{Vy2Sz>SZ+DIy99-}r^V8tpl>6Kv~=Ub9DO!K3bpK&6{au9}md1z?T~RI?^)L za7r#`(RZwV-!EBOfMvb%a8C?_uhm`)vo}WK5WV)Kft%D~7VZ)Fw*x$*`jY)JKB5ta z*L~PB(Tdv{4_YPc$VKfC96Sdw93^@ahN&}+T?zTyjTf*a)E}qGjji%d&F05T$EZpt z@{IioV}s~wtaHpx+7x_gL5*O%qvUk4v1mmosgG%wS;;alekSVVo%*|otc#7MtU`MP zvHc5*5w;6QX-F-aNLRnnP=@9`a!&SuoHp9SbU>TcVVmcsOZ8Rwycsh1%$w5>Hfhm& z3s?IcU@4{eMM8qtJQ;+q6)&Bu!e#=swv32Q(&x5X_f%#f!@o=*YolWHCxK z#08Csf2bzRVt$a%!PsOQiQzPrYS*6m~w~rk=hDS9x#05auP=EBFU|51{v^84U(b~9$k%k{kwzZ3-U+JHJcZd zc})&214p-AW2b9eZAM7O{|BecaiVnEILQXMcd}M+$6Z4=4bl1LoA<4tN_Ugzvgz>D z6cA6#4Z*ASiZ&8#86?pHjY4a!L{3}gV*WV$wZAMQA;kF5BJqV$sa^G^m&y6$7kc2j z<&doyu5A$oJ9!GpRsAL=))?%?Y^B>J8ptiU7_NT5;DVJNm)faxnJql)eDhXhfYAf} z?Hk%#I)iMR9zjJD#Jk;>w9TWFrhp(;5iP6jgQ6Q9;eS+f8)rMD@`=?WS^~D z`geXXA0py{t3OdJ;0Xo#blQ~`#9HC_tE(=< zOp%PdUOU)xac$Yfg;{j+zZ0hEp=bdHf~HxGP;QRo69)mxtJ1ShrrEUwaBE<`FXI7eEqh>)f29GMQt--aWV zMRDgX0`h_AUD0T;+onceaW4;``d#Dz_*j;0{3Bz=cY_eP&oL zC0i>sSk1 zXm{OlrxjOgLynpcS6IL<#{;e{NjIZ&qr>hKMo--kIR}=WaFxJP`eNe9E!K0Ui5_E# z`|VbhC34#q+<_;r1p`{?&V#dL$FTBZL3fOq&@2mF+VD2CTBa!R->>TNAvS-Qqah9x zGnZ~2MJ|1?VjBX#jVWfo!VMVfr8^o}wZ3o?oJ$eAL>|m4cs+$LHYJxQ3 zR}GRjX+~6ax8B-<*OPFGA%pU}vRXLJby8F1zQoz|5^Z7%P@2~)uW+*?zbg<-xEa1N-ipwKSYUQXqT^|7 zx_jj@>zki?DSgm(PK5dA2nG}iiur=B@*mDoUe1)K3CB-Ezd)NiVm0Pt91TD5pgjb_ zI;BXdy1YIeyy+=)nChIdA&MzCX9`JWG9|Ltn!U;Btx)@4Ry#~BQ1h7wHZ@R(%jVid z|3rI!LvRBL4STnv<#(Q#BYqHqWuxm{wRe~sqLPb7bTSc^&U?3VbMy0CTtT+$L2pfM z3cGx@H`Z3Tqe%x?y${Pv3Q92zewt-(=RRx1CN+Wqce*;MmV5T3@I*7lxm@w;`#;x+ z1VV@fMg{H^eQf+AIfr_kL>P2kN1#0Fbw{8JO!rRFF(|sDvpjkEVE_iW10{7yQwYAjm7NYU4}!Ce z%IUK&V$(mpjKk!4td#f|df~URHcG0WIG+Y@x90|S_al=Q`9{U zBo8r{_MBOC4LE7O%Iob7088&ribIFxS)eM_rlEFMk%Z)2UQbDykd~ul7M;tc-*GWR zZG{eD1bh4K#J{KyJcT);##pLkUN_M5%|1dms*l#BUDTGZTX-+FOiU^i5u4T6NV7iT z34&_xQ+d)`zrDabycvLmv5T0jS2zoRO*oaTuQ6?{nhYK%FRELruGtPWrw|fQe0XA( z@jedR^U1D=9)h(JsyEN^N5|#r$+Qr+aLTRd33iPt-!H!a`yo^tA}ff6}-b^&s&cZSzm{gJ?^(Wbmc3*YX=`$(p3z zwv6yrR@D0x?&Dh_TQB4tA^^a(G1RADxh#c{a zWf21R1*&M4uXbE)quQy+Qn0cgyb+1e9oWLnCe~O$q$7Rq$cylXJ$}vbyb~c7D59hE zkoU|TH`pToD-{R7PMV_uIRqTaFjl{49))Y6eY^l@1fVf!=;Q-YJL^OGBli+0WNo-8T$Sg{ekh|vk-4Y(z;F6 zpKfFB1L1?;ZUmgcQ52xFe5>#=#QlR6eNyF&S`ea2J9V(Ne*{WF)|UkT7vBg2P~+rl zc3tR_lwzzh`vsw7Wey=g%62X>v6DHL@Bo*BsiI#ks8gO$bw*CbtCS;;wv*uXtg z-eEN=)t)5=lR$ZP8KRDTO0U`YDA#2#vw2xCojm;4%Yw_|8{sLU-oN~WQ}fA|E?#&f z%HX~J`($%S^W_TV2AH!oD|XsauMt{=dwBF58po9OKgCzP7#R$J==peyCHM0LB36&i z_yOVYllun8uuVv3t#n&hAKhYi#;Ja?{8x)f5_y+D{NS9}9R@J%ir}#7O0KBo;ctD< zEt($PA=gGLMelrxFe*Uw>!b#aHntduwb z44HfONOoL6_JT8jHb`^qzBv#aB~Bo#WswdyWp)&18O1K!W>BGiHwYiny{U4=G5C1L z^>QJOu*54rF5Jio4CJ!NeahOaZ<=ExwX`75w>z%=-U94C%6bB)TEE?OJ}0E1NqsG zL>Vb)in&baxP?EMvWcstelgWZlXk*c{HcS+QSF2V?q`E%RI<(U>zT>cxWdQM3bAtv zp7`drrDKtu4f_7fc1g8}iN{=GQT;@GBSD9n^tcs6^d@QhC5vv^7K4&!3FU9E*8dvA zr2I5L(L?Nbk66K9p0$vKGQsyw7|B1x;jj8{EkL)TLK-qvGG*H16cf=MuIF0)ZxysT zVTD>3vc!h8;UKpTf()s`Y@^IC6UbVs`)^aDOk`%A2Qrr;{peH2|K$$8A=#i46a=Ia z5(I?v|6R8~xv>E?d&Na1^nmM?d1W5_I@q2-_$}BF79r#)Xoh(@?LM>cp?Gt)#$sFP z4HO_;FqARi2WjM9WAA8rUd%}gf&vFMgZ}KK|BUN3|H)&(=hGWppm++o853ziUhg{- zt%*V~i24Ai3<;(3f(Jr|*#|*-`Z`ZjwmTZo_FZ_1YVf zIJGivLkX`ozzD}?i$#5a!~I`inRiWR?w*3-(H!=WkCAerW|%GXfD}Dpf!eP{m`Bt> zHNQS`zeHf>FPrz0(UX$kin?qop3StUd}l$JE%-RrUrf&zq}Yx+cb}9fXnK&*mEz*N zT(WIOCb=E(={a3iyq4=$uldUFzw(ou^iPTGCF8t;e-i_8RVT+x zghn89Bl9zKx{X%{MOtQOtzIBz^3d+|Mlf5fZ)<_)U}IVibM2Rys4JWn%lG5@`3zqY zNMciXMr?|M$`R)S_>ynJ&t61Kk2vEt-3zOzgVA7}Ri+On82@N6h!T5voA5e)-_(RK z<6{0^^Z8tn4zg*1-`z@AE!+OPY$dKNb$AGV&l|pBE>}f8oXpN63LFo5bT@(S^H6VyPtUEWS~P+@YW;uwH_5s&@sf`B);L((~8& z2<(#Bh&$yweX*|`erx=~%E(xUd&D>cVBd5OGf6E5%(TA_Ns`!;BFhD;ef=dw`;HV; z(?>{k6!)D2XN!=fAX=prl&4v!RF=5T9w$r3RfqW2t&=9PzY+cy^9;J)gR&nWAVpvx zAYA_sb0mH#Fl2w6Mjig(9}wJhl$RCBdjdkhx59rm#n-dX)$aoRUdPx)@ z*s7YDnIt_Q`@_+i@#xlPb(28i=P>21p%gf(ydTKV39e3h=qBj`X-i8B%bqt2iw!{l z_=04Lu=K|ctVm8@Nfc2|FCnvV+YBr*)`$o%L^dZrPHLkyIbq*iy$vKD3E>g-@Xi8& zCQC>|RX61oawM%5F+^w=zh-nj&7dW7K|TRM{gO0DNV>e^e>-3hApIdM0u zB2gW^k^c%`n+dQdRBp_}QQCEUT)+a3N;#8nBCQfoD{9N&Pe|0^L)W!em4 zB2|Ic6B!Z03=!euNRS9mPm_Vf{4>Vng8A|mcd&BV*M~-j(-z4LDbc^mRJsRHi=K&e z;jnz)X>zt+*|<%(qqEQZhCi-6n0)wP-x0wIa?N87Dy2U;Me=!gJR(AfKyeTXL%>HM! zp?_I;Y?Mr5(uk-x1#1FD0DXQ)M-@T_$bO-x&rab24^&1&N^* zX?|0f`Zek*)9D-(JOoT}-uT~KOa;6>e~|`?SD#85OGGeWAwVEB@~BOX9~Fdqx67|A z{mC!*&pvC_=iM|?f*mG+Y(BpNwBZNcH=1)>kUZ(X+t=KwSXEv!2i8$~=nouJ5MHhV zi97w#|K@H$`)}B*cMp>8MbACp#AIIR1T3Qn8=*MVT))vb9!2wyvSh{CqdqIO`8KSx z?m?yI^>(O)3EN7bu;)-OAq~|t5$v^0VQZgFquaWTL|6VM|3}z2{7e!FAYb|hNO3*i zOA4*Kk)ls)Dh?@o{*{ew^+c++(E7jSxR|6)JI0e3)Z9RKU&1#Qn`otRs~$>A3E~A{ zvWRFu`a!+zb90HOCgbR3-)qg^a%8oh>+x`(@B_>mOjhf^Rxoa49Ib?|&cxGlFp7At zU-l)XcqcL&l?F2%V*$(pPNx67=V6_y)N9d&&sQy(q@RB)&XGIQwPIXL&W1buHS1;7 z%J(b_F-|b3fMp0PvHC@lOh=lP&JP7hB98vIS7aPYn~iarfYchN&?VoyApzkc-bPh! zkmCMe^8Rq@>*@d4b(CEx=SG)Q$-F2%X|~YFmS&*J8P~W`$pH~cUNx~u+?|qH$XAeX zFIetc(@dnozD24BV!AsyF)MC|zvN`ycx^a|n*;RsT#32^Tn?(!`1ft1xiW;OZVPK> zhQ@!qmRWV#X11=mszNo#N-bsc@~7u-;K6cwt&-xX&CK}L=^G?cJt53B;WA@?V11yq zMNt3MbP^mmxuYd&SZq`9$iAmWXBOEG4Yh={$|RA&{zm*?UaR3p@F9|?#bf}#Hu;lL zWap52<*l54E0X!4P&-+s&b2K#wrW{#+ieet?_|zxtNk#+zMtlNj*}F4WKzk`evjO< z-ZS1CJ3zn}s8e8SEL$Z9OS#3}kOYDv{iRkp8Ve);nRp#^h0j5#kw6MM+u#y0GXiUU7+yi8A&Mx@E9-<%C z;OnfBUoK%i@Ht)-aR>;KE`34C|MBe)!)?3a^FO~}O;63GK!Vc_RtE?;o^|Enrum+g zn*J!R=~{3QUhR0qy`NkYk>JzydWg8+Ijv@rOZKzIh_i7m8akSzglO*>(t`PGGd;oz zwGAf@rmmHG0)4L|alntPJe-_j7MIHtG>{GTJ~MKv5i#->80oAkc=;{5lAgg2pAZYu zQf);d82QWJ&QL?4wrzN5JA)J_FfVmW><8&LSraX#Das<+b16uaVT^0I$@Ladld3)o znm!9&p*3yQs0Tj}I5y;qU#B@VieIA!m%2FAO9B##Q#4nu<`plD49qy6s88x z5RTJf^AxMGMzR7Fw(z~@+7J~4!EDv_C7+$FfcBif>ZLxu14^%$4 zy-=~OY7waE(J0tdhRgSuC#liMA(5B)(wmpcl9m4X8_0&cFtJyn81XEv-+ zCqAryPQge)6osY{X5Qqwqg2k;`zy>Ed#qu59R`_N_COWw3gRrGR<;Ml}FCW?>Cu82m?U7 zd>hMJrN)%rd(s5oQZaQ7PNuP$MIpJwK)Y0pS8~+crS|;4VqEk52lsZN)C(0_cLQx< z<5N`aipemSM9uT%LmGJoOlP#{)WEda zpEbKkvFU(=%xXoPd>>mP8;i?M;e;$^Cu&Z))>NaVsNYpK8cX)oRkivp&Vcz-rTQd8 zCE9DMBdi`_`DqN4D28(5@}@>T3v!v-UVAVKitgI5zBD1}Lb)v%LGgG6QcF14-3%4G zUMe^5YybkpKn(^*Nc$w|{7Te{RX(?w23uG#2Fz9})L<$7=xM_g-f2YYd?#NWRjb^&>=yObBwT+JPe#C^!| z)U84Q#R2NS-a^5O!-EIW2)9^nbIM5mI@iKV7lf9Iks%;V_cNnhp=DB(-+Rt!*`o-%fYNM@ri)r3V>)Be=!mKz>1gA~)0 zWTDEYyGGup35-b8R^?Ug)2SRc4?Yq#5Fn#eIG05c5hDpSS>Edj&CuZrOjLb6$1K8_ z>P;;e`Gb~~DF4a&1~e{GCSFyP=l8vVX$`UbDBF&4?a#;{eFz7o#=V{=%O?rJ9cGM! z&^c2y?2xSF<-wlwKt*AQk%DxKixbP5wqmioR3_JxT(YazOTZtOMRV|GDm|Qb;OzW` zng%73v$K_}r`3s>_=PHYyd|m`6;PR(Q(AWC)i|u+i?C3VK_rCp+8Y*+ zHQ6;9x!ftg!6u@}qPe5OJPFg*%i0Zop2cv~bEy2{vj8b`86NGcSu@|I*tFZ7tb9@5 zln6cF*zXdmj@_^_!CfG!fxI5^2mLns!y5>-IFi5t1Gqq~7ZY9uPYB23jh-viAX+!9 zC;SnEKTDtw7ZWFz0ZwpG(-d$!{tH)4npfr9%@HiZDK?`t8q(70dY*kCkcYcTml11@ z{SMb7*Ti#)wWD-HXNbWdr#eodky0 zfY6q-46HX;v7xdb`j9`a1zDrOvscBSoIi=T_b1>TQHVNdguf=)aUNp6H4t~2l@Yh@ zbBOkk7_uL73|IEu2?M1H>v%r}SFI?*K zmn-K)nSrk9#|P*;Nu7__CXX&U>}N`a9hdL+RHlF`x2QMMoLFX8SxO{YcGNYy1r26^ z=r}%9RR7D1Xl2G>>AYAA+a>RE{xATnZX7J!uP86S!n8l3o92)HqRHX_&Yit!4e?G2 zkWRdl1XVH5MvF~o(lzoC(A<~c@O#QMTUkBXk@h`8+qW1)S8RGk=-05#i3LpxO|iDv zy2P_$9%j}xl1i`A5l`6$)%?hmJNy5t(#Q^W>Y8HXI6!-&0F}2IZ3d!!I?1V;J>%Z!4b&G&Lf|Ue4Vs z4Aw^whQ-7Ah!q#gLnP><|A~j7BKRohu~J1RZ>>ipY0q%_^iRzKNVu?@d55-rKy^XE zb6`A}sCe909yEAK-U$g?iYfWSgUI;!8G#C8FA{e3WWv#lF%Aaxq(j*`XUG)j;$8;1 z4XWzEZ34p&QQWKQKgnD`<+!QXHBVJ`gSJC92vgNERseD}TIPsLb$fB&y!g&wvX45g+lD9=bgnJcT%z9#qhv3-j zV{ULwxx4?hX;Wy4LAQDuX9f;OL)z-!=wlwITKRjt#1Z+=8j&$84%7bf_3Vza3haOwzZe; z$%97^y%2|QHiG~FqUp@Ii2$|h4XgdH`nTt8MI(eof7p6kGXG%dsQxSNhOKw~jy&wJ zt$?n0ma3i$vJO&Ld|A3zwfH0*q^VsYIN0(=h%eW-`?J2?&C!j+reyyZ+doS+<}^FK z?lJ4rux+IevIazAO(&3%AMjOI!?)r4D%^o6?&J{(qj;gfgw88~;_Z-41Gyqvg>Qkhgz|}^rDtzMV;{^beu=w#GLNR>K|Gjk^-S>!a;iG| z3vpr`j3caeM55Uf1G(L-8H;2@O&mZIN#da(Hq8gLttnLzeQYzn_FQ(hQaVW)w1p z;y>kylQ8UvXr{18rC4>YpI8s=xIe0mUG#z(*o=5rSTI(YuU8I)?fR12(7(fDy%5s& zG>iR^Vqc*$oj|8q;7en~qveFEUgs&q*T^i3^v_X}+}G%`kP{Kz#+KJeI7w-ch$-T4 zKdJ42-TF5XUpgu4$4as!-y z(z>CT2XGguGK^PD}pjD>m#xS%=v^J z^1xaHIHWp_8O>Gy`WEFj<8}1W_#>(n7Nb2&VjE|OJ-NRpv8gG7e|Kt+=6$aCI3R0;_{BiV5TO^vp(JTXyS4fJ7lb?w#%Su&Zv|)^?j4JS<$zGb2qbnldJ-b<&I+-XjR}({B7wI)70|kEtxhL$lR-kh9#G4@R>PGLDmcXr&&QYX{{aPZ|J;1g{mFA(}_- zT?~%9C-<@+15r=<*a}~=k;`KI*Y8XLeO4=NH&?I30Yg>+KUMGeEM41nz`Km*Yl_jg zQbq>-;o+FsuU)7OhT_!)CO5|UjBm_8LJM+8>-I1{QndGyPi|SS6Js+LVl^XI8Du`_ z?z|WuuIt6V)|0w&lMaECAuittL#L_ZJGrP)yr%Mr7Chz;@JiJ6=NnuLU6>b&Mfjl; z^EoI5tMaCIM{O}l=Dc(t@G?~4c;l|Hx}pZfIjQRa*&j2}zx$>RjWGpWuO>*-OXf5+ zf7YQL{gt%ix0}5g)(M~P&{+*m!rB`b1ibHvs}<{tnCO)y)!PBeOJN0SQJcX`6?YE9 zN}qMO4#lov?7wRf)<>{(RIyl&&aO0hBt!(Dz*9)+C^)4BE38*ab2 zg}L$!_5PxsKcOmm)|!ATnzzVdahO4DyqC&hiBK(@+8d&7jP6l;OXX8-I=Iz=8dp|h z5~3vNPl?|X2nVIj#7R=U?|OACt@T&Y{G%SHN-+~qyrXZd@fYW6zJcC@8J5r-9PeI5{R~WP))G6h_7d zA!21M@0A`(Q5+q~$|Y(({(GeOtTgK@@)gN#u+Yue<*#bTP5k*8!8$nBlyG!Ldwlzj z=g%VG>+^s-@Zq&KkS`cC?f?xfPlwBKU*rcCvwC3AEbw@i6gKIT*TPivX+f_ye}AHr z-gp}pR;ANpvDXpi4aZ4Ghwg-Ch`39;xlme1?`K*#GzW-Fs7y0sAD~Ubx4(IbGF>u` zOVM%IT#$J8t%@#im9z~En&(Q%v0t7NN%*bH!rps=ODgW*soit)i~n0Mn( zyweQ^0V#r*WDH_7>jsDH9RU_ykD-D`!ed1?N*a+dm5peQRnU|ZA2|js{SEuSZyXIMY8BSY8oJ}$EI(nG*_<~u{mp$s9zhpt)D~yu8 zOO-nI{>y%~CBT0_H2V1KVa||HZWg_U6d;3WVy$^H2;?sl{V7n`EU1b&ShDQEtMp6x zFO(B%8Bdy~kwpuNBbGle_7~bnRPy9!*hkdfZ_oK}(BqwL$3tT?<(GNH4*PvJW$cS6 z0g+7Brg-z7OYr`=F;Ala3YEXs6Sn;}CJU~RI#g`T=iI}WPARun6!^0^cE&r1kbq}B z0A+Elc^G5qdpb%*J9modgc;!!XO`yzbK1+kK5SMQEiEYD==_^PC?1HIV)Ql31_NIa z+x9x@m1`6+56-->BYYB~kN-w6h)R#|9*0sXVXA+XUG${4V)8B62<9pWjX*Ss{+s2$ zK>yl*QETC3<#fViA72(AOI}J;q+kwI#|AnjUjuz%rA3I1Ek%avmqreGyL^kjhjU}l z7lQw71*88wWf^0S+kXm)+`m%RPuq{DLRJsH7t{bZST2I(@pjIaP1l~A&Xdb6%UQq= zbeI0W%xdI|&RjUN@krSCnb}N)v+$^R*H2;Cw4n)e0!=2AezH=4?U3CspEL?dG%b}# zkOwv$^SCk`2Vs?MiY3&pRp*FMRCE5Ra=p@0!!B3S1B)yAYt9|0;T(X7qBQpo+ZB#Iyr{)v{6ba&lDC)eT%8C}7kU zH?*#f!cP)Ujxr;6?Y$ zi_a&~Ffq>g9<2)^Y&tvONv9=}td>Ri zv_M4oF`>akdA%c!i}i8AJ{|lnEI9NOyWh0G05R@?bt<@xDV|MvuZ0lv~L4 zd>WwGZiSx5!oIO@|7MRf?}@mFdz5qZbwZeb)!OcmhgE<7__>5`+9ijzWx?p<*EciC4>}7K znw)*nrno9W;*O;X2a@NK8d7fjzmdCiOIj-QAsE0YFfeP&d0H< zz5WsD4TnHp*#cki3?2x?fl?Yp`uZXG8o76A|5tiJAu7l1C41{6oBxE{@gwU3Rwt?JS>Tlt@#k8@vbdcRs@!!paHfl=ZN zn%js!DLBiNe*R02kvSC3L7L?eonBI5wMQ>`J6NmHnq1jU-k1?)R>jAZxmqN@TYI*< zl^algSS>lwExpx`4>EMeKf|z7u8|oyCp8bWN2kF)NjSkptV;y6u7NQ_?^y}ec`(+6D~F>&<>SP7w*to*_faM3aaVTc>X7(#dt9T;kF*tI%waeL zjre)Sai)ZDJeb_6PWqz=aps$5r?#^nD$@LGz6(z@;L{t=3dbU9M?=k8wfav zmK$CWN3KHbT;MBeO%$WjWH=4qbw9D=u5;&FnCB9u!bK}*sY!eJxw3Ul~u&_8V57}4_D_H+X=8h``UK9wQbwBZQE}DTidp6+qT_px3=xK_r9CFIMh3UYvn}Dk7B#j) zM@$7Kr$>E$xF*FTk}2Mlfc*}vZ4O)DAhpCOXo6|6dwY!h58JBRVV*H&+Tyc(-ByVI%myd<%f}t48z{XjI!^ zsf53X#{T%D2x)4V#JQ&#Y|jhTy2cG8bcLb90w(xtjdy+ z>}L$jH+15JVt>`?$d)NO@grTg2ntD-Q?aCS@G?$C>Ddod^9iHdqwUW(Xj~8{t~Dd0 z6a>+ThPfrI?LV_s8?rEbnKVwRb-f^t5PP{Wef{)-9OVsBhkepB+cY%|gjYOu!m-mmB&*_d6-xHpbt+b@q&6_-*gz(h+x%-M>fd5HB-02Jc^e7W! zL;(8iLwptXFv|vTceV%-r2PFaN}l%bO`8-ieoSI>Y>@G3SVgop0q}8C7?` zNR(GW7{(njV$V<%V8lHGYf`QDc3wx9DwLW@HL5@yt_9yaOX1}fMV~s91x>&7J_BES zc1n-*8yyw>7DoT9fa8re<$_mtu4=eo1*B1X4Rnm)z49z&=)lP%JaI8$>(fgT(lj0bhM)R7C{@(`Uc;5U5=C_c`yeTt9jPW3$o zFrR{D^P0$4x7v{%{ySM--90#r0j1!UTCb#;UO^h#&fXsXwZ;aL2OK4D`EC59P>M%o zx6D!e;yE<$?bTW;RPIGI!v9zql$4A2D zC3UtoW~)nQD~rdcv#qVIwWS3jGwa`9J4AZ>icoVqAiW~Lp{%5&!^S7y&4xwLY*@9q zqRK^2!-cTE$BOUO5sHSTLnL89Xy8`LF5%Sh%24$N5xd2w@O?ZSxyCLj%TQ{dQ-pvW z2|nG9y|BTMbXt{{S?cy&_r*6m*U`nvmf(139k>u1Z8OK5ip(BDq~+=zD*hgHV4&W9 zw8-!;VEXcpS`r&4_Tq1z$idJK3Y0%9c)0AuPN;=-Frh)_peiOCXpMQ}Pe)l9*>VZ~ zBDeT(zwqw%@Wh*Sc9EHbBS`$bEt~M+BU|8IGev|Xz3){gLFVLs3}&^#ZJOSR_x>o_ z8zz=wrkPQ)^e7qdPk_+3JD~;qwQlWYX=1{V1Tfz6l3=f;9rxl@hM^Mrf{CX(KgW=w ztL9yN%j$SsuUkE4JS7ngu7&`s+-!xo+_Q%;sJ$|WFj!a%Fa;>ASJb923Ib$Fv!O~jk7ug z8ckAA{~Xc^8XBxGznI_QVy-0*ddGOvu5&D)PP*XuHzo$ddcG^$aK_bZ+jtTN0$R<( z@L5tBY!F^1_FKwnCG4hF3eXU7O5G?o?bAh&(>>)O^p~-q0<>#dAbn@z$E9>fw8fh~ z2zUS@Ga&Bf7R_35p@ASa<`C+op``nfaH4IG)UxJg+3|WhS=+xSYR#yHCeEV5T$fIz ztoE0;S0^!atm$dD2;IQa3Uhw50(Pn|OgR~6D5t!FB>LHZ?fEYtU}(a2rBO>clo(!1 zPM>$)lQhWw&9vN&w;XwYN1}&K&GzS3k(>QYJtrgAcFt%p%$jDc?z~`z6H@Cj6y;iekBGRsGi@W#=(W|tV#rn;<3#h)M^Q{`q z+uKSnJ)u0o*_~~(`qX+?Jm49=l>f%XS)dx8KUIH^7H?Z#JZbJvStk2-Yh4LT<8u#EK^hd}k!Dkg8Q&gPSLHTwO~JPb|Vl6NwUjel^CDPhnA2Ou!#7>yb1egY7# z6hD-Y&K&h*BsJ+ImBJSM5oBe)TzeJcrw&a+3i$@c<36i<$Xd<$W^Q7a#sQ95x&0r!+pmmmzi}P_aTFml&&D=oQvE!SWWR ztG}I&oYgzezxM9s^#iAG)`9R1w!lV|a*cKJzTnQuAJspV9smQC4*jNa(DrJ#1z?$@ zm&~DTj1=}5X6}-sXkw-Oj2#vDOCSv3`$M-v!}g8*!wJjalJ#dIQ-^1n;Cc?%O zxc8me(piz^>CCD%M{;ID(%#>!X(MVvn#+x8`mTr2Gy3Thcs!5h4m8iOTgiY(mX3+{nb8E%RX2FD7@<;t~Glr->}0(ozdSAGtO+m zl^02ttRAh@!=G;K@1&)mpo?CikA9oN7(G7%9AT@(>>lJ1^BhTf*Vw4^oKC154U?U& zDmuY5!2lO4)ae+3Tws#1IP)zg?%7!Mk*)^YBUDcP0YI6NH#*8Eh6<7`wMa8t-Kn;yrzThd2K^G2!oU!G(yC14v}P>HU!;93-WP4+ zE*|5K?kV-9vK52HOO3j8Ctb1X7`PAz<*&`Gs8<1Q+VUC;Kh4xgmE-5ePJH-|0W?aa zof_i>0fb?roa>X4UN=-cL{;qQHHo21`K$&R?s=Jpn!8|!<-{-2LnvYp={ijvPmGTqFMbh!Pp zy{LQ3HSIjq1}U|Tb}`0U+31bVRY*@tr@S4r&j46zVp=R4IS)pt2VF!$Nu@W5c4~Vm z^?jgo8QxXa;H}}SC5mn)c3UKmu4hr?wQya@C2F_hkM;Bb5Mkuu@+tQ=^`^a)_qI9ux#v7wezQZ_DiR6HmuyI4@Xz`h-6z-qIW*J=fIM&g*V8&6V!>BZ4xncIe%^lc=RP;dovS`$ECQ&t>!-<34hY`dI(YFlBcOaK*5b zeIJF%ek;ImcQc4Hr#Kx2)D?C;;z^*sH7+ObTc)P!toCFy$s-%9`}~H*10{RR}}JA&5F-YYx9`qK9u*@MOA;}GvIn~=V0B2_(bAR z?G!pB7q`sWnffugMOm_lmkSSntl$zptR>*bKZPybgccsdTfPYvFeStFo8$;6wFrvARZ5EO*N^=8N#lmMcH>j@%&N_A zntg{O^rFL>a$eCT8bBrDLNrX?1JX?OZ3hCfMzoTZgV_q@47X?#jd>?x0^+#KVm3pe zZ!}$gFGAB(e13}gJjC#vxVEgFs@zpG7(0q`OLUD!Ua|UZ^B}VwYp52&%FlPVIAdThp%9kL|V|40^w?_2hWKjt?%frhB z4U9DGQCt1L6E=q}8*uVfY&6yMGwWj&8;nf@BV_MR1~4oEmnh$nTj5_}Vi6FkLWtfC zt2x_QfwsQwh)p7@dcfv@SqhC>f4ea%8xYjVReX4>r9JAuyCs zoH2V|96j2-qn+NA<>fQ?#7#c7^}mBtFG9Mq`AM$*PWsXnT63cN#{`~=>q~LmQFms{ ziJPI|yK?Y~4X;FnV}U0hxT0~XgCD`3{R&U_1vsLVIv5`V3v`|8u4kMS34d!y5cy{aveFg5X^uuPFWv;)5#bR7aA+UB`A@%hE|BEqBJDSPMa{02la za>;`q5UQ0cMhM%%;Ax!7h3qJw+J)>SWVnQ{M@SM#P|AjkpjWp;@QYgYmwVku=L7QO zKWzu+I$qCkO8N&{na}zpL=p@YYKNhN8Esk=#fI70|L$bCV(rQ$Qo~j;AA`IX) zxQs1`I3j)&N`GPfd(XM!d!vXWRWucV09a=)%0s)v zTm!=M&XqdnBk@SXT=}ypR9-;yp9q&fkT|`9%?rZcm25SN=1YB2LRE2WBug3~-l=d& z5z90d=S;mZeMPkTia<2o#ijG60v`FlwihYFE*rgIms|OSFk3Xdp1`hdpSkq&0pDQQ zcxpSq4fw9ce=cqjz=5h=)LEW`pf|NnP+Bmu2J|ILJTwA@1=rtD;0cB&!a2DT{T5FS zb=TeGt!kshzJq)24Ikyo;f>CPZM?{OQ)8*~v4!81ln~8Hq}Bw#oALu(keT(6oS-T` zq=Qe@pyUhc9tr|B`d?|@ZMGFf0A&lihR0y0>?r-a1A!v*4d7icDT^I zgA(>qXYUvl8jpLD#6OZhVE7@SG*k|_hj@!I$bx-nOi1`)FPtibK%{6A{$hQtd~eXz z!Ax1EHPMsW&~nXFX0#ob<=o;A2@(p8d{At49qsf&<}F7;kb4eyK~w~dfXueEgv!`~ zJbfW)zZ6jvsSkM->0I)&UAS$t$GOJ0>@dg=LKO(OfYK?%(o7(lO#Lud2Sb0{XHQh6M-1t)EmvR^*-ov_q1AC# z$!~_m!H4+mA2sV@9PrcKVsZ5)^LLL8+Lj{p55#HRb_=dA3iE5@l<%nTEp}VFUSi@B zQLE5xly=6V8K(sYvOV5+0YpK#;GUrXDe7k`Xc|aAeC|euxab2U zo+xBSS^?m3qWcMilEXlSQ$(5R)3f|(2}b5DEHs(vAH6rl)rT&jq!9}!>?~bD4W9o+fWk&CKfIY3;uvsqOloL=Gxp= zQ{&l{Cap7a@}ZWx`MNnKy4{o7ns z!j%>QqExkUEj88A=ApKJON;r5n*rCvNBr0Zi+YpNlgnCvauV#f)pV* zJDmOXx&qYfZ)F-BUtaq9np5jUoUPgH=?r!4#9G{1tMnna0Le0zqCJR%98amCsrHIJ zbcj0UE4J?1J^1c_5j5R3fAQdN_W9s%4nV;PtY4uBY+i#q1V$`i_}6a1dsS~!LhJ4T zF`*7lDuzA_Q4d^O92Q5;s~hB0p3ymfI<)%~^QvXL`fJr5=;z;m>j}5@mHTdRIDdZM ztBMSzbTWKcnL<6Pc9*2wtWDa-4Zlv-%t-=NPk;t%6`=?gERH5~;V#Be1KqZ)0%F&D z#$Ke+s1*WRdQlU>o=2!-qM5}GH@TMpMaWXv9^1bHx-cg?&qDm|Nd^}*$`ufhKPU#dj1j3&e)0$vBJ)}zK2!q#3$;i9|8c%14 zp3Scq=I(V={99AgEF{2>{Vir&adWiw?jC8Jx7p6zB2uz!b>0+hN<5hwhj6vVaEM{A z%{^Bn4sny_WeI+L5Y7jlzkB141J%@oq>MV(FPb8#XKp@Tp%oZVppdSJbupd&Ub_nXaq~mYrPgXy|+)*Tmgkfymenw?1 zA&8UH73sw-me3ofSOTViJjMuv2ovPAz{?S2vJO1Xz#<|18;pCbp?}HEY-pr^)HwO% zA7{cpqhMjs!1(|sLjqU=CGcJ#izK(Eehg+`6^s``ejV~Fcf9z$dJUf1<{-l@ZWV==HLKdq zZWqEezna+sl*MeSR$HxW{#;tyy!gFow^;Z7bll8{Lj-@H$8Ept=*{v?{m{O|&h>qi zP=s41v@Xbyb!%rrSmE?6PsnlC0i2Nf?u*lOebwcT#P1F2L$N}#1(3T83SaPO7b5Irj*hZaSP0TJiE4Phqw zu`Yu{LHaubJbc|#GErWV?7gBVGJL)nNFCcl8lDxi=Y7n1`Ufv3OAN1|i@Ha9RV5!d zhz2w+0;hWy_ix@ibOaodE=6H4o@WNWNwXY26>_hy8~{mg`-HagZol=ZwtG8$m^Lx&Pu&-u->q8%yII ze~!RK3BP?}9Hi;WiRpe2zQ5#2n4ACbP@K1CUo`&hA`n17lfl!A8K87BcF1*EB80#2 zmY?Px@s2q0AhT$@@>ZYLHytPQ5S&JTLWD?gcblZ|AK8~UrtqKv2+6DSdd2qQr^(_i zdsypf)T~>!^v7*cCg;?6f$uc8+|@r z$zo@(bLcV@`5J8j$coWnLb!uj3kNtF$Vm`mz`d+6#n^;H^*9>45VBf&ze37-k8Qrg zV$kV@wmp+0S)Cj1n?wG~Av{D7dw-wCT53*}tgb6%z&M4@VB;|fuw0H_X)YIve|i*k z4;4ueL|mHIMa}wkrOCPi*j~ZIiH7t@w+SR_>h0Q! z9@7Ec`@LU7ju}#FLJ!3CGHJ+}t~uiBzu|Pq-Aj5i^ZYp@I~yt)H^Ev!hQ+=G0oggd zJ_-ae!kcg{Xz4Q`a~rMkMOSdqg`jC+|4p3D!s#atIsIkSl)@I3L!#n zU^2;_6%h59TKFR?v!jybGFCl^(8;)f6abaSHdn$WQ-#P>jEg8>sRTgI1{hh3;O1Cq$^w+D?c|ZgH6Gq& z2a|jN)A1RM=s7V7mQfu;VDZKv!Ho^7xO{;WBnfN{ghQHihN!9(W<0lwtY^dDMaQ+% zJ0@y5vjVGc6i9U(C>QK&;U%$T`Y3?Gb+C}rm;lYZjQVE1ct*%VDN`*yvGhegEMd=a z#@Fnkk!drDP}KIWkg(DqC{#@NjDfF>i8>Qdx*Wc?=A zB}Jvn_Hg0(pHDHdegs0$cDnx+GZoz2MF!UW6^2L=|KZ(LmI!z{;Bi_ZKkE zuF+^m-AebljGy;xX`LAO0s;Dt&fT%D)48j6R3ni&YFWeA&a|0<#@>^aQBt4oLJuXKv z&6$5?3m0Nz-4N}43J&ss_JK_l0s_PcXtTdqayqnORv-*N@Q!YP(He*GiZUzi_f_oh z7KrgRFHLHBHVe78VIe+&Y74~W;vYnJ6LLqDZoBFVw_I?Wa-gs>Z293(WhE8>4aImk zb;Do%)VN=Y#*<~FNMs$Fc?%31O68SsOk6?sDxQx0vL#Moo4ZuQ4Sy3&ar1ank$LLq zC*`coMjABJJYOrCk~TqV3^1te8JR)FN9gnwex)`l&DsY7_%e@pta!e@zai zb2aFe;AYDRupialGld)$bs+dA`@p`ED!a`VE+F`1Y$$~E3T($b6H^w`InzQ$pU(`hQ3j_C zhg*Tyyv0v~?M;H{k0IorSV7A514vA3=@zK3y5eqj$G;sbPAS*$-)*PY@CnCC{-~}iyg_6Z=}ycFf%U3|t4 z+}u+Zy0+9N6g1pk3}DXB9Z@UC+&r;~Ch}mVV#IHkSs<78)tXl_ zpJsRFRdVO`og0p=LA(!@3I#UwE$B%(!fWi_(l;UhMeN4Vn6SGY@=1Vm&maq{1#ed} z)6{z*?D}gH&aM| zX-r5is?zx3QVnwCrJW(vSFnq->06WC0cc9r9QOvvjv=XWfp_Om&$R zG=%c}WAxr7%W|mz22r?={hc!)3UIU11YcYKEwDf!na>3HdT_=cIesf@fWjaT{X{a7 zd~P(BOTn|LymC5SytjXK@gOCS3~KQ)jSxTrwZTqLzPdd6-qAD#s+1arI4}mQqOHal z_{<=yCrB}_>0{N$kq_Krpg_fL>{KzoY_(a^HbaWPTdN9mp8j2{hNWGiZf3cnEw?DA z##kLU0wMxX{>I(}amozQ?if%3JKw1_Tt{dvH;Gm2GTUR*`t0ibfF!0I#+^!|XKNOt zcDIUxw~Gj!GXpU)h@~Eg)!KBvV$J9yj+#?di>N2!Mk*^e%k=u(S6n-Xvnz4$ET~Bw z*T_W>Ew>H$HnqBlE49&y@M`fFjg{=?jz&>hn`Hzvv$S~Y>DXDqYZn!;S=lglFHuv% zU(wpt6tLM`C_*Jq>KmWv(W_Oap!sEY87JnhEpdIf zVUYwdbyvZgfu>R-aSrp!4ze+4o~%}c9V6tLvd2beTpHFjtKt^QZ*)#!MmA}~- z#7v$Mct4*07vo{t`PcON>$_^IrLc%o%_0}A=`4$#;kbnUw|ws_pE#8-^GF?!VP40% zi#`~o4AENuHD@fDHBNuzU-Aq5Vd-4wNIq%6WwcLngc+BE{PsXQSvLi;)F^9PJ$2bs zCGHxKNkws7tJZ^EX)H${DW+eCdob275aPO#tXPK&D4$86-Bd$AC+}207m6PIj$Iq2 zGFJ@`N|Z4dZ3LMT2c9R-{IzW~SmNhYJhu`z)>FT+?ufJ@{o*-cZJYZY5!{Mi8hmH~ zJh?0|OFC?+uAj2jv7Z<-*mwq(n{`$P?DsWXl|-wY3ZZ187#0 zZSVH6nJDvwpLESz#Q5eTnXZ(Ui@h)4p!g33j5z6#{?ZgqETkkKmIF>WdFWxKwwck> zuMw=qEqSnGx3hywX0F-XTon}xe_M}#Aq|332<3q2cB-WlY4IwO1TbM?!p%tU7Lowe zkaCAW)e*XY0Y;ee^-e)r`1>OJ8Rozex>;vri;?sV3i`;cZmbfW&vKyn<-=7#HSG!# zbjZA&-pEmgF&>Xx>QOuKbQ0_c+bWw8QN2qu)Z2ikN#kc2Jt$wl5isi}5|qSX`K(AP zhdlL^(?tohk?tc!v_++WUr5;v92lz2aMTzL$An}_5=*1PbI*P9HiuN>?qrhiZo14w zbRar<*nOvyJGqp<#TZhR1{yX%b~Sy&>;~!l4VVVXwmuF?wk%gMSMtsTTeh)PH{JeR z@@prHNwEyKo2(I)i-oX&yF=nQJKM)IkwPT+MYb2`9kc-fXijunnb0K^Emm5Yaia8K zYEa{O<^W7|e^qkY7A6C9o~(5;E!-Ahz9wkhv1rbTlnB#KLb{d;o~R2*e5y~`-Uh|g zN_*^OB^=!*UNt=@x@f%Dymh`3MPs*OPf?^mK07S(In_7rL^Qzp6L+Iv?whvvq4IdP z{9;iqN#A{|PwJt6{zk}GoQt?b;)!8$UDQl)1?=0Cr+YZ;V*dukHHDZ|)ikarITsWE zjpa2-gJ0BrKRGt8qyGfJIwDZy@x{MV{TCq^n<8UHoCATcJ}+BYqEa5)`#Zroirg;& zpG4VV5VeY9Pg=!cFb%Y4h&2$uz@OlVYEp(Kbi$JEhxq8gOjl=xF{aL~Fh}u1xNPi% zTNXU$h(B#kON&W3WJvXq6#Yh_fdg6?xklKBJf_UMz8!~_gL)O9uJx-}Q%4%|3@P1l z0puM84B`o>Jh!WO;LmOx+>1^+?Y3QqjQ}mV&b@As2F7FW-~T5A<>S@4far`S04LHbxh4 zubmPn?vRWJJd67+(_6|J;y8ISZuARQg%*Z#=wWU-fDS}yAGoPG&aIgD#62i~;LPxE z1*rQ?gr#mkv3USDfvUD@>hi^>_K6Yobv16Ovk#0uO=GDlNGXUs)*GW*fU;YeW}A61 z#&W0MwUM@F)s#ts!mhzZ!w#>6bxyy#{$jq2VD&j69eNgh6BOdo{WNx2fyR6jG27$^ zkrt47v~VJ&EV%;av=?-7OkqkHc%XLw_WgI&-v|xBl9u9)cPB!XhsjrrX45Yk`{0Zh z!xcHkyBSvwKd4xzC}wvaIG!|i8WQdXZzeyT4hf)!zh>_UaPHXI*IeFWVu2LOK%Rr{ z9A2oDmYOc%y|B~>X6r}AF~%l(*!LmDbaXE25oCQFFg#o=VI>;TZ+`Cqc;fnCOdHR- zN0u#skJIc{*^!!N-y_$C4Qkeu=Z}UJIHc?ZoN8`oLFm5_ByO$T79eS&1Y)~-r#+zU zUs!|t`kSI2ix}^o)Rf+Pm~#t==$mtLR7cy%8rfC8LMoNxBsE%Poj-7N=~=d;)DF zzZ5ok_6O_E9;4clP5X-1>_B^sElTL`nj@Rb)Yfxb$&aiOf6Z>k`>i&gb@sxi`sjN`zGUdHQ3dzvFuPA>rR4F@7`JiHyP#T9mrzXh`qS zzo*!B)AJP12yMQ#z*rHNg(2m(0#{-ujFEWExSzM{fb$k+MQS4`+e{f*Ux?D{@1AR_ zX$l%VFLPU1zQorl%eES=M0ZL2DDAlbxXs~~xIV-Iu!pTTBt@%a4E$@z;mZr=G=&&v zqiXw=-uwQQRK*LGC~NyP2#ex0Kz7bMGH?7A1A#`H-6JQ-wOl&2PItWvX{JSsUxxM| zST$+|#wFg`)|=6T!TC@eXpF!WT}Tr?FxlP^+n)4jiV*0u;?KGWSj+of7WcpLkSL@|_Z@OZD(I zP5wvx(lh8(F0&J&0Gz7?XqOu>6F6~9?A0C7oRq<^Q`}~|`wJX)P_*uz Ac-xW1# z){S32Cnt6?HmLcD&~7U(;X$!yC#c{BkMEFE^u#7L>^&RuEYO0<>O(2Xl(1glD6OoL z_6a5Un|;_18~O#E@V@IdkAM*72PxbyrFDo#EJubGK}GVag->PY8`KS8o!^+V6B@@| z?~c(^>YXo)tj{R+~MkU2Ovf)*aa=>{ltsx;eDzwsK!V742EyP;Rak)pM zcVsKl+m%d1OrlI>-%g^es!%kSA{f?;v0O+$fTQ<38@z zBZq4ECfhWH7|_~e90-GYg(CHj(j?6IxZB*)eye_RN`L?B`(>A6yhxEMMWne;@AOBZ z$!`|rh4|3VFFnv_qWu0W>{K|WRrert?W>vEZr{3ILa+p({b&fF2+lYpie&*~dMEN5 z-LMB=wr=PH!o5tk2c(w-zo6mD@+$ z$#VWGF_I6ZawAC2>iE~8@j$_-{^t<|OQJcMSYa9R06of9)vF$waK616SZRR#%K_T8 zE1DQkP$8ut5Unm?n@N~SX^nBjMvs4uk?eU9r^*rMGWiCNq*-M}KFkWUna5O30hY&x zZ*JZ+Pv14-h=pHaj8QL=_qmhut+CaQ^$)QRgg&gzo}>ocQp)1pcWb4^cwPn@e|#-y zb+r@FVsbuZCw?v2+}5e{uXG)!UP7o^5l6(hB4d07GF?GHR7bpZ2b4FxIAglxmKTcN zaMGFd^FqsI*@YL*pZ=vYjPBi0mQ(j!DUUna&Nz#uGA{(bkP~Vhaib?X)tON0qE;2E zxFH1Y!oP62K;_=QC*0}#?d3(+n>wIAI!+;`Qzi$c+J;K!i~v$93T2MB&CU*?C)Xz^ zM+jt(P@LU>wN`IbFT)$UGZdVkL2md{UhI^#SZxBNj0pY+-`Qy?(JH0V>ZP-LC;xmS znBf-V!;N*(?%MX#${^RLle0{t&eoE`)1V>O99+qorc8~}TVAwVGwFA!Rg^3TQ0?5x zZOtTt4ZA@FVRdLbH}uIgj6EkmnH$gJzV2y&snt^twJjEtwj!ll?NEs-yO=_jZ z5$d&OS&XEOdgx2;xz+F*ly5klyG6vfKfFe>2n#2EQVqwF%a*yqEa!|i|3uh22Q9O5 zz&IlU7VxF0OS}J&mTb?UP&qv#mA%djb&AE}*uT9p5=Xuc*9iVRgq^Xs&q{FnT_bn; z1@miHrD;qQ^Z5R&CzJIxx#7nN3kvJr``h5~h;ZpyhKAyGzdt1sasf8$Gt&MVRcV1g z>#ecpg|1hWJNsyrs8GhsA4KlR_vXolv=u%CkVMTHuqlt2D{SeGZFPoya{k3@%!goR z#~OY@zayCD%)?t8R6F?K&FgR&QvoVY<#vn95B{zd8_t0_=^9j(p*vYNNIiBH7DjQMAS1{DpJ{2WeQwRQIf2w=j9W|<<&Lxc$dcDc< zPVu_|FTlc~6G^Rv!-0syp+tB`eCf@1_zTvO-eFGiqJ%0!26(ZLCPK!GiIv34FSRlo zm$H%K0Yz{*ahdSS&berCz$$%77Sq!cK*iAHR|Q>l=vr9SltkiJv9GS9N`vvail%Sh zl>cDWy6EN!Lk$~WeZ;MUc(T!wh)G&?SY3cOF13O(omF@)r}Pw9>8ADfDCOBK05d;n zDp0j>G`9*!SSDJV=efOv|6uz=t6(%|bI$JOU#%#0+seT(&9UQOAPo>3?*!2r=pXzO z^=yNOZ-OSh(OfY2{JmBluz%z#4Z?po^Z#h@_){1Fz^@f_J`*~UsRQs4srm-g5$Fcm z2@EOdWX@vI)(CW3o+t4fpjkhH zlL>a`00xI^AD3OelU$FJ*^iep0)M!_ocu5cSnAry5(!}|jHy8xAjQ#uf3>EXQtWagX!Sprml<~z5l0~%5gSJn z8JENN+n=`P@72G@m(AWPv#BSvnb`ihM)%8qKQrmE&}lVc93|F3opK8BxY!%p_V!j4 zS&oM!HX2fo7VDcMazs~_;j7BPgq&7ly_=Ca#8g4VbUNt?-X>R8tXcs>nr!v7&4pqB zz+cB6LBy`ImD$WT=}*v1^k-AhN~b#LCqpM)92OjEDw7ZUlkL$|=$n?=L~2#hNZj;W z)#u`&%rZCY=PUZjMp%f+_hH#}TVU65mak7+e$KUXP1GDEFkC<&93 zZ>7}}=qbr5R+92xz6V6#rTN zzJ3O&jO1X0JKKLfRky!d{i8ep;J82cDRWHn5wAJHy9a#8fcW^4W*`Ii&*1QX{Zgpk zw0jJ%Rl$8mdV+03wX#eqRxRlZv?b%qI~HL|pE*`vBK^7KW`y}|4cs<1soLtT-Iu?X zu9S%?&(vL0D%mTo(YGQy-B<=21oE3F+9N81aqOjDDa!Or+D}hPNLbv>%B3T|AFlc^9x7hym``{pff)G#@iWOkV?TnpDu$!Vss?QEgv)xA^fGVij*CT zNMVhn@Xmpxy{}ONU<>A$Z&eKvZF;8WCeC4fe6=bstP0(dhhX<3+46m{eQ>KbHBoT{ z{UgH{kZTC6|s75dx1o(1b~;)PtuK*PHHh?iQ+B;=}VqviJnUy9vy5%r$ZT zoqfLVkU|;KnseQ)IP%on9TZe1VaQcog5Qalpm6J+C~__S1{h~JR&Ol zLsu3lR3Rb@fK%bsj&xy-NyY~;i7(8HA}fLW1DTfd5_1AUcswF_X%Ju%_hhy?LAI6? zzO6N~@bR%DMA(pf`=rJ+j`P-UAI z;|9^=i57<}4&>8tiIyZvfa!9_rK?T!iHVGymX4su!^=IVf`xIdN-P|l=s<+_14MTb zG4AMhtaGCBFiFKMY<9S;YW?8SevyP1%z&~^0`^Ubw_xIGn1(y}p_|RUr!u~V7|!Y1 zyz~)>s*(-UyPr()1wl1)VEIMxA3Qs0@&zYJH669dZSe{W9y1_K3*%ori{igpV!Hoc zo4v1)NzrTQ06mo@LA200VXIA)Q;#<^WVFqEQ6WX(sCkSUbw}-fY=`vZQ50KLaw)UX z-NTUCb*E8Sz;A)cJ6n|eKlT>gToz3y-8cLjOOJEA27SPW;LpNHp%fsz@cm7M(SxBt zS-|UsQzxvZR_hpl!Do0_FBjuc2^jWb3+T)j$l?Fxwl^A8|I)$dD^HgCMP$&OIAn;eHZ9;DLS2fB<_Y~hCW@( z+0=^6Pzl9AzUk!FE@=Y>T#wU#GgvGuqS0G*cJ5lMt34=Iu;UUQs9NFDl#1u|$mRK! zYJoA60fwrl+*B&qRNod=>FHHf{BMr43joXKbSeeEUlH~?txvjYNfKx4F;QCAOD#LQ z!Wu`hX0{oRT%g_&iN@5fJJjOsL_iGc`6sKR^qX%Uqa)D1?1H&NvoSvZc7 z=`o^YLmo8}4u%pHlUzI=`T1-iI-hYwtUNidfcLoVf;*6)>j!chUI8lca2}kJVkqn8 z1st0V_v1nc*TEdH@~b)J8TQhAff0{DZ2e6#{$_`o<^Ee8r0mMH7`k^8J4F|r^mi;B zh#(hP_y>x+%+UkK&!V2VGuQE#ITLDe?!#;n)6!=ABj~WhTByH;OQ{I2D_yPxiAMIw zHSi`KU5iln05n&ZOL>L|uCjgFiJ-<~CE#@6#V`PL-$V$boiHl?IN((i37SpDPw=$* zbO4(^yg!iEMTnID-&~z<-hwDO2&%Oo7+p_zp&S3<8;^`(3d)w{Czyyo4oWZi8+^i9 zDD{mH7{e5kt%IMC3Q_bp5KJqckA7T)UosxtD<+e}PjHksUV^hi;!={=aPQHqC6WEodO1YIGnV%Kz;kbU=!Rmm%5_ z`j1>=)&^XXAv-A&RHt#WnFmp%5)#r8lo^{w6Ev$~mOOisscBQw?5wk8&4{`U;+YiP zb8F2qhK?;!L8&xiWKGY_KTGIGY0Hft%jK*+gv`(STkK2kZrAS1Rnm{wqZ1I#odY&Q z!rda0erY}oLz+`vARz)}Jm3~)$Eze-BjnZ^yH3dwQ@<)70}_3D02sSYg%K0dI^u!< zyE|7P?Dg;0rx&QoF4ka{r!UJ*cZh`p{GJ^ze}7(E*ewG7?!+Oampf;$$K5LuU zzWcBJV|Qz9Lx7Q#Xxpu+RmWz|a^vnf*$#W-t_9_sJ!7N0#_Y;YyEI=$Qxl@zNu+Wq zNKyWBi?M!1L|0K<7GU>>vHQNVDoiEl56bb4z*Wx|2Ld8# z7mp7lBrhqMMF!WOMtDAkf{lhs!(SQCz5D!H>IVCp#-`)V{{om8x4h(V*{oQ2%%lIc z9GGbzTpgscXD)2LXllpKwm5i<$_17gTD2OPpD7-sTP0*?Jpr0~HtJ7pv_D8)MW+Vx z5E@+S{!dls9n{3uhH;1>%|Z=rks2TrDFPzBNJ~Hkg4A4;-ix4u1Owb4Ua8)ti4Zzi zXcEK#mm(q{BGMxQN|z3iq97lBdo#}Gm)(Efv-3OWJ-f3zv*(>T&qF-juL9!;P&Kn4 zHDFCIZy+sG7Dr<=+8ggQ_10yMl{p@*p0uhT&p;oF>$h$sq*tSe@oA0gREl)1R z0QHp%Qe!#^6JL)0={H-u7%-o?xAe4;uc_M1J$*#)OCP^4Ms$e%0m-Uzj0M@CVBveA z)xQtjd&2dcIJ&je0DC39;D$Ntx>uyCo|%Q2W3bbW7x&-u_`eZEp-S*%+ece+J)X>o zX*F|re)xmMN)NUH_S@bK6WA)*9V0$%Y9#lX{l4W77(U2`YJ&Sq==Ph1L!(g*=^Zx_ zlsQ?;1T}SE#YVbzd+=be`;o=l3SYDBW{i{kqcW63v*bi41}Erx)z`JOFPvCfI`d%B zejxc2U-P1^%F9MS0c$)}9hXN)y0sipoJq069r=t2l@GF~DrRCj2g=8Ik`Gtzxtt6TQmKgPue z-n}h4E0YwFa4&zx8`Gv&nW^*78wC&3c>ab~lilx{6RI-tuk7(PGWaUD+-NEX`~ZD` zdR-m4E1YyGOIUHGO%RTMc&{mRe9Yt8xZ-5br-`p>R&c5_U+b$4yMx#>h(sVi;}2CfBsTprdM8`tYR#hEa2$c)F;jD z*(Xtb<8id3^EiGb)Ta32O{K8KHiGARXiz(Ma5c+xTu)DC$X^>)j-xjp{kUwl=PaqJ zqm((!uC3ujWk>;=E6ulG=qzQth`%1#5}f0B zr>AH$s!J1}2evd&E`3CM1xDw_6BeImmsh#x7qkCO&OSW#u2BCDyHkx|UfoBdr*h%_ z`+~$BoKx^l1^O}Qn<)~!$tgtjeuC6 zp;MsI@pmzqg676&9tquxe_GC!Z5ZMsq&}3E%1#c%Z(kS4a?_5s`@%^?6!@!Pn1BQY zpT9ov8hvwrdCV`N9W|xWEBGX(^nyX2=_IsCRX)pF(PMaswS!GE_HxB$H;U}F%K49t z{PFN_+D4^-2y1`e;}2@fY!Rm!T|}55eDUNCs`pjK-2RD~ z#+;cJ_AGbx`B8P=B=zhLr`0YkpyN3qRJX7K-}Q=|a`7bx zu71sUc^_}Y{4j5oj*wEkmV(5n#a(CF4rgR>FsG1)wnNn)hAsp(Qwyb0X{M!PncW_E z(uJoCYbjjcBCNrO9jDIwv`?oXU3Qq#9mwuN_QCltpNOkqb(8(&&QxV#7L6tO%#pLF zZ`g&LsTz>9oqmxkzojQpi%9gY@w%&1Q?E_TZr9q!MhT!!roJ_~qFmyac#;wl_>JG&K$?~J`#5~&R@Rdi`vbFG zmG3juYOjmSh*0cDzuoORB2GENZ8v`{U+RJOG!~ z5;F|HrQdk8B8;RC$xkuL&7p)9F2e8J<2H^UE>{Q7dgEpsjGWx zPr0*ilC#fppWULoo~e|ni52Q*3(&D)#64vpSFyU-GgV1W@srzrV5eAo_;^iFrG|Ww zBRK}>DK$SQm>EwwmnDG3u|5xarfJt>{TL&HHn-r+F=FpS^eV4QQU2lN2;ui!nmpgy z#ZypUbUM%k`_V-*@`g;_9D`yavIrf--iW;> z+h#Sts%tZy6{TC~uedz8yeu*OI_@X&Ck(8a2S~Q_x0#q0zx%d=BTJr4=KfhQu*-7=9lmv16*>w5%j3bVeA| zTjd1)wZeP?fgKR2`8uAJ+!gu3>~jE+#lSe3M*VfGs1X}{Qj+{ z(=DY-XG-h9@=L}Ptij-wK|ZkWD=!%TRTXwJ^jGsd%>4!}OufYkp4;LC z=yVq8N51(BWCNq35TM`=CqR=>gVyJ_p+}=e2UDW{2eZTofb84QTk`*cO?2>E{4bal zwB1&Qefj%;&0wff4kZG+w}oI7kal@*8wLB6Lks^X5CZv^I6=aW5UeN!a^j3uDKu0BAwYlDvKEQ2@ze6gzx8*8JbIo#EN#lND3r`{e19lpr|boG3? z?!K>ofjqQ{4}TMaJ?jNGzZn5^#SFSWF9*O7iV_(8ofDv2uhBsgBcRy09Q+cR8T!f~ Q>rd#Eb%7 diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 0c85a1f7519..3fa8f862f75 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,7 @@ distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.1-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.4-bin.zip networkTimeout=10000 +validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists diff --git a/gradlew b/gradlew index aeb74cbb43e..1aa94a42690 100755 --- a/gradlew +++ b/gradlew @@ -83,7 +83,8 @@ done # This is normally unused # shellcheck disable=SC2034 APP_BASE_NAME=${0##*/} -APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit # Use the maximum available, or set MAX_FD != -1 to use that value. MAX_FD=maximum @@ -130,10 +131,13 @@ location of your Java installation." fi else JAVACMD=java - which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + if ! command -v java >/dev/null 2>&1 + then + die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. Please set the JAVA_HOME variable in your environment to match the location of your Java installation." + fi fi # Increase the maximum file descriptors if we can. @@ -141,7 +145,7 @@ if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then case $MAX_FD in #( max*) # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked. - # shellcheck disable=SC3045 + # shellcheck disable=SC2039,SC3045 MAX_FD=$( ulimit -H -n ) || warn "Could not query maximum file descriptor limit" esac @@ -149,7 +153,7 @@ if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then '' | soft) :;; #( *) # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked. - # shellcheck disable=SC3045 + # shellcheck disable=SC2039,SC3045 ulimit -n "$MAX_FD" || warn "Could not set maximum file descriptor limit to $MAX_FD" esac @@ -198,11 +202,11 @@ fi # Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' -# Collect all arguments for the java command; -# * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of -# shell script including quotes and variable substitutions, so put them in -# double quotes to make sure that they get re-expanded; and -# * put everything else in single quotes, so that it's not re-expanded. +# Collect all arguments for the java command: +# * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments, +# and any embedded shellness will be escaped. +# * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be +# treated as '${Hostname}' itself on the command line. set -- \ "-Dorg.gradle.appname=$APP_BASE_NAME" \ diff --git a/myRobot/build.gradle b/myRobot/build.gradle index 4273eab9134..17407cd72a2 100644 --- a/myRobot/build.gradle +++ b/myRobot/build.gradle @@ -31,7 +31,9 @@ ext { apply from: "${rootDir}/shared/opencv.gradle" -mainClassName = 'frc.robot.Main' +application { + mainClass = 'frc.robot.Main' +} apply plugin: 'com.github.johnrengelman.shadow' diff --git a/wpilibjIntegrationTests/build.gradle b/wpilibjIntegrationTests/build.gradle index 999d209036b..b79337aff8c 100644 --- a/wpilibjIntegrationTests/build.gradle +++ b/wpilibjIntegrationTests/build.gradle @@ -11,7 +11,9 @@ ext { apply from: "${rootDir}/shared/opencv.gradle" -mainClassName = 'edu.wpi.first.wpilibj.test.AntJunitLauncher' +application { + mainClass = 'edu.wpi.first.wpilibj.test.AntJunitLauncher' +} apply plugin: 'com.github.johnrengelman.shadow' From 6cc4924282c0e74e1266a715f337ab71f1852c95 Mon Sep 17 00:00:00 2001 From: Gold856 <117957790+Gold856@users.noreply.github.com> Date: Thu, 5 Oct 2023 00:15:34 -0400 Subject: [PATCH 33/76] [wpilib] SendableChooser: implement Sendable instead of NTSendable (#5718) --- .../smartdashboard/SendableChooserBase.cpp | 4 --- .../frc/smartdashboard/SendableChooser.h | 2 +- .../frc/smartdashboard/SendableChooser.inc | 16 ++------- .../frc/smartdashboard/SendableChooserBase.h | 9 ++--- .../smartdashboard/SendableChooserTest.cpp | 1 + .../smartdashboard/SendableChooser.java | 36 +++---------------- 6 files changed, 12 insertions(+), 56 deletions(-) diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SendableChooserBase.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SendableChooserBase.cpp index 9fa5b69428e..f19d9e4612a 100644 --- a/wpilibc/src/main/native/cpp/smartdashboard/SendableChooserBase.cpp +++ b/wpilibc/src/main/native/cpp/smartdashboard/SendableChooserBase.cpp @@ -19,8 +19,6 @@ SendableChooserBase::SendableChooserBase(SendableChooserBase&& oth) m_defaultChoice(std::move(oth.m_defaultChoice)), m_selected(std::move(oth.m_selected)), m_haveSelected(std::move(oth.m_haveSelected)), - m_instancePubs(std::move(oth.m_instancePubs)), - m_activePubs(std::move(oth.m_activePubs)), m_instance(std::move(oth.m_instance)) {} SendableChooserBase& SendableChooserBase::operator=(SendableChooserBase&& oth) { @@ -29,8 +27,6 @@ SendableChooserBase& SendableChooserBase::operator=(SendableChooserBase&& oth) { m_defaultChoice = std::move(oth.m_defaultChoice); m_selected = std::move(oth.m_selected); m_haveSelected = std::move(oth.m_haveSelected); - m_instancePubs = std::move(oth.m_instancePubs); - m_activePubs = std::move(oth.m_activePubs); m_instance = std::move(oth.m_instance); return *this; } diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.h b/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.h index 42e9806a1a3..f06c2524db6 100644 --- a/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.h +++ b/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.h @@ -91,7 +91,7 @@ class SendableChooser : public SendableChooserBase { */ void OnChange(std::function); - void InitSendable(nt::NTSendableBuilder& builder) override; + void InitSendable(wpi::SendableBuilder& builder) override; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc b/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc index 18f983be6a2..e40befdb74b 100644 --- a/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc +++ b/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc @@ -12,7 +12,7 @@ #include #include -#include +#include #include "frc/smartdashboard/SendableChooser.h" @@ -58,16 +58,9 @@ void SendableChooser::OnChange(std::function listener) { template requires std::copy_constructible && std::default_initializable -void SendableChooser::InitSendable(nt::NTSendableBuilder& builder) { +void SendableChooser::InitSendable(wpi::SendableBuilder& builder) { builder.SetSmartDashboardType("String Chooser"); - { - std::scoped_lock lock(m_mutex); - m_instancePubs.emplace_back( - nt::IntegerTopic{builder.GetTopic(kInstance)}.Publish()); - m_instancePubs.back().Set(m_instance); - m_activePubs.emplace_back( - nt::StringTopic{builder.GetTopic(kActive)}.Publish()); - } + builder.PublishConstInteger(kInstance, m_instance); builder.AddStringArrayProperty( kOptions, [=, this] { @@ -109,9 +102,6 @@ void SendableChooser::InitSendable(nt::NTSendableBuilder& builder) { std::scoped_lock lock(m_mutex); m_haveSelected = true; m_selected = val; - for (auto& pub : m_activePubs) { - pub.Set(val); - } if (m_previousVal != val && m_listener) { choice = m_choices[val]; listener = m_listener; diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooserBase.h b/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooserBase.h index 46cbf0444ba..f0b4fedcbb0 100644 --- a/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooserBase.h +++ b/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooserBase.h @@ -7,11 +7,8 @@ #include #include -#include -#include -#include -#include #include +#include #include namespace frc { @@ -22,7 +19,7 @@ namespace frc { * It contains static, non-templated variables to avoid their duplication in the * template class. */ -class SendableChooserBase : public nt::NTSendable, +class SendableChooserBase : public wpi::Sendable, public wpi::SendableHelper { public: SendableChooserBase(); @@ -41,8 +38,6 @@ class SendableChooserBase : public nt::NTSendable, std::string m_defaultChoice; std::string m_selected; bool m_haveSelected = false; - wpi::SmallVector m_instancePubs; - wpi::SmallVector m_activePubs; wpi::mutex m_mutex; int m_instance; std::string m_previousVal; diff --git a/wpilibc/src/test/native/cpp/smartdashboard/SendableChooserTest.cpp b/wpilibc/src/test/native/cpp/smartdashboard/SendableChooserTest.cpp index 5728a072ab4..b25934deefd 100644 --- a/wpilibc/src/test/native/cpp/smartdashboard/SendableChooserTest.cpp +++ b/wpilibc/src/test/native/cpp/smartdashboard/SendableChooserTest.cpp @@ -9,6 +9,7 @@ #include #include +#include class SendableChooserTest : public ::testing::TestWithParam {}; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java index bfca78100e2..7f3b93c1f4a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java @@ -6,16 +6,10 @@ import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; -import edu.wpi.first.networktables.IntegerPublisher; -import edu.wpi.first.networktables.IntegerTopic; -import edu.wpi.first.networktables.NTSendable; -import edu.wpi.first.networktables.NTSendableBuilder; -import edu.wpi.first.networktables.StringPublisher; -import edu.wpi.first.networktables.StringTopic; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.sendable.SendableRegistry; -import java.util.ArrayList; import java.util.LinkedHashMap; -import java.util.List; import java.util.Map; import java.util.concurrent.atomic.AtomicInteger; import java.util.concurrent.locks.ReentrantLock; @@ -33,7 +27,7 @@ * * @param The type of the values to be stored */ -public class SendableChooser implements NTSendable, AutoCloseable { +public class SendableChooser implements Sendable, AutoCloseable { /** The key for the default value. */ private static final String DEFAULT = "default"; @@ -67,14 +61,6 @@ public SendableChooser() { @Override public void close() { SendableRegistry.remove(this); - m_mutex.lock(); - try { - for (StringPublisher pub : m_activePubs) { - pub.close(); - } - } finally { - m_mutex.unlock(); - } } /** @@ -136,15 +122,12 @@ public void onChange(Consumer listener) { } private String m_selected; - private final List m_activePubs = new ArrayList<>(); private final ReentrantLock m_mutex = new ReentrantLock(); @Override - public void initSendable(NTSendableBuilder builder) { + public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("String Chooser"); - IntegerPublisher instancePub = new IntegerTopic(builder.getTopic(INSTANCE)).publish(); - instancePub.set(m_instance); - builder.addCloseable(instancePub); + builder.publishConstInteger(INSTANCE, m_instance); builder.addStringProperty(DEFAULT, () -> m_defaultChoice, null); builder.addStringArrayProperty(OPTIONS, () -> m_map.keySet().toArray(new String[0]), null); builder.addStringProperty( @@ -162,12 +145,6 @@ public void initSendable(NTSendableBuilder builder) { } }, null); - m_mutex.lock(); - try { - m_activePubs.add(new StringTopic(builder.getTopic(ACTIVE)).publish()); - } finally { - m_mutex.unlock(); - } builder.addStringProperty( SELECTED, null, @@ -185,9 +162,6 @@ public void initSendable(NTSendableBuilder builder) { listener = null; } m_previousVal = val; - for (StringPublisher pub : m_activePubs) { - pub.set(val); - } } finally { m_mutex.unlock(); } From e28274324c4905329b9c9d6b2cdce8c207b63d89 Mon Sep 17 00:00:00 2001 From: Gold856 <117957790+Gold856@users.noreply.github.com> Date: Thu, 5 Oct 2023 00:22:57 -0400 Subject: [PATCH 34/76] [wpilib] Make BooleanEvent more consistent (#5436) Co-authored-by: Tyler Veness Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> --- .../main/native/cpp/event/BooleanEvent.cpp | 51 +- .../native/include/frc/event/BooleanEvent.h | 14 +- .../native/cpp/event/BooleanEventTest.cpp | 487 ++++++++++++++++++ .../wpi/first/wpilibj/event/BooleanEvent.java | 39 +- .../first/wpilibj/event/BooleanEventTest.java | 357 ++++++++++++- 5 files changed, 890 insertions(+), 58 deletions(-) create mode 100644 wpilibc/src/test/native/cpp/event/BooleanEventTest.cpp diff --git a/wpilibc/src/main/native/cpp/event/BooleanEvent.cpp b/wpilibc/src/main/native/cpp/event/BooleanEvent.cpp index 5b8ce63d686..fd9c831fe9d 100644 --- a/wpilibc/src/main/native/cpp/event/BooleanEvent.cpp +++ b/wpilibc/src/main/native/cpp/event/BooleanEvent.cpp @@ -7,56 +7,61 @@ using namespace frc; BooleanEvent::BooleanEvent(EventLoop* loop, std::function condition) - : m_loop(loop), m_condition(std::move(condition)) {} + : m_loop(loop), m_condition(std::move(condition)) { + m_state = std::make_shared(m_condition()); + m_loop->Bind( + // NOLINTNEXTLINE(clang-analyzer-cplusplus.NewDeleteLeaks) + [condition = m_condition, state = m_state] { *state = condition(); }); +} BooleanEvent::operator std::function() { - return m_condition; + return [state = m_state] { return *state; }; } bool BooleanEvent::GetAsBoolean() const { - return m_condition(); + return *m_state; } void BooleanEvent::IfHigh(std::function action) { - m_loop->Bind([condition = m_condition, action = std::move(action)] { - if (condition()) { + m_loop->Bind([state = m_state, action = std::move(action)] { + if (*state) { action(); } }); } BooleanEvent BooleanEvent::operator!() { - return BooleanEvent(this->m_loop, [lhs = m_condition] { return !lhs(); }); + return BooleanEvent(this->m_loop, [state = m_state] { return !*state; }); } BooleanEvent BooleanEvent::operator&&(std::function rhs) { return BooleanEvent(this->m_loop, - [lhs = m_condition, rhs] { return lhs() && rhs(); }); + [state = m_state, rhs] { return *state && rhs(); }); } BooleanEvent BooleanEvent::operator||(std::function rhs) { return BooleanEvent(this->m_loop, - [lhs = m_condition, rhs] { return lhs() || rhs(); }); + [state = m_state, rhs] { return *state || rhs(); }); } BooleanEvent BooleanEvent::Rising() { - return BooleanEvent( - this->m_loop, [lhs = m_condition, m_previous = m_condition()]() mutable { - bool present = lhs(); - bool past = m_previous; - m_previous = present; - return !past && present; - }); + return BooleanEvent(this->m_loop, + [state = m_state, m_previous = *m_state]() mutable { + bool present = *state; + bool past = m_previous; + m_previous = present; + return !past && present; + }); } BooleanEvent BooleanEvent::Falling() { - return BooleanEvent( - this->m_loop, [lhs = m_condition, m_previous = m_condition()]() mutable { - bool present = lhs(); - bool past = m_previous; - m_previous = present; - return past && !present; - }); + return BooleanEvent(this->m_loop, + [state = m_state, m_previous = *m_state]() mutable { + bool present = *state; + bool past = m_previous; + m_previous = present; + return past && !present; + }); } BooleanEvent BooleanEvent::Debounce(units::second_t debounceTime, @@ -64,5 +69,5 @@ BooleanEvent BooleanEvent::Debounce(units::second_t debounceTime, return BooleanEvent( this->m_loop, [debouncer = frc::Debouncer(debounceTime, type), - lhs = m_condition]() mutable { return debouncer.Calculate(lhs()); }); + state = m_state]() mutable { return debouncer.Calculate(*state); }); } diff --git a/wpilibc/src/main/native/include/frc/event/BooleanEvent.h b/wpilibc/src/main/native/include/frc/event/BooleanEvent.h index 745a53c0959..7c19c90e8b4 100644 --- a/wpilibc/src/main/native/include/frc/event/BooleanEvent.h +++ b/wpilibc/src/main/native/include/frc/event/BooleanEvent.h @@ -40,9 +40,10 @@ class BooleanEvent { BooleanEvent(EventLoop* loop, std::function condition); /** - * Check whether this event is active or not. + * Check whether this event is active or not as of the last loop poll. * - * @return true if active. + * @return true if active, false if not active. If the event was never polled, + * it returns the state at event construction. */ bool GetAsBoolean() const; @@ -69,7 +70,7 @@ class BooleanEvent { [](EventLoop* loop, std::function condition) { return T(loop, condition); }) { - return ctor(m_loop, m_condition); + return ctor(m_loop, [state = m_state] { return *state; }); } /** @@ -84,7 +85,8 @@ class BooleanEvent { * Composes this event with another event, returning a new event that is * active when both events are active. * - *

The new event will use this event's polling loop. + *

The events must use the same event loop. If the events use different + * event loops, the composed signal won't update until both loops are polled. * * @param rhs the event to compose with * @return the event that is active when both events are active @@ -95,7 +97,8 @@ class BooleanEvent { * Composes this event with another event, returning a new event that is * active when either event is active. * - *

The new event will use this event's polling loop. + *

The events must use the same event loop. If the events use different + * event loops, the composed signal won't update until both loops are polled. * * @param rhs the event to compose with * @return the event that is active when either event is active @@ -131,5 +134,6 @@ class BooleanEvent { private: EventLoop* m_loop; std::function m_condition; + std::shared_ptr m_state; // A programmer's worst nightmare. }; } // namespace frc diff --git a/wpilibc/src/test/native/cpp/event/BooleanEventTest.cpp b/wpilibc/src/test/native/cpp/event/BooleanEventTest.cpp new file mode 100644 index 00000000000..28209b4e7f0 --- /dev/null +++ b/wpilibc/src/test/native/cpp/event/BooleanEventTest.cpp @@ -0,0 +1,487 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include + +#include "frc/event/BooleanEvent.h" +#include "frc/event/EventLoop.h" + +using namespace frc; + +TEST(BooleanEventTest, BinaryCompositions) { + EventLoop loop; + int andCounter = 0; + int orCounter = 0; + + EXPECT_EQ(0, andCounter); + EXPECT_EQ(0, orCounter); + + (BooleanEvent(&loop, [] { return true; }) && BooleanEvent(&loop, [] { + return false; + })).IfHigh([&] { ++andCounter; }); + (BooleanEvent(&loop, [] { return true; }) || BooleanEvent(&loop, [] { + return false; + })).IfHigh([&] { ++orCounter; }); + + loop.Poll(); + + EXPECT_EQ(0, andCounter); + EXPECT_EQ(1, orCounter); +} + +/** + * Tests that composed edge events only execute on edges (two + * rising edge events composed with and() should only execute when both signals + * are on the rising edge) + */ +TEST(BooleanEventTest, BinaryCompositionsWithEdgeDecorators) { + EventLoop loop; + bool boolean1 = false; + bool boolean2 = false; + bool boolean3 = false; + bool boolean4 = false; + int counter = 0; + + auto event1 = BooleanEvent(&loop, [&] { return boolean1; }).Rising(); + auto event2 = BooleanEvent(&loop, [&] { return boolean2; }).Rising(); + auto event3 = BooleanEvent(&loop, [&] { return boolean3; }).Rising(); + auto event4 = BooleanEvent(&loop, [&] { return boolean4; }).Rising(); + (event1 && event2).IfHigh([&] { ++counter; }); + (event3 || event4).IfHigh([&] { ++counter; }); + EXPECT_EQ(0, counter); + + boolean1 = true; + boolean2 = true; + boolean3 = true; + boolean4 = true; + loop.Poll(); // Both actions execute + + EXPECT_EQ(2, counter); + + loop.Poll(); // Nothing should happen since nothing is on rising edge + + EXPECT_EQ(2, counter); + + boolean1 = false; + boolean2 = false; + boolean3 = false; + boolean4 = false; + loop.Poll(); // Nothing should happen + + EXPECT_EQ(2, counter); + + boolean1 = true; + loop.Poll(); // Nothing should happen since only Bool 1 is on rising edge + + EXPECT_EQ(2, counter); + + boolean2 = true; + loop.Poll(); // Bool 2 is on rising edge, but Bool 1 isn't, nothing should + // happen + + EXPECT_EQ(2, counter); + + boolean1 = false; + boolean2 = false; + loop.Poll(); // Nothing should happen + + EXPECT_EQ(2, counter); + + boolean1 = true; + boolean2 = true; + loop.Poll(); // Bool 1 and 2 are on rising edge, increments counter once + + EXPECT_EQ(3, counter); + + boolean3 = true; + loop.Poll(); // Bool 3 is on rising edge, increments counter once + + EXPECT_EQ(4, counter); + + loop.Poll(); // Nothing should happen, Bool 3 isn't on rising edge + + EXPECT_EQ(4, counter); + + boolean4 = true; + loop.Poll(); // Bool 4 is on rising edge, increments counter once + + EXPECT_EQ(5, counter); + + loop.Poll(); // Nothing should happen, Bool 4 isn't on rising edge + + EXPECT_EQ(5, counter); +} + +TEST(BooleanEventTest, BinaryCompositionLoopSemantics) { + EventLoop loop1; + EventLoop loop2; + bool boolean1 = true; + bool boolean2 = true; + int counter1 = 0; + int counter2 = 0; + + (BooleanEvent(&loop1, [&] { return boolean1; }) && BooleanEvent(&loop2, [&] { + return boolean2; + })).IfHigh([&] { ++counter1; }); + (BooleanEvent(&loop2, [&] { return boolean2; }) && BooleanEvent(&loop1, [&] { + return boolean1; + })).IfHigh([&] { ++counter2; }); + + EXPECT_EQ(0, counter1); + EXPECT_EQ(0, counter2); + + loop1 + .Poll(); // 1st event executes, Bool 1 and 2 are true, increments counter + + EXPECT_EQ(1, counter1); + EXPECT_EQ(0, counter2); + + loop2 + .Poll(); // 2nd event executes, Bool 1 and 2 are true, increments counter + + EXPECT_EQ(1, counter1); + EXPECT_EQ(1, counter2); + + boolean2 = false; + loop1.Poll(); // 1st event executes, Bool 2 is still true because loop 2 + // hasn't updated it, increments counter + + EXPECT_EQ(2, counter1); + EXPECT_EQ(1, counter2); + + loop2.Poll(); // 2nd event executes, Bool 2 is now false because this loop + // updated it, does nothing + + EXPECT_EQ(2, counter1); + EXPECT_EQ(1, counter2); + + loop1.Poll(); // All bools are updated at this point, nothing should happen + + EXPECT_EQ(2, counter1); + EXPECT_EQ(1, counter2); + + boolean2 = true; + loop2.Poll(); // 2nd event executes, Bool 2 is true because this loop updated + // it, increments counter + + EXPECT_EQ(2, counter1); + EXPECT_EQ(2, counter2); + + loop1.Poll(); // 1st event executes, Bool 2 is true because loop 2 updated + // it, increments counter + + EXPECT_EQ(3, counter1); + EXPECT_EQ(2, counter2); + + boolean1 = false; + loop2.Poll(); // 2nd event executes, Bool 1 is still true because loop 1 + // hasn't updated it, increments counter + + EXPECT_EQ(3, counter1); + EXPECT_EQ(3, counter2); + + loop1.Poll(); // 1st event executes, Bool 1 is false because this loop + // updated it, does nothing + + EXPECT_EQ(3, counter1); + EXPECT_EQ(3, counter2); + + loop2.Poll(); // All bools are updated at this point, nothing should happen + + EXPECT_EQ(3, counter1); + EXPECT_EQ(3, counter2); +} + +/** Tests the order of actions bound to an event loop. */ +TEST(BooleanEventTest, PollOrdering) { + EventLoop loop; + bool boolean1 = true; + bool boolean2 = true; + bool enableAssert = false; + int counter = 0; + + (BooleanEvent( // This event binds an action to the event loop first + &loop, + [&] { + if (enableAssert) { + ++counter; + EXPECT_EQ(1, counter % 3); + } + return boolean1; + }) && // The composed event binds an action to the event loop third + // This event binds an action to the event loop second + BooleanEvent(&loop, [&] { + if (enableAssert) { + ++counter; + EXPECT_EQ(2, counter % 3); + } + return boolean2; + // This binds an action to the event loop fourth + })).IfHigh([&] { + if (enableAssert) { + ++counter; + EXPECT_EQ(0, counter % 3); + } + }); + enableAssert = true; + loop.Poll(); + loop.Poll(); + loop.Poll(); + loop.Poll(); +} + +TEST(BooleanEventTest, EdgeDecorators) { + EventLoop loop; + bool boolean = false; + int counter = 0; + + BooleanEvent(&loop, [&] { return boolean; }).Falling().IfHigh([&] { + --counter; + }); + BooleanEvent(&loop, [&] { return boolean; }).Rising().IfHigh([&] { + ++counter; + }); + + EXPECT_EQ(0, counter); + + boolean = false; + loop.Poll(); + + EXPECT_EQ(0, counter); + + boolean = true; + loop.Poll(); + + EXPECT_EQ(1, counter); + + boolean = true; + loop.Poll(); + + EXPECT_EQ(1, counter); + + boolean = false; + loop.Poll(); + + EXPECT_EQ(0, counter); +} + +/** + * Tests that binding actions to the same edge event will result in all actions + * executing. + */ +TEST(BooleanEventTest, EdgeReuse) { + EventLoop loop; + bool boolean = false; + int counter = 0; + + auto event = BooleanEvent(&loop, [&] { return boolean; }).Rising(); + event.IfHigh([&] { ++counter; }); + event.IfHigh([&] { ++counter; }); + + EXPECT_EQ(0, counter); + + loop.Poll(); + + EXPECT_EQ(0, counter); + + boolean = true; + loop.Poll(); + + EXPECT_EQ(2, counter); + + loop.Poll(); + + EXPECT_EQ(2, counter); + + boolean = false; + loop.Poll(); + + EXPECT_EQ(2, counter); + + boolean = true; + loop.Poll(); + + EXPECT_EQ(4, counter); +} + +/** + * Tests that all actions execute on separate edge events constructed from the + * original event. + */ +TEST(BooleanEventTest, EdgeReconstruct) { + EventLoop loop; + bool boolean = false; + int counter = 0; + + auto event = BooleanEvent(&loop, [&] { return boolean; }); + event.Rising().IfHigh([&] { ++counter; }); + event.Rising().IfHigh([&] { ++counter; }); + + EXPECT_EQ(0, counter); + + loop.Poll(); + + EXPECT_EQ(0, counter); + + boolean = true; + loop.Poll(); + + EXPECT_EQ(2, counter); + + loop.Poll(); + + EXPECT_EQ(2, counter); + + boolean = false; + loop.Poll(); + + EXPECT_EQ(2, counter); + + boolean = true; + loop.Poll(); + + EXPECT_EQ(4, counter); +} + +/** Tests that all actions bound to an event will still execute even if the + * signal is changed during the loop poll */ +TEST(BooleanEventTest, MidLoopBooleanChange) { + EventLoop loop; + bool boolean = false; + int counter = 0; + + auto event = BooleanEvent(&loop, [&] { return boolean; }).Rising(); + event.IfHigh([&] { + boolean = false; + ++counter; + }); + event.IfHigh([&] { ++counter; }); + + EXPECT_EQ(0, counter); + + loop.Poll(); + + EXPECT_EQ(0, counter); + + boolean = true; + loop.Poll(); + + EXPECT_EQ(2, counter); + + loop.Poll(); + + EXPECT_EQ(2, counter); + + boolean = false; + loop.Poll(); + + EXPECT_EQ(2, counter); + + boolean = true; + loop.Poll(); + + EXPECT_EQ(4, counter); +} + +/** + * Tests that all actions bound to composed events will still execute even if + * the composed signal changes during the loop poll. + */ +TEST(BooleanEventTest, MidLoopBooleanChangeWithComposedEvents) { + EventLoop loop; + bool boolean1 = false; + bool boolean2 = false; + bool boolean3 = false; + bool boolean4 = false; + int counter = 0; + + auto event1 = BooleanEvent(&loop, [&] { return boolean1; }); + auto event2 = BooleanEvent(&loop, [&] { return boolean2; }); + auto event3 = BooleanEvent(&loop, [&] { return boolean3; }); + auto event4 = BooleanEvent(&loop, [&] { return boolean4; }); + event1.IfHigh([&] { + boolean2 = false; + boolean3 = false; + ++counter; + }); + (event3 || event4).IfHigh([&] { + boolean1 = false; + ++counter; + }); + (event1 && event2).IfHigh([&] { + boolean4 = false; + ++counter; + }); + + EXPECT_EQ(0, counter); + + boolean1 = true; + boolean2 = true; + boolean3 = true; + boolean4 = true; + loop.Poll(); // All three actions execute, incrementing the counter three + // times and setting all booleans to false + + EXPECT_EQ(3, counter); + + loop.Poll(); // Nothing should happen since everything was set to false + + EXPECT_EQ(3, counter); + + boolean1 = true; + boolean2 = true; + loop.Poll(); // Bool 1 and 2 are true, increments counter twice, Bool 2 gets + // set to false + + EXPECT_EQ(5, counter); + + boolean1 = false; + loop.Poll(); // Nothing should happen + + EXPECT_EQ(5, counter); + + boolean1 = true; + boolean3 = true; + loop.Poll(); // Bool 1 and 3 are true, increments counter twice, Bool 3 gets + // set to false + + EXPECT_EQ(7, counter); + + boolean1 = false; + boolean4 = true; + loop.Poll(); // Bool 4 is true, increments counter once + + EXPECT_EQ(8, counter); +} + +TEST(BooleanEventTest, Negation) { + EventLoop loop; + bool boolean = false; + int counter = 0; + + (!BooleanEvent(&loop, [&] { return boolean; })).IfHigh([&] { ++counter; }); + + EXPECT_EQ(0, counter); + + loop.Poll(); + + EXPECT_EQ(1, counter); + + boolean = true; + loop.Poll(); + + EXPECT_EQ(1, counter); + + boolean = false; + loop.Poll(); + + EXPECT_EQ(2, counter); + + boolean = true; + loop.Poll(); + + EXPECT_EQ(2, counter); +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/event/BooleanEvent.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/event/BooleanEvent.java index 0786f34d05a..c8f58461c28 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/event/BooleanEvent.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/event/BooleanEvent.java @@ -7,6 +7,7 @@ import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; import edu.wpi.first.math.filter.Debouncer; +import java.util.concurrent.atomic.AtomicBoolean; import java.util.function.BiFunction; import java.util.function.BooleanSupplier; @@ -28,6 +29,9 @@ public class BooleanEvent implements BooleanSupplier { /** Condition. */ private final BooleanSupplier m_signal; + /** The state of the condition in the current loop poll. Nightmare to manage. */ + private final AtomicBoolean m_state = new AtomicBoolean(false); + /** * Creates a new event with the given signal determining whether it is active. * @@ -37,16 +41,19 @@ public class BooleanEvent implements BooleanSupplier { public BooleanEvent(EventLoop loop, BooleanSupplier signal) { m_loop = requireNonNullParam(loop, "loop", "BooleanEvent"); m_signal = requireNonNullParam(signal, "signal", "BooleanEvent"); + m_state.set(m_signal.getAsBoolean()); + m_loop.bind(() -> m_state.set(m_signal.getAsBoolean())); } /** - * Check the state of this signal (high or low). + * Check the state of this signal (high or low) as of the last loop poll. * - * @return true for the high state, false for the low state. + * @return true for the high state, false for the low state. If the event was never polled, it + * returns the state at event construction. */ @Override public final boolean getAsBoolean() { - return m_signal.getAsBoolean(); + return m_state.get(); } /** @@ -57,7 +64,7 @@ public final boolean getAsBoolean() { public final void ifHigh(Runnable action) { m_loop.bind( () -> { - if (m_signal.getAsBoolean()) { + if (m_state.get()) { action.run(); } }); @@ -72,11 +79,11 @@ public BooleanEvent rising() { return new BooleanEvent( m_loop, new BooleanSupplier() { - private boolean m_previous = m_signal.getAsBoolean(); + private boolean m_previous = m_state.get(); @Override public boolean getAsBoolean() { - boolean present = m_signal.getAsBoolean(); + boolean present = m_state.get(); boolean ret = !m_previous && present; m_previous = present; return ret; @@ -93,11 +100,11 @@ public BooleanEvent falling() { return new BooleanEvent( m_loop, new BooleanSupplier() { - private boolean m_previous = m_signal.getAsBoolean(); + private boolean m_previous = m_state.get(); @Override public boolean getAsBoolean() { - boolean present = m_signal.getAsBoolean(); + boolean present = m_state.get(); boolean ret = m_previous && !present; m_previous = present; return ret; @@ -132,7 +139,7 @@ public BooleanEvent debounce(double seconds, Debouncer.DebounceType type) { @Override public boolean getAsBoolean() { - return m_debouncer.calculate(m_signal.getAsBoolean()); + return m_debouncer.calculate(m_state.get()); } }); } @@ -144,35 +151,37 @@ public boolean getAsBoolean() { * @return the negated event */ public BooleanEvent negate() { - return new BooleanEvent(m_loop, () -> !m_signal.getAsBoolean()); + return new BooleanEvent(m_loop, () -> !m_state.get()); } /** * Composes this event with another event, returning a new signal that is in the high state when * both signals are in the high state. * - *

The new event will use this event's polling loop. + *

The events must use the same event loop. If the events use different event loops, the + * composed signal won't update until both loops are polled. * * @param other the event to compose with * @return the event that is active when both events are active */ public BooleanEvent and(BooleanSupplier other) { requireNonNullParam(other, "other", "and"); - return new BooleanEvent(m_loop, () -> m_signal.getAsBoolean() && other.getAsBoolean()); + return new BooleanEvent(m_loop, () -> m_state.get() && other.getAsBoolean()); } /** * Composes this event with another event, returning a new signal that is high when either signal * is high. * - *

The new event will use this event's polling loop. + *

The events must use the same event loop. If the events use different event loops, the + * composed signal won't update until both loops are polled. * * @param other the event to compose with * @return a signal that is high when either signal is high. */ public BooleanEvent or(BooleanSupplier other) { requireNonNullParam(other, "other", "or"); - return new BooleanEvent(m_loop, () -> m_signal.getAsBoolean() || other.getAsBoolean()); + return new BooleanEvent(m_loop, () -> m_state.get() || other.getAsBoolean()); } /** @@ -185,6 +194,6 @@ public BooleanEvent or(BooleanSupplier other) { * @return an instance of the subclass. */ public T castTo(BiFunction ctor) { - return ctor.apply(m_loop, m_signal); + return ctor.apply(m_loop, m_state::get); } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/event/BooleanEventTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/event/BooleanEventTest.java index 59caf23e5b8..834a0ef3f24 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/event/BooleanEventTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/event/BooleanEventTest.java @@ -30,42 +30,217 @@ void testBinaryCompositions() { assertEquals(1, orCounter.get()); } + /** + * Tests that composed edge events only execute on edges (two rising edge events composed with + * and() should only execute when both signals are on the rising edge). + */ + @Test + void testBinaryCompositionsWithEdgeDecorators() { + var loop = new EventLoop(); + var bool1 = new AtomicBoolean(false); + var bool2 = new AtomicBoolean(false); + var bool3 = new AtomicBoolean(false); + var bool4 = new AtomicBoolean(false); + var counter = new AtomicInteger(0); + + var event1 = new BooleanEvent(loop, bool1::get).rising(); + var event2 = new BooleanEvent(loop, bool2::get).rising(); + var event3 = new BooleanEvent(loop, bool3::get).rising(); + var event4 = new BooleanEvent(loop, bool4::get).rising(); + event1.and(event2).ifHigh(counter::incrementAndGet); + event3.or(event4).ifHigh(counter::incrementAndGet); + assertEquals(0, counter.get()); + + bool1.set(true); + bool2.set(true); + bool3.set(true); + bool4.set(true); + loop.poll(); // Both actions execute + + assertEquals(2, counter.get()); + + loop.poll(); // Nothing should happen since nothing is on rising edge + + assertEquals(2, counter.get()); + + bool1.set(false); + bool2.set(false); + bool3.set(false); + bool4.set(false); + loop.poll(); // Nothing should happen + + assertEquals(2, counter.get()); + + bool1.set(true); + loop.poll(); // Nothing should happen since only Bool 1 is on rising edge + + assertEquals(2, counter.get()); + + bool2.set(true); + loop.poll(); // Bool 2 is on rising edge, but Bool 1 isn't, nothing should happen + + assertEquals(2, counter.get()); + + bool1.set(false); + bool2.set(false); + loop.poll(); // Nothing should happen + + assertEquals(2, counter.get()); + + bool1.set(true); + bool2.set(true); + loop.poll(); // Bool 1 and 2 are on rising edge, increments counter once + + assertEquals(3, counter.get()); + + bool3.set(true); + loop.poll(); // Bool 3 is on rising edge, increments counter once + + assertEquals(4, counter.get()); + + loop.poll(); // Nothing should happen, Bool 3 isn't on rising edge + + assertEquals(4, counter.get()); + + bool4.set(true); + loop.poll(); // Bool 4 is on rising edge, increments counter once + + assertEquals(5, counter.get()); + + loop.poll(); // Nothing should happen, Bool 4 isn't on rising edge + + assertEquals(5, counter.get()); + } + @Test void testBinaryCompositionLoopSemantics() { var loop1 = new EventLoop(); var loop2 = new EventLoop(); - + var bool1 = new AtomicBoolean(true); + var bool2 = new AtomicBoolean(true); var counter1 = new AtomicInteger(0); var counter2 = new AtomicInteger(0); - new BooleanEvent(loop1, () -> true) - .and(new BooleanEvent(loop2, () -> true)) + new BooleanEvent(loop1, bool1::get) + .and(new BooleanEvent(loop2, bool2::get)) .ifHigh(counter1::incrementAndGet); - new BooleanEvent(loop2, () -> true) - .and(new BooleanEvent(loop1, () -> true)) + new BooleanEvent(loop2, bool2::get) + .and(new BooleanEvent(loop1, bool1::get)) .ifHigh(counter2::incrementAndGet); assertEquals(0, counter1.get()); assertEquals(0, counter2.get()); - loop1.poll(); + loop1.poll(); // 1st event executes, Bool 1 and 2 are true, increments counter assertEquals(1, counter1.get()); assertEquals(0, counter2.get()); - loop2.poll(); + loop2.poll(); // 2nd event executes, Bool 1 and 2 are true, increments counter assertEquals(1, counter1.get()); assertEquals(1, counter2.get()); + + bool2.set(false); + loop1.poll(); // 1st event executes, Bool 2 is still true because loop 2 hasn't updated it, + // increments counter + + assertEquals(2, counter1.get()); + assertEquals(1, counter2.get()); + + loop2.poll(); // 2nd event executes, Bool 2 is now false because this loop updated it, does + // nothing + + assertEquals(2, counter1.get()); + assertEquals(1, counter2.get()); + + loop1.poll(); // All bools are updated at this point, nothing should happen + + assertEquals(2, counter1.get()); + assertEquals(1, counter2.get()); + + bool2.set(true); + loop2.poll(); // 2nd event executes, Bool 2 is true because this loop updated it, increments + // counter + + assertEquals(2, counter1.get()); + assertEquals(2, counter2.get()); + + loop1 + .poll(); // 1st event executes, Bool 2 is true because loop 2 updated it, increments counter + + assertEquals(3, counter1.get()); + assertEquals(2, counter2.get()); + + bool1.set(false); + loop2.poll(); // 2nd event executes, Bool 1 is still true because loop 1 hasn't updated it, + // increments counter + + assertEquals(3, counter1.get()); + assertEquals(3, counter2.get()); + + loop1.poll(); // 1st event executes, Bool 1 is false because this loop updated it, does nothing + + assertEquals(3, counter1.get()); + assertEquals(3, counter2.get()); + + loop2.poll(); // All bools are updated at this point, nothing should happen + + assertEquals(3, counter1.get()); + assertEquals(3, counter2.get()); } + /** Tests the order of actions bound to an event loop. */ @Test - void testEdgeDecorators() { - var bool = new AtomicBoolean(false); + void testPollOrdering() { + var loop = new EventLoop(); + var bool1 = new AtomicBoolean(true); + var bool2 = new AtomicBoolean(true); + var enableAssert = new AtomicBoolean(false); var counter = new AtomicInteger(0); + // This event binds an action to the event loop first + new BooleanEvent( + loop, + () -> { + if (enableAssert.get()) { + counter.incrementAndGet(); + assertEquals(1, counter.get() % 3); + } + return bool1.get(); + }) + // The composed event binds an action to the event loop third + .and( + // This event binds an action to the event loop second + new BooleanEvent( + loop, + () -> { + if (enableAssert.get()) { + counter.incrementAndGet(); + assertEquals(2, counter.get() % 3); + } + return bool2.get(); + })) + // This binds an action to the event loop fourth + .ifHigh( + () -> { + if (enableAssert.get()) { + counter.incrementAndGet(); + assertEquals(0, counter.get() % 3); + } + }); + enableAssert.set(true); + loop.poll(); + loop.poll(); + loop.poll(); + loop.poll(); + } + @Test + void testEdgeDecorators() { var loop = new EventLoop(); + var bool = new AtomicBoolean(false); + var counter = new AtomicInteger(0); new BooleanEvent(loop, bool::get).falling().ifHigh(counter::decrementAndGet); new BooleanEvent(loop, bool::get).rising().ifHigh(counter::incrementAndGet); @@ -93,6 +268,7 @@ void testEdgeDecorators() { assertEquals(0, counter.get()); } + /** Tests that binding actions to the same edge event will result in all actions executing. */ @Test void testEdgeReuse() { var loop = new EventLoop(); @@ -112,23 +288,24 @@ void testEdgeReuse() { bool.set(true); loop.poll(); - assertEquals(1, counter.get()); // FIXME?: natural sense dictates counter == 2!! + assertEquals(2, counter.get()); loop.poll(); - assertEquals(1, counter.get()); + assertEquals(2, counter.get()); bool.set(false); loop.poll(); - assertEquals(1, counter.get()); + assertEquals(2, counter.get()); bool.set(true); loop.poll(); - assertEquals(2, counter.get()); + assertEquals(4, counter.get()); } + /** Tests that all actions execute on separate edge events constructed from the original event. */ @Test void testEdgeReconstruct() { var loop = new EventLoop(); @@ -148,8 +325,7 @@ void testEdgeReconstruct() { bool.set(true); loop.poll(); - // unlike the previous test ... - assertEquals(2, counter.get()); // as natural sense dictates, counter == 2 + assertEquals(2, counter.get()); loop.poll(); @@ -165,4 +341,155 @@ void testEdgeReconstruct() { assertEquals(4, counter.get()); } + + /** + * Tests that all actions bound to an event will still execute even if the signal is changed + * during the loop poll. + */ + @Test + void testMidLoopBooleanChange() { + var loop = new EventLoop(); + var bool = new AtomicBoolean(false); + var counter = new AtomicInteger(0); + + var event = new BooleanEvent(loop, bool::get).rising(); + event.ifHigh( + () -> { + bool.set(false); + counter.incrementAndGet(); + }); + event.ifHigh(counter::incrementAndGet); + + assertEquals(0, counter.get()); + + loop.poll(); + + assertEquals(0, counter.get()); + + bool.set(true); + loop.poll(); + + assertEquals(2, counter.get()); + + loop.poll(); + + assertEquals(2, counter.get()); + + bool.set(false); + loop.poll(); + + assertEquals(2, counter.get()); + + bool.set(true); + loop.poll(); + + assertEquals(4, counter.get()); + } + + /** + * Tests that all actions bound to composed events will still execute even if the composed signal + * changes during the loop poll. + */ + @Test + void testMidLoopBooleanChangeWithComposedEvents() { + var loop = new EventLoop(); + var bool1 = new AtomicBoolean(false); + var bool2 = new AtomicBoolean(false); + var bool3 = new AtomicBoolean(false); + var bool4 = new AtomicBoolean(false); + var counter = new AtomicInteger(0); + + var event1 = new BooleanEvent(loop, bool1::get); + var event2 = new BooleanEvent(loop, bool2::get); + var event3 = new BooleanEvent(loop, bool3::get); + var event4 = new BooleanEvent(loop, bool4::get); + event1.ifHigh( + () -> { + bool2.set(false); + bool3.set(false); + counter.incrementAndGet(); + }); + event3 + .or(event4) + .ifHigh( + () -> { + bool1.set(false); + counter.incrementAndGet(); + }); + event1 + .and(event2) + .ifHigh( + () -> { + bool4.set(false); + counter.incrementAndGet(); + }); + + assertEquals(0, counter.get()); + + bool1.set(true); + bool2.set(true); + bool3.set(true); + bool4.set(true); + loop.poll(); // All three actions execute, incrementing the counter three times and setting all + // booleans to false + + assertEquals(3, counter.get()); + + loop.poll(); // Nothing should happen since everything was set to false + + assertEquals(3, counter.get()); + + bool1.set(true); + bool2.set(true); + loop.poll(); // Bool 1 and 2 are true, increments counter twice, Bool 2 gets set to false + + assertEquals(5, counter.get()); + + bool1.set(false); + loop.poll(); // Nothing should happen + + assertEquals(5, counter.get()); + + bool1.set(true); + bool3.set(true); + loop.poll(); // Bool 1 and 3 are true, increments counter twice, Bool 3 gets set to false + + assertEquals(7, counter.get()); + + bool1.set(false); + bool4.set(true); + loop.poll(); // Bool 4 is true, increments counter once + + assertEquals(8, counter.get()); + } + + @Test + void testNegation() { + var loop = new EventLoop(); + var bool = new AtomicBoolean(false); + var counter = new AtomicInteger(0); + + new BooleanEvent(loop, bool::get).negate().ifHigh(counter::incrementAndGet); + + assertEquals(0, counter.get()); + + loop.poll(); + + assertEquals(1, counter.get()); + + bool.set(true); + loop.poll(); + + assertEquals(1, counter.get()); + + bool.set(false); + loop.poll(); + + assertEquals(2, counter.get()); + + bool.set(true); + loop.poll(); + + assertEquals(2, counter.get()); + } } From 4a79742c0ebd384f9d5fd8bfc743c27039a842a6 Mon Sep 17 00:00:00 2001 From: Dustin Spicuzza Date: Thu, 5 Oct 2023 00:49:07 -0400 Subject: [PATCH 35/76] [wpiutil] Set WPI_{UN}IGNORE_DEPRECATED to empty when all else fails (#5728) --- wpiutil/src/main/native/include/wpi/deprecated.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/wpiutil/src/main/native/include/wpi/deprecated.h b/wpiutil/src/main/native/include/wpi/deprecated.h index c5b4af27a29..ccd6bbaa407 100644 --- a/wpiutil/src/main/native/include/wpi/deprecated.h +++ b/wpiutil/src/main/native/include/wpi/deprecated.h @@ -16,6 +16,8 @@ _Pragma("GCC diagnostic ignored \"-Wdeprecated-declarations\"") #elif defined(_WIN32) #define WPI_IGNORE_DEPRECATED _Pragma("warning(disable : 4996)") +#else +#define WPI_IGNORE_DEPRECATED #endif #endif @@ -25,6 +27,8 @@ #define WPI_UNIGNORE_DEPRECATED _Pragma("GCC diagnostic pop") #elif defined(_WIN32) #define WPI_UNIGNORE_DEPRECATED _Pragma("warning(default : 4996)") +#else +#define WPI_UNIGNORE_DEPRECATED #endif #endif From 36a9931ee5d144a224509e5678e2d739ba68d761 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Wed, 4 Oct 2023 22:02:42 -0700 Subject: [PATCH 36/76] [ntcore] Networking improvements (#5659) - Utilize TrySend to properly backpressure network traffic - Split updates into reasonable sized frames using WS fragmentation - Use WS pings for network aliveness (requires 4.1 protocol revision) - Measure RTT only at start of connection, rather than periodically (this avoids them being affected by other network traffic) - Refactor network queue - Refactor network ping, ping from server as well - Improve meta topic performance - Implement unified approach for network value updates (currently client and server use very different approaches) that respects requested subscriber update frequency This adds a new protocol version (4.1) due to WS bugs in prior versions. --- ntcore/doc/networktables4.adoc | 51 +- ntcore/src/main/native/cpp/NetworkClient.cpp | 50 +- ntcore/src/main/native/cpp/NetworkClient.h | 5 +- ntcore/src/main/native/cpp/NetworkServer.cpp | 54 +- ntcore/src/main/native/cpp/net/ClientImpl.cpp | 253 +++------ ntcore/src/main/native/cpp/net/ClientImpl.h | 25 +- ntcore/src/main/native/cpp/net/Message.h | 2 + .../native/cpp/net/NetworkOutgoingQueue.h | 335 ++++++++++++ .../src/main/native/cpp/net/NetworkPing.cpp | 30 ++ ntcore/src/main/native/cpp/net/NetworkPing.h | 28 + ntcore/src/main/native/cpp/net/ServerImpl.cpp | 509 ++++++++---------- ntcore/src/main/native/cpp/net/ServerImpl.h | 193 ++++--- .../native/cpp/net/WebSocketConnection.cpp | 293 +++++++--- .../main/native/cpp/net/WebSocketConnection.h | 70 ++- .../src/main/native/cpp/net/WireConnection.h | 118 ++-- .../src/main/native/cpp/net/WireDecoder.cpp | 17 +- ntcore/src/main/native/cpp/net/WireDecoder.h | 3 +- .../wpi/first/networktables/TimeSyncTest.java | 47 +- .../native/cpp/net/MockWireConnection.cpp | 26 - .../test/native/cpp/net/MockWireConnection.h | 59 +- .../test/native/cpp/net/ServerImplTest.cpp | 117 ++-- 21 files changed, 1352 insertions(+), 933 deletions(-) create mode 100644 ntcore/src/main/native/cpp/net/NetworkOutgoingQueue.h create mode 100644 ntcore/src/main/native/cpp/net/NetworkPing.cpp create mode 100644 ntcore/src/main/native/cpp/net/NetworkPing.h delete mode 100644 ntcore/src/test/native/cpp/net/MockWireConnection.cpp diff --git a/ntcore/doc/networktables4.adoc b/ntcore/doc/networktables4.adoc index 5384ebff1e8..4357aaf940a 100644 --- a/ntcore/doc/networktables4.adoc +++ b/ntcore/doc/networktables4.adoc @@ -1,16 +1,37 @@ -= Network Tables Protocol Specification, Version 4.0 += Network Tables Protocol Specification, Version 4.1 WPILib Developers -Protocol Revision 4.0, 2/14/2021 +Protocol Revision 4.1, 10/1/2023 :toc: :toc-placement: preamble :sectanchors: A pub/sub WebSockets protocol based on NetworkTables concepts. +[[motivation4.1]] +== Motivation for Version 4.1 + +While NetworkTables 4.0 made a large number of improvements to the 3.0 protocol, a few weaknesses have been discovered in "real world" use: + +* Keep alives are not required. This can result in very long timeframes before a disconnect is detected. +* Periodic synchronization of timestamps is impacted by high variability of round trip time measurements on a stream connection shared with other data (due to network queueing in adverse network connections), resulting in values being "too old" even if actually more recent due to a change in base time +* Disconnect loops can be caused by large amounts of data values being sent in response to a "subscribe all" type of message (e.g. subscribe with empty or `$` prefix), resulting in data transmission being blocked for an excessive amount of time +* Publishing operations are not clearly subscriber-driven; the information is available via metatopics but not automatically sent to clients when clients publish + +Version 4.1 makes the following key changes to address these weaknesses: + +* Mandate the server and client send periodic WebSockets PING messages and track PONG responses +* Recommend that timestamp synchronization occur immediately following connection establishment and prior to any other control messages +* Recommend text and binary combining into a single WebSockets frame be limited to the network MTU (unless necessary to transport the message) +* Recommend WebSockets fragmentation be used on large frames to enable rapid handling of PING messages +* Add an option for topics to be marked transient (in which case no last value is retained by the server or sent to clients on initial subscription) +* Recommend clients subscribe to the `$sub$` meta-topic for each topic published by the client, and use this information to control what value updates are sent over the network to the server + +Version 4.1 uses a different WebSockets subprotocol string than version 4.0, so it is easy for both clients and servers to simultaneously support both versions 4.0 and 4.1. Due to WebSockets implementation bugs in version 4.0, version 4.1 implementations must not send WebSockets PING messages on version 4.0 connections. + [[motivation]] -== Motivation +== Motivation for Version 4.0 -Currently in NetworkTables there is no way to synchronize user value updates and NT update sweeps, and if user value updates occur more frequently than NT update sweeps, the intermediate values are lost. This prevents NetworkTables from being a viable transport layer for seeing all value changes (e.g. for plotting) at rates higher than the NetworkTables update rate (e.g. for capturing high frequency PID changes). While custom code can work around the second issue, it is more difficult to work around the first issue (unless full timestamps are also sent). +In <>, there is no way to synchronize user value updates and NT update sweeps, and if user value updates occur more frequently than NT update sweeps, the intermediate values are lost. This prevents NetworkTables from being a viable transport layer for seeing all value changes (e.g. for plotting) at rates higher than the NetworkTables update rate (e.g. for capturing high frequency PID changes). While custom code can work around the second issue, it is more difficult to work around the first issue (unless full timestamps are also sent). Adding built-in support for capturing and communicating all timestamped data value changes with minimal additional user code changes will make it much easier for inexperienced teams to get high resolution, accurate data to dashboard displays with the minimal possible bandwidth and airtime usage. Assuming the dashboard performs record and playback of NT updates, this also meets the desire to provide teams a robust data capture and playback mechanism. @@ -67,6 +88,15 @@ When the client receives an -1 ID message from the server, it shall compute the Due to the fact there can be multiple publishers for a single topic and unpredictable network delays / clock drift, there is no global total order for timestamps either globally or on a per-topic basis. While single publishers for real-time data will be the norm, and in that case the timestamps will usually be in order, applications that use timestamps need to be able to handle out-of-order timestamps. +[[aliveness]] +=== Connection Aliveness Checking + +With a version 4.1 connection, both the client and the server should send periodic WebSockets PING messages and look for a PONG response within a reasonable period of time. On version 4.0 connections, or if this is not possible (e.g. the underlying WebSockets implementation does not have the ability to send PING messages), the client should use timestamp messages for aliveness testing. If no response is received after an appropriate amount of time, the client or server shall disconnect the WebSockets connection and try to re-establish a new connection. + +As the WebSockets protocol allows PONG responses to be sent in the middle of another message stream, WebSockets PING messages are preferred, as this allows for a shorter timeout period that is not dependent on the size of the transmitted messages. Sending a ping every 200 ms with a timeout of 1 second is recommended in this case. + +If using timestamp messages for aliveness checking, the client should use a timeout long enough to account for the largest expected message size (as the server can only respond after such a message has been completely transmitted). Sending a ping every 1 second with a timeout of 3 seconds is recommended in this case. + [[reconnection]] === Caching and Reconnection Handling @@ -127,10 +157,12 @@ The server may operate in-process to an application (e.g. a robot program). In Clients are responsible for keeping server connections established (e.g. via retries when a connection is lost). Topic IDs must be treated as connection-specific; if the connection to the server is lost, the client is responsible for sending new <> and <> messages as required for the application when a new connection is established, and not using old topic IDs, but rather waiting for new <> messages to be received. -Except for offline-published values with timestamps of 0, the client shall not send any other published values to the server until its clock is synchronized with the server per the <> section. +Except for offline-published values with timestamps of 0, the client shall not send any other published values to the server until its clock is synchronized with the server per the <> section. Clients should measure RTT prior to sending any control messages (to avoid other traffic disrupting the measurement). Clients may publish a value at any time following clock synchronization. Clients may subscribe to meta-topics to determine whether or not to publish a value change (e.g. based on whether there are any subscribers, or based on specific <>). +Clients should subscribe to the `$sub$` meta topic for each topic published and use this metadata to determine how frequently to send updates to the network. However, this is not required--clients may choose to ignore this and send updates at any time. + [[meta-topics]] === Server-Published Meta Topics @@ -300,10 +332,17 @@ Both clients and servers shall support unsecure connections (`ws:`) and may supp Servers shall support a resource name of `/nt/`, where `` is an arbitrary string representing the client name. The client name does not need to be unique; multiple connections to the same name are allowed; the server shall ensure the name is unique (for the purposes of meta-topics) by appending a '@' and a unique number (if necessary). To support this, the name provided by the client should not contain an embedded '@'. Clients should provide a way to specify the resource name (in particular, the client name portion). -Both clients and servers shall support/use subprotocol `networktables.first.wpi.edu` for this protocol. Clients and servers shall terminate the connection in accordance with the WebSocket protocol unless both sides support this subprotocol. +Both clients and servers should support/use subprotocol `v4.1.networktables.first.wpi.edu` (for version 4.1) and `networktables.first.wpi.edu` (for version 4.0). Version 4.1 should be preferred, with version 4.0 as a fallback, using standard WebSockets subprotocol negotiation. Clients and servers shall terminate the connection in accordance with the WebSocket protocol unless both sides support a common subprotocol. The unsecure standard server port number shall be 5810, the secure standard port number shall be 5811. +[[fragmentation]] +=== Fragmentation + +Combining multiple text or binary messages into a single WebSockets frame should be limited such that the WebSockets frame does not exceed the MTU unless otherwise required to fit the total binary data size. + +Client and server implementations should fragment WebSockets messages to roughly the network MTU in order to facilitate rapid handling of PING and PONG messages. + [[data-types]] == Supported Data Types diff --git a/ntcore/src/main/native/cpp/NetworkClient.cpp b/ntcore/src/main/native/cpp/NetworkClient.cpp index 1affb0082f9..fa849a471eb 100644 --- a/ntcore/src/main/native/cpp/NetworkClient.cpp +++ b/ntcore/src/main/native/cpp/NetworkClient.cpp @@ -125,8 +125,8 @@ void NetworkClientBase::DoDisconnect(std::string_view reason) { if (m_readLocalTimer) { m_readLocalTimer->Stop(); } - if (m_sendValuesTimer) { - m_sendValuesTimer->Stop(); + if (m_sendOutgoingTimer) { + m_sendOutgoingTimer->Stop(); } m_localStorage.ClearNetwork(); m_localQueue.ClearQueue(); @@ -150,9 +150,9 @@ NetworkClient3::NetworkClient3(int inst, std::string_view id, loop, kReconnectRate, m_logger, [this](uv::Tcp& tcp) { TcpConnected(tcp); }); - m_sendValuesTimer = uv::Timer::Create(loop); - if (m_sendValuesTimer) { - m_sendValuesTimer->timeout.connect([this] { + m_sendOutgoingTimer = uv::Timer::Create(loop); + if (m_sendOutgoingTimer) { + m_sendOutgoingTimer->timeout.connect([this] { if (m_clientImpl) { HandleLocal(); m_clientImpl->SendPeriodic(m_loop.Now().count(), false); @@ -206,9 +206,9 @@ void NetworkClient3::TcpConnected(uv::Tcp& tcp) { auto clientImpl = std::make_shared( m_loop.Now().count(), m_inst, *wire, m_logger, [this](uint32_t repeatMs) { DEBUG4("Setting periodic timer to {}", repeatMs); - if (m_sendValuesTimer) { - m_sendValuesTimer->Start(uv::Timer::Time{repeatMs}, - uv::Timer::Time{repeatMs}); + if (m_sendOutgoingTimer) { + m_sendOutgoingTimer->Start(uv::Timer::Time{repeatMs}, + uv::Timer::Time{repeatMs}); } }); clientImpl->Start( @@ -302,18 +302,18 @@ NetworkClient::NetworkClient( m_readLocalTimer->timeout.connect([this] { if (m_clientImpl) { HandleLocal(); - m_clientImpl->SendControl(m_loop.Now().count()); + m_clientImpl->SendOutgoing(m_loop.Now().count(), false); } }); m_readLocalTimer->Start(uv::Timer::Time{100}, uv::Timer::Time{100}); } - m_sendValuesTimer = uv::Timer::Create(loop); - if (m_sendValuesTimer) { - m_sendValuesTimer->timeout.connect([this] { + m_sendOutgoingTimer = uv::Timer::Create(loop); + if (m_sendOutgoingTimer) { + m_sendOutgoingTimer->timeout.connect([this] { if (m_clientImpl) { HandleLocal(); - m_clientImpl->SendValues(m_loop.Now().count(), false); + m_clientImpl->SendOutgoing(m_loop.Now().count(), false); } }); } @@ -324,7 +324,7 @@ NetworkClient::NetworkClient( m_flush->wakeup.connect([this] { if (m_clientImpl) { HandleLocal(); - m_clientImpl->SendValues(m_loop.Now().count(), true); + m_clientImpl->SendOutgoing(m_loop.Now().count(), true); } }); } @@ -369,37 +369,41 @@ void NetworkClient::TcpConnected(uv::Tcp& tcp) { wpi::SmallString<128> idBuf; auto ws = wpi::WebSocket::CreateClient( tcp, fmt::format("/nt/{}", wpi::EscapeURI(m_id, idBuf)), "", - {{"networktables.first.wpi.edu"}}, options); + {"v4.1.networktables.first.wpi.edu", "networktables.first.wpi.edu"}, + options); ws->SetMaxMessageSize(kMaxMessageSize); - ws->open.connect([this, &tcp, ws = ws.get()](std::string_view) { + ws->open.connect([this, &tcp, ws = ws.get()](std::string_view protocol) { if (m_connList.IsConnected()) { ws->Terminate(1006, "no longer needed"); return; } - WsConnected(*ws, tcp); + WsConnected(*ws, tcp, protocol); }); } -void NetworkClient::WsConnected(wpi::WebSocket& ws, uv::Tcp& tcp) { +void NetworkClient::WsConnected(wpi::WebSocket& ws, uv::Tcp& tcp, + std::string_view protocol) { if (m_parallelConnect) { m_parallelConnect->Succeeded(tcp); } ConnectionInfo connInfo; uv::AddrToName(tcp.GetPeer(), &connInfo.remote_ip, &connInfo.remote_port); - connInfo.protocol_version = 0x0400; + connInfo.protocol_version = + protocol == "v4.1.networktables.first.wpi.edu" ? 0x0401 : 0x0400; INFO("CONNECTED NT4 to {} port {}", connInfo.remote_ip, connInfo.remote_port); m_connHandle = m_connList.AddConnection(connInfo); - m_wire = std::make_shared(ws); + m_wire = + std::make_shared(ws, connInfo.protocol_version); m_clientImpl = std::make_unique( m_loop.Now().count(), m_inst, *m_wire, m_logger, m_timeSyncUpdated, [this](uint32_t repeatMs) { DEBUG4("Setting periodic timer to {}", repeatMs); - if (m_sendValuesTimer) { - m_sendValuesTimer->Start(uv::Timer::Time{repeatMs}, - uv::Timer::Time{repeatMs}); + if (m_sendOutgoingTimer) { + m_sendOutgoingTimer->Start(uv::Timer::Time{repeatMs}, + uv::Timer::Time{repeatMs}); } }); m_clientImpl->SetLocal(&m_localStorage); diff --git a/ntcore/src/main/native/cpp/NetworkClient.h b/ntcore/src/main/native/cpp/NetworkClient.h index 7839db7393e..73e113415f9 100644 --- a/ntcore/src/main/native/cpp/NetworkClient.h +++ b/ntcore/src/main/native/cpp/NetworkClient.h @@ -76,7 +76,7 @@ class NetworkClientBase : public INetworkClient { // used only from loop std::shared_ptr m_parallelConnect; std::shared_ptr m_readLocalTimer; - std::shared_ptr m_sendValuesTimer; + std::shared_ptr m_sendOutgoingTimer; std::shared_ptr> m_flushLocal; std::shared_ptr> m_flush; @@ -138,7 +138,8 @@ class NetworkClient final : public NetworkClientBase { private: void HandleLocal(); void TcpConnected(wpi::uv::Tcp& tcp) final; - void WsConnected(wpi::WebSocket& ws, wpi::uv::Tcp& tcp); + void WsConnected(wpi::WebSocket& ws, wpi::uv::Tcp& tcp, + std::string_view protocol); void ForceDisconnect(std::string_view reason) override; void DoDisconnect(std::string_view reason) override; diff --git a/ntcore/src/main/native/cpp/NetworkServer.cpp b/ntcore/src/main/native/cpp/NetworkServer.cpp index e8bfd16669f..a3400a11934 100644 --- a/ntcore/src/main/native/cpp/NetworkServer.cpp +++ b/ntcore/src/main/native/cpp/NetworkServer.cpp @@ -50,8 +50,8 @@ class NetworkServer::ServerConnection { int GetClientId() const { return m_clientId; } protected: - void SetupPeriodicTimer(); - void UpdatePeriodicTimer(uint32_t repeatMs); + void SetupOutgoingTimer(); + void UpdateOutgoingTimer(uint32_t repeatMs); void ConnectionClosed(); NetworkServer& m_server; @@ -61,7 +61,7 @@ class NetworkServer::ServerConnection { int m_clientId; private: - std::shared_ptr m_sendValuesTimer; + std::shared_ptr m_outgoingTimer; }; class NetworkServer::ServerConnection3 : public ServerConnection { @@ -82,7 +82,9 @@ class NetworkServer::ServerConnection4 final std::string_view addr, unsigned int port, wpi::Logger& logger) : ServerConnection{server, addr, port, logger}, - HttpWebSocketServerConnection(stream, {"networktables.first.wpi.edu"}) { + HttpWebSocketServerConnection(stream, + {"v4.1.networktables.first.wpi.edu", + "networktables.first.wpi.edu"}) { m_info.protocol_version = 0x0400; } @@ -93,30 +95,32 @@ class NetworkServer::ServerConnection4 final std::shared_ptr m_wire; }; -void NetworkServer::ServerConnection::SetupPeriodicTimer() { - m_sendValuesTimer = uv::Timer::Create(m_server.m_loop); - m_sendValuesTimer->timeout.connect([this] { +void NetworkServer::ServerConnection::SetupOutgoingTimer() { + m_outgoingTimer = uv::Timer::Create(m_server.m_loop); + m_outgoingTimer->timeout.connect([this] { m_server.HandleLocal(); - m_server.m_serverImpl.SendValues(m_clientId, m_server.m_loop.Now().count()); + m_server.m_serverImpl.SendOutgoing(m_clientId, + m_server.m_loop.Now().count()); }); } -void NetworkServer::ServerConnection::UpdatePeriodicTimer(uint32_t repeatMs) { +void NetworkServer::ServerConnection::UpdateOutgoingTimer(uint32_t repeatMs) { + DEBUG4("Setting periodic timer to {}", repeatMs); if (repeatMs == UINT32_MAX) { - m_sendValuesTimer->Stop(); + m_outgoingTimer->Stop(); } else { - m_sendValuesTimer->Start(uv::Timer::Time{repeatMs}, - uv::Timer::Time{repeatMs}); + m_outgoingTimer->Start(uv::Timer::Time{repeatMs}, + uv::Timer::Time{repeatMs}); } } void NetworkServer::ServerConnection::ConnectionClosed() { // don't call back into m_server if it's being destroyed - if (!m_sendValuesTimer->IsLoopClosing()) { + if (!m_outgoingTimer->IsLoopClosing()) { m_server.m_serverImpl.RemoveClient(m_clientId); m_server.RemoveConnection(this); } - m_sendValuesTimer->Close(); + m_outgoingTimer->Close(); } NetworkServer::ServerConnection3::ServerConnection3( @@ -136,7 +140,7 @@ NetworkServer::ServerConnection3::ServerConnection3( m_server.AddConnection(this, m_info); INFO("CONNECTED NT3 client '{}' (from {})", name, m_connInfo); }, - [this](uint32_t repeatMs) { UpdatePeriodicTimer(repeatMs); }); + [this](uint32_t repeatMs) { UpdateOutgoingTimer(repeatMs); }); stream->error.connect([this](uv::Error err) { if (!m_wire->GetDisconnectReason().empty()) { @@ -163,7 +167,7 @@ NetworkServer::ServerConnection3::ServerConnection3( }); stream->StartRead(); - SetupPeriodicTimer(); + SetupOutgoingTimer(); } void NetworkServer::ServerConnection4::ProcessRequest() { @@ -228,13 +232,17 @@ void NetworkServer::ServerConnection4::ProcessWsUpgrade() { m_websocket->SetMaxMessageSize(kMaxMessageSize); - m_websocket->open.connect([this, name = std::string{name}](std::string_view) { - m_wire = std::make_shared(*m_websocket); + m_websocket->open.connect([this, name = std::string{name}]( + std::string_view protocol) { + m_info.protocol_version = + protocol == "v4.1.networktables.first.wpi.edu" ? 0x0401 : 0x0400; + m_wire = std::make_shared( + *m_websocket, m_info.protocol_version); // TODO: set local flag appropriately std::string dedupName; std::tie(dedupName, m_clientId) = m_server.m_serverImpl.AddClient( name, m_connInfo, false, *m_wire, - [this](uint32_t repeatMs) { UpdatePeriodicTimer(repeatMs); }); + [this](uint32_t repeatMs) { UpdateOutgoingTimer(repeatMs); }); INFO("CONNECTED NT4 client '{}' (from {})", dedupName, m_connInfo); m_info.remote_id = dedupName; m_server.AddConnection(this, m_info); @@ -251,7 +259,7 @@ void NetworkServer::ServerConnection4::ProcessWsUpgrade() { m_server.m_serverImpl.ProcessIncomingBinary(m_clientId, data); }); - SetupPeriodicTimer(); + SetupOutgoingTimer(); }); } @@ -372,7 +380,7 @@ void NetworkServer::Init() { if (m_readLocalTimer) { m_readLocalTimer->timeout.connect([this] { HandleLocal(); - m_serverImpl.SendControl(m_loop.Now().count()); + m_serverImpl.SendAllOutgoing(m_loop.Now().count(), false); }); m_readLocalTimer->Start(uv::Timer::Time{100}, uv::Timer::Time{100}); } @@ -398,9 +406,7 @@ void NetworkServer::Init() { if (m_flush) { m_flush->wakeup.connect([this] { HandleLocal(); - for (auto&& conn : m_connections) { - m_serverImpl.SendValues(conn.conn->GetClientId(), m_loop.Now().count()); - } + m_serverImpl.SendAllOutgoing(m_loop.Now().count(), true); }); } m_flushAtomic = m_flush.get(); diff --git a/ntcore/src/main/native/cpp/net/ClientImpl.cpp b/ntcore/src/main/native/cpp/net/ClientImpl.cpp index 71849ad5c97..309b01a0d05 100644 --- a/ntcore/src/main/native/cpp/net/ClientImpl.cpp +++ b/ntcore/src/main/native/cpp/net/ClientImpl.cpp @@ -20,17 +20,12 @@ #include "NetworkInterface.h" #include "WireConnection.h" #include "WireEncoder.h" +#include "net/NetworkOutgoingQueue.h" #include "networktables/NetworkTableValue.h" using namespace nt; using namespace nt::net; -static constexpr uint32_t kMinPeriodMs = 5; - -// maximum amount of time the wire can be not ready to send another -// transmission before we close the connection -static constexpr uint32_t kWireMaxNotReadyUs = 1000000; - ClientImpl::ClientImpl( uint64_t curTimeMs, int inst, WireConnection& wire, wpi::Logger& logger, std::function @@ -41,13 +36,16 @@ ClientImpl::ClientImpl( m_logger{logger}, m_timeSyncUpdated{std::move(timeSyncUpdated)}, m_setPeriodic{std::move(setPeriodic)}, - m_nextPingTimeMs{curTimeMs + kPingIntervalMs} { + m_ping{wire}, + m_nextPingTimeMs{curTimeMs + (wire.GetVersion() >= 0x0401 + ? NetworkPing::kPingIntervalMs + : kRttIntervalMs)}, + m_outgoing{wire, false} { // immediately send RTT ping - auto out = m_wire.SendBinary(); auto now = wpi::Now(); DEBUG4("Sending initial RTT ping {}", now); - WireEncodeBinary(out.Add(), -1, 0, Value::MakeInteger(now)); - m_wire.Flush(); + m_wire.SendBinary( + [&](auto& os) { WireEncodeBinary(os, -1, 0, Value::MakeInteger(now)); }); m_setPeriodic(m_periodMs); } @@ -62,14 +60,15 @@ void ClientImpl::ProcessIncomingBinary(uint64_t curTimeMs, int64_t id; Value value; std::string error; - if (!WireDecodeBinary(&data, &id, &value, &error, -m_serverTimeOffsetUs)) { + if (!WireDecodeBinary(&data, &id, &value, &error, + -m_outgoing.GetTimeOffset())) { ERR("binary decode error: {}", error); break; // FIXME } DEBUG4("BinaryMessage({})", id); - // handle RTT ping response - if (id == -1) { + // handle RTT ping response (only use first one) + if (!m_haveTimeOffset && id == -1) { if (!value.IsInteger()) { WARN("RTT ping response with non-integer type {}", static_cast(value.type())); @@ -77,15 +76,18 @@ void ClientImpl::ProcessIncomingBinary(uint64_t curTimeMs, } DEBUG4("RTT ping response time {} value {}", value.time(), value.GetInteger()); - m_pongTimeMs = curTimeMs; + if (m_wire.GetVersion() < 0x0401) { + m_pongTimeMs = curTimeMs; + } int64_t now = wpi::Now(); int64_t rtt2 = (now - value.GetInteger()) / 2; if (rtt2 < m_rtt2Us) { m_rtt2Us = rtt2; - m_serverTimeOffsetUs = value.server_time() + rtt2 - now; - DEBUG3("Time offset: {}", m_serverTimeOffsetUs); + int64_t serverTimeOffsetUs = value.server_time() + rtt2 - now; + DEBUG3("Time offset: {}", serverTimeOffsetUs); + m_outgoing.SetTimeOffset(serverTimeOffsetUs); m_haveTimeOffset = true; - m_timeSyncUpdated(m_serverTimeOffsetUs, m_rtt2Us, true); + m_timeSyncUpdated(serverTimeOffsetUs, m_rtt2Us, true); } continue; } @@ -110,152 +112,65 @@ void ClientImpl::HandleLocal(std::vector&& msgs) { // common case is value if (auto msg = std::get_if(&elem.contents)) { SetValue(msg->pubHandle, msg->value); - // setvalue puts on individual publish outgoing queues } else if (auto msg = std::get_if(&elem.contents)) { Publish(msg->pubHandle, msg->topicHandle, msg->name, msg->typeStr, msg->properties, msg->options); - m_outgoing.emplace_back(std::move(elem)); + m_outgoing.SendMessage(msg->pubHandle, std::move(elem)); } else if (auto msg = std::get_if(&elem.contents)) { if (Unpublish(msg->pubHandle, msg->topicHandle)) { - m_outgoing.emplace_back(std::move(elem)); + m_outgoing.SendMessage(msg->pubHandle, std::move(elem)); } } else { - m_outgoing.emplace_back(std::move(elem)); + m_outgoing.SendMessage(0, std::move(elem)); } } } -bool ClientImpl::DoSendControl(uint64_t curTimeMs) { - DEBUG4("SendControl({})", curTimeMs); - - // rate limit sends - if (curTimeMs < (m_lastSendMs + kMinPeriodMs)) { - return false; - } - - // start a timestamp RTT ping if it's time to do one - if (curTimeMs >= m_nextPingTimeMs) { - // if we didn't receive a response to our last ping, disconnect - if (m_nextPingTimeMs != 0 && m_pongTimeMs == 0) { - m_wire.Disconnect("timed out"); - return false; - } - - if (!CheckNetworkReady(curTimeMs)) { - return false; - } - auto now = wpi::Now(); - DEBUG4("Sending RTT ping {}", now); - WireEncodeBinary(m_wire.SendBinary().Add(), -1, 0, Value::MakeInteger(now)); - // drift isn't critical here, so just go from current time - m_nextPingTimeMs = curTimeMs + kPingIntervalMs; - m_pongTimeMs = 0; - } +void ClientImpl::SendOutgoing(uint64_t curTimeMs, bool flush) { + DEBUG4("SendOutgoing({}, {})", curTimeMs, flush); - if (!m_outgoing.empty()) { - if (!CheckNetworkReady(curTimeMs)) { - return false; + if (m_wire.GetVersion() >= 0x0401) { + // Use WS pings + if (!m_ping.Send(curTimeMs)) { + return; } - auto writer = m_wire.SendText(); - for (auto&& msg : m_outgoing) { - auto& stream = writer.Add(); - if (!WireEncodeText(stream, msg)) { - // shouldn't happen, but just in case... - stream << "{}"; + } else { + // Use RTT pings; it's unsafe to use WS pings due to bugs in WS message + // fragmentation in earlier NT4 implementations + if (curTimeMs >= m_nextPingTimeMs) { + // if we didn't receive a response to our last ping, disconnect + if (m_nextPingTimeMs != 0 && m_pongTimeMs == 0) { + m_wire.Disconnect("connection timed out"); + return; } + + auto now = wpi::Now(); + DEBUG4("Sending RTT ping {}", now); + m_wire.SendBinary([&](auto& os) { + WireEncodeBinary(os, -1, 0, Value::MakeInteger(now)); + }); + // drift isn't critical here, so just go from current time + m_nextPingTimeMs = curTimeMs + kRttIntervalMs; + m_pongTimeMs = 0; } - m_outgoing.resize(0); } - m_lastSendMs = curTimeMs; - return true; -} - -void ClientImpl::DoSendValues(uint64_t curTimeMs, bool flush) { - DEBUG4("SendValues({})", curTimeMs); - - // can't send value updates until we have a RTT + // wait until we have a RTT measurement before sending messages if (!m_haveTimeOffset) { return; } - // ensure all control messages are sent ahead of value updates - if (!DoSendControl(curTimeMs)) { - return; - } - - // send any pending updates due to be sent - bool checkedNetwork = false; - auto writer = m_wire.SendBinary(); - for (auto&& pub : m_publishers) { - if (pub && !pub->outValues.empty() && - (flush || curTimeMs >= pub->nextSendMs)) { - for (auto&& val : pub->outValues) { - if (!checkedNetwork) { - if (!CheckNetworkReady(curTimeMs)) { - return; - } - checkedNetwork = true; - } - DEBUG4("Sending {} value time={} server_time={} st_off={}", pub->handle, - val.time(), val.server_time(), m_serverTimeOffsetUs); - int64_t time = val.time(); - if (time != 0) { - time += m_serverTimeOffsetUs; - // make sure resultant time isn't exactly 0 - if (time == 0) { - time = 1; - } - } - WireEncodeBinary(writer.Add(), Handle{pub->handle}.GetIndex(), time, - val); - } - pub->outValues.resize(0); - pub->nextSendMs = curTimeMs + pub->periodMs; - } - } + m_outgoing.SendOutgoing(curTimeMs, flush); } -void ClientImpl::SendInitialValues() { - DEBUG4("SendInitialValues()"); - - // ensure all control messages are sent ahead of value updates - if (!DoSendControl(0)) { - return; - } - - // only send time=0 values (as we don't have a RTT yet) - auto writer = m_wire.SendBinary(); - for (auto&& pub : m_publishers) { - if (pub && !pub->outValues.empty()) { - bool sent = false; - for (auto&& val : pub->outValues) { - if (val.server_time() == 0) { - DEBUG4("Sending {} value time={} server_time={}", pub->handle, - val.time(), val.server_time()); - WireEncodeBinary(writer.Add(), Handle{pub->handle}.GetIndex(), 0, - val); - sent = true; - } - } - if (sent) { - std::erase_if(pub->outValues, - [](const auto& v) { return v.server_time() == 0; }); - } - } +void ClientImpl::UpdatePeriodic() { + if (m_periodMs < kMinPeriodMs) { + m_periodMs = kMinPeriodMs; } -} - -bool ClientImpl::CheckNetworkReady(uint64_t curTimeMs) { - if (!m_wire.Ready()) { - uint64_t lastFlushTime = m_wire.GetLastFlushTime(); - uint64_t now = wpi::Now(); - if (lastFlushTime != 0 && now > (lastFlushTime + kWireMaxNotReadyUs)) { - m_wire.Disconnect("transmit stalled"); - } - return false; + if (m_periodMs > kMaxPeriodMs) { + m_periodMs = kMaxPeriodMs; } - return true; + m_setPeriodic(m_periodMs); } void ClientImpl::Publish(NT_Publisher pubHandle, NT_Topic topicHandle, @@ -276,13 +191,11 @@ void ClientImpl::Publish(NT_Publisher pubHandle, NT_Topic topicHandle, if (publisher->periodMs < kMinPeriodMs) { publisher->periodMs = kMinPeriodMs; } + m_outgoing.SetPeriod(pubHandle, publisher->periodMs); // update period - m_periodMs = std::gcd(m_periodMs, publisher->periodMs); - if (m_periodMs < kMinPeriodMs) { - m_periodMs = kMinPeriodMs; - } - m_setPeriodic(m_periodMs); + m_periodMs = UpdatePeriodCalc(m_periodMs, publisher->periodMs); + UpdatePeriodic(); } bool ClientImpl::Unpublish(NT_Publisher pubHandle, NT_Topic topicHandle) { @@ -291,53 +204,34 @@ bool ClientImpl::Unpublish(NT_Publisher pubHandle, NT_Topic topicHandle) { return false; } bool doSend = true; - if (m_publishers[index]) { - // Look through outgoing queue to see if the publish hasn't been sent yet; - // if it hasn't, delete it and don't send the server a message. - // The outgoing queue doesn't contain values; those are deleted with the - // publisher object. - auto it = std::find_if( - m_outgoing.begin(), m_outgoing.end(), [&](const auto& elem) { - if (auto msg = std::get_if(&elem.contents)) { - return msg->pubHandle == pubHandle; - } - return false; - }); - if (it != m_outgoing.end()) { - m_outgoing.erase(it); - doSend = false; - } - } m_publishers[index].reset(); // loop over all publishers to update period - m_periodMs = kPingIntervalMs + 10; + m_periodMs = kMaxPeriodMs; for (auto&& pub : m_publishers) { if (pub) { m_periodMs = std::gcd(m_periodMs, pub->periodMs); } } - if (m_periodMs < kMinPeriodMs) { - m_periodMs = kMinPeriodMs; - } - m_setPeriodic(m_periodMs); + UpdatePeriodic(); + + // remove from outgoing handle map + m_outgoing.EraseHandle(pubHandle); return doSend; } void ClientImpl::SetValue(NT_Publisher pubHandle, const Value& value) { - DEBUG4("SetValue({}, time={}, server_time={}, st_off={})", pubHandle, - value.time(), value.server_time(), m_serverTimeOffsetUs); + DEBUG4("SetValue({}, time={}, server_time={})", pubHandle, value.time(), + value.server_time()); unsigned int index = Handle{pubHandle}.GetIndex(); if (index >= m_publishers.size() || !m_publishers[index]) { return; } auto& publisher = *m_publishers[index]; - if (publisher.outValues.empty() || publisher.options.sendAll) { - publisher.outValues.emplace_back(value); - } else { - publisher.outValues.back() = value; - } + m_outgoing.SendValue( + pubHandle, value, + publisher.options.sendAll ? ValueSendMode::kAll : ValueSendMode::kNormal); } void ClientImpl::ServerAnnounce(std::string_view name, int64_t id, @@ -375,17 +269,4 @@ void ClientImpl::ProcessIncomingText(std::string_view data) { WireDecodeText(data, *this, m_logger); } -void ClientImpl::SendControl(uint64_t curTimeMs) { - DoSendControl(curTimeMs); - m_wire.Flush(); -} - -void ClientImpl::SendValues(uint64_t curTimeMs, bool flush) { - DoSendValues(curTimeMs, flush); - m_wire.Flush(); -} - -void ClientImpl::SendInitial() { - SendInitialValues(); - m_wire.Flush(); -} +void ClientImpl::SendInitial() {} diff --git a/ntcore/src/main/native/cpp/net/ClientImpl.h b/ntcore/src/main/native/cpp/net/ClientImpl.h index 6e97e8dd393..b72ce38c567 100644 --- a/ntcore/src/main/native/cpp/net/ClientImpl.h +++ b/ntcore/src/main/native/cpp/net/ClientImpl.h @@ -16,6 +16,8 @@ #include #include "NetworkInterface.h" +#include "NetworkOutgoingQueue.h" +#include "NetworkPing.h" #include "PubSubOptions.h" #include "WireConnection.h" #include "WireDecoder.h" @@ -46,8 +48,7 @@ class ClientImpl final : private ServerMessageHandler { void ProcessIncomingBinary(uint64_t curTimeMs, std::span data); void HandleLocal(std::vector&& msgs); - void SendControl(uint64_t curTimeMs); - void SendValues(uint64_t curTimeMs, bool flush); + void SendOutgoing(uint64_t curTimeMs, bool flush); void SetLocal(LocalInterface* local) { m_local = local; } void SendInitial(); @@ -59,14 +60,9 @@ class ClientImpl final : private ServerMessageHandler { // in options as double, but copy here as integer; rounded to the nearest // 10 ms uint32_t periodMs; - uint64_t nextSendMs{0}; - std::vector outValues; // outgoing values }; - bool DoSendControl(uint64_t curTimeMs); - void DoSendValues(uint64_t curTimeMs, bool flush); - void SendInitialValues(); - bool CheckNetworkReady(uint64_t curTimeMs); + void UpdatePeriodic(); // ServerMessageHandler interface void ServerAnnounce(std::string_view name, int64_t id, @@ -96,20 +92,23 @@ class ClientImpl final : private ServerMessageHandler { // indexed by server-provided topic id wpi::DenseMap m_topicMap; + // ping + NetworkPing m_ping; + // timestamp handling - static constexpr uint32_t kPingIntervalMs = 3000; + static constexpr uint32_t kRttIntervalMs = 3000; uint64_t m_nextPingTimeMs{0}; uint64_t m_pongTimeMs{0}; uint32_t m_rtt2Us{UINT32_MAX}; bool m_haveTimeOffset{false}; - int64_t m_serverTimeOffsetUs{0}; // periodic sweep handling - uint32_t m_periodMs{kPingIntervalMs + 10}; - uint64_t m_lastSendMs{0}; + static constexpr uint32_t kMinPeriodMs = 5; + static constexpr uint32_t kMaxPeriodMs = NetworkPing::kPingIntervalMs; + uint32_t m_periodMs{kMaxPeriodMs}; // outgoing queue - std::vector m_outgoing; + NetworkOutgoingQueue m_outgoing; }; } // namespace nt::net diff --git a/ntcore/src/main/native/cpp/net/Message.h b/ntcore/src/main/native/cpp/net/Message.h index 43f858a2210..d2a02b001ee 100644 --- a/ntcore/src/main/native/cpp/net/Message.h +++ b/ntcore/src/main/native/cpp/net/Message.h @@ -70,6 +70,7 @@ struct ClientMessage { using Contents = std::variant; + using ValueMsg = ClientValueMsg; Contents contents; }; @@ -103,6 +104,7 @@ struct ServerValueMsg { struct ServerMessage { using Contents = std::variant; + using ValueMsg = ServerValueMsg; Contents contents; }; diff --git a/ntcore/src/main/native/cpp/net/NetworkOutgoingQueue.h b/ntcore/src/main/native/cpp/net/NetworkOutgoingQueue.h new file mode 100644 index 00000000000..52dd1f24011 --- /dev/null +++ b/ntcore/src/main/native/cpp/net/NetworkOutgoingQueue.h @@ -0,0 +1,335 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "Handle.h" +#include "Message.h" +#include "WireConnection.h" +#include "WireEncoder.h" +#include "networktables/NetworkTableValue.h" +#include "ntcore_c.h" + +namespace nt::net { + +static constexpr uint32_t kMinPeriodMs = 5; + +inline uint32_t UpdatePeriodCalc(uint32_t period, uint32_t aPeriod) { + uint32_t newPeriod; + if (period == UINT32_MAX) { + newPeriod = aPeriod; + } else { + newPeriod = std::gcd(period, aPeriod); + } + if (newPeriod < kMinPeriodMs) { + return kMinPeriodMs; + } + return newPeriod; +} + +template +uint32_t CalculatePeriod(const T& container, F&& getPeriod) { + uint32_t period = UINT32_MAX; + for (auto&& item : container) { + if (period == UINT32_MAX) { + period = getPeriod(item); + } else { + period = std::gcd(period, getPeriod(item)); + } + } + if (period < kMinPeriodMs) { + return kMinPeriodMs; + } + return period; +} + +template +concept NetworkMessage = + std::same_as || + std::same_as; + +enum class ValueSendMode { kDisabled = 0, kAll, kNormal, kImm }; + +template +class NetworkOutgoingQueue { + public: + NetworkOutgoingQueue(WireConnection& wire, bool local) + : m_wire{wire}, m_local{local} { + m_queues.emplace_back(100); // default queue is 100 ms period + } + + void SetPeriod(NT_Handle handle, uint32_t periodMs); + + void EraseHandle(NT_Handle handle) { m_handleMap.erase(handle); } + + template + void SendMessage(NT_Handle handle, T&& msg) { + m_queues[m_handleMap[handle].queueIndex].Append(handle, + std::forward(msg)); + m_totalSize += sizeof(Message); + } + + void SendValue(NT_Handle handle, const Value& value, ValueSendMode mode); + + void SendOutgoing(uint64_t curTimeMs, bool flush); + + void SetTimeOffset(int64_t offsetUs) { m_timeOffsetUs = offsetUs; } + int64_t GetTimeOffset() const { return m_timeOffsetUs; } + + public: + WireConnection& m_wire; + + private: + using ValueMsg = typename MessageType::ValueMsg; + + void EncodeValue(wpi::raw_ostream& os, NT_Handle handle, const Value& value); + + struct Message { + Message() = default; + template + Message(T&& msg, NT_Handle handle) + : msg{std::forward(msg)}, handle{handle} {} + + MessageType msg; + NT_Handle handle; + }; + + struct Queue { + explicit Queue(uint32_t periodMs) : periodMs{periodMs} {} + template + void Append(NT_Handle handle, T&& msg) { + msgs.emplace_back(std::forward(msg), handle); + } + std::vector msgs; + uint64_t nextSendMs = 0; + uint32_t periodMs; + }; + + std::vector m_queues; + + struct HandleInfo { + unsigned int queueIndex = 0; + int valuePos = -1; // -1 if not in queue + }; + wpi::DenseMap m_handleMap; + size_t m_totalSize{0}; + uint64_t m_lastSendMs{0}; + int64_t m_timeOffsetUs{0}; + unsigned int m_lastSetPeriodQueueIndex = 0; + unsigned int m_lastSetPeriod = 100; + bool m_local; + + // maximum total size of outgoing queues in bytes (approximate) + static constexpr size_t kOutgoingLimit = 1024 * 1024; +}; + +template +void NetworkOutgoingQueue::SetPeriod(NT_Handle handle, + uint32_t periodMs) { + // it's quite common to set a lot of things in a row with the same period + unsigned int queueIndex; + if (m_lastSetPeriod == periodMs) { + queueIndex = m_lastSetPeriodQueueIndex; + } else { + // find and possibly create queue for this period + auto it = + std::find_if(m_queues.begin(), m_queues.end(), + [&](const auto& q) { return q.periodMs == periodMs; }); + if (it == m_queues.end()) { + queueIndex = m_queues.size(); + m_queues.emplace_back(periodMs); + } else { + queueIndex = it - m_queues.begin(); + } + m_lastSetPeriodQueueIndex = queueIndex; + m_lastSetPeriod = periodMs; + } + + // map the handle to the queue + auto [infoIt, created] = m_handleMap.try_emplace(handle); + if (!created && infoIt->getSecond().queueIndex != queueIndex) { + // need to move any items from old queue to new queue + auto& oldMsgs = m_queues[infoIt->getSecond().queueIndex].msgs; + auto it = std::remove_if(oldMsgs.begin(), oldMsgs.end(), + [&](const auto& e) { return e.handle == handle; }); + auto& newMsgs = m_queues[queueIndex].msgs; + for (auto i = it, end = oldMsgs.end(); i != end; ++i) { + newMsgs.emplace_back(std::move(*i)); + } + oldMsgs.erase(it, oldMsgs.end()); + } + + infoIt->getSecond().queueIndex = queueIndex; +} + +template +void NetworkOutgoingQueue::SendValue(NT_Handle handle, + const Value& value, + ValueSendMode mode) { + if (m_local) { + mode = ValueSendMode::kImm; // always send local immediately + } + // backpressure by stopping sending all if the buffer is too full + if (mode == ValueSendMode::kAll && m_totalSize >= kOutgoingLimit) { + mode = ValueSendMode::kNormal; + } + switch (mode) { + case ValueSendMode::kDisabled: // do nothing + break; + case ValueSendMode::kImm: // send immediately + m_wire.SendBinary([&](auto& os) { EncodeValue(os, handle, value); }); + break; + case ValueSendMode::kAll: { // append to outgoing + auto& info = m_handleMap[handle]; + auto& queue = m_queues[info.queueIndex]; + info.valuePos = queue.msgs.size(); + queue.Append(handle, ValueMsg{handle, value}); + m_totalSize += sizeof(Message) + value.size(); + break; + } + case ValueSendMode::kNormal: { + // replace, or append if not present + auto& info = m_handleMap[handle]; + auto& queue = m_queues[info.queueIndex]; + if (info.valuePos != -1 && + static_cast(info.valuePos) < queue.msgs.size()) { + auto& elem = queue.msgs[info.valuePos]; + if (auto m = std::get_if(&elem.msg.contents)) { + // double-check handle, and only replace if timestamp newer + if (elem.handle == handle && + (m->value.time() == 0 || value.time() >= m->value.time())) { + int delta = value.size() - m->value.size(); + m->value = value; + m_totalSize += delta; + return; + } + } + } + info.valuePos = queue.msgs.size(); + queue.Append(handle, ValueMsg{handle, value}); + m_totalSize += sizeof(Message) + value.size(); + break; + } + } +} + +template +void NetworkOutgoingQueue::SendOutgoing(uint64_t curTimeMs, + bool flush) { + if (m_totalSize == 0) { + return; // nothing to do + } + + // rate limit frequency of transmissions + if (curTimeMs < (m_lastSendMs + kMinPeriodMs)) { + return; + } + + if (!m_wire.Ready()) { + return; // don't bother, still sending the last batch + } + + // what queues are ready to send? + wpi::SmallVector queues; + for (unsigned int i = 0; i < m_queues.size(); ++i) { + if (!m_queues[i].msgs.empty() && + (flush || curTimeMs >= m_queues[i].nextSendMs)) { + queues.emplace_back(i); + } + } + if (queues.empty()) { + return; // nothing needs to be sent yet + } + + // Sort transmission order by what queue has been waiting the longest time. + // XXX: byte-weighted fair queueing might be better, but is much more complex + // to implement. + std::sort(queues.begin(), queues.end(), [&](const auto& a, const auto& b) { + return m_queues[a].nextSendMs < m_queues[b].nextSendMs; + }); + + for (unsigned int queueIndex : queues) { + auto& queue = m_queues[queueIndex]; + auto& msgs = queue.msgs; + auto it = msgs.begin(); + auto end = msgs.end(); + int unsent = 0; + for (; it != end && unsent == 0; ++it) { + if (auto m = std::get_if(&it->msg.contents)) { + unsent = m_wire.WriteBinary( + [&](auto& os) { EncodeValue(os, it->handle, m->value); }); + } else { + unsent = m_wire.WriteText([&](auto& os) { + if (!WireEncodeText(os, it->msg)) { + os << "{}"; + } + }); + } + } + if (unsent == 0) { + // finish writing any partial buffers + unsent = m_wire.Flush(); + } + int delta = it - msgs.begin() - unsent; + for (auto&& msg : std::span{msgs}.subspan(0, delta)) { + if (auto m = std::get_if(&msg.msg.contents)) { + m_totalSize -= sizeof(Message) + m->value.size(); + } else { + m_totalSize -= sizeof(Message); + } + } + msgs.erase(msgs.begin(), it - unsent); + for (auto&& kv : m_handleMap) { + auto& info = kv.getSecond(); + if (info.queueIndex == queueIndex) { + if (info.valuePos < delta) { + info.valuePos = -1; + } else { + info.valuePos -= delta; + } + } + } + + // try to stay on periodic timing, unless it's falling behind current time + if (unsent == 0) { + queue.nextSendMs += queue.periodMs; + if (queue.nextSendMs < curTimeMs) { + queue.nextSendMs = curTimeMs + queue.periodMs; + } + } + } + + m_lastSendMs = curTimeMs; +} + +template +void NetworkOutgoingQueue::EncodeValue(wpi::raw_ostream& os, + NT_Handle handle, + const Value& value) { + int64_t time = value.time(); + if constexpr (std::same_as) { + if (time != 0) { + time += m_timeOffsetUs; + // make sure resultant time isn't exactly 0 + if (time == 0) { + time = 1; + } + } + } + WireEncodeBinary(os, Handle{handle}.GetIndex(), time, value); +} + +} // namespace nt::net diff --git a/ntcore/src/main/native/cpp/net/NetworkPing.cpp b/ntcore/src/main/native/cpp/net/NetworkPing.cpp new file mode 100644 index 00000000000..fdbd26c132a --- /dev/null +++ b/ntcore/src/main/native/cpp/net/NetworkPing.cpp @@ -0,0 +1,30 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "NetworkPing.h" + +#include "WireConnection.h" + +using namespace nt::net; + +bool NetworkPing::Send(uint64_t curTimeMs) { + if (curTimeMs < m_nextPingTimeMs) { + return true; + } + // if we didn't receive a timely response to our last ping, disconnect + uint64_t lastPing = m_wire.GetLastPingResponse(); + // DEBUG4("WS ping: lastPing={} curTime={} pongTimeMs={}\n", lastPing, + // curTimeMs, m_pongTimeMs); + if (lastPing == 0) { + lastPing = m_pongTimeMs; + } + if (m_pongTimeMs != 0 && curTimeMs > (lastPing + kPingTimeoutMs)) { + m_wire.Disconnect("connection timed out"); + return false; + } + m_wire.SendPing(curTimeMs); + m_nextPingTimeMs = curTimeMs + kPingIntervalMs; + m_pongTimeMs = curTimeMs; + return true; +} diff --git a/ntcore/src/main/native/cpp/net/NetworkPing.h b/ntcore/src/main/native/cpp/net/NetworkPing.h new file mode 100644 index 00000000000..304e01f23d9 --- /dev/null +++ b/ntcore/src/main/native/cpp/net/NetworkPing.h @@ -0,0 +1,28 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +namespace nt::net { + +class WireConnection; + +class NetworkPing { + public: + static constexpr uint32_t kPingIntervalMs = 200; + static constexpr uint32_t kPingTimeoutMs = 1000; + + explicit NetworkPing(WireConnection& wire) : m_wire{wire} {} + + bool Send(uint64_t curTimeMs); + + private: + WireConnection& m_wire; + uint64_t m_nextPingTimeMs{0}; + uint64_t m_pongTimeMs{0}; +}; + +} // namespace nt::net diff --git a/ntcore/src/main/native/cpp/net/ServerImpl.cpp b/ntcore/src/main/native/cpp/net/ServerImpl.cpp index 14dcfed2c43..da32102cacf 100644 --- a/ntcore/src/main/native/cpp/net/ServerImpl.cpp +++ b/ntcore/src/main/native/cpp/net/ServerImpl.cpp @@ -25,6 +25,7 @@ #include "Log.h" #include "NetworkInterface.h" #include "Types_internal.h" +#include "net/WireEncoder.h" #include "net3/WireConnection3.h" #include "net3/WireEncoder3.h" #include "networktables/NetworkTableValue.h" @@ -80,6 +81,76 @@ static void WriteOptions(mpack_writer_t& w, const PubSubOptionsImpl& options) { mpack_finish_map(&w); } +void ServerImpl::PublisherData::UpdateMeta() { + { + Writer w; + mpack_start_map(&w, 2); + mpack_write_str(&w, "uid"); + mpack_write_int(&w, pubuid); + mpack_write_str(&w, "topic"); + mpack_write_str(&w, topic->name); + mpack_finish_map(&w); + if (mpack_writer_destroy(&w) == mpack_ok) { + metaClient = std::move(w.bytes); + } + } + { + Writer w; + mpack_start_map(&w, 2); + mpack_write_str(&w, "client"); + if (client) { + mpack_write_str(&w, client->GetName()); + } else { + mpack_write_str(&w, ""); + } + mpack_write_str(&w, "pubuid"); + mpack_write_int(&w, pubuid); + mpack_finish_map(&w); + if (mpack_writer_destroy(&w) == mpack_ok) { + metaTopic = std::move(w.bytes); + } + } +} + +void ServerImpl::SubscriberData::UpdateMeta() { + { + Writer w; + mpack_start_map(&w, 3); + mpack_write_str(&w, "uid"); + mpack_write_int(&w, subuid); + mpack_write_str(&w, "topics"); + mpack_start_array(&w, topicNames.size()); + for (auto&& name : topicNames) { + mpack_write_str(&w, name); + } + mpack_finish_array(&w); + mpack_write_str(&w, "options"); + WriteOptions(w, options); + mpack_finish_map(&w); + if (mpack_writer_destroy(&w) == mpack_ok) { + metaClient = std::move(w.bytes); + } + } + { + Writer w; + mpack_start_map(&w, 3); + mpack_write_str(&w, "client"); + if (client) { + mpack_write_str(&w, client->GetName()); + } else { + mpack_write_str(&w, ""); + } + mpack_write_str(&w, "subuid"); + mpack_write_int(&w, subuid); + mpack_write_str(&w, "options"); + WriteOptions(w, options); + mpack_finish_map(&w); + if (mpack_writer_destroy(&w) == mpack_ok) { + metaTopic = std::move(w.bytes); + } + } +} + void ServerImpl::ClientData::UpdateMetaClientPub() { if (!m_metaPub) { return; @@ -87,12 +158,9 @@ void ServerImpl::ClientData::UpdateMetaClientPub() { Writer w; mpack_start_array(&w, m_publishers.size()); for (auto&& pub : m_publishers) { - mpack_start_map(&w, 2); - mpack_write_str(&w, "uid"); - mpack_write_int(&w, pub.first); - mpack_write_str(&w, "topic"); - mpack_write_str(&w, pub.second->topic->name); - mpack_finish_map(&w); + mpack_write_object_bytes( + &w, reinterpret_cast(pub.second->metaClient.data()), + pub.second->metaClient.size()); } mpack_finish_array(&w); if (mpack_writer_destroy(&w) == mpack_ok) { @@ -107,18 +175,9 @@ void ServerImpl::ClientData::UpdateMetaClientSub() { Writer w; mpack_start_array(&w, m_subscribers.size()); for (auto&& sub : m_subscribers) { - mpack_start_map(&w, 3); - mpack_write_str(&w, "uid"); - mpack_write_int(&w, sub.first); - mpack_write_str(&w, "topics"); - mpack_start_array(&w, sub.second->topicNames.size()); - for (auto&& name : sub.second->topicNames) { - mpack_write_str(&w, name); - } - mpack_finish_array(&w); - mpack_write_str(&w, "options"); - WriteOptions(w, sub.second->options); - mpack_finish_map(&w); + mpack_write_object_bytes( + &w, reinterpret_cast(sub.second->metaClient.data()), + sub.second->metaClient.size()); } mpack_finish_array(&w); if (mpack_writer_destroy(&w) == mpack_ok) { @@ -154,11 +213,10 @@ void ServerImpl::ClientData4Base::ClientPublish(int64_t pubuid, } // add publisher to topic - topic->publishers.Add(publisherIt->getSecond().get()); + topic->AddPublisher(this, publisherIt->getSecond().get()); // update meta data m_server.UpdateMetaTopicPub(topic); - UpdateMetaClientPub(); // respond with announce with pubuid to client DEBUG4("client {}: announce {} pubuid {}", m_id, topic->name, pubuid); @@ -175,14 +233,13 @@ void ServerImpl::ClientData4Base::ClientUnpublish(int64_t pubuid) { auto topic = publisher->topic; // remove publisher from topic - topic->publishers.Remove(publisher); + topic->RemovePublisher(this, publisher); // remove publisher from client m_publishers.erase(publisherIt); // update meta data m_server.UpdateMetaTopicPub(topic); - UpdateMetaClientPub(); // delete topic if no longer published if (!topic->IsPublished()) { @@ -234,14 +291,7 @@ void ServerImpl::ClientData4Base::ClientSubscribe( // update periodic sender (if not local) if (!m_local) { - if (m_periodMs == UINT32_MAX) { - m_periodMs = sub->periodMs; - } else { - m_periodMs = std::gcd(m_periodMs, sub->periodMs); - } - if (m_periodMs < kMinPeriodMs) { - m_periodMs = kMinPeriodMs; - } + m_periodMs = UpdatePeriodCalc(m_periodMs, sub->periodMs); m_setPeriodic(m_periodMs); } @@ -252,30 +302,28 @@ void ServerImpl::ClientData4Base::ClientSubscribe( std::vector dataToSend; dataToSend.reserve(m_server.m_topics.size()); for (auto&& topic : m_server.m_topics) { - bool removed = false; - if (replace) { - removed = topic->subscribers.Remove(sub.get()); - } + auto tcdIt = topic->clients.find(this); + bool removed = tcdIt != topic->clients.end() && replace && + tcdIt->second.subscribers.erase(sub.get()); // is client already subscribed? - bool wasSubscribed = false; - bool wasSubscribedValue = false; - for (auto subscriber : topic->subscribers) { - if (subscriber->client == this) { - wasSubscribed = true; - if (!subscriber->options.topicsOnly) { - wasSubscribedValue = true; - } - } - } + bool wasSubscribed = + tcdIt != topic->clients.end() && !tcdIt->second.subscribers.empty(); + bool wasSubscribedValue = + wasSubscribed ? tcdIt->second.sendMode != ValueSendMode::kDisabled + : false; bool added = false; if (sub->Matches(topic->name, topic->special)) { - topic->subscribers.Add(sub.get()); + if (tcdIt == topic->clients.end()) { + tcdIt = topic->clients.try_emplace(this).first; + } + tcdIt->second.AddSubscriber(sub.get()); added = true; } if (added ^ removed) { + UpdatePeriod(tcdIt->second, topic.get()); m_server.UpdateMetaTopicSub(topic.get()); } @@ -294,13 +342,8 @@ void ServerImpl::ClientData4Base::ClientSubscribe( for (auto topic : dataToSend) { DEBUG4("send last value for {} to client {}", topic->name, m_id); - SendValue(topic, topic->lastValue, kSendAll); + SendValue(topic, topic->lastValue, ValueSendMode::kAll); } - - // update meta data - UpdateMetaClientSub(); - - Flush(); } void ServerImpl::ClientData4Base::ClientUnsubscribe(int64_t subuid) { @@ -313,28 +356,24 @@ void ServerImpl::ClientData4Base::ClientUnsubscribe(int64_t subuid) { // remove from topics for (auto&& topic : m_server.m_topics) { - if (topic->subscribers.Remove(sub)) { - m_server.UpdateMetaTopicSub(topic.get()); + auto tcdIt = topic->clients.find(this); + if (tcdIt != topic->clients.end()) { + if (tcdIt->second.subscribers.erase(sub)) { + UpdatePeriod(tcdIt->second, topic.get()); + m_server.UpdateMetaTopicSub(topic.get()); + } } } // delete it from client (future value sets will be ignored) m_subscribers.erase(subIt); - UpdateMetaClientSub(); - // loop over all publishers to update period - m_periodMs = UINT32_MAX; - for (auto&& sub : m_subscribers) { - if (m_periodMs == UINT32_MAX) { - m_periodMs = sub.getSecond()->periodMs; - } else { - m_periodMs = std::gcd(m_periodMs, sub.getSecond()->periodMs); - } - } - if (m_periodMs < kMinPeriodMs) { - m_periodMs = kMinPeriodMs; + // loop over all subscribers to update period + if (!m_local) { + m_periodMs = CalculatePeriod( + m_subscribers, [](auto& x) { return x.getSecond()->periodMs; }); + m_setPeriodic(m_periodMs); } - m_setPeriodic(m_periodMs); } void ServerImpl::ClientData4Base::ClientSetValue(int64_t pubuid, @@ -350,7 +389,8 @@ void ServerImpl::ClientData4Base::ClientSetValue(int64_t pubuid, } void ServerImpl::ClientDataLocal::SendValue(TopicData* topic, - const Value& value, SendMode mode) { + const Value& value, + ValueSendMode mode) { if (m_server.m_local) { m_server.m_local->NetworkSetValue(topic->localHandle, value); } @@ -395,27 +435,45 @@ void ServerImpl::ClientDataLocal::SendPropertiesUpdate(TopicData* topic, void ServerImpl::ClientDataLocal::HandleLocal( std::span msgs) { DEBUG4("HandleLocal()"); + if (msgs.empty()) { + return; + } // just map as a normal client into client=0 calls + bool updatepub = false; + bool updatesub = false; for (const auto& elem : msgs) { // NOLINT // common case is value, so check that first if (auto msg = std::get_if(&elem.contents)) { ClientSetValue(msg->pubHandle, msg->value); } else if (auto msg = std::get_if(&elem.contents)) { ClientPublish(msg->pubHandle, msg->name, msg->typeStr, msg->properties); + updatepub = true; } else if (auto msg = std::get_if(&elem.contents)) { ClientUnpublish(msg->pubHandle); + updatepub = true; } else if (auto msg = std::get_if(&elem.contents)) { ClientSetProperties(msg->name, msg->update); } else if (auto msg = std::get_if(&elem.contents)) { ClientSubscribe(msg->subHandle, msg->topicNames, msg->options); + updatesub = true; } else if (auto msg = std::get_if(&elem.contents)) { ClientUnsubscribe(msg->subHandle); + updatesub = true; } } + if (updatepub) { + UpdateMetaClientPub(); + } + if (updatesub) { + UpdateMetaClientSub(); + } } void ServerImpl::ClientData4::ProcessIncomingText(std::string_view data) { - WireDecodeText(data, *this, m_logger); + if (WireDecodeText(data, *this, m_logger)) { + UpdateMetaClientPub(); + UpdateMetaClientSub(); + } } void ServerImpl::ClientData4::ProcessIncomingBinary( @@ -438,11 +496,8 @@ void ServerImpl::ClientData4::ProcessIncomingBinary( if (pubuid == -1) { auto now = wpi::Now(); DEBUG4("RTT ping from {}, responding with time={}", m_id, now); - { - auto out = m_wire.SendBinary(); - WireEncodeBinary(out.Add(), -1, now, value); - } - m_wire.Flush(); + m_wire.SendBinary( + [&](auto& os) { WireEncodeBinary(os, -1, now, value); }); continue; } @@ -452,40 +507,8 @@ void ServerImpl::ClientData4::ProcessIncomingBinary( } void ServerImpl::ClientData4::SendValue(TopicData* topic, const Value& value, - SendMode mode) { - if (m_local) { - mode = ClientData::kSendImmNoFlush; // always send local immediately - } - switch (mode) { - case ClientData::kSendDisabled: // do nothing - break; - case ClientData::kSendImmNoFlush: // send immediately - WriteBinary(topic->id, value.time(), value); - if (m_local) { - Flush(); - } - break; - case ClientData::kSendAll: // append to outgoing - m_outgoingValueMap[topic->id] = m_outgoing.size(); - m_outgoing.emplace_back(ServerMessage{ServerValueMsg{topic->id, value}}); - break; - case ClientData::kSendNormal: { - // replace, or append if not present - auto [it, added] = - m_outgoingValueMap.try_emplace(topic->id, m_outgoing.size()); - if (!added && it->second < m_outgoing.size()) { - if (auto m = - std::get_if(&m_outgoing[it->second].contents)) { - if (m->topic == topic->id) { // should always be true - m->value = value; - break; - } - } - } - m_outgoing.emplace_back(ServerMessage{ServerValueMsg{topic->id, value}}); - break; - } - } + ValueSendMode mode) { + m_outgoing.SendValue(topic->GetIdHandle(), value, mode); } void ServerImpl::ClientData4::SendAnnounce(TopicData* topic, @@ -497,14 +520,18 @@ void ServerImpl::ClientData4::SendAnnounce(TopicData* topic, sent = true; if (m_local) { - WireEncodeAnnounce(SendText().Add(), topic->name, topic->id, topic->typeStr, - topic->properties, pubuid); - Flush(); - } else { - m_outgoing.emplace_back(ServerMessage{AnnounceMsg{ - topic->name, topic->id, topic->typeStr, pubuid, topic->properties}}); - m_server.m_controlReady = true; + int unsent = m_wire.WriteText([&](auto& os) { + WireEncodeAnnounce(os, topic->name, topic->id, topic->typeStr, + topic->properties, pubuid); + }); + if (unsent == 0 && m_wire.Flush() == 0) { + return; + } } + m_outgoing.SendMessage(topic->GetIdHandle(), + AnnounceMsg{topic->name, topic->id, topic->typeStr, + pubuid, topic->properties}); + m_server.m_controlReady = true; } void ServerImpl::ClientData4::SendUnannounce(TopicData* topic) { @@ -515,13 +542,16 @@ void ServerImpl::ClientData4::SendUnannounce(TopicData* topic) { sent = false; if (m_local) { - WireEncodeUnannounce(SendText().Add(), topic->name, topic->id); - Flush(); - } else { - m_outgoing.emplace_back( - ServerMessage{UnannounceMsg{topic->name, topic->id}}); - m_server.m_controlReady = true; + int unsent = m_wire.WriteText( + [&](auto& os) { WireEncodeUnannounce(os, topic->name, topic->id); }); + if (unsent == 0 && m_wire.Flush() == 0) { + return; + } } + m_outgoing.SendMessage(topic->GetIdHandle(), + UnannounceMsg{topic->name, topic->id}); + m_outgoing.EraseHandle(topic->GetIdHandle()); + m_server.m_controlReady = true; } void ServerImpl::ClientData4::SendPropertiesUpdate(TopicData* topic, @@ -532,50 +562,33 @@ void ServerImpl::ClientData4::SendPropertiesUpdate(TopicData* topic, } if (m_local) { - WireEncodePropertiesUpdate(SendText().Add(), topic->name, update, ack); - Flush(); - } else { - m_outgoing.emplace_back( - ServerMessage{PropertiesUpdateMsg{topic->name, update, ack}}); - m_server.m_controlReady = true; - } -} - -void ServerImpl::ClientData4::SendOutgoing(uint64_t curTimeMs) { - if (m_outgoing.empty()) { - return; // nothing to do - } - - // rate limit frequency of transmissions - if (curTimeMs < (m_lastSendMs + kMinPeriodMs)) { - return; - } - - if (!m_wire.Ready()) { - uint64_t lastFlushTime = m_wire.GetLastFlushTime(); - uint64_t now = wpi::Now(); - if (lastFlushTime != 0 && now > (lastFlushTime + kWireMaxNotReadyUs)) { - m_wire.Disconnect("transmit stalled"); + int unsent = m_wire.WriteText([&](auto& os) { + WireEncodePropertiesUpdate(os, topic->name, update, ack); + }); + if (unsent == 0 && m_wire.Flush() == 0) { + return; } - return; } + m_outgoing.SendMessage(topic->GetIdHandle(), + PropertiesUpdateMsg{topic->name, update, ack}); + m_server.m_controlReady = true; +} - for (auto&& msg : m_outgoing) { - if (auto m = std::get_if(&msg.contents)) { - WriteBinary(m->topic, m->value.time(), m->value); - } else { - WireEncodeText(SendText().Add(), msg); +void ServerImpl::ClientData4::SendOutgoing(uint64_t curTimeMs, bool flush) { + if (m_wire.GetVersion() >= 0x0401) { + if (!m_ping.Send(curTimeMs)) { + return; } } - m_outgoing.resize(0); - m_outgoingValueMap.clear(); - m_lastSendMs = curTimeMs; + m_outgoing.SendOutgoing(curTimeMs, flush); } -void ServerImpl::ClientData4::Flush() { - m_outText.reset(); - m_outBinary.reset(); - m_wire.Flush(); +void ServerImpl::ClientData4::UpdatePeriod(TopicData::TopicClientData& tcd, + TopicData* topic) { + uint32_t period = + CalculatePeriod(tcd.subscribers, [](auto& x) { return x->periodMs; }); + DEBUG4("updating {} period to {} ms", topic->name, period); + m_outgoing.SetPeriod(topic->GetIdHandle(), period); } bool ServerImpl::ClientData3::TopicData3::UpdateFlags(TopicData* topic) { @@ -593,21 +606,21 @@ void ServerImpl::ClientData3::ProcessIncomingBinary( } void ServerImpl::ClientData3::SendValue(TopicData* topic, const Value& value, - SendMode mode) { + ValueSendMode mode) { if (m_state != kStateRunning) { - if (mode == kSendImmNoFlush) { - mode = kSendAll; + if (mode == ValueSendMode::kImm) { + mode = ValueSendMode::kAll; } } else if (m_local) { - mode = ClientData::kSendImmNoFlush; // always send local immediately + mode = ValueSendMode::kImm; // always send local immediately } TopicData3* topic3 = GetTopic3(topic); bool added = false; switch (mode) { - case ClientData::kSendDisabled: // do nothing + case ValueSendMode::kDisabled: // do nothing break; - case ClientData::kSendImmNoFlush: // send immediately and flush + case ValueSendMode::kImm: // send immediately ++topic3->seqNum; if (topic3->sentAssign) { net3::WireEncodeEntryUpdate(m_wire.Send().stream(), topic->id, @@ -622,7 +635,7 @@ void ServerImpl::ClientData3::SendValue(TopicData* topic, const Value& value, Flush(); } break; - case ClientData::kSendNormal: { + case ValueSendMode::kNormal: { // replace, or append if not present wpi::DenseMap::iterator it; std::tie(it, added) = @@ -639,7 +652,7 @@ void ServerImpl::ClientData3::SendValue(TopicData* topic, const Value& value, } } // fallthrough - case ClientData::kSendAll: // append to outgoing + case ValueSendMode::kAll: // append to outgoing if (!added) { m_outgoingValueMap[topic->id] = m_outgoing.size(); } @@ -666,7 +679,7 @@ void ServerImpl::ClientData3::SendAnnounce(TopicData* topic, // subscribe to all non-special topics if (!topic->special) { - topic->subscribers.Add(m_subscribers[0].get()); + topic->clients[this].AddSubscriber(m_subscribers[0].get()); m_server.UpdateMetaTopicSub(topic); } @@ -720,7 +733,7 @@ void ServerImpl::ClientData3::SendPropertiesUpdate(TopicData* topic, } } -void ServerImpl::ClientData3::SendOutgoing(uint64_t curTimeMs) { +void ServerImpl::ClientData3::SendOutgoing(uint64_t curTimeMs, bool flush) { if (m_outgoing.empty() || m_state != kStateRunning) { return; // nothing to do } @@ -743,6 +756,7 @@ void ServerImpl::ClientData3::SendOutgoing(uint64_t curTimeMs) { for (auto&& msg : m_outgoing) { net3::WireEncode(out.stream(), msg); } + m_wire.Flush(); m_outgoing.resize(0); m_outgoingValueMap.clear(); m_lastSendMs = curTimeMs; @@ -790,7 +804,7 @@ void ServerImpl::ClientData3::ClearEntries() { auto publisherIt = m_publishers.find(topic3it.second.pubuid); if (publisherIt != m_publishers.end()) { // remove publisher from topic - topic->publishers.Remove(publisherIt->second.get()); + topic->RemovePublisher(this, publisherIt->second.get()); // remove publisher from client m_publishers.erase(publisherIt); @@ -841,10 +855,7 @@ void ServerImpl::ClientData3::ClientHello(std::string_view self_id, options.prefixMatch = true; sub = std::make_unique( this, std::span{{prefix}}, 0, options); - m_periodMs = std::gcd(m_periodMs, sub->periodMs); - if (m_periodMs < kMinPeriodMs) { - m_periodMs = kMinPeriodMs; - } + m_periodMs = UpdatePeriodCalc(m_periodMs, sub->periodMs); m_setPeriodic(m_periodMs); { @@ -855,7 +866,7 @@ void ServerImpl::ClientData3::ClientHello(std::string_view self_id, topic->lastValue) { DEBUG4("client {}: initial announce of '{}' (id {})", m_id, topic->name, topic->id); - topic->subscribers.Add(sub.get()); + topic->clients[this].AddSubscriber(sub.get()); m_server.UpdateMetaTopicSub(topic.get()); TopicData3* topic3 = GetTopic3(topic.get()); @@ -922,7 +933,7 @@ void ServerImpl::ClientData3::EntryAssign(std::string_view name, } // add publisher to topic - topic->publishers.Add(publisherIt->getSecond().get()); + topic->AddPublisher(this, publisherIt->getSecond().get()); // update meta data m_server.UpdateMetaTopicPub(topic); @@ -972,7 +983,7 @@ void ServerImpl::ClientData3::EntryUpdate(unsigned int id, unsigned int seq_num, std::make_unique(this, topic, topic3->pubuid)); if (isNew) { // add publisher to topic - topic->publishers.Add(publisherIt->getSecond().get()); + topic->AddPublisher(this, publisherIt->getSecond().get()); // update meta data m_server.UpdateMetaTopicPub(topic); @@ -1037,7 +1048,7 @@ void ServerImpl::ClientData3::EntryDelete(unsigned int id) { auto publisherIt = m_publishers.find(topic3it->second.pubuid); if (publisherIt != m_publishers.end()) { // remove publisher from topic - topic->publishers.Remove(publisherIt->second.get()); + topic->RemovePublisher(this, publisherIt->second.get()); // remove publisher from client m_publishers.erase(publisherIt); @@ -1159,8 +1170,6 @@ std::pair ServerImpl::AddClient( clientData->UpdateMetaClientPub(); clientData->UpdateMetaClientSub(); - wire.Flush(); - DEBUG3("AddClient('{}', '{}') -> {}", name, connInfo, index); return {std::move(dedupName), index}; } @@ -1197,17 +1206,14 @@ void ServerImpl::RemoveClient(int clientId) { // remove all publishers and subscribers for this client wpi::SmallVector toDelete; for (auto&& topic : m_topics) { - auto pubRemove = - std::remove_if(topic->publishers.begin(), topic->publishers.end(), - [&](auto&& e) { return e->client == client.get(); }); - bool pubChanged = pubRemove != topic->publishers.end(); - topic->publishers.erase(pubRemove, topic->publishers.end()); - - auto subRemove = - std::remove_if(topic->subscribers.begin(), topic->subscribers.end(), - [&](auto&& e) { return e->client == client.get(); }); - bool subChanged = subRemove != topic->subscribers.end(); - topic->subscribers.erase(subRemove, topic->subscribers.end()); + bool pubChanged = false; + bool subChanged = false; + auto tcdIt = topic->clients.find(client.get()); + if (tcdIt != topic->clients.end()) { + pubChanged = !tcdIt->second.publishers.empty(); + subChanged = !tcdIt->second.subscribers.empty(); + topic->clients.erase(tcdIt); + } if (!topic->IsPublished()) { toDelete.push_back(topic.get()); @@ -1641,15 +1647,17 @@ ServerImpl::TopicData* ServerImpl::CreateTopic(ClientData* client, wpi::SmallVector subscribersBuf; auto subscribers = aClient->GetSubscribers(name, topic->special, subscribersBuf); - for (auto subscriber : subscribers) { - topic->subscribers.Add(subscriber); - } // don't announce to this client if no subscribers if (subscribers.empty()) { continue; } + auto& tcd = topic->clients[aClient.get()]; + for (auto subscriber : subscribers) { + tcd.AddSubscriber(subscriber); + } + if (aClient.get() == client) { continue; // don't announce to requesting client again } @@ -1688,17 +1696,9 @@ void ServerImpl::DeleteTopic(TopicData* topic) { } // unannounce to all subscribers - wpi::SmallVector clients; - clients.resize(m_clients.size()); - for (auto&& sub : topic->subscribers) { - clients[sub->client->GetId()] = true; - } - for (size_t i = 0, iend = clients.size(); i < iend; ++i) { - if (!clients[i]) { - continue; - } - if (auto aClient = m_clients[i].get()) { - aClient->SendUnannounce(topic); + for (auto&& tcd : topic->clients) { + if (!tcd.second.subscribers.empty()) { + tcd.first->SendUnannounce(topic); } } @@ -1755,32 +1755,9 @@ void ServerImpl::SetValue(ClientData* client, TopicData* topic, } } - // propagate to subscribers; as each client may have multiple subscribers, - // but we only want to send the value once, first map to clients and then - // take action based on union of subscriptions - - // indexed by clientId - wpi::SmallVector toSend; - toSend.resize(m_clients.size()); - - for (auto&& subscriber : topic->subscribers) { - int id = subscriber->client->GetId(); - if (subscriber->options.topicsOnly) { - continue; - } else if (subscriber->options.sendAll) { - toSend[id] = ClientData::kSendAll; - } else if (toSend[id] != ClientData::kSendAll) { - toSend[id] = ClientData::kSendNormal; - } - } - - for (size_t i = 0, iend = toSend.size(); i < iend; ++i) { - auto aClient = m_clients[i].get(); - if (!aClient || client == aClient) { - continue; // don't echo back - } - if (toSend[i] != ClientData::kSendDisabled) { - aClient->SendValue(topic, value, toSend[i]); + for (auto&& tcd : topic->clients) { + if (tcd.second.sendMode != ValueSendMode::kDisabled) { + tcd.first->SendValue(topic, value, tcd.second.sendMode); } } } @@ -1811,18 +1788,17 @@ void ServerImpl::UpdateMetaTopicPub(TopicData* topic) { return; } Writer w; - mpack_start_array(&w, topic->publishers.size()); - for (auto&& pub : topic->publishers) { - mpack_start_map(&w, 2); - mpack_write_str(&w, "client"); - if (pub->client) { - mpack_write_str(&w, pub->client->GetName()); - } else { - mpack_write_str(&w, ""); + uint32_t count = 0; + for (auto&& tcd : topic->clients) { + count += tcd.second.publishers.size(); + } + mpack_start_array(&w, count); + for (auto&& tcd : topic->clients) { + for (auto&& pub : tcd.second.publishers) { + mpack_write_object_bytes( + &w, reinterpret_cast(pub->metaTopic.data()), + pub->metaTopic.size()); } - mpack_write_str(&w, "pubuid"); - mpack_write_int(&w, pub->pubuid); - mpack_finish_map(&w); } mpack_finish_array(&w); if (mpack_writer_destroy(&w) == mpack_ok) { @@ -1835,20 +1811,17 @@ void ServerImpl::UpdateMetaTopicSub(TopicData* topic) { return; } Writer w; - mpack_start_array(&w, topic->subscribers.size()); - for (auto&& sub : topic->subscribers) { - mpack_start_map(&w, 3); - mpack_write_str(&w, "client"); - if (sub->client) { - mpack_write_str(&w, sub->client->GetName()); - } else { - mpack_write_str(&w, ""); + uint32_t count = 0; + for (auto&& tcd : topic->clients) { + count += tcd.second.subscribers.size(); + } + mpack_start_array(&w, count); + for (auto&& tcd : topic->clients) { + for (auto&& sub : tcd.second.subscribers) { + mpack_write_object_bytes( + &w, reinterpret_cast(sub->metaTopic.data()), + sub->metaTopic.size()); } - mpack_write_str(&w, "subuid"); - mpack_write_int(&w, sub->subuid); - mpack_write_str(&w, "options"); - WriteOptions(w, sub->options); - mpack_finish_map(&w); } mpack_finish_array(&w); if (mpack_writer_destroy(&w) == mpack_ok) { @@ -1863,41 +1836,23 @@ void ServerImpl::PropertiesChanged(ClientData* client, TopicData* topic, DeleteTopic(topic); } else { // send updated announcement to all subscribers - wpi::SmallVector clients; - clients.resize(m_clients.size()); - for (auto&& sub : topic->subscribers) { - clients[sub->client->GetId()] = true; - } - for (size_t i = 0, iend = clients.size(); i < iend; ++i) { - if (!clients[i]) { - continue; - } - if (auto aClient = m_clients[i].get()) { - aClient->SendPropertiesUpdate(topic, update, aClient == client); - } + for (auto&& tcd : topic->clients) { + tcd.first->SendPropertiesUpdate(topic, update, tcd.first == client); } } } -void ServerImpl::SendControl(uint64_t curTimeMs) { - if (!m_controlReady) { - return; - } - m_controlReady = false; - +void ServerImpl::SendAllOutgoing(uint64_t curTimeMs, bool flush) { for (auto&& client : m_clients) { if (client) { - // to ensure ordering, just send everything - client->SendOutgoing(curTimeMs); - client->Flush(); + client->SendOutgoing(curTimeMs, flush); } } } -void ServerImpl::SendValues(int clientId, uint64_t curTimeMs) { +void ServerImpl::SendOutgoing(int clientId, uint64_t curTimeMs) { if (auto client = m_clients[clientId].get()) { - client->SendOutgoing(curTimeMs); - client->Flush(); + client->SendOutgoing(curTimeMs, false); } } diff --git a/ntcore/src/main/native/cpp/net/ServerImpl.h b/ntcore/src/main/native/cpp/net/ServerImpl.h index b1f367e33bf..3ea5a82a2bf 100644 --- a/ntcore/src/main/native/cpp/net/ServerImpl.h +++ b/ntcore/src/main/native/cpp/net/ServerImpl.h @@ -16,12 +16,17 @@ #include #include +#include #include #include #include +#include "Handle.h" +#include "Log.h" #include "Message.h" #include "NetworkInterface.h" +#include "NetworkOutgoingQueue.h" +#include "NetworkPing.h" #include "PubSubOptions.h" #include "VectorSet.h" #include "WireConnection.h" @@ -57,8 +62,8 @@ class ServerImpl final { explicit ServerImpl(wpi::Logger& logger); - void SendControl(uint64_t curTimeMs); - void SendValues(int clientId, uint64_t curTimeMs); + void SendAllOutgoing(uint64_t curTimeMs, bool flush); + void SendOutgoing(int clientId, uint64_t curTimeMs); void HandleLocal(std::span msgs); void SetLocal(LocalInterface* local); @@ -88,9 +93,75 @@ class ServerImpl final { private: static constexpr uint32_t kMinPeriodMs = 5; + class ClientData; struct PublisherData; struct SubscriberData; - struct TopicData; + + struct TopicData { + TopicData(std::string_view name, std::string_view typeStr) + : name{name}, typeStr{typeStr} {} + TopicData(std::string_view name, std::string_view typeStr, + wpi::json properties) + : name{name}, typeStr{typeStr}, properties(std::move(properties)) { + RefreshProperties(); + } + + bool IsPublished() const { + return persistent || retained || publisherCount != 0; + } + + // returns true if properties changed + bool SetProperties(const wpi::json& update); + void RefreshProperties(); + bool SetFlags(unsigned int flags_); + + NT_Handle GetIdHandle() const { return Handle(0, id, Handle::kTopic); } + + std::string name; + unsigned int id; + Value lastValue; + ClientData* lastValueClient = nullptr; + std::string typeStr; + wpi::json properties = wpi::json::object(); + unsigned int publisherCount{0}; + bool persistent{false}; + bool retained{false}; + bool special{false}; + NT_Topic localHandle{0}; + + void AddPublisher(ClientData* client, PublisherData* pub) { + if (clients[client].publishers.insert(pub).second) { + ++publisherCount; + } + } + + void RemovePublisher(ClientData* client, PublisherData* pub) { + if (clients[client].publishers.erase(pub)) { + --publisherCount; + } + } + + struct TopicClientData { + wpi::SmallPtrSet publishers; + wpi::SmallPtrSet subscribers; + ValueSendMode sendMode = ValueSendMode::kDisabled; + + bool AddSubscriber(SubscriberData* sub) { + bool added = subscribers.insert(sub).second; + if (!sub->options.topicsOnly && sendMode == ValueSendMode::kDisabled) { + sendMode = ValueSendMode::kNormal; + } else if (sub->options.sendAll) { + sendMode = ValueSendMode::kAll; + } + return added; + } + }; + wpi::SmallDenseMap clients; + + // meta topics + TopicData* metaPub = nullptr; + TopicData* metaSub = nullptr; + }; class ClientData { public: @@ -109,16 +180,14 @@ class ServerImpl final { virtual void ProcessIncomingText(std::string_view data) = 0; virtual void ProcessIncomingBinary(std::span data) = 0; - enum SendMode { kSendDisabled = 0, kSendAll, kSendNormal, kSendImmNoFlush }; - virtual void SendValue(TopicData* topic, const Value& value, - SendMode mode) = 0; + ValueSendMode mode) = 0; virtual void SendAnnounce(TopicData* topic, std::optional pubuid) = 0; virtual void SendUnannounce(TopicData* topic) = 0; virtual void SendPropertiesUpdate(TopicData* topic, const wpi::json& update, bool ack) = 0; - virtual void SendOutgoing(uint64_t curTimeMs) = 0; + virtual void SendOutgoing(uint64_t curTimeMs, bool flush) = 0; virtual void Flush() = 0; void UpdateMetaClientPub(); @@ -132,13 +201,14 @@ class ServerImpl final { int GetId() const { return m_id; } protected: + virtual void UpdatePeriodic(TopicData* topic) {} + std::string m_name; std::string m_connInfo; bool m_local; // local to machine ServerImpl::SetPeriodicFunc m_setPeriodic; // TODO: make this per-topic? uint32_t m_periodMs{UINT32_MAX}; - uint64_t m_lastSendMs{0}; ServerImpl& m_server; int m_id; @@ -175,6 +245,9 @@ class ServerImpl final { void ClientSetValue(int64_t pubuid, const Value& value); + virtual void UpdatePeriod(TopicData::TopicClientData& tcd, + TopicData* topic) {} + wpi::DenseMap m_announceSent; }; @@ -186,12 +259,13 @@ class ServerImpl final { void ProcessIncomingText(std::string_view data) final {} void ProcessIncomingBinary(std::span data) final {} - void SendValue(TopicData* topic, const Value& value, SendMode mode) final; + void SendValue(TopicData* topic, const Value& value, + ValueSendMode mode) final; void SendAnnounce(TopicData* topic, std::optional pubuid) final; void SendUnannounce(TopicData* topic) final; void SendPropertiesUpdate(TopicData* topic, const wpi::json& update, bool ack) final; - void SendOutgoing(uint64_t curTimeMs) final {} + void SendOutgoing(uint64_t curTimeMs, bool flush) final {} void Flush() final {} void HandleLocal(std::span msgs); @@ -204,50 +278,31 @@ class ServerImpl final { ServerImpl& server, int id, wpi::Logger& logger) : ClientData4Base{name, connInfo, local, setPeriodic, server, id, logger}, - m_wire{wire} {} + m_wire{wire}, + m_ping{wire}, + m_outgoing{wire, local} {} void ProcessIncomingText(std::string_view data) final; void ProcessIncomingBinary(std::span data) final; - void SendValue(TopicData* topic, const Value& value, SendMode mode) final; + void SendValue(TopicData* topic, const Value& value, + ValueSendMode mode) final; void SendAnnounce(TopicData* topic, std::optional pubuid) final; void SendUnannounce(TopicData* topic) final; void SendPropertiesUpdate(TopicData* topic, const wpi::json& update, bool ack) final; - void SendOutgoing(uint64_t curTimeMs) final; + void SendOutgoing(uint64_t curTimeMs, bool flush) final; + + void Flush() final {} - void Flush() final; + void UpdatePeriod(TopicData::TopicClientData& tcd, TopicData* topic) final; public: WireConnection& m_wire; private: - std::vector m_outgoing; - wpi::DenseMap m_outgoingValueMap; - - bool WriteBinary(int64_t id, int64_t time, const Value& value) { - return WireEncodeBinary(SendBinary().Add(), id, time, value); - } - - TextWriter& SendText() { - m_outBinary.reset(); // ensure proper interleaving of text and binary - if (!m_outText) { - m_outText = m_wire.SendText(); - } - return *m_outText; - } - - BinaryWriter& SendBinary() { - m_outText.reset(); // ensure proper interleaving of text and binary - if (!m_outBinary) { - m_outBinary = m_wire.SendBinary(); - } - return *m_outBinary; - } - - // valid when we are actively writing to this client - std::optional m_outText; - std::optional m_outBinary; + NetworkPing m_ping; + NetworkOutgoingQueue m_outgoing; }; class ClientData3 final : public ClientData, private net3::MessageHandler3 { @@ -265,12 +320,13 @@ class ServerImpl final { void ProcessIncomingText(std::string_view data) final {} void ProcessIncomingBinary(std::span data) final; - void SendValue(TopicData* topic, const Value& value, SendMode mode) final; + void SendValue(TopicData* topic, const Value& value, + ValueSendMode mode) final; void SendAnnounce(TopicData* topic, std::optional pubuid) final; void SendUnannounce(TopicData* topic) final; void SendPropertiesUpdate(TopicData* topic, const wpi::json& update, bool ack) final; - void SendOutgoing(uint64_t curTimeMs) final; + void SendOutgoing(uint64_t curTimeMs, bool flush) final; void Flush() final { m_wire.Flush(); } @@ -305,6 +361,7 @@ class ServerImpl final { std::vector m_outgoing; wpi::DenseMap m_outgoingValueMap; int64_t m_nextPubUid{1}; + uint64_t m_lastSendMs{0}; struct TopicData3 { explicit TopicData3(TopicData* topic) { UpdateFlags(topic); } @@ -323,50 +380,19 @@ class ServerImpl final { } }; - struct TopicData { - TopicData(std::string_view name, std::string_view typeStr) - : name{name}, typeStr{typeStr} {} - TopicData(std::string_view name, std::string_view typeStr, - wpi::json properties) - : name{name}, typeStr{typeStr}, properties(std::move(properties)) { - RefreshProperties(); - } - - bool IsPublished() const { - return persistent || retained || !publishers.empty(); - } - - // returns true if properties changed - bool SetProperties(const wpi::json& update); - void RefreshProperties(); - bool SetFlags(unsigned int flags_); - - std::string name; - unsigned int id; - Value lastValue; - ClientData* lastValueClient = nullptr; - std::string typeStr; - wpi::json properties = wpi::json::object(); - bool persistent{false}; - bool retained{false}; - bool special{false}; - NT_Topic localHandle{0}; - - VectorSet publishers; - VectorSet subscribers; - - // meta topics - TopicData* metaPub = nullptr; - TopicData* metaSub = nullptr; - }; - struct PublisherData { PublisherData(ClientData* client, TopicData* topic, int64_t pubuid) - : client{client}, topic{topic}, pubuid{pubuid} {} + : client{client}, topic{topic}, pubuid{pubuid} { + UpdateMeta(); + } + + void UpdateMeta(); ClientData* client; TopicData* topic; int64_t pubuid; + std::vector metaClient; + std::vector metaTopic; }; struct SubscriberData { @@ -377,6 +403,7 @@ class ServerImpl final { subuid{subuid}, options{options}, periodMs(std::lround(options.periodicMs / 10.0) * 10) { + UpdateMeta(); if (periodMs < kMinPeriodMs) { periodMs = kMinPeriodMs; } @@ -386,6 +413,7 @@ class ServerImpl final { const PubSubOptionsImpl& options_) { topicNames = {topicNames_.begin(), topicNames_.end()}; options = options_; + UpdateMeta(); periodMs = std::lround(options_.periodicMs / 10.0) * 10; if (periodMs < kMinPeriodMs) { periodMs = kMinPeriodMs; @@ -394,10 +422,15 @@ class ServerImpl final { bool Matches(std::string_view name, bool special); + void UpdateMeta(); + ClientData* client; std::vector topicNames; int64_t subuid; PubSubOptionsImpl options; + std::vector metaClient; + std::vector metaTopic; + wpi::DenseMap topics; // in options as double, but copy here as integer; rounded to the nearest // 10 ms uint32_t periodMs; diff --git a/ntcore/src/main/native/cpp/net/WebSocketConnection.cpp b/ntcore/src/main/native/cpp/net/WebSocketConnection.cpp index 24da0373373..986156cdbd0 100644 --- a/ntcore/src/main/native/cpp/net/WebSocketConnection.cpp +++ b/ntcore/src/main/native/cpp/net/WebSocketConnection.cpp @@ -6,134 +6,243 @@ #include +#include #include +#include #include #include +#include using namespace nt; using namespace nt::net; -static constexpr size_t kAllocSize = 4096; -static constexpr size_t kTextFrameRolloverSize = 4096; -static constexpr size_t kBinaryFrameRolloverSize = 8192; -static constexpr size_t kMaxPoolSize = 16; +// MTU - assume Ethernet, IPv6, TCP; does not include WS frame header (max 10) +static constexpr size_t kMTU = 1500 - 40 - 20; +static constexpr size_t kAllocSize = kMTU - 10; +// leave enough room for a "typical" message size so we don't create lots of +// fragmented frames +static constexpr size_t kNewFrameThresholdBytes = kAllocSize - 50; +static constexpr size_t kFlushThresholdFrames = 32; +static constexpr size_t kFlushThresholdBytes = 16384; +static constexpr size_t kMaxPoolSize = 32; -WebSocketConnection::WebSocketConnection(wpi::WebSocket& ws) - : m_ws{ws}, - m_text_os{m_text_buffers, [this] { return AllocBuf(); }}, - m_binary_os{m_binary_buffers, [this] { return AllocBuf(); }} {} +class WebSocketConnection::Stream final : public wpi::raw_ostream { + public: + explicit Stream(WebSocketConnection& conn) : m_conn{conn} { + auto& buf = conn.m_bufs.back(); + SetBuffer(buf.base + buf.len, kAllocSize - buf.len); + } -WebSocketConnection::~WebSocketConnection() { - for (auto&& buf : m_buf_pool) { - buf.Deallocate(); + ~Stream() final { + m_disableAlloc = true; + flush(); } - for (auto&& buf : m_text_buffers) { - buf.Deallocate(); + + private: + size_t preferred_buffer_size() const final { return 0; } + void write_impl(const char* data, size_t len) final; + uint64_t current_pos() const final { return m_conn.m_framePos; } + + WebSocketConnection& m_conn; + bool m_disableAlloc = false; +}; + +void WebSocketConnection::Stream::write_impl(const char* data, size_t len) { + if (len > kAllocSize) { + // only called by raw_ostream::write() when the buffer is empty and a large + // thing is being written; called with a length that's a multiple of the + // alloc size + assert((len % kAllocSize) == 0); + assert(m_conn.m_bufs.back().len == 0); + while (len > 0) { + auto& buf = m_conn.m_bufs.back(); + std::memcpy(buf.base, data, kAllocSize); + buf.len = kAllocSize; + m_conn.m_framePos += kAllocSize; + m_conn.m_written += kAllocSize; + data += kAllocSize; + len -= kAllocSize; + // fragment the current frame and start a new one + m_conn.m_frames.back().opcode &= ~wpi::WebSocket::kFlagFin; + m_conn.StartFrame(wpi::WebSocket::Frame::kFragment); + } + SetBuffer(m_conn.m_bufs.back().base, kAllocSize); + [[unlikely]] return; } - for (auto&& buf : m_binary_buffers) { - buf.Deallocate(); + + auto& buf = m_conn.m_bufs.back(); + buf.len += len; + m_conn.m_framePos += len; + m_conn.m_written += len; + if (!m_disableAlloc && buf.len >= kAllocSize) { + // fragment the current frame and start a new one + [[unlikely]] m_conn.m_frames.back().opcode &= ~wpi::WebSocket::kFlagFin; + m_conn.StartFrame(wpi::WebSocket::Frame::kFragment); + SetBuffer(m_conn.m_bufs.back().base, kAllocSize); } } -void WebSocketConnection::Flush() { - FinishSendText(); - FinishSendBinary(); - if (m_frames.empty()) { - return; - } +WebSocketConnection::WebSocketConnection(wpi::WebSocket& ws, + unsigned int version) + : m_ws{ws}, m_version{version} { + m_ws.pong.connect([this](auto data) { + if (data.size() != 8) { + return; + } + m_lastPingResponse = + wpi::support::endian::read64(data.data()); + }); +} - // convert internal frames into WS frames - m_ws_frames.clear(); - m_ws_frames.reserve(m_frames.size()); - for (auto&& frame : m_frames) { - m_ws_frames.emplace_back(frame.opcode, - std::span{frame.bufs->begin() + frame.start, - frame.bufs->begin() + frame.end}); +WebSocketConnection::~WebSocketConnection() { + for (auto&& buf : m_bufs) { + buf.Deallocate(); } + for (auto&& buf : m_buf_pool) { + buf.Deallocate(); + } +} - ++m_sendsActive; - m_ws.SendFrames(m_ws_frames, [selfweak = weak_from_this()](auto bufs, auto) { +void WebSocketConnection::SendPing(uint64_t time) { + auto buf = AllocBuf(); + buf.len = 8; + wpi::support::endian::write64(buf.base, time); + m_ws.SendPing({buf}, [selfweak = weak_from_this()](auto bufs, auto err) { if (auto self = selfweak.lock()) { - size_t numToPool = - (std::min)(bufs.size(), kMaxPoolSize - self->m_buf_pool.size()); - self->m_buf_pool.insert(self->m_buf_pool.end(), bufs.begin(), - bufs.begin() + numToPool); - for (auto&& buf : bufs.subspan(numToPool)) { - buf.Deallocate(); - } - if (self->m_sendsActive > 0) { - --self->m_sendsActive; - } + self->m_err = err; + self->ReleaseBufs(bufs); } else { for (auto&& buf : bufs) { buf.Deallocate(); } } }); - m_frames.clear(); - m_text_buffers.clear(); - m_binary_buffers.clear(); - m_text_pos = 0; - m_binary_pos = 0; - m_lastFlushTime = wpi::Now(); } -void WebSocketConnection::Disconnect(std::string_view reason) { - m_reason = reason; - m_ws.Close(1005, reason); +void WebSocketConnection::StartFrame(uint8_t opcode) { + m_frames.emplace_back(opcode, m_bufs.size(), m_bufs.size() + 1); + m_bufs.emplace_back(AllocBuf()); + m_bufs.back().len = 0; +} + +void WebSocketConnection::FinishText() { + assert(!m_bufs.empty()); + auto& buf = m_bufs.back(); + assert(buf.len < kAllocSize + 1); // safe because we alloc one more byte + buf.base[buf.len++] = ']'; } -void WebSocketConnection::StartSendText() { - // limit amount per single frame - size_t total = 0; - for (size_t i = m_text_pos; i < m_text_buffers.size(); ++i) { - total += m_text_buffers[i].len; +int WebSocketConnection::Write( + State kind, wpi::function_ref writer) { + bool first = false; + if (m_state != kind || + (m_state == kind && m_framePos >= kNewFrameThresholdBytes)) { + // start a new frame + if (m_state == kText) { + FinishText(); + } + m_state = kind; + if (!m_frames.empty()) { + m_frames.back().opcode |= wpi::WebSocket::kFlagFin; + } + StartFrame(m_state == kText ? wpi::WebSocket::Frame::kText + : wpi::WebSocket::Frame::kBinary); + m_framePos = 0; + first = true; } - if (total >= kTextFrameRolloverSize) { - FinishSendText(); + { + Stream os{*this}; + if (kind == kText) { + os << (first ? '[' : ','); + } + writer(os); } - - if (m_in_text) { - m_text_os << ','; - } else { - m_text_os << '['; - m_in_text = true; + ++m_frames.back().count; + if (m_frames.size() > kFlushThresholdFrames || + m_written >= kFlushThresholdBytes) { + return Flush(); } + return 0; } -void WebSocketConnection::FinishSendText() { - if (m_in_text) { - m_text_os << ']'; - m_in_text = false; +int WebSocketConnection::Flush() { + m_lastFlushTime = wpi::Now(); + if (m_state == kEmpty) { + return 0; } - if (m_text_pos >= m_text_buffers.size()) { - return; + if (m_state == kText) { + FinishText(); } - m_frames.emplace_back(wpi::WebSocket::Frame::kText, &m_text_buffers, - m_text_pos, m_text_buffers.size()); - m_text_pos = m_text_buffers.size(); - m_text_os.reset(); -} + m_state = kEmpty; + m_written = 0; -void WebSocketConnection::StartSendBinary() { - // limit amount per single frame - size_t total = 0; - for (size_t i = m_binary_pos; i < m_binary_buffers.size(); ++i) { - total += m_binary_buffers[i].len; + if (m_frames.empty()) { + return 0; } - if (total >= kBinaryFrameRolloverSize) { - FinishSendBinary(); + m_frames.back().opcode |= wpi::WebSocket::kFlagFin; + + // convert internal frames into WS frames + m_ws_frames.clear(); + m_ws_frames.reserve(m_frames.size()); + for (auto&& frame : m_frames) { + m_ws_frames.emplace_back( + frame.opcode, std::span{&m_bufs[frame.start], &m_bufs[frame.end]}); + } + + auto unsentFrames = m_ws.TrySendFrames( + m_ws_frames, [selfweak = weak_from_this()](auto bufs, auto err) { + if (auto self = selfweak.lock()) { + self->m_err = err; + self->ReleaseBufs(bufs); + } else { + for (auto&& buf : bufs) { + buf.Deallocate(); + } + } + }); + m_ws_frames.clear(); + if (m_err) { + m_frames.clear(); + m_bufs.clear(); + return m_err.code(); + } + + int count = 0; + for (auto&& frame : + wpi::take_back(std::span{m_frames}, unsentFrames.size())) { + count += frame.count; } + m_frames.clear(); + m_bufs.clear(); + return count; } -void WebSocketConnection::FinishSendBinary() { - if (m_binary_pos >= m_binary_buffers.size()) { - return; +void WebSocketConnection::Send( + uint8_t opcode, wpi::function_ref writer) { + wpi::SmallVector bufs; + wpi::raw_uv_ostream os{bufs, [this] { return AllocBuf(); }}; + if (opcode == wpi::WebSocket::Frame::kText) { + os << '['; + } + writer(os); + if (opcode == wpi::WebSocket::Frame::kText) { + os << ']'; } - m_frames.emplace_back(wpi::WebSocket::Frame::kBinary, &m_binary_buffers, - m_binary_pos, m_binary_buffers.size()); - m_binary_pos = m_binary_buffers.size(); - m_binary_os.reset(); + wpi::WebSocket::Frame frame{opcode, os.bufs()}; + m_ws.SendFrames({{frame}}, [selfweak = weak_from_this()](auto bufs, auto) { + if (auto self = selfweak.lock()) { + self->ReleaseBufs(bufs); + } else { + for (auto&& buf : bufs) { + buf.Deallocate(); + } + } + }); +} + +void WebSocketConnection::Disconnect(std::string_view reason) { + m_reason = reason; + m_ws.Fail(1005, reason); } wpi::uv::Buffer WebSocketConnection::AllocBuf() { @@ -142,5 +251,13 @@ wpi::uv::Buffer WebSocketConnection::AllocBuf() { m_buf_pool.pop_back(); return buf; } - return wpi::uv::Buffer::Allocate(kAllocSize); + return wpi::uv::Buffer::Allocate(kAllocSize + 1); // leave space for ']' +} + +void WebSocketConnection::ReleaseBufs(std::span bufs) { + size_t numToPool = (std::min)(bufs.size(), kMaxPoolSize - m_buf_pool.size()); + m_buf_pool.insert(m_buf_pool.end(), bufs.begin(), bufs.begin() + numToPool); + for (auto&& buf : bufs.subspan(numToPool)) { + buf.Deallocate(); + } } diff --git a/ntcore/src/main/native/cpp/net/WebSocketConnection.h b/ntcore/src/main/native/cpp/net/WebSocketConnection.h index 8a1d243f42f..3e76c3450ca 100644 --- a/ntcore/src/main/native/cpp/net/WebSocketConnection.h +++ b/ntcore/src/main/native/cpp/net/WebSocketConnection.h @@ -9,9 +9,8 @@ #include #include -#include +#include #include -#include #include #include "WireConnection.h" @@ -22,56 +21,77 @@ class WebSocketConnection final : public WireConnection, public std::enable_shared_from_this { public: - explicit WebSocketConnection(wpi::WebSocket& ws); + WebSocketConnection(wpi::WebSocket& ws, unsigned int version); ~WebSocketConnection() override; WebSocketConnection(const WebSocketConnection&) = delete; WebSocketConnection& operator=(const WebSocketConnection&) = delete; - bool Ready() const final { return m_sendsActive == 0; } + unsigned int GetVersion() const final { return m_version; } - TextWriter SendText() final { return {m_text_os, *this}; } - BinaryWriter SendBinary() final { return {m_binary_os, *this}; } + void SendPing(uint64_t time) final; - void Flush() final; + bool Ready() const final { return !m_ws.IsWriteInProgress(); } + + int WriteText(wpi::function_ref writer) final { + return Write(kText, writer); + } + int WriteBinary(wpi::function_ref writer) final { + return Write(kBinary, writer); + } + int Flush() final; + + void SendText(wpi::function_ref writer) final { + Send(wpi::WebSocket::Frame::kText, writer); + } + void SendBinary(wpi::function_ref writer) final { + Send(wpi::WebSocket::Frame::kBinary, writer); + } uint64_t GetLastFlushTime() const final { return m_lastFlushTime; } + uint64_t GetLastPingResponse() const final { return m_lastPingResponse; } + void Disconnect(std::string_view reason) final; std::string_view GetDisconnectReason() const { return m_reason; } private: - void StartSendText() final; - void FinishSendText() final; - void StartSendBinary() final; - void FinishSendBinary() final; + enum State { kEmpty, kText, kBinary }; + int Write(State kind, wpi::function_ref writer); + void Send(uint8_t opcode, + wpi::function_ref writer); + + void StartFrame(uint8_t opcode); + void FinishText(); wpi::uv::Buffer AllocBuf(); + void ReleaseBufs(std::span bufs); wpi::WebSocket& m_ws; + + class Stream; + // Can't use WS frames directly as span could have dangling pointers struct Frame { - Frame(uint8_t opcode, wpi::SmallVectorImpl* bufs, - size_t start, size_t end) - : opcode{opcode}, bufs{bufs}, start{start}, end{end} {} - uint8_t opcode; - wpi::SmallVectorImpl* bufs; + Frame(uint8_t opcode, size_t start, size_t end) + : start{start}, end{end}, opcode{opcode} {} size_t start; size_t end; + unsigned int count = 0; + uint8_t opcode; }; - std::vector m_frames; std::vector m_ws_frames; // to reduce allocs - wpi::SmallVector m_text_buffers; - wpi::SmallVector m_binary_buffers; + std::vector m_frames; + std::vector m_bufs; std::vector m_buf_pool; - wpi::raw_uv_ostream m_text_os; - wpi::raw_uv_ostream m_binary_os; - size_t m_text_pos = 0; - size_t m_binary_pos = 0; - bool m_in_text = false; - int m_sendsActive = 0; + size_t m_framePos = 0; + size_t m_written = 0; + wpi::uv::Error m_err; + State m_state = kEmpty; std::string m_reason; uint64_t m_lastFlushTime = 0; + uint64_t m_lastPingResponse = 0; + unsigned int m_version; }; } // namespace nt::net diff --git a/ntcore/src/main/native/cpp/net/WireConnection.h b/ntcore/src/main/native/cpp/net/WireConnection.h index e4571b99ca4..2c876cc29f3 100644 --- a/ntcore/src/main/native/cpp/net/WireConnection.h +++ b/ntcore/src/main/native/cpp/net/WireConnection.h @@ -8,105 +8,53 @@ #include -#include +#include -namespace nt::net { +namespace wpi { +class raw_ostream; +} // namespace wpi -class BinaryWriter; -class TextWriter; +namespace nt::net { class WireConnection { - friend class TextWriter; - friend class BinaryWriter; - public: virtual ~WireConnection() = default; - virtual bool Ready() const = 0; + virtual unsigned int GetVersion() const = 0; - virtual TextWriter SendText() = 0; + virtual void SendPing(uint64_t time) = 0; - virtual BinaryWriter SendBinary() = 0; + virtual bool Ready() const = 0; - virtual void Flush() = 0; + // These return <0 on error, 0 on success. On buffer full, a positive number + // is is returned indicating the number of previous messages (including this + // call) that were NOT sent, e.g. 1 if just this call to WriteText or + // WriteBinary was not sent, 2 if the this call and the *previous* call were + // not sent. + [[nodiscard]] + virtual int WriteText( + wpi::function_ref writer) = 0; + [[nodiscard]] + virtual int WriteBinary( + wpi::function_ref writer) = 0; + + // Flushes any pending buffers. Return value equivalent to + // WriteText/WriteBinary (e.g. 1 means the last WriteX call was not sent). + [[nodiscard]] + virtual int Flush() = 0; + + // These immediately send the data even if the buffer is full. + virtual void SendText( + wpi::function_ref writer) = 0; + virtual void SendBinary( + wpi::function_ref writer) = 0; virtual uint64_t GetLastFlushTime() const = 0; // in microseconds - virtual void Disconnect(std::string_view reason) = 0; + // Gets the timestamp of the last ping we got a reply to + virtual uint64_t GetLastPingResponse() const = 0; // in microseconds - protected: - virtual void StartSendText() = 0; - virtual void FinishSendText() = 0; - virtual void StartSendBinary() = 0; - virtual void FinishSendBinary() = 0; -}; - -class TextWriter { - public: - TextWriter(wpi::raw_ostream& os, WireConnection& wire) - : m_os{&os}, m_wire{&wire} {} - TextWriter(const TextWriter&) = delete; - TextWriter(TextWriter&& rhs) : m_os{rhs.m_os}, m_wire{rhs.m_wire} { - rhs.m_os = nullptr; - rhs.m_wire = nullptr; - } - TextWriter& operator=(const TextWriter&) = delete; - TextWriter& operator=(TextWriter&& rhs) { - m_os = rhs.m_os; - m_wire = rhs.m_wire; - rhs.m_os = nullptr; - rhs.m_wire = nullptr; - return *this; - } - ~TextWriter() { - if (m_os) { - m_wire->FinishSendText(); - } - } - - wpi::raw_ostream& Add() { - m_wire->StartSendText(); - return *m_os; - } - WireConnection& wire() { return *m_wire; } - - private: - wpi::raw_ostream* m_os; - WireConnection* m_wire; -}; - -class BinaryWriter { - public: - BinaryWriter(wpi::raw_ostream& os, WireConnection& wire) - : m_os{&os}, m_wire{&wire} {} - BinaryWriter(const BinaryWriter&) = delete; - BinaryWriter(BinaryWriter&& rhs) : m_os{rhs.m_os}, m_wire{rhs.m_wire} { - rhs.m_os = nullptr; - rhs.m_wire = nullptr; - } - BinaryWriter& operator=(const BinaryWriter&) = delete; - BinaryWriter& operator=(BinaryWriter&& rhs) { - m_os = rhs.m_os; - m_wire = rhs.m_wire; - rhs.m_os = nullptr; - rhs.m_wire = nullptr; - return *this; - } - ~BinaryWriter() { - if (m_wire) { - m_wire->FinishSendBinary(); - } - } - - wpi::raw_ostream& Add() { - m_wire->StartSendBinary(); - return *m_os; - } - WireConnection& wire() { return *m_wire; } - - private: - wpi::raw_ostream* m_os; - WireConnection* m_wire; + virtual void Disconnect(std::string_view reason) = 0; }; } // namespace nt::net diff --git a/ntcore/src/main/native/cpp/net/WireDecoder.cpp b/ntcore/src/main/native/cpp/net/WireDecoder.cpp index 0ce80178192..536a62fb0c8 100644 --- a/ntcore/src/main/native/cpp/net/WireDecoder.cpp +++ b/ntcore/src/main/native/cpp/net/WireDecoder.cpp @@ -107,21 +107,22 @@ static bool ObjGetStringArray(wpi::json::object_t& obj, std::string_view key, template requires(std::same_as || std::same_as) -static void WireDecodeTextImpl(std::string_view in, T& out, +static bool WireDecodeTextImpl(std::string_view in, T& out, wpi::Logger& logger) { wpi::json j; try { j = wpi::json::parse(in); } catch (wpi::json::parse_error& err) { WPI_WARNING(logger, "could not decode JSON message: {}", err.what()); - return; + return false; } if (!j.is_array()) { WPI_WARNING(logger, "expected JSON array at top level"); - return; + return false; } + bool rv = false; int i = -1; for (auto&& jmsg : j) { ++i; @@ -187,6 +188,7 @@ static void WireDecodeTextImpl(std::string_view in, T& out, // complete out.ClientPublish(pubuid, *name, *typeStr, *properties); + rv = true; } else if (*method == UnpublishMsg::kMethodStr) { // pubuid int64_t pubuid; @@ -196,6 +198,7 @@ static void WireDecodeTextImpl(std::string_view in, T& out, // complete out.ClientUnpublish(pubuid); + rv = true; } else if (*method == SetPropertiesMsg::kMethodStr) { // name auto name = ObjGetString(*params, "name", &error); @@ -288,6 +291,7 @@ static void WireDecodeTextImpl(std::string_view in, T& out, // complete out.ClientSubscribe(subuid, topicNames, options); + rv = true; } else if (*method == UnsubscribeMsg::kMethodStr) { // subuid int64_t subuid; @@ -297,6 +301,7 @@ static void WireDecodeTextImpl(std::string_view in, T& out, // complete out.ClientUnsubscribe(subuid); + rv = true; } else { error = fmt::format("unrecognized method '{}'", *method); goto err; @@ -404,15 +409,17 @@ static void WireDecodeTextImpl(std::string_view in, T& out, err: WPI_WARNING(logger, "{}: {}", i, error); } + + return rv; } #ifdef __clang__ #pragma clang diagnostic pop #endif -void nt::net::WireDecodeText(std::string_view in, ClientMessageHandler& out, +bool nt::net::WireDecodeText(std::string_view in, ClientMessageHandler& out, wpi::Logger& logger) { - ::WireDecodeTextImpl(in, out, logger); + return ::WireDecodeTextImpl(in, out, logger); } void nt::net::WireDecodeText(std::string_view in, ServerMessageHandler& out, diff --git a/ntcore/src/main/native/cpp/net/WireDecoder.h b/ntcore/src/main/native/cpp/net/WireDecoder.h index baee7241d0f..b4f7b4e157e 100644 --- a/ntcore/src/main/native/cpp/net/WireDecoder.h +++ b/ntcore/src/main/native/cpp/net/WireDecoder.h @@ -52,7 +52,8 @@ class ServerMessageHandler { const wpi::json& update, bool ack) = 0; }; -void WireDecodeText(std::string_view in, ClientMessageHandler& out, +// return true if client pub/sub metadata needs updating +bool WireDecodeText(std::string_view in, ClientMessageHandler& out, wpi::Logger& logger); void WireDecodeText(std::string_view in, ServerMessageHandler& out, wpi::Logger& logger); diff --git a/ntcore/src/test/java/edu/wpi/first/networktables/TimeSyncTest.java b/ntcore/src/test/java/edu/wpi/first/networktables/TimeSyncTest.java index c539d2027d9..9048ef44019 100644 --- a/ntcore/src/test/java/edu/wpi/first/networktables/TimeSyncTest.java +++ b/ntcore/src/test/java/edu/wpi/first/networktables/TimeSyncTest.java @@ -34,29 +34,30 @@ void testLocal() { @Test void testServer() { - var poller = new NetworkTableListenerPoller(m_inst); - poller.addTimeSyncListener(false); - - m_inst.startServer("timesynctest.json", "127.0.0.1", 0, 10030); - var offset = m_inst.getServerTimeOffset(); - assertTrue(offset.isPresent()); - assertEquals(0L, offset.getAsLong()); - - NetworkTableEvent[] events = poller.readQueue(); - assertEquals(1, events.length); - assertNotNull(events[0].timeSyncData); - assertTrue(events[0].timeSyncData.valid); - assertEquals(0L, events[0].timeSyncData.serverTimeOffset); - assertEquals(0L, events[0].timeSyncData.rtt2); - - m_inst.stopServer(); - offset = m_inst.getServerTimeOffset(); - assertFalse(offset.isPresent()); - - events = poller.readQueue(); - assertEquals(1, events.length); - assertNotNull(events[0].timeSyncData); - assertFalse(events[0].timeSyncData.valid); + try (var poller = new NetworkTableListenerPoller(m_inst)) { + poller.addTimeSyncListener(false); + + m_inst.startServer("timesynctest.json", "127.0.0.1", 0, 10030); + var offset = m_inst.getServerTimeOffset(); + assertTrue(offset.isPresent()); + assertEquals(0L, offset.getAsLong()); + + NetworkTableEvent[] events = poller.readQueue(); + assertEquals(1, events.length); + assertNotNull(events[0].timeSyncData); + assertTrue(events[0].timeSyncData.valid); + assertEquals(0L, events[0].timeSyncData.serverTimeOffset); + assertEquals(0L, events[0].timeSyncData.rtt2); + + m_inst.stopServer(); + offset = m_inst.getServerTimeOffset(); + assertFalse(offset.isPresent()); + + events = poller.readQueue(); + assertEquals(1, events.length); + assertNotNull(events[0].timeSyncData); + assertFalse(events[0].timeSyncData.valid); + } } @Test diff --git a/ntcore/src/test/native/cpp/net/MockWireConnection.cpp b/ntcore/src/test/native/cpp/net/MockWireConnection.cpp deleted file mode 100644 index a5ddb1fa9cb..00000000000 --- a/ntcore/src/test/native/cpp/net/MockWireConnection.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "MockWireConnection.h" - -using namespace nt::net; - -void MockWireConnection::StartSendText() { - if (m_in_text) { - m_text_os << ','; - } else { - m_text_os << '['; - m_in_text = true; - } -} - -void MockWireConnection::FinishSendText() { - if (m_in_text) { - m_text_os << ']'; - m_in_text = false; - } - m_text_os.flush(); - Text(m_text); - m_text.clear(); -} diff --git a/ntcore/src/test/native/cpp/net/MockWireConnection.h b/ntcore/src/test/native/cpp/net/MockWireConnection.h index 578bee0c3be..797ca253b7d 100644 --- a/ntcore/src/test/native/cpp/net/MockWireConnection.h +++ b/ntcore/src/test/native/cpp/net/MockWireConnection.h @@ -20,37 +20,52 @@ namespace nt::net { class MockWireConnection : public WireConnection { public: - MockWireConnection() : m_text_os{m_text}, m_binary_os{m_binary} {} + MOCK_METHOD(unsigned int, GetVersion, (), (const, override)); + + MOCK_METHOD(void, SendPing, (uint64_t time), (override)); MOCK_METHOD(bool, Ready, (), (const, override)); - TextWriter SendText() override { return {m_text_os, *this}; } - BinaryWriter SendBinary() override { return {m_binary_os, *this}; } + int WriteText(wpi::function_ref writer) override { + std::string text; + wpi::raw_string_ostream os{text}; + writer(os); + return DoWriteText(text); + } + int WriteBinary( + wpi::function_ref writer) override { + std::vector binary; + wpi::raw_uvector_ostream os{binary}; + writer(os); + return DoWriteBinary(binary); + } + + void SendText(wpi::function_ref writer) override { + std::string text; + wpi::raw_string_ostream os{text}; + writer(os); + DoSendText(text); + } + void SendBinary( + wpi::function_ref writer) override { + std::vector binary; + wpi::raw_uvector_ostream os{binary}; + writer(os); + DoSendBinary(binary); + } + + MOCK_METHOD(int, DoWriteText, (std::string_view contents)); + MOCK_METHOD(int, DoWriteBinary, (std::span contents)); - MOCK_METHOD(void, Text, (std::string_view contents)); - MOCK_METHOD(void, Binary, (std::span contents)); + MOCK_METHOD(void, DoSendText, (std::string_view contents)); + MOCK_METHOD(void, DoSendBinary, (std::span contents)); - MOCK_METHOD(void, Flush, (), (override)); + MOCK_METHOD(int, Flush, (), (override)); MOCK_METHOD(uint64_t, GetLastFlushTime, (), (const, override)); + MOCK_METHOD(uint64_t, GetLastPingResponse, (), (const, override)); MOCK_METHOD(void, Disconnect, (std::string_view reason), (override)); - - protected: - void StartSendText() override; - void FinishSendText() override; - void StartSendBinary() override {} - void FinishSendBinary() override { - Binary(m_binary); - m_binary.resize(0); - } - - private: - std::string m_text; - wpi::raw_string_ostream m_text_os; - std::vector m_binary; - wpi::raw_uvector_ostream m_binary_os; - bool m_in_text{false}; }; } // namespace nt::net diff --git a/ntcore/src/test/native/cpp/net/ServerImplTest.cpp b/ntcore/src/test/native/cpp/net/ServerImplTest.cpp index 41951efba0e..64238111eed 100644 --- a/ntcore/src/test/native/cpp/net/ServerImplTest.cpp +++ b/ntcore/src/test/native/cpp/net/ServerImplTest.cpp @@ -50,7 +50,7 @@ class ServerImplTest : public ::testing::Test { TEST_F(ServerImplTest, AddClient) { ::testing::StrictMock wire; - EXPECT_CALL(wire, Flush()); + // EXPECT_CALL(wire, Flush()); MockSetPeriodicFunc setPeriodic; auto [name, id] = server.AddClient("test", "connInfo", false, wire, setPeriodic.AsStdFunction()); @@ -63,8 +63,8 @@ TEST_F(ServerImplTest, AddDuplicateClient) { ::testing::StrictMock wire2; MockSetPeriodicFunc setPeriodic1; MockSetPeriodicFunc setPeriodic2; - EXPECT_CALL(wire1, Flush()); - EXPECT_CALL(wire2, Flush()); + // EXPECT_CALL(wire1, Flush()); + // EXPECT_CALL(wire2, Flush()); auto [name1, id1] = server.AddClient("test", "connInfo", false, wire1, setPeriodic1.AsStdFunction()); @@ -79,6 +79,14 @@ TEST_F(ServerImplTest, AddDuplicateClient) { TEST_F(ServerImplTest, AddClient3) {} +template +static std::string EncodeText1(const T& msg) { + std::string data; + wpi::raw_string_ostream os{data}; + net::WireEncodeText(os, msg); + return data; +} + template static std::string EncodeText(const T& msgs) { std::string data; @@ -97,6 +105,23 @@ static std::string EncodeText(const T& msgs) { return data; } +template +static std::vector EncodeServerBinary1(const T& msg) { + std::vector data; + wpi::raw_uvector_ostream os{data}; + if constexpr (std::same_as) { + if (auto m = std::get_if(&msg.contents)) { + net::WireEncodeBinary(os, m->topic, m->value.time(), m->value); + } + } else if constexpr (std::same_as) { + if (auto m = std::get_if(&msg.contents)) { + net::WireEncodeBinary(os, Handle{m->pubHandle}.GetIndex(), + m->value.time(), m->value); + } + } + return data; +} + template static std::vector EncodeServerBinary(const T& msgs) { std::vector data; @@ -150,29 +175,30 @@ TEST_F(ServerImplTest, PublishLocal) { // subscribes ::testing::StrictMock wire; MockSetPeriodicFunc setPeriodic; + EXPECT_CALL(wire, GetVersion()).WillRepeatedly(Return(0x0401)); { ::testing::InSequence seq; - EXPECT_CALL(wire, Flush()); // AddClient() - EXPECT_CALL(setPeriodic, Call(100)); // ClientSubscribe() - EXPECT_CALL(wire, Flush()); // ClientSubscribe() + // EXPECT_CALL(wire, Flush()).WillOnce(Return(0)); // AddClient() + EXPECT_CALL(setPeriodic, Call(100)); // ClientSubscribe() + // EXPECT_CALL(wire, Flush()).WillOnce(Return(0)); // ClientSubscribe() + EXPECT_CALL(wire, GetLastPingResponse()).WillOnce(Return(0)); + EXPECT_CALL(wire, SendPing(100)); EXPECT_CALL(wire, Ready()).WillOnce(Return(true)); // SendControl() - { - std::vector smsgs; - smsgs.emplace_back(net::ServerMessage{net::AnnounceMsg{ - "test", 3, "double", std::nullopt, wpi::json::object()}}); - smsgs.emplace_back(net::ServerMessage{net::AnnounceMsg{ - "test2", 8, "double", std::nullopt, wpi::json::object()}}); - EXPECT_CALL(wire, Text(StrEq(EncodeText(smsgs)))); // SendControl() - } - EXPECT_CALL(wire, Flush()); // SendControl() + EXPECT_CALL( + wire, DoWriteText(StrEq(EncodeText1(net::ServerMessage{net::AnnounceMsg{ + "test", 3, "double", std::nullopt, wpi::json::object()}})))) + .WillOnce(Return(0)); + EXPECT_CALL( + wire, DoWriteText(StrEq(EncodeText1(net::ServerMessage{net::AnnounceMsg{ + "test2", 8, "double", std::nullopt, wpi::json::object()}})))) + .WillOnce(Return(0)); + EXPECT_CALL(wire, Flush()).WillOnce(Return(0)); // SendControl() EXPECT_CALL(wire, Ready()).WillOnce(Return(true)); // SendControl() - { - std::vector smsgs; - smsgs.emplace_back(net::ServerMessage{net::AnnounceMsg{ - "test3", 11, "double", std::nullopt, wpi::json::object()}}); - EXPECT_CALL(wire, Text(StrEq(EncodeText(smsgs)))); // SendControl() - } - EXPECT_CALL(wire, Flush()); // SendControl() + EXPECT_CALL( + wire, DoWriteText(StrEq(EncodeText1(net::ServerMessage{net::AnnounceMsg{ + "test3", 11, "double", std::nullopt, wpi::json::object()}})))) + .WillOnce(Return(0)); + EXPECT_CALL(wire, Flush()).WillOnce(Return(0)); // SendControl() } auto [name, id] = server.AddClient("test", "connInfo", false, wire, setPeriodic.AsStdFunction()); @@ -193,7 +219,7 @@ TEST_F(ServerImplTest, PublishLocal) { server.HandleLocal(msgs); } - server.SendControl(100); + server.SendAllOutgoing(100, false); // publish after send control { @@ -203,7 +229,7 @@ TEST_F(ServerImplTest, PublishLocal) { server.HandleLocal(msgs); } - server.SendControl(200); + server.SendAllOutgoing(200, false); } TEST_F(ServerImplTest, ClientSubTopicOnlyThenValue) { @@ -225,31 +251,28 @@ TEST_F(ServerImplTest, ClientSubTopicOnlyThenValue) { } ::testing::StrictMock wire; + EXPECT_CALL(wire, GetVersion()).WillRepeatedly(Return(0x0401)); MockSetPeriodicFunc setPeriodic; { ::testing::InSequence seq; - EXPECT_CALL(wire, Flush()); // AddClient() - EXPECT_CALL(setPeriodic, Call(100)); // ClientSubscribe() - EXPECT_CALL(wire, Flush()); // ClientSubscribe() + // EXPECT_CALL(wire, Flush()).WillOnce(Return(0)); // AddClient() + EXPECT_CALL(setPeriodic, Call(100)); // ClientSubscribe() + // EXPECT_CALL(wire, Flush()).WillOnce(Return(0)); // ClientSubscribe() + EXPECT_CALL(wire, GetLastPingResponse()).WillOnce(Return(0)); + EXPECT_CALL(wire, SendPing(100)); EXPECT_CALL(wire, Ready()).WillOnce(Return(true)); // SendValues() - { - std::vector smsgs; - smsgs.emplace_back(net::ServerMessage{net::AnnounceMsg{ - "test", 3, "double", std::nullopt, wpi::json::object()}}); - EXPECT_CALL(wire, Text(StrEq(EncodeText(smsgs)))); // SendValues() - } - EXPECT_CALL(wire, Flush()); // SendValues() - EXPECT_CALL(setPeriodic, Call(100)); // ClientSubscribe() - EXPECT_CALL(wire, Flush()); // ClientSubscribe() + EXPECT_CALL( + wire, DoWriteText(StrEq(EncodeText1(net::ServerMessage{net::AnnounceMsg{ + "test", 3, "double", std::nullopt, wpi::json::object()}})))) + .WillOnce(Return(0)); + EXPECT_CALL(wire, Flush()).WillOnce(Return(0)); // SendValues() + EXPECT_CALL(setPeriodic, Call(100)); // ClientSubscribe() + // EXPECT_CALL(wire, Flush()).WillOnce(Return(0)); // ClientSubscribe() EXPECT_CALL(wire, Ready()).WillOnce(Return(true)); // SendValues() - { - std::vector smsgs; - smsgs.emplace_back(net::ServerMessage{ - net::ServerValueMsg{3, Value::MakeDouble(1.0, 10)}}); - EXPECT_CALL( - wire, - Binary(wpi::SpanEq(EncodeServerBinary(smsgs)))); // SendValues() - } + EXPECT_CALL( + wire, DoWriteBinary(wpi::SpanEq(EncodeServerBinary1(net::ServerMessage{ + net::ServerValueMsg{3, Value::MakeDouble(1.0, 10)}})))) + .WillOnce(Return(0)); EXPECT_CALL(wire, Flush()); // SendValues() } @@ -268,7 +291,7 @@ TEST_F(ServerImplTest, ClientSubTopicOnlyThenValue) { server.ProcessIncomingText(id, EncodeText(msgs)); } - server.SendValues(id, 100); + server.SendOutgoing(id, 100); // subscribe normal; will not resend announcement, but will send value { @@ -279,7 +302,7 @@ TEST_F(ServerImplTest, ClientSubTopicOnlyThenValue) { server.ProcessIncomingText(id, EncodeText(msgs)); } - server.SendValues(id, 200); + server.SendOutgoing(id, 200); } TEST_F(ServerImplTest, ZeroTimestampNegativeTime) { @@ -319,7 +342,7 @@ TEST_F(ServerImplTest, ZeroTimestampNegativeTime) { MockSetPeriodicFunc setPeriodic; { ::testing::InSequence seq; - EXPECT_CALL(wire, Flush()); // AddClient() + // EXPECT_CALL(wire, Flush()); // AddClient() } auto [name, id] = server.AddClient("test", "connInfo", false, wire, setPeriodic.AsStdFunction()); From b581f850d94dd61eb91bc9bda96053368ed3beed Mon Sep 17 00:00:00 2001 From: Ryan Blue Date: Thu, 5 Oct 2023 01:09:28 -0400 Subject: [PATCH 37/76] [commands] Clean up C++ includes after Requirements was added (#5719) --- .../src/main/native/include/frc2/command/Command.h | 2 -- .../src/main/native/include/frc2/command/CommandPtr.h | 3 +-- .../src/main/native/include/frc2/command/Commands.h | 3 +-- .../main/native/include/frc2/command/FunctionalCommand.h | 3 +-- .../src/main/native/include/frc2/command/InstantCommand.h | 3 +-- .../native/include/frc2/command/MecanumControllerCommand.h | 7 +++---- .../src/main/native/include/frc2/command/NotifierCommand.h | 3 +-- .../src/main/native/include/frc2/command/PIDCommand.h | 3 +-- .../main/native/include/frc2/command/ProfiledPIDCommand.h | 3 +-- .../src/main/native/include/frc2/command/ProxyCommand.h | 1 - .../src/main/native/include/frc2/command/RamseteCommand.h | 3 +-- .../src/main/native/include/frc2/command/RunCommand.h | 3 +-- .../native/include/frc2/command/SequentialCommandGroup.h | 1 - .../src/main/native/include/frc2/command/StartEndCommand.h | 3 +-- .../native/include/frc2/command/SwerveControllerCommand.h | 7 +++---- .../native/include/frc2/command/TrapezoidProfileCommand.h | 3 +-- .../src/main/native/include/frc2/command/button/Trigger.h | 2 -- .../src/test/native/cpp/frc2/command/CommandTestBase.h | 5 +++-- 18 files changed, 20 insertions(+), 38 deletions(-) diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h index 0d13ef33269..414785f2056 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h @@ -5,9 +5,7 @@ #pragma once #include -#include #include -#include #include #include diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h index fa29f9a7e40..ec21b511922 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h @@ -6,14 +6,13 @@ #include #include -#include #include -#include #include #include #include #include "frc2/command/Command.h" +#include "frc2/command/Requirements.h" namespace frc2 { /** diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Commands.h b/wpilibNewCommands/src/main/native/include/frc2/command/Commands.h index 7653188dc65..beefcd8c3e2 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/Commands.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/Commands.h @@ -6,15 +6,14 @@ #include #include -#include #include -#include #include #include #include #include #include "frc2/command/CommandPtr.h" +#include "frc2/command/Requirements.h" #include "frc2/command/SelectCommand.h" namespace frc2 { diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h index 5fa57f17137..ac117656757 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h @@ -5,11 +5,10 @@ #pragma once #include -#include -#include #include "frc2/command/Command.h" #include "frc2/command/CommandHelper.h" +#include "frc2/command/Requirements.h" namespace frc2 { /** diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h index 1e552cc4473..3ac32ecb0a9 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h @@ -5,11 +5,10 @@ #pragma once #include -#include -#include #include "frc2/command/CommandHelper.h" #include "frc2/command/FunctionalCommand.h" +#include "frc2/command/Requirements.h" namespace frc2 { /** diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h index 9a88186dcda..e189ef1b95e 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h @@ -4,9 +4,7 @@ #include #include -#include #include -#include #include #include @@ -23,8 +21,9 @@ #include #include -#include "Command.h" -#include "CommandHelper.h" +#include "frc2/command/Command.h" +#include "frc2/command/CommandHelper.h" +#include "frc2/command/Requirements.h" #pragma once diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h index 9bb063f56a3..d17867b1dc7 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h @@ -5,14 +5,13 @@ #pragma once #include -#include -#include #include #include #include "frc2/command/Command.h" #include "frc2/command/CommandHelper.h" +#include "frc2/command/Requirements.h" namespace frc2 { /** diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h index 4d2648b298b..bede6d0d514 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h @@ -5,13 +5,12 @@ #pragma once #include -#include -#include #include #include "frc2/command/Command.h" #include "frc2/command/CommandHelper.h" +#include "frc2/command/Requirements.h" namespace frc2 { /** diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h index 01aa90a8ee6..9ea5db5c7e3 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h @@ -5,8 +5,6 @@ #pragma once #include -#include -#include #include #include @@ -14,6 +12,7 @@ #include "frc2/command/Command.h" #include "frc2/command/CommandHelper.h" +#include "frc2/command/Requirements.h" namespace frc2 { /** diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProxyCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProxyCommand.h index d39ef054b0c..3b7eecc6741 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/ProxyCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProxyCommand.h @@ -5,7 +5,6 @@ #pragma once #include -#include #include diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h index 1339bb15646..56fb8ff92dd 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h @@ -5,9 +5,7 @@ #pragma once #include -#include #include -#include #include #include @@ -21,6 +19,7 @@ #include "frc2/command/Command.h" #include "frc2/command/CommandHelper.h" +#include "frc2/command/Requirements.h" namespace frc2 { /** diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h index 021ba4009f0..49e4be55484 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h @@ -5,11 +5,10 @@ #pragma once #include -#include -#include #include "frc2/command/CommandHelper.h" #include "frc2/command/FunctionalCommand.h" +#include "frc2/command/Requirements.h" namespace frc2 { /** diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h b/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h index dac6cc7d45b..fdcd1bc22fc 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h @@ -12,7 +12,6 @@ #include #include #include -#include #include #include #include diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h index 20d890a10db..2367be8866e 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h @@ -5,11 +5,10 @@ #pragma once #include -#include -#include #include "frc2/command/CommandHelper.h" #include "frc2/command/FunctionalCommand.h" +#include "frc2/command/Requirements.h" namespace frc2 { /** diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h index 835a0894216..ae928b8b4a9 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h @@ -4,9 +4,7 @@ #include #include -#include #include -#include #include #include @@ -21,8 +19,9 @@ #include #include -#include "Command.h" -#include "CommandHelper.h" +#include "frc2/command/Command.h" +#include "frc2/command/CommandHelper.h" +#include "frc2/command/Requirements.h" #pragma once diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h index ccbb07b6615..69cc8d8b238 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h @@ -5,14 +5,13 @@ #pragma once #include -#include -#include #include #include #include "frc2/command/Command.h" #include "frc2/command/CommandHelper.h" +#include "frc2/command/Requirements.h" namespace frc2 { /** diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h index c4433a58135..533e0a6787b 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h @@ -6,9 +6,7 @@ #include #include -#include #include -#include #include #include diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h index 8fb507072ea..586432dff05 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h @@ -13,6 +13,7 @@ #include "frc2/command/CommandHelper.h" #include "frc2/command/CommandScheduler.h" +#include "frc2/command/Requirements.h" #include "frc2/command/SubsystemBase.h" #include "gmock/gmock.h" #include "make_vector.h" @@ -50,8 +51,8 @@ class MockCommand : public CommandHelper { .WillRepeatedly(::testing::Return(true)); } - MockCommand(std::initializer_list requirements, - bool finished = false, bool runWhenDisabled = true) { + explicit MockCommand(Requirements requirements, bool finished = false, + bool runWhenDisabled = true) { m_requirements.insert(requirements.begin(), requirements.end()); EXPECT_CALL(*this, GetRequirements()) .WillRepeatedly(::testing::Return(m_requirements)); From 4095c468003b1583e621903a595eb772b4274c1c Mon Sep 17 00:00:00 2001 From: Thad House Date: Thu, 5 Oct 2023 10:39:11 -0700 Subject: [PATCH 38/76] [build] Update enterprise plugin (#5730) Removes a deprecation warning --- settings.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/settings.gradle b/settings.gradle index 3e41f162093..38ebe2a3487 100644 --- a/settings.gradle +++ b/settings.gradle @@ -10,7 +10,7 @@ pluginManagement { } plugins { - id "com.gradle.enterprise" version "3.0" + id "com.gradle.enterprise" version "3.15.1" } // Set the flag to tell gradle to ignore unresolved headers From 287fc7428bd9885004eee1aecf402c5c96fa6b6d Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Sat, 7 Oct 2023 20:42:55 -0700 Subject: [PATCH 39/76] [ntcore] Fix notification of SetDefaultEntryValue (#5733) --- ntcore/src/main/native/cpp/LocalStorage.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/ntcore/src/main/native/cpp/LocalStorage.cpp b/ntcore/src/main/native/cpp/LocalStorage.cpp index ecece477618..98ab73bc026 100644 --- a/ntcore/src/main/native/cpp/LocalStorage.cpp +++ b/ntcore/src/main/native/cpp/LocalStorage.cpp @@ -954,17 +954,20 @@ bool LocalStorage::Impl::SetDefaultEntryValue(NT_Handle pubsubentryHandle, if (topic->type == NT_UNASSIGNED) { topic->type = value.type(); } + Value newValue; if (topic->type == value.type()) { - topic->lastValue = value; + newValue = value; } else if (IsNumericCompatible(topic->type, value.type())) { - topic->lastValue = ConvertNumericValue(value, topic->type); + newValue = ConvertNumericValue(value, topic->type); } else { return true; } - topic->lastValue.SetTime(0); - topic->lastValue.SetServerTime(0); + newValue.SetTime(0); + newValue.SetServerTime(0); if (publisher) { - PublishLocalValue(publisher, topic->lastValue, true); + PublishLocalValue(publisher, newValue, true); + } else { + topic->lastValue = newValue; } return true; } From 761867b51c1c583d708c1b7b67c05755103b4aea Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Sun, 8 Oct 2023 00:34:36 -0700 Subject: [PATCH 40/76] [ntcore] Fix moving outgoing queue to new period (#5735) std::remove_if() is destructive--it can assume the removed elements are not used, but NetworkOutgoingQueue needs them to stay intact to be moved to a different queue. Use std::stable_partition() instead. --- ntcore/src/main/native/cpp/net/NetworkOutgoingQueue.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ntcore/src/main/native/cpp/net/NetworkOutgoingQueue.h b/ntcore/src/main/native/cpp/net/NetworkOutgoingQueue.h index 52dd1f24011..f5670ae98ea 100644 --- a/ntcore/src/main/native/cpp/net/NetworkOutgoingQueue.h +++ b/ntcore/src/main/native/cpp/net/NetworkOutgoingQueue.h @@ -163,8 +163,9 @@ void NetworkOutgoingQueue::SetPeriod(NT_Handle handle, if (!created && infoIt->getSecond().queueIndex != queueIndex) { // need to move any items from old queue to new queue auto& oldMsgs = m_queues[infoIt->getSecond().queueIndex].msgs; - auto it = std::remove_if(oldMsgs.begin(), oldMsgs.end(), - [&](const auto& e) { return e.handle == handle; }); + auto it = std::stable_partition( + oldMsgs.begin(), oldMsgs.end(), + [&](const auto& e) { return e.handle != handle; }); auto& newMsgs = m_queues[queueIndex].msgs; for (auto i = it, end = oldMsgs.end(); i != end; ++i) { newMsgs.emplace_back(std::move(*i)); From 3e728138f5d6ba7503faf663c921e015c4a60a70 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Sun, 8 Oct 2023 00:35:10 -0700 Subject: [PATCH 41/76] [ntcore] Add RTT-only subprotocol (#5731) This is useful for aliveness checking by clients that can't send WebSocket PING messages. --- ntcore/doc/networktables4.adoc | 7 +++- ntcore/src/main/native/cpp/NetworkServer.cpp | 40 ++++++++++++++++++-- 2 files changed, 43 insertions(+), 4 deletions(-) diff --git a/ntcore/doc/networktables4.adoc b/ntcore/doc/networktables4.adoc index 4357aaf940a..da157e59a40 100644 --- a/ntcore/doc/networktables4.adoc +++ b/ntcore/doc/networktables4.adoc @@ -95,7 +95,7 @@ With a version 4.1 connection, both the client and the server should send period As the WebSockets protocol allows PONG responses to be sent in the middle of another message stream, WebSockets PING messages are preferred, as this allows for a shorter timeout period that is not dependent on the size of the transmitted messages. Sending a ping every 200 ms with a timeout of 1 second is recommended in this case. -If using timestamp messages for aliveness checking, the client should use a timeout long enough to account for the largest expected message size (as the server can only respond after such a message has been completely transmitted). Sending a ping every 1 second with a timeout of 3 seconds is recommended in this case. +If using timestamp messages for aliveness checking on the primary connection, the client should use a timeout long enough to account for the largest expected message size (as the server can only respond after such a message has been completely transmitted). Sending a ping every 1 second with a timeout of 3 seconds is recommended in this case. If provided by the server, the <> can be used in addition to the primary connection for aliveness testing with a shorter timeout. [[reconnection]] === Caching and Reconnection Handling @@ -343,6 +343,11 @@ Combining multiple text or binary messages into a single WebSockets frame should Client and server implementations should fragment WebSockets messages to roughly the network MTU in order to facilitate rapid handling of PING and PONG messages. +[[rtt-subprotocol]] +=== RTT Subprotocol + +Servers should provide subprotocol `rtt.networktables.first.wpi.edu` for RTT-only messages. This subprotocol provides a separate channel that can be used for RTT messages to avoid delays caused by other value transmissions. Clients that cannot send WebSocket PING messages are recommended to use this subprotocol (if available) for aliveness testing. Connections using this subprotocol do not appear in the client connections list. No text frames are used; only <> with Topic ID of -1 (RTT measurement) should be sent by the client and responded to by the server. + [[data-types]] == Supported Data Types diff --git a/ntcore/src/main/native/cpp/NetworkServer.cpp b/ntcore/src/main/native/cpp/NetworkServer.cpp index a3400a11934..21eebd91c1b 100644 --- a/ntcore/src/main/native/cpp/NetworkServer.cpp +++ b/ntcore/src/main/native/cpp/NetworkServer.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include #include @@ -28,6 +29,8 @@ #include "InstanceImpl.h" #include "Log.h" #include "net/WebSocketConnection.h" +#include "net/WireDecoder.h" +#include "net/WireEncoder.h" #include "net3/UvStreamConnection3.h" using namespace nt; @@ -82,9 +85,10 @@ class NetworkServer::ServerConnection4 final std::string_view addr, unsigned int port, wpi::Logger& logger) : ServerConnection{server, addr, port, logger}, - HttpWebSocketServerConnection(stream, - {"v4.1.networktables.first.wpi.edu", - "networktables.first.wpi.edu"}) { + HttpWebSocketServerConnection( + stream, + {"v4.1.networktables.first.wpi.edu", "networktables.first.wpi.edu", + "rtt.networktables.first.wpi.edu"}) { m_info.protocol_version = 0x0400; } @@ -238,6 +242,36 @@ void NetworkServer::ServerConnection4::ProcessWsUpgrade() { protocol == "v4.1.networktables.first.wpi.edu" ? 0x0401 : 0x0400; m_wire = std::make_shared( *m_websocket, m_info.protocol_version); + + if (protocol == "rtt.networktables.first.wpi.edu") { + INFO("CONNECTED RTT client (from {})", m_connInfo); + m_websocket->binary.connect([this](std::span data, bool) { + while (!data.empty()) { + // decode message + int64_t pubuid; + Value value; + std::string error; + if (!net::WireDecodeBinary(&data, &pubuid, &value, &error, 0)) { + m_wire->Disconnect(fmt::format("binary decode error: {}", error)); + break; + } + + // respond to RTT ping + if (pubuid == -1) { + m_wire->SendBinary([&](auto& os) { + net::WireEncodeBinary(os, -1, wpi::Now(), value); + }); + } + } + }); + m_websocket->closed.connect([this](uint16_t, std::string_view reason) { + auto realReason = m_wire->GetDisconnectReason(); + INFO("DISCONNECTED RTT client (from {}): {}", m_connInfo, + realReason.empty() ? reason : realReason); + }); + return; + } + // TODO: set local flag appropriately std::string dedupName; std::tie(dedupName, m_clientId) = m_server.m_serverImpl.AddClient( From 18ef57fa58b7f493088cf1843a3069b8765216fd Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 8 Oct 2023 19:37:20 -0700 Subject: [PATCH 42/76] Fix test organization --- .../controller/ElevatorFeedforwardTest.java | 14 ++++++----- .../controller/ElevatorFeedforwardTest.cpp | 23 +++++++++---------- 2 files changed, 19 insertions(+), 18 deletions(-) diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java index 690f16bfced..4cb9c06068d 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java @@ -22,21 +22,23 @@ class ElevatorFeedforwardTest { @Test void testCalculate() { + var elevatorMotor = new ElevatorFeedforward(ks, kg, kv, ka); + assertEquals(1, m_elevatorFF.calculate(0), 0.002); assertEquals(4.5, m_elevatorFF.calculate(2), 0.002); assertEquals(6.5, m_elevatorFF.calculate(2, 1), 0.002); assertEquals(-0.5, m_elevatorFF.calculate(-2, 1), 0.002); - var r = VecBuilder.fill(2.0); - var nextR = VecBuilder.fill(3.0); + var A = Matrix.mat(Nat.N1(), Nat.N1()).fill(-kv / ka); var B = Matrix.mat(Nat.N1(), Nat.N1()).fill(1.0 / ka); + final double dt = 0.02; + var plantInversion = new LinearPlantInversionFeedforward(A, B, dt); - var plantInversion = new LinearPlantInversionFeedforward(A, B, 0.020); - var elevatorMotor = new ElevatorFeedforward(ks, kg, kv, ka); - + var r = VecBuilder.fill(2.0); + var nextR = VecBuilder.fill(3.0); assertEquals( plantInversion.calculate(r, nextR).get(0, 0) + ks + kg, - elevatorMotor.calculate(2.0, 3.0, 0.020), + elevatorMotor.calculate(2.0, 3.0, dt), 0.002); } diff --git a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp index 6747433d01a..a2db70dfd51 100644 --- a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp @@ -7,8 +7,8 @@ #include #include "frc/EigenCore.h" -#include "frc/controller/LinearPlantInversionFeedforward.h" #include "frc/controller/ElevatorFeedforward.h" +#include "frc/controller/LinearPlantInversionFeedforward.h" #include "units/acceleration.h" #include "units/length.h" #include "units/time.h" @@ -19,7 +19,7 @@ static constexpr auto Ka = 2_V * 1_s * 1_s / 1_m; static constexpr auto Kg = 1_V; TEST(ElevatorFeedforwardTest, Calculate) { - + frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka}; EXPECT_NEAR(elevatorFF.Calculate(0_m / 1_s).value(), Kg.value(), 0.002); EXPECT_NEAR(elevatorFF.Calculate(2_m / 1_s).value(), 4.5, 0.002); @@ -27,17 +27,16 @@ TEST(ElevatorFeedforwardTest, Calculate) { 0.002); EXPECT_NEAR(elevatorFF.Calculate(-2_m / 1_s, 1_m / 1_s / 1_s).value(), -0.5, 0.002); - - Vectord<1> r{2.0}; - Vectord<1> nextR{3.0}; - Matrixd<1, 1> A{-Kv / Ka}; - Matrixd<1, 1> B{1.0 / Ka}; - - frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, 0.020_s}; - frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka}; - + + frc::Matrixd<1, 1> A{-Kv.value() / Ka.value()}; + frc::Matrixd<1, 1> B{1.0 / Ka.value()}; + constexpr units::second_t dt = 20_ms; + frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt}; + + frc::Vectord<1> r{2.0}; + frc::Vectord<1> nextR{3.0}; EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value() + Kg.value(), - elevatorFF.Calculate(2_mps, 3_mps, 0.020_s).value(), 0.002); + elevatorFF.Calculate(2_mps, 3_mps, dt).value(), 0.002); } TEST(ElevatorFeedforwardTest, AchievableVelocity) { From f6879df82611ccc0db695d315842248d625a83aa Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 8 Oct 2023 19:57:07 -0700 Subject: [PATCH 43/76] Fix Java formatting --- .../wpi/first/math/controller/ElevatorFeedforwardTest.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java index 4cb9c06068d..6c978ac9868 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java @@ -22,8 +22,6 @@ class ElevatorFeedforwardTest { @Test void testCalculate() { - var elevatorMotor = new ElevatorFeedforward(ks, kg, kv, ka); - assertEquals(1, m_elevatorFF.calculate(0), 0.002); assertEquals(4.5, m_elevatorFF.calculate(2), 0.002); assertEquals(6.5, m_elevatorFF.calculate(2, 1), 0.002); @@ -38,7 +36,7 @@ void testCalculate() { var nextR = VecBuilder.fill(3.0); assertEquals( plantInversion.calculate(r, nextR).get(0, 0) + ks + kg, - elevatorMotor.calculate(2.0, 3.0, dt), + m_elevatorFF.calculate(2.0, 3.0, dt), 0.002); } From 61a42a3c2ddd45794836d2637d779c6a941f4f38 Mon Sep 17 00:00:00 2001 From: Nick Armstrong Date: Wed, 25 Oct 2023 13:52:57 -0400 Subject: [PATCH 44/76] Imported ElevatorySimSysId java --- .../wpilibj/simulation/ElevatorSimSysId.java | 133 ++++++++++++++++++ 1 file changed, 133 insertions(+) create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSimSysId.java diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSimSysId.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSimSysId.java new file mode 100644 index 00000000000..7ffb24effa0 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSimSysId.java @@ -0,0 +1,133 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.simulation; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.system.Discretization; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; + +/** Represents a simulated elevator mechanism. */ +public class ElevatorSimSysId extends ElevatorSim { + + // The min allowable height for the elevator. + private final double m_minHeight; + + // The max allowable height for the elevator. + private final double m_maxHeight; + + private final Matrix gravityMatrix; + + /** + * Creates a state-space model of a simulated elevator mechanism. + * + * @param kG The gravity gain , volts + * @param kV The velocity gain, in volts/(unit/sec) + * @param kA The acceleration gain, in volts/(unit/sec^2) + * @param gearbox The type of and number of motors in the elevator + * gearbox. + * @param gearing The gearing of the elevator (numbers greater than 1 + * represent reductions). + * @param drumRadiusMeters The radius of the drum that the elevator spool is + * wrapped around. + * @param minHeightMeters The min allowable height of the elevator. + * @param maxHeightMeters The max allowable height of the elevator. + */ + public ElevatorSimSysId( + double kG, + double kV, + double kA, + DCMotor gearbox, + double gearing, + double drumRadiusMeters, + double minHeightMeters, + double maxHeightMeters) { + this( + kG, + kV, + kA, + gearbox, + gearing, + drumRadiusMeters, + minHeightMeters, + maxHeightMeters, + null); + } + + /** + * Creates a state-space model of a simulated elevator mechanism. + * + * @param kG The gravity gain , volts + * @param kV The velocity gain, in volts/(unit/sec) + * @param kA The acceleration gain, in volts/(unit/sec^2) * @param gearbox The type of and number of motors in the elevator + * gearbox. + * @param gearing The gearing of the elevator (numbers greater than 1 + * represent reductions). + * @param drumRadiusMeters The radius of the drum that the elevator spool is + * wrapped around. + * @param minHeightMeters The min allowable height of the elevator. + * @param maxHeightMeters The max allowable height of the elevator. + * @param measurementStdDevs The standard deviations of the measurements. + */ + public ElevatorSimSysId( + double kG, + double kV, + double kA, + DCMotor gearbox, + double gearing, + double drumRadiusMeters, + double minHeightMeters, + double maxHeightMeters, + Matrix measurementStdDevs) { + + super( + LinearSystemId.identifyPositionSystem(kV, kA), + gearbox, + gearing, + drumRadiusMeters, + minHeightMeters, + maxHeightMeters, + true, + measurementStdDevs); + + m_minHeight = minHeightMeters; + m_maxHeight = maxHeightMeters; + + this.gravityMatrix = Matrix.mat(Nat.N2(), Nat.N1()) + .fill(0, -kG / kA); + } + + /** + * Updates the state of the elevator. + * + * @param currentXhat The current state estimate. + * @param u The system inputs (voltage). + * @param dtSeconds The time difference between controller updates. + */ + @Override + protected Matrix updateX(Matrix currentXhat, Matrix u, double dtSeconds) { + + var discABpair = Discretization.discretizeAB(m_plant.getA(), m_plant.getB(), dtSeconds); + var discA = discABpair.getFirst(); + var discB = discABpair.getSecond(); + Matrix bplusc = m_plant.getB().solve(gravityMatrix); + Matrix discc = discB.times(bplusc); + var updatedXhat = discA.times(m_x).plus(discB.times(m_u)).plus(discc); + + // We check for collisions after updating x-hat. + if (wouldHitLowerLimit(updatedXhat.get(0, 0))) { + return VecBuilder.fill(m_minHeight, 0); + } + if (wouldHitUpperLimit(updatedXhat.get(0, 0))) { + return VecBuilder.fill(m_maxHeight, 0); + } + return updatedXhat; + } +} From ec83c325f58358360ebb3947b7c219d389ffdd8d Mon Sep 17 00:00:00 2001 From: Nick Armstrong Date: Wed, 25 Oct 2023 14:21:03 -0400 Subject: [PATCH 45/76] minor fixes to formatting --- .../edu/wpi/first/wpilibj/simulation/ElevatorSimSysId.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSimSysId.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSimSysId.java index 7ffb24effa0..cb62fe29321 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSimSysId.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSimSysId.java @@ -66,7 +66,8 @@ public ElevatorSimSysId( * * @param kG The gravity gain , volts * @param kV The velocity gain, in volts/(unit/sec) - * @param kA The acceleration gain, in volts/(unit/sec^2) * @param gearbox The type of and number of motors in the elevator + * @param kA The acceleration gain, in volts/(unit/sec^2) + * @param gearbox The type of and number of motors in the elevator * gearbox. * @param gearing The gearing of the elevator (numbers greater than 1 * represent reductions). @@ -86,7 +87,6 @@ public ElevatorSimSysId( double minHeightMeters, double maxHeightMeters, Matrix measurementStdDevs) { - super( LinearSystemId.identifyPositionSystem(kV, kA), gearbox, From c3fd2358164b5d3598790a38bdc3215f93fc82c3 Mon Sep 17 00:00:00 2001 From: Nick Armstrong Date: Wed, 25 Oct 2023 16:25:46 -0400 Subject: [PATCH 46/76] revised Constructor for ElevatorSim --- .../first/wpilibj/simulation/ElevatorSim.java | 67 +++++++++ .../wpilibj/simulation/ElevatorSimSysId.java | 133 ------------------ 2 files changed, 67 insertions(+), 133 deletions(-) delete mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSimSysId.java diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java index 6d655f72b41..dcf647c26db 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java @@ -105,6 +105,73 @@ public ElevatorSim( null); } + /** + * Creates a simulated elevator mechanism. + * + * @param gearbox The type of and number of motors in the elevator gearbox. + * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions). + * @param carriageMassKg The mass of the elevator carriage. + * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around. + * @param minHeightMeters The min allowable height of the elevator. + * @param maxHeightMeters The max allowable height of the elevator. + * @param simulateGravity Whether gravity should be simulated or not. + * @param startingHeightMeters The starting height of the elevator. + */ + public ElevatorSim( + double kG, + DCMotor gearbox, + double gearing, + double drumRadiusMeters, + double minHeightMeters, + double maxHeightMeters, + boolean simulateGravity, + double startingHeightMeters) { + this( + kG, + gearing, + drumRadiusMeters, + minHeightMeters, + maxHeightMeters, + simulateGravity, + startingHeightMeters, + null); + } + + /** + * Creates a simulated elevator mechanism. + * + * @param gearbox The type of and number of motors in the elevator gearbox. + * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions). + * @param carriageMassKg The mass of the elevator carriage. + * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around. + * @param minHeightMeters The min allowable height of the elevator. + * @param maxHeightMeters The max allowable height of the elevator. + * @param simulateGravity Whether gravity should be simulated or not. + * @param startingHeightMeters The starting height of the elevator. + * @param measurementStdDevs The standard deviations of the measurements. + */ + public ElevatorSim( + double kG, + DCMotor gearbox, + double gearing, + double drumRadiusMeters, + double minHeightMeters, + double maxHeightMeters, + boolean simulateGravity, + double startingHeightMeters, + Matrix measurementStdDevs) { + this( + gearbox, + gearing, + kG * gearbox.KtNMPerAmp * gearing / 9.8 / gearbox.rOhms / drumRadiusMeters, + drumRadiusMeters, + minHeightMeters, + maxHeightMeters, + simulateGravity, + startingHeightMeters, + measurementStdDevs); + } + /** * Creates a simulated elevator mechanism. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSimSysId.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSimSysId.java deleted file mode 100644 index cb62fe29321..00000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSimSysId.java +++ /dev/null @@ -1,133 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package edu.wpi.first.wpilibj.simulation; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Nat; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N2; -import edu.wpi.first.math.system.Discretization; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.wpilibj.simulation.ElevatorSim; - -/** Represents a simulated elevator mechanism. */ -public class ElevatorSimSysId extends ElevatorSim { - - // The min allowable height for the elevator. - private final double m_minHeight; - - // The max allowable height for the elevator. - private final double m_maxHeight; - - private final Matrix gravityMatrix; - - /** - * Creates a state-space model of a simulated elevator mechanism. - * - * @param kG The gravity gain , volts - * @param kV The velocity gain, in volts/(unit/sec) - * @param kA The acceleration gain, in volts/(unit/sec^2) - * @param gearbox The type of and number of motors in the elevator - * gearbox. - * @param gearing The gearing of the elevator (numbers greater than 1 - * represent reductions). - * @param drumRadiusMeters The radius of the drum that the elevator spool is - * wrapped around. - * @param minHeightMeters The min allowable height of the elevator. - * @param maxHeightMeters The max allowable height of the elevator. - */ - public ElevatorSimSysId( - double kG, - double kV, - double kA, - DCMotor gearbox, - double gearing, - double drumRadiusMeters, - double minHeightMeters, - double maxHeightMeters) { - this( - kG, - kV, - kA, - gearbox, - gearing, - drumRadiusMeters, - minHeightMeters, - maxHeightMeters, - null); - } - - /** - * Creates a state-space model of a simulated elevator mechanism. - * - * @param kG The gravity gain , volts - * @param kV The velocity gain, in volts/(unit/sec) - * @param kA The acceleration gain, in volts/(unit/sec^2) - * @param gearbox The type of and number of motors in the elevator - * gearbox. - * @param gearing The gearing of the elevator (numbers greater than 1 - * represent reductions). - * @param drumRadiusMeters The radius of the drum that the elevator spool is - * wrapped around. - * @param minHeightMeters The min allowable height of the elevator. - * @param maxHeightMeters The max allowable height of the elevator. - * @param measurementStdDevs The standard deviations of the measurements. - */ - public ElevatorSimSysId( - double kG, - double kV, - double kA, - DCMotor gearbox, - double gearing, - double drumRadiusMeters, - double minHeightMeters, - double maxHeightMeters, - Matrix measurementStdDevs) { - super( - LinearSystemId.identifyPositionSystem(kV, kA), - gearbox, - gearing, - drumRadiusMeters, - minHeightMeters, - maxHeightMeters, - true, - measurementStdDevs); - - m_minHeight = minHeightMeters; - m_maxHeight = maxHeightMeters; - - this.gravityMatrix = Matrix.mat(Nat.N2(), Nat.N1()) - .fill(0, -kG / kA); - } - - /** - * Updates the state of the elevator. - * - * @param currentXhat The current state estimate. - * @param u The system inputs (voltage). - * @param dtSeconds The time difference between controller updates. - */ - @Override - protected Matrix updateX(Matrix currentXhat, Matrix u, double dtSeconds) { - - var discABpair = Discretization.discretizeAB(m_plant.getA(), m_plant.getB(), dtSeconds); - var discA = discABpair.getFirst(); - var discB = discABpair.getSecond(); - Matrix bplusc = m_plant.getB().solve(gravityMatrix); - Matrix discc = discB.times(bplusc); - var updatedXhat = discA.times(m_x).plus(discB.times(m_u)).plus(discc); - - // We check for collisions after updating x-hat. - if (wouldHitLowerLimit(updatedXhat.get(0, 0))) { - return VecBuilder.fill(m_minHeight, 0); - } - if (wouldHitUpperLimit(updatedXhat.get(0, 0))) { - return VecBuilder.fill(m_maxHeight, 0); - } - return updatedXhat; - } -} From 3a47616232c229c43397423fdaa5e670890e602f Mon Sep 17 00:00:00 2001 From: Nick Armstrong Date: Wed, 25 Oct 2023 20:05:14 -0400 Subject: [PATCH 47/76] Fix to Constructor --- .SysId/sysid-window.json | 51 +++++++++++++++++++ .SysId/sysid.json | 5 ++ .../first/wpilibj/simulation/ElevatorSim.java | 1 + 3 files changed, 57 insertions(+) create mode 100644 .SysId/sysid-window.json create mode 100644 .SysId/sysid.json diff --git a/.SysId/sysid-window.json b/.SysId/sysid-window.json new file mode 100644 index 00000000000..cc7fc048669 --- /dev/null +++ b/.SysId/sysid-window.json @@ -0,0 +1,51 @@ +{ + "MainWindow": { + "GLOBAL": { + "fps": "120", + "height": "991", + "maximized": "0", + "style": "0", + "userScale": "2", + "width": "958", + "xpos": "1", + "ypos": "38" + } + }, + "Window": { + "###Analyzer": { + "Collapsed": "0", + "Pos": "400,31", + "Size": "450,687" + }, + "###Generator": { + "Collapsed": "0", + "Pos": "6,31", + "Size": "387,375" + }, + "###Logger": { + "Collapsed": "0", + "Pos": "6,412", + "Size": "387,481" + }, + "###Program Log": { + "Collapsed": "0", + "Pos": "400,725", + "Size": "450,168" + }, + "Debug##Default": { + "Collapsed": "0", + "Pos": "60,60", + "Size": "400,400" + }, + "Diagnostic Plots": { + "Collapsed": "0", + "Pos": "886,-4", + "Size": "590,690" + }, + "Override Units": { + "Collapsed": "0", + "Pos": "600,361", + "Size": "400,180" + } + } +} diff --git a/.SysId/sysid.json b/.SysId/sysid.json new file mode 100644 index 00000000000..de74a00f6f6 --- /dev/null +++ b/.SysId/sysid.json @@ -0,0 +1,5 @@ +{ + "SysId": { + "Analysis Type": "General Mechanism" + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java index dcf647c26db..8e98f00cc45 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java @@ -128,6 +128,7 @@ public ElevatorSim( double startingHeightMeters) { this( kG, + gearbox, gearing, drumRadiusMeters, minHeightMeters, From 1839f750159f14d57a26b9c344c8dbba70c9f2cd Mon Sep 17 00:00:00 2001 From: Nick Armstrong Date: Wed, 25 Oct 2023 20:13:29 -0400 Subject: [PATCH 48/76] fixed missing import --- .../main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java index 8e98f00cc45..54ffa0fef39 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java @@ -13,6 +13,8 @@ import edu.wpi.first.math.system.NumericalIntegration; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.LinearSystemSim; + /** Represents a simulated elevator mechanism. */ public class ElevatorSim extends LinearSystemSim { From de21da150ffae3fb1907dfca79dad1b1471d47eb Mon Sep 17 00:00:00 2001 From: Nick Armstrong Date: Wed, 25 Oct 2023 20:17:45 -0400 Subject: [PATCH 49/76] fixed JavaDocs --- .../java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java index 54ffa0fef39..6a19f29a6d4 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java @@ -110,9 +110,9 @@ public ElevatorSim( /** * Creates a simulated elevator mechanism. * + * @param kG The gravity gain. * @param gearbox The type of and number of motors in the elevator gearbox. * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions). - * @param carriageMassKg The mass of the elevator carriage. * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around. * @param minHeightMeters The min allowable height of the elevator. * @param maxHeightMeters The max allowable height of the elevator. @@ -143,9 +143,9 @@ public ElevatorSim( /** * Creates a simulated elevator mechanism. * + * @param kG The gravity gain. * @param gearbox The type of and number of motors in the elevator gearbox. * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions). - * @param carriageMassKg The mass of the elevator carriage. * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around. * @param minHeightMeters The min allowable height of the elevator. * @param maxHeightMeters The max allowable height of the elevator. From 354979b9167f2c4171459526f87b1906a351b2da Mon Sep 17 00:00:00 2001 From: Nick Armstrong Date: Wed, 25 Oct 2023 20:23:18 -0400 Subject: [PATCH 50/76] removed redundant import --- .../main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java | 1 - 1 file changed, 1 deletion(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java index 6a19f29a6d4..705e773df5b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java @@ -13,7 +13,6 @@ import edu.wpi.first.math.system.NumericalIntegration; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.wpilibj.simulation.LinearSystemSim; /** Represents a simulated elevator mechanism. */ From d64f5b75b964f830e9e711fa014fea13fbb66113 Mon Sep 17 00:00:00 2001 From: Nick Armstrong Date: Wed, 25 Oct 2023 20:35:55 -0400 Subject: [PATCH 51/76] removed extra space --- .../main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java | 1 - 1 file changed, 1 deletion(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java index 705e773df5b..23e0ed56cb3 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java @@ -14,7 +14,6 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; - /** Represents a simulated elevator mechanism. */ public class ElevatorSim extends LinearSystemSim { // Gearbox for the elevator. From 59294cf5ca658b7c5f819e96d6fe82b7c38095b0 Mon Sep 17 00:00:00 2001 From: Nick Armstrong Date: Thu, 26 Oct 2023 16:31:00 -0400 Subject: [PATCH 52/76] removed sysid folder --- .SysId/sysid-window.json | 51 ---------------------------------------- .SysId/sysid.json | 5 ---- 2 files changed, 56 deletions(-) delete mode 100644 .SysId/sysid-window.json delete mode 100644 .SysId/sysid.json diff --git a/.SysId/sysid-window.json b/.SysId/sysid-window.json deleted file mode 100644 index cc7fc048669..00000000000 --- a/.SysId/sysid-window.json +++ /dev/null @@ -1,51 +0,0 @@ -{ - "MainWindow": { - "GLOBAL": { - "fps": "120", - "height": "991", - "maximized": "0", - "style": "0", - "userScale": "2", - "width": "958", - "xpos": "1", - "ypos": "38" - } - }, - "Window": { - "###Analyzer": { - "Collapsed": "0", - "Pos": "400,31", - "Size": "450,687" - }, - "###Generator": { - "Collapsed": "0", - "Pos": "6,31", - "Size": "387,375" - }, - "###Logger": { - "Collapsed": "0", - "Pos": "6,412", - "Size": "387,481" - }, - "###Program Log": { - "Collapsed": "0", - "Pos": "400,725", - "Size": "450,168" - }, - "Debug##Default": { - "Collapsed": "0", - "Pos": "60,60", - "Size": "400,400" - }, - "Diagnostic Plots": { - "Collapsed": "0", - "Pos": "886,-4", - "Size": "590,690" - }, - "Override Units": { - "Collapsed": "0", - "Pos": "600,361", - "Size": "400,180" - } - } -} diff --git a/.SysId/sysid.json b/.SysId/sysid.json deleted file mode 100644 index de74a00f6f6..00000000000 --- a/.SysId/sysid.json +++ /dev/null @@ -1,5 +0,0 @@ -{ - "SysId": { - "Analysis Type": "General Mechanism" - } -} From 8f54a9e88b61699f628ac3af856b327f4fcc31d5 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sat, 28 Oct 2023 14:37:51 +0000 Subject: [PATCH 53/76] Formatting fixes --- .../first/wpilibj/simulation/ElevatorSim.java | 38 +++++++++---------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java index 23e0ed56cb3..3d727d6c38d 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java @@ -127,15 +127,15 @@ public ElevatorSim( boolean simulateGravity, double startingHeightMeters) { this( - kG, - gearbox, - gearing, - drumRadiusMeters, - minHeightMeters, - maxHeightMeters, - simulateGravity, - startingHeightMeters, - null); + kG, + gearbox, + gearing, + drumRadiusMeters, + minHeightMeters, + maxHeightMeters, + simulateGravity, + startingHeightMeters, + null); } /** @@ -162,17 +162,17 @@ public ElevatorSim( double startingHeightMeters, Matrix measurementStdDevs) { this( - gearbox, - gearing, - kG * gearbox.KtNMPerAmp * gearing / 9.8 / gearbox.rOhms / drumRadiusMeters, - drumRadiusMeters, - minHeightMeters, - maxHeightMeters, - simulateGravity, - startingHeightMeters, - measurementStdDevs); + gearbox, + gearing, + kG * gearbox.KtNMPerAmp * gearing / 9.8 / gearbox.rOhms / drumRadiusMeters, + drumRadiusMeters, + minHeightMeters, + maxHeightMeters, + simulateGravity, + startingHeightMeters, + measurementStdDevs); } - + /** * Creates a simulated elevator mechanism. * From c08b4ae17c66d9815431670ac6899d4f1c1034e0 Mon Sep 17 00:00:00 2001 From: Nick Armstrong Date: Tue, 31 Oct 2023 08:14:02 -0400 Subject: [PATCH 54/76] removed drumRadius and mass from gains constructor --- .../first/wpilibj/simulation/ElevatorSim.java | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java index 3d727d6c38d..fd70a7560f6 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java @@ -109,9 +109,10 @@ public ElevatorSim( * Creates a simulated elevator mechanism. * * @param kG The gravity gain. + * @param kV The velocity gain. + * @param kA The acceleration gain. * @param gearbox The type of and number of motors in the elevator gearbox. * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions). - * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around. * @param minHeightMeters The min allowable height of the elevator. * @param maxHeightMeters The max allowable height of the elevator. * @param simulateGravity Whether gravity should be simulated or not. @@ -119,18 +120,20 @@ public ElevatorSim( */ public ElevatorSim( double kG, + double kV, + double kA, DCMotor gearbox, double gearing, - double drumRadiusMeters, double minHeightMeters, double maxHeightMeters, boolean simulateGravity, double startingHeightMeters) { this( kG, + kV, + kA, gearbox, gearing, - drumRadiusMeters, minHeightMeters, maxHeightMeters, simulateGravity, @@ -142,9 +145,10 @@ public ElevatorSim( * Creates a simulated elevator mechanism. * * @param kG The gravity gain. + * @param kV The velocity gain. + * @param kA The acceleration gain. * @param gearbox The type of and number of motors in the elevator gearbox. * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions). - * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around. * @param minHeightMeters The min allowable height of the elevator. * @param maxHeightMeters The max allowable height of the elevator. * @param simulateGravity Whether gravity should be simulated or not. @@ -153,9 +157,10 @@ public ElevatorSim( */ public ElevatorSim( double kG, + double kV, + double kA, DCMotor gearbox, double gearing, - double drumRadiusMeters, double minHeightMeters, double maxHeightMeters, boolean simulateGravity, @@ -164,8 +169,8 @@ public ElevatorSim( this( gearbox, gearing, - kG * gearbox.KtNMPerAmp * gearing / 9.8 / gearbox.rOhms / drumRadiusMeters, - drumRadiusMeters, + kA * gearbox.KtNMPerAmp * kV * gearbox.KvRadPerSecPerVolt / gearbox.rOhms, + gearing / kV / gearbox.KvRadPerSecPerVolt, minHeightMeters, maxHeightMeters, simulateGravity, From f0937542c3ba4680e3cd5de4c4c32615a4bb28db Mon Sep 17 00:00:00 2001 From: Nick Armstrong Date: Tue, 31 Oct 2023 10:12:17 -0400 Subject: [PATCH 55/76] eliminate private fields or gearing and drumRadius --- .../first/wpilibj/simulation/ElevatorSim.java | 37 ++----------------- 1 file changed, 4 insertions(+), 33 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java index fd70a7560f6..215f95ee03c 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java @@ -19,12 +19,6 @@ public class ElevatorSim extends LinearSystemSim { // Gearbox for the elevator. private final DCMotor m_gearbox; - // Gearing between the motors and the output. - private final double m_gearing; - - // The radius of the drum that the elevator spool is wrapped around. - private final double m_drumRadius; - // The min allowable height for the elevator. private final double m_minHeight; @@ -41,8 +35,6 @@ public class ElevatorSim extends LinearSystemSim { * {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double, * double, double)}. * @param gearbox The type of and number of motors in the elevator gearbox. - * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions). - * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around. * @param minHeightMeters The min allowable height of the elevator. * @param maxHeightMeters The max allowable height of the elevator. * @param simulateGravity Whether gravity should be simulated or not. @@ -52,8 +44,6 @@ public class ElevatorSim extends LinearSystemSim { public ElevatorSim( LinearSystem plant, DCMotor gearbox, - double gearing, - double drumRadiusMeters, double minHeightMeters, double maxHeightMeters, boolean simulateGravity, @@ -61,8 +51,6 @@ public ElevatorSim( Matrix measurementStdDevs) { super(plant, measurementStdDevs); m_gearbox = gearbox; - m_gearing = gearing; - m_drumRadius = drumRadiusMeters; m_minHeight = minHeightMeters; m_maxHeight = maxHeightMeters; m_simulateGravity = simulateGravity; @@ -77,8 +65,6 @@ public ElevatorSim( * {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double, * double, double)}. * @param gearbox The type of and number of motors in the elevator gearbox. - * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions). - * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around. * @param minHeightMeters The min allowable height of the elevator. * @param maxHeightMeters The max allowable height of the elevator. * @param startingHeightMeters The starting height of the elevator. @@ -87,8 +73,6 @@ public ElevatorSim( public ElevatorSim( LinearSystem plant, DCMotor gearbox, - double gearing, - double drumRadiusMeters, double minHeightMeters, double maxHeightMeters, boolean simulateGravity, @@ -96,8 +80,6 @@ public ElevatorSim( this( plant, gearbox, - gearing, - drumRadiusMeters, minHeightMeters, maxHeightMeters, simulateGravity, @@ -108,32 +90,26 @@ public ElevatorSim( /** * Creates a simulated elevator mechanism. * - * @param kG The gravity gain. * @param kV The velocity gain. * @param kA The acceleration gain. * @param gearbox The type of and number of motors in the elevator gearbox. - * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions). * @param minHeightMeters The min allowable height of the elevator. * @param maxHeightMeters The max allowable height of the elevator. * @param simulateGravity Whether gravity should be simulated or not. * @param startingHeightMeters The starting height of the elevator. */ public ElevatorSim( - double kG, double kV, double kA, DCMotor gearbox, - double gearing, double minHeightMeters, double maxHeightMeters, boolean simulateGravity, double startingHeightMeters) { this( - kG, kV, kA, gearbox, - gearing, minHeightMeters, maxHeightMeters, simulateGravity, @@ -144,7 +120,6 @@ public ElevatorSim( /** * Creates a simulated elevator mechanism. * - * @param kG The gravity gain. * @param kV The velocity gain. * @param kA The acceleration gain. * @param gearbox The type of and number of motors in the elevator gearbox. @@ -156,21 +131,17 @@ public ElevatorSim( * @param measurementStdDevs The standard deviations of the measurements. */ public ElevatorSim( - double kG, double kV, double kA, DCMotor gearbox, - double gearing, double minHeightMeters, double maxHeightMeters, boolean simulateGravity, double startingHeightMeters, Matrix measurementStdDevs) { this( + LinearSystemId.identifyPositionSystem(kV, kA) , gearbox, - gearing, - kA * gearbox.KtNMPerAmp * kV * gearbox.KvRadPerSecPerVolt / gearbox.rOhms, - gearing / kV / gearbox.KvRadPerSecPerVolt, minHeightMeters, maxHeightMeters, simulateGravity, @@ -204,8 +175,6 @@ public ElevatorSim( this( LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing), gearbox, - gearing, - drumRadiusMeters, minHeightMeters, maxHeightMeters, simulateGravity, @@ -326,7 +295,9 @@ public double getCurrentDrawAmps() { // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is // spinning 10x faster than the output // v = r w, so w = v/r - double motorVelocityRadPerSec = getVelocityMetersPerSecond() / m_drumRadius * m_gearing; + double kA = 1 / m_plant.getB().get(1, 0); + double kV = -m_plant.getA().get(1, 1) * kA; + double motorVelocityRadPerSec = getVelocityMetersPerSecond() * kV * gearbox.KvRadPerSecPerVolt; var appliedVoltage = m_u.get(0, 0); return m_gearbox.getCurrent(motorVelocityRadPerSec, appliedVoltage) * Math.signum(appliedVoltage); From 411a82cc43b73a765b76e1035d8bb74691e0e98f Mon Sep 17 00:00:00 2001 From: Nick Armstrong Date: Tue, 31 Oct 2023 11:01:26 -0400 Subject: [PATCH 56/76] fixed typo --- .../main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java index 215f95ee03c..e00ebe02c9a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java @@ -297,7 +297,7 @@ public double getCurrentDrawAmps() { // v = r w, so w = v/r double kA = 1 / m_plant.getB().get(1, 0); double kV = -m_plant.getA().get(1, 1) * kA; - double motorVelocityRadPerSec = getVelocityMetersPerSecond() * kV * gearbox.KvRadPerSecPerVolt; + double motorVelocityRadPerSec = getVelocityMetersPerSecond() * kV * m_gearbox.KvRadPerSecPerVolt; var appliedVoltage = m_u.get(0, 0); return m_gearbox.getCurrent(motorVelocityRadPerSec, appliedVoltage) * Math.signum(appliedVoltage); From 112a12145509930bdf42aa156f46f836c0b2503f Mon Sep 17 00:00:00 2001 From: Nick Armstrong Date: Tue, 31 Oct 2023 11:14:39 -0400 Subject: [PATCH 57/76] fixed doc issue --- .../main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java | 1 - 1 file changed, 1 deletion(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java index e00ebe02c9a..0637ba04d7a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java @@ -123,7 +123,6 @@ public ElevatorSim( * @param kV The velocity gain. * @param kA The acceleration gain. * @param gearbox The type of and number of motors in the elevator gearbox. - * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions). * @param minHeightMeters The min allowable height of the elevator. * @param maxHeightMeters The max allowable height of the elevator. * @param simulateGravity Whether gravity should be simulated or not. From d12f404857e5d8266c5b689abdcca5b587e154a1 Mon Sep 17 00:00:00 2001 From: Nick Armstrong Date: Tue, 31 Oct 2023 12:13:34 -0400 Subject: [PATCH 58/76] line width linting issue --- .../first/wpilibj/simulation/ElevatorSim.java | 135 ++++++++++-------- 1 file changed, 74 insertions(+), 61 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java index 0637ba04d7a..4887ae0beb7 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java @@ -31,15 +31,17 @@ public class ElevatorSim extends LinearSystemSim { /** * Creates a simulated elevator mechanism. * - * @param plant The linear system that represents the elevator. This system can be created with - * {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double, - * double, double)}. - * @param gearbox The type of and number of motors in the elevator gearbox. - * @param minHeightMeters The min allowable height of the elevator. - * @param maxHeightMeters The max allowable height of the elevator. - * @param simulateGravity Whether gravity should be simulated or not. + * @param plant The linear system that represents the elevator. + * This system can be created with + * {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double, + * double, double)}. + * @param gearbox The type of and number of motors in the elevator + * gearbox. + * @param minHeightMeters The min allowable height of the elevator. + * @param maxHeightMeters The max allowable height of the elevator. + * @param simulateGravity Whether gravity should be simulated or not. * @param startingHeightMeters The starting height of the elevator. - * @param measurementStdDevs The standard deviations of the measurements. + * @param measurementStdDevs The standard deviations of the measurements. */ public ElevatorSim( LinearSystem plant, @@ -61,14 +63,16 @@ public ElevatorSim( /** * Creates a simulated elevator mechanism. * - * @param plant The linear system that represents the elevator. This system can be created with - * {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double, - * double, double)}. - * @param gearbox The type of and number of motors in the elevator gearbox. - * @param minHeightMeters The min allowable height of the elevator. - * @param maxHeightMeters The max allowable height of the elevator. + * @param plant The linear system that represents the elevator. + * This system can be created with + * {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double, + * double, double)}. + * @param gearbox The type of and number of motors in the elevator + * gearbox. + * @param minHeightMeters The min allowable height of the elevator. + * @param maxHeightMeters The max allowable height of the elevator. * @param startingHeightMeters The starting height of the elevator. - * @param simulateGravity Whether gravity should be simulated or not. + * @param simulateGravity Whether gravity should be simulated or not. */ public ElevatorSim( LinearSystem plant, @@ -90,12 +94,13 @@ public ElevatorSim( /** * Creates a simulated elevator mechanism. * - * @param kV The velocity gain. - * @param kA The acceleration gain. - * @param gearbox The type of and number of motors in the elevator gearbox. - * @param minHeightMeters The min allowable height of the elevator. - * @param maxHeightMeters The max allowable height of the elevator. - * @param simulateGravity Whether gravity should be simulated or not. + * @param kV The velocity gain. + * @param kA The acceleration gain. + * @param gearbox The type of and number of motors in the elevator + * gearbox. + * @param minHeightMeters The min allowable height of the elevator. + * @param maxHeightMeters The max allowable height of the elevator. + * @param simulateGravity Whether gravity should be simulated or not. * @param startingHeightMeters The starting height of the elevator. */ public ElevatorSim( @@ -120,14 +125,15 @@ public ElevatorSim( /** * Creates a simulated elevator mechanism. * - * @param kV The velocity gain. - * @param kA The acceleration gain. - * @param gearbox The type of and number of motors in the elevator gearbox. - * @param minHeightMeters The min allowable height of the elevator. - * @param maxHeightMeters The max allowable height of the elevator. - * @param simulateGravity Whether gravity should be simulated or not. + * @param kV The velocity gain. + * @param kA The acceleration gain. + * @param gearbox The type of and number of motors in the elevator + * gearbox. + * @param minHeightMeters The min allowable height of the elevator. + * @param maxHeightMeters The max allowable height of the elevator. + * @param simulateGravity Whether gravity should be simulated or not. * @param startingHeightMeters The starting height of the elevator. - * @param measurementStdDevs The standard deviations of the measurements. + * @param measurementStdDevs The standard deviations of the measurements. */ public ElevatorSim( double kV, @@ -139,7 +145,7 @@ public ElevatorSim( double startingHeightMeters, Matrix measurementStdDevs) { this( - LinearSystemId.identifyPositionSystem(kV, kA) , + LinearSystemId.identifyPositionSystem(kV, kA), gearbox, minHeightMeters, maxHeightMeters, @@ -151,15 +157,18 @@ public ElevatorSim( /** * Creates a simulated elevator mechanism. * - * @param gearbox The type of and number of motors in the elevator gearbox. - * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions). - * @param carriageMassKg The mass of the elevator carriage. - * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around. - * @param minHeightMeters The min allowable height of the elevator. - * @param maxHeightMeters The max allowable height of the elevator. - * @param simulateGravity Whether gravity should be simulated or not. + * @param gearbox The type of and number of motors in the elevator + * gearbox. + * @param gearing The gearing of the elevator (numbers greater than + * 1 represent reductions). + * @param carriageMassKg The mass of the elevator carriage. + * @param drumRadiusMeters The radius of the drum that the elevator spool is + * wrapped around. + * @param minHeightMeters The min allowable height of the elevator. + * @param maxHeightMeters The max allowable height of the elevator. + * @param simulateGravity Whether gravity should be simulated or not. * @param startingHeightMeters The starting height of the elevator. - * @param measurementStdDevs The standard deviations of the measurements. + * @param measurementStdDevs The standard deviations of the measurements. */ public ElevatorSim( DCMotor gearbox, @@ -184,13 +193,16 @@ public ElevatorSim( /** * Creates a simulated elevator mechanism. * - * @param gearbox The type of and number of motors in the elevator gearbox. - * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions). - * @param carriageMassKg The mass of the elevator carriage. - * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around. - * @param minHeightMeters The min allowable height of the elevator. - * @param maxHeightMeters The max allowable height of the elevator. - * @param simulateGravity Whether gravity should be simulated or not. + * @param gearbox The type of and number of motors in the elevator + * gearbox. + * @param gearing The gearing of the elevator (numbers greater than + * 1 represent reductions). + * @param carriageMassKg The mass of the elevator carriage. + * @param drumRadiusMeters The radius of the drum that the elevator spool is + * wrapped around. + * @param minHeightMeters The min allowable height of the elevator. + * @param maxHeightMeters The max allowable height of the elevator. + * @param simulateGravity Whether gravity should be simulated or not. * @param startingHeightMeters The starting height of the elevator. */ public ElevatorSim( @@ -215,10 +227,11 @@ public ElevatorSim( } /** - * Sets the elevator's state. The new position will be limited between the minimum and maximum + * Sets the elevator's state. The new position will be limited between the + * minimum and maximum * allowed heights. * - * @param positionMeters The new position in meters. + * @param positionMeters The new position in meters. * @param velocityMetersPerSecond New velocity in meters per second. */ public void setState(double positionMeters, double velocityMetersPerSecond) { @@ -296,7 +309,8 @@ public double getCurrentDrawAmps() { // v = r w, so w = v/r double kA = 1 / m_plant.getB().get(1, 0); double kV = -m_plant.getA().get(1, 1) * kA; - double motorVelocityRadPerSec = getVelocityMetersPerSecond() * kV * m_gearbox.KvRadPerSecPerVolt; + double KvRadPerSecPerVolt = m_gearbox.KvRadPerSecPerVolt; + double motorVelocityRadPerSec = getVelocityMetersPerSecond() * kV * KvRadPerSecPerVolt; var appliedVoltage = m_u.get(0, 0); return m_gearbox.getCurrent(motorVelocityRadPerSec, appliedVoltage) * Math.signum(appliedVoltage); @@ -315,24 +329,23 @@ public void setInputVoltage(double volts) { * Updates the state of the elevator. * * @param currentXhat The current state estimate. - * @param u The system inputs (voltage). - * @param dtSeconds The time difference between controller updates. + * @param u The system inputs (voltage). + * @param dtSeconds The time difference between controller updates. */ @Override protected Matrix updateX(Matrix currentXhat, Matrix u, double dtSeconds) { // Calculate updated x-hat from Runge-Kutta. - var updatedXhat = - NumericalIntegration.rkdp( - (x, _u) -> { - Matrix xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u)); - if (m_simulateGravity) { - xdot = xdot.plus(VecBuilder.fill(0, -9.8)); - } - return xdot; - }, - currentXhat, - u, - dtSeconds); + var updatedXhat = NumericalIntegration.rkdp( + (x, _u) -> { + Matrix xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u)); + if (m_simulateGravity) { + xdot = xdot.plus(VecBuilder.fill(0, -9.8)); + } + return xdot; + }, + currentXhat, + u, + dtSeconds); // We check for collisions after updating x-hat. if (wouldHitLowerLimit(updatedXhat.get(0, 0))) { From 9c7001373f74bc638431c55d54ec51de6f4cf20f Mon Sep 17 00:00:00 2001 From: Nick Armstrong Date: Tue, 31 Oct 2023 12:58:45 -0400 Subject: [PATCH 59/76] line length fixes --- .../wpi/first/wpilibj/simulation/ElevatorSim.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java index 4887ae0beb7..b9d74c53277 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java @@ -33,8 +33,8 @@ public class ElevatorSim extends LinearSystemSim { * * @param plant The linear system that represents the elevator. * This system can be created with - * {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double, - * double, double)}. + * {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, + * double, double, double)}. * @param gearbox The type of and number of motors in the elevator * gearbox. * @param minHeightMeters The min allowable height of the elevator. @@ -65,8 +65,8 @@ public ElevatorSim( * * @param plant The linear system that represents the elevator. * This system can be created with - * {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double, - * double, double)}. + * {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, + * double, double, double)}. * @param gearbox The type of and number of motors in the elevator * gearbox. * @param minHeightMeters The min allowable height of the elevator. @@ -309,8 +309,8 @@ public double getCurrentDrawAmps() { // v = r w, so w = v/r double kA = 1 / m_plant.getB().get(1, 0); double kV = -m_plant.getA().get(1, 1) * kA; - double KvRadPerSecPerVolt = m_gearbox.KvRadPerSecPerVolt; - double motorVelocityRadPerSec = getVelocityMetersPerSecond() * kV * KvRadPerSecPerVolt; + double motorVelocityRadPerSec = getVelocityMetersPerSecond() * + kV * m_gearbox.KvRadPerSecPerVolt; var appliedVoltage = m_u.get(0, 0); return m_gearbox.getCurrent(motorVelocityRadPerSec, appliedVoltage) * Math.signum(appliedVoltage); From f7f7faf93b7352b36b33c605451cc2f926dcc321 Mon Sep 17 00:00:00 2001 From: Nick Armstrong Date: Tue, 31 Oct 2023 13:12:43 -0400 Subject: [PATCH 60/76] spotlessApply --- .../first/wpilibj/simulation/ElevatorSim.java | 134 ++++++++---------- 1 file changed, 61 insertions(+), 73 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java index b9d74c53277..322d897479c 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java @@ -31,17 +31,15 @@ public class ElevatorSim extends LinearSystemSim { /** * Creates a simulated elevator mechanism. * - * @param plant The linear system that represents the elevator. - * This system can be created with - * {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, - * double, double, double)}. - * @param gearbox The type of and number of motors in the elevator - * gearbox. - * @param minHeightMeters The min allowable height of the elevator. - * @param maxHeightMeters The max allowable height of the elevator. - * @param simulateGravity Whether gravity should be simulated or not. + * @param plant The linear system that represents the elevator. This system can be created with + * {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double, + * double, double)}. + * @param gearbox The type of and number of motors in the elevator gearbox. + * @param minHeightMeters The min allowable height of the elevator. + * @param maxHeightMeters The max allowable height of the elevator. + * @param simulateGravity Whether gravity should be simulated or not. * @param startingHeightMeters The starting height of the elevator. - * @param measurementStdDevs The standard deviations of the measurements. + * @param measurementStdDevs The standard deviations of the measurements. */ public ElevatorSim( LinearSystem plant, @@ -63,16 +61,14 @@ public ElevatorSim( /** * Creates a simulated elevator mechanism. * - * @param plant The linear system that represents the elevator. - * This system can be created with - * {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, - * double, double, double)}. - * @param gearbox The type of and number of motors in the elevator - * gearbox. - * @param minHeightMeters The min allowable height of the elevator. - * @param maxHeightMeters The max allowable height of the elevator. + * @param plant The linear system that represents the elevator. This system can be created with + * {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double, + * double, double)}. + * @param gearbox The type of and number of motors in the elevator gearbox. + * @param minHeightMeters The min allowable height of the elevator. + * @param maxHeightMeters The max allowable height of the elevator. * @param startingHeightMeters The starting height of the elevator. - * @param simulateGravity Whether gravity should be simulated or not. + * @param simulateGravity Whether gravity should be simulated or not. */ public ElevatorSim( LinearSystem plant, @@ -94,13 +90,12 @@ public ElevatorSim( /** * Creates a simulated elevator mechanism. * - * @param kV The velocity gain. - * @param kA The acceleration gain. - * @param gearbox The type of and number of motors in the elevator - * gearbox. - * @param minHeightMeters The min allowable height of the elevator. - * @param maxHeightMeters The max allowable height of the elevator. - * @param simulateGravity Whether gravity should be simulated or not. + * @param kV The velocity gain. + * @param kA The acceleration gain. + * @param gearbox The type of and number of motors in the elevator gearbox. + * @param minHeightMeters The min allowable height of the elevator. + * @param maxHeightMeters The max allowable height of the elevator. + * @param simulateGravity Whether gravity should be simulated or not. * @param startingHeightMeters The starting height of the elevator. */ public ElevatorSim( @@ -125,15 +120,14 @@ public ElevatorSim( /** * Creates a simulated elevator mechanism. * - * @param kV The velocity gain. - * @param kA The acceleration gain. - * @param gearbox The type of and number of motors in the elevator - * gearbox. - * @param minHeightMeters The min allowable height of the elevator. - * @param maxHeightMeters The max allowable height of the elevator. - * @param simulateGravity Whether gravity should be simulated or not. + * @param kV The velocity gain. + * @param kA The acceleration gain. + * @param gearbox The type of and number of motors in the elevator gearbox. + * @param minHeightMeters The min allowable height of the elevator. + * @param maxHeightMeters The max allowable height of the elevator. + * @param simulateGravity Whether gravity should be simulated or not. * @param startingHeightMeters The starting height of the elevator. - * @param measurementStdDevs The standard deviations of the measurements. + * @param measurementStdDevs The standard deviations of the measurements. */ public ElevatorSim( double kV, @@ -157,18 +151,15 @@ public ElevatorSim( /** * Creates a simulated elevator mechanism. * - * @param gearbox The type of and number of motors in the elevator - * gearbox. - * @param gearing The gearing of the elevator (numbers greater than - * 1 represent reductions). - * @param carriageMassKg The mass of the elevator carriage. - * @param drumRadiusMeters The radius of the drum that the elevator spool is - * wrapped around. - * @param minHeightMeters The min allowable height of the elevator. - * @param maxHeightMeters The max allowable height of the elevator. - * @param simulateGravity Whether gravity should be simulated or not. + * @param gearbox The type of and number of motors in the elevator gearbox. + * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions). + * @param carriageMassKg The mass of the elevator carriage. + * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around. + * @param minHeightMeters The min allowable height of the elevator. + * @param maxHeightMeters The max allowable height of the elevator. + * @param simulateGravity Whether gravity should be simulated or not. * @param startingHeightMeters The starting height of the elevator. - * @param measurementStdDevs The standard deviations of the measurements. + * @param measurementStdDevs The standard deviations of the measurements. */ public ElevatorSim( DCMotor gearbox, @@ -193,16 +184,13 @@ public ElevatorSim( /** * Creates a simulated elevator mechanism. * - * @param gearbox The type of and number of motors in the elevator - * gearbox. - * @param gearing The gearing of the elevator (numbers greater than - * 1 represent reductions). - * @param carriageMassKg The mass of the elevator carriage. - * @param drumRadiusMeters The radius of the drum that the elevator spool is - * wrapped around. - * @param minHeightMeters The min allowable height of the elevator. - * @param maxHeightMeters The max allowable height of the elevator. - * @param simulateGravity Whether gravity should be simulated or not. + * @param gearbox The type of and number of motors in the elevator gearbox. + * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions). + * @param carriageMassKg The mass of the elevator carriage. + * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around. + * @param minHeightMeters The min allowable height of the elevator. + * @param maxHeightMeters The max allowable height of the elevator. + * @param simulateGravity Whether gravity should be simulated or not. * @param startingHeightMeters The starting height of the elevator. */ public ElevatorSim( @@ -227,11 +215,10 @@ public ElevatorSim( } /** - * Sets the elevator's state. The new position will be limited between the - * minimum and maximum + * Sets the elevator's state. The new position will be limited between the minimum and maximum * allowed heights. * - * @param positionMeters The new position in meters. + * @param positionMeters The new position in meters. * @param velocityMetersPerSecond New velocity in meters per second. */ public void setState(double positionMeters, double velocityMetersPerSecond) { @@ -309,8 +296,8 @@ public double getCurrentDrawAmps() { // v = r w, so w = v/r double kA = 1 / m_plant.getB().get(1, 0); double kV = -m_plant.getA().get(1, 1) * kA; - double motorVelocityRadPerSec = getVelocityMetersPerSecond() * - kV * m_gearbox.KvRadPerSecPerVolt; + double motorVelocityRadPerSec = + getVelocityMetersPerSecond() * kV * m_gearbox.KvRadPerSecPerVolt; var appliedVoltage = m_u.get(0, 0); return m_gearbox.getCurrent(motorVelocityRadPerSec, appliedVoltage) * Math.signum(appliedVoltage); @@ -329,23 +316,24 @@ public void setInputVoltage(double volts) { * Updates the state of the elevator. * * @param currentXhat The current state estimate. - * @param u The system inputs (voltage). - * @param dtSeconds The time difference between controller updates. + * @param u The system inputs (voltage). + * @param dtSeconds The time difference between controller updates. */ @Override protected Matrix updateX(Matrix currentXhat, Matrix u, double dtSeconds) { // Calculate updated x-hat from Runge-Kutta. - var updatedXhat = NumericalIntegration.rkdp( - (x, _u) -> { - Matrix xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u)); - if (m_simulateGravity) { - xdot = xdot.plus(VecBuilder.fill(0, -9.8)); - } - return xdot; - }, - currentXhat, - u, - dtSeconds); + var updatedXhat = + NumericalIntegration.rkdp( + (x, _u) -> { + Matrix xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u)); + if (m_simulateGravity) { + xdot = xdot.plus(VecBuilder.fill(0, -9.8)); + } + return xdot; + }, + currentXhat, + u, + dtSeconds); // We check for collisions after updating x-hat. if (wouldHitLowerLimit(updatedXhat.get(0, 0))) { From b0c6a0a1f94da0ef958d95884184e8a8a2706d67 Mon Sep 17 00:00:00 2001 From: Nick Armstrong Date: Tue, 31 Oct 2023 14:04:27 -0400 Subject: [PATCH 61/76] C++ code commit --- .../native/cpp/simulation/ElevatorSim.cpp | 25 ++++++++++++----- .../include/frc/simulation/ElevatorSim.h | 27 ++++++++++++++----- 2 files changed, 38 insertions(+), 14 deletions(-) diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index 98d0f8b3fa4..ca5583ce8c0 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -13,17 +13,14 @@ using namespace frc; using namespace frc::sim; ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 1>& plant, - const DCMotor& gearbox, double gearing, - units::meter_t drumRadius, units::meter_t minHeight, + const DCMotor& gearbox, units::meter_t minHeight, units::meter_t maxHeight, bool simulateGravity, units::meter_t startingHeight, const std::array& measurementStdDevs) : LinearSystemSim(plant, measurementStdDevs), m_gearbox(gearbox), - m_drumRadius(drumRadius), m_minHeight(minHeight), m_maxHeight(maxHeight), - m_gearing(gearing), m_simulateGravity(simulateGravity) { SetState(startingHeight, 0_mps); } @@ -36,8 +33,20 @@ ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing, const std::array& measurementStdDevs) : ElevatorSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass, drumRadius, gearing), - gearbox, gearing, drumRadius, minHeight, maxHeight, - simulateGravity, startingHeight, measurementStdDevs) {} + gearbox, minHeight, maxHeight, simulateGravity, + startingHeight, measurementStdDevs) {} + +template + requires std::same_as +ElevatorSim::ElevatorSim(decltype(1_V / Velocity_t(1)) kV, + decltype(1_V / Acceleration_t(1)) kA, + const DCMotor& gearbox, units::meter_t minHeight, + units::meter_t maxHeight, bool simulateGravity, + units::meter_t startingHeight, + const std::array& measurementStdDevs) + : ElevatorSim(LinearSystemId::IdentifyPositionSystem(kV, kA), gearbox, + minHeight, maxHeight, simulateGravity, startingHeight, + measurementStdDevs) {} void ElevatorSim::SetState(units::meter_t position, units::meters_per_second_t velocity) { @@ -75,9 +84,11 @@ units::ampere_t ElevatorSim::GetCurrentDraw() const { // is spinning 10x faster than the output. // v = r w, so w = v / r + auto kA = 1.0 / m_plant.B(1, 0); + auto kV = -m_plant.A(1, 1) * kA; units::meters_per_second_t velocity{m_x(1)}; units::radians_per_second_t motorVelocity = - velocity / m_drumRadius * m_gearing * 1_rad; + velocity * kV * m_gearbox.Kv * 1_rad; // Perform calculation and return. return m_gearbox.Current(motorVelocity, units::volt_t{m_u(0)}) * diff --git a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h index 705ddbcd251..48bdc4cc49a 100644 --- a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h @@ -27,10 +27,6 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { * LinearSystemId::ElevatorSystem(). * @param gearbox The type of and number of motors in your * elevator gearbox. - * @param gearing The gearing of the elevator (numbers greater - * than 1 represent reductions). - * @param drumRadius The radius of the drum that your cable is - * wrapped around. * @param minHeight The minimum allowed height of the elevator. * @param maxHeight The maximum allowed height of the elevator. * @param simulateGravity Whether gravity should be simulated or not. @@ -38,7 +34,6 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { * @param measurementStdDevs The standard deviation of the measurements. */ ElevatorSim(const LinearSystem<2, 1, 1>& plant, const DCMotor& gearbox, - double gearing, units::meter_t drumRadius, units::meter_t minHeight, units::meter_t maxHeight, bool simulateGravity, units::meter_t startingHeight, const std::array& measurementStdDevs = {0.0}); @@ -64,6 +59,26 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { units::meter_t minHeight, units::meter_t maxHeight, bool simulateGravity, units::meter_t startingHeight, const std::array& measurementStdDevs = {0.0}); + /** + * Creates a simulated elevator mechanism. + * + * @param kV The velocity gain. + * @param kA The acceleration gain. + * @param gearbox The type of and number of motors in the elevator gearbox. + * @param minHeightMeters The min allowable height of the elevator. + * @param maxHeightMeters The max allowable height of the elevator. + * @param simulateGravity Whether gravity should be simulated or not. + * @param startingHeightMeters The starting height of the elevator. + * @param measurementStdDevs The standard deviation of the measurements. + */ + template + requires std::same_as + ElevatorSim(decltype(1_V / Velocity_t(1)) kV, + decltype(1_V / Acceleration_t(1)) kA, + const DCMotor& gearbox, units::meter_t minHeight, + units::meter_t maxHeight, bool simulateGravity, + units::meter_t startingHeight, + const std::array& measurementStdDevs = {0.0}); using LinearSystemSim::SetState; @@ -146,10 +161,8 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { private: DCMotor m_gearbox; - units::meter_t m_drumRadius; units::meter_t m_minHeight; units::meter_t m_maxHeight; - double m_gearing; bool m_simulateGravity; }; } // namespace frc::sim From b3eb155a57d65ca4a5052413de506c63540dc2e1 Mon Sep 17 00:00:00 2001 From: Nick Armstrong Date: Tue, 31 Oct 2023 14:10:15 -0400 Subject: [PATCH 62/76] c++ documentation fix --- wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h index 48bdc4cc49a..7b5a81470dc 100644 --- a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h @@ -65,8 +65,8 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { * @param kV The velocity gain. * @param kA The acceleration gain. * @param gearbox The type of and number of motors in the elevator gearbox. - * @param minHeightMeters The min allowable height of the elevator. - * @param maxHeightMeters The max allowable height of the elevator. + * @param minHeight The min allowable height of the elevator. + * @param maxHeight The max allowable height of the elevator. * @param simulateGravity Whether gravity should be simulated or not. * @param startingHeightMeters The starting height of the elevator. * @param measurementStdDevs The standard deviation of the measurements. From 89312ad98a6733edb59269fda0b581958358a6c0 Mon Sep 17 00:00:00 2001 From: Nick Armstrong Date: Tue, 31 Oct 2023 14:13:54 -0400 Subject: [PATCH 63/76] additional cpp doc fix --- wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h index 7b5a81470dc..873565d6817 100644 --- a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h @@ -68,7 +68,7 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { * @param minHeight The min allowable height of the elevator. * @param maxHeight The max allowable height of the elevator. * @param simulateGravity Whether gravity should be simulated or not. - * @param startingHeightMeters The starting height of the elevator. + * @param startingHeight The starting height of the elevator. * @param measurementStdDevs The standard deviation of the measurements. */ template From d282d199be2283edd25acf558b9647c07c0ac183 Mon Sep 17 00:00:00 2001 From: Nick Armstrong Date: Tue, 31 Oct 2023 14:23:32 -0400 Subject: [PATCH 64/76] template adjustment cpp --- wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp | 3 ++- wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h | 5 +++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index ca5583ce8c0..670db344618 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -37,7 +37,8 @@ ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing, startingHeight, measurementStdDevs) {} template - requires std::same_as + requires std::same_as || + std::same_as ElevatorSim::ElevatorSim(decltype(1_V / Velocity_t(1)) kV, decltype(1_V / Acceleration_t(1)) kA, const DCMotor& gearbox, units::meter_t minHeight, diff --git a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h index 873565d6817..2d02f511d3f 100644 --- a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h @@ -71,8 +71,9 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { * @param startingHeight The starting height of the elevator. * @param measurementStdDevs The standard deviation of the measurements. */ - template - requires std::same_as +template + requires std::same_as || + std::same_as ElevatorSim(decltype(1_V / Velocity_t(1)) kV, decltype(1_V / Acceleration_t(1)) kA, const DCMotor& gearbox, units::meter_t minHeight, From 1d5da552102a299abc6ec20abfd39e7cba36439c Mon Sep 17 00:00:00 2001 From: Nick Armstrong Date: Tue, 31 Oct 2023 14:31:48 -0400 Subject: [PATCH 65/76] added units include --- wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index 670db344618..58643e3ceeb 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -8,6 +8,7 @@ #include "frc/system/NumericalIntegration.h" #include "frc/system/plant/LinearSystemId.h" +#include "units/length.h" using namespace frc; using namespace frc::sim; From 2e8e9fc6ba8777a1f9eaa4a9286ef7353c6dd706 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Tue, 31 Oct 2023 15:45:12 -0400 Subject: [PATCH 66/76] fix to template --- wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp | 4 ---- .../src/main/native/include/frc/simulation/ElevatorSim.h | 9 +++++++++ 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index 58643e3ceeb..26c979310e3 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -8,7 +8,6 @@ #include "frc/system/NumericalIntegration.h" #include "frc/system/plant/LinearSystemId.h" -#include "units/length.h" using namespace frc; using namespace frc::sim; @@ -37,9 +36,6 @@ ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing, gearbox, minHeight, maxHeight, simulateGravity, startingHeight, measurementStdDevs) {} -template - requires std::same_as || - std::same_as ElevatorSim::ElevatorSim(decltype(1_V / Velocity_t(1)) kV, decltype(1_V / Acceleration_t(1)) kA, const DCMotor& gearbox, units::meter_t minHeight, diff --git a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h index 2d02f511d3f..2c91f67ee36 100644 --- a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h @@ -19,6 +19,15 @@ namespace frc::sim { */ class ElevatorSim : public LinearSystemSim<2, 1, 1> { public: + template + using Velocity_t = units::unit_t< + units::compound_unit>>; + + template + using Acceleration_t = units::unit_t>, + units::inverse>>; + /** * Constructs a simulated elevator mechanism. * From afdfd6c97b9634fc86b284b77cc0b90e6bc731dd Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Tue, 31 Oct 2023 15:56:39 -0400 Subject: [PATCH 67/76] unit issue --- wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index 26c979310e3..53ddfce0c1f 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -86,7 +86,7 @@ units::ampere_t ElevatorSim::GetCurrentDraw() const { auto kV = -m_plant.A(1, 1) * kA; units::meters_per_second_t velocity{m_x(1)}; units::radians_per_second_t motorVelocity = - velocity * kV * m_gearbox.Kv * 1_rad; + velocity * kV.value() * m_gearbox.Kv * 1_rad; // Perform calculation and return. return m_gearbox.Current(motorVelocity, units::volt_t{m_u(0)}) * From 1f77f7e6a0b946ccc397484700c55ba4a0688544 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Tue, 31 Oct 2023 16:02:29 -0400 Subject: [PATCH 68/76] another unit fix --- wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index 53ddfce0c1f..35f128928d9 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -82,11 +82,11 @@ units::ampere_t ElevatorSim::GetCurrentDraw() const { // is spinning 10x faster than the output. // v = r w, so w = v / r - auto kA = 1.0 / m_plant.B(1, 0); - auto kV = -m_plant.A(1, 1) * kA; + double kA = 1.0 / m_plant.B(1, 0); + double kV = -m_plant.A(1, 1) * kA; units::meters_per_second_t velocity{m_x(1)}; units::radians_per_second_t motorVelocity = - velocity * kV.value() * m_gearbox.Kv * 1_rad; + velocity * kV * m_gearbox.Kv * 1_rad; // Perform calculation and return. return m_gearbox.Current(motorVelocity, units::volt_t{m_u(0)}) * From f6a0327e8db9d6d4725c29918a9ba7f5e68a196b Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Tue, 31 Oct 2023 17:06:24 -0400 Subject: [PATCH 69/76] units fix again --- .../native/cpp/simulation/ElevatorSim.cpp | 23 ++++++------ .../include/frc/simulation/ElevatorSim.h | 36 ++++++++++--------- 2 files changed, 33 insertions(+), 26 deletions(-) diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index 35f128928d9..0a90a329e3b 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -33,18 +33,21 @@ ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing, const std::array& measurementStdDevs) : ElevatorSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass, drumRadius, gearing), - gearbox, minHeight, maxHeight, simulateGravity, - startingHeight, measurementStdDevs) {} + gearbox, minHeight, maxHeight, + simulateGravity, startingHeight, measurementStdDevs) {} + template + requires std::same_as || + std::same_as ElevatorSim::ElevatorSim(decltype(1_V / Velocity_t(1)) kV, - decltype(1_V / Acceleration_t(1)) kA, - const DCMotor& gearbox, units::meter_t minHeight, + decltype(1_V / Acceleration_t(1)) kA, const DCMotor& gearbox, + units::meter_t minHeight, units::meter_t maxHeight, bool simulateGravity, units::meter_t startingHeight, const std::array& measurementStdDevs) - : ElevatorSim(LinearSystemId::IdentifyPositionSystem(kV, kA), gearbox, - minHeight, maxHeight, simulateGravity, startingHeight, - measurementStdDevs) {} + : ElevatorSim(LinearSystemId::IdentifyPositionSystem(kV, kA), + gearbox, minHeight, maxHeight, + simulateGravity, startingHeight, measurementStdDevs) {} void ElevatorSim::SetState(units::meter_t position, units::meters_per_second_t velocity) { @@ -83,10 +86,10 @@ units::ampere_t ElevatorSim::GetCurrentDraw() const { // v = r w, so w = v / r double kA = 1.0 / m_plant.B(1, 0); - double kV = -m_plant.A(1, 1) * kA; + double kV = kA * m_plant.A(1, 1); units::meters_per_second_t velocity{m_x(1)}; units::radians_per_second_t motorVelocity = - velocity * kV * m_gearbox.Kv * 1_rad; + units::radians_per_second_t{velocity.value() * kV * m_gearbox.Kv.value()}; // Perform calculation and return. return m_gearbox.Current(motorVelocity, units::volt_t{m_u(0)}) * @@ -117,4 +120,4 @@ Vectord<2> ElevatorSim::UpdateX(const Vectord<2>& currentXhat, return Vectord<2>{m_maxHeight.value(), 0.0}; } return updatedXhat; -} +} \ No newline at end of file diff --git a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h index 2c91f67ee36..d797dc5720c 100644 --- a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h @@ -19,7 +19,7 @@ namespace frc::sim { */ class ElevatorSim : public LinearSystemSim<2, 1, 1> { public: - template + template using Velocity_t = units::unit_t< units::compound_unit>>; @@ -36,6 +36,10 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { * LinearSystemId::ElevatorSystem(). * @param gearbox The type of and number of motors in your * elevator gearbox. + * @param gearing The gearing of the elevator (numbers greater + * than 1 represent reductions). + * @param drumRadius The radius of the drum that your cable is + * wrapped around. * @param minHeight The minimum allowed height of the elevator. * @param maxHeight The maximum allowed height of the elevator. * @param simulateGravity Whether gravity should be simulated or not. @@ -68,28 +72,28 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { units::meter_t minHeight, units::meter_t maxHeight, bool simulateGravity, units::meter_t startingHeight, const std::array& measurementStdDevs = {0.0}); + /** - * Creates a simulated elevator mechanism. + * Constructs a simulated elevator mechanism. * - * @param kV The velocity gain. - * @param kA The acceleration gain. - * @param gearbox The type of and number of motors in the elevator gearbox. - * @param minHeight The min allowable height of the elevator. - * @param maxHeight The max allowable height of the elevator. - * @param simulateGravity Whether gravity should be simulated or not. - * @param startingHeight The starting height of the elevator. + * @param kV The velocity gain. + * @param kA The acceleration gain. + * @param gearbox The type of and number of motors in your + * elevator gearbox. + * @param minHeight The minimum allowed height of the elevator. + * @param maxHeight The maximum allowed height of the elevator. + * @param simulateGravity Whether gravity should be simulated or not. + * @param startingHeight The starting height of the elevator. * @param measurementStdDevs The standard deviation of the measurements. */ -template + template requires std::same_as || std::same_as ElevatorSim(decltype(1_V / Velocity_t(1)) kV, - decltype(1_V / Acceleration_t(1)) kA, - const DCMotor& gearbox, units::meter_t minHeight, - units::meter_t maxHeight, bool simulateGravity, - units::meter_t startingHeight, + decltype(1_V / Acceleration_t(1)) kA, const DCMotor& gearbox, + units::meter_t minHeight, units::meter_t maxHeight, + bool simulateGravity, units::meter_t startingHeight, const std::array& measurementStdDevs = {0.0}); - using LinearSystemSim::SetState; /** @@ -175,4 +179,4 @@ template units::meter_t m_maxHeight; bool m_simulateGravity; }; -} // namespace frc::sim +} // namespace frc::sim \ No newline at end of file From 51e5bac4a15ce91305253968f670927491f1b902 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Tue, 31 Oct 2023 21:24:29 +0000 Subject: [PATCH 70/76] Formatting fixes --- .../native/cpp/simulation/ElevatorSim.cpp | 22 +++++++++---------- .../include/frc/simulation/ElevatorSim.h | 11 +++++----- 2 files changed, 17 insertions(+), 16 deletions(-) diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index 0a90a329e3b..0ac426d0a6b 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -33,21 +33,21 @@ ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing, const std::array& measurementStdDevs) : ElevatorSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass, drumRadius, gearing), - gearbox, minHeight, maxHeight, - simulateGravity, startingHeight, measurementStdDevs) {} + gearbox, minHeight, maxHeight, simulateGravity, + startingHeight, measurementStdDevs) {} - template - requires std::same_as || - std::same_as +template + requires std::same_as || + std::same_as ElevatorSim::ElevatorSim(decltype(1_V / Velocity_t(1)) kV, - decltype(1_V / Acceleration_t(1)) kA, const DCMotor& gearbox, - units::meter_t minHeight, + decltype(1_V / Acceleration_t(1)) kA, + const DCMotor& gearbox, units::meter_t minHeight, units::meter_t maxHeight, bool simulateGravity, units::meter_t startingHeight, const std::array& measurementStdDevs) - : ElevatorSim(LinearSystemId::IdentifyPositionSystem(kV, kA), - gearbox, minHeight, maxHeight, - simulateGravity, startingHeight, measurementStdDevs) {} + : ElevatorSim(LinearSystemId::IdentifyPositionSystem(kV, kA), gearbox, + minHeight, maxHeight, simulateGravity, startingHeight, + measurementStdDevs) {} void ElevatorSim::SetState(units::meter_t position, units::meters_per_second_t velocity) { @@ -120,4 +120,4 @@ Vectord<2> ElevatorSim::UpdateX(const Vectord<2>& currentXhat, return Vectord<2>{m_maxHeight.value(), 0.0}; } return updatedXhat; -} \ No newline at end of file +} diff --git a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h index d797dc5720c..049afe5b8b1 100644 --- a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h @@ -19,7 +19,7 @@ namespace frc::sim { */ class ElevatorSim : public LinearSystemSim<2, 1, 1> { public: - template + template using Velocity_t = units::unit_t< units::compound_unit>>; @@ -90,9 +90,10 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { requires std::same_as || std::same_as ElevatorSim(decltype(1_V / Velocity_t(1)) kV, - decltype(1_V / Acceleration_t(1)) kA, const DCMotor& gearbox, - units::meter_t minHeight, units::meter_t maxHeight, - bool simulateGravity, units::meter_t startingHeight, + decltype(1_V / Acceleration_t(1)) kA, + const DCMotor& gearbox, units::meter_t minHeight, + units::meter_t maxHeight, bool simulateGravity, + units::meter_t startingHeight, const std::array& measurementStdDevs = {0.0}); using LinearSystemSim::SetState; @@ -179,4 +180,4 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { units::meter_t m_maxHeight; bool m_simulateGravity; }; -} // namespace frc::sim \ No newline at end of file +} // namespace frc::sim From 59680e30710bc69c95c33544cbc503de1c4171f8 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Tue, 31 Oct 2023 17:28:25 -0400 Subject: [PATCH 71/76] possible unit fix --- wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index 0a90a329e3b..8f6a68e9747 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -86,10 +86,9 @@ units::ampere_t ElevatorSim::GetCurrentDraw() const { // v = r w, so w = v / r double kA = 1.0 / m_plant.B(1, 0); - double kV = kA * m_plant.A(1, 1); + decltype(1_V / Velocity_t(1)) kV = decltype(1_V / Velocity_t(1)){kA * m_plant.A(1, 1)}; units::meters_per_second_t velocity{m_x(1)}; - units::radians_per_second_t motorVelocity = - units::radians_per_second_t{velocity.value() * kV * m_gearbox.Kv.value()}; + units::radians_per_second_t motorVelocity = velocity * kV * m_gearbox.Kv; // Perform calculation and return. return m_gearbox.Current(motorVelocity, units::volt_t{m_u(0)}) * From 111925953ca00557baf4d3851de6e963e3819edd Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Tue, 31 Oct 2023 17:32:58 -0400 Subject: [PATCH 72/76] unit type declared properly --- wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index a6e0b884c6a..3d09975319c 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -86,9 +86,10 @@ units::ampere_t ElevatorSim::GetCurrentDraw() const { // v = r w, so w = v / r double kA = 1.0 / m_plant.B(1, 0); - decltype(1_V / Velocity_t(1)) kV = decltype(1_V / Velocity_t(1)){kA * m_plant.A(1, 1)}; + using Kv_t = units::unit_t>>; + Kv_t Kv = Kv_t{kA * m_plant.A(1, 1)}; units::meters_per_second_t velocity{m_x(1)}; - units::radians_per_second_t motorVelocity = velocity * kV * m_gearbox.Kv; + units::radians_per_second_t motorVelocity = velocity * Kv * m_gearbox.Kv; // Perform calculation and return. return m_gearbox.Current(motorVelocity, units::volt_t{m_u(0)}) * From 0dd63520c90ece40d8a0ea7680d06ad0cba2749c Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Tue, 31 Oct 2023 21:41:52 +0000 Subject: [PATCH 73/76] Formatting fixes --- wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index 3d09975319c..defbe0d7830 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -86,8 +86,9 @@ units::ampere_t ElevatorSim::GetCurrentDraw() const { // v = r w, so w = v / r double kA = 1.0 / m_plant.B(1, 0); - using Kv_t = units::unit_t>>; - Kv_t Kv = Kv_t{kA * m_plant.A(1, 1)}; + using Kv_t = units::unit_t>>; + Kv_t Kv = Kv_t{kA * m_plant.A(1, 1)}; units::meters_per_second_t velocity{m_x(1)}; units::radians_per_second_t motorVelocity = velocity * Kv * m_gearbox.Kv; From 5a7f05a112a2cd80123053173ed87026b3e7a86f Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Tue, 31 Oct 2023 17:50:36 -0400 Subject: [PATCH 74/76] comment removed --- wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index defbe0d7830..02817302147 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -84,7 +84,6 @@ units::ampere_t ElevatorSim::GetCurrentDraw() const { // Reductions are greater than 1, so a reduction of 10:1 would mean the motor // is spinning 10x faster than the output. - // v = r w, so w = v / r double kA = 1.0 / m_plant.B(1, 0); using Kv_t = units::unit_t>>; From 2a4f4d862c927c27bd0f7ae58f69a0c4564c05e4 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Tue, 31 Oct 2023 18:27:28 -0400 Subject: [PATCH 75/76] fixed documentation --- wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h index 049afe5b8b1..5aa785b914a 100644 --- a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h @@ -36,8 +36,6 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { * LinearSystemId::ElevatorSystem(). * @param gearbox The type of and number of motors in your * elevator gearbox. - * @param gearing The gearing of the elevator (numbers greater - * than 1 represent reductions). * @param drumRadius The radius of the drum that your cable is * wrapped around. * @param minHeight The minimum allowed height of the elevator. From 2e472b0096dc7e3677941fde6f30615ec9050b47 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Tue, 31 Oct 2023 18:38:45 -0400 Subject: [PATCH 76/76] fixed docs again --- wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h index 5aa785b914a..3d5c43398e5 100644 --- a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h @@ -36,8 +36,6 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { * LinearSystemId::ElevatorSystem(). * @param gearbox The type of and number of motors in your * elevator gearbox. - * @param drumRadius The radius of the drum that your cable is - * wrapped around. * @param minHeight The minimum allowed height of the elevator. * @param maxHeight The maximum allowed height of the elevator. * @param simulateGravity Whether gravity should be simulated or not.