From 199301ad26bb72c7d70792f161d6bebc7a9661b1 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Wed, 28 Nov 2018 17:01:31 -0800 Subject: [PATCH 01/51] Revert "Remove characterization run configurations" This reverts commit 70de7557 --- ...ploy_AccelerationCharacterizationRobot.xml | 21 +++++++++++++++++++ .../Deploy_VelocityCharacterizationRobot.xml | 21 +++++++++++++++++++ .../Deploy_WheelbaseTestRobot.xml | 21 +++++++++++++++++++ 3 files changed, 63 insertions(+) create mode 100644 .idea/runConfigurations/Deploy_AccelerationCharacterizationRobot.xml create mode 100644 .idea/runConfigurations/Deploy_VelocityCharacterizationRobot.xml create mode 100644 .idea/runConfigurations/Deploy_WheelbaseTestRobot.xml diff --git a/.idea/runConfigurations/Deploy_AccelerationCharacterizationRobot.xml b/.idea/runConfigurations/Deploy_AccelerationCharacterizationRobot.xml new file mode 100644 index 00000000..493ed106 --- /dev/null +++ b/.idea/runConfigurations/Deploy_AccelerationCharacterizationRobot.xml @@ -0,0 +1,21 @@ + + + + + + + true + + + diff --git a/.idea/runConfigurations/Deploy_VelocityCharacterizationRobot.xml b/.idea/runConfigurations/Deploy_VelocityCharacterizationRobot.xml new file mode 100644 index 00000000..b8a25465 --- /dev/null +++ b/.idea/runConfigurations/Deploy_VelocityCharacterizationRobot.xml @@ -0,0 +1,21 @@ + + + + + + + true + + + diff --git a/.idea/runConfigurations/Deploy_WheelbaseTestRobot.xml b/.idea/runConfigurations/Deploy_WheelbaseTestRobot.xml new file mode 100644 index 00000000..72f542de --- /dev/null +++ b/.idea/runConfigurations/Deploy_WheelbaseTestRobot.xml @@ -0,0 +1,21 @@ + + + + + + + true + + + From ff0aa0aa868516fe1ee6127e7e643b98e40185da Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Wed, 28 Nov 2018 17:01:35 -0800 Subject: [PATCH 02/51] Revert "Remove MotionProfileTesting run config" This reverts commit 9965c02d --- .../Deploy_MotionProfileTesting.xml | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 .idea/runConfigurations/Deploy_MotionProfileTesting.xml diff --git a/.idea/runConfigurations/Deploy_MotionProfileTesting.xml b/.idea/runConfigurations/Deploy_MotionProfileTesting.xml new file mode 100644 index 00000000..b88dd855 --- /dev/null +++ b/.idea/runConfigurations/Deploy_MotionProfileTesting.xml @@ -0,0 +1,21 @@ + + + + + + + true + + + From 183cdafb6b83e58ea62775708f33f58a7b6edaff Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Wed, 28 Nov 2018 17:01:50 -0800 Subject: [PATCH 03/51] Revert "Remove characterization robots" This reverts commit a2fffca9 --- .../AccelerationCharacterizationRobot.java | 233 +++++++++++++++++ .../robots/VelocityCharacterizationRobot.java | 238 ++++++++++++++++++ .../util/robots/WheelbaseTestRobot.java | 180 +++++++++++++ 3 files changed, 651 insertions(+) create mode 100644 lib/src/main/java/org/team1540/rooster/util/robots/AccelerationCharacterizationRobot.java create mode 100644 lib/src/main/java/org/team1540/rooster/util/robots/VelocityCharacterizationRobot.java create mode 100644 lib/src/main/java/org/team1540/rooster/util/robots/WheelbaseTestRobot.java diff --git a/lib/src/main/java/org/team1540/rooster/util/robots/AccelerationCharacterizationRobot.java b/lib/src/main/java/org/team1540/rooster/util/robots/AccelerationCharacterizationRobot.java new file mode 100644 index 00000000..7e032da0 --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/util/robots/AccelerationCharacterizationRobot.java @@ -0,0 +1,233 @@ +package org.team1540.rooster.util.robots; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.command.Scheduler; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.io.File; +import java.io.FileNotFoundException; +import java.io.PrintWriter; +import java.util.LinkedList; +import java.util.List; +import org.apache.commons.math3.stat.regression.SimpleRegression; +import org.team1540.rooster.preferencemanager.Preference; +import org.team1540.rooster.preferencemanager.PreferenceManager; +import org.team1540.rooster.wrappers.ChickenTalon; + +/** + * Self-contained robot class to characterize a drivetrain's acceleration term. + * + * When deployed to a three-motor-per-side robot with left motors on motors 1, 2, and 3 and right + * motors on motors 4, 5, and 6, whenever the A button is pressed and held during teleop the robot + * will carry out an acceleration characterization as described in Eli Barnett's paper "FRC + * Drivetrain Characterization" until the button is released. (Encoders should be on motors 1 and + * 4.) The results will be saved in a file in the /home/lvuser/dtmeasure directory named + * "measureaccel-" followed by the Unix timestamp of the test start and ".csv". + */ +public class AccelerationCharacterizationRobot extends IterativeRobot { + + @Preference("kV") + public double kV; + @Preference("VIntercept") + public double vIntercept; + + @Preference("Invert Left Motor") + public boolean invertLeft = false; + @Preference("Invert Right Motor") + public boolean invertRight = true; + @Preference(persistent = false) + public int accelMeasurementWindow = 6; + + @Preference(persistent = false) + public double setpoint = 0.6; + + private ChickenTalon driveLeftMotorA = new ChickenTalon(1); + private ChickenTalon driveLeftMotorB = new ChickenTalon(2); + private ChickenTalon driveLeftMotorC = new ChickenTalon(3); + private ChickenTalon[] driveLeftMotors = new ChickenTalon[]{driveLeftMotorA, driveLeftMotorB, + driveLeftMotorC}; + private ChickenTalon driveRightMotorA = new ChickenTalon(4); + private ChickenTalon driveRightMotorB = new ChickenTalon(5); + private ChickenTalon driveRightMotorC = new ChickenTalon(6); + private ChickenTalon[] driveRightMotors = new ChickenTalon[]{driveRightMotorA, driveRightMotorB, + driveRightMotorC}; + private ChickenTalon[] driveMotorAll = new ChickenTalon[]{driveLeftMotorA, driveLeftMotorB, + driveLeftMotorC, driveRightMotorA, driveRightMotorB, driveRightMotorC}; + private ChickenTalon[] driveMotorMasters = new ChickenTalon[]{driveLeftMotorA, driveRightMotorA}; + + private PrintWriter csvWriter = null; + + private Notifier notifier = new Notifier(this::run); + + @SuppressWarnings("SuspiciousNameCombination") + private void run() { + if (!isOperatorControl() && csvWriter != null) { + csvWriter.close(); + csvWriter = null; + } + if (leftVelocities.size() == accelMeasurementWindow) { + leftVelocities.remove(0); + rightVelocities.remove(0); + leftVoltages.remove(0); + rightVoltages.remove(0); + times.remove(0); + } + double leftVelocity = driveLeftMotorA.getSelectedSensorVelocity(); + double rightVelocity = driveRightMotorA.getSelectedSensorVelocity(); + + leftVelocities.add(leftVelocity); + rightVelocities.add(rightVelocity); + + double accelCausingVoltageLeft = + driveLeftMotorA.getMotorOutputVoltage() - (kV * leftVelocity + + vIntercept); + double accelCausingVoltageRight = + driveRightMotorA.getMotorOutputVoltage() - (kV * rightVelocity + + vIntercept); + leftVoltages.add(accelCausingVoltageLeft); + rightVoltages.add(accelCausingVoltageRight); + times.add((double) System.currentTimeMillis() / 1000.0); + + if (leftVelocities.size() == accelMeasurementWindow) { + double lAccel = bestFitSlope(times, leftVelocities); + double rAccel = bestFitSlope(times, rightVelocities); + SmartDashboard.putNumber("Left Accel", lAccel); + SmartDashboard.putNumber("Right Accel", rAccel); + } + + if (joystick.getRawButton(1)) { // if button A is pressed + if (csvWriter == null) { + // create a new CSV writer, reset everything + reset(); + try { + File dir = new File("/home/lvuser/dtmeasure/"); + if (!dir.exists()) { + dir.mkdirs(); + } + csvWriter = new PrintWriter(new File( + "/home/lvuser/dtmeasure/measureaccel-" + System.currentTimeMillis() + ".csv")); + csvWriter.println("lvoltage,laccel,rvoltage,raccel"); + } catch (FileNotFoundException e) { + throw new RuntimeException(e); + } + driveLeftMotorA.set(ControlMode.PercentOutput, 0); + driveRightMotorA.set(ControlMode.PercentOutput, 0); + } else { + SmartDashboard.putNumber("Left Output", driveLeftMotorA.getMotorOutputPercent()); + SmartDashboard.putNumber("Left Velocity", leftVelocity); + SmartDashboard.putNumber("Right Output", driveRightMotorA.getMotorOutputPercent()); + SmartDashboard.putNumber("Right Velocity", rightVelocity); + + if (leftVelocities.size() == accelMeasurementWindow) { + double lAccel = bestFitSlope(times, leftVelocities); + double rAccel = bestFitSlope(times, rightVelocities); + csvWriter.println( + leftVoltages.get(1) + "," + lAccel + "," + rightVoltages.get(1) + "," + rAccel); + System.out.println(leftVelocities.toString()); + System.out.println(times.toString()); + System.out.println(lAccel); + } + driveLeftMotorA.set(ControlMode.PercentOutput, setpoint); + driveRightMotorA.set(ControlMode.PercentOutput, setpoint); + } + } else { + if (csvWriter != null) { + csvWriter.close(); + csvWriter = null; + } + driveLeftMotorA.set(ControlMode.PercentOutput, 0); + driveRightMotorA.set(ControlMode.PercentOutput, 0); + } + } + + private Joystick joystick = new Joystick(0); + + private List leftVelocities = new LinkedList<>(); + private List leftVoltages = new LinkedList<>(); + private List rightVelocities = new LinkedList<>(); + private List rightVoltages = new LinkedList<>(); + private List times = new LinkedList<>(); + + @Override + public void robotInit() { + PreferenceManager.getInstance().add(this); + reset(); + notifier.startPeriodic(0.01); + + SmartDashboard.setDefaultNumber("Setpoint", 0.6); + } + + @Override + public void robotPeriodic() { + Scheduler.getInstance().run(); // process preferences + } + + @Override + public void teleopPeriodic() { + + } + + @Override + public void teleopInit() { + reset(); + for (ChickenTalon talon : driveMotorAll) { + talon.setBrake(true); + } + } + + @Override + public void disabledInit() { + for (ChickenTalon talon : driveMotorAll) { + talon.setBrake(false); + } + } + + @SuppressWarnings("Duplicates") + public void reset() { + driveLeftMotorA.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); + driveRightMotorA.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); + + driveLeftMotorA.setSensorPhase(true); + + for (ChickenTalon talon : driveLeftMotors) { + talon.setInverted(invertLeft); + } + + driveRightMotorA.setSensorPhase(true); + + for (ChickenTalon talon : driveRightMotors) { + talon.setInverted(invertRight); + } + + driveLeftMotorB.set(ControlMode.Follower, driveLeftMotorA.getDeviceID()); + driveLeftMotorC.set(ControlMode.Follower, driveLeftMotorA.getDeviceID()); + + driveRightMotorB.set(ControlMode.Follower, driveRightMotorA.getDeviceID()); + driveRightMotorC.set(ControlMode.Follower, driveRightMotorA.getDeviceID()); + + for (ChickenTalon talon : driveMotorAll) { + talon.setBrake(true); + } + + for (ChickenTalon talon : driveMotorAll) { + talon.configClosedloopRamp(0); + talon.configOpenloopRamp(0); + talon.configPeakOutputForward(1); + talon.configPeakOutputReverse(-1); + talon.enableCurrentLimit(false); + talon.configVoltageCompSaturation(12); + talon.enableVoltageCompensation(true); + } + } + + private static double bestFitSlope(List xVals, List yVals) { + SimpleRegression reg = new SimpleRegression(); + for (int i = 0; i < xVals.size(); i++) { + reg.addData(xVals.get(i), yVals.get(i)); + } + return reg.getSlope(); + } +} diff --git a/lib/src/main/java/org/team1540/rooster/util/robots/VelocityCharacterizationRobot.java b/lib/src/main/java/org/team1540/rooster/util/robots/VelocityCharacterizationRobot.java new file mode 100644 index 00000000..c1c5a43d --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/util/robots/VelocityCharacterizationRobot.java @@ -0,0 +1,238 @@ +package org.team1540.rooster.util.robots; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.Scheduler; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import org.apache.commons.math3.stat.regression.SimpleRegression; +import org.jetbrains.annotations.NotNull; +import org.team1540.rooster.preferencemanager.Preference; +import org.team1540.rooster.preferencemanager.PreferenceManager; +import org.team1540.rooster.util.SimpleCommand; +import org.team1540.rooster.wrappers.ChickenTalon; + +//TODO MORE DOCS + +/** + * Self-contained robot class to characterize a drivetrain's velocity term. + * + * Whenever the A button is pressed and held during teleop the robot will carry out a quasi-static + * velocity characterization as described in Eli Barnett's paper "FRC Drivetrain Characterization" + * until the button is released. The results (kV, vIntercept, and an R^2 value) are output to the + * SmartDashboard. + */ +public class VelocityCharacterizationRobot extends IterativeRobot { + + @Preference(persistent = false) + public int lMotor1ID = -1; + @Preference(persistent = false) + public int lMotor2ID = -1; + @Preference(persistent = false) + public int lMotor3ID = -1; + @Preference(persistent = false) + public int rMotor1ID = -1; + @Preference(persistent = false) + public int rMotor2ID = -1; + @Preference(persistent = false) + public int rMotor3ID = -1; + @Preference(persistent = false) + public boolean invertLeftMotor = false; + @Preference(persistent = false) + public boolean invertRightMotor = false; + @Preference(persistent = false) + public boolean invertLeftSensor = false; + @Preference(persistent = false) + public boolean invertRightSensor = false; + @Preference(persistent = false) + public boolean brake = false; + @Preference(persistent = false) + public double tpu = 1; + + + private ChickenTalon driveLeftMotorA; + private ChickenTalon driveLeftMotorB; + private ChickenTalon driveLeftMotorC; + private ChickenTalon driveRightMotorA; + private ChickenTalon driveRightMotorB; + private ChickenTalon driveRightMotorC; + + private SimpleRegression leftRegression = new SimpleRegression(); + private SimpleRegression rightRegression = new SimpleRegression(); + private boolean running = false; + + private Joystick joystick = new Joystick(0); + + private static final double SATURATION_VOLTAGE = 12; + @Preference("Voltage Ramp Rate (V/sec)") + public double rampRate = 0.25; + + @Preference("Invert Left Motor") + public boolean invertLeft = false; + @Preference("Invert Right Motor") + public boolean invertRight = true; + + private double appliedOutput = 0; + + public long lastTime; + + @Override + public void robotInit() { + PreferenceManager.getInstance().add(this); + Command reset = new SimpleCommand("Reset", () -> { + if (lMotor1ID != -1) { + driveLeftMotorA = new ChickenTalon(lMotor1ID); + } else { + System.err.println("Left Motor 1 must be set!"); + return; + } + if (lMotor2ID != -1) { + driveLeftMotorB = new ChickenTalon(lMotor2ID); + driveLeftMotorB.set(ControlMode.Follower, driveLeftMotorA.getDeviceID()); + } else { + if (driveLeftMotorB != null) { + driveLeftMotorB.set(ControlMode.PercentOutput, 0); + } + driveLeftMotorB = null; + } + if (lMotor3ID != -1) { + driveLeftMotorC = new ChickenTalon(lMotor3ID); + driveLeftMotorC.set(ControlMode.Follower, driveLeftMotorA.getDeviceID()); + } else { + if (driveLeftMotorC != null) { + driveLeftMotorC.set(ControlMode.PercentOutput, 0); + } + driveLeftMotorC = null; + } + + if (rMotor1ID != -1) { + driveRightMotorA = new ChickenTalon(rMotor1ID); + } else { + System.err.println("Right Motor 1 must be set!"); + return; + } + if (rMotor2ID != -1) { + driveRightMotorB = new ChickenTalon(rMotor2ID); + driveRightMotorB.set(ControlMode.Follower, driveRightMotorA.getDeviceID()); + } else { + if (driveRightMotorB != null) { + driveRightMotorB.set(ControlMode.PercentOutput, 0); + } + driveRightMotorB = null; + } + if (rMotor3ID != -1) { + driveRightMotorC = new ChickenTalon(rMotor3ID); + driveRightMotorC.set(ControlMode.Follower, driveRightMotorA.getDeviceID()); + } else { + if (driveRightMotorC != null) { + driveRightMotorC.set(ControlMode.PercentOutput, 0); + } + driveRightMotorC = null; + } + for (ChickenTalon motor : new ChickenTalon[]{driveLeftMotorA, driveLeftMotorB, + driveLeftMotorC, driveRightMotorA, driveRightMotorB, + driveRightMotorC}) { + if (motor != null) { + motor.configClosedloopRamp(0); + motor.configOpenloopRamp(0); + motor.configPeakOutputForward(1); + motor.configPeakOutputReverse(-1); + motor.enableCurrentLimit(false); + motor.setBrake(brake); + } + } + }); + reset.setRunWhenDisabled(true); + reset.start(); + SmartDashboard.putData(reset); + + Command zero = new SimpleCommand("Zero", () -> { + if (driveLeftMotorA != null) { + driveLeftMotorA.setSelectedSensorPosition(0); + } + + if (driveRightMotorA != null) { + driveRightMotorA.setSelectedSensorPosition(0); + } + }); + zero.setRunWhenDisabled(true); + SmartDashboard.putData(zero); + } + + private static void putRegressionData(@NotNull SimpleRegression regression, String prefix) { + // getSlope, getIntercept, and getRSquare all have the same criteria for returning NaN + if (!Double.isNaN(regression.getSlope())) { + SmartDashboard.putNumber(prefix + " Calculated kV", regression.getSlope()); + SmartDashboard.putNumber(prefix + " Calculated vIntercept", regression.getIntercept()); + SmartDashboard.putNumber(prefix + " rSquared", regression.getRSquare()); + } else { + SmartDashboard.putNumber(prefix + " Calculated kV", 0); + SmartDashboard.putNumber(prefix + " Calculated vIntercept", 0); + SmartDashboard.putNumber(prefix + " rSquared", 0); + } + } + + @Override + public void robotPeriodic() { + putRegressionData(leftRegression, "Left"); + putRegressionData(rightRegression, "Right"); + Scheduler.getInstance().run(); + if (driveLeftMotorA != null) { + driveLeftMotorA.setSensorPhase(invertLeftSensor); + } + for (ChickenTalon talon : new ChickenTalon[]{driveLeftMotorA, driveLeftMotorB, + driveLeftMotorC}) { + if (talon != null) { + talon.setInverted(invertLeftMotor); + } + } + + if (driveRightMotorA != null) { + driveRightMotorA.setSensorPhase(invertRightSensor); + } + for (ChickenTalon talon : new ChickenTalon[]{driveRightMotorA, driveRightMotorB, + driveRightMotorC}) { + if (talon != null) { + talon.setInverted(invertRightMotor); + } + } + } + + @Override + public void teleopPeriodic() { + if (joystick.getRawButton(1)) { // if button A is pressed + if (!running) { + // reset everything + running = true; + leftRegression.clear(); + rightRegression.clear(); + appliedOutput = 0; + driveLeftMotorA.set(ControlMode.PercentOutput, 0); + driveRightMotorA.set(ControlMode.PercentOutput, 0); + } else { + double leftVelocity = (driveLeftMotorA.getSelectedSensorVelocity() * 10) / tpu; + if (leftVelocity != 0) { + leftRegression.addData(leftVelocity, driveLeftMotorA.getMotorOutputVoltage()); + } + + double rightVelocity = (driveRightMotorA.getSelectedSensorVelocity() * 10) / tpu; + if (rightVelocity != 0) { + rightRegression.addData(leftVelocity, driveRightMotorA.getMotorOutputVoltage()); + } + + appliedOutput += + (rampRate / SATURATION_VOLTAGE) * ((System.currentTimeMillis() - lastTime) / 1000.0); + lastTime = System.currentTimeMillis(); + driveLeftMotorA.set(ControlMode.PercentOutput, appliedOutput); + driveRightMotorA.set(ControlMode.PercentOutput, appliedOutput); + } + } else { + appliedOutput = 0; + driveLeftMotorA.set(ControlMode.PercentOutput, 0); + driveRightMotorA.set(ControlMode.PercentOutput, 0); + running = false; + } + lastTime = System.currentTimeMillis(); + } +} diff --git a/lib/src/main/java/org/team1540/rooster/util/robots/WheelbaseTestRobot.java b/lib/src/main/java/org/team1540/rooster/util/robots/WheelbaseTestRobot.java new file mode 100644 index 00000000..77e4d022 --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/util/robots/WheelbaseTestRobot.java @@ -0,0 +1,180 @@ +package org.team1540.rooster.util.robots; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.Scheduler; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import org.team1540.rooster.preferencemanager.Preference; +import org.team1540.rooster.preferencemanager.PreferenceManager; +import org.team1540.rooster.util.SimpleCommand; +import org.team1540.rooster.wrappers.ChickenTalon; + +/** + * Class to determine a robot's wheelbase width. For use instructions, load onto a robot and check + * the console. + */ +public class WheelbaseTestRobot extends IterativeRobot { + + @Preference(persistent = false) + public boolean logDataToCSV = false; + @Preference(persistent = false) + public int lMotor1ID = -1; + @Preference(persistent = false) + public int lMotor2ID = -1; + @Preference(persistent = false) + public int lMotor3ID = -1; + @Preference(persistent = false) + public int rMotor1ID = -1; + @Preference(persistent = false) + public int rMotor2ID = -1; + @Preference(persistent = false) + public int rMotor3ID = -1; + @Preference(persistent = false) + public boolean invertLeftMotor = false; + @Preference(persistent = false) + public boolean invertRightMotor = false; + @Preference(persistent = false) + public boolean invertLeftSensor = false; + @Preference(persistent = false) + public boolean invertRightSensor = false; + @Preference(persistent = false) + public boolean brake = false; + @Preference(persistent = false) + public double encoderTPU = 1; + @Preference(persistent = false) + public double setpoint = 0.5; + + private ChickenTalon lMotor1; + private ChickenTalon lMotor2; + private ChickenTalon lMotor3; + private ChickenTalon rMotor1; + private ChickenTalon rMotor2; + private ChickenTalon rMotor3; + + private Joystick joystick = new Joystick(0); + + @Override + public void robotInit() { + System.out.println("Initializing Wheelbase Test Robot"); + System.out.println( + "To change the motors to be used, change the preference values and then run the Reset command to " + + "allow the values to take effect. To disable a motor, set its motor ID to -1. Motor 1 will be " + + "configured as the master Talon and motors 2, 3, and 4 will be slaved to it in follower mode."); + + PreferenceManager.getInstance().add(this); + + Command reset = new SimpleCommand("Reset", () -> { + if (lMotor1ID != -1) { + lMotor1 = new ChickenTalon(lMotor1ID); + } else { + System.err.println("Left Motor 1 must be set!"); + return; + } + if (lMotor2ID != -1) { + lMotor2 = new ChickenTalon(lMotor2ID); + lMotor2.set(ControlMode.Follower, lMotor1.getDeviceID()); + } else { + if (lMotor2 != null) { + lMotor2.set(ControlMode.PercentOutput, 0); + } + lMotor2 = null; + } + if (lMotor3ID != -1) { + lMotor3 = new ChickenTalon(lMotor3ID); + lMotor3.set(ControlMode.Follower, lMotor1.getDeviceID()); + } else { + if (lMotor3 != null) { + lMotor3.set(ControlMode.PercentOutput, 0); + } + lMotor3 = null; + } + + if (rMotor1ID != -1) { + rMotor1 = new ChickenTalon(rMotor1ID); + } else { + System.err.println("Right Motor 1 must be set!"); + return; + } + if (rMotor2ID != -1) { + rMotor2 = new ChickenTalon(rMotor2ID); + rMotor2.set(ControlMode.Follower, rMotor1.getDeviceID()); + } else { + if (rMotor2 != null) { + rMotor2.set(ControlMode.PercentOutput, 0); + } + rMotor2 = null; + } + if (rMotor3ID != -1) { + rMotor3 = new ChickenTalon(rMotor3ID); + rMotor3.set(ControlMode.Follower, rMotor1.getDeviceID()); + } else { + if (rMotor3 != null) { + rMotor3.set(ControlMode.PercentOutput, 0); + } + rMotor3 = null; + } + for (ChickenTalon motor : new ChickenTalon[]{lMotor1, lMotor2, lMotor3, rMotor1, rMotor2, + rMotor3}) { + if (motor != null) { + motor.configClosedloopRamp(0); + motor.configOpenloopRamp(0); + motor.configPeakOutputForward(1); + motor.configPeakOutputReverse(-1); + motor.enableCurrentLimit(false); + motor.setBrake(brake); + } + } + }); + reset.setRunWhenDisabled(true); + reset.start(); + SmartDashboard.putData(reset); + + Command zero = new SimpleCommand("Zero", () -> { + if (lMotor1 != null) { + lMotor1.setSelectedSensorPosition(0); + } + + if (rMotor1 != null) { + rMotor1.setSelectedSensorPosition(0); + } + }); + zero.setRunWhenDisabled(true); + SmartDashboard.putData(zero); + } + + @Override + public void teleopInit() { + System.out.println("Zero encoders, then press A until the robot completes 10 revolutions"); + } + + @Override + public void teleopPeriodic() { + if (joystick.getRawButton(1)) { // button A + lMotor1.set(ControlMode.PercentOutput, setpoint); + rMotor1.set(ControlMode.PercentOutput, -setpoint); + } else { + lMotor1.set(ControlMode.PercentOutput, 0); + rMotor1.set(ControlMode.PercentOutput, 0); + } + } + + @Override + public void robotPeriodic() { + Scheduler.getInstance().run(); + if (lMotor1 != null && rMotor1 != null) { + SmartDashboard.putNumber("LPOS", lMotor1.getSelectedSensorPosition()); + SmartDashboard.putNumber("RPOS", rMotor1.getSelectedSensorPosition()); + + double leftDistance = lMotor1.getSelectedSensorPosition(); + double rightDistance = -rMotor1.getSelectedSensorPosition(); + + SmartDashboard.putNumber("Left Distance", leftDistance); + SmartDashboard.putNumber("Right Distance", rightDistance); + + SmartDashboard.putNumber("Calculated width (assuming 10 rots)", + (((leftDistance + rightDistance) / 2) / (10 * Math.PI)) / encoderTPU); + } + } +} From 4c7944da2e6018610d7b6603fb33081c7f43a380 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Wed, 28 Nov 2018 17:01:55 -0800 Subject: [PATCH 04/51] Revert "Remove TurningRateClosedLoopProcessor" This reverts commit 6df447e9 --- .../TurningRateClosedLoopProcessor.java | 107 ++++++++++++++++++ 1 file changed, 107 insertions(+) create mode 100644 lib/src/main/java/org/team1540/rooster/drive/pipeline/TurningRateClosedLoopProcessor.java diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/TurningRateClosedLoopProcessor.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/TurningRateClosedLoopProcessor.java new file mode 100644 index 00000000..7e3d37bc --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/TurningRateClosedLoopProcessor.java @@ -0,0 +1,107 @@ +package org.team1540.rooster.drive.pipeline; + +import java.util.OptionalDouble; +import java.util.function.DoubleSupplier; + +/** + * Modifies velocity setpoints to keep a desired turning rate. + */ +public class TurningRateClosedLoopProcessor implements Processor { + + private DoubleSupplier yawRateSupplier; + private double p, i, d; + + private double iAccum; + + private double lastError; + + private long lastTime = -1; + + private boolean invertSides; + + @Override + public TankDriveData apply(TankDriveData driveData) { + if (driveData.turningRate.isPresent()) { + // simple PID controller + + double actual = this.yawRateSupplier.getAsDouble(); + double target = driveData.turningRate.getAsDouble(); + + double output = 0; + double error = target - actual; + + // p gain + output += error * p; + + if (lastTime != -1) { + double dt = (System.currentTimeMillis() - lastTime) / 1000.0; + + // i gain + iAccum += error * dt; + output += i * iAccum; + + // d gain + double dError = (error - lastError) / dt; + output += d * dError; + } + + lastError = error; + lastTime = System.currentTimeMillis(); + + // multiplying the output by -1 effectively flips the sides + output *= invertSides ? -1 : 1; + + return new TankDriveData( + new DriveData( + driveData.left.position, + OptionalDouble.of(driveData.left.velocity.orElse(0) - output), + driveData.left.acceleration, + driveData.left.additionalFeedForward + ), + new DriveData( + driveData.right.position, + OptionalDouble.of(driveData.right.velocity.orElse(0) + output), + driveData.right.acceleration, + driveData.right.additionalFeedForward + ), + driveData.heading, + driveData.turningRate + ); + } else { + // we can't do anything here without a setpoint so just pass the boi through + return driveData; + } + } + + public TurningRateClosedLoopProcessor(DoubleSupplier yawRateSupplier, double p) { + this(yawRateSupplier, p, 0); + } + + public TurningRateClosedLoopProcessor(DoubleSupplier yawRateSupplier, double p, double i) { + this(yawRateSupplier, p, i, 0); + } + + public TurningRateClosedLoopProcessor(DoubleSupplier yawRateSupplier, double p, double i, + double d) { + this(yawRateSupplier, p, i, d, false); + } + + public TurningRateClosedLoopProcessor(DoubleSupplier yawRateSupplier, double p, + boolean invertSides) { + this(yawRateSupplier, p, 0, invertSides); + } + + public TurningRateClosedLoopProcessor(DoubleSupplier yawRateSupplier, double p, double i, + boolean invertSides) { + this(yawRateSupplier, p, i, 0, invertSides); + } + + public TurningRateClosedLoopProcessor(DoubleSupplier yawRateSupplier, double p, double i, + double d, boolean invertSides) { + this.yawRateSupplier = yawRateSupplier; + this.p = p; + this.i = i; + this.d = d; + this.invertSides = invertSides; + } +} From d8185fc962ab83d8dcad5205d4e2385cb053d2ce Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Wed, 28 Nov 2018 17:02:18 -0800 Subject: [PATCH 05/51] Revert "Remove motion-profile-related items" This reverts commit 6a43c7bf --- .../rooster/drive/pipeline/ProfileInput.java | 60 +++ .../motionprofiling/FollowProfile.java | 147 +++++++ .../motionprofiling/FollowProfileFactory.java | 411 ++++++++++++++++++ .../motionprofiling/MotionProfile.java | 149 +++++++ .../motionprofiling/MotionProfileUtils.java | 59 +++ .../MotionProfilingProperties.java | 4 + .../motionprofiling/ProfileFollower.java | 253 +++++++++++ .../motionprofiling/RunMotionProfiles.java | 2 + .../motionprofiling/SetpointConsumer.java | 30 ++ .../rooster/motionprofiling/package-info.java | 25 ++ .../rooster/testing/MotionProfileTesting.kt | 312 +++++++++++++ 11 files changed, 1452 insertions(+) create mode 100644 lib/src/main/java/org/team1540/rooster/drive/pipeline/ProfileInput.java create mode 100644 lib/src/main/java/org/team1540/rooster/motionprofiling/FollowProfile.java create mode 100644 lib/src/main/java/org/team1540/rooster/motionprofiling/FollowProfileFactory.java create mode 100644 lib/src/main/java/org/team1540/rooster/motionprofiling/MotionProfile.java create mode 100644 lib/src/main/java/org/team1540/rooster/motionprofiling/MotionProfileUtils.java create mode 100644 lib/src/main/java/org/team1540/rooster/motionprofiling/ProfileFollower.java create mode 100644 lib/src/main/java/org/team1540/rooster/motionprofiling/SetpointConsumer.java create mode 100644 lib/src/main/java/org/team1540/rooster/motionprofiling/package-info.java create mode 100644 test/src/main/kotlin/org/team1540/rooster/testing/MotionProfileTesting.kt diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/ProfileInput.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/ProfileInput.java new file mode 100644 index 00000000..49267798 --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/ProfileInput.java @@ -0,0 +1,60 @@ +package org.team1540.rooster.drive.pipeline; + +import edu.wpi.first.wpilibj.Timer; +import java.util.OptionalDouble; +import org.jetbrains.annotations.NotNull; +import org.team1540.rooster.motionprofiling.MotionProfile; +import org.team1540.rooster.motionprofiling.MotionProfile.Point; + +public class ProfileInput implements + org.team1540.rooster.drive.pipeline.Input { + + private MotionProfile left; + private MotionProfile right; + + private Timer timer = new Timer(); + + public ProfileInput(@NotNull MotionProfile left, @NotNull MotionProfile right) { + this.left = left; + this.right = right; + } + + @NotNull + private static Point getCurrentSegment(@NotNull MotionProfile trajectory, double currentTime) { + // Start from the current time and find the closest point. + int startIndex = Math.toIntExact(Math.round(currentTime / trajectory.get(0).dt)); + + int length = trajectory.size(); + int index = startIndex; + if (startIndex >= length - 1) { + index = length - 1; + } + return trajectory.get(index); + } + + @Override + public org.team1540.rooster.drive.pipeline.TankDriveData get() { + if (timer.get() <= 0) { + timer.start(); + } + + double timeValue = timer.get(); + + Point leftPoint = getCurrentSegment(left, timeValue); + Point rightPoint = getCurrentSegment(right, timeValue); + + return new org.team1540.rooster.drive.pipeline.TankDriveData( + new org.team1540.rooster.drive.pipeline.DriveData( + OptionalDouble.of(leftPoint.position), + OptionalDouble.of(leftPoint.velocity), + OptionalDouble.of(leftPoint.acceleration), + OptionalDouble.empty()), + new org.team1540.rooster.drive.pipeline.DriveData( + OptionalDouble.of(rightPoint.position), + OptionalDouble.of(rightPoint.velocity), + OptionalDouble.of(rightPoint.acceleration), + OptionalDouble.empty()), + OptionalDouble.of(leftPoint.heading), + OptionalDouble.empty()); + } +} diff --git a/lib/src/main/java/org/team1540/rooster/motionprofiling/FollowProfile.java b/lib/src/main/java/org/team1540/rooster/motionprofiling/FollowProfile.java new file mode 100644 index 00000000..642578c2 --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/motionprofiling/FollowProfile.java @@ -0,0 +1,147 @@ +package org.team1540.rooster.motionprofiling; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.command.Subsystem; +import java.util.function.DoubleSupplier; +import org.jetbrains.annotations.NotNull; +import org.team1540.rooster.motionprofiling.ProfileFollower.ProfileDriveSignal; +import org.team1540.rooster.util.AsyncCommand; + +/** + * {@link edu.wpi.first.wpilibj.command.Command Command} to execute a motion profile. This is an + * {@link AsyncCommand}-based wrapper around {@link ProfileFollower} which handles creating the + * instance, running it in a fast loop, and sending the output to the motors. + * + * @see FollowProfileFactory + * @see ProfileFollower + */ +public class FollowProfile extends AsyncCommand { + + @NotNull + private MotionProfile left; + @NotNull + private MotionProfile right; + + @NotNull + private SetpointConsumer leftSetpointConsumer; + @NotNull + private SetpointConsumer rightSetpointConsumer; + + @NotNull + private DoubleSupplier headingSupplier; + + private double lVelCoeff; + private double lVelIntercept; + private double lAccelCoeff; + private double rVelCoeff; + private double rVelIntercept; + private double rAccelCoeff; + private double headingP; + private double headingI; + + private double gyroIAccum; + + private ProfileFollower follower; + + /** + * Timer for the amount of time since profiling started (used to find current point) + */ + private Timer timer = new Timer(); + + /** + * Creates a {@code FollowProfile} command. + * + * @param subsystems The required subsystems for this command. + * @param leftSetpointConsumer The {@link SetpointConsumer} for the left-side motors. + * @param rightSetpointConsumer The {@link SetpointConsumer} for the right-side motors. + * @param headingSupplier A {@link DoubleSupplier} that returns the robot's current heading in + * radians from 0 to 2π. + * @param loopFreq The interval, in milliseconds, between profile loop execution. + * @param lVelCoeff The left velocity coefficient (kV), in bump units per profile unit per + * second. + * @param lVelIntercept The left velocity intercept (VIntercept), in bump units. + * @param lAccelCoeff The left acceleration coefficient (kA), in bump units per profile unit per + * second squared. + * @param rVelCoeff The right velocity coefficient (kV), in bump units per profile unit per + * second. + * @param rVelIntercept The right velocity intercept (VIntercept), in bump units. + * @param rAccelCoeff The right acceleration coefficient (kA), in bump units per profile unit per + * second squared. + * @param headingP The P coefficient for the heading controller, in profile units per radian. + * @param headingI The I coefficient for the heading controller, in profile units per + * radian-second. + */ + public FollowProfile(@NotNull MotionProfile left, @NotNull MotionProfile right, + @NotNull Subsystem[] subsystems, + @NotNull SetpointConsumer leftSetpointConsumer, + @NotNull SetpointConsumer rightSetpointConsumer, + @NotNull DoubleSupplier headingSupplier, + long loopFreq, double lVelCoeff, double lVelIntercept, double lAccelCoeff, double rVelCoeff, + double rVelIntercept, double rAccelCoeff, double headingP, double headingI) { + super(loopFreq); + for (Subsystem subsystem : subsystems) { + requires(subsystem); + } + this.left = left; + this.right = right; + this.leftSetpointConsumer = leftSetpointConsumer; + this.rightSetpointConsumer = rightSetpointConsumer; + this.headingSupplier = headingSupplier; + this.lVelCoeff = lVelCoeff; + this.lVelIntercept = lVelIntercept; + this.lAccelCoeff = lAccelCoeff; + this.rVelCoeff = rVelCoeff; + this.rVelIntercept = rVelIntercept; + this.rAccelCoeff = rAccelCoeff; + this.headingP = headingP; + this.headingI = headingI; + + follower = new ProfileFollower(left, right, lVelCoeff, lVelIntercept, lAccelCoeff, rVelCoeff, + rVelIntercept, rAccelCoeff, headingP, headingI); + } + + /** + * Get the time since starting the motion profile. + * + * @return The time, in seconds, since beginning to execute a motion profile, or 0 if not + * currently executing a profile. + */ + public double getExecutionTime() { + return isRunning() ? timer.get() : 0; + } + + /** + * Get the underlying {@link ProfileFollower} + * + * @return The underlying {@link ProfileFollower}. Any modifications to this object will affect + * this command. + */ + public ProfileFollower getFollower() { + return follower; + } + + @Override + protected void runPeriodic() { + // check finish status + if (follower.isProfileFinished(timer.get())) { + markAsFinished(); + } + + ProfileDriveSignal sig = follower.get(headingSupplier.getAsDouble(), timer.get()); + + leftSetpointConsumer.set(sig.leftSetpoint, sig.leftBump); + rightSetpointConsumer.set(sig.rightSetpoint, sig.rightBump); + } + + @Override + protected void runInitial() { + follower.reset(); + timer.start(); + } + + @Override + protected void runEnd() { + timer.stop(); + timer.reset(); + } +} diff --git a/lib/src/main/java/org/team1540/rooster/motionprofiling/FollowProfileFactory.java b/lib/src/main/java/org/team1540/rooster/motionprofiling/FollowProfileFactory.java new file mode 100644 index 00000000..164099c9 --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/motionprofiling/FollowProfileFactory.java @@ -0,0 +1,411 @@ +package org.team1540.rooster.motionprofiling; + +import edu.wpi.first.wpilibj.command.Subsystem; +import jaci.pathfinder.Pathfinder; +import jaci.pathfinder.Trajectory; +import java.io.File; +import java.util.function.DoubleSupplier; +import org.jetbrains.annotations.NotNull; + +/** + * Produces multiple similar {@link FollowProfile} commands for one subystem using common tuning + * values. In general, one {@code FollowProfileFactory} should be created for each subsystem to be + * motion-profiled. It can then be pre-loaded with common configuration and tuning values specific + * to a subsystem, and kept as a single instance to create any needed {@code FollowProfile} commands + * for that subsystem. + *

+ * All setters in this class follow a builder pattern; i.e they return an instance of the object + * they were called on. This allows for multiple set methods to be chained. + * + * @see FollowProfile + */ +public class FollowProfileFactory { + + @NotNull + private Subsystem[] subsystems; + @NotNull + private SetpointConsumer leftSetpointConsumer; + @NotNull + private SetpointConsumer rightSetpointConsumer; + + @NotNull + private DoubleSupplier headingSupplier = () -> 0; + + private long loopFreq = 20; + + private double lVelCoeff; + private double lVelIntercept; + private double lAccelCoeff; + private double rVelCoeff; + private double rVelIntercept; + private double rAccelCoeff; + private double headingP = 0; + private double headingI = 0; + + /** + * Constructs a {@code FollowProfileFactory} with the provided left and right set functions and + * subsystems. + * + * @param left The {@link SetpointConsumer} for the left-side motors. + * @param right The {@link SetpointConsumer} for the right-side motors. + * @param subsystems The {@link Subsystem Subystems} that produced {@link FollowProfile} commands + * should require. + */ + public FollowProfileFactory(@NotNull SetpointConsumer left, @NotNull SetpointConsumer right, + @NotNull Subsystem... subsystems) { + this.leftSetpointConsumer = left; + this.rightSetpointConsumer = right; + this.subsystems = subsystems; + } + + /** + * Gets the subsystems used by produced {@link FollowProfile} commands. + * + * @return The required subsystems. Each of these will be declared as a requirement for each + * produced command using {@code requires()}. + */ + @NotNull + public Subsystem[] getSubsystems() { + return subsystems; + } + + /** + * Sets the subsystems required by produced {@link FollowProfile} commands. + * + * @param subsystems The required subsystems. Each of these will be declared as a requirement for + * each produced command using {@code requires()}. + * @return This {@code FollowProfileFactory} in a builder pattern. + */ + @NotNull + public FollowProfileFactory setSubsystems(@NotNull Subsystem... subsystems) { + this.subsystems = subsystems; + return this; + } + + /** + * Gets the {@link SetpointConsumer} for the left-side motors. + * + * @return The currently used {@link SetpointConsumer} for the left side. + */ + @NotNull + public SetpointConsumer getLeftSetpointConsumer() { + return leftSetpointConsumer; + } + + /** + * Sets the {@link SetpointConsumer} for the left-side motors. + * + * @param leftSetpointConsumer A {@link SetpointConsumer} that takes a setpoint in profile units + * and bump in bump units. + */ + @NotNull + public FollowProfileFactory setLeftSetpointConsumer( + @NotNull SetpointConsumer leftSetpointConsumer) { + this.leftSetpointConsumer = leftSetpointConsumer; + return this; + } + + /** + * Gets the {@link SetpointConsumer} for the left-side motors. + * + * @return The currently used {@link SetpointConsumer} for the right side. + */ + @NotNull + public SetpointConsumer getRightSetpointConsumer() { + return rightSetpointConsumer; + } + + /** + * @param rightSetpointConsumer A {@link SetpointConsumer} that takes a setpoint in profile units + * and bump in bump units and passes them to the drivetrain. + * @return This {@code FollowProfileFactory} in a builder pattern. + */ + @NotNull + public FollowProfileFactory setRightSetpointConsumer( + @NotNull SetpointConsumer rightSetpointConsumer) { + this.rightSetpointConsumer = rightSetpointConsumer; + return this; + } + + /** + * Gets the {@link DoubleSupplier} used for the input of the heading loop. + * + * @return The heading supplier. + */ + @NotNull + public DoubleSupplier getHeadingSupplier() { + return headingSupplier; + } + + /** + * Sets the {@link DoubleSupplier} used for the input of the heading loop. + * + * @param headingSupplier A {@link DoubleSupplier} that returns the robot's heading in radians + * from 0 to 2π. + * @return This {@code FollowProfileFactory} in a builder pattern. + */ + @NotNull + public FollowProfileFactory setHeadingSupplier(@NotNull DoubleSupplier headingSupplier) { + this.headingSupplier = headingSupplier; + return this; + } + + /** + * Gets the delay between profile loop execution. Defaults to 20ms. + * + * @return The time between profile loop execution, in milliseconds. + */ + public long getLoopFreq() { + return loopFreq; + } + + /** + * Sets the delay between profile loop execution. Defaults to 20ms. + * + * @param loopFreq The time between loop executions, in milliseconds. + * @return This {@code FollowProfileFactory} in a builder pattern. + */ + @NotNull + public FollowProfileFactory setLoopFreq(long loopFreq) { + this.loopFreq = loopFreq; + return this; + } + + /** + * Gets the left velocity feed-forward coefficient. This is equivalent to the kV term in drive + * characterization. Defaults to 0 (no feed-forward). + * + * @return The velocity feed-forward coefficient, in bump units per profile unit per second. + */ + public double getLVelCoeff() { + return lVelCoeff; + } + + /** + * Sets the left velocity feed-forward coefficent. This is equivalent to the kV term in drive * + * characterization. Defaults to 0 (no feed-forward). + * + * @param velCoeff The velocity feed-forward coefficient, in bump units per profile unit per + * second. + * @return This {@code FollowProfileFactory} in a builder pattern. + */ + @NotNull + public FollowProfileFactory setLVelCoeff(double velCoeff) { + this.lVelCoeff = velCoeff; + return this; + } + + /** + * Gets the left velocity intercept, or VIntercept. (See Eli Barnett's drive characterization + * paper for an explanation of why this is needed.) + * + * @return The velocity intercept, in bump units. + */ + public double getLVelIntercept() { + return lVelIntercept; + } + + /** + * Sets the left velocity intercept, or VIntercept. (See Eli Barnett's drive characterization + * paper for an explanation of why this is needed.) Defaults to 0. + * + * @param velIntercept The velocity intercept, in bump units. + * @return This {@code FollowProfileFactory} in a builder pattern. + */ + @NotNull + public FollowProfileFactory setLVelIntercept(double velIntercept) { + this.lVelIntercept = velIntercept; + return this; + } + + /** + * Gets the left acceleration feed-forward. This is equivalent to the kA term in drive + * characterization. Defaults to 0 (no feed-forward). + * + * @return The currently set acceleration coefficient, in bump units per profile unit per second + * squared. + */ + public double getLAccelCoeff() { + return lAccelCoeff; + } + + /** + * Sets the acceleration feed-forward. This is equivalent to the kA term in drive + * characterization. Defaults to 0 (no feed-forward). + * + * @param accelCoeff The acceleration coefficient, in bump units per profile unit per second + * squared. + * @return This {@code FollowProfileFactory} in a builder pattern. + */ + @NotNull + public FollowProfileFactory setLAccelCoeff(double accelCoeff) { + this.lAccelCoeff = accelCoeff; + return this; + } + + /** + * Gets the right velocity feed-forward coefficient. This is equivalent to the kV term in drive + * characterization. Defaults to 0 (no feed-forward). + * + * @return The velocity feed-forward coefficient, in bump units per profile unit per second. + */ + public double getRVelCoeff() { + return rVelCoeff; + } + + /** + * Sets the right velocity feed-forward coefficent. This is equivalent to the kV term in drive * + * characterization. Defaults to 0 (no feed-forward). + * + * @param velCoeff The velocity feed-forward coefficient, in bump units per profile unit per + * second. + * @return This {@code FollowProfileFactory} in a builder pattern. + */ + @NotNull + public FollowProfileFactory setRVelCoeff(double velCoeff) { + this.rVelCoeff = velCoeff; + return this; + } + + /** + * Gets the right velocity intercept, or VIntercept. (See Eli Barnett's drive characterization + * paper for an explanation of why this is needed.) + * + * @return The velocity intercept, in bump units. + */ + public double getRVelIntercept() { + return rVelIntercept; + } + + /** + * Sets the right velocity intercept, or VIntercept. (See Eli Barnett's drive characterization + * paper for an explanation of why this is needed.) Defaults to 0. + * + * @param velIntercept The velocity intercept, in bump units. + * @return This {@code FollowProfileFactory} in a builder pattern. + */ + @NotNull + public FollowProfileFactory setRVelIntercept(double velIntercept) { + this.rVelIntercept = velIntercept; + return this; + } + + /** + * Gets the right acceleration feed-forward. This is equivalent to the kA term in drive + * characterization. Defaults to 0 (no feed-forward). + * + * @return The currently set acceleration coefficient, in bump units per profile unit per second + * squared. + */ + public double getRAccelCoeff() { + return rAccelCoeff; + } + + /** + * Sets the acceleration feed-forward. This is equivalent to the kA term in drive + * characterization. Defaults to 0 (no feed-forward). + * + * @param accelCoeff The acceleration coefficient, in bump units per profile unit per second + * squared. + * @return This {@code FollowProfileFactory} in a builder pattern. + */ + @NotNull + public FollowProfileFactory setRAccelCoeff(double accelCoeff) { + this.rAccelCoeff = accelCoeff; + return this; + } + + /** + * Gets the P coefficient for the heading PI loop. Defaults to 0. + * + * @return The currently set P coefficient, in profile units per radian. + */ + public double getHeadingP() { + return headingP; + } + + /** + * Sets the P coefficient for the heading PI loop. Defaults to 0. + * + * @param headingP The P coefficient, in profile units per radian. + * @return This {@code FollowProfileFactory} in a builder pattern. + */ + @NotNull + public FollowProfileFactory setHeadingP(double headingP) { + this.headingP = headingP; + return this; + } + + + /** + * Gets the I coefficient for the heading PI loop. Defaults to 0. + * + * @return The currently set I coefficient, in profile units per radian-second. + */ + public double getHeadingI() { + return headingI; + } + + /** + * Sets the I coefficient for the heading PI loop. Defaults to 0. + * + * @param headingI The I coefficient, in profile units per radian-second. + * @return This {@code FollowProfileFactory} in a builder pattern. + */ + @NotNull + public FollowProfileFactory setHeadingI(double headingI) { + this.headingI = headingI; + return this; + } + + /** + * Creates a new {@link FollowProfile} command to follow the provided profile. This is a + * convienience method that calls {@link #create(File, File)} with two files as described below. + *

+ * With a given profile name, this method will attempt to load a profile with name "profile" from + * two files; the left side from /home/lvuser/profiles/profile_left.csv and the right side from + * /home/lvuser/profiles/profile_right.csv. + * + * @param profileName The name of the profile. + * @return A new {@link FollowProfile} command with the previously configured settings following + * the provided profile. + */ + @NotNull + public FollowProfile create(@NotNull String profileName) { + return create(new File("/home/lvuser/profiles/" + profileName + "_left.csv"), + new File("/home/lvuser/profiles/" + profileName + "_right.csv")); + } + + /** + * Creates a new {@link FollowProfile} command to follow the provided profile. This is a + * convinience method that calls {@link #create(MotionProfile, MotionProfile)} after loading the + * trajectories using {@link Pathfinder#readFromCSV(File)} and converting them using {@link + * MotionProfileUtils#createProfile(Trajectory)}. + * + * @param leftFile The file to load the left profile from; should be a pathfinder-formatted CSV. + * @param rightFile The file to load the right profile from; should be a pathfinder-formatted + * CSV. + * @return A new {@link FollowProfile} command with the previously configured settings following * + * the provided profile. + */ + @NotNull + public FollowProfile create(@NotNull File leftFile, @NotNull File rightFile) { + return create(MotionProfileUtils.createProfile(Pathfinder.readFromCSV(leftFile)), + MotionProfileUtils.createProfile(Pathfinder.readFromCSV(rightFile))); + } + + /** + * Creates a new {@link FollowProfile} command to follow the provided profile. + * + * @param left The profile for the left side. + * @param right The profile for the right side. + * @return A new {@link FollowProfile} command with the previously configured settings following + * the provided profile. + */ + @NotNull + public FollowProfile create(@NotNull MotionProfile left, @NotNull MotionProfile right) { + return new FollowProfile(left, right, subsystems, leftSetpointConsumer, rightSetpointConsumer, + headingSupplier, loopFreq, lVelCoeff, lVelIntercept, lAccelCoeff, rVelCoeff, + rVelIntercept, rAccelCoeff, headingP, headingI); + } +} diff --git a/lib/src/main/java/org/team1540/rooster/motionprofiling/MotionProfile.java b/lib/src/main/java/org/team1540/rooster/motionprofiling/MotionProfile.java new file mode 100644 index 00000000..04c4531d --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/motionprofiling/MotionProfile.java @@ -0,0 +1,149 @@ +package org.team1540.rooster.motionprofiling; + +import java.util.Objects; +import org.jetbrains.annotations.Contract; +import org.jetbrains.annotations.NotNull; +import org.jetbrains.annotations.Nullable; + +/** + * A sequence of {@link Point Points} that can be executed by a {@link FollowProfile}. + */ +public class MotionProfile { + + /** + * The {@link Point Points} in the motion profile. + */ + @NotNull + public final Point[] points; + + /** + * Create a new {@link MotionProfile} from an array of points. + * + * @param points The points to use. + */ + public MotionProfile(@NotNull Point[] points) { + this.points = points; + } + + /** + * Gets the nth {@link Point} (0-indexed) in the motion profile. + * + * @param index The index of the point to get. + * @return The point at the specified index. + * @throws ArrayIndexOutOfBoundsException if {@code index} ≥ {@link #size()}. + */ + @Contract(pure = true) + public Point get(int index) { + return points[index]; + } + + /** + * Get the number of {@link Point points} in the profile. + * + * @return The number of points in the profile; specifically, {@code points.length}. + */ + @Contract(pure = true) + public int size() { + return points.length; + } + + /** + * A single instant within a {@link MotionProfile}. + */ + public static class Point { + + /** + * The time change since the previous point, in seconds. + */ + public double dt; + /** + * The x-position of the robot in {@linkplain org.team1540.rooster.motionprofiling profile + * units}, or 0 if not applicable. + */ + public double x; + /** + * The y-position of the robot in {@linkplain org.team1540.rooster.motionprofiling profile + * units}, or 0 if not applicable. + */ + public double y; + /** + * The position of the profiled mechanism, in {@linkplain org.team1540.rooster.motionprofiling + * profile units}. + */ + public double position; + /** + * The velocity of the profiled mechanism, in {@linkplain org.team1540.rooster.motionprofiling + * profile units} per second. + */ + public double velocity; + /** + * The acceleration of the profiled mechanism, in {@linkplain org.team1540.rooster.motionprofiling + * profile units} per second squared. + */ + public double acceleration; + /** + * The jerk of the profiled mechanism, in {@linkplain org.team1540.rooster.motionprofiling + * profile units} per second cubed. + */ + public double jerk; + /** + * The robot's heading in radians, or 0 if not applicable. + */ + public double heading; + + /** + * Creates a new {@code Point}. + * + * @param dt The time change since the previous point, in seconds. + * @param x The x-position of the robot in {@linkplain org.team1540.rooster.motionprofiling + * profile units}, or 0 if not applicable. + * @param y The y-position of the robot in {@linkplain org.team1540.rooster.motionprofiling + * profile units}, or 0 if not applicable. + * @param position The position of the profiled mechanism, in {@linkplain + * org.team1540.rooster.motionprofiling profile units}. + * @param velocity The velocity of the profiled mechanism, in {@linkplain + * org.team1540.rooster.motionprofiling profile units} per second. + * @param acceleration The acceleration of the profiled mechanism, in {@linkplain + * org.team1540.rooster.motionprofiling profile units} per second squared. + * @param jerk The jerk of the profiled mechanism, in {@linkplain org.team1540.rooster.motionprofiling + * profile units} per second cubed. + * @param heading The robot's heading in radians, or 0 if not applicable. + */ + public Point(double dt, double x, double y, double position, double velocity, + double acceleration, double jerk, double heading) { + this.dt = dt; + this.x = x; + this.y = y; + this.position = position; + this.velocity = velocity; + this.acceleration = acceleration; + this.jerk = jerk; + this.heading = heading; + } + + @Contract(value = "null -> false", pure = true) + @Override + public boolean equals(@Nullable Object o) { + if (this == o) { + return true; + } + if (!(o instanceof Point)) { + return false; + } + Point point = (Point) o; + return Double.compare(point.dt, dt) == 0 && + Double.compare(point.x, x) == 0 && + Double.compare(point.y, y) == 0 && + Double.compare(point.position, position) == 0 && + Double.compare(point.velocity, velocity) == 0 && + Double.compare(point.acceleration, acceleration) == 0 && + Double.compare(point.jerk, jerk) == 0 && + Double.compare(point.heading, heading) == 0; + } + + @Override + public int hashCode() { + return Objects.hash(dt, x, y, position, velocity, acceleration, jerk, heading); + } + } +} diff --git a/lib/src/main/java/org/team1540/rooster/motionprofiling/MotionProfileUtils.java b/lib/src/main/java/org/team1540/rooster/motionprofiling/MotionProfileUtils.java new file mode 100644 index 00000000..b2bcd7f5 --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/motionprofiling/MotionProfileUtils.java @@ -0,0 +1,59 @@ +package org.team1540.rooster.motionprofiling; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.DemandType; +import com.ctre.phoenix.motorcontrol.can.BaseMotorController; +import jaci.pathfinder.Trajectory; +import java.util.Arrays; +import org.jetbrains.annotations.Contract; +import org.jetbrains.annotations.NotNull; +import org.team1540.rooster.motionprofiling.MotionProfile.Point; + +public class MotionProfileUtils { + + private MotionProfileUtils() { + } + + /** + * Create a {@link SetpointConsumer} that sets the setpoint of the provided CTRE motor controller + * with no adjustment to the position setpoint. + * + * @param motorController The motor controller. + * @return A {@code SetpointConsumer} to use for motion profiling. + */ + public static SetpointConsumer createSetpointConsumer(BaseMotorController motorController) { + return createSetpointConsumer(motorController, 1); + } + + /** + * Create a {@link SetpointConsumer} that sets the setpoint of the provided CTRE motor controller, + * multiplying the position setpoint by the adjustmant. + * + * @param motorController The motor controller. + * @return A {@code SetpointConsumer} to use for motion profiling. + */ + public static SetpointConsumer createSetpointConsumer(BaseMotorController motorController, + double adjustment) { + return (setpoint, bump) -> + motorController + .set(ControlMode.Position, setpoint * adjustment, DemandType.ArbitraryFeedForward, + bump); + } + + /** + * Creates a ROOSTER {@link MotionProfile} from a Pathfinder {@link Trajectory}. + * + * @param trajectory The {@link Trajectory} to convert. + * @return A {@link MotionProfile} containing the same points. Profile points are copied over, so + * subsequent changes to the {@link Trajectory} will not affect the produced {@link + * MotionProfile}. + */ + @Contract("_ -> new") + @NotNull + public static MotionProfile createProfile(@NotNull Trajectory trajectory) { + return new MotionProfile(Arrays.stream(trajectory.segments).map( + s -> new Point(s.dt, s.x, s.y, s.position, s.velocity, s.acceleration, s.jerk, s.heading)) + .toArray(Point[]::new)); + } + +} diff --git a/lib/src/main/java/org/team1540/rooster/motionprofiling/MotionProfilingProperties.java b/lib/src/main/java/org/team1540/rooster/motionprofiling/MotionProfilingProperties.java index deecf25e..019d99e2 100644 --- a/lib/src/main/java/org/team1540/rooster/motionprofiling/MotionProfilingProperties.java +++ b/lib/src/main/java/org/team1540/rooster/motionprofiling/MotionProfilingProperties.java @@ -4,6 +4,10 @@ import java.util.function.DoubleConsumer; import java.util.function.DoubleSupplier; +/** + * @deprecated Replaced by the {@link FollowProfile}-based system. + */ +@Deprecated public class MotionProfilingProperties { private double encoderTicksPerUnit = 1023 * 0.1 * Math.PI; diff --git a/lib/src/main/java/org/team1540/rooster/motionprofiling/ProfileFollower.java b/lib/src/main/java/org/team1540/rooster/motionprofiling/ProfileFollower.java new file mode 100644 index 00000000..dbe663d7 --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/motionprofiling/ProfileFollower.java @@ -0,0 +1,253 @@ +package org.team1540.rooster.motionprofiling; + +import static java.lang.Math.atan2; +import static java.lang.Math.cos; +import static java.lang.Math.sin; + +import org.jetbrains.annotations.NotNull; +import org.team1540.rooster.motionprofiling.MotionProfile.Point; + +/** + * Helper to execute motion profiles. This class has no dependency on WPILib/any external library. + * + * The profile-following algorithm used here is a derivative of the algorithm used by Team 2471. The + * output of a heading-based PI loop is added to the profile's position setpoint and passed to the + * motor controllers, with additional velocity, acceleration, and static friction feed-forwards (in + * line with Eli Barnett's drive + * characterization method) added via throttle bump. + *

+ * It is designed to use native Talon SRX position PID control with a throttle bump, but the output + * could instead be used to control a RIO-side PID loop. + *

+ * This class is stateful; it keeps track of the last time {@link #get(double, double)} get()} was + * called, and also has an integral accumulator for the gyro PI controller. With that in mind, if + * using a {@code FollowProfile} instance multiple times, call {@link #reset()} before beginning to + * execute the second, third, etc. profiles. + */ +public class ProfileFollower { + + @NotNull + private MotionProfile left; + @NotNull + private MotionProfile right; + + private double lVelCoeff; + private double lVelIntercept; + private double lAccelCoeff; + private double rVelCoeff; + private double rVelIntercept; + private double rAccelCoeff; + private double headingP; + private double headingI; + + private double gyroIAccum; + + private double profTime; + + private double lastTime = -1; + + /** + * Creates a {@code ProfileFollower}. + * + * For an explanation of units, see the {@linkplain org.team1540.rooster.motionprofiling package + * docs}. + * + * @param lVelCoeff The left velocity coefficient (kV), in bump units per profile unit per + * second. + * @param lVelIntercept The left velocity intercept (VIntercept), in bump units. + * @param lAccelCoeff The left acceleration coefficient (kA), in bump units per profile unit per + * second squared. + * @param rVelCoeff The right velocity coefficient (kV), in bump units per profile unit per + * second. + * @param rVelIntercept The right velocity intercept (VIntercept), in bump units. + * @param rAccelCoeff The right acceleration coefficient (kA), in bump units per profile unit per + * second squared. + * @param headingP The P coefficient for the heading controller, in profile units per radian. + * @param headingI The I coefficient for the heading controller, in profile units per + * radian-second. + */ + public ProfileFollower( + @NotNull MotionProfile left, + @NotNull MotionProfile right, double lVelCoeff, double lVelIntercept, double lAccelCoeff, + double rVelCoeff, double rVelIntercept, double rAccelCoeff, double headingP, + double headingI) { + this.left = left; + this.right = right; + this.lVelCoeff = lVelCoeff; + this.lVelIntercept = lVelIntercept; + this.lAccelCoeff = lAccelCoeff; + this.rVelCoeff = rVelCoeff; + this.rVelIntercept = rVelIntercept; + this.rAccelCoeff = rAccelCoeff; + this.headingP = headingP; + this.headingI = headingI; + } + + /** + * Get the output from the motion profile at a given time (usually the current time). + * + * @param heading The current gyro heading, from 0 to 2π inclusive. + * @param time The time, in seconds, since motion profile execution began. This is used to find + * the segment to execute. + * @return A {@link ProfileDriveSignal} describing the necessary drive commands. + */ + @NotNull + public ProfileDriveSignal get(double heading, double time) { + Point leftSegment = getCurrentSegment(left, time); + Point rightSegment = getCurrentSegment(right, time); + + double headingTarget = leftSegment.heading; + + // basically magic https://stackoverflow.com/a/2007279 + double headingError = atan2(sin(heading - headingTarget), cos(heading - headingTarget)); + + double timeSinceLast = lastTime == -1 ? 0 : time - lastTime; + gyroIAccum += headingError * timeSinceLast; + + lastTime = time; + + double gyroPOut = headingError * headingP; + double gyroIOut = gyroIAccum * headingI; + + double leftVelFOut = lVelCoeff * leftSegment.velocity; + double rightVelFOut = rVelCoeff * rightSegment.velocity; + + double leftVelInterceptOut = + leftSegment.velocity == 0 ? 0 : Math.copySign(lVelIntercept, leftSegment.velocity); + double rightVelInterceptOut = + rightSegment.velocity == 0 ? 0 : Math.copySign(rVelIntercept, rightSegment.velocity); + + double leftAccelFOut = lAccelCoeff * leftSegment.acceleration; + double rightAccelFOut = rAccelCoeff * rightSegment.acceleration; + + return new ProfileDriveSignal( + leftSegment.position - gyroPOut - gyroIOut, + leftVelFOut + leftVelInterceptOut + leftAccelFOut, + rightSegment.position + gyroPOut + gyroIOut, + rightVelFOut + rightVelInterceptOut + rightAccelFOut + ); + } + + /** + * Reset the profile follower. + * + * This resets the follower so that it can be used multiple times. + */ + public void reset() { + gyroIAccum = 0; + lastTime = -1; + } + + /** + * Get the current integral accumulator for the gyro PI controller. + * + * @return The integral accumulator. + */ + public double getGyroIAccum() { + return gyroIAccum; + } + + + /** + * Get the current gyro error being fed into the PI controller. + * + * @param heading The current gyro heading reading, in radians from 0 to 2π. + * @param time The current time, in seconds. + * @return The heading error, in radians. + */ + public double getGyroError(double heading, double time) { + double tgtHeading = getCurrentPointLeft(time).heading; + + return atan2(sin(heading - tgtHeading), cos(heading - tgtHeading)); + } + + /** + * Get the currently executing {@code Point} on the left side. + * + * @param time The current time, in seconds. + * @return The current {@code Point}. + */ + @NotNull + public Point getCurrentPointLeft(double time) { + return getCurrentSegment(left, time); + } + + + /** + * Get the currently executing {@code Point} on the right side. + * + * @param time The current time, in seconds. + * @return The current {@code Point}. + */ + @NotNull + public Point getCurrentPointRight(double time) { + return getCurrentSegment(right, time); + } + + /** + * Gets the total time to execute the profile. + * + * @return The time to execute the currently loaded profile, in seconds. + */ + public double getProfileTime() { + return profTime; + } + + /** + * Get whether the profile is finished (i.e. time > {@link #getProfileTime()}} + * + * @param time The current time since the profile began executing, in seconds. + * @return {@code true} if the profile is finished; {@code false} otherwise. + */ + public boolean isProfileFinished(double time) { + return time > profTime; + } + + /** + * A signal to be sent to the robot drive, consisting of left and right position setpoints and + * feed-forward bumps. + */ + public static class ProfileDriveSignal { + + /** + * The left-side position setpoint, in {@linkplain org.team1540.rooster.motionprofiling profile + * units}. + */ + public final double leftSetpoint; + /** + * The left-side feed-forward throttle bump, in fractions of motor throttle (i.e. 0.5 == 50% of + * max motor throttle). + */ + public final double leftBump; + /** + * The right-side position setpoint, in {@linkplain org.team1540.rooster.motionprofiling profile + * units}. + */ + public final double rightSetpoint; + /** + * The right-side feed-forward throttle bump, in fractions of motor throttle (i.e. 0.5 == 50% of + * max motor throttle). + */ + public final double rightBump; + + public ProfileDriveSignal(double lSetpoint, double lBump, double rSetpoint, double rBump) { + this.leftSetpoint = lSetpoint; + this.leftBump = lBump; + this.rightSetpoint = rSetpoint; + this.rightBump = rBump; + } + } + + @NotNull + private static Point getCurrentSegment(@NotNull MotionProfile trajectory, double currentTime) { + // Start from the current time and find the closest point. + int startIndex = Math.toIntExact(Math.round(currentTime / trajectory.get(0).dt)); + + int length = trajectory.size(); + int index = startIndex; + if (startIndex >= length - 1) { + index = length - 1; + } + return trajectory.get(index); + } +} diff --git a/lib/src/main/java/org/team1540/rooster/motionprofiling/RunMotionProfiles.java b/lib/src/main/java/org/team1540/rooster/motionprofiling/RunMotionProfiles.java index f53a4c26..76548332 100644 --- a/lib/src/main/java/org/team1540/rooster/motionprofiling/RunMotionProfiles.java +++ b/lib/src/main/java/org/team1540/rooster/motionprofiling/RunMotionProfiles.java @@ -9,7 +9,9 @@ /** * Executes a set of motion profiles (with respective properties.) + * @deprecated Replaced by the {@link FollowProfile}-based system. */ +@Deprecated public class RunMotionProfiles extends Command { private int slotId = 0; diff --git a/lib/src/main/java/org/team1540/rooster/motionprofiling/SetpointConsumer.java b/lib/src/main/java/org/team1540/rooster/motionprofiling/SetpointConsumer.java new file mode 100644 index 00000000..ba2c51a0 --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/motionprofiling/SetpointConsumer.java @@ -0,0 +1,30 @@ +package org.team1540.rooster.motionprofiling; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.DemandType; + +/** + * Functional interface to pass commanded values from {@link FollowProfile} to the motors. + * + * @see FollowProfile + */ +@FunctionalInterface +public interface SetpointConsumer { + + /** + * Sets the setpoint of the motors to be profiled. Usually implemented using the {@link + * com.ctre.phoenix.motorcontrol.can.TalonSRX#set(ControlMode, double, DemandType, double) + * set(ControlMode, double, DemandType, double)} method of CTRE's {@link + * com.ctre.phoenix.motorcontrol.can.TalonSRX TalonSRX}, with the {@code ControlMode} set to + * {@link ControlMode#Position} and the {@code DemandType} set to {@link + * DemandType#ArbitraryFeedForward}. + *

+ * See the {@linkplain org.team1540.rooster.motionprofiling package documentation} for an + * explanation of profile units, bump units, etc.. + * + * @param setpoint The position PID setpoint, in profile units. (Unit conversion to Talon SRX + * native units etc. should be performed within this method.) + * @param bump The throttle bump to apply, in bump units. + */ + void set(double setpoint, double bump); +} diff --git a/lib/src/main/java/org/team1540/rooster/motionprofiling/package-info.java b/lib/src/main/java/org/team1540/rooster/motionprofiling/package-info.java new file mode 100644 index 00000000..4890fd89 --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/motionprofiling/package-info.java @@ -0,0 +1,25 @@ +/** + * Utilities for executing motion profiles. + * + *

Summary

+ * + * This package's main business logic is in the {@link org.team1540.rooster.motionprofiling.FollowProfile} + * command, which follows a given Pathfinder motion profile. (A technical explanation of the + * profile-following algorithm can be found in the {@link org.team1540.rooster.motionprofiling.FollowProfile} + * documentation.) Additionally, this package contains {@link org.team1540.rooster.motionprofiling.FollowProfileFactory}, + * a class to create multiple {@code FollowProfile} instances using common configuration values (for + * example, multiple autonomous routines for a drivetrain). + * + *

A Note on Units

+ * Many problems relating to motion profiling stem from units not being converted properly. As such, + * to simplify things, this package is unit-agnostic (with the exception of heading which is + * required to be in radians) and does no unit conversion on its own, instead asking that provided + * coefficients incorporate necessary unit conversion. When used in package documentation, the term + * profile units means the units used in motion profiles given to the {@link + * org.team1540.rooster.motionprofiling.FollowProfile} command (usually inches, feet or meters); the + * term native units means the units passed to the motor controllers (usually in terms of + * raw encoder counts); and the term bump units means the units passed to the throttle-bump + * parameter of {@link org.team1540.rooster.motionprofiling.SetpointConsumer#set(double, double) + * SetpointConsumer.set()} (usually percentage of motor throttle). + */ +package org.team1540.rooster.motionprofiling; diff --git a/test/src/main/kotlin/org/team1540/rooster/testing/MotionProfileTesting.kt b/test/src/main/kotlin/org/team1540/rooster/testing/MotionProfileTesting.kt new file mode 100644 index 00000000..124ae6ac --- /dev/null +++ b/test/src/main/kotlin/org/team1540/rooster/testing/MotionProfileTesting.kt @@ -0,0 +1,312 @@ +package org.team1540.rooster.testing + +import com.ctre.phoenix.motorcontrol.ControlMode +import com.ctre.phoenix.motorcontrol.FeedbackDevice +import com.kauailabs.navx.frc.AHRS +import edu.wpi.first.wpilibj.IterativeRobot +import edu.wpi.first.wpilibj.Joystick +import edu.wpi.first.wpilibj.SPI +import edu.wpi.first.wpilibj.command.Scheduler +import edu.wpi.first.wpilibj.command.Subsystem +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard +import jaci.pathfinder.Pathfinder +import jaci.pathfinder.Trajectory +import jaci.pathfinder.Waypoint +import jaci.pathfinder.modifiers.TankModifier +import org.team1540.rooster.Utilities +import org.team1540.rooster.motionprofiling.FollowProfile +import org.team1540.rooster.motionprofiling.FollowProfileFactory +import org.team1540.rooster.motionprofiling.MotionProfileUtils +import org.team1540.rooster.preferencemanager.Preference +import org.team1540.rooster.preferencemanager.PreferenceManager +import org.team1540.rooster.util.Executable +import org.team1540.rooster.util.SimpleLoopCommand +import org.team1540.rooster.wrappers.ChickenTalon +import java.util.function.DoubleSupplier +import kotlin.math.PI + +private val driver = Joystick(0) + +class MotionProfileTestingRobot : IterativeRobot() { + private lateinit var factory: FollowProfileFactory + + private val navx = AHRS(SPI.Port.kMXP) + + private var command: FollowProfile? = null + + @JvmField + @Preference("lkV", persistent = false) + var lKV = 0.0 + + @JvmField + @Preference("lkA", persistent = false) + var lKA = 0.0 + + @JvmField + @Preference("lVIntercept", persistent = false) + var lVIntercept = 0.0 + + @JvmField + @Preference("rkV", persistent = false) + var rKV = 0.0 + + @JvmField + @Preference("rkA", persistent = false) + var rKA = 0.0 + + @JvmField + @Preference("rVIntercept", persistent = false) + var rVIntercept = 0.0 + + @JvmField + @Preference("MP Loop Freq", persistent = false) + var loopFreqMs = 0 + + @JvmField + @Preference("MP Heading P", persistent = false) + var hdgP = 0.0 + + @JvmField + @Preference("MP Heading I", persistent = false) + var hdgI = 0.0 + + @JvmField + @Preference("MP Drive P", persistent = false) + var driveP = 0.0 + + @JvmField + @Preference("MP Drive D", persistent = false) + var driveD = 0.0 + + @JvmField + @Preference("MP Delta-T", persistent = false) + var deltaT = 0.0 + + @JvmField + @Preference("MP Max Vel", persistent = false) + var maxVel = 0.0 + + @JvmField + @Preference("MP Max Accel", persistent = false) + var maxAccel = 0.0 + + @JvmField + @Preference("MP Max Jerk", persistent = false) + var maxJerk = 0.0 + + @JvmField + @Preference("Wheelbase", persistent = false) + var wheelbase = 0.0 + + @JvmField + @Preference("Drive TPU", persistent = false) + var tpu = 0.0 + + override fun robotInit() { + PreferenceManager.getInstance().add(this) + DriveTrain + + DriveTrain.brake = false + SmartDashboard.setDefaultNumber("Test Profile X", 0.0) + SmartDashboard.setDefaultNumber("Test Profile Y", 0.0) + SmartDashboard.setDefaultNumber("Test Profile Angle", 0.0) + + factory = FollowProfileFactory( + MotionProfileUtils.createSetpointConsumer(DriveTrain.left1, tpu), + MotionProfileUtils.createSetpointConsumer(DriveTrain.left2, tpu), + DriveTrain + ).apply { + lVelIntercept = lVIntercept + lVelCoeff = lKV + lAccelCoeff = lKA + rVelIntercept = rVIntercept + rVelCoeff = rKV + rAccelCoeff = rKA + loopFreq = loopFreqMs.toLong() + headingP = hdgP + headingI = hdgI + headingSupplier = DoubleSupplier { processedHeading } + } + } + + override fun autonomousPeriodic() { + } + + override fun robotPeriodic() { + Scheduler.getInstance().run() + if (DriveTrain.p != driveP) DriveTrain.p = driveP + if (DriveTrain.d != driveP) DriveTrain.d = driveD + SmartDashboard.putNumber("Raw heading", navx.yaw.toDouble()) + SmartDashboard.putNumber("Processed heading", processedHeading) + SmartDashboard.putNumber("Heading Error", command?.follower?.getGyroError(processedHeading, command?.executionTime + ?: 0.0) ?: 0.0) + SmartDashboard.putNumber("Heading IAccum", command?.follower?.gyroIAccum ?: 0.0) + } + + override fun disabledInit() { + DriveTrain.brake = false + } + + override fun disabledPeriodic() { + } + + override fun teleopPeriodic() { + } + + val processedHeading: Double + get() { + val yaw = Math.toRadians(navx.yaw.toDouble()) + return if (yaw < 0) PI - yaw else PI + yaw + } + + override fun autonomousInit() { + DriveTrain.brake = true + DriveTrain.zero() + factory.apply { + lVelIntercept = lVIntercept + lVelCoeff = lKV + lAccelCoeff = lKA + rVelIntercept = rVIntercept + rVelCoeff = rKV + rAccelCoeff = rKA + loopFreq = loopFreqMs.toLong() + headingP = hdgP + headingI = hdgI + leftSetpointConsumer = MotionProfileUtils.createSetpointConsumer(DriveTrain.left1, tpu) + rightSetpointConsumer = MotionProfileUtils.createSetpointConsumer(DriveTrain.right1, tpu) + } + + // generate new trajectory + val (leftTrajectory, rightTrajectory) = TankModifier(Pathfinder.generate(arrayOf( + Waypoint(0.0, 0.0, 0.0), + Waypoint( + SmartDashboard.getNumber("Test Profile X", 0.0), + SmartDashboard.getNumber("Test Profile Y", 0.0), + SmartDashboard.getNumber("Test Profile Angle", 0.0) + ) + ), Trajectory.Config( + Trajectory.FitMethod.HERMITE_CUBIC, + Trajectory.Config.SAMPLES_FAST, + deltaT, + maxVel, + maxAccel, + maxJerk + ))).modify(wheelbase).trajectories + + command = factory.create(MotionProfileUtils.createProfile(leftTrajectory), MotionProfileUtils.createProfile(rightTrajectory)) + + command?.start() + } + + override fun teleopInit() { + DriveTrain.brake = true + } + +} + +private val TankModifier.trajectories get() = leftTrajectory to rightTrajectory + +private object DriveTrain : Subsystem() { + val left1 = ChickenTalon(1) + val left2 = ChickenTalon(2) + val left3 = ChickenTalon(3) + + val right1 = ChickenTalon(4) + val right2 = ChickenTalon(5) + val right3 = ChickenTalon(6) + + val masters = arrayOf(left1, right1) + val lefts = arrayOf(left1, left2, left3) + val rights = arrayOf(right1, right2, right3) + + val all = arrayOf(*lefts, *rights) + + var p = 0.0 + set(value) { + field = value + for (talon in masters) talon.config_kP(0, value) + } + + var d = 0.0 + set(value) { + field = value + for (talon in masters) talon.config_kD(0, value) + } + + var brake = true + set(value) { + field = value + for (talon in all) talon.setBrake(value) + } + + override fun initDefaultCommand() { + defaultCommand = SimpleLoopCommand("Teleop Drive", Executable { + left1.set(ControlMode.PercentOutput, Utilities.processDeadzone(driver.getRawAxis(1), 0.1)) + right1.set(ControlMode.PercentOutput, Utilities.processDeadzone(driver.getRawAxis(5), 0.1)) + }, this) + } + + fun zero() { + left1.setSelectedSensorPosition(0) + right1.setSelectedSensorPosition(0) + } + + private fun reset() { + left1.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder) + right1.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder) + + left1.setSensorPhase(true) + + for (talon in lefts) { + talon.inverted = false + } + + right1.setSensorPhase(true) + + for (talon in rights) { + talon.inverted = true + } + + left2.set(ControlMode.Follower, left1.deviceID.toDouble()) + left3.set(ControlMode.Follower, left1.deviceID.toDouble()) + + right2.set(ControlMode.Follower, right1.deviceID.toDouble()) + right3.set(ControlMode.Follower, right1.deviceID.toDouble()) + + for (talon in all) { + talon.setBrake(brake) + } + + for (talon in masters) { + talon.config_kP(0, p) + talon.config_kI(0, 0.0) + talon.config_kD(0, d) + talon.config_kF(0, 0.0) + talon.config_IntegralZone(0, 0) + talon.configVoltageCompSaturation(12.0) + talon.enableVoltageCompensation(true) + } + } + + override fun periodic() { + SmartDashboard.putNumber("LPOS", left1.selectedSensorPosition) + SmartDashboard.putNumber("RPOS", right1.selectedSensorPosition) + SmartDashboard.putNumber("LVEL", left1.selectedSensorVelocity) + SmartDashboard.putNumber("RVEL", right1.selectedSensorVelocity) + SmartDashboard.putNumber("LTHROT", left1.motorOutputPercent) + SmartDashboard.putNumber("RTHROT", right1.motorOutputPercent) + SmartDashboard.putNumber("LCURR", left1.outputCurrent + left2.outputCurrent + left3.outputCurrent) + SmartDashboard.putNumber("RCURR", right1.outputCurrent + right2.outputCurrent + right3.outputCurrent) + SmartDashboard.putNumber("LVOLT", left1.motorOutputVoltage) + SmartDashboard.putNumber("RVOLT", right1.motorOutputVoltage) + SmartDashboard.putNumber("LERR", left1.closedLoopError.toDouble()) + SmartDashboard.putNumber("RERR", right1.closedLoopError.toDouble()) + SmartDashboard.putNumber("LTGT", right1.getClosedLoopTarget(0).toDouble()) + SmartDashboard.putNumber("RTGT", right1.getClosedLoopTarget(0).toDouble()) + } + + init { + reset() + } +} + From 2f0945ac62f722ef72de8ba42b7a00a0f8e6d666 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Fri, 30 Nov 2018 10:39:10 -0800 Subject: [PATCH 06/51] Add/document status getters and reset() method --- .../TurningRateClosedLoopProcessor.java | 34 +++++++++++++++++++ 1 file changed, 34 insertions(+) diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/TurningRateClosedLoopProcessor.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/TurningRateClosedLoopProcessor.java index 7e3d37bc..0cf330ad 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/TurningRateClosedLoopProcessor.java +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/TurningRateClosedLoopProcessor.java @@ -73,6 +73,40 @@ public TankDriveData apply(TankDriveData driveData) { } } + /** + * Gets the current value of the integral accumulator. + * + * @return The integral accumulator, in radians. If the PID loop has not yet been run (i.e. {@link + * #apply(TankDriveData)} apply()} has not yet been called since instantiation/the last call to + * {@link #reset()}), returns 0. + */ + public double getIAccum() { + return iAccum; + } + + /** + * Gets the closed-loop error from the last run of the PID loop. + * + * @return The closed-loop error, in radians per second. If the PID loop has not yet been run + * (i.e. {@link #apply(TankDriveData)} apply()} has not yet been called since instantiation/the + * last call to {@link #reset()}), returns 0. + */ + public double getError() { + return lastError; + } + + /** + * Resets the PID loop. This clears the integral accumulator and error values, and should be + * called if the {@code TurningRateClosedLoopProcessor} is being reused for multiple discrete + * occasions (i.e. executions of different motion profile segments). Calling this is functionally + * equivalent to creating a new processor instance. + */ + public void reset() { + iAccum = 0; + lastError = 0; + lastTime = -1; + } + public TurningRateClosedLoopProcessor(DoubleSupplier yawRateSupplier, double p) { this(yawRateSupplier, p, 0); } From e5b892c32e7c89a6c905b93fd930c8514db02b40 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Fri, 30 Nov 2018 16:58:10 -0800 Subject: [PATCH 07/51] Extract PID logic from TurningRateClosedLoopProcessor Also documentation! --- .../rooster/drive/pipeline/PIDProcessor.java | 113 ++++++++++++++ .../TurningRateClosedLoopProcessor.java | 141 ------------------ .../pipeline/TurningRatePIDProcessor.java | 132 ++++++++++++++++ 3 files changed, 245 insertions(+), 141 deletions(-) create mode 100644 lib/src/main/java/org/team1540/rooster/drive/pipeline/PIDProcessor.java delete mode 100644 lib/src/main/java/org/team1540/rooster/drive/pipeline/TurningRateClosedLoopProcessor.java create mode 100644 lib/src/main/java/org/team1540/rooster/drive/pipeline/TurningRatePIDProcessor.java diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/PIDProcessor.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/PIDProcessor.java new file mode 100644 index 00000000..2db9992e --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/PIDProcessor.java @@ -0,0 +1,113 @@ +package org.team1540.rooster.drive.pipeline; + +/** + * Processor to execute a generic PID loop. + * + * @param The input type of the processor. + * @param The output type of the processor. + */ +public abstract class PIDProcessor implements Processor { + + private double p, i, d; + + private double iAccum; + + private double lastError; + + private long lastTime = -1; + + /** + * Create a new {@code PIDProcessor}. + * + * @param p The P coefficient. + * @param i The I coefficient. + * @param d The D coefficient. + */ + protected PIDProcessor(double p, double i, double d) { + this.p = p; + this.i = i; + this.d = d; + } + + /** + * Extract the PID loop target from the data passed to the processor, and calculate the current + * error. + * + * @param data The data instance that was passed to the processor's {@link #apply(Object) apply()} + * method. + * @return The error. + */ + protected abstract double getError(I data); + + /** + * Create the processor output. + * + * @param data The data instance that was passed to the processor's {@link #apply(Object) apply()} + * method. + * @param loopOutput The output of the PID loop + * @return The processor's output; this will be returned from the the processor's {@link + * #apply(Object) apply()} method. + */ + protected abstract O createOutput(I data, double loopOutput); + + @Override + public O apply(I input) { + double output = 0; + double error = getError(input); + + // p gain + output += error * p; + + if (lastTime != -1) { + double dt = (System.currentTimeMillis() - lastTime) / 1000.0; + + // i gain + iAccum += error * dt; + output += i * iAccum; + + // d gain + double dError = (error - lastError) / dt; + output += d * dError; + } + + lastError = error; + lastTime = System.currentTimeMillis(); + + return createOutput(input, output); + } + + + /** + * Gets the current value of the integral accumulator. + * + * @return The integral accumulator. If the PID loop has not yet been run (i.e. {@link + * #apply(Object) apply()} has not yet been called since instantiation/the last call to {@link + * #reset()}), returns 0. + */ + public double getIAccum() { + return iAccum; + } + + /** + * Gets the closed-loop error from the last run of the PID loop. + * + * @return The closed-loop error. If the PID loop has not yet been run (i.e. {@link #apply(Object) + * apply()} has not yet been called since instantiation/the last call to {@link #reset()}), + * returns 0. + */ + public double getError() { + return lastError; + } + + /** + * Resets the PID loop. This clears the integral accumulator and error values, and should be + * called if the {@code TurningRateClosedLoopProcessor} is being reused for multiple discrete + * occasions (i.e. executions of different motion profile segments). Calling this is functionally + * equivalent to creating a new processor instance. + */ + public void reset() { + iAccum = 0; + lastError = 0; + lastTime = -1; + } +} diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/TurningRateClosedLoopProcessor.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/TurningRateClosedLoopProcessor.java deleted file mode 100644 index 0cf330ad..00000000 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/TurningRateClosedLoopProcessor.java +++ /dev/null @@ -1,141 +0,0 @@ -package org.team1540.rooster.drive.pipeline; - -import java.util.OptionalDouble; -import java.util.function.DoubleSupplier; - -/** - * Modifies velocity setpoints to keep a desired turning rate. - */ -public class TurningRateClosedLoopProcessor implements Processor { - - private DoubleSupplier yawRateSupplier; - private double p, i, d; - - private double iAccum; - - private double lastError; - - private long lastTime = -1; - - private boolean invertSides; - - @Override - public TankDriveData apply(TankDriveData driveData) { - if (driveData.turningRate.isPresent()) { - // simple PID controller - - double actual = this.yawRateSupplier.getAsDouble(); - double target = driveData.turningRate.getAsDouble(); - - double output = 0; - double error = target - actual; - - // p gain - output += error * p; - - if (lastTime != -1) { - double dt = (System.currentTimeMillis() - lastTime) / 1000.0; - - // i gain - iAccum += error * dt; - output += i * iAccum; - - // d gain - double dError = (error - lastError) / dt; - output += d * dError; - } - - lastError = error; - lastTime = System.currentTimeMillis(); - - // multiplying the output by -1 effectively flips the sides - output *= invertSides ? -1 : 1; - - return new TankDriveData( - new DriveData( - driveData.left.position, - OptionalDouble.of(driveData.left.velocity.orElse(0) - output), - driveData.left.acceleration, - driveData.left.additionalFeedForward - ), - new DriveData( - driveData.right.position, - OptionalDouble.of(driveData.right.velocity.orElse(0) + output), - driveData.right.acceleration, - driveData.right.additionalFeedForward - ), - driveData.heading, - driveData.turningRate - ); - } else { - // we can't do anything here without a setpoint so just pass the boi through - return driveData; - } - } - - /** - * Gets the current value of the integral accumulator. - * - * @return The integral accumulator, in radians. If the PID loop has not yet been run (i.e. {@link - * #apply(TankDriveData)} apply()} has not yet been called since instantiation/the last call to - * {@link #reset()}), returns 0. - */ - public double getIAccum() { - return iAccum; - } - - /** - * Gets the closed-loop error from the last run of the PID loop. - * - * @return The closed-loop error, in radians per second. If the PID loop has not yet been run - * (i.e. {@link #apply(TankDriveData)} apply()} has not yet been called since instantiation/the - * last call to {@link #reset()}), returns 0. - */ - public double getError() { - return lastError; - } - - /** - * Resets the PID loop. This clears the integral accumulator and error values, and should be - * called if the {@code TurningRateClosedLoopProcessor} is being reused for multiple discrete - * occasions (i.e. executions of different motion profile segments). Calling this is functionally - * equivalent to creating a new processor instance. - */ - public void reset() { - iAccum = 0; - lastError = 0; - lastTime = -1; - } - - public TurningRateClosedLoopProcessor(DoubleSupplier yawRateSupplier, double p) { - this(yawRateSupplier, p, 0); - } - - public TurningRateClosedLoopProcessor(DoubleSupplier yawRateSupplier, double p, double i) { - this(yawRateSupplier, p, i, 0); - } - - public TurningRateClosedLoopProcessor(DoubleSupplier yawRateSupplier, double p, double i, - double d) { - this(yawRateSupplier, p, i, d, false); - } - - public TurningRateClosedLoopProcessor(DoubleSupplier yawRateSupplier, double p, - boolean invertSides) { - this(yawRateSupplier, p, 0, invertSides); - } - - public TurningRateClosedLoopProcessor(DoubleSupplier yawRateSupplier, double p, double i, - boolean invertSides) { - this(yawRateSupplier, p, i, 0, invertSides); - } - - public TurningRateClosedLoopProcessor(DoubleSupplier yawRateSupplier, double p, double i, - double d, boolean invertSides) { - this.yawRateSupplier = yawRateSupplier; - this.p = p; - this.i = i; - this.d = d; - this.invertSides = invertSides; - } -} diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/TurningRatePIDProcessor.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/TurningRatePIDProcessor.java new file mode 100644 index 00000000..8265f454 --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/TurningRatePIDProcessor.java @@ -0,0 +1,132 @@ +package org.team1540.rooster.drive.pipeline; + +import java.util.Objects; +import java.util.OptionalDouble; +import java.util.function.DoubleSupplier; +import org.jetbrains.annotations.NotNull; + +/** + * Modifies velocity set-points to keep a desired turning rate. This {@link Processor} accepts a + * {@link TankDriveData} and uses the {@linkplain TankDriveData#turningRate turning rate} as an + * input to the loop. + * + * @see Processor + * @see PIDProcessor + */ +public class TurningRatePIDProcessor extends PIDProcessor { + + private DoubleSupplier yawRateSupplier; + private boolean invertSides; + + @Override + protected double getError(TankDriveData data) { + return data.turningRate.isPresent() ? data.turningRate.getAsDouble() - yawRateSupplier + .getAsDouble() : 0; + } + + @Override + protected TankDriveData createOutput(TankDriveData data, double loopOutput) { + // multiplying the output by -1 effectively flips the sides + loopOutput *= invertSides ? -1 : 1; + + return new TankDriveData( + new DriveData( + data.left.position, + OptionalDouble.of(data.left.velocity.orElse(0) - loopOutput), + data.left.acceleration, + data.left.additionalFeedForward + ), + new DriveData( + data.right.position, + OptionalDouble.of(data.right.velocity.orElse(0) + loopOutput), + data.right.acceleration, + data.right.additionalFeedForward + ), + data.heading, + data.turningRate + ); + } + + /** + * Creates a new {@code TurningRateClosedLoopProcessor} with P and I D coefficients of 0 that does + * not invert sides. + * + * @param yawRateSupplier A {@link DoubleSupplier} that supplies the current yaw rate in radians + * per second. + * @param p The P coefficient of the PID loop. + */ + public TurningRatePIDProcessor(DoubleSupplier yawRateSupplier, double p) { + this(yawRateSupplier, p, 0); + } + + /** + * Creates a new {@code TurningRateClosedLoopProcessor} with a D coefficient of 0 that does not + * invert sides. + * + * @param yawRateSupplier A {@link DoubleSupplier} that supplies the current yaw rate in radians + * per second. + * @param p The P coefficient of the PID loop. + * @param i The I coefficient of the PID loop. + */ + public TurningRatePIDProcessor(DoubleSupplier yawRateSupplier, double p, double i) { + this(yawRateSupplier, p, i, 0); + } + + /** + * Creates a new {@code TurningRateClosedLoopProcessor} that does not invert sides. + * + * @param yawRateSupplier A {@link DoubleSupplier} that supplies the current yaw rate in radians + * per second. + * @param p The P coefficient of the PID loop. + * @param i The I coefficient of the PID loop. + * @param d The D coefficient of the PID loop. + */ + public TurningRatePIDProcessor(DoubleSupplier yawRateSupplier, double p, double i, + double d) { + this(yawRateSupplier, p, i, d, false); + } + + /** + * Creates a new {@code TurningRateClosedLoopProcessor} with I and D coefficients of 0. + * + * @param yawRateSupplier A {@link DoubleSupplier} that supplies the current yaw rate in radians + * per second. + * @param p The P coefficient of the PID loop. + * @param invertSides Whether to invert the output of the PID loop before sending it onwards. + */ + public TurningRatePIDProcessor(DoubleSupplier yawRateSupplier, double p, + boolean invertSides) { + this(yawRateSupplier, p, 0, invertSides); + } + + /** + * Creates a new {@code TurningRateClosedLoopProcessor} with a D coefficient of 0. + * + * @param yawRateSupplier A {@link DoubleSupplier} that supplies the current yaw rate in radians + * per second. + * @param p The P coefficient of the PID loop. + * @param i The I coefficient of the PID loop. + * @param invertSides Whether to invert the output of the PID loop before sending it onwards. + */ + public TurningRatePIDProcessor(DoubleSupplier yawRateSupplier, double p, double i, + boolean invertSides) { + this(yawRateSupplier, p, i, 0, invertSides); + } + + /** + * Creates a new {@code TurningRateClosedLoopProcessor}. + * + * @param yawRateSupplier A {@link DoubleSupplier} that supplies the current yaw rate in radians + * per second. + * @param p The P coefficient of the PID loop. + * @param i The I coefficient of the PID loop. + * @param d The D coefficient of the PID loop. + * @param invertSides Whether to invert the output of the PID loop before sending it onwards. + */ + public TurningRatePIDProcessor(@NotNull DoubleSupplier yawRateSupplier, double p, double i, + double d, boolean invertSides) { + super(p, i, d); + this.yawRateSupplier = Objects.requireNonNull(yawRateSupplier); + this.invertSides = invertSides; + } +} From 9c1eec269f1266788fea82cb2cf952d36e9fd232 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Fri, 30 Nov 2018 17:44:10 -0800 Subject: [PATCH 08/51] Create HeadingPIDProcessor --- .../drive/pipeline/HeadingPIDProcessor.java | 125 ++++++++++++++++++ 1 file changed, 125 insertions(+) create mode 100644 lib/src/main/java/org/team1540/rooster/drive/pipeline/HeadingPIDProcessor.java diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/HeadingPIDProcessor.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/HeadingPIDProcessor.java new file mode 100644 index 00000000..77d69703 --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/HeadingPIDProcessor.java @@ -0,0 +1,125 @@ +package org.team1540.rooster.drive.pipeline; + +import java.util.OptionalDouble; +import java.util.function.DoubleSupplier; + +public class HeadingPIDProcessor extends PIDProcessor { + + private DoubleSupplier headingSupplier; + private boolean outputToPosition; + private boolean invertSides; + + /** + * Create a new {@code HeadingPIDProcessor} without inverted sides that outputs to feed-forward + * setpoints. + * @param p The P coefficient. + * @param headingSupplier A supplier that returns the current heading, in radians from -π to π. + */ + public HeadingPIDProcessor(double p, DoubleSupplier headingSupplier) { + this(p, 0, headingSupplier); + } + + /** + * Create a new {@code HeadingPIDProcessor} without inverted sides that outputs to feed-forward + * setpoints. + * + * @param p The P coefficient. + * @param i The I coefficient. + * @param headingSupplier A supplier that returns the current heading, in radians from -π to π. + */ + public HeadingPIDProcessor(double p, double i, DoubleSupplier headingSupplier) { + this(p, i, 0, headingSupplier); + } + + /** + * Create a new {@code HeadingPIDProcessor} without inverted sides that outputs to feed-forward + * setpoints. + * + * @param p The P coefficient. + * @param i The I coefficient. + * @param d The D coefficient. + * @param headingSupplier A supplier that returns the current heading, in radians from -π to π. + */ + public HeadingPIDProcessor(double p, double i, double d, DoubleSupplier headingSupplier) { + this(p, i, d, headingSupplier, false); + } + + /** + * Create a new {@code HeadingPIDProcessor} without inverted sides. + * + * @param p The P coefficient. + * @param i The I coefficient. + * @param d The D coefficient. + * @param headingSupplier A supplier that returns the current heading, in radians from -π to π. + * @param outputToPosition Whether to output to the position setpoints (as opposed to the + * feed-forward setpoints.) + */ + public HeadingPIDProcessor(double p, double i, double d, DoubleSupplier headingSupplier, + boolean outputToPosition) { + this(p, i, d, headingSupplier, outputToPosition, false); + } + + /** + * Create a new {@code HeadingPIDProcessor}. + * + * @param p The P coefficient. + * @param i The I coefficient. + * @param d The D coefficient. + * @param headingSupplier A supplier that returns the current heading, in radians from -π to π. + * @param outputToPosition Whether to output to the position setpoints (as opposed to the + * feed-forward setpoints.) + * @param invertSides Whether to invert the output sides. (If {@code true}, the loop output will + * be added to the left side and subtracted from the right, and vice versa.) + */ + public HeadingPIDProcessor(double p, double i, double d, DoubleSupplier headingSupplier, + boolean outputToPosition, boolean invertSides) { + super(p, i, d); + this.headingSupplier = headingSupplier; + this.outputToPosition = outputToPosition; + this.invertSides = invertSides; + } + + @Override + protected double getError(TankDriveData data) { + if (data.heading.isPresent()) { + double heading = headingSupplier.getAsDouble(); + double headingTarget = data.heading.getAsDouble(); + + // basically magic https://stackoverflow.com/a/2007279 + return Math.atan2(Math.sin(heading - headingTarget), Math.cos(heading - headingTarget)); + } else { + return 0; + } + } + + @Override + protected TankDriveData createOutput(TankDriveData data, double loopOutput) { + // multiplying the output by -1 effectively flips the sides + loopOutput *= invertSides ? -1 : 1; + + return new TankDriveData( + new DriveData( + outputToPosition ? + OptionalDouble.of(data.left.position.orElse(0) - loopOutput) + : data.left.position, + data.left.velocity, + data.left.acceleration, + !outputToPosition ? + OptionalDouble.of(data.left.additionalFeedForward.orElse(0) - loopOutput) + : data.left.additionalFeedForward + ), + new DriveData( + outputToPosition ? + OptionalDouble.of(data.right.position.orElse(0) + loopOutput) + : data.right.position, + data.right.velocity, + data.right.acceleration, + !outputToPosition ? + OptionalDouble.of(data.right.additionalFeedForward.orElse(0) + loopOutput) + : data.right.additionalFeedForward + ), + data.heading, + data.turningRate + ); + } +} From 6baae98b72c1055c86c2efdb9aaf1f923b986d8f Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Fri, 30 Nov 2018 19:58:38 -0800 Subject: [PATCH 09/51] Create HeadingPIDPipelineTestRobot --- .../rooster/testing/DrivePipelineTesting.kt | 58 +++++++++++++++++++ 1 file changed, 58 insertions(+) diff --git a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt index 0321f531..dc1b62af 100644 --- a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt +++ b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt @@ -15,6 +15,7 @@ import org.team1540.rooster.util.SimpleAsyncCommand import org.team1540.rooster.util.SimpleCommand import org.team1540.rooster.util.SimpleLoopCommand import org.team1540.rooster.wrappers.ChickenTalon +import java.util.OptionalDouble import java.util.function.DoubleSupplier /** @@ -114,6 +115,63 @@ class AdvancedJoystickInputPipelineTestRobot : DrivePipelineTestRobot() { override val command get() = _command } +class HeadingPIDPipelineTestRobot : DrivePipelineTestRobot() { + + @JvmField + @Preference(persistent = false) + var p = 0.0 + @JvmField + @Preference(persistent = false) + var i = 0.0 + @JvmField + @Preference(persistent = false) + var d = 0.0 + @JvmField + @Preference(persistent = false) + var hdgSet = 0.0 + @JvmField + @Preference(persistent = false) + var invertSides = false + + private var headingPIDProcessor: HeadingPIDProcessor? = null + + override fun robotInit() { + PreferenceManager.getInstance().add(this) + val reset = SimpleCommand("reset", Executable { + headingPIDProcessor = HeadingPIDProcessor(p, i, d, + { Math.toRadians(PipelineNavx.navx.yaw.toDouble()) }, + false, invertSides) + _command = SimpleAsyncCommand("Drive", 20, + Input { + TankDriveData( + DriveData(OptionalDouble.empty()), + DriveData(OptionalDouble.empty()), + OptionalDouble.of(hdgSet), + OptionalDouble.empty()) + } + headingPIDProcessor!! + (CTREOutput(PipelineDriveTrain.left1, PipelineDriveTrain.right1)) + ) + + + }).apply { + setRunWhenDisabled(true) + start() + } + + SmartDashboard.putData(reset) + } + + override fun robotPeriodic() { + super.robotPeriodic() + + headingPIDProcessor?.error?.let { SmartDashboard.putNumber("Error", it) } + headingPIDProcessor?.iAccum?.let { SmartDashboard.putNumber("Iaccum", it) } + SmartDashboard.putNumber("hdg", Math.toRadians(PipelineNavx.navx.yaw.toDouble())) + } + + private lateinit var _command: Command + override val command get() = _command +} + /** * Common drive train object to be used by all pipeline test robots. */ From 73655c41b2cb4baec14c80e30360dd228f289daf Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Fri, 30 Nov 2018 20:07:03 -0800 Subject: [PATCH 10/51] Create TurningRatePIDPipelineTestRobot --- .../rooster/testing/DrivePipelineTesting.kt | 55 +++++++++++++++++++ 1 file changed, 55 insertions(+) diff --git a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt index dc1b62af..3915ba5d 100644 --- a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt +++ b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt @@ -172,6 +172,61 @@ class HeadingPIDPipelineTestRobot : DrivePipelineTestRobot() { override val command get() = _command } +class TurningRatePIDPipelineTestRobot : DrivePipelineTestRobot() { + + @JvmField + @Preference(persistent = false) + var p = 0.0 + @JvmField + @Preference(persistent = false) + var i = 0.0 + @JvmField + @Preference(persistent = false) + var d = 0.0 + @JvmField + @Preference(persistent = false) + var turnSet = 0.0 + @JvmField + @Preference(persistent = false) + var invertSides = false + + private var turningRatePIDProcessor: TurningRatePIDProcessor? = null + + override fun robotInit() { + PreferenceManager.getInstance().add(this) + val reset = SimpleCommand("reset", Executable { + turningRatePIDProcessor = TurningRatePIDProcessor({ Math.toRadians(PipelineNavx.navx.rate) }, p, i, d, invertSides) + _command = SimpleAsyncCommand("Drive", 20, + Input { + TankDriveData( + DriveData(OptionalDouble.empty()), + DriveData(OptionalDouble.empty()), + OptionalDouble.empty(), + OptionalDouble.of(turnSet)) + } + turningRatePIDProcessor!! + FeedForwardProcessor(1.0, 0.0, 0.0) + (CTREOutput(PipelineDriveTrain.left1, PipelineDriveTrain.right1)) + ) + + + }).apply { + setRunWhenDisabled(true) + start() + } + + SmartDashboard.putData(reset) + } + + override fun robotPeriodic() { + super.robotPeriodic() + + turningRatePIDProcessor?.error?.let { SmartDashboard.putNumber("Error", it) } + turningRatePIDProcessor?.iAccum?.let { SmartDashboard.putNumber("Iaccum", it) } + SmartDashboard.putNumber("hdg", Math.toRadians(PipelineNavx.navx.yaw.toDouble())) + } + + private lateinit var _command: Command + override val command get() = _command +} + /** * Common drive train object to be used by all pipeline test robots. */ From c6b8380faeff044ea22cd2d090b74923fcee9875 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Fri, 30 Nov 2018 20:07:17 -0800 Subject: [PATCH 11/51] Add test robot run configurations --- .../Deploy_HeadingPIDPipelineTestRobot.xml | 21 +++++++++++++++++++ ...Deploy_TurningRatePIDPipelineTestRobot.xml | 21 +++++++++++++++++++ 2 files changed, 42 insertions(+) create mode 100644 .idea/runConfigurations/Deploy_HeadingPIDPipelineTestRobot.xml create mode 100644 .idea/runConfigurations/Deploy_TurningRatePIDPipelineTestRobot.xml diff --git a/.idea/runConfigurations/Deploy_HeadingPIDPipelineTestRobot.xml b/.idea/runConfigurations/Deploy_HeadingPIDPipelineTestRobot.xml new file mode 100644 index 00000000..e75f62f7 --- /dev/null +++ b/.idea/runConfigurations/Deploy_HeadingPIDPipelineTestRobot.xml @@ -0,0 +1,21 @@ + + + + + + + true + + + \ No newline at end of file diff --git a/.idea/runConfigurations/Deploy_TurningRatePIDPipelineTestRobot.xml b/.idea/runConfigurations/Deploy_TurningRatePIDPipelineTestRobot.xml new file mode 100644 index 00000000..38da5f8f --- /dev/null +++ b/.idea/runConfigurations/Deploy_TurningRatePIDPipelineTestRobot.xml @@ -0,0 +1,21 @@ + + + + + + + true + + + \ No newline at end of file From 14f2f4a22b5624659b7bbb277acc203a4ff643b8 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 3 Dec 2018 15:45:33 -0800 Subject: [PATCH 12/51] Add arbitrary file deployment --- test/build.gradle | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/test/build.gradle b/test/build.gradle index 1745e1e3..ca155f13 100644 --- a/test/build.gradle +++ b/test/build.gradle @@ -37,6 +37,19 @@ deploy { */ debug = project.hasProperty("deploy-debug") } + + /* + This deploys anything that's in the test/deploy/ directory to the /home/lvuser/roostertest + directory on the roboRIO. This is useful for deploying motion profiles, etc. Note that + (until we switch to a version of GradleRIO that deploys a file tree) any directory structure + will be flattened and the files will just be dumped in. + */ + fileCollectionArtifact("testFiles") { + targets << "roborio" + + directory = "roostertest" + files = fileTree(dir: "deploy") + } } } From 777cad5d6595e18e53798e6a307ba3fdf22d5193 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 3 Dec 2018 16:04:31 -0800 Subject: [PATCH 13/51] Add .csv files to the .gitignore --- .gitignore | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.gitignore b/.gitignore index 62e84356..cc3fb06a 100644 --- a/.gitignore +++ b/.gitignore @@ -157,3 +157,8 @@ gradle-app.setting # # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 # gradle/wrapper/gradle-wrapper.properties + +### ROOSTER-specific + +# motion profiles +*.csv From 73aa609537f08344e16a0a0b2ff29d5664092d2a Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Tue, 4 Dec 2018 14:22:58 -0800 Subject: [PATCH 14/51] Create ProfileContainer Aka CSVProfileManager with a different name. --- .../motionprofiling/ProfileContainer.java | 169 ++++++++++++++++++ 1 file changed, 169 insertions(+) create mode 100644 lib/src/main/java/org/team1540/rooster/motionprofiling/ProfileContainer.java diff --git a/lib/src/main/java/org/team1540/rooster/motionprofiling/ProfileContainer.java b/lib/src/main/java/org/team1540/rooster/motionprofiling/ProfileContainer.java new file mode 100644 index 00000000..086f942a --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/motionprofiling/ProfileContainer.java @@ -0,0 +1,169 @@ +package org.team1540.rooster.motionprofiling; + +import edu.wpi.first.wpilibj.DriverStation; +import jaci.pathfinder.Pathfinder; +import java.io.File; +import java.util.Arrays; +import java.util.HashMap; +import java.util.HashSet; +import java.util.Map; +import java.util.Set; +import org.jetbrains.annotations.NotNull; +import org.jetbrains.annotations.Nullable; + +/** + * Class to preload motion profiles from a specified folder on disk. The preloaded motion profiles + * are stored in RAM so as to be quickly accessible. + * + *

The folder provided should contain profile CSV files (formatted so as to be read by {@link + * Pathfinder#readFromCSV(File)}) where each profile named {@code name} is stored in two files, + * {@code name}_left.csv for the left-side profile and {@code name}_right.csv for the right-side + * profile. An example folder structure would be as follows (where the profiles/ directory is + * provided to the constructor): + * + * 

+ * profiles/
+ *   foo_left.csv
+ *   foo_right.csv
+ *   bar_left.csv
+ *   bar_right.csv
+ *
+ * + * This would cause the {@code ProfileContainer} to load two profiles named {@code foo} and {@code + * bar}. + * + *

This class is immutable after instantiation. + */ +public class ProfileContainer { + + @NotNull + private Map profiles; + + /** + * Creates a new {@code ProfileContainer}. This constructor also searches the provided directory + * for profiles and loads all the profiles into RAM. For this reason, initialization may take some + * time (especially for large amounts of profiles). + * + * @param profileDirectory The directory containing the profiles. See the {@linkplain + * ProfileContainer class documentation} for a description of the folder structure. + * @throws RuntimeException If an I/O error occurs during profile loading. + */ + public ProfileContainer(@NotNull File profileDirectory) { + if (!profileDirectory.isDirectory()) { + throw new IllegalArgumentException("Not a directory"); + } + + File[] lFiles = profileDirectory.listFiles((file) -> file.getName().endsWith("_left.csv")); + File[] rFiles = profileDirectory.listFiles((file) -> file.getName().endsWith("_right.csv")); + + if (lFiles == null || rFiles == null) { + // according to listFiles() docs, it will only return null if the file isn't a directory + // (which we've already checked) or if an IO error occurs. Thus, if lFiles or rFiles is + // null we know an IO error happened. Not throwing an IOException because checked exceptions + // are bad, in constructors even more so. + throw new RuntimeException("IO error occurred while reading files"); + } + + Set profileNames = new HashSet<>(); + + for (File f : lFiles) { + profileNames.add(f.getName().substring(0, f.getName().length() - "_left.csv".length())); + } + + for (File f : rFiles) { + profileNames.add(f.getName().substring(0, f.getName().length() - "_right.csv".length())); + } + + // initialize the map once we know the number of profiles so it doesn't expand. + // Why? p e r f o r m a n c e + + profiles = new HashMap<>(profileNames.size()); + + for (String name : profileNames) { + System.out.println("Loading profile " + name); + File leftFile = Arrays.stream(lFiles) + .filter(file -> file.getName().equals(name + "_left.csv")) + .findFirst() + .orElseGet(() -> { + DriverStation + .reportWarning("Left-side file for profile " + name + " does not exist", false); + return null; + }); + File rightFile = Arrays.stream(rFiles) + .filter(file -> file.getName().equals(name + "_right.csv")) + .findFirst() + .orElseGet(() -> { + DriverStation + .reportWarning("Right-side file for profile " + name + " does not exist", false); + return null; + }); + + if (leftFile != null && rightFile != null) { + MotionProfile left = MotionProfileUtils.createProfile(Pathfinder.readFromCSV(leftFile)); + MotionProfile right = MotionProfileUtils.createProfile(Pathfinder.readFromCSV(rightFile)); + + profiles.put(name, new DriveProfile(left, right)); + } + } + } + + /** + * Returns the motion profile set with the specified name, or {@code null} if the profile does not + * exist. + * + * @param name The name of the profile. + * @return A {@link DriveProfile} containing the left and right profiles, or {@code null} if the + * profile does not exist. + */ + @Nullable + public DriveProfile get(String name) { + return profiles.get(name); + } + + /** + * Get the {@link Set} of profile names in the {@code ProfileContainer}. + * + * @return A {@link Set} containing the names of each profile. + */ + @NotNull + public Set getProfileNames() { + return profiles.keySet(); + } + + /** + * Returns whether or not the {@code ProfileContainer} contains a specific profile. + * + * @param name The name of the profile to check. + * @return {@code true} if the profile exists (i.e. if {@link #get(String) get(name)} would not + * return {@code null}), {@code false} otherwise. + */ + public boolean hasProfile(String name) { + return profiles.containsKey(name); + } + + /** + * Data class for holding left and right trajectories in one object. + */ + public static class DriveProfile { + + @NotNull + private final MotionProfile left; + @NotNull + private final MotionProfile right; + + private DriveProfile(@NotNull MotionProfile left, @NotNull MotionProfile right) { + this.left = left; + this.right = right; + } + + @NotNull + public MotionProfile getLeft() { + return left; + } + + @NotNull + public MotionProfile getRight() { + return right; + } + } +} From c711e5a0adc12fef49a6ad3a406d908c18b7d455 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Tue, 4 Dec 2018 19:59:27 -0800 Subject: [PATCH 15/51] Create ProfileContainerTestingRobot --- .../Deploy_ProfileContainerTestingRobot.xml | 21 +++++++++++++++++++ .../testing/ProfileContainerTestingRobot.kt | 18 ++++++++++++++++ 2 files changed, 39 insertions(+) create mode 100644 .idea/runConfigurations/Deploy_ProfileContainerTestingRobot.xml create mode 100644 test/src/main/kotlin/org/team1540/rooster/testing/ProfileContainerTestingRobot.kt diff --git a/.idea/runConfigurations/Deploy_ProfileContainerTestingRobot.xml b/.idea/runConfigurations/Deploy_ProfileContainerTestingRobot.xml new file mode 100644 index 00000000..d2ee725f --- /dev/null +++ b/.idea/runConfigurations/Deploy_ProfileContainerTestingRobot.xml @@ -0,0 +1,21 @@ + + + + + + + true + + + \ No newline at end of file diff --git a/test/src/main/kotlin/org/team1540/rooster/testing/ProfileContainerTestingRobot.kt b/test/src/main/kotlin/org/team1540/rooster/testing/ProfileContainerTestingRobot.kt new file mode 100644 index 00000000..a422f917 --- /dev/null +++ b/test/src/main/kotlin/org/team1540/rooster/testing/ProfileContainerTestingRobot.kt @@ -0,0 +1,18 @@ +package org.team1540.rooster.testing + +import edu.wpi.first.wpilibj.TimedRobot +import org.team1540.rooster.motionprofiling.ProfileContainer +import java.io.File +import kotlin.system.measureTimeMillis + +class ProfileContainerTestingRobot : TimedRobot() { + private lateinit var container: ProfileContainer + + override fun robotInit() { + val time = measureTimeMillis { + container = ProfileContainer(File("/home/lvuser/roostertest")) + } + println("Initialized profile container in $time ms") + println("Current profiles loaded: ${container.profileNames}") + } +} From 0dedf81f8918665414b3aaed8ca2fab9cae52a7e Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Thu, 6 Dec 2018 08:55:57 -0800 Subject: [PATCH 16/51] Move functional interfaces to a new package This is a case of breaking backcompat to make things "n i c e r," but eh. It's nicer. This can always be reverted if we hate it. --- .../java/org/team1540/rooster/drive/JoystickScaling.java | 2 +- .../drive/pipeline/AdvancedArcadeJoystickInput.java | 1 + .../org/team1540/rooster/drive/pipeline/CTREOutput.java | 1 + .../rooster/drive/pipeline/FeedForwardProcessor.java | 1 + .../org/team1540/rooster/drive/pipeline/PIDProcessor.java | 2 ++ .../org/team1540/rooster/drive/pipeline/ProfileInput.java | 3 ++- .../rooster/drive/pipeline/SimpleJoystickInput.java | 1 + .../rooster/drive/pipeline/TurningRatePIDProcessor.java | 1 + .../org/team1540/rooster/drive/pipeline/UnitScaler.java | 1 + .../org/team1540/rooster/drive/pipeline/package-info.java | 7 +++---- .../team1540/rooster/{util => functional}/Executable.java | 4 ++-- .../rooster/{drive/pipeline => functional}/Input.java | 6 ++---- .../rooster/{drive/pipeline => functional}/Output.java | 6 ++---- .../rooster/{drive/pipeline => functional}/Processor.java | 5 ++--- .../java/org/team1540/rooster/util/DashboardUtils.java | 1 + .../java/org/team1540/rooster/util/SimpleAsyncCommand.java | 1 + .../main/java/org/team1540/rooster/util/SimpleCommand.java | 1 + .../java/org/team1540/rooster/util/SimpleLoopCommand.java | 1 + .../team1540/rooster/drive/pipeline/PipelineExtensions.kt | 2 +- .../org/team1540/rooster/testing/DrivePipelineTesting.kt | 3 ++- .../org/team1540/rooster/testing/MotionProfileTesting.kt | 2 +- 21 files changed, 30 insertions(+), 22 deletions(-) rename lib/src/main/java/org/team1540/rooster/{util => functional}/Executable.java (51%) rename lib/src/main/java/org/team1540/rooster/{drive/pipeline => functional}/Input.java (86%) rename lib/src/main/java/org/team1540/rooster/{drive/pipeline => functional}/Output.java (86%) rename lib/src/main/java/org/team1540/rooster/{drive/pipeline => functional}/Processor.java (87%) diff --git a/lib/src/main/java/org/team1540/rooster/drive/JoystickScaling.java b/lib/src/main/java/org/team1540/rooster/drive/JoystickScaling.java index 345d1cac..d7cc44b0 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/JoystickScaling.java +++ b/lib/src/main/java/org/team1540/rooster/drive/JoystickScaling.java @@ -1,6 +1,6 @@ package org.team1540.rooster.drive; -import org.team1540.rooster.drive.pipeline.Processor; +import org.team1540.rooster.functional.Processor; @FunctionalInterface public interface JoystickScaling extends Processor { diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java index 2db11286..2f9d9099 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java @@ -4,6 +4,7 @@ import java.util.function.DoubleSupplier; import org.jetbrains.annotations.NotNull; import org.team1540.rooster.Utilities; +import org.team1540.rooster.functional.Input; /** * Modified arcade drive joystick input. diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/CTREOutput.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/CTREOutput.java index 49825fcd..1ceb9eb2 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/CTREOutput.java +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/CTREOutput.java @@ -6,6 +6,7 @@ import java.util.Objects; import org.jetbrains.annotations.Contract; import org.jetbrains.annotations.NotNull; +import org.team1540.rooster.functional.Output; /** * {@link Output} to pass drive commands to Talon SRX and Victor SPX motor controllers. For output diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/FeedForwardProcessor.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/FeedForwardProcessor.java index e2032e89..ef614c97 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/FeedForwardProcessor.java +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/FeedForwardProcessor.java @@ -3,6 +3,7 @@ import java.util.OptionalDouble; import org.jetbrains.annotations.Contract; import org.jetbrains.annotations.NotNull; +import org.team1540.rooster.functional.Processor; /** * {@link Processor} to apply an Oblarg-style diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/PIDProcessor.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/PIDProcessor.java index 2db9992e..307e428b 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/PIDProcessor.java +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/PIDProcessor.java @@ -1,5 +1,7 @@ package org.team1540.rooster.drive.pipeline; +import org.team1540.rooster.functional.Processor; + /** * Processor to execute a generic PID loop. * diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/ProfileInput.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/ProfileInput.java index 49267798..f09d40a0 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/ProfileInput.java +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/ProfileInput.java @@ -3,11 +3,12 @@ import edu.wpi.first.wpilibj.Timer; import java.util.OptionalDouble; import org.jetbrains.annotations.NotNull; +import org.team1540.rooster.functional.Input; import org.team1540.rooster.motionprofiling.MotionProfile; import org.team1540.rooster.motionprofiling.MotionProfile.Point; public class ProfileInput implements - org.team1540.rooster.drive.pipeline.Input { + Input { private MotionProfile left; private MotionProfile right; diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/SimpleJoystickInput.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/SimpleJoystickInput.java index 49c7558f..f2aa386f 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/SimpleJoystickInput.java +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/SimpleJoystickInput.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj.Joystick; import java.util.OptionalDouble; import org.team1540.rooster.Utilities; +import org.team1540.rooster.functional.Input; /** * Simple tank-style input from a WPILib {@link Joystick}. The left and right joysticks are used to diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/TurningRatePIDProcessor.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/TurningRatePIDProcessor.java index 8265f454..966e07f1 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/TurningRatePIDProcessor.java +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/TurningRatePIDProcessor.java @@ -4,6 +4,7 @@ import java.util.OptionalDouble; import java.util.function.DoubleSupplier; import org.jetbrains.annotations.NotNull; +import org.team1540.rooster.functional.Processor; /** * Modifies velocity set-points to keep a desired turning rate. This {@link Processor} accepts a diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/UnitScaler.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/UnitScaler.java index f9b10675..32afd06a 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/UnitScaler.java +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/UnitScaler.java @@ -1,6 +1,7 @@ package org.team1540.rooster.drive.pipeline; import java.util.OptionalDouble; +import org.team1540.rooster.functional.Processor; /** * Scales units by a desired factor. For details of the scaling, see {@link #apply(TankDriveData) diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/package-info.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/package-info.java index 22280472..3d01b5dc 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/package-info.java +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/package-info.java @@ -1,9 +1,8 @@ /** * Collection of useful inputs, processors, and outputs. Contains "stock" or "in-the-box" classes - * that implement the {@link org.team1540.rooster.drive.pipeline.Input Input}, {@link - * org.team1540.rooster.drive.pipeline.Processor Processor}, and {@link - * org.team1540.rooster.drive.pipeline.Output Output} interfaces (as well as those interfaces - * themselves). + * that implement the {@link org.team1540.rooster.functional.Input Input}, {@link + * org.team1540.rooster.functional.Processor Processor}, and {@link org.team1540.rooster.functional.Output + * Output} interfaces. * *

Unless otherwise noted, all methods, constructors, etc. in this package throw a {@link * java.lang.NullPointerException} if any parameters are {@code null}. diff --git a/lib/src/main/java/org/team1540/rooster/util/Executable.java b/lib/src/main/java/org/team1540/rooster/functional/Executable.java similarity index 51% rename from lib/src/main/java/org/team1540/rooster/util/Executable.java rename to lib/src/main/java/org/team1540/rooster/functional/Executable.java index 44e4f346..6a46fea2 100644 --- a/lib/src/main/java/org/team1540/rooster/util/Executable.java +++ b/lib/src/main/java/org/team1540/rooster/functional/Executable.java @@ -1,7 +1,7 @@ -package org.team1540.rooster.util; +package org.team1540.rooster.functional; /** - * Interface for actions that can be passed to a {@link SimpleCommand}. + * Functional interface for a function that takes no arguments and returns no output. */ @FunctionalInterface public interface Executable { diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/Input.java b/lib/src/main/java/org/team1540/rooster/functional/Input.java similarity index 86% rename from lib/src/main/java/org/team1540/rooster/drive/pipeline/Input.java rename to lib/src/main/java/org/team1540/rooster/functional/Input.java index 4b4547a7..f61ec5f4 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/Input.java +++ b/lib/src/main/java/org/team1540/rooster/functional/Input.java @@ -1,17 +1,15 @@ -package org.team1540.rooster.drive.pipeline; +package org.team1540.rooster.functional; import java.util.function.Consumer; import java.util.function.Function; import java.util.function.Supplier; -import org.team1540.rooster.util.Executable; /** * Extension of a {@link Supplier} adding additional composition methods. * * {@code Input} can be used exactly like {@link Supplier} (and library functions should not take * {@code Outputs} as method parameters, instead using {@link Supplier}). However, library functions - * pertaining to drive pipelines should return an {@code Input} where they would normally return a - * {@link Supplier}. + * should return an {@code Input} where they would normally return a {@link Supplier}. * * @param The type of the input. */ diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/Output.java b/lib/src/main/java/org/team1540/rooster/functional/Output.java similarity index 86% rename from lib/src/main/java/org/team1540/rooster/drive/pipeline/Output.java rename to lib/src/main/java/org/team1540/rooster/functional/Output.java index 2f9f67d3..73641cb6 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/Output.java +++ b/lib/src/main/java/org/team1540/rooster/functional/Output.java @@ -1,17 +1,15 @@ -package org.team1540.rooster.drive.pipeline; +package org.team1540.rooster.functional; import java.util.function.Consumer; import java.util.function.Function; import java.util.function.Supplier; -import org.team1540.rooster.util.Executable; /** * Extension of a {@link Consumer} adding an additional composition method. * * {@code Output} can be used exactly like {@link Consumer} (and library functions should not take * {@code Outputs} as method parameters, instead using {@link Consumer}). However, library functions - * pertaining to drive pipelines should return an {@code Output} where they would normally return a - * {@link Consumer}. + * should return an {@code Output} where they would normally return a {@link Consumer}. * * @param The type of the output. */ diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/Processor.java b/lib/src/main/java/org/team1540/rooster/functional/Processor.java similarity index 87% rename from lib/src/main/java/org/team1540/rooster/drive/pipeline/Processor.java rename to lib/src/main/java/org/team1540/rooster/functional/Processor.java index adc0e0a8..e5359c59 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/Processor.java +++ b/lib/src/main/java/org/team1540/rooster/functional/Processor.java @@ -1,4 +1,4 @@ -package org.team1540.rooster.drive.pipeline; +package org.team1540.rooster.functional; import java.util.function.Consumer; import java.util.function.Function; @@ -9,8 +9,7 @@ * * {@code Processor} can be used exactly like {@link Function} (and library functions should not * take {@code Processors} as method parameters, instead using {@link Function}). However, library - * functions pertaining to drive pipelines should return a {@code Processor} where they would - * normally return a {@link Function}. + * functions should return a {@code Processor} where they would normally return a {@link Function}. * * @param The type of the output. */ diff --git a/lib/src/main/java/org/team1540/rooster/util/DashboardUtils.java b/lib/src/main/java/org/team1540/rooster/util/DashboardUtils.java index 42b9563e..fdb67d0a 100644 --- a/lib/src/main/java/org/team1540/rooster/util/DashboardUtils.java +++ b/lib/src/main/java/org/team1540/rooster/util/DashboardUtils.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.Objects; import org.jetbrains.annotations.NotNull; +import org.team1540.rooster.functional.Executable; import org.team1540.rooster.preferencemanager.PreferenceManager; /** diff --git a/lib/src/main/java/org/team1540/rooster/util/SimpleAsyncCommand.java b/lib/src/main/java/org/team1540/rooster/util/SimpleAsyncCommand.java index 93fef841..8412021d 100644 --- a/lib/src/main/java/org/team1540/rooster/util/SimpleAsyncCommand.java +++ b/lib/src/main/java/org/team1540/rooster/util/SimpleAsyncCommand.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.Objects; import org.jetbrains.annotations.NotNull; +import org.team1540.rooster.functional.Executable; /** * A simple way to construct an {@link AsyncCommand}.

To create a {@code SimpleCommand} easily, diff --git a/lib/src/main/java/org/team1540/rooster/util/SimpleCommand.java b/lib/src/main/java/org/team1540/rooster/util/SimpleCommand.java index b9ce154a..af6981ac 100644 --- a/lib/src/main/java/org/team1540/rooster/util/SimpleCommand.java +++ b/lib/src/main/java/org/team1540/rooster/util/SimpleCommand.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.Objects; import org.jetbrains.annotations.NotNull; +import org.team1540.rooster.functional.Executable; /** * A simple way to construct an {@link InstantCommand}.

To create a {@code SimpleCommand} diff --git a/lib/src/main/java/org/team1540/rooster/util/SimpleLoopCommand.java b/lib/src/main/java/org/team1540/rooster/util/SimpleLoopCommand.java index 5f3f4e81..a236015e 100644 --- a/lib/src/main/java/org/team1540/rooster/util/SimpleLoopCommand.java +++ b/lib/src/main/java/org/team1540/rooster/util/SimpleLoopCommand.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.Objects; import org.jetbrains.annotations.NotNull; +import org.team1540.rooster.functional.Executable; /** * A simple way to construct a {@link Command} which executes every tick.

To create a {@code diff --git a/lib/src/main/kotlin/org/team1540/rooster/drive/pipeline/PipelineExtensions.kt b/lib/src/main/kotlin/org/team1540/rooster/drive/pipeline/PipelineExtensions.kt index 4c27ac01..f1cd7bd0 100644 --- a/lib/src/main/kotlin/org/team1540/rooster/drive/pipeline/PipelineExtensions.kt +++ b/lib/src/main/kotlin/org/team1540/rooster/drive/pipeline/PipelineExtensions.kt @@ -2,7 +2,7 @@ package org.team1540.rooster.drive.pipeline -import org.team1540.rooster.util.Executable +import org.team1540.rooster.functional.Executable import java.util.function.Consumer import java.util.function.Supplier diff --git a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt index 3915ba5d..7e294873 100644 --- a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt +++ b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt @@ -8,9 +8,10 @@ import edu.wpi.first.wpilibj.command.Scheduler import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard import org.team1540.rooster.Utilities import org.team1540.rooster.drive.pipeline.* +import org.team1540.rooster.functional.Executable +import org.team1540.rooster.functional.Input import org.team1540.rooster.preferencemanager.Preference import org.team1540.rooster.preferencemanager.PreferenceManager -import org.team1540.rooster.util.Executable import org.team1540.rooster.util.SimpleAsyncCommand import org.team1540.rooster.util.SimpleCommand import org.team1540.rooster.util.SimpleLoopCommand diff --git a/test/src/main/kotlin/org/team1540/rooster/testing/MotionProfileTesting.kt b/test/src/main/kotlin/org/team1540/rooster/testing/MotionProfileTesting.kt index 124ae6ac..55847000 100644 --- a/test/src/main/kotlin/org/team1540/rooster/testing/MotionProfileTesting.kt +++ b/test/src/main/kotlin/org/team1540/rooster/testing/MotionProfileTesting.kt @@ -14,12 +14,12 @@ import jaci.pathfinder.Trajectory import jaci.pathfinder.Waypoint import jaci.pathfinder.modifiers.TankModifier import org.team1540.rooster.Utilities +import org.team1540.rooster.functional.Executable import org.team1540.rooster.motionprofiling.FollowProfile import org.team1540.rooster.motionprofiling.FollowProfileFactory import org.team1540.rooster.motionprofiling.MotionProfileUtils import org.team1540.rooster.preferencemanager.Preference import org.team1540.rooster.preferencemanager.PreferenceManager -import org.team1540.rooster.util.Executable import org.team1540.rooster.util.SimpleLoopCommand import org.team1540.rooster.wrappers.ChickenTalon import java.util.function.DoubleSupplier From 134509e04091e34456747aeb8e57980f8e4d23e9 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Thu, 6 Dec 2018 09:17:38 -0800 Subject: [PATCH 17/51] Add utility function for looping through motors --- .../rooster/testing/DrivePipelineTesting.kt | 24 ++++++++++++++----- 1 file changed, 18 insertions(+), 6 deletions(-) diff --git a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt index 7e294873..e89c81c9 100644 --- a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt +++ b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt @@ -96,12 +96,12 @@ class AdvancedJoystickInputPipelineTestRobot : DrivePipelineTestRobot() { + UnitScaler(tpu, 0.1) + (CTREOutput(PipelineDriveTrain.left1, PipelineDriveTrain.right1))) - listOf(PipelineDriveTrain.left1, PipelineDriveTrain.right1).forEach { - it.configClosedloopRamp(ramp) - it.config_kP(0, p) - it.config_kI(0, i) - it.config_kD(0, d) - it.config_kF(0, 0.0) + PipelineDriveTrain.masters { + configClosedloopRamp(ramp) + config_kP(0, p) + config_kI(0, i) + config_kD(0, d) + config_kF(0, 0.0) } }).apply { @@ -295,6 +295,18 @@ private object PipelineDriveTrain { inverted = true set(ControlMode.Follower, right1.deviceID.toDouble()) } + + fun all(block: ChickenTalon.() -> Unit) { + for (talon in listOf(left1, left2, left3, right1, right2, right3)) { + talon.block() + } + } + + fun masters(block: ChickenTalon.() -> Unit) { + for (talon in listOf(left1, right1)) { + talon.block() + } + } } /** From b774fad927c6bff5fed245464b716eb5dc772efd Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Thu, 6 Dec 2018 09:25:33 -0800 Subject: [PATCH 18/51] Fix wrapping --- .../java/org/team1540/rooster/drive/pipeline/ProfileInput.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/ProfileInput.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/ProfileInput.java index f09d40a0..61ac942d 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/ProfileInput.java +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/ProfileInput.java @@ -7,8 +7,7 @@ import org.team1540.rooster.motionprofiling.MotionProfile; import org.team1540.rooster.motionprofiling.MotionProfile.Point; -public class ProfileInput implements - Input { +public class ProfileInput implements Input { private MotionProfile left; private MotionProfile right; From 73c33f1f2bec194b9b588d1a8bd3197eb78166fb Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Thu, 6 Dec 2018 11:45:18 -0800 Subject: [PATCH 19/51] Create HeadingTransformProcessor --- .../pipeline/HeadingTransformProcessor.java | 59 +++++++++++++++++++ 1 file changed, 59 insertions(+) create mode 100644 lib/src/main/java/org/team1540/rooster/drive/pipeline/HeadingTransformProcessor.java diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/HeadingTransformProcessor.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/HeadingTransformProcessor.java new file mode 100644 index 00000000..e3c2296d --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/HeadingTransformProcessor.java @@ -0,0 +1,59 @@ +package org.team1540.rooster.drive.pipeline; + +import java.util.OptionalDouble; +import org.team1540.rooster.functional.Processor; + +/** + * {@link Processor} to convert headings on [0,2π] to [-π, π] and vice versa. (This class + * also supports degrees by passing {@code true} as the second argument to {@link + * #HeadingTransformProcessor(boolean, boolean)}. + * + * This class is a {@link Processor} that uses {@link TankDriveData}. All fields are passed through + * as-is except for the heading field which is converted if it is present. + */ +public class HeadingTransformProcessor implements Processor { + + private boolean outputPositive; + private boolean radians; + + /** + * Create a new {@code HeadingTransformProcessor} that uses radians. + * + * @param outputPositive Whether to output values in the range [0,2π] (i.e. [0,360]). If {@code + * true}, creates a {@code HeadingTransformProcessor} to convert angles on [-π, π] to angles + * on [0,2π]; if {@code false}, creates one that does the reverse. + */ + public HeadingTransformProcessor(boolean outputPositive) { + this(outputPositive, true); + } + + /** + * Create a new {@code HeadingTransformProcessor}. + * + * @param outputPositive Whether to output values in the range [0,2π] (or [0,360]). If {@code + * true}, creates a {@code HeadingTransformProcessor} to convert angles on [-π, π] to angles + * on [0,2π]; if {@code false}, creates one that does the reverse. + * @param radians Whether to use radians. If {@code true}, assumes inputs are in radians; + * otherwise, uses degrees. + */ + public HeadingTransformProcessor(boolean outputPositive, boolean radians) { + this.outputPositive = outputPositive; + this.radians = radians; + } + + @Override + public TankDriveData apply(TankDriveData data) { + if (data.heading.isPresent()) { + double heading = data.heading.getAsDouble(); + double halfOfCircle = radians ? Math.PI : 180; + double processed = heading + ((outputPositive ? 1 : -1) * halfOfCircle); + return new TankDriveData( + data.left, + data.right, + OptionalDouble.of(processed), + data.turningRate); + } else { + return data; + } + } +} From d1b89e4fa45b8ab81b8be131049020152afd2729 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Thu, 6 Dec 2018 11:49:55 -0800 Subject: [PATCH 20/51] Create MotionProfilePipelineTestRobot --- .../rooster/testing/DrivePipelineTesting.kt | 121 ++++++++++++++++++ 1 file changed, 121 insertions(+) diff --git a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt index e89c81c9..8ccd4a2b 100644 --- a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt +++ b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt @@ -10,12 +10,14 @@ import org.team1540.rooster.Utilities import org.team1540.rooster.drive.pipeline.* import org.team1540.rooster.functional.Executable import org.team1540.rooster.functional.Input +import org.team1540.rooster.motionprofiling.ProfileContainer import org.team1540.rooster.preferencemanager.Preference import org.team1540.rooster.preferencemanager.PreferenceManager import org.team1540.rooster.util.SimpleAsyncCommand import org.team1540.rooster.util.SimpleCommand import org.team1540.rooster.util.SimpleLoopCommand import org.team1540.rooster.wrappers.ChickenTalon +import java.io.File import java.util.OptionalDouble import java.util.function.DoubleSupplier @@ -228,6 +230,125 @@ class TurningRatePIDPipelineTestRobot : DrivePipelineTestRobot() { override val command get() = _command } +class MotionProfilePipelineTestRobot : DrivePipelineTestRobot() { + @JvmField + @Preference(persistent = false) + var hdgP = 0.0 + @JvmField + @Preference(persistent = false) + var hdgI = 0.0 + @JvmField + @Preference(persistent = false) + var hdgD = 0.0 + @JvmField + @Preference(persistent = false) + var driveP = 0.0 + @JvmField + @Preference(persistent = false) + var driveI = 0.0 + @JvmField + @Preference(persistent = false) + var driveD = 0.0 + @JvmField + @Preference(persistent = false) + var invertSides = false + @JvmField + @Preference(persistent = false) + var kV = 0.0 + @JvmField + @Preference(persistent = false) + var kA = 0.0 + @JvmField + @Preference(persistent = false) + var vIntercept = 0.0 + @JvmField + @Preference(persistent = false) + var profileName = "" + @JvmField + @Preference(persistent = false) + var tpu = 1.0 + @JvmField + @Preference(persistent = false) + var ramp = 0.0; + + private var hdgPIDProcessor: HeadingPIDProcessor? = null + private var profileInput: ProfileInput? = null + private var hdgTgt = 0.0 + + override fun robotInit() { + PreferenceManager.getInstance().add(this) + val reset = SimpleCommand("reset", Executable { + val container = ProfileContainer(File("/home/lvuser/roostertest")) + println("Loaded profiles: ${container.profileNames}") + + val profile = container[profileName] + if (profile == null) { + System.err.println("Motion profile not found, aborting reset") + return@Executable + } + + PipelineNavx.navx.zeroYaw() + + profileInput = ProfileInput(profile.left, profile.right) + + hdgPIDProcessor = HeadingPIDProcessor(hdgP, hdgI, hdgD, { Math.toRadians(PipelineNavx.navx.yaw.toDouble()) }, true, invertSides) + _command = SimpleAsyncCommand("Drive", 20, profileInput!! + + FeedForwardProcessor(kV, vIntercept, kA) + + HeadingTransformProcessor(false) // pathfinder motprofs have headings from 0 to 2pi and we need to convert those + + hdgPIDProcessor!! + + UnitScaler(tpu, 0.1) + + CTREOutput(PipelineDriveTrain.left1, PipelineDriveTrain.right1) + ) + + PipelineDriveTrain.masters { + config_kP(0, driveP) + config_kI(0, driveI) + config_kD(0, driveD) + config_kF(0, 0.0) + } + PipelineDriveTrain.all { + configClosedloopRamp(ramp) + } + + }).apply { + setRunWhenDisabled(true) + start() + } + + SmartDashboard.putData(reset) + } + + private lateinit var _command: Command + override val command get() = _command + + override fun robotPeriodic() { + super.robotPeriodic() + + hdgPIDProcessor?.let { + SmartDashboard.putNumber("HDG ERR", Math.toDegrees(it.error)) + SmartDashboard.putNumber("HDG IACC", it.iAccum) + } + + profileInput?.get()?.let { + SmartDashboard.putNumber("LPOSTGT", it.left.position.orElse(0.0)) + SmartDashboard.putNumber("LVELTGT", it.left.velocity.orElse(0.0)) + SmartDashboard.putNumber("LACCTGT", it.left.acceleration.orElse(0.0)) + SmartDashboard.putNumber("RPOSTGT", it.right.position.orElse(0.0)) + SmartDashboard.putNumber("RVELTGT", it.right.velocity.orElse(0.0)) + SmartDashboard.putNumber("RACCTGT", it.right.acceleration.orElse(0.0)) + SmartDashboard.putNumber("HTGT", Math.toDegrees(hdgTgt)) + } + + SmartDashboard.putNumber("HDG", PipelineNavx.navx.yaw.toDouble()) + SmartDashboard.putNumber("LPOS", PipelineDriveTrain.left1.selectedSensorPosition / tpu) + SmartDashboard.putNumber("RPOS", PipelineDriveTrain.right1.selectedSensorPosition / tpu) + SmartDashboard.putNumber("LVEL", (PipelineDriveTrain.left1.selectedSensorVelocity * 10) / tpu) + SmartDashboard.putNumber("RVEL", (PipelineDriveTrain.right1.selectedSensorVelocity * 10) / tpu) + SmartDashboard.putNumber("LERR", PipelineDriveTrain.left1.closedLoopError / tpu) + SmartDashboard.putNumber("RERR", PipelineDriveTrain.right1.closedLoopError / tpu) + } +} + /** * Common drive train object to be used by all pipeline test robots. */ From 00040c7245699f102de0c366e4db1d3ebd5742f0 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Thu, 6 Dec 2018 12:07:36 -0800 Subject: [PATCH 21/51] Allow deployTestFiles to be used in continuous mode --- test/build.gradle | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/test/build.gradle b/test/build.gradle index ca155f13..f0e06e1c 100644 --- a/test/build.gradle +++ b/test/build.gradle @@ -53,6 +53,13 @@ deploy { } } +// add the deploy directory as an input; now we can run this in continuous mode and files are +// deployed as soon as they change +// (to run in continuous mode add "--continuous" to the command line, i.e. "deployTestFiles --continuous") +deployTestFiles { + inputs.dir("deploy") +} + jar { // declare the robot class property as an input, as normally since we // set it at runtime this wouldn't run if the class changed due to up-to-date checking From b287565cecd12d45a025c6d0fcbf84f856f51e05 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Thu, 6 Dec 2018 12:10:12 -0800 Subject: [PATCH 22/51] Remove a semicolon --- .../kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt index 8ccd4a2b..e87011ee 100644 --- a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt +++ b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt @@ -269,7 +269,7 @@ class MotionProfilePipelineTestRobot : DrivePipelineTestRobot() { var tpu = 1.0 @JvmField @Preference(persistent = false) - var ramp = 0.0; + var ramp = 0.0 private var hdgPIDProcessor: HeadingPIDProcessor? = null private var profileInput: ProfileInput? = null From 7dbc9d6c77b9d95d611a66b19342202e2ef6394f Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Thu, 6 Dec 2018 12:22:10 -0800 Subject: [PATCH 23/51] Use a better method to get current targets Now we use a Processor to grab the output of the ProfileInput straight out of the pipeline. Whee, flexibility! --- .../org/team1540/rooster/testing/DrivePipelineTesting.kt | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt index e87011ee..2d68c913 100644 --- a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt +++ b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt @@ -10,6 +10,7 @@ import org.team1540.rooster.Utilities import org.team1540.rooster.drive.pipeline.* import org.team1540.rooster.functional.Executable import org.team1540.rooster.functional.Input +import org.team1540.rooster.functional.Processor import org.team1540.rooster.motionprofiling.ProfileContainer import org.team1540.rooster.preferencemanager.Preference import org.team1540.rooster.preferencemanager.PreferenceManager @@ -274,6 +275,7 @@ class MotionProfilePipelineTestRobot : DrivePipelineTestRobot() { private var hdgPIDProcessor: HeadingPIDProcessor? = null private var profileInput: ProfileInput? = null private var hdgTgt = 0.0 + private var lastProf: TankDriveData? = null override fun robotInit() { PreferenceManager.getInstance().add(this) @@ -293,6 +295,7 @@ class MotionProfilePipelineTestRobot : DrivePipelineTestRobot() { hdgPIDProcessor = HeadingPIDProcessor(hdgP, hdgI, hdgD, { Math.toRadians(PipelineNavx.navx.yaw.toDouble()) }, true, invertSides) _command = SimpleAsyncCommand("Drive", 20, profileInput!! + + Processor { lastProf = it; it } + FeedForwardProcessor(kV, vIntercept, kA) + HeadingTransformProcessor(false) // pathfinder motprofs have headings from 0 to 2pi and we need to convert those + hdgPIDProcessor!! @@ -329,7 +332,7 @@ class MotionProfilePipelineTestRobot : DrivePipelineTestRobot() { SmartDashboard.putNumber("HDG IACC", it.iAccum) } - profileInput?.get()?.let { + lastProf?.let { SmartDashboard.putNumber("LPOSTGT", it.left.position.orElse(0.0)) SmartDashboard.putNumber("LVELTGT", it.left.velocity.orElse(0.0)) SmartDashboard.putNumber("LACCTGT", it.left.acceleration.orElse(0.0)) From 87f7b9fd3b7640ede8c8ba4289d750f636b28989 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Thu, 6 Dec 2018 16:33:54 -0800 Subject: [PATCH 24/51] Properly apply WheelbaseTestRobot inversions --- .../team1540/rooster/util/robots/WheelbaseTestRobot.java | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/lib/src/main/java/org/team1540/rooster/util/robots/WheelbaseTestRobot.java b/lib/src/main/java/org/team1540/rooster/util/robots/WheelbaseTestRobot.java index 77e4d022..484d7bec 100644 --- a/lib/src/main/java/org/team1540/rooster/util/robots/WheelbaseTestRobot.java +++ b/lib/src/main/java/org/team1540/rooster/util/robots/WheelbaseTestRobot.java @@ -68,6 +68,7 @@ public void robotInit() { Command reset = new SimpleCommand("Reset", () -> { if (lMotor1ID != -1) { lMotor1 = new ChickenTalon(lMotor1ID); + lMotor1.setSensorPhase(invertLeftSensor); } else { System.err.println("Left Motor 1 must be set!"); return; @@ -93,6 +94,7 @@ public void robotInit() { if (rMotor1ID != -1) { rMotor1 = new ChickenTalon(rMotor1ID); + rMotor1.setSensorPhase(invertRightSensor); } else { System.err.println("Right Motor 1 must be set!"); return; @@ -126,6 +128,13 @@ public void robotInit() { motor.setBrake(brake); } } + + for (ChickenTalon motor : new ChickenTalon[]{lMotor1, lMotor2, lMotor3}) { + motor.setInverted(invertLeftMotor); + } + for (ChickenTalon motor : new ChickenTalon[]{rMotor1, rMotor2, rMotor3}) { + motor.setInverted(invertRightMotor); + } }); reset.setRunWhenDisabled(true); reset.start(); From 92dcc8fba5af42b80e06210acfb1e17d073b6066 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Thu, 6 Dec 2018 20:05:32 -0800 Subject: [PATCH 25/51] Zero sensors on teleop init instead of on reset --- .../org/team1540/rooster/testing/DrivePipelineTesting.kt | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt index 2d68c913..9f10fabf 100644 --- a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt +++ b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt @@ -289,8 +289,6 @@ class MotionProfilePipelineTestRobot : DrivePipelineTestRobot() { return@Executable } - PipelineNavx.navx.zeroYaw() - profileInput = ProfileInput(profile.left, profile.right) hdgPIDProcessor = HeadingPIDProcessor(hdgP, hdgI, hdgD, { Math.toRadians(PipelineNavx.navx.yaw.toDouble()) }, true, invertSides) @@ -350,6 +348,13 @@ class MotionProfilePipelineTestRobot : DrivePipelineTestRobot() { SmartDashboard.putNumber("LERR", PipelineDriveTrain.left1.closedLoopError / tpu) SmartDashboard.putNumber("RERR", PipelineDriveTrain.right1.closedLoopError / tpu) } + + override fun teleopInit() { + super.teleopInit() + + PipelineDriveTrain.masters { setSelectedSensorPosition(0) } + PipelineNavx.navx.zeroYaw() + } } /** From 4afa25c9f6b51afeaa124318f12b199db3c6d6d0 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Thu, 6 Dec 2018 20:05:53 -0800 Subject: [PATCH 26/51] Collect data after heading processing --- .../kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt index 9f10fabf..4eb60284 100644 --- a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt +++ b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt @@ -293,10 +293,10 @@ class MotionProfilePipelineTestRobot : DrivePipelineTestRobot() { hdgPIDProcessor = HeadingPIDProcessor(hdgP, hdgI, hdgD, { Math.toRadians(PipelineNavx.navx.yaw.toDouble()) }, true, invertSides) _command = SimpleAsyncCommand("Drive", 20, profileInput!! - + Processor { lastProf = it; it } + FeedForwardProcessor(kV, vIntercept, kA) + HeadingTransformProcessor(false) // pathfinder motprofs have headings from 0 to 2pi and we need to convert those + hdgPIDProcessor!! + + Processor { lastProf = it; hdgTgt = it.heading.asDouble;it } + UnitScaler(tpu, 0.1) + CTREOutput(PipelineDriveTrain.left1, PipelineDriveTrain.right1) ) From 5598a9c25fb3bffa4741d8e65c486c53a66d944a Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Thu, 6 Dec 2018 20:06:37 -0800 Subject: [PATCH 27/51] Don't transform the heading Apparently pathfinder actually is -pi to pi, or something is just funky. Either way, stuff lines up without the transformprocessor. --- .../kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt | 1 - 1 file changed, 1 deletion(-) diff --git a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt index 4eb60284..f16a6bb6 100644 --- a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt +++ b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt @@ -294,7 +294,6 @@ class MotionProfilePipelineTestRobot : DrivePipelineTestRobot() { hdgPIDProcessor = HeadingPIDProcessor(hdgP, hdgI, hdgD, { Math.toRadians(PipelineNavx.navx.yaw.toDouble()) }, true, invertSides) _command = SimpleAsyncCommand("Drive", 20, profileInput!! + FeedForwardProcessor(kV, vIntercept, kA) - + HeadingTransformProcessor(false) // pathfinder motprofs have headings from 0 to 2pi and we need to convert those + hdgPIDProcessor!! + Processor { lastProf = it; hdgTgt = it.heading.asDouble;it } + UnitScaler(tpu, 0.1) From 4002d7b6bc68e0ceaaa6fac6e50ba56851242e74 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Fri, 7 Dec 2018 21:33:06 -0800 Subject: [PATCH 28/51] Zero sensors in reset() --- .../org/team1540/rooster/testing/DrivePipelineTesting.kt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt index f16a6bb6..be16cf2b 100644 --- a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt +++ b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt @@ -291,6 +291,9 @@ class MotionProfilePipelineTestRobot : DrivePipelineTestRobot() { profileInput = ProfileInput(profile.left, profile.right) + PipelineDriveTrain.masters { setSelectedSensorPosition(0) } + PipelineNavx.navx.zeroYaw() + hdgPIDProcessor = HeadingPIDProcessor(hdgP, hdgI, hdgD, { Math.toRadians(PipelineNavx.navx.yaw.toDouble()) }, true, invertSides) _command = SimpleAsyncCommand("Drive", 20, profileInput!! + FeedForwardProcessor(kV, vIntercept, kA) From 7a6e1cd734f6edf51514ef8df126ea308d94ffa9 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Fri, 7 Dec 2018 21:46:24 -0800 Subject: [PATCH 29/51] Add customizable suffixes to ProfileContainer --- .../motionprofiling/ProfileContainer.java | 33 +++++++++++++++---- 1 file changed, 26 insertions(+), 7 deletions(-) diff --git a/lib/src/main/java/org/team1540/rooster/motionprofiling/ProfileContainer.java b/lib/src/main/java/org/team1540/rooster/motionprofiling/ProfileContainer.java index 086f942a..b0359599 100644 --- a/lib/src/main/java/org/team1540/rooster/motionprofiling/ProfileContainer.java +++ b/lib/src/main/java/org/team1540/rooster/motionprofiling/ProfileContainer.java @@ -39,6 +39,22 @@ public class ProfileContainer { @NotNull private Map profiles; + /** + * Creates a new {@code ProfileContainer} that searches the provided directory using a left suffix + * of "{@code _left.csv}" and a right suffix of "{@code _right.csv}" + * + * This constructor also searches the provided directory for profiles and loads all the profiles + * into RAM. For this reason, initialization may take some time (especially for large amounts of + * profiles). + * + * @param profileDirectory The directory containing the profiles. See the {@linkplain + * ProfileContainer class documentation} for a description of the folder structure. + * @throws RuntimeException If an I/O error occurs during profile loading. + */ + public ProfileContainer(@NotNull File profileDirectory) { + this(profileDirectory, "_left.csv", "_right.csv"); + } + /** * Creates a new {@code ProfileContainer}. This constructor also searches the provided directory * for profiles and loads all the profiles into RAM. For this reason, initialization may take some @@ -46,15 +62,18 @@ public class ProfileContainer { * * @param profileDirectory The directory containing the profiles. See the {@linkplain * ProfileContainer class documentation} for a description of the folder structure. + * @param leftSuffix The suffix to use to identify left-side profile files. + * @param rightSuffix The suffix to use to identify right-side profile files. * @throws RuntimeException If an I/O error occurs during profile loading. */ - public ProfileContainer(@NotNull File profileDirectory) { + public ProfileContainer(@NotNull File profileDirectory, @NotNull String leftSuffix, + @NotNull String rightSuffix) { if (!profileDirectory.isDirectory()) { throw new IllegalArgumentException("Not a directory"); } - File[] lFiles = profileDirectory.listFiles((file) -> file.getName().endsWith("_left.csv")); - File[] rFiles = profileDirectory.listFiles((file) -> file.getName().endsWith("_right.csv")); + File[] lFiles = profileDirectory.listFiles((file) -> file.getName().endsWith(leftSuffix)); + File[] rFiles = profileDirectory.listFiles((file) -> file.getName().endsWith(rightSuffix)); if (lFiles == null || rFiles == null) { // according to listFiles() docs, it will only return null if the file isn't a directory @@ -67,11 +86,11 @@ public ProfileContainer(@NotNull File profileDirectory) { Set profileNames = new HashSet<>(); for (File f : lFiles) { - profileNames.add(f.getName().substring(0, f.getName().length() - "_left.csv".length())); + profileNames.add(f.getName().substring(0, f.getName().length() - leftSuffix.length())); } for (File f : rFiles) { - profileNames.add(f.getName().substring(0, f.getName().length() - "_right.csv".length())); + profileNames.add(f.getName().substring(0, f.getName().length() - rightSuffix.length())); } // initialize the map once we know the number of profiles so it doesn't expand. @@ -82,7 +101,7 @@ public ProfileContainer(@NotNull File profileDirectory) { for (String name : profileNames) { System.out.println("Loading profile " + name); File leftFile = Arrays.stream(lFiles) - .filter(file -> file.getName().equals(name + "_left.csv")) + .filter(file -> file.getName().equals(name + leftSuffix)) .findFirst() .orElseGet(() -> { DriverStation @@ -90,7 +109,7 @@ public ProfileContainer(@NotNull File profileDirectory) { return null; }); File rightFile = Arrays.stream(rFiles) - .filter(file -> file.getName().equals(name + "_right.csv")) + .filter(file -> file.getName().equals(name + rightSuffix)) .findFirst() .orElseGet(() -> { DriverStation From 2d0d4b32d888a92ecc62bcfa027565918de25377 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Sun, 9 Dec 2018 19:40:24 -0800 Subject: [PATCH 30/51] Add hashCode() and equals() for data classes --- .../rooster/drive/pipeline/DriveData.java | 22 +++++++++++++++++++ .../rooster/drive/pipeline/TankDriveData.java | 22 +++++++++++++++++++ 2 files changed, 44 insertions(+) diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/DriveData.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/DriveData.java index 68367ebc..0734d7b8 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/DriveData.java +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/DriveData.java @@ -2,6 +2,7 @@ import java.util.Objects; import java.util.OptionalDouble; +import org.jetbrains.annotations.Contract; import org.jetbrains.annotations.NotNull; /** @@ -82,4 +83,25 @@ public String toString() { + (additionalFeedForward.isPresent() ? ", feedforward " + additionalFeedForward .getAsDouble() : ""); } + + @Override + @Contract(value = "null -> false", pure = true) + public boolean equals(Object o) { + if (this == o) { + return true; + } + if (!(o instanceof DriveData)) { + return false; + } + DriveData driveData = (DriveData) o; + return position.equals(driveData.position) && + velocity.equals(driveData.velocity) && + acceleration.equals(driveData.acceleration) && + additionalFeedForward.equals(driveData.additionalFeedForward); + } + + @Override + public int hashCode() { + return Objects.hash(position, velocity, acceleration, additionalFeedForward); + } } diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/TankDriveData.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/TankDriveData.java index ee169a1c..4e474074 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/TankDriveData.java +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/TankDriveData.java @@ -2,6 +2,7 @@ import java.util.Objects; import java.util.OptionalDouble; +import org.jetbrains.annotations.Contract; import org.jetbrains.annotations.NotNull; /** @@ -65,4 +66,25 @@ public String toString() { + (heading.isPresent() ? ", heading " + heading.getAsDouble() : "") + (turningRate.isPresent() ? ", turning rate " + turningRate.getAsDouble() : ""); } + + @Override + @Contract(value = "null -> false", pure = true) + public boolean equals(Object o) { + if (this == o) { + return true; + } + if (!(o instanceof TankDriveData)) { + return false; + } + TankDriveData that = (TankDriveData) o; + return left.equals(that.left) && + right.equals(that.right) && + heading.equals(that.heading) && + turningRate.equals(that.turningRate); + } + + @Override + public int hashCode() { + return Objects.hash(left, right, heading, turningRate); + } } From 1ac33b147e9157469cf32e2ecb9d2bd3a5e635b6 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 10 Dec 2018 16:16:40 -0800 Subject: [PATCH 31/51] Turn off brake mode on disable --- .../rooster/testing/DrivePipelineTesting.kt | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt index be16cf2b..5ec5df96 100644 --- a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt +++ b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt @@ -277,6 +277,8 @@ class MotionProfilePipelineTestRobot : DrivePipelineTestRobot() { private var hdgTgt = 0.0 private var lastProf: TankDriveData? = null + private var notifier: Notifier? = null + override fun robotInit() { PreferenceManager.getInstance().add(this) val reset = SimpleCommand("reset", Executable { @@ -356,6 +358,21 @@ class MotionProfilePipelineTestRobot : DrivePipelineTestRobot() { PipelineDriveTrain.masters { setSelectedSensorPosition(0) } PipelineNavx.navx.zeroYaw() + + PipelineDriveTrain.all { + setBrake(true) + } + } + + override fun disabledInit() { + super.disabledInit() + notifier = Notifier { + Thread.sleep(1000) + PipelineDriveTrain.all { + setBrake(false) + } + } + notifier?.startSingle(1.0) } } From 62262daf0fc5127a1704016213239112833ec3b4 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 10 Dec 2018 16:50:41 -0800 Subject: [PATCH 32/51] Make data class manipulation more ergonomic --- .../rooster/drive/pipeline/DriveData.java | 185 ++++++ .../rooster/drive/pipeline/TankDriveData.java | 614 ++++++++++++++++++ 2 files changed, 799 insertions(+) diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/DriveData.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/DriveData.java index 0734d7b8..b221ee54 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/DriveData.java +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/DriveData.java @@ -2,6 +2,7 @@ import java.util.Objects; import java.util.OptionalDouble; +import java.util.function.Function; import org.jetbrains.annotations.Contract; import org.jetbrains.annotations.NotNull; @@ -44,6 +45,14 @@ public class DriveData { @NotNull public final OptionalDouble additionalFeedForward; + /** + * Create a new {@code DriveData} with all fields empty. + */ + public DriveData() { + this(OptionalDouble.empty(), OptionalDouble.empty(), OptionalDouble.empty(), + OptionalDouble.empty()); + } + /** * Create a new {@code DriveData} with all fields empty except for the provided velocity. * @@ -75,6 +84,182 @@ public DriveData(@NotNull OptionalDouble position, @NotNull OptionalDouble veloc this.additionalFeedForward = Objects.requireNonNull(additionalFeedForward); } + /** + * Creates a copy of this {@code DriveData} with a different {@link #position} value (all other + * fields remain the same). + * + * @param position The new value for {@link #position}. + * @return A new {@code DriveData} as described above. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public DriveData withPosition(double position) { + return new DriveData(OptionalDouble.of(position), velocity, acceleration, + additionalFeedForward); + } + + /** + * Creates a copy of this {@code DriveData} with a different {@link #velocity} value (all other + * fields remain the same). + * + * @param velocity The new value for {@link #velocity}. + * @return A new {@code DriveData} as described above. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public DriveData withVelocity(double velocity) { + return new DriveData(position, OptionalDouble.of(velocity), acceleration, + additionalFeedForward); + } + + /** + * Creates a copy of this {@code DriveData} with a different {@link #acceleration} value (all + * other fields remain the same). + * + * @param acceleration The new value for {@link #acceleration}. + * @return A new {@code DriveData} as described above. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public DriveData withAcceleration(double acceleration) { + return new DriveData(position, velocity, OptionalDouble.of(acceleration), + additionalFeedForward); + } + + /** + * Creates a copy of this {@code DriveData} with a different {@link #additionalFeedForward} value + * (all other fields remain the same). + * + * @param additionalFeedForward The new value for {@link #additionalFeedForward}. + * @return A new {@code DriveData} as described above. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public DriveData withAdditionalFeedForward(double additionalFeedForward) { + return new DriveData(position, velocity, acceleration, + OptionalDouble.of(additionalFeedForward)); + } + + /** + * Creates a copy of this {@code DriveData} with a modified {@link #position}. + * + * @param function A {@link Function} that takes this {@code DriveData}'s current {@link + * #position} and returns a new {@link #position}. + * @return A copy of this {@code DriveData} with all fields identical except for the {@link + * #position} field. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public DriveData modifyPosition(@NotNull Function function) { + return new DriveData(function.apply(position), velocity, acceleration, additionalFeedForward); + } + + /** + * Creates a copy of this {@code DriveData} with a modified {@link #velocity}. + * + * @param function A {@link Function} that takes this {@code DriveData}'s current {@link + * #velocity} and returns a new {@link #velocity}. + * @return A copy of this {@code DriveData} with all fields identical except for the {@link + * #velocity} field. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public DriveData modifyVelocity(@NotNull Function function) { + return new DriveData(position, function.apply(velocity), acceleration, additionalFeedForward); + } + + /** + * Creates a copy of this {@code DriveData} with a modified {@link #acceleration}. + * + * @param function A {@link Function} that takes this {@code DriveData}'s current {@link + * #acceleration} and returns a new {@link #acceleration}. + * @return A copy of this {@code DriveData} with all fields identical except for the {@link + * #acceleration} field. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public DriveData modifyAcceleration(@NotNull Function function) { + return new DriveData(position, velocity, function.apply(acceleration), additionalFeedForward); + } + + /** + * Creates a copy of this {@code DriveData} with a modified {@link #additionalFeedForward}. + * + * @param function A {@link Function} that takes this {@code DriveData}'s current {@link + * #additionalFeedForward} and returns a new {@link #additionalFeedForward}. + * @return A copy of this {@code DriveData} with all fields identical except for the {@link + * #additionalFeedForward} field. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public DriveData modifyAdditionalFeedForward( + @NotNull Function function) { + return new DriveData(position, velocity, acceleration, function.apply(additionalFeedForward)); + } + + /** + * Adds the provided value to this {@code DriveData}'s {@link #position}. If there is already a + * value present in {@link #position}, the returned {@code DriveData}'s {@link #position} will be + * equal to the sum of that value plus the parameter; otherwise, the {@link #position} will be + * equal to the value of the provided position parameter. + * + * @param position The position value to add. + * @return A new {@link DriveData} as described above. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public DriveData plusPosition(double position) { + return modifyPosition((old) -> OptionalDouble.of(old.orElse(0) + position)); + } + + + /** + * Adds the provided value to this {@code DriveData}'s {@link #velocity}. If there is already a + * value present in {@link #velocity}, the returned {@code DriveData}'s {@link #velocity} will be + * equal to the sum of that value plus the parameter; otherwise, the {@link #velocity} will be + * equal to the value of the provided velocity parameter. + * + * @param velocity The velocity value to add. + * @return A new {@link DriveData} as described above. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public DriveData plusVelocity(double velocity) { + return modifyVelocity((old) -> OptionalDouble.of(old.orElse(0) + velocity)); + } + + /** + * Adds the provided value to this {@code DriveData}'s {@link #acceleration}. If there is already + * a value present in {@link #acceleration}, the returned {@code DriveData}'s {@link + * #acceleration} will be equal to the sum of that value plus the parameter; otherwise, the {@link + * #acceleration} will be equal to the value of the provided acceleration parameter. + * + * @param acceleration The acceleration value to add. + * @return A new {@link DriveData} as described above. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public DriveData plusAcceleration(double acceleration) { + return modifyAcceleration((old) -> OptionalDouble.of(old.orElse(0) + acceleration)); + } + + /** + * Adds the provided value to this {@code DriveData}'s {@link #additionalFeedForward}. If there is + * already a value present in {@link #additionalFeedForward}, the returned {@code DriveData}'s + * {@link #additionalFeedForward} will be equal to the sum of that value plus the parameter; + * otherwise, the {@link #additionalFeedForward} will be equal to the value of the provided + * additionalFeedForward parameter. + * + * @param additionalFeedForward The additionalFeedForward value to add. + * @return A new {@link DriveData} as described above. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public DriveData plusAdditionalFeedForward(double additionalFeedForward) { + return modifyAdditionalFeedForward( + (old) -> OptionalDouble.of(old.orElse(0) + additionalFeedForward)); + } + @Override public String toString() { return (position.isPresent() ? "position " + position.getAsDouble() : "") diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/TankDriveData.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/TankDriveData.java index 4e474074..35174659 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/TankDriveData.java +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/TankDriveData.java @@ -2,6 +2,7 @@ import java.util.Objects; import java.util.OptionalDouble; +import java.util.function.Function; import org.jetbrains.annotations.Contract; import org.jetbrains.annotations.NotNull; @@ -40,6 +41,619 @@ public class TankDriveData { @NotNull public final OptionalDouble turningRate; + /** + * Creates a new {@code TankDriveData} with all fields empty. + */ + public TankDriveData() { + this(new DriveData(), new DriveData(), OptionalDouble.empty(), OptionalDouble.empty()); + } + + /** + * Creates a copy of this {@code TankDriveData} with a modified {@link #heading}. + * + * @param function A {@link Function} that takes this {@code DriveData}'s current {@link #heading} + * and returns a new {@link #heading}. + * @return A copy of this {@code TankDriveData} with all fields identical except for the {@link + * #heading} field. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public TankDriveData modifyHeading(@NotNull Function function) { + return new TankDriveData(left, right, function.apply(heading), turningRate); + } + + /** + * Creates a copy of this {@code TankDriveData} with a modified {@link #turningRate}. + * + * @param function A {@link Function} that takes this {@code DriveData}'s current {@link + * #turningRate} and returns a new {@link #turningRate}. + * @return A copy of this {@code TankDriveData} with all fields identical except for the {@link + * #turningRate} field. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public TankDriveData modifyTurningRate( + @NotNull Function function) { + return new TankDriveData(left, right, heading, function.apply(turningRate)); + } + + /** + * Creates a copy of this {@code TankDriveData} with a modified left {@link DriveData#position + * position}. + * + * @param function A {@link Function} that takes this {@code DriveData}'s current left {@link + * DriveData#position position} and returns a new {@link DriveData#position position}. + * @return A copy of this {@code TankDriveData} with all fields identical except for the left + * {@link DriveData#position position} field. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public TankDriveData modifyLeftPosition( + @NotNull Function function) { + return new TankDriveData(left.modifyPosition(function), right, heading, turningRate); + } + + /** + * Creates a copy of this {@code TankDriveData} with a modified left {@link DriveData#velocity + * velocity}. + * + * @param function A {@link Function} that takes this {@code DriveData}'s current left {@link + * DriveData#velocity velocity} and returns a new {@link DriveData#velocity velocity}. + * @return A copy of this {@code TankDriveData} with all fields identical except for the left + * {@link DriveData#velocity velocity} field. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public TankDriveData modifyLeftVelocity( + @NotNull Function function) { + return new TankDriveData(left.modifyVelocity(function), right, heading, turningRate); + } + + /** + * Creates a copy of this {@code TankDriveData} with a modified left {@link DriveData#acceleration + * acceleration}. + * + * @param function A {@link Function} that takes this {@code DriveData}'s current left {@link + * DriveData#acceleration acceleration} and returns a new {@link DriveData#acceleration + * acceleration}. + * @return A copy of this {@code TankDriveData} with all fields identical except for the left + * {@link DriveData#acceleration acceleration} field. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public TankDriveData modifyLeftAcceleration( + @NotNull Function function) { + return new TankDriveData(left.modifyAcceleration(function), right, heading, turningRate); + } + + /** + * Creates a copy of this {@code TankDriveData} with a modified left {@link + * DriveData#additionalFeedForward additionalFeedForward}. + * + * @param function A {@link Function} that takes this {@code DriveData}'s current left {@link + * DriveData#additionalFeedForward additionalFeedForward} and returns a new {@link + * DriveData#additionalFeedForward additionalFeedForward}. + * @return A copy of this {@code TankDriveData} with all fields identical except for the left + * {@link DriveData#additionalFeedForward additionalFeedForward} field. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public TankDriveData modifyLeftAdditionalFeedForward( + @NotNull Function function) { + return new TankDriveData(left.modifyAdditionalFeedForward(function), right, heading, + turningRate); + } + + /** + * Creates a copy of this {@code TankDriveData} with a modified right {@link DriveData#position + * position}. + * + * @param function A {@link Function} that takes this {@code DriveData}'s current right {@link + * DriveData#position position} and returns a new {@link DriveData#position position}. + * @return A copy of this {@code TankDriveData} with all fields identical except for the right + * {@link DriveData#position position} field. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public TankDriveData modifyRightPosition( + @NotNull Function function) { + return new TankDriveData(left, right.modifyPosition(function), heading, turningRate); + } + + /** + * Creates a copy of this {@code TankDriveData} with a modified right {@link DriveData#velocity + * velocity}. + * + * @param function A {@link Function} that takes this {@code DriveData}'s current right {@link + * DriveData#velocity velocity} and returns a new {@link DriveData#velocity velocity}. + * @return A copy of this {@code TankDriveData} with all fields identical except for the right + * {@link DriveData#velocity velocity} field. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public TankDriveData modifyRightVelocity( + @NotNull Function function) { + return new TankDriveData(left, right.modifyVelocity(function), heading, turningRate); + } + + /** + * Creates a copy of this {@code TankDriveData} with a modified right {@link + * DriveData#acceleration acceleration}. + * + * @param function A {@link Function} that takes this {@code DriveData}'s current right {@link + * DriveData#acceleration acceleration} and returns a new {@link DriveData#acceleration + * acceleration}. + * @return A copy of this {@code TankDriveData} with all fields identical except for the right + * {@link DriveData#acceleration acceleration} field. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public TankDriveData modifyRightAcceleration( + @NotNull Function function) { + return new TankDriveData(left, right.modifyAcceleration(function), heading, turningRate); + } + + /** + * Creates a copy of this {@code TankDriveData} with a modified right {@link + * DriveData#additionalFeedForward additionalFeedForward}. + * + * @param function A {@link Function} that takes this {@code DriveData}'s current right {@link + * DriveData#additionalFeedForward additionalFeedForward} and returns a new {@link + * DriveData#additionalFeedForward additionalFeedForward}. + * @return A copy of this {@code TankDriveData} with all fields identical except for the right + * {@link DriveData#additionalFeedForward additionalFeedForward} field. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public TankDriveData modifyRightAdditionalFeedForward( + @NotNull Function function) { + return new TankDriveData(left, right.modifyAdditionalFeedForward(function), heading, + turningRate); + } + + /** + * Creates a copy of this {@code TankDriveData} with modified left and right positions. + * + * @param lfunc A {@link Function} that takes this {@code DriveData}'s current left {@link + * DriveData#position position} and returns a new {@link DriveData#position position}. + * @param rfunc A {@link Function} that takes this {@code DriveData}'s current right {@link + * DriveData#position position} and returns a new {@link DriveData#position position}. + * @return A copy of this {@code TankDriveData} with all fields identical except for the left and + * right {@link DriveData#additionalFeedForward additionalFeedForward} fields. + */ + @NotNull + @Contract(value = "_, _ -> new", pure = true) + public TankDriveData modifyPosition(@NotNull Function lfunc, + @NotNull Function rfunc) { + return new TankDriveData(left.modifyPosition(lfunc), right.modifyPosition(rfunc), heading, + turningRate); + } + + /** + * Creates a copy of this {@code TankDriveData} with modified left and right velocitys. + * + * @param lfunc A {@link Function} that takes this {@code DriveData}'s current left {@link + * DriveData#velocity velocity} and returns a new {@link DriveData#velocity velocity}. + * @param rfunc A {@link Function} that takes this {@code DriveData}'s current right {@link + * DriveData#velocity velocity} and returns a new {@link DriveData#velocity velocity}. + * @return A copy of this {@code TankDriveData} with all fields identical except for the left and + * right {@link DriveData#additionalFeedForward additionalFeedForward} fields. + */ + @NotNull + @Contract(value = "_, _ -> new", pure = true) + public TankDriveData modifyVelocity(@NotNull Function lfunc, + @NotNull Function rfunc) { + return new TankDriveData(left.modifyVelocity(lfunc), right.modifyVelocity(rfunc), heading, + turningRate); + } + + + /** + * Creates a copy of this {@code TankDriveData} with modified left and right accelerations. + * + * @param lfunc A {@link Function} that takes this {@code DriveData}'s current left {@link + * DriveData#acceleration acceleration} and returns a new {@link DriveData#acceleration + * acceleration}. + * @param rfunc A {@link Function} that takes this {@code DriveData}'s current right {@link + * DriveData#acceleration acceleration} and returns a new {@link DriveData#acceleration + * acceleration}. + * @return A copy of this {@code TankDriveData} with all fields identical except for the left and + * right {@link DriveData#additionalFeedForward additionalFeedForward} fields. + */ + @NotNull + @Contract(value = "_, _ -> new", pure = true) + public TankDriveData modifyAcceleration(@NotNull Function lfunc, + @NotNull Function rfunc) { + return new TankDriveData(left.modifyAcceleration(lfunc), right.modifyAcceleration(rfunc), + heading, + turningRate); + } + + /** + * Creates a copy of this {@code TankDriveData} with modified left and right feed-forwards. + * + * @param lfunc A {@link Function} that takes this {@code DriveData}'s current left {@link + * DriveData#additionalFeedForward additionalFeedForward} and returns a new {@link + * DriveData#additionalFeedForward additionalFeedForward}. + * @param rfunc A {@link Function} that takes this {@code DriveData}'s current right {@link + * DriveData#additionalFeedForward additionalFeedForward} and returns a new {@link + * DriveData#additionalFeedForward additionalFeedForward}. + * @return A copy of this {@code TankDriveData} with all fields identical except for the left and + * right {@link DriveData#additionalFeedForward additionalFeedForward} fields. + */ + @NotNull + @Contract(value = "_, _ -> new", pure = true) + public TankDriveData modifyAdditionalFeedForward( + @NotNull Function lfunc, + @NotNull Function rfunc) { + return new TankDriveData(left.modifyAdditionalFeedForward(lfunc), + right.modifyAdditionalFeedForward(rfunc), heading, + turningRate); + } + + /** + * Adds the provided value to this {@code TankDriveData}'s {@link #heading}. If there is already a + * value present in {@link #heading}, the returned {@code TankDriveData}'s {@link #heading} will + * be equal to the sum of that value plus the parameter; otherwise, the left {@link #heading} will + * be equal to the value of the provided heading parameter. + * + * @param heading The heading to add. + * @return A new {@code TankDriveData} with all fields identical except for the left {@link + * #heading} field. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public TankDriveData plusHeading(double heading) { + return modifyHeading((old) -> OptionalDouble.of(old.orElse(0) + heading)); + } + + /** + * Adds the provided value to this {@code TankDriveData}'s {@link #turningRate}. If there is + * already a value present in {@link #turningRate}, the returned {@code TankDriveData}'s {@link + * #turningRate} will be equal to the sum of that value plus the parameter; otherwise, the left + * {@link #turningRate} will be equal to the value of the provided turningRate parameter. + * + * @param turningRate The turningRate to add. + * @return A new {@code TankDriveData} with all fields identical except for the left {@link + * #turningRate} field. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public TankDriveData plusTurningRate(double turningRate) { + return modifyTurningRate((old) -> OptionalDouble.of(old.orElse(0) + turningRate)); + } + + + /** + * Adds the provided value to this {@code TankDriveData}'s left {@link DriveData#position + * position}. If there is already a value present in {@link #left left.}{@link DriveData#position + * position}, the returned {@code TankDriveData}'s left {@link DriveData#position position} will + * be equal to the sum of that value plus the parameter; otherwise, the left {@link + * DriveData#position position} will be equal to the value of the provided position parameter. + * + * @param position The position to add. + * @return A new {@code TankDriveData} with all fields identical except for the left {@link + * DriveData#position position} field. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public TankDriveData plusLeftPosition(double position) { + return new TankDriveData(left.plusPosition(position), right, heading, turningRate); + } + + /** + * Adds the provided value to this {@code TankDriveData}'s left {@link DriveData#velocity + * velocity}. If there is already a value present in {@link #left left.}{@link DriveData#velocity + * velocity}, the returned {@code TankDriveData}'s left {@link DriveData#velocity velocity} will + * be equal to the sum of that value plus the parameter; otherwise, the left {@link + * DriveData#velocity velocity} will be equal to the value of the provided velocity parameter. + * + * @param velocity The velocity to add. + * @return A new {@code TankDriveData} with all fields identical except for the left {@link + * DriveData#velocity velocity} field. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public TankDriveData plusLeftVelocity(double velocity) { + return new TankDriveData(left.plusVelocity(velocity), right, heading, turningRate); + } + + + /** + * Adds the provided value to this {@code TankDriveData}'s left {@link DriveData#acceleration + * acceleration}. If there is already a value present in {@link #left left.}{@link + * DriveData#acceleration acceleration}, the returned {@code TankDriveData}'s left {@link + * DriveData#acceleration acceleration} will be equal to the sum of that value plus the parameter; + * otherwise, the left {@link DriveData#acceleration acceleration} will be equal to the value of + * the provided acceleration parameter. + * + * @param acceleration The acceleration to add. + * @return A new {@code TankDriveData} with all fields identical except for the left {@link + * DriveData#acceleration acceleration} field. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public TankDriveData plusLeftAcceleration(double acceleration) { + return new TankDriveData(left.plusAcceleration(acceleration), right, heading, turningRate); + } + + /** + * Adds the provided value to this {@code TankDriveData}'s left {@link + * DriveData#additionalFeedForward additionalFeedForward}. If there is already a value present in + * {@link #left left.}{@link DriveData#additionalFeedForward additionalFeedForward}, the returned + * {@code TankDriveData}'s left {@link DriveData#additionalFeedForward additionalFeedForward} will + * be equal to the sum of that value plus the parameter; otherwise, the left {@link + * DriveData#additionalFeedForward additionalFeedForward} will be equal to the value of the + * provided additionalFeedForward parameter. + * + * @param additionalFeedForward The additionalFeedForward to add. + * @return A new {@code TankDriveData} with all fields identical except for the left {@link + * DriveData#additionalFeedForward additionalFeedForward} field. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public TankDriveData plusLeftAdditionalFeedForward(double additionalFeedForward) { + return new TankDriveData(left.plusAdditionalFeedForward(additionalFeedForward), right, heading, + turningRate); + } + + + /** + * Adds the provided value to this {@code TankDriveData}'s right {@link DriveData#position + * position}. If there is already a value present in {@link #right right.}{@link + * DriveData#position position}, the returned {@code TankDriveData}'s right {@link + * DriveData#position position} will be equal to the sum of that value plus the parameter; + * otherwise, the right {@link DriveData#position position} will be equal to the value of the + * provided position parameter. + * + * @param position The position to add. + * @return A new {@code TankDriveData} with all fields identical except for the right {@link + * DriveData#position position} field. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public TankDriveData plusRightPosition(double position) { + return new TankDriveData(left, right.plusPosition(position), heading, turningRate); + } + + /** + * Adds the provided value to this {@code TankDriveData}'s right {@link DriveData#velocity + * velocity}. If there is already a value present in {@link #right right.}{@link + * DriveData#velocity velocity}, the returned {@code TankDriveData}'s right {@link + * DriveData#velocity velocity} will be equal to the sum of that value plus the parameter; + * otherwise, the right {@link DriveData#velocity velocity} will be equal to the value of the + * provided velocity parameter. + * + * @param velocity The velocity to add. + * @return A new {@code TankDriveData} with all fields identical except for the right {@link + * DriveData#velocity velocity} field. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public TankDriveData plusRightVelocity(double velocity) { + return new TankDriveData(left, right.plusVelocity(velocity), heading, turningRate); + } + + + /** + * Adds the provided value to this {@code TankDriveData}'s right {@link DriveData#acceleration + * acceleration}. If there is already a value present in {@link #right right.}{@link + * DriveData#acceleration acceleration}, the returned {@code TankDriveData}'s right {@link + * DriveData#acceleration acceleration} will be equal to the sum of that value plus the parameter; + * otherwise, the right {@link DriveData#acceleration acceleration} will be equal to the value of + * the provided acceleration parameter. + * + * @param acceleration The acceleration to add. + * @return A new {@code TankDriveData} with all fields identical except for the right {@link + * DriveData#acceleration acceleration} field. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public TankDriveData plusRightAcceleration(double acceleration) { + return new TankDriveData(left, right.plusAcceleration(acceleration), heading, turningRate); + } + + /** + * Adds the provided value to this {@code TankDriveData}'s right {@link + * DriveData#additionalFeedForward additionalFeedForward}. If there is already a value present in + * {@link #right right.}{@link DriveData#additionalFeedForward additionalFeedForward}, the + * returned {@code TankDriveData}'s right {@link DriveData#additionalFeedForward + * additionalFeedForward} will be equal to the sum of that value plus the parameter; otherwise, + * the right {@link DriveData#additionalFeedForward additionalFeedForward} will be equal to the + * value of the provided additionalFeedForward parameter. + * + * @param additionalFeedForward The additionalFeedForward to add. + * @return A new {@code TankDriveData} with all fields identical except for the right {@link + * DriveData#additionalFeedForward additionalFeedForward} field. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public TankDriveData plusRightAdditionalFeedForward(double additionalFeedForward) { + return new TankDriveData(left, right.plusAdditionalFeedForward(additionalFeedForward), heading, + turningRate); + } + + /** + * Adds the provided values to this {@code TankDriveData}'s left and right {@link + * DriveData#position position}. If there is already a value present in either {@link + * DriveData#position position}, the returned {@code TankDriveData}'s {@link DriveData#position + * positions} will be equal to the sum of that value plus the parameter; otherwise, the {@link + * DriveData#position position} will be equal to the value of the provided parameter for each + * side. + * + * @param leftPosition The position to add to the left side. + * @param rightPosition The position to add to the right side. + * @return A new {@code TankDriveData} with all fields identical except for the left and right + * {@link DriveData#position position} fields. + */ + @NotNull + @Contract(value = "_, _ -> new", pure = true) + public TankDriveData plusPositions(double leftPosition, double rightPosition) { + return new TankDriveData(left.plusPosition(leftPosition), right.plusPosition(rightPosition), + heading, turningRate); + } + + /** + * Adds the provided values to this {@code TankDriveData}'s left and right {@link + * DriveData#velocity velocity}. If there is already a value present in either {@link + * DriveData#velocity velocity}, the returned {@code TankDriveData}'s {@link DriveData#velocity + * velocities} will be equal to the sum of that value plus the parameter; otherwise, the {@link + * DriveData#velocity velocity} will be equal to the value of the provided parameter for each + * side. + * + * @param leftVelocity The velocity to add to the left side. + * @param rightVelocity The velocity to add to the right side. + * @return A new {@code TankDriveData} with all fields identical except for the left and right + * {@link DriveData#velocity velocity} fields. + */ + @NotNull + @Contract(value = "_, _ -> new", pure = true) + public TankDriveData plusVelocities(double leftVelocity, double rightVelocity) { + return new TankDriveData(left.plusVelocity(leftVelocity), right.plusVelocity(rightVelocity), + heading, turningRate); + } + + /** + * Adds the provided values to this {@code TankDriveData}'s left and right {@link + * DriveData#acceleration acceleration}. If there is already a value present in either {@link + * DriveData#acceleration acceleration}, the returned {@code TankDriveData}'s {@link + * DriveData#acceleration accelerations} will be equal to the sum of that value plus the + * parameter; otherwise, the {@link DriveData#acceleration acceleration} will be equal to the + * value of the provided parameter for each side. + * + * @param leftAcceleration The acceleration to add to the left side. + * @param rightAcceleration The acceleration to add to the right side. + * @return A new {@code TankDriveData} with all fields identical except for the left and right + * {@link DriveData#acceleration acceleration} fields. + */ + @NotNull + @Contract(value = "_, _ -> new", pure = true) + public TankDriveData plusAccelerations(double leftAcceleration, double rightAcceleration) { + return new TankDriveData(left.plusAcceleration(leftAcceleration), + right.plusAcceleration(rightAcceleration), + heading, turningRate); + } + + /** + * Adds the provided values to this {@code TankDriveData}'s left and right {@link + * DriveData#additionalFeedForward additionalFeedForward}. If there is already a value present in + * either {@link DriveData#additionalFeedForward additionalFeedForward}, the returned {@code + * TankDriveData}'s {@link DriveData#additionalFeedForward additionalFeedForwards} will be equal + * to the sum of that value plus the parameter; otherwise, the {@link + * DriveData#additionalFeedForward additionalFeedForward} will be equal to the value of the + * provided parameter for each side. + * + * @param leftAdditionalFeedForward The additionalFeedForward to add to the left side. + * @param rightAdditionalFeedForward The additionalFeedForward to add to the right side. + * @return A new {@code TankDriveData} with all fields identical except for the left and right + * {@link DriveData#additionalFeedForward additionalFeedForward} fields. + */ + @NotNull + @Contract(value = "_, _ -> new", pure = true) + public TankDriveData plusAdditionalFeedForwards(double leftAdditionalFeedForward, + double rightAdditionalFeedForward) { + return new TankDriveData(left.plusAdditionalFeedForward(leftAdditionalFeedForward), + right.plusAdditionalFeedForward(rightAdditionalFeedForward), + heading, turningRate); + } + + + /** + * Creates a copy of this {@code TankDriveData} and sets its {@link #heading}. If there is already + * a value present in {@link #heading}, it will be overwritten. + * + * @param heading The new heading. + * @return A {@code TankDriveData} with all fields identical except for {@link #heading}. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public TankDriveData withHeading(double heading) { + return new TankDriveData(left, right, OptionalDouble.of(heading), turningRate); + } + + /** + * Creates a copy of this {@code TankDriveData} and sets its {@link #turningRate}. If there is + * already a value present in {@link #turningRate}, it will be overwritten. + * + * @param turningRate The new turningRate. + * @return A {@code TankDriveData} with all fields identical except for {@link #turningRate}. + */ + @NotNull + @Contract(value = "_ -> new", pure = true) + public TankDriveData withTurningRate(double turningRate) { + return new TankDriveData(left, right, heading, OptionalDouble.of(turningRate)); + } + + /** + * Creates a copy of this {@code TankDriveData} and sets its left and right {@link + * DriveData#position positions}. If there is already a value present in either {@link + * DriveData#position position}, it will be overwritten during copying. + * + * @param leftPosition The position to set on the left side. + * @param rightPosition The position to set on the right side. + * @return A new {@code TankDriveData} with all fields identical except for the left and right + * {@link DriveData#position position} fields. + */ + @NotNull + @Contract(value = "_, _ -> new", pure = true) + public TankDriveData withPositions(double leftPosition, double rightPosition) { + return new TankDriveData(left.withPosition(leftPosition), right.withPosition(rightPosition), + heading, turningRate); + } + + /** + * Creates a copy of this {@code TankDriveData} and sets its left and right {@link + * DriveData#velocity velocities}. If there is already a value present in either {@link + * DriveData#velocity velocity}, it will be overwritten during copying. + * + * @param leftVelocity The velocity to set on the left side. + * @param rightVelocity The velocity to set on the right side. + * @return A new {@code TankDriveData} with all fields identical except for the left and right + * {@link DriveData#velocity velocity} fields. + */ + @NotNull + @Contract(value = "_, _ -> new", pure = true) + public TankDriveData withVelocities(double leftVelocity, double rightVelocity) { + return new TankDriveData(left.withVelocity(leftVelocity), right.withVelocity(rightVelocity), + heading, turningRate); + } + + /** + * Creates a copy of this {@code TankDriveData} and sets its left and right {@link + * DriveData#acceleration accelerations}. If there is already a value present in either {@link + * DriveData#acceleration acceleration}, it will be overwritten during copying. + * + * @param leftAcceleration The acceleration to set on the left side. + * @param rightAcceleration The acceleration to set on the right side. + * @return A new {@code TankDriveData} with all fields identical except for the left and right + * {@link DriveData#acceleration acceleration} fields. + */ + @NotNull + @Contract(value = "_, _ -> new", pure = true) + public TankDriveData withAccelerations(double leftAcceleration, double rightAcceleration) { + return new TankDriveData(left.withAcceleration(leftAcceleration), + right.withAcceleration(rightAcceleration), heading, turningRate); + } + + /** + * Creates a copy of this {@code TankDriveData} and sets its left and right {@link + * DriveData#additionalFeedForward additionalFeedForwards}. If there is already a value present in + * either {@link DriveData#additionalFeedForward additionalFeedForward}, it will be overwritten + * during copying. + * + * @param leftAdditionalFeedForward The additionalFeedForward to set on the left side. + * @param rightAdditionalFeedForward The additionalFeedForward to set on the right side. + * @return A new {@code TankDriveData} with all fields identical except for the left and right + * {@link DriveData#additionalFeedForward additionalFeedForward} fields. + */ + @NotNull + @Contract(value = "_, _ -> new", pure = true) + public TankDriveData withAdditionalFeedForwards(double leftAdditionalFeedForward, + double rightAdditionalFeedForward) { + return new TankDriveData(left.withAdditionalFeedForward(leftAdditionalFeedForward), + right.withAdditionalFeedForward(rightAdditionalFeedForward), heading, turningRate); + } + /** * Creates a new {@code TankDriveData} with the supplied values. * From 1d78e2a15b1bb6f59958a2b5bdcb3d32cf267d84 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 10 Dec 2018 17:07:26 -0800 Subject: [PATCH 33/51] Use ergonomic methods --- .../pipeline/AdvancedArcadeJoystickInput.java | 6 +-- .../drive/pipeline/FeedForwardProcessor.java | 28 ++++------- .../drive/pipeline/HeadingPIDProcessor.java | 48 +++++++------------ .../pipeline/HeadingTransformProcessor.java | 7 +-- .../drive/pipeline/SimpleJoystickInput.java | 18 +------ .../pipeline/TurningRatePIDProcessor.java | 18 +------ 6 files changed, 30 insertions(+), 95 deletions(-) diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java index 2f9d9099..58b0ca37 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java @@ -125,10 +125,6 @@ public TankDriveData get() { double omega = (rightVelocity - leftVelocity) / trackWidth; - return new TankDriveData( - new DriveData(OptionalDouble.of(leftVelocity)), - new DriveData(OptionalDouble.of(rightVelocity)), - OptionalDouble.empty(), - OptionalDouble.of(omega)); + return new TankDriveData().withVelocities(leftVelocity, rightVelocity).withTurningRate(omega); } } diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/FeedForwardProcessor.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/FeedForwardProcessor.java index ef614c97..95f9f3e0 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/FeedForwardProcessor.java +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/FeedForwardProcessor.java @@ -1,6 +1,5 @@ package org.team1540.rooster.drive.pipeline; -import java.util.OptionalDouble; import org.jetbrains.annotations.Contract; import org.jetbrains.annotations.NotNull; import org.team1540.rooster.functional.Processor; @@ -20,9 +19,11 @@ public class FeedForwardProcessor implements Processor *

  • The feed-forward starts at 0.
    • *
  • The product of the velocity (if present) and the velocity feed-forward is added.
    • - *
  • The product of the acceleration (if present) and the acceleration feed-forward is added.
    • + *
  • The product of the acceleration (if present) and the acceleration feed-forward is + * added.
    • *
  • If the velocity is present and nonzero, the throttle bump (with the sign of the * velocity) is added.
    • * @@ -56,20 +58,8 @@ private double getThrottle(double wantedSpeed, double wantedAccel) { @Override @NotNull public TankDriveData apply(@NotNull TankDriveData command) { - return new TankDriveData( - new DriveData(command.left.position, - command.left.velocity, - command.left.acceleration, - OptionalDouble.of(command.left.additionalFeedForward.orElse(0) - + getThrottle(command.left.velocity.orElse(0), - command.left.acceleration.orElse(0)))), - new DriveData(command.right.position, - command.right.velocity, - command.right.acceleration, - OptionalDouble.of(command.right.additionalFeedForward.orElse(0) - + getThrottle(command.right.velocity.orElse(0), - command.right.acceleration.orElse(0)))), - command.heading, - command.turningRate); + return command.plusAdditionalFeedForwards( + getThrottle(command.left.velocity.orElse(0), command.left.acceleration.orElse(0)), + getThrottle(command.right.velocity.orElse(0), command.right.acceleration.orElse(0))); } } diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/HeadingPIDProcessor.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/HeadingPIDProcessor.java index 77d69703..441dfcee 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/HeadingPIDProcessor.java +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/HeadingPIDProcessor.java @@ -1,6 +1,5 @@ package org.team1540.rooster.drive.pipeline; -import java.util.OptionalDouble; import java.util.function.DoubleSupplier; public class HeadingPIDProcessor extends PIDProcessor { @@ -12,8 +11,10 @@ public class HeadingPIDProcessor extends PIDProcessor Date: Mon, 10 Dec 2018 19:02:31 -0800 Subject: [PATCH 34/51] Use ergonomic methods in pipeline examples --- docs/Drive Pipelines.md | 28 +++++----------------------- 1 file changed, 5 insertions(+), 23 deletions(-) diff --git a/docs/Drive Pipelines.md b/docs/Drive Pipelines.md index df7d6068..7a0314bd 100644 --- a/docs/Drive Pipelines.md +++ b/docs/Drive Pipelines.md @@ -64,14 +64,8 @@ Most "stock" pipeline elements pass around `TankDriveData` instances to encapsul An input that returns the same `TankDriveData` every time: ```java -TankDriveData tankDriveData = new TankDriveData( - new DriveData(0), - new DriveData(0), - OptionalDouble.empty(), - OptionalDouble.empty()); - -Executable pipeline = ((Input) () -> tankDriveData) - .then(new CTREOutput(leftTalon, rightTalon)) +Executable pipeline = ((Input) () -> new TankDriveData().withVelocities(0, 0)) + .then(new CTREOutput(leftTalon, rightTalon)) ``` #### Custom Processor @@ -80,21 +74,9 @@ A processor that multiplies the received position by two: ```java Executable pipeline = new SimpleJoystickInput(new Joystick(0), 1, 5, false, false) - .then(data -> return new TankDriveData( - new DriveData( - d.left.position.isPresent() ? OptionalDouble.of(d.left.position.getAsDouble() * 2) : d.left.position, - d.left.velocity, - d.left.acceleration, - d.left.additionalFeedForward - ), - new DriveData( - d.right.position.isPresent() ? OptionalDouble.of(d.right.position.getAsDouble() * 2) : d.right.position, - d.right.velocity, - d.right.acceleration, - d.right.additionalFeedForward - ), - d.heading, d.turningRate); - )) + .then(data -> data.modifyPositions( + (left) -> left.isPresent() ? left.getAsDouble() * 2 : left, + (right) -> right.isPresent() ? right.getAsDouble() * 2 : right) .then(new CTREOutput(leftTalon, rightTalon)) ``` From 23cdd9b3fd22dafce8cb06a3d507528ff4622b8e Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Tue, 11 Dec 2018 09:52:10 -0800 Subject: [PATCH 35/51] Remove ProfileFollower-based following system Has been wholly superseded by the pipeline system and was never in a release of ROOSTER so we don't need to keep it around for compatibility. --- .../motionprofiling/FollowProfile.java | 147 ------- .../motionprofiling/FollowProfileFactory.java | 411 ------------------ .../motionprofiling/MotionProfile.java | 39 +- .../motionprofiling/MotionProfileUtils.java | 29 -- .../MotionProfilingProperties.java | 3 +- .../motionprofiling/ProfileFollower.java | 253 ----------- .../motionprofiling/RunMotionProfiles.java | 4 +- .../motionprofiling/SetpointConsumer.java | 30 -- .../rooster/motionprofiling/package-info.java | 25 +- .../rooster/testing/MotionProfileTesting.kt | 312 ------------- 10 files changed, 23 insertions(+), 1230 deletions(-) delete mode 100644 lib/src/main/java/org/team1540/rooster/motionprofiling/FollowProfile.java delete mode 100644 lib/src/main/java/org/team1540/rooster/motionprofiling/FollowProfileFactory.java delete mode 100644 lib/src/main/java/org/team1540/rooster/motionprofiling/ProfileFollower.java delete mode 100644 lib/src/main/java/org/team1540/rooster/motionprofiling/SetpointConsumer.java delete mode 100644 test/src/main/kotlin/org/team1540/rooster/testing/MotionProfileTesting.kt diff --git a/lib/src/main/java/org/team1540/rooster/motionprofiling/FollowProfile.java b/lib/src/main/java/org/team1540/rooster/motionprofiling/FollowProfile.java deleted file mode 100644 index 642578c2..00000000 --- a/lib/src/main/java/org/team1540/rooster/motionprofiling/FollowProfile.java +++ /dev/null @@ -1,147 +0,0 @@ -package org.team1540.rooster.motionprofiling; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.command.Subsystem; -import java.util.function.DoubleSupplier; -import org.jetbrains.annotations.NotNull; -import org.team1540.rooster.motionprofiling.ProfileFollower.ProfileDriveSignal; -import org.team1540.rooster.util.AsyncCommand; - -/** - * {@link edu.wpi.first.wpilibj.command.Command Command} to execute a motion profile. This is an - * {@link AsyncCommand}-based wrapper around {@link ProfileFollower} which handles creating the - * instance, running it in a fast loop, and sending the output to the motors. - * - * @see FollowProfileFactory - * @see ProfileFollower - */ -public class FollowProfile extends AsyncCommand { - - @NotNull - private MotionProfile left; - @NotNull - private MotionProfile right; - - @NotNull - private SetpointConsumer leftSetpointConsumer; - @NotNull - private SetpointConsumer rightSetpointConsumer; - - @NotNull - private DoubleSupplier headingSupplier; - - private double lVelCoeff; - private double lVelIntercept; - private double lAccelCoeff; - private double rVelCoeff; - private double rVelIntercept; - private double rAccelCoeff; - private double headingP; - private double headingI; - - private double gyroIAccum; - - private ProfileFollower follower; - - /** - * Timer for the amount of time since profiling started (used to find current point) - */ - private Timer timer = new Timer(); - - /** - * Creates a {@code FollowProfile} command. - * - * @param subsystems The required subsystems for this command. - * @param leftSetpointConsumer The {@link SetpointConsumer} for the left-side motors. - * @param rightSetpointConsumer The {@link SetpointConsumer} for the right-side motors. - * @param headingSupplier A {@link DoubleSupplier} that returns the robot's current heading in - * radians from 0 to 2π. - * @param loopFreq The interval, in milliseconds, between profile loop execution. - * @param lVelCoeff The left velocity coefficient (kV), in bump units per profile unit per - * second. - * @param lVelIntercept The left velocity intercept (VIntercept), in bump units. - * @param lAccelCoeff The left acceleration coefficient (kA), in bump units per profile unit per - * second squared. - * @param rVelCoeff The right velocity coefficient (kV), in bump units per profile unit per - * second. - * @param rVelIntercept The right velocity intercept (VIntercept), in bump units. - * @param rAccelCoeff The right acceleration coefficient (kA), in bump units per profile unit per - * second squared. - * @param headingP The P coefficient for the heading controller, in profile units per radian. - * @param headingI The I coefficient for the heading controller, in profile units per - * radian-second. - */ - public FollowProfile(@NotNull MotionProfile left, @NotNull MotionProfile right, - @NotNull Subsystem[] subsystems, - @NotNull SetpointConsumer leftSetpointConsumer, - @NotNull SetpointConsumer rightSetpointConsumer, - @NotNull DoubleSupplier headingSupplier, - long loopFreq, double lVelCoeff, double lVelIntercept, double lAccelCoeff, double rVelCoeff, - double rVelIntercept, double rAccelCoeff, double headingP, double headingI) { - super(loopFreq); - for (Subsystem subsystem : subsystems) { - requires(subsystem); - } - this.left = left; - this.right = right; - this.leftSetpointConsumer = leftSetpointConsumer; - this.rightSetpointConsumer = rightSetpointConsumer; - this.headingSupplier = headingSupplier; - this.lVelCoeff = lVelCoeff; - this.lVelIntercept = lVelIntercept; - this.lAccelCoeff = lAccelCoeff; - this.rVelCoeff = rVelCoeff; - this.rVelIntercept = rVelIntercept; - this.rAccelCoeff = rAccelCoeff; - this.headingP = headingP; - this.headingI = headingI; - - follower = new ProfileFollower(left, right, lVelCoeff, lVelIntercept, lAccelCoeff, rVelCoeff, - rVelIntercept, rAccelCoeff, headingP, headingI); - } - - /** - * Get the time since starting the motion profile. - * - * @return The time, in seconds, since beginning to execute a motion profile, or 0 if not - * currently executing a profile. - */ - public double getExecutionTime() { - return isRunning() ? timer.get() : 0; - } - - /** - * Get the underlying {@link ProfileFollower} - * - * @return The underlying {@link ProfileFollower}. Any modifications to this object will affect - * this command. - */ - public ProfileFollower getFollower() { - return follower; - } - - @Override - protected void runPeriodic() { - // check finish status - if (follower.isProfileFinished(timer.get())) { - markAsFinished(); - } - - ProfileDriveSignal sig = follower.get(headingSupplier.getAsDouble(), timer.get()); - - leftSetpointConsumer.set(sig.leftSetpoint, sig.leftBump); - rightSetpointConsumer.set(sig.rightSetpoint, sig.rightBump); - } - - @Override - protected void runInitial() { - follower.reset(); - timer.start(); - } - - @Override - protected void runEnd() { - timer.stop(); - timer.reset(); - } -} diff --git a/lib/src/main/java/org/team1540/rooster/motionprofiling/FollowProfileFactory.java b/lib/src/main/java/org/team1540/rooster/motionprofiling/FollowProfileFactory.java deleted file mode 100644 index 164099c9..00000000 --- a/lib/src/main/java/org/team1540/rooster/motionprofiling/FollowProfileFactory.java +++ /dev/null @@ -1,411 +0,0 @@ -package org.team1540.rooster.motionprofiling; - -import edu.wpi.first.wpilibj.command.Subsystem; -import jaci.pathfinder.Pathfinder; -import jaci.pathfinder.Trajectory; -import java.io.File; -import java.util.function.DoubleSupplier; -import org.jetbrains.annotations.NotNull; - -/** - * Produces multiple similar {@link FollowProfile} commands for one subystem using common tuning - * values. In general, one {@code FollowProfileFactory} should be created for each subsystem to be - * motion-profiled. It can then be pre-loaded with common configuration and tuning values specific - * to a subsystem, and kept as a single instance to create any needed {@code FollowProfile} commands - * for that subsystem. - *

    - * All setters in this class follow a builder pattern; i.e they return an instance of the object - * they were called on. This allows for multiple set methods to be chained. - * - * @see FollowProfile - */ -public class FollowProfileFactory { - - @NotNull - private Subsystem[] subsystems; - @NotNull - private SetpointConsumer leftSetpointConsumer; - @NotNull - private SetpointConsumer rightSetpointConsumer; - - @NotNull - private DoubleSupplier headingSupplier = () -> 0; - - private long loopFreq = 20; - - private double lVelCoeff; - private double lVelIntercept; - private double lAccelCoeff; - private double rVelCoeff; - private double rVelIntercept; - private double rAccelCoeff; - private double headingP = 0; - private double headingI = 0; - - /** - * Constructs a {@code FollowProfileFactory} with the provided left and right set functions and - * subsystems. - * - * @param left The {@link SetpointConsumer} for the left-side motors. - * @param right The {@link SetpointConsumer} for the right-side motors. - * @param subsystems The {@link Subsystem Subystems} that produced {@link FollowProfile} commands - * should require. - */ - public FollowProfileFactory(@NotNull SetpointConsumer left, @NotNull SetpointConsumer right, - @NotNull Subsystem... subsystems) { - this.leftSetpointConsumer = left; - this.rightSetpointConsumer = right; - this.subsystems = subsystems; - } - - /** - * Gets the subsystems used by produced {@link FollowProfile} commands. - * - * @return The required subsystems. Each of these will be declared as a requirement for each - * produced command using {@code requires()}. - */ - @NotNull - public Subsystem[] getSubsystems() { - return subsystems; - } - - /** - * Sets the subsystems required by produced {@link FollowProfile} commands. - * - * @param subsystems The required subsystems. Each of these will be declared as a requirement for - * each produced command using {@code requires()}. - * @return This {@code FollowProfileFactory} in a builder pattern. - */ - @NotNull - public FollowProfileFactory setSubsystems(@NotNull Subsystem... subsystems) { - this.subsystems = subsystems; - return this; - } - - /** - * Gets the {@link SetpointConsumer} for the left-side motors. - * - * @return The currently used {@link SetpointConsumer} for the left side. - */ - @NotNull - public SetpointConsumer getLeftSetpointConsumer() { - return leftSetpointConsumer; - } - - /** - * Sets the {@link SetpointConsumer} for the left-side motors. - * - * @param leftSetpointConsumer A {@link SetpointConsumer} that takes a setpoint in profile units - * and bump in bump units. - */ - @NotNull - public FollowProfileFactory setLeftSetpointConsumer( - @NotNull SetpointConsumer leftSetpointConsumer) { - this.leftSetpointConsumer = leftSetpointConsumer; - return this; - } - - /** - * Gets the {@link SetpointConsumer} for the left-side motors. - * - * @return The currently used {@link SetpointConsumer} for the right side. - */ - @NotNull - public SetpointConsumer getRightSetpointConsumer() { - return rightSetpointConsumer; - } - - /** - * @param rightSetpointConsumer A {@link SetpointConsumer} that takes a setpoint in profile units - * and bump in bump units and passes them to the drivetrain. - * @return This {@code FollowProfileFactory} in a builder pattern. - */ - @NotNull - public FollowProfileFactory setRightSetpointConsumer( - @NotNull SetpointConsumer rightSetpointConsumer) { - this.rightSetpointConsumer = rightSetpointConsumer; - return this; - } - - /** - * Gets the {@link DoubleSupplier} used for the input of the heading loop. - * - * @return The heading supplier. - */ - @NotNull - public DoubleSupplier getHeadingSupplier() { - return headingSupplier; - } - - /** - * Sets the {@link DoubleSupplier} used for the input of the heading loop. - * - * @param headingSupplier A {@link DoubleSupplier} that returns the robot's heading in radians - * from 0 to 2π. - * @return This {@code FollowProfileFactory} in a builder pattern. - */ - @NotNull - public FollowProfileFactory setHeadingSupplier(@NotNull DoubleSupplier headingSupplier) { - this.headingSupplier = headingSupplier; - return this; - } - - /** - * Gets the delay between profile loop execution. Defaults to 20ms. - * - * @return The time between profile loop execution, in milliseconds. - */ - public long getLoopFreq() { - return loopFreq; - } - - /** - * Sets the delay between profile loop execution. Defaults to 20ms. - * - * @param loopFreq The time between loop executions, in milliseconds. - * @return This {@code FollowProfileFactory} in a builder pattern. - */ - @NotNull - public FollowProfileFactory setLoopFreq(long loopFreq) { - this.loopFreq = loopFreq; - return this; - } - - /** - * Gets the left velocity feed-forward coefficient. This is equivalent to the kV term in drive - * characterization. Defaults to 0 (no feed-forward). - * - * @return The velocity feed-forward coefficient, in bump units per profile unit per second. - */ - public double getLVelCoeff() { - return lVelCoeff; - } - - /** - * Sets the left velocity feed-forward coefficent. This is equivalent to the kV term in drive * - * characterization. Defaults to 0 (no feed-forward). - * - * @param velCoeff The velocity feed-forward coefficient, in bump units per profile unit per - * second. - * @return This {@code FollowProfileFactory} in a builder pattern. - */ - @NotNull - public FollowProfileFactory setLVelCoeff(double velCoeff) { - this.lVelCoeff = velCoeff; - return this; - } - - /** - * Gets the left velocity intercept, or VIntercept. (See Eli Barnett's drive characterization - * paper for an explanation of why this is needed.) - * - * @return The velocity intercept, in bump units. - */ - public double getLVelIntercept() { - return lVelIntercept; - } - - /** - * Sets the left velocity intercept, or VIntercept. (See Eli Barnett's drive characterization - * paper for an explanation of why this is needed.) Defaults to 0. - * - * @param velIntercept The velocity intercept, in bump units. - * @return This {@code FollowProfileFactory} in a builder pattern. - */ - @NotNull - public FollowProfileFactory setLVelIntercept(double velIntercept) { - this.lVelIntercept = velIntercept; - return this; - } - - /** - * Gets the left acceleration feed-forward. This is equivalent to the kA term in drive - * characterization. Defaults to 0 (no feed-forward). - * - * @return The currently set acceleration coefficient, in bump units per profile unit per second - * squared. - */ - public double getLAccelCoeff() { - return lAccelCoeff; - } - - /** - * Sets the acceleration feed-forward. This is equivalent to the kA term in drive - * characterization. Defaults to 0 (no feed-forward). - * - * @param accelCoeff The acceleration coefficient, in bump units per profile unit per second - * squared. - * @return This {@code FollowProfileFactory} in a builder pattern. - */ - @NotNull - public FollowProfileFactory setLAccelCoeff(double accelCoeff) { - this.lAccelCoeff = accelCoeff; - return this; - } - - /** - * Gets the right velocity feed-forward coefficient. This is equivalent to the kV term in drive - * characterization. Defaults to 0 (no feed-forward). - * - * @return The velocity feed-forward coefficient, in bump units per profile unit per second. - */ - public double getRVelCoeff() { - return rVelCoeff; - } - - /** - * Sets the right velocity feed-forward coefficent. This is equivalent to the kV term in drive * - * characterization. Defaults to 0 (no feed-forward). - * - * @param velCoeff The velocity feed-forward coefficient, in bump units per profile unit per - * second. - * @return This {@code FollowProfileFactory} in a builder pattern. - */ - @NotNull - public FollowProfileFactory setRVelCoeff(double velCoeff) { - this.rVelCoeff = velCoeff; - return this; - } - - /** - * Gets the right velocity intercept, or VIntercept. (See Eli Barnett's drive characterization - * paper for an explanation of why this is needed.) - * - * @return The velocity intercept, in bump units. - */ - public double getRVelIntercept() { - return rVelIntercept; - } - - /** - * Sets the right velocity intercept, or VIntercept. (See Eli Barnett's drive characterization - * paper for an explanation of why this is needed.) Defaults to 0. - * - * @param velIntercept The velocity intercept, in bump units. - * @return This {@code FollowProfileFactory} in a builder pattern. - */ - @NotNull - public FollowProfileFactory setRVelIntercept(double velIntercept) { - this.rVelIntercept = velIntercept; - return this; - } - - /** - * Gets the right acceleration feed-forward. This is equivalent to the kA term in drive - * characterization. Defaults to 0 (no feed-forward). - * - * @return The currently set acceleration coefficient, in bump units per profile unit per second - * squared. - */ - public double getRAccelCoeff() { - return rAccelCoeff; - } - - /** - * Sets the acceleration feed-forward. This is equivalent to the kA term in drive - * characterization. Defaults to 0 (no feed-forward). - * - * @param accelCoeff The acceleration coefficient, in bump units per profile unit per second - * squared. - * @return This {@code FollowProfileFactory} in a builder pattern. - */ - @NotNull - public FollowProfileFactory setRAccelCoeff(double accelCoeff) { - this.rAccelCoeff = accelCoeff; - return this; - } - - /** - * Gets the P coefficient for the heading PI loop. Defaults to 0. - * - * @return The currently set P coefficient, in profile units per radian. - */ - public double getHeadingP() { - return headingP; - } - - /** - * Sets the P coefficient for the heading PI loop. Defaults to 0. - * - * @param headingP The P coefficient, in profile units per radian. - * @return This {@code FollowProfileFactory} in a builder pattern. - */ - @NotNull - public FollowProfileFactory setHeadingP(double headingP) { - this.headingP = headingP; - return this; - } - - - /** - * Gets the I coefficient for the heading PI loop. Defaults to 0. - * - * @return The currently set I coefficient, in profile units per radian-second. - */ - public double getHeadingI() { - return headingI; - } - - /** - * Sets the I coefficient for the heading PI loop. Defaults to 0. - * - * @param headingI The I coefficient, in profile units per radian-second. - * @return This {@code FollowProfileFactory} in a builder pattern. - */ - @NotNull - public FollowProfileFactory setHeadingI(double headingI) { - this.headingI = headingI; - return this; - } - - /** - * Creates a new {@link FollowProfile} command to follow the provided profile. This is a - * convienience method that calls {@link #create(File, File)} with two files as described below. - *

    - * With a given profile name, this method will attempt to load a profile with name "profile" from - * two files; the left side from /home/lvuser/profiles/profile_left.csv and the right side from - * /home/lvuser/profiles/profile_right.csv. - * - * @param profileName The name of the profile. - * @return A new {@link FollowProfile} command with the previously configured settings following - * the provided profile. - */ - @NotNull - public FollowProfile create(@NotNull String profileName) { - return create(new File("/home/lvuser/profiles/" + profileName + "_left.csv"), - new File("/home/lvuser/profiles/" + profileName + "_right.csv")); - } - - /** - * Creates a new {@link FollowProfile} command to follow the provided profile. This is a - * convinience method that calls {@link #create(MotionProfile, MotionProfile)} after loading the - * trajectories using {@link Pathfinder#readFromCSV(File)} and converting them using {@link - * MotionProfileUtils#createProfile(Trajectory)}. - * - * @param leftFile The file to load the left profile from; should be a pathfinder-formatted CSV. - * @param rightFile The file to load the right profile from; should be a pathfinder-formatted - * CSV. - * @return A new {@link FollowProfile} command with the previously configured settings following * - * the provided profile. - */ - @NotNull - public FollowProfile create(@NotNull File leftFile, @NotNull File rightFile) { - return create(MotionProfileUtils.createProfile(Pathfinder.readFromCSV(leftFile)), - MotionProfileUtils.createProfile(Pathfinder.readFromCSV(rightFile))); - } - - /** - * Creates a new {@link FollowProfile} command to follow the provided profile. - * - * @param left The profile for the left side. - * @param right The profile for the right side. - * @return A new {@link FollowProfile} command with the previously configured settings following - * the provided profile. - */ - @NotNull - public FollowProfile create(@NotNull MotionProfile left, @NotNull MotionProfile right) { - return new FollowProfile(left, right, subsystems, leftSetpointConsumer, rightSetpointConsumer, - headingSupplier, loopFreq, lVelCoeff, lVelIntercept, lAccelCoeff, rVelCoeff, - rVelIntercept, rAccelCoeff, headingP, headingI); - } -} diff --git a/lib/src/main/java/org/team1540/rooster/motionprofiling/MotionProfile.java b/lib/src/main/java/org/team1540/rooster/motionprofiling/MotionProfile.java index 04c4531d..03b8c148 100644 --- a/lib/src/main/java/org/team1540/rooster/motionprofiling/MotionProfile.java +++ b/lib/src/main/java/org/team1540/rooster/motionprofiling/MotionProfile.java @@ -6,7 +6,7 @@ import org.jetbrains.annotations.Nullable; /** - * A sequence of {@link Point Points} that can be executed by a {@link FollowProfile}. + * A sequence of {@link Point Points} that can be used by motion profiling systems. */ public class MotionProfile { @@ -57,33 +57,27 @@ public static class Point { */ public double dt; /** - * The x-position of the robot in {@linkplain org.team1540.rooster.motionprofiling profile - * units}, or 0 if not applicable. + * The x-position of the robot, or 0 if not applicable. */ public double x; /** - * The y-position of the robot in {@linkplain org.team1540.rooster.motionprofiling profile - * units}, or 0 if not applicable. + * The y-position of the robot, or 0 if not applicable. */ public double y; /** - * The position of the profiled mechanism, in {@linkplain org.team1540.rooster.motionprofiling - * profile units}. + * The position of the profiled mechanism. */ public double position; /** - * The velocity of the profiled mechanism, in {@linkplain org.team1540.rooster.motionprofiling - * profile units} per second. + * The velocity of the profiled mechanism, in position units per second. */ public double velocity; /** - * The acceleration of the profiled mechanism, in {@linkplain org.team1540.rooster.motionprofiling - * profile units} per second squared. + * The acceleration of the profiled mechanism, in position units per second squared. */ public double acceleration; /** - * The jerk of the profiled mechanism, in {@linkplain org.team1540.rooster.motionprofiling - * profile units} per second cubed. + * The jerk of the profiled mechanism, in position units per second cubed. */ public double jerk; /** @@ -95,18 +89,13 @@ public static class Point { * Creates a new {@code Point}. * * @param dt The time change since the previous point, in seconds. - * @param x The x-position of the robot in {@linkplain org.team1540.rooster.motionprofiling - * profile units}, or 0 if not applicable. - * @param y The y-position of the robot in {@linkplain org.team1540.rooster.motionprofiling - * profile units}, or 0 if not applicable. - * @param position The position of the profiled mechanism, in {@linkplain - * org.team1540.rooster.motionprofiling profile units}. - * @param velocity The velocity of the profiled mechanism, in {@linkplain - * org.team1540.rooster.motionprofiling profile units} per second. - * @param acceleration The acceleration of the profiled mechanism, in {@linkplain - * org.team1540.rooster.motionprofiling profile units} per second squared. - * @param jerk The jerk of the profiled mechanism, in {@linkplain org.team1540.rooster.motionprofiling - * profile units} per second cubed. + * @param x The x-position of the robot, or 0 if not applicable. + * @param y The y-position of the robot, or 0 if not applicable. + * @param position The position of the profiled mechanism. + * @param velocity The velocity of the profiled mechanism, in position units per second. + * @param acceleration The acceleration of the profiled mechanism, in position units per second + * squared. + * @param jerk The jerk of the profiled mechanism, in position units per second cubed. * @param heading The robot's heading in radians, or 0 if not applicable. */ public Point(double dt, double x, double y, double position, double velocity, diff --git a/lib/src/main/java/org/team1540/rooster/motionprofiling/MotionProfileUtils.java b/lib/src/main/java/org/team1540/rooster/motionprofiling/MotionProfileUtils.java index b2bcd7f5..fd9e0563 100644 --- a/lib/src/main/java/org/team1540/rooster/motionprofiling/MotionProfileUtils.java +++ b/lib/src/main/java/org/team1540/rooster/motionprofiling/MotionProfileUtils.java @@ -1,8 +1,5 @@ package org.team1540.rooster.motionprofiling; -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.DemandType; -import com.ctre.phoenix.motorcontrol.can.BaseMotorController; import jaci.pathfinder.Trajectory; import java.util.Arrays; import org.jetbrains.annotations.Contract; @@ -14,32 +11,6 @@ public class MotionProfileUtils { private MotionProfileUtils() { } - /** - * Create a {@link SetpointConsumer} that sets the setpoint of the provided CTRE motor controller - * with no adjustment to the position setpoint. - * - * @param motorController The motor controller. - * @return A {@code SetpointConsumer} to use for motion profiling. - */ - public static SetpointConsumer createSetpointConsumer(BaseMotorController motorController) { - return createSetpointConsumer(motorController, 1); - } - - /** - * Create a {@link SetpointConsumer} that sets the setpoint of the provided CTRE motor controller, - * multiplying the position setpoint by the adjustmant. - * - * @param motorController The motor controller. - * @return A {@code SetpointConsumer} to use for motion profiling. - */ - public static SetpointConsumer createSetpointConsumer(BaseMotorController motorController, - double adjustment) { - return (setpoint, bump) -> - motorController - .set(ControlMode.Position, setpoint * adjustment, DemandType.ArbitraryFeedForward, - bump); - } - /** * Creates a ROOSTER {@link MotionProfile} from a Pathfinder {@link Trajectory}. * diff --git a/lib/src/main/java/org/team1540/rooster/motionprofiling/MotionProfilingProperties.java b/lib/src/main/java/org/team1540/rooster/motionprofiling/MotionProfilingProperties.java index 019d99e2..6238b7d9 100644 --- a/lib/src/main/java/org/team1540/rooster/motionprofiling/MotionProfilingProperties.java +++ b/lib/src/main/java/org/team1540/rooster/motionprofiling/MotionProfilingProperties.java @@ -5,7 +5,8 @@ import java.util.function.DoubleSupplier; /** - * @deprecated Replaced by the {@link FollowProfile}-based system. + * @deprecated Replaced by the {@link org.team1540.rooster.drive.pipeline drive pipeline}-based + * system. */ @Deprecated public class MotionProfilingProperties { diff --git a/lib/src/main/java/org/team1540/rooster/motionprofiling/ProfileFollower.java b/lib/src/main/java/org/team1540/rooster/motionprofiling/ProfileFollower.java deleted file mode 100644 index dbe663d7..00000000 --- a/lib/src/main/java/org/team1540/rooster/motionprofiling/ProfileFollower.java +++ /dev/null @@ -1,253 +0,0 @@ -package org.team1540.rooster.motionprofiling; - -import static java.lang.Math.atan2; -import static java.lang.Math.cos; -import static java.lang.Math.sin; - -import org.jetbrains.annotations.NotNull; -import org.team1540.rooster.motionprofiling.MotionProfile.Point; - -/** - * Helper to execute motion profiles. This class has no dependency on WPILib/any external library. - * - * The profile-following algorithm used here is a derivative of the algorithm used by Team 2471. The - * output of a heading-based PI loop is added to the profile's position setpoint and passed to the - * motor controllers, with additional velocity, acceleration, and static friction feed-forwards (in - * line with Eli Barnett's drive - * characterization method) added via throttle bump. - *

    - * It is designed to use native Talon SRX position PID control with a throttle bump, but the output - * could instead be used to control a RIO-side PID loop. - *

    - * This class is stateful; it keeps track of the last time {@link #get(double, double)} get()} was - * called, and also has an integral accumulator for the gyro PI controller. With that in mind, if - * using a {@code FollowProfile} instance multiple times, call {@link #reset()} before beginning to - * execute the second, third, etc. profiles. - */ -public class ProfileFollower { - - @NotNull - private MotionProfile left; - @NotNull - private MotionProfile right; - - private double lVelCoeff; - private double lVelIntercept; - private double lAccelCoeff; - private double rVelCoeff; - private double rVelIntercept; - private double rAccelCoeff; - private double headingP; - private double headingI; - - private double gyroIAccum; - - private double profTime; - - private double lastTime = -1; - - /** - * Creates a {@code ProfileFollower}. - * - * For an explanation of units, see the {@linkplain org.team1540.rooster.motionprofiling package - * docs}. - * - * @param lVelCoeff The left velocity coefficient (kV), in bump units per profile unit per - * second. - * @param lVelIntercept The left velocity intercept (VIntercept), in bump units. - * @param lAccelCoeff The left acceleration coefficient (kA), in bump units per profile unit per - * second squared. - * @param rVelCoeff The right velocity coefficient (kV), in bump units per profile unit per - * second. - * @param rVelIntercept The right velocity intercept (VIntercept), in bump units. - * @param rAccelCoeff The right acceleration coefficient (kA), in bump units per profile unit per - * second squared. - * @param headingP The P coefficient for the heading controller, in profile units per radian. - * @param headingI The I coefficient for the heading controller, in profile units per - * radian-second. - */ - public ProfileFollower( - @NotNull MotionProfile left, - @NotNull MotionProfile right, double lVelCoeff, double lVelIntercept, double lAccelCoeff, - double rVelCoeff, double rVelIntercept, double rAccelCoeff, double headingP, - double headingI) { - this.left = left; - this.right = right; - this.lVelCoeff = lVelCoeff; - this.lVelIntercept = lVelIntercept; - this.lAccelCoeff = lAccelCoeff; - this.rVelCoeff = rVelCoeff; - this.rVelIntercept = rVelIntercept; - this.rAccelCoeff = rAccelCoeff; - this.headingP = headingP; - this.headingI = headingI; - } - - /** - * Get the output from the motion profile at a given time (usually the current time). - * - * @param heading The current gyro heading, from 0 to 2π inclusive. - * @param time The time, in seconds, since motion profile execution began. This is used to find - * the segment to execute. - * @return A {@link ProfileDriveSignal} describing the necessary drive commands. - */ - @NotNull - public ProfileDriveSignal get(double heading, double time) { - Point leftSegment = getCurrentSegment(left, time); - Point rightSegment = getCurrentSegment(right, time); - - double headingTarget = leftSegment.heading; - - // basically magic https://stackoverflow.com/a/2007279 - double headingError = atan2(sin(heading - headingTarget), cos(heading - headingTarget)); - - double timeSinceLast = lastTime == -1 ? 0 : time - lastTime; - gyroIAccum += headingError * timeSinceLast; - - lastTime = time; - - double gyroPOut = headingError * headingP; - double gyroIOut = gyroIAccum * headingI; - - double leftVelFOut = lVelCoeff * leftSegment.velocity; - double rightVelFOut = rVelCoeff * rightSegment.velocity; - - double leftVelInterceptOut = - leftSegment.velocity == 0 ? 0 : Math.copySign(lVelIntercept, leftSegment.velocity); - double rightVelInterceptOut = - rightSegment.velocity == 0 ? 0 : Math.copySign(rVelIntercept, rightSegment.velocity); - - double leftAccelFOut = lAccelCoeff * leftSegment.acceleration; - double rightAccelFOut = rAccelCoeff * rightSegment.acceleration; - - return new ProfileDriveSignal( - leftSegment.position - gyroPOut - gyroIOut, - leftVelFOut + leftVelInterceptOut + leftAccelFOut, - rightSegment.position + gyroPOut + gyroIOut, - rightVelFOut + rightVelInterceptOut + rightAccelFOut - ); - } - - /** - * Reset the profile follower. - * - * This resets the follower so that it can be used multiple times. - */ - public void reset() { - gyroIAccum = 0; - lastTime = -1; - } - - /** - * Get the current integral accumulator for the gyro PI controller. - * - * @return The integral accumulator. - */ - public double getGyroIAccum() { - return gyroIAccum; - } - - - /** - * Get the current gyro error being fed into the PI controller. - * - * @param heading The current gyro heading reading, in radians from 0 to 2π. - * @param time The current time, in seconds. - * @return The heading error, in radians. - */ - public double getGyroError(double heading, double time) { - double tgtHeading = getCurrentPointLeft(time).heading; - - return atan2(sin(heading - tgtHeading), cos(heading - tgtHeading)); - } - - /** - * Get the currently executing {@code Point} on the left side. - * - * @param time The current time, in seconds. - * @return The current {@code Point}. - */ - @NotNull - public Point getCurrentPointLeft(double time) { - return getCurrentSegment(left, time); - } - - - /** - * Get the currently executing {@code Point} on the right side. - * - * @param time The current time, in seconds. - * @return The current {@code Point}. - */ - @NotNull - public Point getCurrentPointRight(double time) { - return getCurrentSegment(right, time); - } - - /** - * Gets the total time to execute the profile. - * - * @return The time to execute the currently loaded profile, in seconds. - */ - public double getProfileTime() { - return profTime; - } - - /** - * Get whether the profile is finished (i.e. time > {@link #getProfileTime()}} - * - * @param time The current time since the profile began executing, in seconds. - * @return {@code true} if the profile is finished; {@code false} otherwise. - */ - public boolean isProfileFinished(double time) { - return time > profTime; - } - - /** - * A signal to be sent to the robot drive, consisting of left and right position setpoints and - * feed-forward bumps. - */ - public static class ProfileDriveSignal { - - /** - * The left-side position setpoint, in {@linkplain org.team1540.rooster.motionprofiling profile - * units}. - */ - public final double leftSetpoint; - /** - * The left-side feed-forward throttle bump, in fractions of motor throttle (i.e. 0.5 == 50% of - * max motor throttle). - */ - public final double leftBump; - /** - * The right-side position setpoint, in {@linkplain org.team1540.rooster.motionprofiling profile - * units}. - */ - public final double rightSetpoint; - /** - * The right-side feed-forward throttle bump, in fractions of motor throttle (i.e. 0.5 == 50% of - * max motor throttle). - */ - public final double rightBump; - - public ProfileDriveSignal(double lSetpoint, double lBump, double rSetpoint, double rBump) { - this.leftSetpoint = lSetpoint; - this.leftBump = lBump; - this.rightSetpoint = rSetpoint; - this.rightBump = rBump; - } - } - - @NotNull - private static Point getCurrentSegment(@NotNull MotionProfile trajectory, double currentTime) { - // Start from the current time and find the closest point. - int startIndex = Math.toIntExact(Math.round(currentTime / trajectory.get(0).dt)); - - int length = trajectory.size(); - int index = startIndex; - if (startIndex >= length - 1) { - index = length - 1; - } - return trajectory.get(index); - } -} diff --git a/lib/src/main/java/org/team1540/rooster/motionprofiling/RunMotionProfiles.java b/lib/src/main/java/org/team1540/rooster/motionprofiling/RunMotionProfiles.java index 76548332..3bd922db 100644 --- a/lib/src/main/java/org/team1540/rooster/motionprofiling/RunMotionProfiles.java +++ b/lib/src/main/java/org/team1540/rooster/motionprofiling/RunMotionProfiles.java @@ -9,7 +9,9 @@ /** * Executes a set of motion profiles (with respective properties.) - * @deprecated Replaced by the {@link FollowProfile}-based system. + * + * @deprecated Replaced by the {@link org.team1540.rooster.drive.pipeline drive pipeline}-based + * system. */ @Deprecated public class RunMotionProfiles extends Command { diff --git a/lib/src/main/java/org/team1540/rooster/motionprofiling/SetpointConsumer.java b/lib/src/main/java/org/team1540/rooster/motionprofiling/SetpointConsumer.java deleted file mode 100644 index ba2c51a0..00000000 --- a/lib/src/main/java/org/team1540/rooster/motionprofiling/SetpointConsumer.java +++ /dev/null @@ -1,30 +0,0 @@ -package org.team1540.rooster.motionprofiling; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.DemandType; - -/** - * Functional interface to pass commanded values from {@link FollowProfile} to the motors. - * - * @see FollowProfile - */ -@FunctionalInterface -public interface SetpointConsumer { - - /** - * Sets the setpoint of the motors to be profiled. Usually implemented using the {@link - * com.ctre.phoenix.motorcontrol.can.TalonSRX#set(ControlMode, double, DemandType, double) - * set(ControlMode, double, DemandType, double)} method of CTRE's {@link - * com.ctre.phoenix.motorcontrol.can.TalonSRX TalonSRX}, with the {@code ControlMode} set to - * {@link ControlMode#Position} and the {@code DemandType} set to {@link - * DemandType#ArbitraryFeedForward}. - *

    - * See the {@linkplain org.team1540.rooster.motionprofiling package documentation} for an - * explanation of profile units, bump units, etc.. - * - * @param setpoint The position PID setpoint, in profile units. (Unit conversion to Talon SRX - * native units etc. should be performed within this method.) - * @param bump The throttle bump to apply, in bump units. - */ - void set(double setpoint, double bump); -} diff --git a/lib/src/main/java/org/team1540/rooster/motionprofiling/package-info.java b/lib/src/main/java/org/team1540/rooster/motionprofiling/package-info.java index 4890fd89..cc7256cf 100644 --- a/lib/src/main/java/org/team1540/rooster/motionprofiling/package-info.java +++ b/lib/src/main/java/org/team1540/rooster/motionprofiling/package-info.java @@ -1,25 +1,8 @@ /** - * Utilities for executing motion profiles. + * Utilities for motion profiles. * - *

    Summary

    - * - * This package's main business logic is in the {@link org.team1540.rooster.motionprofiling.FollowProfile} - * command, which follows a given Pathfinder motion profile. (A technical explanation of the - * profile-following algorithm can be found in the {@link org.team1540.rooster.motionprofiling.FollowProfile} - * documentation.) Additionally, this package contains {@link org.team1540.rooster.motionprofiling.FollowProfileFactory}, - * a class to create multiple {@code FollowProfile} instances using common configuration values (for - * example, multiple autonomous routines for a drivetrain). - * - *

    A Note on Units

    - * Many problems relating to motion profiling stem from units not being converted properly. As such, - * to simplify things, this package is unit-agnostic (with the exception of heading which is - * required to be in radians) and does no unit conversion on its own, instead asking that provided - * coefficients incorporate necessary unit conversion. When used in package documentation, the term - * profile units means the units used in motion profiles given to the {@link - * org.team1540.rooster.motionprofiling.FollowProfile} command (usually inches, feet or meters); the - * term native units means the units passed to the motor controllers (usually in terms of - * raw encoder counts); and the term bump units means the units passed to the throttle-bump - * parameter of {@link org.team1540.rooster.motionprofiling.SetpointConsumer#set(double, double) - * SetpointConsumer.set()} (usually percentage of motor throttle). + * This class contains common data classes to encapsulate motion profiles as well as a utility + * ({@link org.team1540.rooster.motionprofiling.ProfileContainer}) to load them from disk. It also + * contains legacy profile execution code, kept for backwards compatibility. */ package org.team1540.rooster.motionprofiling; diff --git a/test/src/main/kotlin/org/team1540/rooster/testing/MotionProfileTesting.kt b/test/src/main/kotlin/org/team1540/rooster/testing/MotionProfileTesting.kt deleted file mode 100644 index 55847000..00000000 --- a/test/src/main/kotlin/org/team1540/rooster/testing/MotionProfileTesting.kt +++ /dev/null @@ -1,312 +0,0 @@ -package org.team1540.rooster.testing - -import com.ctre.phoenix.motorcontrol.ControlMode -import com.ctre.phoenix.motorcontrol.FeedbackDevice -import com.kauailabs.navx.frc.AHRS -import edu.wpi.first.wpilibj.IterativeRobot -import edu.wpi.first.wpilibj.Joystick -import edu.wpi.first.wpilibj.SPI -import edu.wpi.first.wpilibj.command.Scheduler -import edu.wpi.first.wpilibj.command.Subsystem -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard -import jaci.pathfinder.Pathfinder -import jaci.pathfinder.Trajectory -import jaci.pathfinder.Waypoint -import jaci.pathfinder.modifiers.TankModifier -import org.team1540.rooster.Utilities -import org.team1540.rooster.functional.Executable -import org.team1540.rooster.motionprofiling.FollowProfile -import org.team1540.rooster.motionprofiling.FollowProfileFactory -import org.team1540.rooster.motionprofiling.MotionProfileUtils -import org.team1540.rooster.preferencemanager.Preference -import org.team1540.rooster.preferencemanager.PreferenceManager -import org.team1540.rooster.util.SimpleLoopCommand -import org.team1540.rooster.wrappers.ChickenTalon -import java.util.function.DoubleSupplier -import kotlin.math.PI - -private val driver = Joystick(0) - -class MotionProfileTestingRobot : IterativeRobot() { - private lateinit var factory: FollowProfileFactory - - private val navx = AHRS(SPI.Port.kMXP) - - private var command: FollowProfile? = null - - @JvmField - @Preference("lkV", persistent = false) - var lKV = 0.0 - - @JvmField - @Preference("lkA", persistent = false) - var lKA = 0.0 - - @JvmField - @Preference("lVIntercept", persistent = false) - var lVIntercept = 0.0 - - @JvmField - @Preference("rkV", persistent = false) - var rKV = 0.0 - - @JvmField - @Preference("rkA", persistent = false) - var rKA = 0.0 - - @JvmField - @Preference("rVIntercept", persistent = false) - var rVIntercept = 0.0 - - @JvmField - @Preference("MP Loop Freq", persistent = false) - var loopFreqMs = 0 - - @JvmField - @Preference("MP Heading P", persistent = false) - var hdgP = 0.0 - - @JvmField - @Preference("MP Heading I", persistent = false) - var hdgI = 0.0 - - @JvmField - @Preference("MP Drive P", persistent = false) - var driveP = 0.0 - - @JvmField - @Preference("MP Drive D", persistent = false) - var driveD = 0.0 - - @JvmField - @Preference("MP Delta-T", persistent = false) - var deltaT = 0.0 - - @JvmField - @Preference("MP Max Vel", persistent = false) - var maxVel = 0.0 - - @JvmField - @Preference("MP Max Accel", persistent = false) - var maxAccel = 0.0 - - @JvmField - @Preference("MP Max Jerk", persistent = false) - var maxJerk = 0.0 - - @JvmField - @Preference("Wheelbase", persistent = false) - var wheelbase = 0.0 - - @JvmField - @Preference("Drive TPU", persistent = false) - var tpu = 0.0 - - override fun robotInit() { - PreferenceManager.getInstance().add(this) - DriveTrain - - DriveTrain.brake = false - SmartDashboard.setDefaultNumber("Test Profile X", 0.0) - SmartDashboard.setDefaultNumber("Test Profile Y", 0.0) - SmartDashboard.setDefaultNumber("Test Profile Angle", 0.0) - - factory = FollowProfileFactory( - MotionProfileUtils.createSetpointConsumer(DriveTrain.left1, tpu), - MotionProfileUtils.createSetpointConsumer(DriveTrain.left2, tpu), - DriveTrain - ).apply { - lVelIntercept = lVIntercept - lVelCoeff = lKV - lAccelCoeff = lKA - rVelIntercept = rVIntercept - rVelCoeff = rKV - rAccelCoeff = rKA - loopFreq = loopFreqMs.toLong() - headingP = hdgP - headingI = hdgI - headingSupplier = DoubleSupplier { processedHeading } - } - } - - override fun autonomousPeriodic() { - } - - override fun robotPeriodic() { - Scheduler.getInstance().run() - if (DriveTrain.p != driveP) DriveTrain.p = driveP - if (DriveTrain.d != driveP) DriveTrain.d = driveD - SmartDashboard.putNumber("Raw heading", navx.yaw.toDouble()) - SmartDashboard.putNumber("Processed heading", processedHeading) - SmartDashboard.putNumber("Heading Error", command?.follower?.getGyroError(processedHeading, command?.executionTime - ?: 0.0) ?: 0.0) - SmartDashboard.putNumber("Heading IAccum", command?.follower?.gyroIAccum ?: 0.0) - } - - override fun disabledInit() { - DriveTrain.brake = false - } - - override fun disabledPeriodic() { - } - - override fun teleopPeriodic() { - } - - val processedHeading: Double - get() { - val yaw = Math.toRadians(navx.yaw.toDouble()) - return if (yaw < 0) PI - yaw else PI + yaw - } - - override fun autonomousInit() { - DriveTrain.brake = true - DriveTrain.zero() - factory.apply { - lVelIntercept = lVIntercept - lVelCoeff = lKV - lAccelCoeff = lKA - rVelIntercept = rVIntercept - rVelCoeff = rKV - rAccelCoeff = rKA - loopFreq = loopFreqMs.toLong() - headingP = hdgP - headingI = hdgI - leftSetpointConsumer = MotionProfileUtils.createSetpointConsumer(DriveTrain.left1, tpu) - rightSetpointConsumer = MotionProfileUtils.createSetpointConsumer(DriveTrain.right1, tpu) - } - - // generate new trajectory - val (leftTrajectory, rightTrajectory) = TankModifier(Pathfinder.generate(arrayOf( - Waypoint(0.0, 0.0, 0.0), - Waypoint( - SmartDashboard.getNumber("Test Profile X", 0.0), - SmartDashboard.getNumber("Test Profile Y", 0.0), - SmartDashboard.getNumber("Test Profile Angle", 0.0) - ) - ), Trajectory.Config( - Trajectory.FitMethod.HERMITE_CUBIC, - Trajectory.Config.SAMPLES_FAST, - deltaT, - maxVel, - maxAccel, - maxJerk - ))).modify(wheelbase).trajectories - - command = factory.create(MotionProfileUtils.createProfile(leftTrajectory), MotionProfileUtils.createProfile(rightTrajectory)) - - command?.start() - } - - override fun teleopInit() { - DriveTrain.brake = true - } - -} - -private val TankModifier.trajectories get() = leftTrajectory to rightTrajectory - -private object DriveTrain : Subsystem() { - val left1 = ChickenTalon(1) - val left2 = ChickenTalon(2) - val left3 = ChickenTalon(3) - - val right1 = ChickenTalon(4) - val right2 = ChickenTalon(5) - val right3 = ChickenTalon(6) - - val masters = arrayOf(left1, right1) - val lefts = arrayOf(left1, left2, left3) - val rights = arrayOf(right1, right2, right3) - - val all = arrayOf(*lefts, *rights) - - var p = 0.0 - set(value) { - field = value - for (talon in masters) talon.config_kP(0, value) - } - - var d = 0.0 - set(value) { - field = value - for (talon in masters) talon.config_kD(0, value) - } - - var brake = true - set(value) { - field = value - for (talon in all) talon.setBrake(value) - } - - override fun initDefaultCommand() { - defaultCommand = SimpleLoopCommand("Teleop Drive", Executable { - left1.set(ControlMode.PercentOutput, Utilities.processDeadzone(driver.getRawAxis(1), 0.1)) - right1.set(ControlMode.PercentOutput, Utilities.processDeadzone(driver.getRawAxis(5), 0.1)) - }, this) - } - - fun zero() { - left1.setSelectedSensorPosition(0) - right1.setSelectedSensorPosition(0) - } - - private fun reset() { - left1.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder) - right1.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder) - - left1.setSensorPhase(true) - - for (talon in lefts) { - talon.inverted = false - } - - right1.setSensorPhase(true) - - for (talon in rights) { - talon.inverted = true - } - - left2.set(ControlMode.Follower, left1.deviceID.toDouble()) - left3.set(ControlMode.Follower, left1.deviceID.toDouble()) - - right2.set(ControlMode.Follower, right1.deviceID.toDouble()) - right3.set(ControlMode.Follower, right1.deviceID.toDouble()) - - for (talon in all) { - talon.setBrake(brake) - } - - for (talon in masters) { - talon.config_kP(0, p) - talon.config_kI(0, 0.0) - talon.config_kD(0, d) - talon.config_kF(0, 0.0) - talon.config_IntegralZone(0, 0) - talon.configVoltageCompSaturation(12.0) - talon.enableVoltageCompensation(true) - } - } - - override fun periodic() { - SmartDashboard.putNumber("LPOS", left1.selectedSensorPosition) - SmartDashboard.putNumber("RPOS", right1.selectedSensorPosition) - SmartDashboard.putNumber("LVEL", left1.selectedSensorVelocity) - SmartDashboard.putNumber("RVEL", right1.selectedSensorVelocity) - SmartDashboard.putNumber("LTHROT", left1.motorOutputPercent) - SmartDashboard.putNumber("RTHROT", right1.motorOutputPercent) - SmartDashboard.putNumber("LCURR", left1.outputCurrent + left2.outputCurrent + left3.outputCurrent) - SmartDashboard.putNumber("RCURR", right1.outputCurrent + right2.outputCurrent + right3.outputCurrent) - SmartDashboard.putNumber("LVOLT", left1.motorOutputVoltage) - SmartDashboard.putNumber("RVOLT", right1.motorOutputVoltage) - SmartDashboard.putNumber("LERR", left1.closedLoopError.toDouble()) - SmartDashboard.putNumber("RERR", right1.closedLoopError.toDouble()) - SmartDashboard.putNumber("LTGT", right1.getClosedLoopTarget(0).toDouble()) - SmartDashboard.putNumber("RTGT", right1.getClosedLoopTarget(0).toDouble()) - } - - init { - reset() - } -} - From 15eaf4c5e2c7606d94a20ff3002557e35bd8c44c Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Fri, 14 Dec 2018 12:31:08 -0800 Subject: [PATCH 36/51] Split AdvancedArcadeJoystickInput into more components --- .../pipeline/AdvancedArcadeJoystickInput.java | 33 ++-------- .../FeedForwardToVelocityProcessor.java | 66 +++++++++++++++++++ .../VelocityToTurningRateProcessor.java | 33 ++++++++++ .../rooster/testing/DrivePipelineTesting.kt | 5 +- 4 files changed, 107 insertions(+), 30 deletions(-) create mode 100644 lib/src/main/java/org/team1540/rooster/drive/pipeline/FeedForwardToVelocityProcessor.java create mode 100644 lib/src/main/java/org/team1540/rooster/drive/pipeline/VelocityToTurningRateProcessor.java diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java index 58b0ca37..8edf01f2 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java @@ -25,8 +25,6 @@ */ public class AdvancedArcadeJoystickInput implements Input { - private double maxVelocity; - private double trackWidth; private boolean reverseBackwards; @NotNull private DoubleSupplier throttleInput; @@ -38,32 +36,21 @@ public class AdvancedArcadeJoystickInput implements Input { /** * Creates a new {@code AdvancedArcadeJoystickInput} that does not reverse while going backwards. * - * @param maxVelocity The maximum velocity of the robot; joystick values will be scaled to this - * amount. This should be in position units per second to keep with the specification of {@link - * TankDriveData}. - * @param trackWidth The track width of the robot (distance between the wheels); this should be in - * the same position units as maxVelocity. * @param throttleInput A {@link DoubleSupplier} that supplies the input for the throttle, from -1 * to 1 inclusive. * @param softTurnInput A {@link DoubleSupplier} that supplies the input for the soft-turn, from * -1 (full left) to 1 (full right) inclusive. * @param hardTurnInput A {@link DoubleSupplier} that supplies the input for the soft-turn, from */ - public AdvancedArcadeJoystickInput(double maxVelocity, double trackWidth, - @NotNull DoubleSupplier throttleInput, + public AdvancedArcadeJoystickInput(@NotNull DoubleSupplier throttleInput, @NotNull DoubleSupplier softTurnInput, @NotNull DoubleSupplier hardTurnInput) { - this(maxVelocity, trackWidth, true, throttleInput, softTurnInput, hardTurnInput); + this(false, throttleInput, softTurnInput, hardTurnInput); } /** * Creates a new {@code AdvancedArcadeJoystickInput}. * - * @param maxVelocity The maximum velocity of the robot; joystick values will be scaled to this - * amount. This should be in position units per second to keep with the specification of {@link - * TankDriveData}. - * @param trackWidth The track width of the robot (distance between the wheels); this should be in - * the same position units as maxVelocity. * @param reverseBackwards If {@code true}, reverses the direction of the soft turn when the * throttle is negative. * @param throttleInput A {@link DoubleSupplier} that supplies the input for the throttle, from -1 @@ -72,12 +59,10 @@ public AdvancedArcadeJoystickInput(double maxVelocity, double trackWidth, * -1 (full left) to 1 (full right) inclusive. * @param hardTurnInput A {@link DoubleSupplier} that supplies the input for the soft-turn, from */ - public AdvancedArcadeJoystickInput(double maxVelocity, double trackWidth, - boolean reverseBackwards, @NotNull DoubleSupplier throttleInput, + public AdvancedArcadeJoystickInput(boolean reverseBackwards, + @NotNull DoubleSupplier throttleInput, @NotNull DoubleSupplier softTurnInput, @NotNull DoubleSupplier hardTurnInput) { - this.maxVelocity = maxVelocity; - this.trackWidth = trackWidth; this.reverseBackwards = reverseBackwards; this.throttleInput = throttleInput; this.softTurnInput = softTurnInput; @@ -117,14 +102,6 @@ public TankDriveData get() { rightPower = rightPowerRaw; } - // omega (dtheta / dt or yaw rate) is just the difference in velocities (powers) divided by the - // track width - - double leftVelocity = leftPower * maxVelocity; - double rightVelocity = rightPower * maxVelocity; - - double omega = (rightVelocity - leftVelocity) / trackWidth; - - return new TankDriveData().withVelocities(leftVelocity, rightVelocity).withTurningRate(omega); + return new TankDriveData().withAdditionalFeedForwards(leftPower, rightPower); } } diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/FeedForwardToVelocityProcessor.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/FeedForwardToVelocityProcessor.java new file mode 100644 index 00000000..4afe9b48 --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/FeedForwardToVelocityProcessor.java @@ -0,0 +1,66 @@ +package org.team1540.rooster.drive.pipeline; + +import java.util.OptionalDouble; +import java.util.function.DoubleSupplier; +import org.team1540.rooster.functional.Processor; + +/** + * {@link Processor} that converts a feed-forward/throttle into a velocity setpoint. This processor + * multiplies each feed-forward by the maximum velocity of the robot. This is useful when running + * closed-loop teleop drive code (as joystick inputs usually provide feed-forwards). + */ +public class FeedForwardToVelocityProcessor implements Processor { + + private DoubleSupplier maxVelocitySupplier; + private boolean clearFeedForwards; + + /** + * Creates a new {@code FeedForwardToVelocityProcessor} that clears feed-forwards in the returned + * data. + * + * @param maxVelocity The maximum velocity of the robot. + */ + public FeedForwardToVelocityProcessor(double maxVelocity) { + this(maxVelocity, true); + } + + /** + * Creates a new {@code FeedForwardToVelocityProcessor}. + * + * @param maxVelocity The maximum velocity of the robot. + * @param clearFeedForwards Whether to set the feed-forwards in the {@link TankDriveData} to empty + * {@link OptionalDouble OptionalDoubles}. If {@code true}, the value will be cleared; if {@code + * false}, it will be passed through as-is. + */ + public FeedForwardToVelocityProcessor(double maxVelocity, boolean clearFeedForwards) { + this(() -> maxVelocity, clearFeedForwards); + } + + /** + * Creates a new {@code FeedForwardToVelocityProcessor}. + * + * @param maxVelSupplier A supplier that supplies the maximum velocity of the robot. + * @param clearFeedForwards Whether to set the feed-forwards in the {@link TankDriveData} to empty + * {@link OptionalDouble OptionalDoubles}. If {@code true}, the value will be cleared; if {@code + * false}, it will be passed through as-is. + */ + public FeedForwardToVelocityProcessor(DoubleSupplier maxVelSupplier, boolean clearFeedForwards) { + this.maxVelocitySupplier = maxVelSupplier; + this.clearFeedForwards = clearFeedForwards; + } + + @Override + public TankDriveData apply(TankDriveData data) { + double maxVelocity = maxVelocitySupplier.getAsDouble(); + return new TankDriveData( + new DriveData(data.left.position, + OptionalDouble.of(data.left.additionalFeedForward.orElse(0) * maxVelocity), + data.left.acceleration, + clearFeedForwards ? OptionalDouble.empty() : data.left.additionalFeedForward), + new DriveData(data.right.position, + OptionalDouble.of(data.right.additionalFeedForward.orElse(0) * maxVelocity), + data.right.acceleration, + clearFeedForwards ? OptionalDouble.empty() : data.right.additionalFeedForward), + data.heading, data.turningRate); + } +} diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/VelocityToTurningRateProcessor.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/VelocityToTurningRateProcessor.java new file mode 100644 index 00000000..56ad962c --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/VelocityToTurningRateProcessor.java @@ -0,0 +1,33 @@ +package org.team1540.rooster.drive.pipeline; + +import org.team1540.rooster.functional.Processor; + +/** + * {@link Processor} to calculate a turning rate from left and right velocities. The {@link + * TankDriveData} returned from this method will have all fields identical to the one passed, except + * for the {@link TankDriveData#turningRate turningRate} field. + */ +public class VelocityToTurningRateProcessor implements Processor { + + private double trackWidth; + + /** + * Creates a new {@code VelocityToTurningRateProcessor}. + * + * @param trackWidth The track width of the robot (distance between left and right wheels). + */ + public VelocityToTurningRateProcessor(double trackWidth) { + this.trackWidth = trackWidth; + } + + @Override + public TankDriveData apply(TankDriveData tankDriveData) { + if (tankDriveData.left.velocity.isPresent() && tankDriveData.right.velocity.isPresent()) { + return tankDriveData.withTurningRate( + (tankDriveData.right.velocity.getAsDouble() - tankDriveData.left.velocity.getAsDouble()) + / trackWidth); + } else { + return tankDriveData; + } + } +} diff --git a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt index 5ec5df96..b7cf9910 100644 --- a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt +++ b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt @@ -87,14 +87,15 @@ class AdvancedJoystickInputPipelineTestRobot : DrivePipelineTestRobot() { override fun robotInit() { PreferenceManager.getInstance().add(this) val reset = SimpleCommand("reset", Executable { - _command = SimpleAsyncCommand("Drive", 20, AdvancedArcadeJoystickInput( - maxVelocity, trackWidth, revBack, + _command = SimpleAsyncCommand("Drive", 20, AdvancedArcadeJoystickInput(revBack, DoubleSupplier { Utilities.scale(-Utilities.processDeadzone(joystick.getY(GenericHID.Hand.kLeft), 0.1), power) }, DoubleSupplier { Utilities.scale(Utilities.processDeadzone(joystick.getX(GenericHID.Hand.kRight), 0.1), power) }, DoubleSupplier { Utilities.scale((Utilities.processDeadzone(joystick.getTriggerAxis(GenericHID.Hand.kRight), 0.1) - Utilities.processDeadzone(joystick.getTriggerAxis(GenericHID.Hand.kLeft), 0.1)), power) }) + + FeedForwardToVelocityProcessor(maxVelocity) + + VelocityToTurningRateProcessor(trackWidth) + (FeedForwardProcessor(1 / maxVelocity, 0.0, 0.0)) + UnitScaler(tpu, 0.1) + (CTREOutput(PipelineDriveTrain.left1, PipelineDriveTrain.right1))) From 5288970b1709d9cf0fe92d5798997279fc19cd02 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Tue, 18 Dec 2018 08:50:29 -0800 Subject: [PATCH 37/51] Begin motion profiling documentation --- docs/Motion Profiling.md | 108 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 108 insertions(+) create mode 100644 docs/Motion Profiling.md diff --git a/docs/Motion Profiling.md b/docs/Motion Profiling.md new file mode 100644 index 00000000..00588449 --- /dev/null +++ b/docs/Motion Profiling.md @@ -0,0 +1,108 @@ +# Motion Profiling with ROOSTER + +This is a start-to-finish guide to implementing the robot side of motion profiling using ROOSTER. Generation is a separate matter (and not covered by ROOSTER at the moment)—this covers the steps after you've created motion profile CSV files and deployed them to a folder on the RoboRIO. + +## Loading Profiles + +Profiles can be pre-loaded from a designated folder on the RoboRIO using a `ProfileContainer`. A `ProfileContainer` automatically loads motion profiles from Pathfinder-style CSV files and keeps them in RAM until they are needed. + +For an example, let's say that your folder structure is like so: + +``` +/home/lvuser/ + profiles/ + foo_left.csv + foo_right.csv + bar_left.csv + bar_right.csv +``` + +To load these into two motion profiles named "foo" and "bar" (each containing left and right sides), use a `ProfileContainer` as follows: + +```java +import edu.wpi.first.wpilibj.IterativeRobot; +import org.team1540.rooster.motionprofiling.ProfileContainer; + +public class Robot extends IterativeRobot { + public static final ProfileContainer profiles; + + @Override + public void robotInit() { + profiles = new ProfileContainer(new File("/home/lvuser/profiles")); + } +} +``` + +You can then access the left and right profiles in a `Command` or anywhere else you might need them: + +```java +import edu.wpi.first.wpilibj.command.Command; +import org.team1540.rooster.motionprofiling.ProfileContainer; +import org.team1540.rooster.motionprofiling.MotionProfile; + +public class ProfileCommand extends Command { + @Override + protected void initialize() { + ProfileContainer.DriveProfile profiles = Robot.profiles.get("foo"); + MotionProfile left = profiles.getLeft(); + MotionProfile right = profiles.getRight(); + + // do something with the profiles + } + + // isFinished, etc. +} +``` + +### Changing the Suffix + +Let's say your profiles use a different naming scheme. For instance: + +``` +/home/lvuser/ + profiles/ + foo.left.csv + foo.right.csv + bar.left.csv + bar.right.csv +``` + +The `ProfileContainer` supports differing file formats by specifying the suffixes in the constructor: + +```java +import edu.wpi.first.wpilibj.IterativeRobot; +import org.team1540.rooster.motionprofiling.ProfileContainer; + +public class Robot extends IterativeRobot { + public static final ProfileContainer profiles; + + @Override + public void robotInit() { + profiles = new ProfileContainer(new File("/home/lvuser/profiles"), ".left.csv", ".right.csv"); + } +} +``` + +Once you have your left and right side profiles, you need to do something with them. Execution is a fairly complicated topic involving a number of different steps. + +## Empirical Testing + +The first step in accurate profile execution is an accurate model of your robot's behavior. While all of these quantities can be calculated theoretically, it is much better to do them empirically. + +### Ticks Per Unit and Track Width Testing + +Ticks per unit (TPU) is a number representing the correspondence between encoder ticks and real-world position of the wheels on the ground. While TPU can be theoretically calculated by simply dividing the wheel circumference by the number of ticks per rotation of the wheel, it is better measured empirically to account for tread wear and other factors. + +Track width (also erroneously called "wheelbase" in certain libraries) is the distance between your robot's left and right wheels, and its theoretical calculation is even simpler—just measure the distance. However, wheelbase is very susceptible to effects from wheel-scrub and other factors (especially if you are using multiple sets of traction wheels). Note that this measurement is often fed into your generation solution, so you may want to go back and remake your paths. + +Both track width and TPU can be tested with the `WheelbaseTestRobot`, in the `org.team1540.rooster.util.robots` package. `WheelbaseTestRobot` is a fully self-contained robot class that should be deployed onto your robot. To deploy the `WheelbaseTestRobot`, set your robot class to `org.team1540.rooster.util.robots.WheelbaseTestRobot` and deploy. + +Once you deploy and connect a driver station, it will throw several values onto SmartDashboard or Shuffleboard. Set your left and right motor IDs and inversions as necessary, then run the Reset and Zero commands. + +#### Ticks Per Unit Testing + +To measure TPU, push your robot some defined distance (for example, 10 feet) and note the LPOS and RPOS readouts on the SmartDashboard. (They should be near identical.) Then divide them by the distance your robot traveled to get your ticks per unit. For example, if you push your robot ten feet and measure LPOS and RPOS of about 4500, then your ticks per unit (in this case ticks per foot) would be 450. + +#### Track Width Testing + +Run the Zero command again, then enter your TPU value from the last step into the "encoderTPU" readout. Additionally, it is reccomended to set the "brake" readout to `true` and run the Reset command. Then, enable the robot (in tele-op mode) and hold down the A button on controller 0 until the robot completes ten full revolutions. Your empirically calculated wheelbase width (in whatever units you used to calculate your TPU) will then be placed in the "Calculated Width" readout. \ No newline at end of file From cfb78b23c88b2c1fb17f09d9c8d144105f33e540 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Tue, 18 Dec 2018 10:50:33 -0800 Subject: [PATCH 38/51] Enable voltage compensation --- .../rooster/util/robots/VelocityCharacterizationRobot.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/lib/src/main/java/org/team1540/rooster/util/robots/VelocityCharacterizationRobot.java b/lib/src/main/java/org/team1540/rooster/util/robots/VelocityCharacterizationRobot.java index c1c5a43d..3ed95fdc 100644 --- a/lib/src/main/java/org/team1540/rooster/util/robots/VelocityCharacterizationRobot.java +++ b/lib/src/main/java/org/team1540/rooster/util/robots/VelocityCharacterizationRobot.java @@ -134,6 +134,8 @@ public void robotInit() { driveLeftMotorC, driveRightMotorA, driveRightMotorB, driveRightMotorC}) { if (motor != null) { + motor.configVoltageCompSaturation(SATURATION_VOLTAGE); + motor.enableVoltageCompensation(true); motor.configClosedloopRamp(0); motor.configOpenloopRamp(0); motor.configPeakOutputForward(1); From 5a1f6ff723b56f54c4688acb2d993328839bf4bb Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Wed, 19 Dec 2018 14:33:14 -0800 Subject: [PATCH 39/51] Remove VelocityToTurningRateProcessor Unsure if useful or workable. May re-add. --- .../VelocityToTurningRateProcessor.java | 33 ------------------- .../rooster/testing/DrivePipelineTesting.kt | 1 - 2 files changed, 34 deletions(-) delete mode 100644 lib/src/main/java/org/team1540/rooster/drive/pipeline/VelocityToTurningRateProcessor.java diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/VelocityToTurningRateProcessor.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/VelocityToTurningRateProcessor.java deleted file mode 100644 index 56ad962c..00000000 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/VelocityToTurningRateProcessor.java +++ /dev/null @@ -1,33 +0,0 @@ -package org.team1540.rooster.drive.pipeline; - -import org.team1540.rooster.functional.Processor; - -/** - * {@link Processor} to calculate a turning rate from left and right velocities. The {@link - * TankDriveData} returned from this method will have all fields identical to the one passed, except - * for the {@link TankDriveData#turningRate turningRate} field. - */ -public class VelocityToTurningRateProcessor implements Processor { - - private double trackWidth; - - /** - * Creates a new {@code VelocityToTurningRateProcessor}. - * - * @param trackWidth The track width of the robot (distance between left and right wheels). - */ - public VelocityToTurningRateProcessor(double trackWidth) { - this.trackWidth = trackWidth; - } - - @Override - public TankDriveData apply(TankDriveData tankDriveData) { - if (tankDriveData.left.velocity.isPresent() && tankDriveData.right.velocity.isPresent()) { - return tankDriveData.withTurningRate( - (tankDriveData.right.velocity.getAsDouble() - tankDriveData.left.velocity.getAsDouble()) - / trackWidth); - } else { - return tankDriveData; - } - } -} diff --git a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt index b7cf9910..1293765f 100644 --- a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt +++ b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt @@ -95,7 +95,6 @@ class AdvancedJoystickInputPipelineTestRobot : DrivePipelineTestRobot() { - Utilities.processDeadzone(joystick.getTriggerAxis(GenericHID.Hand.kLeft), 0.1)), power) }) + FeedForwardToVelocityProcessor(maxVelocity) - + VelocityToTurningRateProcessor(trackWidth) + (FeedForwardProcessor(1 / maxVelocity, 0.0, 0.0)) + UnitScaler(tpu, 0.1) + (CTREOutput(PipelineDriveTrain.left1, PipelineDriveTrain.right1))) From 96e1c2a38ad95cd89c69396d78808220455b1b48 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Wed, 19 Dec 2018 14:35:16 -0800 Subject: [PATCH 40/51] Fix erroneous documentation --- .../rooster/drive/pipeline/AdvancedArcadeJoystickInput.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java index 8edf01f2..0e64a1ed 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java @@ -16,9 +16,8 @@ * here. *

    * This class is an {@link Input} that provides a {@link TankDriveData}. The resulting {@link - * TankDriveData} will have the left and right velocities set with units corresponding to the max - * velocity set on construction, as well as the turning rate in radians per second. All other values - * are empty {@link OptionalDouble OptionalDoubles}. + * TankDriveData} will have the left and right feed-forwards set to throttles between -1 and 1. All + * other values are empty {@link OptionalDouble OptionalDoubles}. * * @see Input * @see FeedForwardProcessor From e4358648b80069736730c390346b720bb5630514 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Wed, 19 Dec 2018 14:37:29 -0800 Subject: [PATCH 41/51] Document HeadingPIDProcessor --- .../team1540/rooster/drive/pipeline/HeadingPIDProcessor.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/HeadingPIDProcessor.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/HeadingPIDProcessor.java index 441dfcee..ffff6a6c 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/HeadingPIDProcessor.java +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/HeadingPIDProcessor.java @@ -2,6 +2,9 @@ import java.util.function.DoubleSupplier; +/** + * A {@link PIDProcessor for maintaining a robot's heading.} + */ public class HeadingPIDProcessor extends PIDProcessor { private DoubleSupplier headingSupplier; From 271376c507e854dd350f6484c17350e900d13de4 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Wed, 19 Dec 2018 14:44:15 -0800 Subject: [PATCH 42/51] Improve ProfileInput Add documentation and an isFinished() method. --- .../rooster/drive/pipeline/ProfileInput.java | 35 ++++++++++++++++--- 1 file changed, 30 insertions(+), 5 deletions(-) diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/ProfileInput.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/ProfileInput.java index 61ac942d..2715e19c 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/ProfileInput.java +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/ProfileInput.java @@ -7,6 +7,13 @@ import org.team1540.rooster.motionprofiling.MotionProfile; import org.team1540.rooster.motionprofiling.MotionProfile.Point; +/** + * Class to get commanded drive values from a set of motion profiles. + * + * This class is non-reusable; the first call to {@link #get()} begins a timer to determine where + * the input is in the profile. To execute multiple profiles, create multiple {@code + * ProfileInputs}. + */ public class ProfileInput implements Input { private MotionProfile left; @@ -14,13 +21,21 @@ public class ProfileInput implements Input { private Timer timer = new Timer(); + private boolean finished; + + /** + * Create a new {@code ProfileInput}. + * + * @param left The left-side profile to execute. + * @param right The right-side profile to execute. + */ public ProfileInput(@NotNull MotionProfile left, @NotNull MotionProfile right) { this.left = left; this.right = right; } @NotNull - private static Point getCurrentSegment(@NotNull MotionProfile trajectory, double currentTime) { + private Point getCurrentSegment(@NotNull MotionProfile trajectory, double currentTime) { // Start from the current time and find the closest point. int startIndex = Math.toIntExact(Math.round(currentTime / trajectory.get(0).dt)); @@ -28,12 +43,13 @@ private static Point getCurrentSegment(@NotNull MotionProfile trajectory, double int index = startIndex; if (startIndex >= length - 1) { index = length - 1; + finished = true; } return trajectory.get(index); } @Override - public org.team1540.rooster.drive.pipeline.TankDriveData get() { + public TankDriveData get() { if (timer.get() <= 0) { timer.start(); } @@ -43,13 +59,13 @@ public org.team1540.rooster.drive.pipeline.TankDriveData get() { Point leftPoint = getCurrentSegment(left, timeValue); Point rightPoint = getCurrentSegment(right, timeValue); - return new org.team1540.rooster.drive.pipeline.TankDriveData( - new org.team1540.rooster.drive.pipeline.DriveData( + return new TankDriveData( + new DriveData( OptionalDouble.of(leftPoint.position), OptionalDouble.of(leftPoint.velocity), OptionalDouble.of(leftPoint.acceleration), OptionalDouble.empty()), - new org.team1540.rooster.drive.pipeline.DriveData( + new DriveData( OptionalDouble.of(rightPoint.position), OptionalDouble.of(rightPoint.velocity), OptionalDouble.of(rightPoint.acceleration), @@ -57,4 +73,13 @@ public org.team1540.rooster.drive.pipeline.TankDriveData get() { OptionalDouble.of(leftPoint.heading), OptionalDouble.empty()); } + + /** + * Returns whether the end of the profile has been reached. + * + * @return {@code true} if the end of the profile has been reached, {@code false} otherwise. + */ + public boolean isFinished() { + return finished; + } } From e575c4409daefc35884df6ed91e101f8b6f62c38 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Wed, 19 Dec 2018 14:48:59 -0800 Subject: [PATCH 43/51] Expand motion profiling docs This isn't fully complete (needs a section on how to tune the drivetrain PID and some other things) but I'm satisfied for now. --- docs/Motion Profiling.md | 49 +++++++++++++++++++++++++++++++++++-- docs/img/responsecurve.png | Bin 0 -> 121802 bytes 2 files changed, 47 insertions(+), 2 deletions(-) create mode 100644 docs/img/responsecurve.png diff --git a/docs/Motion Profiling.md b/docs/Motion Profiling.md index 00588449..fa51743a 100644 --- a/docs/Motion Profiling.md +++ b/docs/Motion Profiling.md @@ -95,7 +95,7 @@ Ticks per unit (TPU) is a number representing the correspondence between encoder Track width (also erroneously called "wheelbase" in certain libraries) is the distance between your robot's left and right wheels, and its theoretical calculation is even simpler—just measure the distance. However, wheelbase is very susceptible to effects from wheel-scrub and other factors (especially if you are using multiple sets of traction wheels). Note that this measurement is often fed into your generation solution, so you may want to go back and remake your paths. -Both track width and TPU can be tested with the `WheelbaseTestRobot`, in the `org.team1540.rooster.util.robots` package. `WheelbaseTestRobot` is a fully self-contained robot class that should be deployed onto your robot. To deploy the `WheelbaseTestRobot`, set your robot class to `org.team1540.rooster.util.robots.WheelbaseTestRobot` and deploy. +Both track width and TPU can be tested with the `WheelbaseTestRobot`, in the `org.team1540.rooster.util.robots` package. `WheelbaseTestRobot` is a fully self-contained robot class that should be deployed onto your robot. To deploy the `WheelbaseTestRobot` from a project with ROOSTER already added as a dependency, set your robot class to `org.team1540.rooster.util.robots.WheelbaseTestRobot` and deploy. Once you deploy and connect a driver station, it will throw several values onto SmartDashboard or Shuffleboard. Set your left and right motor IDs and inversions as necessary, then run the Reset and Zero commands. @@ -105,4 +105,49 @@ To measure TPU, push your robot some defined distance (for example, 10 feet) and #### Track Width Testing -Run the Zero command again, then enter your TPU value from the last step into the "encoderTPU" readout. Additionally, it is reccomended to set the "brake" readout to `true` and run the Reset command. Then, enable the robot (in tele-op mode) and hold down the A button on controller 0 until the robot completes ten full revolutions. Your empirically calculated wheelbase width (in whatever units you used to calculate your TPU) will then be placed in the "Calculated Width" readout. \ No newline at end of file +Run the Zero command again, then enter your TPU value from the last step into the "encoderTPU" readout. Additionally, it is reccomended to set the "brake" readout to `true` and run the Reset command. Then, enable the robot (in tele-op mode) and hold down the A button on controller 0 until the robot completes ten full revolutions. Your empirically calculated wheelbase width (in whatever units you used to calculate your TPU) will then be placed in the "Calculated Width" readout. + +### Feed-Forward Testing + +The next step in your testing is creating an accurate model of your drivetrain performance. A standard (non-shifting) FRC drivetrain's performance can be generally described with three numbers: namely, a velocity constant, a voltage intercept, and an acceleration constant. (For shifting drivetrains, you simply have two sets: one for each gear.) The velocity constant is essentially how many additional volts must be provided to the motor to achieve a speed increase of 1 unit. The acceleration constant is the same, but with acceleration. The voltage intercept is a bit more interesting: it is the y-intercept of a line with velocity on the x-axis and voltage on the y-axis. In other words, it is the voltage needed to overcome static friction effects. + +![response curve](img/responsecurve.png) + +This picture shows the approximate velocity-voltage curve (with voltage on the y-axis) for our 2018 robot. Note how the Y-intercept is nonzero; i.e. a velocity of just above zero requires a good deal of additional voltage to overcome static friction. Testing shows that in FRC drivetrains, this static friction remains regardless of additional velocity, as seen on the graph. + +#### Determining Velocity Curve + +Determining your velocity constant and voltage intercept is done in the same test. The theory of the test is essentially collecting data on the motors' velocity as the voltage is slowly increased, and then using linear regression to fit a line of best fit to the data. This process is handled by the `VelocityCharacterizationRobot` in ROOSTER. Deploy the `VelocityCharacterizationRobot` by following the steps described [here](#Ticks Per Unit and Track Width Testing) but using `org.team1540.rooster.util.robots.VelocityCharacterizationRobot`. Configure your inversions, motor IDs, and TPU as described above, then run the Reset command. Place the robot so that it has as long a distance as possible to drive straight, enable the robot, and hold down the A button until your robot reaches an obstacle. Your left and right kV (velocity constant) and vIntercept (voltage intercept) should be placed on the SmartDashboard. Note that these constants will use volts as their unit, and it is likely that you will want the feed-forwards to be in percent throttle—in that case, simply divide them by 12. + +#### Determining Acceleration Constant + +In general, it is theoretically possible to determine the acceleration constant empirically. However, if you find yourself unable to do so, you can divide 12 by your robot's theoretical maximum acceleration. The maximum acceleration is equal to your number of motors, multiplied by each motor's stall torque, multiplied by your gear ratio, divided by your wheel radius, divided by the mass of your robot. (Make sure your units line up when making this calculation: maximum torque is often given in newton-meters for FRC motors.) + +Documentation on how to determine acceleration coefficient empirically coming once we figure out how to do it properly. + +## Running the Profile + +Motion profile execution is tightly integrated into the drive pipeline system. As such, creating a motion profile execution system is as simple as constructing a drive pipeline with a number of stages, and wrapping it in a `SimpleAsyncCommand`: + +```java +Executable pipeline = new ProfileInput(leftProfile, rightProfile) + .andThen(new FeedForwardProcessor(velocityConstant, velocityIntercept, accelerationConstant)) + .andThen(new HeadingPIDProcessor(hdgP, hdgI, hdgD, Gyro::getHeadingInRadians, true)) + .andThen(new UnitScaler(tpu, 0.1)) + .andThen(CTREOutput(leftMotor1, rightMotor1)); + +Command command = new SimpleAsyncCommand("Run profile", 20, pipeline); +command.start(); +``` + +Breaking the pipeline segments down in detail: + +- The `ProfileInput` takes left and right profiles and runs through them according to a timer. +- The `FeedForwardProcessor` processes velocity, acceleration, and voltage constants and adds them to the additional feed-forward. +- The `HeadingPIDProcessor` adds a heading-based PID loop to keep the robot driving at the correct heading, and adds the output to the position setpoint on each side. Note that it takes a lambda or method reference in addition to coefficients. This is used for getting the heading in radians from the gyro. (Note that the range should be from -π to π as described in its documentation.) +- The `UnitScaler` re-scales units from whatever your profile units happen to be in to the raw encoder ticks accepted by the motors. +- The `CTREOutput` takes the fully processed command and sends it to the left and right motors. + +Since drive pipelines are `Executable`, they can be passed to a `SimpleCommand`. `SimpleAsyncCommand` is a variant of `SimpleCommand` which executes the code in a different thread at a fixed interval—in this case, 20 milliseconds. + +The `HeadingPIDProcessor` should be tuned in the manner of a normal PID loop; additionally, you should tune the Talon SRX closed-loop position PID. (More on how to do this coming soon.) \ No newline at end of file diff --git a/docs/img/responsecurve.png b/docs/img/responsecurve.png new file mode 100644 index 0000000000000000000000000000000000000000..97262eb52c410bc2856e5f42851a4af38133e400 GIT binary patch literal 121802 zcmZ^K1yozzwlz|qxD_d0v`7;u#jTVUO3?x>?(Py?f)^--;2NM5FB;q(fhi%4Gj&${qfRziv3t} zcv9SkhDN|{EhD2UFC)XG>f&f&ZD)>#_Aw$+6GuzEpE6T7E+%FSgEWTBn>X6#Aj!5{HXxpi!$L`-C0$XYrEHq0iD5~6*N0-4qzk^v13WOqXPB3ZSD^Iw=8AWjyx?%D zuUrGCkmr@VHn&#qSACQ#8AEPu1LsNi_dExbI% zME4}%ZGBp9MP3kH4EhuMmh+aQbm<$#4;{=e8371b-_$i+Sv@xxs_xB0= zdjW~pW$uzmOINM&$Jbk~PDoRial2=j2+TT`)UIBxe5`(QL~Zj#>ci&SGVO}~Yw$SV zmoT?(CKD#U1Zu_VPB&-Ad2-?~6~k#dC!u-26oimZC;2;^l@}|b+3`+3jXzayF+uF% zXc;l4N2=~cOO|2jGUX%nQU_RfGE2gk2&~$A@*nGAH|^6Ssy7mv+aG(EY(&;0?c`+p zbNX7%1{t3hY84AZJY&D?+~~aY%!}6GHp<`;FRoKs1P@D`tFY`-m$eMJT7@9_9{Fop@{3^9t7%uOt zx`SN73=_l$Q{wCjeb2?dZ5vq?W{*BCHs(}U78dsKIEHrNo;MA zqir3wZYvseS-0T@nC{aR2^(g2nG?@Qedp5S6m3U4&`;uJ!CA%-4+K8NAb)S%;lL# zsNP%An7NNs~)QPYGef0+2ASB<9sqwKBF&%`Z;Y9fhH zgLmaQ8C&?(jF>5}d)t5T>JAa&xBsdz;~^wBJ*_Y)At>&?FpfG>FzgY98SJqrcW(=m z*M*)6JcGbRJOPr#hSAAuO-G`)1YnB8&YPq1Cp6L!Bbg(nil^~Uw?m$XFu!|l%(3!x zg-1_pA;jZ7bF6eVQ)6dM^7|pC5q1EF1N#9-CaXIMBk7GiYaCJkxAhO-nPKu?xy<9T z;|$}t;}mK_+re>_aoKUfY87ggajHeCMT*vSTTWwYZ3*sIJJ->P?!+aB`C3U{2}JzhlZ13}cdoV_$lIS7_98oqplc z*wH8}Y}Q~aPAj@DQdWPZ`bu3Q*IT?zo~{+lT};Te6gaR*fBFA0AWYQ_GtAB_tkfx-hA>;SbDiwXr6Ns)%bd+<-*F| z&_2^0b^+esoy~Gh5tSBA6)kq#aXZ*uZJcT(ceC7b_#;s~C#D0c-n1IoTK$6~xAaHm zyyV>E%wl(c%=YgtLTHTm57oBgq*UpWZ9DIgKp(G`t<(HZc#OlA{YzdwThGoT5%tJz z_>hT%QKs?5^X=!MH1bikv^CEOsdcI0v{W>QLamPP_?-FW`L%7w?Zz!_tZ0ly;nh=T z(a&dwhJQHLE|&D{wKRl!{kkGM^**(voTcQ6#E8_1^htW7z&NNk_-WAg>w9jYV#VU& zulrMlHB|yjjh)w(gf?p?EEO|$6MWxzd;NYmDQ={s*J|iW&H2vh%$31@z*bE0;ZqPl zkr7wDlls4FI=y~1pxVL8;>sr5a$~8E*0I(<_cg&anDFe*l{A)NzG>%H)8I934qA@P z3CaT95!cP9n`|S)BZhi<`rUvVJ#RfFeRVydhDFy2*GPBW1NX&@<%2`XrI!A?oUll_ zOt>_pO~PJ+U&6yjlGc$gQ>fMEmIY`EtVOt&D?8?Ue7_VtRAyvhtX0Vyl`$kYEIU+Q zDgb{4KL`8V8eGp^MczSBZNx`UQ)r!Dli(BJ@(|hKk3aoR%th=?Vn+vM(4(EFTBUm} zwr=+F#Ty=1w=Jb4$;htX#^*{-8N42ysk}B?c3NK3T~51B&Yq@EX!u^3D0R1dtomqd z=Jzbz z6(=&11oo$0|8DPXhp9c~jNoiim#oT}PTafK__~unDXENRDY1!#NN!4+>pERcJju-W z%1g|bP`LbL7{AzcxsrVBgM}XC(oy_l>__SIL;jrC#r!x`WK|cI+~x&lj1)Fc`9m#T z2wW_XuHkm?Kl~cegUq`%_^t9DE8{Y!a#{CHgoN#VEy_Eives<%0Drg-dydObG^~-u z37rug^q#8JR>2R#2d77kB8MC@P2X6qK+77ypt?~RWZsBvrsHe>xR;qkviekU!>gNtX1nySn;lWmOMHp^1zQ8{?!CH_*>|Oz$1#vz)Q? zxP4qGX>9cd`&r7U$-w{M%B9mv&_ZtcE+9MXIr-mBb>Lqa3g`@deUILEMBK;!if<_x zSCO#ET=cy>%bh@sD2h+G3-0Y*fjTyo>rgdN;X}99?I3t)4%Gr+%Fv}j1Fa__o|ejim3? zDLba$Cd}0 zP)YP1Yrg0^Tf*bt-p3)geja@Y6{746>*x@=MvI|AM=>+d(M^jBqw?_LS_s?w^?EW? z*(=4i1i7Ev*yaT=ZY;qfow)t*E!ZCe2VAF*+8{JEO16J4ba^$_qsN#6+gd}*RZB@x z)YQ=)U~J}SVh-@McY17%h9>SQ`gm(^?rO~BX>aEM67`f|{#Ogp$NPVZfy_+*YT{}u z!K|gE$|U3HV$LK0cmv>Nmc(OXViI>Tvk+B#FZ*BSk53ZJR<5p2qClXBhX=reAK>U> z3FH$I5dreP2EKm%>aoQuke7q2vF9rX5X-+i`QQD#HwT%zSUb5|J327^)333KqnoP) zGxI+K{rB(R^ECIg{-2Q?K>xL@#{~lasR8lfGgoVi|Bu-}HUBpI*Sh{aocKST ziK<$An%il=x3)KT06k7klJB*Ep!mPW`M;|EXQuyQs`VeH{CvXyY5E^k|6%&iTtrn| ztj!&~pn*edYLqmIuCjVYq z!xMe4MZ20+&g&JOA2PEb$IFKB$GfMvxP(mFy{%NH!9XM4)AN%W!#xgQ+x*7vHhP;E zEU9Hk2MmKXAS^0S9laNh^$gS@*kb`tKFU6oe^ta*Gr6;^=+}EdE5_xK3Cwx@`!#=S zp}Je+yWi2_K$eR-Q+RT0thU?lS}3glW&Yp4UqEMDpR4?r!2E)e`9TEVb!+YTIFzBh zjMvLyLl4&r59SVSckVJf!j$Pr$;rdVYw%%_O{l4?tgOk(>~)r?+rZGsh^Q}N>v%_J zXHaTZmdErsgKqwT)K$OAP%Mkw(5t%nRVHglkK99Q>hs`~nL5WTY&}|R zoQX_WxSDBDFf>BNz0~~da}UCZ=wEA@9@MW9!PDKNq>c2uXT>BWO3@MDVqNZnkl~jt zK9{vu_9iin;6tA!khABo07CesR|#(I5{^$mL4S-ZGV;P+b>mWwhKFt`&G-BbLISUoUmGOIVN8e*<$FD?xLP zO|i77;bHR-h%5@?J~AZ2DazP6St9^+1`=1 zrnQmJC_BB$?h|>E^ag+N+JjxPNJQ2BcaM9CwPy5awVo)cglAxvp%74p==~`lD(GB6 zBDHQktiZ>@aRqfq@z-m_UYJ#t})4XxP2LA010*7NTk8;kTd zJd$3oAw8!zH;pLxO>J2TXepXMPV0C5u{GOqmx#rI*q6eB=m*xump{b9J{G?6MXg=# zia@ZjluED<12jn2g|HbohtARF^4|6&hp~`2hT*nZOT6JdM5oYc4XYSsQt*G5U$;jbv1a(HHX;{;Z9yr_hrV!gYbjDiibe z&ExM7yoEkPvH#A^99_3HHDbttW6cxH!B2K0^7HxZ!VUWj;GdteycupS?ml`8}olOyr%WgGa3$dMMJogG^Q5?$NM6>vel=yKc50XU`hHBqH9`7`4E z=#LmN=XJhWo6VUuE5;^4Wo?|}!Z}3m4Q*ECBE7xK9@asprCb0Jj?DDS z`J#*|5WD+rSrlP%H;15$wMXK&hmUqADuMg;ke%ZsI`!RmzS~vV^JTOGavw}`0FdlO zR#MDqUD+uW<=J#2%Ihan#+PGMUO)^AXC>vD8WD=xlj z*h)C6ujV*HNpF3;p!m%#M~K9?!xd|2#oQ1EDf-h+Zf@=aF-V0p4I`t+QkTr8(k&Vl z(AScp6@i07NecTqlDB3(<)mb6PEJaqlWg^Iv~%T26Y@OHeInUv>)_xpVvW%%Oxcx= zYVY+BKxbSslxe~itO$sp4n!_P4hJJP0B#-7)f+z&U!Q7jfV>{ftNANXLa|Z5Ag|zqh|2{+MA|(@t*wZ7_TBzO_1J``V1ir0 zGlzYk-Q*~@R|hDDrqpB?< zF1E1F_Odclm=OOkcft(K18HVE~M%l$4|X8?!|~l&G3r$te`w zF#)DY)0Jz(rpR=}gh%z^DMflfAU+9Z47g81mfDGh!LtjZ&#nIxeq!MdXPiz2=+SwJ zvvUYm|776v1REL!2?+#AC723?F=mPFq~S)g)U!Af=%A>j&ZoMu0Rq^7bHu6=ok@9F zY2#I{gy1F=!%&^-C`hoI%|9yT1@@I1Xr0D+1QZ4pfx~pF+PcGtS)(KD(~#|fMB(H{ zIb{)1H6$oH%xd|X);E8dCLyy<1~smMZL$0;S67&RZ(xC=!p9cY z%|vW*z?MoRsH$n@%W8DPsc9%*@NqZ0k?=?o(6xizMe&CAVFe9$DUBE>4HrE~Ox$-u z%Dvgsnr$zWG<>=AMeV5V?&Mbw98BN+5PDIAS&)1tD#?Eg z18F%{z~Cf103UPX&*J)Kd*InQ^)!SSW_~>1C0UN*u$jfhXRT*3Ks}Qk#6g+ZcZmEh z^%-y@T~lcQ8)tlg7HBoK)9+G(i@`Lu0wZ8O(c36@fDpA*MI4(UXYKd zTV<^xUi|EzoZ_tVn{(e*EByfy=()yP9HWI5wzQ&lO?H15u*>f%lPVvLf9iH|%+pq# z61XFYoYt9-z&tpUPXC%oMn&i6s}$vH_ev=|I9@&;=p1VcN)~6UtyXy8Gml^lUb|#- zz#BRG;5$_Om08ndurZ%_RZOvE1BR7c+OS^MDYo`hajUdQ1QlECA3a{ihizoNdIeQm zwKY|s_Z>b4BwCCk0SzK{H-D0!MKtGqg@MC2OeYv3A@`E*lfBAh@#dm&A_Ge5=(=8# z5%dG;E*4U&l_Sk%8Fyw3-KxS0a}|khnm=FWeKc^GU^z{hOFZv)_LHKJ+qnep_6iOS z&T7E?*Y0Z3=U!qg5)bIe)=GTrmQ@U>6a&)&fglH_0z?`Sp>C z3~s^sLV3;RRB0`#COB?fyAH$qgP+%SZ)9sUU>`Dl8~e@qKvJ6|`vMx%vMoHOLOuaE zk+3o4SgK4gkFm@Uu~Ul2mhBUQ$#@kSei3uNyKvhF`%eZ_^JS?G)72Qm{h$35eq!Iu z+~{4ZL~O{hag4gZ^{zwzYLn$$Hd!0l^?1N6VwLSXJ>@WS#GGad9)-9eVb#8LQ>pOf z$7AV2wAK5f=dWNd+mZv=d@g#8bhBx1g0PK+otP^#Ib`nx16|YO6d@$FldYVB*P%}D zE}pff8P%TJM$p&5;@`g$vzf`aPHh^?zY{wjgi#!DeY(r-eC3vJMW?a&olGNeV6EWO zTDYM6T`qJtVfyQ6ZsYqQ&BYqTn&uheau&(#@$%))pN_$!F^exuA8vEoht3@Xi|zN` ziS5iAzR#}U{xW^1z$H@q+TnnO!r_4Fv$3idM;yzO^w$S@qvFbWe3ej;iU@)ewvSTZ zBIC4$`9R?1Ucn2Ovy_Q|Qu}Xw2}E{Qdr=siBTLUxQfc%i{tCLceD>NhFMOND8FR$C zUlmKVO%Q<56*YvWsK%&9*Q>;+{OJi>O;iRlopv>3lpPeiPT?8Aj+ z3S_WGh_EIF-S3$zHh!$)#XA9f7iY`V)@)DN7Jm*gUO8Nd_scfn&^Ag(JxE6s2>SW( z76ktqj@eWV1Z(`HBF22QZDC3`i9_X<*#~O4$ojI?XLb;99cJQA+R9;icSz2yy?k>o ztq;_nCbr_axK5zIrsY!2e+!fxEv$vvvJ|7*%K}Qn@&DjYn^%!oE7TbO$o*U6W24_l zMjeCQGR_&AMW=oRjZ;tl2d8~3@Y5hV#-FiOzeFk%QoR@7gX;EW3T_lPZPKfhf3OL5>$;Y`0tP`;dF&nwfV8t8)MeZJ22!52ASGg{i=5xSj8vC>4aYkmT6Ir+Ard!7R=jXHpLW`V<_ zk?=WD6CuUN-*4ka#ZUV2Pjgv5VlG~)(5Tt&t`zUA7#E~=yx36(%w6fvxnN!=EAh#zdKY`QJdO1Avrez5>p8jEhP)K(VBBtE zZ-^+UxyFEaM=-y(qvt2#nnv}poHoKgj(;&^&#D&0ybdc-8Fvd@()K>e8h$s z@2L9eQ8_u@yUCD@cc3jEH~DO_5jOVdi1X?BO@>I0n?bD*iKM1I;>Ak9V4O@wW%tXi zN`OCo{ku7m+G;A%$r2KrhIM5hrTTXu6A~&Bvi;Cr0q*W27~I0b!scCQdRPCzz;_W5 z(D;w&sI=o_=ft{C6}nka=o=1|Oji^L1hVk*nt_VCefmHbURhsn^%G0j)R|!6MTi`u zB*txMC>Fn%*zVD1L?_PF?QOG9pcwKkJ}nCdyK5lw2y}gBE#T_}hnAI<{kA1KXt(su zAvn;%6#_0x3JN0MAUb<*1G&#KQ&ODxqCuay`6N97rKP1s+wbhGh$L$5=kmWr9b+>3 z20~6ICll}_N2rEaMm~27S9j2$Ev#?|qIZ=>J};YU{AduKtg%Z~0ULP->Soy~nxYaa zcPDp8R(G;6a1+&=42KFXm-W-mVrK19t;&%zPK!fGB6W80J%hPj(E-S>AU|};Rr*F% zkkUc7fsT$#nw6<3QBf1}I`k@JL<7xqWT@Yc=Q&drT`|Pr z*MNB|N5>)<62`DhAhn*6tlNIYx1^~p=DLy_Cxk-U7ey|w-6kC++-=RlWre~B?|hX) zqK<}&Vh8!b!q{2sgnlmeuX#>hTCqiMn3AqNkz(CZ_MP@SD0}fO?p#`bq)zPM#|2~i z_u5J9{r&|3id!9hX`+QH5s#WxDK)AexGL!Fw9Re3q&W`qw7uKZNS98_^ge zbmH*ggY(un=&yMY9JJi_%3?kNX*ctKa3Un}PSMM>2mEG4XPME{z0v9dUU~j(n8fhg z1_pCQcf@TvcQe*w>KYu)*Xz8|GmKFG%5I60Wp*sWANV*~5)h2(LI;(48WqALb}-6u zbmI0$08s03y0?~{QEhIX5&;w#-Q-x4*0D1Dm}&tf-uZF=u{$4Gc&b(4$!gF+#T_J8 z{S(=jCnzEt##ismTpRON+vsLqpIi(elr>Xt9A7k9F!r%F5w%PNSXciY%a*)G9&ReL zuoivwga{Ni95FQe`s@6(1i=WgzQ|JQ*X79W6lv`pwPpF}p+8!zt1f#-gAGq4P3R+h zUF;&Lxavfajlb~h9=$}J44oiro(H~dW`zexpt1(sF`0?Bv%?ZhubLFQ!}XZanqaaGZ1RZZ4ZT3Ii;dm>^97kNiqg z0s8c3TQ)^j<;lB=8{K!d=$68|bq;Rx*^%y@@%YUb`x6-oC-Y4n<@hNQJ{M!TNoxnB z9ZwlK$&k-*`Qn)aA4W37{$^(jypazitQ1amLWI{(Dz{@hcIUcxn&A0v1=qagm&WhL z9rpoK;_6hkD47Y_==Ml5CDi^gKR!A8Bx5308k>03Y|V{R;^%)!1r& z=X9ebhGsqTGOBPmO+XFSuIBdyn>!4^nJkaZ?uZ-dCfOpXmHzp&zk!&o)kc9~8c7qB zyOGeEBVS3*U|Bb5#${*nj7hcwyHi9`dD! z;qd?}HX0DjZkXrFb25b_+CZ<2aH@e8WKD7Rx!6?A&E02WX zR8$HP*vND0`B%JK$gKQ3t)&;QGs z=|p;CKVptDdGUDKrn3GxJntypHr}O7-9#p5616BkS(cC7Tq*922-El3^Ba@?0+FOv z;lh*2z!=CE#!)k}I1_<5(~3!twW;?5Y0z7CQrK#F5FJlz?AJJZpY5o_7r!l)R#iSY z9K3_id0yg{lPqN~sQ(=B{bIe=F;!aI^I1Otu&=)6;k*ipr3bd z{rQ*7Lk#WfV~S!!v_Vw?ozyKnyLj&6Zl>EL>URDAqgN!zb^OM677MkcPn3>ge1z#zYc?La`|Yi4zZn{HqA?Z-GIke_FSk?f@Uujf zLh-FAlsQ*jRjFPTX;!+)if*q}GE#vhzF)IzVQQR*lYyqa3g70x?X<;4oSD%!ty5{_ zz04;0ACvc z^p(d*n#ZR!t=6}ku$4hX&!tpJ!55~Q`=Yh5o!p~%jCkKvN6I^q%vrD10)u+@M( z^RVvuykZUWFA{38dzkat$`a6!kWC82Tz!=OyCAGop^gkwWAO~GmhozG93MN+Umm1| zh_KyEOg}QYmL%(~@@$MQPitJE$!udZd&RmHx8Hh~Fc;8=;`(WnV}z@B^>6h+!Ezu!oeN~L9^zXd**cdt-n3eJ|-uc(Qb24Y7sXXp0>U|mg|@(*$Vh_ zeI}Fs_)*vTP0u?sXCFx{?&fPRkV6L2eWBg^_%Fq z(&lzBX%)G|eU{9JG|9v8+UDy;h(0SWO5h3;i>%&r|*!F_Q-K?TKj^{iL8CJLf(q zo&L`MO^H0S`$fyyxB?RRZ~`1|TDo%pJreiz!?c2akBNo4t3698TLZ5R3VsO*d&%oR zkThM#mcQrm>41HFy5kg0I<*apA}4Ngt68VFn2p;`=@%2tF@hUo)rJh|xIzUDkbKsr zGli(79>2>j+#0tL3E$yittHzzeLoMsqA|N^v%ie577OzQuqb+R2W7#rD;`?Rdp~L%wLmE3|N<~rA-NoFn8$#dmehYHn*L+b^E>O4No_=+I*>;## zA;skq55Mmc55&L?2BOcsFvG-pBZN*kPu#?+`nD_)cNvGsg?bHB$lNC6*lyKs4tV4> zL@i`skFnh8pzEA%Wt&W&T*Wj;2B{5W@b0^va=c3t@jA)%(SfgRJ)qVg=}T7`OMGqD z#cejdaQLu!vcAPZ%ca5T!XX&ZyshLoAo9_$?8j#_T9Po$&Z1J42z0uB_^3H(Mmgz^ z?9^vjX3E`>l)Em|!jpPO8U59Pd!AF_*@25{e#@KuhIMR$wnb9Sp~&^F%I-Mx5&J|& zZ;qsO^mN5;fGzjjhIxlr-Aqsu_rTAH{^l{|?k}mMuW4^?Ji+w)C-)4a^ME4&hZF^YVKVHJ!F|u0P&5-KvBA#dOIb~Jhg9TrsJRH z(x-aYgMMbS*VjKmi?Yn-`^GL>PemdJyDPib_)<6F5pcaXE~r{%MY_0+W^4JS+mx5< zlBKVJmXmW%SQgKTJCVYv=$mX=J0ugMGl-2$#@biaq!ccJ5%#H7&qesc^pj>rMnQY$ z+e3bR_dzbv-1cP2Da(DYqvQEOGdSkkKq*R>H~}U~P>Q(-yh!_{i**ElZ^seSE)nhf zs)RZI6Ni;fdAnT~Qqb@RsS87GZD*AoK=aYN(*pnN0&F){@dlq!pT)HS-zIkpi(c{L zc#a{Tr5giF;(L}`UabxslLl}1`M(=juQ2!6x%kW;^l*&O7|9ZhECHexV5vzq4hD~l zy%1I^j}@~3%e5hazUJD$ewkZYQ5|lu4LU=fZ|1>H^LGC5u8SPtg&d$MeoVzxq#fRv8|Fkp>FH;YdFbC z#hu}@i*ExlRPI7*GcsUb zf9_Q-)8;qlBzjYh%|V7W9c}6```v1?*8b8lsp4m@6yRIQGOxd$3CCfTN@T?5J6-bv zl@F>anpD1?&E}3#_3aA`PW8-X&pFSp= zIWV$Spg05g_9}~7VGa_oLVnwgo=i`tq*A9lsVuVgDhVyD{{}W61*K+W4BwdWhmB71 zy?XU3RmE>d9eETPHmq_tsq$yfMSNJs2le*X=+~aRIW7xn8HH^6D8b~5hzQEfKY#uR z63i&Oj&j5awq=wAiy2h&SsEgxeJDf40h`7qzMUnC(@`#z!gdp13&@cKZ;(?XtwVnj z=1(0a5f`VJsu%I*2*hTx@u0hxwo|9@x+mM$?0GCHxO>JFJnhRV47KkB6+~7@Cs%r+ zFB$DP4Z*P`PWP!>^)I=8cv{_r4^w4L_)6`vY%f?sP+RO3e+EobNq4 zEt}>Ie=^O%2=q!`^~G8dzn#WjjExf=C0*fO^ z`d^+Z1>tTz{k?!M@UEaY^vW;dl)*4(t!CY&TNs!0p?wgez7{x(>Zh`n~t3QV;59g<9*;e-421PsFsLC{Hqv z)o$D>%Tb2Sr|!*1W(OyOz|pM9z`ohpG_O4vTyIVR(9nG+u*V^FC4U;B;@c^H))F?f z*y1w`@xL_<9$uU()gAtmBS)Btl(<@tN8X-}diZPO_qqg7$i)b;IHJ%lTK#TLq^83= z@-SEbN%G$b@E!%?`>@<9`YeF-}(tb-HKlO9?s!M1>f6?GzP2FPhx78DA#b9Us-I$#&21wQ84gu_U7AI+U?$yQ%? zw?yU2G~`#~LB)^8Esn2OfrD#WPd zP037~e_I!!wK^58-Z3G;81{T@qw}FBbTc5nSfG4is+|24*Z#Qk{He~e4xRyh!xxgU zJJQ|yGG;g-&MFkn)&3-%1k<+vm>j+^zpB@XoRr9hhvpeG3pO%Jf#)-Bv`o!Pptay$ z+5O%jhj3yBZN99AO|R3=`M9er$F?y&@PS}eeWit4&qiqm&T*`5xncWzo_Cfd!0;>3 zsD!4uK8NEL6QYOjN$aQ1P9u*VCP);v2gK5tx$uTaQ@b@W^QPSlWeqdi~X>PVVR+$M%^*|@i;^OSW(L93p zy!>7+ho?1$2{bBZ8>EdSLy|UkFoHt80t9JH(Rw>?gS7Q^-8??pq**cx`wr6M;-|1n zR%GK)${Fmvu8ZQ|!4vPr_g!w@Zan_IO#%8IN+P+S9(Ykx#%@w9auh)5r&qEZ5lkk? z>va!_j;aNG#6vCr@Crdr(mO-WaV2xq-0#cFJa>O#IKsKR97_#L&#_E9Qr?jDLZUt! z?_;>;@+@OVk3F}gLQwT8RUiDcY6uoWo#GPQ+OVy%CzOq4{mn(^kYjzu3O{ zakqSve63@ht|&eE(fqYJrk?E`2J(?c)z;SUMa#x~bnK53*_v-o#7uI&>5ISbl3hopJGy(Y`=i4Ax7cwF!-5ug$THs^b3WT?wr#%N zDPGXbYtK(~>JWCm4VjT@F~qM72#!468!DNpYdhB{b>giI`anPXv6T4*{=#wfZeJ`5 z@_M%%I4!_+itla=qmn>oXh@i3RW!TJ$uH6WOl@^hRSI2XQ<r9K@WzAH!<*;jXx^I3IzaYN3dwL3Crt3Ge{LCQP z^u%$rvH4ASJqKYBl-sMn*wcN(2)+|RQaMT*UD!GLt>>0Hw+Ec#BO;#(VTMzQSATLY zP1-eC;dc*oo_cLc+RhwI6%>Yx{+)xgVl9GBS(vr@;jBQ){(fEMzT1ytAedNk>wwuw zx;G;Fu+j9L;T||JH01Tc(>G)l$LrzZ|1dbm$4Ozm3uTYI+<3lr?*baC;G=DEsn#R? zqWo46k*`c zj!F{i^$|dOosER)!tK81*83<8p#7j&u^G`)h-IDf~Xe{o;74^*)_jfBYNCo0n zBa6MwokgF0FXVy{J+D~Z-?>R?@`&;xQl|zmy5^}b2Edeb_t#A$`KuG(J-klO*ye6H z(LDmZkC>+y+N)2Wg@M9uxSrYFww@%G*Au<~2d;KT0(k89!f88`z&i>3w0_mFK<)NA zF|p=SSt((L_;4kAEl5X#A3eij!Z4B?Zl+yZs>`mq%2ysqZlmJMfat~%B;!k+(43fu zCrYN741kHGe3GM~se|rH1Cw*Ik&S@m_%#nL!?Ys{#*-xPzSnbqht*HnB|T$-r;U1d z7v0zl1|>K9X;W;Of|9U2%F6i!{+lb5jGcH@#jeRkNvJn(frAogE9!}O^ZEA>rG=r+ z%B$p?EP;okK4Z(9BSNY)k9nt}8A*YIS@VT?nDRs4KX2U3J2}Btu%01YgTg3#`xqhJpKGH_1qP&of-O$Bo!`h=b{!{7$ zu%XH8ONdbEVQnk~f&Z+R#(#ugwURz&yLBKS~NwTT?KM8o;f=V)Vbgf(kP0?yZ|rL5O6P)FB^o5zy8?`&fPa z>6POSN+l{wCrj(L%I_9-O?X2bjp2D=QupMd+z>w0(Zw>$duxv~ z6jd%VN|=9~@8FcXHSos4l*7I1OV$sgcy_6?AKrCQkJ0|ZtSi0sPw6jlSfcrzg!L2rEMpT(T3g42Ojpj&t)}MZbFf}~pxc;brIW#1bgBH$ z%?G!TTDpH4%5YFadi^MHt$bc_<65?1u^-D}$Y)r;`O|AXR0MRI=Q9-M&^F$@^Gibc zPS*eBXY<8tjbswH-loSKxyR(i*UY7#vihy|*%A17-BZ0AN0WHK`XK(J#1EhJaaJKXQLBMl{H6>%gkY zReBt{ENDvt<|h|!rX!J#`}qf<4HT!*hyx+VCY!JZ*T(Zm%IW&GroC@Pd1z}AGrfS+ zv94j+*Vg#$G8y&9uu!?}q34Rc-nX{?;b*_t-o_uQ3J2LJ(iewplRQ`eN)3pHVM9 zIal7zF386Cgh#L*s%*U{lhW}O&Ct@s!>u&ysjfD^q0h4RX{t}A0Gv{D*k{o*)9Ycb zjMv9QF)h#hMm@S&l5L)qure+9vfCl%FM|ss7q5m<;oEmup76}E9ujYIuAkSGU`)UH z{1Na4ju1Pj;Omg)RHP@Vd=9C zgtcRw0*|&s(8lEpgG@_*yn)80RtJ6jf=Qqy?93>akIkL62E3DL1NPHvEBhra2&b8T z@gqh*v<*u>igA#?8)#O(I|?_>@X{Bbk$lWB47So5B*8K~U(x?FyE?j8d*2<SP5w=gmJMV9qD3VJ|E&WAC49JK$aMdy;nJxG~&RMqnf zist9z!o$3R8zj z*>#J$O7mpuRO}A^KDZ7N2?3<&S}`MJ#fxaGOlhCf^FbIzt?v&OH;1~yHirc^6;nM7 zst|h4BO$3(^I!QKBG))fSQZ1hVRyA|yPnBURJ^kv)dpCE^_j~wc)lf3(_Kd5FkI^U zM&WOfHfyYl4Cr1@n2u0EGEW&lEM5!ylT#_0Sb>ReIR3T-%ct@FE5m;xV)K~PxC z&IzJ8`4@qO`UnGYZSzqEmXhwSM?*JbDFwYuSMcI_S8^@)g+Y5e*(E$PmDA_Vc}RL%MG_;8g2&rJuc zX+g@^BU`dH)2UkT+4+a9!N)M5#Vj*bn@`GbgWf4aq|Zl=~g-KT$z&m5q$TuS;D7U^l$5){w*_@b|`0Y(j;7qijUSe2pupD8E zXLs}{Vf}Y@zNNztQKk>Y{!6#XS-k`^4eLt0*|=f!pYh$7<45+5Q$G3x>ImL0O-?_( ztOYZd;NcpX+IL(hGUw^E1>m=kM$R^Z^t&5EJf2J~wQH(9B|@nMxq7x^lx0mIiQeDJ zjOchuCMKY|FZ_C^ib5eOK0K1wH`9+80h1%1-7unbSpOtpdK_>EPhbNRL@y=Ra|(AM zL|dD)F106%{})m3;ZJ4%!2cVeGDD$bRKkro_8v)ALghBHJ2K*MtYhTZAz4Z0F_Mt% zy*KCBI}VP`A&%7v2M6D)`}6(&e*XZ@^>D81{Tk2LmFOIosXUX{!foca&8cYK)o(6c zm*0(Md3gK+rytOlnOl%~n2JTmbqH-EnXN(QTOS0xDt8wI1wl6I$h9~DxbHvV*;kvk zX~3y%7gL6~LfZoDUTffftuQv9A;(C%`M$UM+hy0d$O653cpmR$TC-gX^TTk>zopl> zE6XOy16w!isgT;xmki$D-GhB|nyLSNW4Llk;|@v6dv=DYwL4dwL2w7Q7%mE-s}F=u z=p3yIVKzT?q#1ao%Y0TX@|5Sq_9A9}`CjFHz4y+@`{^n|uuG&W9FbJBWA6+K@}8}m zX(FeJJCiGxWb`U}%}@Qu?b2?8Y$xgAquBnGwrAV3-X30l^wxM2bPhgI%6seM)|9bh z=gH1g-nM&4RP7{H5Lw0RpnG_09>g#LH%BwH++at@avqlCso!}b+2Zd!#Q@v$IUJn| zR+(j(lwv|>tIj%i!l9*6IuCx^Tb`FejxTldl4@S18P4SJI(dH`uhoRZZJ3%(aah^jlpQkP?usnUC@b$I7_DF3 zOKg1S`zKR5fjdBUSo@I>Erg1}FT>c;yy4mgdqdqnCaBGEF*r$JEN>vyiA(>xi0?Pf zpeJ~<+bAjf7_<%D*_1I^5Sh5Nygd#$?5Sd`6NzY^=GXi?Jo<<~`O)(R?_4t3J%jFt zD61z5z-z7@8b)$=f15?=yKa5S!D&8a8OUDbR2&N*0%GpPI{4;T7#V)Zx=}C$JP@5`IFH&BA&p+2sk%!f0>#B<@+Iky}cr!Y$v!BQiBJ9bZ~l;X!zjy@u5<(!$-Q z#Zq16b)q4}ha-9RRm`i@x4FSFDJce`Y2f+e48NrwKFJoBK?L?9y}QK3dQ--OX#0T_ z9qF%L(!iESsDAi`<}Sw}nuV(Jny`mt3N$|3i7oTGY|cWke!eGUp(P@vV#axqavAn; zRkfkE&`T#AY4I^9OwIVA>lOo7nxyr`FaZZL7)d46#;0AnpRMCc!s-jV-qCHId^_qE z{iS)Qr&L}sn)qFXXSV`&?c86b>0b7^0$$``PuPTza&>M(6iVnz9($ z)WljJWa!oMU%%*DSrTpsv!b*EA;33j7&z-RWvjSaNw&-*NJqkh$WjF~y~mfVNp~~m zGHb8gOrT|rfVj}c0b;8qILSAsSgq*A1jX?a<%n2noe;IGMN((xe#Y(B0n~yFeMUxm z@7&c|3q66RLgM^GQocf;-h3`ieEP^@z^Kyc?@+(P1s#HL9gi4{b4`y6$J- ztMpf&jCUX8--9?Q4)opqXCXNh>QpihLzcBDWgh;~TYAx&WCoY}eGSWNz%3r+Iju%` zcfPVbC%WkD8g$4&*oro5b6S=OIs)X9U<-xPC6M@&t+|eknP7vZuQmQ)W(N6vcuZo& zs1L@P+%dxbfxMRYc^CKKIj3KPJLuTzCl`@HUMKIPRnS z2#QG3u9me_(D_iokfZ?H&`67Y+sXcpNh6@UtSqZ!xsa|zO35!?26$G!k3MgZT}b=_6++6N;SC`{%s z>-ABzg;^hMLOA(z+MhApMWNW{M3>S(i%sjWf%GcF!f%?DJv!PP6U?eJLyl~U)04|s zkX!Lo6e`wKJ6-IBjiiHB{(OB|H9d4}<=^TzZ7wR=R9hE#kB(6%g7JyM)3g=&+1)&< zQlL+6{+$)fDj0SjZM-Jj<2s?ijo*;?qkTL0T#2}a{Y@-f$a_9;Kaia;2om4M3jTNZ zB7EH9()%v*p13+aY0grV<@lk5WxK4m8kSREs9rpjX(oR8Co>E;5YH)N4xH*+E>EKe zgZv15_Q>-Sv1aLQwCy5;6K~Yv9sZvxw&=c*!xvaqZ75d5DW6j@j4#kN-YY-c@QJ#~ zhfvz=I%}|$O0@9*1^3os{K5-<-VMClbUldn({0schrg`cbas4hL*R{pjkU8s@y8mO z&b%JUEH?lZcRMcAwku+1jZ}FuK5vkw*mnGNMmh?3YV_HZx4l52g7FYrx?ef#vhP?b z6HV%{ha;tGD#^eT?xsKIMR`uw_U7Q%la;z`DeKC%v$v%I1BbSCR`!8MLn-O*q+REf z?5l~kPW^&XN1aN?o3ry?bav;DR)w6+(YFqbixK$#zqRjkuKFq3$Bt#{{V-ak`)lpC zmXt{LH6;awEyUQa(2Yx`NMf<%agMro8`l<87h0Q>dC1?;J&N*y9lWmdp2e^ijaW|E zy);ZQ#Y!xUqUuc6El^ss-*(5XZZ5*K=+nxUsqt9tAae`+P8dvyZ~TdP<>!AQpNe7|i&g{In(Pb0=N zNr#)@GQyK7YuyS+TT~)?(CDl-oe_&DZ1`2PQIVF}b}O#?W23hJLZQ7H3B#Jc#hP|?otgAm3cJb?W^nMS$zzN6w2G?_8H3A;i`i`;+ZCse7ohsXGGX=!nKLxcRr#$=~p5yM3p3cPmpKYs9nNUUES&h&a9gP;fkQqO}3 z{#3e^NI9b#O8>yXg_d?p77%BjujHgwO!9VbE%w8B~weh_i+V96r@IDo3^mN^`(46L|(6Atb z-<0;*G{6*IYq_!48&Z}>Umi?*u^;vg=-^wli4{2D6N`kV`-D7~${q9-M3|6f=}VlC z=pL~xJpy&}8*Xz}1)Vre*Bw+mCkze{aJq|r!`Q<=*F0^e>rWbrj#G=k3-%+4@=bxo zH9eH4tEz)}6WygW>DUgIbU+Ojh0|NO8Po)-ZLcS-D-8potko#j1(JUsrNA)s9Mvet zFHj>Wc1F$`eOD~1$o(EUWeCGlY)3nds5jGf6t}YD`AQoNoa^CMBRC?D4Bn$$JBuVaoY)qpkxQE|eSG9rLzwn~n&8@`hY* zM|PF<6D^y}R0nc*$Y|Mzc>pcf>33a^DRK5%v%6=c>6#(C2oE6TDq)u@Rn7&ayMogi zF+AIBGL(W-WiE4d4RSQzAKrQLG!~e)?N5RX@{V@qQbHRi_qF*&_cTm9y9 z-_Laj5VHNwi84V6-c)guuHZ}77_P``fg<#vv)I0nzj{vVI>LjmHfe!x*1V=ZD5@k% zkb|$s;0sN?A+5)TucL!9|8a$FdH-pCw+TAgx!%-TOq4`^MHbGed{B8ZD4QjrDyIZ< zZ9~MLV7N?h$i^R@xx<$4vzbAz)dz(f?sk@R!ap``fm50G)HU8v%~_;G3VS7%)!>NN z>Yo%B5Amc2x1w!Rls#xgyURm zuliXB!vo@N47)BMt-zp5O$-9)d&jRo&UnThX7H4U_x14{{0F6aDL8XhMn2iITM!H} zOhztM#&=&!nyJ;_wbhATt58UtpIu?(GX@OV#h?YmK?pE11bF(1~hB&P0iKmS4GtWRZ?s;AWw0OVz66mds4ixqirgwo$+ zx1pMqc}uC3eD^Si!tOeqZS&KlWO{?GkGE2$u?FwXTY$rGLy|P%LN*f?*a4W-SzL)p zvT%{^Bxgas{aiw@)@RV){*J| zOG~&em4bei?frf?3n{)J66B4TI?=|#c=>nv+{yykqjyzuD&bCp80E|p^jVgSbs(|4 z1$CQ{00PcdmO)W7Dt2wHRIzSGCRRSo49;R-?mB<%_5}`+eR`QP4z63lMY{sDW^fCa z8xg(%g%8VCaD&M!v4++k#P9zseYh3pzxM~=~tE$Bkcb5uQsHZ~xMk%^q@K7&mzDQ7EZdH8?pVel! zaXS9XosdV9B}qth7>tX0PN*;!l~z4OMdM9yO9N9odf$dvL}@=wl^1^}bI{uD1>()m` zS$Nu3)QEL#LlV0M<0s{c#9_z)U9V}ztIHgU+3R#E4_!25IjqXcKDHdy_2oNDGSI+^ z;$H6+aw{p>?ly055Xw~LUzMsjG@aaRDr`EC-e`P9a5j+1r4EqJ=Z4+_ zq44Y=XBAhqtGIP*XgI-$5CR$jxo$n`%&R`OQGPausS%<0uttlN?+K^%)LIz)xncO6 z2JXs5772Uh#iYe4)SX}M%x2i_Fsy7Bliyu?uV7RIzDw;#ifSm@;v^feRdPc}BG>bG z#~k9)+bWQaXe%o*)$<>3JcrZxJP)#_1S+sRPoSX<`VuLW`LxsOK>7c&Ti?_6S-bxI zYW~GX!H1`DElWyYDKe8}$8JD}PfNo+AL_&^;S7P6HT*5DMi z0U_9tc`tn2!sa@LNkMIV&HPJ>Bd^(h!j~7LO5_(zm01*mINtb^&*JIKOi@Fciaj|!$I6K*%!D(oPd@=TAg*x2x1cWj{EuNW5IfV zjwC4pgp6g)t__EL7Vq2c2K;Z9ps_BlH+3i9-*VF4eP?XR6z;h~J7L4h;_`*YcK_$g z{12dPwGVRr4FdJTe3z`vQL#$Wt$%)g&v-UBxw_}?BK%aFd-dil!&?@ALC+w*SOK>p z=K{cV@$En?13|Pf?EsFeYzDS)JT)-X;9uMD3PO-gx0#UGyj^=>;C?|N`F!O<+r<{0 z2^lt#bi87W%*MW|delEJ$)1OT$!uM3`Qq0}R@`oLC;bmJut#2usq^)H!|lY|s%G2W z1YG=L8G3)(%GtJ_?#%nIBa)0C2|b@vA4yZ>@m+XOnkl{)*yZ=X^3E>w{4qPETV)iK zFLa&myS@{wr*@v>jqCL*w|u@C;l4cqWy>A}_}?t9lrfXD!wAY+sleK-`Zpd(<&Wpl zn`BVVN=V{t-R6k)Hh2AFz%#OcufsRa|3KR1{|C~3prwcY63}^>1+L6!pV8bVza%mm zY~4yW9#Rbgqyp&d+wktML9nGy$VpzE<+2Q%{@*X6g8vnvuRyM zGf8HgKK0+vWPX)Th|iQ3TU5a~7HbcS`R!F?<_sV=`z^hMR+AV__OI2@&ghDB>XJKw za#Y1rtIDh+Gi9J#IQ^2;GVEFP36>IEpMCYL{Cs};(tCis%68j#d-^_pqM%ZsvHMH5 z1seOTWHcfX9;U2n%;~z}`}ndrU|}(L1(%#UsL|H!S77Ux`%vwOUh0ix6(gaRT14H^ zA{@u8o~*&kBd}Fs%5nced`t``n~y_0H#c`!WT)hV&3H+#))yI`neo{m3O3)~gzFfi z1B__b#vJ`pQ74kv>wa&U2MNUS-y`cI@tcWi4PT4QL_WUX5jx7|2zdp+sq z@|goUjC%`ebS>G1gL}^-PEP}$%&xQ7x|j^v1-(oTEY3s(K$m!Rn($_3$$^hwLbQkS zT3F*zWX6GV(QK2p2VC}1s%|dt^gc-lvNEb&Vc5cOcWSa9dE zR?r~5m9JRJK)px&`}WqjlLN>E7F%-1{hR1`7RxAJv%r=i|{(RRaRC)00epvD1USC@K`SvYAr!z z6~x7t-AqhOhOpQS!rxRgvcE$b9`e6TGw{x*HcHnIv4Ko~3%n1Vmuz!|_U_qMy7R#k=J*l6Ja==+<4f-4$^o=ai8`w3^!uVZk`NP*85!AOWL z$>Lw!25x9=?HhpccvUR0N>0W%hSgE~IE9Lnfl~jMWnKWHwkuNK(9lo_=yDRfSZzcd#%ZuVZ$YxAqtr(u{CyDh?csK8x{ZXUE-L2 z`-a~)cnPl-L_NB4J0WS<>GfbY@w?xz`uRy^w_e1dR$1z~Auq12)qd6@=tRHDaeCYC z%uf;Uzh%8Npz5%td{gTsN`R0CDx%jL0YLN@`<3x0Ze+iGpZn4Y$=sk!W#R;pf9_K@=KMo*%P%EUTimu?N9e%z z3^{X6YO;sr!*pmpi$u6F(Yn-xj>a_vVxu~gKg;O$!> zKMx64iMQYCy1X8AbQ6$4eMCop677zlHU&src&t1XwN&tMjK=3>;40W?>n$0C)lG&Y z5}Zq|ml&S5x(z$I3ow)yDw1A_N_lOZKH8ns6&6rw@Km}@tUsZFq4K(2&ozW)9C>jx zM~Yh)2OV9wCh&t)lO3eQUehv#XUTbKqonf@`TL#tI>)qNwSXJ(?Wo497fPsFad}&6 zc20G=tVOoeNp9QGr=#RV%=gv25XPsj_;D97Sz)xfuXsg!kBt<|l}^ic zK(x-g-d1!fxsuS~^Y*Qo-SY!Km-)t@?li6c0m1)Y?#R^O-HEzNtNVBUh9cPa@t~u? zQs`eAxF(0o+Og7kS`-c?16tB2VSj<(2>$mvJ6E91NBEgU)f=O~$win1(-#cG2ixn^wWS+V;wB5Fr>U7@8 zpRgTWyKBLAygTEJ5L# z@>X?5yf945$8<&Z>VrM~q8HJVSi^&Sew}?6=(ovm%!h&K)$J)ZS|Mq*m(C)l8zpt3 z{dbaxW*K~eA1Wu=?D*dll)TZmOh4M20`s-T@3i{L&6!?q&aC1*fddftTleAS^_5Iq z7);s5D!_dk)$}9KF-7PED}Da0o7%tyv9!KT>n)zPTkY(qK8)q9QR$gxO1mN$@orfh z_uF*dBQ8X)x~$oX?fW<7ibsArDQ5gDr>I|zA}1L7jo#y? z79mJ|CVzf>j{-p~+DYs0l8uSFAWVa94UxmQv)=OKC11n?lkNGIo8k#oF;f0qDcU+`B_wy+q_R50oHT18ackjduR9<-G|0rJ-P zj{ksW1UaB#;itxLkiZ1AvsEpZT?1Xg+(C1^Db4HbRB3+n@b&gQO(Nx0Jm22sm@aN_ z*B?c~0Ef*Hi<)Ufd!W{Q&qPU`;yQ~mPmv*4hP2GL7c{@XuTxwalhoPo`pYU^`u_jn z_=;Jf;2d3%KB?d{fWiXz1r*}Mgt^`DK%36hd-UtM1qQN(< zsuneafy2Yf`AC|&bLPdA_CQ`%pa7Lpp1 z@TcDuzLH-d440{Kgv_E!UD{Wk!{;ilhT$T{#vs9?>F6L#)!~c>#0TCcf zf>W?89vq(&&)`<4pQle*P$KpVDw$^mXN*8wPuXtY#f7Ko7lcqXZ3)T88!FXqpMj7n zKN}=Uy0=a2xTQgw=SIbU)?xKhl*8!cK-+<}w0xu#ZHb96^?;1Y%=m zi~a<)0gK-ZXWtl*3daqKbDEyebkmaObeHr(AKmnp-WDBom+nY<$BZv+{@~fKC^{}) zcyTVWFbD_NoAB7hCZ`)%IM@GlO}DrN7Ahf}&D+yc7Atv6^SX#%&P@HI=E5!!1tBf( z-RWDVYZicP6WT#)IUkrVQ$DH$&h@S;-av`?NY?7LtS`}s;Vgj;>AXijb?zKv|9fNZ_3LbkF=q8`ekkTKNY;NJ7TjXN@19XeaDb(XZU zObWjla9tK282dF90F z!bQ3zZpdWE*<}l_Xd@^K|KXk(?`pbPLlJWf0Qz@ zjaSW{Xyknm9U@_FiKTb7_UGDOP6fm{Ix-H~c0M(Mdj-@mr!DKd5Pyz@hp3urQ;p6* zUD}0hxx?|f^jWq2yM|?8*C@^}AWoD&mdD^d`5Em8YxL7)Ew=_g_00RvlD$74d_s>= z7t+MZMgf74=2G6_+>;Z0VMWVAiDh3-D&+bu$=!K@? z8}$O-vNC!VB%zfniaIEejR^@i>Kgq2jA;!48z2o!p|loUM|pXClxG0`n z*xB3Capa`ygAsZ%e{B>0B{Ud0GEa!{`;8GKObvJ=85g|68~$K3<7UkI2_9jn(jDU+ zrRz6z-2=%b-9G^6eDBHL*Z?h6HyTI(tF&oX!UyXf>9-utW|4_(GzdDK!13~2ygxY( zy7c)=qNghEHA@C{MqWH+PN_}J@E!cmhVB$0Zp~$2hI>{PAO~2Z)-3=F(DtH z9E>^HGP&$Q?1%Aqu=4ZsTf_ym9=GhJEs};7rH-+l@eS+A=7Wk02MmN#pOyg&pLzIh zUuK5V3NEwh=9R@}L8#2~97$YsiNyr)sZ}nFr>ELDWU^*$YzGQtv1`ml!HI`x@#$gY zYlk|-^}aEvvQZA0GsWfCKpmSKR_%;&S@OcgrC4pH!4=|ZIh7?dJ+fjLCNckB&(gGj zXErKrW3=Opin@?yj=YR6EO0jNInA2(WPy>V_zLN@`@J)kM897d_6KV9Lh=WgTkv0X zPfey}K)N{rY4ovG2jeE>K)JruugLjU+smd<)uS4}tBR9WUY209tjVBxj8M{w!|LIj z=CttA>47P`04gCm1k?wE17YA39g$V1?FC)XlTSkYWS_rAa0xh~`LArR3^FOaQQ={Z5-yW{pnYM-OrB z{(Aartv83y?6kOhdujzdtf=gALSf;yQVE|-TU&ANQ?>t?|F+BHOY!W}$%4UiM(p0g zTR!~x@ggkntS)G-`D6&@oj(U_n@22B^0P)iuE+_l9?D=Ny{>&UMKuv?Xwhq7-4Wj+q99pBPaG4 zIlk)nsYlYYQIl)FqR`yzrR3%G#qRb-+gbCObkwn>f9uH!ARoM8*(KkuJfiihwx1r#Qzu1AdW5kF-d| zex=c2C0Ao=XE^Kp;SU=cN-SyEY%|uHeO|vpAh1hsI6cx_>zAnsCnG@%OMUY`ZD*EM z$>%kUD&{_Gy*E5BPBxlGCUZuB+!(uo*UwTCyvr#nXjsL{?=?!Jwu= z5n4nGpT@-zSQ@{zZy@qJsa=1_^Lc+bjK&lW8*UAozS`9b^h>zA14Ln4DY+wXuwR;#oB}Pc-BcNjX8FH@xLAZKl zg+~vMspwjQ<1ymL)p6(S^_%`%pU?G_K)s2HiT2x5B@E7R_)kt11O~|4oAj0eZ$#dA z|4YN#C*8r7m9MK$i?=9y&1T(gQs@^kHjs%1*rLiA{o;{>`7rBcXq}9V26eD{v=NBcv;~^63{5-U zP5eF(b+&FfSLhPW2I~uv1v`c7ARLSAGv8PG0}GEtN7({skBT4d_PWQxfNk{5uB7q- zGG)*~2o1t94*pfVm7Vb~^{ka0aP-nA4q!894Rt-^C4aP|5XmLc6&RV%mP%*aY+I8> z?_MgRuJyX zOAF16nqk;9k3()iVAlwEx9J)4Nd)`uQ<+*-`U75A{E=_jKc?s29OM`oCIDpDY1D#Ap4X~+-mW&i0W zlI>N(#ikd0B|gf+rnfKD71=-ZhZ?W3Q5d4zl8`BOf5ZcyBllzLu!Rq79Ox%&id z>w68og;CCFTi~jCIjuf-wCLenQR+O*VsyS8))!~Y54{}{TVPec`=$&$Di18L0rP@O zalTsPgw(ZkW4?A?C8v2lPU4-dK{@eX*ufG6wd>|J8_4Qs;JOtCc6c4D_a zDCq5FH8LCN+NcAXb;0B)?>;fS-wlG$i|1#$eS#!B#ySaD6pLpQx3XEUZTyq<ur_iR?pLfNu09rX&EH^b z%NQXLA_uL{e_E$a zx9q}88J^yURPM$+xgYH75CvjZ(4`H&eSAE{&j+@tET0ta54) zh9>{$_Q2t5@j>fzob~D#EsJN$1W?-n=xFsh zhJZ0r@VlZO!|v}U!%Mq!8K-DtZyUeotNFMdQSK+ZB+32CYw>xC@(2Hw6BZl+kS2Ro zK-zTL z0=}&AV9~!@AD)Edy5^ohl~>~leAEh`lk$c_WQdXMfeU0mUa*}OPS)%4+5jx88Af6s z@w9Cr9J23rec`SH;2Eljrw!P`2U8+I;rDR(?wh;=CWRz#M6Zi)$WNZND$-$+yYL z<~xg7)3(4>%l2_>bVC^a5_v-^+j1rB|DJvPFb|&He(aqWEW77@B~JVybY=giKxtHo zZ5NH=0w{1paq$FVV8lVv|@3&Zm8{;M5Txjq>_jKpF=Mu?s zx11y>Ub~lqkQ{_Fix_6KZ7+Wc_MrW14@h=vju^O_yctqLS@L@*HLMycU*WK-%&OCa zQ~mDC+JRea&K2xKP4XO8VNG}VW_=H#57o7RzH|-+rdSVCCfV`A@0ihAEa*6MW3Wmf zWpKo*W`s0u4whvzsJkftku3{Hx|t21-Ys;&UpLq;F2=sE5zKI5C#@np&_~$GoZUJ* zP;3Tf5Ew4d6tehbYr61|irJ-}RDUo6PP&n(=y`MW-h0)!l`*236Ui|GAi8CPNdHe7 zrnVNM#l&Tt>^BV3{(jzo4uGP3P@I|_G%vp^&^bImaayEz{IOG|d2{2lSwSGTzm}_D zz6e#m>{Y;h0Y>ZUPh_Ul)mSU*T84D*gmVg5_LfnQm>^ZcqeTVj`x9YQYW|HcvYO(y zgSWos5!}wVieJwq?hOwKbJe88;JNZd?EQ9PPNw%-Y+6tLgr>bTlimwT1iniku%y3t zwF(bIyIOrK-Rk%8$!)2v4r$&Qls@qC68>aW$uPI^lMMeCduscKuQ`!DIv?~b8F6y! z4cqxL;i8nRk{72v@eOpNOrKsH9!9|<=$Z6Eq@3=rz|Rf}B5DvARh69Ds$9kqn-R1_ zimvs8P$cI757hB2ew}A4o1{c9uL9apHmzD|MeAFzlc;*i(1X?=no2VaWT+P-Zw2wl z9)pCg>Z$8__-EGo6*z_09tiv~iXEGo7qa902>`$!YJFcJyWMj;GXG6n%Q=X|!*z&q zFO}tZ?jm?-cKV@Ws|iMJ0$a~goFjAGvBbRhY2kK+<6{+jL}{W$ zfs08wgz~|Y@GttCMVo%6!iHatG`^w`KjX%jLvOS5m0Rk8*6V~TTd^7CLvtL29MDc;BM2bC_$482Kjxl@HRTg(4oquHbz7_1phd8OE?!8`5T9 zl($w~|KsF(PSCZRY$l4CkRLvL0WmiT2nyumZo6#I9rAt3>}lmo_vj{4!HUO9NdeaC zdprR!7Tl64_ywoHQ=w~>(IEne6@#T#~UUq2TgUlJ*Qte z;svL@BDC5zemg(O^7orMPw$=|$&fO^XG-wYsNOjGLEzspY?PLzYt~SFihLR}xdFS^ z05rA$q{ueIN7qF~itdAw?yqksFkC_E=t;=83PI|GPJ_5K2~A)RG<%W=VYq$MkU%&% zf6F`mQb4bmo%==rfgKinR~Bx-qw+@OtOQWjk@iE|BlGqukha`i`jkCcPtPiQzdVx9 z7^ogwF50DsczDXX@$mX!jLNF=JWBM7VYPhDKVw*#8)T}6l~EB=oc+&2hRVpY#OMsG zDLf31i9pB4(?{8bI}pf%euV-k$B{s%RltGOhONs`V8sQ$?+aPPoU+2}Q;Oi}osKyS zJzHtk6(;Wsx=&ehoC?1hDAHkbhZ&^NSA`VKwqRx7Tep$j_1H;e(%KC|LsPbRd|RcB zYqM_w&?68Yyvc?iPU76k)D?g5C!C^0z}nr~=N|P6jDpnYXx8*kMS8~fOipISk*c-> zhyU?uZchUEuxqEy=WhkGt433wumOx(k#=TJJheB#Zka5@)LyKjf71aw^xLNdcT)!| zHiO!~5lQ7f2`3(@&yChY8i4!1uUZ$Zc}3c(t@=jP5&Kutyy^cf z>hm5czOVKhIG{w5@{~1GGW;X-9?ZT3_;xxEfjA{i^Z5*R!HNzYr#OZ4uh{#20FzxU z+o8b|l@ptJI~$u&u%-`PyGYH*=}=t*`uPSS^oSw&bB}ML>BJXoE4tbA--er*K-5vO zjY<)36CcwRoM>;GUE!P!c#4;=#pum{I3L6YAw~ep<9uX}K1+PhACT~D3m$1REiPF2 z!5Q#I5Mt`UM8Wi>m^wG%#H4WGBJMaD&-nrCHRb~f4E1Df{e~MC@RRC`0xn}VrduQK zu|G>IUY(A|AL{BF=knE4?&Zsu;#E~u_JGgPQqCKz*Pfc1;Z|Q%RAdh{?C+?;w?2pA zmeNnvE&;PQQdM65*DY(HBThNScI%c0$+Wdwg8@2tcfZ3@Qhr{5pHoFDk`Ysmaf~6y z-VjdJ?T*gJjgoVQ#3P{4;&N@&)??^{)(Z+OD`<7&+VvKoU2kC_3I-Q#iukFe)YQ}< zRMsm}B~WD0wZHX+Y0iug#-ST~x5X*xx@l}ViSM!~hZT#x-K$6u6rgXfB8Y z%>`*G8lR=7?+>X|#BS{z+5zQ$OV#fwgCbO7;w-DV_G1_DZQ0f!3}QKi4G(M2l2JWQ zZ^7-D?8%yInYav!%z7Wew0aB$O084PW7Kuo#3+scJGSdF)X9dD@>F8Vok2uIG%e|8 z($w5f2y|&ws)J_a?y$q(6I^=)PTOPEe5$r~2t~P{lLX+x4;<--O>LPr6n%E3%(j8s z)!$vP7UNJ#i*FUZWGQTMriiOyY;NKSOjn4$9S+b=ohuK1qwcq78~;A|yM%wsH&OA= zXeWx8BU_#$?=e}kUpO$kl|Erjn?lfZ`Em_s;(3FN|Ifo>M+hZbbD?Z`!+UoNarxFM zGFZ#UCLF-k(LkXwUt()~=EW)e{^mp5@_{7Q;1fd$uo07ez*@*3&pCrA;fG+~E z<92SWUQ5k1cu-)9toh3|D^q8&EC8nuvgAZ&WySS~{E4LeZ{)nE(yjaz6(mf_1Iw{9 zQ-%KPFSXo$R0|L5z3RIeqBE*LSL@ZLI5%{8X0y@~MSyo8E3-cJPK_CP8LHCWu z$nlC9KEjSbTq;b(vmZaNVz+RKqa7Nt2r5it@8X|Iwd9J(@Hk(g=Bg;oEKjCX{dpQH z*KrcH?e*bZ4VA&Tda9~(km zAIB-PY|>OAT&D+FUq~$I)Efd31sf<8Zw3MiawP+Jzuxf4`~ODHpDkJuub#L*!aV6h^=RS2 z?5A8gwzyr%<6kFUx+0TKaOZNSM~}1}ANCIBsXmE}@Vu$%z_=DR9lJAth1Q#7n zb_~0H&h{iSSdJ7z6(^QwcZxMQlwvgkQ&1|-zu|@~VbbDQm0w2AY>@#|MhCM8k=D`cI0;)(hO^=U8=~*~kaY0=r`<$h^=bA-OU+t3M)BuW^zx*PYqD?}D^*_Wv%bBx zGcZpI7f5q$Bq*q$`4YF|4J?t=G#C}c`NUcapkEW7R7i`Wwjj>U7NA~~0X&+qORMbb zNaS*^Ne4C9{h70BqSeh2H0*$qm``JJ{;)PG-kOH+g=@_=Sl}14J*?(CaYOv6wNd8u+Of@s1Fw4KJbF3Zar}z7Y2-z& zzL6qS3A_Q6*Qqv#QPm-|a-;Xn2`aZ~J)uQleJ3Sm;CydJP}VXD@u2S_*|LA*XI`Ao zN?4tKT%T9r>Wi~~u6pOcu6X7C6LCxHM@=luL*2GSt!*|=-^opdN2BmbM)!0CEUys7`Vqdh#>b0%db_toPjr?W)YX*UB z((zZ%aI5}nedh)1IF~-&&eFvM1gdT_zjR{?ZSub9UpvO$q=P1#mq$g04X+`5!s9({ynHjhjc#p#C zb#WgPU)&0ZC zyqCSPiue4xfX5g+WMcL}d*(KYxAQFolKbu8p&R4Arh0X(WpABdutu0VOvcyfgM#MW zKstPCY6a=W1TL?gfn#F_AAdt@=C**W>}=uQ+XoTzv#A)OnTg5H`_^z}5dvc>%EPa& zs1F>=q+;OUA`2^8sjyL%F-RmH6m6_^Pc|cPDBble#j?*s+nQBi*z1ktJn_1*w-tVO zmc+Lj<2G__K#H;*e};Zqt#xlJv|67InOCeE|8Q{U=G9NLp;)8>Z!|o#5GOJBR%f+a z;0Zn%P+7QrQ#7n?co@0Sda+fmL@bgWoW2sd7Vo;ipWSF%P|RhHK)k9d^yI{Nt@?kV zIg!Djh7UD+>#j)2$b1YiI+Z$*vCjI3&93DG`X7)se`>1uZI&i1GqB)dF+o-p$LvXt z|E;#~O0{6DtSBwOs9I&vdWKc=M-e+kQu+j3n_i$a2PJb47?>`o{v?g$M=We`zfWij z=4iMwhiT9u^FcsZO$;o#IIQkF9BMrC`D`=+@_+dH%7-S~_+JG9 zX%UeaAt0bqA}|^h1XM~Nl+I0hqc>^{5GhHeTSP%5M|U&2YcRUIn}O%{cg}fpUYx&R z*Y3FP>yuxTWJZ~5|I3%4S+ec8b220CQ{#l?EgJicI}Q#>s-L}gJLL|D4_jI&S41>V zXPk3TLsK~*=H`;n=*VNS{*6pc`R1T-l0=C20G^CZ)g}LN z%?_`ES%?I}J^4PU+ETkhov|2Hav>&+6dir?tO<;?zk9n%#1WILUPLd9QJTy}Ov=Q2 zyDJ8oLe5`aX3yfo(s75uKeEZ3CJOgCyyTSTO}9XXH92Z-wn4L!e5O7};^*tR4qeE@ z;35SPt!Q`riTE=G8EE~ZCTe5aPx2!RV98XuJ6=*y7^~m^zL3a3SvqA8&=*qCrx2;+ zvyt(UaI)&X=Ogs)Jxt*ta01h&rWd2ydx4?NbJa0j+0I`X8?$M6hqTL!MlI9m_S3X) zcHqd)^r<>+`+Cb3^mo*=6h_5ReuV)+FAE}>9B!z+xsbN2CS#2N%WvR-)>2(XZWE#PRbm!+M5;8 zAGB?t&+R_w`2Z{8n{p?wvfYP*6$V|kk5HV!4C6SR=KZPg8lUVNx?^v{9pWwD-}j%s zy^&RV`n+&V;!BzsHq$KIJ~V`yVPYSTPK<+%SeZU;ko0&mygE?h^gM7h-0>xIU5*#} z6m-xC^;&9!t}d-~k9A$G*xVgA#ylQ2aqUf`28Uyb*<|l*Nq+GB1o4bG1YX9|F0rMo z=g^?mfI`tNS6U@PSi$|D$(D~&6sOasJvkkexwgjIwl^`MRlV-y+r;Cu?-pLQp{74B zmh`LAE*Ij?5Zgx+tC_l^s|(*F7lv{d%3E&xO^3@xHB}gRknLCrb9^n7tJgXKp7iM| zXx-Flnz8e!1xTQDhA_xCy|ML>5!4WU9qK7NAT_`^Ebqyd9wcjZwx!1YWBgaF1<~Yz zV`O7>nEXNU&O_}ut;Ktt)-3qEGFT!KB$d94I#N$@66p6{J{`rSW9FwP$ncenWG^!Cgd}YBSBWW`?3lDG9 z&&)CSpZBHzm21<8TTFvJjwEd?Y`ZX7VR*7S-?(J5c76I>Z4LC;+cv04_a7$>d;E?H zsV~Fwm#yCA5+d%ZoR;?--q%rt-M2kdiFDFGvyVbqqHiK$2> zD$Vr(S+>33A$Gj+q4S!)vQa5~NF2-ng3aXk*1g!`Ef)uBdU2(*G#)eD9lfui7>$Mm zc@M#p#s&6NSJYL5^J|M|RK(BmA+g~f+V~oB*4@lRb$n;j*S5az@ z@bTL1>EU%5TRrG}BePbPAjBNX!`%fon!Ka=HjO5!*s!elHZ3YOzu54`Vw`fha50}? zU#j?TWw_Gt{(lq=w^cWF7gH1*Z5qUXtug&#wS!c9Qx4clKvSjj-MVox@NJlTYAyBw*SDiuM@CxeqsnZMp>DAudJL74pGBr(CypANB=*6O=24}%~)nW}*dH=}|J%I!lW`;=>97`*55@K5* z>O^^HudgJY-n_C;%l63MsxM_QuWZt>3z)-^BJXWLN=9a3ZM{Dzi15XL*qE6Cu~flr zCuLMrN-B{yNhr3F8HgbhnNrcx;)%PaZ@Q7g+k9ya`_ts?R&OcM8hGV|{DdKC{wO62cQyjrw|f zNBw#_pUInD0z<@8Bxr}JY-Gmn$g9>__9mI(j;LXz@&pBGX%;GueZMm-C4j?QErN;C zq14R683$Z&e__b7u$h*IhJN}3_CZ>Ylnp{N0o&3=WIWtGAlohGf+a{ZEiDzc~Pv=2PDlt*z#OddD9kRs&q+pbvo~t*6Z+Mww zZrv;Mw51D~K~QXv;^2&oYaxl;DbWBV1!gNKkMe3XUKSYNC|)0*!>5W;4&*m`5 zpP{$CN66I$9F^+{)_xJs6}pnU9ZAYmFu^+cO_n z`##+o)ax$d`)IXn`iZDdjFvU!6)Eh3s611T{Y+S-|9wtsjut)drNrU8$&%VqS~0Vx zS(lQK{`tRLs6OS$3B=Em;j%{ul$zg**oqC(z&6Yq^HXAxzogLF4evzB&Crhq0w5xA zwX4^9&xp1m)Ks`I;y)yyq8P352gJ^xIkoeUUU$FQ4FAnHGWn**23ZL@>|c6)O5aK9 zmWD)vpMIS?e(it8RKe4RVe-_pd<$(tPu`L)ny0TzM2h60Y;)r(>V2K%$c8!P$D-tK zX4Pcr%yihYHdeXLWtjC@x9H${raplskcqVPbau%M$cu@TyJorcI6bYpxH}KWDZRC08amXH1!(qxHp!bsHrf%mVER_BHTS{Q){>Ju+-2hJ*;Fgi zH|ei=!IO^?^RwU{`FE+z!~{JUKZ0HA-Yy1+2e1{fdA@Zn&vq==I}=L&zg+-CysI}p z*=KvpWREyB2KqSTjvk+dQh28)hncG*1|hr#nt?3_YYYT5Ed?z4i1T|~X zc?k3kLU@7UgAmKBi{p$8i$!es7@tUyxj+Pu(bolr>Kk6u)rgY_11i{Di$X2;oi}7C zjQJI5lheJ)U40WVTyWw52_N~2a4V|gQ}i978<1seZ?Z&p2t8lkZ0MFe!d@rM{kW3w z@$_d{a!qNvc)$16TE1=vuBfWp;npOZcV&05Oa#W6`+yM@3Dg(Xt|{gP2{qnAht<9^ zbV3yv*ThYK7vC9!(%#8z?J~uonr4Gz2hc&i(!ygGVa8vzb9`$$xzg-y8plYkwl?0- zx=v(mOPU7>wU&d)uzX1C3?XxlgJo{-2YZ8sxuBz4#7ETXFRFB{&%{i(Mu zkE2k&v?|=~!-OWGF(Q1nGtj;rI>}BCI;0M#m0e-n)*2Ge$Cul=1vR=ppvYHQq7a;ScfJ(BMM<8U0@{XWodYQxmG>B@gia&U$o#j zff+#^J1==h-c#bsGXd_cazq0ak{NwaB;>!j$VqBNYpgq?C{j+F^A_W z15!K4p@Vlf^BtEdJ-s-7ZeY9i<BTIX-s?-m?!K# z$ps34;S8|7yFud1)AH-nV~vzRKU_rtqCG;_FyW{DX1&@jmXn3sE4?1{oroE7xk2>x z&BKLi#SiNRvKLZRh-eX2;kyDafQo@%%DUv|ngX1#t)DGyh4eCsbzS|9r-R1RA1;~g zpRAq8%*QTht^W6=!e5aiFmmQ*fdvl0{uMU_=$gtR0amBi*$HQ5Og8(E3K=A!N;Q4Z zJ)72o^BeE)lHl;tKod4zpVx(mRL#OR?CVbM=6+J}8qzei%naGjgWqzkc~w8`&YOAQ z8JIk`S!w6qw`#Ci9@i-GE#o71W7~UNOlQjx>h;!X zyiUU(I<2C+VH>4Z5O`o|1jC>XXp})DJrs2pz(tZx%QQcH`KXP`1a@8#7cy;$y~r;N z`(*oS-yd>9Brtbz+&uMb2(zK+@C+Dm&j4u_ld}xNwg%Ed7RQfWV3(|f?;br(1)CJ< zvNIppG#ji>H|rd_#@W3!&>l;9Q?`Y>s*mn&O;avu`ywG(+>)>#X&i2aa!2ZqT z$ghd%`OM$a>pTr2wr2P*duXYBI%K&36uHxD58!%UNpYw5pDHBo)UeB!Lr`iQbO(*=l9^!=2P(L`%Y`AN6V!X&yslxXY>?vX!l{6q} zqu)?%2`hH1(`ZxnZ-w$6&sTa|8s!l!JO@F$Gdv({P)R>lxeEH96;b$B+p?d0uc>zh zCiMOX%v~m9bfocopv@T z_^8vQCVthrp7A`-^SDGzCz)ST>#Ju@jO2gz-8z&yvbp|qOx#vgu`%cSsoB5E%v%4O zBaw0lyn=Cdv7O869C+us!iGvIkcj4=kuv)TnHl@*ZEV+CF2AV?nY-=f?`F@@F%mB) zeqKljP{Zt+?vUBbXB^md#t>KdtlUY+qDa4myO~|A;oWJYXwb)26zGJzW~@V|f_74a zZH%)uE8=1baXctmPl5g(h8=-N=;oBwdc9i5JV~fq2)9Y6#j6%dTX96}p5=cjJQvTh zw@WLAr%xa6;MQ}Mrgd}z>NlJlymQ}=+74+hl$cW9YyJTAc?cJ`SY<|iUHL(-zVY2Avdf$7X<4W z9!RL(Hg`}PI{@q_WWvRp80C!vo3>H_q-W|Lf1zzxEd>6?vC}U+u$|zoOzUKjZ))tm zYh~PhJl$iCaGERAj%l%P$1Nvp#tWIVU@SKLaQo33fd7uTg0Z|k;;%^$r=h<+3*DGT z@_gUyw%ttE_Kk9=Hd*qXG}8G$_NM^B85tkZJ+{vYbfoEaT{=uz?u<|;rprii@UZ(| zWEK-&s(#AWeurp~SGCSx%koQyIGlU3P1 zSxbAMm8wEuEAYA4NG^sh{zmN=usL5ER_I4fRDIND#jGzE{!soUdlUz8Hlo(jS znmpugrtkSc1%_?xNs0F*!{d%OB8)A4zLOTdKhr1OaVKu7FnSl%TqY?}!22KZG{12L zhby}`)%|W^A)yO|W+~j5RVw1g1e}*)R2ZJBo}9bdEM`u=+j>>zGmUU?J)u(2GnYgm8ENY-u_uCWsmaq?$R49ZK05n zizw0ju=FI%16#)7{Or!W=6|$ItQ>O5{zI|&&$iN-*Gl<#gKRu+x|;CDrTc4D;{hz| zu`DG%X4q3a96nMVUI6TKyBlRn1+thJ)}+#}WumZl7bts59O7udiUK!&@(pVcz6^Wa z9%{{`$Z7#CkfRViEEyZWiC++9hqn_lXpAfDk}*B?@6!vMq4KPPzTk7C>fwHKJE;yk zs&f78+A8Qht{h>QhRImeAj#1Wks&>QjmrdYU6a$;JRSYt!tDcp)VkL+ru$8$c>f+O z{wuXNjxUTB_MWpX%dx9_>*T%sdwWV_^_Ho%kF}9T|CSCKv&e(WbV9+$&ERCNN_|5y z@@m5(icvAMNCt%<7uT@EfSVi*3K)}SGp*cmeBk& z@x0J{Oh>lOul+LlY0}>t^q?|E{MXJU22B+gcMqdGpfU@m*>>*xc4X}oQeu-d>XW9y z`O9MUk@oIJHO!kM_kfyV+w{jh`%Y=%wf*>uPOowra(QJq#?}hW? z>rJ7#Ooxs!&9uXkT2lT^#<@>qc3*0ew$GxQj)a}`T%K;*;mss2^E%VcdALh$9vQQ! zUMde87R91M?~7-NyfNq354F2czRuEI$Sgl2COL0w`~1`}V8YUu1I%==W_w?Cft5jy9of5-*;KOU2FyHS)-#BE*5|WHG>)z0YB(bu?nnbbnXb zx3sj}=a3Jg$s@rk_S`H_m-n75;8_Te=1-W2V*qpAe+Sj@pUbZ20Xs4a!^0noylPl@ zdCLbXM;8xRCv>Dg_uG&Xii?UKFal&pz#(PXcyxItDR^Ul-zk?nX1dX@w^4PgQ6ad! z`m6}y=s0k}Dnf&5<#yJ;z4fD(so6e{2S$btBPegJuiI=!Bdh4#HUfiy!WaR49<9G8 zm4sQolkSWX(SsTO@ov5u5zhELwS#+4^R=8Rng~j0DVLM5W4Qw+?58vATp`$l(Jth$ zb+5zm?!`@K?nurvwzFXR9%2{D4|8NoDrQ_@C9~nvM*qThP`H!OvYz0j_G~kfUCF55 zpz@B+rb@a$YDTv1=T22$bG8$?!VM0@Zvu%x3ilnpj?G5X8YFyx&|G_4PEwN$Qn_Kwa9zt{G{=#bd6Y)L0Qczz;Uo|dj{A}Q6veLY0&hSYT zu88d#V0R)|t_I4vHRt}mtty5N!Sx1!c_Lau^i)ehz9RB=S=1*DItcGa^iBB%w#p)A zG`Woo#)*+iidPJL#U|}zE;nygJC_`V4$?|soOg(65;QEwsr*)W>-$VZ#GLriEuj?Z z#dnKc)&OWnwuc%!{QmS@wXuam@AH5~Z<08Yk8N_c$OQ)LF(GelB)wkgP2LW%8P3vJ zBNeO_C1Mky7y~Q~<}HD9dyUxFt^odT!q6@5y{4+M5rEt+93W|5uSRU%-+e&#{s!#c z8ClhXkAA{)4od|Cc3*0PC0moY`acVGmWfJcuN$_!8z~ zOP*EOzTK%gzFW20ms$*Hp;WS)rBTCyTK_^n`>7d|##42TY%+N^VRuD+octs70FOzx z9|wQK15HO%pscDL$DF;~&hNHIm;**|MvrjD)y_)W9CIKAL~^R_vyjAKrR;|b5jYvx zloO661RGiud~kcZxd54PXQ&Ffl!{y;zPHd_^~Ma5?jvT(NXj7gq7JOq4s7A`%E;rm5zm9;DboV{kD0~TE0ZsQENvF3Z)a=tpi>?!D2$qE<{iF(!FN5@~NVHp&7N}E)ruOEA24d$J zOc!Vn7cVwQx)ena|b+ddXe+^a%w!N*Tu9VoVMHB&4|;j z;p!Q)n5jWy7R~I)DVu%N`1!AhROR9$GOCWroM~mkPaBh=FSfm_c8r~fA60sjs-AiD znz2rK-)2znzmC?*U!q(E@j2TqdxJ9bRg3bh9MiFdL*oh6#w7l=F;?&&(=?-#I678w zA|(#2^aF8I3AmXc%MA4=(?A^OcCSr~<#hbCb69s?iThVf50# zb$HhA4Ok~?jwjW&rs7Al3OE)r#UXKlccnDrE&sb&irrJA>T&%u6+{B))^1yh_7nz1 z#CeRI!Qs`9!^!@5J{V)WJ5GbmQ7atohpDcqs)G@*t%sl%;Z#>0mdGtbx9b;&2_D(4 zTPedb2;S%FmlBs+fULJ2w@Q=twIUG^_#D6pz%W86Zg$3xn5TZa$Q)`uWp-JT+!l2Z zNWxp8&1-=(yi@k8$hS8y5gQ*?RE43b?$2NXX4)*Y?~i;zrwnasC8uw+Z2aTMp-6%d zy_Z4H9z20K^{K%M$Fs;^Zc1dN-(Jtrna_IKHq<&ErA&FA8=N?^tR67a8QZ9T4%Jd zJg8Xao@Nt~V@cqw9qJ@jVoAYN9uV<*yQXWv9d&>{emJyr3fdRczX~oQ`pv^F?%~~O z)bQ+{s%$+(`VkIG%_N@m?5<2P9vbz*CDN#meEN(QqByH))X*gCR? zAB()TR%Lw2uKf;rPh^Cf12O+erFHRUoxaz>ugrt|)riiJst*VAc6>{bJy|9=oywR_CE!MZljU8d{uq?f;pj3_G;V5+(Qp7B}krgaIFY@`{f_uNKojyg5nv0fB!p2s5wc=zC2roS5Lc9+ymTFNO zS>3=dKAEVsA3=eb+11YCN_KU+_Bp28dz}on7UYlB0758O26<7SFZ5#FxJ2Voo88-> zKZx-ro3Eqg6;$BQICjUyXO;D@_Hhua&yrm`mMMnQLBiyqNAkCfF zcd`CyWE>U)?V(pwYyq!i*3@NM8TW>m`z!C4rnOfsmzwxaI!*ym462m7JmY38w2R*SY?P236A*B?LiR;h*u*hj z(bRyq4{nohU3!{N#)WYSDz+&f$=6)zUR$ zWpsA3175U8Yt3>;tvW$Ot`bJIn`1P8Ydut`&ww5&zQ1RG37Ld>j_VBBvpIj~#c+u( z-G++~z*y|EMRoc6{6(2Ie4EGL_IS8!E1(VjsPVfDnr3C_5H;PuJg6WTy|ksi!fWfT6@rv8o*B|9QEHz$4>q4;s=2Yl-fQydx1AQQNR#)CatQ72gEX#c`WkXfss4LGt{5J<@yS zu%Ul9COeY)M*x%2!?OqYVPlU9+I23LHKR3P+;tbw?zQ}dzX$H4T~NL8#Lz!tp=N{3 ze(7p;rxBVoR?IL&LtN;?GAepJ8-`jgt9JRCQ0T`s(95(Jj3|n{p-iU7hK!q|G zBd?iuf475h%I(k|)1~ju_a>ySHSV7$NJ~Q`S2=pM2wZtwNpXUBq2|XwV%zu^T=u8X zU)A}NKr&0hq1(}X5d~yb9Z7oG;r(sYfb!noelwfuI=^D($-h1h!qx2GI@AJvdekmz zRpwya=W5*f>Ui8xJHf~|9Bg}U2%f#dF)3?SPZIbacIU~H5G|vz4^L6GjcLir-7uOE z>eM|6?+ElO6s`|J6+$WV&bR+A3x_;2tGc~sdt+9=5|r4%T2e}LJ~`^_KK?8K4aoa| zy4EZ#vKKTg8R@5y49$NGBSj_>Vqxnq(`K&%W%tfZCLGdu_v>>jx~z7mICbHMloI&n z#k;sY>dTDrd&oseE+M=f)9}Z&=Z#@b^%mTetpzA^GQ?RL#}psiD;kMz?+7A>sp!@i93$IS*cxc#hg8 zCZ>MX&eqh&&;R7AsX3-wk&=?KQBbS&IJA`6eS6l3wYoP?Y1UGaQ6yrTM-92P>Cwp( zuebH~tmq0D`3ANG!zb^J*yCsjK>7cV#~=y&o^L;SMhL_T#2`#D`s%_A*BCGzyf13i zhkNO(;kD)r3~y%!fui^K_rI5OgYWP+8kbmVJcade)Hl6mDQ{T?MklNYwk@^lb}Ot1 zjWy2s%Bj$@N^_#9nU#f#Wz*u2aR$w1RQExZ|sOV4A zqCiPqD11EMkWrRw?u>((nGV+%?r{5(*-PYrag5%B^_&E0>bd@_Dp>@v6*OH{D=GcN zpNy70&p$Cbfau}VTMxm{Zh!yb+xpPT(h}APDcP&r_0~dP45xSb{5@+}PO>Zb+V2e* zwkhvbMxQp(qTfbvDA+rBM6vr^RU+=t=&j`$`ub5n+&4Gfi;&l|>p6=)IAdO6WG?Ypk7>tEWyUP`oz1n5@9OJg5(vDTM13%%ea~Y(KO&|`{)Z0}G)c&&Mf+vBefAlB z%q0+WK9~4e`(2#eWlQse0l&+L!gkoQarMk zmXhIzj5~1o>jiVEi^&)IB~AwN$6m?HE`WTO55i9V@i%p3ou7_jS{}$ydWP{(R7?*w z;}>GWOtg7$tGt*Ak8P;L;ET+a9EUMBFCv<%z$#5`Y!ap!`%dii#ole255qCiQ8-OPmGwd zzS0yh(j(o{I|Cd8gIJ^|m#JYxeAr>_$5j*cbR{o!f{B^otQ!)_Ljx%;csS)JMVh4( zGIg7+fIV=vhv0EOFM7H3SGHRB1Te!!nl`k_bL9br@Y2DX5W6eH6W+ZWM{n+a< zk|SC(?4D5|oe;wV-cptgY$d(F$Gmz(ZkL;f%sed|?drBKL`v)}+PTx~xWMLxI*KEm^Z5Yz5A;evg{c z_4AX))g+>l^dfWqtGh$?XPm%yB(zyiRp#e5*knkOY8!OXdLegZ7o22?t%S(-WH9CU_xFJ7`iOOwbaKv^K5GvuLCs_`-hc8KX30Cg>+7`?UwH@ zR9)FSuDeyXw7FbP&nb~gqA;_9I$v40KZb8pf)Uw#R?DX8Q{1y&*T2K4c_VLMb_nWr zupD~Gpb~#r_bHzxsjwvh!W=p~4?7BvaQl!N1Z|m}R4IQ~ z12qPvQUsP8RJ85PM8O;+|T4x*!uu$M<@p4E?<_f-~hFF+~G3DBe(V zXDAuH2zdLCWxor5+u#%*>TV;J4ViP>n2`-#t>20B@!3aA$R+`@c%6i?P>ZEMaH1`G ztS=PTjsZLml4$tb_-rzFN#S)YO+k|;edldqBlF@pok!R`NHSDo+WmVTSyHRXm^QU( zgID9vfjj^W)5>F1uMd|cfEd7z&nZpS<+cL`B!yB!mYrkW0cbrytdaJU@1jNon3L1?ag#ZDJT@ICZfv|2U<@WtQ5 zjb!Jx3%Vv?O||mpdTOy6Y#3wwWzK9U(=MOS8H}1CQl1c4AbV$js>_|W9HVWC&zf1P zg4Wk-43K~-j6Q{|-AgxS8nEw`V<@XWUu@`9C~a659k5%22RwYl>afHw`?}d=z#ZPZ6Fj;NJm=LTCye0dAMnDCtVzc_`3tVW> zQnD6X)KzxY@|A9Wym0-lZI!EbaSpfXxb|(B*En+I&K7<#3!73y2PZ7;8-zi>d+?7f zZUs)cnU0qL1wWV_Ii8z1Xo3j$iVeS3J`UK^xT*>#e{T()S5{kI3<{v-#nsCJBRPz^Nbl;XX);(tF2k~uL(+`*}%>-K@( z@eQ^L_cSwRYP%`*t9ew)Y78^uGK|(JsWmZee?`Wm+hBvr+)qa2sW|G0Tn)!?XYkl~ zrho2#yH|T=E&=2F!@~!?swUAs-t!4CfQ-Bvu0eLF+D`+Ao&u^1i@h?Vcldw7G;;xF z4G!UIwx;j(d#ay3geg0o+ie;uan$uqDM891JT&_GTI$u?qDF_MpLqWh(kd`@1pT&7 z5lg2Tmp`a+Jvo-d=XFBu7#^`&r7mi-&AQ9ry`gahFkcqo`jcPZ_RaaD>of|5tQIwE z#2N}UELelQC$T@k9>@1W7IPm%_hS?~b~kuZ#g_-i;drgpLijJ%cBz%FZ^D_3IF&R_ zMBu#nk|cZ(rYsiW5_a&jBDeeCZVj8xAk+T4ik3O1b3bJRx&k&~kRE*O%_e`dYr9~i zbcAG|bW@`#9YLLZH({R6-1KUn|6sQPJuQEF+~339Jo7hV`$X-Ydj$B+pvH}GyTnOC zMnhLuRAgD8(2sSe(wzQZ48kn~Ut@)Y>W$i=eDb z4RmfgQv}VVrU_%q_-?C~Q96p#n2oX3q3N}Uvy1JbKbKtV$L)jXZeSH}9>jn>qnSI28XT4mf0G|{TLbeE7WG*DmCHq+BQ5&9xr#Yke zFDeTIJQNK4OMcXHs0aZBB#dCq$RO8Oo{rx4lQt#blD6vXwyh;6;0o zmUo^Z4?V^5;4mDYZEQoGbP|e1S~P`z84F}YA;Uht7VwQ0zzDc-W9>MlI8@@}`sN+T>$j~^)! z&!Ut)wg(=cV}W2{mA}6Y5Oa>&AhnQ|ir0EK7^DiCAj4o=(sTH;pYv z{mQuI{lV8IhzB z)AdsOCgFrxI~I24@=BmV?-_us8?MBG5!9TUbHny16%u4*M8xFM+Ro3Ka=Bwl7_a;m z37V}YUv7nhgpAgauV2FeUsE<5yGX!91k>SYebO5oO^VnN@b*z`IJI!(LeC<{=)9nm(KN@fJ z4bc^Cm}PCuvM-c%JIgbwv3b8!WurJ#?mdN*e0;dFs;QH$73~`i0AK0%9PMy(uSD38^(~IAy!!AB=rtGUAdwgsD z^8Nc+#fNb-sgRUV5^wkUTQ`CD!KcBEr{#v$0(nplx02F$uwuFlX^mgVEmET+&g^qs zplrJ&ZG?@qHyL`44s_<((P&9QN|C{@1QbJ8R0td-Qk-lR(>EYZH3Wl$J!2;8h>sdv6gP6E zX2hhA^Q%eBIBxDmC+y9>ufnsGlvm>QaXdmRQOzGkUQLCyiJPU>JdT&bYA?SWALJMcN9PK$ z0X4U0<`CG1bz}r1F1)G|^E`R`^PJ8FN!jC8#bmnxaM8R&?HtEZXDH%D=gN1-vPeOW z;}?ykA%3CKX2ZJ!z2X{r+QiyY@2hbhS=Wow+7hsU-cmN_r3&z3&U4KS0Vqw&Bu7$e z{o^jvl=S6s7pd)_U0;9J)dFud`Mr;a&uWR+!Ev{mC-S4a_C=LA{SUIqea89*Jyowch3kv z@f)Ern#qH0Q}bk4sY!psa(m8~Q7ob4%JlT~$Wtbw_rgy!2LKW+rp#`<3mCsS%AiQ5 zndW#MCuj&epzzBPnRA8RD7QC8fs`Z1Z6d?NCu*pRSl=z?QG&=ujO`>iqu&)105 zmsFM#AGNhU>T|0UWypL`@xZ6;39xC{e`}n@fdWos7H&!1+N5Y8yR3@VqcO3ml=eIA zo;n(R(SXa_{@kBDBtzSJ;VXq6_i(bf8ZyOI-`|IA>)CS<0ZVia!X}&Ey&xR)WxX>7!PZU(nEqQA znJz!rbzN@!aTjU`?RFZPu-eXaF&dXLDRb)_EPEaRekvAGXngZo`$h|;Q)79{7*xt% z+VxR;(iZS}txGn5R>G%+{f%39hX7kW>3F*m)!35DtcOB zpnrrX^0r|Vhg^l$^{nbi$3M9zvH=lF*!LcJklBaK|_c3m~zL)~Kr^5Z(Io5gy{C>HI=%eACz z?HUqpx!JM-nVTNi5dS22(MGvyDfh$!OWBNln{-1!lgV}fj0)~SQoV|KC5ec^0j85W zV6r9ZpYK4vs@Qd*n(%*v2g*^IjO20OZ~8Y4{mA`{;=2!r78twQ43qaTkGp$5x|{-P z%wY7<;%zu94bSPxwp^Ce0@*E_j9((dio4D3Km@_$L%{zScsW96fdIl!Pe46)NKWeK z?(OTQO`8^;_1KB(0nW|d|B~uJh~#ndI8S>1CwPp@_QOHNLHe>DhnDl$pHN*sblZPcL~NWQ8cPF*EnI{OJ*_*N;`tw8znHQy1`bg7?mi_)#{ z^nVIf46W*SiR~U86k%Ylr_Tc~RIxto@ z-K2&J$Yn@`m6kX}b88;GBaULX-QYQVPCHiNu&6Nbqw-Ld18A|-)avVMkgrnxXTo6y z$-)Fy%6X4tyo$cumqwJ)dFxLUI0<$ zeJB90F-QbE|jt$jsYEtuPNcyHYFE{>26xZU|cCT?V( z!IK8FnBMMSU86MbG-16uFz}sKUgYsCdQ1Eq7%0=hv#8|!x5lkwx8cgUWDpzqgSX+G zu8x9}==D;d*;xLtbyERx`TJrc-(N$sYjD{)PoUypk|On=P5Jy@<f* z+b??uddMKsbd>0}n3!At5^SAz_b}O;Z)rA7?qTE(S{n&t`g*xr{ohV4I(gI-p{;A} zTJp8C=2ysmtwPd_^87y^!zqn%2mXwudsN#k_Sx1ea*@(H#45fc==k}41#@%50mxeC zQ|>l-UOJiU5~)&mn8%$n5W;_D{MyH5uN3uHmv3oVB5S!?&~EAabSuWqD3$QLa#_@& zK}T?hpmc$6E3m~o&zX|D=+U5Iu-j{X8$lj6%d*%0@U5u|BS(cfl;3mWTuYtzeTOA6mdXz`=wkbHf%5f$NSAa zpnB@l8;3FUzdY5bs#~|iY{J*s22aHupWV@7Zts@~E1Q-yg8PgUv(q?S2M~BM1O)wm z?Eo}DJAey4Os(1Jp6{GpG)g=Pax`^-Qq$EUUZ zS^tOYeL33c$>Oy(`>kq0K+6bVh^dp}*{z4<6V@*a;<_sistP-``K0GRu+X~qoN&oM z>CLBHiG3O*lr#``7Q#?-7%O`CoLMcQo4%{O+q-TC+-PM^$T-QY-eTwzSl$7PW~jvA5VY zTAS7;RW({O_Kp#WJxU3&w-TGis{8T%-FwbG_niBWa~yIKlFxWQ@8@|v388Ytn1^4Uq=Us0o9pzE}Bl9$8hl)LhWp+ipHF@GpNHsZL5U9%R_+gCGv#JN3MLl z)E;^<_0ywil~=$XqHdIzmqI{q%wHMk<#DWgD<9BnXt~z@xv#&!cW5ZyUmp5?(-Z#K zhkwqX#;jJ;ogtG(2?7} zvBI%^FGBr17FrEH$PXX&9KCQV>C>(GA&MB!%1leS@SX(D6RQyfO9j7u@bMCRqa|U@ zUdnX|x)P-C)uDuaY3+;v>nhW1=&VXuGy2_I;QJ^g2OvS@cJH}_J(9i|F0b{GeQ0W` z>LY^d=qbj z@m84}NQ*i7N5P57{Fd(CLsLI8DqXtJ8y!WeUaOjZKNlEw8>N~fZ6-#h7Hub3r4M2MzpnY*Z0UTU$*IOU!;5HeEoWGFRd03?w>%)Em`menMS>AnN}m$9E1J4`Oyfs1=oqoI&G!0>e^ zc*FARp%C_Q_Tn?=&{B`;`$P}z*lK=5GW=xFL}9&K`eW~GlD>qC$kiT$hOq>ZrGrPD zQ7L=5S^Nz}`0!VY>z*1j^COVanDW!UM`?f>=qLS-oH{35@ara zFXLiu&~ms|v_~*yOArTqE>T`4oE#{EYU2;CYb`;VhQXSw`Zm%sVqJ)iH~ZDNd`f*X_^cOdJFA@{c^sR@+eAY0M~rfUXYoDoYA9z6=VXumBFhZ3+;BrI%1fhCmq9XWQU%UFTZ zS(6xJ;|{N3g?vvcQ&neY39X@zfGh#h(t7dJ!Rp^^{xvpd7AkFdi`M7sP6} z*w`-K*2jWgCC*B$Cr5DSnT$M#{HoIX+M^hU)AIb)LB!y5sgH)}zJH znbz?09lG1aHkcM+29kc2Sb$Hjp-ttXprWB6>j*E2W+eK1dV>8~ z%M8`_T>*P~*gvzZ)#8~EoGmG^q$(y^0kq>7W+K}z=qiU^TPSHL7z2_f890LSp zwC$bd$Vfo9g~i0~`Nd@=ZOhhcf~MWx2(q(&R;V*insWW^o0gWHIB|Ua^x@4Wbs1Jp zp= z{29raThgmNim)wO*CqzoO~M2~Z1VC;I#Pa2_vY+qZ{) zPzK~%4iTvUT308(Q%~q1D8aN@{;|_+gYv(fw>#A`0?lA6lxXPqqfP0AUA03QZDYihZI7#JXAv`wZP>pg+bagwhsGGy5I z6*!WFXE2i2I!+rfO7IM39iY)4j7O);QrIOkXj*Uj0<&+0PuR zPK5C&^ucRLKas`n9XWj9AyoygNE_<@3<&g>a{K* zscpaY)c`RSOY~m zs7iaR&1U=A&E7m7G({xqX=J4|dTMkv1776u?U%8%dNg~HE=ynHeG)u#l>aFeV*zE1 zxotehjq&`$l*a=4W9L~|ocV8s+4q0VI>OY+#>A9ArlIQi{)_)VZ$kG7Nn$yz#CU>Nn<#rM|F6Zy0O^(F3 znDgc%MW@5g*pwlnEZDHQs;*J-HpcKLFUprt2Y8!6KD)T@O@8Frr@mh{CzGLD)P_5h zwYrVEWG~7fj?{OjTST}g`|j6~yKjCVr;SRjeAnqIxI$Vu>YJ!}!}I1ROKhUU;wjrY z(|_E*%Tas&{~EPvQ`|XFK(KE0$=#Iu`~C^9-?Nl|EW^k1Gz;j`AuwIlY61I&HIPUC3ey(1bjX6 zY#Y(UEyO0rvprp{{SPhs_e+NK)UaC_{x0(Noz2PtzO8T1*i4P@Chnyp#75!fneP3D z)r5t0@f%^9hT_1SIWhJBG5m|~eH$wg?i~`>EU1YBsP7tkS^tgQU2C2^RI_!MA!4MD z+)9#@N`WkZ=kma6Y~ON&!5=*7cP_>=j{~zP@O$+li|GB|R!=5(Z;dbiQ2mdxxW-TN zck>NkodCKSUqzMDR9J;q6Y4#K{+fwp2WVnVIQbO5Ad_U#meU_EZH|cS3hz?cC8Nvu zg9YR6|74Io#W-T)W@w6fm|}yM=*$_H5>}Id9hH;}KgW6V&Ewog&u%OF#k&K0ci6m2 z{WmL1E!60QT;LuFujyj;e2XK)fe=Hlr#&eEC)7B5hrNUo2It^kq-J6Ga*yG#J>4J1 z0t7BO-RqU~PFYQbNdKebHG>+R%e9hMyBpSDQoe!TG_Oq9qmhElhY#_4{kU=r6FV2? z@$Y_Jd>+4rz*K4U`!*$h%wX!vl*anoy+!m+H;=43e+}0BZBGC)*=F*L5l*%L#@$Yr zGX~ibD_3015pcZj@j;f1)kZiH>eh~)q->o0qc4nw;opNpgXMyZ%AppLJ5$N*Mra6w z7m4-)Y*Q&)jVn82{)S&?#inlHN9y*I7gVhevyCMWITxE^-f7=AYJ76kYBu79Gh_tqi{rcRD(KNJO zY3tXIw;*NRe*@1O{I@n39cKkO0#@&ch(*qxtIgCrs{XMLEDnLgHCINGb6@|UdcU9V zp4@kZtLeWL52MI47mG~$fO#xR%G`gCs@}W`EVO&a>ow|kyRxE0UM_hEHdkd)RkGVR zUpESF{;oCdO=>C!NdIADTxN2CiHyLd`2z}K|9lFFiL=1!ZDrq+D1Nfk%TOI1?p)YJ zPf=i|B)xr?O5!_)6cFxykYAGZz&Ft|2`aCfdd2ctDo^_V0|J20*yf0xhGli9V2iC$ zb{V&)0S6;;6&F*FCamRE{&oTzPGbop>%flEB6VXR6(T~b;$QH6HZ}n>6E#ALjGKLF z0t|R=Ns7hS=x>joh#j?Gm)w1BaC%g}LkV?KHKaZGBPy$X@Z<=Bv3(7#6!AC@k;x%S z(SO#X%}Q1|>g(Q06TjmP-qh7jV5w<*86MOf8L4B*epK}y}zmkLYYw| zSfy$pdJg*(R^%Z@$lJeU)P-kLi7r^*8=FCX%kYGYu|U|IYjy zmm1i+_oc*5^>iZF@b~?6jc*h_x%W1qr1`4{D@SvA$?z~aDAI)r-TKn zb@2kpHB;JBfuE^W0zRiW6~+x?kLLHM6TK>cQFuE6Sc?ISP=}>I32qhpiA#33jZxv-VI(|uayQ2{DFb?ak6Gxqw^)W!CBWd_?Dbr zn)>uhi`ZS2wSKC@#{I)#;}(tcyP0>&^Rrfr>@(}>#PNC8ibSi6pL#?pe>0@LW?(qW zaYD zUz2@#@u;BG35Rlk|KmyWoG-DHznsy%q=-Sk=eg8aDfHpyRlHGC!}%pkFfBV3e7&{Q z((h-S2(CCwRtCaBcSP-0;5al@-{EGe09=UQ1B8PH=YLHeq2>;c-@&$9H8TwEcs$i^ z2wCtnSACaIc*KITtj91V17(%R;JjjEd3g@@N7d|`wL@=XU6E4(8Fp`3{!rAm#ypnf zi_2r&NfCJiVYs1xL6=cI?{-cxeBXe?lK<6w4Gs5a5!~Z{!rxwqA!0q<8F~KdLVwq) zAN+odFVZUm$IURu2M3clkgpS>?PaVJ3I7oSaR4Fk4XdGfF-RZg5;_5+M%(-vG0eqS zPtFb;J#xD*D?kYBLA1Al*^v^gi<7oO`Cdpgc(ai(N-HglC{M<+ zgdMyA`=*MK?RNjfzASpnkJ3k^?$fF9Y-%hDYTUus>oP0*I+}4*1G{-ygo%3{#2_DP z2ey2Jp5_?NCvW|fxF7hcWP35^V$j6&?)efQ#| zO9cwbZB^mSf=`d4ospNz`ILF{aTQGTnpmCaH3UAq&CSit6-cx*gFwkZjGZ<0Me1mt zaN#G&T5imES65_b76Osd-TmslzJC0VAAJ1Zc4g+Z$sKk&w$jp=qZFh?9+8H2le{q6 zDOZ;oEfvJ;z3Mc&k1M)1*I`Yx0w!IH>dsN7A4+?b`2l7S>fNj0^+^+l6y_nZUj3a2 z;^#JLfLuWv$Tb{~{iDjAX1>@H_Oui)^{z(<$ko#o^Br8#gE%lxhK8mPeT{0nDQ8$@fHJ9sMd7(G4}-Ex{SF#+ZQJhI0O`HK4lg(T^?vD=F`}GLx*Gg#UeVdznl(63^}F@ zAdlHguDz${l9}oV!@7v2*Im0L6kcmi5_v>y(;55-f?JD_hakRjLiQhr~T+N%hU zt+eSA7L)y4QV?T_pojNGHns;y4PO0UPz7?P+Y9llM=N#=aNVmjcjL#?}cL27TzIlUL))xwJI za=t(*>ha;ve{SADYIEJ4XC!t-sQ;rWpLlW%)Fo!W93IbjbC%g1B*jOQ4k304e@01vpvdeN6K|JLYyK>==rmjI z+h>4RUObP$v~B5E;8H?LY!&>1nk0s#R?&(h>1I<&Hy=4qw*Z8p!p^*?{+Rj=U;Uih~iymChe@Zu`saKFm`UR+OuEzgvrBrlyw1(vqky28VrU zaamLbl!B7zxVRKARq|)fduT_m-m%9=U;nh4_%H#a-`24bL;haR+f1yim3n45LQVuu9$+61O9X!ID;Yi7R=dVT!EZp38v+IH{Lsow7XDj{^H4@Ae%FCS?H&~%@t@K8XKcc1$`}S{cizd zHIl!?$9uDDVt3TsUN1! zE{FKfS82r@`J7eZx51|8`WATO540>2?5G((rIvp<&~fK{+WjBL=_0Y0e}1137c(R;51(j zfC!;!!W!M{S$z4u!ZT><&vie_ z<}sk?_kBLt%OT`1f8Y?1BRil%EM^14;+mcHTaHHqCIWqxX99n7*Z-AkT>7MzSQ~P% zZ_bC0n#@u-E#8~WubE@&_+(o?u2Lq#XtuPcc4z)}Q(6p$<9*`?LV9fp%NO#h z*YW#F2G0n&_i!+tcTQTTPq~gS9R~l;fDd>aBHlSb3V1@N3uu~dHEJ(#)m7dsB>l8l zZEsX~rSji(wH#(CJf&1JpKzi=@;bB;tj04_`c(CX@<*;x;ke-2o^P!_TGWaF-RC~Q z9X1ExKnBEf5-H&+?ytS^WrCQ*q4qzbZ)c zJ4otgl20x2qp$hhO8O6n6;W@)io6ng4>So9C`TuG$lox0 zZEwVkPZ$;L0ybH_^hp(HX#hwDNZvjEgb2uK|LP-BL8y|lo-SGxU19SLCSbIM+)b*a zzhxUH^#M`v@jYGWcPFk7@>)I=ie^~ebKh1YeXBD%>_g$HDYLQND`Qe)8oXC(cCQ1avHq0lKbHT#!Mo0wdXy)} zMx+&|4o7E{-+O@~n+K4)jTs)Sd`xURB{*=pJ?(mtmPEqu>wg~#aWt2gDm7vitoX zp?;hOkys|TmoDqp3jG77?llE;nEbD!GXb<8qlgsw`85CZnLc%3#S}rlNYeGyZDe&p;v7TT z+eRvReE99NeD+VK5!R6+eM_H1YeKzVZ1*?<$B&Yde^~2eD)e{8Bky%|e{bgTd4u_W z63_X;k%N_r08$Yc+@)Xp;=B0-E|sO62vDL5$mh8yVe;YMX)nZ- z_hL-{%9cfByLIeifL6)TBpx7~X$yemc|<{hyYc@SWG+ur#z27Ne7m^^;CAAWxhtX` zAA7$|bwl6Buv~*?bD89b29A{jceb)a-d|HdYi-vL|7h*IXi7VxUJm*sWx42Wp+C~^ zNd)do?1gfiDgN6Nx_GmbL?$uqp44>kOYMclQ@bX^E@|F6lBh7&$$`5~kvzF~js0Io zR<~>cRobC-(GGpmH*7)Qiv!~~>V6nLnRPth6*}s@_=_|7uY$z6|HIh?c_}hOdWj19 zXD95YVTJOfspB0&v}MYMX|1Um%o6V~p{witurFuRh_z)06iKHb{G>9CXRi8^7`beO z52)atNVfOQ$BT)bBe9?)MpL-%kb;0I?H}%WOhL%93nIrKYz1Vv@uFve{(h*bpLI)r&9yWo zh-k|aj~rw24E+XfkX@cHky z64=tB{*^%*qtsdL{MexmMSe3+-v~cs2nb0&d&s@pi`&|lIj@LJW0J3XIrOu)*4PKH znWe;#5{~4$AkalA)O633*v@zjQFb~{8)z%xQRfD1u%ubp;)T*>*Iy7vjSFaE zHWuprC;#N(b#MzLxyQ(z_*?{>>GKc6i1u+wXk{sc>&Y4=V}T*O#|o#B`Ha@bD2wyt z9n$~oD`U)^;f88Fkna~11bd8BQtn*>O~(;ZymbzLC%m9ZnlmnW#jLC~Ui&*89WB3Z z&hn5;CviU!qgh+$9M@oMZL$(VWqosMEGm=u#itUM;dgkzJ_;7r{2#!^|>?#tq(fSdnxrc z>fNYAr(S))8P$Io~6-XW)=u2iR`O7l?oBh^6He(_|!4_&6ev zNPgUiI+Z!zOF;>2jF;7-9?%MyS%|)bRz3SNKW`otRU52rYsw2h39Bo9HrH`fUtiz1 z0x-{LYerVrBG3^8BW!$KIsJ8_08m{T9&ycd@9E9h5;%y3)q981pZ+$7e)Qp^04cf4 zmYHVFe~X>tR{O-lv*3wQ|ASTYe)W;>Az*ur2(1k&qDs|YOZ)#Z8ty)5V!;; zut~djkx4!QL=hfx8n*acmAIaRgDUZu6Kx*GiSE3svkg_RhoT5Ad`A+K&<;;Ru#~7M zSAj-W;z)reJ^(B+341PjeDG6cUw`fSV}q0Kv)OAOdXiKp#X_}X zqrr!j9XnT9y`*>3akA!CIVmky?h@{iEk|L@$O+F_$!}RK#YoJ=_X;-lm4;#>n$g!y zEdDKg->frWjG@NQZv#BR31IbForEhkCncz4)Yt^52ELTa5F+YiI7g5Rha#z2!Br1l z#ZW@;4n(A)b5V*|N|#tt3Pz4-x0r<8+?+Aa#bDF7^O^IEXh{q9S68qU6z+*U^lFf6 z0k!hl@Xu{bhj^(p!7+M9ZDJQGtd{s=`(BjAA+tGwG%7-PC*SR>aba+?C&3`93@O^= z4hnyVQ6^j7+>%$qZdwZI^zG>)p(PTHwz|4r z&UjvF|NRGBZH%y!feGYogm|EZKN;;$I+S%|Vpw5D?bdSGV$Hy39A}GcZkhb{#RU>V zKgFw=Jg+MA8dnyRX&Dq!ic{>Qq2fE9FvhyX&h^^<&=}gBl4Lyi5>PBuo?tVWDU~!B zGdcK!HpnIlBif*4TS@rtUzK2bv^^^XfCj33$WF>JTMfbE7DMv4v~Dwg?|%T2(lncA z3OVl*O3vE2Har^Wd47=OnRz_qS;7IEJKfXE=dVtwIP=#U7^Ep;Ba>$^gFK#(QqKT) zUw0$Vz-Dx|v*ah$I#}N^7>wLk|1({LG-w@{X1A|=I5(q;cd}BLi&&;<>JRP z9wSmgH8!bs#Y6H%Ba#m#BywevM5$zt5+D>v>bMnh?voeSeYLydSdOjWgt}s8!;uhL zlOC;sUrZ@KSekX2u)Yhj5ocEsi(EFPv-9*aPN|<4hJ?V%uf#eUXhFoMt1lXnp$oUJ zQ<$Bt>lDCRAzlVcTA9uT1&Qhtz)8cAl<0JEbh_ULwbf z{MbSAp`S8QsO4#+%^*l7KWmD>78EQKf%BdZd%;_AdSSDzj(RBd({Fg%eD&JgCNGg=6HVm#`95gBc_gcI$?i@J&wc z9Ni+r=|9Q7paEHT6!FE1kb~Y^<##l69vWE#Yo(te+5Ik#V%cT9l&ydNDE>q%`+)HQ z^V~Dw9A?(Rf=|;V&B`wdUP$?7xn>DU&K}HCFUz0gwX^@v8jOZah4pOqCjn zqq^8wdUsu|_X=Yf1RFcyt>TLQ-xP;%M#gs=W7Uaz#_TKrJfSJo`(sRIwdDaVGb$$4 zrm@BMph>nqP}iiTT)7l4_J{+F()JqB*e8F7jq5RtlGN4_c~#gNO7J!xVSQvhMw12^ zU3##TK5iN~y|5u8_n5z@c8)QFQN8J&HQWa&>w2FAp84R`hWC1m@w?YOoGnPZ=QCHq zCpmB$_MPh%>{4>u+@yrUHyyf}qGk&FiZAiOv?ym7kmYpu|8Wt8pye(89nn2Xc*wp7FRH^)LWl-+ zalTz^7a*opuTDRf$(f^U{cnxX{gdgI$Ms%(B~dqsdkv_KT+ud1Y5E6}NJIxnZo&hhHBdT_Ba-dfpuFkvTC? z9hI!sX_-ZJobwJuuSc&e?pirAlqw*Si2ApITH3z&x4m4ynqBD?(Q;H>*4-ilgm-bv zS1Ie_=fiLLXE;~U-I=Unt!v%1@_K)#l?QBp)Vr{>JK}l5{Q!JGM@tkqPyj+4FqtPx z^S?j~xim3O=N2u0l~5llvT974KBIjZ8Ucilxy)HmN)+rR(0)-1*k$7s+_bVK2K}eK zIswZ2#Avg>rmMNk|MKX%g7j) z>7!l7JkqGLc-)R$ltUSw$8v=6e%nMCUf2oY78SPvI2NoF2M|XKZ0|S?u``L{JFb&(%`cam}N$_ zOs%xP+3ge$cG<^8yY7bjqcHl*Z!l6weNvs;jWyf(2d|pPXc@+ZKU6%4B&l^67i^0P zbKe}YlgxUa-5~Dq0n@+PKEo>)Y+XfSo4S!qQto=J9(`A0V6+^>{Z4vRaXM1CN1gqx z?B5N@9?>mgd2ZspkJ;c|A7XH`>-m>%FAhTfz*0y>7}n{1ear4Cd?PnStS>8YXb~JA z^RR&7bF(SdMk_akq%S7d>WR74PqsS96|xKkRf=Wxzi6G6poNv#+HiCFtzRyp1u!aq z*WQtw!{4yRfFDugLOw*PwO8Lv!~Ys;9yh;yNV!W7YWwT)il)#~Q=9XQ^BNeYdA>jm z5HlqT?kmKGM@&FmJf&;s1!5(TDP7W*ecFGI`zBeZt&Su_=GzP z?dYyzD`eD085pq}=l{Hvl&5?nP28Q1Z+df=pEj5AadNDFUGvIF^j|EXdzwo|A_IjHmz^^(oY%;@&her4MhgK< zfT}kN-(jQt_)g2pxu%63(e`*-$ZU}(4h3OD2|v-<_vs_U<2sufHpo5GWTLZgbv?vZ z$?2K%T5yy;Qc7#clteCIvUH*)ujW54{KL%}MuLcrp#o+vlSHSBrFi&Z!HUE&;K1Gm zngvDUZs4m*4^Q1*Ki`5!9YKhBg=(oe0#%DV;#-S8wZ}SXG2Q#BsZ_ zt(U%nsQN{xj#9s?u5i^&{hQIm*vT)mVTg==<3W#4cO*&|m|Q-ahfC(ap{iso&uGwD zwOL-D1%2T$k+esgo~#3Ixim^BwcW zf&g!+7MfBB5MF>Mu~MHOD0)g1AXLe<$FDdA)VER0dzx|nn3M4+5bW!%?ZcJGft2)S%zp!cTG9C9dRWTihzi%E4vWrqi!0TR#0Ql={Y4@j| zFs77n6~^Z;u$j`^`w)T!IW-oo(gbm^a$Q{ zL`;tq8g_yTU8_qa98fZ@x}d-lai96M?^6|!tg~S}xF<`Z21FOWJuPmvBRjNI+GbdZ zS@^OY9Kfb`iL%V1+hTiGSKsropm0XbAyLQ;uv0mde)v@SyrUKha^5;Qs0h_ex-uI@ zuF;c{E%g8e2F%=xpQ!iN{QmZo9y0cCk3a6-dAV_1-u$E}cAr@Ch{ITLFB`maHk}EY z))QuUeiTTR#M*afDLAnn8Nb{bZ3O6BQ^vM^{rWEvmYyoAS3(ye;^mt&%TKCX-e-op z3qrCzNT5J@`#eU>PDKsj+N^H?HM^tiBC6=mEXxh)3fJ<7qqhCsPWdd0x4D z9|~=7l>Us|Qz+QFX}8sWU$J$O&rO}Gk4o0=vKRHH>=OHv81_Aldq(l*1&y`~Httw+ ztHvB>*F#KQ`pY}RwDrEM7AqvhP711@8+oFS4+>Np`bmpZ1WhV{GGeKVofMU2M1TGk zdh}bP@zY5I*lSU z;?61(=gBh_VRM-3YyWI}L@w1$oo1nY{{!hiN|>m@mTD_%bX#m7uRN(ZJM0LSQDH|I zDYJshrfUQYRmIf%<30rXkbjp}mKreFfEaH4f)z>7D%`o2ALX$8h3e#Pgy=63>&V5@ zQYwk8sLpr#KW^5){|9!8oF3&yF* zq{4{rvrafWg#BydC2R^~#1|cM(_<;ukJ`SC-K%^t@~dWL#C~&1hN0TII@6K9BhOBD zlGH>kB5pVCXt^(YC9%(3yq}lj=N-)op#@P@GYmDa>6h6Un^VO3lk5^(xYBV%n3`^rY z4{kA5JMw?7v9=cS57DLtUoR<<`gdQK)s({8YDfdwJ)&ELykFz+eiEGyL7Q__tL4JJ zE+UqZCC~?zhP{8lma4PJk=j*{t2Taw$!d`*&zEB~5B-#Xw%yB~WEo-(`jJJ-18q54xZ)(-}1`J`dnm=595(KhMaVGvg6gt=|BS32okojl8Vu9dmWX)$Nn zxy@F&(;k1VPilrS`K7Ce>f@LDRv_SYb2PunC zvgG5=s6FG#AEy;QTN@!0t^Ruh{J_Qbs0Z^=m!*wEl?bhEcydu?Nt6=nRckc3&+I9I z|G4qTe72KY;fe6LEcCGhz9)aGh11@2GSha4dBgX0Z8c~z&(1apA#Mwh5r;y*VFgIU z5QwL%@VDv#b9>7ctLr%}j@8Lr;KOy{m^K$6Mcgk1#64_8qWPYEWpn6B8ZQww+9*Y9 zo4`*J?g8)SdbEu+6+7oqzJPm7`O+fc->bf$QbXpqv zwg?{WjE*u)oQr=p+wYp}xEacL_pYU@{TpSh@}=n|Kman~*R`UQFLGlG(CMNY>*{zf zqaR+@*4)A1D0A8?dJ*_>)lsI+?5vDCL<>L}7F6_9=JY)WREiK?ZnvRJdzrKRs? z>d2N>SO+)Afug~G=oJL^@_;&AMOj%IqP-i!!~Zq6un3ITJa$o>zmAWHh#*jMD0nh( zv{*lgYadwXB(h^i&d%WXpPC#DDd)s&F|FU!TjAg)MSnZ2W*)3$hj;q|S>!YxWuw&h ziQ6sSZ(EMqh0{zMcsW|Eir_mn>>|QdmS<0F&dlF_7l~x73eGThIJMR3PET!(m~|;W zLVKMr4Sz&OLKs|t(h0eRUw6lkWcy^Z-Kop7!9wZYPcv&fzhSeDD#mgqTM-!vs=to3i^7an*m|+*EVg1P1A?bhRknmDsc9j+Mi-wM>v#{O zPU>-Hf5PK-Gq1`Pn=haJe!&B$R@m2{tg_Q_Lh+2ijybW`@oVH+j1kDz914aZ@40VT z4})1$gMuH(h{|!Pt&|bU+vZ3p_18*t?!}>RWF%ZEzrneguYSYPOmi@`KTFE)D+dJv zcO!NEhqJ>ge0?_6OI%Fc@@DX}{7>RA?4*dW?uVW`9S!Z8p!wJF0Jcb7I!ril459=s#}*5ZV^W6UoQ~PAO-&LCg8R#B#UZU~fZ0_1=(-7f|ek z=Yv@5c>UR|-#m4%t9TA?UDY@IEDM#R1@P4WLc;3TzW1-s+{|OJ){3=j6Z-I+cak`!>hpB2{ribk09 zvetsZ3h2LVo2TPx#s1cdR0<@e6sAF-%QI?9O;IMdQXGXNGCmG}@LiaShF>g8EJsAx zSgz*ZJVrN`CeqIM#F%vy1ojGcJgImYD=F`j@_?0c$n0B~X*{J8!wGu}uF0wr*oAeJ z@~v~aqr0LeOH+?2GnA+NZ)P>FnJ$$A5;K5Icb7x`gc&`t;0h9t(Y9C-a{P4paKboH z3E8tVRNgPjS#C^~xp+&oixhalUsf-Mh&wu6XD?8Y{c0b#MAI4|x-Yb)M7W;HNXNh9 zx6vz{(@ak)WFtz~Bm6|-L9&d$MWrWJ&jL|XF#zp*9|4742TO0727}uljBC`+(pDbk zZ>a+ihO)CA@#C$3BfzbY_VK9Pc$c43V-Gxzzg{%uG^LCiY=Ngfi3Nu69SELW-AqQE!=DZ8G%7Oi(LZB1s47;t-b7+WJ zLZO_>;B#4ecy;!GUJV|)wR>dtHR8s2?L2}-#b5&kx0Mmq(R&~sNw5H{ClvX~E=)30CS8#bsxMNNJDojF7r>6P(YPkJwyLsee?nwQ`qPe2jmI49@ds;FmBk5=T_5B+g2Bab&r^ zu)q8dT+`VnIs;`FhBfTBlk|(#KNz|JV2K_L-W!2ISEwwokI07sGnD3GMHWbt!MF10 zmjlo8u8m2GX_FLRttrw3B%aaW1ymNCO7lMb#a0_y@rJ)wV~W~RUjKcv_}J&&p!&Pz z4}sh?b@bPpWL`j9_S&1xuSY+>Tyb=eC?V7JrYDygtKyrMyT$KlRHD7mCCkHn9VHjB zx0o`Ui0vPFaD>QCXe^uHrOi;E-ZoBxyWnSr)s6+++G=?{`KOV)Uw+#6r`A*6?F2j~ z#{C(2c_$w5WH2=EW^qV$?7}am021r(?U&=ao7d@62~i~2!`bl@`>08^Ez;^MD~?0r+;f8}flgLs7i zJk4?4#3*4V{fvqtHfjnL@Yr7QGQMx>o-N;kJCY&-Wg4M`j0M$v@A(347%UR|@d_g*=kg9n>YOiN}(U#PUrlSAvFT^!;JQPNu4@*B}xe9nUU%eKgNc zzE6*4r5S&Yo2dbMOG{H8vnCb1--=`X6$4!g)p05+E)3yCK?>9vC@)r7z z8#w)m%>WoO$Lr%ue!!=VRb{-#bSkg}J~yyoi93sawIkzU#XY`5wyzi;HyHYUwjTsn=WFhc&2X3!?6R3uU?J zMJ@F@KAlK4^bBPsWug6W)Ecx>R-JP;$4xTBcO#hNc5ihAu6TO4;atDYgJ8eZ;r-QV z!_ZX~${`(}C2fe~o=hL(q3hJ0UCtNxf zHXgnipUx>(YCe8ww*xpq@GDvY!?_t-2 z#$W8t#kd-tOpSNuMXhLCbYErY+^R9U#8l~{mqO>W1M_HdxLnA&eQQOxZP!pInLe|ir0PrJtfnkI+BI*T2In|w7GJN zJ+qq0W1Hju#nxMgHTlPH-yjGgF%@MrC<-WDqX&wB5(1*q-E5=VQPKiKK&49rRJulY zOG!9ljBZBfD4&b(?|0wN^T+ci$FXB$yB44K`99Cr`I;G3zMN=q*15l$Nmy5TDx{iYvsp zczf|jpMNhuTKjVjAf4S2+0w1ds<_oCbS;C4%J0)oPdz4!gGaDW6+uScVnu{0^j6+2 zzNrVb%&haJWY%=5KW7S9A1OB=+@&0>hPFCpIPtscpj!x$*-3mTFXCBWS@uamls@fz z7FvMkOGV2D-5v>WTvYpRaPgaK6_)%rVe%z|eC&V+S+SeF-+0`Y3iYTcjeX0$LZwdS zxM9WD%7t#uydZLmiZH|;mQkW2Ch=!+fuYoRVA==W;#}A-Z#Sj_I@R5pg~x{Z-_(Ed zlDSxQcS$Yv9@zOnv@#!cbX2$``ZLDQnoH-qB0mTjsrYxjnpq!&6^m67hXQ-XI^v#m zp%`*9+geCC0+oFQeempncs_XV%c~}{{)}3!J~T9xS8sn6{q?Cq%oWh730 z0HwtSQ|NO`glukoF{y}h(3V?|DuR>qyI1UTx2V0c$V?+MOPRR~an{PNn3L%GWW~Lc zD-@nF5z7kWkNVo8+nF($@m3{$ynPtZusA`^Wqy<0CoV_@T*jl5y3fUq`v%@|Xio;Ffg^7k z?2|k{c2BwaLcwUsL!_>&~gPYFk)yEs8h)<{ps2q4PI4 zZyX#o6Tms$)+10A5X_NF2TO}<^GqkAAurLhc`5e{|^g5*uHdbzh**m zznmVtnZFWfA};_B)1U)`H%~+NDBc#o+%0VcOfV2@iFZ|GobWE<5D8@&4ByW2q*%{& zvh(P~mfc{Ts%uTrD5{>R`A6PIV~#!G47mq+Hgt=}j#IpBHLiJZ6Oi13g!qXazQv>4 zjP*V|%(wX(XUPOjh&qz_Uvt{6>}N4 zoN1_lZMVf8y{Cppzb&GSc78&l!m`Z-KZH^gtUUcCxQ8bK(kLN?vTBfd86!;<0 z-AQ%ms8es^#Aup?ESE;(!B^x)m5tTVIkG2Xy4L9Oj|?Y#B(b|kWCd@g?rqv4@Jacn zWnDSzB&VqY*wz@XJk2<~M6El1z;9$&h6aj0#f{JTbv?L~YWqE#JB2`XbDjk}{Gk1o}*>yW0!}>3(~_qh^^*6%sm>o;w%)&h5j?=tB}0 z72T4?);z}zz13Ne;~y)7F93k*=cCA0HKM+{Byp$k7>T*gBI>3vkfH6tV~||)3Se5e z?S@+FasT7urp-#Yd8w3YstyLzsjq&G0v8JYRRkJjEPgB){#sJ0h@if0YQaJK!2497u9e&~umEgGG?oogHm{rMotBElE6GK8r)Cg*Zf#-TK*GSc3QU?B)dhBYF$a{Jt-BGF)Wf~=?mKykw+jW+@~l0|bkU0l>X&melQxe#FwFY!+IVESbBaVynxreI*X1<` z#IS_T@9A7BoU7TO@4O>YYw_CWS~Set^nvj+v$~^};~nZf`S5RumD>69;x+e!TjXLY zfD8anRd6nnrwN#O2{Nln0jv6V4cL&T76RXThq@CGAvj{R1jLNvkIY}f#n5d4QrvvC zyGNyJ=M(q3UJ=su-#6|tGAdLNFCFnTAFGq1Lo3Dr+oU_^vNQi0pTTDe> zOpF1%=vUoj`?@7^P5UZ@HsBOYn;YHUGEX(o{=GgXP;OJ{%bUK%A02$T%D&+Y~0ip$~A$kPj%=@cO!5?|1 zE5t_cGx?7Zi3O7FP}p$(C?g5t2#016q)=XLuVEkzYo9GK0qtK1+BRVARrDn$5%6b> z&Iwqm>)VS2c|9x}Og?xbCImEGG6R%0RfgzncUvj%Ud(^Fn0Y%ta1ihlU8RyD(OApP zxg;iM_k++Xv6fr~uVeaOw59amvhn|-Eqnhj+A&Ek ziq!tP^*eheG;3;DX^@szo`fvuyMhie!I*cl6YUXjcsMsPuI0CGmCXuthoOM?z(H+g zWEd0^bYPK}yOCX5lLtLMX!O?U~!^M1etEe$FklduxaC{~{q zt^|aY?_Ix5kLC;G!D8q^D1hEL`maCK*lVjAAXj<2Z8Adkt#FHoCb)r>16O@e@|Lp;XnSLC(35t;r%sHr ze72!Ui{Ge1_1bLDaCqX%upe!q;I>U0wCD7cs~*oWkUxi?|EArJ&y@ZLS0`EZEG3aY zVtMdg?$iOvcCRQDkOYe8YlP-&zU;EXEx;W=saMOqXNMVSeQ%REO>ja2KK4gT?UT#) zD2^0^(ts&SmRo>1MHI-@d{VxWWu_C)iQgotr+Fyg`-?H|6P3Oe= zyHYUVitjKszchHa2R6zROsh1okA*B|4 z#Fii+>-xeND#_t#T=Nv_3M`r3HTK5Zj|e4e%f&wiZKXH9xmcZ;o428Wd1E=bH>K)l zdFYBu0;fmcOkT0*9?=Q9IaTUhTRzv!PUaIp{M&urmnifS4-g+2aih`X{9K?NjAxYO z0u6I-jNe(V%u(Nl%PhWN3K->%9+&^DoBO?J^A4XofK2!SkFa~MXTW$V&uSZ@#@R(=$6+tZbbquxi5i88n`|D21|6HnpaG zMpfG1;29(7alSj%7;CoQR=8Yhs`2l%`rKcGEv_n;ZjqyE>r)tOwK3XJI7V3*Or?q2 z^j!C0yw~GVD30k&keaGjIQ{sOW9i<2K(EQ;mLo~vX^=C548OA80Az076#}XWpXpu;sx+}n2mhAQ z60301ndD!F0})3NQ3jVs3AUhOtx^@TeHdf~GrQyq*|;%V9<(am(jEYnGaK<^1S7RR z>(+fVSM(xmAoPzMh*WB47k8tc$g^eY=UGn+1L1N`Pf}7$AY;-qMCM?c>4W2XS-kNT z_W{@Rwoyi`@Iz(f*D-;?@al7GS<@~eM@=Atqufl-Q(WpU9n8;sGC{#lni&)rZEKst za!=i}SvG*0rI9bB$xl`~1fcAE_P^+7n2-|U1U2*mTYICi?%L8t!nfHdhT>+Bo_Uu) zexV92P=XGG{PRY;^~q%xjl&YeGeR*}2v=Vau(tmzocH#?qyu1v~d+1WmS<; zG?E=OTK{c<<$8P1&wY`-N*kKr@xa#;Sb)C=FsnSea>FnP`6~p<#Wi1l)?EyL?a8jO z*!rLLR1^P1Au6%XRH={9)y}`rDpe0m`|is#-IxP;z-yB0&N6@gTx7hHfBCOb*ITDvJdi_@-Ph(t=G^^RJf{_JtAu5a+}^%cmeoV6fdf^ zB+&yAqMpw8Jho1jRDw-TZv`aezds!O%#7UlfLsD}owXnvtL?AO-lIS$Z)>rhm!Dtv z*z|F-G$;Y~{UEU_rD@6!v};{`rNaDO3AFW>xjp*UKIk@*B*{#iL1;Bu%+we(uI!&v zHDe4#DqZgOTmW^?HcMo{0^r}}<;D_Dd|MQ#g10nVGY=h_^6oYi(v9YAdM=FZ@H~qh zg_K;qkqZn#6$t#tDe5+gP%Z|HhtHENTjqoe;R4^HISJUF9eh~R2H8ndvB}f0)D9&~ zDC3^IOcC8PO|77F%wO8GJM-zO@oX3Uicyt<<;$jV@eS3wrAT})%}q6MO%?0HHAbpJ zhxN2++>KL^pT}gr<}UXL@r{IT=s8B?Ld{OFi?=~9=`GRrdQo=Xs3BI_uQQF7jnWC{ zv80y9QckZgXSTNzGT0i}x5q1uFN?_?KlzG38uT^Ntxr5-a<52m?wqWAy3s|i!rDKq zEw{Lhqhd!}hFBB{az6=|{8AMtg+Jt=wz>;I+AgG^>2? z8Z0^_UcTTizViMv=!;lhwhBy$Ebq#f=4Nlx{a*<}e0s`TGhJ#5x(c8tyQdyM9g?X< zkpE3#9YKJDP8Q7yGbi2b2{OjWtwHZQDZ2bkTN@C9d%rV=et!;@ZcLx7Q^j|rxpDfz zqTod(DUcBcLDZz}3k5&pz!^>}ZLi9k4U?xe@w@EZ3WDt$5h!oniWaZv69HdB4j0HHih5B7tP1BA?HGr2RaE1?K zm!Ga*{++IR(mlNCD%pD z%E-zhu9p+3g@vGI&SDTREvbXW_B+mc3MfBigRXBK z>YFXP#&bR}T)R><_#6s!iEwam6iR5c#1Y!YA}*>knHV4ulM?4PXBxl%K{VneT>M^b z)mR514~*)TCO0jFEzYBy;p?-GCoGLg85x5jLzAYY_)_y2hKeCEpcD~XO9p}~6U#~~ z@jc4QdP!ALC>N(B%|a`By?&IqZtne`aGp1)VF5ku`4~}0BX}14uR}zld9kw?4_%!)#(Lwg0*pN)K z&1Gw?Isal%6fEZJU&PKgTgDQG$mZo*&eWHNt4PF}WTSlQ-U&HqbA({!)7jpiTse{a zAqKt5=yuoqVZ&k?a<%1M!DGBhl|!0U0F{^K-r{C}HwVhVm04_Ojy9!<^UbBOa^xXJ zV$>5E>W3S7q9<*9qt~Sn%Jm#{0!h*rj0*eTqT|Hb`pH=jV#{Y~0Z7Ol0aO-7;&WA3 zlU8!cz0$!S! zVNLM&Ui+MyZUijlE8bqVpP%e(sVlgg_UUct%+7a^21wYTfY>q^uOLG4D$^DxU9-#8mEYZZtoZ6kL1==3jZxTPZ4=yR4 z2-)lwSPiRS@gsxG`|h=V?*LWimfY)-!-s8h-$#%`;W4pIw$gZNch*W8LC0{pb}4h# zONwL@{3}S0Ks9@Z%(KG0expvm6J{ocWs-q>2Nn_k7Em~CyCfb2$TQUbL|_=DY+Iej zragfJheO{;<71_F`!L-TM5d``jzuGmEN}Y#!Nm<_(JGeHPv(dz4FGMf#?8;4esY{B zQv`ZxA2Nw^vKiSScfM7N;;N{Y`;CbiJfiLkwLS*KiEfC;=N@x<(rDp zM)GpU^#c9bo7EtZLBITg*=A*Z6v8n2foApB?*@|fPw5oTB(mJS_Kh^a*}bUum$F9#$NLvD_7wW%${ z*XkK8A4H2{*f}@@o4ogzA@IG-P!#|th(C{$+5c@uI8NZQS|0v{#HZCQ(E{5mta!9d zS@U;92^cz2>kfkq2+apI6`K02N$Ik>jn-E5Fi6c?0~H?>P+iybo+-}kFOQAb<3VCK zQT-7I{7UAb!q0aNtm`GczJA9l$aD6aqKviR5YI*P1IvF$NI-kRe7y0+65F-;4aR zW@f`$ga$ba10W`#tVC_Twa zV(k3>=n4D43MDN+&-x30+D+1`AUr5%CGX!nv;CUR{Y<1SM+Sz{<2+WMNb0g#`7{9X zIx=CLVl-(xUHvINxt!n@>=>7%>~q3g%iUFu&0x%h%2i^ZNoH7En6fMs7N3_^ zfF(0v0Y@@XJFS*}eV+fN-(+G0xPoK4wQ0M@B_azV)W6F+byH$o@I7#=TR8y?xY(g_ zIysgxx8J#%X>SuGZ8=j z)@i(}Mb1t-VPkZ&&y4d*?rQx{?&2!iN&qY~V_xHS8-A?*42-3Q3~aci4GcM%vm2A} zchC53F-&M+er83*&`Vyuwn5%3#MqG?yb6(V*Nmp|%ABAf<162I5T6YR@im|MvKK*K zRFxvNGn*WfbK|ON1#uE?(g+yLo!L%m^6kfdBRiEp9we$Ol`7+qnTK&KV--vH zPJrH=G_J8o>oI};u;#ze`N^Z!AHqc|nu@H#S3BP*)+^Txne?E?d>Mb_ShNYy$s*1$ zlgKmZ2V`P2oE6wKK@|=UsaHwwlshlci`-vtkA#2PC_GzXE!ja=MS=eW^43NDw>wyjG6XKoBD_1DP(vn z?jALg={k4%?VYE(!g@H%KdtjS7~&x$r3j7?Rv+$Ms!*F_@;~!!0XOL2{=+NcV{of9 z_&zh^fBX8P3x1Qoe!Nq^jznF|WSdV~l#2oUlgqot^S-Bqp4k1P+h%}~!>9_6wg4XN zRxT^$-BZ5d+<{3JSXbgH`!ADcCz9VB3;jo`Db0RGmd^WbFs6=}P`h>~y>oiKkT=VJ zNTM`Ap~(z9P2;4n`q#rcsFv*l#d-r`au{~V@%Q>35d_BDgxvfrb9C^Ou(bQQrXP5^ zX5~NMuTV?lozm1-FRe|HqP)Y=qHDTONBdrLA*1o9LDxZ{QLWd^eJh88 zjSotVyUoo$rtyR7!B){}_CAOh=J@DOv=D+$3)7MBr(jAl0pNpcgJxkcXgxR}oO zkk(QN?fF#d!%1AYD=EJ)5GAMyiQF3%i0Rew_b^EBeJy+jt`U%>;KguUfO7DoR~GotWc|#!o>J77}Ez`YNGC(=ite@2RzlX?Gefk*`z8usck-k;T!@k5|F{3|GQ(wSss_0WowDlIL3(Qm90!MI zJkA2qWF&qTKyJS!#3X3+Y#nm!fkF6KQ2o9=0~5nn?|tRF+}7LTPS-u(CzsXAG$$Gv zmH&dKaCYXO6~!{kfCRIFPwkqtj6@<33#kov8&y-11MV&AK1G0s~+BZ8q%XquZ5}TZcFc zYF&G^`8V5UDI*=Jm<}}0Hdv4RIPU-gQ&-N~k9`8OJR}hn8hMdM6<6)y-z1WYNjRdD zXlm)6j2{6Dv+HIe5P(~rPQmZm26TpD-1|9D#YI6HYGB}ph){c|V0yPzT&T!>%|o1E zY{A$ywiOa9PQyE2%9(-XYH(m#PhG!CAz4&iZDfL|je+|zrUOb^)NmLXMeJvrXqG#_ zogy{w{*un<&`zOp@*D{inksyM_*ZA?KY{?;jQECe@m=a?{KB5`0>aC{tM!0h(c9dVL`2$JUHTn`U0tVniu;YTh$YP1 z(!i0qh0(vGTKAY-+La4|!nh}mof!K2C1R|TjM`O3jl#ZQ@)bDJZGE`c^wcY?rW+bs z<~C)=#KLlK+Ib@+MjZ!C`RBNqi!>zOf-OGA|9oL@KjUA$`X&Hs*(9MoZ(HUNyj!7g zZ#)X@eppslV0>z&UTb$F0#pY?M9N4Y4Q#@{{|W+RRO*A-}%9P zNe?qEzkkbVdn?-$9An$Y85DtzmTu2O!{T*$KcrEBZEZ8cI>}fUSaO5g$?4$aezx%0 zxf7t_et-=EgLmenz33M|B^A5rjtTB>uSo}r1TUWX2cub_EzZvkF%ygSqNdW9^f8Yf zLIeZZdV$FHSiI&lEOka#_&r#GHvl%s0&ZMgisBf*iKj<%bnbrJgpCDbn*;@hx8^vM zQMxN(UP0O=EfPTCYO0+7^sDA7rj8c0`^LM83cGmZe#*>e3+0%ene+Y`-7r=;KN-r} zjt)idjlwGUzk{_AVL+nt)<~Cm4D>!@@U?*?gqj*jNzY5b4)jXj2wInaLO~A2lR=Di zHSrjQyQT|UoVyC9_Ee;Eb8TTuqd&D>4pz0^ySK=>4I$r7maxp^Sn>uRNa9;+(`Dad zFe&1WTRv97F(%4D=vHV(2c_NvPKWXeWF&uOf)sx7+|n1=SgNmgUX1dde7-kIVahS! zEKi(5QT>rkn1S}r_{qX`6OKkMPENegmt9hOQ(3^a!Ue!}Ko;-q!I+)VHSIF93$jSI zc7ewbBue)wE8yaZuJjWxK^d5e+$rkk>q7NpDTKithQX@tJ7%6k^}33no!F*Nt_mwa zuDJEAHG~o}IGafAo_dX(>l=WLFhVgjEf|ve-ruJ@CqAn}VBd9A8d=R!o-nU)r}jQ0 zTvrKG~rC3#GKTr~hDOZUz-f^m0NCL7G%M=Ta zWm6bOiM4xQlbi*3Hx;EH7Q`&CNIE;07iU5V=^fL;_)>>Z9uCUzU$h10DMWeaX)Q)Jc3QeOS5q&L|{DA3yYn$4!ZRcEC4sZ^|`oy@1wf4 z;FOJlxYS~GP3u!pIXTDpu?Z2I&8LcWVq}y010KWT{p4|5By2s`k6g2fkzkG9TO?we z&zThWi2XQE0dq2P z+W8OX^kem_<%>g4Er`iZ$Jy`iK3Lw1BU;`0LJ+XX2F%6f>~cPbAI?us9!bgy?6owc zEM78oTA8mEoG**#b@WZiShU{dAlGY`q!|ZXByJChR>xek5RJ*_%^@aEacwcgYtp`{!nqYZd_jpo3qvk*$E^+ zr)CmQ6l3b`IG$J?c4ylF@=Z+ksrXwlGIKQ^_L9Yg_7`U2CAT6Sj33g$W=o@!JB&bk zrp!XE=OK~Q2T4tWMcxx0u$`rQrgIO27e+fcmC{plV+RXcf`BSf78j`j#Xn(UoC02d zl!k;56q1hmG`ih2C2Z*IMS^1Qt>K&+-yPy6KL4DPSw-i~N1?1-)aSL-aOQs{;FN6D z<<=1}$!7~{UbD4zMvRd| zmFFfy{&et6)9U~h7V^h8i+uKPUY_1-(h_g-(rP@Ozb#pNlo;ix|4G?#@<~ShKcBNH zoxZW$;D^PQf-)~TcF7(`Fj9tm57%uXET;s?*Re}Xd%ayp{atI*X;%8sk}Kra@{;XD zSL5OFt&rWQXqL(2>GzHt6g18h!~>FguX4 zA8(D9D#Qm>0{yN)=@N=E`p@+hUBkX9zq8G(EI!r&+_)(_D474Gg68a6-+iSPuF(7Y z-~|S8!IE@^Y6SPgtxgj*}brj8==HN$FQ`iL+BQhCaDymdePd z|AG$0cDSB(+}f`Ru-YB&32Dz&GB4w7aM%QnLr+c)>ULE`o7!GAnp)hbJ2Z?~!6b9N zNs(HgAKGpos;=$deIn)r9HewwT&+YeRk}i(Dx=}$9xLziKx*@4ML~I1K)Vkty%urx zN0$T_9n5sygN21J_2EyhroWoxr9}m$ZNUL)?92hu9w!dBZ<3T+iH3Xp@P^4~PLiAw zNqajeHC|5LCmcDx>R#S8BlH0YvLH`Gjy8pAg}}%tV}sh~vO=m%<#$MvE$ki@JhM<( z2(&sT8m#Pfcwr&04tn^~mmuz8&p6(EzuBu-4*RDCW4@vnf)Ql`RlE<_bpt}LGP{62 zFrA-gb=COFacs=B4H)*%l6Bt7XsgI2SN=si+@D3 z(B0)SK`MVQ{NZX7Up*-^;^yPk9ISn`%cBbdsGKH1ht~=SATo$Y_i!8=aUC~asMN{w z)cqux&JpvKP|4y}(&~CkbwXDDkJXl6^mKdwZ)PmGA>WL@pSf@52~{#}a=Vx<54!$n z{(XO&LXU~&<^xe}NV%oMcCF`<(rb;&!?i|mh0?o5wF)t2rk)KSSIMyZK zz~42mJF4CPS&Pqd`n6vCWaTvb$$(4GZdlmvSn=S$BR;~`Hh;G&kPH2n-oFQ=|2{S7 zJd}F^2fUwI?JoV-eHikdB4$g2rG_~LJkFJORbO!Z$C(-39ALXtqL}jp*L}C+`M~08 ztKtYM2(8?b?fu&|Cp`eDKL@!qTa@0{Z{6uN(l;+(u9jv43Y)W>ey~|IJEzG3Fb@{oe4e%Dkk~@w+$?%?`b~!{Xf_rNS`AhjPm>!IAibHX`PcDE`7iPdY z0ttIUvlruO|hUF`?tIFXG?@3)_Iok=TbBH=N zm75vhQIvwN=CaQ#f;TPI&lLWaIw?%7bVRJW77q}SOpWp-a=k#V*n}-b4mREA$^|dA zFjf7wk#n*bS7*U*#%Gwf-xks%ck(BHt_I(@{Nks~>4L1jbU$OEeEs3po&4a*%mTnp z@|0zs*mZD*72S(se&qLHnRJo~k1}d{S5MLG=j~H+Fnf;5>^xqxUP)M#1LmR27~nlU z0n`&+)g>xy*J*jc)4R8x&1;#f+g%}3uM8cXBv4SbXFC#^v+65xc*V1zndNJFO?xE7 zqg?HSe4w~i5$~>5^9@<*6uDm9N1mZvU-kp8gEDm);hY#%ALRSpGyxcL;&zFqx{asD^2YdMhtfn#BoeOjfD1UOaJH**?l=NP7{| z{>ws{W#X@3nAE<&6Qqy+B}; zV-kAEuw8li)FXroneo2HHu<2nnrKq%e9^hz}%!=h6TjMCY*m7a%TW$$$f#4x)8tf zXCk+%?U`JlOM%fVRh@mOF6`;`6GPdg?a6?179G}9Z&B4M%K5G*!t-tDvg^xORb}|O z@o7G)Zllw?r064T>9dKE48Y!v;b6WdO&V(PE+2OOZP$16t9<@akq+#8!&2Mmfd_>} zysNN6Q&6tU=g6faqw`-En$=KdRh?4(j)7<6Dq$Nst9{u+bOKO}+FTrP!8do-_&#cQ zV?O7*ccHj+0!pLVf{p4r>(vU^(ArD=Q|`p9A3L7Q-9@*+%dxY2l9k^LI^NDi6`qGr zo_5UACR9>xX4fHC0KE|k+cg7#qDYN^jBev0W`DDIVN_T3Gk1C&(YaVikj&M0%-tYjZ(A<34`uK4J4Ve4SSRfXM0Ub!M+=~+z5iix zpvUzk-_vnosHTn`DsOpC9o+m8?=Q?+mRB$xa`620@Yo25UT1esfcZVUwCL`Foc#dhRM2W@l& zr0|`&9S$$Jx}H2z8Y9DzHyZtMTj(Yl_OSC)-~LMmIsfo5fR?2+f4=7Q7I$;*U}?I0 zTsArq%^~!_+ekR(~5WoAJp;;){5ePH`KH ztprMssrDMaX64jou0(Q+*hC7QO3z7D`;w~h+k@tJrXs|K)lOM%^P9Qg6Yg+^B94pK z?X(Rgfe{Ngx*j=2;MDmw99iYy@}(Sub_m+=-Wsj++R_jINM<6?E#4Bipnr`(Awc5{ofWAPUUsqfcFw*NTY*#JBgl;-6x z9)D9_NeCf?6r~76=V74+q7aC1ANlMy=L|w^<63&ZN#{2APJ;m5zTm!XyK({ht6M9& zW5Rx1p~bmupFB(L!qTsp6V@rjd@s1-LMdG5GnXo#l5lgD0sx7W=8vo|+7+U|9cEFh z-6bUeQjH~0jap`=uxHE7mwy=RaJ{ZR5EOCS)5HTm9-Be0T}PCS;3&X^KnR-IG1nDe zdtRs;_iH4{$TExQ^>aeH7wqo_ek*;t%~^aws%FGZ$vCO2TCf4gGkhr^aS%xh>fh*! zIhJ;{^&1@9$k&FxX6-SnI5+Ze2Uok|zn_cT*I8NmQ8RQtt1i9C(OQJ99CAvkQ|RP6 zNqo2QZ0{P4xzfm*Tz>O@F5)k51*H(hd4Gy{l^IOd8)1Gs!)w-x>8N!Z0BQ7hkcEED zV^tqR^ixXR{LfUua#C?06fZ&X%$#JhB28EV>U?XyS zj8yt@nyPAd-9{1{E)sdF2j;A_qA`JxkC=`|0f7%-UHwo|%G@2dK6}=Rx+Q^V_4l?- zopvL6=O3b3Ag0{{;oiUFGAjQDEjgh)qqI2(=~7n*QYA;k{OzQTsMeJ?_skT+rYNym zh9(B)15D0)_C+L}pPEX9hHHjGILKC`WD~kjIi<~z0p${L0rCnnM#;OUFZ6rT$oYlSHmncu+FLmJv&D@IUs6;ofb1l+R07n5ji}WNW>I8i!upGd$p1 zU2R0Oj>xp{{Y_3lVA`M000=`3Y|PM0VDg1cZ~q< zewS(O+%n-)-yC^t3v(u!KpWi4h&)7$yac_ac#oYes zw0|2N;oyC7LOz-+<$XM7UoQVp=5FO?GR>IXzadDKL%cJY@3?;F80Tll`Wp~I?!j87 zq8Q~7-%W?wsnQ4Z723V_`T|n!C0iXD@I4z?@jN?|D~dx0q5-z*N$JYAOO`7xv}4FE zduy=c^F0O&{Q}*L0PLm3AR-?$x9BYflGycBspFwvX!8=ICMbcQ|UOJBQ-Lf@sWZ z3(xo)zTb3pw!BP=mlhhb&ZsC$OwQ1y8ym_Y{6hE6$2WyHLZ;gl-xWj~lX8cV)3*;H zO5)Nv-X$)!v9qY6q}^}4Ik)Vo@^^W0xwfX@@*7zHf*dFKW0F@I&9#-WZol3Xb$1rx zUW(pBYl)!o_F39h9j`3j^nONYlkbj?`Fu2gjb@Vb*7aQ26jP!ag)1mky*G1X8k5iu z%LS|!5G5)ToH}}pUZ|(*DoTXmr#UN#oL}CJ4^0-BjWxb29aBtF?5=ygEw6CW7+Nw}=}tZ<6Z5ET_l9lC;oeYG*G+}xmU>6#BU-3*rbKYJG&fa_yGSr>qKn}l56l_AFf?w zD-r~oQsN!YYAATDq?RzS`>Qf?<>>t!-Mj|N+wiKeXMl}inxx2L90>k+67kZu{LjY9 z1;DBp-Ia@xiy@O?Yxu!$zH`FIq4Qb1{B|DbORu{$DF6Qr{`7OLSC2x?iyjB*1cR&H z#p!`&noQtAhpY3)04uEj*LzX#Y+g#HffIW~2V?&>IytTv{CDpwt6IL5RFD*iQy9L^ zX+wv1HTgFf>G->3P42uXe-Q+rKQJ*|GqR;&9WKRKmyP1mKko4=)PJLG7Xll-G9z{y zt828%p2;-B60I$&p3zx$Bx!9I`RA;C7oWv{nOUPO-zy$ui}U|?NWmZ?=WGtNC>Fu2 zM$fHy{WHBD;eS@&E)<=!q66|72@~!-?|jR1c|7bG+ZaI)S;M^?mUh_?7RuMN#)RAX z79c6JmiSu_r0Ud7Ms|JJYbJF$x?XAY{PRA_L>_+`Df5QEfbOTm7LT{9JJ0=C{BE`& zw+E&g3@B2hwx2(sn)R~BIe@;Kib=$FIkG2^hD9JUg4jfrnDMiXYb2%x`lE@&SZlrg+!iBTD|!UfBH8b8x?B1bL#F<4pR(O~;*14@fGVp3X+JvQXo zZrFTsmQZ>=LAzHJEcGt6&NcE|-jat7ZuUUu)R_s}n0O*odUn@L3zY($nAQ+$8O*;B z_50&?cz+uRFwDJ5ra=YPy#5fcel60<@v}5v5M8$sz9ZLx`@+0AUwN>{LQaNVPSWM6 z!?+ZGMvY_4)CC%Mv^AwNHR_&(F{RGwlBdef?(%<*4LadK`vM(98lvBdjc$>9l1F?Q ztAnxBYaO4m_?{`PF+ zIa+DDDU}7fHv#a%;8c^2Llcznbn#35&yBGN3$av7i{OzQu}$!2A-4isHF5vzG7g6! zryq}=S{N<_KHa-w(~56iF;`xi_CJbm7$&9L`#37~s1s%Q9;;sOgBNyKc*T-_Cy+~A z=HH&d-Xb8NL;{LWQNDOjHh_e7sP&0FL@kM=H04tLAU78_E+3<0qj00q#rox-QThTmAs!RyQC_z)7qIBp^U8O#8{5IzT{hZ#i{=Z zB(DYvW6z4P4`b@L$tK=+aOQtWX}ZBl8brk!!`k}|sl)xQZ$t7q>YpFMM8i(8GEJ=?nV43P-h2&5ArbG{D%>cD7;g_Ke#{4Y9>GzY z=&ov1^6qZk7xyb_PoQn+f%1!jC6{UYLS)oo9QyfF*F5r0VLGM)-dNjIp7x$%B}r3=qe z6EowTGv&I#IUylr$=6OMI4Sj}B?-Ine^>y!EjQyL=E0qu8}$3p5eR(re8MNc)&29J zWn4Ep%&i>6^Ft1)@{ZWIgH5@2X@OXl>;tZJfg<(N^X$9MPqBBaJc0nRq&CH@>0^UF z=Ur4MURgRA*DgOSAuY*eBG#mJ{1=RxSZVY-#p(29G}-``7zs|-eN_*Ypp!M&s@Fjw zpYIw|@pBFFt@|au^iZEi?_Gn<7gm1oD(+2y{~xl>JRHhD{P!V*P>E5t`j%uJlCe{X z7NN+N-6Y9QGh-QBwuDjktd)@6*!P{WlWm3>`@Riho5A2b{mymHb*}69(;pts<1?Pm z=e}R}>n-ABiUgud?~PggV;4}A9XMSFyeRBqkgC@UYs=+JGfQ-)^T~}-Q?03!p>*Sf zKH?jm+W%rCT82kiXi^9D*%{T=);}%ohDT}de%zej|6A@b4mWcpUbgXPnIqaA&sOEhSpOUkm zUa1iHLH-O3n=<~Y2s>d*>V72=kKb~%kwQJMk|)izqW5@Dt;YmS%^i5px4m|t1?EXTrXuZY|gIsZG+H06yz`x?H|p~$Ru4B7k6JWJ!I+4##sJEve;cKXC7 zmJmltyYzDW%sgWB<al73M9TY}JVzXDtj$QhlU2vQ0Jq3=VQn$R?RzhtvxeNtIdL zuNSIVK4|Fp6Ok%{H7a}Z|A*dcx64Lq9YZYvTI!I#YEMfL?{2MooXG|6uerpdcD*)w zQ^Hq!N-%s(^G#fmoQ=bBwf_+?3e={~12{MfFp}mK?>#L`2WqaGCwS1H4m1Uk818?? z2gu?PL~&hiCQI{Q`9T}~=&CgbNQQYe_<_=kHkYWg+3(-4MLm_jM|+6@3FJ`K zl>H$YC-=kO=P%{AQ-Dts{T(V0;F1gkxUlfubMlY;M5(TN>tOioFWYNmE3bdw0_PQ; zT?~mGxe!kxxvrCIfH>CH*46{Lq*N^`o5MoMC7h$@-~$b4HCm09C!>(DCDWL7D07=HZ4DpjGPpu$tX;%YayUiI7+}*h3tx`7pPr=kP$O z!g2^0v*Y3dy|rra^$oC?ab8mA)%lz-%L0;kpFa9Qjpoz;m`^pH0F$~i+;UcKZbNgo z?pn!mJQUA}l;pY`cA4Vxytpm#Is@`N%eHHaf4XG$rQj}l%1^YFfc7K3r3XoA&o(dt z3#EL?HCClz?a&tAgB@efxoqdIBAs4fShXM~s(zihdVfOe9@HB8*WvaI9gZ#i#$7dM z03RUz6#e+=Cd69s?Wdm>Gh3t_SDxjkUw<+~*?pJaggi%G=Q*I2y$ad(k20ENpsT)f zRb}iF=8_vTGO&u#_cLv+BT)6(?2s5Fu^(RwNAG}&UBA(!-)j7^rFfc05$OW!jcs+V zHUYC7O)YQc-ZJA~J(^%cQt0h@6#V|eaV#4zRRY+*vRq#O>`4)N>r10Dmx_7WxnG8u z?OkFy4sXv~z+GGgErIienTyqCTiLgzsch6PmRV|l-Gy|h)@F#;a(^m3JIP1eF?+$Y zRo^G!6!VUfCXAVHH$yz>BLOM|1-K@QITC^8y)!Z_`q=~sOCNPs!`$Q1s2n`A}xWF z{T{4#cjL)UN}ZSQGvZXK7QfHdzFSYZNx?N7H9&yAV)y(PkB1I_ZQ-VeBg`K?dy>K1 zmsfyX;4;OiN|uk9uVxm~Ol`OSIWtnM?BA_qYHMwcgq@#VLtOL%HB=5IVW%NcPYLOHO;!zyg*7>G*%~S3TJHF|EW-ew- z^mfR(<>r>R#(xzifn?J1c&cE#3E^ET#b~LPV1*qP>ooi4Y3496{#)(8dO&<*jTJ4s z`tlbgpc<4CPOu+v|C&?tGvxU)ff1P!Rb1hDC_0ekbbnLGavpi!yLQpv$)iEalFr@Z zebv>9iXt9J$0ec8O*tLIgX)a)@Y0!-P+qkSrwUF!Iu#Ous$QR8YZd(j+tfjS4!E}9 zLJ}>$FTC}8=^K^dgO3#~+cYs%`HBJEdY?64n1G72KWCY4{P*b#V|YP0F|-MbtJ!QF zJCgAmhmeFr8zkhotANq!o`1eUwyuET(u3-vD4@$b<+Av$2bHpRy6wVK5dd?9AHONW z_s@?~CWuPrpd!QR(1OJ>;3Bvr2%9t)jNuputFVvGd zxm}(QNEugLE9^o|-0K_)_6^YZ(QJ}J$50ASH$1XY*Kuo!eL@EhOWz!Vc~6wiIa1^g z^YYo0g)H4O0^pt2B;C$?xQPbygG&i8@aeDlrdxOHJK?1TE8OU?9)*Dlr^ITikCH|V z-qO?H>JO=vrnL%S=HQ$}0{u>#rQ-U05^HVn_23;ooQ$j06)Bj>dXIGSyeCwmAkwfJ zFbXqsZqNnF)v&q_{q1nKxJ>nUj3Zkp{g>{y=085t*XiR8>GC^niX6}CpUTnXcqXe9 z$x0PbhrTzfG2==|1kBX{wkHNTRI|3vH$}oWg32ZVD@j?VL&0)}6s%V4&SBVQUYB9! zojW$bB#Bq%1n(~32>}&91%FA#=v;S&HQp_Zib~|hIv)7AsjpnusSLLaYz6NRXWV*mNjNiw{{e9wH0)#p<71wF=YPVxAmMF_%#G0wtB-SzAv3}*c8YVo8 z+~R@}?n#;lJl6lawQo-Gr2eye)l1e!9!nzkoQyo7D!c;9>`?jlg+3dlg*GwrOV_5` z+}D3mVut|@XvFc#n+ii7ZY8GuIdR3pO*Q-`qII}V9Z(oL)$W*zCKMjO;dk7;rJQB6 zR$->BPcl6Q4(XgAw}0~p`4XVee@%oPN?7}KrJ7Ws;c$(^e8@GL%(0HbgrRpxe`SBl zid#UJ)+BWwF?;l&4};QNtP6rT8VGv!KH9L`_p4(l2Q7< z8<5y$LtLSRQ9)NtU-8qJW%ZDzJM47-DU)R|u zG1;6|dZB>T@K~s*hXVlT!la8)tcVO(ke;T(Gs3~9fl7e&Gl{!M=ttJ1SzBuLO~iF$MlJc6}(q{9iW z^ZM|d|ExK0XRp~ESlC7?K4h7l0L4N;UeeAUUg3LiyGfV*{Nu(e|IJCi;Z?5_iTpr* z@VIm4IuK7GVPGym(C@1xDElqDn}v*USEFH3w?C zd%2UIs1NU(m5Q&AImz_i#C?L(e4sA4YPS(q{&EKaGap#1$O-O`5plOyL@|Rxx{+o> z`Hwc}yanIVPAL7t`sz4YJ3LOimt4KYCx2srS3%SnU#-nw?N_u>+oe?A7TW{UGrW6_ zR;nE}{NNqD3GhX5Zh;n>KMRGT4jD&A118ZmqUrwf_cJRQ4O-uj(-s$7FFQg(UUXdc z&pa3<-_U+e!9fqzoyW{o%Wq73*%*vDC^@Gc*O;r=1KPZG7(V|)AE+muLAHd^y5s!| zhj}jk#&61H*0YLB(;LtUtjLT;C12I585jTOlt$~r;;|T^vR6{FZVH~O6=N3Ve8a?C zhC6ATALF*PJD~&hH-(n5-Hx@`u4Vo@ba{>*vd3|^WL02+19JnPCPDe4>U-Gbv}R-Sa|X@r6@Q?;}>y5$MMhBRKww|C`M~u9iPt$E`VS(tlPN3$na0X@e?#fgc%vG2m|JP$N>G& zK%rVym%V%HL@3ix`MBOOmPNcrT{jfESPGcH3`3?qwAEFGg*iSFSwr-`;jd9L;Ui%~ zV>CuQuC>`jxb6r%R?UBu3`LSJVRSaG08RHP(m#jQFSNoHV?N7Unhs+!vkGm{S@_-D zhYz;@8`+Mz^FJKIerf9tB`T_OuciGgy`Jn}vGIpkmv>y>FaPY(-Lgy9&Og0y{tEXs zsARlqv;!+Hjbsa6-u4{l_wKOjNHb1zmd{@-P8nV<beMOu7%$W9w(>m#cQi3SjGoyu#v? z`sw@P#f&9wucV@ki&@SV+BzOPQKyDY5&0wKl``D3zxST-8LY`vZGJWBdU9%)smQ4^ zuly2manJPyTW_LHjqOI&B2>3q(UMDa-^mbXW4@84FRQlp`bDsw9TK+~_g~EQb^vdm zg$fZ={pKXdhUryUK6z4*_lclwt*rU}Rm4xfkZg(^^TLiv$ z_)b`H^f^xQj+K+IA)fat zhu6!ak)jmQ&px53YQ&UFXzi&UIk*Vy8FMy?l{JNZJF{V0Mjf$QjLv@)jNcAdnmKYC zx3Ds-xMr(g?GRqvnmEE0OY=iAisNH=#lwLa@TQ>C%Lh4MFJ<*bM(o9c-q;ved!Ljz z>;+o>HAFo;5$r}S7ARvoi=%#xXunE6CpX9!%@@Lx%i5)$_)zjg zj8e4^Ye3R-;V39`+D}%~Xg9rh?G=Gt+ox>2Ryn4IPeyhPhtO{-4khB^5^#6hDZ39c zygu`oWZre$y}BfI`I7I=hz9c^frNy4n!7C`>thhhEb|`h^|-n|eXg_|7U_sqZVv1Z z>yw~DPdjRhaL!f8qgrsF?0;%jx`Av*wo8A*N4=_8>X^L-1Wy!Pec{ny73~6nQy`0$P znBJY)u#j7P(Rm>={EK+PrN6uhZVa`1--^{*CCww0O?)xYjW%Xe)CiLsB)VPV^Uqt#lEP8#5N<)=kt|n{Cu}dwfl+~nT96!^ z+iKC^<;S6C+$IJ(ln;6;zc*mw*(F6yWj;Sm%M$7Yv(1cNRi+e}#R-i2wVg~xB5K$P zJAd#Ce(U?N1eLD(qhJulB8K~1{{3Ahp;ck9Cl@{GwG(X4Z9DvIlv&V9q>>;g9cv%a z@JgZk8=VDgF|*-sLAhS%doeCGcjh$51czddaqcyiFAqQ5$o1SUf8%gV`lZNT0pIlf zw(LQ)V>xB)vQ*a50!OPlZUKfb=$P&Kn(&trvVBq`IDO;8@$cMK7MOjWhB4MoLyoY zs4jQ6pUlNce+J^LcO_oWe4398=N z?m|)3YEN`(WbzEXr+L{<<>4ENjmZcRd4%=5lZk!e)#=YEl3lWtWJ-W{?rjIQKciDm za*Ah}A*84Im$oR%`nnsJ!*4reE@+zu7N{$8LQH2-s!f<>du*Xr2VcVszp~f;1xtds@!muSv`E0yX{`H}lD&(BkwRgD|HG`cpdR zvhWGpmWNr2vdhc^?gw8~x&5ilfsCy*$scH1$Hl5<<}TfV10k0;cWyPuF$S z6X|VhdnBfow0TqAqxh9LA$}Mt)77a^HdZ9c;=)0LN~jPaOm*qLem;)WdYQN&4B~dU zkZkmhceJYr_nU9uUo?BIdM$_6|{ zx?CjDrhBw);Wm<+I?)neK$Pk+u=O|Aj%K>{iN$qcBpbp7_hWubzOkX0OENxooduWj z0v)5U2#g$`1f4O^T1&?bkZ3$oq&tl*jQsTOEWN~hU6SThLF6@!B_)3JA>mRInuSu8 z;@+KD6OD>ydXFBZIOB?Mt{z<}3NNjNv-)z01kZne+{qCO2riz&OFE=1KMl~-00oLl z&)s*GuJavlS->o}0#K@nL9vWi>Wq2iUj+^r>COiBjsC|df*V;cX7pWnT;4K?woUim z>N}oX7LX^V`Sf!kCN;q>392U>ml)ty^v;+AzS*D}_w2@)Wj#LM6$cWdgZ2+Gu{KQ+ z(^u!x<@Z}*_zEY@FYJ)r3#R}Y5^WWzIZ@%5jBoJq?$kS?N6w(fM{WP z;*IQ<5Dto%H>7vUvfIWh0O}Bh_>rgqz^bG_RduSFy(e z0olt~aO3yc#LeWUB`1(Qn4s78W%0ak;XaGQ7?<@|*bPBMar1&w!gZcA|ABCKndQ|H zLKthmXUdr#l!blc?K)_7qw)EtU0!d{7>JtY<`af(H@s7XO%Wdq3lo8&W}83Xe7W40>I z{_&a9UC}gMr!|jc12-wGD@Cj7zl_Rsark>Ml0P8_@l48a6k6+8EFQ8amL{Ip2oVUv zJ*{*Yv?4ipKF-N%wtm)DVf?}*-T`ll6O5NfjbOQkyGs<*caEYDpUrm^U)niyX!t z2}R@*=Gu*1>!16XCXYt-RH(3JY~&fWiZR%XI#!Q38()eac`eXVICJ~(nP&tDE3_K5 z9)+GelACRU*rqGaCt;`SFC93+nTz)kt~Nx!3of8hQ#_7j|@4JfnBd9|=R@ayatsiiQO2 zI1o#jG}+hnetMp-y~O4|#liDOf%?pcK5{r-c!`dNV9NWX`}h)_8s^XTT%B;kUQ%b} zw}MtWHQ=qhASwgz45eI4r;`~m3{cLo5HTn$6f<7b>HtBF1;$mZvO1f~;n%GUc!#A_ zN8@B$3NGiia(^hAc}*ufq!cvot*=w=YvppG)bdBguL*^i1~+Fx%RK-Q;Bpsd$>*PCIB$-InssD}rTNwdfrj)i39_f6s zM*iH?-V|qNUy$>4w~U|SKaVVlKOPtXU}M*@$}bP6RKVMCf4hU5=Mg)qbZ7lf=n%zd=5-bG!MvpVnLw$YKSOZ?@QYA+a{kbGidv z7w;|)QyM!Isas6;a>ux;KaW&=!1ut`H{9=L6j(Z6U61#gUrV*=A6)-hZ_sIHw>xu# zobuOOzHY!{<`G-dghCCYml*u}HL}o8URAn<61Df6!u3IGn-@vTN=GXVt;8)=-vsaJ z1ObxjRz=H6cFzE;h@^VC!ijY{yv<}_Sj$XF$80@k9*uf3CU*~{4LbnBj~6$etx`6- zopaVBBUER161|CBZDghs^bKUTMtmb9*Nr;~gbt@JGxc{$c59A>%~}249a}hlg5vV& zK?r`5(Rp&cqA(SPZP+#?mxJZNR3y$)FVr*;YD@E}h$cLzlh)(LI+Kzu73}Zq_MLnD z>*Brp?WtatYXSZIM!6Wfl1Q_<+3TL+KT7R0YBjYWg5nNg**jdhk&oSH8Rmhx%|8Xk z%&;y>pH#J*OAgf@VI(j#~!PT^3*F(?USqCZ(k0~B_zrGOnO*r zu-O{zxG|S^+HCe4{ySg0K#jv0Qg*Ad0*~egi&IIulV1Ci?s_<2# zKF*6G%r@+8UnYOrt*CMKIaDVagB;j%fi;)PI9$H`CHdn;%#GD~wqAEAYxCd6yI*nZW0ca)XMDMR0>0iG zL3xFzG(~Lw-f_`uaNNvXvX_Mk)E)+Gb66piAj=Yph>YU_LyL{HqlJP1>#xZTClm0R z{uhT4hF^2$&w5jSEM;h~Zp9k)J^5;I4_lKKzj$>sV#id8Z!7s2+02%F2R{HkTw8IaeQl3V-TvG?;|~-#9G> zR;4N#vBt0Tk>I=rsdD!1iV2m4MXDUtd}gS6oFlf%@uY)w@=9vI@TSdDfb>2R-J9j= z$ttv7qj+u$hS5ggw`;`n3hUMuLaE(xfj*0k)SkvF0ZM>?i@;&$6T)JG1^l2{Cn97#M~ z?2mjGaB+PQ&(!*w0|>5jY!;~4v=0ah>}F_x!PicJP!LIBQZS!;o_*9}$7s8I%8T@d z=nO<M~xVHK^&LWrT!_?{32I?X> zG%|HzP14#MZ941~!GTSl8|ezbXwKUO4rCGkoEb|3_wB3#OHWqx?XEuBY92 z!S$f=RfM-s7kGMH4PqR4j$;iA-Yo9XX0uIKSa@%iqii=~o$B`u$k~2J*~GrH)xHnR z@>PVzops~f0DnImwW&3xmS6>6EdG0gB5#x=EEgEo?>nj&`_Q8>7|Fa%G>$>d-KXzMln$jmf408YSEKW6n3LZm@W#t zGqhNP)3^Iu;`>c85@hXIL#N+w#wHI&i%7*=}YOQsb(9&QS0mPxsRbX&y5PjZ$7h@he|v zXmbA;bH&S~!hxxuP-+`>oVInsPp+Rnt|xv;Fr8)NQ&<21^kv|P3sNz1UFf__-fm^D zJvlz8ZGFPVxbmzn2^R{6ce?71%3&tL;8O#$SGt<^qzu=Rc>mzp$@y72 zc9l!W0Y;O=fy>002O=FI%)aA|ett$CB*iKHZ>8>+-=MMdYZ-S^^8Sry&kF$rhids`r$JY_~_r zD;ZHCp7a*JuPn}EzE>Q5DgVWHal0v9@&58yzM0-!)J$&k%!u1|N$;(Jy^;!o-`R;1 z;9uP3p)J0M`hi&-;)7 ziQ1+|Q-b`?hx+N~d`VuIQ-M@Smr6Hd+oB^oy!p+E^NuO?iO8-*~DWSoadFq`Y zA$#FX*I2^lf$DO(6EK$`2v^cxE$a$?Rqk8foD*%MQr%=ZDL56@8n*7m`BI&yt_SwF z$m(D1rqqJc0up&#D`RzT=lt4|SUi~jOv&i{jBX{`J_I5{ zuM*CZthk*MrFrzVZB1XANDO7G;n`AIZP>;MSGrH;dvEH=)?{xfED`JHxe)j;y;DO$ z1{N^R!CY6UP?A`)E~IJiU0QT5&Pjl;R#mHn@v&Pp?q;Ezk?$YBd%Imu8)?$bm3UZ4XLH-r7}) zrAx1S#7-Bos?>KD;c%d3S!N8vPtc{q?qiPVAyS?6kaZUH_m<^|%e;;o#Kx`D8TSoz z7*q1k3e@L?W(6e#j!zkWKOg)Gu_Z#G1XpX-1BegOt>q4v-eJl^j`cdx4pVEP{Uh2t zn7hkt61;kDp{h9E;|(TVXkfb6$){T?m;pcw@MK#abkAX9TYvhzMx8{zxzGNnkE^B+ zEq%#`$GFEV^_ii?*GF6b1OeXjJku*vfbXI<{U|40guP7@Tz`LthDd8RX_oL|dAx6*dq>xURy>e;st>0TS!k{mKg3G2P)@PbITfiui|V!1tALC89K;pnw=vlS;npv4p-;|}K<-4? zDnVDIf<;mxjx#YvMn>G5`p!HlBF4qvRg1@;;uYcgJC67!Teg3dCSJWh;oUzvvlg!H zoHxxk=5IPyzwulu-K=@PFBI22P!2q5^;;NWd(eI?=B{t!F|oE)BhWUlSw|+@61~(S zK>Qs`XvTnh#g`*p5ui-; z$W|)x9-3ypJ&v0pdB%NzI|*CY;X_9FRUUVxQ@dP(Tbe&f7(I^_&)ST6f#q$Oo8*M1 z@Z;+49JUG!+?+gHN%208BvaBaD9-dx*NmBwqge=?1H9D7+_W9y%~&NyStf6P!fDZB z-=~;+0b17Po|)P+EQ;lR3m~4D910$%)5n8yNpRMkUv9f|>NXpg+_N=DcxUFop(&3A zy9y0_3o4R(oz}wTEAL8ujFZf23YVJcH~DQ9`Tbd~Z*yv-X8eA8BcG_MXcT5F8)ac| zm|oM{Y(|QvOO|4ed?HP&m;WXxH zH=QsZcPw5Z$0HC-T=@MO*HWsY6U?f!-F%BnCOc<4m{o!TeAhAnJSAVX+`*8Jt%7}X zt7)HTGID^V^TA>J zd83dy-5xFuH9SJ#^rkTI2$L2$+P$1M-ll&U4NSf0pAapc7iqeAG1g>zz-z3uh4$C- zV~_QSO1BO{tGfb1CRkf`ZkM3UI_)C6TIwfclb9)##He~AQ^j#mjTdnBJNa8n00}58 zNXUB!diV55xD8C!-%8}^>^nB#Z5qle%jhqx_Up9?s%^z zYpTb|WHq6nui&xS#WV&pj!~~VB(EFX!wXlMI#zQezRCr*VA3KS;X8_QQU<2!7r0oi z4rDN!cyvQa75jIl)GcE0vXdLutQ@aAPj!4~ z)9m+Ogr)Z0%`lH%7UT4Vvnf+A{ynYD< z{uJfh%oUn&7QR6hk#St(9(vN~(s(8lRy*m#h^iJvIK!~!)p)ay{YyWxbh-?z-r8$! zEITL(ox`}wKdgfx3W1JvM0kc#g$jq_G^IK z-|Q~zuN_Bd>UN(=lG$GJ+DQ8=y%Uk@E7HSRY{{9CBD72sZu8`<#J`m;;TQp|H))`^ z9#k39T^)56a-XZ-XsF6~FT?SnBu45hT`70;lGJ!dVQ`+tWA%5xGjeZc=;ihWh9#G` zA}f()K46tEH<+~RR@(ALQW-0DVmzAEa2EpCoKEf2XbN#E&T%Ks%+3?*(;F}+tysvR+LlGDw0yu|o?~ByOJimZz4| z{~3-F4sj~FqdV8qXPrHB9eTlH>w{ai)AUa$am%T$wVtfW9{MD?;c!T(8hbSDGg;N| z=X}{voBwT1PPlTl+`6V`TKqkPx>_CakI&x)Udpv;o57=Bq^vq92-?%3+kLI|zScq0 zFIChXb}Nxty{(&J0JCd7R?iG*X1fijZ#(P1A_EMZ+!iX?n;847O4 zyePZErJ@eIxRgTjMnNnhfH+v1ck4@C^_NMnF z{H(TUdS`BT5^L&KXd<)jry)ehNd~fVwsd1lJ2Hi;Vz(|VeDPFtW|=)x_t7n}Pk_Dn zRWb%@&P*BRHi5+JHpE5^1n=#-C9VhW8G=p1$dd8l|2{?ipY-Abs_HeB$z3;Hx@R1Y zWdzAe$jbWIOxyqMg6Qvtk#yKnV|Nn*r@x1Mx0%)NT_zv_ zZRzkQONFT%MxbnH1h}Od?@hO39*#rcEbAxd$F3+QbD$ft7RHXr)s3fQ{G`y9;zAmH z><)PojNg>-o}lUkkqDF4ngsgqPDHMOe6h7!-EfSSEpJS$bA6g?e=?iL8sGq=F?(rS zraXzWaVe~)BOfaAyE-4QOcHn5HxN2_^w`OCQB|k%mT>JSfX6jsV6MNJom?{|@q+p5 zA}W=8sjjdyO#IZ=MPH^)Hlo1GOW-7_#4O<@w!hH+(mPUe}?Okb4>U@ z`oUVwX0f-^8hi?B4|Ylu+Okm4gu@qyb^YHf_+MZYw;%MwXjm3WwMKkthi#JgPJ<3K zia~i%*>%7}c+UUSBVvqI5m-sfeDvX|UZqxO*XZr}0|mwjxHluQBbi)P>ANR^@B?0p zjzpOO{9;-iE5e&aC;x)QO9xo?Vh*Er0%J{^S8q9hXef_uy%~s{j!W2Eql}ho7TKwM z(lh;mDwCzZ?*`+BZEEcAfk>^iDG#pQJx$CSUI=)+YYG~MZPsffOPa1x`a;qp=gw_+ z;6{S-%0z5A9ht2c9*wVe7%oOzZHpxAG-x3Lf7@<`xVct#uKCMISUn*$U&xuc!#2Oy z)9@&+Ml#d+@J~LkOj_JL_Zb4%N;>a19v^iQT*+U;E34PdPhVHM&p#*QrhK%M8~yvb z1GmCoPY6$FT0CY6b@-+6J;r=c`!FBAe%o%qz?&H^Ci0d}m{8s-|5M9J?@{D`2EqzW~h7 z_1*kSpUeT}ukCD7&Af;5FMsiRsHZ$qE%85k?Ut=uW^a1`n;7wj&7eX1lbSUS+0|Kr z4q1sX@6J~@p4mL`{v%Fd?tPRM55{C&k$W9(3h8m8E}sR@bN{8az*!;x7#`NWXFm!~ z57y*A=H+izIbc4#`TdbGZ$aOUY)`sBP0S+gl z&&6EDJFKdQY}Nxp6G|K6ECR)R-n|G&*bYZu(1DiRqYoYuvQp`-QM$K8YhuiuEl}BB zSYlY=S-8W>t!8?D&An^*u`%W0?H4~WKkJ)HRW97)^b4}vWKsMq?bnXWlt7Ml6-12y zE28W!&Hq%@2E5nk~xwxMBoAHe>UvHq6;-)ey7cco4v zrMOMGPb0;XzwB;vETeJenx^Moeowi~yW4n!)SOIocRm~OaUxQL$&mvp_Xb*&e_iZ;CqdZ6!!7K23t5Qth$ zuyekE_4mZuQN|cA&VwdyHSCO*Ja^dI<;w4I(oVsR+j(tVzfL!< zceBS`(`)|OjdIB=7DtVW_muFB7kkn8skgg|XsbKH{PidM!yL24h_TOx(2KgNPhV5R zHcSo`koH11ye{U+jug3iJ;bxYJzF40nGts3j<3f>_Unp~s5Hl-{N!q$Z!!VU<9rK; zT5)L4p9CdZa*NRQ(9w*GzAt4l5pQGhr#~U~_kZ*rKfv)88K_F&Yvd*%-L7xs^%`ZF zOOghxH-;)HYK|Ig4P(otPf)zy9JY9~c~yN00iKj?tI7|fxra>~0opD_#-(>G0kgvp zpR(KiA6Bi}Mur@6k5igczJIstO%9J9&3i|s%{ z!CPrF`MfblaS8Z(&Vv(W#})o&Q7&c!;UZ}Kez$bA<&NgjIzzI0H*@r|qh}e3Q`17) zDHnLAW9w!mkjN7F=0sEK!PRFtpdu7zp?kaPQ)FY9`W#%wYa> zCk6exYk5vZ~NBzgzzonbP)^Q*6S6Xzl?DCZupN4QT3gxGV%N9!{tD^5}t$n#Qx6K zHn2$kK(7cq!?D%IjLS1D((W+hQCW&`)45-!Rr3xLDjxIObc}34kr!(%4qLmKEac&16s^g6c3${a?dnak zb0u*%yo;GUVe5n_s#_4NeAn(u9&C*|yg^C^5A4r02i_bk@a4`XNhN*=_tCaUnekm2 z{8{X3TK@qipHT9Xx88r<{ZOt$cM|oTj-#>3XIf?IIlsfk?F?Qs6E1`IU~8t@cy@wJ zHt+8U*@)`Tz(0jKuJSWb@t1~&7^=nW8@8F&)W_8k>aDi?Rt@Jhp1$R*K)16Hx3k0? zexAQrWYO_0;Z=1k$t(ujrfqLmGMUCeQk#xw9#go6LXv0kPGI)0oGst)qdfqL#L&* zq16393x9S9!G=fvH1u}mOGEjqTJ2bjV`^_()V=qPG%+LR2kQrKIlD zBbQ$D-WKS%kwo&4T;MSMn&49`hnk5>z0vBKKO-VP&P>}bFl%%ioxn?Z(`eAg=45?? zW1dOVYXSY@eR$QpJM!|lD@}Wf*X+@~89g*DdC=o-N)p!3Nu# zDk~-pE#dgKWM5=tdU*qocI(@=ohJsyEEe=7Fjw?3%JTR6mC7Z51uB}qB(Tb3#$q+|oSNR~vuKU9FC?x+< z`>}xsg)kK?K99ERy@22Kmv--J2UWElcBVjPd3vtaHA`UI`FzdxK$a01N~f#|+`mf9 zJA1G*>;)FmRHdKIMdz5b_G%xQ63A4vR z0{E{oKo&OM;^^63xpcMx;N&5+`12vN$|#6^x3^-w zIdRiPMv^`08;Va{TDVx=F-A=B?$NU=262Mc?z|N5~_w$$6p(+VMj z91SjAc6#1TPCfNE-bw`(RY-Ziyj5TAj8WsIxFTFVt z0-`$k3lY+ZEaPSF_k*lN6wDv#al)k%_7YAZM+ILTw>b{gQ{osK_|SjjoV!|64wSXq z+n1BV{qvwBc-}dFhQVzePKbSYyVC`I;Vf2Asz}Vb4PKPy`uz?g{e1?>^8aSO1=+9b zUVmCOM>b_FAgj6!*6*uT){v2_mk|dc#$-oB^BHs4_@R{^M#zHEZic0Mew$_XYgdC~ zo~Wh)E?Td4&CD&_uE%n}z0#v1g|@csZdTJA{+^rTm73d&B6(BHCbO3>EuUS;1Hox* z(Qlh8j2kM%#w)6egE_JIx%M3?bJM6r{D?T0muo$MJK3Y};=RQ175ww<`w)*g8`#Jl zQ!0KcjWZ%wJa4TZO1^p8@Ot* zWSm_B4?~98?;;%Br11LQZ^+(a;l4uGm(G&y1=}JKuU8Gre_k)V%j1$OsRtRgVx)3OnbB$l6DZ%R-|Cwc zFh1CVMyh!Vs9C;uj35C`QANiEeCOCW{r`oWpuQDOJ^F(&N14#YweDzr{wQ$at^XHt zs+V43Y{s%lL1ucFEVSUI2knXi`>L1W3C9FF#uJ^O0(a-?wMU3my>&nGsDSiv zS%mV-jsabMXa{#6F+GA1^TO_(@2yj<_u&0E$!+O+Uvn6_hUl20vZLW_y;qPgpH`sf ziP{yxO~jI4GbyZyz2bKCDu$68EMr5i^|*+WlM@|^X;+O+KNQSKe@wKvX77{M2E!B-EXOeb2Mt@!#0Reyqe_1W-Oo<`Xo z0BgoD?Nu6K5!OWAh#+E7@wSxj z-CuYG|N8V}3Y&31cl4WK03QqaY@5GOQ!=B?clwU+hZ15h$a^|_%heAt%#YY!d4}k7 zhgZu|6k&MT0&&%cvEiB;WUZeZ-v5iR^Nech3A??Bf{1`fl~6>QbP%b5pdunt1!;y} zM5NbH0+BAgH)$$Odhb0DdQobC00~ukhtTiw|Gsyv`|WTAf82)p?AyKz&DT--1=nbY`#v~9bb1I z95su(S_`9SjJpD_8D2M4&Wam2{+OvS**%XR__(N&j%)dpW&F0Nk~l-RJYYEcXNJgY zXVY3qa`3Q&8cM|bLS|N6E7KEO<@u<4FO^WlNdvf)d7e=Ml{%;8C;y-j(Lzh4T0rwT z6|514*ki4oxc2J=33|nP4KCBwkpYjlDnDMe^@U1xQXp}a1UqTk8xrfVWeeSuXkr zw>xTOw#80W=EPh7N}p5@BPyVkpL6l!B=*f)L?ZkBonNikjMxf=RjNc^KR?h3HMS1P zc3fncoHTd$K1xMD|JZr0Lc^g$1zNB12o)YTt-_i}@+Un#(5?PBe#ZN!4Z2vHoHv1{ zF}ulT$*}NAnXFoe@)M32&}5Z6lQjIL-QK<3BwB%nEiLH;8%)k6!kJVJyr6Ji`^n=L zyzAMem>ZdxWZ3C;qHAAs6rOkx+7SF!zZWYdVzRppGUe_j+KAUb5ZZ!I=TBntoyHa$Z zG_j|&T>9an#@?~&YZ5i4-S~1aVO?UPI!XEi{)H6nXySM#&u~b(pP8zUE82C-wma^w z<&UiirfcB#K2n#PEi_Qt8YqWwEV^mJx(Ux`XCO|E=V%$?+1oSXb@q*)YrT3ahe&>& zO;fE^4*~}~!5Fk8oneH@Hyy7>5aUJ9YVFzyjN@2@R<;%lS!^9OrUn%iZ@tr+*8i)y zAFos-&=>ZKE1cC=P_^5kE+*P>$4e@xYRP6rf+*H>qSFm9;lWRytmS@R`P^(BkKV~R zA2EmmRLWm~rAg*|_BK?zamO9-bnRQdzudMKw{|0(BEvniT5e2+s1Wv<*Q~U?wdJ>F z*ZsmvngT62eK||1yBCldJx1!cB5uZL5rKFaziLFcgqw_{{Pw3-D6~8CDYMyT zB(p^GXS|W4k7tTZm4BoXK{DGxBi)pVz`fn>kG(!Zt?1(0~Vk%o6rd839V^|U^gyT7Hu#&Aj}a5cfNXooF{*$0~$YyL|qcazrc@2l_b zQ=absPXro>mm)QoQwezwLmFxBBbE=} zaUPhxtFWl%W?83-93EF??vsaN439~L`jkAr>fJNq%txq`AcALzR1y#hE~Eok!ai5m z26`RV`%Z$qO}};UYPKA-$^rcF={J;J-yNAJCkRXau$1c^>e+G75oj$Da53m;Zu=Eh zh>UPybE@jY5BmBlM1$s=xY33*}j5y5(Ddf(DpGOv3xpXRrbol)7`+CwVj zln~R8&;+5#kRO$kt0h_>(7D@p#BYT{Z)(Aa#&R*dGrSfFIwksRAGL?@U_)Mik7~{G z<=+pl%h9)mU9yQemw^{S-b$ula2OJV_;$yCE5Ept25$w`1|3=Fq{icdFwD@P^(JsXPbIE2bbgdxI>3P7_JgLDBR&p5;B_L15K`vplJLN+l3;zu_m zKRpqK!$OR{7O2&~xFf&!UNtMBdijft$X6O(ojzSvIdlJf)~k_sk*~}$>3r^6E2Tdpz~T-;0;z?%rvDpC{+b4*w zLTfdd3l0TR5QRum{}WY^@DCUC&@*8>{aV0Tu@C@lweuD>>cHgC&Aju{67a!47k+th zKaBk}m%Q@xJ>+|GBA?9kbpAz*DX|>?^_^6A69JrR(;_U)02SpkL!LNJa>*$`K1{Z7 zAL`ou>GvLD-E_Vt3@q7HEi_@G#0+BZ_x}C#Q&!Q@?M#1Sh+{nH?Ud zxH6w&7K7FzIA|_BoemQMGo>_KtkZ*U#!_?nPq(dND?Z{VE(lK?GzmTC^TvW2%~2GL zr-8bbWR>U&VF9{a^8ghln+v4Gv~hPbl9LW1>C-LlzB`t0vQ6vnl+u*Z^~vHdF_rX1 zk!$nN4u_%t%Q7Q$#3X9!^87^n%O_4`+ats-w!O?wOBqz=$2Z z6}_Zxt>89U^v%_a8L}{k81>&aYCUt8_-b@{xV62JiAbC&ThI2w|7l)Ex?|gX%YF-y zjMOF@^z&XOP^KDOAc?X#!7lOq=GtYM|O5dhR&>%tboCCn$Pd`aQ`P89dsX zc-$Sx>E?iOrQP{?e0!&0=iS9NE^n$94(+y%`2jA+p)P^IR{mB(;^uj^j5|%54s>E> zZL3Rs4cz5>1j*P%whO=X7Lt3a4;bZ|+VX}zBqDc=mvrew?RNE8aD#i_oGu?TQ?N)f zmW3!>WFS`^h`9`n=#~180?pzNZnm&fAGO_=gqWc|WklIwIMws4B!R#syKcYObhYi&qSx?qk3rNm*cZ3Nz zq)w1+b3P^!`!8*$ArOx3Py-3>mUky>u$R@7w!MC|tdCUjnFj)KclH4lkf<-8*09cb zx3cWMU_ShDtKV>1?AMfdys=0W7N};;K~i(U--2hF%fDb^G`T9P+kLxC2mcx9Hkzwq z=j}+>k-VvOEAw`E70|i&RbLCLm-4lCB-OJI^=h6n%}GwkM}Yo|&tWW#XH}ZrdUuKp zqGXPB+qw~2u`s>i1gYcRGqYKD zG&Q^k{^0!qcYLNY+xX9Qu)3mvSrHZ4CQC}+Fmzq>z!mEqYiYA&AStFFUfgW4$vq6a zNPjST)^--?7UAnO&%TY#UJ+Er@$B^LolOXGm+=!>oa)tY=Jseh({VW*{Tz%`eCq9F z5*Md|B_&kutC&7{~0ci$*hsc5uF z=smirv}Nm1Yy$-Ek}j}$twd(-*mAk;Gm6>uD;z0By{)w+rnY4XenOUHB!r*UX&K3; z2a3E+P6W<~(s7>gs3CO_RH@oEC74@zZ;7N$rk_@PE*$|)=HeNGCuwG;tLZ0z)Z{6r zg$a*B_rjDDRTEE2JepRi#wK;yxjeCR(Zs!-@#)xX?bZ98N88|qczU`lX#TYzVRj}}PC#Bg8_OwdpP=5oXS zo%Ahu-)&H?ol_ZYD<^$%e~D|kxegm^>K*d`s!(i(lfiJUJoI<(ubF$21#;drx_}U6 z`u`WAfO~{$T_x&=XLIY73sNQYK5*hYV+&N7kRM?F*rbAFt zD6D)fSp^^pEmW#X#b~0(2C0W<;8&3fm?Ixe*nyn$_LQZq!~BA{t6-h`E-qRTwl3wE z!lUv}iumU!`cIP>BMj>y1U5lUww9%l8kp&X%(fBLv4sk~RPGkcu?Qe;U5*1$b)e!5 z+TeIe#YeMtM&`iJmcSH~tBR)8anoEvs`-kdl06^g5cegoZZ-Y&<4#FWLuV}glx?B) zyg)%KxmWD}oY%BJ+FE2M>m1gP8E|YcXG-l5B#HWiF8xnxF(s^DpT_!@{u8whzcgNT zHO{6-u{a7gekwd);e|Dl4$m<@38=LqW{G1`;~otev~%mGN`yrQA4+iRFhI3*Zzj>Rel(y$-)DxcoHs z@=X)|M%8%vhA!$gPit8W=z|wkiCMmc0CTBYin2`E9qH&@`eKj@uy>x2zo9_QCDxkc zyfImHchO^qYsYElVw1Uc=NCZgu-tT*G(4rQwccphr-JZWLlniA?)l4%LHt0uIehc(cPpYI&4^g;_EP~5KcnE&#Vkqq@*ca6yX30Oy$l!c zkdk->MjDm5Gd>pn@8ZujDfl{tzuB0HJ9P^z=T_An)O>aeoT>8&_B!WcahZYbxNu#n z?x+Ct34?>5q_eq42dzpoU5>mNyS!P~t;WM|Mbj_yF|oUCekc`}n`#Sb@!g*uyzIZv z-W`OgQ`|`UrqQ+7gK7v@KKSQ~`n&CWTNf7WvAyhXmzS=6pn5=oV^z35)Zq%KYvnlw5qq<(Aj3n!w=oXce>EX(!#5 zkNwxhtB#-llQ3K--TbxsG4lyj6DqYku88ZlP3$DQ;f49h2SlFT?B{E_UR(#Y&HCqe zpTPmeP1hG(#7enf=(#ee4l zsSkL&41b+D(RaMWe+EQN!PWl}HASOWt&>Do~N zXW8Ruqhw1?aG4JZDWV_^fSBstH4k);JO;J@6;uDauH^QUwgeSWs`!Y=(9EV`{WkA* zldZ@PpY^f)vL73j1VbqB{X>M)V!!HsQ@nP|$$ZHHfOlBX@NO$X)+6^~{ zlJ`?Rjk(<);egq`M)TF*s#Vm;0kd{k*S#8WuTjAJdoOg$Dm7I|hM{$M*`Y?e9QCBe2xlzfz+# z%3qk=YW%I}JKF}Ar3BH|U?{3JycF?Sv8PWzDL<=qb=C#IKl}QRha7$_%77;1-J<8{ z{qLl5+*4?Ld*d&ax_;MJK+hvK1~g-?Hd!p%m!M*)$|tJg68S`sVFf`1|{_h#b} z*%Z%N3@)eT3v?jsupd%NrSXgz!@Lsck!u<&q&#}R6kY2J^JQ)PtDV4q+(X|KxkW)A zK6PD)44sMAc|iVDeU#p88#v}Z;_-?ly+kph7Q0m61eONd^j$B#d!RETJ+|WNS8O^I z+@;?)DYrViyYFmd^iOA~yAXxCW}1U@HK}p&_^)pJWQP z`nMD7`Fxw|Q7alxaSl8SA;JL-J5^$St$Z5^+BXN})R`DlcIe73$yy@j{14=HF%|K?UAI#y&!gP>Pu7E(MP2$QM z_hM0ZEQU48`5ro1D>Mc2*0VmG|K&5?|CH)(>nF%~DdhC+7A+fEm^Pj%9%KO#9#fN0o`s4W^BLg?C=zG9n^W|Bw*hx+|C+ z8R*Fn#0cekx=fBq*=O;E>6kOOUp8IFM?kJ%{q4FYvrvQ`w+;@p_C$epyMUL1{$r7F z?s_cSq%L33QJ8H~(YTJsFZ&ye4KM$MSCThU3*60_{UmMCn53yESx~&!&J9=9P9!)` zL@zCtafttBxTv`M8!I&}Tq{8(o z0{$WY&oA0^w9kfbg&!$phe3K5$bee23r7~mJuz&tTj`4di``LRAqp1ArgrhSYrA>~ z_SVTaQR|y4y>VQG5O?%$Mrm@c+k``ngnw^#BJq3hueVma?c-sdJn%9=!nld(YGVXE zPC^5$P3<4;rUG)1bg-2}z%;W;mQKWQUW`MqxU|(bnbXUI6ShL8jl?q8VXwcDsc-j+u+*%9{|Rf zoPyw78+Ui zjVAT+16yZD8*!%qqq$3=(==3KQX<3^K3JaV>D^A`96YR+CPBF(4xmkNs-}$j<$L0o zJ+hiu>^a!cX8kJ_Qy%^jU2dkhz?ku$ZR+|8%g%Dt!EHGFH&4Z>C7cRbQ}Bc;JX#ij z?l?%1(nrQ9=Ywsm)Z=9Q)riI1%cDCIj3r(Esy`7SU?-w2YX_j}U)QU+X(|{ucR#)e z7AGZZoYz!)zgxq&VWZeJ<14O6yarHv2qEy|9l!gCCJ8vwy|3$q*RKz$E%%7fc>h5r zj3Pl}AJ@W1Z|Y}1Wp9fW8zVs=ARI?}AW6$&(@%BZdeXRqDN_V+MPVb2v(z3~hKc@t zc#$x8Gc$QHfEYpom3k9=!HXg2!x7}u%t z5%0A-5-{WOU(MX^%sW-!%ME17H(2`kmKf!ngIADE8o>yQq8^I!h|q?Kq-oc8n6{$d z6tfiys6Kqf?$%kL{U-iqSAcAev{RtWblbwK&|y8&*Jb=HQ|hVhcMYmQHq+XrADGIj zb2oJF`iSqpK#XL*WS(QMt8i;anA^TGek6u{AK&3fuINtd{x!&aX&ICrSEWbcGv>%H zW0J30BDO4_x?HmORX5xF4tRw0_X2N54AWZWM;x7l-l!U*Eak$l@FQK>FLVeSBQl5t zULH~^^ZE~+;jH^z(1I$B&BQB*;IcJ#Uy=-!3bUCfbWTXFqz&js zAIT)1!JYur^>yEydq3GhNR1MoWg=*IxB;CdY-^h1kD(+v?ztb|DUgKi=v=@*HJ18t zgRndL4&V%^tp|!IHAaBMgK8l7g~4{UwjE5p$P%}^oGL!;nZ&zAd;G{;a4l-(b4=hn zXQ11IzA1^_z!Po02ZW7G#^?ViMZsGX*=6MiKAd*D2Q{`cu~_R(VM7N=kB&|tsY&-!M@*QxB5IgT+A&Wa4;^?NmRDnKd8#vl{6#X!T#{K z1A?^6)5#+{n0!lH%dN*bSb;RB%Y9H56L_@9XdxIu!&O9BRVa8byMtxTf3V?YQ#LYD zt!*N#I{n~~>I%y5ZWk$9Z~Xvv*lua4XQt74HyO#rm80L<#;z?fYzt^Oop*hL!6ADt zD#(=!<&g%78RD^6N^-*Q%BmW67Bh)~tTx-7(PMM8!+p>>EQw})!j?u!hmIGW^4cA? z-D55pfMHgmWGCFFldiukP&Y#Pn%1WBOLiBXeHUwMu+v^sXxAi1EjqrP0xdsxdvsm* zqD_Xir3?k8gYBt?2yPpPB6wQ6(KU9fxz>8_3+erW`(QWoN?yxDz93Ytn%Q@ZCX!kgI{VlUtPW8S3xckj#z+dEIK2_%sfA zl`GG2?2oEkrj2I%i7cVP}OxdTN-DcB1d%W!zAcsoz2FgP}h;(y}kxE z`KjZGj$a#3v1tBc|5|=MqJAf?etc9(+c6_CWT?P`|V@{WrbYuM)W58G2hPyy3Xqm?qs-!gY)0 zeqN5%*UU`Vc)|&*g|N-Tfx0=AD)1d%AdvCjZ1(2&){V;wk|q6ynN8%QHQo3<{DRcJ zc4oy<%vM2EVMTUb^53Y2u{Cn1xV_-Sl>l-y_pIxn-8;`x1q5U1qJ>y;bP zD--QH%k)LCIbznOBSw0NhoF+kC=uz8A|-$Zt;#=Fv2H7A!j+0(e*)7UenR{EoMURF+R`Dmhx$3Vvd z6KZTYV&$WvePq?$XAEoaKXJPod-@sPt*pXJsO}sJv`DUXeFDfm%@GE*e^je;nR^y| zbKsB1x$-L{KHl~GAYGO%_)ImdJNLCeaW0)l|Ks~rxvMkioO{$^7X940Kig6TzQ%}X zo=f^=YR`B)Qh=|t*@zchjf7prHP=UFrQs#Z&(*xy0_0g(B3=vAeDnOCLVTe^6clf7 zmb70i4SdO9my}^a8?+B**hNtW-EopwgMmE2#etrrY)Wtef2!IN^_juxn{V?BOHJ2> zeflIL%FSk!zPJ2cG%LqDJRCSObzELs7S_G-4^0#CypNaT-<>fgGYGCwpL1**6&>+` zLQqm@<|f+B4GxWrGOzI3;?LXoVeg9?rk$7lQ2^D4#PgQd58|Y@IVXC#o8rnrx<5Di zmweMx9st|c@^cNldwhxf-C~Vnfl0Om$;?6qxfTVL(ab#$vn0#uMidNeEtp#;N@ERQ zda zivD=>BhtvgD;rYIun3IK$8Ge!|3;^iWGN$eG> z=BapvN7)$-RkS)S_hIefF?}pZ&RR@9^jducBOJ-AG~D-P>!MsO^UQXt&yAZ@=}NtsRJ(}kLZgU3zkWty*f&D<&{SXY^07?> z+p$@y=VnqodbbRZ=Yo_>J49Fnl9j-JNGDbwaX#${the*+gnNitC zXrc>FnEa2xx`52bEA7akQ!fZPLsD6epPMdH2?6u=2`Z}GGG!^=>ZoAc4py9zzN$Vaya`);uH9 zpD1SZp|p#1rEKltlAI8{a0Hh7Wyhwx%X@;`MQh)bmJg3qz9~6s&(Z0e;SC^ay&!m< zwSNo)Xif@|+ZEIp$qUn$f7{x*v9Ni0GS9*ozd)f1q{joi zYtgmS%gxS0w2EFJSZ*VFOUZm&*;Bk1f_QPQ8Jsvw`z{u>{42BpN5 zSs%&dy0|*{3WECpJ9AyL0a_vWxvbA-UE`^ecq6GmU7PKEi_AUFTUir^OF#_oz!jPc z6@x2Cpz}q1Z%MR&IzP>*5Q0Q8O|$x^zocu5-lQLYZTJ`GZphK%5G^cCG)Vtsnm9)& z=y$Ct8j|+9?2_@;zztlxFPoX+rR<8(A>CG0;L?VicMD~!Gtx{|@{QuRQFRhB`QDB@ zO3pkfUkcy;AcVmqZR+mqPAGXx@X*L_T&sVSW+wdel;{$k&`VeSsXSSayq!Y3wTy0B z>lmFge7Cibr4b(>ZS2KYFN_WrdCzz9vTM2~Et+g`@ALGHvN~zpd?bkz>i2I2w&7T@QJyFks z#mX*QSrK$n3)wzEIOsy5jv`3vS)>yt!;+h#Mx#AU^b2VO!0-%o&la%z8tS~O;7e1+ zEh6#Y;nkDa1hS^fvx->BJ*QB;R|*p;pCF=c{andHn8JxtR!CqG{N6A&>iMJZWS5jD z)75_nJFlqK00OnNuhUS<`SYICb~T7_uuT(My26*0%*?YBsnb4rc#ddSC3r9$^CNPH z)Mm3n44HYvk$qFi!fELZQa-buL@d-y><(UbPg<+kNbUmXCx|z*Nov{K%26YF=En#; z7FU70(Ui;*w{@ocDkFq9{rkfrq$E?>8e8Jcx`K8n=qHHx+;4DryhD*{nKE6rHz+Bw zD#X0^^>2c|+kXOoVAgG|6MZt>CoCL*+afV)39fXH4)&3q=sW7)MGeRDZ{7NW30lEm{?<1Fz0ug)IcVa8COS?rR=%hyZi7S1;+PJnI3=& zjK3HMNE2ImAVIMOrC1?JGCs)+v69WYg-ZaMp?D%Qe;~8n$6I=j^rg&)17^Q~T82Ar zZllX2Y<^{skOra`@9}peWBsV5X$7{6%2~%>b<%4cIf}&)r$*4>9q5kPn}Gu>TfVQg z^FCg6N=|DQN#wKyvMdI1%eP^0pZ!k7T8oXucBJ{2;v%aOKJ8w=;Sh#QG%)i|z5Oj5 zVNA7ZV~D;J;O`O+9~G$c+luOUxMu+0Lz(<|n8yNOGHf4)-)Dh5J%QASH;s>;6i~w- zl$W3vC2mi)B$zJ)IaIC{%kzb&c>F^++)yC0{CmDQ%92?!2y&FKR=+B<>k6MJFKOpglO zyKFn>>00rqvLWdWz2UnTW+?@pRNzjjkm{Iv6%QR1G~+enIrqHn7ZH4?!uXl|b@sC% zS-{7z->c>`B6GQ>1_MN+&zmP9ZoCtyDY_=??YP4aCsff5jiVm`rHB0YD&(YoJ7pNY zLf>m=Q12c_7IuRxdo0A*oM7=$UyLp_i|1d~lP0B%B*u)wdp8Y-nX<*&6hjCT10T7X zvb8V?>XhW&>JN!`l|rA(({|R3`e;^76AvUTBKCGn%~M9xn)Xyw zfpmzP=SUoKg`>OvD;Z@scX!B-d|*1$02wUpC>I!;zXNYnkpzQ1mqh3*1Ait{J0MDVf8u3 zJI*5+$sfahWQhz9InE1=w6zrx>A$w7FjwP1;`yYBOZB@G4dvFpy z*t2EpNCQMi_EN`Cpn=?`RP5W=X1Ab%dpZJoPJ89HuFE`|HS~*Rfok_3#P5<_T+DD- zOx0GaUSZ4aHT`pRQuijOs_BvPiM1Y0IO9Uq+7g50z8K9W^%}QdFh$zb3R&nSrteTQK{o}^)`~)8YQ-4to&_-wQ7zhN}()s-@c_klqN28)P1-n z+BbOiz@4t;1?<5}w$GG8BzsB4=zAjxM*;om_A0!0xt>~8N?5sz$nC}+E9tRR&M*>t z!|X2!pS%eKMa7aAR3Uylt3PZ*_kI)WIDg(icJ1t^Omi+*u<1ML&wA^u@)JcPEBHsV z8}?t?JfY&=zH^y*eR5sbiVtgbSWR<|Jh#8zYOSBY+L|Ya@&8s(?G;?);p|0~3hY?F zsuNb7Io8eeyc^D=U(Ma)F0hMn9wr+^FXhz7*%YRs*=~fr*U?7IU>5WYO!G>H`?L~r z0Oo&acb&ndfivxgZRWVDd^@wOw>1T17r!C0OwX!S&B~!!5j;y>-H&0e8JC-uNubZD z>nEuz{KEDCgIA#-ky_Qjuvm1O$9qO|{58U$aokpemORQ+-yIkDLcaS`!O?9{g+Bqb zWI-a|Na`QacS=lB7a-2T$RT4$*sNKO*Njs2CV7VullxIHo{Y9L}n8?c1wppP185 zE(p$N2Zk7)g&zz?2BSROQ;4mAY8*O^|E9hiM&jn}!%sK7o%fPEYYWrSf(DMX8fp;M z8?px}!*vu0dSjpac^n+DHM5Z5O- ze&5pbhEh_~fe_jS-BMeFZWfsE;Ex&Fk45lp{7#s?8J^S#|I&T(OX;lppD@EE!=nP7 zKueRN0d#vt&``@Fi-IIw&C#j>Y<79>`sxJSqOsO=Fhpt%8BWOkU{vOs%T@dk?{^fS z@@$^3osBq~m<9g(a#Z#*?tCPvWeG2Y5{Gw)OJsN{ixOM@yxyyxu!2ORz_B_;-ma$@ z*3|lw>y7i-Zpd-D1GD#1)qYYijC6DF&jf5U=1uc9S}*J7d#J~Vx?DVTUwqaj z?tCrtYHhz@YwD_47joM7I*OfmS>p6709F0C^LaCA?n?^6W)sGtgA8{oSW#>mahil! z^TDkmAaJ#ZwmqtvQWT$9Ip*q>`d%*{l_HwYF{_ZIp*J)dL4)}Nz;P-BFXkf){E;*& z3DmPon{{^#+Ixf-3^YLMO^P(&t7h{whYUh z)DJuEc<*@44xb+dy2YlmtW^y<7jM{lqzQPydpwaV+bmZ&77S=^H)z)&qRKJ&8}T+` zEy<_1`Etu1reolmaNdo$P&kpjUcKfM_s$DD(WnUjHH+JJ=+9fG-5E z7X)X;eHO)o)Oz*_X#4EFH=zE?nMbd?5j9{pl}S3tshR9xdqBxC#p_Jh3B6_ion`hs zXypoPnBtrAukqh=qc;jBliz%JukIwjSBp5W-3G4PJsWk=6BRIXjFDd$?XmYcNS)<1 zI_+DdG0+&MXu}KLpon(nTAcLRntaK|5;3U-dmCpl;H+;kjaiOFpsaOT%f9Z@j5T?!#=!?)rzoUeom^kdx6{Ds{ew4q>&*`T-E1T&s3piB z)}OH-pI5h0(m3ROIf=M*iLiet{UL8^vdrh=Z`9SN=U7US75~OR7}m?}oh!SONEl#u z+l#QjcPZU{MJT&V_>UXNjkxnXwIh+*&;6eeGHx(R6_{jpqv!O2Wvzup1pq)F@6f!UkZMa)`MtmAa#Nk>C6?RxBC2vRg1_$>!HCM~h(KBlt#} z0%V`^L>zG-jw^XKyyN7ve7@Ooecn31HP;OON_7I)+0jE+S@m`5M%;zVJi7w79)P!j zM+wyafki;BPluECW~WqN&o3+|>*!}@h8*X_>sAbTz9@>rANyOEHjBZhF4Qi^)Fd-R z76d5mL-O8Ac14FmRT(des#CxY((9cE>w9EWEkIx}L%O9U#BEShH6x64=;{1Y2{aE*I&&uNcW$6X@wt}h8@tpHajU+`&H^lBYjXU=9 z*T(yHhJ>Hw{zFreXH~iEmpQ2vqxo{1Mwlw%O+=&uGJha3WsmkvStUF-f1PK^US#cR z)3CVUVB2rLb=;#Kl6Zq+86Mx`TP(d=FAXZ4!xC6F?sCe!;}%O;-St%6doIS=g*2m;}6Vg_$Ycy6`cG6 z_>pAPNcT)xksyYTwLJg4W#_tOR}~HOo`l_=&x*3ijNgi!W!JbrNkc%fa=l`Im3#g> z7(zW4yUaQbYu+%xiOERBD!IZsokmGS*GNX`@3&ZeIFvv2GhQN|z1~o}&^a-aJvQ53 zWVv=fH3_9|ul>vP=Zom)ausp4>QI>3}0!*St?!WYmcnVIf zp4&IQi6qVGG2Ox)nB4%b{h3tULWo%9`d%V-NbrPGqG^K_QR4+if&L+#(Jd2K$a zY&lSUSh~xU`RuGSOWZ~pZ`f}2d4~X1e9Vv6cT|p?S8QTyO9!{8wD!fjRA+j7+UoUsqMo1A*D_7^VGR0PM2=Fj+690{j z3l)%fAaU1}F4c7>#93GbEOPku4cENooNni0UIq^eo$t^t>9KB(|IKLM4UqALF|4L> z-NW+x8!YYDD3X}dziA`&St5a>#ZRtd4k}W?kYv5`|%L~*?2wobU(Ka%Mc_*4&E#{+*_8hmw*-AG1^)T`Y!D(#e8wD+2sK~i)ba8J=09wWvd=JW=@IjPEgD- zo2TO(xtL$LZ&@3GAU{sGVXa`eMoMWqD{6HfB~^nO2y^DX>Pmtab^>)zI>5$ohvkZX z0n#Yu&G}kcX|L|!uSk(C!y2_st{F5+2X^nLqMvj{6U*RtTMczlR7HbIf4yy|k5%7> zOTXjzz{`G`qnY_Vx09LHgBjLGt)3e?v#s9Pd1ROB!3ft4wG#=t1n<^A3jtkWtICpX z^-7_Lqo&syEmE&~MQrz-x?3aX1$_QYcZ5XN8d5Mz|MmGB?DIr=rYBuvTRWIIZ8=Zi z^*S6nIXBYPgAemi#+^`ZCSnRYmz|w5fQoYS=Q1ZEvIio#pBbw#U~o~WN8?pKpVOMq z%w>yR2&oWKV4ZykQ(OaUEl*KgEz4Ly8i+a%G{Oh&%J@ld3?abe za5ZO?toK>`{BqCKGfQ}bM&6#|TGoU$O5nR@^r!2c<;>&VA;K3;a{*9tyk01o zD8f~~Cv0`ff9650(z526n6v9Os&+x;3ym*>m;9l6f`p}i`+1YQBrgTY-jMQ@w;!Q# z2I*SM=r;DoAKMObqTk=zNkcqXpQYZjt28lS1qxQNX0{zzKLoY1T%Vdj{)~DaMhv!Q zdxBwgJcGr=7h{9M@kx#c(Tvto9)rZHf2&u}$JVz7R!_0JcA!Sj)vub2Bt&XfLKcU-*)R4|0S#0FgEAui)H@rI2F`BCSi2H6rfZroc^ zeD&ge`xKWNYP;Shoi5Vx=Ta5XkTNq=^}{Knl4|*M9zF3G=TOZ%hClM4K!qX-GL7%e zDD}&1o3u-w{&E!K_mz3Y*R}z?tbIwF>DVM9qOhlhMxM$kC;_qGd~1U*C_P5)e%T;K zwMhvqsRMu_Su1(Z6gAcA%|iuvy1zeND1N5GZZ=w6F%rzzqjUGLa6NTA)uO9Z@f6Lu zQNE=hU6{Gj$6HfZAn3~WnqMTQQVQfJozgYzdkarnv(PZC_xmru)BErSPK)~X-GpXp z7S^5;R`DgRT=Yc5ngP1UkCl-s={R56Y9UR`BCA1>-NR(Y?M~uZ+QZby23w|+_T4kG z%i!D#xQ+0uu9cA}B0B-()w^GvY{RJ6Gc|85XR@QD0)w_{SiUCI)W}#Z)2#mVT^->h zeBbm=EGUH0f>!4xsJuFg~=+$f^|_DSN=TQ%;&K4=Z?<~uRA>XCe9JTiaMx`f)wn`?q0>(F?)ulDdk)SBr$ zTG_)o%$%znRbi`BW}!y~iW^9N1dfXvqWr~?Im02RWH8Pt&eR_|@cUPlFKkbFTu}Bp z_8OL6RaYY{!BF^6L2)uRL(v17pFjK3f_ZfGJAUa4A6c&@ZmD?e#SkbIM?9UOwE@~K5rp|V z^G=CM&g|RZRPSDA5JsILm(jOaWSv&F+TNVH1NBNRtF(z;zm9*(G@+eOO3P|J;KjX? zL8Ydeax6z0vR0AQ->o2N*1ASX+Y_Lhr8o}OIUe27>s1PNa4LfEYUk{SYSug4Z@1e= zR=J`$Z_!O_Y9XJP3AfRR_e?c37z{yWo``ykhjoXO`JCPVh<2UftG==jZd=CYR!Gst z_9bMZvY510Ym_D?@`sC`_Ntj4iJ(NqW1*cIsX-AHg!g6)o6K#hhhNs4;~DnYvviMR z=yFZ_bfM2OLW}v<#ve6@#+9MC`1PTNX9aGd?>>&ii7Axxp5C)vsUMePdU0lcQm`c# zhx4k^+j2JHsk|T!OE$BzHmHrxww4};>Yx3PMxQ-8SJk;aR>svu?^8>x8KpJ}?iIls z@3y)bJLR&b_EK@Or>Q^pn!3l?>4oOke8i~l-Q$IW+Uz}UW)_b8)k=N7*w1nr^s({I z$=~QL{hzOc>9{J^RHTxTl8gP99Ox~Yw)oOrstjVW%U~+VzJi0RV0h9N7Jjv}+S+uv zZQGLB+i`3I{qU&{$7G0R7mmV|#od&!ys~C?)VLD8thKZM0^3~HqWEC+6~~m`Oc35A z;B#z1XF-RbG8#$l<7!@sUHK)!wG&82&J{l3y{Brl`;cO;c#G1S4UbpuJmZ+bA-!xn zCvkEIqh8)IvZml}+a1vxMz4swIn`_ixprkH1uqy&a4F@tj272%tHxs+3*e0Ky}M7^ zdQZ6ty93;#!oHNHl-&}v*}MfZ!hP?)IMjDv z_S(HEaL_yI1}8-P>FOZ`MV2$^?~)jTK9|uetf(I!bds6f5N9T6JZSho0lWu8`1tRH zn>N#4>!NJK)~NLhY}Q$L!EW+i^B?=c#23NG*TMJl8L*f~97xea@U$I#e37xmf4s5b z1x&ELIDrF>aqtbU_xb`p{*LYPb1&QT)v0uz1?M^$@WPhe@Ex&!;VZ1SzyIg=jL-Y* zyw-KQF2d1Y|CTnUYd7)H*&_i37)#7eni|+67%f>jH+3BBxQU+|oO!xOgWYPkw+zym z{JfC#hkyJKGD$gTusP-CZ1j080$_19ip?Ek2_N~sjlG@UkOQA5WLdnh9lJek$49`% zU%YVDC2VK$BG3o#MF2l%k;EUpu)*X5CN{gXysF@zU3cH+lt zV~5T733C+Q@pEG7LU&KV-iUQkHsR-a{Yrgc4WKU?2g#g`!DeH}x(haWFKqG7+QqxQ zg#W)k(^c613#Hjsd<6T!_Oe@C!bQ~j1vYC7^M|$3a~^@s)0g;(_xb`i_&XnbhU2uS z?cfqP=J6U4Z*2L9=lUf-XASuN@89R~gm=&NMOds``20Ht*)!lQ-@P4vzt;fw;)`r- z1yeSo%PWBCZMU^8~YdI#7-4qJCoDPS85}$ zTU@ITwanQ(whL47S**Gpc*0a%%bd;QA5pezeu7W%wq14$SMgacY&)=JKjrF!l{uRR zOSVV&`rEYTM&ZM;J~6gG;lnRiHN8Tl-Pj1{QR^4B!#9DnXAmzD*kH=vBk<+t5!jml z==JwB#%%_aEAPOa5rt6W*}qKINa*gI$;n zaf?OT=67MP2-SGPrio3jK&(~?!|K_CoqD;7tID(ZYQGd)du0D~NRc_l)(hlol zZJ)K>>e{k#sor9EV_VyWtt)|6?~-j><-!nlo!@G|Rxd2<6+ZG^8@tJsA#>?Gmyda! zajY5loa$t+;wak|@ePk#x+fT>-K2aAWTC#ssPhZWb{AW`rSxvFO3>ojD*SS(&dRN& zm)cXlRJVq=ROg8$9ZQ&bzDnq1yo7xcZq_Usf2Po>V8U&hqw0rZCp9zN!}wFb(=kT+6zOm+SPfL_r`nsyrpN!zNPXlJX^nn zu~cr`?n&1;%6oBRZLpZqW9yHk)n2jMc5UngLTrs~<*n?|v^CnMkK(lCg{QbB(^D3O z+hfz+){lWljck zQ5O>PoVPmrzfkvQroMze%sO=!+zc!*pskYI#;!g{cuuIVZ)9$IGHB;hOAGy;7YkbP zT;f%hc#hwTCh$>y6QL&pvl?vo`C8$7F9$VzcRrfq3g2_y>g>OpgB##x;AY^7foje^ zRd}QZaJP+}he?YVh6v9y(uuz6@@V)0X`=l3W$s0N-vjx_XkQuh;2+B^yp4bU1)OKfEZ@6d7K#+e%_c+}DpF1zn z_SN`xzGHyHZhPsl>1fk%|2t~XEq^oMI7gSrblcd~3OsM(wOCPCNUV;!FFpAv+#Z{D zwS9o|?Xu$*$_#}1QQ#S5sNIQ~sP*8gZs)p@3G|Cc@q7EnqnLi7dLrgP1ZG8l*v8KD zY|moZbk&!x?A36)I(s+190O7QnTZJ8#E06QjCbA0T+S=z2D@%#0{vocc}z*P|HW8+ zqdpr3E; Date: Wed, 19 Dec 2018 14:51:12 -0800 Subject: [PATCH 44/51] Remove old motion profiling from README --- README.md | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/README.md b/README.md index 72c78984..e6ba0fb2 100644 --- a/README.md +++ b/README.md @@ -22,13 +22,7 @@ Advanced closed-loop drive code for a tank drive. `org.team1540.rooster.drive.pipeline` -A flexible system for controlling a robot drive. More docs [here](docs/Drive%20Pipelines.md). - -#### Motion Profiling - -`org.team1540.rooster.motionprofiling` - -A system for executing motion profiles on a tank drive. +A flexible system for controlling a robot drive. More docs [here](docs/Drive%20Pipelines.md), with a specific section on motion profiling [here](docs/Motion%20Profiling.md). #### Power Management From 1c9e6c8e48968928ac4d00dcee275c3670287c8b Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Wed, 19 Dec 2018 14:53:51 -0800 Subject: [PATCH 45/51] Add MotionProfilePipelineTestRobot run configuration --- .../Deploy_MotionProfilePipelineTestRobot.xml | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 .idea/runConfigurations/Deploy_MotionProfilePipelineTestRobot.xml diff --git a/.idea/runConfigurations/Deploy_MotionProfilePipelineTestRobot.xml b/.idea/runConfigurations/Deploy_MotionProfilePipelineTestRobot.xml new file mode 100644 index 00000000..716ec911 --- /dev/null +++ b/.idea/runConfigurations/Deploy_MotionProfilePipelineTestRobot.xml @@ -0,0 +1,21 @@ + + + + + + + true + + + \ No newline at end of file From 4214bb9f86e51408aa8e55fcb8f243ff843939f7 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Wed, 19 Dec 2018 14:55:09 -0800 Subject: [PATCH 46/51] Remove MotionProfileTesting run configuration --- .../Deploy_MotionProfileTesting.xml | 21 ------------------- 1 file changed, 21 deletions(-) delete mode 100644 .idea/runConfigurations/Deploy_MotionProfileTesting.xml diff --git a/.idea/runConfigurations/Deploy_MotionProfileTesting.xml b/.idea/runConfigurations/Deploy_MotionProfileTesting.xml deleted file mode 100644 index b88dd855..00000000 --- a/.idea/runConfigurations/Deploy_MotionProfileTesting.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - true - - - From 1aedc00d6648c01a00d7f6caacb14b7006bf303c Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Wed, 19 Dec 2018 14:58:15 -0800 Subject: [PATCH 47/51] Fix link --- .../team1540/rooster/drive/pipeline/HeadingPIDProcessor.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/HeadingPIDProcessor.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/HeadingPIDProcessor.java index ffff6a6c..edaba266 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/HeadingPIDProcessor.java +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/HeadingPIDProcessor.java @@ -3,7 +3,7 @@ import java.util.function.DoubleSupplier; /** - * A {@link PIDProcessor for maintaining a robot's heading.} + * A {@link PIDProcessor} for maintaining a robot's heading. */ public class HeadingPIDProcessor extends PIDProcessor { From 6b493342baaff5fe8ac9f8783a353a0cda714f1b Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Fri, 21 Dec 2018 23:16:43 -0800 Subject: [PATCH 48/51] Fix naming in drive pipeline docs --- docs/Drive Pipelines.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/Drive Pipelines.md b/docs/Drive Pipelines.md index 7a0314bd..cbd2443f 100644 --- a/docs/Drive Pipelines.md +++ b/docs/Drive Pipelines.md @@ -30,7 +30,7 @@ Executable pipeline = new SimpleJoystickInput(new Joystick(0), 1, 5, false, fals Breakdown: - ` SimpleJoystickInput(new Joystick(0), 1, 5, false, false)`: Takes values from a joystick on port 0, with axis 1 as the left and axis 5 on the right, inverting neither -- `.then(new TalonSRXOutput(leftTalon, rightTalon))`: Sends the output of the previous `SimpleJoystickInput` to your `leftTalon` and `rightTalon`. Since the output of `SimpleJoystickInput` only sets the feed-forward (i.e. raw throttle) term, it'll automatically use `PercentOutput` output mode. +- `.then(new CTREOutput(leftTalon, rightTalon))`: Sends the output of the previous `SimpleJoystickInput` to your `leftTalon` and `rightTalon`. Since the output of `SimpleJoystickInput` only sets the feed-forward (i.e. raw throttle) term, it'll automatically use `PercentOutput` output mode. ### Execute a Motion Profile @@ -44,7 +44,7 @@ Breakdown: - `ProfileInput` takes values from two provided `MotionProfile` instances and returns the setpoint for the current time. The timer starts when the pipeline is first executed. - `OpenLoopFeedForward` takes the velocity and acceleration setpoints from the `ProfileInput` and calculates a suitable feed-forward for them using coefficients you provide, Oblarg-style. It then passes those velocities down. -- `TalonSRXOutput`, since it's receiving position setpoints from the `ProfileInput`, tells the Talon closed-loop to PID to those setpoints while providing the feed-forward from the `OpenLoopFeedForward` as an additional bump term. +- `CTREOutput`, since it's receiving position setpoints from the `ProfileInput`, tells the Talon closed-loop to PID to those setpoints while providing the feed-forward from the `OpenLoopFeedForward` as an additional bump term. ### Use in a Command From cd0f8c68594b5d13d17586f84fc274b2035f584b Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Thu, 3 Jan 2019 20:59:11 -0800 Subject: [PATCH 49/51] Add package-info.java for functional interfaces --- .../org/team1540/rooster/functional/package-info.java | 10 ++++++++++ 1 file changed, 10 insertions(+) create mode 100644 lib/src/main/java/org/team1540/rooster/functional/package-info.java diff --git a/lib/src/main/java/org/team1540/rooster/functional/package-info.java b/lib/src/main/java/org/team1540/rooster/functional/package-info.java new file mode 100644 index 00000000..f7d341d6 --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/functional/package-info.java @@ -0,0 +1,10 @@ +/** + * Extensions and additions to java's {@linkplain java.util.function functional interfaces}. The + * extensions of common standard library functional interfaces contained in this package (namely + * {@link org.team1540.rooster.functional.Input}, {@link org.team1540.rooster.functional.Processor}, + * and {@link org.team1540.rooster.functional.Output}) add some default methods to simplify chaining + * (especially in the scenario of constructing a {@linkplain org.team1540.rooster.drive.pipeline + * drive pipeline}). This package also contains the common {@link org.team1540.rooster.functional.Executable} + * functional interface for a function that takes no arguments and returns no result. + */ +package org.team1540.rooster.functional; From 349843bcb90d989184a1ad20da1ac16e11c1ba0b Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Thu, 3 Jan 2019 21:05:27 -0800 Subject: [PATCH 50/51] Fix missing doc --- .../rooster/drive/pipeline/AdvancedArcadeJoystickInput.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java index 0e64a1ed..5ae80848 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java @@ -40,6 +40,7 @@ public class AdvancedArcadeJoystickInput implements Input { * @param softTurnInput A {@link DoubleSupplier} that supplies the input for the soft-turn, from * -1 (full left) to 1 (full right) inclusive. * @param hardTurnInput A {@link DoubleSupplier} that supplies the input for the soft-turn, from + * -1 to 1 inclusive. */ public AdvancedArcadeJoystickInput(@NotNull DoubleSupplier throttleInput, @NotNull DoubleSupplier softTurnInput, @@ -57,6 +58,7 @@ public AdvancedArcadeJoystickInput(@NotNull DoubleSupplier throttleInput, * @param softTurnInput A {@link DoubleSupplier} that supplies the input for the soft-turn, from * -1 (full left) to 1 (full right) inclusive. * @param hardTurnInput A {@link DoubleSupplier} that supplies the input for the soft-turn, from + * -1 to 1 inclusive. */ public AdvancedArcadeJoystickInput(boolean reverseBackwards, @NotNull DoubleSupplier throttleInput, From d5176fa15dded132d8272b3f8e696816775a634b Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 7 Jan 2019 19:45:25 -0800 Subject: [PATCH 51/51] Make name-release-files.sh executable --- travis-scripts/name-release-files.sh | 0 1 file changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 travis-scripts/name-release-files.sh diff --git a/travis-scripts/name-release-files.sh b/travis-scripts/name-release-files.sh old mode 100644 new mode 100755