From 74fd100faef98ea5c88365428fa9e2cf2b23e400 Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Fri, 10 Oct 2025 21:46:39 -0700 Subject: [PATCH 001/118] Remove entire robot/lib/swerve folder and replace all references with subsystem/ctre/CtreDriveConstants --- src/main/java/frc/robot/Constants.java | 16 +- .../swerve/COTSTalonFXSwerveConstants.java | 311 ------------------ .../frc/robot/lib/swerve/DriveController.java | 32 -- .../swerve/PIDHolonomicDriveController.java | 150 --------- .../ProfiledPIDHolonomicDriveController.java | 149 --------- .../lib/swerve/SwerveModuleConstants.java | 28 -- .../drive/ctre/CtreDriveConstants.java | 6 +- 7 files changed, 12 insertions(+), 680 deletions(-) delete mode 100644 src/main/java/frc/robot/lib/swerve/COTSTalonFXSwerveConstants.java delete mode 100644 src/main/java/frc/robot/lib/swerve/DriveController.java delete mode 100644 src/main/java/frc/robot/lib/swerve/PIDHolonomicDriveController.java delete mode 100644 src/main/java/frc/robot/lib/swerve/ProfiledPIDHolonomicDriveController.java delete mode 100644 src/main/java/frc/robot/lib/swerve/SwerveModuleConstants.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 23f1b86..d8daa75 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,6 +1,7 @@ package frc.robot; import static edu.wpi.first.units.Units.DegreesPerSecond; +import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Seconds; @@ -28,7 +29,8 @@ import edu.wpi.first.wpilibj.DriverStation; import frc.robot.lib.control.ControlConstants.PIDFConstants; import frc.robot.lib.control.ControlConstants.ProfiledPIDFConstants; -import frc.robot.lib.swerve.COTSTalonFXSwerveConstants; +// import frc.robot.lib.swerve.COTSTalonFXSwerveConstants; +import frc.robot.subsystems.drive.ctre.CtreDriveConstants; /** * All constants belong here. @@ -55,13 +57,13 @@ public static final class Odometry { public static final class Drive { - public static final COTSTalonFXSwerveConstants SWERVE_MODULE_TYPE = - COTSTalonFXSwerveConstants.SDS.MK4i.KrakenX60(COTSTalonFXSwerveConstants.SDS.MK4i.driveRatios.L3); + // public static final COTSTalonFXSwerveConstants SWERVE_MODULE_TYPE = + // COTSTalonFXSwerveConstants.SDS.MK4i.KrakenX60(COTSTalonFXSwerveConstants.SDS.MK4i.driveRatios.L3); public static final double TRACK_WIDTH = Units.Inches.of(24).in(Units.Meters); public static final double WHEEL_BASE = Units.Inches.of(24).in(Units.Meters); - public static final double WHEEL_CIRCUMFERENCE = SWERVE_MODULE_TYPE.wheelCircumference; - public static final double WHEEL_DIAMETER = SWERVE_MODULE_TYPE.wheelDiameter; + public static final double WHEEL_DIAMETER = 2 * CtreDriveConstants.kWheelRadius.in(Meters);//SWERVE_MODULE_TYPE.wheelDiameter; + public static final double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER*Math.PI;//SWERVE_MODULE_TYPE.wheelCircumference; public static final Translation2d[] MODULE_LOCATIONS = { new Translation2d(-WHEEL_BASE / 2.0, TRACK_WIDTH / 2.0), @@ -70,8 +72,8 @@ public static final class Drive { new Translation2d(WHEEL_BASE / 2.0, -TRACK_WIDTH / 2.0) }; - public static final double DRIVE_GEAR_RATIO = SWERVE_MODULE_TYPE.driveGearRatio; - public static final double ANGLE_GEAR_RATIO = SWERVE_MODULE_TYPE.angleGearRatio; + public static final double DRIVE_GEAR_RATIO = CtreDriveConstants.kDriveGearRatio;//SWERVE_MODULE_TYPE.driveGearRatio; + public static final double ANGLE_GEAR_RATIO = CtreDriveConstants.kSteerGearRatio;//SWERVE_MODULE_TYPE.angleGearRatio; public static final double MAX_SPEED = 5.0; public static final double MAX_ACCEL = 5.0; diff --git a/src/main/java/frc/robot/lib/swerve/COTSTalonFXSwerveConstants.java b/src/main/java/frc/robot/lib/swerve/COTSTalonFXSwerveConstants.java deleted file mode 100644 index e6d3381..0000000 --- a/src/main/java/frc/robot/lib/swerve/COTSTalonFXSwerveConstants.java +++ /dev/null @@ -1,311 +0,0 @@ -package frc.robot.lib.swerve; - -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; - -import edu.wpi.first.math.util.Units; - -/** Contains values and required settings for common COTS swerve modules. */ -public class COTSTalonFXSwerveConstants { - public final double wheelDiameter; - public final double wheelCircumference; - public final double angleGearRatio; - public final double driveGearRatio; - public final double angleKP; - public final double angleKI; - public final double angleKD; - public final InvertedValue driveMotorInvert; - public final InvertedValue angleMotorInvert; - public final SensorDirectionValue cancoderInvert; - - public COTSTalonFXSwerveConstants(double wheelDiameter, double angleGearRatio, double driveGearRatio, double angleKP, double angleKI, double angleKD, InvertedValue driveMotorInvert, InvertedValue angleMotorInvert, SensorDirectionValue cancoderInvert){ - this.wheelDiameter = wheelDiameter; - this.wheelCircumference = wheelDiameter * Math.PI; - this.angleGearRatio = angleGearRatio; - this.driveGearRatio = driveGearRatio; - this.angleKP = angleKP; - this.angleKI = angleKI; - this.angleKD = angleKD; - this.driveMotorInvert = driveMotorInvert; - this.angleMotorInvert = angleMotorInvert; - this.cancoderInvert = cancoderInvert; - } - - /** West Coast Products */ - public static final class WCP { - /** West Coast Products - SwerveX Standard*/ - public static final class SwerveXStandard{ - /** West Coast Products - SwerveX Standard (Falcon 500)*/ - public static final COTSTalonFXSwerveConstants Falcon500(double driveGearRatio) { - double wheelDiameter = Units.inchesToMeters(4.0); - - /** (396 / 35) : 1 */ - double angleGearRatio = ((396.0 / 35.0) / 1.0); - - double angleKP = 1.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - /** West Coast Products - SwerveX Standard (Kraken X60)*/ - public static final COTSTalonFXSwerveConstants KrakenX60(double driveGearRatio) { - double wheelDiameter = Units.inchesToMeters(4.0); - - /** (396 / 35) : 1 */ - double angleGearRatio = ((150.0/7.0) / 1.0); - - double angleKP = 1.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - public static final class driveRatios{ - /** WCP SwerveX Standard X1 - 10 Tooth - (7.85 : 1) */ - public static final double X1_10 = (7.85 / 1.0); - - /** WCP SwerveX Standard X1 - 11 Tooth - (7.13 : 1) */ - public static final double X1_11 = (7.13 / 1.0); - - /** WCP SwerveX Standard X1 - 12 Tooth - (6.54 : 1) */ - public static final double X1_12 = (6.54 / 1.0); - - /** WCP SwerveX Standard X2 - 10 Tooth - (6.56 : 1) */ - public static final double X2_10 = (6.56 / 1.0); - - /** WCP SwerveX Standard X2 - 11 Tooth - (5.96 : 1) */ - public static final double X2_11 = (5.96 / 1.0); - - /** WCP SwerveX Standard X2 - 12 Tooth - (5.46 : 1) */ - public static final double X2_12 = (5.46 / 1.0); - - /** WCP SwerveX Standard X3 - 12 Tooth - (5.14 : 1) */ - public static final double X3_12 = (5.14 / 1.0); - - /** WCP SwerveX Standard X3 - 13 Tooth - (4.75 : 1) */ - public static final double X3_13 = (4.75 / 1.0); - - /** WCP SwerveX Standard X3 - 14 Tooth - (4.41 : 1) */ - public static final double X3_14 = (4.41 / 1.0); - } - } - - /** West Coast Products - SwerveX Flipped*/ - public static final class SwerveXFlipped{ - /** West Coast Products - SwerveX Flipped (Falcon 500)*/ - public static final COTSTalonFXSwerveConstants Falcon500(double driveGearRatio) { - double wheelDiameter = Units.inchesToMeters(4.0); - - /** (468 / 35) : 1 */ - double angleGearRatio = ((468.0 / 35.0) / 1.0); - - double angleKP = 1.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - /** West Coast Products - SwerveX Flipped (Kraken X60)*/ - public static final COTSTalonFXSwerveConstants KrakenX60(double driveGearRatio) { - double wheelDiameter = Units.inchesToMeters(4.0); - - /** (468 / 35) : 1 */ - double angleGearRatio = ((468.0 / 35.0) / 1.0); - - double angleKP = 1.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - public static final class driveRatios{ - /** WCP SwerveX Flipped X1 - 10 Tooth - (8.10 : 1) */ - public static final double X1_10 = (8.10 / 1.0); - - /** WCP SwerveX Flipped X1 - 11 Tooth - (7.36 : 1) */ - public static final double X1_11 = (7.36 / 1.0); - - /** WCP SwerveX Flipped X1 - 12 Tooth - (6.75 : 1) */ - public static final double X1_12 = (6.75 / 1.0); - - /** WCP SwerveX Flipped X2 - 10 Tooth - (6.72 : 1) */ - public static final double X2_10 = (6.72 / 1.0); - - /** WCP SwerveX Flipped X2 - 11 Tooth - (6.11 : 1) */ - public static final double X2_11 = (6.11 / 1.0); - - /** WCP SwerveX Flipped X2 - 12 Tooth - (5.60 : 1) */ - public static final double X2_12 = (5.60 / 1.0); - - /** WCP SwerveX Flipped X3 - 10 Tooth - (5.51 : 1) */ - public static final double X3_10 = (5.51 / 1.0); - - /** WCP SwerveX Flipped X3 - 11 Tooth - (5.01 : 1) */ - public static final double X3_11 = (5.01 / 1.0); - - /** WCP SwerveX Flipped X3 - 12 Tooth - (4.59 : 1) */ - public static final double X3_12 = (4.59 / 1.0); - } - } - } - - /** Swerve Drive Specialities */ - public static final class SDS { - /** Swerve Drive Specialties - MK3 Module*/ - public static final class MK3{ - /** Swerve Drive Specialties - MK3 Module (Falcon 500)*/ - public static final COTSTalonFXSwerveConstants Falcon500(double driveGearRatio) { - double wheelDiameter = Units.inchesToMeters(4.0); - - /** 12.8 : 1 */ - double angleGearRatio = (12.8 / 1.0); - - double angleKP = 1.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.CounterClockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - /** Swerve Drive Specialties - MK3 Module (Kraken X60)*/ - public static final COTSTalonFXSwerveConstants KrakenX60(double driveGearRatio) { - double wheelDiameter = Units.inchesToMeters(4.0); - - /** 12.8 : 1 */ - double angleGearRatio = (12.8 / 1.0); - - double angleKP = 1.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.CounterClockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - public static final class driveRatios{ - /** SDS MK3 - (8.16 : 1) */ - public static final double Standard = (8.16 / 1.0); - /** SDS MK3 - (6.86 : 1) */ - public static final double Fast = (6.86 / 1.0); - } - } - - /** Swerve Drive Specialties - MK4 Module*/ - public static final class MK4{ - /** Swerve Drive Specialties - MK4 Module (Falcon 500)*/ - public static final COTSTalonFXSwerveConstants Falcon500(double driveGearRatio) { - double wheelDiameter = Units.inchesToMeters(4.0); - - /** 12.8 : 1 */ - double angleGearRatio = (12.8 / 1.0); - - double angleKP = 1.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.CounterClockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - /** Swerve Drive Specialties - MK4 Module (Kraken X60)*/ - public static final COTSTalonFXSwerveConstants KrakenX60(double driveGearRatio) { - double wheelDiameter = Units.inchesToMeters(4.0); - - /** 12.8 : 1 */ - double angleGearRatio = ((150.0/7.0) / 1.0); - - double angleKP = 1.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.CounterClockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - public static final class driveRatios{ - /** SDS MK4 - (8.14 : 1) */ - public static final double L1 = (8.14 / 1.0); - /** SDS MK4 - (6.75 : 1) */ - public static final double L2 = (6.75 / 1.0); - /** SDS MK4 - (6.12 : 1) */ - public static final double L3 = (6.12 / 1.0); - /** SDS MK4 - (5.14 : 1) */ - public static final double L4 = (5.14 / 1.0); - } - } - - /** Swerve Drive Specialties - MK4i Module*/ - public static final class MK4i{ - /** Swerve Drive Specialties - MK4i Module (Falcon 500)*/ - public static final COTSTalonFXSwerveConstants Falcon500(double driveGearRatio) { - double wheelDiameter = Units.inchesToMeters(4.0); - - /** (150 / 7) : 1 */ - double angleGearRatio = ((150.0 / 7.0) / 1.0); - - double angleKP = 100.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - /** Swerve Drive Specialties - MK4i Module (Kraken X60)*/ - public static final COTSTalonFXSwerveConstants KrakenX60(double driveGearRatio) { - double wheelDiameter = Units.inchesToMeters(4.0); - - /** (150 / 7) : 1 */ - double angleGearRatio = ((150.0 / 7.0) / 1.0); - - double angleKP = 9.0; // Change to correct speed - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.Clockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - public static final class driveRatios{ - /** SDS MK4i - (8.14 : 1) */ - public static final double L1 = (8.14 / 1.0); - /** SDS MK4i - (6.75 : 1) */ - public static final double L2 = (6.75 / 1.0); - /** SDS MK4i - (6.12 : 1) */ - public static final double L3 = (6.12 / 1.0); - } - } - } -} - - \ No newline at end of file diff --git a/src/main/java/frc/robot/lib/swerve/DriveController.java b/src/main/java/frc/robot/lib/swerve/DriveController.java deleted file mode 100644 index 3409f97..0000000 --- a/src/main/java/frc/robot/lib/swerve/DriveController.java +++ /dev/null @@ -1,32 +0,0 @@ -package frc.robot.lib.swerve; - -import frc.robot.lib.trajectory.RedTrajectory; - -/** - * An interface for a drive controller which traverses a trajectory. - * It takes the current speed and position and returns the calculated {@code ChassisSpeeds}. - */ -@Deprecated -public interface DriveController { - /** - * Sets the trajectory. - * @param trajectory The trajectory to traverse. - */ - public void setTrajectory(RedTrajectory trajectory); - - /** - * Whether the traversal is done. - * @return Whether the traversal is done. - */ - public boolean isDone(); - - /** - * Resets the drive controller. - */ - public void reset(); - - /** - * Gets the current trajectory. - */ - public RedTrajectory getTrajectory(); -} diff --git a/src/main/java/frc/robot/lib/swerve/PIDHolonomicDriveController.java b/src/main/java/frc/robot/lib/swerve/PIDHolonomicDriveController.java deleted file mode 100644 index bcd76fa..0000000 --- a/src/main/java/frc/robot/lib/swerve/PIDHolonomicDriveController.java +++ /dev/null @@ -1,150 +0,0 @@ -package frc.robot.lib.swerve; - -import frc.robot.lib.control.ControlConstants.*; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.Constants; -import frc.robot.lib.control.PIDVController; -import frc.robot.lib.control.ProfiledPIDVController; -import frc.robot.lib.trajectory.RedTrajectory; - -@Deprecated -public class PIDHolonomicDriveController implements DriveController { - private final PIDVController xController; - private final PIDVController yController; - private final ProfiledPIDVController thetaController; - private double accelConstant; - - private RedTrajectory trajectory; - private Pose2d currentPose; - private ChassisSpeeds currentSpeeds; - - private Timer timer = null; - - static Field2d field = new Field2d(); - static { - SmartDashboard.putData("debug", field); - } - - /** - * A drive controller that works with 2 {@link PIDVController}s for translation and one {@link ProfiledPIDVController} for rotation. - * @param translationConstants The {@link PIDFConstants} for the translation of the robot. - * @param rotationConstants The {@link ProfiledPIDFConstants} for the rotation of the robot. - * @param accelConstant The acceleration feedforwards (useful for traversing sharp turns on a trajectory). - */ - public PIDHolonomicDriveController(PIDFConstants translationConstants, ProfiledPIDFConstants rotationConstants, double accelConstant) { - xController = new PIDVController(translationConstants); - yController = new PIDVController(translationConstants); - thetaController = new ProfiledPIDVController(rotationConstants); - thetaController.enableContinuousInput(-Math.PI, Math.PI); - this.accelConstant = accelConstant; - - timer = new Timer(); - } - - @Override - public void setTrajectory(RedTrajectory trajectory) { - this.trajectory = trajectory; - timer.reset(); - timer.start(); - } - - public void setRobotState(Pose2d pose, Twist2d speeds) { - this.currentPose = pose; - this.currentSpeeds = new ChassisSpeeds(speeds.dx, speeds.dy, speeds.dtheta); - } - - public void setInput(Pair current) { - setRobotState(current.getFirst(), current.getSecond()); - } - - public ChassisSpeeds getOutput() { - if (trajectory == null || currentPose == null || currentSpeeds == null || trajectory.isDone()) { - return new ChassisSpeeds(); - } - - RedTrajectory.State targetState = trajectory.advanceTo(timer.get()); - - double vxFF = targetState.speeds.vxMetersPerSecond; - double vyFF = targetState.speeds.vyMetersPerSecond; - - double xAccelFF = MathUtil.applyDeadband( - targetState.accels.ax, - Constants.Drive.MAX_ACCEL * 0.5); - double yAccelFF = MathUtil.applyDeadband( - targetState.accels.ay, - Constants.Drive.MAX_ACCEL * 0.5); - - double angularAccel = MathUtil.applyDeadband( - targetState.accels.alpha, - Constants.Drive.MAX_ROTATION_ACCEL * 0.5); - xAccelFF += -angularAccel * targetState.pose.getRotation().getSin(); - yAccelFF += angularAccel * targetState.pose.getRotation().getCos(); - - xController.setTarget(targetState.pose.getX()); - yController.setTarget(targetState.pose.getY()); - - xController.setFeedforward(vxFF); - yController.setFeedforward(vyFF); - - xController.setInput(new Pair(currentPose.getX(), currentSpeeds.vxMetersPerSecond)); - yController.setInput(new Pair(currentPose.getY(), currentSpeeds.vyMetersPerSecond)); - - double vx = xController.getOutput(); - double vy = yController.getOutput(); - - thetaController.setTarget(targetState.pose.getRotation().getRadians()); - thetaController.setFeedforward(targetState.speeds.omegaRadiansPerSecond); - thetaController.setInput( - new Pair( - currentPose.getRotation().getRadians(), currentSpeeds.omegaRadiansPerSecond)); - - double rotation = thetaController.getOutput(); - - field.setRobotPose(targetState.pose); - SmartDashboard.putData("debug", field); - - return ChassisSpeeds.fromFieldRelativeSpeeds( - vx + xAccelFF * accelConstant, - vy + yAccelFF * accelConstant, - rotation, - currentPose.getRotation()); - } - - @Override - public boolean isDone() { - if (trajectory == null) return true; - if (trajectory.isDone()) { - System.out.println("Done with trajectory, error: " + Math.hypot(xController.error, yController.error)); - return true; - } - return false; - } - - @Override - public void reset() { - xController.reset(); - yController.reset(); - thetaController.reset(); - - trajectory = null; - currentPose = null; - currentSpeeds = null; - - if (timer != null) { - timer.stop(); - timer.reset(); - } - } - - @Override - public RedTrajectory getTrajectory() { - return trajectory; - } -} diff --git a/src/main/java/frc/robot/lib/swerve/ProfiledPIDHolonomicDriveController.java b/src/main/java/frc/robot/lib/swerve/ProfiledPIDHolonomicDriveController.java deleted file mode 100644 index 354dfc2..0000000 --- a/src/main/java/frc/robot/lib/swerve/ProfiledPIDHolonomicDriveController.java +++ /dev/null @@ -1,149 +0,0 @@ -package frc.robot.lib.swerve; - -import frc.robot.lib.control.ControlConstants.*; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.Constants; -import frc.robot.lib.control.PIDVController; -import frc.robot.lib.control.ProfiledPIDVController; -import frc.robot.lib.trajectory.RedTrajectory; - -@Deprecated -public class ProfiledPIDHolonomicDriveController implements DriveController { - private final ProfiledPIDVController xController; - private final ProfiledPIDVController yController; - private final ProfiledPIDVController thetaController; - private double accelConstant; - - private RedTrajectory trajectory; - private Pose2d currentPose; - private ChassisSpeeds currentSpeeds; - - private Timer timer = null; - - static Field2d field = new Field2d(); - static { - SmartDashboard.putData("debug", field); - } - - /** - * A drive controller that works with 2 {@link PIDVController}s for translation and one {@link ProfiledPIDVController} for rotation. - * @param translationConstants The {@link PIDFConstants} for the translation of the robot. - * @param rotationConstants The {@link ProfiledPIDFConstants} for the rotation of the robot. - * @param accelConstant The acceleration feedforwards (useful for traversing sharp turns on a trajectory). - */ - public ProfiledPIDHolonomicDriveController(ProfiledPIDFConstants translationConstants, ProfiledPIDFConstants rotationConstants, double accelConstant) { - xController = new ProfiledPIDVController(translationConstants); - yController = new ProfiledPIDVController(translationConstants); - thetaController = new ProfiledPIDVController(rotationConstants); - thetaController.enableContinuousInput(-Math.PI, Math.PI); - this.accelConstant = accelConstant; - timer = new Timer(); - } - - @Override - public void setTrajectory(RedTrajectory trajectory) { - this.trajectory = trajectory; - timer.reset(); - timer.start(); - } - - public void setRobotState(Pose2d pose, Twist2d speeds) { - this.currentPose = pose; - this.currentSpeeds = new ChassisSpeeds(speeds.dx, speeds.dy, speeds.dtheta); - } - - public void setInput(Pair current) { - setRobotState(current.getFirst(), current.getSecond()); - } - - public ChassisSpeeds getOutput() { - if (trajectory == null || currentPose == null || currentSpeeds == null || trajectory.isDone()) { - return new ChassisSpeeds(); - } - - RedTrajectory.State targetState = trajectory.advanceTo(timer.get()); - - double vxFF = targetState.speeds.vxMetersPerSecond; - double vyFF = targetState.speeds.vyMetersPerSecond; - - double xAccelFF = MathUtil.applyDeadband( - targetState.accels.ax, - Constants.Drive.MAX_ACCEL * 0.5); - double yAccelFF = MathUtil.applyDeadband( - targetState.accels.ay, - Constants.Drive.MAX_ACCEL * 0.5); - - double angularAccel = MathUtil.applyDeadband( - targetState.accels.alpha, - Constants.Drive.MAX_ROTATION_ACCEL * 0.5); - xAccelFF += -angularAccel * targetState.pose.getRotation().getSin(); - yAccelFF += angularAccel * targetState.pose.getRotation().getCos(); - - xController.setTarget(targetState.pose.getX()); - yController.setTarget(targetState.pose.getY()); - - xController.setFeedforward(vxFF); - yController.setFeedforward(vyFF); - - xController.setInput(new Pair(currentPose.getX(), currentSpeeds.vxMetersPerSecond)); - yController.setInput(new Pair(currentPose.getY(), currentSpeeds.vyMetersPerSecond)); - - double vx = xController.getOutput(); - double vy = yController.getOutput(); - - thetaController.setTarget(targetState.pose.getRotation().getRadians()); - thetaController.setFeedforward(targetState.speeds.omegaRadiansPerSecond); - thetaController.setInput( - new Pair( - currentPose.getRotation().getRadians(), currentSpeeds.omegaRadiansPerSecond)); - - double rotation = thetaController.getOutput(); - - field.setRobotPose(targetState.pose); - SmartDashboard.putData("debug", field); - - return ChassisSpeeds.fromFieldRelativeSpeeds( - vx + xAccelFF * accelConstant, - vy + yAccelFF * accelConstant, - rotation, - currentPose.getRotation()); - } - - @Override - public boolean isDone() { - if (trajectory == null) return true; - if (trajectory.isDone()) { - System.out.println("Done with trajectory, error: " + Math.hypot(xController.error, yController.error)); - return true; - } - return false; - } - - @Override - public void reset() { - xController.reset(); - yController.reset(); - thetaController.reset(); - - trajectory = null; - currentPose = null; - currentSpeeds = null; - - if (timer != null) { - timer.stop(); - timer.reset(); - } - } - - @Override - public RedTrajectory getTrajectory() { - return trajectory; - } -} diff --git a/src/main/java/frc/robot/lib/swerve/SwerveModuleConstants.java b/src/main/java/frc/robot/lib/swerve/SwerveModuleConstants.java deleted file mode 100644 index 70ab187..0000000 --- a/src/main/java/frc/robot/lib/swerve/SwerveModuleConstants.java +++ /dev/null @@ -1,28 +0,0 @@ -package frc.robot.lib.swerve; - -import edu.wpi.first.math.geometry.Rotation2d; - -public class SwerveModuleConstants { - public final int driveMotorID; - public final int angleMotorID; - public final int cancoderID; - public final Rotation2d angleOffset; - public final boolean driveInvert; - public final boolean angleInvert; - - /** - * Swerve Module Constants to be used when creating swerve modules. - * @param driveMotorID - * @param angleMotorID - * @param canCoderID - * @param angleOffset - */ - public SwerveModuleConstants(int driveMotorID, int angleMotorID, int canCoderID, Rotation2d angleOffset, boolean driveInvert, boolean angleInvert) { - this.driveMotorID = driveMotorID; - this.angleMotorID = angleMotorID; - this.cancoderID = canCoderID; - this.angleOffset = angleOffset; - this.driveInvert = driveInvert; - this.angleInvert = angleInvert; - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java index 2d74a84..6de0650 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java @@ -84,9 +84,9 @@ public class CtreDriveConstants { // This may need to be tuned to your individual robot private static final double kCoupleRatio = 3.5714285714285716; - private static final double kDriveGearRatio = 6.122448979591837; - private static final double kSteerGearRatio = 21.428571428571427; - private static final Distance kWheelRadius = Inches.of(2); + public static final double kDriveGearRatio = 6.122448979591837; + public static final double kSteerGearRatio = 21.428571428571427; + public static final Distance kWheelRadius = Inches.of(2); private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; From 4e698d89a27a52b14454cd6345ee136cd70aee5b Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Fri, 10 Oct 2025 21:55:03 -0700 Subject: [PATCH 002/118] Remove WheelTracker, Pigeon, SwerveModule, and TalonFxManager from codebase. --- .../frc/robot/subsystems/TalonFXManager.java | 172 -------- .../frc/robot/subsystems/drive/Pigeon.java | 132 ------- .../robot/subsystems/drive/SwerveModule.java | 369 ------------------ .../robot/subsystems/drive/WheelTracker.java | 280 ------------- 4 files changed, 953 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/TalonFXManager.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/Pigeon.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/SwerveModule.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/WheelTracker.java diff --git a/src/main/java/frc/robot/subsystems/TalonFXManager.java b/src/main/java/frc/robot/subsystems/TalonFXManager.java deleted file mode 100644 index e92723b..0000000 --- a/src/main/java/frc/robot/subsystems/TalonFXManager.java +++ /dev/null @@ -1,172 +0,0 @@ -package frc.robot.subsystems; - -import java.util.Map; - -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import frc.robot.Constants; -import frc.robot.lib.drivers.Phoenix6Util; - -import java.util.HashMap; - -public class TalonFXManager { - /** - * Holds all created TalonFX motor instances, keyed by a unique name - */ - private static final Map motors = new HashMap<>(); - - /** - * Creates and registers a motor with the given name, CAN ID, and applies a named default config. - * @param name Unique identifier for this motor - * @param canId CAN device ID - * @param configKey Key of a default configuration to apply - * @return The newly created TalonFX instance - */ - public static TalonFX createMotor(String name, int canId, String configKey) { - if (motors.containsKey(name)) { - throw new IllegalArgumentException("Motor with name '" + name + "' already exists."); - } - TalonFX motor = new TalonFX(canId, "CV"); - TalonFXConfiguration config = DefaultConfigs.getConfig(configKey); - Phoenix6Util.checkErrorAndRetry(()-> - motor.getConfigurator().apply(config, Constants.LONG_CANT_TIMEOUT_MS)); - motors.put(name, motor); - return motor; - } - - /** - * Retrieves a previously registered motor by its name. - * @param name Unique identifier - * @return TalonFX instance - */ - public static TalonFX getMotor(String name) { - if (!motors.containsKey(name)) { - throw new IllegalArgumentException("No motor registered under name '" + name + "'."); - } - return motors.get(name); - } - - /** - * Applies one of the default configs to an existing motor. - * @param name Registered motor name - * @param configKey Key of the default configuration - */ - public static void applyConfig(String name, String configKey) { - TalonFX motor = getMotor(name); - TalonFXConfiguration config = DefaultConfigs.getConfig(configKey); - Phoenix6Util.checkErrorAndRetry(()-> - motor.getConfigurator().apply(config, Constants.LONG_CANT_TIMEOUT_MS)); - } - - /** - * Static container for default motor configurations. - */ - public static class DefaultConfigs { - private static final Map configs = new HashMap<>(); - - public static TalonFXConfiguration AngleMotorConfig () { - TalonFXConfiguration ANGLE_CONFIG = new TalonFXConfiguration(); - ANGLE_CONFIG.Slot0.kP = Constants.Drive.ANGLE_MOTOR_PIDF_CONSTANTS.kP; - ANGLE_CONFIG.Slot0.kI = Constants.Drive.ANGLE_MOTOR_PIDF_CONSTANTS.kI; - ANGLE_CONFIG.Slot0.kD = Constants.Drive.ANGLE_MOTOR_PIDF_CONSTANTS.kD; - ANGLE_CONFIG.Slot0.kS = 0.0; - ANGLE_CONFIG.Slot0.kV = Constants.Drive.ANGLE_MOTOR_PIDF_CONSTANTS.kF; - - ANGLE_CONFIG.CurrentLimits.StatorCurrentLimitEnable = true; - ANGLE_CONFIG.CurrentLimits.StatorCurrentLimit = Constants.Drive.ANGLE_CURRENT_LIMIT; - - ANGLE_CONFIG.CurrentLimits.SupplyCurrentLimitEnable = true; - ANGLE_CONFIG.CurrentLimits.SupplyCurrentLimit = Constants.Drive.ANGLE_CURRENT_LIMIT; - ANGLE_CONFIG.CurrentLimits.SupplyCurrentLowerLimit = Constants.Drive.ANGLE_CURRENT_THRESHOLD; - ANGLE_CONFIG.CurrentLimits.SupplyCurrentLowerTime = Constants.Drive.ANGLE_CURRENT_THRESHOLD_TIME; - - ANGLE_CONFIG.Voltage.PeakForwardVoltage = 12.0; - ANGLE_CONFIG.Voltage.PeakReverseVoltage = -12.0; - - ANGLE_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; -// configs.put("Angle", ANGLE_CONFIG); - return ANGLE_CONFIG; - } - - public static TalonFXConfiguration DriveMotorConfig(){ - TalonFXConfiguration DRIVE_CONFIG = new TalonFXConfiguration(); - - DRIVE_CONFIG.Slot0.kP = Constants.Drive.DRIVE_MOTOR_PIDF_CONSTANTS.kP; - DRIVE_CONFIG.Slot0.kI = Constants.Drive.DRIVE_MOTOR_PIDF_CONSTANTS.kI; - DRIVE_CONFIG.Slot0.kD = Constants.Drive.DRIVE_MOTOR_PIDF_CONSTANTS.kD; - DRIVE_CONFIG.Slot0.kS = 0.1; - DRIVE_CONFIG.Slot0.kV = Constants.Drive.DRIVE_MOTOR_PIDF_CONSTANTS.kF; - - DRIVE_CONFIG.CurrentLimits.StatorCurrentLimitEnable = true; - DRIVE_CONFIG.CurrentLimits.StatorCurrentLimit = Constants.Drive.DRIVE_CURRENT_LIMIT; - - DRIVE_CONFIG.CurrentLimits.SupplyCurrentLimitEnable = true; - DRIVE_CONFIG.CurrentLimits.SupplyCurrentLimit = Constants.Drive.DRIVE_CURRENT_LIMIT; - DRIVE_CONFIG.CurrentLimits.SupplyCurrentLowerLimit = Constants.Drive.DRIVE_CURRENT_THRESHOLD; - DRIVE_CONFIG.CurrentLimits.SupplyCurrentLowerTime = 0.5; - - DRIVE_CONFIG.Voltage.PeakForwardVoltage = 12.0; - DRIVE_CONFIG.Voltage.PeakReverseVoltage = -12.0; - - DRIVE_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; - - DRIVE_CONFIG.OpenLoopRamps.DutyCycleOpenLoopRampPeriod = Constants.Drive.OPEN_LOOP_RAMP; - DRIVE_CONFIG.OpenLoopRamps.VoltageOpenLoopRampPeriod = Constants.Drive.OPEN_LOOP_RAMP; - //configs.put("Drive", DRIVE_CONFIG); - return DRIVE_CONFIG; - } - { - TalonFXConfiguration DEFAULT_CONFIG = new TalonFXConfiguration(); - DEFAULT_CONFIG.Slot0.kP = Constants.Drive.ANGLE_MOTOR_PIDF_CONSTANTS.kP; - DEFAULT_CONFIG.Slot0.kI = Constants.Drive.ANGLE_MOTOR_PIDF_CONSTANTS.kI; - DEFAULT_CONFIG.Slot0.kD = Constants.Drive.ANGLE_MOTOR_PIDF_CONSTANTS.kD; - DEFAULT_CONFIG.Slot0.kS = 0.0; - DEFAULT_CONFIG.Slot0.kV = Constants.Drive.ANGLE_MOTOR_PIDF_CONSTANTS.kF; - - DEFAULT_CONFIG.CurrentLimits.StatorCurrentLimitEnable = true; - DEFAULT_CONFIG.CurrentLimits.StatorCurrentLimit = Constants.Drive.ANGLE_CURRENT_LIMIT; - - DEFAULT_CONFIG.CurrentLimits.SupplyCurrentLimitEnable = true; - DEFAULT_CONFIG.CurrentLimits.SupplyCurrentLimit = Constants.Drive.ANGLE_CURRENT_LIMIT; - DEFAULT_CONFIG.CurrentLimits.SupplyCurrentLowerLimit = Constants.Drive.ANGLE_CURRENT_THRESHOLD; - DEFAULT_CONFIG.CurrentLimits.SupplyCurrentLowerTime = Constants.Drive.ANGLE_CURRENT_THRESHOLD_TIME; - - DEFAULT_CONFIG.Voltage.PeakForwardVoltage = 12.0; - DEFAULT_CONFIG.Voltage.PeakReverseVoltage = -12.0; - - DEFAULT_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake; - - configs.put("Default", DEFAULT_CONFIG); - } - - /** - * Retrieves a named default config, or throws if not found. - */ - public static TalonFXConfiguration getConfig(String key) { - if (key.matches("Angle")){ - return AngleMotorConfig(); - }else if (key.matches("Drive")){ - return DriveMotorConfig(); - }else{ - return null; - } - // TalonFXConfiguration cfg = configs.get(key); - // if (cfg == null) { - // throw new IllegalArgumentException("No default config found for key '" + key + "'."); - // } - // return cfg; - } - - /** - * Allows registration of additional default configs at runtime. - */ - public static void registerConfig(String key, TalonFXConfiguration config) { - if (configs.containsKey(key)) { - throw new IllegalArgumentException("Config key '" + key + "' already registered."); - } - configs.put(key, config); - } - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/Pigeon.java b/src/main/java/frc/robot/subsystems/drive/Pigeon.java deleted file mode 100644 index e1d1327..0000000 --- a/src/main/java/frc/robot/subsystems/drive/Pigeon.java +++ /dev/null @@ -1,132 +0,0 @@ -package frc.robot.subsystems.drive; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.Pigeon2Configuration; -import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.sim.Pigeon2SimState; - -import frc.robot.Constants; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - - -public class Pigeon extends SubsystemBase { - private static Pigeon pigeonInstance; - public static Pigeon getInstance() { - if (pigeonInstance == null) { - pigeonInstance = new Pigeon(Constants.Port.PIGEON); - } - return pigeonInstance; - } - - private final Pigeon2 gyro; - - private boolean inverted = Constants.Drive.INVERT_GYRO; - private Rotation2d yawAdjustmentAngle = new Rotation2d(); - private Rotation2d rollAdjustmentAngle = new Rotation2d(); - private Rotation2d pitchAdjustmentAngle = new Rotation2d(); - - private double simAngularVelocity = 0.0; - - private Pigeon(Constants.Port constants) { - gyro = new Pigeon2(constants.id, constants.bus); - gyro.getConfigurator().apply(new Pigeon2Configuration()); - } - - public Rotation2d getYaw() { - Rotation2d angle = getUnadjustedYaw().rotateBy(yawAdjustmentAngle.unaryMinus()); - if (inverted) { - return angle.unaryMinus(); - } - return angle; - } - - public Rotation2d getRoll() { - return getUnadjustedRoll().rotateBy(rollAdjustmentAngle.unaryMinus()); - } - - public Rotation2d getPitch() { - return getUnadjustedPitch().rotateBy(pitchAdjustmentAngle.unaryMinus()).unaryMinus(); - } - - /** - * Sets the yaw register to read the specified value. - * - * @param angleDeg New yaw in degrees - */ - public void setYaw(double angleDeg) { - yawAdjustmentAngle = Rotation2d.fromDegrees(getYawStatusSignal().getValueAsDouble()) - .rotateBy(Rotation2d.fromDegrees(angleDeg).unaryMinus()); - } - - /** - * Sets the roll register to read the specified value. - * - * @param angleDeg New yaw in degrees - */ - public void setRoll(double angleDeg) { - rollAdjustmentAngle = - getUnadjustedRoll().rotateBy(Rotation2d.fromDegrees(angleDeg).unaryMinus()); - } - - /** - * Sets the roll register to read the specified value. - * - * @param angleDeg New yaw in degrees - */ - public void setPitch(double angleDeg) { - pitchAdjustmentAngle = - getUnadjustedPitch().rotateBy(Rotation2d.fromDegrees(angleDeg).unaryMinus()); - System.out.println("Reset gyro to " + getPitch().getDegrees()); - } - - public Rotation2d getUnadjustedYaw() { - return Rotation2d.fromDegrees( - BaseStatusSignal.getLatencyCompensatedValueAsDouble(getYawStatusSignal(), getRateStatusSignal())); - } - - public Rotation2d getUnadjustedPitch() { - return Rotation2d.fromDegrees(gyro.getRoll().getValueAsDouble()); - } - - public Rotation2d getUnadjustedRoll() { - return Rotation2d.fromDegrees(gyro.getPitch().getValueAsDouble()); - } - - public StatusSignal getYawStatusSignal() { - return gyro.getYaw(); - } - - public StatusSignal getRateStatusSignal() { - return gyro.getAngularVelocityZDevice(); - } - - public void setSimAngularVelocity(double angularVelocity) { - this.simAngularVelocity = angularVelocity; - } - - @Override - public void simulationPeriodic() { - Pigeon2SimState gyroSimState = gyro.getSimState(); - - gyroSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); - - Rotation2d angleChange = Rotation2d.fromRadians(simAngularVelocity * TimedRobot.kDefaultPeriod); - Rotation2d angle = getUnadjustedYaw().plus(angleChange); - gyroSimState.setRawYaw(angle.getDegrees()); - } - - @Override - public void initSendable(SendableBuilder builder) { - builder.setSmartDashboardType("Subsystem"); - builder.addDoubleProperty("/RollDegrees", this.getRoll()::getDegrees, null); - builder.addDoubleProperty("/PitchDegrees", this.getPitch()::getDegrees, null); - builder.addDoubleProperty("/YawDegrees", this.getYaw()::getDegrees, null); - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java deleted file mode 100644 index 09721a6..0000000 --- a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java +++ /dev/null @@ -1,369 +0,0 @@ -package frc.robot.subsystems.drive; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.controls.*; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.sim.CANcoderSimState; -import com.ctre.phoenix6.sim.TalonFXSimState; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.units.Units; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Constants; -import frc.robot.lib.util.Conversions; -import frc.robot.subsystems.TalonFXManager; - -public class SwerveModule extends SubsystemBase { - public final String name; - private final BaseStatusSignal[] signals = new BaseStatusSignal[4]; - - private final SwerveModuleIO io = new SwerveModuleIO(); - public static class SwerveModuleIO { - public double rotationPosition = 0.0; - public double rotationVelocity = 0.0; - public double drivePosition = 0.0; - public double driveVelocity = 0.0; - - SwerveModuleState currentState = new SwerveModuleState(); - SwerveModuleState targetState = new SwerveModuleState(); - double targetDriveVelocity = 0; - ControlRequest angleRequest = new NeutralOut(); - ControlRequest driveRequest = new NeutralOut(); - } - - private State state = State.DEFAULT; - public static enum State { - DEFAULT, - ANGLE_TUNING, - DRIVE_TUNING - } - - private final TalonFX angleMotor; - private final TalonFX driveMotor; - private final CANcoder cancoder; - - private final DCMotorSim angleMotorSim; - private final DCMotorSim driveMotorSim; - - public SwerveModule(Constants.Drive.ModuleConstants constants, CANcoder cancoder) { - name = constants.name(); - setName("Module " + name); - - angleMotor = TalonFXManager.createMotor(name + " Angle Motor", constants.angleMotorID, "Angle"); - driveMotor = TalonFXManager.createMotor(name + " Drive Motor", constants.driveMotorID, "Drive"); - - driveMotorSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX60Foc(1), 0.001, Constants.Drive.DRIVE_GEAR_RATIO), - DCMotor.getKrakenX60Foc(1)); - angleMotorSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX60Foc(1), 0.001, Constants.Drive.ANGLE_GEAR_RATIO), - DCMotor.getFalcon500Foc(1)); - - this.cancoder = cancoder; - - signals[0] = driveMotor.getRotorPosition(); - signals[1] = driveMotor.getRotorVelocity(); - signals[2] = angleMotor.getRotorPosition(); - signals[3] = angleMotor.getRotorVelocity(); - SmartDashboard.putData(this); - } - - /** - * Sets the target state of this module. - * @param state The target {@code SwerveModuleState}. - */ - public void setTargetState(SwerveModuleState state) { - io.targetState = state; - } - - @Override - public void periodic() { - switch (state) { - case DEFAULT: - setVelocity(io.targetState); - break; - case ANGLE_TUNING: - break; - case DRIVE_TUNING: - break; - default: - setVelocity(io.targetState); - break; - } - - angleMotor.setControl(io.angleRequest); - driveMotor.setControl(io.driveRequest); - - Rotation2d angleRotation = new Rotation2d(cancoder.getPosition().getValue()); - double wheelSpeed = Conversions.RPSToMPS( - driveMotor.getVelocity().getValueAsDouble(), - Constants.Drive.WHEEL_CIRCUMFERENCE, - Constants.Drive.DRIVE_GEAR_RATIO); - - io.currentState = new SwerveModuleState(wheelSpeed, angleRotation); - } - - @Override - public void simulationPeriodic() { - TalonFXSimState mDriveMotorSimState = driveMotor.getSimState(); - TalonFXSimState mAngleMotorSimState = angleMotor.getSimState(); - CANcoderSimState angleEncoderSimState = cancoder.getSimState(); - double batteryVoltage = RobotController.getBatteryVoltage(); - mDriveMotorSimState.setSupplyVoltage(batteryVoltage); - mAngleMotorSimState.setSupplyVoltage(batteryVoltage); - angleEncoderSimState.setSupplyVoltage(batteryVoltage); - - driveMotorSim.setInputVoltage(mDriveMotorSimState.getMotorVoltageMeasure().in(Units.Volts)); - driveMotorSim.update(TimedRobot.kDefaultPeriod); - mDriveMotorSimState.setRawRotorPosition( - driveMotorSim.getAngularPosition().times(Constants.Drive.DRIVE_GEAR_RATIO)); - mDriveMotorSimState.setRotorVelocity( - driveMotorSim.getAngularVelocity().times(Constants.Drive.DRIVE_GEAR_RATIO)); - - angleMotorSim.setInputVoltage(mAngleMotorSimState.getMotorVoltageMeasure().in(Units.Volts)); - angleMotorSim.update(Constants.DT); - mAngleMotorSimState.setRawRotorPosition( - angleMotorSim.getAngularPosition().times(Constants.Drive.ANGLE_GEAR_RATIO)); - mAngleMotorSimState.setRotorVelocity( - angleMotorSim.getAngularVelocity().times(Constants.Drive.ANGLE_GEAR_RATIO)); - - angleEncoderSimState.setRawPosition( - angleMotorSim.getAngularPosition()); - angleEncoderSimState.setVelocity( - angleMotorSim.getAngularVelocity()); - } - - /** - * Gets the current state of the module. - */ - public SwerveModuleState getState() { - return io.currentState; - } - - /** - * Sets the target velocities of the swerve module. - * @param targetState The target {@code SwerveModuleState}. - */ - public void setVelocity(SwerveModuleState targetState) { - boolean flip = setSteeringAngleOptimized(targetState.angle.unaryMinus()); - io.targetDriveVelocity = targetState.speedMetersPerSecond * (flip ? -1.0 : 1.0); - - double rotorSpeed = Conversions.MPSToRPS( - io.targetDriveVelocity, - Constants.Drive.WHEEL_CIRCUMFERENCE, - Constants.Drive.DRIVE_GEAR_RATIO); - - if (Math.abs(rotorSpeed) < 0.01) { - io.driveRequest = new NeutralOut(); - } else { - io.driveRequest = new VelocityVoltage(rotorSpeed); - } - } - - /** - * Sets the angle of the swerve module, and flips it if it is more efficient to. - * @param targetAngle The desire angle of the swerve module. - * @return Whether to flip the drive direction. - */ - private boolean setSteeringAngleOptimized(Rotation2d targetAngle) { - boolean flip = false; - - double currentAngleDegrees = cancoder.getPosition().getValueAsDouble() * 360.0; - Rotation2d currentAngle = Rotation2d.fromDegrees(currentAngleDegrees); - - Rotation2d delta = targetAngle.minus(currentAngle); - double deltaDegrees = delta.getDegrees(); - - if (deltaDegrees > 90.0) { - deltaDegrees -= 180.0; - flip = true; - } else if (deltaDegrees < -90.0) { - deltaDegrees += 180.0; - flip = true; - } - - setSteeringAngleRaw(currentAngleDegrees + deltaDegrees); - return flip; - } - - /** - * Sets the target angle position of the swerve module. - * @param angleDegrees The angle position, in degrees. - */ - private void setSteeringAngleRaw(double angleDegrees) { - double rotorPosition = Conversions.degreesToRotation(angleDegrees, Constants.Drive.ANGLE_GEAR_RATIO); - io.angleRequest = new PositionVoltage(rotorPosition); - } - - public synchronized void refreshSignals() { - io.rotationVelocity = angleMotor.getRotorVelocity().getValueAsDouble(); - io.driveVelocity = driveMotor.getRotorVelocity().getValueAsDouble(); - - io.rotationPosition = BaseStatusSignal.getLatencyCompensatedValueAsDouble( - angleMotor.getRotorPosition(), angleMotor.getRotorVelocity()); - io.drivePosition = BaseStatusSignal.getLatencyCompensatedValueAsDouble( - driveMotor.getRotorPosition(), driveMotor.getRotorVelocity()); - } - - /** - * Gets the distance the drive wheel has traversed. - * @return The distance, in meters. - */ - public double getDriveDistance() { - return Conversions.rotationsToMeters( - io.drivePosition, - Constants.Drive.WHEEL_CIRCUMFERENCE, - Constants.Drive.DRIVE_GEAR_RATIO); - } - - /** - * Gets the speed of the drive wheel; - * @return The speed, in meters per second. - */ - public double getDriveSpeed() { - return Conversions.RPSToMPS( - io.driveVelocity, - Constants.Drive.WHEEL_CIRCUMFERENCE, - Constants.Drive.DRIVE_GEAR_RATIO); - } - - /** - * Gets the current angle of the swerve module. - * @return The angle, as a {@code Rotation2d}. - */ - public Rotation2d getModuleAngle() { - return Rotation2d.fromDegrees(getModuleAngleDegrees()); - } - - /** - * Gets the current angle of the swerve module. - * @return The angle, in degrees. - */ - public double getModuleAngleDegrees() { - return Conversions.rotationsToDegrees(io.rotationPosition, Constants.Drive.ANGLE_GEAR_RATIO); - } - - /** - * Gets the speed of the angle. - * @return The speed, in degrees per second. - */ - public double getModuleAngleSpeed() { - return Units.RotationsPerSecond.of(io.rotationVelocity).in(Units.DegreesPerSecond); - } - - /** - * Retrieves the status signals. - * @return The status signals from this swerve module. - */ - public BaseStatusSignal[] getUsedStatusSignals() { - return signals; - } - - /** - * The system identification routine for the angle motor. - * @return The angle motor routine. - */ - public SysIdRoutine angleRoutine() { - return new SysIdRoutine( - new SysIdRoutine.Config( - Constants.Calibration.AngleMotor.RAMP_RATE, - Constants.Calibration.AngleMotor.DYNAMIC_VOLTAGE, - null, - recordState -> SignalLogger.writeString("/Sysid/Angle/State", recordState.toString()) - ), - new SysIdRoutine.Mechanism( - volts -> io.angleRequest = new VoltageOut(volts), null, this)); - } - - /** - * The system identification routine for the drive motor. - * @return The drive motor routine. - */ - public SysIdRoutine driveRoutine() { - return new SysIdRoutine( - new SysIdRoutine.Config( - Constants.Calibration.DriveMotor.RAMP_RATE, - Constants.Calibration.DriveMotor.DYNAMIC_VOLTAGE, - null, - recordState -> SignalLogger.writeString("/Sysid/Drive/State", recordState.toString()) - ), - new SysIdRoutine.Mechanism( - volts -> io.driveRequest = new VoltageOut(volts), null, this)); - } - - /** - * The command to run a sysid routine of the specified type and direction. - * @param type The type of routine (DRIVE or ANGLE). - * @param direction The direction to run in. - * @return The command. - */ - public Command sysIdDynamic(State type, SysIdRoutine.Direction direction) { - return switch (type) { - case ANGLE_TUNING -> - Commands.runOnce(() -> state = type) - .andThen(angleRoutine().dynamic(direction)) - .andThen(Commands.runOnce(() -> state = State.DEFAULT)); - case DRIVE_TUNING -> - Commands.runOnce(() -> state = type) - .andThen(driveRoutine().dynamic(direction)) - .andThen(Commands.runOnce(() -> state = State.DEFAULT)); - default -> { - DriverStation.reportWarning("Did not provide a valid tuning type", false); - yield Commands.none(); - } - }; - } - - /** - * The command to run a sysid routine of the specified type and direction. - * @param type The type of routine (DRIVE or ANGLE). - * @param direction The direction to run in. - * @return The command. - */ - public Command sysIdQuasistatic(State type, SysIdRoutine.Direction direction) { - return switch (type) { - case ANGLE_TUNING -> - Commands.runOnce(() -> state = type) - .andThen(Commands.runOnce( - () -> io.driveRequest = new NeutralOut())) - .andThen(angleRoutine().quasistatic(direction)) - .andThen(Commands.runOnce(() -> state = State.DEFAULT)); - case DRIVE_TUNING -> - Commands.runOnce(() -> state = type) - .andThen(Commands.runOnce( - () -> io.angleRequest = new NeutralOut())) - .andThen(driveRoutine().quasistatic(direction)) - .andThen(Commands.runOnce(() -> state = State.DEFAULT)); - default -> { - DriverStation.reportWarning("Did not provide a valid tuning type", false); - yield Commands.none(); - } - }; - } - - @Override - public void initSendable(SendableBuilder builder) { - builder.addStringProperty("/State", () -> state.name(), null); - builder.addDoubleProperty("/AngleDegrees", () -> getModuleAngleDegrees(), null); - builder.addDoubleProperty("/AngleSpeedDegreesPerSecond", () -> getModuleAngleSpeed(), null); - builder.addDoubleProperty("/DriveSpeedMetersPerSecond", () -> getDriveSpeed(), null); - builder.addDoubleProperty("/TargetAngleDegrees", () -> io.targetState.angle.getDegrees(), null); - builder.addDoubleProperty("/TargetDriveSpeedMetersPerSecond", () -> io.targetState.speedMetersPerSecond, null); - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/WheelTracker.java b/src/main/java/frc/robot/subsystems/drive/WheelTracker.java deleted file mode 100644 index 1240b9b..0000000 --- a/src/main/java/frc/robot/subsystems/drive/WheelTracker.java +++ /dev/null @@ -1,280 +0,0 @@ -package frc.robot.subsystems.drive; - -import com.ctre.phoenix6.BaseStatusSignal; -import frc.robot.Constants; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -import java.util.ArrayList; -import java.util.List; - -public class WheelTracker { - private final SwerveModule[] modules; - private final Pigeon pigeon; - - private WheelProperties[] wheelProperties = new WheelProperties[4]; - private Pose2d robotPose = new Pose2d(10,10,new Rotation2d(0)); - private Translation2d robotVelocity = new Translation2d(0, 0); - private BaseStatusSignal[] allSignals; - - private double robotHeading; - - private double timestamp; - - private final OdometryThread odometryThread; - - private Field2d robotField = new Field2d(); - - public WheelTracker(SwerveModule[] modules) { - if (modules.length != 4) { - throw new IllegalArgumentException("Odometry needs 4 modules to run"); - } - - this.modules = modules; - pigeon = Pigeon.getInstance(); - - for (int i = 0; i < wheelProperties.length; i++) { - WheelProperties w = new WheelProperties(); - Translation2d robotToWheel = new Translation2d( - Constants.Drive.MODULE_LOCATIONS[i].getX(), - Constants.Drive.MODULE_LOCATIONS[i].getY()); - w.startingPosition = robotToWheel; - wheelProperties[i] = w; - } - - resetModulePoses(); - - allSignals = new BaseStatusSignal[(4 * 4) + 2]; - for (int i = 0; i < 4; ++i) { - var signals = modules[i].getUsedStatusSignals(); - allSignals[(i * 4) + 0] = signals[0]; - allSignals[(i * 4) + 1] = signals[1]; - allSignals[(i * 4) + 2] = signals[2]; - allSignals[(i * 4) + 3] = signals[3]; - } - allSignals[allSignals.length - 2] = pigeon.getYawStatusSignal(); - allSignals[allSignals.length - 1] = pigeon.getRateStatusSignal(); - - for (BaseStatusSignal sig : allSignals) { - sig.setUpdateFrequency(50); - } - odometryThread = new OdometryThread(); - odometryThread.setDaemon(true); - odometryThread.start(); - - robotField.setRobotPose(robotPose); - - SmartDashboard.putData("WheelTracker", robotField); - } - private class OdometryThread extends Thread { - @Override - public void run() { - while (true) { - try { - BaseStatusSignal.waitForAll(1.0, allSignals); - - for (SwerveModule m : modules) { - m.refreshSignals(); // No downside to refreshing io reads from multiple threads - } - - robotHeading = pigeon.getYaw().getRadians(); - updateRobotPose(Timer.getFPGATimestamp()); - } catch (Exception e) { - e.printStackTrace(); - System.out.println("Wheeltracker failed, see above error"); - } - } - } - } - - private Pose2d last_velocity_sample = new Pose2d(); - private double last_sample_timestamp = 0.0; - - private void updateRobotPose(double timestamp) { - double x = 0.0; - double y = 0.0; - Rotation2d heading = Rotation2d.fromRadians(robotHeading); - - double avg_delta = 0.0; - double[] deltas = new double[4]; - for (int i = 0; i < modules.length; i++) { - SwerveModule m = modules[i]; - WheelProperties w = wheelProperties[i]; - updateWheelOdometry(m, w); - double delta = w.estimatedRobotPose - .getTranslation() - .plus(robotPose.getTranslation().unaryMinus()) - .getNorm(); - deltas[i] = delta; - avg_delta += delta; - } - avg_delta /= 4; - - int min__dev_idx = 0; - double min_dev = Double.MAX_VALUE; - List accurateModules = new ArrayList<>(); - for (int i = 0; i < deltas.length; i++) { - WheelProperties w = wheelProperties[i]; - double dev = Math.abs(deltas[i] - avg_delta); - if (dev < min_dev) { - min_dev = dev; - min__dev_idx = i; - } - if (dev <= 0.01) { - accurateModules.add(w); - } - } - - if (accurateModules.isEmpty()) { - accurateModules.add(wheelProperties[min__dev_idx]); - } - - int n = accurateModules.size(); - - for (WheelProperties w : accurateModules) { - x += w.estimatedRobotPose.getTranslation().getX(); - y += w.estimatedRobotPose.getTranslation().getY(); - } - final Pose2d new_pose = new Pose2d(new Translation2d(x / n, y / n), heading); - - // Velocity calcs - double sample_window = timestamp - last_sample_timestamp; - if (sample_window > 0.02) { - final Translation2d translation = (new_pose.transformBy(new Transform2d( - new Translation2d(-last_velocity_sample.getTranslation().getX(), - -last_velocity_sample.getTranslation().getY()), - last_velocity_sample.getRotation().unaryMinus())).getTranslation()); - robotVelocity = new Translation2d( - translation.getX() * (1.0 / sample_window), - translation.getY() * (1.0 / sample_window) - ); - last_sample_timestamp = timestamp; - last_velocity_sample = new_pose; - } - - robotPose = new_pose; - - robotField.setRobotPose(new_pose); - - resetModulePoses(robotPose); - } - - - private void updateWheelOdometry(SwerveModule module, WheelProperties props) { - double currentEncDistance = module.getDriveDistance(); - double deltaEncDistance = currentEncDistance - props.previousEncDistance; - Rotation2d wheelAngle = module.getModuleAngle().rotateBy(Rotation2d.fromRadians(-robotHeading)); - Translation2d deltaPosition = new Translation2d( - wheelAngle.getCos() * deltaEncDistance, - -wheelAngle.getSin() * deltaEncDistance); - double xCorrectionFactor = 1.0; - double yCorrectionFactor = 1.0; - - if (Math.signum(deltaPosition.getX()) == 1.0) { - xCorrectionFactor = (8.6 / 9.173); - // TODO: Put this in constants - - } else if (Math.signum(deltaPosition.getX()) == -1.0) { - xCorrectionFactor = (8.27 / 9.173); - } - - if (Math.signum(deltaPosition.getY()) == 1.0) { - yCorrectionFactor = (3.638 / 4.0); - } else if (Math.signum(deltaPosition.getY()) == -1.0) { - yCorrectionFactor = (3.660 / 4.0); - } - - deltaPosition = new Translation2d(deltaPosition.getX() * xCorrectionFactor, deltaPosition.getY() * yCorrectionFactor); - Translation2d updatedPosition = props.position.plus(deltaPosition); - Pose2d wheelPose = new Pose2d(updatedPosition, Rotation2d.fromRadians(robotHeading)); - props.estimatedRobotPose = new Pose2d(wheelPose.getTranslation().plus(props.startingPosition.unaryMinus()), wheelPose.getRotation()); - - props.position = updatedPosition; - props.previousEncDistance = currentEncDistance; - } - - public void resetModulePoses(Pose2d mRobotPose) { - for (int i = 0; i < modules.length; i++) { - WheelProperties props = wheelProperties[i]; - Translation2d modulePosition = new Pose2d(mRobotPose.getTranslation().plus(props.startingPosition),mRobotPose.getRotation()) - .getTranslation(); - props.position = modulePosition; - } - } - - private void resetModulePoses() { - for (int i = 0; i < modules.length; i++) { - WheelProperties props = wheelProperties[i]; - props.position = props.startingPosition; - } - } - - public synchronized void resetPose(Pose2d pose) { - robotPose = new Pose2d(pose.getTranslation(), pose.getRotation()); - resetModulePoses(robotPose); - } - - public class WheelProperties { - private double previousEncDistance = 0; - private Translation2d position; - private Translation2d startingPosition; - private Pose2d estimatedRobotPose = new Pose2d(); - } - - public synchronized Pose2d getRobotPose() { - return robotPose; - } - - public synchronized Translation2d getMeasuredVelocity() { - return robotVelocity; - } - - public double getTimestamp() { - return timestamp; - } - - public double wheel0_x() { - return wheelProperties[0].position.getX(); - } - - public double wheel0_y() { - return wheelProperties[0].position.getY(); - } - - public double wheel1_x() { - return wheelProperties[1].position.getX(); - } - - public double wheel1_y() { - return wheelProperties[1].position.getY(); - } - - public double wheel2_x() { - return wheelProperties[2].position.getX(); - } - - public double wheel2_y() { - return wheelProperties[2].position.getY(); - } - - public double wheel3_x() { - return wheelProperties[3].position.getX(); - } - - public double wheel3_y() { - return wheelProperties[3].position.getY(); - } - - public double robot_x() { - return robotPose.getTranslation().getX(); - } - - public double robot_y() { - return robotPose.getTranslation().getY(); - } -} From c208b63c48a2aa4d5d552f6213c8bce4327c3d28 Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Fri, 10 Oct 2025 22:49:15 -0700 Subject: [PATCH 003/118] Deprecate RobotState, CancoderManager, AutoPilotCommand and SnapCommand etc --- simgui-ds.json | 5 - src/main/java/frc/robot/Robot.java | 6 +- src/main/java/frc/robot/RobotState.java | 348 ------------------ .../robot/lib/trajectory/RedTrajectory.java | 9 +- .../frc/robot/subsystems/CancoderManager.java | 125 ------- .../drive/commands/AutopilotCommand.java | 88 ----- .../drive/commands/SnapCommand.java | 152 -------- .../drive/commands/TrajectoryCommand.java | 2 +- 8 files changed, 9 insertions(+), 726 deletions(-) delete mode 100644 src/main/java/frc/robot/RobotState.java delete mode 100644 src/main/java/frc/robot/subsystems/CancoderManager.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/commands/AutopilotCommand.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/commands/SnapCommand.java diff --git a/simgui-ds.json b/simgui-ds.json index 7efccf5..2296812 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -4,11 +4,6 @@ "visible": true } }, - "Keyboard 1 Settings": { - "window": { - "visible": true - } - }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6de2f9b..4b660fb 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -40,8 +40,8 @@ public Robot() { } - RobotState.reset(Timer.getFPGATimestamp(), new Pose2d()); - RobotState.resetKalman(); + // RobotState.reset(Timer.getFPGATimestamp(), new Pose2d()); + // RobotState.resetKalman(); Drive.getInstance(); VisionDeviceManager.getInstance(); @@ -81,7 +81,7 @@ public void disabledPeriodic() { /** This autonomous runs the autonomous command selected. */ @Override public void autonomousInit() { - RobotState.setAlliance(DriverStation.getAlliance()); + // RobotState.setAlliance(DriverStation.getAlliance()); autoCommand = autoChooser.getAuto(); if (autoCommand != null) { diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java deleted file mode 100644 index 12a01a1..0000000 --- a/src/main/java/frc/robot/RobotState.java +++ /dev/null @@ -1,348 +0,0 @@ -package frc.robot; - -import java.util.Map; -import java.util.Optional; - -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.Vector; -import edu.wpi.first.math.estimator.ExtendedKalmanFilter; -import edu.wpi.first.math.geometry.*; -import edu.wpi.first.math.numbers.*; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.math.Nat; -import edu.wpi.first.math.StateSpaceUtil; -import frc.robot.lib.localization.VisionPoseAcceptor; -import frc.robot.lib.util.MovingAverage; -import frc.robot.lib.util.Util; -import frc.robot.lib.util.interpolation.InterpolatingTreeMap; -import frc.robot.lib.util.interpolation.InterpolationUtil; -// import frc.robot.subsystems.drive.LegacyDrive; -import edu.wpi.first.math.interpolation.InverseInterpolator; - - -public class RobotState { - public static Optional mLatestVisionUpdate; - - private static Optional mInitialFieldToOdom = Optional.empty(); - private static InterpolatingTreeMap mOdometryToVehicle; - private static InterpolatingTreeMap mFieldToOdometry; - private static ExtendedKalmanFilter mKalmanFilter; - private static VisionPoseAcceptor mPoseAcceptor; - - private static Twist2d mVehicleVelocityMeasured; - private static Twist2d mVehichleVelocityPredicted; - private static MovingAverage mVehicleVelocityMeasuredFiltered; - - private static boolean mHasRecievedVisionUpdate = false; - private static boolean mIsInAuto = false; - private static Optional mAlliance = null; - - public static double mLastTimestamp = 0; - - /** - * Adds new odometry pose update. - * - * @param now Timestamp of observation. - * @param odometryPose Reported pose from odometry. - * @param measuredVelocity Measured field-relative velocity. - * @param predictedVelocity Predicted field-relative velocity (usually swerve - * setpoint). - */ - public static synchronized void reset(double now, Pose2d initialOdomToVehicle) { - mOdometryToVehicle = new InterpolatingTreeMap( - InverseInterpolator.forDouble(), - InterpolationUtil.poseInterpolator, - Constants.Odometry.OBSERVATION_BUFFER_SIZE); - mOdometryToVehicle.put(now, initialOdomToVehicle); - mFieldToOdometry = new InterpolatingTreeMap( - InverseInterpolator.forDouble(), - InterpolationUtil.translationInterpolator, - Constants.Odometry.OBSERVATION_BUFFER_SIZE); - mFieldToOdometry.put(now, getmInitialFieldToOdom()); - mVehicleVelocityMeasured = new Twist2d(); - mVehichleVelocityPredicted = new Twist2d(); - mVehicleVelocityMeasuredFiltered = new MovingAverage( - 25, new Twist2d(), - (Twist2d x, Twist2d y) -> { return new Twist2d(x.dx + y.dx, x.dy + y.dy, x.dtheta + y.dtheta); }, - (Twist2d x, Integer y) -> { return new Twist2d(x.dx / y, x.dy / y, x.dtheta / y); } - ); - - mLastTimestamp = now; - mLatestVisionUpdate = Optional.empty(); - mPoseAcceptor = new VisionPoseAcceptor(); - } - - /** - * Reconstructs Kalman Filter. - */ - public static synchronized void resetKalman() { - mKalmanFilter = new ExtendedKalmanFilter ( - Nat.N2(), // Dimensions of output (x, y) - Nat.N2(), // Dimensions of predicted error shift (dx, dy) (always 0) - Nat.N2(), // Dimensions of vision (x, y) - (x, u) -> u, // The derivative of the output is predicted shift (always 0) - (x, u) -> x, // The output is position (x, y) - Constants.Odometry.STATE_STD_DEVS, // Standard deviation of position (uncertainty propagation with no vision) - Constants.Odometry.LOCAL_MEASUREMENT_STD_DEVS, // Standard deviation of vision measurements - Constants.DT); - } - - public static synchronized void addOdometryUpdate( - double now, Pose2d odometryPose, Twist2d measuredVelocity, Twist2d predictedVelocity) { - mOdometryToVehicle.put(now, odometryPose); - mKalmanFilter.predict( - VecBuilder.fill(0.0, 0.0), Constants.DT); // Propagate error of current vision prediction - mVehicleVelocityMeasured = measuredVelocity; - mVehicleVelocityMeasuredFiltered.add(measuredVelocity); - mVehichleVelocityPredicted = predictedVelocity; - - mLastTimestamp = now; - } - - /** - * Adds new vision pose update. - * - * @param update Info about vision update. - */ - public static synchronized void addVisionUpdate(VisionUpdate update) { - // If it's the first update don't do filtering - if (mLatestVisionUpdate.isEmpty() || mInitialFieldToOdom.isEmpty()) { - double visionTimestamp = update.mTimestamp; - mLastTimestamp = update.mTimestamp; - Pose2d proximateDtPose = mOdometryToVehicle.get((visionTimestamp)); - Translation2d fieldToVision = update.mFieldToCamera - .plus(update.getmRobotToCamera() - .rotateBy(getLatestOdomToVehicle().getValue().getRotation()).unaryMinus()); - Translation2d odomToVehicleTranslation = proximateDtPose.getTranslation(); - Translation2d fieldToOdom = fieldToVision - .plus(odomToVehicleTranslation.unaryMinus()); - mFieldToOdometry.put((visionTimestamp), fieldToOdom); - mInitialFieldToOdom = Optional.of(mFieldToOdometry.lastEntry().getValue()); - mKalmanFilter.setXhat(0, fieldToOdom.getX()); - mKalmanFilter.setXhat(1, fieldToOdom.getY()); - mLatestVisionUpdate = Optional.ofNullable(update); - - // LegacyDrive.getInstance().setOdometry(new Pose2d(fieldToOdom, getLatestOdomToVehicle().getValue().getRotation())); - } else { - double visionTimestamp = mLatestVisionUpdate.get().mTimestamp; - mLastTimestamp = mLatestVisionUpdate.get().mTimestamp; - Pose2d proximateDtPose = mOdometryToVehicle.get((visionTimestamp)); - mLatestVisionUpdate = Optional.ofNullable(update); - Translation2d fieldToVision = mLatestVisionUpdate - .get().mFieldToCamera - .plus(mLatestVisionUpdate - .get() - .getmRobotToCamera() - .rotateBy(proximateDtPose.getRotation()).unaryMinus()); - - if (mPoseAcceptor.shouldAcceptVision( - visionTimestamp, - new Pose2d(fieldToVision, new Rotation2d()), - getLatestFieldToVehicle(), - mVehicleVelocityMeasured, - mIsInAuto)) { - Translation2d fieldToOdom = fieldToVision.plus( - proximateDtPose.getTranslation().unaryMinus()); - try { - Vector stdevs = update.mXyStdev; - mKalmanFilter.correct( - VecBuilder.fill(0.0, 0.0), - VecBuilder.fill( - fieldToOdom.getX(), - fieldToOdom.getY()), - StateSpaceUtil.makeCovarianceMatrix(Nat.N2(), stdevs)); - mFieldToOdometry.put( - (visionTimestamp), - new Translation2d(mKalmanFilter.getXhat(0), mKalmanFilter.getXhat(1))); - if (!getHasRecievedVisionUpdate()) { - mHasRecievedVisionUpdate = true; - } - } catch (Exception e) { - DriverStation.reportError(update.mXyStdev + "//QR Decomposition failed: ", e.getStackTrace()); - } - } - } - } - - /** - * Gets initial odometry error. Odometry initializes to the origin, eile the - * robot starts at an unknown position on the field. - * - * @return Initial odometry error translation. - */ - public static synchronized Translation2d getmInitialFieldToOdom() { - if (mInitialFieldToOdom.isEmpty()) - return new Translation2d(); - return mInitialFieldToOdom.get(); - } - - /** - * @return Latest field relative robot pose. - */ - public static synchronized Pose2d getLatestFieldToVehicle() { - Pose2d odomToVehicle = getLatestOdomToVehicle().getValue(); - - Translation2d fieldToOdom = getLatestFieldToOdom(); - return new Pose2d(Util.translateBy(fieldToOdom, odomToVehicle.getTranslation()), odomToVehicle.getRotation()); - } - - /** - * Gets field relative robot pose from history. Linearly interpolates between - * gaps. - * - * @param timestamp Timestamp to look up. - * @return Field relative robot pose at timestamp. - */ - public static synchronized Pose2d getFieldToVehicle(double timestamp) { - Pose2d odomToVehicle = getOdomToVehicle(timestamp); - - Translation2d fieldToOdom = getFieldToOdom(timestamp); - return new Pose2d(Util.translateBy(fieldToOdom, odomToVehicle.getTranslation()), odomToVehicle.getRotation()); - - } - - /** - * Gets interpolated robot pose using predicted robot velocity from latest - * odometry update. - * - * @param lookaheadTime Scalar for predicted velocity. - * @return Predcited robot pose at lookahead time. - */ - public static synchronized Pose2d getPredictedFieldToVehicle(double lookaheadTime) { - Pose2d odomToVehicle = getPredictedOdomToVehicle(lookaheadTime); - - Translation2d fieldToOdom = getLatestFieldToOdom(); - return new Pose2d(Util.translateBy(fieldToOdom, odomToVehicle.getTranslation()), odomToVehicle.getRotation()); - } - - /** - * @return Latest odometry pose. - */ - public static synchronized Map.Entry getLatestOdomToVehicle() { - return mOdometryToVehicle.lastEntry(); - } - - /** - * Gets odometry pose from history. Linearly interpolates between gaps. - * - * @param timestamp Timestamp to loop up. - * @return Odometry relative robot pose at timestamp. - */ - public static synchronized Pose2d getOdomToVehicle(double timestamp) { - return mOdometryToVehicle.get((timestamp)); - } - - /** - * Gets interpolated odometry pose using predicted robot velocity from latest - * odometry update. - * - * @param lookaheadTime Scalar for predicted velocity. - * @return Predcited odometry pose at lookahead time. - */ - public static synchronized Pose2d getPredictedOdomToVehicle(double lookaheadTime) { - return getLatestOdomToVehicle() - .getValue() - .transformBy(new Transform2d(new Pose2d(), - Util.expMap(Util.scaledTwist2d(mVehichleVelocityPredicted, lookaheadTime)))); - } - - /** - * @return Latest odometry error translation. - */ - public static synchronized Translation2d getLatestFieldToOdom() { - return getFieldToOdom(mFieldToOdometry.lastKey()); - } - - /** - * Gets odometry error translation at timestamp. Linearly interpolates between - * gaps. - * - * @param timestamp Timestamp to look up. - * @return Odometry error at timestamp. - */ - public static synchronized Translation2d getFieldToOdom(double timestamp) { - if (mFieldToOdometry.isEmpty()) - return new Translation2d(); - return mFieldToOdometry.get((timestamp)); - } - - /** - * @return Predicted robot velocity from last odometry update. - */ - public static synchronized Twist2d getPredictedVelocity() { - return mVehichleVelocityPredicted; - } - - /** - * @return Measured robot velocity from last odometry update. - */ - public static synchronized Twist2d getMeasuredVelocity() { - return mVehicleVelocityMeasured; - } - - /** - * @return Measured robot velocity smoothed using a moving average filter. - */ - public static synchronized Twist2d getSmoothedVelocity() { - return mVehicleVelocityMeasuredFiltered.getAverage(); - } - - /** - * @return Gets if estimator has recieved a vision update. - */ - public static synchronized boolean getHasRecievedVisionUpdate() { - return mHasRecievedVisionUpdate; - } - - /** - * Updates tracker to use stricter auto vision filtering. - * @param inAuto If auto filters should be used. - */ - public static synchronized void setIsInAuto(boolean inAuto) { - mIsInAuto = inAuto; - } - - /** - * Class to hold information about a vision update. - */ - public static class VisionUpdate { - private double mTimestamp; - private Translation2d mFieldToCamera; - private Translation2d mRobotToCamera; - private Vector mXyStdev; - - public VisionUpdate( - double timestamp, Translation2d fieldToCamera, Translation2d robotToCamera, Vector xyStdev) { - mTimestamp = timestamp; - mFieldToCamera = fieldToCamera; - mRobotToCamera = robotToCamera; - mXyStdev = xyStdev; - } - - public double getmTimestamp() { - return mTimestamp; - } - - public Translation2d getFieldToVehicle() { - return mFieldToCamera; - } - - public Translation2d getmRobotToCamera() { - return mRobotToCamera; - } - - public Vector getXYStdev() { - return mXyStdev; - } - } - - public static void setAlliance(Optional alliance) { - mAlliance = alliance; - System.out.println(alliance.toString()); - } - - public static Optional getAlliance() { - return mAlliance; - } -} diff --git a/src/main/java/frc/robot/lib/trajectory/RedTrajectory.java b/src/main/java/frc/robot/lib/trajectory/RedTrajectory.java index add8fbd..1875085 100644 --- a/src/main/java/frc/robot/lib/trajectory/RedTrajectory.java +++ b/src/main/java/frc/robot/lib/trajectory/RedTrajectory.java @@ -7,9 +7,10 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc.robot.Constants; -import frc.robot.RobotState; +// import frc.robot.RobotState; import frc.robot.lib.localization.FieldUtil; public class RedTrajectory { @@ -155,7 +156,7 @@ public static ChassisAccels estimate(ChassisSpeeds first, ChassisSpeeds second, public RedTrajectory(Trajectory traj, boolean flipForAlliance) { choreoTrajectory = traj; this.type = TrajectoryType.CHOREO; - this.flipped = flipForAlliance && RobotState.getAlliance().orElse(Alliance.Blue) == Alliance.Red; + this.flipped = flipForAlliance && DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red;//RobotState.getAlliance().orElse(Alliance.Blue) == Alliance.Red; this.name = "ChoreoTrajectory#" + traj.name(); } @@ -167,7 +168,7 @@ public RedTrajectory(Trajectory traj, boolean flipForAlliance) { public RedTrajectory(PathPlannerTrajectory traj, boolean flipForAlliance) { pathplannerTrajectory = traj; this.type = TrajectoryType.PATHPLANNER; - this.flipped = flipForAlliance && RobotState.getAlliance().orElse(Alliance.Blue) == Alliance.Red; + this.flipped = flipForAlliance && DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red;//RobotState.getAlliance().orElse(Alliance.Blue) == Alliance.Red; this.name = "PathPlannerTrajectory#" + traj.hashCode(); } @@ -179,7 +180,7 @@ public RedTrajectory(PathPlannerTrajectory traj, boolean flipForAlliance) { public RedTrajectory(PathPlannerTrajectory traj, boolean flipForAlliance, String name) { pathplannerTrajectory = traj; this.type = TrajectoryType.PATHPLANNER; - this.flipped = flipForAlliance && RobotState.getAlliance().orElse(Alliance.Blue) == Alliance.Red; + this.flipped = flipForAlliance && DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red;//RobotState.getAlliance().orElse(Alliance.Blue) == Alliance.Red; this.name = "PathPlannerTrajectory#" + name; } diff --git a/src/main/java/frc/robot/subsystems/CancoderManager.java b/src/main/java/frc/robot/subsystems/CancoderManager.java deleted file mode 100644 index bcd451b..0000000 --- a/src/main/java/frc/robot/subsystems/CancoderManager.java +++ /dev/null @@ -1,125 +0,0 @@ -package frc.robot.subsystems; - -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.CANcoderConfigurator; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.signals.SensorDirectionValue; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.wpilibj.Timer; -import frc.robot.Constants; - -import java.util.Optional; - -public class CancoderManager { - public static CancoderManager cancodersInstance; - public static CancoderManager getInstance() { - if (cancodersInstance == null) { - cancodersInstance = new CancoderManager(); - } - return cancodersInstance; - } - - private final CANcoder fl; - private final CANcoder fr; - private final CANcoder bl; - private final CANcoder br; - - private final CanTimeObserver frObs; - private final CanTimeObserver flObs; - private final CanTimeObserver blObs; - private final CanTimeObserver brObs; - - private static final double BOOT_UP_ERROR_ALLOWANCE_TIME = 10.0; - - private static class CanTimeObserver { - private final CANcoder cancoder; - private Optional lastTimestamp = Optional.empty(); - private int validUpdates = 0; - private static final int kRequiredValidTimestamps = 10; - - public CanTimeObserver(CANcoder cancoder) { - this.cancoder = cancoder; - } - - public boolean hasUpdate() { - StatusSignal absolutePositionSignal = cancoder.getAbsolutePosition(); - - double timestamp = absolutePositionSignal.getTimestamp().getTime(); - if (lastTimestamp.isEmpty()) { - lastTimestamp = Optional.of(timestamp); - } - if (timestamp > lastTimestamp.get()) { - validUpdates++; - lastTimestamp = Optional.of(timestamp); - } - return validUpdates > kRequiredValidTimestamps; - } - } - - private CANcoder build(Constants.Port canDeviceId) { - CANcoder thisCancoder = new CANcoder(canDeviceId.id, canDeviceId.bus); - CANcoderConfigurator configurator = thisCancoder.getConfigurator(); - CANcoderConfiguration canCoderConfig = new CANcoderConfiguration(); - - canCoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 1.0; - canCoderConfig.MagnetSensor.MagnetOffset = 0.0; - canCoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; - - double startTime = Timer.getFPGATimestamp(); - boolean timedOut = false; - boolean goodInit = false; - int attempt = 1; - while (!goodInit && !timedOut && attempt < 20) { - System.out.println("Initing CANCoder " + canDeviceId.id + " / attempt: " + attempt + " / " - + (Timer.getFPGATimestamp() - startTime) + " seconds elapsed"); - StatusCode settingsCode = configurator.apply(canCoderConfig); - StatusCode sensorCode = thisCancoder.getAbsolutePosition().setUpdateFrequency(20); - - goodInit = settingsCode == StatusCode.OK && sensorCode == StatusCode.OK; - - timedOut = (Timer.getFPGATimestamp()) - startTime >= BOOT_UP_ERROR_ALLOWANCE_TIME; - attempt++; - } - - return thisCancoder; - } - - public CancoderManager() { - fl = build(Constants.Port.FL_CANCODER); - flObs = new CanTimeObserver(fl); - - fr = build(Constants.Port.FR_CANCODER); - frObs = new CanTimeObserver(fr); - - bl = build(Constants.Port.BL_CANCODER); - blObs = new CanTimeObserver(bl); - - br = build(Constants.Port.BR_CANCODER); - brObs = new CanTimeObserver(br); - } - - public boolean allHaveBeenInitialized() { - return flObs.hasUpdate() - && frObs.hasUpdate() - && blObs.hasUpdate() - && brObs.hasUpdate(); - } - - public CANcoder getFrontLeft() { - return fl; - } - - public CANcoder getFrontRight() { - return fr; - } - - public CANcoder getBackLeft() { - return bl; - } - - public CANcoder getBackRight() { - return br; - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/commands/AutopilotCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/AutopilotCommand.java deleted file mode 100644 index c9c4f6a..0000000 --- a/src/main/java/frc/robot/subsystems/drive/commands/AutopilotCommand.java +++ /dev/null @@ -1,88 +0,0 @@ -package frc.robot.subsystems.drive.commands; - -import static edu.wpi.first.units.Units.Centimeters; -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.MetersPerSecond; - -import com.ctre.phoenix6.swerve.SwerveRequest; -import com.therekrab.autopilot.*; -import com.therekrab.autopilot.Autopilot.APResult; - -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.lib.control.ProfiledPIDVController; -import frc.robot.lib.control.ControlConstants.ProfiledPIDFConstants; -import frc.robot.subsystems.drive.Drive; - -/** - * I've tried this enough times according to the documents - * I don't think we need this anymore. - */ -@Deprecated -public class AutopilotCommand extends Command { - public final Drive drive; - private static final APConstraints kConstraints = new APConstraints() - .withAcceleration(5.0) - .withJerk(2.0); - private static final APProfile kProfile = new APProfile(kConstraints) - .withErrorXY(Centimeters.of(2)) - .withErrorTheta(Degrees.of(0.5)) - .withBeelineRadius(Centimeters.of(8)); - public static final Autopilot kAutopilot = new Autopilot(kProfile); - public final APTarget target; - - private final ProfiledPIDVController thetaController; - - public AutopilotCommand(Pose2d target) { - this(Drive.getInstance(), new APTarget(target), Constants.Auto.ROTATION_CONSTANTS); - } - - public AutopilotCommand(Drive drive, APTarget target, ProfiledPIDFConstants rotationConstants) { - this.drive = drive; - this.target = target; - thetaController = new ProfiledPIDVController(rotationConstants); - thetaController.enableContinuousInput(-Math.PI, Math.PI); - addRequirements(drive); - setName("Autopilot to " + target.getReference().toString()); - } - - @Override - public void execute() { - ChassisSpeeds robotRelativeSpeeds = drive.getState().Speeds; - Pose2d pose = drive.getState().Pose; - ChassisSpeeds speeds = drive.getState().Speeds; - - APResult output = kAutopilot.calculate(pose, robotRelativeSpeeds, target); - - /* these speeds are field relative */ - double veloX = output.vx().in(MetersPerSecond); - double veloY = output.vy().in(MetersPerSecond); - Rotation2d headingReference = output.targetAngle(); - - thetaController.setTarget(headingReference.getRadians()); - thetaController.setInput( - new Pair( - pose.getRotation().getRadians(), speeds.omegaRadiansPerSecond)); - - double rotation = thetaController.getOutput(); - - drive.setSwerveRequest(new SwerveRequest.RobotCentric() - .withVelocityX(veloX) - .withVelocityY(veloY) - .withRotationalRate(rotation)); - } - - @Override - public boolean isFinished() { - return kAutopilot.atTarget(drive.getState().Pose, target); - } - - @Override - public void end(boolean interrupted) { - drive.setSwerveRequest(new SwerveRequest.FieldCentric()); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/commands/SnapCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/SnapCommand.java deleted file mode 100644 index cac32e0..0000000 --- a/src/main/java/frc/robot/subsystems/drive/commands/SnapCommand.java +++ /dev/null @@ -1,152 +0,0 @@ -package frc.robot.subsystems.drive.commands; - -import com.ctre.phoenix6.swerve.SwerveRequest; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.lib.util.Util; -import frc.robot.subsystems.drive.Drive; - -/** - * A re-implementation of the autopilot. Please don't kill me - */ -public class SnapCommand extends Command { - private final Drive drive; - private final Pose2d target; - private final Rotation2d finalEntry; - private double maxSpeed; - private double maxAccel; - private double rotationRadius; - private SwerveRequest.FieldCentricFacingAngle request = new SwerveRequest.FieldCentricFacingAngle(); - private Transform2d errorTolerance; - - public SnapCommand(Pose2d target, Rotation2d finalEntry) { - this( - Drive.getInstance(), - target, - finalEntry, - Constants.Drive.MAX_SPEED, - Constants.Drive.MAX_ACCEL, - 1.5, - 0.04, - 2 * Math.PI / 180); - } - - public SnapCommand( - Drive drive, - Pose2d target, - Rotation2d finalEntry, - double maxSpeed, - double maxAccel, - double rotationRadius, - double errorXY, - double errorTheta) - { - this.drive = drive; - this.target = target; - this.finalEntry = finalEntry; - this.maxSpeed = maxSpeed; - this.maxAccel = maxAccel; - this.rotationRadius = rotationRadius; - errorTolerance = new Transform2d(errorXY, errorXY, new Rotation2d(errorTheta)); - } - - @Override - public void execute() { - calculate( - drive.getPose(), - drive.getState().Speeds, - target, - finalEntry); - drive.setSwerveRequest(request); - } - - @Override - public boolean isFinished() { - var delta = drive.getPose().minus(target); - return Math.abs(delta.getTranslation().getX()) < errorTolerance.getTranslation().getX() - && Math.abs(delta.getTranslation().getY()) < errorTolerance.getTranslation().getY() - && Math.abs(delta.getRotation().getRadians()) < errorTolerance.getRotation().getRadians(); - } - - @Override - public void end(boolean interrupted) { - drive.setSwerveRequest(new SwerveRequest.FieldCentric()); - } - - private void calculate( - Pose2d pose, - ChassisSpeeds speeds, - Pose2d target, - Rotation2d finalEntry) - { - Transform2d delta = target.minus(pose); - Translation2d offset = delta.getTranslation(); - Rotation2d entryRotation; - if (!Util.chassisSpeedsEpsilonEquals(speeds, new ChassisSpeeds(), 0.2)) { - entryRotation = new Rotation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); - } else { - entryRotation = offset.getAngle(); - } - var deltaRotation = finalEntry.minus(entryRotation); - var robotRelativeSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds(speeds, pose.getRotation()); - - double disp; - - double theta = - new Rotation2d(offset.getX(), offset.getY()) - .getRadians(); - double radius = offset.getNorm(); - if (theta == 0) { - disp = radius; - } - theta = Math.abs(theta); - double hypot = Math.hypot(theta, 1); - double u1 = radius * hypot; - double u2 = radius * Math.log(theta + hypot) / theta; - disp = 0.5 * (u1 + u2); - - var vx = Math.cos(theta) - theta * Math.sin(theta); - var vy = theta * Math.cos(theta) + Math.sin(theta); - - var vels = new Translation2d(vx, vy); - if (vels.getNorm() > Constants.DEADBAND) { - vels = vels.div(vels.getNorm()); - } else { - vels = new Translation2d(); - } - - double mag; - double stoppingLimit = Math.sqrt(2.0 * maxAccel * disp); - double goalV = Math.min(maxSpeed, stoppingLimit); - double maxDelta = maxAccel * Constants.DT; - if (goalV > vels.getNorm() + maxDelta) { - mag = vels.getNorm() + maxDelta; - } else if (goalV < vels.getNorm() - maxDelta) { - mag = vels.getNorm() - maxDelta; - } else { - mag = goalV; - } - vels = vels.times(mag); - - Rotation2d entryAngle = new Rotation2d(delta.getX(), delta.getY()); - vels = vels.rotateBy(entryAngle); - - ChassisSpeeds fieldRelativeSpeeds = - new ChassisSpeeds(vels.getX(), vels.getY(), 0.0); - - double blend = Math.min(1.0, offset.getNorm() / rotationRadius); - Rotation2d targetHeading = pose.getRotation().interpolate(finalEntry, 1.0 - blend); - - request.withVelocityX(fieldRelativeSpeeds.vxMetersPerSecond) - .withVelocityY(fieldRelativeSpeeds.vyMetersPerSecond) - .withTargetDirection(targetHeading); - } -} - diff --git a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java index b8895a2..d275ba3 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java @@ -12,7 +12,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; -import frc.robot.RobotState; +// import frc.robot.RobotState; import frc.robot.lib.control.ControlConstants.PIDFConstants; import frc.robot.lib.control.ControlConstants.ProfiledPIDFConstants; import frc.robot.lib.control.PIDVController; From d850d4529947d9da126ad3ab14ef35bd9e6b44ba Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Fri, 10 Oct 2025 22:54:28 -0700 Subject: [PATCH 004/118] Deprecate limelight-based vision localization code --- .../robot/lib/localization/CustomADStar.java | 713 ------- .../lib/localization/LimelightHelpers.java | 1642 ----------------- .../lib/localization/VisionPoseAcceptor.java | 85 - 3 files changed, 2440 deletions(-) delete mode 100644 src/main/java/frc/robot/lib/localization/CustomADStar.java delete mode 100644 src/main/java/frc/robot/lib/localization/LimelightHelpers.java delete mode 100644 src/main/java/frc/robot/lib/localization/VisionPoseAcceptor.java diff --git a/src/main/java/frc/robot/lib/localization/CustomADStar.java b/src/main/java/frc/robot/lib/localization/CustomADStar.java deleted file mode 100644 index e82cbfc..0000000 --- a/src/main/java/frc/robot/lib/localization/CustomADStar.java +++ /dev/null @@ -1,713 +0,0 @@ -package frc.robot.lib.localization; - -import com.pathplanner.lib.path.*; -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.Filesystem; -import java.io.BufferedReader; -import java.io.File; -import java.io.FileReader; -import java.util.*; -import java.util.concurrent.locks.ReadWriteLock; -import java.util.concurrent.locks.ReentrantReadWriteLock; -import org.json.simple.JSONArray; -import org.json.simple.JSONObject; -import org.json.simple.parser.JSONParser; - -/** - * Currently the same as the pathplanner one. - */ -public class CustomADStar { - private static final double SMOOTHING_ANCHOR_PCT = 0.8; - private static final double EPS = 2.5; - - private double fieldLength = 16.54; - private double fieldWidth = 8.02; - - private double nodeSize = 0.2; - - private int nodesX = (int) Math.ceil(fieldLength / nodeSize); - private int nodesY = (int) Math.ceil(fieldWidth / nodeSize); - - private final HashMap g = new HashMap<>(); - private final HashMap rhs = new HashMap<>(); - private final HashMap> open = new HashMap<>(); - private final HashMap> incons = new HashMap<>(); - private final Set closed = new HashSet<>(); - private final Set staticObstacles = new HashSet<>(); - private final Set dynamicObstacles = new HashSet<>(); - private final Set requestObstacles = new HashSet<>(); - - private GridPosition requestStart; - private Translation2d requestRealStartPos; - private GridPosition requestGoal; - private Translation2d requestRealGoalPos; - - private double eps; - - private final Thread planningThread; - private boolean requestMinor = true; - private boolean requestMajor = true; - private boolean requestReset = true; - private boolean newPathAvailable = false; - - private final ReadWriteLock pathLock = new ReentrantReadWriteLock(); - private final ReadWriteLock requestLock = new ReentrantReadWriteLock(); - - private List currentWaypoints = new ArrayList<>(); - private List currentPathFull = new ArrayList<>(); - - private IdealStartingState idealStartingState; - - /** Create a new pathfinder that runs AD* locally in a background thread */ - public CustomADStar() { - planningThread = new Thread(this::runThread); - - requestStart = new GridPosition(0, 0); - requestRealStartPos = Translation2d.kZero; - requestGoal = new GridPosition(0, 0); - requestRealGoalPos = Translation2d.kZero; - - staticObstacles.clear(); - dynamicObstacles.clear(); - - File navGridFile = new File(Filesystem.getDeployDirectory(), "pathplanner/navgrid.json"); - if (navGridFile.exists()) { - try (BufferedReader br = new BufferedReader(new FileReader(navGridFile))) { - StringBuilder fileContentBuilder = new StringBuilder(); - String line; - while ((line = br.readLine()) != null) { - fileContentBuilder.append(line); - } - - String fileContent = fileContentBuilder.toString(); - JSONObject json = (JSONObject) new JSONParser().parse(fileContent); - - nodeSize = ((Number) json.get("nodeSizeMeters")).doubleValue(); - JSONArray grid = (JSONArray) json.get("grid"); - nodesY = grid.size(); - for (int row = 0; row < grid.size(); row++) { - JSONArray rowArray = (JSONArray) grid.get(row); - if (row == 0) { - nodesX = rowArray.size(); - } - for (int col = 0; col < rowArray.size(); col++) { - boolean isObstacle = (boolean) rowArray.get(col); - if (isObstacle) { - staticObstacles.add(new GridPosition(col, row)); - } - } - } - - JSONObject fieldSize = (JSONObject) json.get("field_size"); - fieldLength = ((Number) fieldSize.get("x")).doubleValue(); - fieldWidth = ((Number) fieldSize.get("y")).doubleValue(); - } catch (Exception e) { - // Do nothing, use defaults - } - } - - requestObstacles.clear(); - requestObstacles.addAll(staticObstacles); - requestObstacles.addAll(dynamicObstacles); - - requestReset = true; - requestMajor = true; - requestMinor = true; - - newPathAvailable = false; - - planningThread.setDaemon(true); - planningThread.setName("ADStar Planning Thread"); - planningThread.start(); - } - - /** - * Get if a new path has been calculated since the last time a path was retrieved - * - * @return True if a new path is available - */ - public boolean isNewPathAvailable() { - return newPathAvailable; - } - - /** - * Get the most recently calculated path - * - * @param constraints The path constraints to use when creating the path - * @param goalEndState The goal end state to use when creating the path - * @return The PathPlannerPath created from the points calculated by the pathfinder - */ - public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) { - List waypoints; - - pathLock.readLock().lock(); - waypoints = new ArrayList<>(currentWaypoints); - pathLock.readLock().unlock(); - - newPathAvailable = false; - - if (waypoints.size() < 2) { - // Not enough points. Something got borked somewhere - System.out.println("bork"); - return null; - } - - return new PathPlannerPath(waypoints, constraints, idealStartingState, goalEndState); - } - - /** - * Set the start position to pathfind from - * - * @param startPosition Start position on the field. If this is within an obstacle it will be - * moved to the nearest non-obstacle node. - */ - public void setStartPosition(Translation2d startPosition) { - GridPosition startPos = findClosestNonObstacle(getGridPos(startPosition), requestObstacles); - - if (startPos != null && !startPos.equals(requestStart)) { - requestLock.writeLock().lock(); - requestStart = startPos; - requestRealStartPos = startPosition; - - requestMinor = true; - newPathAvailable = false; - requestLock.writeLock().unlock(); - } - } - - public void setIdealStartingState(IdealStartingState idealStartingState) { - this.idealStartingState = idealStartingState; - } - - /** - * Set the goal position to pathfind to - * - * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved - * to the nearest non-obstacle node. - */ - public void setGoalPosition(Translation2d goalPosition) { - GridPosition gridPos = findClosestNonObstacle(getGridPos(goalPosition), requestObstacles); - - if (gridPos != null) { - requestLock.writeLock().lock(); - requestGoal = gridPos; - requestRealGoalPos = goalPosition; - - requestMinor = true; - requestMajor = true; - requestReset = true; - newPathAvailable = false; - requestLock.writeLock().unlock(); - } - } - - /** - * Set the dynamic obstacles that should be avoided while pathfinding. - * - * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents - * opposite corners of a bounding box. - * @param currentRobotPos The current position of the robot. This is needed to change the start - * position of the path if the robot is now within an obstacle. - */ - public void setDynamicObstacles( - List> obs, Translation2d currentRobotPos) { - Set newObs = new HashSet<>(); - - for (var obstacle : obs) { - var gridPos1 = getGridPos(obstacle.getFirst()); - var gridPos2 = getGridPos(obstacle.getSecond()); - - int minX = Math.min(gridPos1.x, gridPos2.x); - int maxX = Math.max(gridPos1.x, gridPos2.x); - - int minY = Math.min(gridPos1.y, gridPos2.y); - int maxY = Math.max(gridPos1.y, gridPos2.y); - - for (int x = minX; x <= maxX; x++) { - for (int y = minY; y <= maxY; y++) { - newObs.add(new GridPosition(x, y)); - } - } - } - - dynamicObstacles.clear(); - dynamicObstacles.addAll(newObs); - requestLock.writeLock().lock(); - requestObstacles.clear(); - requestObstacles.addAll(staticObstacles); - requestObstacles.addAll(dynamicObstacles); - requestLock.writeLock().unlock(); - - pathLock.readLock().lock(); - boolean recalculate = false; - for (GridPosition pos : currentPathFull) { - if (requestObstacles.contains(pos)) { - recalculate = true; - break; - } - } - pathLock.readLock().unlock(); - - if (recalculate) { - setStartPosition(currentRobotPos); - setGoalPosition(requestRealGoalPos); - } - } - - @SuppressWarnings("BusyWait") - private void runThread() { - while (true) { - try { - requestLock.readLock().lock(); - boolean reset = requestReset; - boolean minor = requestMinor; - boolean major = requestMajor; - GridPosition start = requestStart; - Translation2d realStart = requestRealStartPos; - GridPosition goal = requestGoal; - Translation2d realGoal = requestRealGoalPos; - Set obstacles = new HashSet<>(requestObstacles); - - // Change the request booleans based on what will be done this loop - if (reset) { - requestReset = false; - } - - if (minor) { - requestMinor = false; - } else if (major && (eps - 0.5) <= 1.0) { - requestMajor = false; - } - requestLock.readLock().unlock(); - - if (reset || minor || major) { - doWork(reset, minor, major, start, goal, realStart, realGoal, obstacles); - } else { - try { - Thread.sleep(10); - } catch (InterruptedException e) { - throw new RuntimeException(e); - } - } - } catch (Exception e) { - // Something messed up. Reset and hope for the best - requestLock.writeLock().lock(); - requestReset = true; - requestLock.writeLock().unlock(); - } - } - } - - private void doWork( - boolean needsReset, - boolean doMinor, - boolean doMajor, - GridPosition sStart, - GridPosition sGoal, - Translation2d realStartPos, - Translation2d realGoalPos, - Set obstacles) { - if (needsReset) { - reset(sStart, sGoal); - } - - if (doMinor) { - computeOrImprovePath(sStart, sGoal, obstacles); - - List pathPositions = extractPath(sStart, sGoal, obstacles); - List waypoints = - createWaypoints(pathPositions, realStartPos, realGoalPos, obstacles); - - pathLock.writeLock().lock(); - currentPathFull = pathPositions; - currentWaypoints = waypoints; - pathLock.writeLock().unlock(); - - newPathAvailable = true; - } else if (doMajor) { - if (eps > 1.0) { - eps -= 0.5; - open.putAll(incons); - - open.replaceAll((s, v) -> key(s, sStart)); - closed.clear(); - computeOrImprovePath(sStart, sGoal, obstacles); - - List pathPositions = extractPath(sStart, sGoal, obstacles); - List waypoints = - createWaypoints(pathPositions, realStartPos, realGoalPos, obstacles); - - pathLock.writeLock().lock(); - currentPathFull = pathPositions; - currentWaypoints = waypoints; - pathLock.writeLock().unlock(); - - newPathAvailable = true; - } - } - } - - private List extractPath( - GridPosition sStart, GridPosition sGoal, Set obstacles) { - if (sGoal.equals(sStart)) { - return new ArrayList<>(); - } - - List path = new ArrayList<>(); - path.add(sStart); - - var s = sStart; - - for (int k = 0; k < 200; k++) { - HashMap gList = new HashMap<>(); - - for (GridPosition x : getOpenNeighbors(s, obstacles)) { - gList.put(x, g.get(x)); - } - - Map.Entry min = Map.entry(sGoal, Double.POSITIVE_INFINITY); - for (var entry : gList.entrySet()) { - if (entry.getValue() < min.getValue()) { - min = entry; - } - } - s = min.getKey(); - - path.add(s); - if (s.equals(sGoal)) { - break; - } - } - - return path; - } - - private List createWaypoints( - List path, - Translation2d realStartPos, - Translation2d realGoalPos, - Set obstacles) { - if (path.isEmpty()) { - return new ArrayList<>(); - } - - List simplifiedPath = new ArrayList<>(); - simplifiedPath.add(path.get(0)); - for (int i = 1; i < path.size() - 1; i++) { - if (!walkable(simplifiedPath.get(simplifiedPath.size() - 1), path.get(i + 1), obstacles)) { - simplifiedPath.add(path.get(i)); - } - } - simplifiedPath.add(path.get(path.size() - 1)); - - List fieldPosPath = new ArrayList<>(); - for (GridPosition pos : simplifiedPath) { - fieldPosPath.add(gridPosToTranslation2d(pos)); - } - - if (fieldPosPath.size() < 2) { - return new ArrayList<>(); - } - - // Replace start and end positions with their real positions - fieldPosPath.set(0, realStartPos); - fieldPosPath.set(fieldPosPath.size() - 1, realGoalPos); - - List pathPoses = new ArrayList<>(); - pathPoses.add( - new Pose2d(fieldPosPath.get(0), fieldPosPath.get(1).minus(fieldPosPath.get(0)).getAngle())); - for (int i = 1; i < fieldPosPath.size() - 1; i++) { - Translation2d last = fieldPosPath.get(i - 1); - Translation2d current = fieldPosPath.get(i); - Translation2d next = fieldPosPath.get(i + 1); - - Translation2d anchor1 = current.minus(last).times(SMOOTHING_ANCHOR_PCT).plus(last); - Rotation2d heading1 = current.minus(last).getAngle(); - Translation2d anchor2 = current.minus(next).times(SMOOTHING_ANCHOR_PCT).plus(next); - Rotation2d heading2 = next.minus(anchor2).getAngle(); - - pathPoses.add(new Pose2d(anchor1, heading1)); - pathPoses.add(new Pose2d(anchor2, heading2)); - } - pathPoses.add( - new Pose2d( - fieldPosPath.get(fieldPosPath.size() - 1), - fieldPosPath - .get(fieldPosPath.size() - 1) - .minus(fieldPosPath.get(fieldPosPath.size() - 2)) - .getAngle())); - - return PathPlannerPath.waypointsFromPoses(pathPoses); - } - - private GridPosition findClosestNonObstacle(GridPosition pos, Set obstacles) { - if (!obstacles.contains(pos)) { - return pos; - } - - Set visited = new HashSet<>(); - - Queue queue = new LinkedList<>(getAllNeighbors(pos)); - - while (!queue.isEmpty()) { - GridPosition check = queue.poll(); - if (!obstacles.contains(check)) { - return check; - } - visited.add(check); - - for (GridPosition neighbor : getAllNeighbors(check)) { - if (!visited.contains(neighbor) && !queue.contains(neighbor)) { - queue.add(neighbor); - } - } - } - return null; - } - - private boolean walkable(GridPosition s1, GridPosition s2, Set obstacles) { - int x0 = s1.x; - int y0 = s1.y; - int x1 = s2.x; - int y1 = s2.y; - - int dx = Math.abs(x1 - x0); - int dy = Math.abs(y1 - y0); - int x = x0; - int y = y0; - int n = 1 + dx + dy; - int xInc = (x1 > x0) ? 1 : -1; - int yInc = (y1 > y0) ? 1 : -1; - int error = dx - dy; - dx *= 2; - dy *= 2; - - for (; n > 0; n--) { - if (obstacles.contains(new GridPosition(x, y))) { - return false; - } - - if (error > 0) { - x += xInc; - error -= dy; - } else if (error < 0) { - y += yInc; - error += dx; - } else { - x += xInc; - y += yInc; - error -= dy; - error += dx; - n--; - } - } - - return true; - } - - private void reset(GridPosition sStart, GridPosition sGoal) { - g.clear(); - rhs.clear(); - open.clear(); - incons.clear(); - closed.clear(); - - for (int x = 0; x < nodesX; x++) { - for (int y = 0; y < nodesY; y++) { - g.put(new GridPosition(x, y), Double.POSITIVE_INFINITY); - rhs.put(new GridPosition(x, y), Double.POSITIVE_INFINITY); - } - } - - rhs.put(sGoal, 0.0); - - eps = EPS; - - open.put(sGoal, key(sGoal, sStart)); - } - - private void computeOrImprovePath( - GridPosition sStart, GridPosition sGoal, Set obstacles) { - while (true) { - var sv = topKey(); - if (sv == null) { - break; - } - var s = sv.getFirst(); - var v = sv.getSecond(); - - if (comparePair(v, key(sStart, sStart)) >= 0 && rhs.get(sStart).equals(g.get(sStart))) { - break; - } - - open.remove(s); - - if (g.get(s) > rhs.get(s)) { - g.put(s, rhs.get(s)); - closed.add(s); - - for (GridPosition sn : getOpenNeighbors(s, obstacles)) { - updateState(sn, sStart, sGoal, obstacles); - } - } else { - g.put(s, Double.POSITIVE_INFINITY); - for (GridPosition sn : getOpenNeighbors(s, obstacles)) { - updateState(sn, sStart, sGoal, obstacles); - } - updateState(s, sStart, sGoal, obstacles); - } - } - } - - private void updateState( - GridPosition s, GridPosition sStart, GridPosition sGoal, Set obstacles) { - if (!s.equals(sGoal)) { - rhs.put(s, Double.POSITIVE_INFINITY); - - for (GridPosition x : getOpenNeighbors(s, obstacles)) { - rhs.put(s, Math.min(rhs.get(s), g.get(x) + cost(s, x, obstacles))); - } - } - - open.remove(s); - - if (!g.get(s).equals(rhs.get(s))) { - if (!closed.contains(s)) { - open.put(s, key(s, sStart)); - } else { - incons.put(s, Pair.of(0.0, 0.0)); - } - } - } - - private double cost(GridPosition sStart, GridPosition sGoal, Set obstacles) { - if (isCollision(sStart, sGoal, obstacles)) { - return Double.POSITIVE_INFINITY; - } - - return heuristic(sStart, sGoal); - } - - private boolean isCollision(GridPosition sStart, GridPosition sEnd, Set obstacles) { - if (obstacles.contains(sStart) || obstacles.contains(sEnd)) { - return true; - } - - if (sStart.x != sEnd.x && sStart.y != sEnd.y) { - GridPosition s1; - GridPosition s2; - - if (sEnd.x - sStart.x == sStart.y - sEnd.y) { - s1 = new GridPosition(Math.min(sStart.x, sEnd.x), Math.min(sStart.y, sEnd.y)); - s2 = new GridPosition(Math.max(sStart.x, sEnd.x), Math.max(sStart.y, sEnd.y)); - } else { - s1 = new GridPosition(Math.min(sStart.x, sEnd.x), Math.max(sStart.y, sEnd.y)); - s2 = new GridPosition(Math.max(sStart.x, sEnd.x), Math.min(sStart.y, sEnd.y)); - } - - return obstacles.contains(s1) || obstacles.contains(s2); - } - - return false; - } - - private List getOpenNeighbors(GridPosition s, Set obstacles) { - List ret = new ArrayList<>(); - - for (int xMove = -1; xMove <= 1; xMove++) { - for (int yMove = -1; yMove <= 1; yMove++) { - GridPosition sNext = new GridPosition(s.x + xMove, s.y + yMove); - if (!obstacles.contains(sNext) - && sNext.x >= 0 - && sNext.x < nodesX - && sNext.y >= 0 - && sNext.y < nodesY) { - ret.add(sNext); - } - } - } - return ret; - } - - private List getAllNeighbors(GridPosition s) { - List ret = new ArrayList<>(); - - for (int xMove = -1; xMove <= 1; xMove++) { - for (int yMove = -1; yMove <= 1; yMove++) { - GridPosition sNext = new GridPosition(s.x + xMove, s.y + yMove); - if (sNext.x >= 0 && sNext.x < nodesX && sNext.y >= 0 && sNext.y < nodesY) { - ret.add(sNext); - } - } - } - return ret; - } - - private Pair key(GridPosition s, GridPosition sStart) { - if (g.get(s) > rhs.get(s)) { - return Pair.of(rhs.get(s) + eps * heuristic(sStart, s), rhs.get(s)); - } else { - return Pair.of(g.get(s) + heuristic(sStart, s), g.get(s)); - } - } - - private Pair> topKey() { - Map.Entry> min = null; - for (var entry : open.entrySet()) { - if (min == null || comparePair(entry.getValue(), min.getValue()) < 0) { - min = entry; - } - } - - if (min == null) { - return null; - } - - return Pair.of(min.getKey(), min.getValue()); - } - - private double heuristic(GridPosition sStart, GridPosition sGoal) { - return Math.hypot(sGoal.x - sStart.x, sGoal.y - sStart.y); - } - - private int comparePair(Pair a, Pair b) { - int first = Double.compare(a.getFirst(), b.getFirst()); - if (first == 0) { - return Double.compare(a.getSecond(), b.getSecond()); - } else { - return first; - } - } - - private GridPosition getGridPos(Translation2d pos) { - int x = (int) Math.floor(pos.getX() / nodeSize); - int y = (int) Math.floor(pos.getY() / nodeSize); - - return new GridPosition(x, y); - } - - private Translation2d gridPosToTranslation2d(GridPosition pos) { - return new Translation2d( - (pos.x * nodeSize) + (nodeSize / 2.0), (pos.y * nodeSize) + (nodeSize / 2.0)); - } - - /** - * Represents a node in the pathfinding grid - * - * @param x X index in the grid - * @param y Y index in the grid - */ - public record GridPosition(int x, int y) implements Comparable { - @Override - public int compareTo(GridPosition o) { - if (x == o.x) { - return Integer.compare(y, o.y); - } else { - return Integer.compare(x, o.x); - } - } - } -} diff --git a/src/main/java/frc/robot/lib/localization/LimelightHelpers.java b/src/main/java/frc/robot/lib/localization/LimelightHelpers.java deleted file mode 100644 index f627307..0000000 --- a/src/main/java/frc/robot/lib/localization/LimelightHelpers.java +++ /dev/null @@ -1,1642 +0,0 @@ -package frc.robot.lib.localization; - -import edu.wpi.first.networktables.DoubleArrayEntry; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.TimestampedDoubleArray; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; - -import java.io.IOException; -import java.net.HttpURLConnection; -import java.net.MalformedURLException; -import java.net.URL; -import java.util.Map; -import java.util.concurrent.CompletableFuture; - -import com.fasterxml.jackson.annotation.JsonFormat; -import com.fasterxml.jackson.annotation.JsonFormat.Shape; -import com.fasterxml.jackson.annotation.JsonProperty; -import com.fasterxml.jackson.core.JsonProcessingException; -import com.fasterxml.jackson.databind.DeserializationFeature; -import com.fasterxml.jackson.databind.ObjectMapper; -import java.util.concurrent.ConcurrentHashMap; - -/** - * LimelightHelpers provides static methods and classes for interfacing with Limelight vision cameras in FRC. - * This library supports all Limelight features including AprilTag tracking, Neural Networks, and standard color/retroreflective tracking. - */ -public class LimelightHelpers { - private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); - - /** - * Represents a Color/Retroreflective Target Result extracted from JSON Output - */ - public static class LimelightTarget_Retro { - - @JsonProperty("t6c_ts") - private double[] cameraPose_TargetSpace; - - @JsonProperty("t6r_fs") - private double[] robotPose_FieldSpace; - - @JsonProperty("t6r_ts") - private double[] robotPose_TargetSpace; - - @JsonProperty("t6t_cs") - private double[] targetPose_CameraSpace; - - @JsonProperty("t6t_rs") - private double[] targetPose_RobotSpace; - - public Pose3d getCameraPose_TargetSpace() - { - return toPose3D(cameraPose_TargetSpace); - } - public Pose3d getRobotPose_FieldSpace() - { - return toPose3D(robotPose_FieldSpace); - } - public Pose3d getRobotPose_TargetSpace() - { - return toPose3D(robotPose_TargetSpace); - } - public Pose3d getTargetPose_CameraSpace() - { - return toPose3D(targetPose_CameraSpace); - } - public Pose3d getTargetPose_RobotSpace() - { - return toPose3D(targetPose_RobotSpace); - } - - public Pose2d getCameraPose_TargetSpace2D() - { - return toPose2D(cameraPose_TargetSpace); - } - public Pose2d getRobotPose_FieldSpace2D() - { - return toPose2D(robotPose_FieldSpace); - } - public Pose2d getRobotPose_TargetSpace2D() - { - return toPose2D(robotPose_TargetSpace); - } - public Pose2d getTargetPose_CameraSpace2D() - { - return toPose2D(targetPose_CameraSpace); - } - public Pose2d getTargetPose_RobotSpace2D() - { - return toPose2D(targetPose_RobotSpace); - } - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("tx_nocross") - public double tx_nocrosshair; - - @JsonProperty("ty_nocross") - public double ty_nocrosshair; - - @JsonProperty("ts") - public double ts; - - public LimelightTarget_Retro() { - cameraPose_TargetSpace = new double[6]; - robotPose_FieldSpace = new double[6]; - robotPose_TargetSpace = new double[6]; - targetPose_CameraSpace = new double[6]; - targetPose_RobotSpace = new double[6]; - } - - } - - /** - * Represents an AprilTag/Fiducial Target Result extracted from JSON Output - */ - public static class LimelightTarget_Fiducial { - - @JsonProperty("fID") - public double fiducialID; - - @JsonProperty("fam") - public String fiducialFamily; - - @JsonProperty("t6c_ts") - private double[] cameraPose_TargetSpace; - - @JsonProperty("t6r_fs") - private double[] robotPose_FieldSpace; - - @JsonProperty("t6r_ts") - private double[] robotPose_TargetSpace; - - @JsonProperty("t6t_cs") - private double[] targetPose_CameraSpace; - - @JsonProperty("t6t_rs") - private double[] targetPose_RobotSpace; - - public Pose3d getCameraPose_TargetSpace() - { - return toPose3D(cameraPose_TargetSpace); - } - public Pose3d getRobotPose_FieldSpace() - { - return toPose3D(robotPose_FieldSpace); - } - public Pose3d getRobotPose_TargetSpace() - { - return toPose3D(robotPose_TargetSpace); - } - public Pose3d getTargetPose_CameraSpace() - { - return toPose3D(targetPose_CameraSpace); - } - public Pose3d getTargetPose_RobotSpace() - { - return toPose3D(targetPose_RobotSpace); - } - - public Pose2d getCameraPose_TargetSpace2D() - { - return toPose2D(cameraPose_TargetSpace); - } - public Pose2d getRobotPose_FieldSpace2D() - { - return toPose2D(robotPose_FieldSpace); - } - public Pose2d getRobotPose_TargetSpace2D() - { - return toPose2D(robotPose_TargetSpace); - } - public Pose2d getTargetPose_CameraSpace2D() - { - return toPose2D(targetPose_CameraSpace); - } - public Pose2d getTargetPose_RobotSpace2D() - { - return toPose2D(targetPose_RobotSpace); - } - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("tx_nocross") - public double tx_nocrosshair; - - @JsonProperty("ty_nocross") - public double ty_nocrosshair; - - @JsonProperty("ts") - public double ts; - - public LimelightTarget_Fiducial() { - cameraPose_TargetSpace = new double[6]; - robotPose_FieldSpace = new double[6]; - robotPose_TargetSpace = new double[6]; - targetPose_CameraSpace = new double[6]; - targetPose_RobotSpace = new double[6]; - } - } - - /** - * Represents a Barcode Target Result extracted from JSON Output - */ - public static class LimelightTarget_Barcode { - - /** - * Barcode family type (e.g. "QR", "DataMatrix", etc.) - */ - @JsonProperty("fam") - public String family; - - /** - * Gets the decoded data content of the barcode - */ - @JsonProperty("data") - public String data; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("tx_nocross") - public double tx_nocrosshair; - - @JsonProperty("ty_nocross") - public double ty_nocrosshair; - - @JsonProperty("ta") - public double ta; - - @JsonProperty("pts") - public double[][] corners; - - public LimelightTarget_Barcode() { - } - - public String getFamily() { - return family; - } - } - - /** - * Represents a Neural Classifier Pipeline Result extracted from JSON Output - */ - public static class LimelightTarget_Classifier { - - @JsonProperty("class") - public String className; - - @JsonProperty("classID") - public double classID; - - @JsonProperty("conf") - public double confidence; - - @JsonProperty("zone") - public double zone; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("typ") - public double ty_pixels; - - public LimelightTarget_Classifier() { - } - } - - /** - * Represents a Neural Detector Pipeline Result extracted from JSON Output - */ - public static class LimelightTarget_Detector { - - @JsonProperty("class") - public String className; - - @JsonProperty("classID") - public double classID; - - @JsonProperty("conf") - public double confidence; - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("tx_nocross") - public double tx_nocrosshair; - - @JsonProperty("ty_nocross") - public double ty_nocrosshair; - - public LimelightTarget_Detector() { - } - } - - /** - * Limelight Results object, parsed from a Limelight's JSON results output. - */ - public static class LimelightResults { - - public String error; - - @JsonProperty("pID") - public double pipelineID; - - @JsonProperty("tl") - public double latency_pipeline; - - @JsonProperty("cl") - public double latency_capture; - - public double latency_jsonParse; - - @JsonProperty("ts") - public double timestamp_LIMELIGHT_publish; - - @JsonProperty("ts_rio") - public double timestamp_RIOFPGA_capture; - - @JsonProperty("v") - @JsonFormat(shape = Shape.NUMBER) - public boolean valid; - - @JsonProperty("botpose") - public double[] botpose; - - @JsonProperty("botpose_wpired") - public double[] botpose_wpired; - - @JsonProperty("botpose_wpiblue") - public double[] botpose_wpiblue; - - @JsonProperty("botpose_tagcount") - public double botpose_tagcount; - - @JsonProperty("botpose_span") - public double botpose_span; - - @JsonProperty("botpose_avgdist") - public double botpose_avgdist; - - @JsonProperty("botpose_avgarea") - public double botpose_avgarea; - - @JsonProperty("t6c_rs") - public double[] camerapose_robotspace; - - public Pose3d getBotPose3d() { - return toPose3D(botpose); - } - - public Pose3d getBotPose3d_wpiRed() { - return toPose3D(botpose_wpired); - } - - public Pose3d getBotPose3d_wpiBlue() { - return toPose3D(botpose_wpiblue); - } - - public Pose2d getBotPose2d() { - return toPose2D(botpose); - } - - public Pose2d getBotPose2d_wpiRed() { - return toPose2D(botpose_wpired); - } - - public Pose2d getBotPose2d_wpiBlue() { - return toPose2D(botpose_wpiblue); - } - - @JsonProperty("Retro") - public LimelightTarget_Retro[] targets_Retro; - - @JsonProperty("Fiducial") - public LimelightTarget_Fiducial[] targets_Fiducials; - - @JsonProperty("Classifier") - public LimelightTarget_Classifier[] targets_Classifier; - - @JsonProperty("Detector") - public LimelightTarget_Detector[] targets_Detector; - - @JsonProperty("Barcode") - public LimelightTarget_Barcode[] targets_Barcode; - - public LimelightResults() { - botpose = new double[6]; - botpose_wpired = new double[6]; - botpose_wpiblue = new double[6]; - camerapose_robotspace = new double[6]; - targets_Retro = new LimelightTarget_Retro[0]; - targets_Fiducials = new LimelightTarget_Fiducial[0]; - targets_Classifier = new LimelightTarget_Classifier[0]; - targets_Detector = new LimelightTarget_Detector[0]; - targets_Barcode = new LimelightTarget_Barcode[0]; - - } - - - } - - /** - * Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. - */ - public static class RawFiducial { - public int id = 0; - public double txnc = 0; - public double tync = 0; - public double ta = 0; - public double distToCamera = 0; - public double distToRobot = 0; - public double ambiguity = 0; - - - public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { - this.id = id; - this.txnc = txnc; - this.tync = tync; - this.ta = ta; - this.distToCamera = distToCamera; - this.distToRobot = distToRobot; - this.ambiguity = ambiguity; - } - } - - /** - * Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. - */ - public static class RawDetection { - public int classId = 0; - public double txnc = 0; - public double tync = 0; - public double ta = 0; - public double corner0_X = 0; - public double corner0_Y = 0; - public double corner1_X = 0; - public double corner1_Y = 0; - public double corner2_X = 0; - public double corner2_Y = 0; - public double corner3_X = 0; - public double corner3_Y = 0; - - - public RawDetection(int classId, double txnc, double tync, double ta, - double corner0_X, double corner0_Y, - double corner1_X, double corner1_Y, - double corner2_X, double corner2_Y, - double corner3_X, double corner3_Y ) { - this.classId = classId; - this.txnc = txnc; - this.tync = tync; - this.ta = ta; - this.corner0_X = corner0_X; - this.corner0_Y = corner0_Y; - this.corner1_X = corner1_X; - this.corner1_Y = corner1_Y; - this.corner2_X = corner2_X; - this.corner2_Y = corner2_Y; - this.corner3_X = corner3_X; - this.corner3_Y = corner3_Y; - } - } - - /** - * Represents a 3D Pose Estimate. - */ - public static class PoseEstimate { - public Pose2d pose; - public double timestampSeconds; - public double latency; - public int tagCount; - public double tagSpan; - public double avgTagDist; - public double avgTagArea; - - public RawFiducial[] rawFiducials; - public boolean isMegaTag2; - - /** - * Instantiates a PoseEstimate object with default values - */ - public PoseEstimate() { - this.pose = new Pose2d(); - this.timestampSeconds = 0; - this.latency = 0; - this.tagCount = 0; - this.tagSpan = 0; - this.avgTagDist = 0; - this.avgTagArea = 0; - this.rawFiducials = new RawFiducial[]{}; - this.isMegaTag2 = false; - } - - public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, - int tagCount, double tagSpan, double avgTagDist, - double avgTagArea, RawFiducial[] rawFiducials, boolean isMegaTag2) { - - this.pose = pose; - this.timestampSeconds = timestampSeconds; - this.latency = latency; - this.tagCount = tagCount; - this.tagSpan = tagSpan; - this.avgTagDist = avgTagDist; - this.avgTagArea = avgTagArea; - this.rawFiducials = rawFiducials; - this.isMegaTag2 = isMegaTag2; - } - - } - - /** - * Encapsulates the state of an internal Limelight IMU. - */ - public static class IMUData { - public double robotYaw = 0.0; - public double Roll = 0.0; - public double Pitch = 0.0; - public double Yaw = 0.0; - public double gyroX = 0.0; - public double gyroY = 0.0; - public double gyroZ = 0.0; - public double accelX = 0.0; - public double accelY = 0.0; - public double accelZ = 0.0; - - public IMUData() {} - - public IMUData(double[] imuData) { - if (imuData != null && imuData.length >= 10) { - this.robotYaw = imuData[0]; - this.Roll = imuData[1]; - this.Pitch = imuData[2]; - this.Yaw = imuData[3]; - this.gyroX = imuData[4]; - this.gyroY = imuData[5]; - this.gyroZ = imuData[6]; - this.accelX = imuData[7]; - this.accelY = imuData[8]; - this.accelZ = imuData[9]; - } - } - } - - - private static ObjectMapper mapper; - - /** - * Print JSON Parse time to the console in milliseconds - */ - static boolean profileJSON = false; - - static final String sanitizeName(String name) { - if ("".equals(name) || name == null) { - return "limelight"; - } - return name; - } - - /** - * Takes a 6-length array of pose data and converts it to a Pose3d object. - * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. - * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] - * @return Pose3d object representing the pose, or empty Pose3d if invalid data - */ - public static Pose3d toPose3D(double[] inData){ - if(inData.length < 6) - { - //System.err.println("Bad LL 3D Pose Data!"); - return new Pose3d(); - } - return new Pose3d( - new Translation3d(inData[0], inData[1], inData[2]), - new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), - Units.degreesToRadians(inData[5]))); - } - - /** - * Takes a 6-length array of pose data and converts it to a Pose2d object. - * Uses only x, y, and yaw components, ignoring z, roll, and pitch. - * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. - * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] - * @return Pose2d object representing the pose, or empty Pose2d if invalid data - */ - public static Pose2d toPose2D(double[] inData){ - if(inData.length < 6) - { - //System.err.println("Bad LL 2D Pose Data!"); - return new Pose2d(); - } - Translation2d tran2d = new Translation2d(inData[0], inData[1]); - Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); - return new Pose2d(tran2d, r2d); - } - - /** - * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. - * Translation components are in meters, rotation components are in degrees. - * - * @param pose The Pose3d object to convert - * @return A 6-element array containing [x, y, z, roll, pitch, yaw] - */ - public static double[] pose3dToArray(Pose3d pose) { - double[] result = new double[6]; - result[0] = pose.getTranslation().getX(); - result[1] = pose.getTranslation().getY(); - result[2] = pose.getTranslation().getZ(); - result[3] = Units.radiansToDegrees(pose.getRotation().getX()); - result[4] = Units.radiansToDegrees(pose.getRotation().getY()); - result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); - return result; - } - - /** - * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. - * Translation components are in meters, rotation components are in degrees. - * Note: z, roll, and pitch will be 0 since Pose2d only contains x, y, and yaw. - * - * @param pose The Pose2d object to convert - * @return A 6-element array containing [x, y, 0, 0, 0, yaw] - */ - public static double[] pose2dToArray(Pose2d pose) { - double[] result = new double[6]; - result[0] = pose.getTranslation().getX(); - result[1] = pose.getTranslation().getY(); - result[2] = 0; - result[3] = Units.radiansToDegrees(0); - result[4] = Units.radiansToDegrees(0); - result[5] = Units.radiansToDegrees(pose.getRotation().getRadians()); - return result; - } - - private static double extractArrayEntry(double[] inData, int position){ - if(inData.length < position+1) - { - return 0; - } - return inData[position]; - } - - private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName, boolean isMegaTag2) { - DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); - - TimestampedDoubleArray tsValue = poseEntry.getAtomic(); - double[] poseArray = tsValue.value; - long timestamp = tsValue.timestamp; - - if (poseArray.length == 0) { - // Handle the case where no data is available - return null; // or some default PoseEstimate - } - - var pose = toPose2D(poseArray); - double latency = extractArrayEntry(poseArray, 6); - int tagCount = (int)extractArrayEntry(poseArray, 7); - double tagSpan = extractArrayEntry(poseArray, 8); - double tagDist = extractArrayEntry(poseArray, 9); - double tagArea = extractArrayEntry(poseArray, 10); - - // Convert server timestamp from microseconds to seconds and adjust for latency - double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); - - RawFiducial[] rawFiducials = new RawFiducial[tagCount]; - int valsPerFiducial = 7; - int expectedTotalVals = 11 + valsPerFiducial * tagCount; - - if (poseArray.length != expectedTotalVals) { - // Don't populate fiducials - } else { - for(int i = 0; i < tagCount; i++) { - int baseIndex = 11 + (i * valsPerFiducial); - int id = (int)poseArray[baseIndex]; - double txnc = poseArray[baseIndex + 1]; - double tync = poseArray[baseIndex + 2]; - double ta = poseArray[baseIndex + 3]; - double distToCamera = poseArray[baseIndex + 4]; - double distToRobot = poseArray[baseIndex + 5]; - double ambiguity = poseArray[baseIndex + 6]; - rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); - } - } - - return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials, isMegaTag2); - } - - /** - * Gets the latest raw fiducial/AprilTag detection results from NetworkTables. - * - * @param limelightName Name/identifier of the Limelight - * @return Array of RawFiducial objects containing detection details - */ - public static RawFiducial[] getRawFiducials(String limelightName) { - var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); - var rawFiducialArray = entry.getDoubleArray(new double[0]); - int valsPerEntry = 7; - if (rawFiducialArray.length % valsPerEntry != 0) { - return new RawFiducial[0]; - } - - int numFiducials = rawFiducialArray.length / valsPerEntry; - RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; - - for (int i = 0; i < numFiducials; i++) { - int baseIndex = i * valsPerEntry; - int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); - double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); - double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); - double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); - double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); - double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); - double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); - - rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); - } - - return rawFiducials; - } - - /** - * Gets the latest raw neural detector results from NetworkTables - * - * @param limelightName Name/identifier of the Limelight - * @return Array of RawDetection objects containing detection details - */ - public static RawDetection[] getRawDetections(String limelightName) { - var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); - var rawDetectionArray = entry.getDoubleArray(new double[0]); - int valsPerEntry = 12; - if (rawDetectionArray.length % valsPerEntry != 0) { - return new RawDetection[0]; - } - - int numDetections = rawDetectionArray.length / valsPerEntry; - RawDetection[] rawDetections = new RawDetection[numDetections]; - - for (int i = 0; i < numDetections; i++) { - int baseIndex = i * valsPerEntry; // Starting index for this detection's data - int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); - double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); - double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); - double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); - double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); - double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); - double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); - double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); - double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); - double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); - double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); - double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); - - rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y); - } - - return rawDetections; - } - - /** - * Prints detailed information about a PoseEstimate to standard output. - * Includes timestamp, latency, tag count, tag span, average tag distance, - * average tag area, and detailed information about each detected fiducial. - * - * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available." - */ - public static void printPoseEstimate(PoseEstimate pose) { - if (pose == null) { - System.out.println("No PoseEstimate available."); - return; - } - - System.out.printf("Pose Estimate Information:%n"); - System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); - System.out.printf("Latency: %.3f ms%n", pose.latency); - System.out.printf("Tag Count: %d%n", pose.tagCount); - System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); - System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); - System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); - System.out.printf("Is MegaTag2: %b%n", pose.isMegaTag2); - System.out.println(); - - if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { - System.out.println("No RawFiducials data available."); - return; - } - - System.out.println("Raw Fiducials Details:"); - for (int i = 0; i < pose.rawFiducials.length; i++) { - RawFiducial fiducial = pose.rawFiducials[i]; - System.out.printf(" Fiducial #%d:%n", i + 1); - System.out.printf(" ID: %d%n", fiducial.id); - System.out.printf(" TXNC: %.2f%n", fiducial.txnc); - System.out.printf(" TYNC: %.2f%n", fiducial.tync); - System.out.printf(" TA: %.2f%n", fiducial.ta); - System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); - System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); - System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); - System.out.println(); - } - } - - public static Boolean validPoseEstimate(PoseEstimate pose) { - return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0; - } - - public static NetworkTable getLimelightNTTable(String tableName) { - return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); - } - - public static void Flush() { - NetworkTableInstance.getDefault().flush(); - } - - public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { - return getLimelightNTTable(tableName).getEntry(entryName); - } - - public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { - String key = tableName + "/" + entryName; - return doubleArrayEntries.computeIfAbsent(key, k -> { - NetworkTable table = getLimelightNTTable(tableName); - return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); - }); - } - - public static double getLimelightNTDouble(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); - } - - public static void setLimelightNTDouble(String tableName, String entryName, double val) { - getLimelightNTTableEntry(tableName, entryName).setDouble(val); - } - - public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { - getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); - } - - public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); - } - - - public static String getLimelightNTString(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getString(""); - } - - public static String[] getLimelightNTStringArray(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); - } - - - public static URL getLimelightURLString(String tableName, String request) { - String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; - URL url; - try { - url = new URL(urlString); - return url; - } catch (MalformedURLException e) { - System.err.println("bad LL URL"); - } - return null; - } - ///// - ///// - - /** - * Does the Limelight have a valid target? - * @param limelightName Name of the Limelight camera ("" for default) - * @return True if a valid target is present, false otherwise - */ - public static boolean getTV(String limelightName) { - return 1.0 == getLimelightNTDouble(limelightName, "tv"); - } - - /** - * Gets the horizontal offset from the crosshair to the target in degrees. - * @param limelightName Name of the Limelight camera ("" for default) - * @return Horizontal offset angle in degrees - */ - public static double getTX(String limelightName) { - return getLimelightNTDouble(limelightName, "tx"); - } - - /** - * Gets the vertical offset from the crosshair to the target in degrees. - * @param limelightName Name of the Limelight camera ("" for default) - * @return Vertical offset angle in degrees - */ - public static double getTY(String limelightName) { - return getLimelightNTDouble(limelightName, "ty"); - } - - /** - * Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. - * @param limelightName Name of the Limelight camera ("" for default) - * @return Horizontal offset angle in degrees - */ - public static double getTXNC(String limelightName) { - return getLimelightNTDouble(limelightName, "txnc"); - } - - /** - * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. - * @param limelightName Name of the Limelight camera ("" for default) - * @return Vertical offset angle in degrees - */ - public static double getTYNC(String limelightName) { - return getLimelightNTDouble(limelightName, "tync"); - } - - /** - * Gets the target area as a percentage of the image (0-100%). - * @param limelightName Name of the Limelight camera ("" for default) - * @return Target area percentage (0-100) - */ - public static double getTA(String limelightName) { - return getLimelightNTDouble(limelightName, "ta"); - } - - /** - * T2D is an array that contains several targeting metrcis - * @param limelightName Name of the Limelight camera - * @return Array containing [targetValid, targetCount, targetLatency, captureLatency, tx, ty, txnc, tync, ta, tid, targetClassIndexDetector, - * targetClassIndexClassifier, targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, targetVerticalExtentPixels, targetSkewDegrees] - */ - public static double[] getT2DArray(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "t2d"); - } - - /** - * Gets the number of targets currently detected. - * @param limelightName Name of the Limelight camera - * @return Number of detected targets - */ - public static int getTargetCount(String limelightName) { - double[] t2d = getT2DArray(limelightName); - if(t2d.length == 17) - { - return (int)t2d[1]; - } - return 0; - } - - /** - * Gets the classifier class index from the currently running neural classifier pipeline - * @param limelightName Name of the Limelight camera - * @return Class index from classifier pipeline - */ - public static int getClassifierClassIndex (String limelightName) { - double[] t2d = getT2DArray(limelightName); - if(t2d.length == 17) - { - return (int)t2d[10]; - } - return 0; - } - - /** - * Gets the detector class index from the primary result of the currently running neural detector pipeline. - * @param limelightName Name of the Limelight camera - * @return Class index from detector pipeline - */ - public static int getDetectorClassIndex (String limelightName) { - double[] t2d = getT2DArray(limelightName); - if(t2d.length == 17) - { - return (int)t2d[11]; - } - return 0; - } - - /** - * Gets the current neural classifier result class name. - * @param limelightName Name of the Limelight camera - * @return Class name string from classifier pipeline - */ - public static String getClassifierClass (String limelightName) { - return getLimelightNTString(limelightName, "tcclass"); - } - - /** - * Gets the primary neural detector result class name. - * @param limelightName Name of the Limelight camera - * @return Class name string from detector pipeline - */ - public static String getDetectorClass (String limelightName) { - return getLimelightNTString(limelightName, "tdclass"); - } - - /** - * Gets the pipeline's processing latency contribution. - * @param limelightName Name of the Limelight camera - * @return Pipeline latency in milliseconds - */ - public static double getLatency_Pipeline(String limelightName) { - return getLimelightNTDouble(limelightName, "tl"); - } - - /** - * Gets the capture latency. - * @param limelightName Name of the Limelight camera - * @return Capture latency in milliseconds - */ - public static double getLatency_Capture(String limelightName) { - return getLimelightNTDouble(limelightName, "cl"); - } - - /** - * Gets the active pipeline index. - * @param limelightName Name of the Limelight camera - * @return Current pipeline index (0-9) - */ - public static double getCurrentPipelineIndex(String limelightName) { - return getLimelightNTDouble(limelightName, "getpipe"); - } - - /** - * Gets the current pipeline type. - * @param limelightName Name of the Limelight camera - * @return Pipeline type string (e.g. "retro", "apriltag", etc) - */ - public static String getCurrentPipelineType(String limelightName) { - return getLimelightNTString(limelightName, "getpipetype"); - } - - /** - * Gets the full JSON results dump. - * @param limelightName Name of the Limelight camera - * @return JSON string containing all current results - */ - public static String getJSONDump(String limelightName) { - return getLimelightNTString(limelightName, "json"); - } - - /** - * Switch to getBotPose - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } - - /** - * Switch to getBotPose_wpiRed - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } - - /** - * Switch to getBotPose_wpiBlue - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } - - public static double[] getBotPose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } - - public static double[] getBotPose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } - - public static double[] getBotPose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } - - public static double[] getBotPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - } - - public static double[] getCameraPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - } - - public static double[] getTargetPose_CameraSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); - } - - public static double[] getTargetPose_RobotSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); - } - - public static double[] getTargetColor(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "tc"); - } - - public static double getFiducialID(String limelightName) { - return getLimelightNTDouble(limelightName, "tid"); - } - - public static String getNeuralClassID(String limelightName) { - return getLimelightNTString(limelightName, "tclass"); - } - - public static String[] getRawBarcodeData(String limelightName) { - return getLimelightNTStringArray(limelightName, "rawbarcodes"); - } - - ///// - ///// - - public static Pose3d getBotPose3d(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); - return toPose3D(poseArray); - } - - /** - * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the robot's position and orientation in Red Alliance field space - */ - public static Pose3d getBotPose3d_wpiRed(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - return toPose3D(poseArray); - } - - /** - * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the robot's position and orientation in Blue Alliance field space - */ - public static Pose3d getBotPose3d_wpiBlue(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - return toPose3D(poseArray); - } - - /** - * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the robot's position and orientation relative to the target - */ - public static Pose3d getBotPose3d_TargetSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - return toPose3D(poseArray); - } - - /** - * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the camera's position and orientation relative to the target - */ - public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - return toPose3D(poseArray); - } - - /** - * Gets the target's 3D pose with respect to the camera's coordinate system. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the target's position and orientation relative to the camera - */ - public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); - return toPose3D(poseArray); - } - - /** - * Gets the target's 3D pose with respect to the robot's coordinate system. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the target's position and orientation relative to the robot - */ - public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); - return toPose3D(poseArray); - } - - /** - * Gets the camera's 3D pose with respect to the robot's coordinate system. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the camera's position and orientation relative to the robot - */ - public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); - return toPose3D(poseArray); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator - * (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d_wpiBlue(String limelightName) { - - double[] result = getBotPose_wpiBlue(limelightName); - return toPose2D(result); - } - - /** - * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpiblue", false); - } - - /** - * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. - * Make sure you are calling setRobotOrientation() before calling this method. - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator - * (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d_wpiRed(String limelightName) { - - double[] result = getBotPose_wpiRed(limelightName); - return toPose2D(result); - - } - - /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED - * alliance - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpired", false); - } - - /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED - * alliance - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator - * (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d(String limelightName) { - - double[] result = getBotPose(limelightName); - return toPose2D(result); - - } - - /** - * Gets the current IMU data from NetworkTables. - * IMU data is formatted as [robotYaw, Roll, Pitch, Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. - * Returns all zeros if data is invalid or unavailable. - * - * @param limelightName Name/identifier of the Limelight - * @return IMUData object containing all current IMU data - */ - public static IMUData getIMUData(String limelightName) { - double[] imuData = getLimelightNTDoubleArray(limelightName, "imu"); - if (imuData == null || imuData.length < 10) { - return new IMUData(); // Returns object with all zeros - } - return new IMUData(imuData); - } - - ///// - ///// - - public static void setPipelineIndex(String limelightName, int pipelineIndex) { - setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); - } - - - public static void setPriorityTagID(String limelightName, int ID) { - setLimelightNTDouble(limelightName, "priorityid", ID); - } - - /** - * Sets LED mode to be controlled by the current pipeline. - * @param limelightName Name of the Limelight camera - */ - public static void setLEDMode_PipelineControl(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 0); - } - - public static void setLEDMode_ForceOff(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 1); - } - - public static void setLEDMode_ForceBlink(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 2); - } - - public static void setLEDMode_ForceOn(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 3); - } - - /** - * Enables standard side-by-side stream mode. - * @param limelightName Name of the Limelight camera - */ - public static void setStreamMode_Standard(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 0); - } - - /** - * Enables Picture-in-Picture mode with secondary stream in the corner. - * @param limelightName Name of the Limelight camera - */ - public static void setStreamMode_PiPMain(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 1); - } - - /** - * Enables Picture-in-Picture mode with primary stream in the corner. - * @param limelightName Name of the Limelight camera - */ - public static void setStreamMode_PiPSecondary(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 2); - } - - - /** - * Sets the crop window for the camera. The crop window in the UI must be completely open. - * @param limelightName Name of the Limelight camera - * @param cropXMin Minimum X value (-1 to 1) - * @param cropXMax Maximum X value (-1 to 1) - * @param cropYMin Minimum Y value (-1 to 1) - * @param cropYMax Maximum Y value (-1 to 1) - */ - public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { - double[] entries = new double[4]; - entries[0] = cropXMin; - entries[1] = cropXMax; - entries[2] = cropYMin; - entries[3] = cropYMax; - setLimelightNTDoubleArray(limelightName, "crop", entries); - } - - /** - * Sets 3D offset point for easy 3D targeting. - */ - public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) { - double[] entries = new double[3]; - entries[0] = offsetX; - entries[1] = offsetY; - entries[2] = offsetZ; - setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); - } - - /** - * Sets robot orientation values used by MegaTag2 localization algorithm. - * - * @param limelightName Name/identifier of the Limelight - * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC - * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second - * @param pitch (Unnecessary) Robot pitch in degrees - * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second - * @param roll (Unnecessary) Robot roll in degrees - * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second - */ - public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, - double pitch, double pitchRate, - double roll, double rollRate) { - SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); - } - - public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, - double pitch, double pitchRate, - double roll, double rollRate) { - SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); - } - - private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, - double pitch, double pitchRate, - double roll, double rollRate, boolean flush) { - - double[] entries = new double[6]; - entries[0] = yaw; - entries[1] = yawRate; - entries[2] = pitch; - entries[3] = pitchRate; - entries[4] = roll; - entries[5] = rollRate; - setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); - if(flush) - { - Flush(); - } - } - - /** - * Configures the IMU mode for MegaTag2 Localization - * - * @param limelightName Name/identifier of the Limelight - * @param mode IMU mode. - */ - public static void SetIMUMode(String limelightName, int mode) { - setLimelightNTDouble(limelightName, "imumode_set", mode); - } - - /** - * Sets the 3D point-of-interest offset for the current fiducial pipeline. - * https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-3d#point-of-interest-tracking - * - * @param limelightName Name/identifier of the Limelight - * @param x X offset in meters - * @param y Y offset in meters - * @param z Z offset in meters - */ - public static void SetFidcuial3DOffset(String limelightName, double x, double y, - double z) { - - double[] entries = new double[3]; - entries[0] = x; - entries[1] = y; - entries[2] = z; - setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); - } - - /** - * Overrides the valid AprilTag IDs that will be used for localization. - * Tags not in this list will be ignored for robot pose estimation. - * - * @param limelightName Name/identifier of the Limelight - * @param validIDs Array of valid AprilTag IDs to track - */ - public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { - double[] validIDsDouble = new double[validIDs.length]; - for (int i = 0; i < validIDs.length; i++) { - validIDsDouble[i] = validIDs[i]; - } - setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); - } - - /** - * Sets the downscaling factor for AprilTag detection. - * Increasing downscale can improve performance at the cost of potentially reduced detection range. - * - * @param limelightName Name/identifier of the Limelight - * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to 0 for pipeline control. - */ - public static void SetFiducialDownscalingOverride(String limelightName, float downscale) - { - int d = 0; // pipeline - if (downscale == 1.0) - { - d = 1; - } - if (downscale == 1.5) - { - d = 2; - } - if (downscale == 2) - { - d = 3; - } - if (downscale == 3) - { - d = 4; - } - if (downscale == 4) - { - d = 5; - } - setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); - } - - /** - * Sets the camera pose relative to the robot. - * @param limelightName Name of the Limelight camera - * @param forward Forward offset in meters - * @param side Side offset in meters - * @param up Up offset in meters - * @param roll Roll angle in degrees - * @param pitch Pitch angle in degrees - * @param yaw Yaw angle in degrees - */ - public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { - double[] entries = new double[6]; - entries[0] = forward; - entries[1] = side; - entries[2] = up; - entries[3] = roll; - entries[4] = pitch; - entries[5] = yaw; - setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); - } - - ///// - ///// - - public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { - setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); - } - - public static double[] getPythonScriptData(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "llpython"); - } - - ///// - ///// - - /** - * Asynchronously take snapshot. - */ - public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { - return CompletableFuture.supplyAsync(() -> { - return SYNCH_TAKESNAPSHOT(tableName, snapshotName); - }); - } - - private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { - URL url = getLimelightURLString(tableName, "capturesnapshot"); - try { - HttpURLConnection connection = (HttpURLConnection) url.openConnection(); - connection.setRequestMethod("GET"); - if (snapshotName != null && !"".equals(snapshotName)) { - connection.setRequestProperty("snapname", snapshotName); - } - - int responseCode = connection.getResponseCode(); - if (responseCode == 200) { - return true; - } else { - System.err.println("Bad LL Request"); - } - } catch (IOException e) { - System.err.println(e.getMessage()); - } - return false; - } - - /** - * Gets the latest JSON results output and returns a LimelightResults object. - * @param limelightName Name of the Limelight camera - * @return LimelightResults object containing all current target data - */ - public static LimelightResults getLatestResults(String limelightName) { - - long start = System.nanoTime(); - LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); - if (mapper == null) { - mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); - } - - try { - results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); - } catch (JsonProcessingException e) { - results.error = "lljson error: " + e.getMessage(); - } - - long end = System.nanoTime(); - double millis = (end - start) * .000001; - results.latency_jsonParse = millis; - if (profileJSON) { - System.out.printf("lljson: %.2f\r\n", millis); - } - - return results; - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/lib/localization/VisionPoseAcceptor.java b/src/main/java/frc/robot/lib/localization/VisionPoseAcceptor.java deleted file mode 100644 index 461c56b..0000000 --- a/src/main/java/frc/robot/lib/localization/VisionPoseAcceptor.java +++ /dev/null @@ -1,85 +0,0 @@ -package frc.robot.lib.localization; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -public class VisionPoseAcceptor { - private static final double kFieldBorderMargin = 0.5; - private static final double kMaxVisionCorrection = 2.0; // Jump from fused pose - - Pose2d mLastVisionFieldToVehicle = null; - - public static Twist2d log(final Pose2d transform) { - final double dtheta = transform.getRotation().getRadians(); - final double half_dtheta = 0.5 * dtheta; - final double cos_minus_one = transform.getRotation().getCos() - 1.0; - double halftheta_by_tan_of_halfdtheta; - if (Math.abs(cos_minus_one) < 1E-9) { - halftheta_by_tan_of_halfdtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta; - } else { - halftheta_by_tan_of_halfdtheta = -(half_dtheta * transform.getRotation().getSin()) / cos_minus_one; - } - final Translation2d translation_part = transform.getTranslation() - .rotateBy(new Rotation2d(halftheta_by_tan_of_halfdtheta, -half_dtheta)); - return new Twist2d(translation_part.getX(), translation_part.getY(), dtheta); - } - - public Pose2d inverse(Translation2d translation, Rotation2d rotation) { - Rotation2d rotation_inverted = Rotation2d.fromRadians(-rotation.getRadians()); - return new Pose2d(new Translation2d(-translation.getX(), -translation.getY()).rotateBy(rotation_inverted), - rotation_inverted); - } - - public boolean shouldAcceptVision( - double timestamp, - Pose2d visionFieldToVehicle, - Pose2d lastFieldToVehicle, - Twist2d robotVelocity, - boolean isInAuto) { - - // If first update, trust - if (mLastVisionFieldToVehicle == null) { - mLastVisionFieldToVehicle = visionFieldToVehicle; - return true; - } - - // Write last pose early because we return out of the method - mLastVisionFieldToVehicle = visionFieldToVehicle; - - // Check out of field - if (visionFieldToVehicle.getTranslation().getX() < -kFieldBorderMargin - || visionFieldToVehicle.getTranslation().getX() > FieldLayout.FIELD_LENGTH + kFieldBorderMargin - || visionFieldToVehicle.getTranslation().getY() < -kFieldBorderMargin - || visionFieldToVehicle.getTranslation().getY() > FieldLayout.FIELD_WIDTH + kFieldBorderMargin) { - SmartDashboard.putString("Vision validation", "Outside field"); - return false; - } - - if ((robotVelocity.dy == 0 - ? Math.abs(robotVelocity.dx) - : Math.hypot(robotVelocity.dx, robotVelocity.dy)) > 4.0) { - SmartDashboard.putString("Vision validation", "Max velocity"); - return false; - } - - if (isInAuto) { - // Check max correction - final Pose2d inverse = inverse(visionFieldToVehicle.getTranslation(), visionFieldToVehicle.getRotation()); - final Twist2d logTwist = log(new Pose2d( - inverse.getTranslation().plus(lastFieldToVehicle.getTranslation().rotateBy(inverse.getRotation())), - inverse.getRotation().rotateBy(lastFieldToVehicle.getRotation()))); - if ((logTwist.dy == 0 - ? Math.abs(logTwist.dx) - : Math.hypot(logTwist.dx, logTwist.dy)) > kMaxVisionCorrection) { - SmartDashboard.putString("Vision validation", "Max correction"); - return false; - } - } - - SmartDashboard.putString("Vision validation", "OK"); - return true; - } -} From bf549970835d5cdc09bf565c2f00d3481cd997d8 Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Fri, 10 Oct 2025 22:58:28 -0700 Subject: [PATCH 005/118] Deprecated robot/lib/drivers folder and its classes (CanDeviceId and Phoenix6Util) --- .../frc/robot/lib/drivers/CanDeviceId.java | 24 --------- .../frc/robot/lib/drivers/Phoenix6Util.java | 53 ------------------- 2 files changed, 77 deletions(-) delete mode 100644 src/main/java/frc/robot/lib/drivers/CanDeviceId.java delete mode 100644 src/main/java/frc/robot/lib/drivers/Phoenix6Util.java diff --git a/src/main/java/frc/robot/lib/drivers/CanDeviceId.java b/src/main/java/frc/robot/lib/drivers/CanDeviceId.java deleted file mode 100644 index be7c444..0000000 --- a/src/main/java/frc/robot/lib/drivers/CanDeviceId.java +++ /dev/null @@ -1,24 +0,0 @@ -package frc.robot.lib.drivers; - -public class CanDeviceId { - private final int deviceNumber; - private final String bus; - - public CanDeviceId(int deviceNumber, String bus) { - this.deviceNumber = deviceNumber; - this.bus = bus; - } - - // Use the default bus name (empty string). - public CanDeviceId(int deviceNumber) { - this(deviceNumber, ""); - } - - public int getDeviceNumber() { return deviceNumber; } - - public String getBus() { return bus; } - - public boolean equals(CanDeviceId other) { - return other.deviceNumber == deviceNumber && other.bus == bus; - } -} diff --git a/src/main/java/frc/robot/lib/drivers/Phoenix6Util.java b/src/main/java/frc/robot/lib/drivers/Phoenix6Util.java deleted file mode 100644 index 2e8deb5..0000000 --- a/src/main/java/frc/robot/lib/drivers/Phoenix6Util.java +++ /dev/null @@ -1,53 +0,0 @@ -package frc.robot.lib.drivers; - -import com.ctre.phoenix6.StatusCode; - -import edu.wpi.first.wpilibj.DriverStation; - -import java.util.function.Supplier; - -public class Phoenix6Util { - /** - * Checks the specified error code for issues - * - * @param statusCode error code - * @param message message to print if error happens - */ - public static void checkError(StatusCode statusCode, String message) { - if (statusCode != StatusCode.OK) { - DriverStation.reportError(message + " " + statusCode, false); - } - } - - public static boolean checkErrorAndRetry(Supplier function, int numTries) { - StatusCode code = function.get(); - int tries = 0; - while (code != StatusCode.OK && tries < numTries) { - DriverStation.reportWarning("Retrying CTRE Device Config " + code.getName(), false); - code = function.get(); - tries++; - } - if (code != StatusCode.OK) { - DriverStation.reportError("Failed to execute phoenix pro api call after " + numTries + " attempts, status code " + code.getName() + " description: " + code.getDescription(), false); - return false; - } - return true; - } - - /** - * checks the specified error code and throws an exception if there are any - * issues - * - * @param statusCode error code - * @param message message to print if error happens - */ - public static void checkErrorWithThrow(StatusCode statusCode, String message) { - if (statusCode != StatusCode.OK) { - throw new RuntimeException(message + " " + statusCode); - } - } - - public static boolean checkErrorAndRetry(Supplier function) { - return checkErrorAndRetry(function, 1); - } -} From c37ed3c6120d13fd0f00be30f7c32b958a585222 Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Sun, 12 Oct 2025 10:12:58 -0700 Subject: [PATCH 006/118] reformat to prepare for sysID tuning --- .../subsystems/drive/ctre/CtreDriveConstants.java | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java index 6de0650..986f8ef 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java @@ -28,14 +28,21 @@ public class CtreDriveConstants { // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(100).withKI(0).withKD(0.5) - .withKS(0.1).withKV(2.66).withKA(0) + .withKP(100) + .withKI(0) + .withKD(0.5) + .withKS(0.1) + .withKV(2.66) + .withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.1).withKI(0).withKD(0) - .withKS(0).withKV(0.124); + .withKP(0.1) + .withKI(0) + .withKD(0) + .withKS(0) + .withKV(0.124); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors From 78c351d69598f011bdd189de3d62896a2fbf2e03 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Sun, 12 Oct 2025 18:19:35 -0700 Subject: [PATCH 007/118] feat: automode scaffolding and PIDV, validation on first complex path on the field. --- simgui-ds.json | 8 +- .../deploy/pathplanner/paths/testPath.path | 91 +++++++++++++++ src/main/deploy/pathplanner/paths/zisen.path | 87 +++++++++------ src/main/java/frc/robot/Robot.java | 4 +- .../java/frc/robot/auto/AutoRoutines.java | 4 +- .../frc/robot/lib/control/PIDVController.java | 1 + .../lib/control/ProfiledPIDVController.java | 104 +++++++++++++----- src/main/java/frc/robot/lib/util/Util.java | 4 + .../drive/commands/PIDToPoseCommand.java | 40 ++++--- .../drive/commands/TrajectoryCommand.java | 21 ++-- 10 files changed, 264 insertions(+), 100 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/testPath.path diff --git a/simgui-ds.json b/simgui-ds.json index 2296812..e12022f 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -16,13 +16,17 @@ "incKey": 83 }, { - "decKey": 69, "decayRate": 0.0, "incKey": 82, "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 81, + "incKey": 69 } ], - "axisCount": 3, + "axisCount": 5, "buttonCount": 4, "buttonKeys": [ 90, diff --git a/src/main/deploy/pathplanner/paths/testPath.path b/src/main/deploy/pathplanner/paths/testPath.path new file mode 100644 index 0000000..7ecb284 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/testPath.path @@ -0,0 +1,91 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.0, + "y": 0.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.6543032786885246, + "y": 0.15896516393442653 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.870081967215316, + "y": 0.7583504098420167 + }, + "prevControl": { + "x": 2.025922131149742, + "y": 0.386731557383 + }, + "nextControl": { + "x": 1.5354564647707203, + "y": 1.5563035310560531 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.4265368852459013, + "y": 1.6094774590163934 + }, + "prevControl": { + "x": 1.798155737707119, + "y": 1.6574282786944752 + }, + "nextControl": { + "x": 0.3915104179993174, + "y": 1.4759256567752452 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.0, + "y": 0.0 + }, + "prevControl": { + "x": 0.45553278688744697, + "y": 0.9501536885305403 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.57, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/zisen.path b/src/main/deploy/pathplanner/paths/zisen.path index cfcaeb2..092f40d 100644 --- a/src/main/deploy/pathplanner/paths/zisen.path +++ b/src/main/deploy/pathplanner/paths/zisen.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 0.25, - "y": 0.0 + "x": 0.38428120678715416, + "y": 0.028820911848564158 }, "isLocked": false, "linkedName": null @@ -20,28 +20,28 @@ "y": 4.102920081967213 }, "prevControl": { - "x": 0.05027796255960154, - "y": 3.8580280108810805 + "x": 0.05037403257065813, + "y": 3.702002286192967 }, "nextControl": { - "x": -0.050277962559601555, - "y": 4.3478121530533445 + "x": -0.03116664423415783, + "y": 4.350969754990947 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": -0.13186475409099274, - "y": 8.298616803285734 + "x": 0.1, + "y": 6.299 }, "prevControl": { - "x": -0.09533771451619764, - "y": 7.877164046497824 + "x": 0.10557581830210679, + "y": 5.920374832566059 }, "nextControl": { - "x": -0.16839179366578771, - "y": 8.720069560073641 + "x": 0.09434292934831419, + "y": 6.683142596947763 }, "isLocked": false, "linkedName": null @@ -52,12 +52,12 @@ "y": 3.647387295081966 }, "prevControl": { - "x": 0.2877049180298839, - "y": 3.8272028688595094 + "x": 0.11181482097517709, + "y": 4.140432055584381 }, "nextControl": { - "x": 1.7690012112975215, - "y": 2.9726088534753377 + "x": 1.7288144064715079, + "y": 2.505277147468517 }, "isLocked": false, "linkedName": null @@ -68,28 +68,28 @@ "y": 3.647387295081966 }, "prevControl": { - "x": 4.894373757716017, - "y": 3.898766036258368 + "x": 4.930116297867843, + "y": 3.8558936606051346 }, "nextControl": { - "x": 5.271200012763925, - "y": 3.396008553905564 + "x": 5.230480503895737, + "y": 3.445678110838003 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.082786885239971, - "y": 7.435502049180025 + "x": 5.22663934425693, + "y": 6.404559426232486 }, "prevControl": { - "x": 5.111974375425232, - "y": 8.2308611566828 + "x": 5.22034016514926, + "y": 6.860885487783113 }, "nextControl": { - "x": 5.03483606556481, - "y": 6.128842213106792 + "x": 5.248769225007602, + "y": 4.801423342477989 }, "isLocked": false, "linkedName": null @@ -100,12 +100,12 @@ "y": 4.39062499999939 }, "prevControl": { - "x": 5.166700819660269, - "y": 6.2607069672118945 + "x": 5.356425699711494, + "y": 6.167810932873377 }, "nextControl": { - "x": 4.976840630520012, - "y": 2.029537037502777 + "x": 4.717422490835596, + "y": 2.0177143317431057 }, "isLocked": false, "linkedName": null @@ -116,15 +116,36 @@ "y": 0.7103995901636302 }, "prevControl": { - "x": 5.154713114748167, - "y": 1.9810963114751057 + "x": 5.013218512474755, + "y": 1.7634618654482566 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 1.3462614954018397, + "rotationDegrees": 147.49861635916233 + }, + { + "waypointRelativePos": 2.1523390643742495, + "rotationDegrees": -91.35349039382996 + }, + { + "waypointRelativePos": 2.9696121551379466, + "rotationDegrees": 62.38625776485705 + }, + { + "waypointRelativePos": 4.648940423830468, + "rotationDegrees": -153.59782498299396 + }, + { + "waypointRelativePos": 6.563374650139956, + "rotationDegrees": 0.0 + } + ], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4b660fb..b5b19d5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -44,7 +44,9 @@ public Robot() { // RobotState.resetKalman(); Drive.getInstance(); - VisionDeviceManager.getInstance(); + if (Robot.isReal()) { + VisionDeviceManager.getInstance(); + } TelemetryManager.getInstance(); autoChooser = new AutoSelector(); diff --git a/src/main/java/frc/robot/auto/AutoRoutines.java b/src/main/java/frc/robot/auto/AutoRoutines.java index 09128d2..df3e4da 100644 --- a/src/main/java/frc/robot/auto/AutoRoutines.java +++ b/src/main/java/frc/robot/auto/AutoRoutines.java @@ -11,12 +11,12 @@ public final class AutoRoutines { public static Command testPidToPose() { - return new PIDToPoseCommand(new Pose2d(3, 0.5, Rotation2d.fromDegrees(50))); + return new PIDToPoseCommand(new Pose2d(3, 3, Rotation2d.fromDegrees(50))); } public static Command testTrajectoryAuto() { RedTrajectory traj = TrajectoryLoader.loadAutoTrajectory(TrajectoryType.PATHPLANNER, - "zisen").get(); + "testPath").get(); return new TrajectoryCommand(traj); } } diff --git a/src/main/java/frc/robot/lib/control/PIDVController.java b/src/main/java/frc/robot/lib/control/PIDVController.java index 9d57226..38a852a 100644 --- a/src/main/java/frc/robot/lib/control/PIDVController.java +++ b/src/main/java/frc/robot/lib/control/PIDVController.java @@ -34,6 +34,7 @@ public PIDVController(PIDFConstants constants) { public PIDVController(PIDConstants constants) { this(new PIDFConstants(constants)); } + /** * Makes the controller continuous, which means that values repeat. * @param minInput The minimum value. diff --git a/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java b/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java index 2fb6fc0..e467aff 100644 --- a/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java +++ b/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java @@ -2,56 +2,76 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Pair; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.Timer; import frc.robot.lib.control.ControlConstants.*; +/** + * A profiled version of {@link PIDVController} that uses a TrapezoidProfile + * to smoothly reach the target position with velocity and acceleration limits. + * The derivative term is based on velocity feedback (like PIDV). + */ public class ProfiledPIDVController { - private final ProfiledPIDFConstants constants; + private final PIDFConstants constants; + private TrapezoidProfile.Constraints constraints; + private TrapezoidProfile profile; + + private TrapezoidProfile.State goal = new TrapezoidProfile.State(); + private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); - private double target = 0.0; private Pair measurement = new Pair<>(0.0, 0.0); private double feedforward = 0.0; private double integral = 0.0; public double error = 0.0; - private boolean continuous = false; + private boolean isContinuous = false; private double minRange = 0.0; private double maxRange = 0.0; private final Timer timer = new Timer(); /** - * A {@link PIDVController} with a trapezoid profile, used for limiting speed and acceleration. - * @param constants The {@link ProfiledPIDFConstants}. + * Creates a profiled PIDV controller. + * @param constants The {@link PIDFConstants}. + * @param constraints The trapezoidal motion constraints (max velocity and acceleration). */ public ProfiledPIDVController(ProfiledPIDFConstants constants) { - this.constants = constants; + this.constants = new PIDFConstants( + constants.kP, + constants.kI, + constants.kD, + constants.kF); + this.constraints = constants.constraints; + this.profile = new TrapezoidProfile(constraints); timer.start(); } - /** - * Makes the controller continuous, which means that values repeat. - * @param minInput The minimum value. - * @param maxInput The maximum value. - */ + /** Enables continuous input. */ public void enableContinuousInput(double minInput, double maxInput) { - continuous = true; - this.minRange = minInput; - this.maxRange = maxInput; + isContinuous = true; + minRange = minInput; + maxRange = maxInput; } - /** Makes the controller discontinuous */ + /** Disables continuous input. */ public void disableContinuousInput() { - continuous = false; + isContinuous = false; } + /** Sets the current position and velocity measurement. */ public void setInput(Pair input) { - measurement = input; + this.measurement = input; } + /** Sets the goal position for the profile (end velocity = 0). */ public void setTarget(Double target) { - this.target = target; + this.goal = new TrapezoidProfile.State(target, 0.0); + } + + /** Sets the goal with both position and end velocity. */ + public void setTarget(Double position, Double velocity) { + this.goal = new TrapezoidProfile.State(position, velocity); } /** Sets the feedforward value. */ @@ -59,38 +79,66 @@ public void setFeedforward(Double feedforward) { this.feedforward = feedforward; } + /** + * Computes the controller output using the current profile and measurements. + * @return Control output (PID + feedforward). + */ public Double getOutput() { double dt = timer.get(); timer.reset(); - if (dt <= 0.0) return 0.0; + if (dt <= 0.0) { + return 0.0; + } + + setpoint = profile.calculate(dt, goal, setpoint); double position = measurement.getFirst(); double velocity = measurement.getSecond(); - error = continuous - ? MathUtil.inputModulus(target - position, -(maxRange - minRange) / 2.0, (maxRange - minRange) / 2.0) - : target - position; + double targetPosition = setpoint.position; + double targetVelocity = setpoint.velocity; + + error = isContinuous + ? MathUtil.inputModulus(targetPosition - position, -(maxRange - minRange) / 2.0, (maxRange - minRange) / 2.0) + : targetPosition - position; integral += error * dt; - double derivative = feedforward - velocity; + + double derivative = targetVelocity - velocity; return constants.kP * error - + constants.kI * integral - + constants.kD * derivative - + constants.kF * feedforward; + + constants.kI * integral + + constants.kD * derivative + + constants.kF * feedforward; } - /** Sets the integral value. */ + /** Sets the integral term directly. */ public void setIntegral(double integral) { this.integral = integral; } - /** Resets the controller. */ + /** Resets the controller state. */ public void reset() { integral = 0.0; feedforward = 0.0; error = 0.0; + setpoint = new TrapezoidProfile.State(measurement.getFirst(), measurement.getSecond()); timer.reset(); timer.start(); } + + /** Returns the current motion profile setpoint. */ + public TrapezoidProfile.State getSetpoint() { + return setpoint; + } + + /** Returns the goal state. */ + public TrapezoidProfile.State getGoal() { + return goal; + } + + /** Updates motion constraints. */ + public void setConstraints(TrapezoidProfile.Constraints constraints) { + this.constraints = constraints; + } } diff --git a/src/main/java/frc/robot/lib/util/Util.java b/src/main/java/frc/robot/lib/util/Util.java index 703f41c..7f802b4 100644 --- a/src/main/java/frc/robot/lib/util/Util.java +++ b/src/main/java/frc/robot/lib/util/Util.java @@ -232,6 +232,10 @@ public static boolean chassisSpeedsEpsilonEquals(ChassisSpeeds speed1, ChassisSp && MathUtils.epsilonEquals(speed1.omegaRadiansPerSecond, other.omegaRadiansPerSecond, epsilon); } + public static double chassisSpeedsMagnitude(ChassisSpeeds other) { + return Math.sqrt(other.vxMetersPerSecond * other.vxMetersPerSecond + + other.vyMetersPerSecond * other.vyMetersPerSecond); + } public static Translation2d translateBy(Translation2d a, Translation2d b){ return new Translation2d(a.getX()+b.getX(),a.getY()+b.getY()); diff --git a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java index 06272da..e975609 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.lib.control.ControlConstants.ProfiledPIDFConstants; +import frc.robot.lib.util.Util; import frc.robot.lib.control.ProfiledPIDVController; import frc.robot.subsystems.drive.Drive; @@ -18,8 +19,7 @@ public class PIDToPoseCommand extends Command { private final SwerveRequest.ApplyFieldSpeeds request = new SwerveRequest.ApplyFieldSpeeds(); - private final ProfiledPIDVController xController; - private final ProfiledPIDVController yController; + private final ProfiledPIDVController translationController; private final ProfiledPIDVController thetaController; private final Pose2d target; @@ -37,8 +37,7 @@ public PIDToPoseCommand(Pose2d target) { public PIDToPoseCommand(Drive drive, Pose2d target, ProfiledPIDFConstants translationConstants, ProfiledPIDFConstants rotationConstants) { this.drive = drive; this.target = target; - xController = new ProfiledPIDVController(translationConstants); - yController = new ProfiledPIDVController(translationConstants); + translationController = new ProfiledPIDVController(translationConstants); thetaController = new ProfiledPIDVController(rotationConstants); thetaController.enableContinuousInput(-Math.PI, Math.PI); @@ -68,15 +67,19 @@ public ChassisSpeeds calculateSpeeds() { if (target == null || currentPose == null || currentSpeeds == null) { return new ChassisSpeeds(); } + + var delta = target.relativeTo(currentPose); - xController.setTarget(target.getX()); - yController.setTarget(target.getY()); + translationController.setTarget(delta.getTranslation().getNorm()); - xController.setInput(new Pair(currentPose.getX(), currentSpeeds.vxMetersPerSecond)); - yController.setInput(new Pair(currentPose.getY(), currentSpeeds.vyMetersPerSecond)); + translationController.setInput(new Pair( + 0.0, + Util.chassisSpeedsMagnitude( + currentSpeeds))); + + double vMagnitude = translationController.getOutput(); - double vx = xController.getOutput(); - double vy = yController.getOutput(); + var deltaRotation = delta.getTranslation().getAngle(); thetaController.setTarget(target.getRotation().getRadians()); thetaController.setInput( @@ -85,21 +88,16 @@ public ChassisSpeeds calculateSpeeds() { double rotation = thetaController.getOutput(); - return ChassisSpeeds.fromFieldRelativeSpeeds( - vx, - vy, - rotation, - currentPose.getRotation()); + return new ChassisSpeeds( + vMagnitude * deltaRotation.getCos(), + vMagnitude * deltaRotation.getSin(), + rotation); } @Override public boolean isFinished() { - if (Math.hypot(xController.error, yController.error) < 0.03 - && MathUtil.isNear(thetaController.error / Math.PI * 180, 0, 3.0)) - { - return true; - } - return false; + return translationController.error < 0.03 + && MathUtil.isNear(thetaController.error / Math.PI * 180, 0, 3.0); } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java index d275ba3..1760731 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java @@ -5,19 +5,17 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; -// import frc.robot.RobotState; import frc.robot.lib.control.ControlConstants.PIDFConstants; import frc.robot.lib.control.ControlConstants.ProfiledPIDFConstants; import frc.robot.lib.control.PIDVController; import frc.robot.lib.control.ProfiledPIDVController; import frc.robot.lib.trajectory.RedTrajectory; +import frc.robot.subsystems.TelemetryManager; import frc.robot.subsystems.drive.Drive; /** @@ -37,14 +35,10 @@ public class TrajectoryCommand extends Command { private final RedTrajectory trajectory; private Pose2d currentPose; private ChassisSpeeds currentSpeeds; + private RedTrajectory.State targetState = new RedTrajectory.State(); private Timer timer = null; - static Field2d field = new Field2d(); - static { - SmartDashboard.putData("debug", field); - } - public TrajectoryCommand(RedTrajectory trajectory) { this( Drive.getInstance(), @@ -71,6 +65,10 @@ public TrajectoryCommand(Drive drive, RedTrajectory trajectory, PIDFConstants tr timer = new Timer(); addRequirements(drive); + TelemetryManager.getInstance() + .addStructPublisher("Debug/TrajectoryCommand", + Pose3d.struct, () -> new Pose3d( + targetState.pose)); setName("Trajectory " + trajectory.name); } @@ -98,7 +96,7 @@ public ChassisSpeeds calculateSpeeds() { return new ChassisSpeeds(); } - RedTrajectory.State targetState = trajectory.advanceTo(timer.get()); + targetState = trajectory.advanceTo(timer.get()); double vxFF = targetState.speeds.vxMetersPerSecond; double vyFF = targetState.speeds.vyMetersPerSecond; @@ -136,9 +134,6 @@ public ChassisSpeeds calculateSpeeds() { double rotation = thetaController.getOutput(); - field.setRobotPose(targetState.pose); - SmartDashboard.putData("debug", field); - return new ChassisSpeeds( vx + xAccelFF * accelConstant, vy + yAccelFF * accelConstant, From fd640cbaa85ab8604d9cce2e8113643e5b860dc3 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Sun, 19 Oct 2025 15:02:26 -0700 Subject: [PATCH 008/118] fix: pid to pose --- .../deploy/pathplanner/paths/testPath2.path | 91 ++++++++++ src/main/deploy/pathplanner/paths/zisen.path | 171 ------------------ src/main/java/frc/robot/Robot.java | 7 + .../java/frc/robot/auto/AutoRoutines.java | 5 +- .../drive/commands/PIDToPoseCommand.java | 12 +- 5 files changed, 108 insertions(+), 178 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/testPath2.path delete mode 100644 src/main/deploy/pathplanner/paths/zisen.path diff --git a/src/main/deploy/pathplanner/paths/testPath2.path b/src/main/deploy/pathplanner/paths/testPath2.path new file mode 100644 index 0000000..bb4596e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/testPath2.path @@ -0,0 +1,91 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.0, + "y": 0.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.6543032786885246, + "y": 0.15896516393442653 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.194, + "y": 0.9022028688524597 + }, + "prevControl": { + "x": 2.1700245901639343, + "y": 0.4107069672131138 + }, + "nextControl": { + "x": 2.236158480441017, + "y": 1.7664517178933052 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.4265368852459013, + "y": 1.6094774590163934 + }, + "prevControl": { + "x": 1.798155737707119, + "y": 1.6574282786944752 + }, + "nextControl": { + "x": 0.3915104179993174, + "y": 1.4759256567752452 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.0, + "y": 0.0 + }, + "prevControl": { + "x": 0.45553278688744697, + "y": 0.9501536885305403 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.57, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/zisen.path b/src/main/deploy/pathplanner/paths/zisen.path deleted file mode 100644 index 092f40d..0000000 --- a/src/main/deploy/pathplanner/paths/zisen.path +++ /dev/null @@ -1,171 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 0.0, - "y": 0.0 - }, - "prevControl": null, - "nextControl": { - "x": 0.38428120678715416, - "y": 0.028820911848564158 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.0, - "y": 4.102920081967213 - }, - "prevControl": { - "x": 0.05037403257065813, - "y": 3.702002286192967 - }, - "nextControl": { - "x": -0.03116664423415783, - "y": 4.350969754990947 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.1, - "y": 6.299 - }, - "prevControl": { - "x": 0.10557581830210679, - "y": 5.920374832566059 - }, - "nextControl": { - "x": 0.09434292934831419, - "y": 6.683142596947763 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.5993852458972976, - "y": 3.647387295081966 - }, - "prevControl": { - "x": 0.11181482097517709, - "y": 4.140432055584381 - }, - "nextControl": { - "x": 1.7288144064715079, - "y": 2.505277147468517 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.082786885239971, - "y": 3.647387295081966 - }, - "prevControl": { - "x": 4.930116297867843, - "y": 3.8558936606051346 - }, - "nextControl": { - "x": 5.230480503895737, - "y": 3.445678110838003 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.22663934425693, - "y": 6.404559426232486 - }, - "prevControl": { - "x": 5.22034016514926, - "y": 6.860885487783113 - }, - "nextControl": { - "x": 5.248769225007602, - "y": 4.801423342477989 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.082786885239971, - "y": 4.39062499999939 - }, - "prevControl": { - "x": 5.356425699711494, - "y": 6.167810932873377 - }, - "nextControl": { - "x": 4.717422490835596, - "y": 2.0177143317431057 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.082786885239971, - "y": 0.7103995901636302 - }, - "prevControl": { - "x": 5.013218512474755, - "y": 1.7634618654482566 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.3462614954018397, - "rotationDegrees": 147.49861635916233 - }, - { - "waypointRelativePos": 2.1523390643742495, - "rotationDegrees": -91.35349039382996 - }, - { - "waypointRelativePos": 2.9696121551379466, - "rotationDegrees": 62.38625776485705 - }, - { - "waypointRelativePos": 4.648940423830468, - "rotationDegrees": -153.59782498299396 - }, - { - "waypointRelativePos": 6.563374650139956, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -171.98615915245963 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b5b19d5..32e5b66 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,6 +1,11 @@ package frc.robot; import frc.robot.auto.AutoSelector; + +import com.pathplanner.lib.commands.FollowPathCommand; +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.commands.PathfindingCommand; + import edu.wpi.first.hal.AllianceStationID; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -49,6 +54,8 @@ public Robot() { } TelemetryManager.getInstance(); + FollowPathCommand.warmupCommand().schedule();; + autoChooser = new AutoSelector(); } diff --git a/src/main/java/frc/robot/auto/AutoRoutines.java b/src/main/java/frc/robot/auto/AutoRoutines.java index df3e4da..5a36154 100644 --- a/src/main/java/frc/robot/auto/AutoRoutines.java +++ b/src/main/java/frc/robot/auto/AutoRoutines.java @@ -11,12 +11,13 @@ public final class AutoRoutines { public static Command testPidToPose() { - return new PIDToPoseCommand(new Pose2d(3, 3, Rotation2d.fromDegrees(50))); + return new PIDToPoseCommand(new Pose2d(1.5, 1, Rotation2d.fromDegrees(90))); } public static Command testTrajectoryAuto() { RedTrajectory traj = TrajectoryLoader.loadAutoTrajectory(TrajectoryType.PATHPLANNER, - "testPath").get(); + "testPath2").get(); return new TrajectoryCommand(traj); } } + diff --git a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java index e975609..c70761c 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java @@ -5,7 +5,9 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.lib.control.ControlConstants.ProfiledPIDFConstants; @@ -68,9 +70,9 @@ public ChassisSpeeds calculateSpeeds() { return new ChassisSpeeds(); } - var delta = target.relativeTo(currentPose); + var delta = target.getTranslation().minus(currentPose.getTranslation()); - translationController.setTarget(delta.getTranslation().getNorm()); + translationController.setTarget(delta.getNorm()); translationController.setInput(new Pair( 0.0, @@ -78,8 +80,8 @@ public ChassisSpeeds calculateSpeeds() { currentSpeeds))); double vMagnitude = translationController.getOutput(); - - var deltaRotation = delta.getTranslation().getAngle(); + SmartDashboard.putNumber("Debug/PIDToPoseCommand/vmag", vMagnitude); + var deltaRotation = delta.getAngle(); thetaController.setTarget(target.getRotation().getRadians()); thetaController.setInput( @@ -102,7 +104,7 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - drive.setSwerveRequest(new SwerveRequest.FieldCentric()); + drive.setSwerveRequest(new SwerveRequest.ApplyRobotSpeeds()); } public Pose2d getTarget() { From 67d078738e6b698417db8c547fba4c8086fec3b8 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Sun, 19 Oct 2025 15:34:22 -0700 Subject: [PATCH 009/118] tested snap and it work :D --- src/main/java/frc/robot/ControlsMapping.java | 4 ++++ src/main/java/frc/robot/auto/AutoRoutines.java | 3 ++- src/main/java/frc/robot/subsystems/drive/Drive.java | 6 +++--- 3 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index 6665969..e116766 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -3,8 +3,10 @@ import static frc.robot.Robot.controller; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.commands.PIDToPoseCommand; //import frc.robot.subsystems.drive.commands.TeleopCommand; import frc.robot.subsystems.drive.ctre.CtreDrive.SysIdRoutineType; @@ -15,6 +17,8 @@ public static void mapTeleopCommand() { Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER); controller.a().onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); + controller.x().whileTrue(new PIDToPoseCommand( + new Pose2d(2.0, 1.0, Rotation2d.fromDegrees(120.0)))); } public static void mapSysId() { diff --git a/src/main/java/frc/robot/auto/AutoRoutines.java b/src/main/java/frc/robot/auto/AutoRoutines.java index 5a36154..f2567db 100644 --- a/src/main/java/frc/robot/auto/AutoRoutines.java +++ b/src/main/java/frc/robot/auto/AutoRoutines.java @@ -11,7 +11,8 @@ public final class AutoRoutines { public static Command testPidToPose() { - return new PIDToPoseCommand(new Pose2d(1.5, 1, Rotation2d.fromDegrees(90))); + return new PIDToPoseCommand( + new Pose2d(2.0, 1.0, Rotation2d.fromDegrees(120))); } public static Command testTrajectoryAuto() { diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 9176245..54e6758 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -114,9 +114,9 @@ public Command teleopCommand() { teleopRequest.withVelocityX(0).withVelocityY(0).withRotationalRate(0); setSwerveRequest(teleopRequest); }).andThen(run(() -> { - double xDesiredRaw = Robot.controller.getLeftY(); - double yDesiredRaw = Robot.controller.getLeftX(); - double rotDesiredRaw = Robot.controller.getRightX(); + double xDesiredRaw = -Robot.controller.getLeftY(); + double yDesiredRaw = -Robot.controller.getLeftX(); + double rotDesiredRaw = -Robot.controller.getRightX(); double xFancy = Util.applyJoystickDeadband(xDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); double yFancy = Util.applyJoystickDeadband(yDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); double rotFancy = Util.applyJoystickDeadband(rotDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); From 206d515e6422a23946d137d69c3e2abf810a6b6a Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Wed, 29 Oct 2025 17:36:38 -0700 Subject: [PATCH 010/118] minor refactor --- src/main/java/frc/robot/ControlsMapping.java | 4 ++- .../frc/robot/lib/control/PIDController.java | 8 ++--- .../frc/robot/lib/control/PIDVController.java | 30 +++++++++---------- .../lib/control/ProfiledPIDVController.java | 27 ++++++++--------- .../robot/lib/localization/FieldLayout.java | 20 +++++++++++++ .../frc/robot/subsystems/drive/Drive.java | 16 ++++++++++ .../commands/ExtendedTrajectoryCommand.java | 4 +-- .../drive/commands/PIDToPoseCommand.java | 7 ++--- .../drive/commands/TrajectoryCommand.java | 7 ++--- 9 files changed, 79 insertions(+), 44 deletions(-) diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index e116766..51a6994 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -17,7 +17,9 @@ public static void mapTeleopCommand() { Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER); controller.a().onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); - controller.x().whileTrue(new PIDToPoseCommand( + controller.leftBumper().whileTrue(Drive.getInstance().autoAlign(true)); + controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false)); + controller.b().whileTrue(new PIDToPoseCommand( new Pose2d(2.0, 1.0, Rotation2d.fromDegrees(120.0)))); } diff --git a/src/main/java/frc/robot/lib/control/PIDController.java b/src/main/java/frc/robot/lib/control/PIDController.java index c2d6ec6..464f113 100644 --- a/src/main/java/frc/robot/lib/control/PIDController.java +++ b/src/main/java/frc/robot/lib/control/PIDController.java @@ -52,20 +52,20 @@ public void disableContinuousInput() { isContinuousInputEnabled = false; } - public void setInput(Double input) { + public void setInput(double input) { this.measurement = input; } - public void setTarget(Double target) { + public void setTarget(double target) { this.target = target; } /** Sets the feedforward value. */ - public void setFeedforward(Double feedforward) { + public void setFeedforward(double feedforward) { this.feedforward = feedforward; } - public Double getOutput() { + public double getOutput() { double dt = timer.get(); timer.reset(); if (dt <= 0.0) { diff --git a/src/main/java/frc/robot/lib/control/PIDVController.java b/src/main/java/frc/robot/lib/control/PIDVController.java index 38a852a..d1de0f3 100644 --- a/src/main/java/frc/robot/lib/control/PIDVController.java +++ b/src/main/java/frc/robot/lib/control/PIDVController.java @@ -8,8 +8,10 @@ public class PIDVController { private final PIDFConstants constants; + private double positionMeasurement = 0.0; + private double velocityMeasurement = 0.0; + private double target = 0.0; - private Pair measurement = new Pair<>(0.0, 0.0); private double feedforward = 0.0; private double integral = 0.0; @@ -51,39 +53,37 @@ public void disableContinuousInput() { isContinuous = false; } - public void setInput(Pair input) { - this.measurement = input; + public void setInput(double positionMeasurement, double velocityMeasurement) { + this.positionMeasurement = positionMeasurement; + this.velocityMeasurement = velocityMeasurement; } - public void setTarget(Double target) { + public void setTarget(double target) { this.target = target; } /** Sets the feedforward value. */ - public void setFeedforward(Double feedforward) { + public void setFeedforward(double feedforward) { this.feedforward = feedforward; } - public Double getOutput() { + public double getOutput() { double dt = timer.get(); timer.reset(); if (dt <= 0.0) return 0.0; - - double position = measurement.getFirst(); - double velocity = measurement.getSecond(); error = isContinuous - ? MathUtil.inputModulus(target - position, -(maxRange - minRange) / 2.0, (maxRange - minRange) / 2.0) - : target - position; + ? MathUtil.inputModulus(target - positionMeasurement, -(maxRange - minRange) / 2.0, (maxRange - minRange) / 2.0) + : target - positionMeasurement; integral += error * dt; - double derivative = feedforward - velocity; + double derivative = feedforward - velocityMeasurement; return constants.kP * error - + constants.kI * integral - + constants.kD * derivative - + constants.kF * feedforward; + + constants.kI * integral + + constants.kD * derivative + + constants.kF * feedforward; } /** Sets the integral value. */ diff --git a/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java b/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java index e467aff..438d037 100644 --- a/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java +++ b/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java @@ -19,7 +19,8 @@ public class ProfiledPIDVController { private TrapezoidProfile.State goal = new TrapezoidProfile.State(); private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); - private Pair measurement = new Pair<>(0.0, 0.0); + private double positionMeasurement = 0.0; + private double velocityMeasurement = 0.0; private double feedforward = 0.0; private double integral = 0.0; @@ -60,22 +61,23 @@ public void disableContinuousInput() { } /** Sets the current position and velocity measurement. */ - public void setInput(Pair input) { - this.measurement = input; + public void setInput(double positionMeasurement, double velocityMeasurement) { + this.positionMeasurement = positionMeasurement; + this.velocityMeasurement = velocityMeasurement; } /** Sets the goal position for the profile (end velocity = 0). */ - public void setTarget(Double target) { + public void setTarget(double target) { this.goal = new TrapezoidProfile.State(target, 0.0); } /** Sets the goal with both position and end velocity. */ - public void setTarget(Double position, Double velocity) { + public void setTarget(double position, double velocity) { this.goal = new TrapezoidProfile.State(position, velocity); } /** Sets the feedforward value. */ - public void setFeedforward(Double feedforward) { + public void setFeedforward(double feedforward) { this.feedforward = feedforward; } @@ -83,7 +85,7 @@ public void setFeedforward(Double feedforward) { * Computes the controller output using the current profile and measurements. * @return Control output (PID + feedforward). */ - public Double getOutput() { + public double getOutput() { double dt = timer.get(); timer.reset(); if (dt <= 0.0) { @@ -92,19 +94,16 @@ public Double getOutput() { setpoint = profile.calculate(dt, goal, setpoint); - double position = measurement.getFirst(); - double velocity = measurement.getSecond(); - double targetPosition = setpoint.position; double targetVelocity = setpoint.velocity; error = isContinuous - ? MathUtil.inputModulus(targetPosition - position, -(maxRange - minRange) / 2.0, (maxRange - minRange) / 2.0) - : targetPosition - position; + ? MathUtil.inputModulus(targetPosition - positionMeasurement, -(maxRange - minRange) / 2.0, (maxRange - minRange) / 2.0) + : targetPosition - positionMeasurement; integral += error * dt; - double derivative = targetVelocity - velocity; + double derivative = targetVelocity - velocityMeasurement; return constants.kP * error + constants.kI * integral @@ -122,7 +121,7 @@ public void reset() { integral = 0.0; feedforward = 0.0; error = 0.0; - setpoint = new TrapezoidProfile.State(measurement.getFirst(), measurement.getSecond()); + setpoint = new TrapezoidProfile.State(positionMeasurement, velocityMeasurement); timer.reset(); timer.start(); } diff --git a/src/main/java/frc/robot/lib/localization/FieldLayout.java b/src/main/java/frc/robot/lib/localization/FieldLayout.java index 8d47028..37032d4 100644 --- a/src/main/java/frc/robot/lib/localization/FieldLayout.java +++ b/src/main/java/frc/robot/lib/localization/FieldLayout.java @@ -1,15 +1,21 @@ package frc.robot.lib.localization; import java.io.IOException; +import java.util.ArrayList; +import java.util.HashSet; +import java.util.List; +import java.util.Set; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Constants; /** * Contains various field dimensions and useful reference points. Dimensions are @@ -35,11 +41,25 @@ public class FieldLayout { public static final double APRITAG_WIDTH = Units.inchesToMeters(6.50); public static final AprilTagFieldLayout APRILTAG_MAP; + public static final List ALIGN_POSES_RIGHT = new ArrayList<>(); + public static final List ALIGN_POSES_LEFT = new ArrayList<>(); + static { try { APRILTAG_MAP = AprilTagFieldLayout.loadFromResource(AprilTagFields.kDefaultField.m_resourceFile); field = new Field2d(); SmartDashboard.putData(field); + int[] reefIds = {6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22}; + Transform2d leftReefTransform = new Transform2d( + Constants.Drive.TRACK_WIDTH / 2 + Units.inchesToMeters(1.5), -Units.inchesToMeters(13.0 / 2), Rotation2d.kPi); + for (int i : reefIds) { + ALIGN_POSES_LEFT.add(APRILTAG_MAP.getTagPose(i).get().toPose2d().transformBy(leftReefTransform)); + } + Transform2d rightReefTransform = new Transform2d( + Constants.Drive.TRACK_WIDTH / 2 + Units.inchesToMeters(1.5), Units.inchesToMeters(13.0 / 2), Rotation2d.kPi); + for (int i : reefIds) { + ALIGN_POSES_RIGHT.add(APRILTAG_MAP.getTagPose(i).get().toPose2d().transformBy(rightReefTransform)); + } } catch (IOException e) { throw new RuntimeException(e); } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 54e6758..17c9703 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.drive; +import java.util.Set; + import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; @@ -21,9 +23,11 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Robot; +import frc.robot.lib.localization.FieldLayout; import frc.robot.lib.util.Util; import frc.robot.subsystems.TelemetryManager; import frc.robot.subsystems.drive.ctre.CtreDriveConstants; +import frc.robot.subsystems.drive.commands.PIDToPoseCommand; import frc.robot.subsystems.drive.ctre.CtreDrive; import frc.robot.subsystems.drive.ctre.CtreDriveTelemetry; @@ -132,6 +136,18 @@ public Command teleopCommand() { }).handleInterrupt(() -> setSwerveRequest(new SwerveRequest.FieldCentric()))).withName("Teleop"); } + public Command autoAlign(boolean left) { + return Commands.defer(() -> { + Pose2d pose; + if (left) { + pose = getPose().nearest(FieldLayout.ALIGN_POSES_LEFT); + } else { + pose = getPose().nearest(FieldLayout.ALIGN_POSES_RIGHT); + } + return new PIDToPoseCommand(pose).withName("Auto Align"); + }, Set.of(this)); + } + public void addVisionUpdate(Pose2d pose, Time timestamp) { getCtreDrive().addVisionMeasurement(pose, timestamp.in(Units.Seconds)); } diff --git a/src/main/java/frc/robot/subsystems/drive/commands/ExtendedTrajectoryCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/ExtendedTrajectoryCommand.java index 8e354d8..5688318 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/ExtendedTrajectoryCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/ExtendedTrajectoryCommand.java @@ -15,11 +15,10 @@ public class ExtendedTrajectoryCommand extends TrajectoryCommand { @SafeVarargs public ExtendedTrajectoryCommand(Drive drive, RedTrajectory trajectory, Pair... triggers) { super(trajectory); - addRequirements(drive); setName("Extended Trajectory Command"); this.triggers = new ArrayList<>(List.of(triggers)); - for (Pair trigger : triggers) { + for (Pair trigger : triggers) { new Trigger(() -> trajectory.progress > trigger.getFirst()).onTrue(trigger.getSecond()); addRequirements(trigger.getSecond().getRequirements()); } @@ -27,6 +26,7 @@ public ExtendedTrajectoryCommand(Drive drive, RedTrajectory trajectory, Pair trigger : triggers) { if (trigger.getSecond().isScheduled() && !trigger.getSecond().isFinished()){ diff --git a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java index c70761c..34c38be 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java @@ -74,10 +74,10 @@ public ChassisSpeeds calculateSpeeds() { translationController.setTarget(delta.getNorm()); - translationController.setInput(new Pair( + translationController.setInput( 0.0, Util.chassisSpeedsMagnitude( - currentSpeeds))); + currentSpeeds)); double vMagnitude = translationController.getOutput(); SmartDashboard.putNumber("Debug/PIDToPoseCommand/vmag", vMagnitude); @@ -85,8 +85,7 @@ public ChassisSpeeds calculateSpeeds() { thetaController.setTarget(target.getRotation().getRadians()); thetaController.setInput( - new Pair( - currentPose.getRotation().getRadians(), currentSpeeds.omegaRadiansPerSecond)); + currentPose.getRotation().getRadians(), currentSpeeds.omegaRadiansPerSecond); double rotation = thetaController.getOutput(); diff --git a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java index 1760731..b2b1ee2 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java @@ -120,8 +120,8 @@ public ChassisSpeeds calculateSpeeds() { xController.setFeedforward(vxFF); yController.setFeedforward(vyFF); - xController.setInput(new Pair(currentPose.getX(), currentSpeeds.vxMetersPerSecond)); - yController.setInput(new Pair(currentPose.getY(), currentSpeeds.vyMetersPerSecond)); + xController.setInput(currentPose.getX(), currentSpeeds.vxMetersPerSecond); + yController.setInput(currentPose.getY(), currentSpeeds.vyMetersPerSecond); double vx = xController.getOutput(); double vy = yController.getOutput(); @@ -129,8 +129,7 @@ public ChassisSpeeds calculateSpeeds() { thetaController.setTarget(targetState.pose.getRotation().getRadians()); thetaController.setFeedforward(targetState.speeds.omegaRadiansPerSecond); thetaController.setInput( - new Pair( - currentPose.getRotation().getRadians(), currentSpeeds.omegaRadiansPerSecond)); + currentPose.getRotation().getRadians(), currentSpeeds.omegaRadiansPerSecond); double rotation = thetaController.getOutput(); From 9f4c42ce1131c2ccfa9d94bc5f487df394565e4d Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Wed, 29 Oct 2025 17:39:49 -0700 Subject: [PATCH 011/118] refactor of the refactor --- .../frc/robot/subsystems/drive/commands/PIDToPoseCommand.java | 2 -- .../frc/robot/subsystems/drive/commands/TrajectoryCommand.java | 1 - 2 files changed, 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java index 34c38be..4dca0f1 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java @@ -3,9 +3,7 @@ import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java index b2b1ee2..e4acfa2 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java @@ -3,7 +3,6 @@ import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; From db9d7da4b241b60b9b35e9b2677b9fbf75f33275 Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Mon, 24 Nov 2025 17:10:40 -0800 Subject: [PATCH 012/118] feat: sysid tuning for drive-motors, verified on robot. --- src/main/java/frc/robot/ControlsMapping.java | 49 +++++++++++++++---- src/main/java/frc/robot/Robot.java | 15 ++++++ .../subsystems/drive/ctre/CtreDrive.java | 30 ++++++++++-- .../drive/ctre/CtreDriveConstants.java | 32 ++++++++---- 4 files changed, 102 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index e116766..af87f1c 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -2,19 +2,23 @@ import static frc.robot.Robot.controller; +import com.ctre.phoenix6.swerve.SwerveRequest; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.commands.PIDToPoseCommand; //import frc.robot.subsystems.drive.commands.TeleopCommand; import frc.robot.subsystems.drive.ctre.CtreDrive.SysIdRoutineType; +import edu.wpi.first.wpilibj2.command.ProxyCommand; public class ControlsMapping { public static void mapTeleopCommand() { Drive.getInstance().setDefaultCommand((Drive.getInstance().teleopCommand())); - // run sysID functions - Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER); + // // run sysID functions + // Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER); controller.a().onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); controller.x().whileTrue(new PIDToPoseCommand( @@ -22,13 +26,38 @@ public static void mapTeleopCommand() { } public static void mapSysId() { - controller.a().onTrue( - Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kForward)); - controller.b().onTrue( - Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kReverse)); - controller.x().onTrue( - Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kForward)); - controller.y().onTrue( - Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kReverse)); + //set up sysID routine type + controller.a().onTrue(new InstantCommand(()->Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.TRANSLATION))); + controller.b().onTrue(new InstantCommand(()->Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.ROTATION))); + controller.back().onTrue(new InstantCommand(()->Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER))); + //map the sysid routine movement directions + controller.leftBumper().and(controller.x()) + .whileTrue( + new ProxyCommand( + ()->Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kForward) + .finallyDo(interrupted->Drive.getInstance().getCtreDrive().setControl(new SwerveRequest.Idle())) + ) + ); + controller.leftBumper().and(controller.y()) + .whileTrue( + new ProxyCommand( + ()->Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kReverse) + .finallyDo(interrupted->Drive.getInstance().getCtreDrive().setControl(new SwerveRequest.Idle())) + ) + ); + controller.rightBumper().and(controller.x()) + .whileTrue( + new ProxyCommand( + ()->Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kForward) + .finallyDo(interrupted->Drive.getInstance().getCtreDrive().setControl(new SwerveRequest.Idle())) + ) + ); + controller.rightBumper().and(controller.y()) + .whileTrue( + new ProxyCommand( + ()->Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kReverse) + .finallyDo(interrupted->Drive.getInstance().getCtreDrive().setControl(new SwerveRequest.Idle())) + ) + ); } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 32e5b66..074bae6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,6 +9,7 @@ import edu.wpi.first.hal.AllianceStationID; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.Timer; @@ -57,6 +58,17 @@ public Robot() { FollowPathCommand.warmupCommand().schedule();; autoChooser = new AutoSelector(); + + //robot data loggers + boolean usbPresent = new java.io.File("/u").exists(); + if (usbPresent) { + DataLogManager.start("/u/logs"); // USB stick + System.out.println("Log/USB mounts OK"); + } else { + DataLogManager.start(); // falls back to /home/lvuser/logs + System.out.println("Log/USB mounts NOT OK"); + } + DriverStation.startDataLog(DataLogManager.getLog()); } /** @@ -129,6 +141,9 @@ public void teleopPeriodic() { public void testInit() { // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); + + //map test commands + ControlsMapping.mapSysId(); } /** This function is called periodically during test mode. */ diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java index f72acfc..183b5ff 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java @@ -52,6 +52,9 @@ public static enum SysIdRoutineType { private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); + //class field + private double m_lastAppliedVolts =0.; + /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( new SysIdRoutine.Config( @@ -59,12 +62,23 @@ public static enum SysIdRoutineType { Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout null, // Use default timeout (10 s) // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdTranslation_State", state.toString()) - ), + // state -> SignalLogger.writeString("SysIdTranslation_State", state.toString()) + null + ), new SysIdRoutine.Mechanism( - output -> setControl(m_translationCharacterization.withVolts(output)), - null, - this + output -> { + m_lastAppliedVolts = output.in(Volts); + setControl(m_translationCharacterization.withVolts(output)); + }, + log -> { + var s = getStateCopy(); + log.motor("drive") + .voltage(Volts.of(m_lastAppliedVolts)) + .linearPosition(Meters.of(s.Pose.getTranslation().getX())) // or avg wheel distance + .linearVelocity(MetersPerSecond.of(getKinematics() + .toChassisSpeeds(s.ModuleStates).vxMetersPerSecond)); + }, + this ) ); @@ -208,12 +222,18 @@ public void setSysIdRoutine(SysIdRoutineType type) { switch(type) { case ROTATION: m_sysIdRoutineToApply = m_sysIdRoutineRotation; + System.out.println("SysID/Rotation routine set"); + break; case STEER: m_sysIdRoutineToApply = m_sysIdRoutineSteer; + System.out.println("SysID/Steer routine set"); + break; case TRANSLATION: m_sysIdRoutineToApply = m_sysIdRoutineTranslation; + System.out.println("SysID/Translation routine set"); + break; default: break; diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java index 986f8ef..d73ba5d 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java @@ -23,7 +23,25 @@ // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class CtreDriveConstants { - // Both sets of gains need to be tuned to your individual robot. + // mechanical and geometric parameters of drive train + public static final double kDriveGearRatio = 6.122448979591837; + public static final double kSteerGearRatio = 21.428571428571427; + public static final Distance kWheelRadius = Inches.of(2); + + // Both sets of gains need to be tuned to your individual robot via sysid. + + // translation sysid results + static double kS_sysid_drive = 0.12903; + static double kV_sysid_drive = 2.3293; + static double kA_sysid_drive = 0.41181; + static double kP_sysid_drive = 2.2622; + static double kD_sysid_drive = 0.; + // covnert sysid gains into CTRE (rotation/s) units + static double kS_ctre_drive = kS_sysid_drive; + static double kV_ctre_drive = kV_sysid_drive * (2.0*Math.PI*kWheelRadius.in(Meters)/kDriveGearRatio); + static double kA_ctre_drive = kA_sysid_drive * (2.0*Math.PI*kWheelRadius.in(Meters)/kDriveGearRatio); + static double kP_ctre_drive = kP_sysid_drive * (2.0*Math.PI*kWheelRadius.in(Meters)/kDriveGearRatio); + static double kD_ctre_drive = kD_sysid_drive * (2.0*Math.PI*kWheelRadius.in(Meters)/kDriveGearRatio); // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput @@ -38,11 +56,11 @@ public class CtreDriveConstants { // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.1) + .withKP(kP_ctre_drive) .withKI(0) - .withKD(0) - .withKS(0) - .withKV(0.124); + .withKD(kD_ctre_drive) + .withKS(kS_ctre_drive) + .withKV(kV_ctre_drive); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors @@ -91,10 +109,6 @@ public class CtreDriveConstants { // This may need to be tuned to your individual robot private static final double kCoupleRatio = 3.5714285714285716; - public static final double kDriveGearRatio = 6.122448979591837; - public static final double kSteerGearRatio = 21.428571428571427; - public static final Distance kWheelRadius = Inches.of(2); - private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; From d3d92457d6bbecab20f7b5e84853bc04adbc5bbb Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Tue, 25 Nov 2025 15:35:45 -0800 Subject: [PATCH 013/118] finetuning steer-drive coupling ratio --- .../frc/robot/subsystems/drive/ctre/CtreDriveConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java index d73ba5d..2e9d6f6 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java @@ -29,7 +29,7 @@ public class CtreDriveConstants { public static final Distance kWheelRadius = Inches.of(2); // Both sets of gains need to be tuned to your individual robot via sysid. - + // translation sysid results static double kS_sysid_drive = 0.12903; static double kV_sysid_drive = 2.3293; @@ -107,7 +107,7 @@ public class CtreDriveConstants { // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.5714285714285716; + private static final double kCoupleRatio = kDriveGearRatio /3.; //(drive gear ratio)/(bevel gear ratio) 3.5714285714285716; private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; From 28e5885a967627b095a459ee1b969e21ce4d933b Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Sat, 6 Dec 2025 21:13:03 -0800 Subject: [PATCH 014/118] Merged from 2025RobotTest --- simgui-ds.json | 21 +- src/main/deploy/choreo/jason.traj | 488 +++++++++++------- src/main/java/frc/robot/Constants.java | 188 +------ src/main/java/frc/robot/ControlsMapping.java | 20 +- src/main/java/frc/robot/Robot.java | 29 +- .../java/frc/robot/auto/AutoRoutines.java | 3 + .../java/frc/robot/auto/AutoSelector.java | 40 +- src/main/java/frc/robot/auto/Automation.java | 3 - .../frc/robot/lib/control/PIDVController.java | 54 +- .../lib/control/ProfiledPIDVController.java | 276 +++++----- .../{localization => field}/FieldLayout.java | 10 +- .../{localization => field}/FieldUtil.java | 2 +- .../java/frc/robot/lib/sim/Mechanisms.java | 11 + .../robot/lib/trajectory/RedTrajectory.java | 3 +- src/main/java/frc/robot/lib/util/Util.java | 39 ++ .../robot/subsystems/DefaultSubsystem.java | 7 - .../frc/robot/subsystems/LoggingManager.java | 7 - .../robot/subsystems/TelemetryManager.java | 48 +- .../frc/robot/subsystems/drive/Drive.java | 154 ++---- .../subsystems/drive/DriveConstants.java | 55 ++ .../commands/ExtendedTrajectoryCommand.java | 2 +- .../drive/commands/PIDToPoseCommand.java | 98 +++- .../drive/commands/TrajectoryCommand.java | 69 +-- .../drive/ctre/CtreDriveConstants.java | 6 +- .../robot/subsystems/vision/VisionDevice.java | 3 +- .../vision/VisionDeviceManager.java | 1 + 26 files changed, 866 insertions(+), 771 deletions(-) rename src/main/java/frc/robot/lib/{localization => field}/FieldLayout.java (90%) rename src/main/java/frc/robot/lib/{localization => field}/FieldUtil.java (99%) create mode 100644 src/main/java/frc/robot/lib/sim/Mechanisms.java delete mode 100644 src/main/java/frc/robot/subsystems/DefaultSubsystem.java delete mode 100644 src/main/java/frc/robot/subsystems/LoggingManager.java create mode 100644 src/main/java/frc/robot/subsystems/drive/DriveConstants.java diff --git a/simgui-ds.json b/simgui-ds.json index e12022f..23c0a81 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -12,27 +12,29 @@ "incKey": 68 }, { - "decKey": 87, - "incKey": 83 + "decKey": 83, + "incKey": 87 }, { "decayRate": 0.0, - "incKey": 82, "keyRate": 0.009999999776482582 }, {}, { - "decKey": 81, - "incKey": 69 + "decKey": 74, + "incKey": 76 } ], "axisCount": 5, - "buttonCount": 4, + "buttonCount": 7, "buttonKeys": [ 90, 88, 67, - 86 + 86, + 81, + 69, + 82 ], "povConfig": [ { @@ -50,10 +52,7 @@ }, { "axisConfig": [ - { - "decKey": 74, - "incKey": 76 - }, + {}, { "decKey": 73, "incKey": 75 diff --git a/src/main/deploy/choreo/jason.traj b/src/main/deploy/choreo/jason.traj index a562371..b7b18b6 100644 --- a/src/main/deploy/choreo/jason.traj +++ b/src/main/deploy/choreo/jason.traj @@ -3,31 +3,38 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":0.0, "y":0.0, "heading":0.0, "intervals":51, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.4578442573547363, "y":4.232089042663574, "heading":0.0, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.869084358215332, "y":2.1505911350250244, "heading":0.0, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":6.280324459075928, "y":3.2428622245788574, "heading":0.0, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.434422016143799, "y":3.1810357570648193, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":7.125288963317871, "y":1.5220201015472412, "heading":2.297438401528901, "intervals":64, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.6629953384399414, "y":3.0058600902557373, "heading":1.0010398833246623, "intervals":49, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.1074931621551514, "y":0.9861889481544496, "heading":0.8709035133255352, "intervals":51, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.951519966125488, "y":2.7791624069213867, "heading":1.0427221780766034, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.0868842601776123, "y":1.0067977905273438, "heading":0.8818720101391697, "intervals":60, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.147773265838623, "y":3.87143349647522, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":3, "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":1.0}}, "enabled":true}], + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":4, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":1, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":5, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"0 mm", "val":0.0}, "y":{"exp":"0 mm", "val":0.0}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":51, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.4578442573547363 m", "val":1.4578442573547363}, "y":{"exp":"4.232089042663574 m", "val":4.232089042663574}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.869084358215332 m", "val":3.869084358215332}, "y":{"exp":"2.1505911350250244 m", "val":2.1505911350250244}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"6.280324459075928 m", "val":6.280324459075928}, "y":{"exp":"3.2428622245788574 m", "val":3.2428622245788574}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.434422016143799 m", "val":7.434422016143799}, "y":{"exp":"3.1810357570648193 m", "val":3.1810357570648193}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"7.125288963317871 m", "val":7.125288963317871}, "y":{"exp":"1.5220201015472412 m", "val":1.5220201015472412}, "heading":{"exp":"2.297438401528901 rad", "val":2.297438401528901}, "intervals":64, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.6629953384399414 m", "val":3.6629953384399414}, "y":{"exp":"3.0058600902557373 m", "val":3.0058600902557373}, "heading":{"exp":"1.0010398833246623 rad", "val":1.0010398833246623}, "intervals":49, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.1074931621551514 m", "val":1.1074931621551514}, "y":{"exp":"0.9861889481544495 m", "val":0.9861889481544496}, "heading":{"exp":"0.8709035133255352 rad", "val":0.8709035133255352}, "intervals":51, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.9515199661254883 m", "val":3.951519966125488}, "y":{"exp":"2.7791624069213867 m", "val":2.7791624069213867}, "heading":{"exp":"1.0427221780766034 rad", "val":1.0427221780766034}, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.0868842601776123 m", "val":1.0868842601776123}, "y":{"exp":"1.0067977905273438 m", "val":1.0067977905273438}, "heading":{"exp":"0.8818720101391697 rad", "val":0.8818720101391697}, "intervals":60, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.147773265838623 m", "val":3.147773265838623}, "y":{"exp":"3.8714334964752197 m", "val":3.87143349647522}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":3, "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"1 m / s ^ 2", "val":1.0}}}, "enabled":true}, - {"from":3, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}], + {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":4, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":1, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":5, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -35,186 +42,283 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,2.47152,4.09774,5.36975,6.91421], + "waypoints":[0.0,2.31607,4.46155,6.64137,8.82436,11.06268], "samples":[ - {"t":0.0, "x":0.0, "y":0.0, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.20669, "ay":2.82309, "alpha":0.0, "fx":[3.51567,3.51567,3.51567,3.51567], "fy":[48.01996,48.01996,48.01996,48.01996]}, - {"t":0.04846, "x":0.00024, "y":0.00331, "heading":0.0, "vx":0.01002, "vy":0.13681, "omega":0.0, "ax":0.21133, "ay":2.82265, "alpha":0.0, "fx":[3.59474,3.59474,3.59474,3.59474], "fy":[48.01241,48.01241,48.01241,48.01241]}, - {"t":0.09692, "x":0.00098, "y":0.01326, "heading":0.0, "vx":0.02026, "vy":0.2736, "omega":0.0, "ax":0.21639, "ay":2.82216, "alpha":0.0, "fx":[3.68067,3.68067,3.68066,3.68066], "fy":[48.00406,48.00406,48.00406,48.00406]}, - {"t":0.14538, "x":0.00221, "y":0.02983, "heading":0.0, "vx":0.03074, "vy":0.41036, "omega":0.0, "ax":0.2219, "ay":2.82161, "alpha":0.0, "fx":[3.7744,3.7744,3.7744,3.7744], "fy":[47.99477,47.99477,47.99477,47.99477]}, - {"t":0.19384, "x":0.00396, "y":0.05303, "heading":0.0, "vx":0.0415, "vy":0.5471, "omega":0.0, "ax":0.22793, "ay":2.821, "alpha":0.0, "fx":[3.87704,3.87704,3.87704,3.87704], "fy":[47.98439,47.98439,47.98439,47.98439]}, - {"t":0.24231, "x":0.00624, "y":0.08286, "heading":0.0, "vx":0.05254, "vy":0.68381, "omega":0.0, "ax":0.23457, "ay":2.82031, "alpha":0.0, "fx":[3.98993,3.98993,3.98993,3.98993], "fy":[47.97271,47.97271,47.97271,47.97271]}, - {"t":0.29077, "x":0.00906, "y":0.11931, "heading":0.0, "vx":0.06391, "vy":0.82049, "omega":0.0, "ax":0.2419, "ay":2.81954, "alpha":0.0, "fx":[4.11467,4.11467,4.11467,4.11467], "fy":[47.9595,47.9595,47.9595,47.9595]}, - {"t":0.33923, "x":0.01244, "y":0.16238, "heading":0.0, "vx":0.07563, "vy":0.95712, "omega":0.0, "ax":0.25005, "ay":2.81865, "alpha":0.0, "fx":[4.25324,4.25324,4.25324,4.25324], "fy":[47.94444,47.94444,47.94444,47.94444]}, - {"t":0.38769, "x":0.0164, "y":0.21207, "heading":0.0, "vx":0.08775, "vy":1.09372, "omega":0.0, "ax":0.25915, "ay":2.81763, "alpha":0.0, "fx":[4.40806,4.40806,4.40806,4.40806], "fy":[47.92713,47.92713,47.92713,47.92713]}, - {"t":0.43615, "x":0.02096, "y":0.26838, "heading":0.0, "vx":0.10031, "vy":1.23026, "omega":0.0, "ax":0.26939, "ay":2.81645, "alpha":0.0, "fx":[4.58217,4.58217,4.58217,4.58217], "fy":[47.90706,47.90706,47.90706,47.90706]}, - {"t":0.48461, "x":0.02614, "y":0.33131, "heading":0.0, "vx":0.11336, "vy":1.36675, "omega":0.0, "ax":0.28098, "ay":2.81507, "alpha":0.0, "fx":[4.77939,4.77939,4.77939,4.77939], "fy":[47.88355,47.88355,47.88355,47.88355]}, - {"t":0.53307, "x":0.03196, "y":0.40085, "heading":0.0, "vx":0.12698, "vy":1.50317, "omega":0.0, "ax":0.29422, "ay":2.81343, "alpha":0.0, "fx":[5.00463,5.00463,5.00463,5.00463], "fy":[47.85568,47.85568,47.85568,47.85568]}, - {"t":0.58153, "x":0.03846, "y":0.477, "heading":0.0, "vx":0.14124, "vy":1.63952, "omega":0.0, "ax":0.30949, "ay":2.81147, "alpha":0.0, "fx":[5.26432,5.26432,5.26432,5.26432], "fy":[47.82223,47.82223,47.82223,47.82223]}, - {"t":0.62999, "x":0.04567, "y":0.55976, "heading":0.0, "vx":0.15624, "vy":1.77576, "omega":0.0, "ax":0.32728, "ay":2.80907, "alpha":0.0, "fx":[5.56697,5.56697,5.56697,5.56697], "fy":[47.78142,47.78142,47.78142,47.78142]}, - {"t":0.67846, "x":0.05362, "y":0.64911, "heading":0.0, "vx":0.1721, "vy":1.91189, "omega":0.0, "ax":0.34828, "ay":2.80609, "alpha":0.0, "fx":[5.92416,5.92416,5.92415,5.92415], "fy":[47.73075,47.73075,47.73075,47.73075]}, - {"t":0.72692, "x":0.06237, "y":0.74506, "heading":0.0, "vx":0.18898, "vy":2.04788, "omega":0.0, "ax":0.37343, "ay":2.80231, "alpha":0.0, "fx":[6.35198,6.35198,6.35198,6.35198], "fy":[47.66646,47.66646,47.66646,47.66646]}, - {"t":0.77538, "x":0.07197, "y":0.84759, "heading":0.0, "vx":0.20707, "vy":2.18368, "omega":0.0, "ax":0.40409, "ay":2.79739, "alpha":0.0, "fx":[6.87354,6.87354,6.87354,6.87354], "fy":[47.58276,47.58276,47.58276,47.58276]}, - {"t":0.82384, "x":0.08248, "y":0.9567, "heading":0.0, "vx":0.22666, "vy":2.31925, "omega":0.0, "ax":0.44229, "ay":2.79078, "alpha":0.0, "fx":[7.52315,7.52315,7.52315,7.52315], "fy":[47.47029,47.47029,47.47029,47.47029]}, - {"t":0.8723, "x":0.09398, "y":1.07237, "heading":0.0, "vx":0.24809, "vy":2.45449, "omega":0.0, "ax":0.49113, "ay":2.78153, "alpha":0.0, "fx":[8.35399,8.35399,8.35399,8.35399], "fy":[47.31304,47.31304,47.31304,47.31304]}, - {"t":0.92076, "x":0.10658, "y":1.19458, "heading":0.0, "vx":0.27189, "vy":2.58929, "omega":0.0, "ax":0.55573, "ay":2.76793, "alpha":0.0, "fx":[9.45289,9.45289,9.45289,9.45289], "fy":[47.08173,47.08173,47.08173,47.08173]}, - {"t":0.96922, "x":0.12041, "y":1.32331, "heading":0.0, "vx":0.29882, "vy":2.72342, "omega":0.0, "ax":0.645, "ay":2.74653, "alpha":0.0, "fx":[10.9713,10.9713,10.9713,10.9713], "fy":[46.71769,46.71769,46.71769,46.71769]}, - {"t":1.01768, "x":0.13565, "y":1.45852, "heading":0.0, "vx":0.33008, "vy":2.85652, "omega":0.0, "ax":0.77582, "ay":2.70955, "alpha":0.0, "fx":[13.1964,13.1964,13.1964,13.1964], "fy":[46.08868,46.08868,46.08868,46.08868]}, - {"t":1.06614, "x":0.15256, "y":1.60013, "heading":0.0, "vx":0.36768, "vy":2.98783, "omega":0.0, "ax":0.9838, "ay":2.63635, "alpha":0.0, "fx":[16.73414,16.73414,16.73414,16.73414], "fy":[44.84363,44.84363,44.84363,44.84363]}, - {"t":1.11461, "x":0.17153, "y":1.74802, "heading":0.0, "vx":0.41535, "vy":3.11559, "omega":0.0, "ax":1.35376, "ay":2.4577, "alpha":0.0, "fx":[23.02715,23.02715,23.02715,23.02715], "fy":[41.80482,41.80482,41.80482,41.80482]}, - {"t":1.16307, "x":0.19325, "y":1.90189, "heading":0.0, "vx":0.48096, "vy":3.2347, "omega":0.0, "ax":2.07939, "ay":1.85996, "alpha":0.0, "fx":[35.3698,35.3698,35.3698,35.3698], "fy":[31.63742,31.63742,31.63742,31.63742]}, - {"t":1.21153, "x":0.219, "y":2.06083, "heading":0.0, "vx":0.58173, "vy":3.32483, "omega":0.0, "ax":2.76297, "ay":-0.24201, "alpha":0.0, "fx":[46.99738,46.99738,46.99738,46.99738], "fy":[-4.11649,-4.11649,-4.11649,-4.11648]}, - {"t":1.25999, "x":0.25043, "y":2.22167, "heading":0.0, "vx":0.71562, "vy":3.3131, "omega":0.0, "ax":1.84329, "ay":-2.09719, "alpha":0.0, "fx":[31.35387,31.35387,31.35387,31.35387], "fy":[-35.67253,-35.67253,-35.67253,-35.67253]}, - {"t":1.30845, "x":0.28728, "y":2.37977, "heading":0.0, "vx":0.80495, "vy":3.21147, "omega":0.0, "ax":1.13393, "ay":-2.56779, "alpha":0.0, "fx":[19.2878,19.2878,19.2878,19.2878], "fy":[-43.67745,-43.67745,-43.67745,-43.67745]}, - {"t":1.35691, "x":0.32762, "y":2.53238, "heading":0.0, "vx":0.8599, "vy":3.08703, "omega":0.0, "ax":0.77828, "ay":-2.70479, "alpha":0.0, "fx":[13.23825,13.23825,13.23825,13.23825], "fy":[-46.00773,-46.00773,-46.00773,-46.00773]}, - {"t":1.40537, "x":0.3702, "y":2.67881, "heading":0.0, "vx":0.89762, "vy":2.95596, "omega":0.0, "ax":0.57718, "ay":-2.75907, "alpha":0.0, "fx":[9.8177,9.8177,9.8177,9.8177], "fy":[-46.93095,-46.93095,-46.93095,-46.93095]}, - {"t":1.45383, "x":0.41438, "y":2.81882, "heading":0.0, "vx":0.92559, "vy":2.82225, "omega":0.0, "ax":0.44993, "ay":-2.78538, "alpha":0.0, "fx":[7.65322,7.65322,7.65322,7.65322], "fy":[-47.37858,-47.37858,-47.37858,-47.37858]}, - {"t":1.50229, "x":0.45976, "y":2.95232, "heading":0.0, "vx":0.94739, "vy":2.68727, "omega":0.0, "ax":0.36268, "ay":-2.79995, "alpha":0.0, "fx":[6.16915,6.16915,6.16915,6.16915], "fy":[-47.62629,-47.62629,-47.62629,-47.62629]}, - {"t":1.55076, "x":0.5061, "y":3.07926, "heading":0.0, "vx":0.96497, "vy":2.55158, "omega":0.0, "ax":0.29931, "ay":-2.80878, "alpha":0.0, "fx":[5.09116,5.09116,5.09116,5.09116], "fy":[-47.77654,-47.77654,-47.77654,-47.77654]}, - {"t":1.59922, "x":0.55322, "y":3.19961, "heading":0.0, "vx":0.97947, "vy":2.41546, "omega":0.0, "ax":0.25126, "ay":-2.81451, "alpha":0.0, "fx":[4.2738,4.2738,4.2738,4.2738], "fy":[-47.87398,-47.87398,-47.87398,-47.87398]}, - {"t":1.64768, "x":0.60098, "y":3.31336, "heading":0.0, "vx":0.99165, "vy":2.27907, "omega":0.0, "ax":0.2136, "ay":-2.81842, "alpha":0.0, "fx":[3.63325,3.63325,3.63325,3.63325], "fy":[-47.94048,-47.94048,-47.94048,-47.94048]}, - {"t":1.69614, "x":0.64928, "y":3.4205, "heading":0.0, "vx":1.002, "vy":2.14248, "omega":0.0, "ax":0.18331, "ay":-2.82119, "alpha":0.0, "fx":[3.11799,3.11799,3.11799,3.11799], "fy":[-47.98769,-47.98769,-47.98769,-47.98769]}, - {"t":1.7446, "x":0.69806, "y":3.52101, "heading":0.0, "vx":1.01089, "vy":2.00577, "omega":0.0, "ax":0.15842, "ay":-2.82323, "alpha":0.0, "fx":[2.69468,2.69468,2.69468,2.69468], "fy":[-48.02232,-48.02232,-48.02232,-48.02232]}, - {"t":1.79306, "x":0.74723, "y":3.6149, "heading":0.0, "vx":1.01856, "vy":1.86895, "omega":0.0, "ax":0.13761, "ay":-2.82476, "alpha":0.0, "fx":[2.34078,2.34078,2.34078,2.34078], "fy":[-48.04838,-48.04838,-48.04838,-48.04838]}, - {"t":1.84152, "x":0.79676, "y":3.70215, "heading":0.0, "vx":1.02523, "vy":1.73206, "omega":0.0, "ax":0.11997, "ay":-2.82594, "alpha":0.0, "fx":[2.04057,2.04057,2.04057,2.04057], "fy":[-48.06844,-48.06844,-48.06844,-48.06844]}, - {"t":1.88998, "x":0.84658, "y":3.78277, "heading":0.0, "vx":1.03105, "vy":1.59511, "omega":0.0, "ax":0.10481, "ay":-2.82687, "alpha":0.0, "fx":[1.78272,1.78272,1.78272,1.78272], "fy":[-48.08417,-48.08417,-48.08417,-48.08417]}, - {"t":1.93844, "x":0.89667, "y":3.85675, "heading":0.0, "vx":1.03612, "vy":1.45812, "omega":0.0, "ax":0.09165, "ay":-2.8276, "alpha":0.0, "fx":[1.55887,1.55887,1.55887,1.55887], "fy":[-48.09669,-48.09669,-48.09669,-48.09669]}, - {"t":1.98691, "x":0.94699, "y":3.92409, "heading":0.0, "vx":1.04057, "vy":1.32109, "omega":0.0, "ax":0.08011, "ay":-2.8282, "alpha":0.0, "fx":[1.36272,1.36272,1.36272,1.36272], "fy":[-48.10681,-48.10681,-48.10681,-48.10681]}, - {"t":2.03537, "x":0.99751, "y":3.9848, "heading":0.0, "vx":1.04445, "vy":1.18403, "omega":0.0, "ax":0.06993, "ay":-2.82868, "alpha":0.0, "fx":[1.18945,1.18945,1.18945,1.18945], "fy":[-48.11508,-48.11508,-48.11508,-48.11508]}, - {"t":2.08383, "x":1.04821, "y":4.03885, "heading":0.0, "vx":1.04784, "vy":1.04695, "omega":0.0, "ax":0.06086, "ay":-2.82908, "alpha":0.0, "fx":[1.03527,1.03527,1.03527,1.03527], "fy":[-48.12192,-48.12192,-48.12192,-48.12192]}, - {"t":2.13229, "x":1.09906, "y":4.08627, "heading":0.0, "vx":1.05079, "vy":0.90985, "omega":0.0, "ax":0.05275, "ay":-2.82942, "alpha":0.0, "fx":[0.89719,0.89719,0.89719,0.89719], "fy":[-48.12762,-48.12762,-48.12762,-48.12762]}, - {"t":2.18075, "x":1.15004, "y":4.12704, "heading":0.0, "vx":1.05334, "vy":0.77273, "omega":0.0, "ax":0.04544, "ay":-2.8297, "alpha":0.0, "fx":[0.77284,0.77284,0.77284,0.77284], "fy":[-48.13241,-48.13241,-48.13241,-48.13241]}, - {"t":2.22921, "x":1.20114, "y":4.16116, "heading":0.0, "vx":1.05554, "vy":0.6356, "omega":0.0, "ax":0.03882, "ay":-2.82994, "alpha":0.0, "fx":[0.66025,0.66025,0.66025,0.66025], "fy":[-48.13648,-48.13648,-48.13648,-48.13648]}, - {"t":2.27767, "x":1.25234, "y":4.18864, "heading":0.0, "vx":1.05743, "vy":0.49846, "omega":0.0, "ax":0.0328, "ay":-2.83014, "alpha":0.0, "fx":[0.55784,0.55784,0.55784,0.55784], "fy":[-48.13994,-48.13994,-48.13994,-48.13994]}, - {"t":2.32613, "x":1.30362, "y":4.20947, "heading":0.0, "vx":1.05901, "vy":0.36131, "omega":0.0, "ax":0.0273, "ay":-2.83032, "alpha":0.0, "fx":[0.46429,0.46429,0.46429,0.46429], "fy":[-48.14292,-48.14292,-48.14292,-48.14292]}, - {"t":2.37459, "x":1.35497, "y":4.22366, "heading":0.0, "vx":1.06034, "vy":0.22415, "omega":0.0, "ax":0.02225, "ay":-2.83047, "alpha":0.0, "fx":[0.3785,0.3785,0.3785,0.3785], "fy":[-48.14549,-48.14549,-48.14549,-48.14549]}, - {"t":2.42306, "x":1.40639, "y":4.2312, "heading":0.0, "vx":1.06142, "vy":0.08698, "omega":0.0, "ax":0.01761, "ay":-2.8306, "alpha":0.0, "fx":[0.29955,0.29955,0.29955,0.29955], "fy":[-48.14772,-48.14772,-48.14772,-48.14772]}, - {"t":2.47152, "x":1.45784, "y":4.23209, "heading":0.0, "vx":1.06227, "vy":-0.0502, "omega":0.0, "ax":0.01941, "ay":-2.82991, "alpha":0.0, "fx":[0.33008,0.33008,0.33009,0.33009], "fy":[-48.13603,-48.13603,-48.13603,-48.13603]}, - {"t":2.50934, "x":1.49803, "y":4.22817, "heading":0.0, "vx":1.063, "vy":-0.15722, "omega":0.0, "ax":0.02795, "ay":-2.8297, "alpha":0.0, "fx":[0.47541,0.47541,0.47541,0.47541], "fy":[-48.13231,-48.13231,-48.13231,-48.13231]}, - {"t":2.54716, "x":1.53825, "y":4.2202, "heading":0.0, "vx":1.06406, "vy":-0.26424, "omega":0.0, "ax":0.03736, "ay":-2.82943, "alpha":0.0, "fx":[0.63545,0.63545,0.63545,0.63545], "fy":[-48.12771,-48.12771,-48.12771,-48.12771]}, - {"t":2.58497, "x":1.57852, "y":4.20818, "heading":0.0, "vx":1.06547, "vy":-0.37124, "omega":0.0, "ax":0.04777, "ay":-2.82909, "alpha":0.0, "fx":[0.81254,0.81254,0.81254,0.81254], "fy":[-48.12201,-48.12201,-48.12201,-48.12201]}, - {"t":2.62279, "x":1.61885, "y":4.19212, "heading":0.0, "vx":1.06728, "vy":-0.47824, "omega":0.0, "ax":0.05935, "ay":-2.82867, "alpha":0.0, "fx":[1.00955,1.00955,1.00955,1.00955], "fy":[-48.11489,-48.11489,-48.11489,-48.11489]}, - {"t":2.66061, "x":1.65926, "y":4.17201, "heading":0.0, "vx":1.06952, "vy":-0.58521, "omega":0.0, "ax":0.07231, "ay":-2.82815, "alpha":0.0, "fx":[1.23004,1.23004,1.23004,1.23004], "fy":[-48.10597,-48.10597,-48.10597,-48.10597]}, - {"t":2.69843, "x":1.69976, "y":4.14785, "heading":0.0, "vx":1.07226, "vy":-0.69217, "omega":0.0, "ax":0.08692, "ay":-2.82748, "alpha":0.0, "fx":[1.47845,1.47845,1.47845,1.47845], "fy":[-48.09471,-48.09471,-48.09471,-48.09471]}, - {"t":2.73625, "x":1.74037, "y":4.11965, "heading":0.0, "vx":1.07555, "vy":-0.79911, "omega":0.0, "ax":0.10349, "ay":-2.82664, "alpha":0.0, "fx":[1.76042,1.76042,1.76042,1.76042], "fy":[-48.08037,-48.08037,-48.08037,-48.08037]}, - {"t":2.77407, "x":1.78112, "y":4.08741, "heading":0.0, "vx":1.07946, "vy":-0.90601, "omega":0.0, "ax":0.12247, "ay":-2.82556, "alpha":0.0, "fx":[2.08319,2.08319,2.08319,2.08319], "fy":[-48.06192,-48.06192,-48.06192,-48.06192]}, - {"t":2.81189, "x":1.82203, "y":4.05113, "heading":0.0, "vx":1.08409, "vy":-1.01287, "omega":0.0, "ax":0.1444, "ay":-2.82414, "alpha":0.0, "fx":[2.45627,2.45627,2.45627,2.45627], "fy":[-48.03789,-48.03789,-48.03789,-48.03789]}, - {"t":2.84971, "x":1.86314, "y":4.0108, "heading":0.0, "vx":1.08955, "vy":-1.11967, "omega":0.0, "ax":0.17004, "ay":-2.82228, "alpha":0.0, "fx":[2.89231,2.89231,2.89231,2.89231], "fy":[-48.00612,-48.00612,-48.00612,-48.00612]}, - {"t":2.88753, "x":1.90446, "y":3.96644, "heading":0.0, "vx":1.09598, "vy":-1.22641, "omega":0.0, "ax":0.20039, "ay":-2.81976, "alpha":0.0, "fx":[3.40858,3.40858,3.40858,3.40858], "fy":[-47.96336,-47.96336,-47.96336,-47.96336]}, - {"t":2.92535, "x":1.94606, "y":3.91804, "heading":0.0, "vx":1.10356, "vy":-1.33305, "omega":0.0, "ax":0.23688, "ay":-2.8163, "alpha":0.0, "fx":[4.0292,4.0292,4.0292,4.0292], "fy":[-47.90454,-47.90454,-47.90454,-47.90454]}, - {"t":2.96316, "x":1.98796, "y":3.86561, "heading":0.0, "vx":1.11252, "vy":-1.43956, "omega":0.0, "ax":0.28154, "ay":-2.81142, "alpha":0.0, "fx":[4.78892,4.78892,4.78892,4.78892], "fy":[-47.8215,-47.8215,-47.8215,-47.8215]}, - {"t":3.00098, "x":2.03024, "y":3.80916, "heading":0.0, "vx":1.12317, "vy":-1.54588, "omega":0.0, "ax":0.33743, "ay":-2.8043, "alpha":0.0, "fx":[5.73956,5.73956,5.73956,5.73956], "fy":[-47.70035,-47.70035,-47.70035,-47.70035]}, - {"t":3.0388, "x":2.07296, "y":3.74869, "heading":0.0, "vx":1.13593, "vy":-1.65194, "omega":0.0, "ax":0.40928, "ay":-2.79348, "alpha":0.0, "fx":[6.96167,6.96167,6.96167,6.96167], "fy":[-47.51626,-47.51626,-47.51626,-47.51626]}, - {"t":3.07662, "x":2.11621, "y":3.68422, "heading":0.0, "vx":1.15141, "vy":-1.75759, "omega":0.0, "ax":0.50482, "ay":-2.77614, "alpha":0.0, "fx":[8.58689,8.58689,8.58689,8.58689], "fy":[-47.22141,-47.22141,-47.22141,-47.22141]}, - {"t":3.11444, "x":2.16011, "y":3.61576, "heading":0.0, "vx":1.1705, "vy":-1.86258, "omega":0.0, "ax":0.63751, "ay":-2.74639, "alpha":0.0, "fx":[10.84378,10.84378,10.84378,10.84378], "fy":[-46.71531,-46.71531,-46.71531,-46.71531]}, - {"t":3.15226, "x":2.20484, "y":3.54335, "heading":0.0, "vx":1.19461, "vy":-1.96644, "omega":0.0, "ax":0.83234, "ay":-2.69028, "alpha":0.0, "fx":[14.15784,14.15784,14.15784,14.15784], "fy":[-45.76097,-45.76097,-45.76097,-45.76097]}, - {"t":3.19008, "x":2.25061, "y":3.46706, "heading":0.0, "vx":1.22609, "vy":-2.06819, "omega":0.0, "ax":1.13917, "ay":-2.56973, "alpha":0.0, "fx":[19.37688,19.37688,19.37688,19.37688], "fy":[-43.71041,-43.71041,-43.71041,-43.71041]}, - {"t":3.2279, "x":2.2978, "y":3.38701, "heading":0.0, "vx":1.26917, "vy":-2.16537, "omega":0.0, "ax":1.65554, "ay":-2.26094, "alpha":0.0, "fx":[28.16021,28.16021,28.16021,28.1602], "fy":[-38.45802,-38.45802,-38.45802,-38.45802]}, - {"t":3.26572, "x":2.34698, "y":3.3035, "heading":0.0, "vx":1.33178, "vy":-2.25088, "omega":0.0, "ax":2.44315, "ay":-1.34613, "alpha":0.0, "fx":[41.55724,41.55724,41.55724,41.55723], "fy":[-22.89737,-22.89737,-22.89737,-22.89737]}, - {"t":3.30354, "x":2.39909, "y":3.21741, "heading":0.0, "vx":1.42418, "vy":-2.30179, "omega":0.0, "ax":2.73359, "ay":0.539, "alpha":0.0, "fx":[46.49764,46.49764,46.49764,46.49764], "fy":[9.16825,9.16826,9.16826,9.16825]}, - {"t":3.34136, "x":2.45491, "y":3.13074, "heading":0.0, "vx":1.52756, "vy":-2.28141, "omega":0.0, "ax":2.09391, "ay":1.85688, "alpha":0.0, "fx":[35.61686,35.61686,35.61686,35.61686], "fy":[31.58505,31.58506,31.58506,31.58506]}, - {"t":3.37917, "x":2.51418, "y":3.04579, "heading":0.0, "vx":1.60675, "vy":-2.21118, "omega":0.0, "ax":1.54052, "ay":2.34858, "alpha":0.0, "fx":[26.20372,26.20372,26.20372,26.20372], "fy":[39.94865,39.94865,39.94866,39.94865]}, - {"t":3.41699, "x":2.57604, "y":2.96384, "heading":0.0, "vx":1.66501, "vy":-2.12236, "omega":0.0, "ax":1.20271, "ay":2.54488, "alpha":0.0, "fx":[20.45774,20.45774,20.45774,20.45774], "fy":[43.28775,43.28775,43.28775,43.28775]}, - {"t":3.45481, "x":2.63987, "y":2.8854, "heading":0.0, "vx":1.7105, "vy":-2.02611, "omega":0.0, "ax":0.9899, "ay":2.63899, "alpha":0.0, "fx":[16.83784,16.83784,16.83784,16.83784], "fy":[44.88847,44.88847,44.88847,44.88847]}, - {"t":3.49263, "x":2.70527, "y":2.81066, "heading":0.0, "vx":1.74793, "vy":-1.92631, "omega":0.0, "ax":0.84659, "ay":2.69104, "alpha":0.0, "fx":[14.4003,14.4003,14.4003,14.4003], "fy":[45.77374,45.77374,45.77374,45.77374]}, - {"t":3.53045, "x":2.77198, "y":2.73973, "heading":0.0, "vx":1.77995, "vy":-1.82454, "omega":0.0, "ax":0.74441, "ay":2.72293, "alpha":0.0, "fx":[12.66219,12.66219,12.66219,12.66219], "fy":[46.31629,46.31629,46.31629,46.31629]}, - {"t":3.56827, "x":2.83983, "y":2.67268, "heading":0.0, "vx":1.8081, "vy":-1.72156, "omega":0.0, "ax":0.66819, "ay":2.744, "alpha":0.0, "fx":[11.36564,11.36564,11.36564,11.36564], "fy":[46.67473,46.67473,46.67473,46.67473]}, - {"t":3.60609, "x":2.90869, "y":2.60953, "heading":0.0, "vx":1.83337, "vy":-1.61778, "omega":0.0, "ax":0.60928, "ay":2.75874, "alpha":0.0, "fx":[10.3636,10.3636,10.36359,10.36359], "fy":[46.92532,46.92532,46.92532,46.92532]}, - {"t":3.64391, "x":2.97846, "y":2.55032, "heading":0.0, "vx":1.85642, "vy":-1.51345, "omega":0.0, "ax":0.56244, "ay":2.7695, "alpha":0.0, "fx":[9.56699,9.56698,9.56698,9.56698], "fy":[47.10836,47.10836,47.10836,47.10836]}, - {"t":3.68173, "x":3.04907, "y":2.49507, "heading":0.0, "vx":1.87769, "vy":-1.40871, "omega":0.0, "ax":0.52435, "ay":2.77764, "alpha":0.0, "fx":[8.91904,8.91904,8.91904,8.91904], "fy":[47.24681,47.24681,47.24681,47.24681]}, - {"t":3.71955, "x":3.12046, "y":2.44378, "heading":0.0, "vx":1.89752, "vy":-1.30366, "omega":0.0, "ax":0.49278, "ay":2.78397, "alpha":0.0, "fx":[8.38198,8.38198,8.38198,8.38198], "fy":[47.35452,47.35452,47.35452,47.35452]}, - {"t":3.75737, "x":3.19257, "y":2.39646, "heading":0.0, "vx":1.91615, "vy":-1.19838, "omega":0.0, "ax":0.46619, "ay":2.78901, "alpha":0.0, "fx":[7.92975,7.92975,7.92975,7.92975], "fy":[47.44032,47.44032,47.44032,47.44032]}, - {"t":3.79518, "x":3.26537, "y":2.35314, "heading":0.0, "vx":1.93379, "vy":-1.0929, "omega":0.0, "ax":0.4435, "ay":2.79311, "alpha":0.0, "fx":[7.54382,7.54382,7.54382,7.54382], "fy":[47.51,47.51,47.51,47.51]}, - {"t":3.833, "x":3.33883, "y":2.3138, "heading":0.0, "vx":1.95056, "vy":-0.98726, "omega":0.0, "ax":0.42392, "ay":2.79649, "alpha":0.0, "fx":[7.21068,7.21068,7.21068,7.21068], "fy":[47.56755,47.56755,47.56755,47.56755]}, - {"t":3.87082, "x":3.4129, "y":2.27846, "heading":0.0, "vx":1.96659, "vy":-0.8815, "omega":0.0, "ax":0.40684, "ay":2.79933, "alpha":0.0, "fx":[6.92022,6.92022,6.92022,6.92022], "fy":[47.61577,47.61577,47.61577,47.61577]}, - {"t":3.90864, "x":3.48756, "y":2.24713, "heading":0.0, "vx":1.98198, "vy":-0.77564, "omega":0.0, "ax":0.39182, "ay":2.80173, "alpha":0.0, "fx":[6.66477,6.66477,6.66477,6.66477], "fy":[47.65667,47.65667,47.65667,47.65667]}, - {"t":3.94646, "x":3.5628, "y":2.2198, "heading":0.0, "vx":1.99679, "vy":-0.66968, "omega":0.0, "ax":0.37851, "ay":2.80379, "alpha":0.0, "fx":[6.43838,6.43838,6.43838,6.43838], "fy":[47.69174,47.69174,47.69174,47.69174]}, - {"t":3.98428, "x":3.63859, "y":2.19648, "heading":0.0, "vx":2.01111, "vy":-0.56364, "omega":0.0, "ax":0.36664, "ay":2.80558, "alpha":0.0, "fx":[6.23637,6.23636,6.23636,6.23636], "fy":[47.72211,47.72211,47.72211,47.72211]}, - {"t":4.0221, "x":3.71491, "y":2.17717, "heading":0.0, "vx":2.02498, "vy":-0.45754, "omega":0.0, "ax":0.35597, "ay":2.80714, "alpha":0.0, "fx":[6.05501,6.05501,6.05501,6.05501], "fy":[47.74862,47.74862,47.74862,47.74862]}, - {"t":4.05992, "x":3.79174, "y":2.16187, "heading":0.0, "vx":2.03844, "vy":-0.35137, "omega":0.0, "ax":0.34635, "ay":2.80851, "alpha":0.0, "fx":[5.8913,5.8913,5.8913,5.8913], "fy":[47.77195,47.77195,47.77195,47.77195]}, - {"t":4.09774, "x":3.86908, "y":2.15059, "heading":0.0, "vx":2.05154, "vy":-0.24516, "omega":0.0, "ax":0.3382, "ay":2.80899, "alpha":0.0, "fx":[5.75263,5.75263,5.75262,5.75262], "fy":[47.78004,47.78004,47.78004,47.78004]}, - {"t":4.13035, "x":3.93618, "y":2.14409, "heading":0.0, "vx":2.06257, "vy":-0.15354, "omega":0.0, "ax":0.33041, "ay":2.80972, "alpha":0.0, "fx":[5.62013,5.62013,5.62013,5.62013], "fy":[47.7926,47.7926,47.7926,47.7926]}, - {"t":4.16297, "x":4.00362, "y":2.14058, "heading":0.0, "vx":2.07334, "vy":-0.0619, "omega":0.0, "ax":0.3218, "ay":2.81051, "alpha":0.0, "fx":[5.47366,5.47366,5.47366,5.47366], "fy":[47.80604,47.80604,47.80604,47.80604]}, - {"t":4.19558, "x":4.07142, "y":2.14005, "heading":0.0, "vx":2.08384, "vy":0.02977, "omega":0.0, "ax":0.31223, "ay":2.81136, "alpha":0.0, "fx":[5.31092,5.31092,5.31092,5.31092], "fy":[47.82046,47.82046,47.82046,47.82046]}, - {"t":4.2282, "x":4.13955, "y":2.14252, "heading":0.0, "vx":2.09402, "vy":0.12146, "omega":0.0, "ax":0.30153, "ay":2.81227, "alpha":0.0, "fx":[5.12902,5.12902,5.12902,5.12902], "fy":[47.83591,47.83591,47.83591,47.83591]}, - {"t":4.26082, "x":4.20801, "y":2.14798, "heading":0.0, "vx":2.10386, "vy":0.21319, "omega":0.0, "ax":0.2895, "ay":2.81324, "alpha":0.0, "fx":[4.92439,4.92439,4.92438,4.92438], "fy":[47.85247,47.85247,47.85247,47.85247]}, - {"t":4.29343, "x":4.27678, "y":2.15643, "heading":0.0, "vx":2.1133, "vy":0.30494, "omega":0.0, "ax":0.27587, "ay":2.81428, "alpha":0.0, "fx":[4.69249,4.69249,4.69249,4.69249], "fy":[47.87017,47.87017,47.87017,47.87017]}, - {"t":4.32605, "x":4.34586, "y":2.16787, "heading":0.0, "vx":2.1223, "vy":0.39673, "omega":0.0, "ax":0.26029, "ay":2.81539, "alpha":0.0, "fx":[4.42751,4.42751,4.42751,4.42751], "fy":[47.88902,47.88902,47.88902,47.88902]}, - {"t":4.35866, "x":4.41521, "y":2.18231, "heading":0.0, "vx":2.13079, "vy":0.48856, "omega":0.0, "ax":0.24232, "ay":2.81656, "alpha":0.0, "fx":[4.12185,4.12185,4.12185,4.12185], "fy":[47.90895,47.90895,47.90895,47.90895]}, - {"t":4.39128, "x":4.48484, "y":2.19974, "heading":0.0, "vx":2.13869, "vy":0.58042, "omega":0.0, "ax":0.22137, "ay":2.81778, "alpha":0.0, "fx":[3.76543,3.76543,3.76543,3.76543], "fy":[47.92972,47.92972,47.92972,47.92972]}, - {"t":4.42389, "x":4.55471, "y":2.22017, "heading":0.0, "vx":2.14591, "vy":0.67233, "omega":0.0, "ax":0.19662, "ay":2.81903, "alpha":0.0, "fx":[3.34451,3.34451,3.34451,3.34451], "fy":[47.95083,47.95083,47.95083,47.95083]}, - {"t":4.45651, "x":4.62481, "y":2.2436, "heading":0.0, "vx":2.15232, "vy":0.76427, "omega":0.0, "ax":0.16696, "ay":2.82023, "alpha":0.0, "fx":[2.83999,2.83999,2.83999,2.83999], "fy":[47.97129,47.97129,47.97129,47.97129]}, - {"t":4.48913, "x":4.6951, "y":2.27002, "heading":0.0, "vx":2.15777, "vy":0.85625, "omega":0.0, "ax":0.13077, "ay":2.82127, "alpha":0.0, "fx":[2.22443,2.22443,2.22443,2.22443], "fy":[47.98908,47.98908,47.98908,47.98908]}, - {"t":4.52174, "x":4.76554, "y":2.29945, "heading":0.0, "vx":2.16203, "vy":0.94827, "omega":0.0, "ax":0.08566, "ay":2.82193, "alpha":0.0, "fx":[1.45705,1.45705,1.45705,1.45705], "fy":[48.00024,48.00024,48.00024,48.00024]}, - {"t":4.55436, "x":4.8361, "y":2.33188, "heading":0.0, "vx":2.16483, "vy":1.04031, "omega":0.0, "ax":0.02791, "ay":2.82172, "alpha":0.0, "fx":[0.47468,0.47468,0.47468,0.47468], "fy":[47.99669,47.99669,47.99669,47.99669]}, - {"t":4.58697, "x":4.90673, "y":2.36731, "heading":0.0, "vx":2.16574, "vy":1.13234, "omega":0.0, "ax":-0.04855, "ay":2.81963, "alpha":0.0, "fx":[-0.82584,-0.82584,-0.82584,-0.82584], "fy":[47.96118,47.96118,47.96118,47.96118]}, - {"t":4.61959, "x":4.97734, "y":2.40574, "heading":0.0, "vx":2.16416, "vy":1.22431, "omega":0.0, "ax":-0.15427, "ay":2.81334, "alpha":0.0, "fx":[-2.624,-2.624,-2.624,-2.624], "fy":[47.8541,47.8541,47.8541,47.8541]}, - {"t":4.6522, "x":5.04784, "y":2.44717, "heading":0.0, "vx":2.15912, "vy":1.31607, "omega":0.0, "ax":-0.30915, "ay":2.79692, "alpha":0.0, "fx":[-5.25848,-5.25848,-5.25848,-5.25848], "fy":[47.57473,47.57473,47.57473,47.57473]}, - {"t":4.68482, "x":5.1181, "y":2.49158, "heading":0.0, "vx":2.14904, "vy":1.40729, "omega":0.0, "ax":-0.55467, "ay":2.75296, "alpha":0.0, "fx":[-9.43486,-9.43486,-9.43486,-9.43486], "fy":[46.82702,46.82702,46.82702,46.82702]}, - {"t":4.71744, "x":5.1879, "y":2.53895, "heading":0.0, "vx":2.13095, "vy":1.49708, "omega":0.0, "ax":-0.98651, "ay":2.61888, "alpha":0.0, "fx":[-16.78022,-16.78022,-16.78022,-16.78022], "fy":[44.54646,44.54646,44.54646,44.54646]}, - {"t":4.75005, "x":5.25687, "y":2.58917, "heading":0.0, "vx":2.09877, "vy":1.5825, "omega":0.0, "ax":-1.80643, "ay":2.1144, "alpha":0.0, "fx":[-30.7269,-30.72691,-30.7269,-30.72689], "fy":[35.9653,35.96529,35.9653,35.96531]}, - {"t":4.78267, "x":5.32436, "y":2.64191, "heading":0.0, "vx":2.03986, "vy":1.65146, "omega":0.0, "ax":-2.7403, "ay":0.37503, "alpha":0.0, "fx":[-46.61169,-46.61169,-46.61169,-46.61169], "fy":[6.37911,6.37907,6.37908,6.37912]}, - {"t":4.81528, "x":5.38944, "y":2.69597, "heading":0.0, "vx":1.95048, "vy":1.66369, "omega":0.0, "ax":-2.36037, "ay":-1.47423, "alpha":0.0, "fx":[-40.1493,-40.14929,-40.14928,-40.14929], "fy":[-25.07618,-25.07619,-25.0762,-25.07619]}, - {"t":4.8479, "x":5.4518, "y":2.74945, "heading":0.0, "vx":1.87349, "vy":1.61561, "omega":0.0, "ax":-1.78403, "ay":-2.15763, "alpha":0.0, "fx":[-30.34579,-30.34578,-30.34578,-30.34578], "fy":[-36.70073,-36.70073,-36.70073,-36.70073]}, - {"t":4.88051, "x":5.51196, "y":2.801, "heading":0.0, "vx":1.81531, "vy":1.54524, "omega":0.0, "ax":-1.4416, "ay":-2.41077, "alpha":0.0, "fx":[-24.52124,-24.52124,-24.52123,-24.52123], "fy":[-41.00649,-41.00649,-41.00649,-41.00649]}, - {"t":4.91313, "x":5.5704, "y":2.85011, "heading":0.0, "vx":1.76829, "vy":1.46661, "omega":0.0, "ax":-1.235, "ay":-2.52889, "alpha":0.0, "fx":[-21.00702,-21.00702,-21.00701,-21.00701], "fy":[-43.01571,-43.01571,-43.01572,-43.01572]}, - {"t":4.94574, "x":5.62741, "y":2.8966, "heading":0.0, "vx":1.72801, "vy":1.38412, "omega":0.0, "ax":-1.10006, "ay":-2.59423, "alpha":0.0, "fx":[-18.71168,-18.71168,-18.71168,-18.71168], "fy":[-44.12711,-44.12711,-44.12712,-44.12712]}, - {"t":4.97836, "x":5.68319, "y":2.94037, "heading":0.0, "vx":1.69213, "vy":1.29951, "omega":0.0, "ax":-1.00584, "ay":-2.63478, "alpha":0.0, "fx":[-17.10903,-17.10903,-17.10903,-17.10903], "fy":[-44.81678,-44.81678,-44.81678,-44.81678]}, - {"t":5.01098, "x":5.73784, "y":2.98135, "heading":0.0, "vx":1.65932, "vy":1.21358, "omega":0.0, "ax":-0.9366, "ay":-2.66204, "alpha":0.0, "fx":[-15.93136,-15.93136,-15.93136,-15.93136], "fy":[-45.28055,-45.28055,-45.28055,-45.28055]}, - {"t":5.04359, "x":5.79147, "y":3.01951, "heading":0.0, "vx":1.62877, "vy":1.12675, "omega":0.0, "ax":-0.88369, "ay":-2.68148, "alpha":0.0, "fx":[-15.0313,-15.0313,-15.0313,-15.0313], "fy":[-45.61121,-45.61121,-45.61121,-45.61121]}, - {"t":5.07621, "x":5.84412, "y":3.05484, "heading":0.0, "vx":1.59995, "vy":1.03929, "omega":0.0, "ax":-0.84198, "ay":-2.69597, "alpha":0.0, "fx":[-14.32189,-14.32189,-14.32189,-14.32189], "fy":[-45.85765,-45.85765,-45.85765,-45.85765]}, - {"t":5.10882, "x":5.89585, "y":3.0873, "heading":0.0, "vx":1.57249, "vy":0.95136, "omega":0.0, "ax":-0.80829, "ay":-2.70714, "alpha":0.0, "fx":[-13.74879,-13.74879,-13.74879,-13.74879], "fy":[-46.04775,-46.04775,-46.04775,-46.04775]}, - {"t":5.14144, "x":5.94671, "y":3.11689, "heading":0.0, "vx":1.54613, "vy":0.86307, "omega":0.0, "ax":-0.78052, "ay":-2.71601, "alpha":0.0, "fx":[-13.27637,-13.27637,-13.27637,-13.27637], "fy":[-46.1985,-46.1985,-46.1985,-46.1985]}, - {"t":5.17405, "x":5.99673, "y":3.1436, "heading":0.0, "vx":1.52067, "vy":0.77448, "omega":0.0, "ax":-0.75724, "ay":-2.72319, "alpha":0.0, "fx":[-12.88038,-12.88038,-12.88038,-12.88038], "fy":[-46.32075,-46.32075,-46.32075,-46.32075]}, - {"t":5.20667, "x":6.04592, "y":3.16741, "heading":0.0, "vx":1.49597, "vy":0.68567, "omega":0.0, "ax":-0.73745, "ay":-2.72913, "alpha":0.0, "fx":[-12.54374,-12.54374,-12.54374,-12.54374], "fy":[-46.42176,-46.42176,-46.42176,-46.42176]}, - {"t":5.23929, "x":6.09432, "y":3.18832, "heading":0.0, "vx":1.47192, "vy":0.59665, "omega":0.0, "ax":-0.72042, "ay":-2.73412, "alpha":0.0, "fx":[-12.25408,-12.25408,-12.25408,-12.25408], "fy":[-46.50654,-46.50654,-46.50654,-46.50654]}, - {"t":5.2719, "x":6.14194, "y":3.20633, "heading":0.0, "vx":1.44842, "vy":0.50748, "omega":0.0, "ax":-0.70561, "ay":-2.73836, "alpha":0.0, "fx":[-12.00225,-12.00225,-12.00225,-12.00225], "fy":[-46.57865,-46.57865,-46.57865,-46.57865]}, - {"t":5.30452, "x":6.18881, "y":3.22142, "heading":0.0, "vx":1.42541, "vy":0.41816, "omega":0.0, "ax":-0.69262, "ay":-2.742, "alpha":0.0, "fx":[-11.7813,-11.7813,-11.7813,-11.7813], "fy":[-46.6407,-46.6407,-46.6407,-46.6407]}, - {"t":5.33713, "x":6.23493, "y":3.2336, "heading":0.0, "vx":1.40282, "vy":0.32873, "omega":0.0, "ax":-0.68113, "ay":-2.74517, "alpha":0.0, "fx":[-11.5859,-11.5859,-11.5859,-11.5859], "fy":[-46.69464,-46.69464,-46.69464,-46.69464]}, - {"t":5.36975, "x":6.28032, "y":3.24286, "heading":0.0, "vx":1.3806, "vy":0.2392, "omega":0.0, "ax":-0.27897, "ay":-0.95811, "alpha":0.00421, "fx":[-4.74764,-4.74219,-4.74532,-4.7454], "fy":[-16.27328,-16.36655,-16.27527,-16.27335]}, - {"t":5.40485, "x":6.32861, "y":3.25067, "heading":0.0, "vx":1.37081, "vy":0.20557, "omega":0.00015, "ax":-0.36115, "ay":-0.93136, "alpha":-0.00136, "fx":[-6.14668,-6.14911,-6.14783,-6.12858], "fy":[-15.87984,-15.82964,-15.82965,-15.82939]}, - {"t":5.43995, "x":6.37651, "y":3.25731, "heading":0.00001, "vx":1.35813, "vy":0.17287, "omega":0.0001, "ax":-0.44243, "ay":-0.8956, "alpha":0.00239, "fx":[-7.52787,-7.5235,-7.52542,-7.52555], "fy":[-15.22046,-15.27177,-15.22253,-15.22057]}, - {"t":5.47505, "x":6.42391, "y":3.26283, "heading":0.00001, "vx":1.3426, "vy":0.14144, "omega":0.00018, "ax":-0.52106, "ay":-0.85225, "alpha":-0.00201, "fx":[-8.86141,-8.86441,-8.86283,-8.86377], "fy":[-14.53053,-14.48544,-14.48547,-14.48457]}, - {"t":5.51015, "x":6.47071, "y":3.26727, "heading":0.00002, "vx":1.32431, "vy":0.11152, "omega":0.00011, "ax":-0.59513, "ay":-0.80229, "alpha":0.00203, "fx":[-10.12529,-10.12107,-10.12277,-10.12292], "fy":[-13.6354,-13.67833,-13.63745,-13.63553]}, - {"t":5.54526, "x":6.51683, "y":3.27069, "heading":0.00002, "vx":1.30342, "vy":0.08336, "omega":0.00018, "ax":-0.66307, "ay":-0.74714, "alpha":-0.00225, "fx":[-11.27289,-11.27562,-11.27375,-11.29243], "fy":[-12.73503,-12.70033,-12.70039,-12.69851]}, - {"t":5.58036, "x":6.56218, "y":3.27315, "heading":0.00003, "vx":1.28015, "vy":0.05714, "omega":0.00011, "ax":-0.72382, "ay":-0.68848, "alpha":0.00155, "fx":[-12.31391,-12.31112,-12.31138,-12.31151], "fy":[-11.70267,-11.73334,-11.70463,-11.70279]}, - {"t":5.61546, "x":6.60667, "y":3.27473, "heading":0.00003, "vx":1.25474, "vy":0.03297, "omega":0.00016, "ax":-0.77687, "ay":-0.62805, "alpha":-0.00213, "fx":[-13.20637,-13.20809,-13.20599,-13.23685], "fy":[-10.70029,-10.67816,-10.67824,-10.67541]}, - {"t":5.65056, "x":6.65023, "y":3.2755, "heading":0.00003, "vx":1.22747, "vy":0.01092, "omega":0.00009, "ax":-0.82222, "ay":-0.56745, "alpha":0.00111, "fx":[-13.98741,-13.98569,-13.98486,-13.98498], "fy":[-9.64664,-9.66655,-9.64856,-9.64676]}, - {"t":5.68566, "x":6.69281, "y":3.27554, "heading":0.00004, "vx":1.19861, "vy":-0.009, "omega":0.00012, "ax":-0.86027, "ay":-0.50798, "alpha":-0.00184, "fx":[-14.62422,-14.62519,-14.62294,-14.65956], "fy":[-8.65001,-8.63855,-8.63864,-8.6354]}, - {"t":5.72076, "x":6.73435, "y":3.27491, "heading":0.00004, "vx":1.16842, "vy":-0.02683, "omega":0.00006, "ax":-0.89168, "ay":-0.45066, "alpha":0.00076, "fx":[-15.16868,-15.16785,-15.16612,-15.16623], "fy":[-7.66235,-7.67342,-7.66424,-7.66245]}, - {"t":5.75586, "x":6.77482, "y":3.27369, "heading":0.00004, "vx":1.13712, "vy":-0.04265, "omega":0.00009, "ax":-0.91723, "ay":-0.39616, "alpha":-0.00151, "fx":[-15.59346,-15.59387,-15.59154,-15.62871], "fy":[-6.74238,-6.73833,-6.73842,-6.73532]}, - {"t":5.79097, "x":6.81417, "y":3.27195, "heading":0.00005, "vx":1.10492, "vy":-0.05655, "omega":0.00003, "ax":-0.93776, "ay":-0.34489, "alpha":0.00051, "fx":[-15.95231,-15.95196,-15.94975,-15.94984], "fy":[-5.8648,-5.86976,-5.8667,-5.86489]}, - {"t":5.82607, "x":6.85237, "y":3.26975, "heading":0.00005, "vx":1.072, "vy":-0.06866, "omega":0.00005, "ax":-0.95403, "ay":-0.29704, "alpha":-0.00121, "fx":[-16.22002,-16.22014,-16.21776,-16.25342], "fy":[-5.05253,-5.05343,-5.05351,-5.05053]}, - {"t":5.86117, "x":6.88941, "y":3.26716, "heading":0.00005, "vx":1.03852, "vy":-0.07908, "omega":0.00001, "ax":-0.96678, "ay":-0.25261, "alpha":0.00036, "fx":[-16.44597,-16.44584,-16.44341,-16.44349], "fy":[-4.29607,-4.29731,-4.29801,-4.29615]}, - {"t":5.89627, "x":6.92527, "y":3.26423, "heading":0.00005, "vx":1.00458, "vy":-0.08795, "omega":0.00002, "ax":-0.97663, "ay":-0.21153, "alpha":-0.00094, "fx":[-16.60518,-16.60513,-16.60272,-16.63557], "fy":[-3.59569,-3.5998,-3.59987,-3.59717]}, - {"t":5.93137, "x":6.95993, "y":3.26101, "heading":0.00005, "vx":0.9703, "vy":-0.09538, "omega":-0.00001, "ax":-0.9841, "ay":-0.17364, "alpha":0.00026, "fx":[-16.74059,-16.74057,-16.73804,-16.7381], "fy":[-2.95327,-2.95219,-2.95527,-2.95333]}, - {"t":5.96647, "x":6.99338, "y":3.25755, "heading":0.00005, "vx":0.93576, "vy":-0.10147, "omega":0.0, "ax":-0.98966, "ay":-0.13872, "alpha":-0.00074, "fx":[-16.82759,-16.82752,-16.8251,-16.85512], "fy":[-2.35576,-2.36167,-2.36172,-2.3592]}, - {"t":6.00157, "x":7.02562, "y":3.25391, "heading":0.00005, "vx":0.90102, "vy":-0.10634, "omega":-0.00003, "ax":-0.99367, "ay":-0.10656, "alpha":0.00021, "fx":[-16.90322,-16.90323,-16.90068,-16.90072], "fy":[-1.81268,-1.81029,-1.81474,-1.81273]}, - {"t":6.03667, "x":7.05664, "y":3.25011, "heading":0.00005, "vx":0.86614, "vy":-0.11008, "omega":-0.00002, "ax":-0.99642, "ay":-0.07694, "alpha":-0.00059, "fx":[-16.94332,-16.94326,-16.94085,-16.96806], "fy":[-1.30428,-1.31103,-1.31106,-1.30873]}, - {"t":6.07178, "x":7.08642, "y":3.2462, "heading":0.00005, "vx":0.83116, "vy":-0.11278, "omega":-0.00004, "ax":-0.99818, "ay":-0.04964, "alpha":0.00019, "fx":[-16.98002,-16.98003,-16.9775,-16.97752], "fy":[-0.84454,-0.84171,-0.84667,-0.84457]}, - {"t":6.10688, "x":7.11499, "y":3.24221, "heading":0.00005, "vx":0.79613, "vy":-0.11452, "omega":-0.00004, "ax":-0.99914, "ay":-0.02445, "alpha":-0.00047, "fx":[-16.99009,-16.99007,-16.98766,-17.01241], "fy":[-0.41103,-0.41822,-0.41823,-0.41602]}, - {"t":6.14198, "x":7.14231, "y":3.23817, "heading":0.00005, "vx":0.76105, "vy":-0.11538, "omega":-0.00005, "ax":-0.99946, "ay":-0.00118, "alpha":0.00019, "fx":[-17.00175,-17.00175,-16.99926,-16.99926], "fy":[-0.02017,-0.01734,-0.02236,-0.02017]}, - {"t":6.17708, "x":7.16841, "y":3.23412, "heading":0.00004, "vx":0.72597, "vy":-0.11542, "omega":-0.00004, "ax":-0.99927, "ay":0.02036, "alpha":-0.00038, "fx":[-16.99286,-16.99288,-16.99049,-17.01311], "fy":[0.35105,0.34393,0.34393,0.34609]}, - {"t":6.21218, "x":7.19328, "y":3.23008, "heading":0.00004, "vx":0.6909, "vy":-0.11471, "omega":-0.00006, "ax":-0.99869, "ay":0.04031, "alpha":0.00021, "fx":[-16.9886,-16.98859,-16.98613,-16.98611], "fy":[0.68556,0.6881,0.6833,0.68558]}, - {"t":6.24728, "x":7.21692, "y":3.22608, "heading":0.00004, "vx":0.65584, "vy":-0.11329, "omega":-0.00005, "ax":-0.99778, "ay":0.05883, "alpha":-0.00043, "fx":[-16.96743,-16.96749,-16.96514,-16.98802], "fy":[1.00531,0.99846,0.99849,1.00031]}, - {"t":6.28238, "x":7.23932, "y":3.22214, "heading":0.00004, "vx":0.62082, "vy":-0.11123, "omega":-0.00007, "ax":-0.99664, "ay":0.07605, "alpha":0.00023, "fx":[-16.95374,-16.95373,-16.95129,-16.95125], "fy":[1.29357,1.29562,1.29125,1.29361]}, - {"t":6.31749, "x":7.2605, "y":3.21828, "heading":0.00004, "vx":0.58583, "vy":-0.10856, "omega":-0.00006, "ax":-0.9953, "ay":0.09208, "alpha":-0.00038, "fx":[-16.92553,-16.92563,-16.9233,-16.94453], "fy":[1.57072,1.56419,1.56423,1.5659]}, - {"t":6.35259, "x":7.28045, "y":3.21453, "heading":0.00004, "vx":0.5509, "vy":-0.10533, "omega":-0.00007, "ax":-0.99382, "ay":0.10704, "alpha":0.00026, "fx":[-16.90576,-16.90575,-16.90334,-16.90328], "fy":[1.82089,1.82243,1.81853,1.82096]}, - {"t":6.38769, "x":7.29918, "y":3.2109, "heading":0.00003, "vx":0.51601, "vy":-0.10157, "omega":-0.00006, "ax":-0.99223, "ay":0.12102, "alpha":-0.00033, "fx":[-16.87364,-16.87377,-16.87146,-16.89101], "fy":[2.06259,2.0565,2.05657,2.05819]}, - {"t":6.42279, "x":7.31668, "y":3.20741, "heading":0.00003, "vx":0.48119, "vy":-0.09732, "omega":-0.00007, "ax":-0.99056, "ay":0.1341, "alpha":0.00029, "fx":[-16.85028,-16.85029,-16.84789,-16.84781], "fy":[2.28135,2.28226,2.27895,2.28143]}, - {"t":6.45789, "x":7.33296, "y":3.20407, "heading":0.00003, "vx":0.44642, "vy":-0.09262, "omega":-0.00006, "ax":-0.98883, "ay":0.14636, "alpha":-0.0003, "fx":[-16.81615,-16.8163,-16.81402,-16.83241], "fy":[2.4934,2.48779,2.48787,2.48944]}, - {"t":6.49299, "x":7.34802, "y":3.20091, "heading":0.00003, "vx":0.41171, "vy":-0.08748, "omega":-0.00007, "ax":-0.98707, "ay":0.15788, "alpha":0.00031, "fx":[-16.79097,-16.79098,-16.7886,-16.7885], "fy":[2.68601,2.68633,2.68359,2.68612]}, - {"t":6.52809, "x":7.36186, "y":3.19794, "heading":0.00002, "vx":0.37706, "vy":-0.08194, "omega":-0.00006, "ax":-0.98529, "ay":0.16871, "alpha":-0.00029, "fx":[-16.75608,-16.75625,-16.75398,-16.77149], "fy":[2.8731,2.86803,2.86813,2.86964]}, - {"t":6.5632, "x":7.37449, "y":3.19517, "heading":0.00002, "vx":0.34247, "vy":-0.07601, "omega":-0.00007, "ax":-0.9835, "ay":0.17891, "alpha":0.00034, "fx":[-16.73019,-16.73023,-16.72785,-16.72773], "fy":[3.04386,3.04357,3.04142,3.04399]}, - {"t":6.5983, "x":7.3859, "y":3.19261, "heading":0.00002, "vx":0.30795, "vy":-0.06973, "omega":-0.00006, "ax":-0.98171, "ay":0.18853, "alpha":-0.00028, "fx":[-16.6954,-16.69559,-16.69335,-16.70999], "fy":[3.20985,3.20524,3.20536,3.20686]}, - {"t":6.6334, "x":7.39611, "y":3.19028, "heading":0.00002, "vx":0.27349, "vy":-0.06312, "omega":-0.00007, "ax":-0.97993, "ay":0.19761, "alpha":0.00037, "fx":[-16.66951,-16.66957,-16.66719,-16.66706], "fy":[3.36216,3.36124,3.35969,3.3623]}, - {"t":6.6685, "x":7.40511, "y":3.18818, "heading":0.00001, "vx":0.2391, "vy":-0.05618, "omega":-0.00006, "ax":-0.97817, "ay":0.2062, "alpha":-0.00027, "fx":[-16.63534,-16.63553,-16.63331,-16.64921], "fy":[3.51007,3.50603,3.50616,3.50766]}, - {"t":6.7036, "x":7.4129, "y":3.18634, "heading":0.00001, "vx":0.20476, "vy":-0.04894, "omega":-0.00007, "ax":-0.97643, "ay":0.21434, "alpha":0.0004, "fx":[-16.60989,-16.60998,-16.6076,-16.60745], "fy":[3.6468,3.64533,3.64431,3.64696]}, - {"t":6.7387, "x":7.41948, "y":3.18475, "heading":0.00001, "vx":0.17049, "vy":-0.04142, "omega":-0.00005, "ax":-0.97471, "ay":0.22205, "alpha":-0.00027, "fx":[-16.57667,-16.57686,-16.57467,-16.58992], "fy":[3.77927,3.77569,3.77584,3.77736]}, - {"t":6.7738, "x":7.42487, "y":3.18344, "heading":0.00001, "vx":0.13627, "vy":-0.03362, "omega":-0.00006, "ax":-0.97302, "ay":0.22937, "alpha":0.00043, "fx":[-16.55195,-16.55207,-16.54968,-16.54952], "fy":[3.90266,3.90062,3.90015,3.90283]}, - {"t":6.80891, "x":7.42905, "y":3.1824, "heading":0.00001, "vx":0.10212, "vy":-0.02557, "omega":-0.00005, "ax":-0.97136, "ay":0.23633, "alpha":-0.00026, "fx":[-16.51986,-16.52006,-16.51788,-16.53256], "fy":[4.02181,4.01864,4.0188,4.02035]}, - {"t":6.84401, "x":7.43204, "y":3.18165, "heading":0.0, "vx":0.06802, "vy":-0.01728, "omega":-0.00006, "ax":-0.96974, "ay":0.24295, "alpha":0.00045, "fx":[-16.49605,-16.4962,-16.4938,-16.49362], "fy":[4.1337,4.13116,4.13117,4.1339]}, - {"t":6.87911, "x":7.43383, "y":3.18119, "heading":0.0, "vx":0.03398, "vy":-0.00875, "omega":-0.00004, "ax":-0.96814, "ay":0.24925, "alpha":0.00117, "fx":[-16.46526,-16.47961,-16.46327,-16.46307], "fy":[4.24152,4.23711,4.23848,4.24171]}, - {"t":6.91421, "x":7.43442, "y":3.18104, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":7.12529, "y":1.52202, "heading":2.29744, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.5846, "ay":1.10795, "alpha":-0.96298, "fx":[-41.1586,-45.08819,-46.65187,-42.95469], "fy":[24.9646,16.87258,11.84569,21.70113]}, + {"t":0.03619, "x":7.1236, "y":1.52275, "heading":2.29744, "vx":-0.09353, "vy":0.0401, "omega":-0.03485, "ax":-2.5845, "ay":1.10791, "alpha":-0.96315, "fx":[-41.15663,-45.08687,-46.6504,-42.95257], "fy":[24.96463,16.87171,11.84393,21.7007]}, + {"t":0.07238, "x":7.11852, "y":1.52492, "heading":2.29618, "vx":-0.18706, "vy":0.08019, "omega":-0.0697, "ax":-2.58439, "ay":1.10786, "alpha":-0.96323, "fx":[-41.15238,-45.08264,-46.64927,-42.95498], "fy":[24.96818,16.8783,11.84036,21.69096]}, + {"t":0.10857, "x":7.11006, "y":1.52855, "heading":2.29365, "vx":-0.28059, "vy":0.12028, "omega":-0.10456, "ax":-2.58428, "ay":1.10781, "alpha":-0.96323, "fx":[-41.14586,-45.07549,-46.64846,-42.96194], "fy":[24.9752,16.89235,11.83502,21.67185]}, + {"t":0.14475, "x":7.09821, "y":1.53363, "heading":2.28987, "vx":-0.37411, "vy":0.16037, "omega":-0.13942, "ax":-2.58417, "ay":1.10776, "alpha":-0.96316, "fx":[-41.13708,-45.0654,-46.64794,-42.97342], "fy":[24.98565,16.91388,11.82798,21.64332]}, + {"t":0.18094, "x":7.08298, "y":1.54016, "heading":2.28483, "vx":-0.46763, "vy":0.20046, "omega":-0.17428, "ax":-2.58405, "ay":1.10771, "alpha":-0.963, "fx":[-41.12606,-45.05234,-46.64765,-42.98944], "fy":[24.99945,16.94288,11.81935,21.6053]}, + {"t":0.21713, "x":7.06437, "y":1.54814, "heading":2.27852, "vx":-0.56114, "vy":0.24055, "omega":-0.20913, "ax":-2.58392, "ay":1.10764, "alpha":-0.96277, "fx":[-41.11284,-45.03626,-46.64756,-43.00997], "fy":[25.0165,16.97938,11.80928,21.55768]}, + {"t":0.25332, "x":7.04237, "y":1.55757, "heading":2.27095, "vx":-0.65465, "vy":0.28063, "omega":-0.24397, "ax":-2.58378, "ay":1.10758, "alpha":-0.96247, "fx":[-41.09746,-45.01712,-46.64758,-43.03502], "fy":[25.03667,17.02339,11.79795,21.50034]}, + {"t":0.28951, "x":7.01698, "y":1.56845, "heading":2.26212, "vx":-0.74815, "vy":0.32071, "omega":-0.2788, "ax":-2.58363, "ay":1.10751, "alpha":-0.96209, "fx":[-41.07998,-44.99486,-46.64763,-43.06457], "fy":[25.05982,17.07492,11.78559,21.43314]}, + {"t":0.3257, "x":6.98822, "y":1.58078, "heading":2.25203, "vx":-0.84165, "vy":0.36079, "omega":-0.31361, "ax":-2.58347, "ay":1.10743, "alpha":-0.96164, "fx":[-41.06046,-44.96942,-46.64762,-43.0986], "fy":[25.08574,17.13399,11.77243,21.35592]}, + {"t":0.36189, "x":6.95607, "y":1.59456, "heading":2.24068, "vx":-0.93514, "vy":0.40087, "omega":-0.34841, "ax":-2.58329, "ay":1.10734, "alpha":-0.96112, "fx":[-41.03897,-44.94073,-46.64744,-43.13709], "fy":[25.11425,17.20062,11.75878,21.26851]}, + {"t":0.39807, "x":6.92054, "y":1.60979, "heading":2.22807, "vx":-1.02863, "vy":0.44094, "omega":-0.3832, "ax":-2.5831, "ay":1.10724, "alpha":-0.96055, "fx":[-41.01562,-44.90869,-46.64697,-43.18], "fy":[25.14509,17.2748,11.74496,21.17072]}, + {"t":0.43426, "x":6.88162, "y":1.62647, "heading":2.21421, "vx":-1.12211, "vy":0.48101, "omega":-0.41796, "ax":-2.58289, "ay":1.10714, "alpha":-0.95993, "fx":[-40.99047,-44.87322,-46.64606,-43.22726], "fy":[25.178,17.35655,11.73131,21.06235]}, + {"t":0.47045, "x":6.83932, "y":1.64461, "heading":2.19908, "vx":-1.21558, "vy":0.52108, "omega":-0.4527, "ax":-2.58266, "ay":1.10701, "alpha":-0.95926, "fx":[-40.96365,-44.8342,-46.64456,-43.2788], "fy":[25.21267,17.44587,11.71821,20.9432]}, + {"t":0.50664, "x":6.79364, "y":1.66419, "heading":2.1827, "vx":-1.30904, "vy":0.56114, "omega":-0.48741, "ax":-2.5824, "ay":1.10688, "alpha":-0.95857, "fx":[-40.93526,-44.79151,-46.64229,-43.33451], "fy":[25.24877,17.54274,11.70608,20.81306]}, + {"t":0.54283, "x":6.74458, "y":1.68522, "heading":2.16506, "vx":-1.40249, "vy":0.60119, "omega":-0.5221, "ax":-2.58211, "ay":1.10672, "alpha":-0.95786, "fx":[-40.90539,-44.74502,-46.63905,-43.39425], "fy":[25.2859,17.64714,11.69532,20.6717]}, + {"t":0.57902, "x":6.69213, "y":1.7077, "heading":2.14617, "vx":-1.49594, "vy":0.64124, "omega":-0.55676, "ax":-2.58178, "ay":1.10654, "alpha":-0.95715, "fx":[-40.87417,-44.69455,-46.6346,-43.45785], "fy":[25.32367,17.75904,11.68639,20.51891]}, + {"t":0.61521, "x":6.6363, "y":1.73163, "heading":2.12602, "vx":-1.58937, "vy":0.68129, "omega":-0.5914, "ax":-2.5814, "ay":1.10634, "alpha":-0.95647, "fx":[-40.84168,-44.63991,-46.62868,-43.52504], "fy":[25.36159,17.87837,11.67975,20.35448]}, + {"t":0.65139, "x":6.5771, "y":1.75701, "heading":2.10462, "vx":-1.68278, "vy":0.72133, "omega":-0.62601, "ax":-2.58096, "ay":1.10611, "alpha":-0.95583, "fx":[-40.80799,-44.58088,-46.62098,-43.59553], "fy":[25.39916,18.00504,11.67584,20.17818]}, + {"t":0.68758, "x":6.51451, "y":1.78384, "heading":2.08196, "vx":-1.77619, "vy":0.76135, "omega":-0.6606, "ax":-2.58044, "ay":1.10583, "alpha":-0.95527, "fx":[-40.77313,-44.51717,-46.61111,-43.6689], "fy":[25.43579,18.13893,11.67512,19.9898]}, + {"t":0.72377, "x":6.44854, "y":1.81212, "heading":2.05806, "vx":-1.86957, "vy":0.80137, "omega":-0.69517, "ax":-2.57983, "ay":1.10551, "alpha":-0.95481, "fx":[-40.73708,-44.44842,-46.59863,-43.74462], "fy":[25.47083,18.27989,11.67802,19.78911]}, + {"t":0.75996, "x":6.3792, "y":1.84184, "heading":2.0329, "vx":-1.96293, "vy":0.84138, "omega":-0.72973, "ax":-2.5791, "ay":1.10513, "alpha":-0.9545, "fx":[-40.69968,-44.37417,-46.58293,-43.82197], "fy":[25.50355,18.42768,11.68491,19.57587]}, + {"t":0.79615, "x":6.30647, "y":1.87301, "heading":2.00649, "vx":-2.05626, "vy":0.88137, "omega":-0.76427, "ax":-2.5782, "ay":1.10468, "alpha":-0.95438, "fx":[-40.66062,-44.2938,-46.56324,-43.89998], "fy":[25.5331,18.58199,11.69611,19.34976]}, + {"t":0.83234, "x":6.23037, "y":1.90563, "heading":1.97883, "vx":-2.14956, "vy":0.92135, "omega":-0.79881, "ax":-2.57708, "ay":1.10412, "alpha":-0.95451, "fx":[-40.61927,-44.2064,-46.5385,-43.97727], "fy":[25.55844,18.74236,11.71177,19.11038]}, + {"t":0.86853, "x":6.15089, "y":1.9397, "heading":1.94993, "vx":-2.24283, "vy":0.96131, "omega":-0.83335, "ax":-2.57565, "ay":1.10342, "alpha":-0.95497, "fx":[-40.57448,-44.11065,-46.50714,-44.05184], "fy":[25.57833,18.90814,11.73181,18.85713]}, + {"t":0.90472, "x":6.06804, "y":1.97521, "heading":1.91977, "vx":-2.33603, "vy":1.00124, "omega":-0.86791, "ax":-2.57376, "ay":1.10252, "alpha":-0.95585, "fx":[-40.52416,-44.00444,-46.46676,-44.12062], "fy":[25.59112,19.07833,11.7557,18.58897]}, + {"t":0.9409, "x":5.98182, "y":2.01216, "heading":1.88836, "vx":-2.42918, "vy":1.04114, "omega":-0.9025, "ax":-2.57118, "ay":1.10131, "alpha":-0.95733, "fx":[-40.46437,-43.88418,-46.41334,-44.17844], "fy":[25.59445,19.25125,11.78206,18.30404]}, + {"t":0.97709, "x":5.89223, "y":2.05056, "heading":1.8557, "vx":-2.52222, "vy":1.08099, "omega":-0.93714, "ax":-2.56744, "ay":1.09959, "alpha":-0.95966, "fx":[-40.38728,-43.74314,-46.33944,-44.2159], "fy":[25.58457,19.42392,11.80768,17.99856]}, + {"t":1.01328, "x":5.79927, "y":2.0904, "heading":1.82178, "vx":-2.61513, "vy":1.12078, "omega":-0.97187, "ax":-2.56156, "ay":1.09693, "alpha":-0.96345, "fx":[-40.27556,-43.56702,-46.22943,-44.21329], "fy":[25.55439,19.59013,11.825,17.66406]}, + {"t":1.04947, "x":5.70295, "y":2.13168, "heading":1.78661, "vx":-2.70783, "vy":1.16048, "omega":-1.00674, "ax":-2.55098, "ay":1.0922, "alpha":-0.97016, "fx":[-40.08345,-43.3189,-46.04325,-44.12026], "fy":[25.48696,19.73423,11.81334,17.27773]}, + {"t":1.08566, "x":5.60329, "y":2.17439, "heading":1.75018, "vx":-2.80015, "vy":1.2, "omega":-1.04185, "ax":-2.5265, "ay":1.08137, "alpha":-0.9852, "fx":[-39.64354,-42.86527,-45.6363,-43.75486], "fy":[25.32333,19.80033,11.69632,16.75498]}, + {"t":1.12185, "x":5.5003, "y":2.21852, "heading":1.71248, "vx":-2.89158, "vy":1.23914, "omega":-1.0775, "ax":-2.40851, "ay":1.02944, "alpha":-1.05585, "fx":[-37.50972,-41.06487,-43.76459,-41.53308], "fy":[24.55282,19.31179,10.82083,15.35644]}, + {"t":1.15804, "x":5.39408, "y":2.26404, "heading":1.67349, "vx":-2.97874, "vy":1.27639, "omega":-1.11571, "ax":2.41014, "ay":-1.03437, "alpha":0.83965, "fx":[38.3031,40.93627,43.26184,41.48165], "fy":[-23.13155,-19.22049,-12.3303,-15.69477]}, + {"t":1.19422, "x":5.28786, "y":2.30955, "heading":1.63311, "vx":-2.89152, "vy":1.23896, "omega":-1.08532, "ax":2.52783, "ay":-1.08331, "alpha":0.91804, "fx":[39.96487,42.5726,45.42379,44.0295], "fy":[-24.7578,-20.37327,-12.5106,-16.06575]}, + {"t":1.23041, "x":5.18488, "y":2.35368, "heading":1.59383, "vx":-2.80004, "vy":1.19976, "omega":-1.0521, "ax":2.5518, "ay":-1.09325, "alpha":0.93563, "fx":[40.32492,42.81459,45.83566,44.64635], "fy":[-25.05715,-20.78317,-12.64083,-15.90235]}, + {"t":1.2666, "x":5.08522, "y":2.39638, "heading":1.55576, "vx":-2.7077, "vy":1.16019, "omega":-1.01824, "ax":2.56208, "ay":-1.09751, "alpha":0.94468, "fx":[40.50095,42.85939,45.99243,44.96806], "fy":[-25.15256,-21.07817,-12.76947,-15.67315]}, + {"t":1.30279, "x":4.98891, "y":2.43765, "heading":1.51891, "vx":-2.61498, "vy":1.12047, "omega":-0.98406, "ax":2.56777, "ay":-1.09988, "alpha":0.95079, "fx":[40.61874,42.84074,46.06309,45.1854], "fy":[-25.17397,-21.3285,-12.89967,-15.43226]}, + {"t":1.33898, "x":4.89596, "y":2.47748, "heading":1.4833, "vx":-2.52206, "vy":1.08067, "omega":-0.94965, "ax":2.57137, "ay":-1.10139, "alpha":0.95544, "fx":[40.71224,42.79524,46.0941,45.35133], "fy":[-25.15787,-21.55335,-13.03142,-15.19454]}, + {"t":1.37517, "x":4.80637, "y":2.51586, "heading":1.44893, "vx":-2.429, "vy":1.04081, "omega":-0.91507, "ax":2.57384, "ay":-1.10244, "alpha":0.95921, "fx":[40.79414,42.73704,46.10351,45.48657], "fy":[-25.11896,-21.76018,-13.16411,-14.9656]}, + {"t":1.41136, "x":4.72016, "y":2.55281, "heading":1.41582, "vx":-2.33586, "vy":1.00092, "omega":-0.88036, "ax":2.57564, "ay":-1.10322, "alpha":0.96235, "fx":[40.87003,42.67277,46.09989,45.60101], "fy":[-25.06463,-21.95253,-13.29703,-14.74788]}, + {"t":1.44754, "x":4.63731, "y":2.58831, "heading":1.38396, "vx":-2.24265, "vy":0.96099, "omega":-0.84553, "ax":2.57701, "ay":-1.10383, "alpha":0.96498, "fx":[40.94258,42.606,46.08788,45.70011], "fy":[-24.99923,-22.13236,-13.42945,-14.54244]}, + {"t":1.48373, "x":4.55784, "y":2.62236, "heading":1.35336, "vx":-2.14939, "vy":0.92105, "omega":-0.81061, "ax":2.57808, "ay":-1.10432, "alpha":0.9672, "fx":[41.01311,42.53877,46.07025,45.78715], "fy":[-24.92564,-22.30088,-13.56068,-14.3497]}, + {"t":1.51992, "x":4.48175, "y":2.65497, "heading":1.32402, "vx":-2.05609, "vy":0.88108, "omega":-0.77561, "ax":2.57893, "ay":-1.10473, "alpha":0.96906, "fx":[41.08224,42.47236,46.04879,45.86434], "fy":[-24.846,-22.4589,-13.69008,-14.16968]}, + {"t":1.55611, "x":4.40903, "y":2.68613, "heading":1.29596, "vx":-1.96277, "vy":0.84111, "omega":-0.74054, "ax":2.57964, "ay":-1.10508, "alpha":0.97061, "fx":[41.15018,42.40759,46.02473,45.93321], "fy":[-24.76195,-22.60705,-13.81706,-14.00218]}, + {"t":1.5923, "x":4.33969, "y":2.71585, "heading":1.26916, "vx":-1.86941, "vy":0.80111, "omega":-0.70542, "ax":2.58023, "ay":-1.10538, "alpha":0.97187, "fx":[41.21693,42.34503,45.99895,45.99492], "fy":[-24.67489,-22.74582,-13.94108,-13.84686]}, + {"t":1.62849, "x":4.27373, "y":2.74411, "heading":1.24363, "vx":-1.77604, "vy":0.76111, "omega":-0.67025, "ax":2.58073, "ay":-1.10564, "alpha":0.97288, "fx":[41.28239,42.28504,45.97214,46.05035], "fy":[-24.58598,-22.87562,-14.06166,-13.7033]}, + {"t":1.66468, "x":4.21114, "y":2.77093, "heading":1.21937, "vx":-1.68264, "vy":0.7211, "omega":-0.63504, "ax":2.58116, "ay":-1.10587, "alpha":0.97367, "fx":[41.34637,42.22787,45.9448,46.10022], "fy":[-24.49628,-22.99683,-14.17834,-13.571]}, + {"t":1.70086, "x":4.15194, "y":2.79631, "heading":1.19639, "vx":-1.58924, "vy":0.68108, "omega":-0.5998, "ax":2.58154, "ay":-1.10608, "alpha":0.97427, "fx":[41.40863,42.17369,45.91737,46.14514], "fy":[-24.4067,-23.10977,-14.29074,-13.44945]}, + {"t":1.73705, "x":4.09612, "y":2.82023, "heading":1.17469, "vx":-1.49581, "vy":0.64105, "omega":-0.56455, "ax":2.58187, "ay":-1.10627, "alpha":0.9747, "fx":[41.46893,42.12262,45.8902,46.18561], "fy":[-24.3181,-23.21476,-14.39848,-13.33811]}, + {"t":1.77324, "x":4.04368, "y":2.8427, "heading":1.15426, "vx":-1.40238, "vy":0.60102, "omega":-0.52927, "ax":2.58216, "ay":-1.10644, "alpha":0.975, "fx":[41.52704,42.07472,45.86357,46.22206], "fy":[-24.23124,-23.31209,-14.50124,-13.23646]}, + {"t":1.80943, "x":3.99462, "y":2.86373, "heading":1.1351, "vx":-1.30893, "vy":0.56098, "omega":-0.49399, "ax":2.58243, "ay":-1.1066, "alpha":0.97519, "fx":[41.58271,42.03002,45.83774,46.25486], "fy":[-24.14682,-23.40203,-14.59874,-13.14395]}, + {"t":1.84562, "x":3.94894, "y":2.8833, "heading":1.11723, "vx":-1.21548, "vy":0.52093, "omega":-0.4587, "ax":2.58266, "ay":-1.10674, "alpha":0.97528, "fx":[41.63571,41.98854,45.81293,46.28435], "fy":[-24.06548,-23.48484,-14.69071,-13.06007]}, + {"t":1.88181, "x":3.90665, "y":2.90143, "heading":1.10063, "vx":-1.12202, "vy":0.48088, "omega":-0.4234, "ax":2.58288, "ay":-1.10686, "alpha":0.9753, "fx":[41.68583,41.95027,45.78931,46.31082], "fy":[-23.98779,-23.56078,-14.77693,-12.98432]}, + {"t":1.918, "x":3.86773, "y":2.91811, "heading":1.0853, "vx":-1.02855, "vy":0.44082, "omega":-0.38811, "ax":2.58308, "ay":-1.10698, "alpha":0.97527, "fx":[41.73287,41.91518,45.76707,46.33452], "fy":[-23.91428,-23.63005,-14.85719,-12.91623]}, + {"t":1.95418, "x":3.8322, "y":2.93334, "heading":1.07126, "vx":-0.93507, "vy":0.40076, "omega":-0.35282, "ax":2.58326, "ay":-1.10709, "alpha":0.9752, "fx":[41.77666,41.88325,45.74633,46.35568], "fy":[-23.84544,-23.6929,-14.9313,-12.85535]}, + {"t":1.99037, "x":3.80006, "y":2.94712, "heading":1.05849, "vx":-0.84158, "vy":0.3607, "omega":-0.31752, "ax":2.58342, "ay":-1.10718, "alpha":0.97512, "fx":[41.81704,41.85445,45.72724,46.3745], "fy":[-23.78169,-23.7495,-14.99912,-12.80127]}, + {"t":2.02656, "x":3.77129, "y":2.95944, "heading":1.047, "vx":-0.74809, "vy":0.32063, "omega":-0.28224, "ax":2.58358, "ay":-1.10727, "alpha":0.97502, "fx":[41.85386,41.82873,45.7099,46.39115], "fy":[-23.72343,-23.80006,-15.0605,-12.75361]}, + {"t":2.06275, "x":3.74591, "y":2.97032, "heading":1.03679, "vx":-0.6546, "vy":0.28056, "omega":-0.24695, "ax":2.58372, "ay":-1.10735, "alpha":0.97493, "fx":[41.88699,41.80607,45.69441,46.4058], "fy":[-23.67099,-23.84473,-15.11533,-12.71202]}, + {"t":2.09894, "x":3.72391, "y":2.97975, "heading":1.02785, "vx":-0.5611, "vy":0.24049, "omega":-0.21167, "ax":2.58385, "ay":-1.10743, "alpha":0.97486, "fx":[41.91634,41.78641,45.68085,46.41857], "fy":[-23.62467,-23.88367,-15.16349,-12.6762]}, + {"t":2.13513, "x":3.7053, "y":2.98773, "heading":1.02019, "vx":-0.46759, "vy":0.20041, "omega":-0.17639, "ax":2.58397, "ay":-1.10749, "alpha":0.97481, "fx":[41.94181,41.76974,45.66931,46.42959], "fy":[-23.58475,-23.91702,-15.2049,-12.64586]}, + {"t":2.17132, "x":3.69007, "y":2.99426, "heading":1.01381, "vx":-0.37408, "vy":0.16033, "omega":-0.14111, "ax":2.58408, "ay":-1.10755, "alpha":0.97479, "fx":[41.96331,41.75602,45.65984,46.43895], "fy":[-23.55143,-23.94489,-15.2395,-12.62079]}, + {"t":2.2075, "x":3.67823, "y":2.99933, "heading":1.0087, "vx":-0.28056, "vy":0.12025, "omega":-0.10584, "ax":2.58419, "ay":-1.10761, "alpha":0.9748, "fx":[41.98079,41.74522,45.65251,46.44674], "fy":[-23.52492,-23.96737,-15.26721,-12.60077]}, + {"t":2.24369, "x":3.66976, "y":3.00296, "heading":1.00487, "vx":-0.18705, "vy":0.08017, "omega":-0.07056, "ax":2.58429, "ay":-1.10765, "alpha":0.97486, "fx":[41.9942,41.73732,45.64734,46.45303], "fy":[-23.50534,-23.98456,-15.28798,-12.58567]}, + {"t":2.27988, "x":3.66469, "y":3.00513, "heading":1.00232, "vx":-0.09353, "vy":0.04009, "omega":-0.03528, "ax":2.58438, "ay":-1.1077, "alpha":0.97495, "fx":[42.00351,41.73232,45.64439,46.45786], "fy":[-23.49283,-23.99649,-15.30179,-12.57536]}, + {"t":2.31607, "x":3.663, "y":3.00586, "heading":1.00104, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.3017, "ay":-1.64665, "alpha":-0.12964, "fx":[-38.91341,-39.70123,-39.38614,-38.60453], "fy":[-28.34641,-27.23297,-27.68886,-28.76778]}, + {"t":2.35986, "x":3.66079, "y":3.00428, "heading":1.00104, "vx":-0.10078, "vy":-0.0721, "omega":-0.00568, "ax":-2.30141, "ay":-1.64687, "alpha":-0.1296, "fx":[-38.90828,-39.69602,-39.38113,-38.59961], "fy":[-28.35006,-27.23709,-27.69267,-28.77114]}, + {"t":2.40364, "x":3.65417, "y":2.99955, "heading":1.00079, "vx":-0.20155, "vy":-0.14421, "omega":-0.01135, "ax":-2.30108, "ay":-1.64711, "alpha":-0.12955, "fx":[-38.90255,-39.69028,-39.37579,-38.59431], "fy":[-28.35424,-27.24166,-27.69663,-28.77473]}, + {"t":2.44743, "x":3.64314, "y":2.99165, "heading":1.00029, "vx":-0.3023, "vy":-0.21633, "omega":-0.01702, "ax":-2.30073, "ay":-1.64737, "alpha":-0.1295, "fx":[-38.89615,-39.68395,-39.37007,-38.58855], "fy":[-28.35899,-27.24673,-27.70079,-28.77859]}, + {"t":2.49121, "x":3.6277, "y":2.9806, "heading":0.99955, "vx":-0.40304, "vy":-0.28846, "omega":-0.02269, "ax":-2.30033, "ay":-1.64766, "alpha":-0.12945, "fx":[-38.889,-39.67694,-39.36387,-38.58227], "fy":[-28.36437,-27.25238,-27.70523,-28.78276]}, + {"t":2.535, "x":3.60785, "y":2.96639, "heading":0.99856, "vx":-0.50376, "vy":-0.3606, "omega":-0.02836, "ax":-2.2999, "ay":-1.64798, "alpha":-0.12939, "fx":[-38.88099,-39.66915,-39.35712,-38.57537], "fy":[-28.37045,-27.25867,-27.71,-28.78732]}, + {"t":2.57878, "x":3.58358, "y":2.94902, "heading":0.99731, "vx":-0.60446, "vy":-0.43276, "omega":-0.03403, "ax":-2.29942, "ay":-1.64833, "alpha":-0.12932, "fx":[-38.87202,-39.66047,-39.34968,-38.56774], "fy":[-28.3773,-27.2657,-27.71519,-28.79233]}, + {"t":2.62257, "x":3.55491, "y":2.9285, "heading":0.99582, "vx":-0.70514, "vy":-0.50493, "omega":-0.03969, "ax":-2.29888, "ay":-1.64873, "alpha":-0.12925, "fx":[-38.86193,-39.65074,-39.34142,-38.55923], "fy":[-28.38503,-27.27359,-27.72092,-28.7979]}, + {"t":2.66635, "x":3.52183, "y":2.90481, "heading":0.99409, "vx":-0.8058, "vy":-0.57712, "omega":-0.04535, "ax":-2.29828, "ay":-1.64917, "alpha":-0.12917, "fx":[-38.85055,-39.63977,-39.33217,-38.54967], "fy":[-28.39377,-27.28248,-27.72732,-28.80414]}, + {"t":2.71014, "x":3.48435, "y":2.87796, "heading":0.9921, "vx":-0.90643, "vy":-0.64933, "omega":-0.051, "ax":-2.29759, "ay":-1.64968, "alpha":-0.12908, "fx":[-38.83765,-39.62734,-39.32168,-38.53885], "fy":[-28.40368,-27.29256,-27.73456,-28.81121]}, + {"t":2.75392, "x":3.44246, "y":2.84794, "heading":0.98987, "vx":-1.00703, "vy":-0.72156, "omega":-0.05666, "ax":-2.29681, "ay":-1.65025, "alpha":-0.12897, "fx":[-38.82292,-39.61314,-39.30967,-38.52647], "fy":[-28.41498,-27.30407,-27.74287,-28.81929]}, + {"t":2.79771, "x":3.39616, "y":2.81477, "heading":0.98739, "vx":-1.1076, "vy":-0.79382, "omega":-0.0623, "ax":-2.2959, "ay":-1.65092, "alpha":-0.12885, "fx":[-38.80598,-39.59676,-39.29575,-38.51215], "fy":[-28.42792,-27.31731,-27.75252,-28.82865]}, + {"t":2.84149, "x":3.34547, "y":2.77843, "heading":0.98466, "vx":-1.20813, "vy":-0.8661, "omega":-0.06794, "ax":-2.29485, "ay":-1.65169, "alpha":-0.12871, "fx":[-38.78631,-39.57769,-39.27942,-38.49541], "fy":[-28.44287,-27.33268,-27.7639,-28.83961]}, + {"t":2.88528, "x":3.29037, "y":2.73892, "heading":0.98168, "vx":-1.30861, "vy":-0.93842, "omega":-0.07358, "ax":-2.2936, "ay":-1.6526, "alpha":-0.12855, "fx":[-38.7632,-39.55521,-39.25998,-38.47555], "fy":[-28.46034,-27.35077,-27.77754,-28.85265]}, + {"t":2.92907, "x":3.23087, "y":2.69625, "heading":0.97846, "vx":-1.40903, "vy":-1.01078, "omega":-0.07921, "ax":-2.2921, "ay":-1.6537, "alpha":-0.12835, "fx":[-38.73566,-39.52829,-39.23645,-38.45163], "fy":[-28.481,-27.37233,-27.79416,-28.8684]}, + {"t":2.97285, "x":3.16698, "y":2.65041, "heading":0.97499, "vx":-1.50939, "vy":-1.08319, "omega":-0.08483, "ax":-2.29027, "ay":-1.65504, "alpha":-0.12811, "fx":[-38.70226,-39.49549,-39.20744,-38.42226], "fy":[-28.50586,-27.39849,-27.81482,-28.88779]}, + {"t":3.01664, "x":3.09869, "y":2.60139, "heading":0.97128, "vx":-1.60967, "vy":-1.15566, "omega":-0.09044, "ax":-2.28798, "ay":-1.65671, "alpha":-0.12781, "fx":[-38.66085,-39.45461,-39.17081,-38.38538], "fy":[-28.53641,-27.43094,-27.84108,-28.91223]}, + {"t":3.06042, "x":3.02602, "y":2.5492, "heading":0.96732, "vx":-1.70985, "vy":-1.2282, "omega":-0.09603, "ax":-2.28504, "ay":-1.65885, "alpha":-0.12743, "fx":[-38.60805,-39.40222,-39.12326,-38.33771], "fy":[-28.57497,-27.47231,-27.87541,-28.94388]}, + {"t":3.10421, "x":2.94896, "y":2.49383, "heading":0.96311, "vx":-1.80991, "vy":-1.30083, "omega":-0.10161, "ax":-2.28111, "ay":-1.66171, "alpha":-0.12693, "fx":[-38.53823,-39.33257,-39.05924,-38.27384], "fy":[-28.62544,-27.52695,-27.92188,-28.98638]}, + {"t":3.14799, "x":2.86753, "y":2.43528, "heading":0.95867, "vx":-1.90978, "vy":-1.37359, "omega":-0.10717, "ax":-2.2756, "ay":-1.66569, "alpha":-0.12623, "fx":[-38.44123,-39.23533,-38.96876,-38.18395], "fy":[-28.69475,-27.60268,-27.98783,-29.04621]}, + {"t":3.19178, "x":2.78173, "y":2.37354, "heading":0.95397, "vx":-2.00942, "vy":-1.44652, "omega":-0.1127, "ax":-2.26733, "ay":-1.67163, "alpha":-0.12518, "fx":[-38.29673,-39.08977,-38.83179,-38.04844], "fy":[-28.79667,-27.715,-28.08779,-29.13628]}, + {"t":3.23556, "x":2.69157, "y":2.30861, "heading":0.94904, "vx":-2.1087, "vy":-1.51972, "omega":-0.11818, "ax":-2.25353, "ay":-1.68145, "alpha":-0.12345, "fx":[-38.05725,-38.84745,-38.60148,-37.82146], "fy":[-28.96293,-27.89964,-28.25528,-29.28631]}, + {"t":3.27935, "x":2.59708, "y":2.24045, "heading":0.94386, "vx":-2.20737, "vy":-1.59334, "omega":-0.12358, "ax":-2.22585, "ay":-1.7008, "alpha":-0.12001, "fx":[-37.58014,-38.36244,-38.13663,-37.36509], "fy":[-29.28655,-28.26156,-28.58899,-29.58364]}, + {"t":3.32313, "x":2.4983, "y":2.16906, "heading":0.93845, "vx":-2.30483, "vy":-1.66781, "omega":-0.12884, "ax":-2.14233, "ay":-1.75643, "alpha":-0.10966, "fx":[-36.1507,-36.89969,-36.72448,-35.9868], "fy":[-30.20668,-29.29816,-29.55852,-30.44236]}, + {"t":3.36692, "x":2.39533, "y":2.09435, "heading":0.93281, "vx":-2.39863, "vy":-1.74472, "omega":-0.13364, "ax":1.18299, "ay":-1.68186, "alpha":0.27897, "fx":[18.73385,19.79067,21.47107,20.49347], "fy":[-29.29422,-29.41564,-27.94382,-27.77831]}, + {"t":3.4107, "x":2.29143, "y":2.01634, "heading":0.92696, "vx":-2.34684, "vy":-1.81836, "omega":-0.12143, "ax":2.36244, "ay":1.44644, "alpha":0.15003, "fx":[39.92156,40.73422,40.44433,39.63735], "fy":[24.99566,23.67646,24.23349,25.50826]}, + {"t":3.45449, "x":2.19094, "y":1.93811, "heading":0.92164, "vx":-2.24339, "vy":-1.75502, "omega":-0.11486, "ax":2.33738, "ay":1.54375, "alpha":0.14026, "fx":[39.48443,40.30189,40.02753,39.21884], "fy":[26.6599,25.41857,25.87817,27.07841]}, + {"t":3.49828, "x":2.09495, "y":1.86275, "heading":0.91661, "vx":-2.14105, "vy":-1.68743, "omega":-0.10872, "ax":2.32821, "ay":1.57629, "alpha":0.13698, "fx":[39.32334,40.14175,39.87622,39.06765], "fy":[27.2176,26.00234,26.42696,27.60232]}, + {"t":3.54206, "x":2.00344, "y":1.79037, "heading":0.91185, "vx":-2.03911, "vy":-1.61841, "omega":-0.10272, "ax":2.32348, "ay":1.59258, "alpha":0.13535, "fx":[39.239,40.05824,39.79906,38.99029], "fy":[27.49812,26.29527,26.70014,27.86365]}, + {"t":3.58585, "x":1.91638, "y":1.72104, "heading":0.90736, "vx":-1.93738, "vy":-1.54868, "omega":-0.09679, "ax":2.32058, "ay":1.60235, "alpha":0.13437, "fx":[39.18667,40.00679,39.75275,38.94357], "fy":[27.66764,26.47169,26.86296,28.01988]}, + {"t":3.62963, "x":1.83378, "y":1.65476, "heading":0.90312, "vx":-1.83577, "vy":-1.47852, "omega":-0.09091, "ax":2.31863, "ay":1.60887, "alpha":0.13372, "fx":[39.15075,39.97177,39.72215,38.91246], "fy":[27.78158,26.58981,26.97064,28.12358]}, + {"t":3.67342, "x":1.75562, "y":1.59157, "heading":0.89914, "vx":-1.73425, "vy":-1.40808, "omega":-0.08505, "ax":2.31723, "ay":1.61353, "alpha":0.13326, "fx":[39.12441,39.94631,39.7006,38.89035], "fy":[27.86367,26.67457,27.0469,28.19729]}, + {"t":3.7172, "x":1.68191, "y":1.53146, "heading":0.89541, "vx":-1.63279, "vy":-1.33743, "omega":-0.07922, "ax":2.31617, "ay":1.61702, "alpha":0.13292, "fx":[39.10416,39.9269,39.68471,38.87391], "fy":[27.92578,26.73844,27.10356,28.25228]}, + {"t":3.76099, "x":1.61264, "y":1.47445, "heading":0.89195, "vx":-1.53137, "vy":-1.26662, "omega":-0.0734, "ax":2.31535, "ay":1.61973, "alpha":0.13266, "fx":[39.08804,39.91159,39.67259,38.86124], "fy":[27.97451,26.78835,27.14723,28.29482]}, + {"t":3.80477, "x":1.5478, "y":1.42055, "heading":0.88873, "vx":-1.42999, "vy":-1.1957, "omega":-0.06759, "ax":2.31468, "ay":1.62191, "alpha":0.13245, "fx":[39.07487,39.89917,39.66308,38.8512], "fy":[28.01381,26.82847,27.18186,28.32868]}, + {"t":3.84856, "x":1.48741, "y":1.36975, "heading":0.88577, "vx":-1.32864, "vy":-1.12469, "omega":-0.06179, "ax":2.31414, "ay":1.62369, "alpha":0.13228, "fx":[39.0639,39.88889,39.65543,38.84306], "fy":[28.04622,26.86143,27.20996,28.35625]}, + {"t":3.89234, "x":1.43145, "y":1.32206, "heading":0.88307, "vx":-1.22732, "vy":-1.05359, "omega":-0.056, "ax":2.31368, "ay":1.62517, "alpha":0.13214, "fx":[39.05461,39.88024,39.64916,38.83634], "fy":[28.07339,26.889,27.23321,28.37912]}, + {"t":3.93613, "x":1.37993, "y":1.27748, "heading":0.88061, "vx":-1.12601, "vy":-0.98244, "omega":-0.05021, "ax":2.3133, "ay":1.62642, "alpha":0.13202, "fx":[39.04665,39.87285,39.64393,38.83069], "fy":[28.0965,26.9124,27.25277,28.39841]}, + {"t":3.97991, "x":1.33285, "y":1.23603, "heading":0.87842, "vx":-1.02472, "vy":-0.91122, "omega":-0.04443, "ax":2.31297, "ay":1.6275, "alpha":0.13192, "fx":[39.03976,39.86648,39.63948,38.82588], "fy":[28.11638,26.9325,27.26947,28.4149]}, + {"t":4.0237, "x":1.2902, "y":1.19769, "heading":0.87647, "vx":-0.92345, "vy":-0.83996, "omega":-0.03866, "ax":2.31268, "ay":1.62843, "alpha":0.13183, "fx":[39.03377,39.86094,39.63563,38.82171], "fy":[28.13363,26.94994,27.28392,28.42918]}, + {"t":4.06748, "x":1.25198, "y":1.16247, "heading":0.87478, "vx":-0.82219, "vy":-0.76866, "omega":-0.03288, "ax":2.31243, "ay":1.62925, "alpha":0.13175, "fx":[39.02853,39.85609,39.63226,38.81805], "fy":[28.14872,26.9652,27.29658,28.44168]}, + {"t":4.11127, "x":1.2182, "y":1.13038, "heading":0.87334, "vx":-0.72094, "vy":-0.69732, "omega":-0.02712, "ax":2.31221, "ay":1.62997, "alpha":0.13168, "fx":[39.02392,39.85182,39.62925,38.8148], "fy":[28.16199,26.97864,27.30779,28.45273]}, + {"t":4.15506, "x":1.18885, "y":1.10141, "heading":0.87215, "vx":-0.6197, "vy":-0.62595, "omega":-0.02135, "ax":2.31201, "ay":1.63061, "alpha":0.13162, "fx":[39.01988,39.84805,39.62653,38.81188], "fy":[28.17372,26.99054,27.31782,28.4626]}, + {"t":4.19884, "x":1.16393, "y":1.07556, "heading":0.87122, "vx":-0.51847, "vy":-0.55456, "omega":-0.01559, "ax":2.31183, "ay":1.63118, "alpha":0.13157, "fx":[39.01633,39.84471,39.62402,38.80923], "fy":[28.18412,27.00115,27.32689,28.47148]}, + {"t":4.24263, "x":1.14344, "y":1.05284, "heading":0.87053, "vx":-0.41724, "vy":-0.48314, "omega":-0.00983, "ax":2.31167, "ay":1.6317, "alpha":0.13152, "fx":[39.01321,39.84174,39.62168,38.80679], "fy":[28.19336,27.01062,27.33517,28.47955]}, + {"t":4.28641, "x":1.12739, "y":1.03325, "heading":0.8701, "vx":-0.31602, "vy":-0.41169, "omega":-0.00407, "ax":2.31153, "ay":1.63216, "alpha":0.13148, "fx":[39.01049,39.83912,39.61947,38.80451], "fy":[28.20159,27.01911,27.34281,28.48693]}, + {"t":4.3302, "x":1.11577, "y":1.01679, "heading":0.86993, "vx":-0.21481, "vy":-0.34023, "omega":0.00169, "ax":2.31139, "ay":1.63259, "alpha":0.13144, "fx":[39.00812,39.83679,39.61736,38.80238], "fy":[28.20891,27.02674,27.3499,28.49373]}, + {"t":4.37398, "x":1.10858, "y":1.00346, "heading":0.87, "vx":-0.11361, "vy":-0.26874, "omega":0.00745, "ax":2.31127, "ay":1.63297, "alpha":0.13141, "fx":[39.00609,39.83473,39.6153,38.80035], "fy":[28.21542,27.0336,27.35655,28.50004]}, + {"t":4.41777, "x":1.10582, "y":0.99326, "heading":0.87033, "vx":-0.01241, "vy":-0.19724, "omega":0.0132, "ax":2.31116, "ay":1.63333, "alpha":0.13137, "fx":[39.00436,39.83292,39.61329,38.79841], "fy":[28.22119,27.03978,27.36283,28.50594]}, + {"t":4.46155, "x":1.10749, "y":0.98619, "heading":0.8709, "vx":0.08879, "vy":-0.12573, "omega":0.01895, "ax":2.31095, "ay":1.63357, "alpha":0.12618, "fx":[39.01315,39.80868,39.59761,38.81468], "fy":[28.20727,27.07359,27.38363,28.48198]}, + {"t":4.50429, "x":1.1134, "y":0.98231, "heading":0.87171, "vx":0.18756, "vy":-0.05591, "omega":0.02434, "ax":2.31066, "ay":1.63379, "alpha":0.12613, "fx":[39.00861,39.80383,39.59233,38.80966], "fy":[28.21022,27.07728,27.38797,28.48562]}, + {"t":4.54704, "x":1.12353, "y":0.98141, "heading":0.87275, "vx":0.28632, "vy":0.01392, "omega":0.02974, "ax":2.31034, "ay":1.63402, "alpha":0.12609, "fx":[39.00376,39.79861,39.58651,38.80415], "fy":[28.21332,27.08122,27.39279,28.48964]}, + {"t":4.58978, "x":1.13787, "y":0.9835, "heading":0.87403, "vx":0.38507, "vy":0.08377, "omega":0.03513, "ax":2.31, "ay":1.63428, "alpha":0.12604, "fx":[38.99853,39.79295,39.58009,38.79811], "fy":[28.21661,27.08547,27.39815,28.49409]}, + {"t":4.63252, "x":1.15644, "y":0.98857, "heading":0.87553, "vx":0.4838, "vy":0.15362, "omega":0.04051, "ax":2.30962, "ay":1.63456, "alpha":0.12599, "fx":[38.99286,39.78678,39.57301,38.79145], "fy":[28.22012,27.09007,27.40409,28.499]}, + {"t":4.67526, "x":1.17923, "y":0.99663, "heading":0.87726, "vx":0.58252, "vy":0.22348, "omega":0.0459, "ax":2.30921, "ay":1.63486, "alpha":0.12593, "fx":[38.98667,39.78002,39.56518,38.7841], "fy":[28.22394,27.0951,27.41067,28.50442]}, + {"t":4.718, "x":1.20624, "y":1.00767, "heading":0.87922, "vx":0.68122, "vy":0.29336, "omega":0.05128, "ax":2.30875, "ay":1.6352, "alpha":0.12586, "fx":[38.97984,39.77256,39.55651,38.77597], "fy":[28.22812,27.10064,27.41799,28.51044]}, + {"t":4.76074, "x":1.23746, "y":1.02171, "heading":0.88141, "vx":0.7799, "vy":0.36325, "omega":0.05666, "ax":2.30824, "ay":1.63558, "alpha":0.12579, "fx":[38.97227,39.76427,39.54686,38.76693], "fy":[28.23275,27.10679,27.42611,28.51713]}, + {"t":4.80348, "x":1.2729, "y":1.03873, "heading":0.88383, "vx":0.87855, "vy":0.43315, "omega":0.06204, "ax":2.30768, "ay":1.636, "alpha":0.12571, "fx":[38.96379,39.75499,39.5361,38.75683], "fy":[28.23795,27.11368,27.43517,28.52458]}, + {"t":4.84623, "x":1.31256, "y":1.05873, "heading":0.88648, "vx":0.97719, "vy":0.50308, "omega":0.06741, "ax":2.30704, "ay":1.63647, "alpha":0.12563, "fx":[38.95421,39.74454,39.52402,38.7455], "fy":[28.24385,27.12145,27.44531,28.53293]}, + {"t":4.88897, "x":1.35644, "y":1.08173, "heading":0.88937, "vx":1.07579, "vy":0.57302, "omega":0.07278, "ax":2.30631, "ay":1.637, "alpha":0.12553, "fx":[38.94329,39.73265,39.51039,38.73269], "fy":[28.25061,27.13031,27.4567,28.54233]}, + {"t":4.93171, "x":1.40452, "y":1.10772, "heading":0.89248, "vx":1.17437, "vy":0.64299, "omega":0.07814, "ax":2.30549, "ay":1.63762, "alpha":0.12541, "fx":[38.93071,39.71901,39.49491,38.71811], "fy":[28.25847,27.1405,27.46957,28.55299]}, + {"t":4.97445, "x":1.45683, "y":1.1367, "heading":0.89582, "vx":1.27291, "vy":0.71299, "omega":0.0835, "ax":2.30453, "ay":1.63832, "alpha":0.12528, "fx":[38.91607,39.70321,39.47716,38.70136], "fy":[28.26771,27.15235,27.48424,28.56518]}, + {"t":5.01719, "x":1.51334, "y":1.16867, "heading":0.89939, "vx":1.37141, "vy":0.78301, "omega":0.08886, "ax":2.30342, "ay":1.63914, "alpha":0.12513, "fx":[38.89882,39.68467,39.45661,38.68192], "fy":[28.27872,27.1663,27.5011,28.57925]}, + {"t":5.05993, "x":1.57406, "y":1.20363, "heading":0.90318, "vx":1.46986, "vy":0.85307, "omega":0.09421, "ax":2.3021, "ay":1.64011, "alpha":0.12495, "fx":[38.87821,39.66266,39.43253,38.65906], "fy":[28.29201,27.18294,27.52071,28.59568]}, + {"t":5.10267, "x":1.63898, "y":1.24159, "heading":0.90721, "vx":1.56826, "vy":0.92317, "omega":0.09955, "ax":2.30052, "ay":1.64127, "alpha":0.12474, "fx":[38.8532,39.63609,39.40389,38.63178], "fy":[28.30833,27.20308,27.54384,28.61517]}, + {"t":5.14542, "x":1.70811, "y":1.28255, "heading":0.91146, "vx":1.66658, "vy":0.99332, "omega":0.10488, "ax":2.29859, "ay":1.64269, "alpha":0.12448, "fx":[38.82227,39.60343,39.36921,38.59864], "fy":[28.32873,27.22793,27.57158,28.6387]}, + {"t":5.18816, "x":1.78145, "y":1.32651, "heading":0.91595, "vx":1.76483, "vy":1.06353, "omega":0.1102, "ax":2.29618, "ay":1.64446, "alpha":0.12417, "fx":[38.78317,39.56239,39.32627,38.55743], "fy":[28.3548,27.25925,27.6056,28.66773]}, + {"t":5.2309, "x":1.85897, "y":1.37346, "heading":0.92066, "vx":1.86297, "vy":1.13382, "omega":0.11551, "ax":2.29307, "ay":1.64673, "alpha":0.12376, "fx":[38.73233,39.50932,39.27155,38.50473], "fy":[28.389,27.29982,27.64849,28.70457]}, + {"t":5.27364, "x":1.94069, "y":1.42343, "heading":0.92559, "vx":1.96098, "vy":1.2042, "omega":0.1208, "ax":2.28893, "ay":1.64975, "alpha":0.12322, "fx":[38.6638,39.43819,39.19922,38.4348], "fy":[28.43545,27.35426,27.70455,28.75305]}, + {"t":5.31638, "x":2.0266, "y":1.47641, "heading":0.93076, "vx":2.05881, "vy":1.27472, "omega":0.12606, "ax":2.28312, "ay":1.65396, "alpha":0.12248, "fx":[38.56689,39.3381,39.09875,38.33733], "fy":[28.5015,27.43081,27.7815,28.82003]}, + {"t":5.35912, "x":2.11668, "y":1.5324, "heading":0.93615, "vx":2.1564, "vy":1.34541, "omega":0.1313, "ax":2.27441, "ay":1.66025, "alpha":0.12137, "fx":[38.42017,39.18722,38.94904,38.19161], "fy":[28.60175,27.54583,27.89463,28.91912]}, + {"t":5.40187, "x":2.21093, "y":1.59142, "heading":0.94176, "vx":2.25361, "vy":1.41637, "omega":0.13649, "ax":2.25986, "ay":1.67063, "alpha":0.11953, "fx":[38.17356,38.93447,38.70067,37.9493], "fy":[28.76976,27.73701,28.07925,29.0817]}, + {"t":5.44461, "x":2.30931, "y":1.65348, "heading":0.94759, "vx":2.3502, "vy":1.48778, "omega":0.14159, "ax":2.23067, "ay":1.69106, "alpha":0.11589, "fx":[37.67658,38.42598,38.20472,37.4649], "fy":[29.10389,28.11496,28.4391,29.39984]}, + {"t":5.48735, "x":2.4118, "y":1.71862, "heading":0.95364, "vx":2.44554, "vy":1.56005, "omega":0.14655, "ax":2.14256, "ay":1.74966, "alpha":0.10495, "fx":[36.17509,36.88618,36.70888,36.0075], "fy":[30.06708,29.20246,29.46673,30.30891]}, + {"t":5.53009, "x":2.51829, "y":1.7869, "heading":0.95991, "vx":2.53712, "vy":1.63484, "omega":0.15103, "ax":-1.18537, "ay":1.67074, "alpha":-0.28005, "fx":[-18.76694,-19.86803,-21.52031,-20.49593], "fy":[29.12774,29.21764,27.73102,27.59869]}, + {"t":5.57283, "x":2.62564, "y":1.8583, "heading":0.96636, "vx":2.48645, "vy":1.70625, "omega":0.13906, "ax":-2.37231, "ay":-1.42239, "alpha":-0.14751, "fx":[-40.12345,-40.89398,-40.58018,-39.81134], "fy":[-24.53579,-23.26248,-23.87322,-25.10603]}, + {"t":5.61557, "x":2.72975, "y":1.92993, "heading":0.97231, "vx":2.38505, "vy":1.64545, "omega":0.13276, "ax":-2.34716, "ay":-1.52508, "alpha":-0.13724, "fx":[-39.69112,-40.46323,-40.15586,-39.38813], "fy":[-26.28627,-25.09534,-25.6148,-26.76843]}, + {"t":5.65831, "x":2.82955, "y":1.99886, "heading":0.97798, "vx":2.28473, "vy":1.58027, "omega":0.12689, "ax":-2.33788, "ay":-1.55946, "alpha":-0.13379, "fx":[-39.53385,-40.30367,-39.99699,-39.23209], "fy":[-26.86809,-25.70762,-26.20182,-27.3263]}, + {"t":5.70106, "x":2.92507, "y":2.06498, "heading":0.9834, "vx":2.18481, "vy":1.51361, "omega":0.12117, "ax":-2.33307, "ay":-1.57667, "alpha":-0.13208, "fx":[-39.45376,-40.22133,-39.91331,-39.15074], "fy":[-27.1571,-26.01316,-26.49777,-27.60648]}, + {"t":5.7438, "x":3.01632, "y":2.12823, "heading":0.98858, "vx":2.08509, "vy":1.44623, "omega":0.11553, "ax":-2.33012, "ay":-1.58699, "alpha":-0.13105, "fx":[-39.40584,-40.17136,-39.86117,-39.10059], "fy":[-27.32905,-26.19588,-26.6769,-27.77535]}, + {"t":5.78654, "x":3.10331, "y":2.1886, "heading":0.99352, "vx":1.9855, "vy":1.37839, "omega":0.10993, "ax":-2.32814, "ay":-1.59388, "alpha":-0.13037, "fx":[-39.37432,-40.13796,-39.82527,-39.06643], "fy":[-27.44256,-26.31722,-26.79747,-27.88846]}, + {"t":5.82928, "x":3.18604, "y":2.24606, "heading":0.99822, "vx":1.88599, "vy":1.31027, "omega":0.10436, "ax":-2.32671, "ay":-1.5988, "alpha":-0.12989, "fx":[-39.35226,-40.11416,-39.79883,-39.04157], "fy":[-27.52273,-26.40352,-26.88448,-27.96967]}, + {"t":5.87202, "x":3.26453, "y":2.3006, "heading":1.00268, "vx":1.78654, "vy":1.24194, "omega":0.0988, "ax":-2.32563, "ay":-1.60249, "alpha":-0.12954, "fx":[-39.33612,-40.0964,-39.77841,-39.0226], "fy":[-27.58214,-26.46794,-26.95044,-28.03088]}, + {"t":5.91476, "x":3.33876, "y":2.35222, "heading":1.0069, "vx":1.68714, "vy":1.17344, "omega":0.09327, "ax":-2.32479, "ay":-1.60536, "alpha":-0.12926, "fx":[-39.32393,-40.08267,-39.76208,-39.00761], "fy":[-27.62777,-26.51782,-27.00232,-28.07874]}, + {"t":5.9575, "x":3.40875, "y":2.40091, "heading":1.01089, "vx":1.58778, "vy":1.10483, "omega":0.08774, "ax":-2.32412, "ay":-1.60765, "alpha":-0.12904, "fx":[-39.31447,-40.07178,-39.74866,-38.99545], "fy":[-27.66381,-26.55753,-27.04427,-28.11722]}, + {"t":6.00025, "x":3.47449, "y":2.44666, "heading":1.01464, "vx":1.48844, "vy":1.03611, "omega":0.08223, "ax":-2.32356, "ay":-1.60953, "alpha":-0.12886, "fx":[-39.30698,-40.06294,-39.7374,-38.98536], "fy":[-27.69292,-26.58988,-27.07897,-28.14886]}, + {"t":6.04299, "x":3.53599, "y":2.48948, "heading":1.01815, "vx":1.38913, "vy":0.96732, "omega":0.07672, "ax":-2.3231, "ay":-1.6111, "alpha":-0.12872, "fx":[-39.30093,-40.05563,-39.72779,-38.97685], "fy":[-27.71687,-26.61672,-27.10817,-28.17535]}, + {"t":6.08573, "x":3.59324, "y":2.52935, "heading":1.02143, "vx":1.28983, "vy":0.89846, "omega":0.07122, "ax":-2.32271, "ay":-1.61242, "alpha":-0.12859, "fx":[-39.29598,-40.04949,-39.71949,-38.96957], "fy":[-27.7369,-26.63934,-27.13311,-28.19785]}, + {"t":6.12847, "x":3.64625, "y":2.56628, "heading":1.02448, "vx":1.19056, "vy":0.82954, "omega":0.06572, "ax":-2.32237, "ay":-1.61355, "alpha":-0.12849, "fx":[-39.29185,-40.04426,-39.71225,-38.96327], "fy":[-27.75389,-26.65866,-27.15466,-28.2172]}, + {"t":6.17121, "x":3.69501, "y":2.60026, "heading":1.02729, "vx":1.0913, "vy":0.76058, "omega":0.06023, "ax":-2.32208, "ay":-1.61454, "alpha":-0.12839, "fx":[-39.28836,-40.03976,-39.70587,-38.95776], "fy":[-27.76848,-26.67536,-27.17347,-28.23403]}, + {"t":6.21395, "x":3.73953, "y":2.63129, "heading":1.02986, "vx":0.99205, "vy":0.69157, "omega":0.05474, "ax":-2.32183, "ay":-1.6154, "alpha":-0.12832, "fx":[-39.28537,-40.03583,-39.70023,-38.95292], "fy":[-27.78116,-26.68994,-27.19001,-28.24878]}, + {"t":6.25669, "x":3.77982, "y":2.65938, "heading":1.0322, "vx":0.89281, "vy":0.62252, "omega":0.04926, "ax":-2.3216, "ay":-1.61616, "alpha":-0.12825, "fx":[-39.28276,-40.03237,-39.69521,-38.94863], "fy":[-27.79229,-26.70279,-27.20466,-28.26182]}, + {"t":6.29944, "x":3.81586, "y":2.68451, "heading":1.03431, "vx":0.79358, "vy":0.55345, "omega":0.04378, "ax":-2.3214, "ay":-1.61683, "alpha":-0.12818, "fx":[-39.28046,-40.0293,-39.69073,-38.94481], "fy":[-27.80216,-26.7142,-27.2177,-28.27342]}, + {"t":6.34218, "x":3.84765, "y":2.70669, "heading":1.03618, "vx":0.69436, "vy":0.48434, "omega":0.0383, "ax":-2.32122, "ay":-1.61744, "alpha":-0.12813, "fx":[-39.27839,-40.02655,-39.68672,-38.94139], "fy":[-27.81101,-26.72441,-27.22935,-28.28379]}, + {"t":6.38492, "x":3.87521, "y":2.72591, "heading":1.03781, "vx":0.59515, "vy":0.41521, "omega":0.03282, "ax":-2.32106, "ay":-1.61798, "alpha":-0.12808, "fx":[-39.2765,-40.02406,-39.68314,-38.93833], "fy":[-27.81901,-26.73362,-27.2398,-28.2931]}, + {"t":6.42766, "x":3.89853, "y":2.74218, "heading":1.03922, "vx":0.49594, "vy":0.34605, "omega":0.02735, "ax":-2.32091, "ay":-1.61847, "alpha":-0.12804, "fx":[-39.27474,-40.02179,-39.67994,-38.93557], "fy":[-27.82631,-26.74197,-27.2492,-28.30151]}, + {"t":6.4704, "x":3.91761, "y":2.75549, "heading":1.04039, "vx":0.39674, "vy":0.27688, "omega":0.02187, "ax":-2.32078, "ay":-1.61892, "alpha":-0.128, "fx":[-39.27308,-40.01971,-39.67709,-38.93308], "fy":[-27.83303,-26.7496,-27.25765,-28.30912]}, + {"t":6.51314, "x":3.93244, "y":2.76585, "heading":1.04132, "vx":0.29755, "vy":0.20768, "omega":0.0164, "ax":-2.32065, "ay":-1.61933, "alpha":-0.12796, "fx":[-39.27149,-40.01778,-39.67455,-38.93084], "fy":[-27.83928,-26.75659,-27.26527,-28.31603]}, + {"t":6.55589, "x":3.94304, "y":2.77324, "heading":1.04202, "vx":0.19836, "vy":0.13847, "omega":0.01093, "ax":-2.32054, "ay":-1.6197, "alpha":-0.12793, "fx":[-39.26995,-40.01598,-39.67231,-38.92882], "fy":[-27.84512,-26.76305,-27.27213,-28.32231]}, + {"t":6.59863, "x":3.9494, "y":2.77768, "heading":1.04249, "vx":0.09918, "vy":0.06924, "omega":0.00547, "ax":-2.32044, "ay":-1.62005, "alpha":-0.1279, "fx":[-39.26843,-40.01429,-39.67034,-38.92699], "fy":[-27.85064,-26.76903,-27.27831,-28.32804]}, + {"t":6.64137, "x":3.95152, "y":2.77916, "heading":1.04272, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.40665, "ay":-1.48901, "alpha":-0.13507, "fx":[-40.7716,-41.47344,-41.10256,-40.39798], "fy":[-25.60135,-24.44938,-25.07022,-26.18927]}, + {"t":6.68503, "x":3.94923, "y":2.77774, "heading":1.04272, "vx":-0.10507, "vy":-0.06501, "omega":-0.0059, "ax":-2.40655, "ay":-1.48895, "alpha":-0.13507, "fx":[-40.76995,-41.47176,-41.10095,-40.39641], "fy":[-25.6003,-24.44844,-25.06925,-26.1882]}, + {"t":6.72869, "x":3.94235, "y":2.77349, "heading":1.04246, "vx":-0.21014, "vy":-0.13002, "omega":-0.01179, "ax":-2.40645, "ay":-1.48888, "alpha":-0.13506, "fx":[-40.76801,-41.4699,-41.09934,-40.39475], "fy":[-25.59938,-24.44748,-25.06798,-26.18697]}, + {"t":6.77235, "x":3.93088, "y":2.76639, "heading":1.04195, "vx":-0.31521, "vy":-0.19502, "omega":-0.01769, "ax":-2.40633, "ay":-1.48881, "alpha":-0.13506, "fx":[-40.76576,-41.46782,-41.0977,-40.39297], "fy":[-25.59859,-24.44649,-25.06639,-26.18556]}, + {"t":6.81601, "x":3.91482, "y":2.75646, "heading":1.04118, "vx":-0.42027, "vy":-0.26002, "omega":-0.02359, "ax":-2.40621, "ay":-1.48873, "alpha":-0.13505, "fx":[-40.76318,-41.46551,-41.096,-40.39105], "fy":[-25.5979,-24.44545,-25.06447,-26.18395]}, + {"t":6.85967, "x":3.89418, "y":2.74369, "heading":1.04015, "vx":-0.52532, "vy":-0.32502, "omega":-0.02948, "ax":-2.40607, "ay":-1.48865, "alpha":-0.13504, "fx":[-40.76024,-41.46294,-41.09423,-40.38897], "fy":[-25.59729,-24.44435,-25.06219,-26.18213]}, + {"t":6.90333, "x":3.86895, "y":2.72808, "heading":1.03886, "vx":-0.63037, "vy":-0.39001, "omega":-0.03538, "ax":-2.40592, "ay":-1.48855, "alpha":-0.13504, "fx":[-40.75689,-41.46007,-41.09234,-40.38669], "fy":[-25.59675,-24.44316,-25.05955,-26.18007]}, + {"t":6.94699, "x":3.83914, "y":2.70963, "heading":1.03732, "vx":-0.73542, "vy":-0.45501, "omega":-0.04128, "ax":-2.40575, "ay":-1.48845, "alpha":-0.13503, "fx":[-40.75311,-41.45685,-41.09029,-40.38417], "fy":[-25.59625,-24.44187,-25.05651,-26.17775]}, + {"t":6.99065, "x":3.80473, "y":2.68835, "heading":1.03551, "vx":-0.84045, "vy":-0.51999, "omega":-0.04717, "ax":-2.40556, "ay":-1.48833, "alpha":-0.13502, "fx":[-40.74882,-41.45324,-41.08803,-40.38136], "fy":[-25.59575,-24.44044,-25.05304,-26.17512]}, + {"t":7.03431, "x":3.76575, "y":2.66422, "heading":1.03345, "vx":-0.94548, "vy":-0.58497, "omega":-0.05307, "ax":-2.40534, "ay":-1.4882, "alpha":-0.13501, "fx":[-40.74397,-41.44915,-41.08549,-40.37819], "fy":[-25.5952,-24.43883,-25.0491,-26.17215]}, + {"t":7.07797, "x":3.72218, "y":2.63727, "heading":1.03114, "vx":-1.05049, "vy":-0.64995, "omega":-0.05896, "ax":-2.4051, "ay":-1.48805, "alpha":-0.135, "fx":[-40.73847,-41.4445,-41.08258,-40.37458], "fy":[-25.59455,-24.437,-25.04465,-26.16878]}, + {"t":7.12163, "x":3.67402, "y":2.60747, "heading":1.02856, "vx":-1.1555, "vy":-0.71491, "omega":-0.06485, "ax":-2.40482, "ay":-1.48787, "alpha":-0.13499, "fx":[-40.7322,-41.43918,-41.0792,-40.37043], "fy":[-25.59373,-24.43487,-25.03961,-26.16493]}, + {"t":7.16529, "x":3.62128, "y":2.57484, "heading":1.02573, "vx":-1.26049, "vy":-0.77987, "omega":-0.07075, "ax":-2.40449, "ay":-1.48767, "alpha":-0.13498, "fx":[-40.72501,-41.43302,-41.0752,-40.36558], "fy":[-25.59265,-24.43237,-25.03389,-26.16051]}, + {"t":7.20895, "x":3.56395, "y":2.53937, "heading":1.02264, "vx":-1.36547, "vy":-0.84483, "omega":-0.07664, "ax":-2.40411, "ay":-1.48743, "alpha":-0.13497, "fx":[-40.71671,-41.42584,-41.07038,-40.35984], "fy":[-25.59116,-24.42936,-25.02739,-26.15538]}, + {"t":7.25261, "x":3.50205, "y":2.50107, "heading":1.0193, "vx":-1.47044, "vy":-0.90977, "omega":-0.08253, "ax":-2.40365, "ay":-1.48715, "alpha":-0.13495, "fx":[-40.707,-41.41733,-41.06446,-40.35295], "fy":[-25.5891,-24.4257,-25.01993,-26.14938]}, + {"t":7.29627, "x":3.43556, "y":2.45993, "heading":1.01569, "vx":-1.57538, "vy":-0.9747, "omega":-0.08843, "ax":-2.4031, "ay":-1.48681, "alpha":-0.13494, "fx":[-40.69551,-41.40711,-41.05708,-40.34453], "fy":[-25.58621,-24.42115,-25.01128,-26.14225]}, + {"t":7.33993, "x":3.36448, "y":2.41596, "heading":1.01183, "vx":-1.6803, "vy":-1.03961, "omega":-0.09432, "ax":-2.40242, "ay":-1.48639, "alpha":-0.13492, "fx":[-40.68164,-41.39458,-41.04764,-40.33401], "fy":[-25.58213,-24.41536,-25.0011,-26.13362]}, + {"t":7.38359, "x":3.28883, "y":2.36915, "heading":1.00771, "vx":-1.78519, "vy":-1.1045, "omega":-0.10021, "ax":-2.40156, "ay":-1.48586, "alpha":-0.1349, "fx":[-40.6645,-41.37884,-41.0353,-40.32054], "fy":[-25.57629,-24.40782,-24.98887,-26.12293]}, + {"t":7.42725, "x":3.2086, "y":2.31952, "heading":1.00334, "vx":-1.89004, "vy":-1.16938, "omega":-0.1061, "ax":-2.40043, "ay":-1.48516, "alpha":-0.13488, "fx":[-40.64265,-41.35844,-41.01865,-40.30273], "fy":[-25.56777,-24.39767,-24.97373,-26.10927]}, + {"t":7.47091, "x":3.1238, "y":2.26705, "heading":0.99871, "vx":-1.99484, "vy":-1.23422, "omega":-0.11199, "ax":-2.39889, "ay":-1.48421, "alpha":-0.13485, "fx":[-40.6136,-41.33087,-40.99526,-40.27818], "fy":[-25.55501,-24.38343,-24.95422,-26.09109]}, + {"t":7.51457, "x":3.03441, "y":2.21174, "heading":0.99382, "vx":-2.09958, "vy":-1.29902, "omega":-0.11787, "ax":-2.39667, "ay":-1.48283, "alpha":-0.13482, "fx":[-40.57264,-41.2914,-40.96058,-40.24235], "fy":[-25.53503,-24.36233,-24.92757,-26.06543]}, + {"t":7.55823, "x":2.94046, "y":2.15362, "heading":0.98867, "vx":-2.20422, "vy":-1.36376, "omega":-0.12376, "ax":-2.39319, "ay":-1.48068, "alpha":-0.13479, "fx":[-40.50973,-41.22992,-40.90486,-40.18557], "fy":[-25.5015,-24.32841,-24.88786,-26.02603]}, + {"t":7.60189, "x":2.84195, "y":2.09266, "heading":0.98327, "vx":-2.3087, "vy":-1.42841, "omega":-0.12965, "ax":-2.38694, "ay":-1.47681, "alpha":-0.13475, "fx":[-40.39879,-41.12023,-40.80282,-40.08269], "fy":[-25.43799,-24.2662,-24.81972,-25.95659]}, + {"t":7.64555, "x":2.73887, "y":2.02889, "heading":0.97761, "vx":-2.41292, "vy":-1.49288, "omega":-0.13553, "ax":-2.37241, "ay":-1.46782, "alpha":-0.13469, "fx":[-40.14477,-40.8668,-40.56226,-39.84205], "fy":[-25.28459,-24.11932,-24.66711,-25.79779]}, + {"t":7.68921, "x":2.63127, "y":1.96231, "heading":0.97169, "vx":-2.5165, "vy":-1.55697, "omega":-0.14141, "ax":-2.3011, "ay":-1.4237, "alpha":-0.13454, "fx":[-38.91022,-39.62788,-39.37029,-38.65553], "fy":[-24.51372,-23.39073,-23.93584,-25.02661]}, + {"t":7.73287, "x":2.5192, "y":1.89298, "heading":0.96552, "vx":-2.61696, "vy":-1.61913, "omega":-0.14728, "ax":2.3011, "ay":1.4237, "alpha":0.13438, "fx":[38.90742,39.62596,39.37294,38.65763], "fy":[24.51845,23.39346,23.93119,25.0238]}, + {"t":7.77652, "x":2.40714, "y":1.82365, "heading":0.95909, "vx":-2.5165, "vy":-1.55697, "omega":-0.14142, "ax":2.37241, "ay":1.46782, "alpha":0.13464, "fx":[40.13511,40.86293,40.57135,39.84649], "fy":[25.30014,24.12532,24.65194,25.79142]}, + {"t":7.82018, "x":2.29953, "y":1.75707, "heading":0.95291, "vx":-2.41292, "vy":-1.49288, "omega":-0.13554, "ax":2.38694, "ay":1.47681, "alpha":0.13472, "fx":[40.3827,41.11421,40.81797,40.08965], "fy":[25.46373,24.27584,24.7946,25.94633]}, + {"t":7.86384, "x":2.19646, "y":1.6933, "heading":0.94699, "vx":-2.3087, "vy":-1.42841, "omega":-0.12966, "ax":2.39319, "ay":1.48068, "alpha":0.13477, "fx":[40.48751,41.22181,40.92578,40.19499], "fy":[25.53698,24.34161,24.85325,26.01196]}, + {"t":7.9075, "x":2.09794, "y":1.63234, "heading":0.94133, "vx":-2.20422, "vy":-1.36376, "omega":-0.12377, "ax":2.39667, "ay":1.48283, "alpha":0.13481, "fx":[40.54457,41.28127,40.98701,40.25414], "fy":[25.57979,24.37896,24.88391,26.0477]}, + {"t":7.95116, "x":2.00399, "y":1.57422, "heading":0.93593, "vx":-2.09958, "vy":-1.29902, "omega":-0.11789, "ax":2.39889, "ay":1.48421, "alpha":0.13485, "fx":[40.57995,41.31881,41.02692,40.29222], "fy":[25.60861,24.40334,24.90194,26.06985]}, + {"t":7.99482, "x":1.91461, "y":1.51891, "heading":0.93078, "vx":-1.99484, "vy":-1.23422, "omega":-0.112, "ax":2.40043, "ay":1.48516, "alpha":0.13488, "fx":[40.60371,41.34454,41.05529,40.31893], "fy":[25.62976,24.42071,24.91326,26.0847]}, + {"t":8.03848, "x":1.8298, "y":1.46644, "heading":0.92589, "vx":-1.89004, "vy":-1.16938, "omega":-0.10611, "ax":2.40156, "ay":1.48586, "alpha":0.1349, "fx":[40.62055,41.36319,41.07666,40.33879], "fy":[25.64623,24.43384,24.92065,26.09519]}, + {"t":8.08214, "x":1.74957, "y":1.41681, "heading":0.92126, "vx":-1.78519, "vy":-1.1045, "omega":-0.10022, "ax":2.40242, "ay":1.48639, "alpha":0.13492, "fx":[40.63295,41.37727,41.09346,40.3542], "fy":[25.65958,24.4442,24.92556,26.10287]}, + {"t":8.1258, "x":1.67392, "y":1.37, "heading":0.91688, "vx":-1.6803, "vy":-1.03961, "omega":-0.09433, "ax":2.4031, "ay":1.48681, "alpha":0.13494, "fx":[40.64237,41.38824,41.10708,40.36654], "fy":[25.67073,24.45265,24.92885,26.10867]}, + {"t":8.16946, "x":1.60285, "y":1.32603, "heading":0.91277, "vx":-1.57538, "vy":-0.9747, "omega":-0.08844, "ax":2.40365, "ay":1.48715, "alpha":0.13496, "fx":[40.64969,41.39699,41.11839,40.37668], "fy":[25.68023,24.4597,24.93104,26.11314]}, + {"t":8.21312, "x":1.53636, "y":1.28489, "heading":0.90891, "vx":-1.47044, "vy":-0.90977, "omega":-0.08255, "ax":2.40411, "ay":1.48743, "alpha":0.13498, "fx":[40.6555,41.40412,41.12797,40.38517], "fy":[25.68847,24.4657,24.93247,26.11666]}, + {"t":8.25678, "x":1.47445, "y":1.24659, "heading":0.9053, "vx":-1.36547, "vy":-0.84483, "omega":-0.07665, "ax":2.40449, "ay":1.48767, "alpha":0.13499, "fx":[40.66018,41.41003,41.13619,40.3924], "fy":[25.69569,24.47088,24.93338,26.11946]}, + {"t":8.30044, "x":1.41713, "y":1.21112, "heading":0.90195, "vx":-1.26049, "vy":-0.77987, "omega":-0.07076, "ax":2.40482, "ay":1.48787, "alpha":0.13501, "fx":[40.66403,41.415,41.14334,40.39863], "fy":[25.70206,24.4754,24.93393,26.12174]}, + {"t":8.3441, "x":1.36439, "y":1.17849, "heading":0.89887, "vx":-1.1555, "vy":-0.71491, "omega":-0.06486, "ax":2.4051, "ay":1.48805, "alpha":0.13502, "fx":[40.66724,41.41923,41.14959,40.40406], "fy":[25.70773,24.47938,24.93425,26.12362]}, + {"t":8.38776, "x":1.31623, "y":1.14869, "heading":0.89603, "vx":-1.05049, "vy":-0.64995, "omega":-0.05897, "ax":2.40534, "ay":1.4882, "alpha":0.13503, "fx":[40.66997,41.42289,41.15511,40.40882], "fy":[25.71278,24.48291,24.9344,26.1252]}, + {"t":8.43142, "x":1.27266, "y":1.12174, "heading":0.89346, "vx":-0.94548, "vy":-0.58497, "omega":-0.05307, "ax":2.40556, "ay":1.48833, "alpha":0.13504, "fx":[40.67232,41.42608,41.16001,40.41303], "fy":[25.71729,24.48604,24.93447,26.12655]}, + {"t":8.47508, "x":1.23367, "y":1.09762, "heading":0.89114, "vx":-0.84045, "vy":-0.51999, "omega":-0.04718, "ax":2.40575, "ay":1.48845, "alpha":0.13505, "fx":[40.67438,41.4289,41.16436,40.41677], "fy":[25.72132,24.48884,24.9345,26.12772]}, + {"t":8.51874, "x":1.19927, "y":1.07633, "heading":0.88908, "vx":-0.73542, "vy":-0.45501, "omega":-0.04128, "ax":2.40592, "ay":1.48855, "alpha":0.13506, "fx":[40.67622,41.43142,41.16824,40.42011], "fy":[25.7249,24.49133,24.93454,26.12877]}, + {"t":8.5624, "x":1.16945, "y":1.05788, "heading":0.88728, "vx":-0.63037, "vy":-0.39001, "omega":-0.03539, "ax":2.40607, "ay":1.48865, "alpha":0.13506, "fx":[40.6779,41.43368,41.17169,40.42309], "fy":[25.72808,24.49355,24.9346,26.12973]}, + {"t":8.60606, "x":1.14423, "y":1.04227, "heading":0.88573, "vx":-0.52532, "vy":-0.32502, "omega":-0.02949, "ax":2.40621, "ay":1.48873, "alpha":0.13507, "fx":[40.67946,41.43575,41.17477,40.42576], "fy":[25.73088,24.49552,24.93473,26.13063]}, + {"t":8.64972, "x":1.12358, "y":1.0295, "heading":0.88445, "vx":-0.42027, "vy":-0.26002, "omega":-0.02359, "ax":2.40633, "ay":1.48881, "alpha":0.13508, "fx":[40.68093,41.43764,41.17751,40.42816], "fy":[25.73332,24.49727,24.93494,26.13149]}, + {"t":8.69338, "x":1.10753, "y":1.01957, "heading":0.88342, "vx":-0.31521, "vy":-0.19502, "omega":-0.01769, "ax":2.40645, "ay":1.48888, "alpha":0.13508, "fx":[40.68235,41.43941,41.17994,40.4303], "fy":[25.73543,24.4988,24.93524,26.13233]}, + {"t":8.73704, "x":1.09606, "y":1.01247, "heading":0.88264, "vx":-0.21014, "vy":-0.13002, "omega":-0.0118, "ax":2.40655, "ay":1.48895, "alpha":0.13509, "fx":[40.68373,41.44106,41.18207,40.43221], "fy":[25.73722,24.50014,24.93566,26.13317]}, + {"t":8.7807, "x":1.08918, "y":1.00822, "heading":0.88213, "vx":-0.10507, "vy":-0.06501, "omega":-0.0059, "ax":2.40665, "ay":1.48901, "alpha":0.1351, "fx":[40.6851,41.44262,41.18394,40.43392], "fy":[25.73871,24.50129,24.9362,26.13402]}, + {"t":8.82436, "x":1.08688, "y":1.0068, "heading":0.88187, "vx":0.0, "vy":0.0, "omega":0.0, "ax":1.64709, "ay":2.28942, "alpha":-0.70923, "fx":[31.16593,25.31202,24.32813,31.25989], "fy":[36.69018,40.9491,41.53143,36.59875]}, + {"t":8.86167, "x":1.08803, "y":1.00839, "heading":0.88187, "vx":0.06145, "vy":0.08541, "omega":-0.02646, "ax":1.64702, "ay":2.28933, "alpha":-0.70919, "fx":[31.16467,25.31126,24.32734,31.2584], "fy":[36.68901,40.94757,41.52959,36.59737]}, + {"t":8.89897, "x":1.09147, "y":1.01317, "heading":0.88088, "vx":0.12289, "vy":0.17081, "omega":-0.05292, "ax":1.64696, "ay":2.28924, "alpha":-0.70915, "fx":[31.16609,25.31387,24.32352,31.25362], "fy":[36.68539,40.94383,41.52936,36.5986]}, + {"t":8.93628, "x":1.0972, "y":1.02114, "heading":0.87891, "vx":0.18433, "vy":0.25621, "omega":-0.07937, "ax":1.64689, "ay":2.28914, "alpha":-0.70909, "fx":[31.17018,25.31984,24.31667,31.24555], "fy":[36.67931,40.93784,41.53073,36.60242]}, + {"t":8.97358, "x":1.10522, "y":1.03229, "heading":0.87595, "vx":0.24577, "vy":0.34161, "omega":-0.10582, "ax":1.64681, "ay":2.28903, "alpha":-0.70902, "fx":[31.1769,25.32917,24.30681,31.23413], "fy":[36.67079,40.9296,41.53366,36.60885]}, + {"t":9.01089, "x":1.11554, "y":1.04662, "heading":0.872, "vx":0.3072, "vy":0.427, "omega":-0.13227, "ax":1.64673, "ay":2.28891, "alpha":-0.70893, "fx":[31.18623,25.34188,24.29397,31.21933], "fy":[36.65982,40.91907,41.5381,36.61788]}, + {"t":9.04819, "x":1.12814, "y":1.06415, "heading":0.86707, "vx":0.36863, "vy":0.51239, "omega":-0.15872, "ax":1.64664, "ay":2.28878, "alpha":-0.70883, "fx":[31.1981,25.35797,24.27822,31.20106], "fy":[36.6464,40.90622,41.54399,36.62954]}, + {"t":9.0855, "x":1.14304, "y":1.08485, "heading":0.86115, "vx":0.43006, "vy":0.59778, "omega":-0.18516, "ax":1.64654, "ay":2.28864, "alpha":-0.70872, "fx":[31.21245,25.37745,24.25959,31.17925], "fy":[36.63056,40.89099,41.55126,36.64384]}, + {"t":9.12281, "x":1.16023, "y":1.10875, "heading":0.85424, "vx":0.49149, "vy":0.68315, "omega":-0.2116, "ax":1.64644, "ay":2.28849, "alpha":-0.70859, "fx":[31.22923,25.40036,24.23818,31.1538], "fy":[36.6123,40.87334,41.55983,36.66081]}, + {"t":9.16011, "x":1.17971, "y":1.13582, "heading":0.84635, "vx":0.55291, "vy":0.76853, "omega":-0.23804, "ax":1.64632, "ay":2.28832, "alpha":-0.70843, "fx":[31.24834,25.4267,24.21406,31.12461], "fy":[36.59163,40.8532,41.56958,36.68048]}, + {"t":9.19742, "x":1.20148, "y":1.16609, "heading":0.83747, "vx":0.61432, "vy":0.85389, "omega":-0.26446, "ax":1.64619, "ay":2.28814, "alpha":-0.70826, "fx":[31.26969,25.45649,24.18734,31.09153], "fy":[36.56858,40.83049,41.58041,36.70286]}, + {"t":9.23472, "x":1.22554, "y":1.19953, "heading":0.8276, "vx":0.67574, "vy":0.93925, "omega":-0.29089, "ax":1.64605, "ay":2.28793, "alpha":-0.70805, "fx":[31.29316,25.48975,24.15811,31.05443], "fy":[36.54316,40.80514,41.59217,36.72799]}, + {"t":9.27203, "x":1.2519, "y":1.23616, "heading":0.81675, "vx":0.73714, "vy":1.02461, "omega":-0.3173, "ax":1.64589, "ay":2.28771, "alpha":-0.70781, "fx":[31.31861,25.52651,24.12651,31.01313], "fy":[36.51538,40.77705,41.6047,36.75588]}, + {"t":9.30933, "x":1.28054, "y":1.27598, "heading":0.80491, "vx":0.79854, "vy":1.10995, "omega":-0.34371, "ax":1.64572, "ay":2.28745, "alpha":-0.70754, "fx":[31.34589,25.56676,24.09266,30.96744], "fy":[36.48526,40.74609,41.61781,36.78655]}, + {"t":9.34664, "x":1.31148, "y":1.31898, "heading":0.79209, "vx":0.85994, "vy":1.19528, "omega":-0.3701, "ax":1.64552, "ay":2.28717, "alpha":-0.70723, "fx":[31.37482,25.61052,24.05669,30.91715], "fy":[36.4528,40.71213,41.63129,36.81997]}, + {"t":9.38394, "x":1.3447, "y":1.36516, "heading":0.77828, "vx":0.92132, "vy":1.28061, "omega":-0.39648, "ax":1.64529, "ay":2.28684, "alpha":-0.70687, "fx":[31.40518,25.65776,24.01874,30.86201], "fy":[36.41799,40.67501,41.64489,36.85614]}, + {"t":9.42125, "x":1.38022, "y":1.41453, "heading":0.76349, "vx":0.9827, "vy":1.36592, "omega":-0.42285, "ax":1.64503, "ay":2.28647, "alpha":-0.70645, "fx":[31.43671,25.70846,23.97896,30.80172], "fy":[36.38082,40.63452,41.65829,36.89498]}, + {"t":9.45855, "x":1.41802, "y":1.46707, "heading":0.74772, "vx":1.04407, "vy":1.45122, "omega":-0.44921, "ax":1.64472, "ay":2.28603, "alpha":-0.70598, "fx":[31.4691,25.76256,23.93748,30.73597], "fy":[36.34123,40.59043,41.67112,36.93639]}, + {"t":9.49586, "x":1.45812, "y":1.5228, "heading":0.73096, "vx":1.10543, "vy":1.5365, "omega":-0.47555, "ax":1.64436, "ay":2.28553, "alpha":-0.70544, "fx":[31.50197,25.81996,23.89441,30.66433], "fy":[36.29913,40.54241,41.68292,36.98018]}, + {"t":9.53316, "x":1.5005, "y":1.58171, "heading":0.71322, "vx":1.16677, "vy":1.62176, "omega":-0.50186, "ax":1.64394, "ay":2.28492, "alpha":-0.70482, "fx":[31.53482,25.8805,23.84985,30.58632], "fy":[36.25435,40.49006,41.69311,37.02607]}, + {"t":9.57047, "x":1.54517, "y":1.6438, "heading":0.6945, "vx":1.2281, "vy":1.707, "omega":-0.52816, "ax":1.64341, "ay":2.28419, "alpha":-0.70412, "fx":[31.56702,25.94391,23.80382,30.50129], "fy":[36.20662,40.43282,41.70089,37.07359]}, + {"t":9.60778, "x":1.59213, "y":1.70907, "heading":0.67479, "vx":1.28941, "vy":1.79221, "omega":-0.55442, "ax":1.64277, "ay":2.28329, "alpha":-0.7033, "fx":[31.59769,26.0098,23.75623,30.40837], "fy":[36.15547,40.3699,41.70518,37.12203]}, + {"t":9.64508, "x":1.64137, "y":1.77752, "heading":0.65411, "vx":1.35069, "vy":1.87739, "omega":-0.58066, "ax":1.64195, "ay":2.28215, "alpha":-0.70235, "fx":[31.62557,26.0775,23.70684,30.30634], "fy":[36.10015,40.30011,41.70441,37.17022]}, + {"t":9.68239, "x":1.6929, "y":1.84914, "heading":0.63245, "vx":1.41194, "vy":1.96253, "omega":-0.60686, "ax":1.64087, "ay":2.28066, "alpha":-0.70123, "fx":[31.64875,26.14593,23.65504,30.19331], "fy":[36.03932,40.2216,41.69615,37.21625]}, + {"t":9.71969, "x":1.74672, "y":1.92394, "heading":0.60981, "vx":1.47316, "vy":2.04761, "omega":-0.63302, "ax":1.6394, "ay":2.27862, "alpha":-0.69988, "fx":[31.66405,26.21317,23.59956,30.06617], "fy":[35.97056,40.13121,41.6763,37.25672]}, + {"t":9.757, "x":1.80282, "y":2.00192, "heading":0.58619, "vx":1.53432, "vy":2.13261, "omega":-0.65913, "ax":1.63727, "ay":2.27568, "alpha":-0.69818, "fx":[31.66569,26.27554,23.53772,29.9192], "fy":[35.88916,40.02301,41.63736,37.28517]}, + {"t":9.7943, "x":1.86119, "y":2.08306, "heading":0.5616, "vx":1.5954, "vy":2.21751, "omega":-0.68518, "ax":1.63393, "ay":2.27106, "alpha":-0.69587, "fx":[31.64158,26.32515,23.46336,29.74041], "fy":[35.78476,39.88444,41.5635,37.28769]}, + {"t":9.83161, "x":1.92185, "y":2.16736, "heading":0.53604, "vx":1.65635, "vy":2.30223, "omega":-0.71114, "ax":1.62791, "ay":2.26276, "alpha":-0.6923, "fx":[31.56079,26.3415,23.35986,29.49896], "fy":[35.63018,39.68294,41.41403,37.22825]}, + {"t":9.86891, "x":1.98477, "y":2.25482, "heading":0.50951, "vx":1.71708, "vy":2.38665, "omega":-0.73696, "ax":1.61393, "ay":2.24347, "alpha":-0.68504, "fx":[31.31152,26.24961,23.16554,29.08342], "fy":[35.3255,39.29994,41.04164,36.97587]}, + {"t":9.90622, "x":2.04995, "y":2.34542, "heading":0.48202, "vx":1.77729, "vy":2.47034, "omega":-0.76252, "ax":1.54564, "ay":2.14919, "alpha":-0.6517, "fx":[29.91516,25.38582,22.33322,27.52965], "fy":[33.98625,37.68739,39.16142,35.39327]}, + {"t":9.94352, "x":2.11733, "y":2.43907, "heading":0.45358, "vx":1.83495, "vy":2.55052, "omega":-0.78683, "ax":-1.54589, "ay":-2.14768, "alpha":0.74403, "fx":[-30.44689,-25.39744,-21.72664,-27.60979], "fy":[-33.55309,-37.75021,-39.52974,-35.29265]}, + {"t":9.98083, "x":2.18471, "y":2.53273, "heading":0.42422, "vx":1.77728, "vy":2.4704, "omega":-0.75907, "ax":-1.61386, "ay":-2.24274, "alpha":0.71431, "fx":[-31.60438,-26.5442,-22.88548,-28.77084], "fy":[-35.05852,-39.11211,-41.21233,-37.21042]}, + {"t":10.01813, "x":2.24989, "y":2.62332, "heading":0.39591, "vx":1.71707, "vy":2.38673, "omega":-0.73243, "ax":-1.62785, "ay":-2.26233, "alpha":0.70802, "fx":[-31.87422,-26.87051,-23.10929,-28.90296], "fy":[-35.34098,-39.33216,-41.56688,-37.68616]}, + {"t":10.05544, "x":2.31281, "y":2.71079, "heading":0.36858, "vx":1.65635, "vy":2.30233, "omega":-0.70601, "ax":-1.63388, "ay":-2.27079, "alpha":0.70521, "fx":[-32.00987,-27.07237,-23.19871,-28.88656], "fy":[-35.44566,-39.38514,-41.7238,-37.94711]}, + {"t":10.09275, "x":2.37346, "y":2.7951, "heading":0.34224, "vx":1.59539, "vy":2.21762, "omega":-0.67971, "ax":-1.63724, "ay":-2.2755, "alpha":0.70358, "fx":[-32.0979,-27.22995,-23.24563,-28.82255], "fy":[-35.4928,-39.38333,-41.81282,-38.13376]}, + {"t":10.13005, "x":2.43184, "y":2.87624, "heading":0.31689, "vx":1.53431, "vy":2.13273, "omega":-0.65346, "ax":-1.63938, "ay":-2.27852, "alpha":0.70251, "fx":[-32.16231,-27.36524,-23.27506,-28.73885], "fy":[-35.51537,-39.35775,-41.8698,-38.28466]}, + {"t":10.16736, "x":2.48794, "y":2.95422, "heading":0.29251, "vx":1.47316, "vy":2.04773, "omega":-0.62725, "ax":-1.64086, "ay":-2.2806, "alpha":0.70176, "fx":[-32.21253,-27.48671,-23.29639,-28.64638], "fy":[-35.52596,-39.32042,-41.90874,-38.41462]}, + {"t":10.20466, "x":2.54175, "y":3.02902, "heading":0.26911, "vx":1.41194, "vy":1.96265, "omega":-0.60107, "ax":-1.64194, "ay":-2.28214, "alpha":0.70122, "fx":[-32.25311,-27.5983,-23.31383,-28.55037], "fy":[-35.53037,-39.27702,-41.9363,-38.53053]}, + {"t":10.24197, "x":2.59328, "y":3.10065, "heading":0.24669, "vx":1.35069, "vy":1.87752, "omega":-0.57491, "ax":-1.64276, "ay":-2.28332, "alpha":0.70082, "fx":[-32.28656,-27.70208,-23.32945,-28.45364], "fy":[-35.53156,-39.23058,-41.95609,-38.63601]}, + {"t":10.27927, "x":2.64253, "y":3.16911, "heading":0.22524, "vx":1.28941, "vy":1.79234, "omega":-0.54877, "ax":-1.64341, "ay":-2.28425, "alpha":0.70053, "fx":[-32.31446,-27.79924,-23.34429,-28.3579], "fy":[-35.53118,-39.18289,-41.97031,-38.73314]}, + {"t":10.31658, "x":2.68949, "y":3.23438, "heading":0.20477, "vx":1.2281, "vy":1.70712, "omega":-0.52263, "ax":-1.64394, "ay":-2.285, "alpha":0.70032, "fx":[-32.33784,-27.89052,-23.3589,-28.26424], "fy":[-35.53015,-39.13509,-41.98036,-38.82321]}, + {"t":10.35388, "x":2.73416, "y":3.29647, "heading":0.18527, "vx":1.16677, "vy":1.62188, "omega":-0.49651, "ax":-1.64437, "ay":-2.28562, "alpha":0.70019, "fx":[-32.35747,-27.97638,-23.37354,-28.17342], "fy":[-35.52903,-39.08791,-41.98722,-38.90707]}, + {"t":10.39119, "x":2.77654, "y":3.35539, "heading":0.16675, "vx":1.10543, "vy":1.53661, "omega":-0.47039, "ax":-1.64473, "ay":-2.28615, "alpha":0.70011, "fx":[-32.37393,-28.05715,-23.3883,-28.08595], "fy":[-35.52814,-39.04188,-41.99159,-38.98528]}, + {"t":10.42849, "x":2.81663, "y":3.41112, "heading":0.1492, "vx":1.04407, "vy":1.45133, "omega":-0.44427, "ax":-1.64503, "ay":-2.28659, "alpha":0.70007, "fx":[-32.38767,-28.13306,-23.40318,-28.00221], "fy":[-35.52764,-38.99736,-41.99401,-39.05825]}, + {"t":10.4658, "x":2.85444, "y":3.46367, "heading":0.13263, "vx":0.9827, "vy":1.36602, "omega":-0.41815, "ax":-1.64529, "ay":-2.28698, "alpha":0.70007, "fx":[-32.39908,-28.20428,-23.41812,-27.9225], "fy":[-35.52762,-38.95464,-41.99491,-39.12629]}, + {"t":10.5031, "x":2.88995, "y":3.51304, "heading":0.11703, "vx":0.92132, "vy":1.28071, "omega":-0.39204, "ax":-1.64552, "ay":-2.28731, "alpha":0.7001, "fx":[-32.40849,-28.27093,-23.43302,-27.84702], "fy":[-35.5281,-38.91393,-41.99461,-39.18963]}, + {"t":10.54041, "x":2.92318, "y":3.55923, "heading":0.1024, "vx":0.85994, "vy":1.19538, "omega":-0.36592, "ax":-1.64572, "ay":-2.28761, "alpha":0.70015, "fx":[-32.41618,-28.33313,-23.44778,-27.77593], "fy":[-35.52906,-38.87539,-41.99341,-39.24845]}, + {"t":10.57772, "x":2.95411, "y":3.60223, "heading":0.08875, "vx":0.79854, "vy":1.11004, "omega":-0.3398, "ax":-1.6459, "ay":-2.28787, "alpha":0.70022, "fx":[-32.42241,-28.39095,-23.46227,-27.70938], "fy":[-35.53046,-38.83913,-41.99154,-39.30291]}, + {"t":10.61502, "x":2.98276, "y":3.64205, "heading":0.07607, "vx":0.73714, "vy":1.02469, "omega":-0.31368, "ax":-1.64605, "ay":-2.2881, "alpha":0.70029, "fx":[-32.4274,-28.44447,-23.47638,-27.64744], "fy":[-35.53224,-38.80528,-41.9892,-39.35314]}, + {"t":10.65233, "x":3.00911, "y":3.67868, "heading":0.06437, "vx":0.67573, "vy":0.93933, "omega":-0.28755, "ax":-1.6462, "ay":-2.28831, "alpha":0.70038, "fx":[-32.43134,-28.49373,-23.48997,-27.5902], "fy":[-35.53434,-38.7739,-41.98656,-39.39925]}, + {"t":10.68963, "x":3.03318, "y":3.71213, "heading":0.05365, "vx":0.61432, "vy":0.85396, "omega":-0.26142, "ax":-1.64632, "ay":-2.2885, "alpha":0.70047, "fx":[-32.43442,-28.53879,-23.50293,-27.53771], "fy":[-35.53667,-38.74505,-41.98378,-39.44132]}, + {"t":10.72694, "x":3.05495, "y":3.7424, "heading":0.04389, "vx":0.55291, "vy":0.76859, "omega":-0.23529, "ax":-1.64644, "ay":-2.28867, "alpha":0.70055, "fx":[-32.4368,-28.57969,-23.51516,-27.49001], "fy":[-35.53918,-38.7188,-41.98097,-39.47944]}, + {"t":10.76424, "x":3.07443, "y":3.76948, "heading":0.03512, "vx":0.49149, "vy":0.68321, "omega":-0.20916, "ax":-1.64654, "ay":-2.28882, "alpha":0.70064, "fx":[-32.43863,-28.61648,-23.52655,-27.44713], "fy":[-35.54179,-38.69519,-41.97827,-39.51369]}, + {"t":10.80155, "x":3.09162, "y":3.79337, "heading":0.02731, "vx":0.43006, "vy":0.59782, "omega":-0.18302, "ax":-1.64664, "ay":-2.28897, "alpha":0.70071, "fx":[-32.44003,-28.64919,-23.53701,-27.40909], "fy":[-35.54443,-38.67425,-41.97577,-39.54413]}, + {"t":10.83885, "x":3.10652, "y":3.81408, "heading":0.02048, "vx":0.36863, "vy":0.51243, "omega":-0.15688, "ax":-1.64673, "ay":-2.2891, "alpha":0.70078, "fx":[-32.44112,-28.67784,-23.54646,-27.37592], "fy":[-35.54705,-38.65602,-41.97354,-39.57082]}, + {"t":10.87616, "x":3.11912, "y":3.83161, "heading":0.01463, "vx":0.3072, "vy":0.42704, "omega":-0.13074, "ax":-1.64681, "ay":-2.28922, "alpha":0.70084, "fx":[-32.44201,-28.70246,-23.55482,-27.34761], "fy":[-35.54958,-38.64051,-41.97168,-39.5938]}, + {"t":10.91346, "x":3.12944, "y":3.84594, "heading":0.00975, "vx":0.24577, "vy":0.34164, "omega":-0.10459, "ax":-1.64688, "ay":-2.28933, "alpha":0.70089, "fx":[-32.44276,-28.72308,-23.56203,-27.32419], "fy":[-35.55198,-38.62774,-41.97024,-39.61313]}, + {"t":10.95077, "x":3.13746, "y":3.85709, "heading":0.00585, "vx":0.18433, "vy":0.25623, "omega":-0.07845, "ax":-1.64695, "ay":-2.28943, "alpha":0.70092, "fx":[-32.44347,-28.73972,-23.56804,-27.30565], "fy":[-35.55422,-38.61775,-41.96928,-39.62883]}, + {"t":10.98807, "x":3.14319, "y":3.86506, "heading":0.00293, "vx":0.12289, "vy":0.17083, "omega":-0.0523, "ax":-1.64702, "ay":-2.28952, "alpha":0.70094, "fx":[-32.44418,-28.75239,-23.57281,-27.29201], "fy":[-35.55625,-38.61052,-41.96884,-39.64095]}, + {"t":11.02538, "x":3.14663, "y":3.86984, "heading":0.00098, "vx":0.06145, "vy":0.08541, "omega":-0.02615, "ax":-1.64708, "ay":-2.28961, "alpha":0.70095, "fx":[-32.44495,-28.76111,-23.57631,-27.28326], "fy":[-35.55806,-38.60609,-41.96895,-39.6495]}, + {"t":11.06268, "x":3.14777, "y":3.87143, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index cc3effd..01b782d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,41 +1,24 @@ package frc.robot; -import static edu.wpi.first.units.Units.DegreesPerSecond; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Seconds; - import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.path.PathConstraints; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N2; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.units.Units; -import edu.wpi.first.units.VoltageUnit; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Time; -import edu.wpi.first.units.measure.Velocity; -import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.DriverStation; -import frc.robot.lib.control.ControlConstants.PIDFConstants; -import frc.robot.lib.control.ControlConstants.ProfiledPIDFConstants; -import frc.robot.subsystems.drive.ctre.CtreDriveConstants; +import frc.robot.subsystems.drive.DriveConstants; /** - * All constants belong here. + * Miscallenous constants */ public final class Constants { public static final double DT = 0.02; - public static final double DEADBAND = 1e-6; + public static final double EPSILON = 1e-6; public static final double LONG_CANT_TIMEOUT_MS = 0; public static final double TAU = Math.PI * 2; @@ -52,122 +35,6 @@ public static final class Odometry { Math.pow(0.02, 1), // vision Math.pow(0.02, 1)); } - - - public static final class Drive { - // public static final COTSTalonFXSwerveConstants SWERVE_MODULE_TYPE = - // COTSTalonFXSwerveConstants.SDS.MK4i.KrakenX60(COTSTalonFXSwerveConstants.SDS.MK4i.driveRatios.L3); - - public static final double TRACK_WIDTH = Units.Inches.of(24).in(Units.Meters); - public static final double WHEEL_BASE = Units.Inches.of(24).in(Units.Meters); - public static final double WHEEL_DIAMETER = 2 * CtreDriveConstants.kWheelRadius.in(Meters);//SWERVE_MODULE_TYPE.wheelDiameter; - public static final double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER*Math.PI;//SWERVE_MODULE_TYPE.wheelCircumference; - - public static final Translation2d[] MODULE_LOCATIONS = { - new Translation2d(-WHEEL_BASE / 2.0, TRACK_WIDTH / 2.0), - new Translation2d(WHEEL_BASE / 2.0, TRACK_WIDTH / 2.0), - new Translation2d(-WHEEL_BASE / 2.0, -TRACK_WIDTH / 2.0), - new Translation2d(WHEEL_BASE / 2.0, -TRACK_WIDTH / 2.0) - }; - - public static final double DRIVE_GEAR_RATIO = CtreDriveConstants.kDriveGearRatio;//SWERVE_MODULE_TYPE.driveGearRatio; - public static final double ANGLE_GEAR_RATIO = CtreDriveConstants.kSteerGearRatio;//SWERVE_MODULE_TYPE.angleGearRatio; - - public static final double MAX_SPEED = 5.0; - public static final double MAX_ACCEL = 5.0; - public static final double MAX_ROTATION_SPEED = DegreesPerSecond.of(540.0).in(RadiansPerSecond); - public static final double MAX_ROTATION_ACCEL = DegreesPerSecond.of(2880.0).in(RadiansPerSecond); - - public static final int ANGLE_CURRENT_LIMIT = 20; - public static final int ANGLE_CURRENT_THRESHOLD = 30; - public static final double ANGLE_CURRENT_THRESHOLD_TIME = 0.1; - public static final boolean ANGLE_ENABLE_CURRENT_LIMIT = true; - - public static final int DRIVE_CURRENT_LIMIT = 30; - public static final int DRIVE_CURRENT_THRESHOLD = 45; - public static final double DRIVE_CURRENT_THRESHOLD_TIME = 0.1; - public static final boolean DRIVE_ENABLE_CURRENT_LIMIT = true; - - public static final double DRIVE_MOTOR_KV = 12 * Math.PI * WHEEL_DIAMETER / (DRIVE_GEAR_RATIO * MAX_SPEED); - - public static final PIDFConstants ANGLE_MOTOR_PIDF_CONSTANTS = new PIDFConstants( - 2.0, 0.0, 0.0, 0); - public static final PIDFConstants DRIVE_MOTOR_PIDF_CONSTANTS = new PIDFConstants( - 1.2, 0.005, 0.0, DRIVE_MOTOR_KV); - - public static final double OPEN_LOOP_RAMP = 0.25; - public static final double CLOSED_LOOP_RAMP = 0.0; - - public static final boolean INVERT_GYRO = false; - - public static final double MAX_VELOCITY_STABLE = 10; // degrees per second - - public static final double MAX_PITCH_STABLE = 5; // degrees - - public static final LinearVelocity kScoringTranslationMaxSpeed = - Units.Centimeters.of(15.0).per(Units.Seconds); - public static final AngularVelocity kScoringRotationMaxSpeed = - Units.Degrees.of(7.0).per(Units.Seconds); // oh god - - public static final Time POSE_RESET_PREVENTION_TIME = Seconds.of(0.15); - - public static enum ModuleConstants { - /** Module 0 */ - FRONT_LEFT ( - 8, 10, 7, Rotation2d.fromRotations(0.142334), true, false), - /** Module 1 */ - FRONT_RIGHT ( - 9, 11, 6, Rotation2d.fromRotations(0.427246), true, false), - /** Module 2 */ - BACK_LEFT ( - 3, 5, 0, Rotation2d.fromRotations(0.174316), true, false), - /** Module 3 */ - BACK_RIGHT ( - 4, 2, 1, Rotation2d.fromRotations(0.413330), false, false); - - public final int driveMotorID; - public final int angleMotorID; - public final int cancoderID; - public final Rotation2d angleOffset; - public final boolean driveInvert; - public final boolean angleInvert; - - /** - * Swerve Module Constants to be used when creating swerve modules. - * @param driveMotorID - * @param angleMotorID - * @param canCoderID - * @param angleOffset - */ - private ModuleConstants(int driveMotorID, int angleMotorID, int canCoderID, Rotation2d angleOffset, boolean driveInvert, boolean angleInvert) { - this.driveMotorID = driveMotorID; - this.angleMotorID = angleMotorID; - this.cancoderID = canCoderID; - this.angleOffset = angleOffset; - this.driveInvert = driveInvert; - this.angleInvert = angleInvert; - } - } - } - - public static final class Auto { - public static final PIDFConstants TRANSLATION_CONSTANTS = - new PIDFConstants(3.5, 0.0, 0.1, 1.0); - - public static final ProfiledPIDFConstants PROFILED_TRANSLATION_CONSTANTS = - new ProfiledPIDFConstants(3.5, 0.0, 0.1, 1.0, - new TrapezoidProfile.Constraints( - Drive.MAX_SPEED * 0.7, - Drive.MAX_ACCEL * 0.7)); - - public static final ProfiledPIDFConstants ROTATION_CONSTANTS = - new ProfiledPIDFConstants(4.0, 0.0, 0.0, 1.0, - new TrapezoidProfile.Constraints( - Drive.MAX_ROTATION_SPEED, - Drive.MAX_ROTATION_ACCEL)); - public static final double ACCELERATION_CONSTANT = 0.1; - } - public static final class Limelight { //TODO: this must be tuned to specific robot public static enum VisionDeviceConstants { // L_CONSTANTS ( @@ -240,47 +107,6 @@ private VisionDeviceConstants( } } - public static enum Calibration { - DriveRotation ( - Units.Volts.of(1).per(Units.Second), - Units.Volts.of(7)), - - DriveTranslation ( - Units.Volts.of(1).per(Units.Second), - Units.Volts.of(7)), - - AngleMotor ( - Units.Volts.of(1).per(Units.Second), - Units.Volts.of(7)), - - DriveMotor ( - Units.Volts.of(1).per(Units.Second), - Units.Volts.of(4)); - - public final Velocity RAMP_RATE; - public final Voltage DYNAMIC_VOLTAGE; - private Calibration(Velocity rampRate, Voltage dynamicVoltage) { - RAMP_RATE = rampRate; - DYNAMIC_VOLTAGE = dynamicVoltage; - } - } - - public static enum Port { // TODO: this must be tuned to the specific robot - FL_CANCODER (7, "CV"), - FR_CANCODER (6, "CV"), - BL_CANCODER (14, "CV"), - BR_CANCODER (1, "CV"), - LEDS (21, "CV"), - PIGEON (60, "CV"); - - public final int id; - public final String bus; - private Port(int id, String bus) { - this.id = id; - this.bus = bus; - } - } - public static final class Pathplanner { public static RobotConfig config; static { @@ -291,10 +117,10 @@ public static final class Pathplanner { } } public static final PathConstraints GLOBAL_CONSTRAINTS = - new PathConstraints(Drive.MAX_SPEED * 0.85, - Drive.MAX_ACCEL * 0.85, - Drive.MAX_ROTATION_SPEED * 0.85, - Drive.MAX_ROTATION_ACCEL * 0.85); + new PathConstraints(DriveConstants.MAX_SPEED * 0.85, + DriveConstants.MAX_ACCEL * 0.85, + DriveConstants.MAX_ROTATION_SPEED * 0.85, + DriveConstants.MAX_ROTATION_ACCEL * 0.85); public static final double GENERATION_WAIT_TIME = 5; } } diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index 353828b..b59c59f 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -3,33 +3,27 @@ import static frc.robot.Robot.controller; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.drive.commands.PIDToPoseCommand; import frc.robot.subsystems.drive.ctre.CtreDrive.SysIdRoutineType; public class ControlsMapping { public static void mapTeleopCommand() { - Drive.getInstance().setDefaultCommand((Drive.getInstance().teleopCommand())); - // run sysID functions - Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER); - - controller.a().onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); + controller.back().onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); controller.leftBumper().whileTrue(Drive.getInstance().autoAlign(true)); controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false)); - controller.b().whileTrue(new PIDToPoseCommand( - new Pose2d(2.0, 1.0, Rotation2d.fromDegrees(120.0)))); } public static void mapSysId() { - controller.a().onTrue( + // run sysID functions + Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER); + controller.a().whileTrue( Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kForward)); - controller.b().onTrue( + controller.b().whileTrue( Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kReverse)); - controller.x().onTrue( + controller.x().whileTrue( Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kForward)); - controller.y().onTrue( + controller.y().whileTrue( Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kReverse)); } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f8ef887..9c88a50 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -2,12 +2,18 @@ import frc.robot.auto.AutoSelector; +import java.util.Optional; + import com.pathplanner.lib.commands.FollowPathCommand; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.logging.EpilogueBackend; import edu.wpi.first.hal.AllianceStationID; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.simulation.DriverStationSim; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.Controllers; @@ -20,9 +26,10 @@ * the TimedRobot documentation. If you change the name of this class or the package after creating * this project, you must also update the Main.java file in the project. */ +@SuppressWarnings("unused") public class Robot extends TimedRobot { private static final CommandScheduler commandScheduler = CommandScheduler.getInstance(); - public AutoSelector autoChooser; + private AutoSelector autoChooser; private Command autoCommand; public static final CommandXboxController controller = @@ -33,22 +40,12 @@ public class Robot extends TimedRobot { * initialization code. */ public Robot() { - if (Robot.isSimulation()) { - // TODO: remove this - - } - - // RobotState.reset(Timer.getFPGATimestamp(), new Pose2d()); - // RobotState.resetKalman(); - - Drive.getInstance(); if (Robot.isReal()) { VisionDeviceManager.getInstance(); } + Drive.getInstance(); TelemetryManager.getInstance(); - - FollowPathCommand.warmupCommand().schedule();; - + FollowPathCommand.warmupCommand().schedule(); autoChooser = new AutoSelector(); } @@ -71,7 +68,6 @@ public void robotPeriodic() { /** This function is called once each time the robot enters Disabled mode. */ @Override public void disabledInit() { - } /** This function is called periodically during disabled. */ @@ -83,9 +79,7 @@ public void disabledPeriodic() { /** This autonomous runs the autonomous command selected. */ @Override public void autonomousInit() { - // RobotState.setAlliance(DriverStation.getAlliance()); autoCommand = autoChooser.getAuto(); - if (autoCommand != null) { autoCommand.schedule(); } else { @@ -109,7 +103,8 @@ public void teleopInit() { if (autoCommand != null) { autoCommand.cancel(); } - + Drive.getInstance().setDefaultCommand(Drive.getInstance().teleopCommand()); + ControlsMapping.mapTeleopCommand(); } diff --git a/src/main/java/frc/robot/auto/AutoRoutines.java b/src/main/java/frc/robot/auto/AutoRoutines.java index f2567db..8f5ca36 100644 --- a/src/main/java/frc/robot/auto/AutoRoutines.java +++ b/src/main/java/frc/robot/auto/AutoRoutines.java @@ -1,5 +1,6 @@ package frc.robot.auto; +import frc.robot.auto.AutoSelector.Auto; import frc.robot.lib.trajectory.RedTrajectory; import frc.robot.lib.trajectory.TrajectoryLoader; import frc.robot.lib.trajectory.RedTrajectory.TrajectoryType; @@ -10,11 +11,13 @@ import edu.wpi.first.wpilibj2.command.Command; public final class AutoRoutines { + @Auto(name = "Pid Test") public static Command testPidToPose() { return new PIDToPoseCommand( new Pose2d(2.0, 1.0, Rotation2d.fromDegrees(120))); } + @Auto(name = "Trajectory Test") public static Command testTrajectoryAuto() { RedTrajectory traj = TrajectoryLoader.loadAutoTrajectory(TrajectoryType.PATHPLANNER, "testPath2").get(); diff --git a/src/main/java/frc/robot/auto/AutoSelector.java b/src/main/java/frc/robot/auto/AutoSelector.java index fbc7916..4cb4551 100644 --- a/src/main/java/frc/robot/auto/AutoSelector.java +++ b/src/main/java/frc/robot/auto/AutoSelector.java @@ -1,8 +1,13 @@ package frc.robot.auto; +import java.lang.annotation.ElementType; +import java.lang.annotation.Retention; +import java.lang.annotation.RetentionPolicy; +import java.lang.annotation.Target; import java.lang.reflect.Method; import java.util.function.Supplier; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -11,20 +16,37 @@ * A class to select autos */ public class AutoSelector { + @Retention(RetentionPolicy.RUNTIME) + @Target(ElementType.METHOD) + /** An autonomous routine command. */ + public static @interface Auto { + String name() default ""; + } + private final SendableChooser> chooser = new SendableChooser<>(); public AutoSelector() { Method[] autos = AutoRoutines.class.getMethods(); + // Warning: dark and evil magic below for (Method auto : autos) { - var name = auto.getName(); - if (auto.getReturnType().equals(Command.class)) { - chooser.addOption(name, () -> { - try{ - return (Command) auto.invoke(null); - } catch (Exception e) { - return null; - }}); - // System.out.println(name); + if (auto.isAnnotationPresent(Auto.class)) { + String name = auto.getAnnotation(Auto.class).name(); + if (name.equals("")) { + name = auto.getName(); + } + + if (auto.getReturnType() == Command.class) { + chooser.addOption(name, () -> { + try { + return (Command) auto.invoke(null); + } catch (Exception e) { + return null; + } + }); + } else { + DriverStation.reportWarning( + "@Auto annotation for this element is not supported", true); + } } } diff --git a/src/main/java/frc/robot/auto/Automation.java b/src/main/java/frc/robot/auto/Automation.java index 2fe192f..8de0739 100644 --- a/src/main/java/frc/robot/auto/Automation.java +++ b/src/main/java/frc/robot/auto/Automation.java @@ -1,8 +1,5 @@ package frc.robot.auto; -import frc.robot.subsystems.*; - -@SuppressWarnings("unused") public class Automation { } diff --git a/src/main/java/frc/robot/lib/control/PIDVController.java b/src/main/java/frc/robot/lib/control/PIDVController.java index d1de0f3..7f1019c 100644 --- a/src/main/java/frc/robot/lib/control/PIDVController.java +++ b/src/main/java/frc/robot/lib/control/PIDVController.java @@ -1,8 +1,7 @@ package frc.robot.lib.control; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.Pair; -import edu.wpi.first.wpilibj.Timer; +import frc.robot.Constants; import frc.robot.lib.control.ControlConstants.*; public class PIDVController { @@ -15,14 +14,12 @@ public class PIDVController { private double feedforward = 0.0; private double integral = 0.0; - public double error = 0.0; + private double error = 0.0; private boolean isContinuous = false; private double minRange = 0.0; private double maxRange = 0.0; - private final Timer timer = new Timer(); - /** * Creates a PIDV controller, which is a PID controller * where the derivative is replaced by accurate velocity measurements. @@ -30,7 +27,6 @@ public class PIDVController { */ public PIDVController(PIDFConstants constants) { this.constants = constants; - timer.start(); } public PIDVController(PIDConstants constants) { @@ -42,41 +38,49 @@ public PIDVController(PIDConstants constants) { * @param minInput The minimum value. * @param maxInput The maximum value. */ - public void enableContinuousInput(double minInput, double maxInput) { + public PIDVController enableContinuousInput(double minInput, double maxInput) { isContinuous = true; minRange = minInput; maxRange = maxInput; + return this; } /** Makes the controller discontinuous */ - public void disableContinuousInput() { + public PIDVController disableContinuousInput() { isContinuous = false; + return this; } - public void setInput(double positionMeasurement, double velocityMeasurement) { + /** Sets the current position and velocity measurement. */ + public PIDVController setMeasurement(double positionMeasurement, double velocityMeasurement) { this.positionMeasurement = positionMeasurement; this.velocityMeasurement = velocityMeasurement; + return this; } - public void setTarget(double target) { + /** Sets the goal */ + public PIDVController setTarget(double target) { this.target = target; + return this; } /** Sets the feedforward value. */ - public void setFeedforward(double feedforward) { + public PIDVController setFeedforward(double feedforward) { this.feedforward = feedforward; + return this; } public double getOutput() { - double dt = timer.get(); - timer.reset(); - if (dt <= 0.0) return 0.0; - - error = isContinuous - ? MathUtil.inputModulus(target - positionMeasurement, -(maxRange - minRange) / 2.0, (maxRange - minRange) / 2.0) - : target - positionMeasurement; + if (isContinuous) { + error = MathUtil.inputModulus( + target - positionMeasurement, + -(maxRange - minRange) / 2.0, + (maxRange - minRange) / 2.0); + } else { + error = target - positionMeasurement; + } - integral += error * dt; + integral += error * Constants.DT; double derivative = feedforward - velocityMeasurement; @@ -86,17 +90,21 @@ public double getOutput() { + constants.kF * feedforward; } + public double getError() { + return error; + } + /** Sets the integral value. */ - public void setIntegral(double integral) { + public PIDVController setIntegral(double integral) { this.integral = integral; + return this; } /** Resets the controller. */ - public void reset() { + public PIDVController reset() { integral = 0.0; feedforward = 0.0; error = 0.0; - timer.reset(); - timer.start(); + return this; } } diff --git a/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java b/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java index 438d037..e88480f 100644 --- a/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java +++ b/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java @@ -1,143 +1,167 @@ package frc.robot.lib.control; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.Pair; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.Timer; +import frc.robot.Constants; import frc.robot.lib.control.ControlConstants.*; -/** - * A profiled version of {@link PIDVController} that uses a TrapezoidProfile - * to smoothly reach the target position with velocity and acceleration limits. - * The derivative term is based on velocity feedback (like PIDV). - */ +/** Note: broken do not use rn */ public class ProfiledPIDVController { - private final PIDFConstants constants; - private TrapezoidProfile.Constraints constraints; - private TrapezoidProfile profile; + private final PIDFConstants constants; + private TrapezoidProfile.Constraints constraints; + private TrapezoidProfile profile; - private TrapezoidProfile.State goal = new TrapezoidProfile.State(); - private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); + private TrapezoidProfile.State goal = new TrapezoidProfile.State(); + private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); - private double positionMeasurement = 0.0; - private double velocityMeasurement = 0.0; - private double feedforward = 0.0; - private double integral = 0.0; + private double positionMeasurement = 0.0; + private double velocityMeasurement = 0.0; + private double feedforward = 0.0; + private boolean isFeedforwardSet = false; + private double integral = 0.0; - public double error = 0.0; + public double error = 0.0; - private boolean isContinuous = false; - private double minRange = 0.0; - private double maxRange = 0.0; - - private final Timer timer = new Timer(); + private boolean isContinuous = false; + private double minRange = 0.0; + private double maxRange = 0.0; + private double positionTolerance = 0.01; + private double velocityTolerance = Double.POSITIVE_INFINITY; /** - * Creates a profiled PIDV controller. - * @param constants The {@link PIDFConstants}. - * @param constraints The trapezoidal motion constraints (max velocity and acceleration). - */ - public ProfiledPIDVController(ProfiledPIDFConstants constants) { - this.constants = new PIDFConstants( - constants.kP, - constants.kI, - constants.kD, - constants.kF); - this.constraints = constants.constraints; - this.profile = new TrapezoidProfile(constraints); - timer.start(); - } - - /** Enables continuous input. */ - public void enableContinuousInput(double minInput, double maxInput) { - isContinuous = true; - minRange = minInput; - maxRange = maxInput; - } - - /** Disables continuous input. */ - public void disableContinuousInput() { - isContinuous = false; - } - - /** Sets the current position and velocity measurement. */ - public void setInput(double positionMeasurement, double velocityMeasurement) { - this.positionMeasurement = positionMeasurement; - this.velocityMeasurement = velocityMeasurement; - } - - /** Sets the goal position for the profile (end velocity = 0). */ - public void setTarget(double target) { - this.goal = new TrapezoidProfile.State(target, 0.0); - } - - /** Sets the goal with both position and end velocity. */ - public void setTarget(double position, double velocity) { - this.goal = new TrapezoidProfile.State(position, velocity); - } - - /** Sets the feedforward value. */ - public void setFeedforward(double feedforward) { - this.feedforward = feedforward; - } + * Creates a Profiled PIDV controller, which is a PID controller + * where the derivative is replaced by accurate velocity measurements, + * and it's also profiled + * @param constants The {@link ProfiledPIDFConstants}. + */ + public ProfiledPIDVController(ProfiledPIDFConstants constants) { + this.constants = new PIDFConstants(constants.kP, constants.kI, constants.kD, constants.kF); + this.constraints = constants.constraints; + this.profile = new TrapezoidProfile(constraints); + } /** - * Computes the controller output using the current profile and measurements. - * @return Control output (PID + feedforward). + * Makes the controller continuous, which means that values repeat. + * @param minInput The minimum value. + * @param maxInput The maximum value. */ - public double getOutput() { - double dt = timer.get(); - timer.reset(); - if (dt <= 0.0) { - return 0.0; - } - - setpoint = profile.calculate(dt, goal, setpoint); - - double targetPosition = setpoint.position; - double targetVelocity = setpoint.velocity; - - error = isContinuous - ? MathUtil.inputModulus(targetPosition - positionMeasurement, -(maxRange - minRange) / 2.0, (maxRange - minRange) / 2.0) - : targetPosition - positionMeasurement; - - integral += error * dt; - - double derivative = targetVelocity - velocityMeasurement; - - return constants.kP * error - + constants.kI * integral - + constants.kD * derivative - + constants.kF * feedforward; - } - - /** Sets the integral term directly. */ - public void setIntegral(double integral) { - this.integral = integral; - } - - /** Resets the controller state. */ - public void reset() { - integral = 0.0; - feedforward = 0.0; - error = 0.0; - setpoint = new TrapezoidProfile.State(positionMeasurement, velocityMeasurement); - timer.reset(); - timer.start(); - } - - /** Returns the current motion profile setpoint. */ - public TrapezoidProfile.State getSetpoint() { - return setpoint; - } - - /** Returns the goal state. */ - public TrapezoidProfile.State getGoal() { - return goal; - } - - /** Updates motion constraints. */ - public void setConstraints(TrapezoidProfile.Constraints constraints) { - this.constraints = constraints; - } + public ProfiledPIDVController enableContinuousInput(double minInput, double maxInput) { + isContinuous = true; + minRange = minInput; + maxRange = maxInput; + return this; + } + + /** Makes the controller discontinuous */ + public ProfiledPIDVController disableContinuousInput() { + isContinuous = false; + return this; + } + + /** Sets the current position and velocity measurement. */ + public ProfiledPIDVController setMeasurement(double positionMeasurement, double velocityMeasurement) { + this.positionMeasurement = positionMeasurement; + this.velocityMeasurement = velocityMeasurement; + return this; + } + + /** Sets the goal */ + public ProfiledPIDVController setTarget(double target) { + this.goal = new TrapezoidProfile.State(target, 0.0); + return this; + } + + /** Sets the goal */ + public void setTarget(double position, double velocity) { + this.goal = new TrapezoidProfile.State(position, velocity); + } + + /** Sets the feedforward value. */ + public ProfiledPIDVController setFeedforward(double feedforward) { + this.feedforward = feedforward; + isFeedforwardSet = true; + return this; + } + + public double getOutput() { + if (isContinuous) { + double errorBound = (maxRange - minRange) / 2.0; + double goalMinDistance = MathUtil.inputModulus( + goal.position - positionMeasurement, -errorBound, errorBound); + double setpointMinDistance = MathUtil.inputModulus( + setpoint.position - positionMeasurement, -errorBound, errorBound); + goal.position = goalMinDistance + positionMeasurement; + setpoint.position = setpointMinDistance + positionMeasurement; + } + + setpoint = profile.calculate(Constants.DT, setpoint, goal); + + double targetPosition = setpoint.position; + double targetVelocity = setpoint.velocity; + + error = targetPosition - positionMeasurement; + integral += error * Constants.DT; + double derivative = targetVelocity - velocityMeasurement; + + if (!isFeedforwardSet) { + feedforward = targetVelocity; + } + isFeedforwardSet = false; + + return constants.kP * error + + constants.kI * integral + + constants.kD * derivative + + constants.kF * feedforward; + } + + /** Sets the integral value. */ + public ProfiledPIDVController setIntegral(double integral) { + this.integral = integral; + return this; + } + + /** Resets the controller. */ + public ProfiledPIDVController reset() { + integral = 0.0; + feedforward = 0.0; + error = 0.0; + setpoint = new TrapezoidProfile.State(positionMeasurement, velocityMeasurement); + return this; + } + + public TrapezoidProfile.State getSetpoint() { + return setpoint; + } + + public TrapezoidProfile.State getGoal() { + return goal; + } + + public double getError() { + return error; + } + + public ProfiledPIDVController setConstraints(TrapezoidProfile.Constraints constraints) { + this.constraints = constraints; + this.profile = new TrapezoidProfile(constraints); + return this; + } + + public ProfiledPIDVController setTolerance(double positionTolerance, double velocityTolerance) { + this.positionTolerance = positionTolerance; + this.velocityTolerance = velocityTolerance; + return this; + } + + public ProfiledPIDVController setTolerance(double positionTolerance) { + return setTolerance(positionTolerance, Double.POSITIVE_INFINITY); + } + + public boolean atSetpoint() { + return Math.abs(error) < positionTolerance && Math.abs(setpoint.velocity) < velocityTolerance; + } + + public boolean atGoal() { + return atSetpoint() && goal.equals(setpoint); + } } diff --git a/src/main/java/frc/robot/lib/localization/FieldLayout.java b/src/main/java/frc/robot/lib/field/FieldLayout.java similarity index 90% rename from src/main/java/frc/robot/lib/localization/FieldLayout.java rename to src/main/java/frc/robot/lib/field/FieldLayout.java index 37032d4..c16c84f 100644 --- a/src/main/java/frc/robot/lib/localization/FieldLayout.java +++ b/src/main/java/frc/robot/lib/field/FieldLayout.java @@ -1,10 +1,8 @@ -package frc.robot.lib.localization; +package frc.robot.lib.field; import java.io.IOException; import java.util.ArrayList; -import java.util.HashSet; import java.util.List; -import java.util.Set; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; @@ -15,7 +13,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.Constants; +import frc.robot.subsystems.drive.DriveConstants; /** * Contains various field dimensions and useful reference points. Dimensions are @@ -51,12 +49,12 @@ public class FieldLayout { SmartDashboard.putData(field); int[] reefIds = {6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22}; Transform2d leftReefTransform = new Transform2d( - Constants.Drive.TRACK_WIDTH / 2 + Units.inchesToMeters(1.5), -Units.inchesToMeters(13.0 / 2), Rotation2d.kPi); + DriveConstants.TRACK_WIDTH / 2 + Units.inchesToMeters(1.5), -Units.inchesToMeters(13.0 / 2), Rotation2d.kPi); for (int i : reefIds) { ALIGN_POSES_LEFT.add(APRILTAG_MAP.getTagPose(i).get().toPose2d().transformBy(leftReefTransform)); } Transform2d rightReefTransform = new Transform2d( - Constants.Drive.TRACK_WIDTH / 2 + Units.inchesToMeters(1.5), Units.inchesToMeters(13.0 / 2), Rotation2d.kPi); + DriveConstants.TRACK_WIDTH / 2 + Units.inchesToMeters(1.5), Units.inchesToMeters(13.0 / 2), Rotation2d.kPi); for (int i : reefIds) { ALIGN_POSES_RIGHT.add(APRILTAG_MAP.getTagPose(i).get().toPose2d().transformBy(rightReefTransform)); } diff --git a/src/main/java/frc/robot/lib/localization/FieldUtil.java b/src/main/java/frc/robot/lib/field/FieldUtil.java similarity index 99% rename from src/main/java/frc/robot/lib/localization/FieldUtil.java rename to src/main/java/frc/robot/lib/field/FieldUtil.java index 4c2d272..c8666d4 100644 --- a/src/main/java/frc/robot/lib/localization/FieldUtil.java +++ b/src/main/java/frc/robot/lib/field/FieldUtil.java @@ -1,4 +1,4 @@ -package frc.robot.lib.localization; +package frc.robot.lib.field; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; diff --git a/src/main/java/frc/robot/lib/sim/Mechanisms.java b/src/main/java/frc/robot/lib/sim/Mechanisms.java new file mode 100644 index 0000000..8c2ac13 --- /dev/null +++ b/src/main/java/frc/robot/lib/sim/Mechanisms.java @@ -0,0 +1,11 @@ +package frc.robot.lib.sim; + +public class Mechanisms { + private static Mechanisms ssmechInstance; + public static Mechanisms getInstance() { + if (ssmechInstance == null) { + ssmechInstance = new Mechanisms(); + } + return ssmechInstance; + } +} diff --git a/src/main/java/frc/robot/lib/trajectory/RedTrajectory.java b/src/main/java/frc/robot/lib/trajectory/RedTrajectory.java index 1875085..6f39a58 100644 --- a/src/main/java/frc/robot/lib/trajectory/RedTrajectory.java +++ b/src/main/java/frc/robot/lib/trajectory/RedTrajectory.java @@ -10,8 +10,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc.robot.Constants; -// import frc.robot.RobotState; -import frc.robot.lib.localization.FieldUtil; +import frc.robot.lib.field.FieldUtil; public class RedTrajectory { public TrajectoryType type; diff --git a/src/main/java/frc/robot/lib/util/Util.java b/src/main/java/frc/robot/lib/util/Util.java index 7f802b4..b5592f0 100644 --- a/src/main/java/frc/robot/lib/util/Util.java +++ b/src/main/java/frc/robot/lib/util/Util.java @@ -251,4 +251,43 @@ public static double twist2dMagnitude(Twist2d t) { public static ChassisSpeeds fromTwist2d(Twist2d t) { return new ChassisSpeeds(t.dx, t.dy, t.dtheta); } + + public static double trapezoidProfileTimeToTarget( + double currentPosition, double currentSpeed, + double target, double maxSpeed, double maxAccel + ) { + double delta = target - currentPosition; + double distance = Math.abs(delta); + double direction = Math.signum(delta); + double v0 = currentSpeed * direction; + // Decelerating line + double stopDist = (v0 * v0) / (2 * maxAccel); + if (distance < stopDist) { + // Triangle + return v0 / maxAccel; // time to stop + } + double accelDist = (maxSpeed * maxSpeed - v0 * v0) / (2 * maxAccel); + if (accelDist < 0) accelDist = 0; + double decelDist = (maxSpeed * maxSpeed) / (2 * maxAccel); + double minDistance = accelDist + decelDist; + + // Triangle + if (distance < minDistance) { + // Solve for peak velocity vp + double vp = Math.sqrt((2 * maxAccel * distance + v0 * v0) / 2); + + double accelTime = (vp - v0) / maxAccel; + double decelTime = vp / maxAccel; + + return accelTime + decelTime; + } + + // Trapezoid + double accelTime = (maxSpeed - v0) / maxAccel; + double cruiseDist = distance - minDistance; + double cruiseTime = cruiseDist / maxSpeed; + double decelTime = maxSpeed / maxAccel; + + return accelTime + cruiseTime + decelTime; + } } diff --git a/src/main/java/frc/robot/subsystems/DefaultSubsystem.java b/src/main/java/frc/robot/subsystems/DefaultSubsystem.java deleted file mode 100644 index 36e79ba..0000000 --- a/src/main/java/frc/robot/subsystems/DefaultSubsystem.java +++ /dev/null @@ -1,7 +0,0 @@ -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class DefaultSubsystem extends SubsystemBase { - // Unfinished -} diff --git a/src/main/java/frc/robot/subsystems/LoggingManager.java b/src/main/java/frc/robot/subsystems/LoggingManager.java deleted file mode 100644 index 22e3edf..0000000 --- a/src/main/java/frc/robot/subsystems/LoggingManager.java +++ /dev/null @@ -1,7 +0,0 @@ -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj.DataLogManager; - -public class LoggingManager { - DataLogManager x; -} diff --git a/src/main/java/frc/robot/subsystems/TelemetryManager.java b/src/main/java/frc/robot/subsystems/TelemetryManager.java index 4034238..6a1047d 100644 --- a/src/main/java/frc/robot/subsystems/TelemetryManager.java +++ b/src/main/java/frc/robot/subsystems/TelemetryManager.java @@ -3,10 +3,14 @@ import java.util.ArrayList; import java.util.function.Supplier; +import com.ctre.phoenix6.hardware.TalonFX; + import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructArrayPublisher; import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.units.Units; import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.struct.Struct; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -30,7 +34,6 @@ public void publish() { } } - private final ArrayList> structPublishers; public static record StructPublisherEntry ( @@ -75,6 +78,49 @@ public void addStructArrayPublisher(String name, Struct struct, Supplier< supplier)); } + public static void makeSendableTalonFX(String name, TalonFX motor, SendableBuilder builder) { + builder.addDoubleProperty( + name + "/Rotations", + () -> motor + .getPosition() + .getValueAsDouble(), + null); + builder.addDoubleProperty( + name + "/Velocity RPS", + () -> motor + .getVelocity() + .getValueAsDouble(), + null); + builder.addDoubleProperty( + name + "/Volts", + () -> motor + .getMotorVoltage() + .getValue() + .in(Units.Volts), + null); + builder.addDoubleProperty( + name + "/Stator Current", + () -> motor + .getStatorCurrent() + .getValue() + .in(Units.Amps), + null); + builder.addDoubleProperty( + name + "/Supply Current", + () -> motor + .getSupplyCurrent() + .getValue() + .in(Units.Amps), + null); + builder.addDoubleProperty( + name + "/Temperature Celsius", + () -> motor + .getDeviceTemp() + .getValue() + .in(Units.Celsius), + null); + } + public void addSendable(Sendable sendable) { sendables.add(sendable); SmartDashboard.putData(sendable); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 17c9703..71e1dcd 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -1,10 +1,9 @@ package frc.robot.subsystems.drive; -import java.util.Set; +import static frc.robot.subsystems.drive.DriveConstants.*; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; @@ -16,14 +15,13 @@ import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Time; import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Robot; -import frc.robot.lib.localization.FieldLayout; +import frc.robot.lib.field.FieldLayout; import frc.robot.lib.util.Util; import frc.robot.subsystems.TelemetryManager; import frc.robot.subsystems.drive.ctre.CtreDriveConstants; @@ -41,16 +39,13 @@ public static Drive getInstance() { } private SwerveDriveState lastReadState; - public static final SwerveRequest.FieldCentric teleopRequest = - new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + public static final SwerveRequest.FieldCentric teleopRequest = new SwerveRequest.FieldCentric(); public SwerveRequest driveRequest = teleopRequest; private final CtreDrive drivetrain = CtreDriveConstants.createDrivetrain(); - private final CtreDriveTelemetry telemetry = new CtreDriveTelemetry(Constants.Drive.MAX_SPEED); - // Citrus what are you doing - private Time lastPoseResetTime = BaseUnits.TimeUnit.of(0.0); - - private final Field2d elasticPose = new Field2d(); + private final CtreDriveTelemetry telemetry = new CtreDriveTelemetry(MAX_SPEED); + @SuppressWarnings("unused") + private Time lastPoseResetTime = BaseUnits.TimeUnit.of(0.0); // Citrus what are you doing private Drive() { lastReadState = drivetrain.getState(); @@ -58,15 +53,12 @@ private Drive() { return driveRequest; })); - if (!Robot.isReal()) { - // drivetrain.resetPose((new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90.0)))); - } - drivetrain.getOdometryThread().setThreadPriority(31); TelemetryManager.getInstance().addStructPublisher("Mechanisms/Drive", Pose3d.struct, () -> new Pose3d(getPose())); TelemetryManager.getInstance().addSendable(this); } + /** @return the ctre generated drivetrain */ public CtreDrive getCtreDrive() { return drivetrain; } @@ -79,8 +71,7 @@ public void periodic() { public void outputTelemetry() { telemetry.telemeterize(lastReadState); - elasticPose.setRobotPose(getPose()); - SmartDashboard.putData("Elastic Field 2D", elasticPose); + FieldLayout.field.setRobotPose(getPose()); } /** @@ -97,6 +88,9 @@ public Pose2d getPose() { return lastReadState.Pose; } + /** + * @return the chassis speeds, field relative + */ public ChassisSpeeds getFieldSpeeds() { return ChassisSpeeds.fromRobotRelativeSpeeds(lastReadState.Speeds, lastReadState.Pose.getRotation()); } @@ -109,10 +103,14 @@ public void setSwerveRequest(SwerveRequest request) { driveRequest = request; } + /** + * @return the current swerve request + */ public SwerveRequest getSwerveRequest() { return driveRequest; } + /** Open loop during teleop mode */ public Command teleopCommand() { return runOnce(() -> { teleopRequest.withVelocityX(0).withVelocityY(0).withRotationalRate(0); @@ -130,63 +128,74 @@ public Command teleopCommand() { SmartDashboard.putNumber("Sticks/vW", rotDesiredRaw); teleopRequest - .withVelocityX(xFancy * Constants.Drive.MAX_SPEED) - .withVelocityY(yFancy * Constants.Drive.MAX_SPEED) - .withRotationalRate(rotFancy * Constants.Drive.MAX_ROTATION_SPEED); + .withVelocityX(xFancy * MAX_SPEED) + .withVelocityY(yFancy * MAX_SPEED) + .withRotationalRate(rotFancy * MAX_ROTATION_SPEED); }).handleInterrupt(() -> setSwerveRequest(new SwerveRequest.FieldCentric()))).withName("Teleop"); } + /** + * Auto aligns to the nearest reef face + * @param left chooses the left or right face + */ public Command autoAlign(boolean left) { - return Commands.defer(() -> { + return defer(() -> { Pose2d pose; if (left) { pose = getPose().nearest(FieldLayout.ALIGN_POSES_LEFT); } else { pose = getPose().nearest(FieldLayout.ALIGN_POSES_RIGHT); } - return new PIDToPoseCommand(pose).withName("Auto Align"); - }, Set.of(this)); + return new PIDToPoseCommand(pose).withName("Auto Align to " + pose); + }); } + /** Adds a vision update */ public void addVisionUpdate(Pose2d pose, Time timestamp) { getCtreDrive().addVisionMeasurement(pose, timestamp.in(Units.Seconds)); } + /** Adds a vision update with standard deviations */ public void addVisionUpdate(Pose2d pose, Time timestamp, Matrix stdDevs) { getCtreDrive().addVisionMeasurement(pose, timestamp.in(Units.Seconds), stdDevs); } + /** Resets pose estimator to a pose */ public void resetPose(Pose2d pose) { getCtreDrive().resetPose(pose); lastPoseResetTime = - Units.Seconds.of(Utils.getCurrentTimeSeconds()).plus(Constants.Drive.POSE_RESET_PREVENTION_TIME); + Units.Seconds.of(Utils.getCurrentTimeSeconds()).plus(POSE_RESET_PREVENTION_TIME); } + /** A command that resets the pose */ public Command resetPoseCommand(Pose2d pose) { return Commands.runOnce(() -> resetPose(pose)); } + /** Whether the pitch is stable */ public boolean isPitchStable() { return drivetrain.getPigeon2().getAngularVelocityYDevice().getValue().abs(Units.DegreesPerSecond) - < Constants.Drive.MAX_VELOCITY_STABLE + < MAX_VELOCITY_STABLE && drivetrain.getPigeon2().getPitch().getValue().abs(BaseUnits.AngleUnit) - < Constants.Drive.MAX_PITCH_STABLE; + < MAX_PITCH_STABLE; } + /** Whether the roll is stable */ public boolean isRollStable() { return drivetrain.getPigeon2().getAngularVelocityXDevice().getValue().abs(Units.DegreesPerSecond) - < Constants.Drive.MAX_VELOCITY_STABLE + < MAX_VELOCITY_STABLE && drivetrain.getPigeon2().getRoll().getValue().abs(BaseUnits.AngleUnit) - < Constants.Drive.MAX_PITCH_STABLE; + < MAX_PITCH_STABLE; } + /** Whether the robot is stable */ public boolean isStable() { ChassisSpeeds speeds = getState().Speeds; return isPitchStable() && isRollStable() && Units.MetersPerSecond.of(Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond)) - .lte(Constants.Drive.kScoringTranslationMaxSpeed) - && Units.RadiansPerSecond.of(speeds.omegaRadiansPerSecond).lte(Constants.Drive.kScoringRotationMaxSpeed); + .lte(MAX_SPEED_SCORING_TRANSLATION) + && Units.RadiansPerSecond.of(speeds.omegaRadiansPerSecond).lte(MAX_ROTATION_SPEED_SCORING); } @Override @@ -224,85 +233,12 @@ public void initSendable(SendableBuilder builder) { addModuleToBuilder(builder, 3); } + /** Telemeterizes a module */ private void addModuleToBuilder(SendableBuilder builder, int module) { - builder.addDoubleProperty( - "ModuleStates/" + module + "/Drive/Volts", - () -> drivetrain - .getModules()[module] - .getDriveMotor() - .getMotorVoltage() - .getValue() - .in(Units.Volts), - null); - - builder.addDoubleProperty( - "ModuleStates/" + module + "/Rotation/Volts", - () -> drivetrain - .getModules()[module] - .getSteerMotor() - .getMotorVoltage() - .getValue() - .in(Units.Volts), - null); - - builder.addDoubleProperty( - "ModuleStates/" + module + "/Drive/Stator Current", - () -> drivetrain - .getModules()[module] - .getDriveMotor() - .getStatorCurrent() - .getValue() - .in(Units.Amps), - null); - - builder.addDoubleProperty( - "ModuleStates/" + module + "/Drive/Temperature Celsius", - () -> drivetrain - .getModules()[module] - .getDriveMotor() - .getDeviceTemp() - .getValue() - .in(Units.Celsius), - null); - - builder.addDoubleProperty( - "ModuleStates/" + module + "/Rotation/Stator Current", - () -> drivetrain - .getModules()[module] - .getSteerMotor() - .getStatorCurrent() - .getValue() - .in(Units.Amps), - null); - - builder.addDoubleProperty( - "ModuleStates/" + module + "/Drive/Supply Current", - () -> drivetrain - .getModules()[module] - .getDriveMotor() - .getSupplyCurrent() - .getValue() - .in(Units.Amps), - null); - - builder.addDoubleProperty( - "ModuleStates/" + module + "/Rotation/Supply Current", - () -> drivetrain - .getModules()[module] - .getSteerMotor() - .getSupplyCurrent() - .getValue() - .in(Units.Amps), - null); - - builder.addDoubleProperty( - "ModuleStates/" + module + "/Rotation/Temperature Celsius", - () -> drivetrain - .getModules()[module] - .getSteerMotor() - .getDeviceTemp() - .getValue() - .in(Units.Celsius), - null); + TelemetryManager.makeSendableTalonFX("Modules/" + module + "/Drive", + drivetrain.getModules()[module].getDriveMotor(), builder); + + TelemetryManager.makeSendableTalonFX("Modules/" + module + "/Angle", + drivetrain.getModules()[module].getSteerMotor(), builder); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java new file mode 100644 index 0000000..1f32ab6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -0,0 +1,55 @@ +package frc.robot.subsystems.drive; + +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.Time; +import frc.robot.lib.control.ControlConstants.PIDFConstants; +import frc.robot.lib.control.ControlConstants.ProfiledPIDFConstants; +import frc.robot.subsystems.drive.ctre.CtreDriveConstants; + +public final class DriveConstants { + public static final double EPSILON_TRANSLATION = 0.02; // 2 cm + public static final double EPSILON_ROTATION = Units.Degrees.of(2.0).in(Units.Radians); // 2.0 deg in rads + + // Maximums + public static final double MAX_SPEED = Units.MetersPerSecond.of(2.5).in(Units.MetersPerSecond); + public static final double MAX_ACCEL = Units.MetersPerSecondPerSecond.of(5.0).in(Units.MetersPerSecondPerSecond); + public static final double MAX_ROTATION_SPEED = + Units.RotationsPerSecond.of(3.0).in(Units.RadiansPerSecond); + public static final double MAX_ROTATION_ACCEL = + Units.RotationsPerSecondPerSecond.of(5.0).in(Units.RadiansPerSecondPerSecond); + + // Swerve dimensions + public static final double TRACK_WIDTH = Units.Inches.of(24).in(Units.Meters); + public static final double WHEEL_BASE = Units.Inches.of(24).in(Units.Meters); + public static final double WHEEL_DIAMETER = 2 * CtreDriveConstants.kWheelRadius.in(Units.Meters); + public static final double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER * Math.PI; + + // Stability constants + public static final double MAX_VELOCITY_STABLE = 10; // degrees per second + public static final double MAX_PITCH_STABLE = 5; // degrees + public static final LinearVelocity MAX_SPEED_SCORING_TRANSLATION = + Units.Centimeters.of(15.0).per(Units.Seconds); + public static final AngularVelocity MAX_ROTATION_SPEED_SCORING = + Units.Degrees.of(7.0).per(Units.Seconds); // oh god + public static final Time POSE_RESET_PREVENTION_TIME = Units.Seconds.of(0.15); + + // Trajectory and snap constants + public static final PIDFConstants TRANSLATION_CONSTANTS = + new PIDFConstants(4.5, 0.0, 0.1, 1.0); + public static final ProfiledPIDFConstants PROFILED_TRANSLATION_CONSTANTS = + new ProfiledPIDFConstants(1.0, 0.0, 0.1, 1.0, + new TrapezoidProfile.Constraints( + MAX_SPEED, + MAX_ACCEL)); + public static final PIDFConstants ROTATION_CONSTANTS = + new PIDFConstants(4.0, 0.0, 0.1, 1.0); + public static final ProfiledPIDFConstants PROFILED_ROTATION_CONSTANTS = + new ProfiledPIDFConstants(4.0, 0.0, 0.0, 1.0, + new TrapezoidProfile.Constraints( + MAX_ROTATION_SPEED, + MAX_ROTATION_ACCEL)); + public static final double ACCELERATION_CONSTANT = 0.1; +} diff --git a/src/main/java/frc/robot/subsystems/drive/commands/ExtendedTrajectoryCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/ExtendedTrajectoryCommand.java index 5688318..57d57e1 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/ExtendedTrajectoryCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/ExtendedTrajectoryCommand.java @@ -16,7 +16,7 @@ public class ExtendedTrajectoryCommand extends TrajectoryCommand { public ExtendedTrajectoryCommand(Drive drive, RedTrajectory trajectory, Pair... triggers) { super(trajectory); addRequirements(drive); - setName("Extended Trajectory Command"); + setName(trajectory.name + ": Extended Trajectory Command"); this.triggers = new ArrayList<>(List.of(triggers)); for (Pair trigger : triggers) { new Trigger(() -> trajectory.progress > trigger.getFirst()).onTrue(trigger.getSecond()); diff --git a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java index 4dca0f1..9b8789c 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java @@ -1,48 +1,56 @@ package frc.robot.subsystems.drive.commands; +import static frc.robot.subsystems.drive.DriveConstants.*; + import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.lib.control.ControlConstants.ProfiledPIDFConstants; +import frc.robot.lib.control.ControlConstants.PIDFConstants; +import frc.robot.lib.control.PIDVController; import frc.robot.lib.util.Util; -import frc.robot.lib.control.ProfiledPIDVController; import frc.robot.subsystems.drive.Drive; +/** + * A command that moves the drivetrain to a pose. + */ public class PIDToPoseCommand extends Command { public final Drive drive; private final SwerveRequest.ApplyFieldSpeeds request = new SwerveRequest.ApplyFieldSpeeds(); - private final ProfiledPIDVController translationController; - private final ProfiledPIDVController thetaController; + private final PIDVController translationController; + private final PIDVController thetaController; private final Pose2d target; private Pose2d currentPose; private ChassisSpeeds currentSpeeds; + private ChassisSpeeds targetSpeeds = new ChassisSpeeds(); + public PIDToPoseCommand(Pose2d target) { this( Drive.getInstance(), target, - Constants.Auto.PROFILED_TRANSLATION_CONSTANTS, - Constants.Auto.ROTATION_CONSTANTS); + TRANSLATION_CONSTANTS, + ROTATION_CONSTANTS); } - public PIDToPoseCommand(Drive drive, Pose2d target, ProfiledPIDFConstants translationConstants, ProfiledPIDFConstants rotationConstants) { + public PIDToPoseCommand(Drive drive, Pose2d target, PIDFConstants translationConstants, PIDFConstants rotationConstants) { this.drive = drive; this.target = target; - translationController = new ProfiledPIDVController(translationConstants); - thetaController = new ProfiledPIDVController(rotationConstants); + translationController = new PIDVController(translationConstants); + thetaController = new PIDVController(rotationConstants); thetaController.enableContinuousInput(-Math.PI, Math.PI); addRequirements(drive); - setName("PID to " + target.toString()); + setName("(" + target.getX() + ", " + target.getY() + ", " + + target.getRotation().getDegrees() + " deg)" + " :PID to pose"); } @Override @@ -55,7 +63,8 @@ public void execute() { setRobotState( drive.getPose(), drive.getFieldSpeeds()); // updates the request - request.withSpeeds(calculateSpeeds()); + targetSpeeds = calculateSpeeds(); + request.withSpeeds(targetSpeeds); } public void setRobotState(Pose2d pose, ChassisSpeeds speeds) { @@ -65,46 +74,81 @@ public void setRobotState(Pose2d pose, ChassisSpeeds speeds) { public ChassisSpeeds calculateSpeeds() { if (target == null || currentPose == null || currentSpeeds == null) { + // Safety return new ChassisSpeeds(); } + // Calculate difference var delta = target.getTranslation().minus(currentPose.getTranslation()); - translationController.setTarget(delta.getNorm()); - - translationController.setInput( - 0.0, - Util.chassisSpeedsMagnitude( - currentSpeeds)); + // Magnitude target + double vMagnitude = MathUtil.clamp( + translationController.setTarget(delta.getNorm()) + .setMeasurement( + 0.0, // We are exactly where we are + Util.chassisSpeedsMagnitude( + currentSpeeds)) // How fast we are going + .getOutput(), + -MAX_SPEED, MAX_SPEED); - double vMagnitude = translationController.getOutput(); - SmartDashboard.putNumber("Debug/PIDToPoseCommand/vmag", vMagnitude); + // The angle we are at relative to the target var deltaRotation = delta.getAngle(); - thetaController.setTarget(target.getRotation().getRadians()); - thetaController.setInput( - currentPose.getRotation().getRadians(), currentSpeeds.omegaRadiansPerSecond); + double rotation = MathUtil.clamp( + thetaController.setTarget(target.getRotation().getRadians()) // Theta target + .setMeasurement( + currentPose.getRotation().getRadians(), + currentSpeeds.omegaRadiansPerSecond) // We are where we are and we are as fast as how fast we are going + .getOutput(), + -MAX_ROTATION_SPEED, MAX_ROTATION_SPEED); - double rotation = thetaController.getOutput(); + SmartDashboard.putNumber("Debug/PIDToPose/vx", vMagnitude * deltaRotation.getCos()); + SmartDashboard.putNumber("Debug/PIDToPose/vy", vMagnitude * deltaRotation.getSin()); + SmartDashboard.putNumber("Debug/PIDToPose/vrotation", rotation); return new ChassisSpeeds( - vMagnitude * deltaRotation.getCos(), + vMagnitude * deltaRotation.getCos(), // convert from polar to rectangular vMagnitude * deltaRotation.getSin(), rotation); } @Override public boolean isFinished() { - return translationController.error < 0.03 - && MathUtil.isNear(thetaController.error / Math.PI * 180, 0, 3.0); + return translationController.getError() < EPSILON_TRANSLATION + && MathUtil.isNear(thetaController.getError(), 0, EPSILON_ROTATION); // Within tolerance } @Override public void end(boolean interrupted) { + // Swaps out the drive request to a default robot oriented request drive.setSwerveRequest(new SwerveRequest.ApplyRobotSpeeds()); } + public static double estimateTimeToPose( + Pose2d currentPose, + ChassisSpeeds currentSpeeds, + Pose2d targetPose + ) { + double translationError = + currentPose.getTranslation().getDistance(targetPose.getTranslation()); + + double currentV = Util.chassisSpeedsMagnitude(currentSpeeds); + + return Util.trapezoidProfileTimeToTarget( + translationError, + currentV, + 0, + MAX_SPEED, + MAX_ACCEL); + } + + /** Helper for retrieving the target of this PID to Pose command */ public Pose2d getTarget() { return target; } + + @Override + public void initSendable(SendableBuilder builder) { + super.initSendable(builder); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java index e4acfa2..af5fe0d 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.drive.commands; +import static frc.robot.subsystems.drive.DriveConstants.*; + import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.MathUtil; @@ -8,7 +10,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; import frc.robot.lib.control.ControlConstants.PIDFConstants; import frc.robot.lib.control.ControlConstants.ProfiledPIDFConstants; import frc.robot.lib.control.PIDVController; @@ -42,9 +43,9 @@ public TrajectoryCommand(RedTrajectory trajectory) { this( Drive.getInstance(), trajectory, - Constants.Auto.TRANSLATION_CONSTANTS, - Constants.Auto.ROTATION_CONSTANTS, - Constants.Auto.ACCELERATION_CONSTANT); + TRANSLATION_CONSTANTS, + PROFILED_ROTATION_CONSTANTS, + ACCELERATION_CONSTANT); } /** @@ -68,12 +69,12 @@ public TrajectoryCommand(Drive drive, RedTrajectory trajectory, PIDFConstants tr .addStructPublisher("Debug/TrajectoryCommand", Pose3d.struct, () -> new Pose3d( targetState.pose)); - setName("Trajectory " + trajectory.name); + setName(trajectory.name + " :Trajectory"); } @Override public void initialize() { - timer.start(); + timer.start(); // actually starts the timer drive.setSwerveRequest(request); } @@ -92,69 +93,77 @@ public void setRobotState(Pose2d pose, ChassisSpeeds speeds) { public ChassisSpeeds calculateSpeeds() { if (trajectory == null || currentPose == null || currentSpeeds == null || trajectory.isDone()) { - return new ChassisSpeeds(); + return new ChassisSpeeds(); // Safety } + // Advances along the trajectory targetState = trajectory.advanceTo(timer.get()); + // gets the feedforwards for translation double vxFF = targetState.speeds.vxMetersPerSecond; double vyFF = targetState.speeds.vyMetersPerSecond; double xAccelFF = MathUtil.applyDeadband( targetState.accels.ax, - Constants.Drive.MAX_ACCEL * 0.5); + MAX_ACCEL * 0.5); double yAccelFF = MathUtil.applyDeadband( targetState.accels.ay, - Constants.Drive.MAX_ACCEL * 0.5); - + MAX_ACCEL * 0.5); double angularAccel = MathUtil.applyDeadband( targetState.accels.alpha, - Constants.Drive.MAX_ROTATION_ACCEL * 0.5); + MAX_ROTATION_ACCEL * 0.5); // what did i even want to accomplish from this + + // acceleration feedforwards xAccelFF += -angularAccel * targetState.pose.getRotation().getSin(); yAccelFF += angularAccel * targetState.pose.getRotation().getCos(); - xController.setTarget(targetState.pose.getX()); - yController.setTarget(targetState.pose.getY()); - - xController.setFeedforward(vxFF); - yController.setFeedforward(vyFF); + double vx = xController.setTarget(targetState.pose.getX())// Target setting + .setFeedforward(vxFF) // Feedforward setting + .setMeasurement(currentPose.getX(), currentSpeeds.vxMetersPerSecond) // Measurement setting + .getOutput(); // Output getting - xController.setInput(currentPose.getX(), currentSpeeds.vxMetersPerSecond); - yController.setInput(currentPose.getY(), currentSpeeds.vyMetersPerSecond); - double vx = xController.getOutput(); - double vy = yController.getOutput(); + // same thing but for vy and rotation instead + double vy = yController.setTarget(targetState.pose.getY()) + .setFeedforward(vyFF) + .setMeasurement(currentPose.getY(), currentSpeeds.vyMetersPerSecond) + .getOutput(); - thetaController.setTarget(targetState.pose.getRotation().getRadians()); - thetaController.setFeedforward(targetState.speeds.omegaRadiansPerSecond); - thetaController.setInput( - currentPose.getRotation().getRadians(), currentSpeeds.omegaRadiansPerSecond); - - double rotation = thetaController.getOutput(); + double rotation = thetaController.setTarget(targetState.pose.getRotation().getRadians()) + .setFeedforward(targetState.speeds.omegaRadiansPerSecond) + .setMeasurement( + currentPose.getRotation().getRadians(), + currentSpeeds.omegaRadiansPerSecond) + .getOutput(); return new ChassisSpeeds( vx + xAccelFF * accelConstant, vy + yAccelFF * accelConstant, - rotation); + rotation); // Finally } @Override public boolean isFinished() { if (trajectory == null) { - return true; + return true; // safety } + if (trajectory.isDone()) { - System.out.println("Done with trajectory, error: " + Math.hypot(xController.error, yController.error)); - return true; + System.out.println("Done with trajectory, error: " + + Math.hypot(xController.getError(), yController.getError())); + return true; // We are done guys } + return false; } @Override public void end(boolean interrupted) { + // swap out request at end drive.setSwerveRequest(new SwerveRequest.FieldCentric()); } + /** Helper for getting trajectory from this object */ public RedTrajectory getTrajectory() { return trajectory; } diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java index 986f8ef..57a3e5b 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java @@ -23,7 +23,7 @@ // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class CtreDriveConstants { - // Both sets of gains need to be tuned to your individual robot. + // TODO: Both sets of gains need to be tuned to your individual robot. // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput @@ -84,11 +84,11 @@ public class CtreDriveConstants { public static final CANBus kCANBus = new CANBus("CV", "./logs/example.hoot"); // Theoretical free speed (m/s) at 12 V applied output; - // This needs to be tuned to your individual robot + // TODO: This needs to be tuned to your individual robot public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.21); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot + // TODO: This may need to be tuned to your individual robot private static final double kCoupleRatio = 3.5714285714285716; public static final double kDriveGearRatio = 6.122448979591837; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionDevice.java b/src/main/java/frc/robot/subsystems/vision/VisionDevice.java index 288408e..62c2617 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionDevice.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionDevice.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.vision; -import frc.robot.lib.localization.FieldLayout; import frc.robot.subsystems.drive.Drive; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; @@ -9,11 +8,11 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants.Limelight.VisionDeviceConstants; +import frc.robot.lib.field.FieldLayout; import java.util.ArrayList; import java.util.Arrays; import java.util.List; - import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonUtils; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java b/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java index 5164c17..f59a5bb 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java @@ -19,6 +19,7 @@ public static VisionDeviceManager getInstance() { } // private VisionDevice leftCamera; + @SuppressWarnings("unused") private VisionDevice rightCamera; private VisionDevice frontrCamera; private VisionDevice frontlCamera; From 8b62b6d8e0765939bc9c58b472bbf37e57951197 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Sat, 6 Dec 2025 21:13:09 -0800 Subject: [PATCH 015/118] Trajectory update soon --- simgui-ds.json | 8 ++++---- .../drive/commands/TrajectoryCommand.java | 15 +++++++-------- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 23c0a81..0bea79e 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -7,14 +7,14 @@ "keyboardJoysticks": [ { "axisConfig": [ - { - "decKey": 65, - "incKey": 68 - }, { "decKey": 83, "incKey": 87 }, + { + "decKey": 65, + "incKey": 68 + }, { "decayRate": 0.0, "keyRate": 0.009999999776482582 diff --git a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java index af5fe0d..82cda63 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java @@ -11,15 +11,14 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.lib.control.ControlConstants.PIDFConstants; -import frc.robot.lib.control.ControlConstants.ProfiledPIDFConstants; import frc.robot.lib.control.PIDVController; -import frc.robot.lib.control.ProfiledPIDVController; import frc.robot.lib.trajectory.RedTrajectory; import frc.robot.subsystems.TelemetryManager; import frc.robot.subsystems.drive.Drive; /** * Command that follows a trajectory + * TODO: fix the profiled pid controller */ public class TrajectoryCommand extends Command { public final Drive drive; @@ -29,7 +28,7 @@ public class TrajectoryCommand extends Command { private final PIDVController xController; private final PIDVController yController; - private final ProfiledPIDVController thetaController; + private final PIDVController thetaController; private double accelConstant; private final RedTrajectory trajectory; @@ -44,22 +43,22 @@ public TrajectoryCommand(RedTrajectory trajectory) { Drive.getInstance(), trajectory, TRANSLATION_CONSTANTS, - PROFILED_ROTATION_CONSTANTS, + ROTATION_CONSTANTS, ACCELERATION_CONSTANT); } /** - * A drive controller that works with 2 {@link PIDVController}s for translation and one {@link ProfiledPIDVController} for rotation. + * A drive controller that works with 2 {@link PIDVController}s for translation and one {@link PIDVController} for rotation. * @param translationConstants The {@link PIDFConstants} for the translation of the robot. - * @param rotationConstants The {@link ProfiledPIDFConstants} for the rotation of the robot. + * @param rotationConstants The {@link PIDFConstants} for the rotation of the robot. * @param accelConstant The acceleration feedforwards (useful for traversing sharp turns on a trajectory). */ - public TrajectoryCommand(Drive drive, RedTrajectory trajectory, PIDFConstants translationConstants, ProfiledPIDFConstants rotationConstants, double accelConstant) { + public TrajectoryCommand(Drive drive, RedTrajectory trajectory, PIDFConstants translationConstants, PIDFConstants rotationConstants, double accelConstant) { this.drive = drive; this.trajectory = trajectory; xController = new PIDVController(translationConstants); yController = new PIDVController(translationConstants); - thetaController = new ProfiledPIDVController(rotationConstants); + thetaController = new PIDVController(rotationConstants); thetaController.enableContinuousInput(-Math.PI, Math.PI); this.accelConstant = accelConstant; From 7364323c7933ecf7f9e709224e8fd08b7622d86e Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Sat, 6 Dec 2025 21:25:04 -0800 Subject: [PATCH 016/118] i think later though --- .../java/frc/robot/subsystems/drive/DriveConstants.java | 8 ++++---- .../subsystems/drive/commands/TrajectoryCommand.java | 1 - 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 1f32ab6..c5d5321 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -14,12 +14,12 @@ public final class DriveConstants { public static final double EPSILON_ROTATION = Units.Degrees.of(2.0).in(Units.Radians); // 2.0 deg in rads // Maximums - public static final double MAX_SPEED = Units.MetersPerSecond.of(2.5).in(Units.MetersPerSecond); - public static final double MAX_ACCEL = Units.MetersPerSecondPerSecond.of(5.0).in(Units.MetersPerSecondPerSecond); + public static final double MAX_SPEED = Units.MetersPerSecond.of(3.5).in(Units.MetersPerSecond); + public static final double MAX_ACCEL = Units.MetersPerSecondPerSecond.of(6.0).in(Units.MetersPerSecondPerSecond); public static final double MAX_ROTATION_SPEED = - Units.RotationsPerSecond.of(3.0).in(Units.RadiansPerSecond); + Units.RotationsPerSecond.of(2.0).in(Units.RadiansPerSecond); public static final double MAX_ROTATION_ACCEL = - Units.RotationsPerSecondPerSecond.of(5.0).in(Units.RadiansPerSecondPerSecond); + Units.RotationsPerSecondPerSecond.of(4.0).in(Units.RadiansPerSecondPerSecond); // Swerve dimensions public static final double TRACK_WIDTH = Units.Inches.of(24).in(Units.Meters); diff --git a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java index 82cda63..e1b00f8 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java @@ -121,7 +121,6 @@ public ChassisSpeeds calculateSpeeds() { .setMeasurement(currentPose.getX(), currentSpeeds.vxMetersPerSecond) // Measurement setting .getOutput(); // Output getting - // same thing but for vy and rotation instead double vy = yController.setTarget(targetState.pose.getY()) .setFeedforward(vyFF) From dd0973a8b5521f7f78890ed0c8c8a4f8069f8c79 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Sat, 6 Dec 2025 21:33:15 -0800 Subject: [PATCH 017/118] minor change --- src/main/java/frc/robot/subsystems/drive/Drive.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 71e1dcd..0526668 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -146,8 +146,8 @@ public Command autoAlign(boolean left) { } else { pose = getPose().nearest(FieldLayout.ALIGN_POSES_RIGHT); } - return new PIDToPoseCommand(pose).withName("Auto Align to " + pose); - }); + return new PIDToPoseCommand(pose); + }).withName("Auto Align"); } /** Adds a vision update */ From 7cb20890b00bd302f11d55e7645863051962a922 Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Sun, 14 Dec 2025 12:17:03 -0800 Subject: [PATCH 018/118] FEAT: SysID Rotation routine and logs --- src/main/java/frc/robot/ControlsMapping.java | 14 ++++----- .../subsystems/drive/ctre/CtreDrive.java | 30 ++++++++++++++----- 2 files changed, 30 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index 67da8bd..1139456 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -17,14 +17,14 @@ public class ControlsMapping { public static void mapTeleopCommand() { Drive.getInstance().setDefaultCommand((Drive.getInstance().teleopCommand())); - // // run sysID functions - // Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER); + // // // run sysID functions + // // Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER); - controller.a().onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); - controller.leftBumper().whileTrue(Drive.getInstance().autoAlign(true)); - controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false)); - controller.b().whileTrue(new PIDToPoseCommand( - new Pose2d(2.0, 1.0, Rotation2d.fromDegrees(120.0)))); + // controller.a().onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); + // controller.leftBumper().whileTrue(Drive.getInstance().autoAlign(true)); + // controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false)); + // controller.b().whileTrue(new PIDToPoseCommand( + // new Pose2d(2.0, 1.0, Rotation2d.fromDegrees(120.0)))); } public static void mapSysId() { diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java index 183b5ff..98b5004 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java @@ -111,17 +111,33 @@ public static enum SysIdRoutineType { Volts.of(Math.PI), null, // Use default timeout (10 s) // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdRotation_State", state.toString()) + //state -> SignalLogger.writeString("SysIdRotation_State", state.toString()) + null ), new SysIdRoutine.Mechanism( output -> { - /* output is actually radians per second, but SysId only supports "volts" */ - setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); - /* also log the requested output for SysId */ - SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); + System.out.println("sysid/rotation routine called."); + m_lastAppliedVolts = output.in(Volts); + setControl(m_rotationCharacterization.withRotationalRate(m_lastAppliedVolts)); }, - null, - this + log -> { + var s = getStateCopy(); + log.motor("yaw") + .voltage(Volts.of(m_lastAppliedVolts)) + .angularPosition(Radians.of(s.Pose.getRotation().getRadians())) // get rotation position + .angularVelocity(RadiansPerSecond.of(getKinematics() + .toChassisSpeeds(s.ModuleStates).omegaRadiansPerSecond)); + }, + this + + // output -> { + // /* output is actually radians per second, but SysId only supports "volts" */ + // setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); + // /* also log the requested output for SysId */ + // SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); + // }, + // null, + // this ) ); From 4ab705e297bce1511c7658e436f51007025e75d0 Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Mon, 15 Dec 2025 00:50:11 -0800 Subject: [PATCH 019/118] feat: enable VisualVM to debug RAM issues --- build.gradle | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/build.gradle b/build.gradle index 3a947b7..1d8df5e 100644 --- a/build.gradle +++ b/build.gradle @@ -27,6 +27,13 @@ deploy { // getTargetTypeClass is a shortcut to get the class type using a string frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + // Enable VisualVM connection + // jvmArgs.add("-Dcom.sun.management.jmxremote=true") + // jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") + // jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") + // jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") + // jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") + // jvmArgs.add("-Djava.rmi.server.hostname=10.14.58.2") // Replace TE.AM with team number } // Static files artifact From 35fff997f1da68d231994e5a3387a1d70f4ac9fb Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Mon, 15 Dec 2025 00:50:55 -0800 Subject: [PATCH 020/118] feat: add steer motor offsets of the 2nd bot --- .../subsystems/drive/ctre/CtreDriveConstants.java | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java index 2e9d6f6..249c59d 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java @@ -22,7 +22,10 @@ // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html -public class CtreDriveConstants { +public class CtreDriveConstants { + // which set of robot constants we are deploy + private static final boolean kIs2ndBot = false; //true for the 2nd bot we built for 2026; false for 2025 bot + // mechanical and geometric parameters of drive train public static final double kDriveGearRatio = 6.122448979591837; public static final double kSteerGearRatio = 21.428571428571427; @@ -154,7 +157,7 @@ public class CtreDriveConstants { private static final int kFrontLeftDriveMotorId = 8; private static final int kFrontLeftSteerMotorId = 10; private static final int kFrontLeftEncoderId = 7; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.355224609375); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(kIs2ndBot? -0.462890625: 0.355224609375); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; @@ -165,7 +168,7 @@ public class CtreDriveConstants { private static final int kFrontRightDriveMotorId = 9; private static final int kFrontRightSteerMotorId = 11; private static final int kFrontRightEncoderId = 6; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4296875); + private static final Angle kFrontRightEncoderOffset = Rotations.of(kIs2ndBot? 0.025390625: -0.4296875); private static final boolean kFrontRightSteerMotorInverted = true; private static final boolean kFrontRightEncoderInverted = false; @@ -176,7 +179,7 @@ public class CtreDriveConstants { private static final int kBackLeftDriveMotorId = 3; private static final int kBackLeftSteerMotorId = 5; private static final int kBackLeftEncoderId = 14; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.326416015625); + private static final Angle kBackLeftEncoderOffset = Rotations.of(kIs2ndBot? 0.1435546875: 0.326416015625); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; @@ -187,7 +190,7 @@ public class CtreDriveConstants { private static final int kBackRightDriveMotorId = 4; private static final int kBackRightSteerMotorId = 2; private static final int kBackRightEncoderId = 1; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.0869140625); + private static final Angle kBackRightEncoderOffset = Rotations.of(kIs2ndBot? 0.183837890625: 0.0869140625); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; From 76bd01fe8eefcab5046298d468454377c25ad105 Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Mon, 15 Dec 2025 00:53:15 -0800 Subject: [PATCH 021/118] feat: add radialDeadband to fix yaw impulse when driver releases stick --- src/main/java/frc/robot/lib/util/Util.java | 13 +++++++++++++ src/main/java/frc/robot/subsystems/drive/Drive.java | 8 ++++++-- 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/lib/util/Util.java b/src/main/java/frc/robot/lib/util/Util.java index 7f802b4..36d0ea3 100644 --- a/src/main/java/frc/robot/lib/util/Util.java +++ b/src/main/java/frc/robot/lib/util/Util.java @@ -130,6 +130,19 @@ public static double applyJoystickDeadband(double stickValue, double stickDeadba return Math.signum(deadbandedValue) * ((Math.abs(deadbandedValue) - stickDeadband) / (1.0 - stickDeadband)); //joystick max is always 1.0 } + // dc 12.15.25, apply radial deadband instead of axis-based ones + public static double[] applyRadialDeadband(double x, double y, double deadband) { + double mag = Math.hypot(x, y); + if (mag <= deadband) return new double[] {0.0, 0.0}; + + // Rescale so output reaches 1.0 when mag==1.0 + double scaledMag = (mag - deadband) / (1.0 - deadband); + double ux = x / mag; + double uy = y / mag; + return new double[] {ux * scaledMag, uy * scaledMag}; + } + + public static Rotation2d robotToFieldRelative(Rotation2d rot, boolean is_red_alliance) { if (is_red_alliance) { return rot.rotateBy(Rotation2d.fromDegrees(180.0)); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 17c9703..f49d3a7 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -121,8 +121,12 @@ public Command teleopCommand() { double xDesiredRaw = -Robot.controller.getLeftY(); double yDesiredRaw = -Robot.controller.getLeftX(); double rotDesiredRaw = -Robot.controller.getRightX(); - double xFancy = Util.applyJoystickDeadband(xDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); - double yFancy = Util.applyJoystickDeadband(yDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); + + double[] xy = Util.applyRadialDeadband(xDesiredRaw, yDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); + double xFancy = xy[0]; + double yFancy = xy[1]; + // double xFancy = Util.applyJoystickDeadband(xDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); + // double yFancy = Util.applyJoystickDeadband(yDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); double rotFancy = Util.applyJoystickDeadband(rotDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); SmartDashboard.putNumber("Sticks/vX", xDesiredRaw); From 8e71e1052292cf672dd706f5a518b058364658fb Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Mon, 15 Dec 2025 01:05:49 -0800 Subject: [PATCH 022/118] fix kCoupleRatio value for steer/drive coupling --- .../frc/robot/subsystems/drive/ctre/CtreDriveConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java index 249c59d..551bfa3 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java @@ -110,7 +110,7 @@ public class CtreDriveConstants { // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - private static final double kCoupleRatio = kDriveGearRatio /3.; //(drive gear ratio)/(bevel gear ratio) 3.5714285714285716; + private static final double kCoupleRatio = kDriveGearRatio /(150./7.); //(drive gear ratio)/(steering gear ratio) 3.5714285714285716; private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; From 657d15826c11a717be086c1c0b31fd6e7cfcd512 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Thu, 18 Dec 2025 21:50:53 -0800 Subject: [PATCH 023/118] Pid to pose borke again --- src/main/java/frc/robot/ControlsMapping.java | 4 +- .../lib/field/AprilTagLayoutGenerated.java | 45 +++ .../lib/field/AprilTagLayoutGenerator.html | 215 ++++++++++++++ .../java/frc/robot/lib/field/FieldLayout.java | 35 ++- .../frc/robot/subsystems/drive/Drive.java | 278 ++++++++++-------- .../drive/commands/AutopilotCommand.java | 80 +++++ .../drive/commands/PIDToPoseCommand.java | 16 +- 7 files changed, 540 insertions(+), 133 deletions(-) create mode 100644 src/main/java/frc/robot/lib/field/AprilTagLayoutGenerated.java create mode 100644 src/main/java/frc/robot/lib/field/AprilTagLayoutGenerator.html create mode 100644 src/main/java/frc/robot/subsystems/drive/commands/AutopilotCommand.java diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index b59c59f..caa4807 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -10,8 +10,8 @@ public class ControlsMapping { public static void mapTeleopCommand() { controller.back().onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); - controller.leftBumper().whileTrue(Drive.getInstance().autoAlign(true)); - controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false)); + controller.leftBumper().whileTrue(Drive.getInstance().autopilotAlign(true)); + controller.rightBumper().whileTrue(Drive.getInstance().autopilotAlign(false)); } public static void mapSysId() { diff --git a/src/main/java/frc/robot/lib/field/AprilTagLayoutGenerated.java b/src/main/java/frc/robot/lib/field/AprilTagLayoutGenerated.java new file mode 100644 index 0000000..f4f0834 --- /dev/null +++ b/src/main/java/frc/robot/lib/field/AprilTagLayoutGenerated.java @@ -0,0 +1,45 @@ +package frc.robot.lib.field; + +import java.util.ArrayList; +import java.util.List; + +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.apriltag.AprilTagFieldLayout; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; + +public class AprilTagLayoutGenerated { + public static final List APRILTAG_FIELD_POSES = new ArrayList<>(); + static { + APRILTAG_FIELD_POSES.add(new AprilTag(1, new Pose3d(new Translation3d(16.697198000000D, 0.655320000000D, 1.485900000000D), new Rotation3d(new Quaternion(0.453990499740D, 0.000000000000D, 0.000000000000D, 0.891006524188D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(2, new Pose3d(new Translation3d(16.697198000000D, 7.396480000000D, 1.485900000000D), new Rotation3d(new Quaternion(-0.453990499740D, 0.000000000000D, 0.000000000000D, 0.891006524188D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(3, new Pose3d(new Translation3d(11.560810000000D, 8.055610000000D, 1.301750000000D), new Rotation3d(new Quaternion(-0.707106781187D, 0.000000000000D, 0.000000000000D, 0.707106781187D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(4, new Pose3d(new Translation3d(9.276080000000D, 6.137656000000D, 1.867916000000D), new Rotation3d(new Quaternion(0.965925826289D, 0.000000000000D, 0.258819045103D, 0.000000000000D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(5, new Pose3d(new Translation3d(9.276080000000D, 1.914906000000D, 1.867916000000D), new Rotation3d(new Quaternion(0.965925826289D, 0.000000000000D, 0.258819045103D, 0.000000000000D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(6, new Pose3d(new Translation3d(13.474446000000D, 3.306318000000D, 0.308102000000D), new Rotation3d(new Quaternion(-0.866025403784D, 0.000000000000D, 0.000000000000D, 0.500000000000D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(7, new Pose3d(new Translation3d(13.890498000000D, 4.025900000000D, 0.308102000000D), new Rotation3d(new Quaternion(1.000000000000D, 0.000000000000D, 0.000000000000D, 0.000000000000D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(8, new Pose3d(new Translation3d(13.474446000000D, 4.745482000000D, 0.308102000000D), new Rotation3d(new Quaternion(0.866025403784D, 0.000000000000D, 0.000000000000D, 0.500000000000D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(9, new Pose3d(new Translation3d(12.643358000000D, 4.745482000000D, 0.308102000000D), new Rotation3d(new Quaternion(0.500000000000D, 0.000000000000D, 0.000000000000D, 0.866025403784D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(10, new Pose3d(new Translation3d(12.227306000000D, 4.025900000000D, 0.308102000000D), new Rotation3d(new Quaternion(0.000000000000D, 0.000000000000D, 0.000000000000D, 1.000000000000D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(11, new Pose3d(new Translation3d(12.643358000000D, 3.306318000000D, 0.308102000000D), new Rotation3d(new Quaternion(-0.500000000000D, 0.000000000000D, 0.000000000000D, 0.866025403784D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(12, new Pose3d(new Translation3d(0.851154000000D, 0.655320000000D, 1.485900000000D), new Rotation3d(new Quaternion(0.891006524188D, 0.000000000000D, 0.000000000000D, 0.453990499740D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(13, new Pose3d(new Translation3d(0.851154000000D, 7.396480000000D, 1.485900000000D), new Rotation3d(new Quaternion(-0.891006524188D, 0.000000000000D, 0.000000000000D, 0.453990499740D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(14, new Pose3d(new Translation3d(8.272272000000D, 6.137656000000D, 1.867916000000D), new Rotation3d(new Quaternion(0.000000000000D, -0.258819045103D, 0.000000000000D, 0.965925826289D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(15, new Pose3d(new Translation3d(8.272272000000D, 1.914906000000D, 1.867916000000D), new Rotation3d(new Quaternion(0.000000000000D, -0.258819045103D, 0.000000000000D, 0.965925826289D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(16, new Pose3d(new Translation3d(5.987542000000D, -0.003810000000D, 1.301750000000D), new Rotation3d(new Quaternion(0.707106781187D, 0.000000000000D, 0.000000000000D, 0.707106781187D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(17, new Pose3d(new Translation3d(4.073906000000D, 3.306318000000D, 0.308102000000D), new Rotation3d(new Quaternion(-0.500000000000D, 0.000000000000D, 0.000000000000D, 0.866025403784D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(18, new Pose3d(new Translation3d(3.657600000000D, 4.025900000000D, 0.308102000000D), new Rotation3d(new Quaternion(0.000000000000D, 0.000000000000D, 0.000000000000D, 1.000000000000D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(19, new Pose3d(new Translation3d(4.073906000000D, 4.745482000000D, 0.308102000000D), new Rotation3d(new Quaternion(0.500000000000D, 0.000000000000D, 0.000000000000D, 0.866025403784D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(20, new Pose3d(new Translation3d(4.904740000000D, 4.745482000000D, 0.308102000000D), new Rotation3d(new Quaternion(0.866025403784D, 0.000000000000D, 0.000000000000D, 0.500000000000D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(21, new Pose3d(new Translation3d(5.321046000000D, 4.025900000000D, 0.308102000000D), new Rotation3d(new Quaternion(1.000000000000D, 0.000000000000D, 0.000000000000D, 0.000000000000D))))); + APRILTAG_FIELD_POSES.add(new AprilTag(22, new Pose3d(new Translation3d(4.904740000000D, 3.306318000000D, 0.308102000000D), new Rotation3d(new Quaternion(-0.866025403784D, 0.000000000000D, 0.000000000000D, 0.500000000000D))))); + }; + public static final double FIELD_LENGTH_METERS = 17.548D; + public static final double FIELD_WIDTH_METERS = 8.052D; + public static AprilTagFieldLayout getLayout() { + return new AprilTagFieldLayout(APRILTAG_FIELD_POSES, FIELD_LENGTH_METERS, FIELD_WIDTH_METERS); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/lib/field/AprilTagLayoutGenerator.html b/src/main/java/frc/robot/lib/field/AprilTagLayoutGenerator.html new file mode 100644 index 0000000..8499ecf --- /dev/null +++ b/src/main/java/frc/robot/lib/field/AprilTagLayoutGenerator.html @@ -0,0 +1,215 @@ + + + + + + AprilTag Code Generator + + + + +
+

AprilTag Code Generator

+ + + + + + +
+ + +
+
// Generated code will appear here...
+
+ + + + + \ No newline at end of file diff --git a/src/main/java/frc/robot/lib/field/FieldLayout.java b/src/main/java/frc/robot/lib/field/FieldLayout.java index c16c84f..dd8752b 100644 --- a/src/main/java/frc/robot/lib/field/FieldLayout.java +++ b/src/main/java/frc/robot/lib/field/FieldLayout.java @@ -2,8 +2,13 @@ import java.io.IOException; import java.util.ArrayList; +import java.util.Collections; +import java.util.Comparator; +import java.util.HashMap; import java.util.List; +import com.therekrab.autopilot.APTarget; + import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; @@ -39,6 +44,9 @@ public class FieldLayout { public static final double APRITAG_WIDTH = Units.inchesToMeters(6.50); public static final AprilTagFieldLayout APRILTAG_MAP; + public static final HashMap ENTRY_ANGLES_RIGHT = new HashMap<>(); + public static final HashMap ENTRY_ANGLES_LEFT = new HashMap<>(); + public static final List ALIGN_POSES_RIGHT = new ArrayList<>(); public static final List ALIGN_POSES_LEFT = new ArrayList<>(); @@ -50,19 +58,38 @@ public class FieldLayout { int[] reefIds = {6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22}; Transform2d leftReefTransform = new Transform2d( DriveConstants.TRACK_WIDTH / 2 + Units.inchesToMeters(1.5), -Units.inchesToMeters(13.0 / 2), Rotation2d.kPi); - for (int i : reefIds) { - ALIGN_POSES_LEFT.add(APRILTAG_MAP.getTagPose(i).get().toPose2d().transformBy(leftReefTransform)); - } Transform2d rightReefTransform = new Transform2d( DriveConstants.TRACK_WIDTH / 2 + Units.inchesToMeters(1.5), Units.inchesToMeters(13.0 / 2), Rotation2d.kPi); for (int i : reefIds) { - ALIGN_POSES_RIGHT.add(APRILTAG_MAP.getTagPose(i).get().toPose2d().transformBy(rightReefTransform)); + var tagPose = APRILTAG_MAP.getTagPose(i).get().toPose2d(); + var right = tagPose.transformBy(rightReefTransform); + var left = tagPose.transformBy(leftReefTransform); + ALIGN_POSES_RIGHT.add(right); + ALIGN_POSES_LEFT.add(left); + ENTRY_ANGLES_RIGHT.put(right, tagPose.getRotation().plus(Rotation2d.k180deg)); + ENTRY_ANGLES_LEFT.put(left, tagPose.getRotation().plus(Rotation2d.k180deg)); } } catch (IOException e) { throw new RuntimeException(e); } } + public static Pose2d getNearestPose(Pose2d pose, boolean left) { + return pose.nearest(left ? ALIGN_POSES_LEFT : ALIGN_POSES_RIGHT); + } + + public static APTarget getNearestTarget(Pose2d pose, boolean left) { + var set = left ? ENTRY_ANGLES_LEFT : ENTRY_ANGLES_RIGHT; + var out = Collections.min( + set.keySet(), + Comparator.comparing( + (Pose2d other) -> pose.getTranslation().getDistance(other.getTranslation()) + ).thenComparing( + (Pose2d other) -> + Math.abs(pose.getRotation().minus(other.getRotation()).getRadians()))); + return new APTarget(out).withEntryAngle(set.get(out)); + } + public static Pose2d handleAllianceFlip(Pose2d blue_pose, boolean is_red_alliance) { if (is_red_alliance) { blue_pose = new Pose2d( diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 0526668..5bb245b 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -4,6 +4,8 @@ import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; +import com.therekrab.autopilot.APTarget; +import com.therekrab.autopilot.Autopilot; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; @@ -25,27 +27,30 @@ import frc.robot.lib.util.Util; import frc.robot.subsystems.TelemetryManager; import frc.robot.subsystems.drive.ctre.CtreDriveConstants; +import frc.robot.subsystems.drive.commands.AutopilotCommand; import frc.robot.subsystems.drive.commands.PIDToPoseCommand; import frc.robot.subsystems.drive.ctre.CtreDrive; import frc.robot.subsystems.drive.ctre.CtreDriveTelemetry; public class Drive extends SubsystemBase { private static Drive driveInstance; - public static Drive getInstance() { - if (driveInstance == null) { - driveInstance = new Drive(); - } - return driveInstance; - } + public static Drive getInstance() { + if (driveInstance == null) { + driveInstance = new Drive(); + } + return driveInstance; + } private SwerveDriveState lastReadState; - public static final SwerveRequest.FieldCentric teleopRequest = new SwerveRequest.FieldCentric(); + public static final SwerveRequest.FieldCentric teleopRequest = new SwerveRequest.FieldCentric(); public SwerveRequest driveRequest = teleopRequest; - private final CtreDrive drivetrain = CtreDriveConstants.createDrivetrain(); + private final CtreDrive drivetrain = CtreDriveConstants.createDrivetrain(); private final CtreDriveTelemetry telemetry = new CtreDriveTelemetry(MAX_SPEED); - @SuppressWarnings("unused") - private Time lastPoseResetTime = BaseUnits.TimeUnit.of(0.0); // Citrus what are you doing + @SuppressWarnings("unused") + private Time lastPoseResetTime = BaseUnits.TimeUnit.of(0.0); // Citrus what are you doing + + Autopilot ap; private Drive() { lastReadState = drivetrain.getState(); @@ -54,11 +59,34 @@ private Drive() { })); drivetrain.getOdometryThread().setThreadPriority(31); - TelemetryManager.getInstance().addStructPublisher("Mechanisms/Drive", Pose3d.struct, () -> new Pose3d(getPose())); - TelemetryManager.getInstance().addSendable(this); + TelemetryManager.getInstance().addStructPublisher("Mechanisms/Drive", Pose3d.struct, () -> new Pose3d(getPose())); + TelemetryManager.getInstance().addStructPublisher("Drive/TargetSpeeds", ChassisSpeeds.struct, + () -> { + try { + if (driveRequest instanceof SwerveRequest.ApplyFieldSpeeds) { + return ChassisSpeeds.fromFieldRelativeSpeeds( + ((SwerveRequest.ApplyFieldSpeeds) driveRequest).Speeds, + lastReadState.Pose.getRotation()); + } else if (driveRequest instanceof SwerveRequest.ApplyRobotSpeeds) { + return ((SwerveRequest.ApplyRobotSpeeds) driveRequest).Speeds; + } else if (driveRequest instanceof SwerveRequest.FieldCentric) { + var req = ((SwerveRequest.FieldCentric) driveRequest); + return ChassisSpeeds.fromFieldRelativeSpeeds( + req.VelocityX, + req.VelocityY, + req.RotationalRate, + lastReadState.Pose.getRotation()); + } else if (driveRequest instanceof SwerveRequest.RobotCentric) { + var req = ((SwerveRequest.RobotCentric) driveRequest); + return new ChassisSpeeds(req.VelocityX, req.VelocityY, req.RotationalRate); + } + } finally {} + return lastReadState.Speeds; + }); + TelemetryManager.getInstance().addSendable(this); } - /** @return the ctre generated drivetrain */ + /** @return the ctre generated drivetrain */ public CtreDrive getCtreDrive() { return drivetrain; } @@ -71,161 +99,172 @@ public void periodic() { public void outputTelemetry() { telemetry.telemeterize(lastReadState); - FieldLayout.field.setRobotPose(getPose()); + FieldLayout.field.setRobotPose(getPose()); } - /** - * @return the current state - */ + /** + * @return the current state + */ public SwerveDriveState getState() { return drivetrain.getState(); } - /** - * @return the last read pose - */ + /** + * @return the last read pose + */ public Pose2d getPose() { return lastReadState.Pose; } - /** - * @return the chassis speeds, field relative - */ - public ChassisSpeeds getFieldSpeeds() { - return ChassisSpeeds.fromRobotRelativeSpeeds(lastReadState.Speeds, lastReadState.Pose.getRotation()); - } + /** + * @return the chassis speeds, field relative + */ + public ChassisSpeeds getFieldSpeeds() { + return ChassisSpeeds.fromRobotRelativeSpeeds(lastReadState.Speeds, lastReadState.Pose.getRotation()); + } - /** - * Switches the swerve request - *

Please do not the new swerve request every 20 ms

- */ - public void setSwerveRequest(SwerveRequest request) { + /** + * Switches the swerve request + *

Please do not the new swerve request every 20 ms

+ */ + public void setSwerveRequest(SwerveRequest request) { driveRequest = request; } - /** - * @return the current swerve request - */ - public SwerveRequest getSwerveRequest() { + /** + * @return the current swerve request + */ + public SwerveRequest getSwerveRequest() { return driveRequest; } - /** Open loop during teleop mode */ - public Command teleopCommand() { - return runOnce(() -> { - teleopRequest.withVelocityX(0).withVelocityY(0).withRotationalRate(0); - setSwerveRequest(teleopRequest); - }).andThen(run(() -> { - double xDesiredRaw = -Robot.controller.getLeftY(); - double yDesiredRaw = -Robot.controller.getLeftX(); - double rotDesiredRaw = -Robot.controller.getRightX(); - double xFancy = Util.applyJoystickDeadband(xDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); - double yFancy = Util.applyJoystickDeadband(yDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); - double rotFancy = Util.applyJoystickDeadband(rotDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); - - SmartDashboard.putNumber("Sticks/vX", xDesiredRaw); - SmartDashboard.putNumber("Sticks/vY", yDesiredRaw); - SmartDashboard.putNumber("Sticks/vW", rotDesiredRaw); - - teleopRequest - .withVelocityX(xFancy * MAX_SPEED) - .withVelocityY(yFancy * MAX_SPEED) - .withRotationalRate(rotFancy * MAX_ROTATION_SPEED); - }).handleInterrupt(() -> setSwerveRequest(new SwerveRequest.FieldCentric()))).withName("Teleop"); - } - - /** - * Auto aligns to the nearest reef face - * @param left chooses the left or right face - */ - public Command autoAlign(boolean left) { - return defer(() -> { - Pose2d pose; - if (left) { - pose = getPose().nearest(FieldLayout.ALIGN_POSES_LEFT); - } else { - pose = getPose().nearest(FieldLayout.ALIGN_POSES_RIGHT); - } - return new PIDToPoseCommand(pose); - }).withName("Auto Align"); - } - - /** Adds a vision update */ + /** Open loop during teleop mode */ + public Command teleopCommand() { + return runOnce(() -> { + teleopRequest.withVelocityX(0).withVelocityY(0).withRotationalRate(0); + setSwerveRequest(teleopRequest); + }).andThen(run(() -> { + double xDesiredRaw = -Robot.controller.getLeftY(); + double yDesiredRaw = -Robot.controller.getLeftX(); + double rotDesiredRaw = -Robot.controller.getRightX(); + double xFancy = Util.applyJoystickDeadband(xDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); + double yFancy = Util.applyJoystickDeadband(yDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); + double rotFancy = Util.applyJoystickDeadband(rotDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); + + SmartDashboard.putNumber("Sticks/vX", xDesiredRaw); + SmartDashboard.putNumber("Sticks/vY", yDesiredRaw); + SmartDashboard.putNumber("Sticks/vW", rotDesiredRaw); + + teleopRequest + .withVelocityX(xFancy * MAX_SPEED) + .withVelocityY(yFancy * MAX_SPEED) + .withRotationalRate(rotFancy * MAX_ROTATION_SPEED); + }).handleInterrupt(() -> setSwerveRequest(new SwerveRequest.FieldCentric()))).withName("Teleop"); + } + + /** + * Auto aligns to the nearest reef face + * @param left chooses the left or right face + */ + public Command autoAlign(boolean left) { + return defer(() -> { + Pose2d pose; + if (left) { + pose = getPose().nearest(FieldLayout.ALIGN_POSES_LEFT); + } else { + pose = getPose().nearest(FieldLayout.ALIGN_POSES_RIGHT); + } + return new PIDToPoseCommand(pose); + }).withName("Auto Align"); + } + + /** + * Auto aligns to the nearest reef face + * @param left chooses the left or right face + */ + public Command autopilotAlign(boolean left) { + return defer(() -> { + APTarget pose = FieldLayout.getNearestTarget(getPose(), left); + return new AutopilotCommand(pose); + }).withName("Autopilot Align"); + } + + /** Adds a vision update */ public void addVisionUpdate(Pose2d pose, Time timestamp) { getCtreDrive().addVisionMeasurement(pose, timestamp.in(Units.Seconds)); } - /** Adds a vision update with standard deviations */ + /** Adds a vision update with standard deviations */ public void addVisionUpdate(Pose2d pose, Time timestamp, Matrix stdDevs) { getCtreDrive().addVisionMeasurement(pose, timestamp.in(Units.Seconds), stdDevs); } - /** Resets pose estimator to a pose */ + /** Resets pose estimator to a pose */ public void resetPose(Pose2d pose) { getCtreDrive().resetPose(pose); lastPoseResetTime = - Units.Seconds.of(Utils.getCurrentTimeSeconds()).plus(POSE_RESET_PREVENTION_TIME); + Units.Seconds.of(Utils.getCurrentTimeSeconds()).plus(POSE_RESET_PREVENTION_TIME); } - /** A command that resets the pose */ + /** A command that resets the pose */ public Command resetPoseCommand(Pose2d pose) { return Commands.runOnce(() -> resetPose(pose)); } - /** Whether the pitch is stable */ + /** Whether the pitch is stable */ public boolean isPitchStable() { return drivetrain.getPigeon2().getAngularVelocityYDevice().getValue().abs(Units.DegreesPerSecond) - < MAX_VELOCITY_STABLE - && drivetrain.getPigeon2().getPitch().getValue().abs(BaseUnits.AngleUnit) - < MAX_PITCH_STABLE; + < MAX_VELOCITY_STABLE + && drivetrain.getPigeon2().getPitch().getValue().abs(BaseUnits.AngleUnit) + < MAX_PITCH_STABLE; } - /** Whether the roll is stable */ + /** Whether the roll is stable */ public boolean isRollStable() { return drivetrain.getPigeon2().getAngularVelocityXDevice().getValue().abs(Units.DegreesPerSecond) - < MAX_VELOCITY_STABLE - && drivetrain.getPigeon2().getRoll().getValue().abs(BaseUnits.AngleUnit) - < MAX_PITCH_STABLE; + < MAX_VELOCITY_STABLE + && drivetrain.getPigeon2().getRoll().getValue().abs(BaseUnits.AngleUnit) + < MAX_PITCH_STABLE; } - /** Whether the robot is stable */ + /** Whether the robot is stable */ public boolean isStable() { ChassisSpeeds speeds = getState().Speeds; return isPitchStable() - && isRollStable() - && Units.MetersPerSecond.of(Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond)) - .lte(MAX_SPEED_SCORING_TRANSLATION) - && Units.RadiansPerSecond.of(speeds.omegaRadiansPerSecond).lte(MAX_ROTATION_SPEED_SCORING); + && isRollStable() + && Units.MetersPerSecond.of(Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond)) + .lte(MAX_SPEED_SCORING_TRANSLATION) + && Units.RadiansPerSecond.of(speeds.omegaRadiansPerSecond).lte(MAX_ROTATION_SPEED_SCORING); } @Override public void initSendable(SendableBuilder builder) { - super.initSendable(builder); + super.initSendable(builder); builder.addDoubleProperty( - "Pitch Velocity Degrees Per Second", - () -> drivetrain - .getPigeon2() - .getAngularVelocityYDevice() - .getValue() - .in(Units.DegreesPerSecond), - null); + "Pitch Velocity Degrees Per Second", + () -> drivetrain + .getPigeon2() + .getAngularVelocityYDevice() + .getValue() + .in(Units.DegreesPerSecond), + null); builder.addDoubleProperty( - "Pitch Degrees", - () -> drivetrain.getPigeon2().getPitch().getValue().in(Units.Degrees), - null); + "Pitch Degrees", + () -> drivetrain.getPigeon2().getPitch().getValue().in(Units.Degrees), + null); builder.addDoubleProperty( - "Roll Velocity Degrees Per Second", - () -> drivetrain - .getPigeon2() - .getAngularVelocityXDevice() - .getValue() - .in(Units.DegreesPerSecond), - null); + "Roll Velocity Degrees Per Second", + () -> drivetrain + .getPigeon2() + .getAngularVelocityXDevice() + .getValue() + .in(Units.DegreesPerSecond), + null); builder.addDoubleProperty( - "Roll Degrees", - () -> drivetrain.getPigeon2().getRoll().getValue().in(Units.Degrees), - null); + "Roll Degrees", + () -> drivetrain.getPigeon2().getRoll().getValue().in(Units.Degrees), + null); addModuleToBuilder(builder, 0); addModuleToBuilder(builder, 1); @@ -233,12 +272,11 @@ public void initSendable(SendableBuilder builder) { addModuleToBuilder(builder, 3); } - /** Telemeterizes a module */ + /** Telemeterizes a module */ private void addModuleToBuilder(SendableBuilder builder, int module) { - TelemetryManager.makeSendableTalonFX("Modules/" + module + "/Drive", - drivetrain.getModules()[module].getDriveMotor(), builder); - - TelemetryManager.makeSendableTalonFX("Modules/" + module + "/Angle", - drivetrain.getModules()[module].getSteerMotor(), builder); + TelemetryManager.makeSendableTalonFX("Modules/" + module + "/Drive", + drivetrain.getModules()[module].getDriveMotor(), builder); + TelemetryManager.makeSendableTalonFX("Modules/" + module + "/Angle", + drivetrain.getModules()[module].getSteerMotor(), builder); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/commands/AutopilotCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/AutopilotCommand.java new file mode 100644 index 0000000..31d5b96 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/commands/AutopilotCommand.java @@ -0,0 +1,80 @@ +package frc.robot.subsystems.drive.commands; + +import static frc.robot.subsystems.drive.DriveConstants.*; + +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; +import com.therekrab.autopilot.APConstraints; +import com.therekrab.autopilot.APProfile; +import com.therekrab.autopilot.APTarget; +import com.therekrab.autopilot.Autopilot; +import com.therekrab.autopilot.Autopilot.APResult; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.drive.Drive; + +public class AutopilotCommand extends Command { + private final APTarget target; + private final Drive drive; + private static final APConstraints constraints = new APConstraints() + .withVelocity(MAX_SPEED) + .withAcceleration(MAX_ACCEL) + .withJerk(3.0); + + private static final APProfile profile = new APProfile(constraints) + .withErrorXY(Units.Centimeters.of(2)) + .withErrorTheta(Units.Degrees.of(0.5)) + .withBeelineRadius(Units.Centimeters.of(8)); + + public static final Autopilot autoPilot = new Autopilot(profile); + + private final SwerveRequest.FieldCentricFacingAngle request = new SwerveRequest.FieldCentricFacingAngle() + .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance) + .withDriveRequestType(DriveRequestType.Velocity) + .withHeadingPID( + ROTATION_CONSTANTS.kP, + ROTATION_CONSTANTS.kI, + ROTATION_CONSTANTS.kD); + + public AutopilotCommand(APTarget target) { + this(target, Drive.getInstance()); + } + + public AutopilotCommand(APTarget target, Drive drive) { + this.target = target; + this.drive = drive; + addRequirements(drive); + } + + @Override + public void initialize() { + drive.setSwerveRequest(request); + } + + @Override + public void execute() { + ChassisSpeeds robotRelativeSpeeds = drive.getState().Speeds; + Pose2d pose = drive.getPose(); + + APResult out = autoPilot.calculate(pose, robotRelativeSpeeds, target); + + request + .withVelocityX(out.vx()) + .withVelocityY(out.vy()) + .withTargetDirection(out.targetAngle()); + } + + @Override + public boolean isFinished() { + return autoPilot.atTarget(drive.getPose(), target); + } + + @Override + public void end(boolean interrupted) { + drive.setSwerveRequest(new SwerveRequest.RobotCentric()); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java index 9b8789c..4327be3 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java @@ -11,7 +11,9 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.lib.control.ControlConstants.PIDFConstants; +import frc.robot.lib.control.ControlConstants.ProfiledPIDFConstants; import frc.robot.lib.control.PIDVController; +import frc.robot.lib.control.ProfiledPIDVController; import frc.robot.lib.util.Util; import frc.robot.subsystems.drive.Drive; @@ -25,7 +27,7 @@ public class PIDToPoseCommand extends Command { new SwerveRequest.ApplyFieldSpeeds(); private final PIDVController translationController; - private final PIDVController thetaController; + private final ProfiledPIDVController thetaController; private final Pose2d target; private Pose2d currentPose; @@ -38,14 +40,14 @@ public PIDToPoseCommand(Pose2d target) { Drive.getInstance(), target, TRANSLATION_CONSTANTS, - ROTATION_CONSTANTS); + PROFILED_ROTATION_CONSTANTS); } - public PIDToPoseCommand(Drive drive, Pose2d target, PIDFConstants translationConstants, PIDFConstants rotationConstants) { + public PIDToPoseCommand(Drive drive, Pose2d target, PIDFConstants translationConstants, ProfiledPIDFConstants rotationConstants) { this.drive = drive; this.target = target; translationController = new PIDVController(translationConstants); - thetaController = new PIDVController(rotationConstants); + thetaController = new ProfiledPIDVController(rotationConstants); thetaController.enableContinuousInput(-Math.PI, Math.PI); addRequirements(drive); @@ -83,10 +85,10 @@ public ChassisSpeeds calculateSpeeds() { // Magnitude target double vMagnitude = MathUtil.clamp( - translationController.setTarget(delta.getNorm()) + translationController.setTarget(0.0) .setMeasurement( - 0.0, // We are exactly where we are - Util.chassisSpeedsMagnitude( + delta.getNorm(), // We are exactly where we are + -Util.chassisSpeedsMagnitude( currentSpeeds)) // How fast we are going .getOutput(), -MAX_SPEED, MAX_SPEED); From 19c3d7b93131a0758a6dd45a198f9972a2274ae2 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Fri, 9 Jan 2026 21:09:13 -0800 Subject: [PATCH 024/118] Fixed PIDToPose --- build.gradle | 12 ++++----- src/main/java/frc/robot/Robot.java | 1 - .../lib/control/ProfiledPIDVController.java | 26 ++++++++++--------- .../drive/commands/PIDToPoseCommand.java | 9 +++++-- 4 files changed, 27 insertions(+), 21 deletions(-) diff --git a/build.gradle b/build.gradle index 1d8df5e..c842242 100644 --- a/build.gradle +++ b/build.gradle @@ -28,12 +28,12 @@ deploy { frcJava(getArtifactTypeClass('FRCJavaArtifact')) { // Enable VisualVM connection - // jvmArgs.add("-Dcom.sun.management.jmxremote=true") - // jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") - // jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") - // jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") - // jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") - // jvmArgs.add("-Djava.rmi.server.hostname=10.14.58.2") // Replace TE.AM with team number + jvmArgs.add("-Dcom.sun.management.jmxremote=true") + jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") + jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") + jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") + jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") + jvmArgs.add("-Djava.rmi.server.hostname=10.14.58.2") // Replace TE.AM with team number } // Static files artifact diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3c6281b..0b73ec1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -143,7 +143,6 @@ public void testPeriodic() { /** This function is called once when the robot is first started up. */ @Override public void simulationInit() { - DriverStationSim.setAllianceStationId(AllianceStationID.Blue1); } /** This function is called periodically whilst in simulation. */ diff --git a/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java b/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java index 9de1797..2e76a9c 100644 --- a/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java +++ b/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants; import frc.robot.lib.control.ControlConstants.ProfiledPIDVConstants; @@ -21,6 +20,11 @@ public ProfiledPIDVController(ProfiledPIDVConstants constants) { profile = new TrapezoidProfile(constants.constraints); } + public ProfiledPIDVController setInitialSetpoint(double position, double speed) { + setpoint = new TrapezoidProfile.State(position, speed); + return this; + } + /** * Makes the controller continuous, which means that values repeat. */ @@ -51,23 +55,21 @@ public ProfiledPIDVController setMeasurement(double position, double speed) { } public double getOutput() { - if (controller.isContinuous) { - double errorBound = (controller.maxRange - controller.minRange) / 2.0; + // if (controller.isContinuous) { + // double errorBound = (controller.maxRange - controller.minRange) / 2.0; - double goalDelta = - MathUtil.inputModulus(goal.position - controller.positionMeasurement, -errorBound, errorBound); - double setpointDelta = - MathUtil.inputModulus(setpoint.position - controller.positionMeasurement, -errorBound, errorBound); + // double goalDelta = + // MathUtil.inputModulus(goal.position - controller.positionMeasurement, -errorBound, errorBound); + // double setpointDelta = + // MathUtil.inputModulus(setpoint.position - controller.positionMeasurement, -errorBound, errorBound); - goal.position = goalDelta + controller.positionMeasurement; - setpoint.position = setpointDelta + controller.positionMeasurement; - } + // goal.position = goalDelta + controller.positionMeasurement; + // setpoint.position = setpointDelta + controller.positionMeasurement; + // } // Advance profile by one timestep setpoint = profile.calculate(Constants.DT, setpoint, goal); - SmartDashboard.putNumber("Debug/Profile", setpoint.velocity); - // Use profiled position & velocity as setpoints return controller.setTarget(setpoint.position, setpoint.velocity).getOutput(); } diff --git a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java index c63da29..f662bf3 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java @@ -55,6 +55,10 @@ public PIDToPoseCommand(Drive drive, Pose2d target, PIDVConstants translationCon @Override public void initialize() { + var state = drive.getState(); + thetaController.setInitialSetpoint( + state.Pose.getRotation().getRadians(), + state.Speeds.omegaRadiansPerSecond); drive.setSwerveRequest(request); } @@ -94,12 +98,13 @@ public ChassisSpeeds calculateSpeeds() { // The angle we are at relative to the target var deltaRotation = delta.getAngle(); - double rotation = + double rotation = MathUtil.clamp( thetaController.setTarget(target.getRotation().getRadians()) // Theta target .setMeasurement( currentPose.getRotation().getRadians(), currentSpeeds.omegaRadiansPerSecond) // We are where we are and we are as fast as how fast we are going - .getOutput(); + .getOutput(), + -MAX_ROTATION_SPEED, MAX_ROTATION_SPEED); SmartDashboard.putNumber("Debug/PIDToPose/vx", vMagnitude * deltaRotation.getCos()); SmartDashboard.putNumber("Debug/PIDToPose/vy", vMagnitude * deltaRotation.getSin()); From 7e53dc40674d6f7c63f7a7fb1005ecfdcd4f9a57 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Fri, 9 Jan 2026 21:37:41 -0800 Subject: [PATCH 025/118] last few bugs trust --- src/main/java/frc/robot/ControlsMapping.java | 4 +-- .../subsystems/drive/DriveConstants.java | 16 +++++----- .../drive/commands/AutopilotCommand.java | 5 ++-- .../drive/commands/PIDToPoseCommand.java | 29 ++++++++++++------- .../drive/commands/TrajectoryCommand.java | 15 ++++++---- 5 files changed, 40 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index ef544ff..d8942eb 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -21,8 +21,8 @@ public static void mapTeleopCommand() { controller.a().onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); controller.leftBumper().whileTrue(Drive.getInstance().autoAlign(true)); controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false)); - controller.b().whileTrue(new PIDToPoseCommand( - new Pose2d(2.0, 1.0, Rotation2d.fromDegrees(120.0)))); + controller.x().whileTrue(Drive.getInstance().autopilotAlign(true)); + controller.y().whileTrue(Drive.getInstance().autopilotAlign(false)); } public static void mapSysId() { diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 047505f..1792291 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -9,12 +9,12 @@ import frc.robot.subsystems.drive.ctre.CtreDriveConstants; public final class DriveConstants { - public static final double EPSILON_TRANSLATION = 0.015; // cm - public static final double EPSILON_ROTATION = Units.Degrees.of(1).in(Units.Radians); + public static final double EPSILON_TRANSLATION = 0.02; // cm + public static final double EPSILON_ROTATION = Units.Degrees.of(2).in(Units.Radians); // Maximums - public static final double MAX_SPEED = Units.MetersPerSecond.of(3.5).in(Units.MetersPerSecond); - public static final double MAX_ACCEL = Units.MetersPerSecondPerSecond.of(6.0).in(Units.MetersPerSecondPerSecond); + public static final double MAX_SPEED = Units.MetersPerSecond.of(4.5).in(Units.MetersPerSecond); + public static final double MAX_ACCEL = Units.MetersPerSecondPerSecond.of(9.0).in(Units.MetersPerSecondPerSecond); public static final double MAX_ROTATION_SPEED = Units.RotationsPerSecond.of(2.0).in(Units.RadiansPerSecond); public static final double MAX_ROTATION_ACCEL = @@ -37,16 +37,16 @@ public final class DriveConstants { // Trajectory and snap constants public static final PIDVConstants TRANSLATION_CONSTANTS = - new PIDVConstants(5, 0.001, 0.1); + new PIDVConstants(5, 0.0, 0.1); public static final ProfiledPIDVConstants PROFILED_TRANSLATION_CONSTANTS = - new ProfiledPIDVConstants(5.0, 0.001, 0.1, + new ProfiledPIDVConstants(5.0, 0.0, 0.1, new TrapezoidProfile.Constraints( MAX_SPEED, MAX_ACCEL)); public static final PIDVConstants ROTATION_CONSTANTS = - new PIDVConstants(4.0, 0.001, 0.1); + new PIDVConstants(5.0, 0.0, 0.1); public static final ProfiledPIDVConstants PROFILED_ROTATION_CONSTANTS = - new ProfiledPIDVConstants(5.0, 0.001, 0.1, + new ProfiledPIDVConstants(5.0, 0.0, 0.1, new TrapezoidProfile.Constraints( MAX_ROTATION_SPEED, MAX_ROTATION_ACCEL)); diff --git a/src/main/java/frc/robot/subsystems/drive/commands/AutopilotCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/AutopilotCommand.java index 31d5b96..6a38cc5 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/AutopilotCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/AutopilotCommand.java @@ -16,6 +16,7 @@ import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.DriveConstants; public class AutopilotCommand extends Command { private final APTarget target; @@ -26,8 +27,8 @@ public class AutopilotCommand extends Command { .withJerk(3.0); private static final APProfile profile = new APProfile(constraints) - .withErrorXY(Units.Centimeters.of(2)) - .withErrorTheta(Units.Degrees.of(0.5)) + .withErrorXY(Units.Meters.of(DriveConstants.EPSILON_TRANSLATION)) + .withErrorTheta(Units.Radians.of(DriveConstants.EPSILON_ROTATION)) .withBeelineRadius(Units.Centimeters.of(8)); public static final Autopilot autoPilot = new Autopilot(profile); diff --git a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java index f662bf3..99fccb6 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/PIDToPoseCommand.java @@ -5,6 +5,8 @@ import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.util.sendable.SendableBuilder; @@ -24,7 +26,7 @@ public class PIDToPoseCommand extends Command { private final SwerveRequest.ApplyFieldSpeeds request = new SwerveRequest.ApplyFieldSpeeds(); - private final PIDVController translationController; + private final ProfiledPIDVController translationController; private final ProfiledPIDVController thetaController; private final Pose2d target; @@ -33,21 +35,25 @@ public class PIDToPoseCommand extends Command { private ChassisSpeeds targetSpeeds = new ChassisSpeeds(); + private final Debouncer finishDebouncer; + public PIDToPoseCommand(Pose2d target) { this( Drive.getInstance(), target, - TRANSLATION_CONSTANTS, + PROFILED_TRANSLATION_CONSTANTS, PROFILED_ROTATION_CONSTANTS); } - public PIDToPoseCommand(Drive drive, Pose2d target, PIDVConstants translationConstants, ProfiledPIDVConstants rotationConstants) { + public PIDToPoseCommand(Drive drive, Pose2d target, ProfiledPIDVConstants translationConstants, ProfiledPIDVConstants rotationConstants) { this.drive = drive; this.target = target; - translationController = new PIDVController(translationConstants); + translationController = new ProfiledPIDVController(translationConstants); thetaController = new ProfiledPIDVController(rotationConstants); thetaController.enableContinuousInput(-Math.PI, Math.PI); + finishDebouncer = new Debouncer(0.040, DebounceType.kRising); + addRequirements(drive); setName("(" + target.getX() + ", " + target.getY() + ", " + target.getRotation().getDegrees() + " deg)" + " :PID to pose"); @@ -86,11 +92,11 @@ public ChassisSpeeds calculateSpeeds() { var delta = target.getTranslation().minus(currentPose.getTranslation()); // Magnitude target - double vMagnitude = MathUtil.clamp( - translationController.setTarget(delta.getNorm()) + double vMagnitude = -MathUtil.clamp( + translationController.setTarget(0) .setMeasurement( - 0, // We are exactly where we are - Util.chassisSpeedsMagnitude( + delta.getNorm(), // We are exactly where we are + -Util.chassisSpeedsMagnitude( currentSpeeds)) // How fast we are going .getOutput(), -MAX_SPEED, MAX_SPEED); @@ -118,8 +124,9 @@ public ChassisSpeeds calculateSpeeds() { @Override public boolean isFinished() { - return Math.abs(translationController.getError()) <= EPSILON_TRANSLATION - && MathUtil.isNear(thetaController.getError(), 0, EPSILON_ROTATION); // Within tolerance + return finishDebouncer.calculate( + Math.abs(translationController.getError()) <= EPSILON_TRANSLATION + && MathUtil.isNear(thetaController.getError(), 0, EPSILON_ROTATION)); // Within tolerance } @Override @@ -128,7 +135,7 @@ public void end(boolean interrupted) { drive.setSwerveRequest(new SwerveRequest.ApplyRobotSpeeds()); System.out.printf( "Done with auto-align, error: %.5f m, interrupted: %b\n", - translationController.getError(), interrupted); + -translationController.getError(), interrupted); } public static double estimateTimeToPose( diff --git a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java index e1d3a65..6c32594 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java @@ -120,17 +120,20 @@ public ChassisSpeeds calculateSpeeds() { xAccelFF += -angularAccel * targetState.pose.getRotation().getSin(); yAccelFF += angularAccel * targetState.pose.getRotation().getCos(); - double vx = xController.setTarget(targetState.pose.getX())// Target setting + double vx = xController.setTarget(targetState.pose.getX(), targetState.speeds.vxMetersPerSecond) // Target setting .setMeasurement(currentPose.getX(), currentSpeeds.vxMetersPerSecond) // Measurement setting .getOutput(); // Output getting // same thing but for vy and rotation instead - double vy = yController.setTarget(targetState.pose.getY()) + double vy = yController.setTarget(targetState.pose.getY(), targetState.speeds.vyMetersPerSecond) .setMeasurement(currentPose.getY(), currentSpeeds.vyMetersPerSecond) .getOutput(); - double rotation = thetaController.setTarget(targetState.pose.getRotation().getRadians()) - .setMeasurement( + double rotation = thetaController + .setTarget( + targetState.pose.getRotation().getRadians(), + targetState.speeds.omegaRadiansPerSecond + ).setMeasurement( currentPose.getRotation().getRadians(), currentSpeeds.omegaRadiansPerSecond) .getOutput(); @@ -149,10 +152,10 @@ public boolean isFinished() { if (trajectory.isDone()) { System.out.printf( - "Done with trajectory, error: %.5f m, int error translation: %.5f m, rot: %.5f rad\n", + "Done with trajectory, error: %.5f m, int error translation: %.5f m*s, rot: %.5f deg*s\n", Math.hypot(xController.getError(), yController.getError()), tracker.getTranslationRmsError(), - tracker.getRotationRmsError()); + tracker.getRotationRmsError() / Math.PI * 180); return true; // We are done guys } From c8e75f70e493c760a226f71f89e854b58e7df4ce Mon Sep 17 00:00:00 2001 From: APandit1-cpu Date: Wed, 14 Jan 2026 16:00:07 -0800 Subject: [PATCH 026/118] aarush's test branch --- build.gradle | 10 ++++++ src/main/java/frc/robot/ControlsMapping.java | 4 +++ src/main/java/frc/robot/Robot.java | 8 +++++ vendordeps/AdvantageKit.json | 35 ++++++++++++++++++++ 4 files changed, 57 insertions(+) create mode 100644 vendordeps/AdvantageKit.json diff --git a/build.gradle b/build.gradle index c842242..74588ff 100644 --- a/build.gradle +++ b/build.gradle @@ -58,11 +58,21 @@ def includeDesktopSupport = true // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. + +task(replayWatch, type: JavaExec) { + mainClass = "org.littletonrobotics.junction.ReplayWatch" + classpath = sourceSets.main.runtimeClasspath +} + dependencies { annotationProcessor wpi.java.deps.wpilibAnnotations() implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() + // ... + def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) + annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" + roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index d8942eb..e9859bd 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -11,6 +11,10 @@ import frc.robot.subsystems.drive.commands.PIDToPoseCommand; import frc.robot.subsystems.drive.ctre.CtreDrive.SysIdRoutineType; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; + public class ControlsMapping { public static void mapTeleopCommand() { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0b73ec1..acebca1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,11 @@ import java.util.Optional; +import org.littletonrobotics.junction.LogFileUtil; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGReader; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; + import com.pathplanner.lib.commands.FollowPathCommand; import edu.wpi.first.epilogue.Logged; @@ -60,6 +65,9 @@ public Robot() { System.out.println("Log/USB mounts NOT OK"); } DriverStation.startDataLog(DataLogManager.getLog()); + + + } /** diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json new file mode 100644 index 0000000..bef4a15 --- /dev/null +++ b/vendordeps/AdvantageKit.json @@ -0,0 +1,35 @@ +{ + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "4.1.2", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2025", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-java", + "version": "4.1.2" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "4.1.2", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [] +} \ No newline at end of file From e4c60a856f5be484a7cb37526acf6819a1ab980e Mon Sep 17 00:00:00 2001 From: "akhil.space" <145620839+aerospikeengineer@users.noreply.github.com> Date: Wed, 14 Jan 2026 16:17:25 -0800 Subject: [PATCH 027/118] Basic Shooter Code Barebones code --- .../frc/robot/subsystems/shooter/Shooter.java | 115 ++++++++++++++++++ .../subsystems/shooter/ShooterConstants.java | 46 +++++++ vendordeps/libgrapplefrc2025.json | 71 +++++++++++ 3 files changed, 232 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/shooter/Shooter.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java create mode 100644 vendordeps/libgrapplefrc2025.json diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java new file mode 100644 index 0000000..0712b04 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -0,0 +1,115 @@ +package frc.robot.subsystems.shooter; + +import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.NeutralOut; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import au.grapplerobotics.LaserCan; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.TelemetryManager; + +public class Shooter extends SubsystemBase { + private static Shooter ShooterInstance; + public static Shooter getInstance() { + if (ShooterInstance == null) { + ShooterInstance = new Shooter(); + } + return ShooterInstance; + } + + private final TalonFX leftMotor; + private final TalonFX rightMotor; + + private final LaserCan shooterLaser; + + private final Debouncer shooterDebouncer; + + private boolean inRangeShooter; + + private double lastReadSpeed; + private ControlRequest request = new NeutralOut(); + + private Shooter() { + super(); + + leftMotor = new TalonFX(ShooterConstants.Motors.LEFT.id); + rightMotor = new TalonFX(ShooterConstants.Motors.RIGHT.id); + + shooterLaser = new LaserCan(ShooterConstants.Lasers.FRONT.id); + + leftMotor.getConfigurator().apply(ShooterConstants.getConfig()); + rightMotor.getConfigurator().apply(ShooterConstants.getConfig()); + leftMotor.setNeutralMode(NeutralModeValue.Brake); + rightMotor.setNeutralMode(NeutralModeValue.Brake); + rightMotor.setControl( + new Follower(leftMotor.getDeviceID(), true)); + + shooterDebouncer = new Debouncer(0.06, DebounceType.kBoth); + + inRangeShooter = false; + + TelemetryManager.getInstance().addSendable(this); + } + + @Override + public void periodic() { + // Read inputs + lastReadSpeed = leftMotor.getVelocity().getValueAsDouble(); + inRangeShooter = shooterDebouncer.calculate(getShooterLaser()); + leftMotor.setControl(request); + } + + /** Replaces the request */ + private void setRequest(ControlRequest request) { + this.request = request; + } + + /** Stops the shooter */ + public Command stop() { + return runOnce(() -> setRequest(new NeutralOut())).withName("Stopped"); + } + + /** Gets the intake laser measurement */ + private double getMeasurementShooter() { + var measurement = shooterLaser.getMeasurement(); + if (measurement != null) { + return measurement.distance_mm; + } else { + return Double.POSITIVE_INFINITY; + } + } + + /** Gets whether the intake laser detects a coral */ + private boolean getShooterLaser() { + return getMeasurementShooter() < 100; + } + + public Command shoot() { + return runOnce(() -> setRequest( + new VelocityVoltage(ShooterConstants.SHOOT_SPEED)) + ).andThen( + Commands.waitUntil(() -> !inRangeShooter), + Commands.waitSeconds(0.15), + stop() + ).withName("Shooting"); + } + + @Override + public void initSendable(SendableBuilder builder) { + super.initSendable(builder); + builder.addDoubleProperty( + "/Speed", + () -> lastReadSpeed, + null); + TelemetryManager.makeSendableTalonFX("/Left", leftMotor, builder); + TelemetryManager.makeSendableTalonFX("/Right", rightMotor, builder); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java new file mode 100644 index 0000000..9c4c69b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -0,0 +1,46 @@ +package frc.robot.subsystems.shooter; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.VoltageConfigs; + +public final class ShooterConstants { + public static final double SHOOT_SPEED = 25; // rotations per second + + /** Motor ids */ + public static enum Motors { + LEFT(12), + RIGHT(13); + public final int id; + private Motors(int id) { + this.id = id; + } + } + + /** Laser ids */ + public static enum Lasers { + //BACK(30), + FRONT(31); + public final int id; + private Lasers(int id) { + this.id = id; + } + } + + /** Config for shooter motors */ + public static TalonFXConfiguration getConfig() { + return new TalonFXConfiguration() + .withSlot0(new Slot0Configs() + .withKV(0.0) + .withKP(0.3) + .withKI(0.0) + .withKD(0.0)) + .withCurrentLimits(new CurrentLimitsConfigs() + .withStatorCurrentLimit(30) + .withSupplyCurrentLimit(30)) + .withVoltage(new VoltageConfigs() + .withPeakForwardVoltage(12.0) + .withPeakReverseVoltage(-12.0)); + } +} diff --git a/vendordeps/libgrapplefrc2025.json b/vendordeps/libgrapplefrc2025.json new file mode 100644 index 0000000..508fd7b --- /dev/null +++ b/vendordeps/libgrapplefrc2025.json @@ -0,0 +1,71 @@ +{ + "fileName": "libgrapplefrc2025.json", + "name": "libgrapplefrc", + "version": "2025.0.8", + "frcYear": "2025", + "uuid": "8ef3423d-9532-4665-8339-206dae1d7168", + "mavenUrls": [ + "https://storage.googleapis.com/grapple-frc-maven" + ], + "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2025.json", + "javaDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcjava", + "version": "2025.0.8" + } + ], + "jniDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2025.0.8", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrccpp", + "version": "2025.0.8", + "libName": "grapplefrc", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2025.0.8", + "libName": "grapplefrcdriver", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file From b4b2e12bb4bc415cf64df71200bed2b5554cb3aa Mon Sep 17 00:00:00 2001 From: shakability Date: Wed, 14 Jan 2026 16:56:23 -0800 Subject: [PATCH 028/118] i am awake --- src/main/java/frc/robot/subsystems/climb/Climb.java | 5 +++++ 1 file changed, 5 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/climb/Climb.java diff --git a/src/main/java/frc/robot/subsystems/climb/Climb.java b/src/main/java/frc/robot/subsystems/climb/Climb.java new file mode 100644 index 0000000..04d2ba8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climb/Climb.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.climb; + +public class Climb { + +} From 204c13c72b2a0a43fb7e5938a47419996f57b472 Mon Sep 17 00:00:00 2001 From: Ethan Lian Date: Wed, 14 Jan 2026 17:25:09 -0800 Subject: [PATCH 029/118] dsljfs --- .vscode/launch.json | 12 +++++++++--- .../java/frc/robot/subsystems/indexer/Indexer.java | 11 +++++++++++ .../robot/subsystems/indexer/IndexerConstants.java | 6 ++++++ 3 files changed, 26 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/indexer/Indexer.java create mode 100644 src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java diff --git a/.vscode/launch.json b/.vscode/launch.json index 5b804e8..ff2ba02 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -4,18 +4,24 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ - + { + "type": "java", + "name": "Indexer", + "request": "launch", + "mainClass": "frc.robot.subsystems.indexer.Indexer", + "projectName": "Robot2026" + }, { "type": "wpilib", "name": "WPILib Desktop Debug", "request": "launch", - "desktop": true, + "desktop": true }, { "type": "wpilib", "name": "WPILib roboRIO Debug", "request": "launch", - "desktop": false, + "desktop": false } ] } diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java new file mode 100644 index 0000000..25cb192 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -0,0 +1,11 @@ +package frc.robot.subsystems.indexer; +import static frc.robot.subsystems.indexer.IndexerConstants; + +public class Indexer { + /* forever loop that constantly + * 1) finds shooterMax + * 2) finds shooterFuel + * 3) calculates shooterSpace + * 4) Based on shooter + */ +} diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java new file mode 100644 index 0000000..d56ef60 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java @@ -0,0 +1,6 @@ +/* intake -> storage -> indexer */ +public class IndexerConstants { + int shooterFuel = 3; /* something that detects the # of fuel in shooter */ + int shooterMax = 5; /* max capacity of shooter */ + int shooterSpace = 0; /* space left */ +} \ No newline at end of file From 77b51a7745ef5484d67249644469d01e14a2cab6 Mon Sep 17 00:00:00 2001 From: "akhil.space" <145620839+aerospikeengineer@users.noreply.github.com> Date: Sat, 17 Jan 2026 11:13:15 -0800 Subject: [PATCH 030/118] Addiitonal Shooter Changes --- .../frc/robot/subsystems/shooter/Shooter.java | 30 +++++++++---------- .../subsystems/shooter/ShooterConstants.java | 4 +-- 2 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 0712b04..0f620cb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -25,14 +25,14 @@ public static Shooter getInstance() { return ShooterInstance; } - private final TalonFX leftMotor; - private final TalonFX rightMotor; + private final TalonFX topMotor; + private final TalonFX bottomMotor; private final LaserCan shooterLaser; private final Debouncer shooterDebouncer; - private boolean inRangeShooter; + private boolean inRangeShooter; private double lastReadSpeed; private ControlRequest request = new NeutralOut(); @@ -40,17 +40,17 @@ public static Shooter getInstance() { private Shooter() { super(); - leftMotor = new TalonFX(ShooterConstants.Motors.LEFT.id); - rightMotor = new TalonFX(ShooterConstants.Motors.RIGHT.id); + topMotor = new TalonFX(ShooterConstants.Motors.TOP.id); + bottomMotor = new TalonFX(ShooterConstants.Motors.BOTTOM.id); shooterLaser = new LaserCan(ShooterConstants.Lasers.FRONT.id); - leftMotor.getConfigurator().apply(ShooterConstants.getConfig()); - rightMotor.getConfigurator().apply(ShooterConstants.getConfig()); - leftMotor.setNeutralMode(NeutralModeValue.Brake); - rightMotor.setNeutralMode(NeutralModeValue.Brake); - rightMotor.setControl( - new Follower(leftMotor.getDeviceID(), true)); + topMotor.getConfigurator().apply(ShooterConstants.getConfig()); + bottomMotor.getConfigurator().apply(ShooterConstants.getConfig()); + topMotor.setNeutralMode(NeutralModeValue.Brake); + bottomMotor.setNeutralMode(NeutralModeValue.Brake); + bottomMotor.setControl( + new Follower(topMotor.getDeviceID(), true)); shooterDebouncer = new Debouncer(0.06, DebounceType.kBoth); @@ -62,9 +62,9 @@ private Shooter() { @Override public void periodic() { // Read inputs - lastReadSpeed = leftMotor.getVelocity().getValueAsDouble(); + lastReadSpeed = topMotor.getVelocity().getValueAsDouble(); inRangeShooter = shooterDebouncer.calculate(getShooterLaser()); - leftMotor.setControl(request); + bottomMotor.setControl(request); } /** Replaces the request */ @@ -109,7 +109,7 @@ public void initSendable(SendableBuilder builder) { "/Speed", () -> lastReadSpeed, null); - TelemetryManager.makeSendableTalonFX("/Left", leftMotor, builder); - TelemetryManager.makeSendableTalonFX("/Right", rightMotor, builder); + TelemetryManager.makeSendableTalonFX("/Top", topMotor, builder); + TelemetryManager.makeSendableTalonFX("/Bottom", bottomMotor, builder); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 9c4c69b..76bdae3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -10,8 +10,8 @@ public final class ShooterConstants { /** Motor ids */ public static enum Motors { - LEFT(12), - RIGHT(13); + TOP(12), + BOTTOM(13); public final int id; private Motors(int id) { this.id = id; From da38c89fed5e1a83ee8fa99971ba51f4e8a18c70 Mon Sep 17 00:00:00 2001 From: seaspace1250 <117596196+seaspace1250@users.noreply.github.com> Date: Sat, 17 Jan 2026 13:22:40 -0800 Subject: [PATCH 031/118] feat: initial intake code --- .../frc/robot/subsystems/intake/Intake.java | 115 ++++++++++++++++++ .../subsystems/intake/IntakeConstants.java | 70 +++++++++++ 2 files changed, 185 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/intake/Intake.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeConstants.java diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java new file mode 100644 index 0000000..654c8a8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -0,0 +1,115 @@ +package frc.robot.subsystems.intake; + +import com.ctre.phoenix6.controls.NeutralOut; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.TelemetryManager; + +import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.NeutralOut; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +public class Intake extends SubsystemBase { + private static Intake intakeInstance; + + public static Intake getInstance() { + if (intakeInstance == null) { + intakeInstance = new Intake(); + } + return intakeInstance; + } + + private final TalonFX WheelMotor; + private final TalonFX BarMotor; + + //private final Debouncer intakeDebouncer; + + private ControlRequest wheelRequest = new NeutralOut(); + private ControlRequest barRequest = new NeutralOut(); + + //private boolean inRangeIntake; + + private Intake() { + super(); + WheelMotor = new TalonFX(IntakeConstants.Motors.WHEEL.id); + BarMotor = new TalonFX(IntakeConstants.Motors.BAR.id); + WheelMotor.getConfigurator().apply(IntakeConstants.getWheelConfig()); + BarMotor.getConfigurator().apply(IntakeConstants.getBarConfig()); + WheelMotor.setNeutralMode(NeutralModeValue.Brake); + BarMotor.setNeutralMode(NeutralModeValue.Brake); //? + + TelemetryManager.getInstance().addSendable(this); + //setDefaultCommand(); + + } + + + @Override + public void periodic(){ + // + + BarMotor.setControl(barRequest); + WheelMotor.setControl(wheelRequest); + } + + public Command stopWheel() { + return runOnce(() -> setRequestWheel(new NeutralOut())).withName("Stopped"); + } + + public Command stopBar() { + return runOnce(() -> setRequestBar(new NeutralOut())).withName("Stopped"); // should be voltage for upright bar? + } + + private void setRequestWheel(ControlRequest request) { + this.wheelRequest = request; + } + + private void setRequestBar(ControlRequest request) { + this.barRequest = request; + } + + public Command setWheelIntaking() { + return setWheelSpeed(IntakeConstants.INTAKE_SPEED); + } + + public Command setWheelStop() { + return runOnce(() -> setRequestWheel(new NeutralOut())); + } + + public Command setWheelSpeed(double speed) { + return runOnce(() -> setRequestWheel ( + new VelocityVoltage(speed)) + ).withName(""); + } + + + //---------BAR----------- + + + public Command setBarDown() { + return setBarPosition(IntakeConstants.BAR_POSITION_DOWN); + } + + public Command setBarUp() { + return setBarPosition(IntakeConstants.BAR_POSITION_UP); + } + + public Command setBarPosition(double position) { + double checkedPos = MathUtil.clamp(position, IntakeConstants.BAR_POS_MIN, IntakeConstants.BAR_POS_MAX); + return runOnce(() -> setRequestBar( + new PositionVoltage(checkedPos)) + ).withName(""); + } + + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java new file mode 100644 index 0000000..2cc56a8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -0,0 +1,70 @@ +package frc.robot.subsystems.intake; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.VoltageConfigs; +import com.ctre.phoenix6.signals.GravityTypeValue; + +public class IntakeConstants { + public static final double INTAKE_SPEED = 20; //rotations per second? + // public static final double BAR_VOLTAGE = 4.0; //? + public static final double BAR_POSITION_DOWN = 0.30; + public static final double BAR_POSITION_UP = 0.0; + public static final double BAR_GEAR_RATIO = 1.0; + public static final double BAR_POS_MIN = 0.0; + public static final double BAR_POS_MAX = 0.30; + //etc + + public static enum Motors { //TODO: set motor ids; use separate file for ports? + WHEEL(-1), + BAR(-1); + public final int id; + private Motors(int id) { + this.id = id; + } + } + + // public static enum Lasers { //TODO: set laser ids + // LEFT(-1), + // RIGHT(-1); + // public final int id; + // private Lasers(int id) { + // this.id = id; + // } + // } + + + public static TalonFXConfiguration getWheelConfig() { //TODO: values + return new TalonFXConfiguration() + .withSlot0(new Slot0Configs() + .withKV(0.0) + .withKP(0.3) + .withKI(0.0) + .withKD(0.0)) + .withCurrentLimits(new CurrentLimitsConfigs() + .withStatorCurrentLimit(30) + .withSupplyCurrentLimit(30)) + .withVoltage(new VoltageConfigs() + .withPeakForwardVoltage(12.0) + .withPeakReverseVoltage(-12.0)); + } + + + public static TalonFXConfiguration getBarConfig() { //TODO: values + return new TalonFXConfiguration() + .withSlot0(new Slot0Configs() + .withKV(0.0) + .withKP(0.3) + .withKI(0.0) + .withKD(0.0) + .withKG(0.0).withGravityType(GravityTypeValue.Arm_Cosine)) + .withCurrentLimits(new CurrentLimitsConfigs() + .withStatorCurrentLimit(30) + .withSupplyCurrentLimit(30)) + .withVoltage(new VoltageConfigs() + .withPeakForwardVoltage(12.0) + .withPeakReverseVoltage(-12.0)); + } + +} From f867e8e067fc42e0dc0ae4dc50db4c2cad7045ac Mon Sep 17 00:00:00 2001 From: shakability Date: Sat, 17 Jan 2026 13:48:06 -0800 Subject: [PATCH 032/118] i think im done but you're gonna need the motor id as well as to tune all the stuff marked in climbconstants --- .../frc/robot/subsystems/climb/Climb.java | 177 +++++++++++++++++- .../subsystems/climb/ClimbConstants.java | 69 +++++++ 2 files changed, 243 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/climb/ClimbConstants.java diff --git a/src/main/java/frc/robot/subsystems/climb/Climb.java b/src/main/java/frc/robot/subsystems/climb/Climb.java index 04d2ba8..1d6f827 100644 --- a/src/main/java/frc/robot/subsystems/climb/Climb.java +++ b/src/main/java/frc/robot/subsystems/climb/Climb.java @@ -1,5 +1,176 @@ package frc.robot.subsystems.climb; -public class Climb { - -} +import static frc.robot.subsystems.climb.ClimbConstants.*; + +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.NeutralOut; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.sim.TalonFXSimState; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.Units; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.BatterySim; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.lib.util.Util; +import frc.robot.subsystems.TelemetryManager; + +public class Climb extends SubsystemBase { + private static Climb climbInstance; + public static Climb getInstance() { + if (climbInstance == null) { + climbInstance = new Climb(); + } + return climbInstance; + } + + private final TalonFX climbMotor; + + private double lastReadHeight; + private double lastReadSpeed; + + private double targetHeight = 0; + private ControlRequest request = new NeutralOut(); + + private Climb() { + super(); + climbMotor = new TalonFX(CLIMB_MOTOR_ID); + climbMotor.getConfigurator().apply(getConfig()); + climbMotor.setNeutralMode(NeutralModeValue.Brake); + TelemetryManager.getInstance().addSendable(this); + setDefaultCommand(stop()); + } + + @Override + public void periodic() { + // Read the height from the motor encoder + lastReadHeight = rotationsToMeters( + climbMotor.getPosition().getValueAsDouble()); + lastReadSpeed = rotationsToMeters( + climbMotor.getVelocity().getValueAsDouble()); + // updates the motor + climbMotor.setControl(request); + } + + /** Swaps the control request */ + private void setRequest(ControlRequest request) { + this.request = request; + } + + private Command moveToScoringHeight(Heights height) { + return moveToTarget(height.height).withName(height.name() + ": Unsafe, Moving"); + } + + /** Attempts to move the end effector to a height, in meters */ + private Command moveToTarget(double targetHeight) { + return runOnce( + () -> { + this.targetHeight = targetHeight; + setRequest(new MotionMagicVoltage( + metersToRotations( + targetHeight))); + } + ).andThen( + Commands.waitUntil(() -> isNearTarget()) + ).withName(String.format("%.2f: Unknown, Moving", targetHeight)); + } + + /** Stops the elevator */ + public Command stop() { + return runOnce( + () -> setRequest( + new PositionVoltage( + metersToRotations(targetHeight)))) + .withName("Stopped"); + } + + /** Sysid commands + * @param dynamic If true, then runs dynamic test. If false, quasistatic + */ + public Command sysId(boolean dynamic, SysIdRoutine.Direction direction) { + return defer(() -> { + VoltageOut request = new VoltageOut(0); + SysIdRoutine sysIdRoutine = new SysIdRoutine( + new SysIdRoutine.Config( + null, // Default ramp rate (1 V) + null, // Default step voltage (7 V) + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdElevator_State", state.toString()) + ), + new SysIdRoutine.Mechanism( + output -> request.withOutput(output), + null, + this + ) + ); + if (dynamic) { + return sysIdRoutine.dynamic(direction); + } else { + return sysIdRoutine.quasistatic(direction); + } + }); + } + + public double estimateTimeToTarget(Heights height) { + return estimateTimeToTarget(height.height); + } + + public double estimateTimeToTarget(double targetHeight) { + return Util.trapezoidProfileTimeToTarget( + lastReadHeight, lastReadSpeed, + targetHeight, MAX_SPEED, MAX_ACCEL) + + 0.3; // Jerk offset + } + + public double estimateTimeRemaining() { + return Util.trapezoidProfileTimeToTarget( + lastReadHeight, lastReadSpeed, targetHeight, + MAX_SPEED, MAX_ACCEL) + + 0.16; // Jerk offset + } + + /** Checks if the end effector is within 1 cm of the target */ + public boolean isNearTarget() { + return MathUtil.isNear( + lastReadHeight, + targetHeight, + EPSILON); + } + + @Override + public void initSendable(SendableBuilder builder) { + super.initSendable(builder); + builder.addDoubleProperty( + "Height", + () -> lastReadHeight, + null); + builder.addDoubleProperty( + "Speed", + () -> lastReadSpeed, + null); + builder.addDoubleProperty( + "Target Height", + () -> targetHeight, + null); + TelemetryManager.makeSendableTalonFX("ClimbMotor", climbMotor, builder); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java b/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java new file mode 100644 index 0000000..599a8ab --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java @@ -0,0 +1,69 @@ +package frc.robot.subsystems.climb; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.VoltageConfigs; + +import edu.wpi.first.math.util.Units; +import frc.robot.Constants; + +public class ClimbConstants { + + public static final double EPSILON = 0.03; // Meters + + public static final double SPROCKET_RADIUS = 0.0412; // Effective pitch radius + public static final double GEAR_RATIO = 9; + public static final double SPROCKET_CIRCUMFERENCE = SPROCKET_RADIUS * Constants.TAU; + public static final double METERS_PER_ROTATION = 0.028776; // Approximated using measurement + public static final double CARRIAGE_WEIGHT = 7.55; // kg + + // Yo uu hav eto tTune all of this above me + + public static final double MAX_ACCEL = 1.5; + public static final double MAX_SPEED = 1.0; // m/s + + public static final int CLIMB_MOTOR_ID = 67; // change this + + public static enum Heights { + BASE(0.003), // small offset to prevent stalling (allegedly) + UP(Units.inchesToMeters(27.0)); + public final double height; + private Heights(double height) { + this.height = height; + } + } + + public static TalonFXConfiguration getConfig() { + return new TalonFXConfiguration() + .withSlot0(new Slot0Configs() + .withKS(0.125) + .withKV(0.0) + .withKP(1.0) + .withKI(0.0) + .withKD(0.05) + .withKG(0.375)) + .withMotionMagic(new MotionMagicConfigs() + .withMotionMagicAcceleration(metersToRotations(MAX_ACCEL)) // 1.0 m/s^2 + .withMotionMagicCruiseVelocity(metersToRotations(MAX_SPEED)) + .withMotionMagicJerk(320)) + .withCurrentLimits(new CurrentLimitsConfigs() + .withStatorCurrentLimit(60) + .withSupplyCurrentLimit(60)) + .withVoltage(new VoltageConfigs() + .withPeakForwardVoltage(12.0) + .withPeakReverseVoltage(-12.0)); + } + + /** Conversion utility */ + public static double rotationsToMeters(double rotations) { + return rotations * METERS_PER_ROTATION; + } + + /** Conversion utility */ + public static double metersToRotations(double meters) { + return meters / METERS_PER_ROTATION; + } + +} From 96a29e30a95e88d4000fa4f6b718d9d75d4a926e Mon Sep 17 00:00:00 2001 From: "akhil.space" <145620839+aerospikeengineer@users.noreply.github.com> Date: Sat, 17 Jan 2026 14:57:38 -0800 Subject: [PATCH 033/118] sping up --- .../frc/robot/subsystems/shooter/Shooter.java | 43 +++++++++++++++++-- .../subsystems/shooter/ShooterConstants.java | 5 ++- 2 files changed, 44 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 0f620cb..01ca276 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -87,19 +87,56 @@ private double getMeasurementShooter() { } } - /** Gets whether the intake laser detects a coral */ + public Command spinUp() + { + ShooterConstants.SHOOT_SPEED_BOTTOM = 25 + ShooterConstants.SHOOT_SPEED_INCREMENT; + ShooterConstants.SHOOT_SPEED_TOP = 25 - ShooterConstants.SHOOT_SPEED_INCREMENT; + } + + public Command spinDown() + { + ShooterConstants.SHOOT_SPEED_BOTTOM = 25 - ShooterConstants.SHOOT_SPEED_INCREMENT; + ShooterConstants.SHOOT_SPEED_TOP = 25 + ShooterConstants.SHOOT_SPEED_INCREMENT; + } + + /** Gets whether the shooter laser detects the "fuel" (balls) */ private boolean getShooterLaser() { return getMeasurementShooter() < 100; } - public Command shoot() { + public Command topShoot() { + return runOnce(() -> setRequest( + new VelocityVoltage(ShooterConstants.SHOOT_SPEED_TOP)) + ).andThen( + Commands.waitUntil(() -> !inRangeShooter), + Commands.waitSeconds(0.15), + stop() + ).withName("Top Shooting"); + } + + public Command bottomShoot() { return runOnce(() -> setRequest( - new VelocityVoltage(ShooterConstants.SHOOT_SPEED)) + new VelocityVoltage(ShooterConstants.SHOOT_SPEED_BOTTOM)) + ).andThen( + Commands.waitUntil(() -> !inRangeShooter), + Commands.waitSeconds(0.15), + stop() + ).withName("Bottom Shooting"); + } + + public Command shoot() + { + + topShoot(); + bottomShoot(); + + return runOnce(() -> setRequest(request) ).andThen( Commands.waitUntil(() -> !inRangeShooter), Commands.waitSeconds(0.15), stop() ).withName("Shooting"); + } @Override diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 76bdae3..ad84a91 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -6,7 +6,10 @@ import com.ctre.phoenix6.configs.VoltageConfigs; public final class ShooterConstants { - public static final double SHOOT_SPEED = 25; // rotations per second + public static double SHOOT_SPEED_TOP = 25; // rotations per second + public static double SHOOT_SPEED_BOTTOM = 25; // rotations per second + + public static final double SHOOT_SPEED_INCREMENT = 5; // rotation per second /** Motor ids */ public static enum Motors { From db2419b6213e29e1ec550c9b5bd39a2aed8ca198 Mon Sep 17 00:00:00 2001 From: "akhil.space" <145620839+aerospikeengineer@users.noreply.github.com> Date: Sat, 17 Jan 2026 16:01:28 -0800 Subject: [PATCH 034/118] Separate Shooters for Each Motor Hi. My name is Shooter. TalonFX: --- .../subsystems/shooter/BottomShooter.java | 124 ++++++++++++++++++ .../shooter/{Shooter.java => TopShooter.java} | 60 +++------ 2 files changed, 140 insertions(+), 44 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/shooter/BottomShooter.java rename src/main/java/frc/robot/subsystems/shooter/{Shooter.java => TopShooter.java} (70%) diff --git a/src/main/java/frc/robot/subsystems/shooter/BottomShooter.java b/src/main/java/frc/robot/subsystems/shooter/BottomShooter.java new file mode 100644 index 0000000..4ea3e3b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/BottomShooter.java @@ -0,0 +1,124 @@ +package frc.robot.subsystems.shooter; + +import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.controls.NeutralOut; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import au.grapplerobotics.LaserCan; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.TelemetryManager; + +public class BottomShooter extends SubsystemBase { + private static BottomShooter ShooterInstance; + public static BottomShooter getInstance() { + if (ShooterInstance == null) { + ShooterInstance = new BottomShooter(); + } + return ShooterInstance; + } + + private final TalonFX bottomMotor; + + private final LaserCan shooterLaser; + + private final Debouncer shooterDebouncer; + + private boolean inRangeShooter; + + private double lastReadSpeed; + private ControlRequest request = new NeutralOut(); + + private BottomShooter() { + super(); + + bottomMotor = new TalonFX(ShooterConstants.Motors.BOTTOM.id); + + shooterLaser = new LaserCan(ShooterConstants.Lasers.FRONT.id); + + bottomMotor.getConfigurator().apply(ShooterConstants.getConfig()); + bottomMotor.setNeutralMode(NeutralModeValue.Brake); + + shooterDebouncer = new Debouncer(0.06, DebounceType.kBoth); + + inRangeShooter = false; + + TelemetryManager.getInstance().addSendable(this); + } + + @Override + public void periodic() { + // Read inputs + lastReadSpeed = bottomMotor.getVelocity().getValueAsDouble(); + inRangeShooter = shooterDebouncer.calculate(getShooterLaser()); + } + + /** Replaces the request */ + private void setRequest(ControlRequest request) { + this.request = request; + } + + /** Stops the shooter */ + public Command stop() { + return runOnce(() -> setRequest(new NeutralOut())).withName("Stopped"); + } + + /** Gets the intake laser measurement */ + private double getMeasurementShooter() { + var measurement = shooterLaser.getMeasurement(); + if (measurement != null) { + return measurement.distance_mm; + } else { + return Double.POSITIVE_INFINITY; + } + } + + public Command spinUp() + { + ShooterConstants.SHOOT_SPEED_BOTTOM = 25 + ShooterConstants.SHOOT_SPEED_INCREMENT; + return runOnce(() -> setRequest(request) + ).andThen( + stop() + ).withName("Increase Bottom Motor Speed"); + } + + public Command spinDown() + { + ShooterConstants.SHOOT_SPEED_BOTTOM = 25 - ShooterConstants.SHOOT_SPEED_INCREMENT; + return runOnce(() -> setRequest(request) + ).andThen( + stop() + ).withName("Lower Bottom Motor Speed"); + } + + /** Gets whether the shooter laser detects the "fuel" (balls) */ + private boolean getShooterLaser() { + return getMeasurementShooter() < 100; + } + + public Command BottomShoot() { + return runOnce(() -> setRequest( + new VelocityVoltage(ShooterConstants.SHOOT_SPEED_BOTTOM)) + ).andThen( + Commands.waitUntil(() -> !inRangeShooter), + Commands.waitSeconds(0.15), + stop() + ).withName("Bottom Shooting"); + } + + @Override + public void initSendable(SendableBuilder builder) { + super.initSendable(builder); + builder.addDoubleProperty( + "/Speed", + () -> lastReadSpeed, + null); + TelemetryManager.makeSendableTalonFX("/Bottom", bottomMotor, builder); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/TopShooter.java similarity index 70% rename from src/main/java/frc/robot/subsystems/shooter/Shooter.java rename to src/main/java/frc/robot/subsystems/shooter/TopShooter.java index 01ca276..5d42a39 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/TopShooter.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.shooter; import com.ctre.phoenix6.controls.ControlRequest; -import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; @@ -16,17 +15,16 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.TelemetryManager; -public class Shooter extends SubsystemBase { - private static Shooter ShooterInstance; - public static Shooter getInstance() { +public class TopShooter extends SubsystemBase { + private static TopShooter ShooterInstance; + public static TopShooter getInstance() { if (ShooterInstance == null) { - ShooterInstance = new Shooter(); + ShooterInstance = new TopShooter(); } return ShooterInstance; } private final TalonFX topMotor; - private final TalonFX bottomMotor; private final LaserCan shooterLaser; @@ -37,20 +35,15 @@ public static Shooter getInstance() { private double lastReadSpeed; private ControlRequest request = new NeutralOut(); - private Shooter() { + private TopShooter() { super(); topMotor = new TalonFX(ShooterConstants.Motors.TOP.id); - bottomMotor = new TalonFX(ShooterConstants.Motors.BOTTOM.id); shooterLaser = new LaserCan(ShooterConstants.Lasers.FRONT.id); topMotor.getConfigurator().apply(ShooterConstants.getConfig()); - bottomMotor.getConfigurator().apply(ShooterConstants.getConfig()); topMotor.setNeutralMode(NeutralModeValue.Brake); - bottomMotor.setNeutralMode(NeutralModeValue.Brake); - bottomMotor.setControl( - new Follower(topMotor.getDeviceID(), true)); shooterDebouncer = new Debouncer(0.06, DebounceType.kBoth); @@ -64,7 +57,6 @@ public void periodic() { // Read inputs lastReadSpeed = topMotor.getVelocity().getValueAsDouble(); inRangeShooter = shooterDebouncer.calculate(getShooterLaser()); - bottomMotor.setControl(request); } /** Replaces the request */ @@ -89,17 +81,23 @@ private double getMeasurementShooter() { public Command spinUp() { - ShooterConstants.SHOOT_SPEED_BOTTOM = 25 + ShooterConstants.SHOOT_SPEED_INCREMENT; - ShooterConstants.SHOOT_SPEED_TOP = 25 - ShooterConstants.SHOOT_SPEED_INCREMENT; + ShooterConstants.SHOOT_SPEED_TOP = 25 + ShooterConstants.SHOOT_SPEED_INCREMENT; + return runOnce(() -> setRequest(request) + ).andThen( + stop() + ).withName("Increase Top Motor Speed"); } public Command spinDown() { - ShooterConstants.SHOOT_SPEED_BOTTOM = 25 - ShooterConstants.SHOOT_SPEED_INCREMENT; - ShooterConstants.SHOOT_SPEED_TOP = 25 + ShooterConstants.SHOOT_SPEED_INCREMENT; + ShooterConstants.SHOOT_SPEED_TOP = 25 - ShooterConstants.SHOOT_SPEED_INCREMENT; + return runOnce(() -> setRequest(request) + ).andThen( + stop() + ).withName("Lower Top Motor Speed"); } - /** Gets whether the shooter laser detects the "fuel" (balls) */ + /* Gets whether the shooter laser detects the "fuel" (balls) */ private boolean getShooterLaser() { return getMeasurementShooter() < 100; } @@ -114,31 +112,6 @@ public Command topShoot() { ).withName("Top Shooting"); } - public Command bottomShoot() { - return runOnce(() -> setRequest( - new VelocityVoltage(ShooterConstants.SHOOT_SPEED_BOTTOM)) - ).andThen( - Commands.waitUntil(() -> !inRangeShooter), - Commands.waitSeconds(0.15), - stop() - ).withName("Bottom Shooting"); - } - - public Command shoot() - { - - topShoot(); - bottomShoot(); - - return runOnce(() -> setRequest(request) - ).andThen( - Commands.waitUntil(() -> !inRangeShooter), - Commands.waitSeconds(0.15), - stop() - ).withName("Shooting"); - - } - @Override public void initSendable(SendableBuilder builder) { super.initSendable(builder); @@ -147,6 +120,5 @@ public void initSendable(SendableBuilder builder) { () -> lastReadSpeed, null); TelemetryManager.makeSendableTalonFX("/Top", topMotor, builder); - TelemetryManager.makeSendableTalonFX("/Bottom", bottomMotor, builder); } } \ No newline at end of file From dc5934a551531a3619bb54be31107e7d161cf4cd Mon Sep 17 00:00:00 2001 From: Ethan Lian Date: Tue, 20 Jan 2026 15:21:16 -0800 Subject: [PATCH 035/118] w rizz fixes --- .vscode/launch.json | 14 ++++ .../frc/robot/subsystems/indexer/Indexer.java | 76 ++++++++++++++++--- .../subsystems/indexer/IndexerConstants.java | 12 ++- 3 files changed, 88 insertions(+), 14 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index ff2ba02..38fd932 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -4,6 +4,20 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ + { + "type": "java", + "name": "Main", + "request": "launch", + "mainClass": "frc.robot.Main", + "projectName": "Robot2026" + }, + { + "type": "java", + "name": "IndexerConstants", + "request": "launch", + "mainClass": "frc.robot.subsystems.indexer.IndexerConstants", + "projectName": "Robot2026" + }, { "type": "java", "name": "Indexer", diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 25cb192..446ea70 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -1,11 +1,67 @@ package frc.robot.subsystems.indexer; -import static frc.robot.subsystems.indexer.IndexerConstants; - -public class Indexer { - /* forever loop that constantly - * 1) finds shooterMax - * 2) finds shooterFuel - * 3) calculates shooterSpace - * 4) Based on shooter - */ -} +import static frc.robot.subsystems.indexer.IndexerConstants.*; + +import java.util.ResourceBundle.Control; + +import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.units.measure.Velocity; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + + +public class Indexer extends SubsystemBase{ + private ControlRequest request; + + private TalonFX motor; + private SomeBrand laser; // fix later + + private boolean ballz = false; + + private Indexer() { + motor = new TalonFX(IndexerConstants.MOTOR_ID); + } + + private void IndexerSensor() { + laser = new SomeBrand(IndexerConstants.LASER_ID); + } + + @Override + public void periodic() { + motor.setControl(request); + laser.setControl(request); + } + + public void setRequest(ControlRequest request) { + this.request = request; + } + + public Command setSpeed(double speed) { + return runOnce(() -> setRequest(new VelocityVoltage(speed))); + } + + public Command activateIndexer() { + return setSpeed(IndexerConstants.ActiveSpeed); + } + + public Command deactivateIndexer() { + return setSpeed(IndexerConstants.InactiveSpeed); + } + + @Override + + public void setRequest2(ControlRequest request) { + this.request = request; + } + + public Command checkBalsl() { + return runOnce(() -> {hasBall? = laser.get(); /* whatever laser detection is */}); + } + + public Command + + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java index d56ef60..6645e0f 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java @@ -1,6 +1,10 @@ -/* intake -> storage -> indexer */ +package frc.robot.subsystems.indexer; + public class IndexerConstants { - int shooterFuel = 3; /* something that detects the # of fuel in shooter */ - int shooterMax = 5; /* max capacity of shooter */ - int shooterSpace = 0; /* space left */ + + public static final int MOTOR_ID = 0; + public static final int LASER_ID = 0; + public static final boolean ActiveSpeed = 0; + public static final boolean InactiveSpeed = 0; + } \ No newline at end of file From 98843b127cb1e736ae4c994b3a7ecb12597423af Mon Sep 17 00:00:00 2001 From: shakability Date: Tue, 20 Jan 2026 16:17:40 -0800 Subject: [PATCH 036/118] few light changes --- .../frc/robot/subsystems/climb/Climb.java | 22 ++----------------- 1 file changed, 2 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/climb/Climb.java b/src/main/java/frc/robot/subsystems/climb/Climb.java index 1d6f827..002b656 100644 --- a/src/main/java/frc/robot/subsystems/climb/Climb.java +++ b/src/main/java/frc/robot/subsystems/climb/Climb.java @@ -75,8 +75,8 @@ private void setRequest(ControlRequest request) { this.request = request; } - private Command moveToScoringHeight(Heights height) { - return moveToTarget(height.height).withName(height.name() + ": Unsafe, Moving"); + public Command moveToScoringHeight(Heights height) { + return moveToTarget(height.height).withName(height.name() + ": Move To Height"); } /** Attempts to move the end effector to a height, in meters */ @@ -130,24 +130,6 @@ public Command sysId(boolean dynamic, SysIdRoutine.Direction direction) { }); } - public double estimateTimeToTarget(Heights height) { - return estimateTimeToTarget(height.height); - } - - public double estimateTimeToTarget(double targetHeight) { - return Util.trapezoidProfileTimeToTarget( - lastReadHeight, lastReadSpeed, - targetHeight, MAX_SPEED, MAX_ACCEL) - + 0.3; // Jerk offset - } - - public double estimateTimeRemaining() { - return Util.trapezoidProfileTimeToTarget( - lastReadHeight, lastReadSpeed, targetHeight, - MAX_SPEED, MAX_ACCEL) - + 0.16; // Jerk offset - } - /** Checks if the end effector is within 1 cm of the target */ public boolean isNearTarget() { return MathUtil.isNear( From 266fd99429779904141928b264b5d1ff5d54dd53 Mon Sep 17 00:00:00 2001 From: "akhil.space" <145620839+aerospikeengineer@users.noreply.github.com> Date: Tue, 20 Jan 2026 16:22:09 -0800 Subject: [PATCH 037/118] Combined Shooter into one file --- .../subsystems/shooter/BottomShooter.java | 124 ------------------ .../shooter/{TopShooter.java => Shooter.java} | 67 ++++++---- .../subsystems/shooter/ShooterConstants.java | 3 - 3 files changed, 38 insertions(+), 156 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/shooter/BottomShooter.java rename src/main/java/frc/robot/subsystems/shooter/{TopShooter.java => Shooter.java} (69%) diff --git a/src/main/java/frc/robot/subsystems/shooter/BottomShooter.java b/src/main/java/frc/robot/subsystems/shooter/BottomShooter.java deleted file mode 100644 index 4ea3e3b..0000000 --- a/src/main/java/frc/robot/subsystems/shooter/BottomShooter.java +++ /dev/null @@ -1,124 +0,0 @@ -package frc.robot.subsystems.shooter; - -import com.ctre.phoenix6.controls.ControlRequest; -import com.ctre.phoenix6.controls.NeutralOut; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import au.grapplerobotics.LaserCan; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.filter.Debouncer.DebounceType; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.subsystems.TelemetryManager; - -public class BottomShooter extends SubsystemBase { - private static BottomShooter ShooterInstance; - public static BottomShooter getInstance() { - if (ShooterInstance == null) { - ShooterInstance = new BottomShooter(); - } - return ShooterInstance; - } - - private final TalonFX bottomMotor; - - private final LaserCan shooterLaser; - - private final Debouncer shooterDebouncer; - - private boolean inRangeShooter; - - private double lastReadSpeed; - private ControlRequest request = new NeutralOut(); - - private BottomShooter() { - super(); - - bottomMotor = new TalonFX(ShooterConstants.Motors.BOTTOM.id); - - shooterLaser = new LaserCan(ShooterConstants.Lasers.FRONT.id); - - bottomMotor.getConfigurator().apply(ShooterConstants.getConfig()); - bottomMotor.setNeutralMode(NeutralModeValue.Brake); - - shooterDebouncer = new Debouncer(0.06, DebounceType.kBoth); - - inRangeShooter = false; - - TelemetryManager.getInstance().addSendable(this); - } - - @Override - public void periodic() { - // Read inputs - lastReadSpeed = bottomMotor.getVelocity().getValueAsDouble(); - inRangeShooter = shooterDebouncer.calculate(getShooterLaser()); - } - - /** Replaces the request */ - private void setRequest(ControlRequest request) { - this.request = request; - } - - /** Stops the shooter */ - public Command stop() { - return runOnce(() -> setRequest(new NeutralOut())).withName("Stopped"); - } - - /** Gets the intake laser measurement */ - private double getMeasurementShooter() { - var measurement = shooterLaser.getMeasurement(); - if (measurement != null) { - return measurement.distance_mm; - } else { - return Double.POSITIVE_INFINITY; - } - } - - public Command spinUp() - { - ShooterConstants.SHOOT_SPEED_BOTTOM = 25 + ShooterConstants.SHOOT_SPEED_INCREMENT; - return runOnce(() -> setRequest(request) - ).andThen( - stop() - ).withName("Increase Bottom Motor Speed"); - } - - public Command spinDown() - { - ShooterConstants.SHOOT_SPEED_BOTTOM = 25 - ShooterConstants.SHOOT_SPEED_INCREMENT; - return runOnce(() -> setRequest(request) - ).andThen( - stop() - ).withName("Lower Bottom Motor Speed"); - } - - /** Gets whether the shooter laser detects the "fuel" (balls) */ - private boolean getShooterLaser() { - return getMeasurementShooter() < 100; - } - - public Command BottomShoot() { - return runOnce(() -> setRequest( - new VelocityVoltage(ShooterConstants.SHOOT_SPEED_BOTTOM)) - ).andThen( - Commands.waitUntil(() -> !inRangeShooter), - Commands.waitSeconds(0.15), - stop() - ).withName("Bottom Shooting"); - } - - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.addDoubleProperty( - "/Speed", - () -> lastReadSpeed, - null); - TelemetryManager.makeSendableTalonFX("/Bottom", bottomMotor, builder); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/TopShooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java similarity index 69% rename from src/main/java/frc/robot/subsystems/shooter/TopShooter.java rename to src/main/java/frc/robot/subsystems/shooter/Shooter.java index 5d42a39..1db77a4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TopShooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -15,33 +15,41 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.TelemetryManager; -public class TopShooter extends SubsystemBase { - private static TopShooter ShooterInstance; - public static TopShooter getInstance() { +public class Shooter extends SubsystemBase { + private static Shooter ShooterInstance; + public static Shooter getInstance() { if (ShooterInstance == null) { - ShooterInstance = new TopShooter(); + ShooterInstance = new Shooter(); } return ShooterInstance; } private final TalonFX topMotor; + private final TalonFX bottomMotor; + private final LaserCan shooterLaser; private final Debouncer shooterDebouncer; private boolean inRangeShooter; - private double lastReadSpeed; + private double lastReadSpeedTop; + private double lastReadSpeedBottom; private ControlRequest request = new NeutralOut(); - private TopShooter() { + private Shooter() { super(); - topMotor = new TalonFX(ShooterConstants.Motors.TOP.id); + bottomMotor = new TalonFX(ShooterConstants.Motors.BOTTOM.id); shooterLaser = new LaserCan(ShooterConstants.Lasers.FRONT.id); + bottomMotor.getConfigurator().apply(ShooterConstants.getConfig()); + bottomMotor.setNeutralMode(NeutralModeValue.Brake); + + topMotor = new TalonFX(ShooterConstants.Motors.TOP.id); + topMotor.getConfigurator().apply(ShooterConstants.getConfig()); topMotor.setNeutralMode(NeutralModeValue.Brake); @@ -55,7 +63,8 @@ private TopShooter() { @Override public void periodic() { // Read inputs - lastReadSpeed = topMotor.getVelocity().getValueAsDouble(); + lastReadSpeedTop = topMotor.getVelocity().getValueAsDouble(); + lastReadSpeedBottom = bottomMotor.getVelocity().getValueAsDouble(); inRangeShooter = shooterDebouncer.calculate(getShooterLaser()); } @@ -79,32 +88,14 @@ private double getMeasurementShooter() { } } - public Command spinUp() - { - ShooterConstants.SHOOT_SPEED_TOP = 25 + ShooterConstants.SHOOT_SPEED_INCREMENT; - return runOnce(() -> setRequest(request) - ).andThen( - stop() - ).withName("Increase Top Motor Speed"); - } - - public Command spinDown() - { - ShooterConstants.SHOOT_SPEED_TOP = 25 - ShooterConstants.SHOOT_SPEED_INCREMENT; - return runOnce(() -> setRequest(request) - ).andThen( - stop() - ).withName("Lower Top Motor Speed"); - } - /* Gets whether the shooter laser detects the "fuel" (balls) */ private boolean getShooterLaser() { return getMeasurementShooter() < 100; } - public Command topShoot() { + public Command topShoot(int targetSpeed) { return runOnce(() -> setRequest( - new VelocityVoltage(ShooterConstants.SHOOT_SPEED_TOP)) + new VelocityVoltage(targetSpeed)) ).andThen( Commands.waitUntil(() -> !inRangeShooter), Commands.waitSeconds(0.15), @@ -112,13 +103,31 @@ public Command topShoot() { ).withName("Top Shooting"); } + public Command BottomShoot(int targetSpeed) { + return runOnce(() -> setRequest( + new VelocityVoltage(targetSpeed)) + ).andThen( + Commands.waitUntil(() -> !inRangeShooter), + Commands.waitSeconds(0.15), + stop() + ).withName("Bottom Shooting"); + } + @Override public void initSendable(SendableBuilder builder) { super.initSendable(builder); + builder.addDoubleProperty( "/Speed", - () -> lastReadSpeed, + () -> lastReadSpeedTop, null); TelemetryManager.makeSendableTalonFX("/Top", topMotor, builder); + + builder.addDoubleProperty( + "/Speed", + () -> lastReadSpeedBottom, + null); + TelemetryManager.makeSendableTalonFX("/Bottom", bottomMotor, builder); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index ad84a91..cad087f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -6,9 +6,6 @@ import com.ctre.phoenix6.configs.VoltageConfigs; public final class ShooterConstants { - public static double SHOOT_SPEED_TOP = 25; // rotations per second - public static double SHOOT_SPEED_BOTTOM = 25; // rotations per second - public static final double SHOOT_SPEED_INCREMENT = 5; // rotation per second /** Motor ids */ From 84376c524eac3552379a68133a6a2a4f6936a0ca Mon Sep 17 00:00:00 2001 From: "akhil.space" <145620839+aerospikeengineer@users.noreply.github.com> Date: Tue, 20 Jan 2026 16:29:47 -0800 Subject: [PATCH 038/118] Commented out Laser logic We don't need laser right now --- .../frc/robot/subsystems/shooter/Shooter.java | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 1db77a4..4353746 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -6,7 +6,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; -import au.grapplerobotics.LaserCan; +//import au.grapplerobotics.LaserCan; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.util.sendable.SendableBuilder; @@ -28,7 +28,7 @@ public static Shooter getInstance() { private final TalonFX bottomMotor; - private final LaserCan shooterLaser; + //private final LaserCan shooterLaser; private final Debouncer shooterDebouncer; @@ -43,7 +43,7 @@ private Shooter() { bottomMotor = new TalonFX(ShooterConstants.Motors.BOTTOM.id); - shooterLaser = new LaserCan(ShooterConstants.Lasers.FRONT.id); + //shooterLaser = new LaserCan(ShooterConstants.Lasers.FRONT.id); bottomMotor.getConfigurator().apply(ShooterConstants.getConfig()); bottomMotor.setNeutralMode(NeutralModeValue.Brake); @@ -55,7 +55,7 @@ private Shooter() { shooterDebouncer = new Debouncer(0.06, DebounceType.kBoth); - inRangeShooter = false; + //inRangeShooter = false; TelemetryManager.getInstance().addSendable(this); } @@ -65,7 +65,7 @@ public void periodic() { // Read inputs lastReadSpeedTop = topMotor.getVelocity().getValueAsDouble(); lastReadSpeedBottom = bottomMotor.getVelocity().getValueAsDouble(); - inRangeShooter = shooterDebouncer.calculate(getShooterLaser()); + //inRangeShooter = shooterDebouncer.calculate(getShooterLaser()); } /** Replaces the request */ @@ -78,7 +78,8 @@ public Command stop() { return runOnce(() -> setRequest(new NeutralOut())).withName("Stopped"); } - /** Gets the intake laser measurement */ + /** Gets the shooter laser measurement (Not needed as of now) */ + /* private double getMeasurementShooter() { var measurement = shooterLaser.getMeasurement(); if (measurement != null) { @@ -87,11 +88,13 @@ private double getMeasurementShooter() { return Double.POSITIVE_INFINITY; } } + */ - /* Gets whether the shooter laser detects the "fuel" (balls) */ + /* Gets whether the shooter laser detects the "fuel" (balls) not needed right now private boolean getShooterLaser() { return getMeasurementShooter() < 100; } + */ public Command topShoot(int targetSpeed) { return runOnce(() -> setRequest( From f80a160ee6a9e70b5f23d1daaf27ac06cf4bbe07 Mon Sep 17 00:00:00 2001 From: seaspace1250 <117596196+seaspace1250@users.noreply.github.com> Date: Tue, 20 Jan 2026 16:40:12 -0800 Subject: [PATCH 039/118] added current bar position and wheel velocity --- .../frc/robot/subsystems/intake/Intake.java | 41 +++++++++---------- 1 file changed, 20 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 654c8a8..36d4679 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -29,47 +29,45 @@ public static Intake getInstance() { return intakeInstance; } - private final TalonFX WheelMotor; - private final TalonFX BarMotor; - - //private final Debouncer intakeDebouncer; + private final TalonFX wheelMotor; + private final TalonFX barMotor; + private double wheelSpeed; + private double barPosition; private ControlRequest wheelRequest = new NeutralOut(); private ControlRequest barRequest = new NeutralOut(); - //private boolean inRangeIntake; - private Intake() { super(); - WheelMotor = new TalonFX(IntakeConstants.Motors.WHEEL.id); - BarMotor = new TalonFX(IntakeConstants.Motors.BAR.id); - WheelMotor.getConfigurator().apply(IntakeConstants.getWheelConfig()); - BarMotor.getConfigurator().apply(IntakeConstants.getBarConfig()); - WheelMotor.setNeutralMode(NeutralModeValue.Brake); - BarMotor.setNeutralMode(NeutralModeValue.Brake); //? - - TelemetryManager.getInstance().addSendable(this); - //setDefaultCommand(); + wheelMotor = new TalonFX(IntakeConstants.Motors.WHEEL.id); + barMotor = new TalonFX(IntakeConstants.Motors.BAR.id); + wheelMotor.getConfigurator().apply(IntakeConstants.getWheelConfig()); + barMotor.getConfigurator().apply(IntakeConstants.getBarConfig()); + wheelMotor.setNeutralMode(NeutralModeValue.Brake); + barMotor.setNeutralMode(NeutralModeValue.Brake); //? + TelemetryManager.getInstance().addSendable(this); } @Override public void periodic(){ - // - - BarMotor.setControl(barRequest); - WheelMotor.setControl(wheelRequest); + wheelSpeed = wheelMotor.getVelocity().getValueAsDouble(); + barPosition = barMotor.getPosition().getValueAsDouble(); + barMotor.setControl(barRequest); + wheelMotor.setControl(wheelRequest); } + //---------------stop---------------- public Command stopWheel() { return runOnce(() -> setRequestWheel(new NeutralOut())).withName("Stopped"); } public Command stopBar() { - return runOnce(() -> setRequestBar(new NeutralOut())).withName("Stopped"); // should be voltage for upright bar? + return runOnce(() -> setRequestBar(new PositionVoltage(barPosition))).withName("Stopped"); //needs testing } + //----------------set request--------------- private void setRequestWheel(ControlRequest request) { this.wheelRequest = request; } @@ -78,6 +76,7 @@ private void setRequestBar(ControlRequest request) { this.barRequest = request; } + //----------------wheel---------------- public Command setWheelIntaking() { return setWheelSpeed(IntakeConstants.INTAKE_SPEED); } @@ -93,7 +92,7 @@ public Command setWheelSpeed(double speed) { } - //---------BAR----------- + //---------bar----------- public Command setBarDown() { From 46bb9fdba3dfe89b4a1d276d8f14f9bd3061e6db Mon Sep 17 00:00:00 2001 From: Ethan Lian Date: Tue, 20 Jan 2026 17:30:59 -0800 Subject: [PATCH 040/118] L rizz fixes --- .../frc/robot/subsystems/indexer/Indexer.java | 78 +++++++++++-------- .../subsystems/indexer/IndexerConstants.java | 5 +- .../subsystems/indexer/IndexerLaser.java | 0 vendordeps/libgrapplefrc2025.json | 72 +++++++++++++++++ 4 files changed, 121 insertions(+), 34 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/indexer/IndexerLaser.java create mode 100644 vendordeps/libgrapplefrc2025.json diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 446ea70..3351576 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -1,67 +1,81 @@ package frc.robot.subsystems.indexer; -import static frc.robot.subsystems.indexer.IndexerConstants.*; - -import java.util.ResourceBundle.Control; - import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import au.grapplerobotics.LaserCan; +import au.grapplerobotics.ConfigurationFailedException; -public class Indexer extends SubsystemBase{ +public class Indexer extends SubsystemBase { private ControlRequest request; private TalonFX motor; - private SomeBrand laser; // fix later + private LaserCan lc; + + private double getDistanceMm() { + LaserCan.Measurement measurement = lc.getMeasurement(); + if (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) { + return measurement.distance_mm; + } else { + return -1; + } + } - private boolean ballz = false; + private boolean hasBall; + + private boolean hasBall() { + return hasBall; + } + + private void checkForBall() { + double x = getDistanceMm(); + if (x == IndexerConstants.LaserCan_DefaultMeasurement) { + boolean hasBall = false; + } else { + boolean hasBall = true; + } + } private Indexer() { motor = new TalonFX(IndexerConstants.MOTOR_ID); - } - private void IndexerSensor() { - laser = new SomeBrand(IndexerConstants.LASER_ID); + lc = new LaserCan(IndexerConstants.LASER_ID); + try { + lc.setRangingMode(LaserCan.RangingMode.SHORT); + lc.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16)); + lc.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); + } catch (ConfigurationFailedException e) { + System.out.println("Configuration failed! " + e); + } } @Override public void periodic() { motor.setControl(request); - laser.setControl(request); - } + checkForBall(); + }; public void setRequest(ControlRequest request) { this.request = request; - } + }; - public Command setSpeed(double speed) { + public Command setSpeed(double speed) { return runOnce(() -> setRequest(new VelocityVoltage(speed))); - } + }; public Command activateIndexer() { - return setSpeed(IndexerConstants.ActiveSpeed); + return Commands.either ( + setSpeed(IndexerConstants.ActiveSpeed), + setSpeed(IndexerConstants.InactiveSpeed), + this::hasBall + ); } public Command deactivateIndexer() { return setSpeed(IndexerConstants.InactiveSpeed); - } - - @Override - - public void setRequest2(ControlRequest request) { - this.request = request; - } - - public Command checkBalsl() { - return runOnce(() -> {hasBall? = laser.get(); /* whatever laser detection is */}); - } - - public Command - - + }; } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java index 6645e0f..e7f5641 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java @@ -4,7 +4,8 @@ public class IndexerConstants { public static final int MOTOR_ID = 0; public static final int LASER_ID = 0; - public static final boolean ActiveSpeed = 0; - public static final boolean InactiveSpeed = 0; + public static final double ActiveSpeed = 0; + public static final double InactiveSpeed = 0; + public static final double LaserCan_DefaultMeasurement = 1; } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerLaser.java b/src/main/java/frc/robot/subsystems/indexer/IndexerLaser.java new file mode 100644 index 0000000..e69de29 diff --git a/vendordeps/libgrapplefrc2025.json b/vendordeps/libgrapplefrc2025.json new file mode 100644 index 0000000..e450f4e --- /dev/null +++ b/vendordeps/libgrapplefrc2025.json @@ -0,0 +1,72 @@ +{ + "fileName": "libgrapplefrc2025.json", + "name": "libgrapplefrc", + "version": "2025.0.8", + "frcYear": "2025", + "uuid": "8ef3423d-9532-4665-8339-206dae1d7168", + "mavenUrls": [ + "https://storage.googleapis.com/grapple-frc-maven" + ], + "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2025.json", + "javaDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcjava", + "version": "2025.0.8" + } + ], + "jniDependencies": [ + + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2025.0.8", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrccpp", + "version": "2025.0.8", + "libName": "grapplefrc", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2025.0.8", + "libName": "grapplefrcdriver", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file From 2ae95928e67be27a14127ed78d08c24e2de8e203 Mon Sep 17 00:00:00 2001 From: seaspace1250 <117596196+seaspace1250@users.noreply.github.com> Date: Tue, 20 Jan 2026 18:48:37 -0800 Subject: [PATCH 041/118] gave names to setbarpos and setwheelspeed --- src/main/java/frc/robot/subsystems/intake/Intake.java | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 36d4679..a9378e9 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -26,7 +26,7 @@ public static Intake getInstance() { if (intakeInstance == null) { intakeInstance = new Intake(); } - return intakeInstance; + return intakeInstance; } private final TalonFX wheelMotor; @@ -88,7 +88,7 @@ public Command setWheelStop() { public Command setWheelSpeed(double speed) { return runOnce(() -> setRequestWheel ( new VelocityVoltage(speed)) - ).withName(""); + ).withName("wheel speed set"); } @@ -107,8 +107,6 @@ public Command setBarPosition(double position) { double checkedPos = MathUtil.clamp(position, IntakeConstants.BAR_POS_MIN, IntakeConstants.BAR_POS_MAX); return runOnce(() -> setRequestBar( new PositionVoltage(checkedPos)) - ).withName(""); + ).withName("bar pos set"); } - - } \ No newline at end of file From b1cee636a463c8cfeaa0390aea79973421fe9e76 Mon Sep 17 00:00:00 2001 From: Ethan Lian Date: Wed, 21 Jan 2026 15:36:06 -0800 Subject: [PATCH 042/118] mid rizz fixes --- .../frc/robot/subsystems/indexer/Indexer.java | 22 +++++++++---------- .../subsystems/indexer/IndexerLaser.java | 0 2 files changed, 11 insertions(+), 11 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/indexer/IndexerLaser.java diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 3351576..22a4f3d 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -16,7 +16,7 @@ public class Indexer extends SubsystemBase { private TalonFX motor; private LaserCan lc; - private double getDistanceMm() { + private double getDistanceMm() /* command to sense distance from camera */ { LaserCan.Measurement measurement = lc.getMeasurement(); if (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) { return measurement.distance_mm; @@ -25,13 +25,13 @@ private double getDistanceMm() { } } - private boolean hasBall; + private boolean hasBall; /* boolean that's modified by checkForBall() */ - private boolean hasBall() { + private boolean hasBall() /* setup formatting for boolean hasBall so it can be used in activateIndexer().either */ { return hasBall; } - private void checkForBall() { + private void checkForBall() /* command that modifies hasBall */ { double x = getDistanceMm(); if (x == IndexerConstants.LaserCan_DefaultMeasurement) { boolean hasBall = false; @@ -40,7 +40,7 @@ private void checkForBall() { } } - private Indexer() { + private Indexer() /* setup, adding motor and laser */ { motor = new TalonFX(IndexerConstants.MOTOR_ID); lc = new LaserCan(IndexerConstants.LASER_ID); @@ -54,18 +54,18 @@ private Indexer() { } @Override - public void periodic() { + public void periodic() /* check for balls and makes sure motor is constantly running at desired speed */ { motor.setControl(request); checkForBall(); - }; + } - public void setRequest(ControlRequest request) { + public void setRequest(ControlRequest request) /* type conversion/abstraction */ { this.request = request; - }; + } public Command setSpeed(double speed) { return runOnce(() -> setRequest(new VelocityVoltage(speed))); - }; + } public Command activateIndexer() { return Commands.either ( @@ -77,5 +77,5 @@ public Command activateIndexer() { public Command deactivateIndexer() { return setSpeed(IndexerConstants.InactiveSpeed); - }; + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerLaser.java b/src/main/java/frc/robot/subsystems/indexer/IndexerLaser.java deleted file mode 100644 index e69de29..0000000 From db1b6d83c4f2eeb64562bc2e382ca063c9a08db9 Mon Sep 17 00:00:00 2001 From: seaspace1250 <117596196+seaspace1250@users.noreply.github.com> Date: Mon, 26 Jan 2026 16:04:26 -0800 Subject: [PATCH 043/118] gave names to commands --- src/main/java/frc/robot/subsystems/intake/Intake.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index a9378e9..897bab6 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -88,7 +88,7 @@ public Command setWheelStop() { public Command setWheelSpeed(double speed) { return runOnce(() -> setRequestWheel ( new VelocityVoltage(speed)) - ).withName("wheel speed set"); + ).withName("wheel speed set " + (speed)); } @@ -107,6 +107,6 @@ public Command setBarPosition(double position) { double checkedPos = MathUtil.clamp(position, IntakeConstants.BAR_POS_MIN, IntakeConstants.BAR_POS_MAX); return runOnce(() -> setRequestBar( new PositionVoltage(checkedPos)) - ).withName("bar pos set"); + ).withName("bar pos set" + (checkedPos)); } } \ No newline at end of file From 6a1ec2c7f7da3de479173d4351e26204da83a99e Mon Sep 17 00:00:00 2001 From: Ethan Lian Date: Mon, 26 Jan 2026 17:16:12 -0800 Subject: [PATCH 044/118] Merging 2025 to 2026 wpi --- ...applefrc2025.json => libgrapplefrc2026.json} | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) rename vendordeps/{libgrapplefrc2025.json => libgrapplefrc2026.json} (87%) diff --git a/vendordeps/libgrapplefrc2025.json b/vendordeps/libgrapplefrc2026.json similarity index 87% rename from vendordeps/libgrapplefrc2025.json rename to vendordeps/libgrapplefrc2026.json index e450f4e..2f620b5 100644 --- a/vendordeps/libgrapplefrc2025.json +++ b/vendordeps/libgrapplefrc2026.json @@ -1,26 +1,25 @@ { - "fileName": "libgrapplefrc2025.json", + "fileName": "libgrapplefrc2026.json", "name": "libgrapplefrc", - "version": "2025.0.8", - "frcYear": "2025", + "version": "2026.0.0", + "frcYear": "2026", "uuid": "8ef3423d-9532-4665-8339-206dae1d7168", "mavenUrls": [ "https://storage.googleapis.com/grapple-frc-maven" ], - "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2025.json", + "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2026.json", "javaDependencies": [ { "groupId": "au.grapplerobotics", "artifactId": "libgrapplefrcjava", - "version": "2025.0.8" + "version": "2026.0.0" } ], "jniDependencies": [ - { "groupId": "au.grapplerobotics", "artifactId": "libgrapplefrcdriver", - "version": "2025.0.8", + "version": "2026.0.0", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +36,7 @@ { "groupId": "au.grapplerobotics", "artifactId": "libgrapplefrccpp", - "version": "2025.0.8", + "version": "2026.0.0", "libName": "grapplefrc", "headerClassifier": "headers", "sharedLibrary": true, @@ -54,7 +53,7 @@ { "groupId": "au.grapplerobotics", "artifactId": "libgrapplefrcdriver", - "version": "2025.0.8", + "version": "2026.0.0", "libName": "grapplefrcdriver", "headerClassifier": "headers", "sharedLibrary": true, From f1d3e726bb05f3729bbececf8736c59473391a0d Mon Sep 17 00:00:00 2001 From: audreywtu Date: Mon, 26 Jan 2026 17:17:27 -0800 Subject: [PATCH 045/118] nudge --- .../frc/robot/lib/control/Nudge copy.java | 42 ++++++++ .../java/frc/robot/lib/control/Nudge.java | 100 ++++++++++++++++++ 2 files changed, 142 insertions(+) create mode 100644 src/main/java/frc/robot/lib/control/Nudge copy.java create mode 100644 src/main/java/frc/robot/lib/control/Nudge.java diff --git a/src/main/java/frc/robot/lib/control/Nudge copy.java b/src/main/java/frc/robot/lib/control/Nudge copy.java new file mode 100644 index 0000000..a339792 --- /dev/null +++ b/src/main/java/frc/robot/lib/control/Nudge copy.java @@ -0,0 +1,42 @@ +package frc.robot.lib.control; + +import static frc.robot.Robot.controller; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.event.EventLoop; +import frc.robot.autos.modes.TeleopAutoMode; + +public class Controller { //possibly extends [control] + //private TeleopAutoMode mTeleopAutoMode = null; + private SwerveDrive m_SwerveDrive = null; + + private final EventLoop m_loop = new EventLoop(); + + public Controller( + //TeleopAutoMode teleopAutoMode, + ) { + //mTeleopAutoMode = teleopAutoMode; + m_SwerveDrive = SwerveDrive.getInstance(); + } + + public void processKeyCommand() { + /*if (mTeleopAutoMode == null) + return;*/ + + m_loop.poll(); + + if (controller.getPOV() == 90) { + m_SwerveDrive.feedTeleopSetpoint(new ChassisSpeeds( + 0, -0.4, 0)); + } else if (controller.getPOV() == 0) { + m_SwerveDrive.feedTeleopSetpoint(new ChassisSpeeds( + 0.4, 0, 0)); + } else if (controller.getPOV() == 270) { + m_SwerveDrive.feedTeleopSetpoint(new ChassisSpeeds( + 0, 0.4, 0)); + } else if (controller.getPOV() == 180) { + m_SwerveDrive.feedTeleopSetpoint(new ChassisSpeeds( + -0.4, 0, 0)); + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/lib/control/Nudge.java b/src/main/java/frc/robot/lib/control/Nudge.java new file mode 100644 index 0000000..d5a2b70 --- /dev/null +++ b/src/main/java/frc/robot/lib/control/Nudge.java @@ -0,0 +1,100 @@ +package frc.robot.lib.control; +/* +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; */ + +import static frc.robot.Robot.controller; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +//import edu.wpi.first.math.trajectory.constraint.MaxVelocityConstraint; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.event.EventLoop; +import frc.robot.Constants; //62 - 67 +//import frc.robot.FieldLayout; +import frc.robot.RobotState; //97 +//import frc.robot.autos.actions.*; +import frc.robot.autos.modes.TeleopAutoMode; //27, 43, 47, 57-59 +import frc.robot.lib.util.Util; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +public class Controller { //possibly extends [control] + private XboxController mXboxController1 = null; // private final instead, port num + private TeleopAutoMode mTeleopAutoMode = null; + private Joystick m_JoyStick = null; + private SwerveDrive m_SwerveDrive = null; + + private final EventLoop m_loop = new EventLoop(); + + private boolean isFieldRelative = true; + + private final int translationAxis = XboxController.Axis.kLeftY.value; + private final int strafeAxis = XboxController.Axis.kLeftX.value; + private final int rotationAxis = XboxController.Axis.kRightX.value; + + public Controller( + XboxController xboxController1, + TeleopAutoMode teleopAutoMode, + Joystick joystick + ) { + controller = xboxController1; + mTeleopAutoMode = teleopAutoMode; + m_JoyStick = joystick; + m_SwerveDrive = SwerveDrive.getInstance(); + + controller.start(m_loop).ifHigh( + () -> isFieldRelative = !isFieldRelative + ); + } + + public void processKeyCommand() { + if (mTeleopAutoMode == null) + return; + + m_loop.poll(); + + double translationVal = -MathUtil.applyDeadband(m_JoyStick.getRawAxis(translationAxis), Constants.stickDeadband) + * Constants.SwerveConstants.maxSpeed; + double strafeVal = -MathUtil.applyDeadband(m_JoyStick.getRawAxis(strafeAxis), Constants.stickDeadband) + * Constants.SwerveConstants.maxSpeed; + double rotationVal = -MathUtil.applyDeadband(m_JoyStick.getRawAxis(rotationAxis), Constants.stickDeadband) + * Constants.Swerve.maxAngularVelocity; + + if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red) { + translationVal = -translationVal; + strafeVal = -strafeVal; + } + + if (controller.getPOV() == 90) { + m_SwerveDrive.feedTeleopSetpoint(new ChassisSpeeds( + 0, -0.4, 0)); + } else if (controller.getPOV() == 0) { + m_SwerveDrive.feedTeleopSetpoint(new ChassisSpeeds( + 0.4, 0, 0)); + } else if (controller.getPOV() == 270) { + m_SwerveDrive.feedTeleopSetpoint(new ChassisSpeeds( + 0, 0.4, 0)); + } else if (controller.getPOV() == 180) { + m_SwerveDrive.feedTeleopSetpoint(new ChassisSpeeds( + -0.4, 0, 0)); + } else { + if (isFieldRelative) { + m_SwerveDrive.feedTeleopSetpoint(ChassisSpeeds.fromFieldRelativeSpeeds( + translationVal, strafeVal, rotationVal, + Util.robotToFieldRelative(m_SwerveDrive.getHeading(), DriverStation.getAlliance().get() == Alliance.Red))); + } else { + m_SwerveDrive.feedTeleopSetpoint(new ChassisSpeeds( + translationVal, strafeVal, rotationVal)); + } + } + + Twist2d velocity = RobotState.getInstance().getMeasuredVelocity(); + + } +} \ No newline at end of file From 4eff1106a4011fd9976d706b611f6e366a21bfd8 Mon Sep 17 00:00:00 2001 From: Ethan Lian Date: Mon, 26 Jan 2026 17:21:37 -0800 Subject: [PATCH 046/118] importing changes + updating laser --- .../frc/robot/subsystems/indexer/Indexer.java | 76 ++++++++++++------- 1 file changed, 48 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 22a4f3d..331f899 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -3,10 +3,11 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; - +import frc.robot.subsystems.TelemetryManager; import au.grapplerobotics.LaserCan; import au.grapplerobotics.ConfigurationFailedException; @@ -16,34 +17,16 @@ public class Indexer extends SubsystemBase { private TalonFX motor; private LaserCan lc; - private double getDistanceMm() /* command to sense distance from camera */ { - LaserCan.Measurement measurement = lc.getMeasurement(); - if (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) { - return measurement.distance_mm; - } else { - return -1; - } - } - - private boolean hasBall; /* boolean that's modified by checkForBall() */ - - private boolean hasBall() /* setup formatting for boolean hasBall so it can be used in activateIndexer().either */ { - return hasBall; - } - - private void checkForBall() /* command that modifies hasBall */ { - double x = getDistanceMm(); - if (x == IndexerConstants.LaserCan_DefaultMeasurement) { - boolean hasBall = false; - } else { - boolean hasBall = true; - } - } + /** boolean that's modified by checkForBall() */ + private boolean hasBall; - private Indexer() /* setup, adding motor and laser */ { + /** setup, adding motor and laser */ + private Indexer() { + super(); motor = new TalonFX(IndexerConstants.MOTOR_ID); - lc = new LaserCan(IndexerConstants.LASER_ID); + + /* new laser configs */ try { lc.setRangingMode(LaserCan.RangingMode.SHORT); lc.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16)); @@ -54,19 +37,23 @@ private Indexer() /* setup, adding motor and laser */ { } @Override - public void periodic() /* check for balls and makes sure motor is constantly running at desired speed */ { + /* check for balls and makes sure motor is constantly running at desired speed */ + public void periodic() { motor.setControl(request); checkForBall(); } - public void setRequest(ControlRequest request) /* type conversion/abstraction */ { + /** type conversion/abstraction */ + public void setRequest(ControlRequest request) { this.request = request; } + /** sets speed (duh) */ public Command setSpeed(double speed) { return runOnce(() -> setRequest(new VelocityVoltage(speed))); } + /** moves motor to speed if sense ball */ public Command activateIndexer() { return Commands.either ( setSpeed(IndexerConstants.ActiveSpeed), @@ -75,7 +62,40 @@ public Command activateIndexer() { ); } + /** turn motor down to zero */ public Command deactivateIndexer() { return setSpeed(IndexerConstants.InactiveSpeed); } + + /** command to sense distance from camera; used to sense if bol */ + private double getDistanceMm() { + LaserCan.Measurement measurement = lc.getMeasurement(); + if (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) { + return measurement.distance_mm; + } else { + return -1; + } + } + + /** setup formatting for boolean hasBall so it can be used in activateIndexer().either */ + private boolean hasBall() { + return hasBall; + } + + /** command that modifies hasBall, uses getDistanceMm() */ + private void checkForBall() { + double x = getDistanceMm(); + if (x == IndexerConstants.LaserCan_DefaultMeasurement) { + hasBall = false; + } else { + hasBall = true; + } + } + + @Override + public void initSendable(SendableBuilder builder) { + super.initSendable(builder); + builder.addBooleanProperty("Has Ball", () -> hasBall, null); + TelemetryManager.makeSendableTalonFX("Indexer motor", motor, builder); + } } \ No newline at end of file From ab79a4cde1f7811f9b1cda55967b7105393cf5f3 Mon Sep 17 00:00:00 2001 From: seaspace1250 <117596196+seaspace1250@users.noreply.github.com> Date: Mon, 26 Jan 2026 17:29:39 -0800 Subject: [PATCH 047/118] added documentation for commands --- .../frc/robot/subsystems/intake/Intake.java | 34 ++++++++++++++++--- 1 file changed, 30 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 897bab6..4f7969f 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -59,10 +59,18 @@ public void periodic(){ } //---------------stop---------------- + + /** + * sends neutral request to wheel + */ + public Command stopWheel() { return runOnce(() -> setRequestWheel(new NeutralOut())).withName("Stopped"); } + /** + * sends position voltage request that attempts to keep bar in place + */ public Command stopBar() { return runOnce(() -> setRequestBar(new PositionVoltage(barPosition))).withName("Stopped"); //needs testing } @@ -77,13 +85,19 @@ private void setRequestBar(ControlRequest request) { } //----------------wheel---------------- + + /** + * sets wheel speed to intake speed + */ + public Command setWheelIntaking() { return setWheelSpeed(IntakeConstants.INTAKE_SPEED); } - public Command setWheelStop() { - return runOnce(() -> setRequestWheel(new NeutralOut())); - } + /** + * sets the wheel speed (in rotations per second) + * @return + */ public Command setWheelSpeed(double speed) { return runOnce(() -> setRequestWheel ( @@ -94,15 +108,27 @@ public Command setWheelSpeed(double speed) { //---------bar----------- - + /** + * sets request to the bar down voltage + * @return + */ public Command setBarDown() { return setBarPosition(IntakeConstants.BAR_POSITION_DOWN); } + /** + * sets request to the bar up voltage + * @return + */ public Command setBarUp() { return setBarPosition(IntakeConstants.BAR_POSITION_UP); } + /** + * sets position request within limits and then requests that position (in rotations) + * @param position + * @return + */ public Command setBarPosition(double position) { double checkedPos = MathUtil.clamp(position, IntakeConstants.BAR_POS_MIN, IntakeConstants.BAR_POS_MAX); return runOnce(() -> setRequestBar( From 76a650587f1f8619c033319af2076f8921f8b48d Mon Sep 17 00:00:00 2001 From: seaspace1250 <117596196+seaspace1250@users.noreply.github.com> Date: Mon, 26 Jan 2026 17:30:51 -0800 Subject: [PATCH 048/118] very minor format change --- src/main/java/frc/robot/subsystems/intake/Intake.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 4f7969f..a7e4b10 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -72,7 +72,8 @@ public Command stopWheel() { * sends position voltage request that attempts to keep bar in place */ public Command stopBar() { - return runOnce(() -> setRequestBar(new PositionVoltage(barPosition))).withName("Stopped"); //needs testing + return runOnce(() -> setRequestBar(new PositionVoltage(barPosition))) + .withName("Stopped"); //needs testing } //----------------set request--------------- From 3e86ff116757d87edf863d99b9a2c921c2b76172 Mon Sep 17 00:00:00 2001 From: audreywtu Date: Mon, 26 Jan 2026 17:56:43 -0800 Subject: [PATCH 049/118] nudge revised --- src/main/java/frc/robot/ControlsMapping.java | 2 + .../frc/robot/lib/control/Nudge copy.java | 42 -------- .../java/frc/robot/lib/control/Nudge.java | 100 ------------------ .../frc/robot/subsystems/drive/Drive.java | 26 +++++ 4 files changed, 28 insertions(+), 142 deletions(-) delete mode 100644 src/main/java/frc/robot/lib/control/Nudge copy.java delete mode 100644 src/main/java/frc/robot/lib/control/Nudge.java diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index e40c703..a71b7ef 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -9,6 +9,7 @@ import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.ctre.CtreDrive.SysIdRoutineType; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; public class ControlsMapping { public static void mapTeleopCommand() { @@ -21,6 +22,7 @@ public static void mapTeleopCommand() { controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false)); controller.x().whileTrue(Drive.getInstance().autopilotAlign(true)); controller.y().whileTrue(Drive.getInstance().autopilotAlign(false)); + new Trigger(() -> controller.getHID().getPOV() != -1).whileTrue(Drive.getInstance().nudgeCommand()); } public static void mapSysId() { diff --git a/src/main/java/frc/robot/lib/control/Nudge copy.java b/src/main/java/frc/robot/lib/control/Nudge copy.java deleted file mode 100644 index a339792..0000000 --- a/src/main/java/frc/robot/lib/control/Nudge copy.java +++ /dev/null @@ -1,42 +0,0 @@ -package frc.robot.lib.control; - -import static frc.robot.Robot.controller; - -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.event.EventLoop; -import frc.robot.autos.modes.TeleopAutoMode; - -public class Controller { //possibly extends [control] - //private TeleopAutoMode mTeleopAutoMode = null; - private SwerveDrive m_SwerveDrive = null; - - private final EventLoop m_loop = new EventLoop(); - - public Controller( - //TeleopAutoMode teleopAutoMode, - ) { - //mTeleopAutoMode = teleopAutoMode; - m_SwerveDrive = SwerveDrive.getInstance(); - } - - public void processKeyCommand() { - /*if (mTeleopAutoMode == null) - return;*/ - - m_loop.poll(); - - if (controller.getPOV() == 90) { - m_SwerveDrive.feedTeleopSetpoint(new ChassisSpeeds( - 0, -0.4, 0)); - } else if (controller.getPOV() == 0) { - m_SwerveDrive.feedTeleopSetpoint(new ChassisSpeeds( - 0.4, 0, 0)); - } else if (controller.getPOV() == 270) { - m_SwerveDrive.feedTeleopSetpoint(new ChassisSpeeds( - 0, 0.4, 0)); - } else if (controller.getPOV() == 180) { - m_SwerveDrive.feedTeleopSetpoint(new ChassisSpeeds( - -0.4, 0, 0)); - } - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/lib/control/Nudge.java b/src/main/java/frc/robot/lib/control/Nudge.java deleted file mode 100644 index d5a2b70..0000000 --- a/src/main/java/frc/robot/lib/control/Nudge.java +++ /dev/null @@ -1,100 +0,0 @@ -package frc.robot.lib.control; -/* -import java.util.ArrayList; -import java.util.Arrays; -import java.util.List; */ - -import static frc.robot.Robot.controller; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -//import edu.wpi.first.math.trajectory.constraint.MaxVelocityConstraint; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.event.EventLoop; -import frc.robot.Constants; //62 - 67 -//import frc.robot.FieldLayout; -import frc.robot.RobotState; //97 -//import frc.robot.autos.actions.*; -import frc.robot.autos.modes.TeleopAutoMode; //27, 43, 47, 57-59 -import frc.robot.lib.util.Util; - -import edu.wpi.first.math.kinematics.ChassisSpeeds; - -public class Controller { //possibly extends [control] - private XboxController mXboxController1 = null; // private final instead, port num - private TeleopAutoMode mTeleopAutoMode = null; - private Joystick m_JoyStick = null; - private SwerveDrive m_SwerveDrive = null; - - private final EventLoop m_loop = new EventLoop(); - - private boolean isFieldRelative = true; - - private final int translationAxis = XboxController.Axis.kLeftY.value; - private final int strafeAxis = XboxController.Axis.kLeftX.value; - private final int rotationAxis = XboxController.Axis.kRightX.value; - - public Controller( - XboxController xboxController1, - TeleopAutoMode teleopAutoMode, - Joystick joystick - ) { - controller = xboxController1; - mTeleopAutoMode = teleopAutoMode; - m_JoyStick = joystick; - m_SwerveDrive = SwerveDrive.getInstance(); - - controller.start(m_loop).ifHigh( - () -> isFieldRelative = !isFieldRelative - ); - } - - public void processKeyCommand() { - if (mTeleopAutoMode == null) - return; - - m_loop.poll(); - - double translationVal = -MathUtil.applyDeadband(m_JoyStick.getRawAxis(translationAxis), Constants.stickDeadband) - * Constants.SwerveConstants.maxSpeed; - double strafeVal = -MathUtil.applyDeadband(m_JoyStick.getRawAxis(strafeAxis), Constants.stickDeadband) - * Constants.SwerveConstants.maxSpeed; - double rotationVal = -MathUtil.applyDeadband(m_JoyStick.getRawAxis(rotationAxis), Constants.stickDeadband) - * Constants.Swerve.maxAngularVelocity; - - if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red) { - translationVal = -translationVal; - strafeVal = -strafeVal; - } - - if (controller.getPOV() == 90) { - m_SwerveDrive.feedTeleopSetpoint(new ChassisSpeeds( - 0, -0.4, 0)); - } else if (controller.getPOV() == 0) { - m_SwerveDrive.feedTeleopSetpoint(new ChassisSpeeds( - 0.4, 0, 0)); - } else if (controller.getPOV() == 270) { - m_SwerveDrive.feedTeleopSetpoint(new ChassisSpeeds( - 0, 0.4, 0)); - } else if (controller.getPOV() == 180) { - m_SwerveDrive.feedTeleopSetpoint(new ChassisSpeeds( - -0.4, 0, 0)); - } else { - if (isFieldRelative) { - m_SwerveDrive.feedTeleopSetpoint(ChassisSpeeds.fromFieldRelativeSpeeds( - translationVal, strafeVal, rotationVal, - Util.robotToFieldRelative(m_SwerveDrive.getHeading(), DriverStation.getAlliance().get() == Alliance.Red))); - } else { - m_SwerveDrive.feedTeleopSetpoint(new ChassisSpeeds( - translationVal, strafeVal, rotationVal)); - } - } - - Twist2d velocity = RobotState.getInstance().getMeasuredVelocity(); - - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 8c09c32..9edaf33 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -164,6 +164,32 @@ public Command teleopCommand() { }).handleInterrupt(() -> setSwerveRequest(new SwerveRequest.FieldCentric()))).withName("Teleop"); } + public Command nudgeCommand() { + return runOnce(() -> { + teleopRequest.withVelocityX(0).withVelocityY(0).withRotationalRate(0); + setSwerveRequest(teleopRequest); + }).andThen(run(() -> { + int pov = Robot.controller.getHID().getPOV(); + double xDesiredRaw = Math.sin(pov) * Math.PI / 180.0; + double yDesiredRaw = Math.cos(pov) * Math.PI / 180.0; + double rotDesiredRaw = 0; + + double[] xy = Util.applyRadialDeadband(xDesiredRaw, yDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); + double xFancy = xy[0]; + double yFancy = xy[1]; + double rotFancy = Util.applyJoystickDeadband(rotDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); + + SmartDashboard.putNumber("Sticks/vX", xDesiredRaw); + SmartDashboard.putNumber("Sticks/vY", yDesiredRaw); + SmartDashboard.putNumber("Sticks/vW", rotDesiredRaw); + + teleopRequest + .withVelocityX(xFancy * 0.4) + .withVelocityY(yFancy * 0.4) + .withRotationalRate(rotFancy * 0); + }).handleInterrupt(() -> setSwerveRequest(new SwerveRequest.FieldCentric()))).withName("Teleop"); + } + /** * Auto aligns to the nearest reef face * @param left chooses the left or right face From 1db90cca1c5057bd0a72bf919a2953dbba0594e7 Mon Sep 17 00:00:00 2001 From: audreywtu Date: Mon, 26 Jan 2026 17:59:32 -0800 Subject: [PATCH 050/118] trig things --- src/main/java/frc/robot/subsystems/drive/Drive.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 9edaf33..d2238b2 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -170,8 +170,8 @@ public Command nudgeCommand() { setSwerveRequest(teleopRequest); }).andThen(run(() -> { int pov = Robot.controller.getHID().getPOV(); - double xDesiredRaw = Math.sin(pov) * Math.PI / 180.0; - double yDesiredRaw = Math.cos(pov) * Math.PI / 180.0; + double xDesiredRaw = Math.sin(pov * Math.PI / 180.0); + double yDesiredRaw = Math.cos(pov * Math.PI / 180.0); double rotDesiredRaw = 0; double[] xy = Util.applyRadialDeadband(xDesiredRaw, yDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); From fea01aa0a0c8382ef152d5649d9acd69ec185c1d Mon Sep 17 00:00:00 2001 From: audreywtu Date: Mon, 26 Jan 2026 18:06:43 -0800 Subject: [PATCH 051/118] nudge final !! --- src/main/java/frc/robot/subsystems/drive/Drive.java | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index d2238b2..f777ed6 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -170,8 +170,8 @@ public Command nudgeCommand() { setSwerveRequest(teleopRequest); }).andThen(run(() -> { int pov = Robot.controller.getHID().getPOV(); - double xDesiredRaw = Math.sin(pov * Math.PI / 180.0); - double yDesiredRaw = Math.cos(pov * Math.PI / 180.0); + double xDesiredRaw = Math.cos(pov * Math.PI / 180.0); + double yDesiredRaw = - Math.sin(pov * Math.PI / 180.0); double rotDesiredRaw = 0; double[] xy = Util.applyRadialDeadband(xDesiredRaw, yDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); @@ -179,15 +179,11 @@ public Command nudgeCommand() { double yFancy = xy[1]; double rotFancy = Util.applyJoystickDeadband(rotDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); - SmartDashboard.putNumber("Sticks/vX", xDesiredRaw); - SmartDashboard.putNumber("Sticks/vY", yDesiredRaw); - SmartDashboard.putNumber("Sticks/vW", rotDesiredRaw); - teleopRequest .withVelocityX(xFancy * 0.4) .withVelocityY(yFancy * 0.4) .withRotationalRate(rotFancy * 0); - }).handleInterrupt(() -> setSwerveRequest(new SwerveRequest.FieldCentric()))).withName("Teleop"); + }).handleInterrupt(() -> setSwerveRequest(new SwerveRequest.FieldCentric()))).withName("Nudge"); } /** From 360b99131ed48a46247e8c862257c57623d6d8ad Mon Sep 17 00:00:00 2001 From: audreywtu Date: Mon, 26 Jan 2026 18:20:34 -0800 Subject: [PATCH 052/118] removed rotation --- src/main/java/frc/robot/subsystems/drive/Drive.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index f777ed6..44d4a10 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -172,7 +172,6 @@ public Command nudgeCommand() { int pov = Robot.controller.getHID().getPOV(); double xDesiredRaw = Math.cos(pov * Math.PI / 180.0); double yDesiredRaw = - Math.sin(pov * Math.PI / 180.0); - double rotDesiredRaw = 0; double[] xy = Util.applyRadialDeadband(xDesiredRaw, yDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); double xFancy = xy[0]; @@ -181,8 +180,7 @@ public Command nudgeCommand() { teleopRequest .withVelocityX(xFancy * 0.4) - .withVelocityY(yFancy * 0.4) - .withRotationalRate(rotFancy * 0); + .withVelocityY(yFancy * 0.4); }).handleInterrupt(() -> setSwerveRequest(new SwerveRequest.FieldCentric()))).withName("Nudge"); } From 32778f37f479852b8f55378444bec5451d461304 Mon Sep 17 00:00:00 2001 From: Ethan Lian Date: Mon, 26 Jan 2026 18:26:34 -0800 Subject: [PATCH 053/118] =?UTF-8?q?arvindChanges=E2=84=A2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/subsystems/indexer/Indexer.java | 6 +++--- .../java/frc/robot/subsystems/indexer/IndexerConstants.java | 2 ++ 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 331f899..abc9140 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -67,7 +67,7 @@ public Command deactivateIndexer() { return setSpeed(IndexerConstants.InactiveSpeed); } - /** command to sense distance from camera; used to sense if bol */ + /** command to sense distance from LaserCAN; used to sense if bol */ private double getDistanceMm() { LaserCan.Measurement measurement = lc.getMeasurement(); if (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) { @@ -85,9 +85,9 @@ private boolean hasBall() { /** command that modifies hasBall, uses getDistanceMm() */ private void checkForBall() { double x = getDistanceMm(); - if (x == IndexerConstants.LaserCan_DefaultMeasurement) { + if (x >= IndexerConstants.LaserCan_DefaultMeasurement) { hasBall = false; - } else { + } else if (x != -1) { /* (x != -1) is checking that the camera isn't just returning an error as ball sensed */ hasBall = true; } } diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java index e7f5641..1b12e29 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java @@ -2,6 +2,8 @@ public class IndexerConstants { + // TODO: Set IDs + // TODO (Ethan): Set LaserCan_DefaultMeasurement public static final int MOTOR_ID = 0; public static final int LASER_ID = 0; public static final double ActiveSpeed = 0; From 1d23d88782aa3894ff7dabbc592d8e7d907aedf9 Mon Sep 17 00:00:00 2001 From: seaspace1250 <117596196+seaspace1250@users.noreply.github.com> Date: Wed, 28 Jan 2026 15:38:23 -0800 Subject: [PATCH 054/118] added outtake command which is just the intake in reverse --- src/main/java/frc/robot/subsystems/intake/Intake.java | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index a7e4b10..650f956 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -94,7 +94,13 @@ private void setRequestBar(ControlRequest request) { public Command setWheelIntaking() { return setWheelSpeed(IntakeConstants.INTAKE_SPEED); } - + /** + * sets wheel speed to the negative of intake speed (for outtaking) + * @return + */ + public Command setWheelOutaking() { + return setWheelSpeed(-IntakeConstants.INTAKE_SPEED); + } /** * sets the wheel speed (in rotations per second) * @return From 5cefc3e7b629d22af2c9cef5767526f3fdb0329c Mon Sep 17 00:00:00 2001 From: shakability Date: Wed, 28 Jan 2026 15:42:33 -0800 Subject: [PATCH 055/118] added suggested changes --- .../frc/robot/subsystems/climb/Climb.java | 46 +++++++++---------- .../subsystems/climb/ClimbConstants.java | 1 + 2 files changed, 24 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/climb/Climb.java b/src/main/java/frc/robot/subsystems/climb/Climb.java index 002b656..8d2e0e1 100644 --- a/src/main/java/frc/robot/subsystems/climb/Climb.java +++ b/src/main/java/frc/robot/subsystems/climb/Climb.java @@ -1,37 +1,29 @@ package frc.robot.subsystems.climb; -import static frc.robot.subsystems.climb.ClimbConstants.*; +import static edu.wpi.first.units.Units.*; +import static frc.robot.subsystems.climb.ClimbConstants.CLIMB_MOTOR_ID; +import static frc.robot.subsystems.climb.ClimbConstants.END_EFFECTOR_HEIGHT; +import static frc.robot.subsystems.climb.ClimbConstants.EPSILON; +import static frc.robot.subsystems.climb.ClimbConstants.getConfig; +import static frc.robot.subsystems.climb.ClimbConstants.metersToRotations; +import static frc.robot.subsystems.climb.ClimbConstants.rotationsToMeters; -import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.controls.ControlRequest; -import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.sim.TalonFXSimState; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.units.Units; import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.simulation.BatterySim; -import edu.wpi.first.wpilibj.simulation.ElevatorSim; -import edu.wpi.first.wpilibj.simulation.RoboRioSim; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Constants; -import frc.robot.Robot; -import frc.robot.lib.util.Util; import frc.robot.subsystems.TelemetryManager; +import frc.robot.subsystems.climb.ClimbConstants.Heights; public class Climb extends SubsystemBase { private static Climb climbInstance; @@ -47,7 +39,7 @@ public static Climb getInstance() { private double lastReadHeight; private double lastReadSpeed; - private double targetHeight = 0; + private double targetHeight = END_EFFECTOR_HEIGHT; private ControlRequest request = new NeutralOut(); private Climb() { @@ -104,7 +96,7 @@ public Command stop() { /** Sysid commands * @param dynamic If true, then runs dynamic test. If false, quasistatic - */ + */ public Command sysId(boolean dynamic, SysIdRoutine.Direction direction) { return defer(() -> { VoltageOut request = new VoltageOut(0); @@ -112,13 +104,21 @@ public Command sysId(boolean dynamic, SysIdRoutine.Direction direction) { new SysIdRoutine.Config( null, // Default ramp rate (1 V) null, // Default step voltage (7 V) - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdElevator_State", state.toString()) + null // Use default timeout (10 s) ), new SysIdRoutine.Mechanism( - output -> request.withOutput(output), - null, + output -> request.withOutput(output), + log -> { + log.motor("climbMotor") + .voltage(Volts.of(request.Output)) + .linearPosition( + Meters.of( + rotationsToMeters( + climbMotor.getPosition().getValueAsDouble()))) + .linearVelocity( + MetersPerSecond.of( + rotationsToMeters(climbMotor.getVelocity().getValueAsDouble()))); + }, this ) ); diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java b/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java index 599a8ab..497e785 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java @@ -16,6 +16,7 @@ public class ClimbConstants { public static final double SPROCKET_RADIUS = 0.0412; // Effective pitch radius public static final double GEAR_RATIO = 9; public static final double SPROCKET_CIRCUMFERENCE = SPROCKET_RADIUS * Constants.TAU; + public static final double END_EFFECTOR_HEIGHT = 0.54; // Meters // CHANGE THIS public static final double METERS_PER_ROTATION = 0.028776; // Approximated using measurement public static final double CARRIAGE_WEIGHT = 7.55; // kg From 7bc9d59cbbe95dce12a4a77b99b3562e522dc6e3 Mon Sep 17 00:00:00 2001 From: Ethan Lian Date: Wed, 28 Jan 2026 17:23:15 -0800 Subject: [PATCH 056/118] adding getInstance method, prototype for sensing shooter ready or not --- .../frc/robot/subsystems/indexer/Indexer.java | 60 +++++++++++++++++-- .../subsystems/indexer/IndexerConstants.java | 3 +- 2 files changed, 58 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index abc9140..8392702 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -11,29 +11,53 @@ import au.grapplerobotics.LaserCan; import au.grapplerobotics.ConfigurationFailedException; +// TODO (ethan): only activate if shooter ready +// TODO (ethan): ask tommy setControl(request) public class Indexer extends SubsystemBase { + /** getInstance of indexer */ + private static Indexer instance; + public static Indexer getInstance() { + if (instance == null) { + instance = new Indexer(); + } + return instance; + } private ControlRequest request; private TalonFX motor; - private LaserCan lc; + private LaserCan lc, lcTwo; /** boolean that's modified by checkForBall() */ private boolean hasBall; + private boolean shooterReady = false; + // COMMENTED OUT /** boolean that's modified by checkForBallTwo() */ + // private boolean hasBallTwo; + + /** boolean that's modified by shooter */ + /** setup, adding motor and laser */ private Indexer() { super(); motor = new TalonFX(IndexerConstants.MOTOR_ID); lc = new LaserCan(IndexerConstants.LASER_ID); + lcTwo = new LaserCan(IndexerConstants.LASER_ID_2); /* new laser configs */ try { lc.setRangingMode(LaserCan.RangingMode.SHORT); lc.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16)); lc.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); - } catch (ConfigurationFailedException e) { + } catch (ConfigurationFailedException e) { System.out.println("Configuration failed! " + e); - } + } + try { + lcTwo.setRangingMode(LaserCan.RangingMode.SHORT); + lcTwo.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16)); + lcTwo.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); + } catch (ConfigurationFailedException e) { + System.out.println("Configuration failed! " + e); + } } @Override @@ -41,6 +65,10 @@ private Indexer() { public void periodic() { motor.setControl(request); checkForBall(); + // checkForBallTwo(); + if (shooterReady = true) { + activateIndexer(); + } } /** type conversion/abstraction */ @@ -76,11 +104,24 @@ private double getDistanceMm() { return -1; } } + // COMMENTED OUT /** command to sense if bol going into shooter */ + // private double getDistanceMmTwo() { + // LaserCan.Measurement measurement = lc.getMeasurement(); + // if (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) { + // return measurement.distance_mm; + // } else { + // return -1; + // } + // } /** setup formatting for boolean hasBall so it can be used in activateIndexer().either */ private boolean hasBall() { return hasBall; } + // COMMENTED OUT /** setup formatting for boolean hasBall so that it can be used to tell if shooter ready */ + // private boolean hasBallTwo() { + // return hasBallTwo; + // } /** command that modifies hasBall, uses getDistanceMm() */ private void checkForBall() { @@ -91,8 +132,19 @@ private void checkForBall() { hasBall = true; } } - + // COMMENTED OUT //** command that checks if bol is boutta be shot */ + // private void checkForBallTwo() { + // double x = getDistanceMmTwo(); + // if (x >= IndexerConstants.LaserCan_DefaultMeasurement) { + // hasBallTwo = false; + // } else if (x != -1) { /* (x != -1) is checking that the camera isn't just returning an error as ball sensed */ + // hasBallTwo = true; + // } + // } + @Override + // TODO: AdvantageKit! + /** ????????? */ public void initSendable(SendableBuilder builder) { super.initSendable(builder); builder.addBooleanProperty("Has Ball", () -> hasBall, null); diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java index 1b12e29..5529e6f 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java @@ -6,8 +6,9 @@ public class IndexerConstants { // TODO (Ethan): Set LaserCan_DefaultMeasurement public static final int MOTOR_ID = 0; public static final int LASER_ID = 0; + public static final int LASER_ID_2 = 0; public static final double ActiveSpeed = 0; public static final double InactiveSpeed = 0; public static final double LaserCan_DefaultMeasurement = 1; - + } \ No newline at end of file From 5df56d9d2227b387d23ddc5fd1ebc10180eae9b6 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Wed, 28 Jan 2026 17:41:48 -0800 Subject: [PATCH 057/118] akhil ran out of memory so i took over his shooter --- .../frc/robot/subsystems/shooter/Shooter.java | 78 ++++++------------- 1 file changed, 23 insertions(+), 55 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 4353746..c82bb52 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -6,7 +6,6 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; -//import au.grapplerobotics.LaserCan; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.util.sendable.SendableBuilder; @@ -25,38 +24,22 @@ public static Shooter getInstance() { } private final TalonFX topMotor; - private final TalonFX bottomMotor; - - //private final LaserCan shooterLaser; - - private final Debouncer shooterDebouncer; - - private boolean inRangeShooter; - private double lastReadSpeedTop; private double lastReadSpeedBottom; - private ControlRequest request = new NeutralOut(); + private ControlRequest topRequest = new NeutralOut(); + private ControlRequest bottomRequest = new NeutralOut(); private Shooter() { super(); bottomMotor = new TalonFX(ShooterConstants.Motors.BOTTOM.id); - - //shooterLaser = new LaserCan(ShooterConstants.Lasers.FRONT.id); - bottomMotor.getConfigurator().apply(ShooterConstants.getConfig()); bottomMotor.setNeutralMode(NeutralModeValue.Brake); topMotor = new TalonFX(ShooterConstants.Motors.TOP.id); - topMotor.getConfigurator().apply(ShooterConstants.getConfig()); topMotor.setNeutralMode(NeutralModeValue.Brake); - - shooterDebouncer = new Debouncer(0.06, DebounceType.kBoth); - - //inRangeShooter = false; - TelemetryManager.getInstance().addSendable(this); } @@ -65,54 +48,39 @@ public void periodic() { // Read inputs lastReadSpeedTop = topMotor.getVelocity().getValueAsDouble(); lastReadSpeedBottom = bottomMotor.getVelocity().getValueAsDouble(); - //inRangeShooter = shooterDebouncer.calculate(getShooterLaser()); + topMotor.setControl(topRequest); + bottomMotor.setControl(bottomRequest); } /** Replaces the request */ - private void setRequest(ControlRequest request) { - this.request = request; + private void setTopRequest(ControlRequest request) { + this.topRequest = request; + } + + /** Replaces the request */ + private void setBottomRequest(ControlRequest request) { + this.bottomRequest = request; } /** Stops the shooter */ public Command stop() { - return runOnce(() -> setRequest(new NeutralOut())).withName("Stopped"); - } - - /** Gets the shooter laser measurement (Not needed as of now) */ - /* - private double getMeasurementShooter() { - var measurement = shooterLaser.getMeasurement(); - if (measurement != null) { - return measurement.distance_mm; - } else { - return Double.POSITIVE_INFINITY; - } - } - */ - - /* Gets whether the shooter laser detects the "fuel" (balls) not needed right now - private boolean getShooterLaser() { - return getMeasurementShooter() < 100; + return runOnce( + () -> { + setTopRequest(new NeutralOut()); + setBottomRequest(new NeutralOut()); + } + ).withName("Stopped"); } - */ - public Command topShoot(int targetSpeed) { - return runOnce(() -> setRequest( + public Command topShoot(double targetSpeed) { + return runOnce(() -> setTopRequest( new VelocityVoltage(targetSpeed)) - ).andThen( - Commands.waitUntil(() -> !inRangeShooter), - Commands.waitSeconds(0.15), - stop() ).withName("Top Shooting"); } - public Command BottomShoot(int targetSpeed) { - return runOnce(() -> setRequest( + public Command bottomShoot(double targetSpeed) { + return runOnce(() -> setBottomRequest( new VelocityVoltage(targetSpeed)) - ).andThen( - Commands.waitUntil(() -> !inRangeShooter), - Commands.waitSeconds(0.15), - stop() ).withName("Bottom Shooting"); } @@ -121,13 +89,13 @@ public void initSendable(SendableBuilder builder) { super.initSendable(builder); builder.addDoubleProperty( - "/Speed", + "/TopSpeed", () -> lastReadSpeedTop, null); TelemetryManager.makeSendableTalonFX("/Top", topMotor, builder); builder.addDoubleProperty( - "/Speed", + "/BottomSpeed", () -> lastReadSpeedBottom, null); TelemetryManager.makeSendableTalonFX("/Bottom", bottomMotor, builder); From 2452982209922c5a0ede4336c2171ba7be34ee45 Mon Sep 17 00:00:00 2001 From: APandit1-cpu Date: Wed, 28 Jan 2026 18:03:16 -0800 Subject: [PATCH 058/118] added controller --- src/main/java/frc/robot/ControlsMapping.java | 28 +++++++++++++++++--- 1 file changed, 24 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index e9859bd..c64c116 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -6,18 +6,22 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.commands.PIDToPoseCommand; import frc.robot.subsystems.drive.ctre.CtreDrive.SysIdRoutineType; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj.DataLogManager; -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.XboxController; - +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; public class ControlsMapping { + CommandXboxController controller1; + public static void mapTeleopCommand() { + + + Drive.getInstance().setDefaultCommand((Drive.getInstance().teleopCommand())); // run sysID functions Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER); @@ -27,7 +31,23 @@ public static void mapTeleopCommand() { controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false)); controller.x().whileTrue(Drive.getInstance().autopilotAlign(true)); controller.y().whileTrue(Drive.getInstance().autopilotAlign(false)); + controller.b().whileTrue(HangCommand()); // TODO: Implement hang from armaaan + controller.leftTrigger().whileTrue(intakeCommand()); + controller.rightTrigger().whileTrue(shooterCommand()); + } + + public static Command intakeCommand() { + return Commands.print("Intaking"); + } + public static Command HangCommand() { + return Commands.print("Hanging"); + } + public static Command shooterCommand() { + return Commands.print("Shooting"); + } + + //make a method that returns command, but it returns command.none, replace everything with new instance command with the command name. public static void mapSysId() { // set up sysID routine type From 2d6cff0a163ead7f4ef55d0d17dcbb9af0109a2d Mon Sep 17 00:00:00 2001 From: seaspace1250 <117596196+seaspace1250@users.noreply.github.com> Date: Wed, 28 Jan 2026 19:18:33 -0800 Subject: [PATCH 059/118] ?????????? help --- .../frc/robot/subsystems/intake/Intake.java | 55 ++++++++++++++----- 1 file changed, 41 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 650f956..74c618c 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -1,24 +1,23 @@ package frc.robot.subsystems.intake; +import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.subsystems.TelemetryManager; - -import com.ctre.phoenix6.controls.ControlRequest; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.NeutralOut; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; - +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; +import frc.robot.subsystems.TelemetryManager;; public class Intake extends SubsystemBase { private static Intake intakeInstance; @@ -37,6 +36,7 @@ public static Intake getInstance() { private ControlRequest wheelRequest = new NeutralOut(); private ControlRequest barRequest = new NeutralOut(); + private DCMotor pl = DCMotor.getKrakenX60(1); private Intake() { super(); wheelMotor = new TalonFX(IntakeConstants.Motors.WHEEL.id); @@ -45,17 +45,34 @@ private Intake() { barMotor.getConfigurator().apply(IntakeConstants.getBarConfig()); wheelMotor.setNeutralMode(NeutralModeValue.Brake); barMotor.setNeutralMode(NeutralModeValue.Brake); //? - - TelemetryManager.getInstance().addSendable(this); + SmartDashboard.putData("123123", mech2d); + //TelemetryManager.getInstance().addSendable(this); + mech2droot.append(mech2dpivot); } - + private final Mechanism2d mech2d = new Mechanism2d(20, 20); + private final MechanismRoot2d mech2droot = mech2d.getRoot("Bar Root", 10, 1); + private final MechanismLigament2d mech2dpivot = new MechanismLigament2d("Pivot", 10, 0 + ); + + final SingleJointedArmSim sim = new SingleJointedArmSim( + pl, + 2.0, + 1.0, + 1.0, + IntakeConstants.BAR_POS_MIN, + IntakeConstants.BAR_POS_MAX, + true, + 0.0, + 0.0, 0.0 + ); @Override public void periodic(){ wheelSpeed = wheelMotor.getVelocity().getValueAsDouble(); barPosition = barMotor.getPosition().getValueAsDouble(); barMotor.setControl(barRequest); wheelMotor.setControl(wheelRequest); + mech2dpivot.setAngle(barPosition); } //---------------stop---------------- @@ -94,6 +111,7 @@ private void setRequestBar(ControlRequest request) { public Command setWheelIntaking() { return setWheelSpeed(IntakeConstants.INTAKE_SPEED); } + /** * sets wheel speed to the negative of intake speed (for outtaking) * @return @@ -101,6 +119,7 @@ public Command setWheelIntaking() { public Command setWheelOutaking() { return setWheelSpeed(-IntakeConstants.INTAKE_SPEED); } + /** * sets the wheel speed (in rotations per second) * @return @@ -142,4 +161,12 @@ public Command setBarPosition(double position) { new PositionVoltage(checkedPos)) ).withName("bar pos set" + (checkedPos)); } + + + + public void simulationPeriodic() { + sim.setInput(1.0); + sim.update(0.020); + System.out.println("simulationperiodic"); + } } \ No newline at end of file From a90b28dcb07e187dca777eeb585e7dd3fca152df Mon Sep 17 00:00:00 2001 From: seaspace1250 <117596196+seaspace1250@users.noreply.github.com> Date: Wed, 28 Jan 2026 19:20:05 -0800 Subject: [PATCH 060/118] idk --- simgui-ds.json | 8 ++++---- src/main/java/frc/robot/Robot.java | 2 ++ 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 0bea79e..fed9b27 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -32,9 +32,9 @@ 88, 67, 86, - 81, - 69, - 82 + 66, + 78, + 77 ], "povConfig": [ { @@ -61,7 +61,7 @@ "axisCount": 2, "buttonCount": 4, "buttonKeys": [ - 77, + -1, 44, 46, 47 diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0b73ec1..a68d6b5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -21,6 +21,7 @@ import frc.robot.Constants.Controllers; import frc.robot.subsystems.TelemetryManager; import frc.robot.subsystems.drive.*; +import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.vision.VisionDeviceManager; /** @@ -46,6 +47,7 @@ public Robot() { VisionDeviceManager.getInstance(); } Drive.getInstance(); + Intake.getInstance(); TelemetryManager.getInstance(); FollowPathCommand.warmupCommand().schedule(); autoChooser = new AutoSelector(); From 21107bb38bfa7dc1ba28bdcf49d084ae88fb20b9 Mon Sep 17 00:00:00 2001 From: audreywtu Date: Wed, 28 Jan 2026 19:23:42 -0800 Subject: [PATCH 061/118] woah saved 8 bytes of memory --- src/main/java/frc/robot/subsystems/drive/Drive.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 44d4a10..3098385 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -176,7 +176,6 @@ public Command nudgeCommand() { double[] xy = Util.applyRadialDeadband(xDesiredRaw, yDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); double xFancy = xy[0]; double yFancy = xy[1]; - double rotFancy = Util.applyJoystickDeadband(rotDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); teleopRequest .withVelocityX(xFancy * 0.4) From 04116aa27cd50f1583a5eeecda2107aea72a8495 Mon Sep 17 00:00:00 2001 From: Arvind Venkatesh Date: Wed, 28 Jan 2026 19:30:02 -0800 Subject: [PATCH 062/118] Add controller to indexer (#16) * Remove entire robot/lib/swerve folder and replace all references with subsystem/ctre/CtreDriveConstants * Remove WheelTracker, Pigeon, SwerveModule, and TalonFxManager from codebase. * Deprecate RobotState, CancoderManager, AutoPilotCommand and SnapCommand etc * Deprecate limelight-based vision localization code * Deprecated robot/lib/drivers folder and its classes (CanDeviceId and Phoenix6Util) * reformat to prepare for sysID tuning * feat: automode scaffolding and PIDV, validation on first complex path on the field. * fix: pid to pose * tested snap and it work :D * minor refactor * refactor of the refactor * feat: sysid tuning for drive-motors, verified on robot. * finetuning steer-drive coupling ratio * Merged from 2025RobotTest * Trajectory update soon * i think later though * minor change * FEAT: SysID Rotation routine and logs * feat: enable VisualVM to debug RAM issues * feat: add steer motor offsets of the 2nd bot * feat: add radialDeadband to fix yaw impulse when driver releases stick * fix kCoupleRatio value for steer/drive coupling * Pid to pose borke again * Fixed PIDToPose * last few bugs trust * aarush's test branch * added controller --------- Co-authored-by: dcao6668 Co-authored-by: github-0-0 Co-authored-by: APandit1-cpu --- build.gradle | 17 ++++++++++++ src/main/java/frc/robot/ControlsMapping.java | 27 +++++++++++++++++++- src/main/java/frc/robot/Robot.java | 5 ++++ 3 files changed, 48 insertions(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index 06ca256..fff5f8f 100644 --- a/build.gradle +++ b/build.gradle @@ -27,6 +27,13 @@ deploy { // getTargetTypeClass is a shortcut to get the class type using a string frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + // Enable VisualVM connection + jvmArgs.add("-Dcom.sun.management.jmxremote=true") + jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") + jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") + jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") + jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") + jvmArgs.add("-Djava.rmi.server.hostname=10.14.58.2") // Replace TE.AM with team number } // Static files artifact @@ -52,11 +59,21 @@ def includeDesktopSupport = true // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. + +task(replayWatch, type: JavaExec) { + mainClass = "org.littletonrobotics.junction.ReplayWatch" + classpath = sourceSets.main.runtimeClasspath +} + dependencies { annotationProcessor wpi.java.deps.wpilibAnnotations() implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() + // ... + def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) + annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" + roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index e40c703..40eae72 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -4,14 +4,23 @@ import com.ctre.phoenix6.swerve.SwerveRequest; -import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.ctre.CtreDrive.SysIdRoutineType; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; public class ControlsMapping { + CommandXboxController controller1; + public static void mapTeleopCommand() { + + + Drive.getInstance().setDefaultCommand((Drive.getInstance().teleopCommand())); // run sysID functions Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER); @@ -21,7 +30,23 @@ public static void mapTeleopCommand() { controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false)); controller.x().whileTrue(Drive.getInstance().autopilotAlign(true)); controller.y().whileTrue(Drive.getInstance().autopilotAlign(false)); + controller.b().whileTrue(HangCommand()); // TODO: Implement hang from armaaan + controller.leftTrigger().whileTrue(intakeCommand()); + controller.rightTrigger().whileTrue(shooterCommand()); + } + + public static Command intakeCommand() { + return Commands.print("Intaking"); + } + public static Command HangCommand() { + return Commands.print("Hanging"); + } + public static Command shooterCommand() { + return Commands.print("Shooting"); + } + + //make a method that returns command, but it returns command.none, replace everything with new instance command with the command name. public static void mapSysId() { // set up sysID routine type diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0b73ec1..50ebffd 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,11 @@ import java.util.Optional; +import org.littletonrobotics.junction.LogFileUtil; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGReader; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; + import com.pathplanner.lib.commands.FollowPathCommand; import edu.wpi.first.epilogue.Logged; From fc777a50b075503a81b99c893f93d9f56aa388aa Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Sat, 31 Jan 2026 09:28:56 -0800 Subject: [PATCH 063/118] added sim Hopefully this works here we go --- src/main/java/frc/robot/Robot.java | 2 + .../frc/robot/subsystems/shooter/Shooter.java | 46 +++++++++++++++++-- .../subsystems/shooter/ShooterConstants.java | 14 +----- 3 files changed, 47 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0b73ec1..0dc0a42 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -21,6 +21,7 @@ import frc.robot.Constants.Controllers; import frc.robot.subsystems.TelemetryManager; import frc.robot.subsystems.drive.*; +import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.vision.VisionDeviceManager; /** @@ -46,6 +47,7 @@ public Robot() { VisionDeviceManager.getInstance(); } Drive.getInstance(); + Shooter.getInstance(); TelemetryManager.getInstance(); FollowPathCommand.warmupCommand().schedule(); autoChooser = new AutoSelector(); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index c82bb52..b233d4d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -6,12 +6,15 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.simulation.BatterySim; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; import frc.robot.subsystems.TelemetryManager; public class Shooter extends SubsystemBase { @@ -30,6 +33,9 @@ public static Shooter getInstance() { private ControlRequest topRequest = new NeutralOut(); private ControlRequest bottomRequest = new NeutralOut(); + private FlywheelSim topSim; + private FlywheelSim bottomSim; + private Shooter() { super(); @@ -40,6 +46,21 @@ private Shooter() { topMotor = new TalonFX(ShooterConstants.Motors.TOP.id); topMotor.getConfigurator().apply(ShooterConstants.getConfig()); topMotor.setNeutralMode(NeutralModeValue.Brake); + + if (Robot.isSimulation()) { + topSim = new FlywheelSim( + LinearSystemId.createFlywheelSystem( + DCMotor.getKrakenX60(1), + 0.000489000861, + 1 + ), DCMotor.getKrakenX60(1), 0.0); + bottomSim = new FlywheelSim( + LinearSystemId.createFlywheelSystem( + DCMotor.getKrakenX60(1), + 0.000489000861, + 1 + ), DCMotor.getKrakenX60(1), 0.0); + } TelemetryManager.getInstance().addSendable(this); } @@ -52,6 +73,25 @@ public void periodic() { bottomMotor.setControl(bottomRequest); } + @Override + public void simulationPeriodic() { + topMotor.getSimState() + .setRotorVelocity(topSim.getAngularVelocityRPM() / 60.0); + topMotor.getSimState().setSupplyVoltage(12.0); + topSim.setInput(topMotor.getMotorVoltage().getValueAsDouble()); + topSim.update(0.020); + RoboRioSim.setVInVoltage( + BatterySim.calculateDefaultBatteryLoadedVoltage(topSim.getCurrentDrawAmps())); + + bottomMotor.getSimState() + .setRotorVelocity(bottomSim.getAngularVelocityRPM() / 60.0); + bottomMotor.getSimState().setSupplyVoltage(12.0); + bottomSim.setInput(bottomMotor.getMotorVoltage().getValueAsDouble()); + bottomSim.update(0.020); + RoboRioSim.setVInVoltage( + BatterySim.calculateDefaultBatteryLoadedVoltage(bottomSim.getCurrentDrawAmps())); + } + /** Replaces the request */ private void setTopRequest(ControlRequest request) { this.topRequest = request; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index cad087f..61fad82 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -6,7 +6,7 @@ import com.ctre.phoenix6.configs.VoltageConfigs; public final class ShooterConstants { - public static final double SHOOT_SPEED_INCREMENT = 5; // rotation per second + public static final double GEAR_RATIO = 1; /** Motor ids */ public static enum Motors { @@ -17,17 +17,7 @@ private Motors(int id) { this.id = id; } } - - /** Laser ids */ - public static enum Lasers { - //BACK(30), - FRONT(31); - public final int id; - private Lasers(int id) { - this.id = id; - } - } - + /** Config for shooter motors */ public static TalonFXConfiguration getConfig() { return new TalonFXConfiguration() From 6938988817d6e27c38b649047e51e22fe42df3dc Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Sat, 31 Jan 2026 10:44:23 -0800 Subject: [PATCH 064/118] Shooter simulation working --- .vscode/launch.json | 24 +------ simgui-ds.json | 7 ++- src/main/java/frc/robot/ControlsMapping.java | 3 + .../lib/control/ProfiledPIDVController.java | 18 +++--- .../frc/robot/subsystems/shooter/Shooter.java | 54 +++++++++------- .../subsystems/shooter/ShooterConstants.java | 13 ++-- ...enix6-26.1.0.json => Phoenix6-26.1.1.json} | 62 +++++++++---------- ...plefrc2025.json => libgrapplefrc2026.json} | 16 ++--- 8 files changed, 96 insertions(+), 101 deletions(-) rename vendordeps/{Phoenix6-26.1.0.json => Phoenix6-26.1.1.json} (92%) rename vendordeps/{libgrapplefrc2025.json => libgrapplefrc2026.json} (87%) diff --git a/.vscode/launch.json b/.vscode/launch.json index 396fe88..e6ca564 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -18,26 +18,4 @@ "desktop": false, } ] -} - -{ - // Use IntelliSense to learn about possible attributes. - // Hover to view descriptions of existing attributes. - // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 - "version": "0.2.0", - "configurations": [ - - { - "type": "wpilib", - "name": "WPILib Desktop Debug", - "request": "launch", - "desktop": true, - }, - { - "type": "wpilib", - "name": "WPILib roboRIO Debug", - "request": "launch", - "desktop": false, - } - ] -} +} \ No newline at end of file diff --git a/simgui-ds.json b/simgui-ds.json index 0bea79e..1486df5 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -16,10 +16,15 @@ "incKey": 68 }, { + "decKey": 50, "decayRate": 0.0, + "incKey": 49, "keyRate": 0.009999999776482582 }, - {}, + { + "decKey": 52, + "incKey": 51 + }, { "decKey": 74, "incKey": 76 diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index e40c703..045e581 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.ctre.CtreDrive.SysIdRoutineType; +import frc.robot.subsystems.shooter.Shooter; import edu.wpi.first.wpilibj2.command.Commands; public class ControlsMapping { @@ -21,6 +22,8 @@ public static void mapTeleopCommand() { controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false)); controller.x().whileTrue(Drive.getInstance().autopilotAlign(true)); controller.y().whileTrue(Drive.getInstance().autopilotAlign(false)); + + controller.rightTrigger().whileTrue(Shooter.getInstance().shoot()); } public static void mapSysId() { diff --git a/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java b/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java index 2e76a9c..a256ae9 100644 --- a/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java +++ b/src/main/java/frc/robot/lib/control/ProfiledPIDVController.java @@ -55,17 +55,17 @@ public ProfiledPIDVController setMeasurement(double position, double speed) { } public double getOutput() { - // if (controller.isContinuous) { - // double errorBound = (controller.maxRange - controller.minRange) / 2.0; + if (controller.isContinuous) { + double errorBound = (controller.maxRange - controller.minRange) / 2.0; - // double goalDelta = - // MathUtil.inputModulus(goal.position - controller.positionMeasurement, -errorBound, errorBound); - // double setpointDelta = - // MathUtil.inputModulus(setpoint.position - controller.positionMeasurement, -errorBound, errorBound); + double goalDelta = + MathUtil.inputModulus(goal.position - controller.positionMeasurement, -errorBound, errorBound); + double setpointDelta = + MathUtil.inputModulus(setpoint.position - controller.positionMeasurement, -errorBound, errorBound); - // goal.position = goalDelta + controller.positionMeasurement; - // setpoint.position = setpointDelta + controller.positionMeasurement; - // } + goal.position = goalDelta + controller.positionMeasurement; + setpoint.position = setpointDelta + controller.positionMeasurement; + } // Advance profile by one timestep setpoint = profile.calculate(Constants.DT, setpoint, goal); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index b233d4d..692c6c3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.shooter; +import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.VelocityVoltage; @@ -9,10 +10,12 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.simulation.BatterySim; import edu.wpi.first.wpilibj.simulation.FlywheelSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.subsystems.TelemetryManager; @@ -41,11 +44,11 @@ private Shooter() { bottomMotor = new TalonFX(ShooterConstants.Motors.BOTTOM.id); bottomMotor.getConfigurator().apply(ShooterConstants.getConfig()); - bottomMotor.setNeutralMode(NeutralModeValue.Brake); + bottomMotor.setNeutralMode(NeutralModeValue.Coast); topMotor = new TalonFX(ShooterConstants.Motors.TOP.id); topMotor.getConfigurator().apply(ShooterConstants.getConfig()); - topMotor.setNeutralMode(NeutralModeValue.Brake); + topMotor.setNeutralMode(NeutralModeValue.Coast); if (Robot.isSimulation()) { topSim = new FlywheelSim( @@ -75,21 +78,26 @@ public void periodic() { @Override public void simulationPeriodic() { + topMotor.getSimState().setSupplyVoltage(12); + bottomMotor.getSimState().setSupplyVoltage(12); + + topSim.setInput(topMotor.getSimState().getMotorVoltage()); + topSim.update(0.020); topMotor.getSimState() .setRotorVelocity(topSim.getAngularVelocityRPM() / 60.0); - topMotor.getSimState().setSupplyVoltage(12.0); - topSim.setInput(topMotor.getMotorVoltage().getValueAsDouble()); - topSim.update(0.020); + topMotor.getSimState().addRotorPosition(topSim.getAngularVelocityRPM() / 60.0 * 0.020); RoboRioSim.setVInVoltage( BatterySim.calculateDefaultBatteryLoadedVoltage(topSim.getCurrentDrawAmps())); - bottomMotor.getSimState() - .setRotorVelocity(bottomSim.getAngularVelocityRPM() / 60.0); - bottomMotor.getSimState().setSupplyVoltage(12.0); - bottomSim.setInput(bottomMotor.getMotorVoltage().getValueAsDouble()); + bottomSim.setInput(bottomMotor.getSimState().getMotorVoltage()); bottomSim.update(0.020); RoboRioSim.setVInVoltage( BatterySim.calculateDefaultBatteryLoadedVoltage(bottomSim.getCurrentDrawAmps())); + bottomMotor.getSimState() + .setRotorVelocity(topSim.getAngularVelocityRPM() / 60.0); + bottomMotor.getSimState().addRotorPosition(topSim.getAngularVelocityRPM() / 60.0 * 0.020); + + setDefaultCommand(stop()); } /** Replaces the request */ @@ -106,22 +114,21 @@ private void setBottomRequest(ControlRequest request) { public Command stop() { return runOnce( () -> { - setTopRequest(new NeutralOut()); - setBottomRequest(new NeutralOut()); + setTopRequest(new CoastOut()); + setBottomRequest(new CoastOut()); } ).withName("Stopped"); } - public Command topShoot(double targetSpeed) { - return runOnce(() -> setTopRequest( - new VelocityVoltage(targetSpeed)) - ).withName("Top Shooting"); + public Command shoot(double topSpeed, double bottomSpeed) { + return runOnce(() -> { + setTopRequest(new VelocityVoltage(topSpeed)); + setBottomRequest(new VelocityVoltage(bottomSpeed)); + }).andThen(Commands.repeatingSequence(Commands.none())).withName("Shooting"); } - public Command bottomShoot(double targetSpeed) { - return runOnce(() -> setBottomRequest( - new VelocityVoltage(targetSpeed)) - ).withName("Bottom Shooting"); + public Command shoot() { + return shoot(3000 / 60.0, 3000 / 60.0); } @Override @@ -129,16 +136,15 @@ public void initSendable(SendableBuilder builder) { super.initSendable(builder); builder.addDoubleProperty( - "/TopSpeed", + "TopSpeed", () -> lastReadSpeedTop, null); - TelemetryManager.makeSendableTalonFX("/Top", topMotor, builder); + TelemetryManager.makeSendableTalonFX("Top", topMotor, builder); builder.addDoubleProperty( - "/BottomSpeed", + "BottomSpeed", () -> lastReadSpeedBottom, null); - TelemetryManager.makeSendableTalonFX("/Bottom", bottomMotor, builder); - + TelemetryManager.makeSendableTalonFX("Bottom", bottomMotor, builder); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 61fad82..657827e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -23,12 +23,15 @@ public static TalonFXConfiguration getConfig() { return new TalonFXConfiguration() .withSlot0(new Slot0Configs() .withKV(0.0) - .withKP(0.3) - .withKI(0.0) - .withKD(0.0)) + .withKP(0.5) + .withKI(0.001) + .withKD(0.0) + .withKA(0.1) + .withKS(0.1) + .withKV(0.1)) // placeholder values .withCurrentLimits(new CurrentLimitsConfigs() - .withStatorCurrentLimit(30) - .withSupplyCurrentLimit(30)) + .withStatorCurrentLimit(60) + .withSupplyCurrentLimit(60)) .withVoltage(new VoltageConfigs() .withPeakForwardVoltage(12.0) .withPeakReverseVoltage(-12.0)); diff --git a/vendordeps/Phoenix6-26.1.0.json b/vendordeps/Phoenix6-26.1.1.json similarity index 92% rename from vendordeps/Phoenix6-26.1.0.json rename to vendordeps/Phoenix6-26.1.1.json index dc5dc62..7a0eca0 100644 --- a/vendordeps/Phoenix6-26.1.0.json +++ b/vendordeps/Phoenix6-26.1.1.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-26.1.0.json", + "fileName": "Phoenix6-26.1.1.json", "name": "CTRE-Phoenix (v6)", - "version": "26.1.0", + "version": "26.1.1", "frcYear": "2026", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "26.1.0" + "version": "26.1.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/libgrapplefrc2025.json b/vendordeps/libgrapplefrc2026.json similarity index 87% rename from vendordeps/libgrapplefrc2025.json rename to vendordeps/libgrapplefrc2026.json index 508fd7b..2f620b5 100644 --- a/vendordeps/libgrapplefrc2025.json +++ b/vendordeps/libgrapplefrc2026.json @@ -1,25 +1,25 @@ { - "fileName": "libgrapplefrc2025.json", + "fileName": "libgrapplefrc2026.json", "name": "libgrapplefrc", - "version": "2025.0.8", - "frcYear": "2025", + "version": "2026.0.0", + "frcYear": "2026", "uuid": "8ef3423d-9532-4665-8339-206dae1d7168", "mavenUrls": [ "https://storage.googleapis.com/grapple-frc-maven" ], - "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2025.json", + "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2026.json", "javaDependencies": [ { "groupId": "au.grapplerobotics", "artifactId": "libgrapplefrcjava", - "version": "2025.0.8" + "version": "2026.0.0" } ], "jniDependencies": [ { "groupId": "au.grapplerobotics", "artifactId": "libgrapplefrcdriver", - "version": "2025.0.8", + "version": "2026.0.0", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -36,7 +36,7 @@ { "groupId": "au.grapplerobotics", "artifactId": "libgrapplefrccpp", - "version": "2025.0.8", + "version": "2026.0.0", "libName": "grapplefrc", "headerClassifier": "headers", "sharedLibrary": true, @@ -53,7 +53,7 @@ { "groupId": "au.grapplerobotics", "artifactId": "libgrapplefrcdriver", - "version": "2025.0.8", + "version": "2026.0.0", "libName": "grapplefrcdriver", "headerClassifier": "headers", "sharedLibrary": true, From e1a3a37912b9bcd55170ce71b3f66ba34176a043 Mon Sep 17 00:00:00 2001 From: seaspace1250 <117596196+seaspace1250@users.noreply.github.com> Date: Sat, 31 Jan 2026 12:47:13 -0800 Subject: [PATCH 065/118] added intake commands into control mapping --- src/main/java/frc/robot/ControlsMapping.java | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index 7d6a548..8d0fd0f 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.ctre.CtreDrive.SysIdRoutineType; +import frc.robot.subsystems.intake.Intake; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -32,15 +33,20 @@ public static void mapTeleopCommand() { controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false)); controller.x().whileTrue(Drive.getInstance().autopilotAlign(true)); controller.y().whileTrue(Drive.getInstance().autopilotAlign(false)); - controller.b().whileTrue(HangCommand()); // TODO: Implement hang from armaaan + controller.b().whileTrue(hangCommand()); // TODO: Implement hang from armaaan controller.leftTrigger().whileTrue(intakeCommand()); controller.rightTrigger().whileTrue(shooterCommand()); } public static Command intakeCommand() { - return Commands.print("Intaking"); + return Intake.getInstance().setBarDown().andThen(Intake.getInstance().setWheelIntaking()); } - public static Command HangCommand() { + + public static Command outtakeCommand() { + return Intake.getInstance().setBarDown().andThen(Intake.getInstance().setWheelOutaking()); + } + + public static Command hangCommand() { return Commands.print("Hanging"); } public static Command shooterCommand() { From 9a7dcadf2d70e800c3b2a36ac5672642a150d7be Mon Sep 17 00:00:00 2001 From: Dibavi Date: Sat, 31 Jan 2026 13:48:56 -0800 Subject: [PATCH 066/118] added keybind controls --- .../{ControlsMapping.java => ControlMap.java} | 37 ++++++++++++++++--- src/main/java/frc/robot/Robot.java | 37 +++++++++---------- 2 files changed, 48 insertions(+), 26 deletions(-) rename src/main/java/frc/robot/{ControlsMapping.java => ControlMap.java} (72%) diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlMap.java similarity index 72% rename from src/main/java/frc/robot/ControlsMapping.java rename to src/main/java/frc/robot/ControlMap.java index 40eae72..5d71360 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlMap.java @@ -2,25 +2,35 @@ import static frc.robot.Robot.controller; +import java.lang.reflect.Method; +import java.util.function.Supplier; + import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import frc.robot.auto.AutoRoutines; +import frc.robot.auto.AutoSelector.Auto; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.ctre.CtreDrive.SysIdRoutineType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -public class ControlsMapping { - CommandXboxController controller1; +public class ControlMap { + public static void implement(String key) { + switch (key) { + case "mapTwo": mapTwo(); break; + default: standard(); + } + } - public static void mapTeleopCommand() { - - - + public static void standard() { Drive.getInstance().setDefaultCommand((Drive.getInstance().teleopCommand())); // run sysID functions Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER); @@ -35,6 +45,21 @@ public static void mapTeleopCommand() { controller.rightTrigger().whileTrue(shooterCommand()); } + public static void mapTwo() { + Drive.getInstance().setDefaultCommand((Drive.getInstance().teleopCommand())); + // run sysID functions + Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER); + + controller.a().whileTrue(shooterCommand()); + controller.leftBumper().whileTrue(Drive.getInstance().autoAlign(true)); + controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false)); + controller.x().whileTrue(Drive.getInstance().autopilotAlign(true)); + controller.y().whileTrue(intakeCommand()); + controller.b().whileTrue(Drive.getInstance().autopilotAlign(false)); + controller.leftTrigger().whileTrue(HangCommand()); // TODO: Implement hang from armaaan + controller.rightTrigger().onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); + } + public static Command intakeCommand() { return Commands.print("Intaking"); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 50ebffd..950bd2f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,31 +1,19 @@ package frc.robot; -import frc.robot.auto.AutoSelector; - -import java.util.Optional; - -import org.littletonrobotics.junction.LogFileUtil; -import org.littletonrobotics.junction.networktables.NT4Publisher; -import org.littletonrobotics.junction.wpilog.WPILOGReader; -import org.littletonrobotics.junction.wpilog.WPILOGWriter; - import com.pathplanner.lib.commands.FollowPathCommand; -import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.epilogue.logging.EpilogueBackend; -import edu.wpi.first.hal.AllianceStationID; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.simulation.DriverStationSim; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj2.command.*; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.Controllers; +import frc.robot.auto.AutoSelector; import frc.robot.subsystems.TelemetryManager; -import frc.robot.subsystems.drive.*; +import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.vision.VisionDeviceManager; /** @@ -38,6 +26,9 @@ public class Robot extends TimedRobot { private static final CommandScheduler commandScheduler = CommandScheduler.getInstance(); private AutoSelector autoChooser; private Command autoCommand; + private static final String standardMap = "standard"; + private static final String mapTwo = "mapTwo"; + private final SendableChooser mapChooser = new SendableChooser<>(); public static final CommandXboxController controller = new CommandXboxController(Controllers.DRIVER_CONTROLLER_PORT); @@ -65,6 +56,10 @@ public Robot() { System.out.println("Log/USB mounts NOT OK"); } DriverStation.startDataLog(DataLogManager.getLog()); + + mapChooser.setDefaultOption("Standard Keybinds", standardMap); + mapChooser.addOption("Second Control Map", mapTwo); + SmartDashboard.putData("Keybinds", mapChooser); } /** @@ -123,7 +118,9 @@ public void teleopInit() { } Drive.getInstance().setDefaultCommand(Drive.getInstance().teleopCommand()); - ControlsMapping.mapTeleopCommand(); + String selectedMap = mapChooser.getSelected(); + System.out.println("Keybind selected: " + selectedMap); + ControlMap.implement(selectedMap); } /** This function is called periodically during operator control. */ @@ -137,7 +134,7 @@ public void testInit() { CommandScheduler.getInstance().cancelAll(); //map test commands - ControlsMapping.mapSysId(); + ControlMap.mapSysId(); } /** This function is called periodically during test mode. */ From d5036d86f412aa812699c8e6d8230a6f38111510 Mon Sep 17 00:00:00 2001 From: shakability Date: Mon, 2 Feb 2026 16:01:05 -0800 Subject: [PATCH 067/118] did something Uhh i forgot --- .../frc/robot/subsystems/climb/Climb.java | 20 +++++++++++++++++-- .../subsystems/climb/ClimbConstants.java | 5 +++-- 2 files changed, 21 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/climb/Climb.java b/src/main/java/frc/robot/subsystems/climb/Climb.java index 8d2e0e1..39a92f6 100644 --- a/src/main/java/frc/robot/subsystems/climb/Climb.java +++ b/src/main/java/frc/robot/subsystems/climb/Climb.java @@ -4,6 +4,7 @@ import static frc.robot.subsystems.climb.ClimbConstants.CLIMB_MOTOR_ID; import static frc.robot.subsystems.climb.ClimbConstants.END_EFFECTOR_HEIGHT; import static frc.robot.subsystems.climb.ClimbConstants.EPSILON; +import static frc.robot.subsystems.climb.ClimbConstants.GEAR_RATIO; import static frc.robot.subsystems.climb.ClimbConstants.getConfig; import static frc.robot.subsystems.climb.ClimbConstants.metersToRotations; import static frc.robot.subsystems.climb.ClimbConstants.rotationsToMeters; @@ -17,13 +18,16 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.subsystems.TelemetryManager; -import frc.robot.subsystems.climb.ClimbConstants.Heights; +import frc.robot.subsystems.climb.ClimbConstants.Setpoint; + +import edu.wpi.first.wpilibj.simulation.ElevatorSim; public class Climb extends SubsystemBase { private static Climb climbInstance; @@ -42,6 +46,18 @@ public static Climb getInstance() { private double targetHeight = END_EFFECTOR_HEIGHT; private ControlRequest request = new NeutralOut(); + private final ElevatorSim sim = new edu.wpi.first.wpilibj.simulation.ElevatorSim( + DCMotor.getKrakenX60(1), + GEAR_RATIO, + 1.13, + 0.05, + Setpoint.BASE.height, + Setpoint.UP.height, + true, + 0.0, + 0.0,0.0 + ); + private Climb() { super(); climbMotor = new TalonFX(CLIMB_MOTOR_ID); @@ -67,7 +83,7 @@ private void setRequest(ControlRequest request) { this.request = request; } - public Command moveToScoringHeight(Heights height) { + public Command moveToScoringHeight(Setpoint height) { return moveToTarget(height.height).withName(height.name() + ": Move To Height"); } diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java b/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java index 497e785..5ba54ec 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.VoltageConfigs; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; import frc.robot.Constants; @@ -27,11 +28,11 @@ public class ClimbConstants { public static final int CLIMB_MOTOR_ID = 67; // change this - public static enum Heights { + public static enum Setpoint { BASE(0.003), // small offset to prevent stalling (allegedly) UP(Units.inchesToMeters(27.0)); public final double height; - private Heights(double height) { + private Setpoint(double height) { this.height = height; } } From 25661e3ccc84807480829731acca36df3451d020 Mon Sep 17 00:00:00 2001 From: Ethan Lian Date: Mon, 2 Feb 2026 16:02:42 -0800 Subject: [PATCH 068/118] git --- src/main/java/frc/robot/ControlsMapping.java | 2 +- src/main/java/frc/robot/subsystems/indexer/Indexer.java | 5 ++++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index 40eae72..2ea272c 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -90,4 +90,4 @@ public static void mapSysId() { } })); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 8392702..dc63914 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -1,7 +1,7 @@ package frc.robot.subsystems.indexer; import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.sim.TalonFXSimState; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj2.command.Command; @@ -25,6 +25,7 @@ public static Indexer getInstance() { private ControlRequest request; private TalonFX motor; + private TalonFXSimState motorSim; private LaserCan lc, lcTwo; /** boolean that's modified by checkForBall() */ @@ -40,6 +41,7 @@ public static Indexer getInstance() { private Indexer() { super(); motor = new TalonFX(IndexerConstants.MOTOR_ID); + motorSim = motor.getSimState(); lc = new LaserCan(IndexerConstants.LASER_ID); lcTwo = new LaserCan(IndexerConstants.LASER_ID_2); @@ -64,6 +66,7 @@ private Indexer() { /* check for balls and makes sure motor is constantly running at desired speed */ public void periodic() { motor.setControl(request); + checkForBall(); // checkForBallTwo(); if (shooterReady = true) { From 780135fc1597e69b0327b90676ed3573cf482434 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Mon, 2 Feb 2026 17:00:33 -0800 Subject: [PATCH 069/118] hi it has been tested --- src/main/java/frc/robot/subsystems/shooter/Shooter.java | 4 ++-- .../java/frc/robot/subsystems/shooter/ShooterConstants.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 692c6c3..414d0cc 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -65,6 +65,7 @@ private Shooter() { ), DCMotor.getKrakenX60(1), 0.0); } TelemetryManager.getInstance().addSendable(this); + setDefaultCommand(stop()); } @Override @@ -97,7 +98,6 @@ public void simulationPeriodic() { .setRotorVelocity(topSim.getAngularVelocityRPM() / 60.0); bottomMotor.getSimState().addRotorPosition(topSim.getAngularVelocityRPM() / 60.0 * 0.020); - setDefaultCommand(stop()); } /** Replaces the request */ @@ -128,7 +128,7 @@ public Command shoot(double topSpeed, double bottomSpeed) { } public Command shoot() { - return shoot(3000 / 60.0, 3000 / 60.0); + return shoot(512, -512); } @Override diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 657827e..32d9c8f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -23,7 +23,7 @@ public static TalonFXConfiguration getConfig() { return new TalonFXConfiguration() .withSlot0(new Slot0Configs() .withKV(0.0) - .withKP(0.5) + .withKP(0.3 ) .withKI(0.001) .withKD(0.0) .withKA(0.1) From bcd22cf05b5d8daa0138744c5fb2d61d188fca1a Mon Sep 17 00:00:00 2001 From: shakability Date: Mon, 2 Feb 2026 19:15:45 -0800 Subject: [PATCH 070/118] control ..... Maping!!! --- src/main/java/frc/robot/ControlMap.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/ControlMap.java b/src/main/java/frc/robot/ControlMap.java index 5d71360..69e66fe 100644 --- a/src/main/java/frc/robot/ControlMap.java +++ b/src/main/java/frc/robot/ControlMap.java @@ -22,6 +22,9 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.subsystems.climb.*; +import frc.robot.subsystems.climb.ClimbConstants.Setpoint; + public class ControlMap { public static void implement(String key) { switch (key) { @@ -64,11 +67,10 @@ public static Command intakeCommand() { return Commands.print("Intaking"); } public static Command HangCommand() { - return Commands.print("Hanging"); + return Climb.getInstance().moveToScoringHeight(ClimbConstants.Setpoint.UP).andThen(Climb.getInstance().moveToScoringHeight(ClimbConstants.Setpoint.BASE)); } public static Command shooterCommand() { return Commands.print("Shooting"); - } //make a method that returns command, but it returns command.none, replace everything with new instance command with the command name. From 79154fa7af9e58aa5561bf3ff2515fc2ca5ccf8c Mon Sep 17 00:00:00 2001 From: shakability Date: Wed, 4 Feb 2026 16:37:37 -0800 Subject: [PATCH 071/118] moved the hang command into climb.java YAYYYYYY YAYYY!!!!!!! HAHHAhhaaaa yayyyy --- src/main/java/frc/robot/ControlMap.java | 2 +- src/main/java/frc/robot/subsystems/climb/Climb.java | 6 ++++++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/ControlMap.java b/src/main/java/frc/robot/ControlMap.java index 69e66fe..0d88ee9 100644 --- a/src/main/java/frc/robot/ControlMap.java +++ b/src/main/java/frc/robot/ControlMap.java @@ -67,7 +67,7 @@ public static Command intakeCommand() { return Commands.print("Intaking"); } public static Command HangCommand() { - return Climb.getInstance().moveToScoringHeight(ClimbConstants.Setpoint.UP).andThen(Climb.getInstance().moveToScoringHeight(ClimbConstants.Setpoint.BASE)); + return Climb.HangCommand(); } public static Command shooterCommand() { return Commands.print("Shooting"); diff --git a/src/main/java/frc/robot/subsystems/climb/Climb.java b/src/main/java/frc/robot/subsystems/climb/Climb.java index 39a92f6..046ae92 100644 --- a/src/main/java/frc/robot/subsystems/climb/Climb.java +++ b/src/main/java/frc/robot/subsystems/climb/Climb.java @@ -171,4 +171,10 @@ public void initSendable(SendableBuilder builder) { null); TelemetryManager.makeSendableTalonFX("ClimbMotor", climbMotor, builder); } + + // command + + public static Command HangCommand() { + return Climb.getInstance().moveToScoringHeight(ClimbConstants.Setpoint.UP).andThen(Climb.getInstance().moveToScoringHeight(ClimbConstants.Setpoint.BASE)); + } } \ No newline at end of file From bd867eb5455fee5d5ede18ea2f87eec36c197786 Mon Sep 17 00:00:00 2001 From: seaspace1250 <117596196+seaspace1250@users.noreply.github.com> Date: Wed, 4 Feb 2026 17:55:12 -0800 Subject: [PATCH 072/118] dfijaisdguasa fixed commands and hopefully it works please --- .../frc/robot/subsystems/intake/Intake.java | 187 ++++++++++-------- .../subsystems/intake/IntakeConstants.java | 59 +++--- 2 files changed, 143 insertions(+), 103 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 74c618c..c6e1692 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -1,23 +1,35 @@ package frc.robot.subsystems.intake; +import static frc.robot.subsystems.intake.IntakeConstants.*; + +import javax.lang.model.element.TypeElement; + import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; - +import com.ctre.phoenix6.sim.TalonFXSimState; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; -import frc.robot.subsystems.TelemetryManager;; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.subsystems.TelemetryManager; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.simulation.BatterySim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; + + public class Intake extends SubsystemBase { private static Intake intakeInstance; @@ -29,70 +41,68 @@ public static Intake getInstance() { } private final TalonFX wheelMotor; + private TalonFXSimState wheelMotorSim; private final TalonFX barMotor; + private final TalonFXSimState barMotorSim; + private double wheelSpeed; private double barPosition; private ControlRequest wheelRequest = new NeutralOut(); private ControlRequest barRequest = new NeutralOut(); - private DCMotor pl = DCMotor.getKrakenX60(1); + private SingleJointedArmSim sim; + /* + private MechanismLigament2d ligament; + + private final Mechanism2d mech2d = new Mechanism2d(20, 20); + private final MechanismRoot2d mech2droot = mech2d.getRoot("Bar Root", 10, 1); + */ + private Intake() { super(); - wheelMotor = new TalonFX(IntakeConstants.Motors.WHEEL.id); - barMotor = new TalonFX(IntakeConstants.Motors.BAR.id); - wheelMotor.getConfigurator().apply(IntakeConstants.getWheelConfig()); - barMotor.getConfigurator().apply(IntakeConstants.getBarConfig()); + wheelMotor = new TalonFX(Motors.WHEEL.id); + wheelMotorSim = wheelMotor.getSimState(); + barMotor = new TalonFX(Motors.BAR.id); + barMotorSim = barMotor.getSimState(); + wheelMotor.getConfigurator().apply(getWheelConfig()); + barMotor.getConfigurator().apply(getBarConfig()); wheelMotor.setNeutralMode(NeutralModeValue.Brake); barMotor.setNeutralMode(NeutralModeValue.Brake); //? - SmartDashboard.putData("123123", mech2d); - //TelemetryManager.getInstance().addSendable(this); - mech2droot.append(mech2dpivot); + + // SmartDashboard.putData("123123", mech2d); + // //TelemetryManager.getInstance().addSendable(this); + // mech2droot.append(mech2dpivot); + + if (Robot.isSimulation()) { + sim = new SingleJointedArmSim( + LinearSystemId.createSingleJointedArmSystem(DCMotor.getKrakenX60(1),1, 2), + DCMotor.getKrakenX60(1), + 2.0, + INTAKE_LENGTH, + BAR_POS_MIN, + BAR_POS_MAX, + true, + 0.0, + 0.0, 0.0 + ); + } + + TelemetryManager.getInstance().addSendable(this); + } - private final Mechanism2d mech2d = new Mechanism2d(20, 20); - private final MechanismRoot2d mech2droot = mech2d.getRoot("Bar Root", 10, 1); - private final MechanismLigament2d mech2dpivot = new MechanismLigament2d("Pivot", 10, 0 - ); - final SingleJointedArmSim sim = new SingleJointedArmSim( - pl, - 2.0, - 1.0, - 1.0, - IntakeConstants.BAR_POS_MIN, - IntakeConstants.BAR_POS_MAX, - true, - 0.0, - 0.0, 0.0 - ); + @Override public void periodic(){ wheelSpeed = wheelMotor.getVelocity().getValueAsDouble(); barPosition = barMotor.getPosition().getValueAsDouble(); barMotor.setControl(barRequest); wheelMotor.setControl(wheelRequest); - mech2dpivot.setAngle(barPosition); + //ligament.setAngle(barPosition); } - //---------------stop---------------- - - /** - * sends neutral request to wheel - */ - - public Command stopWheel() { - return runOnce(() -> setRequestWheel(new NeutralOut())).withName("Stopped"); - } - - /** - * sends position voltage request that attempts to keep bar in place - */ - public Command stopBar() { - return runOnce(() -> setRequestBar(new PositionVoltage(barPosition))) - .withName("Stopped"); //needs testing - } - //----------------set request--------------- private void setRequestWheel(ControlRequest request) { this.wheelRequest = request; @@ -102,33 +112,13 @@ private void setRequestBar(ControlRequest request) { this.barRequest = request; } - //----------------wheel---------------- - - /** - * sets wheel speed to intake speed - */ - - public Command setWheelIntaking() { - return setWheelSpeed(IntakeConstants.INTAKE_SPEED); - } - - /** - * sets wheel speed to the negative of intake speed (for outtaking) - * @return - */ - public Command setWheelOutaking() { - return setWheelSpeed(-IntakeConstants.INTAKE_SPEED); - } - /** - * sets the wheel speed (in rotations per second) - * @return - */ - - public Command setWheelSpeed(double speed) { - return runOnce(() -> setRequestWheel ( - new VelocityVoltage(speed)) - ).withName("wheel speed set " + (speed)); + public Command setSetpoint(double wheelSpeed, double barPosition) { + return + setWheelSpeed(wheelSpeed) + .andThen(setBarPosition(barPosition), + Commands.print("something happen")) + .withName("Setpoint reached probably"); } @@ -139,7 +129,7 @@ public Command setWheelSpeed(double speed) { * @return */ public Command setBarDown() { - return setBarPosition(IntakeConstants.BAR_POSITION_DOWN); + return setBarPosition(BAR_POSITION_DOWN); } /** @@ -147,7 +137,7 @@ public Command setBarDown() { * @return */ public Command setBarUp() { - return setBarPosition(IntakeConstants.BAR_POSITION_UP); + return setBarPosition(BAR_POSITION_UP); } /** @@ -156,17 +146,58 @@ public Command setBarUp() { * @return */ public Command setBarPosition(double position) { - double checkedPos = MathUtil.clamp(position, IntakeConstants.BAR_POS_MIN, IntakeConstants.BAR_POS_MAX); - return runOnce(() -> setRequestBar( + double checkedPos = MathUtil.clamp(position, BAR_POS_MIN, BAR_POS_MAX); + return runOnce(() -> + setRequestBar( new PositionVoltage(checkedPos)) ).withName("bar pos set" + (checkedPos)); } - + public Command setWheelSpeed(double speed) { + return runOnce(() -> setRequestWheel( + new VelocityVoltage(speed)) + ).withName("wheel speed set "+ (speed)); + } + public Command waitUntilBarIsAtPosition(double target) { + return Commands.waitUntil(() -> Math.abs(target - barPosition) < BAR_EPSILON); + } + + public Command waitUntilWheelIsAtSpeed(double target) { + return Commands.waitUntil(() -> Math.abs(target - wheelSpeed) < WHEEL_EPSILON); + } + @Override public void simulationPeriodic() { - sim.setInput(1.0); + barMotor.getSimState().setSupplyVoltage(12.0); + sim.setInput(barMotor.getSimState().getMotorVoltage()); sim.update(0.020); - System.out.println("simulationperiodic"); + barMotor.getSimState() + .setRawRotorPosition(sim.getAngleRads() * (1 / Constants.TAU)); + + + + } + + public void initSendable(SendableBuilder builder){ + super.initSendable(builder); + builder.addDoubleProperty("Position", () -> barPosition, null); + TelemetryManager.makeSendableTalonFX("Intake123", barMotor, builder); + + } + + + public Command intake() { + return setSetpoint(INTAKE_SPEED, BAR_POSITION_DOWN) + .andThen(waitUntilBarIsAtPosition(BAR_POSITION_DOWN)); + } + + public Command outtake() { + return setSetpoint(-INTAKE_SPEED, BAR_POSITION_DOWN) + .andThen(waitUntilBarIsAtPosition(BAR_POSITION_DOWN)); + } + + public Command stow() { + return setSetpoint(0.0, BAR_POSITION_UP) + .andThen(waitUntilBarIsAtPosition(BAR_POSITION_UP)); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 2cc56a8..8fc8c03 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -1,19 +1,26 @@ package frc.robot.subsystems.intake; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.VoltageConfigs; import com.ctre.phoenix6.signals.GravityTypeValue; +import edu.wpi.first.units.Units; + public class IntakeConstants { + public static final double BAR_EPSILON = Units.Degrees.of(5).in(Units.Rotations); + public static final double WHEEL_EPSILON = 0.5; // rotations per second public static final double INTAKE_SPEED = 20; //rotations per second? // public static final double BAR_VOLTAGE = 4.0; //? - public static final double BAR_POSITION_DOWN = 0.30; + public static final double BAR_POSITION_DOWN = 0.33; public static final double BAR_POSITION_UP = 0.0; - public static final double BAR_GEAR_RATIO = 1.0; + public static final double BAR_GEAR_RATIO = 30.0; public static final double BAR_POS_MIN = 0.0; public static final double BAR_POS_MAX = 0.30; + public static final double INTAKE_MASS = 10.0; // kg, ideally + public static final double INTAKE_LENGTH = 0.5; //m, hopefully //etc public static enum Motors { //TODO: set motor ids; use separate file for ports? @@ -37,34 +44,36 @@ private Motors(int id) { public static TalonFXConfiguration getWheelConfig() { //TODO: values return new TalonFXConfiguration() - .withSlot0(new Slot0Configs() - .withKV(0.0) - .withKP(0.3) - .withKI(0.0) - .withKD(0.0)) - .withCurrentLimits(new CurrentLimitsConfigs() - .withStatorCurrentLimit(30) - .withSupplyCurrentLimit(30)) - .withVoltage(new VoltageConfigs() - .withPeakForwardVoltage(12.0) - .withPeakReverseVoltage(-12.0)); + .withSlot0(new Slot0Configs() + .withKV(0.0) + .withKP(1.0) + .withKI(0.0) + .withKD(0.0)) + .withCurrentLimits(new CurrentLimitsConfigs() + .withStatorCurrentLimit(30) + .withSupplyCurrentLimit(30)) + .withVoltage(new VoltageConfigs() + .withPeakForwardVoltage(12.0) + .withPeakReverseVoltage(-12.0)); } public static TalonFXConfiguration getBarConfig() { //TODO: values return new TalonFXConfiguration() - .withSlot0(new Slot0Configs() - .withKV(0.0) - .withKP(0.3) - .withKI(0.0) - .withKD(0.0) - .withKG(0.0).withGravityType(GravityTypeValue.Arm_Cosine)) - .withCurrentLimits(new CurrentLimitsConfigs() - .withStatorCurrentLimit(30) - .withSupplyCurrentLimit(30)) - .withVoltage(new VoltageConfigs() - .withPeakForwardVoltage(12.0) - .withPeakReverseVoltage(-12.0)); + .withSlot0(new Slot0Configs() + .withKV(0.0) + .withKP(1.0) + .withKI(0.0) + .withKD(0.0) + .withKG(0.0).withGravityType(GravityTypeValue.Arm_Cosine)) + .withCurrentLimits(new CurrentLimitsConfigs() + .withStatorCurrentLimit(30) + .withSupplyCurrentLimit(30)) + .withVoltage(new VoltageConfigs() + .withPeakForwardVoltage(12.0) + .withPeakReverseVoltage(-12.0)) + .withFeedback(new FeedbackConfigs() + .withSensorToMechanismRatio(BAR_GEAR_RATIO)); } } From 4435c064b490c184e7df926b8cac68edab0e3028 Mon Sep 17 00:00:00 2001 From: Arvind Venkatesh Date: Mon, 9 Feb 2026 19:14:39 -0800 Subject: [PATCH 073/118] feat: calc shot from rusthounds --- .../frc/robot/subsystems/drive/Drive.java | 7 + .../subsystems/shooter/ShooterConstants.java | 6 + .../subsystems/shooter/ShotCalculator.java | 69 +++++ .../shooter/houndlib/BallConstants.java | 38 +++ .../shooter/houndlib/BallPhysics.java | 174 ++++++++++++ .../shooter/houndlib/BallState.java | 19 ++ .../houndlib/ChassisAccelerations.java | 48 ++++ .../houndlib/ShootOnTheFlyCalculator.java | 260 ++++++++++++++++++ 8 files changed, 621 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/houndlib/BallConstants.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/houndlib/BallPhysics.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/houndlib/BallState.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/houndlib/ChassisAccelerations.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/houndlib/ShootOnTheFlyCalculator.java diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 8c09c32..8d27f6b 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -42,6 +42,7 @@ public static Drive getInstance() { } private SwerveDriveState lastReadState; + private SwerveDriveState prevReadState; public static final SwerveRequest.FieldCentric teleopRequest = new SwerveRequest.FieldCentric(); public SwerveRequest driveRequest = teleopRequest; @@ -54,6 +55,7 @@ public static Drive getInstance() { private Drive() { lastReadState = drivetrain.getState(); + prevReadState = lastReadState; drivetrain.setDefaultCommand(drivetrain.applyRequest(() -> { return driveRequest; })); @@ -93,6 +95,7 @@ public CtreDrive getCtreDrive() { @Override public void periodic() { + prevReadState = lastReadState; lastReadState = drivetrain.getState(); outputTelemetry(); } @@ -123,6 +126,10 @@ public ChassisSpeeds getFieldSpeeds() { return ChassisSpeeds.fromRobotRelativeSpeeds(lastReadState.Speeds, lastReadState.Pose.getRotation()); } + public ChassisSpeeds getPrevFieldSpeeds() { + return ChassisSpeeds.fromRobotRelativeSpeeds(prevReadState.Speeds, prevReadState.Pose.getRotation()); + } + /** * Switches the swerve request *

Please do not the new swerve request every 20 ms

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 657827e..c75c2d2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -5,9 +5,15 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.VoltageConfigs; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; + public final class ShooterConstants { public static final double GEAR_RATIO = 1; + public static final Transform3d OFFSET = new Transform3d(); + public static final InterpolatingDoubleTreeMap DISTANCE_TO_SHOT_SPEED = new InterpolatingDoubleTreeMap(); + /** Motor ids */ public static enum Motors { TOP(12), diff --git a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java new file mode 100644 index 0000000..8fcd951 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java @@ -0,0 +1,69 @@ +package frc.robot.subsystems.shooter; + +import frc.robot.subsystems.shooter.houndlib.ChassisAccelerations; +import frc.robot.subsystems.shooter.houndlib.ShootOnTheFlyCalculator; +import frc.robot.subsystems.shooter.houndlib.ShootOnTheFlyCalculator.InterceptSolution; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.subsystems.drive.Drive; + +// stores current target and actively computes effective target +public class ShotCalculator extends SubsystemBase { + private final Drive drivetrain; + + private Pose3d currentEffectiveTargetPose = Pose3d.kZero; + + private double currentEffectiveYaw; + + private InterceptSolution currentInterceptSolution; + + private Pose3d targetLocation = new Pose3d(); + + private double targetDistance = 0.0; + + private double targetSpeedRps = 8; + + public ShotCalculator() { + this.drivetrain = Drive.getInstance(); + } + + @Override + public void periodic() { + Pose2d drivetrainPose = drivetrain.getPose(); + + targetDistance = drivetrainPose.getTranslation().getDistance(targetLocation.toPose2d().getTranslation()); + targetSpeedRps = ShooterConstants.DISTANCE_TO_SHOT_SPEED.get(targetDistance); + + Pose3d shooterPose = new Pose3d(drivetrainPose).plus(ShooterConstants.OFFSET); + + ChassisSpeeds drivetrainSpeeds = drivetrain.getFieldSpeeds(); + ChassisAccelerations drivetrainAccelerations = new ChassisAccelerations(drivetrainSpeeds, drivetrain.getPrevFieldSpeeds(), Constants.DT); + + currentInterceptSolution = ShootOnTheFlyCalculator.solveShootOnTheFly(shooterPose, targetLocation, + drivetrainSpeeds, drivetrainAccelerations, targetSpeedRps, + 5, 0.01); + + currentEffectiveTargetPose = currentInterceptSolution.effectiveTargetPose(); + currentEffectiveYaw = currentInterceptSolution.requiredYaw(); + } + + public void setTarget(Pose3d targetLocation, double targetSpeedRps) { + this.targetLocation = targetLocation; + this.targetSpeedRps = targetSpeedRps; + } + + public Pose3d getCurrentEffectiveTargetPose() { + return currentEffectiveTargetPose; + } + + public double getCurrentEffectiveYaw() { + return currentEffectiveYaw; + } + + public InterceptSolution getInterceptSolution() { + return currentInterceptSolution; + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/houndlib/BallConstants.java b/src/main/java/frc/robot/subsystems/shooter/houndlib/BallConstants.java new file mode 100644 index 0000000..db9135e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/houndlib/BallConstants.java @@ -0,0 +1,38 @@ +package frc.robot.subsystems.shooter.houndlib; + +public class BallConstants { + public final double mass; + public final double radius; + public final double area; + + public final double rho; + public final double cd; + public final double clGain; + public final double clMax; + + public final double gravity; + public final double spinDecayTau; + + public BallConstants( + double mass, + double radius, + double rho, + double cd, + double clGain, + double clMax, + double gravity, + double spinDecayTau) { + + this.mass = mass; + this.radius = radius; + this.area = Math.PI * radius * radius; + + this.rho = rho; + this.cd = cd; + this.clGain = clGain; + this.clMax = clMax; + + this.gravity = gravity; + this.spinDecayTau = spinDecayTau; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/houndlib/BallPhysics.java b/src/main/java/frc/robot/subsystems/shooter/houndlib/BallPhysics.java new file mode 100644 index 0000000..9d9a9b8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/houndlib/BallPhysics.java @@ -0,0 +1,174 @@ +package frc.robot.subsystems.shooter.houndlib; + +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; + +public final class BallPhysics { + public static final double GRAVITY = 9.81; + + public record ShotSolution( + double launchPitchRad, + double launchSpeed, + double flightTimeSeconds) { + } + + private BallPhysics() { + } + + private static Translation3d gravityForce(BallConstants c) { + return new Translation3d(0, 0, -c.mass * c.gravity); + } + + private static Translation3d dragForce( + Translation3d v, BallConstants c) { + + double speed = v.getNorm(); + if (speed < 1e-6) + return new Translation3d(); + + double scale = -0.5 * c.rho * c.cd * c.area * speed; + return v.times(scale); + } + + private static Translation3d magnusForce( + Translation3d v, Translation3d omega, BallConstants c) { + + double speed = v.getNorm(); + double wMag = omega.getNorm(); + if (speed < 1e-6 || wMag < 1e-6) + return new Translation3d(); + + double spinRatio = wMag * c.radius / speed; + double cl = Math.min(c.clGain * spinRatio, c.clMax); + + Translation3d vHat = v.div(speed); + Translation3d wHat = omega.div(wMag); + + Translation3d direction = new Translation3d(Vector.cross(wHat.toVector(), vHat.toVector())); + double magnitude = 0.5 * c.rho * cl * c.area * speed * speed; + + return direction.times(magnitude); + } + + private static Rotation3d integrateRotation( + Rotation3d current, + Translation3d omega, + double dt) { + + Rotation3d delta = new Rotation3d( + omega.getX() * dt, + omega.getY() * dt, + omega.getZ() * dt); + + return current.plus(delta); + } + + public static void step( + BallState s, BallConstants c, double dt) { + + Translation3d force = gravityForce(c) + .plus(dragForce(s.velocity, c)) + .plus(magnusForce(s.velocity, s.omega, c)); + + Translation3d accel = force.div(c.mass); + + double decay = Math.exp(-dt / c.spinDecayTau); + s.omega = s.omega.times(decay); + + s.velocity = s.velocity.plus(accel.times(dt)); + + s.pose = new Pose3d(s.pose.getTranslation().plus(s.velocity.times(dt)), + integrateRotation(s.pose.getRotation(), s.omega, dt)); + } + + public static ShotSolution solveBallisticWithIncomingAngle( + Pose3d shooterPose, + Pose3d targetPose, + double incomingPitchRad) { + + Translation3d s = shooterPose.getTranslation(); + Translation3d t = targetPose.getTranslation(); + + double dx = t.getX() - s.getX(); + double dy = t.getY() - s.getY(); + double dz = t.getZ() - s.getZ(); + + double d = Math.hypot(dx, dy); + if (d < 1e-9) { + throw new IllegalArgumentException("Horizontal distance too small"); + } + + double tanThetaT = Math.tan(incomingPitchRad); + + double rhs = dz - d * tanThetaT; + if (rhs <= 0) { + throw new IllegalArgumentException( + "No physical solution: dz - d*tan(thetaT) must be > 0"); + } + + double T = Math.sqrt(2.0 * rhs / GRAVITY); + + double vHoriz = d / T; + double vZ0 = vHoriz * tanThetaT + GRAVITY * T; + + double launchSpeed = Math.hypot(vHoriz, vZ0); + double launchPitch = Math.atan2(vZ0, vHoriz); + + return new ShotSolution(launchPitch, launchSpeed, T); + } + + public static ShotSolution solveBallisticWithSpeed( + Pose3d shooterPose, + Pose3d targetPose, + double launchSpeed) { + + Translation3d s = shooterPose.getTranslation(); + Translation3d t = targetPose.getTranslation(); + + double dx = t.getX() - s.getX(); + double dy = t.getY() - s.getY(); + double dz = t.getZ() - s.getZ(); + + double d = Math.hypot(dx, dy); + if (d < 1e-9) { + throw new IllegalArgumentException("Horizontal distance too small"); + } + + double v2 = launchSpeed * launchSpeed; + double g = GRAVITY; + + double discriminant = v2 * v2 - g * (g * d * d + 2.0 * dz * v2); + if (discriminant < 0) { + return new ShotSolution(0, 0, 0); + } + + // LOW-ARC solution (use +Math.sqrt(...) for high arc) + double tanTheta = (v2 + Math.sqrt(discriminant)) / (g * d); + + double launchPitch = Math.atan(tanTheta); + + double vHoriz = launchSpeed * Math.cos(launchPitch); + double time = d / vHoriz; + + return new ShotSolution(launchPitch, launchSpeed, time); + } + + public static double minSpeedForAnyArc( + Pose3d shooterPose, + Pose3d targetPose) { + + Translation3d s = shooterPose.getTranslation(); + Translation3d t = targetPose.getTranslation(); + + double dx = t.getX() - s.getX(); + double dy = t.getY() - s.getY(); + double dz = t.getZ() - s.getZ(); + + double d = Math.hypot(dx, dy); + + return Math.sqrt( + GRAVITY * (Math.hypot(d, dz) + dz)); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/houndlib/BallState.java b/src/main/java/frc/robot/subsystems/shooter/houndlib/BallState.java new file mode 100644 index 0000000..3f8fcc4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/houndlib/BallState.java @@ -0,0 +1,19 @@ +package frc.robot.subsystems.shooter.houndlib; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Translation3d; + +public class BallState { + public Pose3d pose; + public Translation3d velocity; + public Translation3d omega; // rad/s + + public BallState( + Pose3d position, + Translation3d velocity, + Translation3d omega) { + this.pose = position; + this.velocity = velocity; + this.omega = omega; + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/houndlib/ChassisAccelerations.java b/src/main/java/frc/robot/subsystems/shooter/houndlib/ChassisAccelerations.java new file mode 100644 index 0000000..1cc18ec --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/houndlib/ChassisAccelerations.java @@ -0,0 +1,48 @@ +package frc.robot.subsystems.shooter.houndlib; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +/** + * Represents the complete acceleration data of a chassis. Counterpart to + * {@link ChassisSpeeds}. Used when acceleration data is needed for some + * computation (in 2024, shooting on the move). + */ +public class ChassisAccelerations { + public double axMetersPerSecondSquared; + public double ayMetersPerSecondSquared; + public double omegaRadiansPerSecondSquared; + + /** + * Creates a {@link ChassisAccelerations} object based on given acceleration + * data. + * + * @param axMetersPerSecondSquared the acceleration in the x direction, in + * m/s^2 + * @param ayMetersPerSecondSquared the acceleration in the y direction, in + * m/s^2 + * @param omegaRadiansPerSecondSquared the rotational acceleration of the + * chassis (CCW+), in rad/s^2 + */ + public ChassisAccelerations(double axMetersPerSecondSquared, double ayMetersPerSecondSquared, + double omegaRadiansPerSecondSquared) { + this.axMetersPerSecondSquared = axMetersPerSecondSquared; + this.ayMetersPerSecondSquared = ayMetersPerSecondSquared; + this.omegaRadiansPerSecondSquared = omegaRadiansPerSecondSquared; + } + + /** + * Creates a {@link ChassisAccelerations} object based on the current + * {@link ChassisSpeeds}, the {@link ChassisSpeeds} from the previous iteration, + * and the time between iterations. + * + * @param speed the current speed of the chassis + * @param previousSpeed the previous speed of the chassis + * @param dt the time elapsed between measurements (your loop time), + * in seconds + */ + public ChassisAccelerations(ChassisSpeeds speed, ChassisSpeeds previousSpeed, double dt) { + this.axMetersPerSecondSquared = (speed.vxMetersPerSecond - previousSpeed.vxMetersPerSecond) / dt; + this.ayMetersPerSecondSquared = (speed.vyMetersPerSecond - previousSpeed.vyMetersPerSecond) / dt; + this.omegaRadiansPerSecondSquared = (speed.omegaRadiansPerSecond - previousSpeed.omegaRadiansPerSecond) / dt; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/houndlib/ShootOnTheFlyCalculator.java b/src/main/java/frc/robot/subsystems/shooter/houndlib/ShootOnTheFlyCalculator.java new file mode 100644 index 0000000..76b4ef2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/houndlib/ShootOnTheFlyCalculator.java @@ -0,0 +1,260 @@ +package frc.robot.subsystems.shooter.houndlib; + +import java.util.function.Function; + +import frc.robot.subsystems.shooter.houndlib.BallPhysics.ShotSolution; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.interpolation.InterpolatingTreeMap; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +/** + * Provides static methods to calculate the effective target position to aim for + * when shooting on the fly. + */ +public class ShootOnTheFlyCalculator { + /** + * Calculates the time it will take for a projectile to reach a target given the + * robot's pose and the target's pose, and a function describing the + * projectile's velocity. This allows you to have a shooter that may shoot a + * projectile at varying speeds given varying distances from the targe. + * + * @see #calculateEffectiveTargetLocation(Pose2d, Pose3d, ChassisSpeeds, + * ChassisAccelerations, Function, double, double) + * + * @param robotPose the current pose of the robot + * @param targetPose the 3D pose of the target. this should + * be the center of the target, if that + * makes sense (2024 game), but could also + * be offset if desired. this may also be + * deeper into the target area if required + * (2020 game). this should take into + * account any necessary field reflections + * before being passed. + * @param xyDistanceToProjectileVelocity a function that takes in the + * xy-distance from the robot to the goal, + * and returns the velocity of the shot + * projectile in m/s. + * @return the time it will take for the projectile to reach the target + */ + public static double getTimeToShoot(Pose2d robotPose, Pose3d targetPose, + Function xyDistanceToProjectileVelocity) { + Transform3d diff = new Pose3d(robotPose).minus(targetPose); + double xyDistance = new Translation2d(diff.getX(), diff.getY()).getNorm(); + double distance = diff.getTranslation().getNorm(); + double projectileVelocity = xyDistanceToProjectileVelocity.apply(xyDistance); + double time = distance / projectileVelocity; + return time; + } + + public static double getTimeToShoot( + Pose3d shooterPose, + Pose3d targetPose, + double launchSpeed, + double launchPitchRad) { + Translation3d s = shooterPose.getTranslation(); + Translation3d t = targetPose.getTranslation(); + + double dx = t.getX() - s.getX(); + double dy = t.getY() - s.getY(); + + double horizontalDist = Math.hypot(dx, dy); + double vHoriz = launchSpeed * Math.cos(launchPitchRad); + + if (vHoriz <= 1e-6) { + throw new IllegalArgumentException("Horizontal velocity too small"); + } + + return horizontalDist / vHoriz; + } + + /** + * Calculates the effective position of the target given the position, velocity, + * and acceleration of the robot's chassis. Does not account for air resistance, + * though this is very often unnecessary. + * + *

+ * + * When shooting a projectile while moving, the projectile inherits the + * translational velocity of the chassis. Shooting on the fly can be + * accomplished by targeting a "virtual" goal if you are moving, which acts to + * negate the forces applied on the projectile due to the movement of the + * chassis. + * + *

+ * + * An iterative approach (see {@code goalPositionIterations}) is required + * because the time taken for the projectile to travel to the target will change + * given a different target location. To account for this, we re-simulate the + * projectile's travel with a new shot time derived from the new virtual goal + * position several times. + * + * + *

+ * + * When solving this problem mathematically, the acceleration of the chassis + * does not matter in the final velocities of the projectile (it will not + * inherit the acceleration of the chassis). The + * {@code accelerationCompensationFactor} is necessary due to other errors: + * + * (1) the time taken to move the projectile through a shooter is non-zero, so + * (2) if the chassis is accelerating, the velocity of the chassis by the time + * the projectile leaves the robot will have changed. + * + * To account for this, we multiply the acceleration at the time of the shot + * command by a specific value and add it to the velocity at the time of + * the shot. This value is based on the time delta between + * commanding a shot and the shot actually leaving the shooter, meaning that the + * effective velocity generated is the velocity as the projectile leaves the + * shooter. This is extremely complicated to determine theoretically, so if you + * find acceleration to be causing shot inaccuracies, find a value that provides + * adequate compensation (should be around [0,2]). + * + *

+ * + * The easiest way to create the {@code xyDistanceToProjectileVelocity} lambda + * function is as follows: + * + * If the speed of your shooter is always constant, simply create a lambda + * expression that always returns the same value. + * + * If the speed of your shooter is controlled using an + * {@link InterpolatingTreeMap} based on distance from the goal, simply get the + * appropriate shooter speed from that map, and multiply it by some constant + * that describes how fast the projectile moves given a shooter speed. This can + * be calculated experimentally by pointing a camera at the shooter and + * calculating the speed of the projectile based on the distance travelled in n + * frames. If you find the relationship between shooter speed and projectile + * speed is not constant, you can create a second {@link InterpolatingTreeMap}, + * or define it as an equation. + * + * @param robotPose the current pose of the robot + * @param targetPose the 3D pose of the target. this should + * be the center of the target, if that + * makes sense (2024 game), but could also + * be offset if desired. this may also be + * deeper into the target area if required + * (2020 game). this should take into + * account any necessary field reflections + * before being passed. + * @param fieldRelRobotVelocity the field-relative velocity of the + * robot's chassis + * @param fieldRelRobotAcceleration the field-relative acceleration of the + * robot's chassis + * @param xyDistanceToProjectileVelocity a function that takes in the + * xy-distance from the robot to the goal, + * and returns the velocity of the shot + * projectile in m/s. + * @param goalPositionIterations the number of iterations to use when + * iteratively solving for the pose of the + * target. a higher number of iterations + * will increase the accuracy of the + * result, but will also reduce + * performance. + * @param accelerationCompensationFactor the value to multiply the acceleration + * @return + */ + public static Pose3d calculateEffectiveTargetLocation( + Pose2d robotPose, Pose3d targetPose, + ChassisSpeeds fieldRelRobotVelocity, + ChassisAccelerations fieldRelRobotAcceleration, + Function xyDistanceToProjectileVelocity, + double goalPositionIterations, + double accelerationCompensationFactor) { + + double shotTime = getTimeToShoot(robotPose, targetPose, xyDistanceToProjectileVelocity); + + Pose3d correctedTargetPose = new Pose3d(); + for (int i = 0; i < goalPositionIterations; i++) { + double virtualGoalX = targetPose.getX() + - shotTime * (fieldRelRobotVelocity.vxMetersPerSecond + + fieldRelRobotAcceleration.axMetersPerSecondSquared + * accelerationCompensationFactor); + double virtualGoalY = targetPose.getY() + - shotTime * (fieldRelRobotVelocity.vyMetersPerSecond + + fieldRelRobotAcceleration.ayMetersPerSecondSquared + * accelerationCompensationFactor); + + correctedTargetPose = new Pose3d(virtualGoalX, virtualGoalY, targetPose.getZ(), + targetPose.getRotation()); + + double newShotTime = getTimeToShoot(robotPose, correctedTargetPose, xyDistanceToProjectileVelocity); + + shotTime = newShotTime; + if (Math.abs(newShotTime - shotTime) <= 0.010) { + break; + } + } + + return correctedTargetPose; + } + + public record InterceptSolution( + Pose3d effectiveTargetPose, + double launchPitchRad, + double launchSpeed, + double flightTime, + double requiredYaw) { + } + + public static InterceptSolution solveShootOnTheFly( + Pose3d shooterPose, + Pose3d targetPose, + ChassisSpeeds fieldRelRobotVelocity, + ChassisAccelerations fieldRelRobotAcceleration, + double targetSpeedRps, + int maxIterations, + double timeTolerance) { + + ShotSolution sol = BallPhysics.solveBallisticWithSpeed( + shooterPose, + targetPose, + targetSpeedRps); + + double t = sol.flightTimeSeconds(); + Pose3d effectiveTarget = targetPose; + + for (int i = 0; i < maxIterations; i++) { + + double dx = fieldRelRobotVelocity.vxMetersPerSecond * t; + // + 0.5 * fieldRelRobotAcceleration.axMetersPerSecondSquared * t * t; + + double dy = fieldRelRobotVelocity.vyMetersPerSecond * t; + // + 0.5 * fieldRelRobotAcceleration.ayMetersPerSecondSquared * t * t; + + effectiveTarget = new Pose3d( + targetPose.getX() - dx, + targetPose.getY() - dy, + targetPose.getZ(), + targetPose.getRotation()); + + ShotSolution newSol = BallPhysics.solveBallisticWithSpeed( + shooterPose, + effectiveTarget, + targetSpeedRps); + + if (Math.abs(newSol.flightTimeSeconds() - t) < timeTolerance) { + return new InterceptSolution( + effectiveTarget, + newSol.launchPitchRad(), + newSol.launchSpeed(), + newSol.flightTimeSeconds(), + 0); + } + + sol = newSol; + t = newSol.flightTimeSeconds(); + } + + return new InterceptSolution( + effectiveTarget, + sol.launchPitchRad(), + sol.launchSpeed(), + sol.flightTimeSeconds(), + 0); + } +} \ No newline at end of file From 4d789903ea3227c716e188f730526d0242842ba8 Mon Sep 17 00:00:00 2001 From: audreywtu Date: Mon, 9 Feb 2026 19:18:12 -0800 Subject: [PATCH 074/118] feat: heatmap --- .../robot/subsystems/drive/heatmap/README.md | 18 + .../subsystems/drive/heatmap/assets/style.css | 240 ++++++++ .../robot/subsystems/drive/heatmap/field.png | Bin 0 -> 406169 bytes .../robot/subsystems/drive/heatmap/main.py | 565 ++++++++++++++++++ .../drive/heatmap/saved_layout.json | 282 +++++++++ 5 files changed, 1105 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/drive/heatmap/README.md create mode 100644 src/main/java/frc/robot/subsystems/drive/heatmap/assets/style.css create mode 100644 src/main/java/frc/robot/subsystems/drive/heatmap/field.png create mode 100644 src/main/java/frc/robot/subsystems/drive/heatmap/main.py create mode 100644 src/main/java/frc/robot/subsystems/drive/heatmap/saved_layout.json diff --git a/src/main/java/frc/robot/subsystems/drive/heatmap/README.md b/src/main/java/frc/robot/subsystems/drive/heatmap/README.md new file mode 100644 index 0000000..885f690 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/heatmap/README.md @@ -0,0 +1,18 @@ +<<<<<<< HEAD +# 1458Framework2026_2 +A framework for team 1458's 2026 code. Will be using the command-based framework from WPILIB +======= +# Cycle Time Heatmap + +Welcome to the Cycle Time Heatmap project! + +This project is designed to help visualize and analyze cycle times using a heatmap +To run the code, follow these steps: +1. Clone the repository to your local machine. +2. Run the main python script +3. Install any missing dependencies using pip. +4. Enjoy visualizing your cycle times! + +Note: this project is currently under development, so documentation may be incomplete. Stay tuned for updates! + +>>>>>>> other_repo/main diff --git a/src/main/java/frc/robot/subsystems/drive/heatmap/assets/style.css b/src/main/java/frc/robot/subsystems/drive/heatmap/assets/style.css new file mode 100644 index 0000000..ccc5548 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/heatmap/assets/style.css @@ -0,0 +1,240 @@ +:root { + --bg: #0f1115; + --panel: #1c2128; + --panel-alt: #242b33; + --border: #303a44; + --accent: #3d8bfd; + --accent-glow: 0 0 0 4px rgba(61, 139, 253, 0.25); + --danger: #e55353; + --text: #e6edf3; + --text-dim: #9da7b3; + --radius: 10px; + --transition: 160ms ease; + --font-stack: "Inter", system-ui, "Segoe UI", Roboto, Arial, sans-serif; +} + +html, +body { + background: var(--bg); + color: var(--text); + font-family: var(--font-stack); + margin: 0; + padding: 0; +} + +h1, +h2, +h3, +h4 { + font-weight: 600; + letter-spacing: 0.5px; +} + +.container { + max-width: 1280px; + margin: 28px auto 80px; + padding: 0 28px; +} + +.toolbar { + display: flex; + flex-wrap: wrap; + gap: 10px; + align-items: center; + margin-bottom: 14px; +} + +.btn { + background: linear-gradient(145deg, var(--panel), var(--panel-alt)); + color: var(--text); + border: 1px solid var(--border); + padding: 8px 16px; + border-radius: var(--radius); + font-size: 14px; + font-weight: 500; + cursor: pointer; + line-height: 1.1; + position: relative; + transition: var(--transition); + display: inline-flex; + align-items: center; + gap: 6px; +} +.btn:hover:not(.active) { + border-color: var(--accent); + color: #fff; +} +.btn:active { + transform: translateY(1px); +} +.btn.active { + background: var(--accent); + border-color: var(--accent); + color: #fff; + box-shadow: var(--accent-glow); +} +.btn.danger { + background: linear-gradient(145deg, #3a1212, #501b1b); + border-color: #6b2222; +} +.btn.danger:hover { + background: #822828; +} +.btn.outline { + background: transparent; +} + +.status-label { + font-weight: 600; + font-size: 14px; +} + +.panel-card { + background: var(--panel); + border: 1px solid var(--border); + border-radius: var(--radius); + padding: 14px 18px 24px; + position: relative; + overflow: hidden; +} +.panel-card:before { + content: ""; + position: absolute; + inset: 0; + pointer-events: none; + background: linear-gradient( + 120deg, + rgba(61, 139, 253, 0.12), + transparent 35% + ); + opacity: 0.6; +} + +.graph-wrapper { + margin-top: 4px; +} + +.footer { + margin-top: 42px; + padding: 32px 0 60px; + font-size: 13px; + color: var(--text-dim); + text-align: center; + border-top: 1px solid var(--border); + background: radial-gradient( + circle at 50% 10%, + rgba(61, 139, 253, 0.08), + transparent 60% + ); +} +.footer a { + color: var(--accent); + text-decoration: none; +} +.footer a:hover { + text-decoration: underline; +} + +.inline-badge { + background: #223041; + border: 1px solid #314151; + padding: 2px 8px 3px; + border-radius: 6px; + font-size: 12px; + margin-left: 10px; + letter-spacing: 0.5px; +} + +/* Kinematics Panel */ +.kinematics-panel { + background: var(--panel); + border: 1px solid var(--border); + border-radius: var(--radius); + padding: 12px 18px; + margin-bottom: 14px; +} + +.kinematics-header { + margin: 0 0 10px 0; + font-size: 14px; + font-weight: 600; + color: var(--text); +} + +.kinematics-inputs { + display: flex; + flex-wrap: wrap; + gap: 16px; +} + +.input-group { + display: flex; + align-items: center; + gap: 8px; +} + +.input-group label { + font-size: 13px; + color: var(--text-dim); + min-width: 100px; +} + +.kinematics-input { + background: var(--panel-alt); + color: var(--text); + border: 1px solid var(--border); + border-radius: 6px; + padding: 6px 10px; + font-size: 14px; + width: 90px; + transition: var(--transition); +} + +.kinematics-input:focus { + outline: none; + border-color: var(--accent); + box-shadow: var(--accent-glow); +} + +.kinematics-input:hover { + border-color: var(--accent); +} + +.input-unit { + font-size: 12px; + color: var(--text-dim); + min-width: 35px; +} + +/* Graph tweaks */ +.js-plotly-plot .plotly .modebar { + background: rgba(28, 33, 40, 0.6); + border: 1px solid var(--border); + border-radius: 6px; +} + +@media (max-width: 860px) { + .container { + padding: 0 18px; + } + .toolbar { + gap: 8px; + } + .btn { + padding: 8px 14px; + font-size: 13px; + } + .panel-card { + padding: 12px 14px 20px; + } + .kinematics-inputs { + flex-direction: column; + gap: 10px; + } + .input-group { + width: 100%; + } + .input-group label { + min-width: 110px; + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/heatmap/field.png b/src/main/java/frc/robot/subsystems/drive/heatmap/field.png new file mode 100644 index 0000000000000000000000000000000000000000..93be03a3de53afd8959f9bb96655bf176eaed598 GIT binary patch literal 406169 zcmY&=1yoc~+b$@fQYuQ9fPjd!#4vz>fJh6H!T>`^cZ?DW0xG2-f;7@n(kTr?NO#B3 z-7zr3-Q)M&|G#&waWQk|%sKn){l@b?@3Z}+p{7W7gYE_%9v+#}v!^fc@CbeI@UDbj zBLYV%g6Q|bAAHvrin4eGJq&B$1A&!{stg`pQ3T0_DIxfL-RYUWD<0l$H{3t`8B1m_ z@PlA$Z9O+VRTT)_(Vo}T+|kT}*UR1soQ;PEmG*Knh1*)Vv6xv{SvyE_?$p$CvRIo- za_T--SAsQdGS=?zh{6?lB}=Y+?*hMe4d`3yq-e5jxJVw0^;K0eEfoZf`X60 z8IN2M4sNDij~rat{(FI^7Orp?YbQ5rM+X+%1x?Ky-Q6Tv-QBItA(p0=kIl@@A3riP z6*7M$AYg9#$kbd&;E}nIkh$n%0Wm>K0W;SB-rmjH@_+B|;QHTb029K8`-V?|mmfD| z+co|9{7ze7J%9KLeBc&k^vf;NAbs3~rkKXLcEqU^YC z0yt@ct6Pl`12gjp(mUftV^foCeVrz}ce#?r!u576T2(44sXFnybH((DPm6PCS{j?3 zqa$i=4lhwHOZvnsDL62YFt4EC=Zl;93v9Yly`8ryv246E^1BaTCP~GJ& zcEh@zwDC0)M?Yq7M+!$_x>M;Eff+dI1|AeIoi?NwuR&_%RHA3ksxG3}_A$Z3hkCDG zJ+0EdGB~7&QEOkh!_6FI!0VW65%po+p<;4ue7tdFgetDvvOZzCba?pXkWJZ)c%iXk z#iV?`PV>YB-SdpwDQhL{L-1bbIP381p*g;gp!Fk2lC)p=^t6#%&9tw6szyUo(-jd> zQBB?$XE(PG&CPPBbrIlVJUl!p8eDj^Z9QTWkI9D)2Keh$(PpuXl>5a(ysH;gH8ml~ z2xhpWV`04lJT&zcFWlYzE|}c8gygtE?&uq_dWKENRDc=*}@W`O0J~i zvLc50R3084yPM-e3`|V^t}}dw1_n(_T``*ZI{mMOoxHt6S+4yCi__ZLicPQ#f#|N% z46hWEz0{e>+))x#$y06SZ^2@z%FB&H#x$jFFBzRvu4$GQET%sLfHEtmzueKvlA)$YjrwV#6Khk$^qP#^RiKI7Ua ze~c9wkS1n{<>lo;-QB9+p5GtZaNpe7X+H5ukoK#q_tly_;uf&Gw7egIAvFzl4Oc+X z@WHmtY;(gLJg){eSv=LG-a6*f%-LL6XnkyFXP5FK=h)rNLnXysHg~wG#Hx39wV!Qs zb2CModw$w;ww1zn_iF9&ge}H-NV@)_Bl$w9p5xc)=_yTbl4LVDz0XdhvE@UyY=8ub zwiN?%xA|tPn8K|m*HQ>sc<b1IZ@l38jJ5!tt-+O|$J2BGeX2>&BmTLS^?AmN1vA?rEbC+o7a}_S_*QeL zyG1kly8uyyKxx`FBofKRz>L~bBd#Z#lQi+_qi#4)K-KdHsehK=H6UAhP8fI(^L>aq z^Gu^NsN>c?jmkO&BSfF>M9`Gx=eXT*05dz1W+UPO9kuXm2P)#(AXTL4K#^FXlfb3R z9?HddtD`yS5yy%eUBtQLdJif03W?uE+<(*dz0a(rgo*a}+esRw+chMW0*xvn zT7m?9vF|!+bDn}_b%>Uo)y*<%?I$dMGT~3uvWPD~_~y+3Lg}--`i&z<6aUj3?e*y= zn!G0)&(?EIvoWgt&VLkUIj^6o*sqxJK0^_2aZdZD96!|@l1BDRX(OY&^E5Hm{T&-^ zbSn(`I=RwW?vO;^s-5#}ZEmjU=O&@|NadV*ZV@y-k?PQ*BOEVhj4AQ2?R_o1Jf11+ zV=c>{*M^0!83;a5> zW+A>2c%^em@J1_Z?8?11rFV^}T0hT1C8SE6Sa|=I2Y8(m^BS3Sv%Pxt!4tyx!8BVk z^;aMMug+uQIAMj?b0A=VKwtklgOGvoPSKrfnE}R3B;OP<3Bn3P0R#gb&6JIG9yQbI zLqyft_mUUOzcL!IKbcw^-SW_4q)58GtCSYSIfR^cpx*MR@f%Ru4?vHcV6ay{E)(!l zDYz9cB%}?lkxsp&m+x=T`-ypW)c@%XjkfL`dCXis>6skrOPeB1(Cab-%oB!h7zzP} z*i%B0YyR(@752Xc$%wS8z3|JF&r-)c#oUvdbg7%5qUBJ_PULEG{zHCwO0(lub86p1 z8Bq6%9Ni{>4HfWy!OK=K{Kgg0RI3P=afmAL`Le&XpDtqZk&zai+F`CX7^b`~6nU@S zYzg|rMFry5iTvuVmf<~+DsDle68iXYYz&pwnR7#Ht8dM;0vP0(JjU74*u)H*dZsR_ zp3~wCbyImEbwu3#-fcTwP`saE?GuR(!h!vDLeU-SJEfVak7uE>NNL98K-pA3-(;fZ z<&x6AiHyOu!<7W~NWNI5TK%@62f-$GQIK zzf;2hi(3=lpGWWBF!c$9Y=yQQ;dP5QUj(~JrgXk}(HArV7yiBqeFFbsZvCo1f$Euz z_^*(~A7Z2E>$W%j+ia^|DG0Pm6DyYK%70(!L@Q5ULMB56TAV*v9^5>7qe$R$NoZ3x zE0^lT*r3!mdO)K#yLQgpw}mC_T)H}B)A*z>W?h-i$_4uy_0oz{X@tO?Av^mh$TT}~ zaLAQ0U7Y{+D4TFrf5KIhxmxaC!*u07F|Cyc{>L@dJCe>@ity@To1;QW>$(*8FX(`{ z&)x~Ql+NGTwQn&l*N3xDDy=T&s8>((0#vc?8nc1`NCvnR##Q9Vs_NBtvXDVO6?ox+ zczZkwFRh0==POnZ1uOZe^j)O&SZlP=l?8;j(}k&Xry;tjrA@x(KSARw9|yOwbjLTR zJPy0K`r~@$KI)zFN^ij_u@~_A%hkx>h%|J6(99XJ2@lbV03J~_RcV;A^h;D3*aDXOd-OI|%t7-c3n+UxJX zb$oJi&e@^)WmZ{P`D(~L-sueLkU8CK0t9@CoXud2|AB@CDThCD|EEgO@!c>+gt3H_ z)Hck#uSoyPw!-HY3X0O+m(aDR`42f?U-Z?A+cA>{9J-;@dbGN%C6pARvyMHQZk`hp zn-CLw&z)5_bavk_oHgtf7ca+a z22wfWxg5AE6Ps{>56Pb(?GN2$HV)S?UwmH1VZX(RY;o( z7xsbN0S9BU@W|?`%?Fo6_a;^Tezh+6^E$cxSW)0tH$g{7^7@2s`F-XJ7ioFT4=B`C z1_p+-!K!exy<}_ux>71yQZy6uCGpdKKgF^)p{=dA0knlW^0%{}M;#`XKTsbHGOZT* zFrZ2Y#`tUl@)#C`j#ZY99oW<7x&x4)X8tkujVkK9+al_vgHX5ixQdIOVsba#Rv)4;gtmw+9RcYkYn`@je_v2XM+ZGR$z6>$`R%8qW<_r%Wi3pHQT&)` zVPQdNYir9D^A?qSk?tFCTJzTM;$bU@>QX5Pu5i?Q+fuzMQ`J!g#MJ7tG$Q`cnmm2% zW(y>2nG<#$Jy55!g;-GZmrdrraonp27f>_m#n$^tpnlT1Kz1rAf`j3VMSs{SCG z`cC5_Qh;Fqbk*Zo#BOs#z=|LFZ`hR4 zHSxD#&QJC(&JMKC|GeTwO;x$-85srgB_+m)y4kInm^sV!Zc9l?F+phiD>GNPU3-e^ z>t{zd9%;Pw>x!C`MF#GiW1?=gD7}Hhug35itxHpCY{3~z)!i`}=ZfX2paAXMnEk!`VwHJ%^z4S(%OYy?$LUVNs%}h=4 zRjSiG^_U05=tZ(}P&`TwOkd7p(JnKS1h)=TJjf)RU#;K{eCXM&vME9e8wfgdGc9q#XZ-9Q-Y=`hAe`GOaCRf?OOn zt#dF`_oZvCCP5K7(-ot>x7shXxs~61;HysH<4AfPUEXKs`%wWUeeW8Uh@QToHDu*? zb+tbW7wM4dR>oeAZ%oAl^)0Q)ejnY6VL`^y&x&aL#?W3?$W`1R&qw^rS<;;>%zUZ9dFh0OY(M@<+oJ=7iwx#|X~bX&V! z@T!XS%3*-0)EaR2wLgC(85kMwF%_5JH&Lx4)ze zP72G52=UW!bZ}t3b_LvdVRbco7J0}ffk`3h?-fS2Uot(Q3c*cshH}jB;$oLyh3-;= zQmS+@`yyo$;c!q*;zr?hyh#fzKahmlX=~y!U@BNOu$s@4tKp$-anNvW5Z_%V^3uu! z(E&Y=U&Uov{}Ev3KP>q6Hu)8S-O>gF<>H>Aj~qO8a?1=*Diq98Ex2NF%cH2aHgpu` z$GG$7glWbLMV6PJf3!k!TzGj@zidha#(IqpR@VZsaZyQ00LwL}rOvzFTeh%ffS<#o zqoc7;f=WwW4 zVlY2NMG`UvLBQ{?GF2%iQI)!y->BP!8zX6>@UMDP~pQN;5| zzch`b#p8jghcV7YtI-FmQHx3nqn__8uUenX;U!-zPwaua2 z`bMc`X6sQ5mw)tHj+eSBkB=uS5gSxG-(%DXeSQ?Pq}u!MFV;o(KI%LC;~>4d?-m*v zb^d&sle_M;6$?lVW}m}bGk%vowccm9J-4SC3op@yDD=@rwodLvQkNI57p<@hLf^A4*g{(v{b~Lz`cduCh#@G|vX)Ct z-6@;R7mH2n)Z$I;zx+5kQ>l<$bA(UCOC? z*wZ^04ji~H^12!t-6$@%vn@z;1B9_{VxQb$zlKDK5MVKrX8CW?!B)T2js0>ai}M(~ zzMku2dD$E{MucoGbL%o>wrm++Z92}L0JduPY$X|mIhjT3!hAqJrUiMf{OLc^#V~{P*ptRF09M!^feGBnh=HcV}=iZl?6X#1T z2CJ$tG&`hVqsz0sp5Zr)u8T0p51OoSMA^d5=|BhqX`jC|`MS~UdI}2E$57u3n#)~^ z%Xbvg7dP{DNF3`iBECnues@O;VTu5m!(ud(zxduZe@kPHNwCOP=i0k7YFhF)+ed`d zre=zGh%d=2p#%5W^_S=MNU%Ude)HS77<001gj?K7Ukn7G-?w}5e*5a!>wxYYo1H~A zHsXhehxe_yY20+APKPyciDl5oy1na=kWW;Jq*FaRsFi!Uc-1ROX~K$I#NJ+*&TDUh zunxWc5D-}=*4EcOcV=44Y)64VX-%&^OheNt_l|xzRhGs``p>ay94vNFLjAA|Nh|XM z8Oj)#w-U$os?aNr&5IVR?=mPW*8P~$_vA%W*&ADa<_xaQ0k*HwbBD>p)3dR)(E0G; zosP~o?+*uJ0J;Ek2W%S$BLL@zY0x(qXh$!QN9N~0nyT@n00iFZWvu(r`pdk$yhA_x z$M?GHA|Dlh!`|NaVv(H2rrIMKS-Y~&b%ujW9oM@#m?RPY)s=%M$w8(&KkC^0>)f(- zn#&&ZfCSrpuu_zg@GzT+?qEpdEGjaAVhI|VIXl>%{+{;lytY-zcndE4fpP{H&YJ>B za21c!_LTUymXdihTaa~ZX-1Kl?N{Mr>reA{~^=~|m4sF0ZbE(CC#FnHYBgC2=Wm!;4BRYLaCgI>Z# zBq1pKsns4I>q%kdc1+xeUzCrG59KG(X=(4%n=s$A`=A=cp#;%cZ%TmgFx7+Tl;##b zMwe-&*+Q3duPz?JDOVB6-p_3{fQNjCiZfeDAjWt+;NEBq+kaunv6#aB!mqM$P$#73fC2 zGAw(g8!8x@d2QHHgzXYCH^7)^okE|NB>6TdLK;hhTU{ie#mW*V{=rfetA;Ro(cg7-;Vjoe9WH1Ij~pu|TLI{+yVzHTdhOzYyw@51&rZfC>G`n0J$fr(}p{bi)x^VpWF_W=07F39rUd#%(MzaN5apj;>_F7^jhFhF?= z`%AbM0$z>l3b~_5Jf_}qaBI&zfoPXsTQPFqEt`0))=cP{Wuj)lR}-rGmc9>s>gDg3 zy572B^3m*F*(Cgkg^UoJMAf^IMdK%sdWEm$eyVn?rh@%4F0ZOwhwi|j>+e6c`tnTQ zvN3c0#&bp&q9}W_bWVWLwi%r*ZK=0Ywb;;+EJ1@u!@yjP`KmMeU61D}HHrpCyerYr zM_qNcAn)MUD~=_3M?oVOfPPuRH+-Dls_i(15hjK3y1B+#OYAEY_{I68vofCuZ`&{X zD?K^r5Kcu8i-$en8xW**?^LYWn-l@bzHlpW(`o?cp=OI4M~nMK$_;`6NT{HH_S08uAP{ zdR5hRkNGA-a?m;=4pT_G+_)nnVfrdyi|0weODU%zeCVSAwHHweq6OKrZTRCECdsb~ z*OiTZSJ6<48kEA3;fj#f_JoS;Nc0K*bjOokbN|el+Ak5RtfM}E#Vdu^1QQ6}K6v(A zD%&?(#dc=yCs{}WiavDSQ@+pBUK{Y-e)9~{SNlxzMz^n{rSq}RWoMAWb^A0{`X+HC z;-MXUz`Kzt-gX*EwJ=q?@T+CX`^So9$wcnFMMVPS+S2>m59wb+?-Nq2LPbdGWY?SZ z_lW6WLm22I%c4T_ukyVvJNj+m(TTStQG@mFWoTFLoxaj4G7Yn_p1r5=eaF6dZk|V+`_>H89eN|qqb5X}3fghqOLd?071fx>| ze9X*sUOZN123r^u0OpBE=TNn*fO0R1hb@IH(a#CNi*T=8=J5h~*wyv&&g^|t4b;a{ zVyswd4GVOy0-xg#CW_MY}m$=4A`XD3cx$r!!{V=u`bjn zj%Xg=&^W03QkvQ!du~Y#!ElthZux_*obzkfInzt)+K-1J)zc1_;tj8$*)Ux{rflpl z%+K)QQjDsgCIjkp^;%-U)o+vbJRg^A85^97uKAhb4+j!{3#t{9VWfA#{HiBd+URsg zKxp8rAMSNGrBvIeZJ)>S|`qGms z0%w2JeDS`N94_RhmgW1owaYcKjqZs4jL&iM(3$~sS;(Ed#-3KZWy~0m9;|KO0x@A2 zee($I_k;#Z$Q{kb!%L-^qur>t&vte3nBST)vs^1$S=Kl{X9a%%g(Jjqtz*ASnlJ^t zIR=`Ctl=lO2;B4X?i5v5e{eZl{86c7&yrog^f(q0@vAOZWZ310sfC~lLEdXgE$(PQ zM80#Wf;#1CGWVBLwLx$3s>P=;>b^`(pzGX|j6 zdvD6RjRqt*@E+PLm#XpX&|Z&u<$AaM&6RFpAG;fad4)qFn!YqaVxW)$8H${Og4p3F zYM}u3ChO%*0uvnEn*}Xo@KqE4COvQTb6g#XMneEjK=;aheW+F6m3>y4zZ}n^e)Cx@ zI|mt|R}TEgX&1!kF9%tlbDA1EiY}o!C5rU+addS)`Fic#!ooXc&Y2fE1ndOo#SPJv z-#!<-I+h{sSl_%abHqL#ZQtoqy~jPtzxQ4PlLFydoxDx4AHxUb2!nnV3Lp_`dlTWd z>Q*!6i)9D{fMT>z#k+|CC{3If4)k;caZKdPn7MjVym;ZLcMbzsmOv zOw;8gmpZIuECn0o6Gylx9)ebqwB!hz(lu^N`IFBi;{!|{SL}ic->C=Cz8?XO1RY=D zcvXNB=Kyrm)6>KIR^>sn$lU!$-_#77DSuvyq$~Rrt~HTmy;3SH*i4hbtcjMeihc*bk0{jTwKgnKd^|%8Z$8}Kp0N)0 zgdv$=aZx~hHfrB|c1ZAXgtBLnfa=YSr@RV(jFEu4*_&%5_wb-<9IG+0yFR_mr^zh| zV2}l%m5MH1gg#;fZtUYb6n;w-RUUcQ9R==W*n!mbV6MjmP0yZ4K@Ov97q)a+wac; zfVSLu3xO8m8Dt+yx~6!8KAzj!u7!1#US6}Fc_19AnVtg+^4{}dCLtj)2|Y@F($L~= z97prs`VY4f`c?IJ*)~P@>~^?5Q@}b+OawK6UJey|r+aYk<8dpm!_`}KVG6VLdKVg1 zV92(k1vqlYr2asnzIkW5CODXanU%^hE4@Di#eBAFc!RMTzyPY?F88U|0NcpHwWUABcxK!m`%}E4^KfAvn4H@Blx_i%I zduv{_X4<$frl=)lC+Qj*TOo@dF)~`@y>z1|Svn%nfJ}wJV#zlh&82Rn(rK|SveZF3 z30psAFJe@N6tCW1uj^eO5pH3bF59>D3KR)HfBv=xrCZ9sT|E8uWt6Tr~8BOuCn?~S-=6{DYLBcB?3 z*r!x465bx35JRTLw*S=bU@_;Xt$azS))10i%xu6) zqFnBg)bosFRJW+I*GeM&*c6m__N4JlNjgH-A?q_Q-}S^i-iR+B@ay8S6NEk zyjm94yh-*@*({bktY44d+AvC*Bon<4RTkA8koxJ{@g^_bG&afG-hQ3p8wSTl)aAZW z`THPD;VO=4_uY?5o*Pt`ZUxk>bmr|4dY_Te(Y<^}AClSAHqnN} z;VgE_rVQvaa6~#V_)isnntxD4}gR(1=x$XK4o(JJj5i8S# z35JqT0sdQuK8LN;udxf%l_1gqWO>re($M=o=GJWG``ijgINZmG{_wt`hVNHS*ih8F z>aY(_3NMUCdDW+?T!voH%{K>+`R(G3dU}Kg)TxVz{Px(Q<9${6uGN6-=A{=)>~hnS zZe{K$`}Xz~dIUrFmBrwKUe)$o$>PL3sj#LND8OL$#)L%+1h_&J9M^NgbQurucwZD7Z zHdk(ID_E0n=hyaAi47{qXA)fHSX)~zP2VaVTm7;AHxA0 z4ifr25O5Zl-SXI5?6~TUUIVSh7ix@$_GMB})ApD(c`s>19fo!%Y)5}rA(((rL%Ts% zRRe3vvLf`LA$2fHdaWkZfJq+L3fx-)b{eIgG(TSQ@S6W6oA)I&uDkwe(W%soZ_T?x zKTPH|G||uQG|i2e6YA7X_SC4srCZg4kJXP9A2GLyS-eCHtJ92WFTFOjiqddbFlFWI z%IsHWOKS$j*#5KBs8UCTwXh}ySXR{HESFokO8c)uwwy=%9s{1VH~K{5RlU!aVoq!7 z&n3K1=Ys)5vvb+^^!-If#{H>3Z>bCa9MfH(P$)lG;b@~c5_G0Ou|M^;IleN#T7%2P z+4w7ozY`kGc95Wjoz`e|w;ZT}bnZ2X8yVv*-C}L`+cYfx#FQMlw`38g z5Y7p_EA(a#E~DdCR{o=yb?3&rYaiNuJCzi4Gaa;yZ&zc*NdCg#+W9O~dFrKGyz&?n z+(IK;#U`VNMJQc0a);NYH#R}H2SRLKCS)&(AB|Jf77J}Erj^3QbI5-Ht%pO!@0yz6{Cvpz5h4FE5l!yT zNuPHtUjE&x6f3=jDLQFZw{vw4vyXBoIbzqtBS)h*_9JO3@*q?%gQ7dyJ|5>~MJaBUK8nyZ-YyC@SWDXxBEbQD++0XXR6 z2PE(5HIQR{4~f1ZR#Tna&q!r5T#G29ZqIFYm9){4cDZr@gUPJc!duq1rT)C zhZ(fnFu~j#V&ZG7U8%ytIsN2Fc3Kf*#_%EjApXxzl@-BGZ|QTpIeL%+S3p=W<_##! zGeOvs4L{GI4yAFCRf*eOg2wF*BVK;jTrqTy2Rh6|u*ku|(v$H&eV`_7nFT^E6_@1L`D^}U=s8mJbh-@QYN=pLH8BF z7YudwhlNK$xOWw_S(0M>Ef0d5=bgkD!matYj@nB%n5j{Aqm_0WR8#sLgWuUCO6sIJ z){?D_GLdbSOZGGsKJ-Hfjd9?D|W~}ddY~+_QrhV6;2`4>TfnTrW zqq7>knDFL-XuQdpnDMd2%#& z^X6LTBGjnvM)xBKV<+)DU_TO`+jF{pN4k46Z+GSaJBJ%8keluu75XvG&^y+?2if3T zmjw?m@A1xVbeI?e`G2My_a_!+DfH|Cv9}ckHKnr{L&-A+B(OsOV?gtv#k2n0$a}+Z zwki6dE{?qeLe5d+w`4Q8W?W_Ewb>Iu)#JkZ0_efLu9y_^k_8h&9*i1pFftdj>zlql z7faDI;M+Cuou^v=ch0B|QlXb9zS>fC=9}|m@8^8Kgp&c5w4Lzo&)qbUwE0~b!g2k_ zYBcxbGwiJo2in_X+vD4tG<;tb^M9!r7cJqS2=Vw*`-zmzVAQ$IguX9f!B zm?W79IfP-bDNG9~Ofy=kb!o}bJH38fE6`hKWx)V%osT7Md#nrTk+O21N*MMCGn?8{ zvz3kze?URs!X*@%m>vZ^KVms^K>5*0^7>Cm3hZn75&w3`kN>V&tz>J>=n@~fxa14u zj?cz*KYYr2z)?Qfd5dG8qd4|UZO*@kFzM&eTW1M~5HxQdQPc5}Esv`B1MMGQN?`k^ zHT_^Z#`mvcZ%C^^4A?i~*jdmgex5P#?$-aVAeeI0vKwB1xEuc8lJ;zo`d^p@C$Sz5 z8~)h+S}b*M^ZGjX_ROCU(C7ZC!S(J+lY^ieFxxY$krkJ-809105dD*Bfyq@WnJDv# zqll1_30^JB9OnJQ`|5@V{;F?#&7^;nTTSg}#%@z;oz*@wp45<(Zmu+2qorRShdt3=mqA{~q}rloT!BTFz_8NGVCrC_7Tt z&$&O)YbM9R_g3F7earA=xByY_!A$}gzEu)DAbEX}gXaqo^^^A^q*(viq>FYfc@*TNbmnCu z`4`F7e)jY#nd@JZ0KNdPO;7BC-L*S!I)ZzsoI6eJm&XnXJQsQJ3__}}IGlkVx-d7t zto$~RhQ8+*VdJM$Y5FDZC|TLNn!+6u{Y{#@_&EBLuYrCPoI9UEz+sXjbdZ{6B1T|X zAez(e(9Z1|z0su9kW6{;L{~G>y>oLjt!VY)REMmLTy7uF6M!FG zDwP$j9FX%!3!$a5nHBwb&5$*}HpgBI#HdJ}B>w1Nd5O$pHGTS-?RVlZ5!J^yZlX!Sco6#P`^e6J!bE<4KBy=;3LGjzaAF1~Cf3hCeX;!T z{|3&3hSE`?!GhWOSfW9Olfk^V38B?vUG#(1c8jX+ZDBpkMl-dMWxYzXZ9!oLpEiYy ztYbdaUjOI+sB(M%Pkg_>1P7|O^Y^GOe&TWev3j%EK?lRLXhBSiHU=OHIw4nGw*v>@ zlb)WQ(-RHU;GhwEapnrrE9+-g{ZX8i{Z9-JN|$)TSn^59r1j|t_@X%S@o5*0T#k0r zz`zT>)vi{O@t<7J?;3r#;9662`t_GT9`{R{S;7ExOzC@ebFq~itZ;j4GP!+i`oWVV zd$*PE<1Jg%$UH%cjRE}w`;m(u1S0n3xZJ6=Y`Pi2s!VU^HuP8v0ERZ17bRmKsZ*Dh zvZ~eX(Bfp)TXCTj`fB!yR4VrWV)v7o%q{CBRr~nP<_78}g_$;$l3&poY-AlWJUu0?8|Y&m~uzTg(p&1zQeo5;W#K|KBJ6hZk_s zfGN_y&3d6U!L@YK9*9ZE>e}+UxK@x$DRim@KLK}p>&mYtQNN1ycFljUS(=@yOFjWVCo7$3V&n@{|ki?!ReY%0TsC$c` zwK0V+i1bF8OE8M(tME2sxC(pd!!z3ILD{51#&F$F))8+#H^m1cPsBQGc3v{nMA_}I zZ}`jmlvG}F&iAsf{|;8RuIW2;~NM0{oYMem33y7cLGOfs3mN41UHPJ9St_}N9e8fl2HN+Z6~Q&(IMHvP!; zSvySoOLNl7(Zd+ON6H@U^RZI;xg9qdB zxvlZB5|y<5QCnCJEOTGPd~Joj-IB@Ut;i5Tw_j=OH)F|xV|U|o`QT4mr7kU%_F1jC zr4{X`t*8yn;q8cDWeH&VDnF|b{5wCYeW8MU+}K#;YieR~(t7->Wdsw@y@MCD_bPSK ztFyrqD9@ui&U@ZRi)yo=7_uq*U0)vo3Unanr?@jFMtZ+Jm*_n#`pE}iiHqIr%e}hi zf=%;IV9z(`#8cY4+x4_c! zAs2MP`WFVc-JF8q$I}KSqbFcf)ROYb?u@9zv40&-njH#sbb$IZ1JDSxKB5`_C%F8N zt1%9l{3c!bvoCwEVX2G$6g|3=G_|JOI1P5F;KOiWL?y|T@(k)SGJ%Tq8-fino-Lq7 z{48_StvX`#k>!QOtK;k8lv>FulcwC#j)%Mp>MNfdUp!aXW~(aXO5SffkC-x-URoDp zR-Bd`2#><8$&xC6nLVo`YlsjlXj^fNt(*6k^ao}Xw$Jf>jq#uxB&P0}L{ zrO1A%tOwJFTt^ehylZWh2OVZJpdn`VeMCEKnN+UWn64;tGwot^rS(>IH(dC{lW6BKUz;0jyFv~O+y->;L-QX~xFrxs zNLQMi7kgN_CrO_WUB$5@k`V3tgI)jJ*1#tzL(R}=vV3Coub>Eq-{H&}KFCQxjjyeI z^wbX15pO&hd6gbA(D{RZ8?CyP72O&Ou#EsdLhrENo-q^B!hY1QZs?bl5yEh2v0s`I z+3H~+SruLiIecq+M^MCu#2=5^VeN4D1y7!`}=o!uX*zAtJ1UN9E z^|x4vVL_&-EH&e@w28=rplJLjf8Sf?h6JQ7&n4FJKHKTXe&hgFE;~HR? ze;?k-4nxs6Gvu~T9LPoTNM8A`Zc^e@d~fQkMW9RNK<|g;^g)le`r#Au!6r@Y;7l*)hb^in3B9RBlFeoj8wu z6F8dGW+Jj@nM6f$a@=!7QS-Q405q;QqNt)wO*_dWWb2`AXZH&NnT=d|eFkHHLZjeS zq=n?GyV-jPN|Mj4Aj^X7Z=lR)Tfrq`XAx68=!5&xk9+=$M+BH&6LJ)GPIdmp9k{*V z)^?CeVd;-?97cMc>z&Y<^dWwlDpwMnN zC+lJ>d=t_Oqq4h7$pB!W&5hh%J<+ z%Fl46)MHxv$x=q%FhP2p~ zez}uRK%gV^jdZ;rTshrIK>6UoKF&GXxgk=6c}0d6#cC?g+`rUPeF?vCIybM2 z9jPnM8C9^NQ)yC!CNVv|vvsWAZ->kl*yVxgptKfrt92jd5N86i!c9(EOTrZ3Nouk~U z08(-LPY|xYYSrK4gt>)wZS$+3KLGf0qM0|OVPr&KYkUHwsM~2iC^l}xRbI_hQA*QP zMo!V?ba-`Fb}R6(vk9onR&lKES8Rn?o8Jlp?3%cKuYi7=q=NB%4(Gq(Z2^;xq75nGS*$Rk5gB2Bx1KC74egH0}~zq|zdb1(e^0xk*9&o3z$n6QPf zMf^VUdw&uOfA+RP=e9QSSylnbfr%bHK6{-LuX>v}kotkV(7tdtT69<@=#9$Jmqx`V z1yKq=d$+FvQeT4^#}%j~xb?R(`(O4l-+Z$kb(gEKo(wOExL#YvS>`&LH5h*l7vtUz zU5D_Ss5mWk$;u*Hh$GMLi#4ki`h543zjV$xk zT)`@`>f^rh9^IECL{-d!adb~scKB5mL1BH5LXMGAU**}%Q$QxPr@-Z?{^V7Qpa%y6 ze>&P;g%?M0>R&{;FP`a?{rIN+2U{i%-=(1^MX#$}0c$xgcHT!aL5}ZzV~#TcMU6-9 z!@L4om7{f7_o+&E663PZ=Xg?hyH#F2e4a9};Cq=Nsk->1oFz!H;|?)9NU_l~i9?lQ zb=3{J3aDvX8tWvBw8A;+Q;H+y^mfeaGAvX6i1fRnl}eATE1;EhvfCr!Xof)GJJ*H) z6<_J(4qG_0a&GZBd)4?WcA=mXl=Xie$+2_QS*X2o$+X{+9-&olx1{Qpva0%mnhL`e z^-?rMO7O7ad6LNfzEWFTCb#2H5)%{OCC$*0Jppo0k2f-~9`T=CvS6=YTK|)pX_0~k zT-ZDyktE!vY0&x_hv(hfJWN8p8&)yfwgp6a8oX&_CiD#?_ac7O)&4i%#gYve!L|A- z0=eFN;`b)%p#YL@5fR?`p2P|ug=ZYH37LZ`&`8NuyrpoiC2OlJ{pAXb2$}orPOF$y z9(-fG3orxC{7*q2d}f9T?4f-$a|e(qfG6Oj`oyb5vjPg4*9^>Q(>C|Q;a&>`CZ6K| zH`tRezSO~M`}`4ZVr4~)Q%W*P`}$j!-0u_@qi8(Sukux=r11ndQ@jl{=hN#yeTSge z(dfEazfbXRYXfkIF(TcnAna-z4b7uM@nDJ+JeHI$vbLLwwsdOx)J;*$Y?UYNC z+ZxQcn8OpJIcU(k725wfgpIW3b=kzKhXf|RlxBl4kanYR#zoE^{@vr)?d*#pGzB)3 ztNds%YPirec7mw_DhzkIbHJ+}yZ)oI`9*Mwk2Snmhf~?7yt-W`xqVPUN8*DWUx?~i z^>9ljPt0`YW$}plb9|g874*)GYJvJ65Yz-Mt(dinhkspRFC_jhXOtFOFPZ;KgoYp) zn!gVIl$29T4)R3ZZN%yX>!`ggXRcH#I&zq)?X@dIyVd(xSKbfI|vMw?haL(hRLANSAa;Gjw-|NGpxR&^0hf4c+fG{-5Vv=d5);oVDg7 zB6HtY?ER}f&F2>J#>43*c=Q++wW+BIBun^Mg)#j4SbT=rf{8SSsls<;fc_`!O(xdh z{@NL)i4j&NpZGDuCt3No3iBe2rB#&%)k5^Q|W&#SJf2K&!1AHL&4y%~E<+~k$ zK}w*1%$7b4+ty)h=D@B6I!eBvj*vZ07pgM$eSviJfuNcJEP6bn_-yn|`b9d6$>4)j z=bw(YJ~RBxcy~;xgwxz@&umHZgj9}ad#@Dpq4Y|>vYM^M0IfY1tBq-0F@54g3e<0X ztN2#>N)*Ggq-JlCk!Y873c00Md06`~x#F#F+7BY#{XK?2zyQu*IyqkKIsxXXTG_5C zz_zuK?_I?R(I&D$6jBS!odB!mC2;o9PxVwKU&~i%6Q;^j_6k#hrm7VVK*pDQx{6vO z^Y~QlHd6x2@)FOi*rhlaOWM(l5l~Q~{RKG>z@av4uk$EcxB;Q0%AD9Ycqh+JFs}RN zS=ZPVBY>$)^#*gwtsibAqVCr=>Y-}JV|z8EoQ7D;#q{jx6xw3QgSOOtq+zW)W-z+% zap6up>q@r(YlGnzF;aUCxWKm+%t3MbY0WVuG)g!E!v$bWJycW&0j*Abu>#0c0e=2G z^uGKEWFC~^LRP2szo%=9_w}`X4D!@jI5+<_tPhos=j&HTV+0qJd=I^sFXKp|G+4Ci zKY=eTNFkC2u*Oy(D&=`Bicx}Sz{{N^5@%|=9@T@RId7@{?@fmza8y48H@1OgbpWDb#^7dJmBgW(5j2tktvm*p< zX%@-*`IwS{FBB7h(jkkX6VK1j!3Jo+^Zeg#_@#$A4$cBy2t9`70xAj*^vZ30fD_h{ zksNjHYBpnf<+a<~5B9xZlNRbC45e=`hg0j5 zruva^@3~igF|;UW-|?@c+_(HCh`DDBDyMr3%lK8jU{l(M`_=t02Yw5x z1-^S-#qJnz?elYNut0&`5v0QeAYqq^qpBzW&MhKXDyw<@%XFlfqxEfITWFHz_3CSKD2^Si zo6#&ste^Z&Nj>@U2=#a6I}xaltyLc`uT-~bA8DTFp_wNqg~s`|G%?4nZfysyrUasP z@+crI+&KeXnF$6!`DFWK6K|KVpsBJOU&n zFs8%9!=EDyFw06ldmzJlZCKq&-yr!ky;@?h10V(5hRt6D?;nKcq8XKU9#A~%I9n-x z{|AA5ubiSM)rCHC&|dqkw`(>sSv%#HF@*wJ5B^}$uFoj3!=xE^wC9Ek>fMbmpk4qT z7RS9MCIFe_dF+Xju~EQE;>}suqzLs~Mon$zF(?x$x?DV>i;MW~zY-^^f=kcEt|xmA zuzEfJ&qKo{XV3L@gQC6&u$g5#zV06AFNyueW481{A6@&es@Utm<{j(+dMS(|21L&Q zyl)BJj#*^jMPcvZ%Tc6kE(xzKhzIU?0sG;DIiF)Aj7$UYK!tCaZN2{$FDoy_)VitO zy33IeIpxB{7CN%K9b6m$_1Wxx8 zy{?XGmqGs6b+XZvMCH|dS%NN4sh=jz9+-Oxn2z1gRBxadQhfPGmo*fFl6;;j1MK=? zo~RI7s5RVxhE&wapmIIgG@Icm;VkDRW9+!Rwoj7dR9}BySg_GJ@rXLK(x!n_BlrXB z&1q^$fcYY+%*X+Guo(F7coCFWJ;FUuqY=JL#AEKG{LKvXF^-01ptlalFG+f;6#41B zazu02z8+Nnl0VpUKed~=x89p3VOH-+WNk~xvc*Vat&eRsG^E5KPdST-yx7%cd#HZ` z=pdGK8KUl^-YG&_(|t)gA&+q%Mc#YJq8Ydx6*_41qkIaFR{ z3HMB!-A{OM|3d}7G{JyK<1A6~Z(RM7_=9Fs{WwBNDPf1oF5Gop#U_LLdc?CWUu6yj zj2a3_3AnBG#532*vfl@$i!|h(OLnlj1d{;!594;j>{Am@v#THgrkE-OiC_N)3kU8L;|R3~wMU4vvbMwah;yUkZh`QhDTJen}_GucGc za`?0C_i)h_!_%FgzypN7@~J(5q3EgrHZr3NeFd3#4^_~SX4-9D%+ zyU0N(0*DX;3i*&H`0|hcTmAl_-*+DPi($=K5=>CY3PxaxAMcC3ZjZBFI`$IMIa7ii z{y+n)cwxNy1N!>cWKS_S`&P}XWVyL!=Rb&NTyOu@U;U2c^huI)rzxj?2M}XVSd`Ul zuiKmZ2V5P;WtqC(l$}dM1gqCF*UePG#?)*6B$vpmRa>nK- zfD$)D?{FrGpEKRoJO@gNeqVm@$|J%tN&{4Hh>B0GUBH?LKV2v{|5-Q{iUnO6z&8z! ztzW{nqlD@kTWN*~UagUR&j~k?18Fw_BDpK>0G937GEoPo2d{RPG-CfPUbP)`mb?3o z9o9U~u0+?aD6(K|bKTY<+xxx$vmf{7tqA6E-&0F#H3WUKUcl^w@w7v zfbIY&>ZQG>?~KNb31j~DUqn>?c&4|$r(pR${7!r0BtxW|)-ghAh3fjD{|-+5EmN5u zqE+6j?QEAy90!8~%LQlKp-R3)Y zJ|jTb6kFlAU$Fj`Nv8g#j;`T_4zPKtu_pKtcP>pVo|CncOStsYOz-HOxg~Nx+M6S^ z#$n^iyGrVVzMynT?CW^mokv|>+)bi65AYPcko1yL!dlN4q?eqHJ(Kq4lREr*M+OgI z(;vIK#qAgr{1g!DoyHX8-z9#|h{5uB7GX5r^X?z?!I$*772FAbbNjmyR@-m#s~Y6u z^%Cqv%8TaeHng5jqF={1zd-EWe}}rTQdsNvKhUxXB?(WScl*suB%uzO+UHif##e1` z?6$5W#>cNINy)AW`&Mb6;Fn_U0{I~9l+AdXroYq)^DA(1{T1#vb_WU1!C+4Lpbh$x zg%42gS^J5z7pvgrh&T?3w{s>OQWPg@B*0B!Lc z05ZuRqxou>{03fvMOaH>xt0ayS0@?>``?p(C~cGj#j)Oa{AQI5 zZsJCyBT*uYAF@{`C|$>lcDj1_b@yu;&7_Qar)~#=Z+8bM7cTBOdix7Y-bA{-pSy|7 z)J>`Y7SLk5=tH=}3L`S|;s0w_nv@sd^^x;;YO{p?Tc!N~2xyh-em^#EuQH?fM0wo0OIBD$=xpiID2Z8i@ylK6lN(*Qx*Kh zTB(2p=vI73^fN1;Vhf4%N)+;g-Q_5HwswHiQwXRC^Cr*SNHbesj?z{d>Zj>-j9HDk zC1WZa=JRk+gKMbi&sIB$+PbKC$5xC7J&i$JoDscWiR*985d8^N%Utj!rIK`W0hI!U} zglpESef<;7f0bil5*f<RFx9P5|P&n55vdPj*I-*Mb$Kj;^)-P1}^>yr87n_>m_SgkU&czWy^n^e^w8D9qE!WI&!+Y^i;#)LCh>0C3diQ>!a`^^zW;%A+P8J&^VoBd(^+B8SJ%qc@Nwt~eG zITwt9vs?Y=g8^2okH~EvU4uA>#rI)dE8kp%2L!^>if7aY_+|MdDnzKNK2bJBK=}Lj zSBp>L$AheEp~AhIMZ z@-pUAFem5AZAuAE>d{)TeDlzH6(u}iHVX;gCn~=!Cdkc$N;C z0-vd{U+G8p?cy{d(3%UV|LgfNN5;SN#r(6`?!Z4TyV>Rhy94F)Xri=~7#F~!KNlD2 z04|fo`q+c!r!aYfQ4sXO%H`cC=vC4ITC~oD9<~#IZ{YOBC2ioeP))c^L&sj>E_>KH z%x=EG&Hj}Dv=GryJ$Jc35zznQ7C@MSU6p3|ckl#3D zLz?^`CPxNVRY6G^PUF2oYS2GHYv@lU5%6?y!MXJCP_{tcY_f3r{MMiac#WluRHU19 zMcjD04M&m0qj5K5*8-s!nsHOv`AQ^R!E1Fr{f%2baDdDPV;FvrXG(kUJ7ii0<`$2+ z;^+<2-xETT85CAGUm*?CsE{qvI~y_PD_$>0z7?6YtgFX{*&8I-_GF3i{PLniP90Zu zHAmzY@BifF?Qb3Y=m7gW4^T7nkygZ*Ra2y`OCa4KUZtI&RCrj3eqLV@B#w`+q+UPY zkgTM0tWv#KK0m?JchtRCiD@IVag^U4_VUscfPFKO){@wZS;zv-PRLcIq)y^= zh2kFEo>J8|=o#jrcer-RfkR(65D8xOiO_oZmL;1v_!^~#o{k5Z6lOLv((Fjz0P3NC zLSraiP;>${IM{{z>l;QE6N%A=3?c)h`~2LNzXzK^onSi|le>-SUW?|f2xHxHqg5EP zvto6&*3DnnA5pYqAC7!GgNBx>cp?nK#UB^Yl#Se?qBgdjYi^{LMRqKae}ys0*DPtnFK&4AqpbRQ3*_ zWNfG(tsC8V5|Y(gi8-F~0z@;3`*qZ?3W;Pa9I0@YV0NQVdfPa|Y%rueX3KG*MBnMs z!#ssb2L`HNYPL<&==pvvl~}ICkoo)V1*cliSRRI{W08O9AU_OXu$A!PBR`M;W=~{> zUj%TiWzE{(l_NWG6%Nc4TLAiTzP~TN@jr14Tssr6^LylY=~VmDD>X8}dVI4dLA-m2 za|#n%s)riLQd((H>kcl7#AuA~F^WZO8aSMtw z7V_rrU%}ng?6g?2hTafID$w6=@x;QQBkwWAhSuBZRgIWMS>`KsxPxqL*;=kjLuQ2p zds;y{x2A=QQ)9CkB#ze7fJ?`MYSc{o+*kY3Ntx`K$**UlGk*KJb>XV%>jT3!>CF<# zs`BINnvW@r9&2~sZq#DRRX$BZ#9f1EZDUTWtPJ0c@5L1|<_ zZ6Kjp85YagI7Icd*1RTK%Y>QO0CpJKCv4jnhS*Wxz0cz`ddrsrhn)OJ)N-dA4(+AJ zfBgV4rV9jaCeR7pzbP|Mkp@8Y#rNdjQU$t_R13w^p4q)NiS-^^#aKHm5&zzCcTs!6 zp})T5ZvmqEHIW8(my)trbytPlMNyxsJ5)d|fR$NDOVzkk$fJAf3FH*AzNz7}JQ8kfxc@_h@n+C??K+BHm5gb2IF+bo8{?Yl zV+e>SN#_!3j6H}&RgTvHC0lf-J3XkA+@b<_UZhLj2U{h^ojUt3Z&&r9Qh=#2Y>d+lb<2L{%z<5;ZiwHnc-1rQbW5APWCxcu+Zz$k{}HBjen56MHKV0RX#uszBAp zOQ0#zpNsWgLplK?L442;kRB+Fh+spgh$i8chT?MxnF2SqB+hDxwvL)Hqh5%d(I-^O z1vE=vC>)?nmbC805wsH^a-#4Mz6BEuSB>94;-z*~F-;!xe>=TU!2Xb9XLy5)Bld!( zrjpU;V2AtlxH;>9=W~q9Oc!X=E1gqa;5h;izd7fy8H$`X8LFLI?+4x3V zm#XSPZDI;nt;)&1EHR?4MYVVz@AypLJ#k_HGXr}>`m=i@TMy;#gM=g0;TdG;8U796 zJOBw{WZ`Yi@@~MWGKWM*J0- zXTtT5HS1h^Je?Hny-YN1_IY6<8#g*o?pg{NXZ8dqztyg9+p(HQwx-Z;=6AkMFNfca zKTP^#!Op|5qoWDz6YVAj66Ut~ilmgV6FNlC8$1z2OfMo2Wo7K96@z+JH}0JlL9mJ^ z;}48-ASxYf#GjP56j1yd8+%0mH)|f_pCW6B-er7glL|Je@?F;@i5eB>gq~%M7`g^M z<%$#KmPXPZJ1x5brRR6%gQDfT787rdk_TB?Ttrq;FIS!y@@`$Fd_K!xl#LhNA5*bu zOI4fCPg1_o32|aMjf#inBP8*6Sh_sn$8~e*gf$U4IcH_|MG&$BL6rcRxdRKE$LoX^ z%873Zq3k@+Y_VQ9rbEZawywaUBr~BM*)yW>B3-C5CJ5}vTKuUs@yWP~ijvXHW{|%Q z%nw9-@1tFIKgDLI=5rS)aFuGg ztmdO)R#aY@G+{SLLR97K6Ntu59@VIH#1!VAZ)5JQUz~$hRVxPCCN9WF4=YhtbV{>& zu2#3k$)@v;n3!C*ERtV)RsPPo5MC2*@yrny%!ukgbb=fXFH?NTFpfATc{SKkBpu6;)ofV1`>|%TBIOABAE}oEU(E4pCn7)XpbKuZAgBA!GB^ z)|wqHE>sW_xKxW?rZ!J;x+fg5JUV#r53*W$rN*i^!aCBENPkWIX+M^%5iU7g>&?ga z%df^(w}D$NjwJ^DAKR5GG3NR)nI*fFVI>Dw(pBb_pb6Q5a-o99lWp|)dBUw4$*E@| zYH>P<*fAjywFFNgrXbvfrILFZMER|l^ISonb%8PvQ#1oI6EC3EI%#Z~2Hg2jn&^a* zy(lpz=CqYg*r3Rs#Nq&gaY<8YpQf?1O<#&>w*NVEbX2+2ek+Jo(3zx&z9r?NZnp_GYn0mVUZJ+0k>(J5Q&PD#^?97+;u+264qN*ir;>QZfe zHyu^Yg)DMKq4J!63urtjG>q}qxB|u?yF|J9!M^%(iwAZYFK8nXSIxZdE_vTwfpU^s z4+wc$32VEHRTXOG=3Utct^X>E5uPaxuIFAXei)R|BO-utm)c9n%h(D^MUr%*kAmAO z%km%ghR-=v7IQiTVCwtKvvg7eYDx+7p|olmr=i~goRZQMNTHsyTA?qF(|tB-mWFP( z$9-vBHx9HAqDf{^-VKIbz>c-c(T3n#D(xdUko7>!;>cF$;h2VYSlO;p6x56kNf zm--l7wBSQwDOVl5SwcsBRh7b2xMc3k@E9K_+^$Zg&}cM=fen}%CP)64r7KnX^5X`m3;pA(!bQCmgChH-^iBe90uK$I`6&N5H3tT zqwPZxFMp}x^ctq2fe>XsOm;mJv)g>h01SPVPq(4AGe`TDhCpsdyYXamBj6{M`j`^LPWzI6 ze&wj&!sa{{4d;YM@aFiDIb-n&?=acmAzy9ZiAN+iU&D*%A8NGf$CHkCu>yZ~`LK+9 z3uo&zYU)kVyUikHW1-3?J7wZ_7Iegv?6A|k7^~~>AJq|zZLF$doEx_92LO}@0@wkC zr$)2${h#Ut{`&d7n>g|_PrbobW=Oia$$^AG0-bk-x0z(y^5R~z zs&S9@raf9~u&Y9y7oQ2_mDk zY<2}9Rx=RdgcusVIBrCPi_UuuRBzfq#%odQiC$+PwSEG6Bz(0xE$42ysy;9&xdxtQ z{4DUL{2UHkZ-{(dWxny8yIR^Wco!y2LJ;?xzZ73P_7zNe5AwJL#AyE`?9UoS;e zURGQqld6VryPw!+v?hl^GX|G*lW+M|9x469M+-9Kp79j;qC1)4?hSqKp1OK@!YAey z69&%68um0J{d@kH)IuiOdV{x*wobhp4Kv1Sb1^sQc&|oAI1T@qjp84n=@`s*rt}acj7XF?w&sdiKT58ozBWi+~Fw6d* ztBwTiO%q55iZ5Vk22PgmPp8#DTB4sS&TKI7hp?#fm;)HtcQ}d2b+NCZ9x*}h!Ortt zVvzFWv#Jg=%O8W@<5>ODs%NC;Qn;0eWaKs}uSJZ~RZZmLRHoZdtxAgEJA^@K#9|BH z86I`7o(tQq_0~^IaILj)#UA-lotxAu9F6D9eJ*5vGd6Ah`f=@%MlA9$rnn#XkH}b_ zZ~U#6g$gR^_)Y)H^cZl|jJfq>wB(pv6CRyzyx?)l_Q~32nU!Q{;G9HR{p+_YhGmHw z^BA}01(cO^n2Jh@&-h`JeI0=zwolcX$gAw=%S2;bv=Hdj64d)WXeJT{%te4I#VAlP zDaZiW$r^t9ZTPLJyL%mmvHLV>Q}e290;Kia@80{)m_~-WE&Usa@gf{!1RzDRwi-!> zl2MU!m4c*$d*RmGjHmU-F;%+I401s!{_tPDF~*8nhKFLk$GRsDIC#?74yP4F*$=R! z?LzamgnER=MB&PZEGmx!*;C}L;uBd+s5cVVB-1Qs%S4wGz0kD@apD&g3Q|ci3YVic z3h|;UE^r^4k>2%{RcW5MA}7Xd`m(x_97YMO%iJg9WZn6QoV$uREuo&5*Kp5O30#Zy z)B03mVh_!xjaa6O-8qiU7M!=de!Kez$D^rZvxD~|-M)N>-Mn+5k&$y~^7|=WD5en` zMUTqQYZHr{ybBc%telkTI+ERE;($%|Ryml1)8#35hQ*ku@}|b>DooQ#bJlWcui1?J zjh7c?hToM4ut+`>2Z4`XUS5-Pb6sR6-q_a@zFQMlmjQUhm_a^Zcnv2PSNpLKW-t=4 zrZ74d;LS&_*DD@$1Z8NvNPI)Ev@%=}6lLLzsco-MMrX?FYjypVZXIYJIhIm#cE z0iQbXL2M!NcFePH9@9F9 z@o_hi2W;upBm|`u3stUL4Qutjc{c)%j*cjvFB_VY>DC~z8Pq4hMo^7tY5VS$BN&tX zc9!0uXx_Rdx9_O$xw-;nnySJWLMd(F8)gOu7|u~=Fo{5PZf{E_|K|5kVI{HeIM%@? zc{1OExO>NILHs$1T_)o4miP`GdmR3%(|ypI%x-@hw+R;Y+S+pQgjLQ*>uNF|xA#6S zN?6$m2wurzeL?OvypdY0gxDg*%#88y_;h(G^f>$$uNzL7*1hh*`!_+V_kKld>nEF< z8-trIhUwOSY!1@CdDr|hZ%*pz`xQ!KRwQLDp{$Nc5daIU12;OK>(>Vf>evDXE|-Wm zZ>}^3EzqZ7!L&#a^MHw$0->$odxJHE^?iGr8xuv^(a~|0*7{fN`atc}A^Zh_pzCbnd4z*A@DsJv9U67E$ z`kna3Q2+ecY4qQR5cgZonS3&3!w|IVClJ z`x#7Z;^Ei{L_oda>^ z^=-$Cgo5$Mx`v#YhNbwl*&kTmJKo2_1J=_yeJkGYcNV2;R9Z@Rd{7`Dtr1=~i=b`U zN$m#0<+lRK!s07$>m0ImFESQjo9iyVA#e6*_om<70xIA~sbX~?wDnU-dYNJIMi236 z6Ds@6`bQ4NAx=q6Vc=sRp$wVOR%0R-gH1j`SH>k!(>nk}w>ms^s~<`9==9=6M#VwS zEto`M`k?oG(9Suqv7Rgh5brM2UXuecBv2Xu0Lrb$;kZC|0fdK_v8O2?TQhvm*f5WL z{;XvF2MEpZJ*vuBWRW_(Ek4)*&I?08|CE0ZU!G#Hz7fAazVmg3UvT~Wpgq8M==hNL z(;ztz?MwRG<%#dd|BZZ1d%N`ph3lp3IYtXqAdIz1y71f1Io6e5>M{-%*Zp6s zSo9)2{c^iHNHq=0mn>>O=S@i831X%%U;-;pC>y`|k%~ew_g*mGp-FiJrW%)GvDT{- zr+4I&+^SM#uUzh1Lo@9rGw*Hp|Gg|k^y-eXcAQ#M*U7alzyX2D`V#kePdk_)C7Z3T zK+>5X{}@#pV$b&U4l~z>*O#$p>!0mfuq>$*7E&``)rTssW@KudQr~P3&y<;0MM8n= z#Y!N>k7&T;`~Gt?G15pC;qbjc5ceVJVjpa8Hsxsr1cl~SlU zsUSHplD&7b;Qby%%eG3`x|rJ@+8y#T85u~#iT}l@8Tz%2-M%z$^@fcKIn|b>Bh1H~ zQ@pQY_A_sv9r=Fym%ck?m9f|@b;U;m!p$wKrZR|^g#(c=Obj9j2HM#p!bIJJ0|`XC zf2o`E1melN9v3HkV1M1|A_px63)%xOsoDvY+{;~iQ|6`*$Rgc^MTMp7n?iH7@Af-} zaYH@u7j7fJ2rGPce!WZ{CoC^#oAA~EViEnalrz^ioiSe}#vXE}n(?9j32G$xlQRjn zp*cZarPJwygWHQv^R$ci$(Khot=eP2D4(;uFa~AF+z)OKxIo z`&=`^jCW$qSNKD#nI(0+Y5UWI*!?j>*6P~5X~EzJ5ZJO_ry>~%M{?IF4=}_GI2XGGPc;jeP;F01Mdyh7DbQ>y#oSEyK+H7+;;lf1iD+K17(CE~1}aD`&?hW_9I-C}fqzDK()T2Jw!u zzvXC1A~W>zrnlFX*PO`B_D%ech}~uj-aNl%oFT7fS#sYH`{TPm_dqz;3veAnX;XP_ z=;D;)4v@8Bw%h3M*W~&bc@;kJ^g81+@pVi?haRL4oBMumql%79RETC&$sIK z&Oz=uX46Od9xz;-bW4GyQ}21mE+i!6)G(^U%x1zI9tQA*`aLnqb!MeUhh&0zu=|gD z2@Q3>i1}nlc=)f8nJn~ONtOoDJ$KCCu}aM*Sa|HOHFQalrCL!E@nGBlmJ)2&r&*Wz zwUb`Gl|}LxTl~iE1FYG*l~@ayP+#n3mhFQ$AFZ8(OE0YJ&(7Z8MUt^@U_I)3-=f{| zjEu~&PD7^iczCV(`Xz4qTB7MvZInPIe^*Uhg!4?#1wg?7=pWaNo`9mKLnX%rDP#t7;#Lz+%zQx=*>Ryf}6GSy6v0tlG{&6gAd8_p_^gPyvJ;(o96T)Le*Q@fuh`~e;-)K&(f6?neMh_V}#$@wh<^o8a1W3IH7AyvqnUgfav0&{W&RFJk zv0aZKRlLF6Z#y--FM{|k82s^4p%1?|VR1teNM0o0&+;s@m%Y>ToGt&G11C3~Xx4xc z7A^M418IXEjiF;OBgXyrlXV1Xf~rz$@2-2pe3>+q zC^rpbec>P;Z;1$cZ*P}R*B5^&uD~eLtz4$&Q;^v2w4veW*N5ad%=&T5$0|eb$ZHy0 z3RjRnC1Tr#L&u-Qw+53zGk;Id&SH2lt0<;39;hACLp&FYd{KYQ4~%>=upi3Ir={6} zL_8!Idyy|JX?$~+)V~R<@Qp=>=1m{1vGK&_JEJ8%E8Z}zKiK6s3zGgwFvwK^>nAIT zu|mxm&)06T?@!h#pWx4d2t2iWHyu7!(EIayU(@?b|9xswouutOhUa8La0DsqALrMT zzml9JOLf239XY&l!3Df`H=ru7*`@q<=AYM{f4+Nm=l{ox+#Ik#)jmGR^~>7=z7V9; zwFqW#2iW?C_3yD$oyCD&=6L7d-~w_efWL91Ha~<)2cll<+a7`reYsesDwE}7=3Ol9 zpR=tTBge%Mt9L5i+)5ToOBx@t>qvNTSlN%=62FQ4hGE?Rk#+<%Pq4&^sj-;myE_$c zB@}5HvL$%IH7L9qU&9xk=YHf^C0FKEuWZjZao9a)TdcIhQV z%q@Gp0f|yzfP!M~v04;xcz~Q3){yV(xXgNB=mO}&zs=eQDAf`56D}lwFQ3-cQ&R!% zk582QRca$MU%$GutGBXq%JiRn#IGjp;A6M9~Q4lBS9XO5Gc1c`_0&oZMWEZiwdhbia6nn z+XBY-uuVOUc~0@M48=F=63k=JY8~HoZ-Eg_L|;}b>y0VE zIj5(UT0HNXa;{5RE}37a77uvZiM2?1omZ3njoLRK^FC}M)H`sI%xecU+C}GAK&Zu) zj}KNxngiu&Dh5H4t4&In1-5jGIvcqn zQ|JDBPy~!?1T)0IINBdTk&{($96xAQ$F-UIPrY@U#5CtgQSz9#AZtqmD%fhl(Xjg)pFfeYB!;6MHzQ10t$gD;8Z<>-gk4P{U*Gtv8ftarfCIlU6 zgEO$%#YEC&$?YyXzv=o04wK3Q3sNJ3sf0%HFWz@<6n+4R3QhIL;ga+iZSCxs<~K@; zj0Pkem4c2)2gpgTLDJ!$r{b>PbOHk^fUH&glC!>n8~~0eFk=(Md1L;2E^AU0o2)&)jJJkSv2~FaHNq#os{Iew@-Ae?Cr< zP+j(f3PaWdRUz2#RgH{y(fxulra{n=UqAFM0Yp$`-|f-koH0(0BaC2r@I?-gGA2iPyZ2_m`KlO z_A}q>S`SO+!UoB~3?(qL4WJt`z%;aTJ{@wqCU&J)$CaCnkV8kkUnK@0wUgfln1X{> z9>z5OOw&ex_2RVsR?DyAi~oe4qH7^onnS{pPBTWmKQNc^MUt3Ks>DpI@|qU;=Cb{~ z@K)rgP?za-aYVkJwT?DzB`Wga=?q7{KtW|8p%E&xG=w=WaC}RNKl_P@DhC%%0rx49 z(Vd?oY2Eog5)G|Vr`*U~HHeH!-U;Mjx?sE83|fp7GddI8PASNRTK7S#9QGbTLwESYkO`?BF+GaZ6tc1(q)4Vf~ z&CWQYR_XkhTVh&dxWDe}pn^2EdHz`fn_rGZ@~C)q=b{YbA~(0a>}g^elOG0>wl(gJ~5!!gsoK zw+X@bkATUyg{2V@v@rSMd{@E}!xEKEK&-uw{<6&bvt=3T4eXiSC8YGGyzBkBc%yJ6 zk zxk;7;`}4x4U`j~})&Ns8HzQ$3Jj#VwfxKHdg5K6ubaKOO*vT2gGhn1v)iH0D+H~bA z*pbqS8u1yVP;xvfCJSweRrwLjaX;7ZVZY1jP|w>H+zd6VfpW(iuEoN} zFiQ(6##6dTo!GCWT7%5FGWpqSxe!Q!Q!ONj9U3!Y%AOG@Zuk2E3|w~M&Bt+Edfq)* zDJ%hzXLVvoM5citHcQb{&m;qN+oE^5j6RQiM4*!5W%yl%reP@45w*j6t&M3yi1(GtEOWlh!MAsmh11^`iI!HQ=9sB@UorbolGD z+Z(T_D^xJcsfSFzwhsF*w(vba`Y{ul)Ib(SY69t>BBG828GM{Ku?y4j;w^oVvZc%g zwuxbMwIAkmKEotg2CH`V!4yxgn9WY$7Z#=~lgwBVrs(860lC4{{MAS6A>APSlJY)@ z$RmB9PMo5O;pI?l)(;h09&@*N<=y><+dL_R)%5Ql^#vx=%BUp}yrOPwv^3}VxSi6;ma#arLLcwJ|8TR*QK z@)<2gzuxqWalM~wR9eC@ti_&eaN5q+UG%No{gVYQA9KGj(lYV$e|l0|t_ zeKj_Qy=?l)*s0S3wsbm@4*$l9?~c)qD3rNCwUDR?;WSsD^n#iQj}Pv+)kEg`W2{{+ z*){P5XYr1|9M)&LR!v_G#g9snZn|8gjzE3Cs&PA1W~gaYM-tbiJAgfwbfjwBJr!c< zI+v!^Ms@mh;8I)_G0HqxyK2T!1{Ep_FFu2rZ7LXF;=eP<{um33eNCI7Z4rtU|I?kh zZ=|}2#eIah<3N}k*ewRykIVckoat{q>5naQ+SQS($rx*;**G5NozGW0b0c_zF+}>< zZ05K|XJ!DKF38}HsZsj^^pRox#&d2!-FW+ax%gcwa94v;8MKgt7rDCf*Ga8dufVAS z1|I_96dnFY-_~gl#MgYTlpJ~_ajdg2`bT)#seB9KTgL15o6I%G))cD2p@W4U={pVg zW#+4Uw&LyLd}u-cS2|%+uTf-{`^rE6Zn`tuOAcDfuNgv2e8tKER4Zx^cl{o{M|N&x zl|tpW?rQxJfETRl#r73k2sA<>D-6ky6Jn~v#H!EEW$U{yPz^5b)UgCEq-E*%9AxtIM9Z*a8v#v!tGMr zy#$HO79MlS<%8!(WA;e4c-;WkbjC|xDo(tU@3c*-C{b%1XnLn44Q)O`;Bjn4Ne9p> zWo2kh#25SEb_Nmx7v72)k3$=J{T&cVDTN_g!}JsE(z0FN5@~J@xAdrR=b!R~&;#eU z8Gs%DHVaZZa71z3&*J+nip$EtT!IGLgK-IP6r@820GZqn#+hRNG}8Pd9GKKSYF?3j zMLn;|z^(Rt&o+D{Uj zK}lwlt!~2THyZDVU3~=Lo~}KI{l3aSSh!0hSc0dIna}CMe+iRo?G&KJUm_G@%}FnM zi%ihvX8rY>+JlHHFVxY`zOawY=_keyu+17NpWQ%G|8%5Gty5+Cc548!(9DreSbW5T zv+#khhTCBQ)*`O;?OBp~V~Z2_h0>kxk<8XA@n17o^tww+TD|AWE6lQG&EcckIv^Rs zK!X0=B|B^iP^OB;t?;mpykGSfG=X+Ld9D0i8XJI%1B3w>R01G=U@OH=`+!M%0xG`G z&t(QwKe4>X(0YreUM&Wnb%yWC@=+-Yzh3dA)9rQiqdWlT7d0A!lMKFx_IVw${Ul5 zRBa1Xe%&IzJt9|%ic(+EzCT4I7pa<(#e4xE>PWt%SM-@B9QQ~xEf%Il_)GN(U-b9S z#6;a35si6@ip&L#HX0UMG3LQ$M=k}G))RSo1-?m>nulXwwYvg=B~3<(E4Gk@OYgF` zP|bYyx@R58sN;&?alCw2eNgK$5Ul+c9ioy@Um!(VRB4B$_H%Hn#22>HX@i9XgMtQ! z?XU5FuldPVogy{P<~xxYnNt^_j`IuR^=941%6nwYQ{I2d_hVPdrHPh4gytnrgyyA^ zVl;E)gK#-PxRbFhC zAZ&C^*!E#;fvkMVV9r2wWo4=%-fyq3$a6_j05^h>gy7-^`CvrrsdvtZyaaICzvjfl z*SB#ki;s2QPIXBe|G3GGEMoa!7VI4H5_a)vM%C4d@Ld}aeel!y44xu@astubHSQR79g;eN}KCXzJycU{I&1Wlq~s6ru+60o)1`=h z$u;}OR=+^E^RoMaCjR?Sv+Eq%<@#V@U+}p(aQun(Ui+dQci9Y)K}>h->1Af`pH%O*Ad?RBf1rpIvk zs)(P$XXYf#uJEgkgP6WC?=EtX#tBTiKnnHo33=2e-hYvLCED*6+S}B@!1FC&7RZ)^ zk5as}W2+HOYc(`DD^;naW-6PqNmJdbz4Cf)6Fg=s>#Z8!=cO|0N6~N52?`8&LzV>HVVRxe&tY_251<5m z(6_vS466}l+~GU)P~E#XY)L{{VKn+?U);#E1lz}M%L2Ah;K#k+tj`nnuX5u%2%b5p z;EwtV#XVyhbd3t8emGUET*<9j0K!$58BxL<|A(@#jEb`Tz6J!OyHh#^2I-U%>5!6c zq`Olb_YU0QB?E{>jCci9e8H#l@+zrmNM= z-$TMkxFq`E#?n~@7?@sb^D4)MVuG2ib#Az54&l!PUK4v>f7m)PT)+9(e+?t%Fb0G@ z^W1!S?uzce18ks{wgw*2In<;C0A9QplwaQsk24IZHS#9x&+>Nm2)cM!vg;RdoE$=_KF*Q0K*24TW!CaT<&5)77% z;ltesju4rN`cJ9B)$e(GSvlkOw6}zL5)-Rk`SWbId6vC3D|;MAy7JoU*83>(YaKB5 z(2hlZHnn;!Uc1<*t#x)Se7Z4Kq9FOWb6nOjCw?Qxq;~b`!_;glgbns5&HKtAFS;D_ zfC4DE$FQnMWj!l=QXvx_WL`Geb*nxB3{im~8K~a;Yrp?za$^i|F2rB%Am{vnFn&Ov zrp^BW0A%CZ6)=iS?zR4Gw89Gp`WEIxp$L}umDS(?yQuyTmYTEPZqo!>CNag8j zi^d%yf<^(j0+a(kCI|H<0buC-A6yvCNwt03hOvPaM7~vGjl)KfcD5ts?La3;%Bqs} zYTl1y_s;ZNGaV4yybbPGf^+8@29W5pCvueFsP(7}&fA z1=0y{7dn{xB`qhQ5<~NEv#4vaXq-#_L8JxSbtSe3a5Gp|^{F=%ko*JAaALO` zuk7sYH~+yy|BRx|fVn09w6Ah5{yg#mm}rksKW|cR#lPIf1C0y$eix|!BoK`5QjPll zk#2VYyVj#J@uBE4mA+yWnGU!G#ck`3Z{`B-w9pVAd1%~^%;`cz{GLq@>4ZYWf?>h2 z9J(zEN{Ez+Hz^c=tD6B;H=r`r3&R<54wS7D)?f7yuyZ!E_9;*1pn+DcyW=|p=m!9y zIfT(j4xhb)1KQ6&{uxojg_W&^P$}vL=L1O3kC+Z6@5*X@;!%*ZTmuu;)tgV|7*Kfq zS5C@^{-P#KcwguE01N_55bv3A$PNmNm4qjbmZ>I0J0p?ek9&|XyGXwODtDS#+M>Q( z5rFs5YlZ<9pdRYy)J*#=cQZ-hF61?5W5N{I;PNc8=KWv~VC@w>;+c#~3L_P#`Jo5% zu#gj-G??2iY)qup!q7h*N^27$6kf3+tuFQ-I&It zy+uFzH%ash-`l-o;F6EG|0ua2(a);favC{D_|@mlvjFAg(-3FC*MIKU z59=>t3y|Ca@qi1|fcxi%J+XioSLYbQeJtL)xgqIbH=pD}m%B^GbcRAUx(By|H>Asa z3@SN%)Cfy>fqC8rkEP_%>*_tm7^|*Or(?i+8M^RRW8A`RAUV0ZV_@DMP}1a2D-9Rg z$8!PS8{~QI`vQ~l;92Y>lt#epRxpSg{ME&Yp z-fvl(&hnar)maTjqtZb*Q;1ZBdf&HbV&{izdmz2JxnY_`^xGG3msqE=DtrrDK=$UK zU7r_JTxCqU02NIj<_3BfG?o0VSmAbi9FtfF48SlE{R1zHsdtGkl@3%GPimVjbjTA` zYscySb((*8TP&s|sa-lgE>8te0wm0vfx;nURgLu_{sEa{IukS=a8RH6r^xmin`#3P z{sL+i5&%1m0A%q^hK(j7^_BndR&u1Y%SA&?K$-naB5a4dQ9+d}Edc8^3Hru^0vu7V z&gqmnAX8nZJuE2Nbc53iA}JOFNQ}()Q3y>J^FJQt0>(z}CA-M{0xR#8hB=+;ce zOck;FYa%rwa1=LCI!1q>N~&o!lch454l($lw5}8krVhBJX=!Nz%)9FT?R%1d8aPwF zUof`VEu0#`CN^N1e$M%_+XJXI2HfwLc^%fYo&ok*ojUser~nuEerNoFAy-T)XUzh` zns728`*8(Rn-(&j0&Z`;wDfU{9qa6|o!LbF(%)sL-JhgRPshdaI2m-2&ivulah?NF zHaOu$D(9Wl3GRFTm(@r{tSVo*N8v13LR)=T9 zv43@VVlM*HGP)#1a2wR3*e^JUOQ;BL#VGU2ud{QCB_)h443mxae-V6L#617buabps z=KQ&L2%6{>rTZpegEi_=b%uu48T)5qeLMZ%5u{Cg(N(OOYxp1{Gk|WQf8nnof(^Lg zMu6r>27*x#PlrHM;WRA+IEh|!15wbzF%ksO0?V{MjwfNOz(};furfg*%7!0~@eoYa z7{g*%6~~9_iZ|$(YY@gIo$YFOD^`mLZv%9R`8l}8@df!1nDECrYp@o!GzP=S$^TJs z0RJZ-TzwNa@EvhCSn_Q$%$wP9{9cMaYR&G9bOz#HB<(Uh6U)M~fWtBOM zPlGCv{d=xz98Cb118ToB_~&cKmlopwRwQf*^uFf44oC8rs|D=g0j}H1$_n5>&j64T!=9cvioTG(M~WdFJcz8N z^@Thhiw0i5{|*;pANAt!%Z&6*O&hJ#8X;TbD>x8|9$lut&Q2(hebi97qr*;@pi z*Jg)sX#IxxG0qK?uLKayFbU8s2%^(fnX3UES?52&r`o`B02E0HC<|x3BbA|~TWwg7 zssfFUH)FniyK4=YTtq<)MQygXIBIT)!+r#4hqN0LeE<+iNK>Z|q!V0!Bw9Pa2{;t0 z>PR(s5rS3NwBaIfMhu+RWC-Se9fRTOk=9S)bly|+M4F!G_razLXYWYIb^`}RP@xC!1AuV4(ftwpjeW zcvE7%=k7fI{8xNHsI5xghOcwX%GL?zy*<{R(%t3a=X>R^M!_sAHmcej+MbCyD(=%Q zl}wJaEDyLr;;+u|ap!K8;MVcW{s!V$7;$iHZIL@}9P_X3z4k{X<2fiBNF;VpRLWCJ zcAoKcPtpQBkMB3&$!Pa{`7zXx&oWQ9L)1}M=2>-DIi@HnHxHd-lyEOEOWN7+(owMC z{b)F-9pKs+IEd#89GN+&vA#|51rfq64*AKZzi~mK!%HUsRw-e;bsO9U@4olIWfm^S z4A?6MaADZ=X^1#Yk#8eG{NfUq;mYnsl?I^wQ|Ig$=hm&!D#DZ$!yvv8I7dRXHeKc* z4t!HJl|;}jT%HVWz{37Ulk#iL;j>8>(o+Dpy!6)~kvf%g=#~{4lzbjgex3hAch4!V zvjlIAE!C-6^?fEtI)@H?`ck|zpei0i(m zqCnYka{yc1O~PoBPXefub>&_fwC1K*DF8wu^%6TKY}n*KN8@TR^+X9*K~lDySHgox zpxs%|t1UsIQxEIrEZ`{U>-IGQTpw|27VP1meL9->m-az0$_7&(gaWwXH5}|-Ru=J| z8$+}aZ>h(T%R-a|!&gq9L^h(ew590{ds{e0Q+5Px6j1UE%IZPSgl{$dYTwdWD|yJz<1WR-1c zNk^FQGF3dW{zG!>t_ooNlb}ZYX#w(2p&LZU*LVP!jsRW<|0{RDeOKY*R!?IqcK0Zg z-{G2>qhe7U4A#@rYx9ONI;?e}P6>)V9KG{)=>G*H2a@e`}~%mYIsuB-IshD@DpPVty$1ONSe~d7Sk7Emc3q2=2S& zq-xhU`*S6pUn_mwN5MHi!-?TeBCS*w;QkPCG6ny9KYHk0jmz?&jlLev2MHkl->SHqd!*?KHZNRc&xNOa`&dU17&Vu zo5TR_5FF1h&(NjGzU9ymQS}Xh&!3t&7!J6S*pFJwY{>${cb8lK@KDD{4-hVf`xM-I z(Ne3JmIy;}&;B?j4oKmw{GE4Pl~hlc#b8sQt%>VUO@$@RFuOf`b91-gcA>mDUvX#! z6kq`YHb5K&nPL zVKUvms_}x)e{pxeWxnydh@RynaBcxP+>S9@bO4b7`J9=0`pCU!B=?|mXh?|E(Q)eZ zF&JwDi=>lYOS}q5N7<>$REere@Pd@MJQNW97l)#3KJQf{=m4FtG!QtAz9mg(B%jZ^ z2DyiIQzXSNRzq7JLF2$yskvlC&|*EW1LuUX?pUayJJR3J-%x)>sli%TtPc(|JqNV9 z6qpI7id6-|-|VeAA9`y7ORfILrLB(6E#KI#9&@^5fqMH_m%ElFFv5BrO&n0g4tb*^ zQFG19&fXR7a)%{fQpHTfiTNFPxhBPky_X?s>KM6>?mY);e_e<(*8-tc$nX=n~7 zbRV2;T5MZfJg@-h5ybr4ss5t}zY_dSW4TK8A6+DKkp8fd!8-T#uHWz=#fYG6@vq)! zVot!mECwLT1D>I;0m-+4|FK@NrUd_a#kH;n_yWR@0I?g8x78m8G_%fke$i1-Qx^6K zdccdAV8n}rja1+G?wC3AADf*&?3lq}OZ%W~;J%{r!H6drkQzQqxVd=P-GY5>OFm+Q zGveM5dqw3X8IJke4{igTP~w&kfAj6pf7r+{|4+SC;Km`Nv-a5H> z_cytf^_m2bi?@ZIqb}?xpRKvW(7z$9u@gCWtM(>A3;Rh}^TKJ(n4?Y=^1|J5F~FM4 z8I@c`>8kYzHR`2Jfx7=aDUOOS2RCo0! z4{e{BH-H6b`wd~izlYPv5QuMu9TCFZ>YRMy3yO+yZMiXipp#VE5b%rp*SR>qz;h*+K3qOUG@BFc zzP>9mOd=2un7IP+O?SW7$ec>;kvuSAN9fxm{ZeKYrtP^u9OJS zvd^B#MCvdgY>^saP)!^}HsAm%((MMw^8oFOc;f+`cK$nrn}n?w2J|xVsaE`vY`zzY z8X7-)V(>DAEaE=^f8g`9KNQvntM3chGp-{0v!^4%W{DPIYZ0@{J5 zV}L6Gpz|sHFQEeLalQ%G%f^gzfE*r3Tq!>tq!aGfXSkV zr{_P@bpT)rm|mna1H^~`E+D|HSvkIbap*$YK*zo{tq2;)wh-yj=o?oc6j^59UQ>q` zzaf35>RpGw)f+x*HNI^)-Q4~LXH)|a4os0C9NK5MrldDo(YK#}4Xj659Y155-Qt?v z+QJ3C+899!gQP`!xi!Q2rRBB+T)9(i@dpKyBD?=h&~BHkYOk*d^c zoL%}L#dB((?famfru%nchj9+BXYS8UI$g9{)N(}3Qvo6Yb?l0+5;V}!6~&3SHuA3~ zWfa*DA<@E)b=GTaZ^xzdvDnc*cj&d!j#hYW;hD)HrkqAV8cjV@UNktwYe!zGT4F%` zs6C@)y0lq0k4BXTU z)G>YBO9{y8AsLN!c#5lmQg}R#E^AdW4m4X_O!jbb<~#1u9W@GtJPV#v6#Vs;A6SnXIhFH}Ef4!1t{ z&`agx2BG9>OINr7y6Nk~V`JE=H36959#&CWDTC4d`>z7a>3Omh#7D;$oCjMQnI_aa)?wYT)r?GPm0* zujmsBr%|~r0@-W+{0Fzw(D!W6F+~_I)5{y)*f&S%#&EHO+&)J zAv9MWX{7}1nw7y#OTOH&YI933y6x!%?*!Z$%)h2Lbmf`e;O7g!dWGk~%FxB0c3z}0 z?9m>&J%oTpJT4IEiCgFIO6cfsMS1GyNa^@;AjHw#6#Atplz5jmk3hl8ki(=*UD~%h zbVv`q?Y`Nf>-m_si($vn&y_H`m3Z2atq9jEV%pc^JLhltGHSrh4MW(8a=Xv(Jdo^; zHbf3WILq1jtt)AS7nZ;o+;|x-=Ho&}V1Q!4XpqDnu~E~-qlJ&Z`!%5^@_z19$dO); z;l7NR(7MHnX2DJQ>tP%~#da;;#wPg7HbM~Za6Pbt%NOXS0(}?fB01OycJ~2wSh05E zk!Ff(R$DY46+g|o1T3RJ=X(NyWEQ{?(Iq1?pJ)=b4#|>x%Fq(GpP4sFwgcpC(7y|+ zPL@GTeqJ<9u-|2-b9yiT98fmI$Qj|qsGcNfU*pSt*7UEX50+zV%F+@1HUFKRSV7!= zHn64A)LrT&TmA5jKN`WImdg*96nEzMz0`LfDi((gFE+eoZlCdhDF*IgHeB+c4g8&t zAmRxBt)DE|7&DJ)UB1A!iRpyV5C{eOZ?p&HRTq9Skm^eKs6Y7Ze}BvL@M*P`+j_({ zts(cHx3LvXd`DSLJjHK7-AUH)+H#pc*pDK-E+fcavc?L}T?xTJ^vTwLN>C^lKw;vh zzwrFRq`RdW_!i3z_VC1A_xD8Vxt=6^-00?gT<}~tf8Em%8F;<|Jzwc@rgH3Q|9S<3 z)}Ostor-7gd~9@6I#Vm2@^%V_V;YV6b8t`#es>c$u}Z^ju5E5*X#t&mPI>ljYHafC ztg2#b;>M@P5y|N!x9Xstcx&wB1qUDc3RPo>&1GLak+iIu%S`1dDg_fcq;j|`}6pE z^PS^!!%!eB9P4FOyycGiWt;Wo;pNBB%f-6mS+-+#bu~XT@9#Nbd%jj1zlZOAJue2y zF9$c)lrP6Ur)Pl=DpilCFEFB)@t696)4*=YGR^nQjy?1GNcjiBZGD&yXXg!jUpMSC z0wcVtCfchfCWT=moXJBcr`e4kWiv%RCKQI-KN9VC4GSM;qm?)DppPB#a>_ffH*QDa zuvlFOD2u}m-jOqC%dUy7e^5gC@p%4cspr>q^%M1*J6Is0zUC=1zs`=z!~@xQLj2G} zyFA&(9{PinCPZ=@EPil!cRG(MUqx-l-pGYS`?@tAx1oWRmtR zwzC}<+13=9c9j^`saa-08Q9lAup5m&#T+WY`h^KmVseJ zu8WkP9X=3Jou;<+eA@ZtkZBaS`nTt~&Jp%zBeu~Mfy$4bV1!<0OpfVuzp zW_1IZzpZb@3DgJqn>g{-D74yd?F!PlSBQ4YQAotrKPSA7=t9N}A5IyI>x7EpKPCshf`c z!JII@=mYx07uzSFCQ8`Psc{+J(`lx_HP*I`hQSDdtW#GlS@83s{ddK?PjVsc6%&iiAGz6CeZR>p4rJfNc$(otG@U8+YOVA0 zi^Wr{NL$A1UsSQurm@lnLGczBZ*8kDboh*@sFSnfd8oAe{JIyd2f~-?H#){n9MuRw zWU&HbVt;RMGe*sUo^)a#Voz*M%?hn5fR-`yg-2DBR}%+M1~*807*&oISF&*COXJNB ze9v87;H2Y=L;7UP_e$Zpi69>}buRTa`n16Y=~*{jHa0u|2e#*P@h8fcUFw#l&#@I> zK4q0e%$G`5BrK51pmM_-yqi8;sSdb0y^Vi4cD%jmv2B{YC@0}#iw)J%?3*Jr$56{i z7{oJ4VX9=9?%62^@SxR5kX* zi$*XrbESN6FN}_{CH(jn(IlnCe(J--uo8kZ`}qLkBKCQ3#Fw-+c61w~rHS8+1mBnu zAn|99+E2ui5^6@Lfqa&d%%O#Y9#Rxcy*G5`4mmL4nKFCL1iXHdU{yTCB(AiNG^(1r zrW1y-u0oaw5+uXK+Y}$#&(&r?Q-C!QInzGxwzqX)(XA$HzFm%kQwS&N^3|iy=W7la z2?uQOB2W7Hwk)MfyBNHX_QU8m`{#v6MmN~nC$OSa#1YJ5ZH_HlyQ9vv%erbO4njSC z35BNIog26v{adY=4)rLeEw~N#E0;c^_fKMR+X?h_wD}e3f}f_e9>_Yx&@UxP6!gW1 zJOx%fHLW&{ld-3+3hd18&7vW-6WsSZxWUq1_$Hnj}jsY zLGFFnRR#7l`pYJ+RWenD#U%QO{;w8T_1(OIg#88?C zzsNru$oX`Qgjt!eG$a~W;Cj?H;^I}Na#;+M)NnEUz-G{|Fa2{K=T8X&0nNoHG2%csCI; z5LvvH84(Usoz|Ckdd8b=VzZ;6qh5nfJjMbP+!S#ciUz@4f9IKd?n1PlO}3EvZ|B;M zz@mXh5HQ_-vv^<+#Mc(aeTq8yTKOI40k7i@9>g}$^O`2A33&D5063W4R1LTvTzgs*5A%-cu3u_l|M*k~UYeUeH+371}M5 zObOZ^5(tRF2h27rWb1?n#k}O4e1Z=22r#}{mp2j8p|%j4`GVrojXOcx6(c*3F?&0L z5~mbW9|1Lv;}5u>v{45TS7Pf|Nmi8V|746#Tl%<2dUeFFh|*n(XlQn&E%JwiM(6u9 zg_loMaG}wnT z%ZKFG9?qU(3kVAu&#cSFt5|1WXe<>&{Pm#ozg{s9MKE5&LB5^r$$v@e5 zojZ%j3izOwho{4&lPs2IF^P~50%9p?+}hLaPNGQ9DBOHiH3(O-S#_V7>}!ain4p=B zFntrqlOo=sb$s*}dg%c1mrW4E72;c=;arND(>yK`^CuPmFXJu(f`Z$Z=J~xs-|3ZE zD#`}eXr5sH%5m6xH^{eO?52%wi_Vx~|3CYvv;w6+QDvMJ&3nH4UI3<-boU`36OzIS zEl&(tI>2*HP%t1)+&?mrqIf9$HaXo5V(8-IGwfwIiExLON9+_3Fts@jjq!n~^-D!j z&^)FqB6A7|i*6isjhoxx51rdu(_Sp@J~rRCY)6Y(wN*c+_d=(`Fvh=(Hx6HQP@J0e zeB=+ro?4IQ?3C^4j>+*%?#{pQ`)jc#JSI}qRhEi99H2Pbsvw2ppXTV9xaXMIHRTt# z#vXo38~5W#b3^!QW-SfosrE4Inw%T%O>O%O+j8|!ZS>BQX`7;Oqtr_?&fgTeUaWZ$ zY9Xy%K~9@!z~D&p3V_{a*y|Hng~bnLkN8nicYTX!wA%{jomI2DiO)_sJQ2;a5 z8m(x{CbFnvvzMIe=us``MyOWpO#OOIqmJ_h(DhVl-lwJ{Y z%SAlG1#-MFn>hxEJZ15(;()le8J4Tj`zMl3Jtsq_-Va9 zGKVzZ7?!s@p(QlvEJ-0??3tO9d-fDB`uzCE(o{A>=u6W)ntE`KyBHSTcPo5lh@har zM*fRizlwOA8Zx|aqPc6!*bRXfa*SfKeA$3}SxDg_amcWpDslHz4{*ZNMrR-Hqle)P zI#0Oq&{AN0>ENi63r4;q`1Wa_mse>^pmfvwPkPez5$G%W;tecmf+0Q#aWSDuCC8xHn+grqA1jmuGJsmttB$ei_vN-hRS#c&Lj`|~x*-TLA{N2s}R;t0RYNw12(PE>Q4&EZSKpQe7 z{C77xY%x2d#9}ldFrOYZ57>OKNHGY4jbH0XMD#04O_&$*yC~hMPEAzF!I29{sSRgX z#=6|$4sH4K)ZFX75Tg>4my{&gSEj{jgYz^DYdVymmm&}mPp#*U9U3?}5|8sP>WJ-p zHBEFD_uztWIM_Hv(iGH%)>c1OfD_PtYTo#>jH<*GzoGHSAPFfQZ zjBqKS#5*-QiYkc$$fyh^GpWoJoC2Cm`yQP@t8(Q$s~oMMuyEOms0=a3WQTmo_d=9} zNczMi0p&A)YTJX0Z5T`7<8q;J6po}S*ZWz^*>1%bNn(a1u9!2?_}LQx>9pwYdEDX3 zU`$brwfQXZsp*e(!p8`FIfAgI+R4qxp_kJKP~hV)Q?Bb@8j%%j%r<0~N_cYYG%sBHSciw>$FL#c>D4UrTX|L(4p}}BC9m(b78YB?hhne;bSl8*Y*Od9<#4roksSL*+ z`PBolgpnxmoAk(^qmK>V-FMs!tqu&xjJ>$V=zU?eFzLnR z1)j@4UIx`|xq9}(Jy|yfE}6O9t&N_EN`{SQktuaZ{>_#!+P&=Dp-1n)?1_As=O(LA zp#81-!z0V@rxu~VA^7S3Q)^cG;L~?x@ub*OprN3LY}6hf;|ue;pMYo8OKUi6#SY;@KMgXPu?7y{NbfR7=j4c`pzr zi>FetM|Z6y1@16A^IAynTtT|#L$CJ07(+^YuF(NGuSUR<(+Ona>l|$TeCvKb86>Ze z3;B{W`jR7)zr{3&Z1|I~NpH&S{IQ>gO*6aBkj<8K0K=buZ;83Ld*UKyn@J3dOPHY@fxd~M4Rx#E0hCaF9{aMbv>VBR?_0IX(G4)<+5;Sv-EBQHFgTs!px{lnR zpG0aaZ}|MmS6AvWrYxN5>GObJhuw7UOtj^oqN8M6l9lkqHlH>%Ts;Qt( zX9G7S zRlL!BFF-8<)`z24n;W=!k)#lp(JTv6#`)nYv2Mn59HT62;okhAT|etdRUC42f0a1W zDb|A>zyciT5MIGrvvIs5SR2Qp`!VsRD^#CxQACW9KR+mtD!&6fk)MBaPHbWrukUNH zSh8}wC4e<#nkWz_C@Q*pc9t+|9&ADaNZg%W6`U?Fld8-6tuvuWmL{_dm|z>+^01=q z28ZDU)62c8HpwY97T8C6^ujrR)Zlg|cyGK{V7t#+D;>q-MXnIcHS9r~N7?>34xxH& z@J*G7nUGgesVMT&BW)oOIci(LgbV9#^V>Le;I(%^mT^03R0078((FfVBw3KO-2I{e zXM`X%^5cHRK;Uy|n8CH_eQvcK8J5#ye32n2z+>}K6P1s zy|&27CaX^CzJmjssQOaLYD%NA@h0)ntnaY_J28eyZ{JryZ@;tRPy~e0;=<-XdTG_S zJQ0xCr55hhEF=)x>5GeP=j8Q=OdGWg zZ?O^Rr~42Vq*V*Yx0}y#Q`eOeWhV7AtPRcce-~=7Y}ptTMf>tP+#c}#AVr$z?cFqO zofh@KKwtRXw1f#;Jbv5R^I9(`C0N_ko-Z-PQD_~|V0&tw924j`lKvoLJUn!FSk*Q0 z?CjGWfB!RLZF+aZJPN9`WQQp7N3*TTy8ic^H7}Z7^}_P1T~mWriLTZg)C@)aFf($^ z4m(fN6Z?vz>g?(k#ow{C7NC&-{!yWM9#L|&v4hZ z;N#1(v;VQA^Hz=hV`W593TERb;3JNNGO-qF2c2$*BlAwNJI{hmX2KRb}q zC2wfO?V=-EBp*99h7FiqQI&Db1U4!1D$GQu#g(b8KbTVOFt|ryoM>BrADr9mJLJ8< zcA&r7_sWUtOc;91WBvsn0f+HJZM}VABadb1_*iX}(U=F-;${a>ZLWvb#hBryO+K{4 zcF#fWcZiRt{f!6t*lUJJ(U(ovZMEaT3T?5Uwd4Ez`E#Kv^F{|pi+yqBfy}fvE_yEC z3}pa*|xi=Q}1qV{{a{!9M6dTf@BO-9fZ7|5J7y zmR$;D@Z*EwiU|C-T?vyQ?Npg~#Jk3EbFp{78yWKX`PqDHR+q*rP4S`-2kAtZ3_NLUys*#zML5YnnK`LmnNVXF#H5jca zP^-f7tBY^Q@EZP!-&N=-SQpy&Hdbr2xIiG~GDw#c4T<3jAAXYglg+y6?vxpozi<5d zlVxh%$=(lE4y4HvpudmT9WCrlT9P#DUWw3rtf;OhwJY?T@?^cXK^+}56j5Y4Tgf;l zkD&Qg09j_9Wl@)h|J3R5Sql7QlkP}0x4airD!@EbqMI~VN!F3ar#O!(A4{m^unK+F z#=gDov_Dzro=&0}ScC7#Xq-_mPtnwXS4X>)jQo9oZ0mW_aC0lBLgnZbxM*DRR}Dl# zHk9xINs-Rr@r^2%f@WndrS3|6oBJu13RUgdZHsd8UcnekY^8Zg#V3x+{R|ee4;6A2 zSA567MOPbqDpWs~ZJ07lKgwm#RDAeRwU2;C^ZpkmS}fmHO=tD0e}6Gk>)l_&*j#1l z`Fa*|_$zLWsQnC77A}lr8`so@1^<>4v5ghSmb(YXsy~Lq2P?o+N@*Dye6&8>& z$k$+!>D?@~1?G>$^Lf=5e_AMAx=wg=CYN+~cz(R-{(P%>nf}${f-~8cROf?lnmbnQ zxHI9y_(`XBI}>F6lruT=x~nW17C_Yfvdf?}0w?)18hVYL%s;Y|I&lc*s2jeYKkVF5 znuvd*ooGGe7UdB2$UO`oZCTv>=|uT(xDZIWf-z1Ke)zj-$2DnWWJ`|-I|-X@Op zGExrZo>;u2aSx1`)J!-h3+n@@4y}&smH+{Hmd9xJQ$H7d)XWczd7D(A8KJ4OQ=vp1 zp^(d+p)4eHz+pm-bH@$GWd_~bV+RI>!i!V&53+r#s+&HkR%32y=%Ou?5;%~_xqhYW z3;!TX^>W`d^77!DPaUs8`>SB>X!^^K1e@6Bxt(3XE|F+~lQ4>W3qFp(N5LI0o%e6u zbk>(l?fucamr5N;MZ_b$osV4SFa`Esi-=#`E|!S}`edCoRvCMvMfyF|gxdA~MgLcT z`m^!YYXCl1>2G>_o$23NPPA%ET1#8Oq}Dy?I{F*L$7MEj0b)6@J{5+Or^M{6VHI6}^ zF`CJ!@8SuJTMwv)%;)&Isx^{@v066j?1bfM1%^@9nhdE$ zaP$EQ8+WKCh>=`=)lrC+u;$I<+V6U_Yejr>dtFnsk_O)~Av#=cF1Q&9z)SQUhl zW2Mq+vtUFuNgQD6rQBoQSW8U_Z6rBb)c(*nQM0ZrQN}!KWlYh{NJZN%Dkv!R{+cY1^ESZiCHxnV zj5)g@u+X!x1NCM0xm^>q6 zh;`oO8=yK;Nf?QbKmLj2GitH*pc5G=Lpmqk^!}3s4lxK_6R_}zwahhbkE15ukK6c8 zJwuL+!3csDXUt0UPcKARz-!Lex#9nZZiT3PJ5n<<+&RMS%SUzP2rz8CK&%hK-I)0E zaCF} zkdf>{skA%-%efW&-xyo)G0Y`|-0vKxLW*Prh>58g-gz zTNyaUtS=)nNkFE-lq#?eW=;|?41U*?srZU91XtoE$9Qg}nW9KQVxE9U2_7{d;o!Gn z@b0i;DaULdT-C}DX^fl*wUXSc!OD*b^tWo2r(UQn_}rwQV<5gutFG0XBqQf?Nr+O! z8DYVQIdA$d@a#AzSwtw5NUwYq!wu|g<+qJlrj^v%tJ+YEyb83Y{q=TJ%F8Dl#A z8K2+_&5D2GXrjuQN6^zK_rmb?1F!3TRE*S%o6a4-JH~V>!=guJ_@w%_sywSVenv>2 zv(JTg*eqNvFkI}Tk;9v)Vt=1h#grBM4-);iA)C=X^s%jccN5ArUE!0!0id|Qrp@XVkF)#x0475) z%fpgBzSlA3B(QUIltPbcz39#-gnVcp+ngrfX`;$W{v|7ny5gf^j3h1%{nyh(X{-{- zpYjIv3p0B>Gd&5sm1L<&akbgym61hz(sGIv9bfv(K1VKpVX^z63E&`XZ*x5c26>c< z2HzwiqkN-}LwyCLd2{l(wn}|}qo#buzpK4tCdA}X8i}X>G%nG`aoB5b?Czo9Z${VZ z@i3DEzAHF?c+Bz&A@AaJsIB5eH*Kk=i8Cu5pfTxDnS~jy{5&4ya#BoYj4WX~t?K)Lhvt-sQ#!Pe3m<3HaP| z>o7u&+jGF5eeqb%;bi4adkKR3?KIE)^crnN(~_){xU!bXfOpGp{+1b_6`o7)ww)1> zRC-cT$vSqls0Px*t%gfX$r%EPGopsO3i@;Y&dY|b^FafdYt0@QwiHfJp>xkeRq%)i zUr%2zErj;AG9Td1H@;t-`GJlCuZ|Z^Qz+U;eM6dq?d2`7ITSg*q^znzD5dg={Cm~f zS7};0>U~V-VS_l8`^O*R!2iP!88eUxl)jdJhvi8JDSnhppC&WKe8|ap&2OctT(` zW5q);O|FxJt&f}>B#@S(Xt5B8RQTO29g(K+T^}hX3HAqy{LesaKTA10$BNe67@UAm zh=Pch`Oy?IlCNAZ>+jP|^B_wEiEjjD+iO%O{YFV}mLzCHyvDJeD_*2_b~!manzk|> z(TGMXktBmthEU*41`Vhh*F4F4dvqy)hF~-_a`yRoXRUr~X%5;&o9$BXt z{`Cm{^Mxs&W#o7D&|n%>0?TX~g`zjTp2Gs$RLQ?0l3laKpN`Hhdu37N$_h>P_U-CW zzP|YoMpgK(KOZ9lLzW^(@ot~X+?bd}J1fJ|d9$)pgd_VMbo?)Lc|6XaIjt{o({<-Q z$Lqhy&yJ%j${|dqV?V?>YqoXq6H2gNd+eVKe+3xcWN_QBiBH^6ky7s`ufIbwf^~|S zNSRRgcX3BdFOMo)-Y3!dqgGVwpeXJF)pAu1@x1Z<2ekdlRUTfGj!`}EYB~+0oZol1 zk4bPcztjj|>cTYd#}c?GaKl8uW2ny0RD%iX9L(yVm-9bZj1BE}koP}0bOgwk71wuU z?OqxjHBF{>V~Y)10!Lh1+>^WAJN@+Sj%(7iz9x(~jPacj8*5Ge`B`-3eM*S)tJUQ>8ZYzap$WeR$!?ROC<> zL7|!E!5q0Fx%5qmNeJ&m3V8TFQAd^H<1hy-QBJDueTE1SkDL^c-e!{+YILYT?+rbh4{rfxD?#)z% z{PvM$G<{y;%gKa&%Osl262bDnRI)_-R(#9(1hJWWuJ?CA7bF63rBVH9wXj%|`eN(7 zcF@?xUXos!DT;)g8W-U%?U1Bl${)Izw+!3#G;mQ-lJ04G+JEn??m=8~c*;qk5)_{_ z2!yi2SZ6k-ZIAQO?^!1qCv`EEW;upUpY?k@9EktCS|eU;c)~j`xw$cG6+i8=I0a@P zWmlJ>N9=2pnC&*iuer$n3TUN_mhe5xHeQ^)&GvTVrQ&W&;;Gg8MREA|VyU>fA^SpNhVp zJL93M5#W2l$Jv|xA5mu+7G>K;Yv~ZAyK%q)krK%Pq`N~v8tLu^>5}g5ZjkOAk?!v9 zmWKWCetRGL55Jg)8RpJ)ooij{>}}c5NukJ&RbbD=TGm_u^C{q+HI8jEAi~iH$atkO7z)?W!Z7ssO=C<#$p@8^rKNe26 zM}OQLS`gxZJ<`vBd`3z#YnLR=8!_UUk&axJC78d^&GxqT&2;LY(d4@tk<(rNQHqwb zBcUn=@`PRh+k#XKI%~P}VL@{a&B^D_MP^lvm<)Xn-O@5|_HU*e;5|Rf(I>l-)61v1 z$|TEb*Ze%Nz-}I!C4jfRjB({q-zXXK8=x4~u6=PL%LVs@Wmy+HP{@>OK2l!Zm-^=9 zweBNYn%)`4+=vFe5uwWh?~u4vUJ3mD$$W$jUkDEUI=fgoeT^CeKG=AuwBGvXHg|5Z4$)&rMFzNj4z; zW!o;Lr0WtEAXtv=I&*nrKe+cf1nn9%8=TK*#HW{Ymp6xQyFhq|?N-PX<;hMgyJ8m+ z8yZl{8qgOU)8kcrmWSzYtm=aE*MfH6XOKml^M=Xx?ariZ0V-v=W9+bO^TS!cS;(Th zCHsu|&%DJ3PVV%zIsD<8!5&K&yTz@UzyS@OWeGR;-0tIaMDuj)3JN8v%tn25ZPIu`BA~-TaODZ6IiBC_e7s_`tMKL{Or6E~ zJkT*~45M#v#6Ox!HAzr=U(3C7S0nZdlW+vVvqQUTg2NA!+A=Iheh9X`@?HDHU>kky z@CvG+yee+V=XYGS12-I>B;h&>FsJYW3Q80+Hb+2lF_TU4?ZXXaS6WxxFV8|YS`SQE zI%b*t4^C39)v>O)Vt~P_1eD~~42u`T)<`Usqy9Ij7{``7c#c1w)Coc9n}8B%+gEsY z;*AKtW_XT2<*y`eEV1R_j@DnUwlQlWsPQ>j8t@|G8-yESZu*-1IUFdrcg$N%OfP#SmrVLLkvVu>JNN$Mqa!qLtU@J5A|Xo`|glJT;Qu}Z=3wBmsoP{3^PDWken|v{sK!F^U{x?7wvyC`E|A0vIu;buA1i}Gj|&TF7t4Dkk<^Js zP{BkMf#!1?-v)9HC%XQFz^}vpv@6ecZ>+OTsgP|V+_&8MuqGwKluY+Hq9|UN2Cgh^ zdcDtdr0Ag_vsjHbA3e>Xk;=Os*MW#_o!!+@?+kt?_4y!ObqKImHoWeY8_fV=d;D25 z;KPfJ2i_B8MRUJ}=k0w8*D2j5E?4CcFqPHi*?2M}+NE`}GH25h8O~C7HS%IH?%Or? zzk^!=c{Y;d$LfH?uM5jo(&F|&OzsjhYX7^1lI1?keWNHdESb{Wl5MGjSa^Zv369X3 ztV$MwD_-GQInxF_Rn>zyNebiqQ%FS*0BpWO63vi~-=5@y8yfiko3+3`4iLX55Iz-N z58vh;0!I(7aw_ooZn6JPv=&-!SW3PizE54>Pl_R{d9P>bG}wxA zzp`fEH(YnU{WZ8xV6{$t><04o9v`(=!2#<^>-Pi)4o!oAx@x^RQ)Q+Oh240)6lijG zwI=koChPPcjlgPadIgwJFl$%!B~;zUx&2eJjDSKBihzs)GgEDH+qZzMEHIn#M~}hU zH*+4}doA)$WyWq2yAl_6nX;D!k#P;{%|5}*hF6hBf>BwNLrlTQBA6n8c)eaka&ZM- zFDl#={^G9+i=u~18H=ektSE$>qc)KV58=JIJ2xB}7z1IWv$aWMOF{UDu?oylQ&IxO zEjTtBNzR_$W@cg_7*zU>nHOom%!b+UQpiLn6#y%|fE-k6@10-(wgUFU9LA|rpRG$$EeNF1$V(wwaS;k`JK!5(T!$q+0Bil6Dw;H7_N zM7FhxfHhSdYzD!)J0E7QIjo9;ICjXf2;lDU;yb&$Q)9k3C^8~Z`GVpR$$W+>m{YQU zH`;#$?8vJbLBMB3`4KBv?1KvnJrC}|)Qa5`b>r=;?zO{?aKqv!2i6V_``=dHx5;1EJjJvy+0;_lN z?a4*<+6kIjm+Vg>X_qm2?GUEBtWzxD$gSqVyO)aI_jvdqr~n_DL+$TXt?!K0+ZVdZ zwpMy*{q`2texGaw|LWKQzzm?0r_7OuHk$u`)69g{G$!=*#5CO^dw76G&?sVgBIQ&t zG8-!}`lvHMn%{VgIRlZ9lWbgETzcN#FF*wo=wS9t7Ydw$X(J3gR*l(+?YvfK&d9jY zkiN$#|rwgp0})>e3gti(8W$Re#NG6SO^q z39y5+hGaS0g`T>ynG=`F_}M9lO-_FGDWFRVfDPCo@1Nn7G)y;|{YysuwQBr)0i9&2Q)aKotW!gRe#315&J9}I zal5yp>`etcc8q)O(JNF0S8WV!J=E$B9t!iVKg>mU0yMwgSh7-Fd2$Wzx0D}x7^GBA z@C@yT-F-bivghONp~k9h?I0QzOjV!HajRiTrbzV|yU`UG`?lrPJNQH4&aoOTf(c#6 zF4McX-q`!{?@PG(`0i1&GA>>=_?GHdAy#g2P{kHrHGGDFC8d8n54I@gm&yVN_wVw3zT`g<$0 z2)<7plKHFZbB6DSdysN;+v$(o!-yHfj7EBTaR+nmncmjOwAD%E&I?M0KZAd8qF1*? zotH5&je2-3INt%yO{z+B@Mju5Pf=1+d9H%+<#f^}D7am`x_d)8N&HFE@{5TwGQ~&w z5lv*cKOUA$<7m^ad|HtK0I$uC26wIT~|#-whRF0Pcb{Ri85vKn9RfW=TV-@^x((FlVK z@{hk-5wHK@xA5>J1+YG4i#h9l&M4R|xA_TRI^gFy|FOGIFy>D%)HKacOt)UKJ<__( zxA@H9=VyKf@DPyK3+}swKe2lA9z+SlEf(#r9v07NnI#zzuMqCbKMyaLqp({GyPP8; zZb7nzJwV%~070MH+S;(J)1DxVyMxk7`OvdLjsNd3bPn;sa5oFk(vHH}L+)-p)!Hnz>$$B5@+gDSTj3r)&CsNL)~iO|urT zuX8}kv?#%)$9heKAs6h=3gY=H%OUEp^`^CAtbFsvV%)qPV;8vh?)~wP(|#Yqdn09>V92D-OiM8pNWy^U zy1RuqD1z@Y$ri6)EnKGxWtg_@femW&r^sk|yel5Q8P)Rg*tefU;9$^AQUQV*>oo9c z)u3{J)53AR_(E^j1E9bS5kMK&_XWhA^RSe1HO$A&I850Na0k)z6b*IvA^fPf>rFkG znG25|YxeBe!pdOP6y-x1vpViN4L`*?fwDL*inm_e$hXf;gBVPjUT#EH-U-Y#j{d3z ziWNXIfhpT!oH-e_`sSYI*;DOrE&kT3yZz4HBBrwqdZS{Aa*aaOQpJ*~@gZ7+*{wc_ zAsuZukC*OksAnX>2dYB2kZtoK(EzhL**5J$4a+F`Eeqt)2qCXIxx>3bsmQ0vsApvw zY#P*j;yO1VNM0a!yo-T7*#(hFR_OVndp&5Ek-zPV>&OQ$PW~FPPcdDxiIv2UwnSCW z!u%*R@bUL)o^AB^AeA3cy>cfJk7l|ug;kLs4fvjq&?OK8m<-vW4;Qqd8Rj#Y(4Y&= z(}xSx!8LV6Bzws0wtmQQ3bcl6_n}SNig8`XiA+;*6x#3MIau=0{O?qd6e_GBj4NV2 zl@I&EmM$BznWO=L5F_tMNK%VZWU+&kQDanPay2IYJY%Jvpps%q7jpXX@jtr!1#-|b zV>85J#L;8~l;Y=4J6)W|F$fB!vI{Ba3wrqQnE7JYjp0p16-f0x`bR8V zkxleU7jx0?MF*pgz-4neP@Lh`U4!qyd;WoBNtdXxsh}M#cpjduV}b(^((13?-A$pM z`rV27J1_I~E-gftQck}08Mnx{_IlqSvQIYE6yIna|0ucOp6ze>}-<317-yTZ@f9IqL5kYm3iJ zMz=~^XaKjGn)$v-i6p}p<>9Zh4%l?n1xlpm(&cOCy0b}^f!{aA%dttw{Cu;0%yLL* zlx3hd9%cK4MB(b@{*a|{7mt?S&Ba&IY|bP#4wbvaD5nGg&)coi;$LE1GzjRz?+@d;-Di?_AgOnnBFTZ(<4TNLW2MtU;fkW(jwj2 zty6U6NgfHFb$ODqBzNeTyzw<9LhLyq^klN+era)H@_Ci+@=P@(91l)tGB`gpCwZ?q zf&yM`2A(W+*r~hcJ7VNN)cwF79ubW%a3Ut4Jrg$=t#2rgKf82;Pv7k;zb8N1gEW2L z7+A)>sSk5VF>00+CRW^K3-Yb$%Cj$YmNWAz=E zGkMtG3O~ckY7oH7ZB?VSAKC#lD#n^x9={H)(P(Z4N9Ky2rGV_};Fgy5tP1n!9V6fg zv%d#M8`JN~Mz`7;TBnk41-11&lwS>Rmbiu4{C6#b2HAQ3|Ly00Cqp7W z#8c>+rvLv=hJ#gQ>_7NAk}li+Os_WsN7P-;2%(l`BS%L(I+~!Mprg}M9O!wUq|S8x zvG;{1g@^(WGh$(71;ni)3U*b0NYW6mEsFmT4Z&&XsMz|(z z86jH$aBahrxo<`jai#GL5>TlJyuRRKikaHXrz||S^6$J{7Q&2XYQ&W#@uHAia(SW~ z=(z<+U%;jEha|QEz($FJ=adBwSKR`tm%B?)BoBzUn!>cAk!%-W*DeUEn0_y~k2b3qJpO-8}4kgZsRmO8@$_ zj)#JgN(z;xTA()WD_lVGn=2y7AaZ;wcsT-UO`i1F!-4mFa!B0asknLQjyI!$n1bpB zvV9_4M6oHyGFvikIlee0PkwyRnZ0}BaP+!t?2lbEWalOhq_u4eUI*zy977&iXe|G> zh5L6rl>FiMNS(QK)|m!m@@_Mwo-qV$GF-UX8!moNAM(u0>5P7UkMElptJAkn9O$nP z>uJM2{Dh|z3-1L~mXkE_eJ1m@3D2)B<2zmo{_=Y$?E=rb8{VV`(A=p|Y`V$eYY0=F zgfA6|Or*{#@Rnu`f&WA}@FDxH$)_9Z)bo@v{RM(IX{6Jl#!bX^oz*M9Kkj6Y@OypR zrtn@~y8(>eCtd*nG<$6)NZ5zgkG1QWdR`@`er)f}vvHb%dlzI~`gl@`*x{+XdDvp; zr0V!LV2ZzfDQUd7uMTPIiUp%#-B}Lx@wt?~7MMGw zARBBw5_=ibA-a2gku;kO_^1wb=RFug?1@NS8T?M0NoVVJ;W!y5GdpNk(e77L5IlD$ zc^G!2qG#_g)%{)5T@{R8tW!qp?*B0X2SQ1p5Tb?Oiv;hq8rXp3NwEa)x$29teJ|ev zJj|Gglp#8Z#{L}Zmem7oC93Q$pje0y^t9M>1**rRY##AEMEBUH087{G* zh7FBj7T*gO^n#oLJx7NWbowHdl|^z&qb$KGbsy1j#jeC)UjB7&@V_u;yC03C(K!FS zPR%++Z-29-sicGGf4lA1ehDQQFucF~_;Ysd6WJYWmWivdQ%VJ2|Kk6x<@t zznM9!o|n3Bj|1eDp3~gi*qc7I<|1W*QEeFj!(BU1US9aTR!Cz@es!DKSg|?V0m8z^ zPct0f)O1Rf80Z_VI$f>*+He+VoVirN>8#S(m@nrokD1m8b8{ z9w(elu2H>lttqszhU%6ZPLvQiXyB=S<0-TJt*%9<$N*BS7pml-K`tnp@gP3|FD;Cz zo0hPd(H=X6$6vFXAIcD;;|O&BI2fn+6@4C;9v=*#Y_CwZInnP_5!G)8W!4UJD;5)0 zCjMTXUHfs`-jefYO)bf1HgcOY%_C+?g<4D#`)ul3hgHoQ`3jQ?!9?M_&23a6v)eOf ze`a5i^psb!^ceNyArNk$QC1mmIt{P4XYawM2O%FXYlN&Qz4F~{`V*$fWxqd`mDQ_# zHwXN~Iog4gXI#7uHWp#=&7*#n0X&P`B$j?oxt)@QBMWXp)7P}e^**M;zxv6MKHwBg zQXQg4TAvg_RzA;bepWY)-z#HNQ#Y@zpV2j+7RJLu`k<82PWY8=z00Ya!`acwlCZ2^ zG1aMX;IBr|bscwjvn1|4X}TO2Z>Ibh!-*AWX;EQbdwT!3G`G4AJ5O0fe6cEg#i8xY zbv0~&epl~UU08>q@#R+Q4=DcrR`d(Rn<27+>;coG!o%AVGFvxV#gKtkiR3xmV6o*y zRI@o7?K(uE1G-CVHyz{XnE0kL8Q=caib$#_$gZR2;YvVfMadbf!T{jWTs0F96YEC} z)HO3nF1vUW)1m&|Na#Xlly*=ai=kB77!=&+w7T=Ayj8sw(T_?-7Pw-^~S!~9Q5Dx zfiVl$FZLNLZPfEAE@iF8L5vMPpw(C3AR1CoYd>&f5AN)eO-5w+$8#T~A)v_bnxn3zq z)zf}a8VH)=r5J0c=eyF>LGU)eNnmQ~$oxz2=ODTAqfWbcH@NqPl-gTR!O=7U=Pv?g z9>0f}qEK;}V%v38ONZN#wRde_4{xkSSD(CvS$-4%Pvn+RN8M(F@M+CF$?SJoNNy<3 z*Pe-Q>e;yQ5_8OySOrTAeN4f^w$hliAW5R!YAJ(L6z7h1Xl1*pz4cqkdt59Ta4OX` zs)^)%k2fv~^Pw`kb?wu%LHolonb&n*|B8n6utQM_c{(?6B@L{=?u#MB%U#N=c8yF& zl5@G(!q-`@tg6LUVzrPS&R2I3EH7EA%yC-%a7MoB)iAKxLM!4J{Dre$ z*?aO}gLAH+x66IDc!ZEv}xwGQlTX*iMRX7R`?;2$IPA`h^}h zW%0K&h3)RTC;7^0)x3gy(pN}|jGU(-SZXD|==Eo17UOQXa>k`G zH>>Y`pdVGmVJs-Da`NuxG^}C1?RYK-*pIx+8&5d3X8C%kGlrOeF6Y-DQ%hZtF17o1 zsnr+pg&15Nt;i|Sm&(o5In~6lyjDRDG2MDjj9dMcN;Cp-f{MCky%@!7p&e)IT@ORq4 zf83BJ-l<3DAKHzE{UN=T_?Nf~D$75cmQZ{>ZEx+B{0H_Sip+*|{%0xu=Ri-t4jIk9 zrWQ2y<}&815J8NXH*^L$oc$R3q)h!7R0=Y}#Std;CY7pcmc?uzQNk4ka~YVwAm_qE zBW;(HD6xCZP*5F(vm?~$C(2CA*t4`H$Kw3KK%<{6{a9^&)!-}EI1-a<;&P*CCzw&y7|-V$8ef|_oYCg&VLg??^Z6`rjV_!GG=q$ibW3-n1UG0%incP zqfkcqn_8nORJ%Bv;BA}~eR^}2mR}5t1jBi^!w`g@Dvnr@NubzAyI6@nzBrC8RKr*D zCp|SLie4H3e|mvK#zKZs;|_5T@l+RSkIe+@Feey3D2Ks<8U>ntY6!mmdS`?n>BuZa zh!C!du9_-O2LO9-C%FMpDiT5QV^-Xuyrq0a{G^+iZV3zsCRCim;$IIulO!^Bfbd7b z>OhV12^0K%wyYm6!ah>CfgszOGZ>@sy0i}Ck;BzB83uC?zQSs4)$c3)=c*k?*vK%U zHGi~PhRhqw9)(y=vs)|kNH1_KLjOr^3}0TZtFc+P0cJjSotq##K-l}QFb0_V8Vrem zdf9Ly)o2UDiQ%h7V4(&1^4epl8Gx2=iVp(5wMQqF&uA%oYwvne`F~ZJ=J0C=QhSw4 zRsJI@=PUYE9JG+&y^sw^5OI>`P%@a#VF&&sq=oC6nD^q+CHz$`#ZzwkwM~>33BQab zO(uaUnf8@6jW!KkZ)1PYDBi72w01sA%`_$$HKA0$X)VZ<%s|QxP-iIwl5oMerOM`x z^wY%6>U|jKh)M$9G0cuQk6&5q4P=pGR(e5E@eVza>c-pB00%cpcB%7B@_4Mb()Xff z7tg(0U1Wi^($5lkGrkRXWqmfdX2kfwDSq=!JW4`34g@dYdsx7wM#j-q4+nsWh2FA| z6C5}8l;C1qgzL~OlY%Y-s7%!n{GvpXtOxBA%gGw4`%+K6#RjOKZQ5a)4trFp>WC03 zQI(c7Fr^1M+59zv90(`4C;~zWL`;DlU~n;YFXpuJ=dG@O@bncRSWUr1;A54*q!z$p zT{an#s!IROG{g3TEs6QaIr&|ST0%e+%XNulX~qc^mOo29i9r)eLM(GV>}xcH06`%~ zC)xK~tFM@W@B-r02Hk4 zdR?}d$=@Zv8?Eja5?syA5*o(^hk|4$H2+p)=6O~N7T|>KR|PHdLrEe~G%>VfisG{s z5(qVXDSFkMx~D>_Y58V!yMHhC?Wcbx3a`?l@Xym!q{G9D5!P>F!TDfxAS%red2TZF z!y$s_h#D?cl*K5YijF9TGezsO7qMFXQt$j zd;%}f*!r|G*y}ZS}ZZ{{b-L#$@uuJ2T4sjv@$~~E%>z&A)Xw!`Z06jGMU6g!Aau56@fYg7yT%H?;y{avB1n7s z3ALwjp>UxdHLd_*J`aX@vjpykxrp#!3-0Nt2OeUbWjZ_O!XdZ9;EIp_SmF-Swfi5(o*K<_FgZh+Tmn-CzTObD3djX2v&x9 ztUo@@`#eQSjvf_UfqR+gUIp~)T$A!Oe1Xp_+ zpa4=hB8$`tapHtr+O`tP!~)3{1^LH`f|RlTf1FVYgqGHS>RJDNjvl_m!5EYR6R6PB zAfVLN!`KS%aoxtU%;W;=%NRqxf#5ZweVVU<|GjXUOgQX`ZX#)EKWjj?~XH; zWwHwhH=&ZdDFpQ2@k~%rN#_(z2`f(!_6CUd3<5Ft|3b`6EX_#*U@LY>YFO8yl684G z#ilf{5;4NU8R7gy`sj#g5BQ@P77^|+q4Z#auS%0D9&bP~VB|B)x8|I>OB8l?-0OGI zgLllJXn96dRPG{4ZtwRrdV4r?I7{Q0tHdnnbHFz1;&lX9wJ^aF?odzgGY&;zZCwV+ z*#ooroRkQ~mx~l+=n&4I$&&}?UHmX07G|?UdU~g&_>M<{GNu z@9uVAN-YYx?E@pEWtrji%WgPo&KRb7+ZOg%Ek~y=yZR_sHR3%y-&qVt<832WbOWW1h=dO2?XLKZ&S8Q1aaA!&94O?STW#IRgLx%{dB=y$2~ObGax$ z=OStHjD36mcj>*N*{z#g#ZsmE*PC(|jFTg^`shU+KH#N+athF0f2O*QL8}{;yPfn_ zv{35cSol@@koSp_RC%h{MP~Qks?|_qKwUR*kiqWsg{ucfplX>xf$*yUTLv$w`3)#U zx&sR2)i#rlID~#lv0JfsaoW4M`eV!)GWMkiS4CL`lV(4WKa?5*?MhWsQwmxc+nui- z4U3Hmc@Iva-xee;i(*}N;6PI(aW`WjY9Y!DHE`PzCD+aqmbbr7ULAnz^PO_LrOHk*A!V2K^{+n znxNz0`^%u;<$DeQS{eHRV@3m<+RCFRi35eO4=h8ml)cO$Plo-@2PH?v=HzO!B3 zd>|mtU{W{T2PUZC5Ib0w5)Ia4u|HW&`U7K|UZ)(B5WxTWE2OLl&MG~FlG6#8I#U+Z zHtA-j)_?DwPx=gYnTp4OrX*1jVy`@CF~v~YcQ@79Pk#J$IIy$XIfrIKv@=9-QBg|) z3pkFc&W{w?6+Q-o3-W&-q|h8w|?z_db#7*p82-sUoT+2e;dHIm%94rm^-OsS<|41sY?aO zQ~ri}ul?^1K)fOVL_rO4)RC*g{QtrO#zXOd#cTV$z40xs$P)WIlB}sKWS^AzUPk*st z(tY{FH{&2&xN&JC23YXAMl^ghL~@mtO=jONwN2`ss*-5oq}^El!w8Zte4iqc8q|WS zv&>l4Vl`1_qAW>#?3BE`{O&N-aSZ!j=&keK_%BdP=K{ddAnFiWRaA}=2)=4@_*cVK zjch>{5$Q~$1~50NG;}o=fQ+>p@c(83uL7 zFl4iFc?4d2Ai!dh)iXia;1rm}M$^gy7kj0NZ-8k-scBXJbh&n7Fu`V!_eqnEDT9-- zg``NL5ovHXIJ>T?5p#bM6k^FmuDfKd-_i?4-Jb~93B7M0hYAa3HJ16^RpK`4*5s(bXZMmZ;l_f=e^@a*ilK;_!V*66+ z>j$M>!@YC!^--e5#{=SD#W93p!II4-?6TP1V8F6bpc++w%(=6j+rHAFRx&oAHSSsk_uC4&_53Uh;^QY3k8q<; zc<)0fl(<(r-EJH6j~|Hi;?QRcadE}JxQyOvWD3>?6UF$x6U3sNT=Qldv6iq;pO(ET=WY{xLF_g^TM)2h417J$)9@bYF zUh4q@9Jme~=j_Uup{KJ8IXj{Z#Gf?ud zcX1Jq0sUW>zb><`y@8+3)BS9jgDNNQ=>#uNfj;6<8A(~4gS1c2!Ty^4#nN?=74z;RM+92|734+(aje&@PF`{#75HS=0Zu2lv# z6H_eLSJpoTktOq~hnFANPWcNa89yIg1C3YWC~fA|mF5Nm3^tG>N|$D#r6Q^R0T@m?Ua>T(uHozJJHg=BQA~V=$04vf`cB8pJ8}8M`P>ku z<>(0l4@-rMD?$@O6LGldN-ikw|C2Ii-^fTH#rftgWE8kgyVzE&(|*YVV~d(ru`UmR zm)VBT4IoU*@7Ie6E-$OMstPv)4gEEwzrl%7VZMLEFMgwVCNy9}Y;Tisb{X<_KTi^T zh%6%35A)}ZRsdMmuh1$=w*_Sv7&KxqOe4Bms^Y@&;%2=H{y8A9csJ*8!{ME2OLK10 z)8_#JIfIROflu%KeNP`K-z!s1Jdd)o)u#E^di|*dh!X{%)u~W!TZHBOFVXkd$&})Y zJHrS$5yU@fTxB*w3tjWI}vEzd&A;=Yj)s38dE`rV-_lLc^(U`U3TC zN3EPOv8X6G1b9F1Du6vgrukts4gW;=JU6!>r(@RQ&mjB@2&#i~I5BkX-neSht za$tc>iYD5iKM0vEbA|62UB<=V=|iVkq4 z(N(1U+5-#Z8S#qiRL4|LvI)%mHyMq^3SAYA!f)U5hJd5iHrkHgBl?9%;kwu zpIu<^l6b&0HnsEVAfGp2T`i9ycmk!qRpfivFyC6u{Kz}5>pHl%%-^7`UhD<0L;Wo< z)cI9fYUA4y%g@?=ihtWRO9{;>f{*UJp8R^?Ld=>LD18(>&IYV|P&p*5!d%$U@$6~! zTn%gAG25l2knPX)9tEvXt8-b?7Tsl&x4CBOWjg z`oVl*vxGS-l{5ky{lRlUZ}rF4?D*S{QwK7rebfqZ%*oAYANYD~|4c~$dU9b~)Gm|? zi5HK_RKB=;+@caFDMNJh=do}Gp*2GNPQ4~EHtgb@N8NX0orz!Q>+n*BX-kY)>kRJU z5<`wRhkV<5Oot>m*6tMk2*?Q>d^|eh)Acm)+wawXIZIm^>$5c=eNoZT&Om_hhwsb2 z6|WPiYZt86q>*F;?#U+D%ymMfOnUlIAiDs`7z;@&-c&wcUfaOYGF`C=ut%bal0QX5 zO^8|RH>Qkm&mTcscg4DED#cR`YU)~YPzKeiHp0PoNop`kJo=m=TqnS`6v25!H)k8G z?-*`W>!*iby_~ep$(vX~G7Q^G6|_smB#SL8N?W~hXCjRYLniC69Fra$@|hEnfOf7wer><4U;B z1fRqN8@unOvWsBEXqSVMsPR&i1gv9r^#FN#W5-SkD)C~GQ*PNf2+^Maob&@9tekRcQfDk5Nd0dNwhZ*gUGFm<0|vPQXN)5M$dep{XOZBubv z6f`Xnqs8gQ;v8z%qvQXr{zF5{m2T3^Q7u&{aUygT$;j+YyAlK=q)qDCmmZ{ESM6B?|}c7f`p&-hO1>j{w}dGW^*ks9r_PHMoKG5}?7jP~cQb94?A3 zQpswIj%Z*5H>Qk01t0N&i=_XmkUp5Y&Ma9YKe!^X$`2a~m%iEFc`l#;D2I9n;v-^f zv@~YJ?88Q6DwB*AJsqM$-j#d=47Dni&yx$R+QRk7t|Ip z2)P}}1c>ZjH?OWAF4#-7tPlVZDtvz!<<9zW2e;RWv%*#1|ayj@=3Sgc^$|dEPQ(M1afSF4A7-E_e8<$&h>GY z56~c!#@}Z2(=`z#`LcPS<<@R{(z5a3LZ;@&?YpRPHeqCDM+6l(8415;Q)YGSTNuFP zNp$6Mcai4(F!*!Q>t}H(kaV4rP;r5mr{s|)-rf1W8Y6=5G)Q4SCu{npE<%v5{Z&%3PwPBGsN~pKS8z7mhOM*u$=0~E zQ&LH67S~svN)}DI?Pm$SONE~S`ndnF6HocNe#WOaHTyWkL(fK1S-4Xxp`WKVL%d;|(oGRPVVXUK=k0 z9b0lV87B{~IUx{GXwEk-&zEzGmvibiq1yQ$06(wA!O6|HeYJ(G)yQT}zg^=MJt#%H z&)8A1`ok&n&tryzBSFsZk3A z%ggjlP_ccawJkBR1F3w>?N`z-3Ns5Iw*$hqX>o@ff(#t%O+(V2d!{vJFnr)Tym{Tom}pYIZ-KQen1m~%u-(jPQ6t+v8M5GuI&5CE7%qzkAb4zt9= z>Y7Jm+opD&t)iF1VR;T|>Q!a99q~j!z@63Qz(8F|nQLc8veTNJ*u%M__h}j1{X2~l zyQD=qKA-|%qX0ZN;li&Eo@gvPBW^<+H)!NXc=Os{<~8VQ+V#gw_u@k7ZP60Q40vEz zYk-jTn+8yam)h1q8*4*7D)4^hH0i1q*o)#OR9d5_sx2Z{?xg-6M4g@_Ki=C0h&4zZ zS>n%Ye5|QUiAc`&-4sQG@;$BlgOkRJ5dg&E&aARC{TQ}^J#1-#p}Ijqw>%$6MZn=P zVhcxxP4v~Tp1iobkLY*C_zPM2XlvXubRtU3U^P9#V&CRJK4r{yBs2#=SC_+Deo*Jo zrFq8c7Hxg^wq7@Ao`$MGkxTXuv!KSAq!@NR>rWf0?uc@LYu)l=b&G+fkd>mmz9r{}lMM1*bq9XSa)V}9KX9e7WmTR0tl7*L`(zu1<_ z0dD}MW7Dyl|Ke}k0D;x2L8?i!0BC(=%DuZxzI=(}(3h{SLWie}InUU=UqDb#V!L#G zyDrjMYY?(s0xl68$K4}FrV61E4grPI=q0V)V~H74dAsdgUWsoY(r^1pHYF{*@wY9Z z_&|v>;IeU^Nbt_v4Zmg;?>77Ew`?_WOYGzYf7@oC0E%9~{{vu&eJ?kwv<{J43)Nfi zs`y$AGlpS_{+mN6jZOc}Ar8W;X-6kzJWKquHoGx`+dZFEi;9c^KtHuh-*BVtQpcpN$;H5 zX(FA_tt^uq4!FxjL&D418?2n?)R>f}aLS5%T)jgrzbn6GeEltJ)1$I|z>Jpe6d#_h z;$-EhW=bHw5nP+mW9l5bt5K%x+1^}!u_YUQtm5a?lUJ*4Y+xObzW7O%8_^bb?0A7R zJtD-H?)!1n2X|uU1~gS-?qQ8+fZ$18v{dCVoBB&ELC8$XjrM^(@Svua_Ocw_~2G4%_*2m;`02lPKg9PCQyibk(91~3xw)hvmBe>YU+ShTRHAVw+d{LPyNvm)+B~4?jy(t8wFEC$+s&Kd=)j(W3Ih zF?V1`Htc$sxI8da^3RVWyP5PgcLVkmzDOs8h6V?AZrajgR`bJ8?DgN~7D|-pQ2PxM zpK!v~d$Sennrg8wbidoUYZZa$dG>QHHq?(5(2aq+7C{{{MM}UVbl7sA{T_mrynD6<5<=oyroGbRJeg6gg?QaHn$@w*Q z&yT4WKT_e@hZb}Hrbihr@hyI<4AX3-Xjxh9b;@_s-VUABv9>jC5EgJmPUFM-3`FTo z7MUfv5l2nEr8zJ+FI%189Ld!pU$M3`*UHy#PD+u=C9R#1FN%jxNy#ck&9{s1U%)S4 zyqWW=8uV-Va$+T@kdDd4D_t-Z%ChYXGzm#5Fi=gMV!u)IybTsxJw9(a)mYM&VSV+{_*m63;yPw+z%ukj?f>q`&r{c(Kz&dQEnodSF6mZaU`Wv z4X2Y$4?ip07q%MY{vOo}<*mMVKvE9>DT##!1lb5Mse+wmOplfgZ%P**cUn|{rAA@K zlEGXW)BiG&YUu96=>z6(G>;SeJ=VpWCOa*HL94;=(N;nVbMckF)Fv-30vnjEqmIT4 z)aKKmBe&ZzbN%Fh6n|MF4=)&6KFx056K>Wrv>VzDzaMS0c>W)__o2tbGc#vIB+0r* zWzkM$xpZ$U8U!RUtheJPfZG#->cvPn$lOmW1~oyYlY8Q|cf+^E z4P0c&5MD_aTT}&09vXiBKA4U=oHnOQux>hQk`2gS3zrnmMML1dzO0dP%O%8C zfNu*>-IK3~EB^N7z)&I|)8K#w6-6Owt#O~WLm>x?4v+ae?x9RQd+CwoUfC4RU!&mS z56E;(?Ef5vB%(}uH4v`w^UY#abH!T_%O!GpOi?}(vE$Rh|6;Xut7;S?0hs)oVK|;%Fy>|lq9u71*m=aZfz1RS_N7JY?4&vW`gBui0*#!8;B@+Xw~}E!^CFAB25~F z3SUg(a7qG#V~7ljVmKupHtu666@$Ul(}+o*h|{?vPvTt~KNb-{a>i&PsC{*vv>_6h zg`Uwya0*rmmdd8Hc=UltAyT+O)(~O9vF1&J$#s_mux=giFeK%)xR7Nk2k}>du zHG4M?K(i8GN|#Unhq1Q|ilghgh7$q_?(QF zcY8b6{e1twf}$vD=w_PUr}tTViRJU*8@h0BM0yyeIpIQ)^}GHj$@%{r3Jvk%>V?jWj)>Nfcp!cxneuPo)-Mu!hg~Qh#U$_J zlWLY3(gNxe9t6c&Bbgnpif@2SGP6y)&M0~G!f4^pA&}yS;Of4d;Tia}Ybkl9R`D0X zwIQ+?azq6cPRMMw69}L}jDD|O4Zr9!nIbaDy-m9gT~N(sPN69etVRfaCj=zZL~jqs zXNM2>8TktS<~`Z^PG!kjsGKq*3kkqM@y=Pu99guggdm+h^o+n)nvh~;BbnFGwNpj=o64UL#|W6 z*x$m@Uwj~BepyM;>F{sw$c(o@NC`Uj(I?{aRE)i+ajyqojXa`{?r}d>HmJ|(;7f>+ z!9re_t4$BZ5v4d^PUD8HNGgQL&p#mQeWry&6i|rRdxT5LWOf5cL_?;gY^S$BCt5XM z;2KX7K`asy7_E%EfrWZFNeJlapnIdo58rkNG!^??x0p{+(zD!ZC&T0ih?Ox*Dl(FB zX(Rk_8-7rCaNJ+59@{&)Y2QA#i1oNR1AD$*)#8qBX?{eJ+tL0gMLfErLp4wCEC2s3 ztFdN!KBK=`mo5cw({qzSR!dD6p3!#8;>xY__-umzWm5D@+S}OJ@Oz$rHLe}n+(Zle zB-K|tXsxD8ok^+L0X1T;-5sQ{>=~}l(_Hw z9yuz@VC31USGNU_>K52}3#?%V!7=M)}HoL zJ89pg^CP^<7a=5>!n4^$CKm_3Be;zZoAONcEr`woBxBdfFppY(2riHMb@7>L4gHDe zO0veZs;3h3$H(h9&M6-AiRc0D(imIgK+ME)dNvc~+HptD_9#VCLL&Fxpq%sL-u`3L z_yq|;Ze8^xoPl#!>gw(2JvNWOC@i3`bFtf+DmE1d%xU$$d`@ELMo0jiM^v#~G!K?8 z@Ar?QP3rSTS3!$?VMf|D4pc@x$bs#YGW`DYqu+$$6TeUfF!ncTgA0Y15ED@sp&>;D zu~OX<_6)zXSg=?is1Go`*c$3}$0(NwkyTa*`cP;mmmfY<2RGn%{Q5N!LdFzB2v`}R zi?sas0k#stA`=!S485-o;UZ?m`~LTqA!b4V;FgG_eWo3Iq)_3L5k>ceo`VJs{fETa z*Z8703NFk7IXS;jL!!3oaOJ~gWz0bLdDkiGW4`KFfcBh_KRL{zM>X^LZ@2_ zVb_nG>q0K~4^v{KT6PDx^`v}Iri5q@jBB33kTaH(SZ?mR`%#gh-;AFbxoCkHV2G3Z zc;q5D(bs-`bZ-Xm5AuE!1Og|Tj25#-H+2400^m~%Sdol~wR9~~UNb@_=QrPTZjABM+D$M*{Mbqy%C6Hyjhqw>XfYD{C?h5NE-=Hr~7d1Nu9?c|63AKmyr2Vt&ux+k>$ND38^Zm5%!pMDmcG-EOiH>abrU@7LGp z0K*(C;!~{V4l8G$(Ve%+wSZgcghH>J4W%T4k_jtRl$1*8`D@#|;}v8bLuQ1PFBwWM zp9mcne4Ltn*l<^jN@|i&rZjz0L1UCD%vB0Db<&(EtONadnr~ zbwQ$F%?=2czT+c44!3EljCm|P*^f!iU8qlg+oMfyH8;l(nVC#jUfRPtrVr@M06rm) z9tCZBF>L`mWfHm`>~%VZ2ikj5Xxa_gVRo5wb7W(ey?eh9ExfUbX?BJ5iGic%%4nQ= ze~Y(C9Ot&NgVCOwU%q+_0k5dO(X2vLxh_6JYOAS80_F(ZAeqSh2l05NiE!Fon2Z#? zK^0N3>J~Gpqn+ryIlH^3^VZeA1=&fSI1QLb?gvf!`wZFet>E8x=N+`SrXyPOfCgJ| za^gcEb2lrg1(*gNR3F74V=TH(@zbstBTQp(`^n;c>?Hd0XB99FCVf1^7+ zr`<#n%=hVJRP#yuJP5PO6lwsaIsMM2RgWjQ3=_Ivmov1;N}~p(2;2T{(g`pj4ays= zXUio~6!qT5RsWVn``O|{MpFIlC&5meZEoZsr{7xC8HoK*&@#QQvpZ640;h)E~94HAD?uelf&#BObMvHAUj`bOx^Fs|$^UgYhF!Fr9~ z9)rx62x?#s=N1wo6P(2%9J?rha@KWlmfQ2RXRxO_tCKvDrT(`{)MTQ~a(JG8tufq_ zy5wwy^)jyR10pJUwwA&G{W!UCV?P@mU6z)XL*qyx;fH-;3-h8~v>x4|E0d+Dtjr1$08!s;%}1ijd3PzfMXM4#YIS|@&CSRw^FgQ+z{S`1Rr z5lqeYfqgfB+8gT4YcrUK{Yu#|EUPNfAgsf4I2$Jz?okp4;R``*{fF&~8XIgmo z46qLY{;~}|AHm14WuDHK<&E{%E4$ZT1Fq%{ETh(O2z}(~JRkRSO5b z{qAw{w=EBXXQsE!xVMM(Di;$jDo1U^M`~Q-q4DuFCI{;k9%h~e*5(eA%S<1$*OGwx z$$)#I^$mm9D|7;N6pugVya#a*s^hBum8Dtkg+GfrT-*!n%>ivaUH3R`0CRcwYrq{7 z!O99}{ih@Z`=)tamvN3yHO=ZJrC&Wx}EH0nS9?34N6XV%A&s0 zQ&p!BgcT?bJjD&T?bn0B(4nY*>nAHLdpYKP87*5wjkHU(W03GKHr%v-CRk zITEsFK>zwAs5JQ@rvc+JCWc6x`&;OdTn}^i6~=)3m?JtHVVLVDL2A2PY74YMyT8#K z1395MWx+Do-oG!`H1B%$lWHR-i(Sv0A9MM_R zK3+?am1KV{*@vt8HnPBy-ps3tlg~{Vty*l~V~Nj}(FZJkC*KC^U1CW9XCYn;*MX1j zh8v+^BM@9~QY{e+uGF>p%^{+1PL3Bp|94y;uV~%uvJw{DL1^Vj5~>Bo5l?g=isgEc ztg3SoxV<6nvOcEFY1;C7A*8afz0mWq=?g4keEC8XBivSP{Jy8>FSSYm`KU!a?Vo#5 zhDOd!LLru%2V|zq@PDtxa&n%2ecBMmRx~tzf)bL3wICx7g`~ZL3i;qI$Cobe>GvvY zrkjwcIzvfl#o@FxVYvY48!uYSyWs07^TtKQsL<=i`3`?~lyf0@7>+1v;l0R6-|*v@ zAtrMcwChLWf7iNErTANj6*eGuuifx%*~`ckGB#htWs|dcSU8flI5Qm26=sqZTCe%A zXG7R0g!K*eV|j*{vNKa`v9!y?Hn}U5&mpKXmx~b zh=~H3L77UIQ_xa3k}G6u=OGPZK>_dIL;RDc4as2gx?|5}x3$ZU7q+%Fy_1REg6i#Z z)Gw7KfUwrwYpw73V$1Dd->lQA_-w0S_*mUm6&5}ch)dNUyX^S;V5}d?>#}huYDfbg zikdoi1j82Qps&n~WBzV(G(Dg*=itffoTur@?oco{gvz{rdp#uTkHxkkkOiIqk`3R6 zrNw}blNBMfK*;-k9Bfhb^(>b<3sL^LFuE8ARWxBhi{Eh1DIPf#7Y0a5=y)N*ZAqm~=n_kpAVTnms-(PZo$k6&}V{-rC z4e$#pc&?}bG?d`F@IK*U7EyCH&7xrLVQe;+prljgJuA9hEh&q?$#K?j3T!e7n5Kx* z3AjDD&&+09psYJl;+){DTZNft&e_M01rei$%;X9xg=6Mz17&@H#WI)9&--Dmk%y@s zr4eaLC9tX~K3Om{ZQWvR83@*hTY=kNil6s8&*@=sU7$T?yh1{Lq5(4 zDog5f!mjr?G~IugmKj2!X1*Q8)j;Z76C`6W8c-r1&9AVgGRvYdzC6tO1ci}+7(k;Ap@*i$?2V7BhPoXV_BI| z8htBd6=dRQWU|7i30j>|;VowMn1x4m{7_`_!8ByzSY#vhz>g31AJ*-Q&!TiiCDa+% zt7437e^@DCQg#`BV@%X?i!{zZaziu$o;!Mv6N|A;C0>@Pl^vq(wjC1?ne_4No0nK2 zh)29S@Zhn)!gtXqx+y#OY0OY9><~Yxo{91!0F9(zOhVA~;Sy<+c#nP(7rTBRZvF1y z#=E&Xs=WOA$*=S2lrKlnFY@eZ=gRAJt>ekWGi-a?1Yj5dVsyYVo?OklL$JdhK@Lf? z#!79JHzS|)oy6>rx{aEt2 z+X_RLDhv1Kz73uBh#gTJ7xP%q;Lee;dRid4im3Az+I^{dLFXi1-hH=6#$g-ByWOWE%T`wK@j9U!31w6T;8aF5!u&`C_ z$HGSqGMeP<8z(*+?;S8fa?S#PPN1EUt>omgayx1S9RbSlJZMEycYyn&+x+ zZSa6^zwYHre;DJL>V}K`)$*KQ&Z(Q^s)TNezA_-Omr&-|(loM_il!p-k3*$K=Zed^ zqv-V8x_ScQUVNxj>{P0&RcSO}DMTDlr$`>SMdUGyxb5W~lFol6pWh=sLI^Ydv04)9 zBcbCB*(1_9SP4@uaY`d`{?bbEbZgyl740<)yMQYZuQ!RE?S~r zwG*AkqWE?4p)Dwf_*!La7ep9o0yxd-RKGiU4|g{LhGb$|qGC@$Vp3wxw^R+^^l>n8Kfm|sRhSfGtPWJa=KkEtr{m*Owj97es8FHp2vywFD=_Jm zdF#Mxs27qnH$*;bokWKe*^e#){Q{8PB4C4d`lI(>xpxccyqu9ctdM7yezbHdlxg-O zQFg=+dg7x>()yF)EwfB6qpb@f-JiCYZlwGi(P7WbOf%1^C7M2LMWlMKA7@_gaT2T{ z1S}St%>_n-SdqzYbNDuYTl@PWa25OrnLvmp?WF(e-Ad~D!D?u`F_MX3X8vwrVF4sj z0J#F5+PYj+5tNOoQ+TE^_h&i12fxWRtktY-l13@n6@09I>HYd{(8N$;@cdldwH~Ja zesmzK(sd&2iN9KiFvc?xjS ze8z=vWjeLR$$do~v7M`qKpn8zelRc57ExEg>=#$|*F}6u&_N?b-5TU-(_f#P4e@9` zzIwXs^gheX#j+aHN}els1=a+IWNqDVYvfiC(0o^hX&wlXRLlTw`XDM@R55_m6erjT}LWLqRDxJH!vJdSo(` zK={W!Ty^gFyYyzbpxVjvb_wU(=)(1H3sUnct_%{eGN|KUvUB$3(xP>Xt2m90c&2|w zDJC)G=XEUOfEZb{Yg+b9>U}p2uADs0qBS?U$*>!_$SfR~#^(L}3HH3!IA2&(cN60n z{_+4)We(kxutk%2L7y44`d>A2Oy=|5#svDqSJ?Bz&p31aCmAS{`@U=Iji+ZzT`7a9 zQO9Ts%C%#Jq3WOnm`9A{D}B*6z~Um<^^t7)O`l=zc)x3N4w`J0bs~^eDl})Kd_lFu|9-ahM&V&r#X7ertKiD<&eU-nkO&Y5s zpwbu3(=E}iQZ1WdcE(y+DCCg0LLvXSopwsI^$X*&o#+n%0=n}Kf|>O!IN2IzmY&5` zA*azeMu#k0q|b|KV>n_ku|Dlp?L&ifnQpNLohnU=jKsH7Y2ewp)efX>|(hS#-oD zDvAft$7urw9*R{OC7R`Eql7QC*OF)*M)DSMvq!^#H;3X_ALR25e~59sb4zeC)Ak^C z$!bzb)z^A`jixnbbLMz?zeMq}&omsR>I7=#9P$-`+xwPC8+R)zNS&Y% zRx3Z#V6q^-^fTDE22+?6^3xOajHqgjpu}Hqzg1^Sry7AK812^pJx)G#kF`kK~_@RxuHAgy}Sad~_7vj)W4?qRnNwD`mg zb+Top1?7uhbBY0F7@X~=5}}QY0?u_o!YPtR>Gz?Am#9DYVIHa#In8c zSVy+J`9Lq0r=m8^UPll*W*!8xl12C0KuW(}^1b^x>Y-$(%hk<_6}>2LTqsPlcobum zKu#XpNBoCZVE-<__Uilz%^0^9L#}bip5!XMv!E&KpzZ$c=(Te%1Et$7_lX^q&B7r8 zBp)i4uYcBI{^$=LJ91f+Utg6of9&j05%M$PTW5R8dRT*9^C~{OY-o6XD1VsR^#l7( zoOF(~c1?swCt^W!81~X-r`HF&K8lF#C@$C zA~g)JSyOaeO<-H4#SX^r&x*AAOOp|kZ(7kPr7XU&*tZ$nzFEJBdR%>A%Z0=Vuno#3 zYU8B1u4t@dFkytwomN@!ku%-6S7sKqxUA+}@v*W2HIMGEpOsMM_T)vRh++v--GBHq zc7r?IzON!BHe@K$));AlTkZ1?s6=PuRnUPb1g-%UT~3>X7bGC-C+K^tNJF7fIyl4r zGmSdX+fT87hd?kwgIs`#u?`}}H9|s>*aDU)h0y0>M=s}7@;pAw|Q3$QFhPwcL1-O}3jgd75G9BW6($pA4xiPm01%G5e88)oPSi zzC_o1l3(RJ-I!(m>c#Sw`RY%iQB*%v&LL#hp>FvPFgR*|kzb!pYQf*Td2lMODZgbI zhiX~b-6Zk{*R-6pQ=jodn)ovho(_+9Dsb8M+H2OH_PWB=Evo_C1N=)PuXe1Q<4aZ3 znEIhX(JeVic~;bYnwddg`C+~76H@>-!X^ejAvdQnrLy^2*rf;d{t(Z#Xr*!rv0?}9<(J$7TEUCNTPM4OT` zj$GP#zf*{CxIrA`OL%Hl=V8zNt7OXK5yA44DrC|=^6HFIR#n?+>`q0W&rirnaNpg) z>>qH4J#xhl2)=`D?r8t2#<*|H-#PpGscnqvPV(eh?84<(n_=-(uu*-AY*RibG=Vw)H6|xfTfqeOVjT!QZT@Q zF8Y9N6D>YW)lE=~%6pZSe>k1PIWt&Q#z);oLbg;nO^^g!+yrxn!YTeuE1FSDWXsx*W3 z@ujy7^Y~AIE8iN^&qZct*Y6;XxE>I!AMgI;lv(VjFy5|~FFT_;FsJJ@=Nsk`Phl|b z^N5HFTi$Rce8#+PGTUebX0c56rM`aDlc`PczFz%sJGsB|0ndcjZ55J_@ll4mQ3nN!wAGS{EJ3&U5ELPaC+acBe`P5 zuWTnq5rQ7>D*k>AK`hYVz?TSnyOLGIjG{0s(Rno^RTT5j8T{pfbCcrYcrB(CaS9T#77 zd;XOOiv}@hX4pn}7)DcPl_J!E9P50RaKg7aRPo_fycRofq2>=D&wPF|Fk{l+*(Nn4LBvAJ!}t9OEvyzp=zAKF8UETU@R zSX}Ol6S&8*3jq6|PM^K25^A)`2~{fl$RyfOM&=n?PNxL(-9j0S2}}IT7q+mj7kAO4 znWC+d6;17-6YpZ6Na%nf*=8g8+Rrp|(8dk9t2*qOu~<*rFDcB8ovqhnGJf=zR5TD& z0nCvnJH{(2-|}B-n;l~EY5HVZAck_rJ3#2~R~A*9>mBNvjD9YT)pqFZ zZa9AL$i}zfAR?n^6$nebV)@2pRDkd3pycQ?Z)&k>>QeQ4O))v>-?8Wb>k=|CNs820 z!^k+~wsFTf?5GpBsFNG0wl+6+4}!`XEEa`tXM>1??+d1(@mv?HKdtcnBHu%tk{bcvhVq$(=j`9 zjT5J#-M8M~URJ?ew*X;5w|;gJJN>F~G+8!vU>jUBj{x2~pAQlz zr!3Ig6NX$%5qEt14z>dV;!lYz0hD!N_2Avqqqo@H)w5BQ-PM@{OG<@)c-__%D#kizm?tZ1@ z_Gpg32qoE`N78<}ptR|Cv&Qu%Q7P&Ipa_EM{{aXUGkX?4x`x8&k5~m=iBhr~&U%d3 z%!pWA9=&t!dJdLtITsff$#N$lXLnW&+8~+Bo`W_*+E*A+(^|X^QZw5U0DS6`N#k8S z4g@;(X_?UKVi$_BjHWv3;EEm2Wctz=xo@(A0Sc? z=Y~DMe##BF@7`bZ3O_1llF(?~%a|l;*;-(ke|FvRWqzgfBau6Kp=Ah|=Xc0_wS#mp z%+D`|ZKnF?-~@;kdst9hLsnL2kU_m5HFCJ&X7IZV}tvSbwi`r!FsN-k3*Jo;s*hU7C3|JM26xaBa) zxeBya)`8DN*8RaMB0qqLy963hiZA8L%dOq7(?p$*#<@)PsSBn3rSj9kas}k2sT{6I zDsZAK%uzP%?)J{xYYLsO)=_+6_@T#a#QcM5c8!_f*@VDy)5MQzko>8YA6||_-Pr^y zZS~zJ0@;$o1%}*e&Sik~R)l$oG`xw^uq2p~YU(mH5qf{d>z zw)u_J=1q}eK&|9&xg1Zf%CLpQA}y3_@9*+T<)!BF+bX{2JuaW;o0_O&E`cP^ue5}m zHvil-ih+c$@gXA(uCmiLY?f&bwBBI&8vjA!-c)jjE*_ zouak!|JHpDZD*sWhq?q+H@LI>7e=cu-PVsovF&GW%#&}Oyit{+7Rhd{@Kx%yBdrWB zSnDVM?b=J<$VuudM>VT^!>YrYH^MGP(@MNliS|!Es{^*z(p4G-YT&%`LpeJ&Y>d$m z!!FWz6~m{%3DgN6+7lyD55j4@KEhln-vGjkznBA^5_y^h$_Bkd^G-d4+3})nxqpcI zTtATkJ6R!qxO<__n}=6y|;F!#>BU9F6~BdBf|A^U^r>#$QCyob0FPP37O~{<-J-`^-OKtibw}8Uf=R z|K>72j>F^oTOA0xaZGIx-5w>G0Fj)@_*kQG-7Bv+ysvQsa887zsw86HnK>eoP@&Q1 z6&%IRRieZzDBx|9z?37DB0MC@41H=*hKH#A=*tR@E6_sdefh8hypsMwk{u&!<}s^o z-(~fG&}wzbqXS-8_x`fDHsVQfd-xC5K~+^yZAah+(cm)4#hy0!dn1jQeVHC{>N#1% zs=WQ9v=oW%1T`19n}ou&(KWD3JLYnfQ*YAej6!Xq_+?VyUI*{f;J|pX4SXgKj_Zr~ zjidsEB_fa|VMi$U(r{Q-e_7Cm3>`6k;}Rtko`;cP<+IR_B%uq|&H;Uq+9A;FNB>?o zVXtb`7SEtj2`j>_L?S}&rcxC=R25HGqw0_ZSSQlrsK_&bYD)fQfE0hXsHkl*Ql=Zkjg(u19o9k5lWu1o#|FocVp7Ref9!XwRLO4ejtO#0J_-Gi^eIm` z?JJ2izk&HWB~PKYRGQ7qp*pe$!|mJ;%g0jLaF!#swMqf~LiAy%KbAjIGu4hH@UcYn zui_%)&OBw+bXMRN{NLulWI}0nWGuLH{f#g!Qy|suY_tyOfV}!` zNBwOMvY=r;Llbb(RJ^>-GnA6z<-_)r1OsHpFjo!zZb;O{khp9K- zu`;NuPrDKIGV6O+B)5>bo=U{z!>->&sq@eRn!sv(f%NE*8W+&19jmhR1h!DYmcjO!*9Jvn)?Y_~F;1d5zWN@}OAy-otj z0Q%P2Y`fmP!Jlk>N3Q3g0>1MS>Y#`ANUQPfFaW=e+?FY}$0qSyIAXDG!t)4&jzO`C zCRZtx58uL_R7z`Q0kXxnf}=mF7M~FNmjnsmZt)5_W&N-*w1zLk1hPO+%unzXK0Lm# z1i6PS3e74K7K&!o-f zQMcnVI_!^@Vhp;C>-LIO_V~)?B%KhPMN6{>Sgve@(Z-N$-@{>08#6oO-hPzlBcgf{ zJe{G4Q4LNYmypc@dHP3h_b=G@nBueI%i}fjMNcfr`-qXR_x@1;I#Wnkn&BOpm zbfV1fQX!+6Q)7=dc`1w@<{oyK{=mRI_&{ctxMlq@Q+zhU2gFtr$rDb9d**?oD53d; z6B+szI~8ob3%|Ruq+*}tG3zN3ZqpKXHgV@kT(Q#}Ho6KPg~YA*jc*%$VQ#EL=_4O(^5O_nKxz8ZR$3Dh^oMSAv~jU=cp_u9A(S z>`=F2yv+MmBN{Lf1`b0QX+6ATG;VoDQBU2iOW|VlN<3s`EI-qgd#UJpta!UWUS!j* z!cH+$uF{xROSZ3(s+biWc;65?F_AIhu4>X&HmQNeImJ#Sq!NbB4ZHP+%Y37{KOzpNd3 zSpd+T*D|+&d0%DBEgw!Rc1nic8v4v#)umnrfqRP%G;tlpyD{vDT#y(~>Nb>uiI<7@UsD3k}wY26G^E};& z0R(V(+tJ>LpZq?GWxg^SGDpA`V2*yCQh&cFWD1T5dbduz9sN=Mqlq?Am{f^iwvl|x zkIy4?uyCSiuyrswtQiTPB5AEURkKpr)B&;^2BG8)8V)a`xc0z`fSswcZ|7?(Y%L`rAaJmZ^M3lG3SV-IQ#%}@f>!Y;GkQMyH(B*Fpl zC|YymBo!<37u9asu4DRpg?g_(JlV~LWKDqN9WcEe+I9Z5Y?mY8BS|<&NJMn8(H%sR z-3e%ibs41^v?)_ZSqPQ>x@+&;e0>W0*!7hE+u?N2(~)j+HHg3I?$TCG-FH4u=o)vK zMRkzkT59Hs*nkk;|BiIB<_!D(&^`W%D{6(0-q~YP{ELYqA7fQc?7o+!u`I)x^Dnx4 z3j@t(6a%dLn-_Gzvgfjb4+enaw#$^-NLcZu012t|;4ufezmL*(O(*zr`H8Z#MRn`T z9iXQhO`NAZSxbW4;^GC{zT>LN?wo%fYd-ZM!6DN)y2h%XXPOD|b1-IkTV__OX{TEE zeFJtZ@WNP&q4s=$gXB`@y0q1Fo;o+i>Bw)i zf#RczD0hHxT2o2G1ko&M-^syRJKN=Au4`_4AcqDeotp&+5o;I|%NQB^5uFGvbbhW! zFz5sfoH~Wvy{`FPFmbBr`@R*!q0>sM0>*mxIZ-Cd01tU`!mra0{d|X>DjZbQE^5Rn z=lObQk126q9k#t7U&6S*`gO2`j2aliP4w_-O1hI#!+~UEH^R6Zg9K{v6aX zOORjUCkr57g#<3FZYbDcWW9jDE-p|_HlGG`&aXQ^P^;%nky~ORh@vFQZ=k|ZzQX_` za+}>30YVhho$Sw1Ojp#EOav${WRrYoGUT<0Fk&Bg5O(81w92?!ZHzhl8$N=Q%YtwB zLBP-hHEw1UU{sJ6k;>Ga^&|KmmyORf9-s&i3*0?Co%!W>7|RPpy{`fXfh}S2 zci(bg!bbe%M~iibM70Vq=>E4)wTX{aKRxoy2#_y2`TF|S zLYmgk>T7Gev-vzqx$s?|#9Fgm4nw|v;zB$n+FR6yl%={InkAlmCx2?g!sj4bM(}iI zErt*!%nQW0Z+7Q(Z#E8lul`WxSg_q`uKluk8pNTdup7JX_HgP%GT&f@35CpsaY|IW zvc|EX&!$;%Sjpi`NXWjhR7sBZtBsMm3ND8&Px-nz;K%tcLsTzA;!TD~z4gy}t(}b@ z6LZ=*%#qx73Qm&m$JsLv+MG@Rf{8_++xk;8Y^`?5*>k6tQkg~8h!eirN3DG+lIgY`$Pe+s6RN;z4J z=c_onBwRHKgou*gijv>1=z8E?g0Z6Uq1G1na8|!E=)#-~i#I5oL=o!s8njhO9S_hS zKPM%D;z8(JnE5pQHyBjz;vSezYhi2bN~rr0ThOM#`KJ4H$#H$OB55JUxQZrDjDVhPrm&hW*I%HoaTdKtwa zt2zHcIb3;TMIV;f2~de2%zUaPIPLJbjf`NbFqkIZQv|zbON(w#Y0@}mIo-YpmWPe6 zNhZVY%7x#FTygn*fG6hpYOU$v5iS5(%BjUC$UfvVIA!f-w!&43^le`yK}2HIs#F=j zFf(lTdv;jTG1-p}>R&Gx*=*azR%NK$y#2Y>jNb#ZdrA>qBxZFo(PxId;AnHQ_4T;N zht!Q3KRz^$`Mxdl`4#Pil-;EGhv2lI(CX1|pEUpfEr1qbCIv{UAe&tM@pY9(MUpHf zVEDFJZ;_RiMOw5ok3Ol^?Embk{4q`y~{LO1z-F zaSbuY=Da!$br!ey@sICaDWp2R61QFj%9GIxUgIn#mjFe=xLrsuQKGe$7#b8$rbs?c zd6K)mvQV=`wVEk+4l94|XGarvfYzJL_J4B#p%2wh>w{w9v%G#E4EYIiS16@UB?^W4 zmlGDLaIP82-~0YfPmX6482NdAN08m=e42F6;sGUJRG16$RLO_?<}X5$eYY$u%WE~_ zLhRn=mvO2@vlT^9rFvyoAgAl2d5JkkH?9Jh1~#4f2$L-?sK1tSMIhQ z;HLOE9}5@wP9`wlblwzGW(5GasI%Mw$f+pn8u9q2v}g_u8njWcJbK?48NuMjC)yXM zKFHNqsS>!8c!@M@85X#0+Y~UXh(o=6hI)Eq%Sze7L&5p1X;av3ICxd<**uyQcD zikiBJEbJEMyd1CtBhHoJ7sQEP;TL5x!NqpULWM@NN+2C3Tv!_`^In2`*z_9O1_A^L zSzti_Mp+?fV-s=!C+wF(R`8Ym@5K7xTbi(p<-E^}#)p#EQYKq8ncWBgWg{k)m5`eM z3|OP1tdNk5BP}J-B));*M2cyIGvM=a+ds47O7R194uJUKYM2lXj!fxA*{;-AA&2gB zlEnqG%8G;(dF-kAd4o>bmFUYsJ>=1;~J8eZ;taHF4)riObJjRDPlaU*XDgJ4t?IdhE^uKJuE z7!3#^ocLD(mkIBB*P5qvfov_)g2gx59}Gpg^ZO^+o$pkrKoJUi* zxklMtnRTsbhx!41o%Oi-kzSvAs?+S_sPEs)J>N>__0%ZBP8>3(a3*@}tj<+?oXIO1 z?0O&JirFn-+=lq^1G~cZ>1rELfj%9?fL;ga+TurdJ^j69kB)hY@qg+=N|RlI#4Ba! zjB4f5*|L9ejW%(fWG{*&cmUlt&{~Gtf!0zRdr)2-#(X0GvrU}MiukN7AkA2H5Bq%w zxN`!Tj#Cise_TCBmndnI8oP73;+r7>k{3+i1M8a>jjAS} zu+?#3`IL=^mB0!4$M=~mwjCi54=1C@hX}prH@APx{II>&H$VPh&F_#e>FJjg8!YFa zHm1kmbl+Xpvq!~JIPd++?rJzk{W}(eWf6-w+LUOy*XM`$?8NzTKp5^Pw(GvY;djM# zEpHg<5W|dO{%DiesI#l}z^@TWGwR7pW5cbQ0TQsrv^+5;uT-%2lK@8+l$TFqTMAw} zp5B*uT-?6`ZFa%D57>TsMjZqR%P0>qa7XUn06l?!=QH~9Gb1cnU6_Z&a1Z)DOEu1> zCrg2hxCQQ?L#eDgV@2<&>@<+t5Y7>1q?rmPAQqcJcI-%tCxQ@%W_bjSZTh7AA>NJi z4sv!wV*5f03q+g-pA2x@i#lLB&%t@E!Eql@xeoA(Xp?=h{R~!TLBtSBl3zugA3vNY z3CZ#hQ#6k{&2fi;{0!H!RH>>ez=NT3mv`6z%(|!)MLjV8oQC}?IDuH6ql)`F`Ib7# z2JH{*l90GEqAi$3QTrfeagQ{fh+H84@G(%_uCBqQZ(xoSQZ{%Y6E?j-83}MD^Y{>X z&LpyKfQJ@sM;Wqph_(rkKlf483^1b5Z~VPth(3)M{Ogfy@FaY@vFA5~G1i>c@6u=4(PG6hLqeA|6dpko zeePju|8!@&#HuPJ*Ir{|sroZkBmQi^xBA8`S4uIiSZj_lE-rv@Q3%(pA53D(mO1Dy z?tnW@91N_9IUX=o@eW7YE7_N%9&qm5`_A0gSROfU; z=D_UO=|Z>$#dK?n2W^(=eth+HztYXj={0GAPl(o|(0hye7s}tdvO-K0?_w-c)*7SofR{>|9D*~4=b+wx($LPrO`b9z~Ot$!*fR69oSCG+~(g0I*h4+Pez~9m?L*z z;|Y%9I$)yO#fsl+i#pi{?L>dk3(Y0%7=$^m1bEXq1@xRR(w;&vvR>oZW6WzV;1bl# z4_0yKCpGAazx%fC`yH1E{oa719M4M|H7@9viNqQEqO4fkfjOvvtK>%&ge`N1HhoPP z@>NA7FKv>rR|Jp|qs3%yG}kBRgmEhFCl%g>yE=c&i_L$=HT^paxn(u(*6jF4xqkRu zSl>c+lHPlFX0GGm?&~yg<{Rv65exrDaNY&=R6huMX9t>yl{O4~PT1{3=@|w6O)@Z{v3lZ=S?&n4#AR z!5azuWl4yCl7z(3-iEL41Ne{wNT9D1gMmoCt-#D3j$9NRJf2*PsK&nMv-w4A2j0%1 zm0G~Fi-%6CR!xLMZsD6^Nv`t}GS3lomfMF3Z1Ql07TdXrO5Ywe>LqCwzxA! z%{fgY9>DQy)yPws65~qv&;UR(r^z>`2`UFbCVk*2CWT8SDs>}t8Ds4=LAcR+$yfd( zGSoZ2FdXLq5drzfMp1F(QLr+73T<*Eu%IAT1mbA9h9+nT70;sZ90As-V%kB$5fxuE zD8A6B0Cp4#QKTvdF)BG81(kxU;Tl5u0^xWH>c*F8E$9Hv7BE*dOOBI`h;aCW-=oE} zRfuukPUN_jD({D(&ZG?FlVe3C$(q6U{VV_K!Os7UiX*~eRlAFFB%fM-GF;Zpf_4KWN8Wg!vUCme!f^lvUM}hG z8w%~4x%u=1j;7WX|9chp_{-e5%K)hnA@be(!0Q@vi##tzv5{~ZUNhbog3#VK>g|hw z9=9$^nPMiw-Y9&PV)5yB%dcmMVQ(mpFK>K_!*7b1ZgH%Bv`M}E4hhjfxtnl58ywuc z)~hl^7b4)7(2mj$AehHVowU!Gsa{4l>Wivkm`C+aKA(#=lOvz z=jGg@byd(mF0NR%aYH)E*hhll2Z6@lfI$kXS(j zg3Z?&ib!)-b_6_@m^x`Lm;!Wm1D+K^x|$7u&5uX}EIf-7=EjrYHy}=tECue94i&(^ z*?POq1ix*~n{{@o8v5u_I|yg=i7U%vibMNLN0ntJ4TLO{ zup`m+r0L7(KC!Pdsa{+DXQ zITo*PD}QRuAD06AT@NT81I=!WIju{r-z(5j1ji&D9Jib1xu+<_qk=B#@wd&H8JYKkTG2gCH5DeI_NK&7{%h~8T0?ysxKav+Bk z+ln(&4UFX~;FD>Rqw#+OgzE#<)>X#uk|x+|WRq9>)LFUNKj31uWhiqY{?F=6$>Pe< z`!LbejO&8LS9bw9e-1-n-^0jtPt_9yXEDUZS7{OgX`59ONkRfSmnW*Jo1lR|Fd>+26!>*jh_lzfdlBN6#)mrgQ30QE3|;SldsgV zcF|6K`WYb}(gql7WF0a8Q#z15bHmnVoFfO4jC&xJzVk7SQmURL{**?P%ceAWJpBV| z1v zU#N8a&p3(M4!xfuEHeA|8imB=RzzN{OSd!#h;)M@DcvF6jdZ7U*U&9BNDD)!bhnh0ba(eX{_a|L z-G67Xn0IF0IA=e5f3{yT__e(mFK@=ZV-9^&_*oW6&wYkkY&L`mLmEaCe~~D&+SuAi zw>6obO&`Kt+u`&6Vfoo=tMpS9m&O6obKpXn!b_@Br*m|~@UW%l4{(U5z>n9=BTZ_% z`6HPVS9}ct68>hLmzh0#lHOC0SM?SM)$M>mSgK76yk?+J0R(YHq=9I44O;F+<%(?7 zU$(UrpJsR=_?S6m)wX`Oj+yGn}Be-jGEX>o1L*PAg?s#tbgDDw#A}LU_RJ9+~=Q zR_cIzRMh7KP7z-hlhcJ);=WFm!?SLTq;C>|w#YyL6U2FI$clWji0tuoxWluXfCmlS zcm5e1k4Qo1M9q(6JT$cc*)XG@U&&X95@x?*@TeS-Yk`CLrSZR#i#3CH3WeXzzJ}8( zl~CZKt10OR3BA-q39efVO}Rf4zfbX4Vq0FjQP^a37J4`UH5c zsO%mpKB@Tw0h1~e(;BG?^t$b*CRGNJFpqkzuo?e)s-3T_&+#G)gV@-X_SyEnUBxk zbdHe32Uu#Hu<*nu%tzO_nD)7EAF$GAGJCE@;@F!*--o!3*D$q+Vw$Xi$T-?Dg&xZB z&z;vEy5t^9RrRYr=lb16WsQtk;_BtM#ryA_rtiGJv)_E%)amYHx@TXmmp|T{OM=;p z58ZBEe$nYKpSP)QZ&zSW0rA{(FjfS>JD`*;w);KzrM8u;M_SbmmTT9u%0*61sf>;( zC>DPHmy5PWihp07T)?1|S&p)}dk<_MCnua{f(U9F^qY8&h(FHN;#Jc4eDZ^e37i(s zZl`c^%-@F}jv1X=XJA?zY0e)<6&+kQd?CF6Wi1 zPAc-s|0eV?`)uWzbM>J+AK3rkx&JpcL2En;B!5MzzJ8JQY&Sgoa5p@^pcAoI7nW+d zVJ^Si8vGg6X;sM(%hZtW^oU8pxXv!#BWt;LxA{v52H1CJr1$E|87oZDGY-DH%y}+H z686RKy8bjZH|+wR@xD@?z@Dg%8$2>U{5MOJs5r3zHTJWPfR7*dgYtq8meWht7#yA1 z4yTYEqhYi6!1dO<4DHa8E7Gg|qFws4R@d06{L?75yPjnIF)Nu|KM`4IMu68V#Cz?3 zp6+Nf;GskG@hx?STPk)}&Q z6$+0VT3tHbezaf)2_84Mbs3k;h7(E~`Q$Eu&trt_!PI0L8uP1#N;UG6jlX`de!lVy z_w4zk0*@n5u#kh<R<%K^bL%x9da8BZ-RBxOQjW6Eg+3Vvs~9<* z58_OBY#?*8)8DioGAK2yS{{bA>6tpgdbd2|t&}L`M4Cin;{Ny)W#rd*bo=c1KW+nK9I8g{ zK!l4%F_-ytdg&xc+Or#NU0e#r$`>iLx06m?ix192hs%Br4l^$NH^^aaKHr(X&0!|L z8l9F@`_Zd10`-h3St;W~;potphh1O293BB5{VQ~O`iS6)O2c%XsxoRcV-oQJea9FC z5kd zj?7Vm1nm&K@4MFv-eP)2>l7nfg%?+{IU4ZF5z1EwzZ><^M5_-S6K(S$cKS0Lo`e#i z)PwxK$ynHqib!7NZk>s20W^@-5S;CB{#8%_jX}imObLpH8$=g$0jsL)=}-_8+HM8= zxqgRt8w=(yo-cSe<~@S;X;^Ga?XF$^W$S0XzjSxzi=h7)CBwltzGcoI)#F~w9ywde z*xU~IjA~~3h2!(_=z-Ekbt8!of+*6AYEnkiXj_E}9^54t4-XzaYDZm3b)_X|(66{N zJo7VBws-l^64D+1S55NbO4Z~jDL6pk8^eBK5)(;r5|+=vH&Uo1&9Etqg&D%&P-2+0 zf3VmvQ0eQG(SBnVNFu0!xl zrdKMm^ja@K`QVxK6`eYo2uqyZQI+pL8Q|>oq+Dkllp&*;MnL3?aj9nZ8GAAVK4BMq zQ-e16^dD(l-(2c1{v8q!Cq^JALL1*m^yDkloF337W=9B@uTV91dPa0>YZ%ecw>kf7 z=3nj^Rm`^&AyZDzK!A%&8IBF0%E49%Ss6Ty&;PuO(}-?YYR+sXg1n*~X0VEhGGf9e zk<5TR&ELfPiw!P;mFzILSx2(c5^nxedtW|Y z-SWTa{?=E?DQA4hdpf!8Rd04&vt=XTEvq>o@MfSLv?+6axuZa{-I@qgh@zqwvGU$e z55Ko#6^zL0ofiv4%G)}wuDE;7h;4N?%2?e0NV_`%SU=~+y>X6^;MwitYf1$H;_x5J zr4=f6Hs8~@=zL?&+d3TtR_8ssyS87fd_28jyh2EEr2Vod z4%x120h)1cv^cyx;bmFUC!+;&!!q|QI~hZkTZd*dIVtfnmbLRb89j}xLT=3bEVB>Q zsn1P0G{_Dc33O!XVdK*W=*1M$rvX8{FU0Hn@k()tUAWW5XG%a3mI!T*;y_?A@*2FL zkhUyO1YS_~XWV_lB*Y}E_{*kV2|2}g(r0D?C1!S>e0l-vMeyMgMv0g`h>ea(X7Bs7 z)Cw(ncci~n&;k3TfA=}g!+%tkap|;TjS?Q^omyR>p_G9T3rd_D5yUEe`hY7WiA;?g z;jdYy_~Uo+^w2@sc0-lrPtCx9G^3qerka3EzE*@1+jtgL1`WpFj0lVfipnG_B>CYs zr>h`?@D8KPCK`E!UXe-dhk7XpOA(tU)iVB;iQ|CPlXTMepsEBYLL~8`;H;`7+LZJ4 z-u+x+{eh?KOe;34f=gSPvi%l5mqz4sx~W)|M6t42tK|f}3}Yj`(=x%QFs9c1KKOVV zJj4h!k;D?hLiSBaho_ohIU>S4k~jI1YoQPd$#5Km+)dU(%uXKOCA6i8-~MkX9S zkCLp_xdQUL3N+N(?3t2alC3bNf+$Zm@DE zNiu0t{XW)6)l$>FX~tWlG_eiBd6XqXX+I{Sc~-)=f_#zg&dKcPXB(($#FvbqQBv(4 z`xV+~v;DHnPOOSV-9`(+d6(3%gGNudBhvIIq}$2EqeEM-S#T+wHg)_>S0MN3xT4$W zoTNNIL98^+_HAorcwC4{IlqH`qM{uLIOmZ$v670sbT4EkD9G7odGf2D-((#k2DK=`KD#IS?r^BR0J*Y2>c`pqVZ{$5H1Wye;4yiBl_RBoKg zClO2&41E^KH2Ieyfmoy8BR`P+Z6Z?^VJ?c#>1jakvYF6bkf~YDA!A0y+_T z5gNY_-6N&{zM%yK#0Zt921d*#5S6yV&^vKw(P39^_p*1?qzlw&)DU`_@RzDw96^B; z+GV^*%A`C7oasPlOO2ts(UXLjopQey71qGZOC{JIx-Cc8hbpZvA;%@;Ku6&v^bvW=mY{OB zhfLw*5lI{?5d~Nf5x)skYQz}`faFT5LwjF?5Cz5u#@oc&bdlx3-d-`YOH{K+hGt*| zA~kYrx@mpUc9_S=5DX0a{)&?}5+@1Ax zlj?nlb!zCg)cZ2_YEpe|(^|uO1S?^-zQku-7z;hd=kMk8bGE6NrQ08FMXO7mU?|b1 zTQ1s#S0F;ATlxRae9hnOCF~8~GfSBwQ3vl8nNsd4hkSADN@f?g+7E8Tb)fYvEM6~J z4fwOz$zt^#?%Kk37j~V;IS<7bGXZ*{OX!C0#-7fdUV^Pjk%SNK;~2>Sg*r_c@5* z+1jLSlrgo4r4pyAzZj~Pgd`TP|83+>#;=0qI++WN9R|YHMhIsj+35duXN%d-yZM-b z8c9Vv%jQ#gOXRdfE<|d?t!u&twR?Qqeew9at;)kk#Grw6N@I4ul?Hljv^?u5Ozz_{ z26vq5>&pazfJtOCaPpPw72Of_U2Ni%4ux){{5gt@Q^pj>YBNvhRymUpV-wQ<#N!1E zSH-@goH|Y1wU;d7Su5DIU%ov3XO?6|^Jjz?q10BO3}!__I$)dfjg|N6Kj#RA=>-u; zqMz)k3nbK^P0r0sdv(PANk=foiAdZY! zGx}E0&f}sFnI0t4zB6Ep542q;+E5yJ8F3Jy5n}XSyu!#bl=LIT>SN$j=@_q0Gb~9b z>T)3bD@zv@A{w&Ju&o;!I;Q(4Fiu&xP`RHJw#Rg21A)S4k2$lO3KCC{$a=UQ0kV+6 z!8fSzXn*GX-tcWUlr8soO+G~S4!Ajy%d`9PabgEJ^K|6;j;y7xcU*h)|vz?8xq_bI(&AF#=bhzQl$F0Q}r>JpONCJrR2@oeDG#w1~<-y zdM`|KSz2Ur+*4}ojFPmRKDvykSpoK!PW)CTh)=!T8M{^790p-+-QK>kzUq9}(dL78 zi=0$(voBG*3l^Mni;H4YoPf2G1p>uN4PMtr07mB0tc%w5i&L&l2cz$l5W`dT{Ds?V zpzi2ybbvUi?FQeUF^8X)Kh@QglHi5H05WPFL)~D9t~(fl+1ZGKe5gbV3=wc4 z%m8oHikh^E>`gxAdP2wDn%kGZO}0Yrg+|8~LPx&|pUx<$9LG)@VI;WFL$ol5lXCQH z`!sC~c8)n4YNS_az)-+0J~%9io=-S?YF`8pdxcBa9`3~Ui!qXuiAH+gOXKL`3gY^4 zg^+;PGNDS&Z^PdE=yXXXcKtk#K0fh`u?fJcg@+f8IkK9fiZJpmKb4X@EJ_i{B7{^J z5W$I?$U8Go#u$;#nzK8k{b9}zACr3fch}?BZz)#ier=A>V;e+m(!QIDoLbYh@jcIn z+wh^B>>I$|@{}u5Rs4sWyMM}~?}RN4hkrQTiJ@u}!f^K{#_ENe!qnUep0{bHkwXr| zoOS*A7NcT)Vh{1ber;keC=Qy4I)BYz>WZ;O({T<5HGbu_$6D68RucmHoeG<2i7cgn4^ z@wfGV!|~?2x?a#!x9OO6>Mq;7L?@?mwAh#cs#xwSpYMdo$jL#3P*Y!j`|0r>P#ZW1 zqj%l3IU^{qU%x*(*ap5#i$pam)2!Ak_Ujm8&{}|$AGG~GW5PmY;Hz6T0kq_d^DP~=NB@aVo%EYE zT$>l(vVUVO@panT%1<)9_-qde%ci;ZXp>sYv9FKI9rj({h!bSGkRG&s=15vh4es|F z*tJei1h$?b4xbgX=B;#v`29AI+7`l>xWP11sZkPQtz=wd#>R%w8x(;}~YWIxv4dDHtAAL>aU2}7gVT1fj9kz<=d_$&(aU0HGf> znvOXHP?7HZsi5sQ19SK^%6Y}uQNX(dXO~Q zWk}9`FP#D@RDftQ84Bet#oj!5{L0A|s|kh6E%_26rs1bEuM35ZBPb33mfArxiV+yE zcj~r`r;%3_zc6yONf$|!T;rvc#%`f7V~ye4r>hD1ivTAVoJncUk;M^OmI$FqV)=8n z!L&wB9!%t{GEgpZ)62}h$*VC ziln3=;CKh;!aW@E&z4}RqX5fzhWJCvb(97|Lwm26$5hh|9A{&FY%4iGWWCXca5qLg zTFqEeiXhIe*>7Ek&*l5C$5ci`_*7vMg<3tVzB=Q(>?H0E&ugcfHA6YuI(2?&^>WuL z4UC6sdlkyoqkyJRSkIqzfH#+t)%NOF@9?fbJ#x3ry=v<0_NDxajjYyaGyk&w{nyO4 zw>zGZ5~4rr)N>Wx{fdsMgt%73%fP>2(|4Jek3z;U4`D0yGKAiTL;^Bl5tzDdc=CoZYW#Y#f-?_mhLo7cA{gSmr3Rinz|A z&+H-cQFC~@=oU3L_Kw`w^9uf9;IwzgD?k zzepN+@7-7E^QX_rW~MZNM4OQWJd=?&Bq0F?waI^?@JSQ8G2sdEgcxVwNz9+Ro zu{}&Jn+2_yG4boKA#JXw%LvO{E85T;Ki`C@6x819 z$cl{r|5`9JyBI+iqJ$R_$)pJq4%$Wle#%5-_N1Vy;kDl(km3x-voIHxx{ zUIBiULttVD%~k=)c}pp)GfGhPr_3KTQUQ$BCm0Uw)-rKx z?vQ2PO^>&mHASrGs21u(U989faWpYX@ZH5U5fn)O!%|5AYnNrTlf?}F<{#p=V=PY< zPSd*o^myOBg*5058bBOpl4|Z{_BCQWz{k>sml^TR4Zxong_A{DQ1mt}?zAu=+FwP?k z_<|ku*Bt`W8}3R7n`*=G4Ae5t68RXKM=zPiH@m$M`O%{zD6GfY_4ZEpNe~dyQcf#& zSAt8&^=h}kpcQTeAXg>U*^7?7p~u_17hEIL0EYN$>?hpvGDwB%MNg$%4>u13P=hx{ zdQQ`FX6fW+L~Y6~rm0UDJM+3RVEAY#6Ujjx;Qguv)nZuZ>Zjoy7#NwaM#UJZ) zWWZPqgcXKY(-4VKRluRk`<92oG*qB$XJE6~&8Uj>_T^~If@~4SG^Eq4>wD^~A}2yF zy+uTTX*%Jw?#1PYL24x|M#=1UXVng0blgcjX^Nq_ z4D}({__u)|D9|_Yz1ZHn$T}~c*)IWWpbX80U`%?Dnzz79qQ{!E9R{@1Y(!q$C<4h| z*R2+xWjwS4(jRIlh5U^oSm6=dg>&*@>k{t50sB&SQ>oioVdn357CZ{n^8t^J316z9 z(*Spz#1nA+$$$Np+r{|jk{@eEdC3#Yx`!K~=G@hJu7YTs)4^+C^_kaq@VC6hC2%n# zlGYd-b`1K?0S_u zl|Zp#BfRvDGg^phhl7X>&+y)6p=>5_G|vo~?O~9jj#!P`0IVLq!(-y|cs`Fa(E?Uo z2J0zJmf`Re>|5V9BtE5&v0SNib^<+t^`8lIA=*9@94FyCvUyh6XLBq-*Kk$sWvoyt zuE^cv1MF6V9gfTpUcIP&nO;{Q=sQiy0N*neqiTt@Y6+flF>7{66x|Esx1FchIxK)& zP=m-YCNQ3*^?mloG*7X*{u>^GubVyXDN{_odLMnUQV#vz2+@u$*yA;U5eeRoZ;9t| zKD94YekF%^c@>+yYw*kOu{LfgTpw@nL@1p!?9m!3llD^Mj|A64=}dD}Df4#8WeV68 zygy?b@+I-Ldc@Ca7{XYJY@i=$IWR3c9vvM8Fu2pxQyKXcg8U!g@hJsd0=zNtmUX;h znb7&D7eYO+u^%Wv@wh}6LskODs8pzdmGK@!aLcDDc%{s>+kwcDYaCJmgSj_rnOT_V z+Jwal%aAX0^wL+1T9{Dxw(fHkQWi5oWI|VdvBT}4u8$E>3>#dG2zV+Qk`Nm_(^-X+ z2iygPG>os%ENr#@MX!zpUBR(HVPzhv60s;hhebcM!_q&AiaVwj4F&8pWaVqFlM?M19Ai_oKR2)Z|=0r&H`g` ze>_VfJ)TUeS&Z1mZVZC?BjMYAvkHD+X}7pw3qpAqLeRTfluOSy?=;K~N-~{|LvW-L zP1fu73o|eO#b2E17>glAaV*Nr_!H5Bt_I_8q!8EPS^g8kjXFqFXMs+q@B&^g9s^z^ zE()!4X?H)6qzgdNk}r~P+$6cUptX3W3$3tC9PCVWkQRido77D9b}xFd7Xn!a%naQm zkYnYIvuhD-6Y~|z6iYSelLjdqqezpM6#)g~c>XkDM^Q6RUaE`SwiFvcTLutaut6Z%#VY_SFXyhZ$W)?Ur;TN~2A zE^O=MvcRBn`=G3Ez-`6hY1?kMmY-K%7Q$G-?v?d^2*+4}4d*RX=xir5bMo%!b&cGyf!cRdj zn^VetU@GHDX1TKdh;}|-Dbxph{!`ZqZP)cR=|%9+UxNLj-PNDJ-{VXGrq+YBb__aG z{7^h2N2p0F)S)3|dpVlQ?e+QMBHR4e63!t=01gea)J3;a27y-}O;-4xDRaIv$S~=`IdO8$QN$Lo`FLo9OU$qwh zj5wIDtXYh$84P{bRI75<+W_l#UAf|9gavE5du=`-a`7(K&8=|GX@cDjtf^2+5;wTO zGN7bD6(s<$sur60jt({(TFo7dhfMM-AUvNWr6AowhLWVgJ6h7MW!24wj>dg!7AtF` z*#psEvksxsv@BTcv}ne8U`_JQwAs?JMt|ce8txk7ob*s@aglxAssxuQT$ZYnf&vZB z1RH#0bGhxA?19!m(rY^tRECx15t!0!6`JHEp%4-XzKC~ozC(GnfLMR+xVSvzWX;nT z{WT$rE80^t4|5IFRA5qxNsr=|O>+WY4e23411x8Krea_@WK>H{k<~a(c;<@N_g%)? zwi!peT*gXL>G%eUvyfWr^~3;aEP)oPkXLnGdrLXQ^ice9=&Jqo`TgJfYQQuo{jFt| zbf_CV3b-7Nj7#FJYSt`gxtEndcpuu-aq);`O1aG)^1~T&(OO>aUMTP&nB?p^7wlbU zOBe`ZMbl?o7k3ZUtL*FTMZ7**qRwHl_w*dSokuRY_>rQD;(?w!jGrs1(#cDA0(Ve6 zltUhS+jsot4*bAM{TYLi2=p5^-xQRk2z!IF`wUoD@87CCUCfzpd|FlpV@6f3shpfL zPB~zBT&cCJrF&S7?|R)w8S}QvnKkMFug`->8RAhNi0QUx@6EnO{v^0IU`*nK>k!w8 zF<;(>Acz5h#r-qOMwhHcA+1$@MF8a#aCZg1guP0{yM zH!vy@(>g**;u4t25Qd!=>|n2J!U;{jZm7*a`3oI5)Qx;RB;|2MT?8hrdP^nd9Wgf9 z1EevhiNX<2+P@v>Np?pYjo`Pm7wB7+&kj5!9nq>IJnEKrXdUjbvSJp9j;plY_jVE( z%L;s8^6@WsiF6beRm1I+G-#%b`5>$TJ*a#Osw5iSEKD&L%SrD^@U^Y!rXh&m4xzlZ z-(+>p(z~8K2MOw#z)qicZnUaC-q?*}%xvm=>X3+`q--a>*|^YWxmSye$~SB3*$c1u zuDIX3&YmCGekq$qlNF_wg|EnAPn!eKLbHKw3x)M3|AE6+QqW4VxcJ__A;a7WRQPf( zQuMg0z^#azciU2LlxFbjljDK&bGuj1pTujPPrH(2C!+hc!I~KF6=4>ez)0MyP`Yc3 z_c2#zi;M3ZjqCfL)ML!l#IZCjqF3j-YWr}}Lc2y1??d_BUOO(%4LJNv@_7dKF5vjv z?*%fHFQG9~tn{d<--pf7iqQ#Q*KE53eBQ-@s%g&t49p;8TZ(uQ(J5zK+Q`k(17hzb zZUqkW+jQNJb*aZiCm%gly45W8gO9eqH=?xWl*E4^d=~5y7|scQHlcdQEs#E9URNpZyRSYF)N`d7TWBO)22OZI=9E*Pob9n3 zCs$X*Um*MHe)6}YRDNZl-Y#KillKGP`oRG_daRWuc)%l3h9xlJieiJ17jUxQoF{xE zmsf#SL_kYY`G3Hsqov*>`K+Fa(gD^{KO94wXrdsr>6zva-7(R6noUU>?QWG z|CyQK8vb_ywCfZd3qp^r8w;LwTipCn&sv+z{XIO>-ZSwicme6~_Dsh5I`2j5ywShb zPX3o@d`3k_PF@=8_CqGE6|jkZSiAQ2)d*ea0>W8x_oc|)%h=*W#qkfWv)D1N&Q}0y zNkX?kB5q5@-RSe%#QdGYsH6k*V^((b_8;8$vDypv&mjZlx(Op816yYC8TB)bKFu8f zUX4c0?~^mp|C#QE^=4eNm;Lj&f!~rrt7TQIw&^pda(E;jY4VpfMtqs#>L-o+$3~s{ zh|FSfo&$Im=X8c(^}4GD?Y*OB@yha)Q6hJuu>;-0U8XQ%hngaLr&*&?%W8l^0|!21 zm~|Kpmz-mD*+cWzvd;NhVgpuz?NWu1ab;w$3xa^_YF`4>Dk>8 zVqm9h+-zCZtg>|?k&wS2hf#N~dXC-a3NlR%x$?|8pJ%y(67BsH+}a!l+?%{d+K%S# ze@d5|R*2j)ayG&(sqK|3@4M)5ChYM;kNY-&AJ6g*Q!NVuMKqlIq;&KjaP_bUi(rV~ zPMEDjo}4DyaytL9n6>QE)b=Wst>a6_TTw>%G>TjM0aE0(4nBEq%j{{@zT!bLzQ;0ZkzM|T!Pl?o;z~Q0p3eMlf-o1G-Hta?+agWYuo+bHczaH9K(Cce zAKF{2C1J5r^0SNKb-QPq^Fvr%eEu3dV{6q$4S~Z)q}^C5#DuVA-aHOa*LmaZcX0CB zukN;sI(C%4=L+x6b)V<8Uzw8qQ=oAID&+6E}9b75krANatKjuNCjT z{Xk8E#x>Jvg~!&e&{_0jy0tGO0g-1VbqtQ+80`f#wKE?Q3ZZ3HRg433Hz(t05;xnE z%SyX5f)yKkx0UZEw>zEC#7GNuRcQn~afIAYlqtqvogy!_>-id-`X>yN9%~azxOxm^ zJ5GvIXh1zX6p0fLrH3kdN!RrlfGCE1d>_RFfnOG3%HLUb6zN9_9!hdV36iQBtw!-A zub{#6KDF=V!iTn|4D(jv?=;kCJ*MT5NqW&`>>m-oe!J{N6MMaSwboLdUe~YP?cK6k<=(7e1s)+wvqLr2eyDNEf?}>* z#eP_0?P$67EAP{u7wt{@+x9q2rU8<4xFd0%nu>#{&TmZ*B|Ff5;M0O?pAzh@aeDO<&TEXhR=4gH{o~WIwqge-8KVe>Cf4 zJHTRa9{Rpm26HgZ3B0d&Kk4DVw$mz0^5*1|AmC0Ck*%d*LmyWQ7FjN6oH4oHLbqLR z>W{~Yubl!>KM^K0_gI6|Y5fA1*1mz$l#^FGPLk%qOm$JBR|JNIlE zQt|mO_u;XyW5+kT=U3jN+n2h1vtz-H;V$9DmkFA{0r%7#m-Kjc?Ka`on7LKNKC2mw zpYq!!$s5C^6=g{cj9p46b+<&5x?V9y#wsrF-=w;Dq*N2!Up(0HW$ZW*MehCw{Bf#` zph=e%ZN0FbM$h4Hl*5$Fq+>4J@=|2fi!#fwTa#tuw79Qcf%IwjRp^SJ`T5SA8D~wJ ztj%b%3fV{4?NL=%mvQX&`-pv}to_+_*kkr>4DBES$Nq+ms0r+HYjg#ys2We014p=yd)Gd_+x%P; zCiG%??hJKw1b}xMXvRvt%pXMB1NHZ~o}4VMtE;Z+B&*#kq;M&WU#oThh@i-RZ@d4m zU~`AB#_^?(Q^K8DRxMPjHC1usZQ?oHP`}x}mth1!fmrc>T(4({Jq-iBi)9XRBr#)uk-yCG_?JOGzVR+uIzAO{IJpT9Qp+doLgLA? z?};-ETk_nT;wY63vka-O1@D@Ik(A#O@Uk3hj{CRb0o>H=Oj zsUP=&l@dv1P4z|oH}wh?%L;IZ-85MEZgVU4M5q?DA^Sr53SWy*dQw%KE0QpYzo(YY zI>YpeTkeVn*L4;?x;mf`6(Y8Fk8B7ScAtNk2GvSb{4SWMuWm1%V(Lq^3WJ(OQr!y& ztec`XiWiHQXkhhlIbL$5TLW(s%Cxs^ISm5=dyylq`HDHFFcnjQrYP>rKne+`wFk)h zkZ=hX(zCiq7SbS9i>Ks^30ol~dcTCV^!A#4h(vCorq})`a`%HudxtuV))3911X6-P z&pEzl{dDf~_czA+8%+NcJ=k zcT6Ug|Hb1|ki6F~dtSP59~3j`QOQpsNbI5Jgaz)xbvGw-S36gqYnnyx0+F-Hjfet9 znnuk;sdH4q)|kro3uTv5SNn5Uh}Z@9*^UCXXLalr(aPEt9X=Uy-KS;*^oo_YXs zgOEcz= zfY~k0Rw3S_ z6v-%k8IPuVpW1n#3kMU{uaSLnl=T!#QKUV_>0Yd`s^8>xo|@e0)gfMt3VbPlv0kOX zL>dz#4*CeCQv^MX0%pi*BIMo$mQO4b@B4|`K!eXwt zN|!h#d!f!4Tz>)Hr4nxM*Yz)ojKY@Mo~s#rqA4-y+p}+@V|Q6TAKLADB4U%K8=C~7 z`uI`|RfsPEguf^r7=QFZt6(c$YZW;`W@Kdj=+Ey<``W$+O~*xXQz;6++D%VY4tY@W zSi4=m=JnI%AcIg+NOp`nJh3zb9M3OeGzU*z&%+liqrHLxZbVwxOPD5T=yET0kvQcj zaFAZc{vtNG6yVJ8$lI}l<< z^)&=CUw!`!<67Hol*uQcG8LhG*H5^+)@u-h(L(y2P7z-&`lcxPbz63=jj@P$y2cn;X6Z18LJNIzK3WW?6fYmF+&&Yu zW;;$?)o(UA<5!^_np#xTEbULWFlTu%PvVI@`~Y>d>^XY!E4w1LQ<-hjg?v82Uorui z?MqACg81>NphmeHUWG~|?aoLw+Ts38i)|~z&}3G%%_9SksrjJXLdLe}LVJ|yEokf$ z7|Y`~qARkiUjX9M%*@OWRYn@AP7BV?y02(me^1h^d?FcdCDW52b(&~e&gqZ{! zBhDGWTB##K)5*^E^7g_WCt~E3qt2ONbq=PG+s2a#C%=Kujy7K46kuM-yb&yYb>k~b zIW)A`5-$BJQ@J;&E%bOUs*m}yZY)P1Uaxj8>jv_@86`HXQkAa};&t8nj+%Z_wSp8c-9gKnsGoyxN>p>FX zh(5#nhaU6aR1DdZ+RtFztUJZ#;=$Bs7`CVWdP2UbB0J#z^UgDu#&t#soBk5laf0Cajuj(Z*tyhl+-8-Xd_Qm8+S|IpUE&K`ZHVZ zvE-?eTqs~Vf2U$QIGUJI$1@gM=SMVj_EqVVTei4McR=uBU44C{-}6T>RRvOU{Q`nJ z(S?r(9-c_t&?aDaY&i0bPrkU|Kl$&5?_~BDi@(0Co*%k5A9AOBAWUC=P$PlA9q!3& z@=D{^7~Nr8`a@Q7jzY~d;?3J%mHzEnHTONT&GUEdslT5qzkydP-yC z=!@Rrea-HGj(r==Dx;o=9L~M7m2KS?^G<|A{T24U<{Hh@{O#%-Q6E{^$Ci@Hlv%ZJ zzgW@P=LMTIX`-BXv$>@NFgYBl5Y;7Dq@QQJD94B?i@x^s4^gR+rKQ5N-53UdVV>=9 zf6Qc$XDH=}-ltZQi9OL3b|NpLmT{UWNtM?(kgE4YQBw%M&Ff)+hQw1uwvr`tmn=mO zyNQokFY<8x>ZZIBv7?Q3XxJU8{tG4{>-2r99-W_B+zz*}(MB}%4Kz^7f z%Z9j&<0O8~XCkJt?kmnk#noVJYi028xF~n_U)graDMAm&+Zw>135PJ%rC>@}5+e+S z6KSypI>>?m(Lt!79Y_$-aUup>DYE{YbS5-Ryz<-VMmtP>;?=Eb;6Tzx%zu?EFZP0G z@NW>@tb&S8RC(AFeI8HDP88KBKjlhhNai(s8&h1Epz^L5`(gXKWUqR+z2mUfV;nf0 zjz_8FwE{-RO*Z!2{i&j8pEgpX%YIib)BD)TFMRa#-Ey2%b$7sEaGWD+$nL(TNq)mY zT*U3^DrSs%=JR3ALuFOM!||3qEh=qPFk6Va^5nexbqD*SX2_MxzT9o9k8X&aj+BP3 z;>Qort`hOhCe6Ah_J#*RcL8vBe3u30mFzW@+UHG|P+6Ch+!6F0g|u3jjW*LyHym#B zwE9O%?}jjs9Q>r$9h^6%i6XNuU}cab9Sm;Q$`QOV`gd3Ctz8DJe@{;0SlS;TAYi#|6-k5$I?UMT{-mKt$b4%Znr}dS)eRUxU}N8#J>m-e>{ifaQB&SGpwRC z(i>;uXu6oeSV}Z6lJ@$8D3rgrisZlJ+ehJ7C(~C~q{{KFP86dQX2|=36@=2Ku!Cl- z$N92ND5WNZ$S8tPB8EmONGVDh8ylfYRm>rPgx=G5092z{s^ov&bm)RpDvR$&5)Pos zY9tkpppgFA8gwM7d@N1ulNGN)p)Q`t8fGk>X9O?B@5B>AAwAg>=0(Xu%$DVV!O<>i z`t@ZWIw9KowJ%gbAtIQ45frla-r{bl_A|jV{oS#VG#j6g;n}Ek(O)u1#rjtgEpxLXm(5iBPPEiAkwKS_IAN-T|V%9;b1eP-slj=Tx<`Tc#gKLMP!OQcS{h z5t58LLb)6e!BUy*dQJ&j|WP8>N&_+7SSOYRJ`Pjagv_Q8Y;b-Unnvrj%w^g z3A_<;>=MP!Bavdz$UpP8vaq)vGm^Z=rcxI!Gcy1t6v0xNb&f-2raxwv!~V>d1qAfG z!{aV?&LSomFQTUZ-SU{EoFd%$re>QBpvOb$31?NTm)nj-F+KF3y#yDPV+pC zun*+?Y9~9+1+8j{wY9ZDM-Cs~>c$3qA?|DPj~fFCZ-wCh;kp(U76jc-zIikEX|~Eg zUL+a_(y60ZV+)omNE<7sqt?y(pQo|=_;8hfC&F)@nEp1yQY*50hAUMnYz#~sdG){`c-0-t5&Ptg09%&-MghXxHwjf5 zpT^3RDk>62w(NZd3^vy9AiMx+;9O@1BRJ1vu)N@0Y>;fLk#AIfVF-$5k~gs=*reb< zY2Y6=^Q>>j3NJ?m;0$f;o%g{P>xCx4ydq>b@#ZeBpu?0U4y`;@oh5f7up&t~9Hme? zTDnk#$icf61Mq4`^OsZvE*AnR_dh}N6f_&4jJ?C9>zpz3!+oMW}1d|iw-BI!LhH_w0x zJFi}MKZe6&Dupb;Hi-blO#kdbP|kXl4zFat`L`7pF0tsl?&`P4eh}s$o;stwtWX&NS1`yORNR>!;kjt%bUM>AtVg?e44K8LF;HDI9uR_Fl z9Cda_;!K-KWg~r1k49@q!#7y@+I9mD=ud_FWSG&;OUM>j&c})%Spgi z{@nHR#<5%5LO=YLWHIx(a;1WfK3H)t=_z7pPoXwRMBiY&+EoYvA05pX#asGOZ)4y4 z`?Z|xzID0;cM5I7JBS4W)7W>RfFs385(!XN_fm$<6`d+cbS$}lQ)a`FcG!mK0z27m zd`~7lFm9AJ3(spN#sUM^HnoQpHBnkzdJ|isF6hww$2IooAIfrHSvyF{cOgiWO*HR7 z!jKw$^56z4ZDMvGhAeo@yWw*DqzzP(EhSeurn`8bC5hL+C9OILto<_sTcz$wF=h&r z2O$?_z;T*4foyq&)7ysZ>F-@7)~HIA!{VQ9en8?Jiu|Y~y8=P8k@92OjmELj7v(rh z90InY{S6egJQ^80dcNa=M*mkcuZ!PkJ;L|#jE}&9k}Ys1=YFA7M&K6}ZzY)|>UIuc zi`MJ6d<&g;CD3nZR+`ADCcTMb28|*y-HFD_@-N_lSbBzncM6D48I2ehi-)bWeBS*Gzogrz%x5pH}I{>JV(xJL4?R&yxqxyFdp)DW}|@ za5#p^^hy7RrLzo(>ifRFN+TW8-H3E|cdNhO^qZ72-!(SK!tuN>QKyHE3ttIcMl(sWngWk5Me=yeR@JxdZ@-ajsHz?{ z$G@@9hS<~eav*kPt4s@Kl-vuq0R`CE4kiZQ8n%Y;+?bn(Ps#cIL^J-CkPn&-^VII zn<9@io(U5x}72y+HvU!t{@XxEF3*j@UPDPl1mvddCk1+L_l+&fm zBpjHI8TqRda+)#nLr7+RR!dpIl=g}(#3U~vjs9l%K)f-*+#mAQ@jb+Ht|$%41#~SY z^A3f0=@3wRsk>oz7=P7z7b(qe=?LAi zW;#`AG*5t7Q}Y%nKXdbdf^qJ^3gkjb^)%jm+0UmKD~%Yoa~QuW0;uvnjbX>ncp zC{s4usE7~z$_YUqf;z`%o5#O}n9@YrSmOT3QEmEJMb=*{J4iz!W?sm;v@#{QI$G0} zUaZ44Vu86OOQ+0DY>CBVw)IHU_ZO99sUn#GWrY!C1=M6q@&^gQXhCWCYnmMTFIs6- z!v%&b^2_wNQ@$P`qv_wyb9GW3u_f{^v>zd={0fYjQtg`^*)i$+Z(XqIU}FyL2XKoT z)#u}RutO&(CI7kBHljJ>P~`csDPVDlf8=QbY0TD`_Xl3;#MNAbYsuNksdSOdtiZG2 z5uP9M=Pxvt&PWE18Hbr&@#$cgVd*=SDKn)Em|=4?aFpBqIG3}}Xg_|Le{vaMZ``kT z&nuCWYccKVKl-K3i8eJg35tr+r>YFKSXo&C3770Uj4yLt54uMi&+Yz0xi42t z|2DcNC7w&+Nzu`mez44qbK@6?e-F$TSjw1n%Q>i$J}2x&;Y)XLqY_Bxl@M*a_Pu}H z!!@mYx+o{}U-Up?|HH&6h4>n^I8K{3F<4M3HICmc`+Hgck4pJ`O-WVcH(P-}IZ_w( zYqvg{xM6ikK;NE095;BQsZj;}9|&$a{Qs(VO%iFzQr-ns!MakFul{^~Gr8^1H3_F? zh<)W^DJb><9rfol{ic3KlFb^(@)(=ZYs{s4+5BSk@C8JGGr1$az~tt_BV}zFSbalJ zIMIbL!5eM73wwe{2Xu{Fj-X2w5RCM{lSFJ${`<#u?miHQfDhAmBDKs9V^P_J0qvs5 z?=y5*y=nu>58Y0D6{8)M5G=`3C#^Zpi!S|DHE0}v<&{Iid8|}7oJZ+SBN;v2`?kf` zQ*n^jQEy)2zZeu*3*QmfkMBE@vBq!e1) z^L!7Wz=0t<5H=ad<+_TKlRfnG;xz{7HX>m^W2!`1g;EVx5=6X74fwLZvB=PrZ-++g zN5@HYX^Nhi*9Mw?Gl&^Jru%oonzK6LQj=Y%&pnOkiktxCC*R)~T9K#3vC0t0g<1E@ zPw&3OPdv*c0vVdxT$uCj4_y92X!lK3P?v~cB`kAAb*g%!3&BNga=VP#Ka3yei zPQ$FmgMVyvqhAKk&|~-nlh+wxdO4;2z41%Aj!6oll7(<8`r!EA}!Q4 zz-ChqF>BXhC_`Q`O+232SqM`(=V#9TITtdB@T|?^y1m`*cj?K zFsH8X85{)g!IWx`CQWCq9iksHN;)rp=v$3G?ZXOuVb>`=-1dge!0C3pj_ve#wliY> ziSMewUK({v(3JB4HYDNKmkYNuU4dhsBT0WEiW1&2Puf+LXY2ptc4l|cFo z+m>gTHcdu~!ovH5AI^0Z_s5EdG`lz~A0$nPEnGYKi3s4Yi%k>yrIbVI(Wcz+)ymcF ziV8c+G?W=U*Qshx;V8~e5(y)-@pWMqVv=ES{2F!Q7snkKe0Y0^G}qUCcyCYs;aA?n z-$;jrvZP$8%dv3=@gKjHr|1Do-!1;Xgdt#I2y{3DqkM*POF00?fy0>LI#iRy+ zdu!x+Ie|kL>aSk`{XQi;04ve;xON5zAw<0{gS+lCy8uDadFIAbh(G)cE|#-@F$uqy z*f)NP^uOp+-B2%ueHX653GDeQSqkmp#Q>JB#r5R^{W49~cPxbHNfB~D7%i~>`ChA7 zrdOoR{*k>{qPbl4<;&HNwn}aGE`7^p4R|ig+}k7LhR*+foywy2qQgo!|7VUj&WJs6 z#L^hKYQvST)%r1lFc-btXKJn`v7FF)vj>l9)eR0t9$#Z*kdUSU&; z0?RK8lq+OPGYDgOJGn!@s4#zvciP{tfY{Cddf#KTx=}0e-t}{%j30OA2+QXvxA=9_yr<%Tu8aqYqG;?EpSmNnno$HV|i93 zMqOGGE&NNzdR|CTyg$41!;;W(kZH2PL!Ne8!=9^aDxR+IGnbd%J9X&(ozRLl2(>-2 z+ZndN6P+xatF0Y0`y-V4h;n}C9%6tZgw^zx{8YEqt)XYaJ={l~30+25CRKfs=3E}Q z2)DO{x7cb9XS*$p#ydC8(IP`+}%9ZQ%Tl;XZm=>dep+7ZfhMpaoP!7EFlMqPc1ICi7P{V{PT6FYTV=~ zQ59*3iH7`-%Z;!)?n)j5|CGp^@dX29NQ3hl(1ZQ&fL8tSwym*B9IC4-Z-Tjrow(Z- zq$JtINs>GkGuogu<<1L|f0|5$*C%St`^0_DTs;4|pQyE3KcZO5i$mQm#$NM3&ifFoF%yvkpESS@7Q_w$j`$@qEPL>n_)Z&co z^{=1wwdc={HK*{X;}@c>mguVe>1#e-d+;RHzN&Fb-Vh#dwZR#8)yn*TOP#8{gQR~- zW&U$3J3@Y2?|oDLZ-hp3W|v_#F$Zvdd2}$Fv7?J z)CIQ-U-)i?uF(P)W{@2tOlHJ##G<~s!w@5g1x5pt)CN5E z4Cd&Yi_a3zQ}34E0DYBY%FNJ(E%VX^p6t5t1;?66<PrF}{U7_k*tNFEs)*iZ{-- zwojU0lvxS{88P0(vCm2OuZMg!Q=>~%@{eY4O_a9JJZ;CmJE^9;C^tTVEv=DfGQ?KY zFk2#gBV;B*d?T0eGksiZdC}^5f{xPXC9PUw1Na_=D9;b{n{=i*HQ3jIAYZNX=&j`g zX7+GHu_-SijaPTO9`1!>WARvpGP8hGITc{LUu^gQVK3v2GW|8$^Wlb4;_q?p#&)&1 z@wT-}!d$31Z>GMI)_Yr<3?zaO6K&9jIs678$LBuS`03=Gu^3yuL85uE8V$ZvOPNbe za=zZwlv{w!9`k&YP>V+{hyTk@|H}mwpOc(Fpg&8}0oTvI z4q{%zx?P_A8!uZwtEu)DL2sEYDfn5r{)4kwAV6^rGUUN}B~8Bl7eP!u_tyyEA9X!F zA@pb>^wU2p`9^w5l!~qM-r}@Lm9)rP?DH#NQ(o@s4n1u_(7kP6$8X3Cyql9Alb4Hw z{FE+cdH08p6{UH}_bv^%4n{9G<)j|&_;seL>rR4;^T6Vw{oyz*M4mCTPzQv;7iZ$cpy1Ih(pxb*zCob$eg4_?vP)?!Hl{fu zeD^PnIpWKs^LoT?{n+txoojXC?wz0nX}XoLX?*vq;+J4;IBu~Y&;7SA?8M1xLEZ0^ zeM7b3VcHw&VBu7rjC%W5Z<2&FXW^L2*#$`gANSS3U!t_xrOq*%o*hYI;Z{EI=6+}V z;y~S_a3y`@V%mZO@}c_;;#fFl3@jI|Z=VAwiDoVI0_&g0FLWu_65N#VF6Xr_rU+qr zr6=X*W6K*qPYT0kN|n14_P(LW76-Eb77^}7Q17EB2{2X6!M5-i1;#j^#pNYR1FVY) zoXPP+Xq?Hr#6>)tq{MTryuCB8uiadY{6Sise}joqi)7QR^gZ-vKf%#|HCIlSJH|}$ zHYla<{sC9(U6i>@TX_zInh6)>PV`c!v5&Qmm`Qg~zt2iD-0mIez&GPQGk@6bv5DF6 znw6c*TqIHV(`@G*(U|^saBWg z%aWP{{z% z2p6=efScq1w~n@E1(9~ws2sx7d7t?V5#~5sT;+-OlCZh0cVuNWUgIAnq3Yqx1UvaY zPTr(IJytx|E4y?I9Z89C#!pJ9&#np|?iAvf>GEH6?bdVBFDsokj=HCZ22WC{+9Kq(-#T4InG2Qv! zt}gK{ZV-GzfAf*ZCTCv}S0260wc)?_89K8k{)mz(+V*kB(?D|+k(ib~_q7Z@GnzKK zsReB``kxv#KikmlLe^VsH-6>UN;`ORG)e{Q#CJxn6JX3U=*^$i;OJvZka*YGV`EsX zuA`2X@I%H2*!9(jwOs};B;99CoN6kIVlHgNvGjYQ=~&eA-XoW<{a8`VutvfUH3c=8 zeUW3FZ{X3w|ArQ7a36;8WqNTL2CLx$V45BaA; zE_k3tB{tP!|5V|m(luo^Pi%>QyC!$F^b+H&*|yE=zvw6c8PwQ=CB_W zNoQZf((I6xk)&v_rJ0{Nd*0$bNpd&&40ei}G>1!AZ;N1Mq-e5`eNqTE>Yf zHKB^o>_F{|5Kg2>&ScI@Y<6uR7{s5$BTrpMfcecL5%802Ax@?unvfd0t-Z1m?Elx~ z+-CG4!e|8jM12L@l{;m1mV%W(%0fZJ5~ zy=`9@thY~)P2lRDB~WINb(Y3a?x0o*x=RU~J2UklMO?$0QJW;`Q`BS?o5Z{3H^%i_ zWjh9aCCT_G&TwjRVAQ;~`+MZK4GN84I1c=Sv=1MNoXuM>y9VRR0?w98@LehDh09kD zc8N1PU!)z}ElTMpDx9rcwPhV9x=JrHT?H1L0!48+gRLz*@cJ75zIaBWkN51gT+#zg zSSG#A>qOPPH09kcGZz?K9ym3!yo0~%Tg)Q6FD~l&i>X}PRjl)vYG?PfQTFM@yKZU4 zT7Rz?{Ns4@47Ty=EoJ7$MUgC;RKlhunD|%fja#b3)(yIlDqY*f&9<66AW?0r`uKy{q8ZO8J8>@eR zIC|^nWrE(5QR2M9>1mp=%7ekri)j@0E0&!!Zko4=FM6nC#HwVZsR~}RI&$U510BQ( zbW_^~o18E5E~U@s&VEjO*ka0pdZcJ9-QNBkS~d*>C8d*pakAFN(X39v}%32DjQ+$95JUA+@O781* zEs9u1CGO+|`B8S9=6_f-F2XkPvT6b?0vsW_Rc9hw-mm6fQ5A9V6!=O?T&3|N*4FyV zzMldyd^;|1Y^Tf?sk9t{XFr;ulm))da|I!ck_)`NGSo@350n9X{bVgrgpP)hC9Y?pswl$pVxS?uB^7 z22HuQ>Ci<*%VWh(xGAHp-9%rm#{PQsDZC=HWdyl!hs;aTb15C*>G^h;#7=)520`~S zp>g1B8e>DIR7mGjW0oH0dQNA|1USbbbNA0<$~d>(2N{>q(jworA04G7IEO@*;4|Ym zsE0cNy``VWX9U37`5Z{ip2(lBlr8qvn>zx^`1me=GHM5lv1#wR^u_Q)=^5`o>fmsb zio}ss#>~!Gn}3Bi$w;m4A7?;47ToD{-&?)SxfKq6AkkRr`YLL%`f~7o03MV8S<78q zlz~Q|a!HILRV6`hPddLpd}(<0)eB)YhjF7y-5abiLjy`sECWI8Z3?kdVJQ7)yX!?ECoU?hN(p_D}&Tq{q^YG`cQn(!i6G);QB)i3}{ ze0M#cw$9n>PLLy@rV)pOh!&IjwUsL94}3l0A(zv0Pz5~GvbeHn zDep;ExH6Z ztQBwQqbHaioxwS2LbAzdWD5Q8icNh|Bx_-%p`ES`a85~l{W_$`Ff)twlZ^gsT-6`MsSYxpJ|*HJG41QdYCfpn8-lfpfum zFCrlHiSfy(q+znMpE$Drl7)T0*S{xEPvi@t;TC)$*_UKQ906u@sq{_--%>U)koWu<5@%bu6WvgH`#w%R1B30&&7S~3S-E)TJ-)kfD{JK2H%;}A z)xCC)a&V3r;|6&UiWD0;l1A_n<(5N?6__4OA$?Q_Y~%sDrIqUZ4fzv!B_8Zmmi;b z!$iK*wr$5jn}%+!C?_+}PHUwmN2cm|Lt=U{XL#SF;F~Sk)HJNYgzVcoegk5?;4C-7 z?D&w~Kq^K=AjV+`;A-s3Mr8qptDTBNSm{0>G3302$^HqZNCRSiN^s9-9?V2cIk9Zq z#r~5%`oD8oht>vS_!Vs<;YlcdBm_!wF&rp-g!ctdY|rWQJLt#M4KYlwbpi|u3IWYv zt&Dd)7Yt?smgWjJag9_?Ggkl)N%oIweuEm$E=d%=sKyo&6x<(m8upJmZ|Pdd9*MV2 z>Cc(%U4ZH%RsS9IWd*m-8Hm!-JbUo*KDt~v6P*wy^-H3QLh>WtM&YWWGV|s#v)WqV z+^L{AKeg$$dHX{r<=?>Xz6_OG~SZJkny{O`Q$RNFKR6;-T!q+z=pON-Y&D5F8d{~)9ik{ zg&W;ZJ%h0x>k&E*H(aCMDA9%$AT9tIJUD9f2h#EM5zNX6l_}+dXZ=1N?_8F-oC>&= zIcSRIi3b!}rZpAO@;|i%08YUBf3y1M6C+c0@wNTVP(NH z<{UmZVfT2wPt)UX(4}b2Fg|qB_H)~B-$mR|?;Uy#-?~XHuqRkEbW=yrnzCpyC($>Ez*5s zhTO*$9>saZdn(OF=*LVFWu~6_dLu_Z>$Wbqg67KS4_OF3_jnB@Q%4qys5@SU(Fp8a z*7f&bQ;PB7Z4_K7oV*0YU(_+sU-O2>(^M*78t6B&So8~NYnx(bE`#&wW2F1({T9Ywb#a{X%Eo=N4)QCwsPe zf4-t}>6jy>U^P_mtY4cy1dltsm%lCQzo@_0T zCQ5)~`|1v$R|FEa0anq@4iLNJb-wvJmP$fB2+YL9*q3?KWdn~Olt9+mzkj6b{|+d8Gs+~Ycdt670iCbPqua>6 z$KM>44Ut_~>TsxAHkrB!0)lL!S zxKv0S>%a`-fOazdG|9^ACj8BNuBy~2ih7hT>xNV`~`T{o)%&1ns1D4JLg_f$%@kjP~@O6c@JPb%Eb zBvRQ8VQsjC$Ds0S9do+5V9WuA^u)?*CN(9qI0nt*r1JKP$~n!9k;NFN)mM|T#7h83 z>-xBeprV4b9W<(NUPS0uigwTJXSS8oPy+B2P;47l(<8A1kXI2vZf{a+)s~|<5Awt#k0oixeKwKxz|lGz z%yLD%_CwZ4VM&??mFtX{kBu;R-atYHQkPuZQy@1VwpiWPQ!Q5XLSDFeK?(o3c{h+R zCZsm7$7geo-#+*e1LFMh^Rh#AP{|8-W9#c@xu;Kd>i2O?8zI=c`*PdQaw`K!QX7SZ zbeH#{l{&|bdDZ;(zimiOhyiXKakRW53$7!sX12Hw=WX;+Qu)ys&-Gt>pVqMBDZT%M z*M#`9yIqg%yv$D_?r&TB*NdBUp6EeW(f4X~Y>yw+`ld#RE(w$glfD#)@LRj$xAhT2 zk}*z$-6cxaP?*L{EFnp(grmva@=ak>RtE-w>w+dbENr@5 z01_&Fl@!c2-HiETn;z4;EkT}R3&gY4@m4*lXZRgUiZX!;L$J}R-L&E;rs&Y?6>12RSVq_87K#PF2&y?wH`7P^je!iN>oJOSiZt1PoQ`}$kjf;9N-3A(Gf8_w z{D(sf`fPDetsLoEd3)bPX>qvnrbWs<%;({U-=l?L<9gk_-jgTU`kr7HCS=Ai5{7 ztVHS9%8RC7N4VF<9;ChQ_jAY1(@V1yiyVQRXO036BE8aO#NK;4K`1XvvT}9B1SGKg z*vBigjjC>prwd;2lyN7K}gL4J2}7$G|Fd5rq>3Bf`wWwb5mWDw27~&j{~M9KwyotIh?C zKixl=g!i3d46vcnD=V3j-~;Vb@cJM&daLL8C|vbCzd ziCoMhKL$3}P2#JmQto%H{(C=T+beQS^Srz?Xnk>LBvC28)9Po0Unx_6tGW!Q$ zW21LHb@UQ>M3UPbCDoy|l}zkYpo{7~)MP?# zS2%qE>?Z$625+6&u~hy?Ja~TlDOhUxyI^z`+Mv~Cq;Q$5=xywnnK1!j-eDt zDPk+G@y{H%lmU}x$)4Yv``7yLybVARNC+fB+brZ9jOX^3%VB-vVMEQTU;xNJB+rRO1t%!x;YW!gE{9V zc{rnVb6@Wb9m6cNvO#;s8i4pCN#L|gJ;lWKzcYl2KMAnm%lbau=mONmDaeU=1Fe`q zE2+do^Jwl0>-^PlBK^LK+F1?Y129DcJX87Y7~=x9s+o}4|CDzZuTRD=Y3uP-i_whPzW!A*`C{qU3pDKy6VEP|SV326=h}|EKkCCp7vl{+|izu_^nG zx3V4m0X!O8Om)^{K&yae>^vBJ|y|_v?=HI_b9H zyWNV(wYA_$2eQ?r$-3rv3&+yliwi~%2m9|Wsik^2f)0|UXPZK1-{ev!9p8dGa`)Gz zVgM=>iuW-&rK7X!CqSq|6GOX9k!k6zh!@@3NO(CQ7WAk9!fN6^aU@*aFy6m&N-TUv zpG%0@N6BimYwP4HsS#SmxIbx{=ZwRiHC=zDRG zdG16H4JBI@*0(B6k)y}GP8slmFhK#`gl&~IpXaoc6zjLAtPOq-Z&~x~ z{N-8|8Fp$(w(xHs?&%R$`0quT>p${vUZ<&V?04`<54>ykm~q(~sU&Z{xgvO6-=Lj9t?FZ-WSw?3GQ< zVVcd$nH<|6fem>!)Q76->iX8!@Ty}VWFElEzq$p&GJz*bVvYc|iY--6HS;aNqQWY!09+si^hVFQ&$HK#mRLYNCMsWqc!fh5hTrJPEr^JFUgF z5ZpEGODYkeCg52*_Cl(&WLB|Y8@Kv8(M`Q4?SI`wu=hRGn2m4 z7cI%cUIH!r{_Ogl(fSSd{hQr2geFSyzdfng_gnR9l$7ZFm;4|Ttr29qg|C$rrToG2 zRhs$kp0Pk&>0YF<*w)`Z(qvOraty7DFnysdCj+a(jAbrYgnI45?>@2F9J%KM95MGN zCOOdwkr>{@y{7dUH6T9bJ;ab#Yh#*v8Yxu-#``6u#UjV$%p*vfZ$2 zZu2NawID$7>t&X^_LzHdmzoRH@bit#l^FTRj}jHSzvG&^osEHF0AfX=fX;HgWw=~X znEfZQ*Rz0qKd@ln^rT_KMu7~f5 zL(RO(>0GoVHD9il^gn(=|7wl?QD8XbTN2j4+Up%2@yGI&r=w%Om~XFT#V{=D8%qF@ zH>jF;r;aq;C#H9gzft5?@ZkwJ*WaywAr_t7@dvRIHeodLp7b{hX3L%_q$RtY#%Qsd ze`0$ty15tEx+tq_kOBt&o0ibOX_5<_{7FJ#PUnS=8HCE;cdg2P&y96-bs$_Tn=kxYtee1T~uEz1a&HDG*$(dTfW(P`?x(^~y`gq5rxUD){s5YS=q<3)hN%Swd; zj3jz)@e5@0zB4o&G5)YmydFm1hw`Zm>!idoO`Q2wVKsl@^g!3{j+3hOYJ|gqjZno~E>Mj3h zf)Ato?_fb>4#o4{VCLx_wgO%L>4j#!^3O|uNGf(^xO@9>Xh@9We>cFtC@#YdNm$9O zZTY@6;)&P>7Lec!VQCT5S}f-hF7#!JAD=<$u2SvvlA9z8id2w8?;x9myLV*TEzoq~f^^1hy~!ykIxk%!BKwKBq6Mq}clK>oczviX zUyY(bM8imQY2$?teRwD`-JCPsPNb+NFQib>krK7FG=gePNTb|*!?y>4w8=gP{AxF4-UIEaH|~ET zqQ6TID$bCM_yz8&KGoAz{6#HaqbKIQ2mz9a3gYH~?deXva4mrz&~r}KQy+8MnLfKB z;XM}2MmDnzlkc^hGxrvg5ii6YK!+sjRrR|w6xL2!PxT$mp$Xfs2K+ihMwF5y_ac_Z zP$cKbX^13=wUa;Pcc_n9u{SfoGRew2)*Q1v2Z;1P1GT2G?He z)c;y=Fu_!~^9!!BO;Ta}6{ub?v)Xsl-_iZ>^zd=~pPJVI)|nI?T_+ASftR@+la`Z8oF6}2V-2&ukG40gK2Pl{Zo zm&_xeD?WJtO9YM8_!Dplp(2z1IVEZRhDH)CjFCslUSjYP?*SpT{!GL35Z@kf^zXYjevxgS8>~a1Ma#?o)b=xDo2dnC zx--}yf}jq6BAW1fjO7L2r#LsqMwtP;5MIn=W&rklfyZXdRr4mp>Vvu;+-kxfW}v^} zGx=MDF^W*xnz5ydOc~8Ix9t-b|Jj;PvM-v<1npDK$1#)73 zjU^xH4nMrHJJQH?xoO{+x!b0)>HN(@+=7@NID%2fscn-t`x$7Wxw{NUO88DfyWp4{ zelP{nNz$ty>}CQqQ2^c+09&y!S(*>IqKI4V8|A=v$#UR(RDzEXqH#n>fBf5?5L3mb&#wX2a#KB`$V z^3GatKH=22Yr*?Hk(`%4N5T8&T$?PnnpNh;eA`3swziq7yODfGy5z%Qww!9tp$0#} zGVf%q`K$6X>q~g1{C8a$79c;KMJ%D8K`HS4U7;qctnyoyp+tU1S{fz8Ezh4x6OH#> z|Mk<}uSXvimD`Z(WP&x7|2~Qb^}JeX+DL-y*w znSzAUfs&GnGO{)v%XMD|@pa$XW65qSX~a-0ajoSmbWZP|cx4YNCCa-f7H&@+qufbFsRB}-O(Zlk4wD_(s~`N) z^;lLK#~q&2dxzlzk(P>+fr{t^fr9E*Nih^7Q&=|5A0~2|1t0%q4cer-59RYj9me3x zt4rf321;(VB;x%<}P{h}<^w|0!%Cs*r7oJr}kYye-jjZ6(T-9 z_!DNOrk=|uBtkZI6mpmUTAz}Gx5f~-f32t>6ldo^mO!yY8v=sf%LQ6Mc+>+xh3hm4 zK2{FoJcUd}9LCb=i-p$L^IJ`lX5}^rRmKXIJvKbxaVYn_=T<)tY=A#IOO{$_`A_ei zfU==b_EOAHd5sxLv^KMz;nzbdGm^E5H^tNB-yb?g4xz&1^^}!WD)m%TR6s0+Y(mk{ zxI4Yf`vQS;8(!oraa*T{iXIqLd?VL8s|lF?`5NO1*=#EzyQ`M&YfGHETsHI^Mlu3R zz`CI2$uZ)Lq_zCR+wNAbo0FDD*8MX#=ebG9W1StDz+CMAw0M#K+8bJhr_DdjXA^jE z|4E~oQ%#TgS`uLDk7wO^(yRw8OLnifl~y9JjID$}WqkM;jKfSwvx~|S%IxNhqYD%> z4oj%478y%BM-UkHP|u%0PaDyO*uzusn~x;@rF#V|R-c+CcRMI|cI%UU42YZzr%G=v zbYQ}6pgO=IZ*P|FUh=ef3XFiMsAf`unOp`3Rf3%O*pT1`-^BeTR#8$fuiAfI+f+z( zg##EUcPa@OQ92=2j179w;pe+Lh&Q(v5V{bH1{ETQ*?&RSW^(HsDI9jS7B`0f8Oz}t zp=TD+LAScp|GkilV`*PDf;mnQIHsx}2I?=Wd__y+l-#S12aw92iQb&D3iQP8lz_MT zF19_-klGx+rh%qlE%`l*T7o%QMcRr%2?!#loARz9xYrJud$<^xf7N*fV2SF||`GCBOF8FT^oX^6D}M1lVeg!_jhNc3=leVe3$H zX~Ms$ZnP*~w_yBE48_=r1_0FofIQdH1Z{_0kx9)2f_5|Xa;TF4z5*kUE(5~*5=SNd zptS$cE)uu*(8x4c)!;A>vBsMgKkC}-PySyATorKl*W%75-sA}MBoclf_>e`d;z+Qq zW;q*^4U8kW8q6NtRc(o#mt#Dqm94Q{+fqgrq31H^i~$?*0IoyhWRc6NGt5!L=jCp! zl!qJD{Pha7W|jBnX-h(_*r|%D2f539^c@5=W}{H*xNmdimeJ{|@h%x@<7@z}`Q_>k+6@NZPGExC0w@TCHM$DwRMl7$Z}?&{xN{bvI{ReDMF~#*suE zfSC!9g^{a>0&VAjf?%zMxz_!BKw?f>*F%;)b zu%vQ0A2H7Lj|y+f6{SElSKerO#-LOP&32mGlVzCfXaX zE_v+Py@^U6Eb_Q*1RTwl#SxJcK|N+Pf}9v&PBkny3bRX3?oK%tN&LVLoLSfG4{By= z&|y`ppPipjf<5n-aRRe&l7jOpY7)e5 zq%F2gx#~H+Y8@^B*7@(G)9&Vs4G32uzN@`CX7y3^?Xt`Bm=>p*ff){>`4RsOr3qJ! z3#X033#9IOOBhpq5R0GTE9ITla|64JS0Yfs;mS}{#6zW(!zr=12VfA%qK=ZjX>^}H ztJc5CkRn&;*G{0{RIvFv{}wMqXUv9Y@36IKdU6v&i$=1=OcO(wfJmD@F^Ea~i-;N7 zY{UzQcbF#elq+JUyCRonM&!;8CoMUo@1Qn0>2G-C;>h4)GK#KE9U9gdHiyaP%Z^^z zRx_(Vqe~AAhg-YS3B>^I5l04pl`yD)M3ma*m&o1>3NOfRmk;)ddfUF;>yFfmu9xNC{4Vf8y6x zs*=dlXd@*n99ReLWk)=x(_2NYH?9So)avxwtOZIEfy_Oa1Z(jCRAR5eesjc4Iaw{@ zmuTBd-y}Use%PoxDj!ASfP$F;cOAhnA!^mcl0vik751}}zkGX#HJj8>f8BH-I z#dOmP_RL!?GQ=+=#o#_pwxm&<5VCFrYK?>}vkWzU0VD$hLiArx5WbK!A*%Jg4$sUs zUBFjp39{)*&#ZC7=YMzcKlk$gdZ=x#I87FJfFOd2_w%mjiWBHK?- z;ZpBYeW@OnuJCpn-M)X!Y(P$Gw`}m2l(Cvh5SXXOfKi`>hFLYRi^N$8d3eODqz3{5 zmngzMehI-c|CJ}mvRCv{LG|(1LLm6~xI&keXdPk+p8&`#u>n-JT~)oMGq-XGy@xHz zP+ndmx(K8cd>&$?s0vgGMbrXbDu+&dSV#I4sU@m837tXD*q)a1<^$densbLBCLVGG zn9yZDLeNHPlI16;@Y52#E;H^RCvoUiC~g%KnsoTKN?T!CeG@R3_bvUIvR0(GM4$L! z3n^lK*oyN8cnTw@2_>M+!4hx1*75XA(8QXR@;}K4)E|y*$4?HUJ9Q#5-O=E9&!_N) zBKv968voWRt1w9;VPh(RdH}(Z0(l3l!B?_zk(3F?pvLU|Pj2?%ZcOE1V^su6}- zFK_P>YT6n_#N37uJ1s~ZHuJOjn9214S7TBphtFh_V1xjmoCt8%g9`La+Fz$om_Fi3 ze2SuwYo~HvPRVj}JE>7+>{2Q-?BlHmNGeX`oJ}&T=_DIKfDec%e|^Dui~D|ye@+>b z27`^soD++91@}wb=HsEWgDb;iX2b(^Wa=lDlqTln_rxJDqnMUA8D60D>aK%pId~W5 z+44U?o;hw|JyECnY++-ZTICs==O}F2_U%=wl8CQ1ob^THl#TRR?b)k6?+m*c8+euA zH1+^*5@;JqFy{vGIg+Nb>d_wVR$Uni72^R!WVQ?a|G4_fs3^avU!_5$ySt=8I;1-U zq`Of@Kysu+y1S)2rKG!t9$=Ijy1To)kN@{w>)v%g@P#!#GdSn$v(Nq&fFIcWpZeWYC+7>_CxbL@4=YO5to%NU_u43KH0H(SWki!1D7rw$W4zs zd#wh1T2Vp8;|CEx>6jurIx4%mLkXR>OMDi$Q8lR%FE+|Yaiq;jP+ENAjq_A1G29>k zrc8@Q;KB~rHW81w& zbMxAiC$kM`Az#uV!Z#~3CrJyzZX5bM6e<%70Ar*({M`EdlTO8fPWjs)gJP#Wes7+b zR2&Eb5HENVc9j74ZBkSlBGIQ zOzPyWtXLInpyt!J+Z#rs*WYc-6q^BdIai zMI6lbJEir|b_9@~eICqfnp84H{9803J_|=4V^;4n)B&eSP#`{EwYGim{+{ekD>)5M z24a1P@rd)N$x}C-dvmjsAiu&;tXi!}XGOLrd>0ZscIkRWD`5n1g5B#6{n)kr4HibH zeC}3MTf+9j_RbH^LoG!JGP!2Q+n=#LiVK<}W1)3Et!$&s0wzt%VxyC$fVjnO{=RWY^xJm?KHm7m(SQF~7oR*{?=_ZnRv!>x1gG-(z3zL*Dwn`e zI_!F(vOHKjs=qmx=>@<4Hw8H>%d;H`-8OtmTjVRw7%ps|8r)#x)o94{WswD~l)S@`CnP1{Jc=O46>{0}0^zvgO55XtKtd3~*P(ZCq{SO=4 z@`7wS!_Di<69r*b03|IMfEH&)P*6pMk14iW{L?m;rBVtw62~LdB@m?xbFkAD3zGm! zf!5@lsi4rpu=5TLr0R(s)rg7w6In3Dy9I1k##ScQZQy}GuE&;_Amr>bAdtpX5GD2a zC^BH2k#16kR5X18!vIGgMPov|fn1)FCrQACfaSniEm)C3Yp|a;dlRHGM54+1@zBv0 zkT$wBWZF3OSr=Gp4jlkA-Ew02FJFET{z>S01`c&k}C!+g9QOT z^Ldrzo7pSXv&O}S4x8HlRu^FU=*l@B!)^O(xlpv~Z}C?_sM>3%WyoMdl~x?Xg+vMD z>S+aD;kwjVy|ZXMR`)A)nAf)}tZY29M%?Ms{I$OMg$&OLB=piI8{8$qw6ZL?d{=zk zTAes-J!H+x#k|KW%)|u~1z=C$UMDOeG03kr0fjp7lZCYafc&5>Vms`w-2wa8jp{8BPl08*3DoO%(+TlQtHITJ5vD^Xy@O3IO&p-n{k;XoEPk z9T^hA#&8C|$5ZlBVe|Y6Gcw}IS$gZ`_G4qhFU80kIp+F{qmwo3wi{mZ^#C?q7^O04 zrnh$lAe_BBG>v~yKt84*X!h3GwC92t&i8Y>+RKM?tJ?p_$%SxaFV5V}7tqC?5VY_K z+oHA!3(TtWhJ+*`Flz$(i}&2cx)@$wEM8wkNBfMB0R}*et3L1;4bZqOD!b^Q2jlx) z%Tfu^52?&en4QgLn`>1Fp4i%5!28WrrSG=VguQjt0E9e4FCPvha+Ud*^9#$tEJt*PDK?3H-04LE$+si9@u0T?RN=D z(_fqq{&{Q*_F;xT6^MCay;>Az|5B5cmxj0!>mnvchY7edb*b6Et$wk{~L6ZqHt(O>GnE=z}O^ zEYn8V!bH>f_9!t_R8F{|4~=I1|6}%N&{_|u5+j=(-~bemdd73d3xyoQ;dHjPxA&bI zZA*g}Vgmo4T0MdeFv0mN1HiJ-9GIiP@XiHrL}T{lE`5f5`~_ssCkHQXV=w$Zef(Sh zbEk=8J^$#R4#Pkaqu67iy|%vTjoze}$P-^$KB%v# z+_GMt(Mim7nh{+7;g4o`51^@UsS;!nJ!TO)n={wyMfb+$p0emXj~&l<&jdVUBa&XN z-n~5PgqYhWPMO3sS!FK@fW3Zf*aIcSG(8l%(d*JHa%l;{K)lCO9EF}7`isTRgo>(H z54%oGYz!zq8@su^OQG<^mbRWj!xV0`y=&up#84KuGD06JN>+fyv32Yh6PSh2^!bn1 zrFAGO_RKtdB(E&Z1G={96J+&zp4A)J3Oi?@M1a2JZ3NVrDwmsMCItl>Ho4cN!I!9< zPkDjGjxhrCoy~UVyB^saOYEp^a>vW_>r)$`V+uXiEq4mN>7&p~RI5c7!1W>YC$%-$ z`Nxhk^XRi*FuV!ys_+c#dqM#0%EiuwKM7yn(_@5;b|Bt9ehvXr)nXbi7A4A^|1L9m zp8T1cAF(J6^IOTAazPV|jLz>XkQuP^tOFPcuI)yvxAs}6*q$x^KY?@{lsroj@z}3$ zMrQ>_0uU87gP72Wb9?5Rc~k$?l!)iaXXl6Jk?o9_6XYNKjv1g=mXQVHanR3uVx7qw z*M2bt9eAEjv(r`{`o(F;vn~!-;$MAH)r1eL6ns2XHm0iBn{F;BD0N$I%p(qN#JSuZf#Ex-qi`KXB{YU45R$!-gwTfNJKf6Dje{@b@5w{6~VzI3+{t1V+ z4H})Cyd^WX@_GK!^)pJXI103J?>?Nfl_$}mApq29Jq*i4*AD0% zsbl`^xPnzfk48JTeBN;iJ8>@aNp)n0SO~<4j=BMQ@%(YYf z&HRqfQ$^e3eJZ(F%j>Pm;4*o~BpIwmHvly^(MB_&H!~{nxZ!LwlUR$)Sy_)n1q-NP zu^r|zT74uOmj4hp3Rt4YLv7|37gled#j|~`;&S|dK~rxo4s~T3tbhE^Ah@T<=^CR=QE z)NFA_&;M6npAi|OYgBxcK3B-$AEZC|@`HeuufVvh2JGB_+@~ORw=dv-WfRSH{4r?^ zmv>itB*an2eFQlx>Ln?xu(`2k;mkxp(z~}WXK8!KtkPBU6|O}lJEBK9k0`mwKzsM( zTV8|2{c9#v?BlcDo!DDufA36yV_0i&9`;DpPd^ihn@K3e!wj@PT0GG0W6mZj$xVP} zus++hurq5$C|ntA|53IFO=x*)ex94vx`&@1qX_TU6MZ9KJbDa0KAY{DTic0stt9pp zgVILj3!1udea*ecnT2N7wK$TG9z*J`QhPp}4A);R>)hC;QFa*rVh^3Co%OlX^g*Le zarw*+2<&*FJ2oF(5$}mzU>;5jRIWhBc}!QmKk7c)hH+sSIRgJ;00$r4oii0%J%hKK zDwK|wxiNv@Fd#&4@Jq={%jn@s+&*sL#-mg50VB9~G+k5A#s0sGP34Q`l=lxt4IdKE z3$TiR>hlC@;XruaZ|1S4T5{$azfePz)B@61sihe(N_&YcpT3vl4na!svMbU_p7V1R zE`SSnuF+fpk^H*m5C&c{XUg_HdfxANTha(4>+-hW0yDzPqb?`!Hm*M=9Y#U{)8_B`G9SS#j=6d_r8A8}0PS;EsBfw|*O171j33%Ut^7zstInUK zD-8uDmK}a^tLf3hRtB86Dv4z<(&Ln+`X&LY!>KfyRbU#PbyIGg3fL-ZJVU7nrvM$u zQ-O)?zcJk?$96vHD=GEIhu91ZjA22xM|snlYE_M9$Kgnp)gzVL?w{%za!S~>Gs!2r z!fZOJfjaiT0-e#@G#`WhN1jZslaaQ4p!TV!zkUv}7;+XW{PS_@YDk=t-B8dUjlP>k&_JEZ` z3v83`G*b2=t+a}Gr{5V~FQcHR5c81NWrdA6{a*fbZxA^!kO}Dop`iN;x zy}MM@)@`9*@+aTtt6C6sK>m-}Qi9iRYOD9!qImGvd)V@qnV>^mRL{c~=*js~Rr!ff zv~`xAOe7~tAOP$xN<@5c$GzS56-8z~Rl-}k1fh3~=q{DO%*M|0lh~I%!8S~&5vGwi z#pnRPxo4lX{~HzB5_~y_?pbVtMq)k+_$A!r$5;FkaT;etOp~}#husWi#3>+bN5#>M z*F$Eo4_CJ!z}pnEhZ*LZ6IIQ!MVo^tOBUqWjx?J%=h0mwYN_e8!M~s%h?@~MzdYnz zlU!oVA6D262)$^;v8>>BbL70GO*NU6bkL$W?cZ#B2%wC(eyw7#Um&1dB8{a$8Rsmh zM=`C3IPb1L;7SkJ%a+O(o7If7WBmtU{<~7T;3_3>(ur}rA>xztzA3lQ2&q2kcTp{Y zt?j`|e&pnA2u+M`Z~$YCPFPrdu>+7MTQ`3=TGkDVm>p=4nJsGpzW9S3P-n2=*@n$E zdm8p>XKg!_KNfi&O5}W!`gS1m1FaBgMWY6w za4=#*XpicdcKja^ce>A|QAM&)1(qaXW65r;k2*9%>|qk;gqV(OJ|ZMcHmx! zu3d85{!;RG^AKM0?-yyZajKFP|GvGP zzX2B~&$4avJ#{|$n)wMxtc(FUR9vIUmkOL!@`3k){<&wN{)JCRO_*>*cIMvKv{A?w z3;=j!5056rq{!f!5PsUA7XG_&vYRbtH%ZM`2av+0^W%7Zos1=458gZo>@YL`c;52- z@w9cruYLf)`c`^tq3E?b{k8qru-uzD^gRz${%l|9-pO;ycDYFmMvIN8&}bOW_&59c7ij(z*6eR%os z`JmO9eMelwAca%q(p&dNdwLt?l53+FG;wpq1gh_T<^>y(UvzeW9x~TKkKU=Z(>}gM zzdwmYUX#nWu2wr_*k@c%3!2QrK-!3UE*rASUGCgBDA4b|3qWn!(mt9B?)eT6J1?k zU44yI_f&M+<*h>ylVk#TN9f-*Er7`s!A-ykjUP`MaCQ(DX8q|?q^Reyv$ONNT76kHB5zldo6y`7xNI($<^Jx& z{{Ngt2MQfB;Al_MD`?aEy&tuL$$DN!ne{BeX$^YDepVfh)O-(IZ*q7gLnPX- zang#FB+R<`BZGKDk3Kb<-t+ka%I^hQL{T87LFfd?cicZ~z?~u`oy!h52qZ4#6YqG( zbIoAIY-=ue39L6M#d>PkE@MLo86vzJy_gk=;U6t1A&+pXiEg1UF43bqC&$~zn+(>3 z*~<`0ygz4}Fu~%^NDn~EasYUIW!epM%{hV6@B8-5y(RHB7sjc}7Fm_|< zTA$*+!guDu-AyIHST|(L#u<_0brK4?l|pgAiHo8giSzPnubES?sfe?Dy5M!91iToo z)9-EqM{_Zxh7Nz*`r_(6hCFv(uH>lfn?!HFD-cw+6Mm2)OH)6R#jFgQU@4r(76Ji! zxi=`LG2E;c52WmpE0k89y!=pvTGi zqU~Xu{_9S=PJHx(Z`#kB;(KDAm1nxFyQkDY9S)r5cF$LGaKN7Oh9jb40s>0bQe-_7 z!-h8k9WqNq4JgwNntkT!f8+VTI5VJ6c4&e-Iy(NNxB|*Oz_}MVEa`)C0X+db(Ukks z+djZWrGy;XmHpYMUkzRy?iQwfkHksqe7m!3p%Cl?vcL`+qZ zA~E-?J`z?&>2JlXctv5B&SG-)_!mztn+;ax$bf-5B7qA5?FNYnOHj)8;p@JjT1uDY z7$FB~tF*pE8-8(YG;6$qBxO4v48_9UE34MNv=U%h{a|X4Hz;H&$OK!Kh4rG*;svE# zGe^Ky9X&~aqyX^p>b`Ak906|n{70b&6G@}X8>819*#F|K095VYHSu4l5S?FnV~Gz7_442`{P$B* zda)CL;R5j24j70UNYZLBN&{(E zF%@9=r=K+d?q7dWb^Q~uapSO(o*e(%(?*w;YABtrIer<4hMgY0VuuFBC9 zYa*Iy&8^BP>jES)qW?@>=5xA+1f$#O}<8-UPZj1h&)OdscgG_F`rUq0Y!MHl)G=HWtl1oYl!Z* zBb1=L{)=gYifHvAPdh;WuGFed#n+@%)EbaJynzbHqWUlB^I6mwr^rJZPJPl-W7_q)`;TENZUiEe@xx%i+QxX8us7_;St4 z-=#?vii{*iFJp*tg7CN$j~cvjp$D`pS%drv26$G1BMSoa#<(k~m=LNMa@A?$!hl@| zv+O1$@usl&C2J5UQMECNs@~<>MsRaL{}(Hif;3x=FAE8PWO<`T)d7@xbY#Mt?kH04 zNdX|*Tw%-(v_O~zKr)ia7JS=o9ld`~_36)51DAT*M0pH@6bauK%htXydBN?5*MQ_% zuq@%tXCPdhNA2R**<=3yUAF9>dr<_f?a~D#K7cq|Jeb1!FMLF@sFteL37(q2cM`ij z$a~cv4M?rfGy|y@&=RQ?@R4Aaq|?G*3E;lTc)+FYJrX-Gn0Xr~;iCwr&bZlocxdn? z{>`v*{J<;u=Jqdi*@ z{6$*0nnXqU=9YwC1RiI9cGLTixL>Y3@g09>v3VJlFY@v-VkpY`{fGIpu+rwtI=$$! zvd-&;3OZBo@kY@8l}O)OoGMeGLk2)4s-G`}AMsMf)`Bv1E3PUv%x2w5VZWb#k5o*QgMR$Qfjf4lIN5i-#|KiuI; ztkXEpNst&9JpWO^;m`iaLmhe2*ZGQelAmx?*~(COvhEU;)gj;ACm#ABYIO{XZfOG5 z`vM^oYC$QCS>y5JgxuP4Vs!v+YNQ~6?J>CM&AWL#lZM(x<*gQ+oh2>!iXf5^aJz3! zw%atI;FqB~nHQTQb#a1kAE%5Li06FOW28}G(#zLm2Oc#whTsvmU0QW&58YaxNozB* zIzAC{qz>UnMWG^fd!xdsN0Z2vM}mx^9#N38dgj;81-GI)ZaMsuCtimpo1!D4!I|2p zyoDy0ajj{!Bec4fJ^BR7^o9$>dVoA)#0-63?>xf!Hi?!>l~!yT)G-oSphVF0hd_JAy|kX@L68cP(9?MvzHuARK3 zXmQMzmWWLsSwqs*+3wzWj-kv-<;Y}4mAJ}|VCK^=LufIMH|`9|t~L)fh-t&nzyjVS zy7|I-ERZs@Y9nsuUF)=B7(X#&eKNOmRy`gF%6xzW??~wp!;lppZb|Sj zbL17W)*b~5>3!pbAN}T9^=QY@?c(8rqhRh%p=K1qo+B2FpRQuVb&=dsx#a;e29B9M zI2XtHXlRAkQ4@TLIGS(4;A1w3RW>=M9f%*Sn9iqhPSn6;u28 z{fg}px>K9q^7)-O7Yx)q#(5IyP10=_qqjGj>{$!Fj7EvxG5{d@vO$n zdZ6}4(qzR=$cFzfrd{I}@|KX?jGI>mPlm4#{WI`#sf5(SR*Zql5l1;Yml< zhQrHXg&p)Dy<@oecY&WMm10wHQ>dIzn4m~ zQaUq4+U(Gx(#^N*Cf|~REZ&jGgYbD7KrBhGM1GYIFSHMAe~F0RB6wBL6xSBxP17Zlj1%cEd^zL-ui! z&3X)JB%$iigH}La&*IB&qQxfr&?QE_WoD#U&u^#2`cXgQ1!)#UGsK>46UN5Jm=#PK z_y-&rT|{sY$@{K0)NRcZbr$={`O zr^tkN+!cF^mEq2;^5pJ0dNJGIq(g$M)59#Q>4Q@*-0^{*nDqeMuG)23oSyX5xTP`r zHNO2~Q#ev-Ya4F))ocRe=cqO1&o;^MXWjP%|Gc600$G;sCNHM<_%kyzAMbMh zf;X2fyuE?qCs(DrF&Iq_81h`-+?WlQoVYz+DrIoyIO{$#rW&6wt;Rl4iC5JVuU{>S zGQ92FqkkAr@k`vzQN)u-2u}-YHKs)5JDN+DU=K}lw1>5s(K5g2pz zdoWZpyO_jwlgCXxIR2LQ^BP}=2wwP{n+8Y%y5jLG%)6+FgJ4V$$pVlLg0u2Dkq_0_ zgy8YkHiWv%Y&9_xEYY4}=T57}S%iRLn!DQk^4eZ;x3SDxVbNIoZtaSI+GO-R@Dq3$ zKEz}8mPaT2;BKzk+S)rR1hH_|mPvAZ;|+@cxnD|b_14|(*8uWQq-=78tTKZh-OlW! zqJR-!DBh)hrPt$X#4z?xJK%xrIRM7J@G<1ea@gR{j~IO;>vt~|9*h)k(hlCRN&0b( zVmoc#_WYutcHGuCQ`H_TqmgkGG;9V5)sbEqrR77C8GEpu3h$=I#c~$MF3zG6^!`cM z$ja$X5uA;8u|wwin2HX=Zgh40eCWrSXI&p}pXel;Q|yXtEi(|{=)nCEP8%pi{fNca z!ZVwSEZJ;%1~le=kEZe1)!X_Rf;Yq^o98SF`pe$E4%3MX5)YtdrV zW}_}v9DT-eklzH4nyX&ld*&>}cFkFRXKbH#)0V=&)N9P1{z(+;Ge3gvG}0$%ti5$Y#T)O{v3o)Y?2Jt!}XNfPe6?dq0&f z;j3|AWE!H!;Yqb@yVQ<&#cUk(BQnl~s`JRhhx~k?U#Q!K0!Yg0FvXsAkK;X8@Xp9N zu1{MG-Fr%3e&%#t@Ou8B6yt=01T?rYF)_PtB7mRI!jZs{2RXbENC#`bS@wRsg-FIE zbpkBtbc3NCEP|9>7va+)D&i6zqCDSFc>=NqNxquRUg5qC!vwI=pKx=BUSJ985 zjT_oDFkj`LGVG(|%|!g=K!e4X_yggUnDUBJaSkQaF{?-TXG`I0*u$#E{iS%xv>+f# zn|%PbMfDly(P+gf=;W`%Q*1sNjo7%gXWl1&^Yd42TY}U+5Wr-r*@G7K-_Ggt z>YEJJ)M{<+e+^DHOwm%8Q_fL`Zk0|!YTL$$LCY(ech2;6bf!i?EQq0{Yf=BSaYh3p zDJJBQ3d^AZb$28!xNbE1LvrRPTWfGzl9lVP{=tvw3#u7zhP0 zQT2eMro^Dipps*2PM@Q`F3HYfkRdZ63L=0Cky%)xBJ~|g9{7eWpjQ2}sHHAxZU19@ z#V0}m-H0(gH7)sWd;aYjp2FFac)?I(6HKsw-|B)gQ0Lk8W+IH|OKa-VN^nm6+cuoN z#B$*Wnhhw%Mi?zEAD!I?))x%3Katy1J0=+zjA>Qa2X&4vpT#{DP~8IfEXA_Xtd|r0 zK=Kx5ri!qDUZi7Xl76|(kX_FO#eNkoBw9jw&aP$O+1Fs}K4M_-OU9cw;A%&mdE+;H z+Pt`4{BZgVN^G#&osdl^4tA*4pFhH)DR*&>HNN`C_N>DFx&A&Xlc>u)bK%^3;48K}m zUA`ThPr{ek|KG}2VQM)f=cLuGR1b0GbAM_trBX89{xQ3BvH#Uzll(Dh2k19Yq)1kU zWDrVJW+=4YsFWiU${?>`qfbs@TZX zTIi%Gfqcf2VOP@>Ne-$CQDjjYe^-IxEay?)*deY?Ce zS+*Tg_=mGzE;hQz{;!$|4FNKJ7BgSHJorN9v!7ccwV%-Fz8G4U(yl-nM>9LLl%O6X zf(|z#B7rS2qitl-TXI#w-!HoBUhu=rwlPA9)UF3hp}4KecVolcPY4$JBt-_iqk?H& zces-Gi4~7q%yI&sIO*)^dnH^M^|SegiVBFoWLU&=a%dWM4Bw+tpO#1(t|gKaZMjiH ztF1~Aw?{QO*hn&vkffoOgsH<-E1M(Cg13VHjYFY&I!U>G?>mO=trMFkz;I(XYM-e( z&hJgEJD{1T*7o$qjtNwT`AK`%v1jt$Q0vHr;I%f63mz!gg=QHZRXJylZQxTsst_LYN%)Si&>ro(~ zVvq)g;fs79=mvNT8Ub9~PDJs-Pwu>`)poI`f&z;};;(*EPLtnaI@Z{brDqJJmDM)u zSURYe{J3H*^o<~uQr1I5Q1aj9Be};kcK$7<|B5ADE@BJs%>~n^3yePqX&DTdJ!cGk zwNP^kb8nFVt?y$9y5N`v%j_a>vh<)Fu@%7@ZG6;H+%k)~eW*~Ll6^965m|6LsqF}B zIkM7QV$Jb4n=lbY68Ou&Jm^qLb79;J=ErLJT4Nh`9>^b?tl%$-hBw>|_J1KBL*^9R zZb$+K2%m^LxFLte=?j(e%H(U9Mrj!`BU+~jBr($WeR1(6>z{;&#`1}6m3A(4B1PVO z&@Y!}eWMxFeZ|n{(mjXeA|3dSOX}lG4nYMGJ(|?F+&b9NJjKWKNjS6EBr(1rjs0h8 z2t}Cl!gci@W>Warb!hBbl3ZU7{CKC0Y$cd0iA6HUjRixJC#-w5J5O4^#eeWN+7Nj< zL^fm<%uR)gDK9~-d zH{-PpI@VRiiGLKLLVYRVw3w!T@jEJKpgcI@8OJyL+A$zy^mZiSo)n ziUak3;U>`^^Z%<^&vRasMa5}oJ6ksA=NMO#eL=&6n|@5RWkv`V6sB83sJ=J$N+x>QW+KfxZ1eUDDIsgFjksNlW@uIr4_41`_(AeTuz)rhW3jhQ6M34^GOXm67r%m?THRRG?N<(!#+%adj*%0;WO8UXndJb9pUc6fwNopCNN6J4+IsQX9dDJz35 zx4woQ-Cm+d{q8(YJ9bH2>%n@SB6=F+S?Y{N!Z3)ABD^y@z7%f_{C3=3vP3l{QVkQI zv+$~s>4F7t(^#RitKoUY|6K23iCPf2-1U68BoD>BX>`WEc4-sGWXz4jx7Nm!i&sRK z25&J#7*ikP&&qE`F755UCS5Ga$SzMz93&qAV;V53Qc`EkMom}OXpdATP4jn$=M|iF zuDzT31~?*0phWhL8gbVqqY^<^M1GudkzxrfY%aA91b+d{K8c8k=yEzWvc=W_FI*iq zIRIvQPXM%vLLws4XfBp?K6|hm;4NC(^>XOeaX;fAU5Zk+%Lh)|_p6fRzl1=~_A=Qw zJ*c)0B10Y$m1i!vvz)#v$2ZtbmpEzs3kY}!*D!lG=;zCvzL>iA6}oBjla)4`EsJ?X zeN@Dqw#k_D9NTlyPQXqaY8sL{{?~WaWD!W-!hLwohYnbL`sG|h-hQBw0@DNt|D6dF8_uVqx_aWj`$Cxcme8qyNN+tZ~=|iE7VWQ=e zo@e*IKhzWbDu$om$1-~hC~#n-7iwcWV4S^I-i2l$tQmR6|WPWSazCSRFCzJy)ETGNN&F|!sXw2K$zzm5J1%(`; zjN67Xd&^KA3h{ftq^ZH={yk)t#jxP-zX&=0v)4rCwjfRh%4B{J$m-M(s7g>P+s#1G zqA8TU$6788T>3l-60jG15L+hyGUqn=L1_@LwEP{NCJv2^^z0zD8=deGk;b+6qMvoW znJ2Ea){ra+KjNKozzJ&e#!HVbxu!q4@VzgKWUijx{WAg4FUsX8WnzE6p^eYeu*a_P z<`_Ie$SN87QotgSP)mC1N|Qpq$MwztY7gQ!I~6E_M49{eD5)E+m6zOR_wBFX173pGEaSf=6EU& zG7xi|fPGRI&g&RJ)L0LI2rI0kLqf_K5%%~hNHLE^Ve^7BSvkc=BDR{G*@o%ius){8 zz-m)zg3X)CRObeqR=J_}8=}+2yndFF&vM%dotnapmItym)3dT{EGzL${ z92&)!{xvQ58wcYPcOiaARLOpdovK}7)T3;6iU(Oh(wr7An#|#8)XeBf8e)q|XH$Eb z6g*Rk4ym*}qMQWn&pWhW?x0$7gX-)1msQn$$1bU;aLA&!*3PgWFgjH5rrC=|dUDy! zHy4UA7InMljE)&UmI@7xjrFSgv$`szW!@;lxs(@s6y)W%S)M0}v3uo9R>dRwz(M6G z=+ywTI56+_TPlL3Vp1Z*@uM7v%@1MJC~gl_SKDDnfkfDz`(n!{&U0?|*wXSb{Vp=b zg5553q*-|yZ3kcUsZZ_OhWn~Sj217JkF28u8ad@!;W=&TWV}x)6(-8c0DE z6Lyo4BlUHurx=Bq%m+0eGaDf+(ip`1zHjO)CCiQT;``_JO%oeSOl=B?*9TbM?y2&# zcAG)XpNHnqfue8au}D_?Yh`;%H>+xiNE8*Z5!;b0>xCqeTsV6@y-HP z0=ud0z=6bGmx!J^ct8#*ue@shtWJ4kDYS`G9Y)OyV~|7C!0PXloha$MbJ&Gyzi3YJg4%RwFd6VkLAEVfR0{lN^%`N2AwH7 zTqe*nq3%Mqq$hNSGrz-z8dt0 zcg+{B;CE}U)UOaetv6Kj%}Z#cZB|~m=|0z|rMP6%6fvM7xS80wyC?fUIQ`}DztZy` zh$a^t+PTnA0g8SUyKhrdlY>C!7!>Myv8^_;#TzXj7#N7yUsQAgw3%NpQg0l6FpD{S zAy&3lDl4t|HBKZ^uMs8DIxY4@h1rSlF&0h5)Lu-kxA$uYI<)Hth(=s69ptHRXFXE) z!{Tf!^>ttHJ!f%lAb`wKcVG3i7oTNU*W(!7tG8*QeTz?O2>h0v6eMkq5kNGX8dT^< znu@L-pQVqRL*!%5&PY$n8mUIxSqo)I>9iOMR8m>-q57Z}vs>?^q4ALQ{Fe!sJ$G_+ zIvx2YwjcTCRQrGR#>)ey3IJRqy&|V)tU9iMixYwqw0gJd^^%@hYxjtvMQrh6P9kLD z-TK4S&V4zW)Zi9_msHq++-GF0eJX4ZF{(pUx6ppw(%cFh6dWX3-DbZ+G?0fe)rt4w z%4?xxmfO6Tyo$6|S=_TF)`IMbe_KVPgYxvYd*@85+XeOUonnZzv#$UF|DdRyG8TpNG%iS9_TlMa%^{y1W9t45Kv6R^q z?{Gd6)V+rXg+0=uS~S?3ei<_~iOnkb_RyA))oO09up_I-_V z5Q*4pM2`c!*~CI^^S!BldNhUrE|+$0kF{0T#dEKQopXE1LcwWnwX6nK96B1zzFQyp z+28|YOcbgI_f&7um!fJ}nygwZN!)5r{rOG{b%N+(CDd6Hg6STnrc5UlUvPQxmI)1h zYBwdF{~DEyWiSb|mjlD8VD=4`Dl3HMOpTYXHAthq@PDpty^^;XZbS-EoR#;L;OGvrAJHi-RJ)ix1A; zCZ=P<4*|ATSdxIW{aLMWCbt{fcPlX`Vl>S^z5P59al9hfo&I7LdvHnEbd%QmQeD?V zE!|6ZKm$HI?BL=Q(PZG++eWhinw1=u8(BAs^E5SWGU3`6ED=^Q)dB`q%&#KnuhW3+ z04(eWq#nH1PIOG3C=grnxA}Rx0ZUw1)UgwfMaMlTNH1Q}V7v0K-Ijmy?J(%WhbjJO zZcJejk@eeK5+H-X%*@W!6<-Y|Bq32e@2JI=4A^n|lwX2=8S1bRrlh9UvPW)gY~U9; z;c|NkeNEv*P)?pSfcL1oteGZYR1?;P#LuFe6(K8_2&PWs6JSkRBi6V ze(>Cr!uZ~ZB*bY296ClXza;;|Ah&yo2lt#KdZ`YlwMh0>ivf)37qq>Kw0kPXk{``F zj?TTHTW1mtp{zcxM_lkS?gUV)* z#3;&PbAri3ltZe}L;kn->O|v1RwN=pR2Hka%HhHXUlhPWi{PRU2{`)~qs)@NBL7?CoXR zGWsxuxBVmfPwA#YmR~PluF?h($5xN=nO@;Tz_m8cI6H#HNySRu3A5J^_{uApY1RR;>o5Stb?x8!QC<{t)pw! zoVzC`I29E7(2GvcOje5Z-B^=(t$7UyMpJzIz=yHZ`2st(wK0eT_eiZ|yQ9U!u{I$2o znirxzOImMmkh6>?e<4RB{(hDr(FVUQK}xJ+XiyuEw1iRo4#x}W5n)c7z|Y>`QBs@r zSr(>F&;t7cXr)6gi)8V?f6tE-Vay5R0Go8D;=OI0Q%f3#+Vo=Rt2TIkZ56r&Kc zU!_oWI7@}i;eCJ*3m59jva}3g%zUO_5=arCvr@uT`=MX+wevfE+MEXSK!+qn#i75VFj`sZpN~1OR3^%W9W#v+?KHJ4^kFQ7e;@~t& z(m5NewiGS`>Gy-cBi7~sWJ9VyLv)Nn8xjHVzG)Dz?YjjenGf%9J7`~41mq0o(d^u-5cH9|Com{;O}D|<1I7T>OI%#?l5iCfKY+PkF4>U(?T-x_xT z{b$I2_B5AtRW@maub^+BK0ke1P$7T=+J zlaxnRE4d{PrPc(+S4>ip3R%RcR*wJ+FJ1NC`t9!UnXhF)LeMVnSHG_NcJE!V1NzSJ zMR9Y|)%lN-tV6WiJRmR$%~p61D~D{7d1f- zc6Qys>ycrtBW*kwePMKV(tywD5-~lSHU#~x2tmG%KiAf|lGwYXxO?=cNqsQl2l>(Y zYc)kxd58wBk6K>Up4qXti`t~}@lEl0xK>RRHzvTd7?QiU|6?!y_ zJNJjwU7tPiKRpw!I>pyT?f5E-JfT*b>9v4e1C)!Rip!W&dSSDJm)8a^P6;R8Pv|EZ z6rB1q4nFYn60oUv_H0RZYM~$Wf+Q9KNOXY#MV5LdDmGQkuPvMqN!r9aN%}3Gh;yh; zg~IQT27vOS^|P8DI+y)Tq*r>~FppO{FM6WYh2EOYh+6Dl{75tC#oWSzB{26%Pbc*| zPs}+TX3pWJz`C3Df71I01OSfy)PA>X+C?W-Molb4k?id3HOJ5rTb{>|ca zp7oFC>qWK_z2w4mS$!|Vrk2_&f5F>Nz$I+Hvn5HLC4VRe`}}ye%GVm&RaJ>MgS=7I zNhdT?Q_n7?vd?KPrW0d}lR70Mk~{s&9rUGf)b&s0ljQ}RpINmFVM(3+Hi7!vm%lAr z$Ia6FrK`B?lYaeQ9kKXoa^ zx+v(}A=Oe|?gK-|`*%)-IB|VrDXc^l*3+!kB$In?yAAt8xiZV=L&bzTYOBqYlxRO_ zqF(dZtmX=8%ObR+Vj{#m|_DZw-5v>F*^4J#qzg(T3vQ zjPde`Du`s+@RPavc)k1S9X1$dEYi(O$9OlHg6a7Bx7eL!@<(?RSRKiwcLSM16pES~ z51p0N2p-x?8KY@(2PEbH!_-?xMcKYzysw1_2q+;SsdRUabPhd)ba!`mcMTye-5?^>`r)xYZFrjTKF-SRQ7G= zz3-e$hx!rxab>dpVWWkv2J9hDt`Xb2;o-M%?^UC@y5?;k4l>wveS`BZzw03ZTId7r zu19?>u-5+*$cL#@tSO}tvr+29%j-%w4gYxxP6S;2SM_{yZr{tjn zG!NsIjRP2b$q$I;teZ{Xxz=p&L;f^Gv2nWLw+m5F0D)cSown4$+nGFSx)q$vo&y;J@xQIl(UDk*YSs-E0EWj15x zdCBXez{_&awerhDy2H8qjFiK1pO->@)_hUDABC66(i7;1wyo&Z29tAD;(IGeyU3G< z^$O^NKG^ke#(e5h*4l*9?xIo-oZ!|P6Ag^U3BQaNo?ycZ2~K=FkH0qak$%zGU2bhB zqI*0_Y7X{F(e4>e4j#~0E8joYXU8LY?fE5bV7z>|$04Gd2QI8CqM{<4lZTHl!xmj? z;g%Ko=^#*`Fe5O@J+Oc$^D0892Nqxa9FMv*+(yl;Gz1I)`z$%ON(?BR$@Iwv_&}t2 z%cX$WjQG8|$;=Q5q*U|^S4F$xA<>&wW<%KE_Fs*|@dSTN0~)*a;v z5~APU^asLHdiybvoV=H`IwMmq+lz>CHLkhe)$jrn zu8-z~X@EhdVWzP@G0k3;+29(sLvhc9DJ+Kk>3pM1mIjX1X#12R`3%zC&~@?g0%ZEj zzR&1v5!rb!Zak5}bs&3EZ`{0p-AIbB_4WUIW->+OR~=m3>>V69JluN|3;4+7jp;Gt zfk^o{am6X-DCZ!&Rd?6weWIQ<;=o^VKE8r&%L1<8_lkzZ^m)@dB>$6Cx<|`WiFr}* zHbm-bVuzQ3+?}ZR32F9-QrYDH1^>9Xx`KF&{0Z~T&CL(e%h`8(rVFj!=|e-Z_t#Yq zX%^r64}BjzeQxHLb4zIl1_n}+TOXI8T$hFownfWb6!uhie=OCm<>nG)60GDkd*$GD z^oVRL-`Z|@9tLYxIXM~I>-Io!Xs^bk-{$o-!P8Qlgi}bp<-`4SzMQUv9U&YSq0Q`8 zy*mBnUkl%mQ-+^W&r_v+EG#A5mu6Ck>O(e}+o#|JMvD@WXm!KimXN^2hW{nov|c%a ze=UnUbYg^BMFqfv12rHZfjssQ>}m}Od&!WWc6CwZH&rT8WB2v)2g0w5+*1xxUCniM zlDGD6%(uML5W9w~5BVPgFDW{WS`iPGyDS73_^;>wIMWmoCY$OTN}1aC2!B>VPh-5O zCvw>dXodfJrPQDkc6iFt<)^{(XRhW~PlDKYBfY-?OQh>Z`h9uc(VaFS_Hp99~w9 zoEAl|y}f=4O%c@cmf1T~bdNWSF;+|*TtOC`-JFm=QN@mys-;2-%`lO-yV=& z_k?TZ4@SnF)f+>q?kBFL-Uul5uCaht;1xUMqgxo%{n$#b07f-pg5TEVOt`gvm7$gQ zYNW{n89!Ql?~tUXEn6RQkE|A^l`gtJ{Z6ulwb^-GTqA#cYLPHVOF*ie3&Gay&##zv`kU z!f$DOhs;i8rFCf@kf_&gvJ;@Pn%5Rg*4uoD$Z_4!AL?$1t;d(aFrTWfk?@Y#mbeYgfOqYhHkO(9d5d8QjaKlQxzsm}{Y%*|yfCbq$bH6EQy zlG=*qZ}v@&=ocy!R3F+hUD#p0*iQ{Ta_V2T+Ed6v_$W^zjpisQMD*q_-2dFv1^y>c z?^X|zBdOPez@Zp|E>e&4Vw%0WP>YWmt89qUK14rmV586I16|uXkI6h|rd+5_Rp_ob z>QC}%5!xgJpyYtJd3<#%-UW}fX&OZ$s$CE9fkDz~L3LaOzuO!E3_4}z!xp_I$7iDRrV6PA-&d@c~`U?-Q zYvc40o7ugoJU;K%zYf+k>-`Q!IDDFNim8Zr1|xe2VlDGYw$IpBttMyexwfcTt@2Sr zO?ho&qCt)Yj0O)o0$;f^pUuwwpHchaLDc1{kYLM4$yEN@7qN64h@RYGt;u{XnfM{& zI5T!lPK+$4#vfP?Gb1ccXxEt{Cbim2%gpL&(uxeJ*e5i{v)Hg=c3#FGB(E>`wbO5Y zKDn#@s@JGh;=ZuJtX8Hz_?e+}gZ@ewNo_|8V`GVt^x5VKdb%{(=M?AU zEdVLuPFiGX@kj1MiT7QuPBEzuI{++deZ?_o7MBFiht9-&J zt&%e79=risg&H`s3p9rGzaV8EeBq1r^g2%(+P97rtbkP{vka{x&SVP?o%DA2Q(H|7 ze_#zq2t)#Hxk)R(IWo=}tfGe#k|3T~!G|j=WEjMduVZGUt6O!6$LkvwCCVQ%}K(o)N1rz!-SgXy3gFSb2?IK@>Geag$_P^ZI=@sj# zOuIL%4WvBp$ky0f85+F?YL?kQs9qBj658!fF&r%ZlmJDI$o_?5-t}7rm+yVkzu zx4^Fo<6-AsfEsVUr-FJGFaiSyq_l`YyR zRZYJU&XDn;x^tIBky@pkNVr6tZbRT;MF})huOC;2t**hC%~77A4tq;EtoqYO+9F;0 zXoUaiKD}7&UJVfh(Z$Fj=c{&@A%v1rA`@msFm@%H^VfkAX+7FvE0wwb6eisAcOj$B z2j+8)o!TlJm8A~0AIR-pIii2sw=!sNsZE=|7sr-?7wRi;p`oiJ8PaU?qUVi;t=NHJ z2f&$vlg2;D;W~jF|C`r_@6aoE4dy`wQJhbkeBRif%gZmI=(26ul$($h4^3{zN&6ky z^KGYVQT1DhPU1b_6J|oES8^`|@c;0}0KTvr|9fAmh^h%iWGz8Kd5AGG>nuB4Br1J* z)wc#U^1PQI2;b%WC&(S@cSl?|53iQXk>$nJV?#HdI?|OXt4>oVpZx78nBI19TIeCh zpfUC4#O1)L2aIozp z0xoXbuo52nq4x18D1U`sD({t6$ZR*kz^?Rf5G?43!dk~uelg3p_m$32bqfbvyT|HD zi3N(Zo9b%C)phK>G8BQ8NT*HUC%QgG`Ustki++*RT`I>|kudOOa9A}}Q>UP&S+wMf z_$wlfeM#{@uhr9DH#T7tZ5gP3%$AO?8dTEIDXLR5tT&3|umTuH*4`N=x7BauO2+iT zWOfIdXe}0LV{`x1Gh-+!8vq2y@$I}8^dE{N(R#}VJ2Yeq5>*iSphbg_4V7c_b4tlz za&=vtkZs?&PquiO#FE3Tma=*V8?~{rgF0yc>>3;%{+@_0$we^NyC` z#G>>5SlD|tL55&U^-b8`52{_w?k!7nCt`#xi6NShc}x+Kt3wZw(1fPf=cPQA$Tq$_dgG%eWD)K9l!m z8bma)$N?Yo%b&dtGJB#Oghy5onaCMo@O2l}Z0@P>7f~jo+%?*biH9+lP)ZbKcoI=PhYHJaRSgz5v;rR6h`sB{+0a4-nqrZV1Y#DjMQ3Gp< zgXh&@Q?pd7>}*veK(X~tH$=-Ob0;lWh-0%A1H=udSYX6h&pn?Dz5gdS9v1t{Nb-wv z9F#^r%1LGda<%#~Q_c{t2Qi&0b-R8(@O*-O1SjD(af1ByBddN=l3s zP3=oruhSZ?ewdJ%BBShJ+dgvUA&%Xyo3DExjkUXwe+UaMK{4*ad^iOysMU9o^p?g5wpY5?z@53F89aVF!ORdeQ#FDTfmQ{MpjB z|Gv>J*E2uRS~O-wxDtb&$`sYmnk&I?UP~~WC|sdxeA2^VGMyrH#@%!i_pscyu;%+P z`{bttry~wL`UQM0N{*kd7#D478duHt!F#-{Ob=t`Y6n-~;Y#~{z6yh<_@{CN0oN3a zeuw~ga*}epJFaQJr;{Y4l?ZNy-L$n03EZxU++R)i)_D@A576*0)D`0@qo^k}TKI45 zrqegOzK$Py>x6N+FW6Jc>f`vj1o@o! z(@NwB$#LY!#Z_nElU9#*$m!e8g{B0N?0-rYv} zJ^6`BB|}ED%}pJvle=oQI2}UiRJzM6Z4bvR4;!X02|0)=E7P=erPw}?(U>@InxOvX z(PNGUDNMz6d0wW&EOj$cBa_+);B_#erGdy202 zY(bpj<=7BCYAsS@U56FL!13INU9KvacIrzW;PQF$a4PS5>H9E^i?K_A`)-;B=j5xX zu7^zQNzpTBL{0~0vOZzU3z7PDa2Mal1R9Or6 zVQoBLW0Cl-VQg>DqI37j;|)E9>HVQ9K=MuZ!{B8i`%HX=*xKr_-xl4mOIu*?sCPPP4N0>AKw zyvUQMRyWe(c@JTU54|Nop+{|Oc`dCwoVw-gHh)IdZ0YcJ$=Mwa6hPgSe5Q)&-GxTS z(D@&>j$U4njY2^QWV(Y3V2O)0ZF^E$8V^+m-YjtINc~LFcPSm+g`x_T0-I(ZpRA3I z4fwsU!vY}!a`T1K9KN>qnXIJOS(`T#-P6;PkeFB;NoRv~+bPZFhjiW;_gFz?dAR~@ zQtOxr%?1K>5b-W$M5Jqk6^I9miy*oCB@G4WV*(070O{il0IXq+S|;A!ccATV-z7aM z3CHJljYO$Jm4};K&(;>>jeGzDm zOv~oxMcpho0-02^xYgr7UhX~96x2k1D(25D8lqn(sEL;~_!Mm-KW-pis%VV;wE&tt zy*(JSl3?Lhl+Oe)L$+Xe8C=Eo3UaT6kvmA+foZqbFklA5_Ba5BEJU01A!R^6jS_lm zKITN4r9hedYt|}RILFUeoob|eeI-+_C^a>eEPF%%%a=fE@$l*4cxy8~Q|%!fKJ}Rb z?U#$yp8dOKe)WNm*^1v}aKvzmE!GX^#$U6OArqy`l?@&Eq<5~7wUQ|GQLnS2G*y13 z2D|pc%A9n{CX}UO`Q2~7hIK;knt%jOFC8aKqZT6gX3S5Q4UJt%0#5Akn+&hs{JRI? zWax|ydC?aoMVwJ(|H3jnlRC9DWbHkd^v)I}K43@E#?bzZFlI;lG>4&rW0pqno~W4H zFkPrfqHVs2@7p%IcK|7QI;>g-;GY3GmS39upSrERxzowM%R_f1Ih)}lR_dcwj-+j( z994e7z?f$l|HA>0B{pZ=Hs^m|YhlK*;EX3n*if6LwU1&uDM^fR1fANy)JEDzuQ>Ok zx)A{pjarm*B-)=9f&?eA+1ZxPt(tYxb!t01E6_uuS&{)29$_pFCsD!qTN+Vf8BgLo zZ!%-Z&LB23Pxw%*lYBju*q zTzp_gQh@d#aeiDQK=3X_yW_{!1N!b0NJdx`O(4f*mwe(=#%%3JA!5 z*C>FM0C&E;FyPd5L)8B^$uXuF8XE@#^IB{=mu*c=O-Dz^Q&dz`@C7vu4Q@5w8bJRy z07QUB#koxCNr<^RcwnzP@OUkoGUJgJ7@3%a(0#5114S54)oUfjl-Y#^BS%O0l}#l&sa>Gn4nxM1khqaZHmmR z-woQm)s~^U&tjc3+nGmIa8yJi4vOPMd%W8c)DY0{cbct*V2^9T;m}Cf`O7b_QzWF5 ztRJ{y!@ijDDQ(fapG%TPy>m;w-(y3N$kyeC9<#%zUGV5?aKv)77(s+Ga=@Q1V^IS! zb}p{oP!gZM)zwu53(u~HN6JCw|5m5l-U7Q`Y)PE6DKb5>J-%$BM|Z#cTu%J-y!xz$ zP26?zwAlL!RUFKd41n2sAa0;QstEf!@Qv=G^~vnCbsABufn0$Gt6XA2L56riT&@;E zk=Arn*^C{@%f4vn(B{J2Mccvrmw}jRVL5!D7`uiySU5tMMuOi|pZZDOuejw!i=&fe zXC54}l}5d~XMu0P+{kUIFf77vqe*57 zqs~W|Oqeyb|KrSx> zlnIR|uHH>2`RTZJwNyNLxGck+5B;cWiB!e`cMzgVSEVs|-Cw}oZIJ8)m5_|pG3^`i zonN2UJyXE`QuQ|xm4HU6;L8N{rYvnYTCgwyUBMe9M!uh)_#aAp-(iG+p4*CC7Trl| zBMyhuzPTT9BXL9dqwJ%0+fEXO+g{dZ@) zysLis-l=4V_P(HXw8V}a%Z18BpLEkvdt()@75x|)C`aGd@|E%Z+2 zEGhViT4I(mzN5>74{?5Caj7t4%Ey#}oKomsY(wm>2XYQ+kT9m`JrNe_52K7ELb`ck zAZkR^2s2&?B#b=Bv`t72w%D(WJJaiZunY|_y&b1Tj&EK-S*yYn4hq{0V`HmTrd5HJ z6fDVlgR{2Wp(UwB?N zV?>cJBl7=HB()Yby1KnF{k{h|GQG=QO11~!pG5m~dr>>q1UVmDCs-TcT6~9xmgiE> z)DQjo_2kh8^7j&N`DD~Dqcma21Ti?An-2q?o}JLpnHV;XXg7|+i{MKF$S#Ec@ETG! zXijdMRy#|dR=Co15dJ{EIwChxb}H+pGv6xTOm}lcD|2vjGJIh)XQa!D&xJ2Q6lVN7 zoDw5?|NW4K#hR|gOL)c4MzZ#uRIuc@OaSB{5CQWM2o z`eQmwN*yJn5IoL%QD;9LUv>tv#9mlC$=2`|DfOG@(+iOm#Y>s16$fT%~MiCtdJ5tV?^1#c2r$VUC)y%c=UyC$<*A!oPp# zV3+;Bv<0sO7aciE>ED@Ii=W$MgZ!*GWlNMh*oS7QOfHNFpTrHBG&*dcfFuU&$nFNM zTteG_f5*ICT+YAkE<&HESxP*?4se{Y_$%YICP!lCNLr7k-y zvI}z4&hlEPB^IY=Da1d|uQ=ZvozO)s5>O@&TUuHIHRulibCphay>;vL4&7q`aP6!p zG+ZB#%0p%OLVhaMUi%_xII?or%Esmx07=tcw@1BE;vap^%5;zBm+x`GZL-M~@JqR4 zW?=H@{cbt^jXn~1@3~f&^c`;MmbXtI@uV9B$=WCSF0OYDo>`uuFt9isRGN>Rh|zdU zC7+r=ZVx}|7xkWSmm6S`i;sW)#utw}tit=kud+}vD}7Bu;QGzO=E!~2?sQcxn3b-t zdBHdo1RIZe2mR$-^6Hnftf3V0aaJe`F}&WIRY*_=N?Qtb|Da6SVYFbBh47+Kp(vmp ztB(ClKeTazjru}nf;`HUqYyJ6=Y@#M7}_&7m+Nfq5qVXfOs7IyJft+-*ZgnA=GRe& zn03|}j$R?lQ+@d0@{Wa%&vSKY1(LzF?3(FnET9t{x!@|Mh}_x7+wM$=3~^!*j9knK zCIhXB7a|a*b{DT5?d<)$xJ*MK*RM}e3dvYFFj1cSlOj{-lfDXwTO!pfIyf@Zb@;7_ zT;OkP@r9TALlaW~W`N+^%Ymfa{ecWe7^iV(&(5yI4DXcYR?pv&l; zq(@%dUEw++{?E4x%peqeRCY&%QBfxyVf+mYk$(YkHcWpI{v(dNbM$CECZFH|J!sn2g&?$vp_NYFUPy+RUQ1HtTZWdf^dLqL2 zbqr-S^EfK0YsIt*obFm7_LSVOa!`S#PJN2zHK;QuU0=l`b1WMZsh+~;lE?S10J%6 z3@>%;g0Ms}Ev{ZOBNjpvy9{=i#Ssb$85|G;nR7)QRd}Ya=77 zC9%i!@v`X8t?`h+_Q0VrDwi4@5$5*K5svisJc$kD&@2@I77ZJ*j@3ijM58L(xstF`TpvO}! z?6cCf>MD&mYaM?H(|vSvb>>;lf|4kqkrrjRCoz0GJVPqC1Ov_AV{iILnfW>`PNhy` z$4);=stl5tq8z=?&qc=2-rmzWYo}g)C6%jq{eSU=)ie|XLhWgZzd3r(1AspTxy+8IAl8Sz(G8yU`B>N%m1D-W|igSf9h-BozL^`>mzXuk_N|oc=r5BA9Md< zXhW8fku*VOT#HAajmRke`>%y8ub+_#_AJXuk|MVs2_xnCdCVJL%;YUiDh}e0H=?Xn zcbd&e^fSYjdLkTpU*Sb{{QbhQ(qPGa%odc786o&aDE~AjNvM^nT3gB%({A)1KreXA z$^a}aEC2C%kiR@05*4eKJZrtk+@0T|VRq;Zf3G%{ZhkyWNRouiTQGxqN*iQp3v0BP zJ-F`BX4<3Iu{OmEWB$1z7Q!TA6W~v|(-&;P+JA7-9wjP>gHm35EnLLuBRd>_kqR-C z%3G`T#{M=vYDW#HQ$`q#n;oSG=Rfc%ZkK^TwKcd-j@j1>R#4Kn^VnS}{ia7v{|@bJjPuc+2meX(>iA;vZLUDzu$FmZlw@3xr3 zWw9_&E11p9-sT8$YIJ_wx;G#{;#N!g>LY7EM`I$bI*r;E!CKR5s{u?r!||GW{=Vs_S{5Qqw6aZRJoAgbe9JS@=LAtRaJk0V!iyc?SJc>Y^^x7HT=i62m5AyOSt*+*?!fg^ zG6~4N-lZjC;K7rj6sb`4Y>Lxt(UF*_gU5WI--iK-)mCczrupGd6ie@n?@{XjLct)4+8ACP$DTttm5 z_UC%^J@4sD^h%Qdo=&IK-SFQ;`(>{AtyRVlt&dfF2uY!sC!+Aq)r)r{i%NTYGabw(H$Ixdg zLJ6Ie#dT0RX%hx;N7)0bciHfo+;sdBp9;jGg**vmNZJ3Ga#u*M43x^L64fm{w4ObZ z`oFKO14wxbvoKop+kHAgDB;&7wtvRxY${!KtYa+i8ym9Z|41a)8S>?`Ap^>4@ApcB z=^vUGHM6uO%DBBr?0kkE&t3||*%JW^Q!LdDV=Vep0&lEtdiY4nm^0S=>%Xr*0e}1m z#?c*fjjGL!6$zlUM5c$oYO1g{zKzHoJDhj&5a}^ws!=_s2-KHsS*vy2a~y@?U#kP& z+Nu?~_%swfFg}nZ$gllG#G})%Q@rn7zfWJf7acX#c3D4NA7Yu+SW?B7yQU{ z!(Dwz`4dy;Pude_Kx%n8+>!SOc7cU?Vv3t^y4TYC5VD9|@bvFChzHNfkiSS7x@_y{f*phNT>ctX(nuJO8VB;(N|Xl z$;O#cELzifW5u=K&%OW;tlG9G(SQe_tO<_pMSHcNDa?eV8{EVc7RdTpm*_DuZ2_1T zdrQAQqDe^dL%MP{Fn(p7>8kMSlJVLt30*PiX}xRa9jHH z=S>c;YEmQ(;@`iU8rHLgIqswk!vK-M!{o)xi$JoT-0=7}yDN%;ykV z6E*Z+%f0r5MteJCS16P0LvX#WUVYwS<|ufT28cSUx3-$6OdkS^)Z?h}3m|_u@ebYI z-qu2O2Wo&a^!WIex9a*I_Ql9U37A+NPtlqA@COwtn)n z9ewmUq&=+YDXS_96>iIY5Nb%K8qa)W=*(2mvCCVg;o4^l&+6BBC#o|{>SuS+W5I?+r@1sPL%YX&8MaoPPnx3;2cDr_OmpFo`{yZOyW(ApO_jE&fI_?y-< zbF_WldwRWg$&YNm5T^<(@*z`#RG2)hS8r#n_+dSB{QJXkE2A+UcuxZD+ZDm+(N{D% zEjR>6;3AAZqW=ChlQ#)+_M?tk{1iGy(RmIo3F|4IO^dP}W21=G%%UL&uk;nn%qqzh{46TX{TtzkH#X`1kfgg6yn5e@qGg zh5DdOyl{>vEe->30`u&JY$&<+v1`qsxy+U)ym|Xtr%P-=+HKxJ_s4y0Kh<<_hTWk; z)wDciSGY9AwDYTg|5Q{!-V!JT0wkn7eY%N>iQK{)O02(9To_W!QD$`$`{(9Alo}2U zZV^Xh-MNUfT`o0EO$p>jUX~-ZvZo*D@;24Ru6tZoa{bs3?gfeXSlcWZuhr;?_0^yz~FUbII>H^E|RQF_1f- zw6?|W@^_rL_<_B59<{vgTQTHzD&&}>W|^+2{!4Fi**nH?_t&gJJ~;q-0}HOiIOL07IbMAcu#$(xOn$3v9&QyA_O zM(jQlMH4I@JyzudmHp(`F3WqTpYd(yqDPvK(aKo8 z_J1#Ks3L9SCWw%Na+X%pD!R`G?%_ z8WNS%w&-TI-*u&db(MZ}`saz=JGZyJS6C=3a#|8^cdBzQ=>A+(L%6aWxUu%lyC;&o z{|F+7ltL>N^m0`5G#Ew7_g*KcNWXuLj)4vi!e~8)*ZPw3;@N^lL8Q;q^d;%gwnNA= zne$&029x>z^S+<8zYJcpBd1U;pV^m`Qi5v0#^V!Qn(z89u3EN^G^aP>582)ZXz`C6 zkH2M`q;2uZ?gfU@9VhA#GIOyUrsuNSp-yQsyxyxdLgAk~v(UqR3)ZbIXp@AeodZbQ zPd+Rwg?HN?kAg00W;BFJEm%Hh0_@h&I@5V10|!Ls+$@~i2ClnhUUT-4wsgSP#5CFVdyC=rY$YhS2L%li?mTAecrmY+GvPuE%PxyZnDofS8U|NLnTs3 zdi`F5B8UWV?+Q(qr8;;t2mR3Ge>*vbHDBaB= z#x#oa*e|r%BnuqAl(44(8yjXX(Te9sWkLshuroEpi-%2};RG?WprLkt8)Z*o{%4=b zlEu|A?hS1;8{?i(z=W-aKFz^PjG)+jXOUmde&!Jq8sPk(C^&H?RTGQdf{` zj)IJ%!X{iP%&9TTWLyjqK?`?_LxRnX zB@4#?Hu;Bn^9VQ+WxY+PLo6e1i&!!bWluPFAd!)Y97FTT-b>db{jM6f>&}2gl=Bjy zgz6ed>#n}=N-*8)@9#$luOX3;4qgjiCte*jV++^$8`h`?PEK<4 zZ=mFjeYv~qgZ0}d9F`CUF|SSUAY9e&!VfFv+KFLHNUi_nez$LrlF)=--5eE{`6u2S zQQN#P&MPG=fW>PtsQ1w~jd(AcLWlvdIXgQWF;6*G$6A{9EMvyI<4vn?*x}(H?jQKE z0=@NZ?H3qv<6_WsQZt!#zOIXlqdRS_3*wlQmxNoEAX9i=*Z$HtMOtAku`u%j+hk>0*fsxM z!$G?Dfcg9@)9yBO+q~i0hX3C@1c^R`@-01=%1%?eKKJ;Pp9qXYFmrA~7J^rLFSKh zKQ(+`DC|Voh$+4y;BWV@=n7}hT^P=}!5g3^Za}mUv!QD6KJhdN4gcq?{M_0$FWYwl zV00z>%*jvr>8anBbLdEIoUeu(Fx$w(fBWYd=hSowSGw1I`tm4*oAljt z?4b5H0si`Nv?=NuT7Y(oAKDmIK3n&a<1eEWIsW(EI(==;u)e+?#E9iCA7VZK#fBvV zj0ckzUjKf*1Q|1(T}Ef_UdsomC;u1^Cern41XA~;6P6KA7T3?62!#JM61Vuo%9Z(_ zY7D+3nzEopRms%SkeI}VkfeP=_(&HmNwe9}6Ux7B@Rk|NOQ=;ZM%kpL>~4-JLA}n4 z+h2=YG|Y_2{D`TJ{f5*iJ8|5Ya<@i7h2|XxYl)fP9LX!N5Q5Ao{f%M}?*4%G)T>Br z_-}3X*BVw^W+m3XeD<#|6ETjVq|OpJJ;P1^gA z(NQcVnj*3>LTGM0PEy>Z&z?`Otz;D6_@6N@4NixiAau7@;Z?E9^NnV4vOI${f_hZV zP=1Vy7tulllhmYfGkKKe6}<)&dd^FFh57LVf4?I0-o@P^5xUTa=)_msvfu&h67`oB zw+9@P0p8iVn0LapYrqEdZ5qmgfSaPj zQw-K7awax>#L6DFfw1&2|&H)d=`j`6EBO%>UUxy~AG z?sG10xe)5+6IV;vwq;8bKv$j63fkkM`PeAe!3<2rjv%=b*jGfVrXwq++DiUu>*$yO zFat13#8rQYLsm-eOhNj4HSFrk{yq^v&$iE=3&n{jT6KII9+Y{+EP^H{Rj%)z{IFSE zJzDEULket2cG^oLv+s?AEW_90n;oipt46wmjKAT>T2Q`PV-0`Wh>FI7mQ+w+yyW}v z43z7F0$nOct7+%i<>jris@8Sj`abm_F69laWcpg#=*H4e{0Zg}r2lT&7Bl1{HsG3> zzEb00i+P)16I^j!yy_$q8YM&PMb6176;tLQxk16Tzt8tEuw2Cz8Wo1VR2j-Etu3}- zWW;I=@RJn64vD9z#=I#mMa4$$j2*3OdqL~AWSS52{d4V{^q$KiaiCzPd#bD9hR3_- z^gB<>9dO*9$veEMVH6hc5bICdoKO5TTu88X_VLFB>1KG-&UsY`NGt%; ziVVrRbmQe7FeKfhFdszu+d2oMK%bM~#GvW*b`Y1hOyMq{?B;I5Z=Nd|N5W7LZCk^= z4e|ya56IVNyI%?s!^7xp4|j*N^YbDTbyydH2*-W;xA;Psk3Camli;kKm3_NTb^9+= zz=)ghcvtr!t*Y%l4O-CglEmv@`S``%#qyTz{iZEAc>YhRo{$&#q)oQ=Mursk+I3gpqh_R3z;6Z|=uJHNZaJM47yBLXwPiLz6M;~FwoZw*d|_-IH$ z)dOMA7n#A==zCj7&viWAcFNz$@{-FuPp5VP-Y;NGfOwpS0ZG1RjRm~?qg99%SQR+a zH-N}6I6T}zQazsH>ei+cinuqbVUqHJGq-%g55l|@>a%0cpRoB8y*s$s+uT;^V9n9( zHTgABUXk92**Vi@d1Y$tP4@EH<&Gi{gFaU9m5;n!(3+l6WB$URsSGtYF>#tAaj z%l4h!n9xL*M}MA^IEed+^WD=*26Or$HS80lt6VY0?CzuIt_QAH-L^k(?^~r?=#V@8 z40<-_SiR~}O$w3?>MdB<;8ePUf86;s=e7Co9NLS1P+++HI?+z1VTYPEVKahYpd7j#>IvB!ZNxU#&a4rqnyN2f5bj!&6!27NhyjmCSO+|lAH2a$t{>X3Rx|}uj_PH_!+oL$? zms*;~4UMJWcx@FJ@Ub0fpuyNpm1EnUfD#&DYWwSGDUtI@ff*)XJk~;TW2>m%Fa^iZ~b6W+r-%;Hlwtbu&_`=iI%j>qiK-81U zVWivVjaHjGDS;F%2^bX68?ht;n}DPRS`I+QxCyh=BquF6z^z1GODkWEK8qfgorrQ* z9^_X5G5G|z0>G2*@5M_={HF+Kf&{1-u41q-@$?xuGdS^Pn%<)7J})he8+j+E>Midj z3Z?lt&2h(n=8@_0GuZm4*F@^HE77ls6bT;XvzK^1%OAQRSr;$Ix2qalWH+wQ2$K4D zX}m`wJl|_25;96oiS=?s zf+UIuN&YsG?-o7%vM0Ywj>_t`=1s0s;drmbbW=nlbm!}7QU;nI?r$DL-~k*5-oxdP ztxxCqIcHj-Vu>l`?e^kCbS8fk(SQIxAoEAW=zpjRiLVUUB8@b; z{!5oVu9xLR!+-V5P>-%Y?FF+e{CE)UtBqFN$X&aNFHV!!J9 zRyX4^dp%o*E-6o?R5}SI7#8l8BiO1CcKn%EIf1qM7Et{KDrJfN#vnXdSkaCMSp44j z>8F7p49w?%-C11&>27R<=%Dn5lmZAD;H){gXLUMNqwrOMKC7-wjaE!Q_b#bn(KJ}N z8CkkjjXZrQw>O&)Nl5U|4PNy?$dQ?7)pGBNx3YHcxANj4eMwAT;u7lq@cG)e&c9j%#)gU=soH&gNNO7HI<;S?(qd{iJ``3yoXon#*p^tn;W*vbH8joa`m6 zBTC59QBle`0--`w)->3W>J|gigH=_qRHk?q;p;&DpD(SOTKEfO+UIink2B<0%HpMV z2ZymsDykG`)J#nZB{fdAotp@Woc}GpsTD(92Z)1?SJ@Jyaoz%p;Mqx)(nwTn2V7o8 z;1G9l4MQWuHO2m)*T&g_CRmHHdvF}W_P%UA*+Llwcl`UTz~7qh)8a^mQ?Fk)SVoGwcYK_s^f78U(uD?y$>(w*-%cPMa0l5+d0#ysO(ub*ME&MphME|7`AqZlMd2liuK{o^`Sy$1)3;L-t^IQfZq zA>y>gx&IGMUmX>7*Zuo|pn#ynAl)^Dg0z4jNDbZ64bt7+UD7F1gHj4er*tEXNJ|bO zl0)Y`yuWq-aN!b_;me$T_THa@E??1^;D53uM-QgD(xXf)a{LNdonT2aGNu8|kWU?w z0u*qhOSDT9X{1`J7F++_u=B_FZ@*}K(&kX%K(c1YrLr@*%yRS?DG$zMR9Ed_b)JY& z_Wy9Fw41jSq@3J8Bg2jOuUPF_Esv(9CA$VYF37(-?Mj8b!nsp`Uaq1#A^bK;M#Nkxg$32ly-L! z>VB=ZiN`gv6WhzpPsEMsdUXFG1YGuwvtLhIKR4vGm^<_A0i~=Z3Ljll^Cj*BP0d4* zM8NScMbP2C1-6yIoB&7$z;zF#z`((Y@akMqUs8c4k@1D|zH$HKCMMB6qO&KUo>^6M zuDs{)U-I35eY>}Ro8Tup&KYp{SQJ>r%F8d^vu;Yx+ZX%Cb2yyjd}{nBpDJlz&VlLe zmqYEkcU;)AXJbAP$<9S zP?~S`%`pU=3Z*@do2jTNoxta*#k@mOlgmMl&2mwqwywUUx+dlLgEv#WzYvta9v}W% zloFZ6?!qUdd4SEL)dIvxyKXace>QgX8i08VbI#zsIg%4{i1=^(!}}KSm8fWfmj0Bx z^kERAb3YdkPd8}sIwvP52Xkfy-_NZ(9*MpB4gBYwoH~JLJrF;+i6u^|w-gPG-lU`-|=2?01P0tLk<`Gh`c0lCJ)v_X87o zU(YiFFR}x#k)r*I8M85Cb(*odQ?$0MsYlO1iTG93A=9oae!63~*fuP7_mA%Gct;5Q z#We2}3JO-aSK)rT79RDTe|_I+H+xgP&S$WrK16lt>(#Xsu1lg)WQkMh{EFu!kErtUvz^M6A(6`yUCRV+0A zxSqo)+n89GUwzln+8=mX`ntVQz`TIo%W=`X$~9_1EL|5RT+_^*zlm`x{S3@!1_Dd+CkXzvK2OaW!Hdr_+Wg=}4&tvB~a z*CZ~xz*nx?>gYl}jQ*DEmFd7`u^R-#?a458S)r5bsJZ)(PpPV`NX2I)?{(%HC9IwC zq_Ca-CN_EQP~RQqdz|+x3vV@dB{~%AWb#3aiOZ0<^5yazi;YUw7HCprW{yj1BzL>E zw_II4~Hk5(J!szKUS^9jq1K6eCmcM_;| z0Nn;B6hzYPUx|1hg@L)IKxG+>ClysyA6t!Imp{#mBRY(|0BeMM_k*?mM6xFg7S^Kq zrE08ef*|lF(dA3eJv$xjQbSjg^coy%+uGu*cJB8Je%BK+pp-$Rq75V1#z zZ?G`w#_Bs14Ra$4B&Ye@6pIjh2UC?_FLoUREYwfUqg`vI^vw2_~bIf~&O zy*k@xC@Izesd12R6;^bM+bE_nlkKaVrR#^?T>s}l+UCCV?Efnn&fS<#l|&jpYCMx8dmi&px$R3H)Nd`|9dk+^qr&%zr|2RA zx>E*-LTqVXhUnDxn8I9zCs8C!wspF8uf4nSe9r(kV??7|zr`b7xANK3&Z3(C(NVn) z4(I@f8I85c2Z=-DcTQJ#;SJzTlt>spOHF38QW!pzXgtBF(sW#@*&#CeZ;QE z?Cc`)_wGCpmnMyxpRYU(901UR6)#wKU2NXF;H(va72%UShl~{`FBcKuSOunFGvnSbLEYZRcmc`!ql^NeGhY~KuU#^~X4+M0L%7*4;)WEM-rBndU(^vFg>yK3?XDCPSuMS5 z9wlkGf|d=rjDAwX2b$~fxIP8G@zo=Gr)t;5Bj z+}%T+WC;5?w~U$~-oC>`7RuFqQX8=yNkeeF7~yXCE>W)`?ejJ}vc=EaHZk!0f*{dD zhGQIe`ztf9tiIj@r zDTf>}LHK;6GLEBRvX?OU#(&s|n;)pju^3;X)=HoJ{sy{~$4OU0pavScF5rIk_b;^h3g~XR^P&SnohrvOSSbP$E9G!| zH+!|uH28sIl;k?N+c@`?XX3$u@gQu1S^f3mYhR16Bg9QLHCN0i^cyx`8Z#ds-@Wex z3Z5i`W$QokTAG|ggAq3mxa1$ppA8I*3`BiaxRcbtg{v%oX8VsRpZfLYoL2s_t@rj2mUNri?J4E*5erew>|zRbhgpWSs#dMb4f3^lml1u z?=JE$&X#V|!RcHTl*DJ8bi>oRO^jNlT5Sw9Sy(%@|Ip$&ZN>XnhnkbtpCs}9Io$Io zXRHIO6_<>sl|zS9evIBQe1y!fruL#ERGl_@--4`;z3PVT9ulqP$1yq-kdSL&FW+dUEFk46YIdoqCRKy9Jc;vuf?lq z=IF@Dvk!ti#*Mi^zIIJ>a||OaH!p8u+YLW`M4027=HmV1x?GlK7;t^o50BdN9HNxqFhSR4QAe0P;PiPZZV! z_HolGj(cf0h-_iWva5m!uh$^!v^$bg#JlV8@MURfX>M+=lb_$n;NeS|{{GzDkCWSO zf?{HuAP6}nCB-|tXwB@&llz8nnYO{*)6K2t;^I~P+NS|-q#8DG;qB&UNx>v>Mu55>@l0o_{y6QTDipN74-%z5j{l>C^r zWGPEYv!j?}qrAN*1?i5cL|lXTw#-gta`Waz1l?ZIL}LqN>KKCrHAuS za#J0XddN_I)S=A1h=V&@u1YhH=lmy8kD_%_6sa9{o3Z$r@ATW<+wC9s$B5L&^t~Zr z9N2(SD@S-9c8U?W&`W+yhNsZ(XNTIFd4Mq#5ytl3ojK-BHootR74s;8J=4T1N>2r9 zN+HVpNJ$e3Rg#~YXsScc9u^7GIoan925m1>x^l0!I-+LHG9JlL!_X^rH)xgd2ma&; zGP>AbtI~M>v=Ab7ovdyQB;@|)7H9f;3HXy_-W7iHbBI9JFY9c+i1k%Hr`x~bXvJyb zl;1e}z|-=3T#*d~baM=&q_mdK04EiM(vOUC`{%Ut#T1q@&vmvY)^j`-$8W5!?e^Vo z11+ZEMh9&^(Ne2-iolXzD9}HsS87b~60=K7r#9|*SD8)h|Y2z8-6R}R}4TC7xgra03Ng1FmfSF`S=rL!|T|Ke7xBy$;4 zW29`R*i4nk^FMxc0ViQ%g)UM?hLVcL)z7Wxu<#n8J|;vZl&8f1QEzDekb{!S+09Mz z>oh|QJx;UeyW8yNljDIT&-^AaR}COih0xX3dE^<5e-G%?#SZUF$b?gr38@B~g`_Gy z7USgi2wIK*Xr#y{?Qp*9mb{v+zE%h?Io;sa_t>jBCyCCWGb0cKQULlao>sY85Zt?N zeb^#TG9nCc&MqztwL&Ok41IFXz^L#7_$A`~0`+9++1Enq_QwEiP)%)m?K58wzC(}_ zU7%tN2yvinL8P?P!`B3Roa1uOUd2AP>0;WT`u+PiR!C4v3em$*v^2T!-Q8UU#F^18 z(&+qXm~(b+ZWTZT_hA!&;D7=XCv6Np5d65Jp42{S&AjN1ysNkyZshprDCil^uD^29 zzjKLa#ILHIkdF*}6>u7;guD6RrdA)}X~A1a8sUZ+%o*$olX(72$G(98Gq_giQIO{! z@5qj;`3~g0!gl(rL6I?Omd5~Qc0sJ#Tl_xDT$DCtT0NVh*@I*9CmRr!qg#};k?M>*TXu_we~&e8|}KuD*5{{Wg<65|GioDj*R1;G2Dh;n&dj?%H1(7qm%SecK78-WFTlZPzD|CQ6_xNqVRn}<# zQ_3z{%R1JYiDW1V6R%>Sxh&){4l+ccI>|chM4B$Jar0pC!K3lSS;Mp$-47FxmkdF% zNAIRd2@5k_CV`5Kgg<}(3JYCdPu*%-DyBKA0G5guT}WrPMW#w6^^@x1g0Pdf*E(VE zOA5tsA07Co*i?B!i?;>l1CYnFe9pNpzp&36#zm;9FDm%e>gVXgLu6GmTq%0FdqWgd zNj_L`Rc4M;xA+~GjCzsRs%3ze@Hves1~W@dwNOo?_IjH;83RsyxHYqBB<8xMEK#pZ zX#4zwi+B%IRwD)nx%$pqeI3#tf3Y9PbzwxK2XPNxR1BAvl1`J7i%{TeK zLcF|Ik2nK!T^z+eea2o`NVT3~!fBQj&wvth3-@9^*Ta>Cz1j`GUZT;tKK!US8|3L= z#mmMW@j~szpoSa{vh@kBBCQhP9*xj-q8KJ+;@|IDUsc)Cj~p*u6qk@(-7b0u6|Nb* zZ6M_NtOJ)*Z*MsD%-;NYeXMEKOa9sxVag}0&tiL=M}P?#VmlS|fJc24jw+Q}uA}3n zbw2MqV$brBfU`N~!lS{`)RCRV8s^XtYn=_0F$d*Fw;}fyo&OeRinQ5~yN)3o^3a+X z4tA&woQi#epQJY2?0_<(yNKcT_^|(8Uz6+E5>*&gwS!tzb>*Ncc%mcWx6<(kjf6wKK67z+}6 zwb1_D`E2hm51;@0#(!P}OZ>M|s}=)|Se)wM;q_)EcM+wgva z3izGIS?HK=P*IAEGhrE4!1pcA+mVJn4PQbXyDZLPTv$qY9yykkxqvP~^nCfj@9Al> zCy{<&#sc?z{-nWC$cD38*ge98K(FCoz(EJ?v9jnzSl~rQ z;7Z`__}yTe?-}U^dj$`R8DJD`c8n9-Q4H62Q13Sj^2peG=k9nndSAOVXQ>Me*MR0N z(>pVhIb|?xaP`o<%SvPeTocFZVcp$tfg{1k{kMCq=3Lb8v$O6jgj(axSq2r&dh!b6w~Sy>B{k~f&?QW%+H zh^}c(&7~+kiJpuRU9SD}WhGW~!^EJIr`dGr;_5@0Nh6YqP)U*f1bO1pIvThHiiR_4 zc^s)rvD@(yPkdPyVZpR7@xw86a6j}`P+BKX+2N%)W%N~j_K zX#hR?KkMXKZ6>ugXye~qbeP?EuYe~M3 z(2S`2;y4HlK5+3Sz1N-hrs{!7kTNv}^!$u_wZ%wzrb%y;Iz+6Ynb(n18?c?zWqK(k zva?{}EQVp2o-1`dV-cXIBk|{*kKxAQqWfagcKM(=U{rtusHLT~af+A!sMf6(`_z~V zU=EBhQ(xOyTVb%&OIT|LL;!PBZt{3RVd1rdgCsDi9Bxf8fXqq3*TO6RuH%$8<4~w4 zZ2=h*P5?D8oZofczxBVpY3M)(-hRF7rMWw1QBqR+27gwy2{1n*9txBKGm!$qT!`=G z=q%}APp%R$KvR}c9YZfn_Jzh*qk-#rn9 zK&S&yf7?yS>tm!U^rxXtf!Y_^7uGq?MH6QQSz&}=r*(HbzjSoJ%evl94D31ccUWp4 zzO zT8%f}x={oTy-`;PeBYQ$`{%Jm)iy={yX3omY0{ep8VDWK_j4p6nyZ}ZH+qxSLJThY5q$Gg7$fIHuW zgv3LUr{egahJn`jTT>IPd}^qTbF*O9?{8q+#6TP0`;U3Hb>vgJ_6}vn()!QvLIDxy zV~#GqD`#<8w!Jgtnd~;NVl*OEFo7q>T~(4&O420LKJ;TaZ6?iCdt#2V10bS0Q*sQO zico_Hz`h^dkqdXLu^bC6{h_{RUYU2Ijh=~DXM2a|D{m8MUaubBfMmXdphM#~CA!EP zaDfPPT;Mq_w80TUrCkA zl3uXbAEQsVxYIsVXWgv*b}c#*wa~jZd>a_{aMgray+uEE*C(z1e zy$ThS7^s7#Ft%9!?Qx4ld-R^}(I#tNbp+AhAW1oeWM*2dD0If^H~4Uc-6)i5=MV3m z3t1M;X{grROhTfy5)6SaH8eysK1}1vA0@cellz2;V~Lgk@4sZh|LEvyooMm9maGj|E!C|-d|blHB#H#yC#2>4&Kc9RYb9nzPE3(}UECGsTov^nr-XpG%P zv15~(IDX9{QBlF5w`bR&b$)25I<@E=xUqnL8I6;ZSI_M&U1C1v!TBLr#Q+RsP&AQd zABD^~yNBB@&QlV-&kyK;Z6`El!yIWRpq^wq9-*50f!;)tQigHxop9wO@9>%3LPplA z>gK5@=1cYepv18nLl$gl+((mg!mu-jM58 z3YDzwn?&cLh0sYu9TTRrLmoY@{0~pFu&d$@@~rc3t{el018@2R*MVI!kqM~_Fci4m z5a?Lxek%4!t9)YrOmIfbTqFUOtjK6h6s4Lvfxa#5YI=O^dLz2qUpw}E)f?e&)XMSd zI?BQz;$2?kyXSY@`srV18{B6vq|BF)QX5n+=>5-DjfbG7DnEwZz) zg6vROPr;Ki64ZHo3}A*-sg=W7GrG}dLP`$DN3z>)VC?~9 z&Y)7=n;uka=g!-;MjadUtlqtEgVr`8k26+uHo+o^{n~qF{K7Xt>pd-&(TDcmUJAUV zDlP`!8Gk(V2?;7HOa1F!&(E)Ipi`j1LC-EH4N)$|ee_^t8}eU~N+`L3qgU1N;UG56&0*s8N>QSqkSH@O*}MDeju3E7sBpAX`J%yvgNY+AK7Hb?!iT4b zsKJ&J{gR2dVF4SW{5p!Y$OydKSE1RCQgMY=)LpqV@m)z!tPN*X>SQQ=H2o?UV?6z2 zF76Y3l4%rHVVrETt1DTzVImm4f2Xj_B+{7e@0-z3tx!@C1g1n?r7pLV>Ypl7_?E_E&C;0!sY!hPsE={W)4C*Jmb8Ni10 zGxS5WIFn`3VbS7b$+(H*V;81=p#q{0as0Ilf)s1XKuQ;W1;M?N+;~Xtx44SwiFh0s zo2f8bJ<;+7K>`I2Hv_O6C_)9vuwcJ`o2Ff%UiO_UhAD)r_q&LlK+jX|tWemS_xLUg zv9HLY6j}+@3Z$wvWK`Ys#eTS;i*WWlHC13K7K`3`@>a`!^vF78kLrD8$$&}yvtbu;3(}ZQc_z@bO z7XPW$S3B;HGn9F!nRBAf=vDdV$o#O}Q?rC-)=dU%PF7WD{pYE_Jy8m9>MDv=q;9hr zPoZ#vpy*R?mvkr5iRhAty)HyL6V7c(Ux=3*3B*Zx+$E$D|FUbvyI@9z!mRi(_c#|K zDc03T#4c&exgI#4j_$a!=lV z%$1~cV_GPd{?aT_qQ(Hjuwxm2hre?1GR1npm~euukMc}i=^;1bQ=FESYL$j61pE9_ z_?0aQ=i}Jz^sg;j=Q_?}yoU878P6RFINzS4hVa{ikMTl`l`d_v`IS4aU%W6q)F%Zt zXhh(yhFjAQbMM&N+BrG`9A6bUi=(m>&_|+2+;mM%ZQ}aTiavB(QBLfa=H|1w2~o4K zQ1|x(>VVnl*|_)hc~E!vh?o-TLsxYS9v&+KZzlwy0IoGr8&Vztcdujfu}hAlP@o%s({3e_c;Y>v@bY9 zwUaAO1hN@`*kGZFP{v<*YNb8wri<7(IO zqCfCj=JsGhh7r~Ukjo1I=tUUa{e5TWXipSQaCN2Zb-j+g=8R!S2itiQRfucFe~}G$ zY+iRetOz`G&HW0Tzfe>&fW=*5duz z14WBVI%m~DCXU&2ca#B3EP2P&fAa6)K}=;8{oUUmG*Rsx>g zaW3(zpl=0>c0qEB!%*?lK(5Su9u^{at#P(?3U<1+sot{$i%~_9^D6l*Zxntc9gI{W znU)FLG@BkRnf6l*JtnO1a~Wxj1Pt-niX2l)TtWSsHZ#Z73KCoKYiA(2=NIn>xH>;v z-C>4*KA}5erv~Y*kMgg=MBC*mChWIk#@=Vbrm&X7{T%<$5kQeNw}0RLGi-@~#^kP> zU;J31%Z69Cx8E0UW#;syiP+0BojVVy%K(9EjiHOe0}i#fkZ}DSdHMuQeSxm}*OZ*b zHjm;)qJA*K1z==KObY{Vp8tZrvjO71LQ-b+#OQ5MZqkWf^7FWy_+|dgXA6@Em#b7P zpOQwnU(;F&omA)kH~*L{2dx_>8<@*gBbht!=6AM+jUrnbPI3iki_+K3=Bd*uxIaF` zWu^;-DFmX!OM~?bw5!x#OeYZ&3q)lpJW!Z_Z)3r@?L;|AC4;MgCcsums%TTk4E%pI zlD@TJ3_o$w7h}*GjqCjPgrT`8>`SRyMTLv~88i4MSFE~jRU@mn0dmi;N#|6)dJL}1Uu(GZ&vjyxv0F%tFES$G9_tk*M~bkoWmwhvAS zk>%snbc=~y51vPL_3?$!S@erJa;fe&A1o-RO+;{U-i%>-5&xDwMuV=cC2th$|E?nt-c7H|N@@Lud=FEn<4fn+?S^@yvB(Abc3& z;`mG$;^>TT7!{=^HE0vm3!w}Vp|lLX2Gjkn{@1khxbz}oo%OHnhWl#l;PAXxSyVZ= z{btfdrQPP-mLg;QK`22Y#E60t*a?q~x45+|j;wrR9J`?wtP0)qDTYb#dd5wcEba4T zDSV^K`ND@>Uk_WlTVc2jyt^JJTgid9qE6?pN0+YgI&M$CHU-=WmBC+<<9Q!%(E-$K zkk=nUCF%U!bDjTOD`DZ;T86XgwUjf-iZ`MS+N$6UCNPqf(ieB!eoKC%b1N2;p7pYj z*T{k0nBz9wF2chd!Li(hDc*RWHqizPF`fqmQWJg+VE^o@xCDHWN-3KR%G-I zui}+b@xEPbzYBRf`8KjK7Hr(!HFgo-ULTlU!!cK^Ck4oGf9jKP!UQp!L;fq1V>&}O zD4*KUdvz>@*+1o zTS7Xi#FsZXqUiMs2O^*@>iAwV5Y`gbx3pHgji-Apt%tw$z)qlxYyDsgibxO@lHi1B zkH0Q9{Ls!xBUX15@J?U)((;7mzbw4pw7Ag>B#KWyXQEaB16l9YJT`dA1(p6NQW;A zzG!ze`Ak5{OFj)V7NV=6wHd4YbWi5(J@E_cDN>8Fn!=pZ%ZflRV|C$Wnr-wPx!St}Y9u|5LsrnJAwuVCY=we#1y4*1e zq_cgnR+DrU*XQbX{_XuKnbIAeS74`M_^xfV1dLWAZQSGKN~D?=iuD~-UI?(JWkX`! zaf>2MenBB@R=Q-G&kLaslQrJpb0=#Qo?Uc&7b*jd34vScw$yN*lZ=_-S6E?m$z zaBZQ^Cm5*3X5WJuY*sv33#Ps)47MIZ$2ybPv#Jdo5oDjd5Ki=|qWe40oYKAW~U5 zCcxfgxMrirF?+u2%>Rx8X0+0AopG0Zn|OC9c8`YLht2-a54KWk&9mc*>fYrlnX$U9 zO;NaEr8ak^SNg0_+WX?p^x7rcv8zEb@PCllkDa3%!{4NVo97FZ;+um+A&;JZ#*1t; z8F;X)jsG)N%A>x)dwwzZkFdZRr2mxyIIZoGtWY52V8t7^S9m|bq zKH#X{{flE%Y^rV^iWG48HfuA%r+5pX)f+(LYkYC{RDAh?_^0Psucp%IFbeg>m>-J9 z(BHEET>7)oeYbYEynlxZv;_9r-VAj*q1CBXfB9;J21A?)z1=Oj4iO#7qnpg=pXFVMW73b}X$l257 zUX;!vHn8eno0<8I#hUkR3ahD>!O32+KvVrEU)B%C)ioKk{N2*HTtZofmM>CEmJLFw zcHMMmhPo2-e8w*k6355CFtfpoozG$Q)g!EIRuE6aM)z`p92}-3AwDQ&YN+srLUFKi zMI%Hu#_9(sS{~YtR&b~budb*LkjP+fuA!sxNy6xGD3gtQ5k2zmp;yoNXUF*t1s% z5dS5{l>D$}II^isPwffGZH!^eIDC-5{aHqw+{IQHIl4nGY%4TX7(#G@Vv4C~cD z8!uRjB)8Kp>zBYWo6(Km={@myd1(=H;u)Y-3r!er>kg}RCdg+NXEf0>TX*YGVqf1# z?-^^BsHIo`X~B#L8>jL0Yt5f*!2K?fB3BnvCaPGEEzh`LxZ2M#GWi^yzf!Yi0aoEp z!!uYxL`^dmr~GYAZg0KHNIxmSNx+N%jE_NYDwqKB*1KvrfIDL^g=4FDiJJJp)$ids z6&LShMgk19d!|)KV18)#+t!7>+epBF2IK+Y0ZoVHx3_ecoOtUsGCTWF_1Kni+W?~= zc~b>F_4?gFpuy_-BvppEutSIM&yP z=hl|`>W6~qBrlyJ57Cp_Q)-}rq!#13;9^h>X{8ys7BROrom|M5(BgCsxcNtIG#Im% z3B`$8Q($;XoA6d-ysy5$aW9n@Q;`RnHe{t6EU4sUKrxa{ZvHL%-TBgO7K^gq>iP7* z5e4Jms|e@B`39-IKOnkfV8p4UX)tbhE`=<8E&a~u*%=s;ru}Ka_MB7w+1EFJq`at( zzC}$x7aQ-YSOm$0B7Wyd0LwO>=T8D*@CjxF7>XDxj;xSdqMuUuw-jkZ1fy{2CVgqm zx>m&-XVdC@U#*3IO{I?+FU`5>6x9J@=e<+e&-K%3B~WA_70U8jkVZUXU~N zKi|xIp%U;q@3_0_K+V3-2bml9|At#}2>@VVlFx=0>k^qjS13ObXEcp$ph_`OjOmc(|gIKW!%H%3_E$rRXK*PDu7+f(r z#j7UXS!2I&NIF%oQ^)c9d2HF5%7KXP$H=|r%=W~5H~aUw!PkSaCCJw3ze&m zLJBI_QP3&sR8G(88EuaJe%1ok3JtobZ&P+Pvo_Teb~O`L-~QOu{IU9`^|e;5yjrb% zqH0R}i@=Jcmn@IEA)k*`sxQ>p(>_0o)xZs{F;SUxmups1oqoxS_ZF-Q zh!s`2G>BmkGT|qQ*{pR@}ZW_o_t6i64dHZ;x3Ll$C&%v*Ih@$7pf>%SpfE zgcH1&IIf(s0g%WNx>zrFAc_9*?Z_N{zq4}%fhn(P(aPa(zGwl*t%aG%rgk0EeV^Di z3j5!`HYK&P zlU<6|?8@j4tK*_dW0ShE=diJ7OJDpV|FmZrs5%hfr|Z7;w6w7vR%h($E4;G%IzDG! zNSH#(!}IUI-TGO>uPQGrvYU&l%e5(lc_b}WWK8N z-cCSYv{8L|H%H3Bf#RP*`8UtD!IMhz9s9JxbmJD$_Xe>OUJ5#SF5q?L>5)6?3(U;3 zRX3<3`|2LH=xP3*BWHTejO%y#Hs{b>4p!aNmUcAuacGw+k3x^>v}$^`WnyElj+|W) zPZO4vDcVn!vf}i{Aphv}g`ZPPC&e_LgW*@F9qE*|o#$2d>Du-&Js-v=zVsG#6MPV} zbR69b)#G;D`|F*THaq{N*0=IU-6(e*PEYXoMJF7=NB6O2@yKhfxrbt!&%tg3xn%2X z)-a}99t3CBP9HZWB#0m^tD+m8R4<-Gg^|6u44gQWI)|ggKwGqs!Ma1=ihGeUh!5Ta zm>UW9;Js3*wIghOId;vWppXJx;%;eg2c5Q1Hhk4rG!OOSeI7ym^JGRojDmPt-XOwH zN5n7b{=>1JV}XfEK*Zp)xHI$fW{&&zTJOH9z7PP&!k~Gl)vsQ6fJheVSE;h`K_PO` zn25ry&t(Z^z!~j<0cYV3lUti)fl43O5UF&U+84QM?GlYLu#EVkk*!f~&z1%f=h*K? zS8eL}PcCud`^e+_*y8&xC1yU&=7uZI?NaGhD&iI?B=`&YK|Ea@O`lU)sd?LQw<~sd7I<}L=U`v3 zp_eg8HIEnh7U*w!*7~pF`dL}0@q78LZHl1&*^0rIsGWob%l)56`?yQTcQuXsAtdhk zo4tVY!ENz;t(5J5{iRM%1(nCW4s5_ngc(Mtp(zF7=?&xh1^6NqP4NAcChtcEf%V>z z(@psk)I=Qa__Oa33t z(H3;SW!vmyVJxsNHz6dDuL2=iAyi^Oc;vqPfw|aIrWt?3*?aR7364gYCRqHONCfYM zbQ(RlE2TZSBfz*_3h7+JLZ1$bf!55~9D-=OYh=s*kpS==cN9GjS^}*14xgRpdv=cM zFLApV>Eg_6dg-U%X*JfhkL~?s6W-;g7-cR$5h~}AXwbchX*FPUa$T+eWW7};Cyw|` z5?i5B{!5N0{Isue>GlBs2&$5yi5(d8|NWpxN}>p?1nfb*RmS zgfG5QIy6n|ajC}YW7YM?%OZ-{&(@imA{>+ZS-rJK5K>hCxF_|yEmd8&yaC&@5DMaH zOVsM|4!ze6R&bU`R3U3k&s4bz^qpqx7{8aHCu4miS&87Tt`2ry13oOV`KX4DS>(;U z5R--6YT_4i(H07v9Co%EtSXwdSRvjjvwt_a0s*%?w|`0ITYcXamMP<}VH3YLZ(~(2 zhh}YLkPu-@rEVyadj|dx4m^1Q44!RU>vGgmrSaoG6F=bBrH^3755ulv!MDYWt87^~ zzZgH?xjSbLJgF5D`0dAQ5=5ss<4~f%^y^Ey`KlnDEMxH`&dJWc)8=N!vOu7Bq)~^u zMbREz&jUB>AuWn}EeH-<4+t138$jCs75K9jZ`3w!bl3kn;4nd4_3z8SqT(6M3p2^H zhEq@*dKf)^GANFY{&5Ekj6IBlvW;ArHKg%Y+$;A2eZX(B* z*YPIqEZ|Ty@DTOf83>TfDt$JX)4K=;VI^9)6g zX>^HF8NxVi$DW0`39D&yN!3ne4O25r`<)l;j)9%yfzBBnUSEfl=7X0h>8PvKETU*tc_ZCJ4p%3&dQQXN{y zDFRQZ1MeE<{hVaNoCanu{+mAR(7f^rXK#Ihk3lyd{{7oQ5{Kd>rn6|W4r!%Dc7qY- z(L}fur5Gbj^at0&ipnXVYAG=^lG1dCNR`a$ug%r9e4nQXD;U4H5N-Q|U1dAkPm$GK zukgV0r5{ru{52;%Ol~i!GOp^~Pj}#IJn>yZ=~>3hIGkS7G}CJ7YV9I@hYbwpSR3~= zUHdd?vrsMDoT;guim&_C?Ext^?)5yZlF1O8I!%6l2bY^m;eUJWfQ{-4K8&B98f1?W zYt%(StLS2A=!fr=%`fi43$OEKCzlwo91Dr(cq#Defz({bKs+6JA-KcNQO|@PjAqXq4hew?DiyU69h|$f?=^);w}0YVCN3v(sZ-Tgv!c1Vnc zTto<4|7W!zRoNussALGI>;Xe?CVKQFh8c04mRVipBwJ)EU!oRMfz~quOs8wo`Pf4V zw0!jFKVg$>a$jKO4o?0c$d#$Yl(7dEx5hGK7(!AU7OhaI@#*`+G`yHP=1RDD8%FcL zB6+k3z<*ed#1_YOE=!iv{P~W+mc63t-Hcrj;PZ>F!cGyC2ey-_oVNJ*y?V-$qY^5TaPav>yg3^KAp zK6v;zqs)-9R=Rt7{tN zsS`B2KZ4QoPv3Hk?&{{!-N+#S1N!VstPm{5#;QRluRbd`CHzK6tfrQhhZVm7IQ%sg z1=c`t&2MW9PUmb7mnoxlW=?B!D90TotT~pE?>|hPp^+GD3Ao0oJ;^W5?&;xUmp}8v z-!c<3f{`_MNnTks>4#uz@O!ej%y|XhXtVVrq}=Wz%rc$Y#6Bef1K4$ zT>+1l-T4OK=OwPm!$P^uR5ozG4+q%(enhYID@A}5fF)Hfmw7f^0W%oN!r$LXVm;K( zQBq^eNpehyO$pQe(lamMaImsFZ`UyICEiKW-|y$)HE`jjkZL>6{8+bA zIh+Y^(V|O0k%!)kI{jAcabeQzYTgY0nX+;B;jZ3)>Afe0oCPLLNAIP#^|Da=hc>@$8 z!(wZ>rem{ovnUZUWC;o*dvq(a{)J;NS7sz- zO?jo>sQ+i$%GKvWWXIX-EK%_T|LQns>lpjTr?>0PZ(<5%2Vx{sgOEMihSEMt3K=F1 z!{^7?0h=!qW@VD;wX$(`o~MPd5k_Fw6Pfs8s|#3~H!JwO7c5Q0R(g8q^T4m43G3}O z7XxQEzMVjy2ZJ4pxzNj(!u?(>i3ej(oky_6NrP!=2ebIUtubnQgzfe!>?WxO_H12~ zensK?I!SIRvOdFLlfu>s6I5~zCGD3-2|ecOe~`N;IM9^z3AU z33%S?{4d+49^=(Y)I_25-7 z+2o6UaN(!oMHOO*$vWK#mh-Um@G4gPd3n42pm6QM!Yu~tsO(k@T1e7!LH(7;?xV0> zi!}?y;y^31uENBSR!76V`OVmV@bi-Fah8Oemm`M<=8`wI6$pmt6ZX-JRK8$E3QcxM)5W6{M1*v=goZw z?>zk9)6v7YH^u&I-*@u4ObQK6%u;BE#weB7jSda*fsr7YadVrK9GGaE*kvt zLn;0}$=XE?qZmLC^SYr>chgzJtkweZ(fMc|Vz~RM=7dNQoj6uv%>Hzx=Gnn9@pK&EINUyTSAGl>V)LUt>MTNX+eKQ9f@5|Lz z+HwTE_cs({!qFEGGeZp%+pSmoU#>ryN}oS2?dmeO5@prxYWPAzm$4m5XMiy8m@cx- zmMsKEzJqlhIhP~1_U*5kMoB2xo5!@;lv*Y_*&yd?_~+b;+bH6RoDGu*2v!kRzdO)s zaSh~tES4Xq9nJeFsml8_)_=(&xK$xmmL%%fv1{jdM!Oux?`>tO;iVM;hFX@7^!O~x zGGt$@UcH19@lej@Qc=4cAz5(u{KfBzLv6ifLxSKjSb!B9f|S%&3bB@PYhjLjn(qWc zR0RDGUgaTNBg{zpm1U2{`yWp>#U1i4U;Fi}6g{F0JbbzNl=|oEZJxYKNmvYRvMo*N zjFL2mGF-GxDI?|4nc&qbPxGTk3VrIWv~+S(s~WU%YWkdgW7s1s=OY!;xRnWXd8!rEj0mh%PVx zDb3vz>!FO7UB!YCibJB-QahCo7ZiSb{dNAc?Q*?ET?)R|?3({W10pfG55xqT#IgvC z>BMh&q=*>OCmdr>#%AuW<)4V&rKa3G@Oevwz=I*|ey}QH>Wlf_60sKxWV-lx^-N^s zySUu(NN=(f2gepN$`VRqXQkI>AZ@wvw$7v}X+*}%{k-$XRB7h1lNbpMQj(A((NpD@ z4n`#l8PUkw4b5fEp#VdU)Bak1*VF$;)LB49-9_zQ5fl)R&Y_1=Bu5&ghLUjTmWGk; zM(LCox~02IQl+~_1f&F{L`u5!9^ZTK_pRk}Em;aPf9LFT&fd@SdoqEwg>;N|kuxJr zUrq^7axQY@Df=#+_MXtxQ-$~yZla3gBG%(3Pw94d?F9rKr)?^?2qLW}xUZj*;(>+h zdb}z6W!K(s#gs?L3MSHbTntqlb$xd3ijCd9M)_m6py}5Qry);238pMBjre@CVvDcB z$2ZC-S#Xor)~-fWH6uu$R%8oDW-*OCKE0xT@&rVTan#>{vttrWoJFN4i(TjUZ?}|6Knq3qYJ(!U;3He-zE#Wg-M2h7*BF+CN*4hld=c z#02h-e_JQGJzq;FL19`&rR+)Yg!GTwJ+O^)zcX0lpy(&}v41&xnX%~#4-Xizt8SSo&5F@?~QP}QJ= z;w0)Vjg^SJVvIEJ?i(^j+27{OXraK_Woas#b{$4uqCm?P;k!VXi{EyHlH|c!#g{kl1>3CrAsu+&9lc>h}WI;}aM6!YR zbNz={{}EhPadfbnXI3&t?nIt|&6*j!#$WW0gm?7at#m$Do*A%-v&v#wvW0S{Tr=zY)$L8 z{daJEBKB^Bqeqa5O`&K}z7Zp4I!P@tg!}F+PwC7q>@gW`xybDOoC@htZFj7|pz4N( zu5p9*b~{)mII{k^6uw^K@<&_zY!k`}-@6v-iX*}NAoZ3e>;fRW;Ft(m%7GAPhLukIqv>84~`wZqTEm?{$?Nqu_3>Sf7%s z{R*nO!MLX3n;mhgk{Am5C#_gaNux6Bu(LKgv#gC1G9l8#fDLq|zy$3K?&QGt4srl3 zfG6s+i6kNB#4n+bPXZ!*jaN$|NEL|@nzRT3Y!Vh`o=)tA=rj=`bAB@(g6(%=1QT}q zv_gx|V}+<9PT2TvBR|b4IduS0C#3;nkK&-oFEqVR)~R9axPR;f^Y{avtnN9bx<$^; zJvrsd_$io$(9sS7_$Fd>5I7_!TKN}-gYG9cD62ET%fCTl-1t0}gAxM=5mhCUfV z&Tbq<4wwplj()M-$LdhZ3*$$9abZ?|Vqp=6*ME1ZN9l21%LGT_j-<_SXd#bj_&izr%aofeu!PRkP^|SNn+!SH*=xjIx_Hn(|e3GeK3@phI_^Z+I1d;}oI#HaG8QX8JzVubX%Jg|rwq*3lMd^*%Nd5U9S# z+qp~xel?4;##+asQ*jI!>ANkMhhZua>+uYSI2gI?xLd$|0ENaj5Qg0~-6jc>03x5y zpU>^VOz@o6&R4d^vv+7nyno#lRwY@{q*BoYFVmDl@h$W<@cEHpLKTZ%ma12M<;5dX z6k$=Jk1w`vC27M5dha(@+0S=$-BFM?-{LuS?WK1k`0oO41Y{+nwYoao8fPOZ16Y>+ zWZt^{yGf~3DaR+}$Eg8o_t)Htg5S>>U6*7JKX%1Y zgn>U-Bl(;m(=hn(2(2uUDP2A&DbN;@U-CrOJ&fn7s$SE3#B4YVdjg&Ug7bC)9Hl?A zei(%@U@Le>B!(afd4BtMuuH!nrVf(SB8-1axy^>7_SUpW+SM_xQ`A%Ul>xINaco>1 zFgB=xNC6<0zx1F~L>&S#Jr+?I5cvacgVskENgiJvYkLnA6p$%_^BS2k4YHY97Jvo< z5Vru&CnLR5mwh&FGyCt|LyekDNB-K;{zpt0>8#q6_j^r+Jin0iaY)4z%5pT*xK5w8 zdQu)0GV?k=lr57(fix`!3+6+f;jDCTDXXwmKf;EUNa6h#%7)pxlahoAXiAh_g&SoN zCL)TyQ2AF{gVbnZtb(=SC!A;~dm%D}9dl6HYUS%g^&lSCX#q;W9(VxBl9~PD%p5F~ z3=X&m1;Rm#3-2xnx2a8k)(LU!HPdaGT-r8poS=Gv<0Rfn6@A0gKJLNPR`53XIIRH@ zsA*83iCk@^JNp7v;TLvB&7`MhDs0CwtNQR-%iy5735#$Qvxxg}Fy_Yt*;R4YF`p~f zWhIUCY}7t+;d_ZwmPetaKnz8Kf!@3;j;o5*b5k*J1bKA%n_vP3y@~a4cVEgA-fivya09&`GdBo?*yd<(@7MhrzvC8yBuG~c*Zul-s#K=r zI7D@es0##Y)PS3Pcyh8mKyp^=iS@u&>@!Fv0mW~_!#7zB?`4vPwhnH``F`f&TxXz| zYiZ^&K7y1N^8b~ired*-OH!l;g=L}nCrAM-{VL+npg)?o4orxp%V?cMl^$Mu_ zJE)997lCk#sz!#6{HSfinremhO@qgrGX%&6){oL)DpT&XQiQek4@ZJvOHwKS`DkK_ z(<#GZijfg(qk5Z|3yyzfAOja&ahxNi)xmbZ0!U`)*AYmNU6>R*-Q>gb)Y#2F=a~N9AR8#G;d*!}w z=TN_Fr_M>5li|hv4gTWTWLpHq&c~xNe&sKR=BPg4KNY*50(azF^%E<$&NwO7xR2(z zhc3(wH>SHn?}$}9X1KAD^aE5wo3cF(8kU~MS}+u1stQ%(e2A`3n-eJBoz7JDn%g-n zMSpgKP;cT9rx875%W8#Ju{cG0U2bDK@ID{&`F_|cNL$ZQ@YH7@{b?hI zuQd~qk3D-@yL4wE+W9snf-ZwsekmfD3y(Hf6KO)}4pn5vCg~7+Rh=tr-H@G>M}?*; z%!*?v4j@8QtFvpzL#YPd@s!Y2(-ZZ-ny1@;Cqpp79NYfA$o-qGpiRraR95t{gvJOn zHe^cmkM3Q!0dBRwy81mqk}q z8kmZvqlI>Q2A>8kO7g#Ecs=JZ`UUr}gNJ!0m=qng6U+De%HloG>67guGEj2z+1Gu* zWyh?YF$Hj}QYPu)5D&9hA7T&9VofBvR0rX*W_2W#?@&jntMYOmq%q)8h-CB8#o+|K1%;1+~dYN$7e~Vbi7xkq3@c zAMpnRRf*7rh&7R*_xA#-`>v=^CoD6sKP@lYJ1qI+TDgjQE}xxx_=Le!5Ts;^>fk?U zWO#6n!D%bww-lj?9w>W^iHdU5s;u|=*Ch(3KrNZ@TI_*6P%Z(c4+%7q9zEJ^F+C>Z z$e6`Xm&;G>zR7s~rr*S=+r%l%J}$vB*~2&d-MdW9ilo+~Dv=l7#Ht@XQ*Q9R84|3>)%|BdqHafZ$r{8Qt&eV%hg z8T1Nfzexa{dh#QUMA4p#l#ek$=mf;;zuEnsOrQ5ZVez;2%5kZij1_adN{s|wyv(?( zu17p<{Wo|dym!S#>gfNvYYU>GVp~H-nU0j_Du8XOT zkqN0Q?I&bkwROri3LQQ4zIt#ab_Hz_VGIO)`k=kO35NE+#%UcK?Eam3tPJcZ;y3L_ zesz^YSHu5FG}(lAb}^;M{pdyU>Voc|k9l18mdmzy&%{`(M|zzpMG0gUtW5O4T_~*i z-Hy;)bu>~hMmcfSqT7ToFO{Ubb0}olM)vvh{S4ND9xyhMxQCWaw5$dd%-26VSs(xD zt1>_B3bxTkf0jth+TgckTR`sK1hbSOE!#Z1Y!luQ1)`^R*Kh-%!)u>m!j*|7hxwDi z^NediBuDk`nm1ik{^aX_(ThnV8?9EuoR1n-z+(^Z(Yp1cZ8*2q7L5K2N61P-RqOX| zHQ0Kc@BM4qGbm!U^~CoalZtYxS2rRB7D!*Ppkz&6W2L3MAPohIBq)yZUPyt%C*-SM zH6_ZT`!lbZ+2l;bPQikg&M?^~MnO$lah4F$j!YzlmQh9bHI36mN}EdBpm)ccEfPId())2t`CpF-CA%2n`wUFh20FW>lMs zf>uQ0vWXI9Kf8-nuA7u>K4Y6 zR$l1x&ZP-nWGICqW{v_r>X~`P=Y4p1O3D8c?}KuXI$UOTMt<75svo_^;2${u=v{Elc3^kdwQ?IQ z^98V9cJnK|Lc_KS9V=!sY_E(#|ACFCU57F(KGV&`h+=juLfdE-KxNI{-LX_=711G6 zHVQS?=JWj1p7>Iecfw)ftcnr?Jmd58{~c-O7dt|$DK6VqgQ6K2MsQJ3k^xiIeS$-= zOa`Y~`^2fz5vMs*n20ss&gS0<168b{Ib-JjUwN?r<)xl!qQ&iD(4qUZXI6Z`yOWJjR2RS4E@Z;_4}9Hn zBy&}3(4UMi^+@E0DOHVm4p+B}gth%1(@thn4R!v752|Y-c*CVDHXe!t9~#{rg(T>J z8JdU3+}xa3)QOf>vQ}ea*6F0`p<7K$>-n=1Qd)$bFp4iM{9-WC9&*L zJZvNc=jWFg3`vx>4r z&tq|>lN3lJSt7x2vI&xe*mnd~oKYcnu(9qwNbBXjL34x`5QVQFiHYr; z&(q^=r?Z+u%x$RmUMI^H1!dR{_@R&0rtPjwVpYi$HFIAs-Mdf$#Q z;9`{LzaRYF(WP!87sI9)v@V2qA8Z=DprqOffDaD1T8e2T+ED52osxAu?ArC^_vJpG zz(5QxpF_za&F`ut&g;&2uUkd7I3~~O&2W%GNfELMETnqUDJ+DTC>WeL&lvHcPVf7^ zLqg`l{R;ed(o%9LI8;$eo74g>mjtyybWf6L(})Oq;_WZ9yV%Q=7^fh9kOpER1|#%= zA(;8@#01|$bs5l!|L%XsB-rd$ZpqPN&cPU^F7TBUj)K_|jf8c7RSL{BxbM4w@a2r;98l)#T+jXDl47k6>4fSCZvrDAatx zH!JA2++qCVYl>9EW9Ek#&Ybb6ghmup(!sUCX0TGg-9r%1VUO~c{cdFo82~deSWK|^ zYR|i#1&6@MN>jC}9Z5elX_#1R6{sg6EN~Dh36@yGZzL3Pol*G}7`9J=w(`Db4cYv{ zC!g$LBC4{M~*4Ec1CQr}k#ksG@8K?yZW2`v}#!r*5S}E_HL?0M*ZFQb`Pm4XQ=QO7TscV;p zz!JoO4HSM8$D6lC8N~p&27?g%{p<{ZsT62eAb>+z%U$&X1myv(qoJ0suxm8Q`baFr z-{()pz|Y+GKJ>47lSk-+zrq7dug-p8Sie4ymQ7&XWzRtdCsmO_B$a5xxVvq&n!zvt z>-)r2_8;<$F8*VJS4J7gEB#iLl_RH!b#L7uNpH(g&;7;KP8vbos##aFt+jDeZC|r} zA$uXZkhOScgifwiv3St-c`XT&Cbi$EP>Q|eSyF17Y7XN$t9@qq?n`yx#ZXeME^7{C4LwY2n%j4S~X-vJ%a z`ceNT8+oPrpSfGnLB7n8Yh8$-7^z;S37AD6A5{O`dBPdjI{4A`H-Id7VlU!k2wAq1 z9RXGDwI}{^r`Fn8>}NTRzDZe01|``W_Rk&=KkK)QYV$_H=Z-WTo(;WYwAp*lvfri1 zRTRIRw8+=(KKgX*KRuUk?9|%u^l;TExWysmLz9t-HQc zqY}ZvFOHj=G2(-z$IjOopPQa#Yir4A$*yU3KO*52H8G1->>j%H zPK%|Rpm~E#2PzDUNi&G_z<=FaJY_F)`!;V&4 zC)a`R{bBu9FY@dL-$tzCc-_V9k|D(_qapD0lnpquiad89;}_md&&uaYC?;ej&Y>_> z?Z;G&vzl5RGYa)|r0Q^9`o5{1kllo_7Zz0>0WFhF8^kn?+SOG!N_!_aXdtM$+-~&a z9nJlgW?aLSNj-aZ1t#TU-r_`JIF9;HbwZV9eMZN<;SSICPX?fdIZ)_2mID}Xh9n_w zZ&?V)BKy0R+B(Pezn7Q49T#1*ZzygwEx6X;!xUVTC%`l6rilKBZGbDdX4aLL=WnW- ziBI&@bJ{KBh`OKwOiWt`T;`!FRCo{EM95RLSQI^ghW9%P`snS7Mp;%vd;3tfD1CjK z4=o&_i1OliKp{iOhYvm}+vgJiermzDLjwO3#4PZJ4h3aUlxC14G{a;Z5~t{m7j=Ju z4uL8L{u0U-)gJA)Ly!`|_5RdXeVV`ER*}*IV%x{>!P~43hbPb{%5yV z=6sPt1VlL=SYG|GJ^PHxXE5>L~qLIJc(Z1yW z?rfntjOSW|KgfoS`gc)r(T8mtDuWC5v+k)de|OBgV1QB|@?2D9<@tlrZzgvJoNMhf zVS*ob+qeMN6+%Y`m`_0M|7F)5s+aOtiVzbJt%0fmP|}K#z0RAZ1?puW8ns4Y<=`%Y zknqmJv5>I%#>tU+R1f1U&;?}>%Mxir3MlU`{YS!y|PWSVM&Rd44LdEXj9+^KNkP|Ib+*~i+FprtG z(D?9oHY9yhNA9fn(lKjbz*!k3YVpE#zTq&*Pihbr;dZK)> z44Maqua(ju_UdJI4Wr;O!`I6!d0Q3SU*?;5UJ~JYtS!W}P0q1J) zJR&Q6u5c;*Y34=K@Ed7ugOXzAkgXk_Pnel&YHTZ8qU`LNTf16@o3i=(q@(Oxv)oHS z0t)W$B${2`gQaP&b^XA1TG=xm3EA1?Eso8tbw-7T3hAmw<31Xkwn}=gADYq+_wADx zX5GGKQtv(?^Qft*ZJD()R|$sw=x}shZUARnxv7m?1E!AwC4*Ia7`A&DG`sHAE4mw- zW30AMli3fq-JZs(g@5=Oaa!Kv>&xf3^aU(^T_ow;VrgB|l8J4nauJj$^loQfkCR=2 zNy8HE#rNi6-xhvpcmF;U+lId7cVyLjNo7g0c7lb1hg5j1AHGt}>i7kmdH}BnrCrTV zWp&D2+t(kP#?ec=WW!;f+XW zBIV5!bP?Lo6-rU;sr9lJ+}Lvb*->ZfWZyaXRW5sJw_7+u{3!xkKcTGR>4m8UbNQB4Q6TA_T?d$;H=T|5{cJwTXz$F_E^(nnoRp@Bip)8hnHVTic>}| zd3qmNx`C~+tNvBnzw$C#>*qJ`8K8G_yaqaWMu!>+tq~L)uXLIG?H6(uhM%dBtekII zCFQ`c>Sb@paj{5Q8j5zk>7?S3?|-(9f4jQUDRcxjq!ZWa7CF=}2JD%|lmq$FpWNmcVgf~$Ks zrA6=Bjdw?s|G)`;^@ot~H?Jn@!0?$0zXrnyZMp=AXizN)-feSzurdAf=g;SNK#CfG ziz&$K<#^%&lbuheYTnDV=ZltkD{dVZfAx^$N0qJ79%^-*5+sR* zKSgiWT@2BiWZCGGE!X*{F|qjJp-A!`m>3gXGz64kng3I+zU&y-wO&#shue-`BiNNs3IgNn)Lhr6H~)`7*x>0tywA->iJMc zC1MN3d`)%}8|rCSRF`y@7wZe+dUj$K9RC5+Z6O=y(KZIc5)L&k2LE*c>qW0dbASAC z@dHyYWgZ-59UbGw#sCz9Ix1gM!Zgdhc;q(4DNtoaD0WN0uZO4i{1&t`eAmizy1L;j zU_69r8X^4bD2uVFJBh}&wxJkNGfb)#XjUX`=^8hEDhg}Sw)sLFR5o;UlvZY0r>*v( zp`gB@Se+M&n<3kTC~&{MpG@Q1m_eq=_z4oXllAC`HdtgM+{o-gQVkZEvuzs5@)>+Q$W>#ST|1zns+ zfaAgCv-?fSPk+ujYAny2xvLZ*@V2w#yxe%?6vWO1Ecf?+Rti`Qhbfoo=hbz>Z5>m; zZQ_nag=FaCJ&=i=2azcRF`4HW1-ebJr?za?2dl>+eJ3Nc z#vvnXxquRJbrAHgx7wj_+2DKGHDLj~I%B`a`kk|$YCiZZ2tHcNE92jk5u@KSrSl|F z&|~;KnDNLv06l1hM(ufvkx}KZNuyct3L>g0fLrv-EfhxXM?TQ774|CC9Xt{MV;agrLt6X8MwyG(KZ&^SLY{#FWzTV?Z{I zpu_v}$HAZPjMRe)mrfF%bz;a04F0{&`9eg6I0epck>rDaRmE#*WoKScm>Fx!i*C9Fcs=d+IHm>1nh?`4 zOOQ$6=K^R1g@wBvoh<_s`-`TXGtxxxHM@f)9bFrGbt%cwAq8~(MbB35@%$f&Ak#^k z7H6YW`A76FKqS^`xCxMNKmK72|3&Y59xQKstuk0_bq#O#JaE|>&-VV{d%iE$anw(9 ze0+Q%wm&Ag@&o)0P=nODQbA%!-m;Cw*@#La58hT{3;Qf2g(VVIjQ9eB>B$+U$%#Fb zHr5udk$HkgB}9;Opuvc9*G@rOnV;X=y*vO&dlh1u;FCD#>k;JF(5%XDR)v% zR=*&1r$P|N2TB(yJG+xqHHE&XF3s=x;Ws#>xY_90lvn#zoqe0HEIwa>DcL1m-It`b zF7|DRwJMdHyxTRoDJ9}54q$=wNf*!8aPSrR-1B#TQ1(qah2*d}=Dzurcr=tiuPVB6 zmoFci>HUsKpLS;VH`aW8sT8xtsWfyVZ*YRzeEggqx-=PXfsiHr#Dfd__9W@6K*9^l z!b#@Zr=vrpBWLj&X)|Hct?bOW?}wQB%p%o&@A+HeBXo$E=w};uP7Res3lXZiwo#NB zZ=r#lFOtf&hAV7Ko5=H-NJPnlkBM%(LfL!m8p#AQaQ`9i0}+#@&!|ZaXcT4y^5cU< zKT~HyvQ?Zq-@Z!$V3C4BrXcJL<4>h6Q~H?-8@MRqJUI>3*f2?ut>Gw+_UV>voC4vLxMu+^kUlX(TVZyWPm(1ER~r}>VOFv z@|6WZz3e(3S^J1@+$-Mo@c+(0!zfq`0KYUlq3g$9?z+W~?a_{&n3rj$awo<~6o^qf z7w%=jC?-uUOAwF>!i@~kf>uHxdq7B6`<+mAd@(cTs;g3wtA~gFV(YO44UMmj{%UKJ z9~L7syny}vg814{;)IZ}cdrE>1GY2@I)sk+^1A7=xttPKD$Vpv1AJWF+YX***j}@JPc=u znI5yDr*x~u;d1){peSbhA#N+SPGCFN7EoN0$c`ZL5*H%~x_Tw?C@%J5Htx9lzvok-s*-wKXXAnd!#9y2oXZSd6aym}xl!pPaGRDDT$Zsn9mJmqH8T$^d@t#R*zXSV&A3K??BJ zHZW?Zy9v*2+bRA|1aLF?TYrNgNc~mN=eR5QI_{~f`E`;R(u5aY)i_5`US#Er;SXfUB^_3An48x!R5SRgFgfr&Zo1P8CJkSv1 zmQKKo%8->Jd1-N1kN71S4m^PfewX)m6jLx)KQxW4k5y2#>`$Bi6-W!r~ z{7mN;)FV$|0X3OuI_*n#kSNrJH7myelG7evTnU4@X-N||oN1=OW%VQ#?$xp>``jZS z0O{loR@Rzqw8P8iZkPw^jy_#aZBVulAV*rR<6oD=*g9Gdit zjQ@RY(-e$9^Yp96(O-A}RSy7uZgH(dQnyuau`+;995~45v-1auTMnhoO{@E#iL)vG z)0`+@vDf!5S7rV(Uh#k3QhWGp+Ov44{jv%MB7V4i=u_6Y+V0A2x($wi!Jh&qa`qyTkaNe^0?J5AtM|9|ebAB2RNd&r-@_t+Sxab#n;d~zhcHw2O zaFn^~mH)pBR^EXI03d#|F;Oka%E<;HWq@h}GN4l5y~778p`7_$qYoeB6EmV8dKpeX zfIbx1v^v+5$Svjo9QUfKr(lIny3szzrp#cq4!)Z`g^hN{L65dBcKD^57#{?^%`2~* zeQnpYFxW(`295NUe16{Y^oM&014D!vIr+CQebyf+pFcS_)9qbz*ST1&z2Lf~JgW5k zHJ@g0nj^fg7`dLrpx^``e=2Eh9Ds{?naDg2X!gq`xqL!mmLeX9Hz5qfhcW;IxDZ*l z6jDZp-ZOxR&2N2gP!no>le>>ht=n0<#6e^cvv3#_nz!<5u$h{{PkhZ)_n-gR2CBE# z^Y}lC^b4bneZ&lkpUz1**-U>zV@f2p*u-O>`+gc4$VK~JgbabeD~lKE`II+=kmsL& z1Dlp9pF2ns+S*>e7kjW&;^`BG)W0xQhLk`fB_SoiY_iLy+!--7xB2UPTET?G)^_xD z7JU{Ubw*5yfW`uAjC8lq=k%F$T3$$6mYBqki(~GcMc=`Tj_6%4S<|ezS@j_HzGHsj zVp^wVT9=y(9Qjv%{X+%vfK0{7_URF0ltwrEL@EWaQkkQRe7|n|SQd|!xQ%IephlS; z=e(H4LyT>P9Yi|tqIL;$B$nzOt!nJ}{NJ0q^11$>!6OU{<{)9~eZ=4X%QWfx2b>fd z{yOQlU=H~Q@({m2RR4a~{3_0ELhv5Ii7F+%r3Vu_XpBm(vE%4a!99ZKmoWts^pm3d zrt^1AIZ=fe$c&lK=bz!X!*TdE<;pen60Zsu`;L15o5*^VKOd^e-HRU%!|Bd?uY$&L z3gM*JsOW3$|HZZ8LZUeQtL*t3I%2<-yjjtW?QP!daU67+Vn%V268v@3C}&FA!Bs}+ z{D+@#(>e+Ko;2IPz?0`B)D0`K$&jwHdSA}iykyR|Lxj2iyNX;6sTP;XN029(7oQ$h zOB){ki2gT-mulTSWAYYl=b~X?D@4j>SjGM{#>_b-0D~bss%WpDMgft!N!Rt3$JB#h z*ER-Ltf%I}jx`2V{@@t%&9LNH` z!1koa)EU`mf*8^kqCjTCG1**nH|JPG5HjOU%IftGf3u70IUV#I_ z=&q+#_&ShRl$o^RU%AR;Jh(ggmg#+>JyO**$V1Lv~fQVP?{N^{)fDbb+I&2pOCHHA?(0ue~iTjQE=`vuL%$*q?;pf7^5P!+ctm$35K z$PpZ^)*~3%SraPO<#gHm_6(Fnn=iX}yi5lNowqYs?~Qan!uVj}yU8~VV;O6a)ppC? z(~ds%pjqMZ!h_svKt>JfU|xzD205ZRUIC&F0kT-{@i2CfbdYosHYOryeL&YE200MZ zFVT67vn9WkMen(j9k=ty-^WU6g_Kg|vxf%cdB2AlD>~xyU;&!S)n_r!^;?mv#75=r zG|i=uu`+}$1QE3k04Sqg)V!1*+(hq*u+gfj-G}7pL)>NL$U`D!QPPC0LJm1Kijp54 zr-XN0J?1uU?0UU^n6=|n=dNAgI1qx}JC=mMiK*X8;f=|Z@Ge?>Z%Ulr^{cDUZa5y> zKf^MdBPZ1a*u-dgv%F5)%thG5MdZa}W7!~{7|I49%S~1a3LIR|nY&a(t$2P5J*Ri| z__lsiDPJR4B#I|QBa6pMRGmxMm)=o1E`22vu4?^h9+`UFaCfP?&*Yc+H{JkXZXgW9 ziKXDrPJUzJ9PqWob+O1-Ab-V^Qq?R>BQSx34^VKq`#j8wJ$iu-eNRPLeBasUOA(q1 zy(8l0L&wbDdu~1r9aAu5voe#%>*v~9;>}KU9an45%8C^hau(iaORJWJMC)Z=Qc;Q5 zN#B=g^wwd*N+|a7Z^L=+LIPALhx>|OXO&Kb@D7GPRQ5Fj8eI``*ndHZ_T1)1Gp3bY(yIBos#^d0Jd|e&hf8ZGx6NACm zU}5#_=+($uCuH|nbNa3Q!tz*b(C!W(r%c7-u>+YF^ z;rHZf`m$W6sYx98@F!!i&radyw8#)3AtDx6T;kO+MxL zEOPbO#3@$jrfc<_AeX*NB4(@Anh+rZ5+Xnr6_{h}wh@ZpYiXV0U{!CohzJUvQ;b=W z7|o{V=32jg9R=dvLF7P@I?nT7ZW&{&-~s?buBx;}pUKV^_}-8ExDTpI*9i|~;5cl1 zJ`vn>TVw2CY3@c=&+ye|=TlL%D<%7FCv8Uhj5`%zMA);MJSFAVb}Tj^+|}wubb6^L zeTKAnj~yvHacrYq@nM6nOf@{@VEx`xMkoAWS=Js4n`OptibM(Z3vqP+qQF|vj1C#3 zF2dv`#c6|EQ!)oLB}FfZFmM`u#hZbM!^<^Msb%EkMhqm?cP<$~3VYu(`b*QBc5KKhK%U{R&`Z zfxwDg=dn;(v zh$e42STwb@tvKjq$sEh!yDN5`DZk~vQledvo#}azad@pB8+=JBqZ)ZXk6p=T?ephj zADgIX%eQ$rn8XutL2sZ}yIXRy&UB_`I%Bojf@R^S%axoE6>=T5!r zT7C0wok5aN^(PqVXqu$fr(>C?=C9T+>0)=MY=rITzb!6#%s;_3e8p&rQ>h#OVB%3@ z=q-5W9iK8YWb2~bq66k+7Duj$q ztyoO=drn%h=YD^YlwAiS{W?eETYb?nnG}?Nts`~t=1*t^4!*bT1AJ~a3;hG48R3az zFckz|d|-l{J?%n1;U||-QYqK()WEMW*Zi&+2_?oc<1IKy(pJLishvEX`()_G^LNS@OZ{rjBh#l- zp+T?P*{-egeWkpNXkUuusfvH{h~-zouevL9@70};ZjqYqq&9iGMb;Ufl;i7%r>pC$ zzb`}%-TPdr175Ai9`Y5ga(0X&X?h!`IV8YUT*UxWKX;*{XEt)x?|DTol^irPY~|RS z2ja=|=gp#y+%1cwK&GXGLu_Ou4rIU-H1a{b-P(~52?|+iJ z?zl7B(A+__;A zq6z%$X03{uKNkP;HNiLqH3X&LHny;`y1#P`0=22Dv`toK&!@DW)$ZBq<&KpZP<|{f zFO{BjH&V?|Det6=(|~c>JRmh?YW}KThucyYg4K#ni@kzKezhEQzy z304<<@mMl!L4u7eS6&`<>k{L`{cha=n-LKj{Aob`yU+Kw$p)5pDHDjk)v3IJvjn-$ zOv%!tPgeCO@zXu$pHG82+}iGXcY3bXyWhDcBattT=u;+LT@w+g-d(?3^>l{r;*=$a-hODX)Hf`)dJed{Ve+{=wkWu!HNQC`({dwGns%CFb4S z;;wCU3zasd8&L{#^vw|p7Hh9oDQld#x{|kZ2#_vv3E64|E1s2=m5QET2Jz)j^~)Zf z!3~!@bc0%A*KJGq-gcw&x9{TCriXg(KWgYGMOy0ht~=lPwnwn!@JNNfwh~%io_)3a zT~qSv%m)zZNc5}tP$oKQ^geO)Lt8@@4ZUW(h7$#b91V3;!Y-~yw?pwKdt!n+q8D#2 zt`3imQzmo*Yec)GI!Zsh((LH*(wlasHr{N6d+Ag6Su^>6OjB8TQ{kom+#_Q#RUJTA zu45JZK3zOX9O4la9I*JX;OXg^3YG*Y2%*w<-L0%PxxtXncsp>B2M_qRK;&h>`1o? z4}=5W9IBMfN{f~4?1pA^DXt7)DM`Pab+6Qn8j=q5lZeY_2Q)YExItEV(ZbIAT@l`% zPm?gdNKDuDb+wrjEop1BChh3G9vQk{`)fZ%_#Zx0;P@w~1z zslzrWwj@H!O$&~(VU_685Sgr8U)$IlTEz1EQmlHn{&<_{pzEBj-Pad{iuLyPQZV`3 zHhmM`xqh~`;-g2f#r^tTNmFPkU3q!XOBh6F`lnFiH-gyR)o_E`3jFZB9=h`uS~^Gi;9AY&r~H6bn=>@4nULy}eO&srnpm<7 z0>8~DRdt}>17bJ(2>WmBfODVa?kCgZJHu6&8PAmX#4)7vrs|RILDxo)+Qj2tFKtel z#+@3)Ws@YQq%!8RK;(<>eRL+U-HmwS`5<&d$!Kp~_t#&MTeW?KP`ogZZx?d9C1Zl{7Y>r9)O- zp6|7lm)m5JbH1I~1($D9IAcPDc?Pf8MBb9anzux8dM}K`R9H-J|FbWBVpr2QoIMY+ zrI)qR*1b6giK-<-aWV1AtO@GsT*<9}zf0KK?lSI(#^%S|5qInKQ@sjM^g=j)EcFUM zsxMrJfs;b?VIwT+-=-9d?V0W&LJI(i;M&l`LrG81Mi;m4O*dqL0^2_*VYT+xI6y3U zcZUuCr1JX=sE~L`0Y=-iqkW%`EMG^kcQU8E0+E zVNwAn1{P1F)a%%uysg!cS?uSpD`tLrcIS9Dix;VN{)KE^2)=dn={&nb<0LH~YiyIK zf|qt5;I4s438hq&mXX0E$QM+O#B9^IW?%|>t6DQ4Ro2`8`7Zt}iw=AIxCJ|TG>Cr# zINpYa2E#VDNT8Z0j|e&1@gD!aG(T~GjY)u?Ush;nsAM}U-N3Pb`JO;{ zGOm0wZk6#Ps-Zt@d;uaiwPMOWCSuuEe5uS-aps}~ed>|G6I4Q?dqL~BlK%S=yw)&D z1jfWc3fTY;+ARxH-ZHe4BrX`$<2-M*HE3l&U}ZnM^?^?2F_O3p5_y8Xtw-&F>O>8n z1@`=Lj_Wats|u#`q(9YNIWKE;gNZA0WwInw-4$19rCOg#m>91cLkz9f{-|}eX{bA> zb+3GRz*2|uc478b-x13hbW%spN#H4SChZ22?`#Id`7Ye0TKK8gyQcTMz`LH=yAZWj zT3*+5Q1AO?z1E_^^~?I70xr)Mh)1lJhy%*v@xb=~q3J85qTsr=73uC~=oINr>F!2K zy1VnHJBLnb0ZEYtr8`F)q!B3*0qOKRywCc6xJ1`5Yi8i=v-h>HurHPsiB-#ZXc^cI zpA4PfdjHei{dZII#`Yt9fx;-h0n^wnMpBBJrJ-=`tD^cBITuBcRP4atB}>l7ex0A! zwjNUkrQ`+)lok#!%WuSDvMLwamPF3KFC90BE!Z?JSvh?kja0xsyhE?lr(B%ksW-?l}BGd)t|KIuB7vULQLco-0DIs z!oNve+Aae>h_`$fCvK&J%F<2yheoLiq`rbu>LRYhas-VEjM~I*XAJ&HAG~cCEVq&} zs8ZaMA5YzlH!@mwEsU%kSZ|(u3qDKhk3p6nJNKIUj=gmG)9BRb@&@>xzMnj&?JeDr$#bmg6AD}M@&kfjmn;qM9&DiJ1Ia|;XUipHO! z=s}_pVAz06IgrcPwms+A27FNciz9KdH)H7SW8_`TIr{#H+|Z2{a*C{z5QRnMFj7@- z*{HvX+TxRVC3Z7K;6^5PEtt6Vmy{hUC3bXc1Q(kvUvs@8k@GP<7PU{^z19jN7N;1$ zMOE%z`_uTLp|SDo57jtC)2T%4aqGV|`Ry8VioY)Z^Vt+E9(4;67jQ06S473KJQ>6ayZ0*GrZmS< zC+O3N#EktgKgS=ajrk!C0)co22Pf6CfRBbpR8WBl8X)#uh+aaf!~LgFp>CZsg!1IQ zp{xcI+4vsFYyJJ_k9NKOI2hA={L>fbhQf&*6J?jFB0;1Y^MKJFy#mYNUU~c&&8cWy z+%>q~LFpnUAsOHEWT{oITiNL_+iTJ;UA4F9c9krdeL<%F^Mhg1sdM(cE;`)9zxCrF zaMcG|PD3GzwC_CVc9~~f>XR_xJ~Of7_S^QDt>#4w@k}1?%Waj{?+)0w>6_r>?R@eJ z$OHk<@n)eL=cJCBQU^9^J#I_xA0CNOHnKdO`YVms?$1g<Kr4HhIA#i3IaBLRVf|0eIbo^hsa%!7jkl1h}@=(4mAuv z9IqiS2IKe)5Tg=9QNwG|FlBHBi58Tyg5~YR?0B-<$828x$d1uOM8y&(rY~+%%tg+w z$>7ohZ!@X}@dnP2kAQ$CT~>$Q1re%-YM0fd#npkAVr7(UGVvwp096jD6jzKDS6a#- z(#>sH_Bv_Cf4zc>oH>WP*^(`vEus}GjmvIvyx)nFLc+=XQEyXjIu(XWGJ8*ag@<}^`t~g?2V?k?bBFKdVLQ^+sqhfJq%hetj zVm=?7(XmC&-rWNnBIQbMOKvBKH1fcI-hphyec7z-TpXi>aXR%lB>12E`YbvpMX{ji zY|sYH(0?&*)(ibyn7g~X9iBVrDk>`0AHRo&;ZL_~Br2#?YKP4`esZU*mJC3-jdEir zriY{wq>kSGbmpSL3B^I4Y`ML%`)D*WGLj3XVoeq<0fCYIeU|d_@}%TsZXTWmxR{P! zmNk!(M2VfXHR5E;oKKM~UH{S&DGUZnN=>~g5q|~#3j4L1LsizQ#Co?wkoBELi-+`U z8e%%niBKx0^H-nn{LHVxB`L?-SDgOD}jqC{xfEZ<3N%!>T>b0A%BzKJkP30ji?|^1MMUdB5oRwNCFx{>Z@4P|YqC zp_~-LqI@KZsHoV|M)fO3*x4Dw4-be$@L1&C)oJC=UyF6Yd!id#1JM`{N8Vcbv-!H1 zv0Hb@%r{z^GH2N7-(#c{T+;($D=Qhn+iQlp40OB1%-c-DG1^__t;}MyenV(GefB*htX-o`-Lak-WLP)jNkyBA- zOKjI^vy`_2);j}Y8=rsX++izjj_zDJ(yD{k7^skz%?w1%PU}GnUZOQ{Fn9Ll|M(S+ z2rU=sLQlbF!w^|#?4nOM(?Y&?>lxwnQi;P{v6;&zeU^MbCV`pAn5rbOH~RQ%-q?9Z z*E@Hw;h)RVh&jXrAEp>{IY~3f?eCZlq7C-jW-g#ZVqJ0zg2nUz1C>Ms6tN412z!6ZnkOEou@**SLo| zt_Xk~^#`{PAUiI4fZfY0hm^X6miQy?j~g*8)KZ#CjcOQhJW5PXPF>nCuQtP(Qu^r` zZWIezXZ(2CFax_zld5_9^J!5qyrWKwVv77DDj8&eJJ|5~n_(4ojcd}DE;`O1c+c}? z>9Wc3T0H}jOJq5gR<^{C+WKZnmKsj;&q(R;nelS7GdyaS0zq63p@ zF(8lLSbcCZv+td0=yHz?yKNcEoHOV))CG?%j2t@@A!%9ZlmQU2;}COxb!g+@bxJR>*W)t^649hi49Fm zem<29P>ox?%=P2e#ksk|8x8>+o0*ZGj#pJx1#}@(Gc!DrlCx7&s*FezsWn;w7$;k`mQpj?YE?qSn?J0$Q;D4nK73_&1jW-mXEQq|!^R%+!P$)BB zEd?N&M*<0DX_GsOO!(f*E>HCppq7@+q}x_jAva=4Tm1^>K}`e@rV!Y{`4gJ4byZ_B zOO6@Xr%|_9Wzt!L`!!L{?@9^9GS{$NIBRfaq0WuiV|dYNiWn8;7q zII(JUHpo1xmnOVfQg?&f=;m9OlB4VgFHV$HR(ou zjY(cN3oIDC#PPWbmCLpdT1Dkh9*_EwVTaf^FS#2yk#0Ws6z)t^a_eV*R84XqEmNqQ z|5348X6$0&fW$}Ad=ucSQ@m*P>d9}eiEEJ}1NFR;%Ucr>1D|XX>qs(8I@7lP zPN2S=ELYWF;=)Q*?^bJGEj4tBG_YfXZ`~qgQA`r&fQK1Rcpi9$C%&YgBKF4bQmdce ze*a^>nICiQHN4pS=VdGI8t?xWDRm=oyiwRm+4de%<<_n+L56lF&!O}v7M3mi!TB@A4_RuH+$+$BrKf{Io`aX*5RPu z>Iy$=1HBeFZiskHOl3wOD6jcu66}U9NA@pM&3rPjOKqe20U`#G2qP}O>f<`nV6VZq0I_PE+9YwaSCqP=PaSX$FrFJ`mtH`!CCoP1!|Dg1`>1&ropHQa1%>g z+Za$sK_$HXc@CZctaje+T`p;UdX~wBX|S@Da4!}~ek1&Kc-AAuEBFR?3qc)&h4Yi8MkWcElo{dK+72T-hq2HW#@}!S{Vz7A5*Fv*jzeZ^7KasxbhbS>%=@NxCUXGxeJqL;AGaL0pFH5u;*<6a(nmZ#*nd}cHb{`c{Fja~ zW&0VZN4e&P7OuY8s%+7j8G&`fp`BB%ovQq;HNU(K zEdwBM5|7tb-@bhV7f5b)^jp%UUIoxzrXj)26Q&M4>;UgIQq^T!E@k8qefo687v9N&Mi=kc1FdwcV_D{mnp0jqN=-$wyHnq@dA^E_J!yt z>!#MaW^lqf25{VLbEkazIm&K*MtCK3%0<3c#DMX2N+>>EsUAPm;>&f=TT94qK$^L#~&{YBQwwewZ8Cj_&OPYGM*5I1fBkLRY*p)<5 z!*{9DeA#zpw2pq#aT2Ut+}u)ZS!J*TfL^G6GR8QGlX6zTCf z?m9qe0r#!YD^3ydK~nXVMCCmPk3{#=>XoAfd%pb(k?Yc%=8xSsn*(;^gYK)dcdL^z zum#BYy?rSUUGVm)5Evm>J3Jx*3<@0ek3avDc;cTtjji3L*7h0??CT?>U4yZ?K9*u%CH z?*=loZ{LW)q$S&yR{rWG_=1v(su`FVJU&XGpr8N)xTqrAEcNc>y@0r;K!p-n^l(l@ zo_odl>KyG1^=dLIs^J#)Or6&GnA29A5KyzAr!C)HXdzCwZMAy$o_U7VGO(-f2-s22 z25E{F-#L(jI}0#DK%;ppN(xq4pNyZPz#OyCl?L~nZXHW6k)+?Tp&zNoqO7c}2A!k6 z(PB~6fR5XK2YoBhxm2tid0za|GYfn00Ty)equaO>(D(FYnEgL@n=cj5!%nV@;?`?w z*%}T3G6f3X5(smya{HPt$I!ej1nvirBeiCR13#N1jIMCVNEziWyO%`L3JhU&@a?9A zJMy_uOk*;+%@0lZQetCc13Al)UrM}eS)-5FvM9>a-05`N=YMaOQ;)kOUZ7u|;FdE! z9Hg4`9zZzxRqt5v2bC>SRIRLc=ROd2!OFBaw8S{JJFuCt@wIG*2ZX21 z!mVE^rH#k*p@Eh1mH0{K3uygIaq2t`WzV{*G8FgXc8KQ|prqX4v~T!o_0-kL&jSVM z$+8CGVj9p1pLgNWOYkJ{i^Mvn(1j=+bxd8Vr)zhr@icI~iAz^!d#}9N#6jGym_*3c zau#QZJ?#5F`_TD#QHZ5&o+x#kJ5p6y24ra>!IW`l)!yzf&HrGEX*&2?=X{*`U9thH z?ys8qi(%@EL}P8u5X@+5-XZBLA`5Y7om{;x7?n!dB+8AFgXQU~Py)DBBdwWI^NQru z)XPwdT+7EF-$qUsA*VR;@-}FWq4;_cAz8-6=aCPNO~0%3B<;fSEt*cYYJ)5)=1ZJG zMXZFe7+aYs6GxSHN1OlZpuUWC4_?k<+fk16g)_RMqtlar*FAaQS7S_9#G6%rXJ?Bm zpHX31eut%=;B%Q`yk9$NBE<|{nEU!gH!o`pAH12L%y{~k!!9}!Y%QIp)vKEE%-W># zA0)Mte{OvoaPa^3sgdG;t%a+Z#eL*yLZc$g0L_m`kBtbNFkuOas+FU%Ly?-fp)VjF zy|xw@q+wJ_=94i8X&c{akgep&-gw8qj}h+@6Itg@0o!I4{MvMM5e z28iA+P_F#Ts%B0ryP;bKVN*FRbm8bac8FWRP2QFj%6`%0BKmdCsl1l0a}_YO3e~Ft zqBP>en4JY#%BK>ew!dwAznJ2{H!);M&Z`4cRaLbhl@C)e?4AGOLFv3wrCsCh?k`)Z zJ>+wUil?BZidjBB7WcQ(@mAt+6?S5D{IBJt>_;-_7gJAKD(D9$vx}>@>QtT1|EL3` zTpQ5cEmy=%;FaH<7hL_^O)+SR=Uaz)`9ewx!$Qo0(}N{6%<;DphtAmJo(~8vi#x3d z0Mb`h7Eyyq=fKymgy6X1fFLeF=eU|?L#G9f{vpvY7+=UDVBmdrr;IgZADNoM+}_>> zT^{HKz#ixUE#T+;N8@@4UfC<`b?S5~P*uJ<_D{UEzLbLdfJ5egzK;h=;X{2NIJAv$ zaA?8z0vXJXXJ^>GPi&mal~|)>r`soEaC{v zBCjp8cwx45Dxbxfr^+J$$Psz6Fa#6^z!Z!Y#@X8vE2N5989zBa>D)S;_xRUSws|9Q zMa*_dfyg} zi58*rtkv<)sGyaVOcAPrwJ0W==S2I}&zAU%J``XqJF6dNX?>_+UW~u1SbA>QStix-A6w|K!t5DK6z`;Y7=q z{8rpw?b!Y04o)g7=3TwC8)2s*3pZk?vSk|Va7TV1o&*L6D{b?9$zXJrmq$2W9Xu9& zCA$NR5W&w$o7H`eNA*@-T{3p^m)L!1)RvpXlCOHV+euhde~<(cEg%Z0cc&bGRT^^U zL5t2UeTN>7i(Xq*%0fPHrYYiMTuGmDnedy6|3FycphB6AV{Tx#k_AByrWu`L+r6~N zvr|rho$5$w&~sU65YerSaE%?9*InHb`2t zG%BHQ`EEaV;dpXV#|#kQ_1rnWp1 zm0!&XsTiVj%g|U8rY6wwRcu?roxUc>`Cd(|n%4c@Z%TG}%~XSI-HMMpRa;w1bJ8u) z>zMra6ip%4MtKp^MnBJwWQwHrT#n^^LPS|D4N?od{7s#v1c#@;?%sT8qA#453THwU zR(9M#7lqz1q`kdS+Fd4r-FnM~7sUQd}DVa-P+!teH#Sz52)GH%S z-|Vv!ejjHWuDI$rlIOIK5i9|LImqI%xZhvszvMVsV+%?j9rYNJ~=6W#=etJ)F{a-pb_ zVOq_#hO5-;Tifl?uB`Y83xBU%!SekGx(Xg~aW!BEcpTEx%|aIfc(YXwm(!GN2@CB9 zPNX9QKYv@S_bGPMvw!SIx{oVN-hE8jwlE^jQzagptAW>*LIo2I+1EFaFo4IX{rXm% z?Z+rGSZ;CguVWz!%Hzdf7Z$5-`$abY;$&Pa7G?55dv2bxj9KO;b^<-D%MtX` zo3lH)0h*Y`kgT_)gYeJ4PUyBFn1iH!k3O*<0RlLQTO6|$B&iZtZ1>-J@~%NR`~H>? zw)TitW^`lWd5D>;!y_9ocE5Ij(;NNiay73R^PmLu(z?33r+qUhW1oz7%2OqNfc7W^ zilZf&qh6g?RP;c2K#@91MnNHCYG(GB2u8MsLog!(yhaH;erqrm1bF?&T-Z8n$&ER< z{||zh4sG9Ww=v4C`VKDxRzJYgG%s)K_Ttc_!~Hoeq9{Pe!Lg5wd;y5R&4!pSS7Rt# zqy(fJPdRxo_av8ph;{4;qtAb{<9_|)mTnF`NoCw-^G(-G)Z5KkHX_!8s5el0xAQ#J zs?UqN0gC3Qnd3VFLO2`eCp_O=hHt zjYeaTa1F#q@W}3B6Z8Q^@z@G$00i6d{ekF`5>`gvsed7;NdU3OlRjrQ`Sst6bk`1| z_pk#RaU3~4ZttQf2g`$79l;y9gE++v%U*Q@<-gn8)?UJ3kEzY!<>|R#-T~FVG?1WQ z@Z-`Lgx2i)d7CMJXYPzuGGA!}n?lh0?{N&Co!krv)&xts8KWHff1V-0#H8v@l+KZz zVst?Jnr0aFpP_poRbgPB*-_MlQuI+TQ!2JdYd1t+6_*#{#)&Vz*)5^o9=*ag-$3Q% zZ}>K(ecW}t--pJvHkVuTMS2vX7<|}O@Zf6#pEBrX7Xz-}58t}R%CFGBWn*9`+KByf zQ1->k`L``qw|<2ltEaoBl~?0*=zRYLQa0-xA6dR1-gR;6%He{Ou7g2pjh=Gm%U5hZ zvGS9nIk+$>ofiuvh{c=-Z7Z3niOw#Rf5#tWT*I_~45EOZ1Tc9L)9`0;e6?Am!ADlT zQt{>`tfp9#Bm4WhQ*$%CQupjxd_#C`M8s>lQXu3O(Uc{Ti)?%O&w7&RyMg`!*n~^kw*^#3N|QedU0>i^V!luPwLb0H~zd8bnpf8;&_0cE0RQ0qQW_v~kv;WkIUI@`oeu=cJR!Ra9wG zMg4vrL9YTo@vJGQ%Xd!vc?(79#@=aD63ZLpd!>8fy>Rt`%diAx7R#KX$L@L+PG+Wi z&&jsV(i3+bdpJOIKvCq>z5=p7z#1z!m=YHJ;YJwH;orqPV$$tY2KJ^9dH^2YRfw=cGbt=J=9#IfbNq^iL};#`XG5lk`P& z1=BwQu`V-oOv6L1hM=G=AV`zzQ@VCxAEc2h=IiD!K5Nv?UBM(mI zH+&o*QFA*FZeKZSKLA>rOnkV{NeIq{2F=hh%E4U#URLF{Pza;;>=`K)l?_1A!Jz_+ zO@I<<0L2=Z>_m#djy^+{ZoVXl2|BKk-Ur4ae=%TS^1A2tSKZ&AKclR9##2Mj>H99~ z-I_Z)$ITxW&4DE}GOlX?T2K66!m7X~n(~dljbnB5ZosaT`Ne?U?=d*&{lTDW~ezp)q8Oxot~w;{7oV?587XaDN?g!*@HJa7PQ_HQT2rP4iMIp808l#De(fEMa`#?-EF5dVf4 z-@eT_7);?o6cnu$6xXP@%Z-`0>-+Fz@n$lbfw)5I=gpsGaEJoKhF~)iBUr^9T!% zgDsG4x=k_6>Ww`k*+j-!NNRW|J(Vk z%SZugx8)9wBpal1GNl2;NnOPe{)}vSwuZOE5iWhqdO4yv%fU_SWiX#SDtIpIzM=fP z(*kwJ3Ve~84*3CX+%Gd)IerojC*%4!a$0#W_mgZHm2_xb9V+mpDA9CA`4AiSyi7cd zR(zI3+}%x5gGZQ)a6_goC%>MAycaM@Q5MIZ6o|FSDKFEg-?1dm$Ydl_s+&?Xp3*Q@ zP09PYAi}CLIQMSd_s06;V36O6F1du2aMl5Lw6`{+1M+D_hP)v*dM$c#)#!5Z;L4Rq zrW40UE2&OKRU(*f{2Qq}Mzt(MRVh@Zy6w~TUM6ZW=TcV9vBRGWY*h_u%JvlmU;*VaJmjKBI(*sQhK^&bylrS}3MpmG22;fmUN*c=^Ij!CaY znu?QQmGl$$;um-l50&AB8vo+i-Xo^ZeK4EqOj=0^6}GHj?d^){#YAO6bcKut<^nxq z-o0ZT0pX{Kno?CDbfvw0bbK65et+(RE*r>#H!?J|@bLj_{djyw0W3?yVg%sO?y>^B z&RUwA=c!R%TP#?ELkgTl+(FXg#11}ns6P&^Hs~WFhleK_pk#!Mq;;5+KD3AlbncV| zP^jsjjj+_9D_1Y~CZ%c^15}+?P;wUB_}`t0@fp`4)1!C)TKjQj`R26y3A|_k009ou&{arwHbU%f@g5NG ztCVl?h;)rzUjx!4#1uZb*2(Pb5<1r3FT+EYcpP%9*I?EwKgD+{lY9uAg`Rd)02~6~ z+zN>P;X4&0kh>wS6_KO!m|Dn~h{gzL`K~KuhWVFd=gz3b`g--_pMOk06FlmBw__4QFtr|uPIA8!T zB#Z{~36Pf;EgepMzlpNi<&y|B;45yQr$IQaEEDvtj}K9EOAEL*n82v^wBs`{3y22q zF!W~99Q@S6%UCL44yv%<%-5~5UdnRuZC&zj95A90o;IVtzQk~!JLv9M1!y62tK#vN zdC5l{_RREUcL?($+)Z;*4i}ER6+f-?{0X?9<7-xfwpaXgnJuq9%mmR;*uOCm?L>UH{?|B8A8^K%L>0p~0963fB2*fyJW@xRfgKoQT|mG&z$VPuN$` z5&!Pq-rX`=o6E<(W)HT5g1Au+lbqOue>`16#phX|8eEudHFG|?+KY~ciXT91RV|F#ID>< zVV@`G@x2WQ=LL>??l`DJPo6*N*kHL3vaRKNu{uClVSt9{kZ4aZ^MwuL9^9SIU{{)@ ze&4a-V1ETR%Qz5;pCF}%lA0QjSC#k4g1`}wE}b*Cro11C+~K-k@i7HcY;5VPFJOWJ zw~oC4IVB|oP)x_Hc^>R$P8Wdy>QZi#&&Y#Zy_?gqeg2}K+u;076!}@NE5(LOT3euM z3hIdG^8gY-y$`t&L^^Q?=(Qa0`Nbg;zy6K~f}YQYuZ}BSaI6Bsl8X*wyGoi4@>^Yq zO}OV@)8N_>q?kDu0^W>{(y$a9NuQktbDJBxDAI&Vs*dC$DY2z+E&%7?OnR^ z`=NT8;tHaQSuR3ng}?jnMird;d2LnSx=P!Lk@N@>V-z|*x?T!9&4a*=au>#{5VXH0urk!Y!&EhuLSQC}?S5k_vFx7qCE6iN zTHk^Uy{o}8W@DI-kPpj*q*g5l2i`5_|5Z+hLC1=Gn?6u65M?K;2}s_^bDLiOb*~<6 zbQ1)Pv5G*Y`%%RBd9J!xl_e?N{p0hky4na|xYbem^opP8Z%Wahu!t<9qWMC3p3*eF zPc#r-#gba|W$}``6{_{VJ$LO?=u8rBdqn567tk!!3EtnX*wi@YqN9nNP{4pB1a8~E zM9)%ug^3e^hMSz8o?cZ$!^()bkWMl-m2AJUZjntvK>+|hz=@=`7C&{g)B6Db2|xjx zHE^C!FIW6DGYLsJR)f(GDDDjii7G1ASwO@`j)p`)?DVw8d)<`NkQ_i40%&kvem+R1 z4xSy&d9D>ge9+y}BF&bCaJ{Uw0ajxQz(xVU=fDcd1MCPyifkPY5Oj24VBTwMznX7l zCmDBS0Iv^9c*9>e2`K5=OfsUft(7OJ3>Im#Jl^X3{Fx3Vk$&oPNJBl9F;8 zs*bSGE>{if_ZZR4EV0Zld0Oog=Gz7EH$Sfh z4CHbP3V@F&FoQXKF`-iBkTGt}rA9D&CL|(qOL(wy;O&1{-`?T1hn-NyB`%%}6tgO` zS5L$0sN^GeyX%^zmsOV{!(*DaOZq!uj76*M>dO+KU7s(c8DOOa}uJ| zxeE^d{&|2p?tqXZLmMJ_^d2I*iGU`EG_23{6BGig)Te!%? znBLVfx9c@LQXlp)Btb7$K|$OX+Z4#(tWDn9nv|!(m6&x285UzErRJ;FxfQLM^77u; zIt1j!4bQLc-lm)Dy&zL+bwGP%ipbj*og3dHx%kM-{YO(9+^W%jyDLLl-NtnQ zyk@BS{`2n?1pCGoZDrptmxOXsl)SmqH$ST1SLvHuyUKY%j#pR)86$1TM0`eXzJD56Kh$LcF#zg4L2Hr znQa9x*}0ByL@^TH@|;UM2q+CtCEB#;aQ%)bN*r3zd_eg@TnnREu{Nxww^`4Fbe&H~|_QE}&{ z8>mcYd8#psY3D?xRmb-O2vs&)LF)uoeBuA4-BXTiQiiyH5D@dFUxuw@x))5^vcgA!<#-{YIDUOC^s=sweQLJ+ z5E)F-$jrzai(iOuarbz*rF0ZBh6TAz#6MOTFw2}ubZ zt}`8>bqoFd=AGo`1@*t&@o^Mm(hXg zWKP&XR=ZOvIDFAB&cpZ4uxn>ddxXl<`?$n5uEc%VeaH_esA4a~yTBHt__!2RNq{z& z5|%r4$gqu`k6%%`r>N``+e?sRxmdo4w2_l5Vx18&5I16|&P-%5$B)FwaG#=rjlvjf zIbFT>Vgp4bHdx;#(8Pw105>-QpY-bJoPn7rXN@2heE*(e)H9vfVB@!*d~k3_T*God!Wn@{-UlpeYdRuIS>G#b z&g^uJhBl9CeXm~BLwJ+#Xp!;)V&H*?*Ncx#G8%InloTE$;d4~*UnA!{r`=7kUOakK z{!`#x+l85xL)Ah5dgw$X5OSa;gvhg@EGPlE=q9jL$p_ z^eB_(=RDB^2-$fR-vI;l^!eQ#&Y1P9zJI;P9!H?LpJ)m?la|CdbaGnl^NKw>QmZ&d z5aD_OWT7I>h7>RVlt-(t7RxpX-SA=NOuL8O>TSm#pApGD8kvilFzsFMZrR+&q`1J2 zE>1$IW#C$bQgv%(D+kZYzs$}hOJXW*n5U2b=HgMjEzV%X3-l2p3O|b>Nr8==Xrpf` z!DLG0H|xp(ImG#il8^eNr`XwH+$A9f5dUDgjDVm3;9Hc#KgJJRGC+Yz0`EQwQir6Z-v0Y~$&~%jXAoyKmf1FW7U5Qz?3!Yu03pW zWP~_@O_5P&Xo{5Xgcto}mkO#JJftH6pbD$lm$S0R@b73@;;-eAjdbD$AA zaMgFW_T?NQeCn&rf}fWYw0%9aK!JiHi8-SubQ@XFK%WKfbbou)ezV40vUw=tm6({T zFUIw%>2qpX4RBHYTTb0O^T`Q7njT`3OUR^YJ#4jQrH+6PQpPeLh9rmfh;i!QT}YSt zsH*e2BjZk}L_1JXa(|z0L9hkZ{i$~t(2yMXsaMWJ{zYB^KFxOOzcp&F?GY?pIkdu0 z(Z`n|CGx$%PFe(ThG+{YD7=Hj2^rm|hy^i;nVC8TBu9VS7iZdBP*I@Ko-kf3W~TWI zz0d5tW8pgH>h%v_RLL1jwmxVTb^g$JHY0l6^yvZhnJGMu8NYN7W;X_>y?I#)NHz0^ zK7dR!H8sP9w~S+EZa#W--^ZH`qbVlZ5Dw*ZU;fWS^Xs{iSp>I%cTfdC9fSY;kHji_U_U2(5j1_pT;lvFH?EZ!r;zgvrT)LHnY9E{LO z%eWb|pRXapp~Q^XMP4y~`pyo+*i;HmEM&%Yl7$G+l$(lIilZ-Log@$ufvLdvnBqFR zXmTN)7l@FUT|7aAyZ#tR#gfOn^;3t|evcPqOoZX*j0iZev>|Wm72o%5!91O;eg6ij z&rl>ESSMe;eCTg>SxHJt;t~}FGvH3+<89+jxA_DAohVNHAqTfi{{>N3TLiNnp_~j0 zO=WC@e>fT@h9&v?gdu2#D^9-Y{-v&oB#qJwvb|CV304Aa4=GiQiOM}i5pktzX8S9| zKFqk~cT>Tt^fZ47Y+pORSSo(En9gTB=S3J*TL!w+y~w=`F2+2DTzO5GU~|i$e!+uf zgcZG$$_P%{>lTSq(hByt0(`afYt%q&zU2&g6iN*ly-#WWXnpFMHv?gj74qgEiqysm z83{hK3OOd;J^Ns#Txo#K&>$tUN@V~o_ZH30qGe3Va^zEI3qzCuA%UBk=;ei1kV4wi_E_ift z8mQqzqgUd(a)W2impE;JIlBV3oV1k-62&XX2n6EM@d$M<>yl!bD#YM`Cs&KKJh-DB zKb@?0iG8V9y}$%O!{|4fLDFq0#+e_ zCbL{G(6)`}rHTj)OkCR|4^!UNHd>K}{g=tiG9j6+e9V&pQ>x6czpb7YMHCW^PQ~#ZU?T$| zMqY7ol`eQP70UUK1mj2)di?21$6pti7Ex$H!SebjW=&-rn5& z54vsD)yvV<@HYR{qHe|uwajkg)N}Au9H1}8?W0I=oB6pDzGN3xBR`XTZOeV~uG*~t zh%`%XGig4=UtW9^w5Vn7314?l9jMb%)Oz6bM?yjidqX+8&(+{Sa~pz^EsY5ED1BmD z>;(YZzP7eD))GSC3i|<67JdQsO0&H|prMtJ7*MVxrJz<&RsxhHumx1D0`SK6zyjUP zJ&Uwf+5!r~o4y1(O+q3)7F0d4%is@)h;Fif(Glv~sVVzDLEhcr^l3#!R2k!h290J|ztLC`M%$<*Q-=d5#?VB6Zl2FKc!J3(I~NL>*t!)UR(f{7Sg58MSW2 z!G}Sb%i{M2=_5vHoCQZJ=ffAXioH^FYCW{pu#hhvXk%ujWe4Q7`1Vfn1R)zfQn&V9 zsSRw3tnMb^%z9B}?;M^t$6-z_`Geguq?YXk8tn@#(^&&tX}LvRLOmK(U&`yy*THn+ ze8UP_5x&QF2%`jwPc$5ZMR!V;Q!(mUpHp!Zz4X!>#FL6IS?G%4waAIa>69nIBpVk( zHA|~OU#;$jEPPLi|831a79IoUQs40@xGGLS-Ym@V)6-})`}lhn2!D2p{ML8E?l%7U zNdzC8j~nq5GZR(G^JjR7n=oLEe0I&{s7S@%m7_TgiQi*YEPeN0B)_)oJ^C08X9LAg zbAw=AesA~R5r-I2WtV68ENUt}nlCl_W$++$Umer6H?PsPkwt~P#nVpvI8RM*HvI4w zH@HxA#@TFn!<}Q=j*jE6|m(6+N++RVy`#_-S0r|LPD$ zMS*hMY*=AjXKBm?R6eBK1ahbu8`e@~a z9gfVkbS@`|t=;G()KbX;+6FtY6z%1*K^Q&wvBXhKqJa0( zewi(UNqTj(wRVVryoyXFITO8)tFHk+K;N>YMCXL^V$i6zvtZ2LJ_TC~wr99zEDU!I-2fBKe%FZuy7?maJhu^z%CF4%Sp zn~8|I=Hj81E5?(f2cVn`bn8Bk(VRyen^Me-*A%(-7eXYrO28%6eus`{pLPS`0G#D~ z)G=}mWd;N_EiPRmQQ@EE^jmf z{TDEp2un@8f~k?DRJG;^`PpkU=i~^|)Bf|UNTtZ=h^%;zV6VtmdL9rZVa;`lxApRY z%Zv>B*P=xFt26}X4II=gE*xd+WlNTlS^3wy$RXBEXi7vKZH|EL}%|8@t+B1ftn&))G@TbyDX5p z5Fg_JRaCPP5td36-yv(;zN<&!nSeqXP?z!PGMvI4D3HRLGTkx% zb`*(+nhO=vFEy8bp@d~+1KNs#nV)=Z(bf!{+cjb&|se0(`L7CE08fr~GP z`0(wu^q{VIJhRHh4cC22BMGO#rPuw+mr32kU<5K!mNQw@FqMeBLR}f7pGd3vEZgeC zdk$aC-^61w{1Glo8gVa^oWYnF_cApZMf08dB~s;p4+AqtzlUj8FC=A8+P*Y=AXc|aJFx%} zA;b|8=@Jo!p&6L>;s1VcEnMO_9Opdej=is4M?&)8fvxJyDCeB=sodj78MiD&v?uA> z|Ds4d7LYoDM@RLMeH6Hrm7f$ueqb_^>N9DVDj>Q-t?2*7VK7P0?Bjcv6@?Pv=N)Qc z7uyfuX~q0udi;v$CsZ$dj0z`EvHvyM2RT}c4X2%uCvhj}pAPJs&vaol%sipcQE=;}&W6(dS0iY+ z@?=U)Az8uw1Kz?7IrpHzo4U}2-a_rB4+sU4(0(xpW@$J>EHl+44JoUkC6Sd>QCrwd z?~joOxoG5Tkq~rn>}+J=ahwH;y{x>v&G(QOeb9UJIvAA2>tY08+knbMANL*=-p+@V zeRn{t-0q{Nln-xHa_6s6(F}dQK;xmD{;A7m$T<2kQ|@Q@Bh4g_$WZOaz7`=|vIPu| zN7x=}AI9r$1nEC{mf}2X8@!drrs2N#kvND#F;H#9j|stdlk_3BCxjA~s&MIMUcBbF zTdGF%pITeoI^OD@))U*+#??K(+W-VsgL|SC<_0?zZwn!D+U%QX%DU=nBgR35+&6P9 z;whw#^kQaa4{i^2i#?}~OPsD1AHpB|~0gtlC#@ggqf^Jgx;G7{5Zl!AF$keC;j2C}&S z+zB#ryLE~1k<})hZG*afi8u=Hk76|Zmb`Yc)&BELl$Lq7Gf>vFFHKOJG?uoiifit? zp6(&syB`U(o5bcUX$e{z>bAl&NY>c>)18SP>dERyXTOx`IT2kp8ajj z#$7gqXycNmMTPo!<;WSa%otm|+RmG< zmabdzfh0#7xU+3S4nq|N-q?Dl4-8>4x|z|)hjZ~yFgmmv13?3bN!03GmV+yN`d9+! zDeg**;vRd#Wmcb}_TrqWu~wLNNofwqmD9O+;4GO8R)NLOxv~dNrDBIkL_}LT$X_yI zps8>Q%BVlIj#+K;>0M3{!(+1PeX22{C1{7 zATDW!`8tL!0goF}2QEP>T$0F7PY`lf^eL7v zJ%Em9)v^$HMmm=1KNWGz{|Bn}Dk34tA$0}=od=HY#;;hUPS-{82Ju`X7B2UPUK$kh zAyG^ogDzao3ud76%6G9Ox{YOv2uu>kE$gS>+)R7$DDMsWT98rTTII>voMKHlbm#tM+H zo1@2|?cPXoFwaDtTA-iNr z&nZLcb*ixG6q&Q}^OiWq%W?nstE}v_@7OAhx&!eL<*9^hgG(@>(BANyE}o#^rF|Vt z?!o79VcOA05;qFWb&oIim?iJV7|+(em5DyTy6RCl3$T%NIrf1T>>yRt5?Lwj;y(|w z-j;(|%Q^-6b_ZknzEP4vZt z#j&U>ihw73{dl2jH>&^aJNeR*3HV~+>FbLzt&s7Br|`N-!&QzKp_Ptd(knBIPyiQ< zr7n2le0@gOPLv~#1C4OTLO*b6`9y-p_6VBab{VAnjt-nzZRpkefWr^w!1?EgcR@t} zwlvNcjPQK-yX#{yM^ooP50CgWuOTlluOT3C8w9_AzgAf<;vmy;aCKk^l9`#2RhY#{ z0zGeiApH;j!IUuUeDP_)(b~2Mdp<9+3LrZCQvnmjxSH)QLCgZZCHke>^%I9BrNo#G z0BsD&>^UneYwEN0qybDtM?UYXUvGST6352I`jVGMBzg|5bHdDe*0848kHlLm3pS7& ziuf~y#t#dU_^c%HpbEV=k*^C4Lmv_Ta5*KK>z@mndnCtoKZ-u)zk&y+8J~WK*OXw2 zcr92aE2vOBWm6sL7ld?BJ5>H8(5Fr@mUOczbL9G94kuenZh~a{t@3LU5x-WGFq5ms zs;&vwQX!5yi}GGzca&#ix)aAU*VCys;w6X_u{65ndON&*r6=8>sxpezK~9)ez_9aEWNOpgk{JFf^$>GGm{F*GRCcrRC>(_~Z^`}Q} zW^#>q>EvDkxbh3L`mA+fe>;b~RVD6^wL!{&J*$PKqLnqF<i!{%|^n#HyLy%5TPUJ*0|?a11JO^{X?e{M7#KnzrnKo2-Nv+I~3BVN`1nm;!6* zEOgIYmdfAwTG-D*)`^Y&rP4Zp}wd8_bq*e zR}R>xrBtl`uCLbtyaWv8cLfa22m^UfBC8CoD&L43jk*hxq;_5Gpvb?_m2kx>m2IUR zTZcCj$iT|xKJkN__ve6VWz!huk$EqX09pZmzv(g0ZQcfE-`q7zwm~;RKE4b|QzMuC zPkLcs+WK)1yXP<(@uGFfc4wC5cksE(zvb?iRSWXOFAQpmD@b;)1U}lND-0;ld*MXA zv|n%Xpd4zf=~cf-GYyneMe9qLGn)T7^i}&Q6STcGL~?@^8mZ8F0jqZu$pt8c9P$f{ zT`!X+YK?$p?$|2{0pydUJjM%U-3mG-B`@7Q;T-bo6UcR%;7ymdU*Z$u?2fB1VxK2% zLNe9r+I9lyt9|0s+T~l9fVPz%2!Vn83B%*Nd)xa%UWO2qK(kYRmLq_8#h4(-hU$TF^0vnuX84><|~G00#8uy)Y8vVQiy733V<=^iRaKbKW<^YR|076KVnZPx%u zECsp;^b?Ru2F_xM{1M@);LJbQ!46IB5gTFv`0?sBZ$np?x|Ed;SH_PYKP>Hiqe0XG zA;v@lzqOrZsGrytZ1Gd!PC+IW9sKie2g~nHsG-)&BI-dAjo(K>x6SYpE>>gis3OO( zsAvYiP`LiVBH0;t-m4#=>+2;8%syEE;+yZohi{70a;ZAn|L0WYl2=5=_S1tB#7dhW;ndMll-BA04-9 zT}6?^+@BMbpwCnPdfSpR+FJr1UX^8Msw+S~IP2U3Jo&%cZ|GZHdtb`Pj>AH3*}IOb zUBP{{{H{~>zK#Kzfev7$ZYGGXtS_HL@wMp@Z1#Ktyi8tqt*4B#lJb5=Mh{Q; zFZQAb|L=U~oq+^^mw;-PBXj&$ZwSLuOT;;s)=#5b(w?=TpQ2b6zMO?;V}%(}<&Rrr zpM^0zuS;KfVGwvsl^*BG_BvyIQfv4tn}M)F(ZX4K>%#Vd7PZummI{#v%br4PloMR0 zd7|xsOw*HXa$GgZ{A4*Ug_2Klx)4^W&CB!$1qwvCWlY&$-^W z|C9>+R`L^HU5G>Fm@4w^OZOVn7$IqTmed(-Qn}A-Kpr!(Z+3@y&|nkv}AMkNP7-&RaUtoMfaagbf69 zbd2)j+#UPhv=Fis&en6Mzk~0rl@PcF{aTG^<~=2#EShh#Vz$)&kT0UdAa%Z1Rmf02 zd@7G1F5MX}U-eo%H(CYxoI6%IcAv1w;&iyvImoq==0@YQ|NH4!dbmf(vwhyUy9GmUglvu^&`?HA%c5>p5LLa`_FmGXQ@OJ1KTlte*(u3uH1&DOiT!!5 zLly|LcMVAuT|qqzM%IQ54mLyUI!|ZXG8qdd8~DKr`Y$&yhcbm<3dMw;oCbkL>-%^3 z;?fe{_FPpJAmorTQPR-ZTe;t#XgUh4m9>lOY4X#Y^Yxn~zhOWfw1XAl&yJ)XfNHaH z4Jm}mUS(BRYj_P>0?rufE6!(+?9>h7$`nfvr}XSagOoAugI5U;pSU)5zRz?P=?5;U z7w?w4!mKE7y!?Y#<1f|*ue(maFFAjaRk-5)bieUAc^Y-u;(&0383Z=qxJ4*;%2aRm&P%Gk>D_^p*wg$_6 zdTrkH*~bz3f?>A9v%;s7#6%BDYh5I1>FEH)E-Q=Us041l@&8?$KQckoZ~vgR!r*OK zxE@<-DkCthZjH(Y=L4Nb2+LN<`rb%#l_K=Bu)jJ&UY9iJnpq3SYD3F-L&snW}<-;`wQ0H(?k_lF!5LMaZ6* znC{t>^xx`4^v zJP1Di;|oYQ;zI)D#H^jY zKLotC%S*oC{-;1Ae0U(Kr-Q*W05&JT12;5&2Ve6zHGldhm@zyz1QF#mT^-nj>?M8q zI7b`=rxLXb<$4Asr|`v=ncn^{wz#<_2^ z^Nc9tD>VR~CRhT%mQT4{gAF&AJ0JTzaS4Rzl~89=fEobP#*4{SAV-S`Y9ee*aQ?jt zb*A~TRi?7BeMOEaWtcN!f8tRM4|_J}&{uo)hCR$mEA5K2izB)0kl^yKhcaLEzI1E~ zQB&CA*YJ(Di&3y!RsPMS{rAp$k>`)YtFY`vRv=&p@_YsOGpAGRCk!R1Kh)3E;?^J;&=3U`EpDSC(2XPss{?7dMQ;+{+YE;+OL24g7XX6Y}lF5X;8?I=fl?gChYi*6|dfz%- z@`472Z0C15I(VsC!#$E+5F;R7@QFWXKu{xxCpvn`_I53SSxMU zuA1VC?JUXoMA*z0tn0IgN7PwET`Gs4&a1 zzM*TN7kwTw*?aAaPDIoT-0l^klY4JEL*(HNzq$}CHD=cYi_Y{028EOl*b(+m$Lbhx4p zd*S=x)51Rhh8koq44ecGtx3eJl}gk;jz9#Vk#Myd^IDP$$8Lb-Z!7H#{Ea4$7(!Et z>#K?AwGQqp?|=KlZW2HT>Sr8;YQ^S^prbzUc?kDZ9U`$2j_D+v)laE4@>!OIt&LH^e6X#l8_?--Zn=0G(Y z@6$OvGQzgM|5l&N007<66Sqr$Qn`l2%k1s{pj9k&wF+mrJ-RcQJ!K4935fIk?H{ew ztty!QoSTOS7~jXhb*$~ZmADJR$R*nAB+BwtDW3<>(bBY&0BL{^kjDY&kUKjY504ck zHUOgB+1WZf?-~#tQtk=dX9b!T71!L4HyVFk3l?v52va|P(@cFg?V!X{SpnuUaF&ni z5{u${84P=a`3nR9b0NeZVH)&-^Amq*cl@Yjv~Fao=pGj`$Rw`EPQRp*U?RbQc{`HE z#gwqj9f(N7thI0wU(~x$V}rlib-TfUv_~Uk8#)@~xyfG`7zA`Do!nIQuK$i^5EK#| zC`9)&9BqJu8zACbmQequ>2420E*`eLWCaK2(A*!c&u^0N4s(Etv8Kenv-E_VD2EIU zprmqYz=|4RyY`;Sdw2o<99zi4ttLuA4MGhZ?2ri|)fH?YGhRD8SlUGT-zA|QjX$kD zIZObF>^&!oD@%87;?3pr-z7S=)*QI=EG)m;>W0#Xv6SW=y?E-LhWzx^8Q>A_932g; zYaRx6je$R0Kp)=!9ayi|06}VV^@#sRaSaDR;^9Vu^tqC-?NiP}y4&rWHq%T%Q3R&W z%1Xe;m;$t$DZA90n;>AdsY~A`MG_R2PZw;&SzXWsE{Gk_fWo?8gpBTqb=xvg&q|A# zL^Za&NES7st;-UCgaxxjDOKObCvIis6|xJ7D0MDxopt7-7G&QYX}K1<>9%C^Q2sLg z=;_#BoTNuJHjS~#sqm?uG^L$rI;BJ>#H?xou;)B3_8yevXuOVsjgbTfx6?mHrLLi?U0U^ieb%FMPi1Z|=*G8L zN>4aRM~sY(?VOx&0T35bm3FK;=er@W*9ls6072483q3Kra|KJm_IK>trXE|jac_o3 z9j+o6_ayzMkWzn6147hWdwaE{=yw2`7vNB$-~=;G!g_j7)iul*vnSWDWfuMjyuCZI zJGH=x9W9HxFY0Q_P9AqZtoj&uY{7hIbQ#FHfMj^lc}+}2*Z99biXSL}O-`5NA|A>f zGpaGv_wGwSsxWE|IXG&8X|dfY!#6#DbiC1+0jqPqWRp_^=;}WcuQ^!}ZIQ(|PR$Yv zm5)$3^QafAn4IVgL+5-2c_2;%HJBlHRL%ZP`=7=msq?_>i!UpqcXXTu3_7_*_tDg#IE{($nXjC$ne5L3+{u1o+qPg4XXWuXC9| z)f@w^=~9CsH!z=SvIZF#u%@d4enJbKSj~OFaM%J@2GmK!al=0Vhz|6)U<(B(@EFP@ zWtX3cdIA%#EkvXePHtROu8cbp=?Sm_2HD8W4CEA^J)|onTR9N~Q|E#xm|ic;NqKN5 z8p@&oW>x3-iXahQ)5ph6ROIV-CokIQ$E(kEk}6}VBA=7tLP*s@Q3vCF>vM4V zTwD_Fo?-9L59jxNs5Bsk**WXqj|Gu>dWKO%U;Y&wYZQAammUJwao$P5B-k(w0Xmpd zB$$viHn*!*n=406?|>}|1uwUNq~wytxA^0y(^=|@w%cL^osyEy&hOScc)^w$*g*lI z6|6o1=nRnZv8}`v#SZd;7aHN63H;*`LTsCZ@F>`r!-UI!^{gM{JLikcrR9FItJ}Kp zPVY~u4}JaY1?W}v103^J6jZsn$_7#`vi@IJZU&`q|G+!{(tBs~41psVxZzYy!kr~X zPJW3sKFu6cpOh~j-tK;nTCQa{u3Dk<(D%rqUTABu5fmy*ts_;MW77ls3-dMwF4^!U znYGy~P+u=;+#(XJf#s}`M1YrPh)UzF4KY_qiE3FUMHcQ@f)64_^6=NzVQ}}93B$s@ zv2oM*%SRE(X=;KMI+QTM-nOqlCYd|RJeoQOmkw(Xt>}h{>nHm|USnXw5LrRl-ZZZA z6C#=-_YvwoH|O7G1iVF|2~)W5mCc-Zpjo`CCMorK1IJ&67q;+4YhVq5F4?FH%AWZR zH_zOqLl?x7q;1QY34~{CKtCRlr^HsF`;|S*B+J$#_@h2Abg8C37|nz}S_1C&{Rf(r z(!4xbrlv7^l(VX%21-arHv1jUNi=%#8pbA`A`BxzQF%|dKZHIHg``mISO599mz?bA zm`q}n4gI#Vc}DZAJF7E4cag4PY5q$`Rh+FY1`Z~MoOB+E=f=)6C4u~f#R0Qu)EvhnwCe81&$ z=H%z3ir5kNbIR(Qn@=$~1IYF|f0n7Gd~jI;Ary&;iERJ_&o4h{@=+liXx45~Ff^E% zrxv;g2T!luEXy)YV98wVE&!%YLN)iq>d_GqXoS(}A-6l|egxuv#%CSwZ2)Mn1HexC zqEJHdWdw@_P7k;VPt6&F&d<+*%=RAGB?5#pd8w8Hur5%v&dbkt0sIm|%&_@+&oaGSHQQ*P_l3F@QN<89vuMPHN94&3iOl6D z2?&MgITXrP7YjU#=j%n?0e!v9Mq7AoHHV51;ctmCWKS zwE#}UuXfL9N#B3kfTOI=CKYs*23mQ*Faz}TF&p_fT-T&mrUW$`GteslB^(1oS1s{ut<5t>L8uFX z^73*(f##k79e}<;&|aMXVsq6|8N!wNciM@INUl`i1wc-C94H@gp=KujlDGQNb*W1C z{*&x*`qUM_)$fS{>fAYa1Gj{-@(-}Fq$S9fB!Gr_>L_SJzLw430|IWD>%T&`e`;@I z!2o)cG;Wweb!7!zaH{(eeMAw`UwE^tY$KS>&$BuKTC$g=BI)P)aJolm+?<4t&V}`=_;JZ-g7HXeAs!UZ;V_crY?|f^$TvK-WprO0ez2BcddzgpxMF)0h2AjdTWnr*TK>eGTK>x0^^K zT9<)L3#p9|)S!6p)gUQmSVEb6HHXfm+Ge9*vsz;FDS5LU&9fqYnlr!S&G-*&epL1aDW)S zE&cK1%iK@X5<^~QOx|wf%iE)9&)cnuN9ATDipLe1He8C?Z(bF@Qkylens=zx;~+cA z7vEvuL2bD5U1*`tS$E9xm%Y=MPlcOPm-yHj z`+T1E8=Dm+k#la7RW?fy7Xf3=_4-Ny^2Q4PG7E|cS7%7pv*IT3|KSCI4ShL(HI!Nb%{L{taKLI zw>aYK4VXrsyV?aVawhTBq)r5B^bOsNI9XOa?yjWk{^NK0Gk10A*L95>iA@Y5X#BXv z5pTBbjTAi+b?FH}S+T{8vCKEY3})~<2rvNp#hD4a%3l@a=OcD!Sv0k@ULc7YK&cDK zodO8t!NS`;J~!uRz8*hyW~1#AXz=u4w5%~xkP+3#50d%ejFw`5kB2FG*TIG$mhi_ws*Z(UYMi~0qJ}j^Ljm3gAAbF z2Prr>eWr!;hv)S!k91I+DNU*(@5h!q1$_^{b7tb{i1Q_Mz8-(${kJ1_!Lw@AHP-b# zx^tkK_Vq^D7!sT{n+Z%xlAOGJeEtBM3T2SBGc9m~@Ut{u|P=+=6LV$k+c^k&) z7t6St+)IV^KbSuxRdw^}{e_n0i~l?1TV`?CsmPYhfthHCc0OQ~8RNB2_GURaM_g;iSwN+hF)uN?aTyE0HSt z^9*FR{td!HGXI|<0l9)=`r9Barf*96v9Bt>I8Y%43r+kBj89u)ns5`roDXX?KI3B>(wprK10Gp7AI>@VtFsE z-f>;g1mwT5r>pC7Wl}6j0Z=+uI__s($NK+#3@*7@G$)W!BO3w{{r6x<5LXL^B2qAu zAMcC_&uaa)d2TO`8hm?D2|Q@?o*ruZQ_4OcXLYkdU*Wmf7cxAbxR_HQj4{l`_rl(8 z{L%Y$88hZE;IF(d-65-!~;Nea@ImlstS-hcpTEz7FA$^h4Mjf3v_(uSuIqA=;(*GJ+b4Ep{fxSjL#5 z17(4uqrrTBk3q1X#4$xj`3GdRX=}9(s;@dd#@+j-o)@HskOu#v8*GM^T=Ao zwA&x1dyfpBC(FeiGhFc2xLEF!ob1Jn`HfiGAW#FAmF=5C#O+Nn#W*+^G4jt>JN+^e zKPSwVTieZxkJ3f_`>K@Alu542VP(RihVkZJ&)W@3Pi~i?K&ka6p)G`Gz9vkGk+G0S zg;l|ZlluYA5gpBF-Ww;D?MBY!uzrSuJ=y{q6^w7=T#udwW~Oj3u9Aphx5y0VB+Rgp zXik?0`rFLpazN(&Q^vjxHIH12b%S(hB~^zCK#*e*ne}%0=3iV)%4H4D|GKhv$fr zVOb`@Sx|*g^z+9LqYU8Q_=AQYwvZh);6@hEAYl^h>}+pWZCo7kHk+QF7L%17JqOf8 z#h*WZT)fqouYm+TjJSW7d!f$eC`=7ndq7XaRS4jR)%&7c91sqnds=A;5{=2RV3S$R^j&>P(PQYL>J zz!jH>_Ff72OiGF94Ql9KxY%DN(S#bw;ggEhMXIe);i|EQ+i%^N0@Yl1j21^8%{EM8@E;_ z;1*<?C z_I0A$_6?OpWBatFjmNKgv44gRo(u+2>T1t31|NyxT+~FnKGU*PH0G#LOBX-JQE8c0*dg9%sP)3>--QG4PqTME~UA1u=5WlcWL)CDCf zQS#7#lPE@Rp@}|WFZ@B9N^z+}vAlEIBc^mhZ89cqKE|U5rc3>T-s)z`g}Hg>>OBE= z9)XTTgxZrIOH-2Lv|b*BjrqfvDsc5*;>J>^%!c0!J6y{m zFRQVp#-7ifavxn^{bv29bZmPZZ{UC{FxUOc<0e?I>mcZmctz>fH{nn(W1;1b2GF>C ze^v@mmad9d#6j?!G68yM_LLo_oWn~GFbp!~ZOw9c^6-JzAah}D!}s51uUxB>da5oz z?stYl##B<`cf==mtVcHP=ZN&X(*~|sOhT>YeX~#K2%^_`XB6JZDd2@am&iAd*IKWq zc)jV~JZZts4MnUUes6KPx{(9Ri<;y)UL`>vK zx9V&uFvI%>g)JaKtsWu*WQXB~h;aIP@rpp02WnA63}>gDsKaaaJ|2oT%OB4D~}gaYV(2s4A2duO@| zxSMeSO$?M9048Y4d*1y4e2*ZN#J?79JTt&&hy-x>J5dj)xJqUn>XWcRBTKHPj0hAE zL4eUEC|2U^EhyGcgt8}#wKze3^1F9)?^+R3(8fj~aAwu9ynI}H2k&7?9lHy30|Z7; zY25>nK-|KCJQF~42Mhuf`3w82-s|0(c?`JbN>>F~_Y*5qfET+2%JIJdvG6oMI*^Ud zptrcZPF_23uCo&9(DK)xz7av`D+aNr+zezO;>W!I+F*<#&RNlCnQcR0a2ChFz_G;` zsFFrD5b)B&*fKu3U4#*_VoIeNb8?>#?Z`Z5%VFwMa$x0k$Z6p(W#F6JLSDXY9k0{3 ztD^G{?E1NAK?V>-`RZ)=(SyTW2ZDPvZ?8tEn+tYk56}HUw+*G2wG*agj$uxh54-Wn zl?3ziKzeazPm8Fw>NwSvX@{Xp%RWB|MID2GzuE^!0lX2!jsiq37*P;^92&SZ<|5Pl zld%tTx~4)W!rm8*+6cC%oHR3QtT>CrxlJi-UH?~w{)rgX-wtOa#$5d&QnUX_g@E(f z^9(s0W#H%R6Usk^-wVGNI3Utj zCl#d!ElC`0^n^|nL3gep>Guq9U4pM?cQ&qzoLe@83dCvi7X#fLJm(%$6vP@Of4~hs zYQCg`pVqrParq}mDVcZrpko6wD*qk^>8x@r?cjBA=o7!-(1Q!gj0v}O_iyfEQpg!~ zs28!|SQ5t*R4sDD|9;Jekf_;o0D%QY77qN&1|Bvs3}J zd6%@l57eJR0u5M8)7CxCQ1?a={M-~%N4xl@1e$aO79Bq{pUbeMXb`2eBCgMG>hNiy zzrU_MQ5La~@}z>?XB4Sn8N!PdKZ_%AVKNV~rKtIbVO;`m2O$=EP2kSDjpS`e%)cKqNy_ARCfc<_M90SB7Z;VDhs1|h{;qK* z;^qmt+)D;+fVe!)f&+B)g7i?T1f|`M#2tUUAUe4uLd8dNNB1)s-NKT(3`kk$G?!y0 zN8Zi9Yo#b!biRwUA&-MF(;R|K4lqRlb3~TAA=kf^-rlh^$XM&_~WIeC1K{z zyh|+ixT65eStwxlmeKT)fK$YHof897uPfPVk%R z>guK*n)3Qe?_GoH2)s0ybb@cYJKTUz z0;GBe26i3oIxd}sCnr8y*_z(7HLMl`MbkP;+l^-r&lCqESWs1>foD&QAY0we>i610 zL}D$tJMCKAhI3atYp%nOp6ykEmHI3{$F&SGKN+T%0jmjDeS3YqLrmu~Fq-JazT)&Y z+u7ZXF$aZG_GbU|bX=)x)2qs_f|u&ps)V}+*R>X(_h@o`4OxTqDo8<9gqQ1cHv-l4_wl1VW3jjYk@$i)->&i1 z#_g2^O*6Hs(QTCZlmTJ!{UE~Ro}GyM_TBVR8+9%9{e`HL^1zEDrh68F2rTd<198EM z%5++NH38elh~rCxInmsy9-=)O>u1ZLAVLYG`DaCIdwx3yb$wmN<6%8MrQGVyw%xUrhug3iBz3pG=#1s8tkX4T+ao@-q3iaM)H9o(MBR10~}l zc@-t>B2V$6`8Q6s$)%Xg1SzY`AB(md7!*otY533!7`ZHO%7HZtboyx$#zEeF&SRdM?O&?dm z&A+r-Sf7fGc<)$HteoBYhUGH8h|gu5sInn6m%AD{BYHdg?>{k_r*YpWVql{moC%3? z8yE3aQ-HyxX~~7u$WDxUcF*Tt*sHhTh6!?NCjU6JpVY%N;6;iF@NQmgsh|=Xxm&xk z5G{Vu&&(s6+Ce@AlDQSepiE@@)wTEFG3C2Usxn7#u#Gj^!^qFl;BYvA(*0-|UR)G! zd0hRS&bXRR{~6f<$K}sJL+9UC!hiVl2~{AT?`U)N9^AbROoa=BuQ(Ts z)6Huq*5+R*u^dGNDY!qBbJJN^Bi^HNnjJQLXZ;Fh=G_%=(BXNAdx%_gjWsbce+P4iGi3bLUDh^-Ew)%2#KPMM;?bOLAhJLiRJBk%>!8h* zuA&?liV%wNF)_@D-0$FPT6~RF+#)MtB1R9T073O;J1A5hc&LLn@`mE$5wZ|11(qp{ zsbk!YmTi zYJq(p&5biT@7OSAW7uDEz_;=}d5Py8qV`eky!n@4<*LzIiB>SFXI6gKGBw<2pQTk$ zCqk?1O{>&*2-YPwDdRt>WiL^lEh3_Orsl_dy;o#H8XL;8S41Ia_90Qlm8j#)-p#6& zn?#@buOSzr=qOqAEc9#XJZAC;d9sUz9O3Rs0BWi>WNIY@fu#rE+W5M+6mlbr*1G+7 zcuN5HzFdR_kM!QOqBDRQ45{TY2Q5-4mBDsjaM@b=9J0aT>e7*RR5QEQV6-R5|$r)T-=Q+qJ zmdY)#tQM0^dST!vh+P4MHMUgLIz{iQEUFg$w6n7_@Nz2?C5vuFC-h!l_m=B(A;9Dk z@UPtgnD`lxQMXMxFKvScQ7RZkIne+<{wr>quWI2$ukxn8tXB&53||lX>TJJmW)OJQ zTeRglvGaQH_?a9IMXo7)c;=a1Dt*s0#**F5d6`G7O>e7In8P{v3;azJ$(&;$jRu$s zCyO{MdwS!B`gjT$%2@7?6b>KiH4YR!_9VL0nuM7btZ{(EC}U@6;X_q#aJih%>Q|}_%P^J zd=nG9h(&JjeP!SIhATMsf$}GfQlI++;RkF{@f!3-oU2#&w*%u>HOAOojvZB474=!U zqwn1x)S3B6CQdnHaRKW zq4!)}U!_cU)luvUOO2ncT7ttv#Rl)qe>~E;FMPTA1?EdVp5>fUFFJ2yRSz!Qu6Nu* zMgY=2fOew;=^E~fhzAGW|H`k-&wL6&TV4`d7Z(?5%J0B)@GjE^w0kiO*3FL-&?DY6 zNGTvtvj{pRgB3^vC78_3iJr{OCJC59i1yyl_1>t>UJ8d+WOn@k`ySu#* z2GIJghWs_Jsz3|GOFrR0_QQs%>Sov@W;D#5RzOhbP6q+T-&%z-W`1ESOG_+;Fn1px zrE;NC)8Cf9=XzY}&(;-{Gi*G}a>q(R?inOgASu|Cod;z$D&H;_Qv8QDO=qJ$y zS~8cEgZvkV=$l?dAsQL7c9fmC9~=b<3T0FuvniaMh>F3f@_RPaPB_Q>Uc1KXR|h# z_chBvKt3W>=HcL#6#G4CX-AnlH@_ZK5*(0&vTWg^=I3KMKb?6v4Z#>S_M;VhY}0!z z&y#F(P9;F#9Y{T}HNua6p+2u*U_c8iSvnA;A{hIu`)lRrmGQjqXBV=2&vNiJ?1eNX z=)5KXiB+fi6&SP^C72F=G6ea0`s5-Zzne@m*zw?4R%2a*4#HPaWv&u}hjlG2Q5>;e zoy#*5`z6X=*FhL7 zcg!Ag%pQ2VXitiZ1^VZUh1y{7Sfv8BYCyLpZ>9+$*@Pia)rD_hF!<>gx?U7%8>U@W zT@5cZxgu^6quSfWjEpadQy?1;1v{wtbAI5DkPl{0`EAcIbkdqhj%jn?F49Z)wjt#2W`c?+jdQ8&Dy+11QWF(skJ&>>0tBh*YnlTb2kBhLpM!4$+(3k`b|{SNwDLshHh^W$*FMw`;ln zb^GZq_Afv^0W(rJSJL+kUv% znu;ig(q_lcY^8 zutE3HB1xU36a%M(XW)DG^rHno`>e~H zuW9vjP5LHz%x{}NU)&3ufIXAf{>7s~o}@8&#djytV{z1TOX21{{K?%Q34C^f?)pK0 zScyYW@+{?EJqE3s*XGA&gIWbsh2j_#zHYTVDOUG0Bjy zjyn!aYYJq7#{Hzsv%J6Vcuv~s1#}a|>e{@&X4>+=Q`I$RbnD^WsHJP4+If(onk`?{ z=OwYWyDJVpm*6b=Pt3TQA%)jQF97r(f4eo_nQPzDs13G6%h5CCC7Ek`Q z|D)+EfTHlewgpiEk?xKqrMnvm$t5JEyStW<4(V=b0Ric5$rTV5M7jk8Tsqa?d-;F! z%`l7u>+G)XJ@=gR6c$&E62foIuIG!Ep_3?`ULEM0z>3?$!(;H^;J`5&uzrPl_pCPo z&m}(oIiR?tjYljxjo9&*EFAJsVv~^fgRvPD>Z$2zl537=@EC9_q>b-9sdV9b4%UT5 z(M%D4#_$RcJAb5Xi;cPi&WXp)#$Q7wtr*cdzq5Zk;{>3_@B4J2$(-m#AoFPJ*#BF? zMH_H*w(lJVeVNP2)Dk(}%Hd0S=VH<)%BjgjPk0Q6!PX57p<4gT4+rY+)A=mZS>)x3 z1dbMZOEjLWSTelnh<0{PQ#Yd}pa5htHcwb{PEUa`wS30Ou>;>gdDEx0^YzHov5vK7ad-)}YWBO(v8j?s>w-!+HRK3pCgO+)(9J zF4eIpH7tM2lKfteC7gB%@i(rQG1#-h*;9mkhAJsVH>2R#`3kvGY+MylQ~er2`1W+| z&AD5=6z59B_1$IYP2SEA-ps?Tc{;&uTC*EO0U4Ul@ zH`ntdTr<()?dEU2D4LMu?Mj-R8#89LmNI%}`e0i6INlNnLkw>!yY^Vs(Wmm;i?^9Y z;$KhQgydRiWj4$WuzUAVPzv`-DFg{70oL)$Is*r}b94I4U$QgJy0nlsD@9A!zIgm4 zQ2B)SGpD648o*B$!zrbLvbWR343-3l42a!Vxz|f*>Cy2~K%=HLRkN}_d3GC4Npoy% zuSUN?A0~I6qjWq*)(tTvdmTMX7s7?%x(?S>s8Cda0P# z=4@>iU=>M>osIWkEgwS`%cM8_{emAw7)=D3$|DBO6%6MgG)UXq#b|%YrD^EPaoihW zmq%eS?2&RF@J5~&6b4Z(0s1yYQe)?v!G*^@QL>ycAzE5EW=UBnZ801ErApW{BYR2k zPc@3;A^t;IdmQ7YFw%bUX7lx^JIp3GT+la}@m+#VY7+A2%}luXbt_aSo>y5{S_?8K z-%cel_q(gt*i)Y4T=UR{kL!P z{LaO=gpU74B9La>F-!mbdwz}m^x?4NR&2_X_qzM{VxY-9bY@*$@V%mtDS-Wdy}P>` zTr`p_>U)d02C-e^kPGq7ZqZ-&{|0?t9ntQqu}vP@?09egP5xYc(8UVBY3%-X;jKfY zkV*0dUR!|#=iLW16*zRpjbA)hzojgjeRkXV`6$S&P(|yb&JTT~Qe(DA$Y=I#QS7FV z;iL(pc1>hqNbT`GIkk1Y=eihTBfG?UF|!HER->&VJ?&Iphg(SnLq7j1Up`r4PLw|<`+{w#ORU;G`|j)wkzGhftBEt(i#Jk4{- z6USX@;(lw#HK0LbbCZ^I1`FVtb`ewKVJ@90Rz`ti2&{d*O@7a7SAP|8l}ubwaP}^uOXRnUwLuLt ziwz^;6NrI=niYr&J8#kDSLCYkG#-eot7 zZ~u<^l0;p0`$r}-32Afd#x?391{j_U=-h46La3^8qr6Y82os(e(M$a&45^puG2K}6 zNwm?$!0@M8k^a&sK2l1TxK^H7biP^l)%Kbf7A!SAdjI6N%bvbt*_f?7Osb#zu@}O+ z&OT->yCrUz`QANe_r_0ZwC5OWGENnWQ7|u37-KHFzl1wL_KnjB=D|FmA@%iiJNk)a zSC(xEAE>PiuO$;7Co+fOUJ1F^P%aECV8AbZTChzc52yM#)-KO`U$}&^3&}jK`-n3T zICt?P42Y&9PIZd9E$(pkdsqSgkb;X= zIs>!2#H&)tOzSOwPJpS7TWRVNA^1rIg9u{L%ApvH_ar~FlNAZ2WOPH`L3U66dWzRS z>t2c&@b+tX(fK4L%%E0p8}Q~~e@;h%bP22m_|<^^24GkVX}w^Sv*spc$6^`%wFF#> z&E0ODPdYOW|5rU!?nV+5RyX@r9HXYBgo5s53*7(P7B{{zlpxLA_wD~kC zSLw^gjTzE!UjvFEz{QIX$5Q2M^>@OL0bx@ueJo|UB+zIg4HCx^7Y0FBWg_4VkYHaZ zCt8e;e?t_s&YT73IHECW(f8GTMy`Qf_j1$^N*708cgS0yP7<}_y!1o7aV}GCFeAKBCP9#X}?Yn(ZX>0p277gGYhUeIyR7v$h`d-gwAwSlwZ8@x5H1<{h ziz$gYyxsStGNCBuHZOHJ+f4itEvD?Vgt3%><#;II9(i$#*#ZfIhFr+ zvxH=C+Qyn*YD++>p2Xl>PiEnmrWhIwvM&ZB3zpy*jYj014?puHlH%EB$rbB zJ{MTkW|rx`ZtB&u4s4nlu_;the)Yw+q66!xcC&qSZS}AQO+`mvflX z>4oQJ&n0_mJbd|v4H@LR3@j|027aq&wAi33_>gG;){E^|8#!|Sf+S=NtU1rBCZlZi zYh$aPjb-V?Ps2hz1(`1uLfMD|d*Q2moAIsRPgjiI{BU5@cih!Ct@+$yU>jH}UCy%E ziA^FAuI?JPqp|302n$f$91>&oSwJ|ooPDZ~MuyZD6em9ik`;JwxJx`_4lRxyk9~1F zumIQc^4fcS&3mu!#o8#}I?g zfG5Wec>MC_RA{oKPG>1W*1r0bktR%wb2XoekzRbeWR=+iPOJ7i)h3mullTo6(TNu0fAwhB*30c04r6-Xa;? ztK{7vw1JW9B6_$SwCY=g8ZTLHm%nbzAGT`s@r`~T$GF?eNb0ezmBdl5oR-eQUT963!L{I5-^9r zieewIz~koN){;L~UkC)1j)bHL7#~BF|As_Mge*y6)cAIlM1i$r8d8WSQ4{nmGWpVSvEo=}EBMdOJk(uOCsV z0cfILyvj}bRy8wbHE%2P;k7@ej0T91+K;@JfBM&tKCH!8IpdJpoWvc0|4I7(UMtsO z0||S~m6d;fw)O2^ToB6O0!1lcjXT~q@7^`tEnolj1;x`Z%lnELd>k9U*5B9F4Z*j> z65&pw!?TJz_~b?jp|usNwUD%F^qn)2xLOWOqrmiqi5+}u#A*6A0xzxVl>pM+*2Jc! z916%M2;NAQce6`^ku4mUw^*sJJO!%zSGM{U>Wqhth*bTW@f|+;j%JlAXlZAMqQ7G} zj5T&!-`H5SWBzGI@Vp}Wxne*9CKF9gr5#?(NQazXKbht)1&2MbbwB4MR2p9a zdcO=KvH?MCPy;7y;*Er*I`pVm2c} zYkw4E{dBBp>}hE`iEF;g)OVQ3`nVcFtWspiUWVTzr*Yi>$G|yk`F%Feai7(_J4QwWJnG!<}uUy(rKLySVmXdIe!%r{czn%}pRTH%^s`KG64{tp+; zXvns^-lq+sq*xHHnW2(H8#*pOc9r&&^Jw; z93TTeuk5?gre5jm`*93JU_&#>L$=coPLKTgTb;Zm*^0QRI^MG7^w26R}&*^Ft0|EM`g;*#UNL&o66AP>*>T@Rwq5?G146;fk0)To=@-na`r`Q;)HJnFWtY zLDk{RvIO9_Pger+9w>RhZUb;I4Qr!FUGMdS{XcmTp2l~(c);R8BSIYVsoeq9oiXg2 zp2cVJ4H%tl#7Ac}`8Dywgr~PK>M+`ie*0y=-s!N7o}d0QWs!N)Hn(Q3NgBV`7)x35 zDQWl+{D#MZ39lY6xB3&m_yjO*y`9Z-=chFFom{=tqj>ZP9I&(48yyAe9iF^NTwu*i zFVmw_Vp0p8veB|8gBA5(4>cMO&x-H_x9-gpsEhP=Tdg3q-6##ERnVQopT4Av0-+b9 zmLT)h0=@%3Yl)kWo#zN{zoF|MZSNf(z(F-09cXv~d!vX)r85q+03I+^_^A6p8h-K= z1L5oRUj`1=vCxs?VOl0sUG^z^hrD`md>5%AR zx14Y=V^QUgMYD z7h_7JKkqKSW371JBpTWL7hc6O!gy7~ew==HR3{3UseC&A*a7=3H1UJc=o)4j^?^F|= zL`HuGEek)~-3;px8| z^fK1yCm~3lVAy%C<6z*Ei?(2|QDE8Mf&3X>E>jdqRcT zxCG0?_Cx|_qQ1`HHLpLKJAdjW@coFyOsd@!GRx@HVo8i>|>r%d1nmV+FX!552sb&SmNBf}#KL>(Cyeg3LAj2^M>}mrF2L{MJ8_3s9_^U*o&QX>ym$WA zxu;phx)Z<;boDcCqv@|}XyLpdm<}MuYil4gZpG5oHOZqRf;}EROas^NrZ<}XH6XBl z|1S6tUI~8xAn;h}qxsjv2^1Fp6-3PMy(N0@@DG!D&0IC$% z(kelm^BIt{0Xlvd?FV`CK`8%iY&fY%py9tD6TkT&*|Ks^u-|DKk^>?N8Qf$Y!ngze z1d9|yNex3?yf9h_%#uBAt%jgTY1yaZ5a?#}y}QP8M~FWYr}VOCg&4bNEe;z=Y!|Sb zr!UlVwcTtFs`(*Z>E$+}sgr`%8PSc=jVZbMKf``jOdDU%{3LEVO}_mcE_Jw_D~ivV z10m#1R*#sTQNx+M$T_5LY8Yr1`aL^$xE!Bx1wq=W+1Ij=Sji)EQcuWCX*L0?wF1}>?mZ!mO0tQ#R^?ASwHotsixr{V z_$BH`O)r_cU&=^}PM ziNfP!H{GtT0B~Y^i~bXBXIozva>*4(;=B8EO$2$X;-Q5`(~hCb7SQTj@+!N*U{eDF zS;lV*%?wNf%R`8^ymSO)N*hMN?r`bY*`ou$@l*H4Ghi1WK7BuOBnVC&xAUUmi9DtW3R(878wT``&aH)zK2 z>&a`$Blv+%K}1Y!h+9oZp*VRG%9T}>rLu#tw^BAt?uzfO>m{Mfr~H!k6yd(F%0%hg zMOun?SO+)xfDI}2#bbYg7QUen?YHm(w_fzRQ(yhi`K0xFbt~y{y-CO*4&5KB2*a{g z0=$N(QPpcwHLuP}TVu())Xmb-_fwtwxeAZQp!@Gf8z29Sg0Gz9{G$H*?>)kHTIBxM zYtiUvtN^*6QQ;&pF(vY$q4v>MJ1ZyIUrhjNv=^<>ar5hS=XB!CC_w}*1j5LZKHV6^ zFF|ix;JBvDXWn>9aqb3?K4aA_5YF+5~w^nP{VL@y|Yziaggx%X`V15Tc>&gz4Ko3ZoBvX?(PG?+Sp34 zRH<0A-&=x-fND8&+=e1ug(G$RE0lWs59i+Pq2;t^13Lj?4%m=5iB19HQ^A9uo zwqr-k_mu7vs_w_#PG7r?M;0~#ed_53(|D6%y$e3z7%Z6U{>+2iSF3WRy?kq&x~o~W z@Axbm?yN`(NC$qmCTI#;#O9YvhY{9KD(4;71-oNaxwM<(mb-uW=Q%I8*Mav1)I?x- zhBoN`IXvKRegon#rTVST&ak;i1_HJ&Dh-)BY2|npvBA}yZlbDvH zNOF&s!%R^#Wo|qW>m97%(o!TBT8c9NhP(ztuu_o{~5_`__BwA@) zTKU4lv{>W_$T5f3xNK5aUTTkOYlk_K#4!p(F11v zZo=JypnnCz01^@sAPnHno6MfH0r@cimdv!?6vobUW_Z-06X&zj?cAfskflP0xhZ(= zYPQ>h5f;jV4x+=%1h;2yMMf(t(<`)tInkXIJLwU!@9ug6k5;IeQfSSN0}#q9Y5m9|I6%3!QXu_kTaBq#bWp@IAAnl?X)m9n!w zmr#y}?V!2A(_YJD^>)xC#!-$_+YfeSCY0L?pObB;Or1YZ3nz#l_@bCDkFWBPj(n=) zvGMdMzpAR5rR4nnrFUULPX1fN>!R2;UemA&LV-A!Vw0?mq*7QG)Q!tKavi8MD#GKYu! z7o}-7kf3>ZnQykEbTo&SMC>p(_=MY=JCH9qQH!b^22z@0VlkmM?x$(O(CB#Ka%IC# zxpqw==ZpELUv@5dRV{hXffq9is)e{Kiul5?;1DmO()?eMsXolxiU%Pv;U8rPw!w=!w%k59zs7bp=0@CsIv}Dt!dVe_ces)A{cV1S;OI9j&H!Q!b?{vf z4P~)8lr0<=7jIJ%2yCXkRh^^i^1Pm5_lfZfi#Me(zI$aLqa{l~@=im)(kt!OD@t-< zhCdBaH@t0`v?;D2_=Yw|l|6TN-siLvY}1wl7F!OddT?NG4!SvA;>6d>Dcud%zgKX* zEPMOMc)@#yKLLECoCM{xbb;{va^JrY$wPY4-k!~umvQ>n`9sX%)ahTk>R%vdm6iC= zb!Scu7Y(%4DN6Wgl4;{Hr|n5~)9(3hb$behzvcE^F#g!pk*2jrnSZcf8oa8nu#b0} zKXnqk9}zKGkc$jE%|1W(#Kyrn0P($0`_@)L5M}puk0}Z-S^bSYe=937#YcxvyA-h1 zeGa-mBMPu}OUWW$(tHp1Q>@kOZyy(K&ZX`>A1GE2I(jMd|8{vPu|;43W1CHK;OzEt z(V;uWSfJ<@C<+GgY2`7nUp(6K4KTUKo48Oj$U3b)T-bC*2AZ~K>>Z!TrJP&0ANX@) z6`Y^*b(&C38!4t?XVeh?-!3h>P~IIxbzM{GVuH8BVmEy1F`G za7iQh!^VE^a2(vbxV^qx+ag}aS$h*mkNzeNcx{e8C!3DCNgUfr>5lc!q zs>}yHwuUu_6lJE(&M7Na(^N=`rY=D9hEB8NgK0o|qA3fl3cjbBrC=Ule*yj8-UA;o z@ajF#EW5e6^>1#{0d2M`;RE#oEYnE(l9H0-hE7*G{|-D-Lkz03O*lfeKK}G@ns;y# z%F{XXCp5suSbwv_C5|ZgttRE(?B4xO^_5X)2=2}iwQ}0V?+9F+yXn#!o!jS zY+I9PW@gUHo<|_at(sn0V9x!VK}~P{IUqfZVz*cq`8xH&Ts&b1mP@EPXl3}Dz65CI z98~z&s;AL%hy7*0x?Vhac zF6NB7J#A!3Ru6?lPG-3P~FH5Cb--R4&veU^oz?< zD(LBr%7Qoe84!wto2l->|GISMgI*2rbOwq5Qb4W$nmrkJ^N+LS$H=vWaa;W1)tD9j z+#%@P#Zw^!X!YM!60B7rsi9*|g4X=xz%{@4CrlbQLaZi?7I;oQai^Gub8&G2!i|?` z0ZFi_aYRVg(CzK5(AMxMxZr?_%gx7U#7~?lM|IYve#UMNq#1#YyzV)F-m*>&2Y#&A z#oH}daFb-jy0CucxSiMd?JY{yswFxM?7gM_jVCSkH6?ZLlwAXzr(4bi5njZlYjSUu z&Q_$mr}3LQM=9ByUe-h2x88q6A9R1u+s?YFf>}Izv4jM08jL$7_ik>o;cyHYO@&kk zzqLnNF!f?H?W&#e-7}WBfbBC6ll8{9dB2*`nwa|fe*WnKjNwlfBSfgJe^StEfL(Z(FK z{dD;{sCn_dOy9JV@RwO{F9alG{PUWf=J=j6%fz70K-?o-DQ1VG_(;^;iqoT;>`kjM z7PwVutv4Vr0PL-883;R4&(F^z43 z3KscDe_XX*h!!MEr9rEvVZjB#o~6u;kFM2=R!1A0wU$SkVp6f~CP{t1fAn6xaww|q zoTOwdIxLh5jZ(hMb$dh~G|0XX78ie~V6*E)yNx}OE9Yn~S2F2%8vv1{gkBLzoR zyyo1bKuQU?@nDlki*A<-;vu^jgF-&EEAP_aqpp9FYIxtfr-)XNfE4|r2NlP066!x4 z?#iOXYF<68k!_E$#A1-T3pd`#RNozGM>3(vpRhQf>CTFe_|^(BiU$xSgO5wsCY z4KlbJ!jg3qfP>7py4~z}e)D5IeH-xim2o(muQi%XESlLFQ@d+MiTO=&zi+u*n9A)}9n zFi0nj3xRg-c4Zfq{^g1fJxqQ>4E^1+h@F5xE~-Li{O5ja?tTHB+%1@8P2fhsO7#-B zJ!||-{$KU>yyHSaAk9tPtiQ<$p?~jdk*#J6we?$AB0Jnw^=@Qu>kyO(ej^PH4eYWQ+90ht<*r9sEF&{*^@`1-Xr(E6`db0GTo@HXJ5fx2X z-41oY{nJ>7*2f;AT*c2bmzsgTl0`f@s-#JMYyJY%Nu=0WNv10WMAS*9?Y8{n;5Wp5 zzzPCX0VhE_esVM!&awoN!b?TuY?%*6is4J1efC<+bZxx0ln)o6+=pa5wF9TsTl^fLa$E z60`jS?OhbIC|T*^qn<9{B;CTWb^qpG1ikUcuOkcp!$UWfx91OzI7o;s+ymwC5g-Ku zT)|0jsMQ(JegCJRA9V-7(}VfXwB-GJhnCLI&*orgka+l|YqNydL$_nj4qXbZ``k4kWta){*d;27C< z*nE?Nm$IH*nFQR09r?zc;v$#AzZkO(MC4l{a=*%hpiB1Ufy=NlnY>XTojmRompB)p zVRB_t1 zC<~9(uG{y;DD!_%<_}22@yD#8IlLvGEON0s%`L?^)#a}oqF#p_dCZ=Q9Al`yCv>P* zYS5R}O#>92%xH>~hE^)>yukcRc50eD%KVP~x3NE$B#&EO@UQOtvFP`6*leeBKWvw@ zoZQ`+-5jriyT6h3_54@`t}N_Pg6zZMj@-|{oYOX-UtxXwAaeRZdiW%ckWCHqmOhG# zJD>FH3P;aedb4aM-jSB&6>cy{v z?s61gDt7tuJMu5Z3k&_lyOOiB{h^EGkrpT-{70{GowJNxm`#Z-J_Wzw>U!uGB9Xkc zpkGKn=t`wiA8lj@%k)g^6qRz~_<0~dng*MtZJ!0(qWm2fGF5Dny|p8CDFzazwSCQqOO0I0ksLBc=q_WI1p zE2?G=3p|4VrN6C@cYX%JH&~ivD}tRRudWs#jS;?NHKU*`Qgqksbw6K$80fjDS#Lk1 zT|e9k+L3E7;R=z65DcUmwczdO_tsD@xA|1Rx47h>Riis@wT9Do`LnS%W>>s#yh#t) z^?PmCabeh>Xv-GZ2K|pk3=B2j?o_HFniWicff3%C&e_@dZwu8T?VoXt;6adQ^qe;b z3-m*Por5Oh=Qw-4ys^9%Lg3DEK!Mg)_P`k|Je6D`l<^ZPL#iT3F9Jw`odyE*UqDaB z8pnU=ojcD4av(DE!@EO;U1j#IEA?4`JKTQKsCd9#Iv0(^*3lpF6<$_2utb`ZM^#Rh zmDK~FIn)YaKPSKC7=Ep#N%%xpSBr=d@?HUtk=|lnIEKC&lA@zK8;ho_K*tJ?2|bR8 zP~^<8XwtK+h0>K*Kj$#jFHrAB*CNCS?QlNtUf(-dBnzk?H9_of$)`k@#GrY>iiTn? zoL9O=foOlQdk7c|_x)W(5|`Cek`4Qs_-?lA=+1xpJwx=9daG}csLb_97ktKmv)!W8 z6)2)SYl;Ou!!_>O6MwDC>OYs2Q=`!j#j5!Vk6xSbj(<)#NCY}<%EGrXs$bb-tx{wg zR6!gyy8H-V{EYl)qqB~|Ba|r1Eap+-=`Q-#B1Ua?Z8l;}tH_Q69?~4u3Z_D2F&k>( zo~KB&$S*(RL>;si*^cie&?*oTk$p%_5|nT{C!HZjfTumVyVx=LMv-anajZ0JDwiG* zOe-~h)$!bafyKWn4^}<$Sn%y`q#Nvjk0SNSJ>DunGm#NwNfQY8tQo7d9-Z(y~N&^4WIO(^F2^(?R?%)dWp}L2k-q z8IN<(%$G0RRAIEur-%M143pc{G>)OIl1IVDP0mNS%wL zH^9YnHVrqOx8+q{K&?7khE2sYtP!RPYq~g;R0!BIl1L;~+H$Togr&1UYrnG)J&#HK zYUboIGym@>&6h3`|M-zYQ|3A3-R3Y(&iAGb|>~ zXj^KegFr+C+NVwOj0Xlpg$fZNCUf&y(g_ayuP7x`oGDL@!AOS`gVQhqKY2n^>5)E)} zr?}MK>BZXKzRNw3hrbxPLf)f9l!mq9UL>yR8QebagmNNZ`7?q zDt{H}v^?UTzwqma#g~E;I1|SMoZxaAw5{u$ev&%hnYF3*8@&W5?igjva9AA3M->(S z3;}aPsi0d%MG_LbD7=YI-hQHz3-F<`%~4^h%1}u^^Ou+D=f+}(4>ma)Be1AYCvCDj zu?tD?TT5aH(Z)jY(O9Hq36I9r z@b&CikjB;N#O!}MB}m*?tndM^34H|d(y#?2Bt3^@2>K7u55=fGSHPRauF@x_LG20) z#Rn-c>lPdL!ii1aq_OFFVPtV7>`us~Ry8o}0AU6oHUfi|%PH5>(RW!b&xA0{mx=V8 z4e*tjiO%dJJ!FdYpdT}KPF!WmvvsuyJj$rpX~&`M?F=u=baG_Xi?x)YFIjhH>8t5! zRf@BOrg4^)VjrQBtT!byW;#ie{&5wz$94OP{uoOvIh(2rS!nQRbGoWBYC{SI!+|gL%G5* zy^5H~;Yp;53|(HPmNlIR8*|A=wp*t-LlK3Z^bc>LABn6d{NH&&W4`)D`1nao597e( zwWL#?;Ir-U;%8W7S2}2wq-u{@>kulSMvTc;hC<>X1o*e_#dW6qbTmVZo! zkFlIBh)LDSuBg`~-iDVK(7E18^f(vYbDmT*_PT;WLT~-p_nlDfv%yqlAgOsUnV51C zWGGPAu9;^Ujwg}bf8;A7Rk*hqP+S#RuV{tx;gLYthGb%sOeNpub>DwP&z|*J3Q`QE z+ML3H%%K-Kb7zTeE6~uHdbrRyB(X zh5;unI$0Pk3QLRys)0IP97dS*h&w9Q&~R%|m^3{)1ZLXbD#1T#CjLEJT|gBvay{hp z&0Sm^2R{}h_!L@s%qy=h9GkvBDL`+p8+H=>ur6iJz0Xs`eDe3jhZhk>@B3c#!aJ|( z+z?j(>iQNJhOcJ|yD{{3yT%)PF>B63Nc>}qxX{7534idsJ#+o<*z_S^EM{`@MdRYp zVAYN8bq`xNs~)nCtsBeTw0Vif#M^}keqA@x_xn!`F8t`e7$+{f)MO2VUcQ2;X^|a= zT6|+2zDTRg!nWByU%h_A1Id)sLA{k>D`a!l$MeMGh$c_A`QnTgegw~PSeOHH$R=^u1F)}wn# z0$_M?mI@fdH>G&jPI*PoQGL-+$lTE01(zL&ecfeoacyd8Q!F{gZMPDFuNQ?S=res?2eB z6Mp9@FH!j`W+x$-WJlh{kQmw3cf^s4ReM{SO-%+QD+ME&+d2D?xK)IqW@^##3D6`A zJnI4v(srsZw_ixx;p0U|ZGBvEY;N>4OsG6Nw`n;F=Q?#|UAd_(#m1FkD1bqMv(!wi z`1a|l+e|ZAE6<12nOiH<-Lycm-!2u4_Lc2TIp)3+3gs*enDlio&GaQZ>lp5|4Hfcm ze5|OQDu0-0G|a2axC{J!52PHl)!0#Kbe zww!c%S+!)9y;Jdh<^Wfy7Et&|8hdYjv>2mJli$ z=!kmViodd`+SmnNvIKCBRVe>Ri^+{J`B{+!sJ#gK7Zj#~5v)(@8(Sb)&@Pyx%ofK0JG z`|l3C7>h&A;*@V>^kf@VaR={6+akH1zZveTrtUkszK+bJ9b$9IRYgLn>3+SmJjbrE z2L}4UG7xy;K-Jdg?*ZxT!4gK8vZR`=!x&pRm0OKK4dF)lOknoH`8x#|B{Hej77pz8 z12ERtr3op}Wie$manW*UzkNRDqpW-_S_`dh9ovnI*%BI$*Zt^#Z?bR$xU7+;|IYeO z|J@F~1FLeU>lX|gc zg>l_1_z*X^XZZx?1q2lsIwz=V(|%V+tV6DLcMj?Bh6>lSXQc;$M7{9lH8dfuI-szu zIgv}h^**UPV^ron@QMN%18f(CcOH-U5Fdw%N9YL-QBWa~$zWVWCXir$L2zOwW3#(NI9CIW|o2qJJ0i zgbor{{DKj|e>=2E7jj->dOAFSf`&rlJYy!3N_AXxxE6<=B3TU(>3e|}x|!p)yjI$*=|Ar0cuOonpFGK_oU z;8tYFB`dY=obE)EunJey|2$L%*A4xm$pF-Ir{71wG2}Env6%8n>rC_+%EYKD>J3FJjD^Io?RfJc35J9k3PFLr2G8fcWn zNH)a|n0?yRC+{d5P)F>AJ7@fzgcwaimh%{Y9|f5UTTHDA z02R@AK?~$Cf!}DE=(nU@LziiVfflVBX(B*5$G@Z8i57zTC$GsI;8g%lG_SsDzNr#8~I%zdTf*F1RZP*vhge$R~^mv z`M4(_xuV4|1UNOFlne-E{6#JHAPtNK;;sW0rqRQyXV{J7gYi*r4T6b)0dsA$z)4-mm zKE31dVLMwpjy0htb9B`k`2U5q=tWJ|vUPO{v2o? z%t3x_Z*jG@v1`H1l0oCui)_cIS#sh3lo;HYo8z9+99N64s5LZY+F16CO}z~lyZ06} zR6bLFhG-^Lg?SmtrtC&Cw6$}<41l$QgGBH=5=kMXO#Bah*T7{xQ`r>WsHuBdUWW9z zC;jh|zUvNz7^)%Ym)p(;qx!53ibV)?%V+GqISFVH#Ky4q?lg)eDXmXFIiNT$MP)bk z4Yf9*jT4NNO|fWr#g@?vbximOv*3ePR%+Zuy$7{{RZQ&K zuI0Nu|93mV1^YJeB?N4-tUNvACnu@CFZkN=FQvnEe_aB1EZ@b|p#Hl%;L^kK{d=o@ zo3CA4n|)iGfzh%}P*B$8rG%hh=8$CV_ovml(g>#g&ZugpahvCmG%_oLp21k7n42K6 z!~s7TFvQ>s=t0TVqeuTuhi=qQsUT9>3!)2Mw~0-;kte|Ce(0sr`fW=Y*X!envs2x^ zdePqBE54CPzT+A<#7+mM6dGCmAdc?uH+290y%qX2Rldf!#qWY@A)Can=^sAxMa36n z-j`jDt^aWUAB-zR;JUnqKG?+&bdPsn6w4pf_5_6)-Opv&xScb@BL~OF*T9uui8*ca z=Ma&s621aNYv=Y> z`blam8ip>=Vy3{Y+)T25yD5c;kDLeL$H{VL(C4Yp)wjCN+~bxu9;;P~_q1;S)cT9p zgv9hVgN&I8t`b*D@86hUWe_M8j`Zx51%fj*3yNM4EC7h*S#V&u43PNe zRDOTnfHn4i^PBf9|LO8FP@g=J38jr|uZtGUGbdv>s~+$n@L6I@XjGBpfU@jXIXOd@ z)=K8sQ?)67rjbo+JjIIEjw8!kZSB|7u1HTCv$md?N4>07V-w5MQ6Wq3w-C*BTP)fV zWt#6ERex<$TcNP5|HuhJNE9SK-`ljtiXAk|LG9fJKNd1rz^|p1~pOFJ7=sD zj%p6Rt*o{4tb0VL@QrT1%^O%4GTUiB$Y%latNfd>Y*lh`%bJyQU|K+zsvsl&kEcTo zQ)c3dM9x0Bs74%-4$9@p+ALX(bKnE-;;LD@r#!)3M3(PWadB^%x|LoVHEknTo}V>t zO*nChe|hYko9A5J!j3U+DLO0MkzHahdKotQuaj_Bp)6lx)=9_S$wkx95z11giAvyH z0jh<ilx;lBF)Urux4K8E!3CGwIC z+U(Xc&8Kq_t}zc?JB+Ja|_>DT%4fQSu79} z)Yp3@VxBn_2byNCg zsNa&*#WlU(-98LnM_ZZjl;}-hf1_rWB@kr#>aK201M*tDV%D2#{hH(-x&P{;fyIs4h#9fUz-HYsVnR?#hAo#%^b7j<7$jTiBn| zk%w^DES+Lxq9t4Byeb{|{++nR@P!uPj5n3Zg1d>Y$nk@~0xWZk=v_SjvY<<=;oji> zI2Tu`i+5aB^)Bcz-|$jQ+dpd(!DlCQzMvA% zJLp*Qw&|DUPis%?vi$TQY+TfQECkAjN(v|+Rcsi5C$0)*&9KvBOZOB>jl*}?8u36Y zcApWZDMW6|Kab3?NJ#=obm#ZiRd3kd^Ct|cMN)24l9wRr+uM~|P9RwUU!`|%OY0fSDP!IVvN;`vT0@ol<{$X@E} zXXvP;tw}C-?S;~`6}YL^n7g8fwe&y^aVaY;c#y0qPRQDSJ<#}=eR=c>0vxoG>oo24 z+S^Pw{w2zm0j{avS!%w5vxL8kJYw9U@mf`nubT2#K(z*R(^aXRR^k@qwtlm0VNhU?+{~ zwpl}#=%o7*{ItOY#Z)sn)jwS}!=d!+ccANNm9IgKWG}-p&2D`Q+@}P{ibc*%_Ho$Z*}B=ZOvM@Du>$d;Lz zJ+iYMdzW!!7a@B)wh)p%4e|w20#pkwRBoo`5hW)r6~JvG)#3z++#RR9m!3}o1*sEt+v-|oFV5@q(&%uF^ zJDN1GK41gfP8|2cMZy(S@29IhRx73-+2p*u%-=si$rVoh?h$+3v;ee|q8)swvk z3JWdFqIL5^A0d{23cDsq(VXNlSeI~W6ePpBi}KvoDgh3QH*3QBL#vkF@`Wg4aRy4XsW-JMN|-PIYrSl5Mahb3@FtXDc; zY(+$hi)IiZV+C>Fq$_S(GV0%dlR{-gmFlS0b!6_0v(o%iwvQszJgBE=(mGJGLs+AR z=e<2EOZG9T3jt7x|IMUhs%`A{5V4(S76B%=MP}jrIq#}SNK;fu941mz`0b0yv)tvm zz-A0Rra;!m(ED{w%ZO;`n+mp}TW|}wW#4&u(KNq|>0O=q8%&cb@su>=$Jb&8ek(x} z!q8xr&rg}O=M^aBx{uxgLFmER0Y(BEIP54M9W{}-k8@W$WnU&e@CL!96^QEW#AK=~ zQ{K0J_l^l5xK%ilBj1cNrfnSl;`0cjjYxckb5}`Qd+NpI<2GW~e^a*pqQKq6t)|EO z;O}3kh2?`s&Fqh3&$9#`FpNYlr z-V8bU62a4eDVDGqh`*wNGXh-Ka+kAXZPUjlO234|a$&&a=;xOWFa|)XKuF&%d~nHI zasMxGi`_qFF$3w2^mV^;j+=6xeDDyK?i1-egns$A%W`IZ`53#=i`6NK6!a{1a7N9*R83kluV1=twN?B7@rGBVzn}jh$C`vMfWHLAc$O;#I zcq>WEnC;T)CCaV&sOAEf^bBB;)_j3n%0BErWTcI^`h$e=mnDAwYUm-st)LS;+!C5$ zPN{ebQf#*Rd+ztZm2G^QVMpAW*LOZnke+bUu~Ex)Ea<2Hm~ak9A>W85R*!=7+X#<5ho zF@wzN>X6(IIT;HPT1#4ic)UQCRiC>^s!h4T^mHypO=e)YII#3Bu~p}is;8*KlP{uj z3imXV^4cO18%@xX^gF~8$oGexu7|Ajo7rIha(?-?B5F79Kc=Tk=gCilVYPCCbB>Y4|#LhUcenA{1njEl2rXC_x1{y zA`hmDLLiaEe6Jbce9X3Fy#d%OmNvCAV|k43F8eqOdjx6L0^i*4f~!<2cmM3*?fC-c z`u)(5s7NMOcJ}G7-%Za;^%S+q$0LfK2sTf-qMn4dD=(TcwxX6oeL8sRO8U0-$`N;kt^R%$(Y0G6OTUwBVRgh{Hq?AzL$77b8+A0 z{igp#zw(faIj~<#?kpHI;9fG+PzXDkJv@J*#Ct|GdwXntrHw3`mpbj z5IYe3@GpokHUKqc8wB)SR();R8(&psh!&3*EZF-L!oY5z7UwC)72xa3n-(8U*h3sBa2iB|UGDUOqlcIogv% zr&Bbg3#6~%qlV_CdR(J-P{Z>X9~j4(f9H7ld4MZkx@~#(Ahz3CH3%z^CCYL?G^3G1 zw9Zm_GC1&U-|T^;ZDfhDhag=XgCkexfsUgfyG84~g_jK~w&ZT>bxQ3d(?YVq)eu>7 zV+>6X1Qy*tU@s=MVX9h34Y(z`c8Y6V-j&3AzYtL`i#dkarryhq)-2bnP!9E!1e&$LM;7fXqconRx8G=msZ>uL~}|cOw}lDs#qAF*la$FErF(k z?|@6T31tFCd%pJ~Yum=m7*_Byi@j)Vtj>SN)5e+z__8-~fzL4}Axr1xvg_uZC-jx0 z;N`LkL43AQ14UnL;`NWsf;sf=R_xIlp@1I+@+gr7L8<_ceGIz2Cvg`3*JW!5hiBKT zUrqRoxd1#3lI%d{S`|*EdL;9n)DyBTi&%%Yn)a0BLsyrV0VSGnRigju?7qy{%KCcn zZHi{63iB1AIc-h*4OH&mN`6hVA9J^myBE*QfTwKQ$(QmAxbkqIE3dyi(oU^d2OUas z%_w&MqZfPesH+2r=xgc=!2Oy~p z^Zi4ZBX)uMpwMe^WkK5O&D754=#Z4tKWa?l7mbi1ZbTS@;>U#BAiqCVV+QR2+*Ej$ zJL>bJWb(rn$WpwiK}=7UQKdUP2emtnIIcbb6>EkXvPHS;$zld!DKBkM@?L zZ3+KG0Zu6YgG5jN$? z#1)8{&)&ZyJu+Q9$b=bC&$Ja%l8ck!Jyz7h^ms(5yH~IdIM6+o4)qH49Z=>x?L5G#e<+%- z5?eyn4O0zTb(cJ7q{Z>LyxZN_@mJ3#-fYvVJ%$jyF;6nF=CE0o(e|jNLsxCd$-)A=3e<j#2W{HthLF z@5g#2dWk*491FPbBRsCK*GX{RS}er0ZEGa{(eFoWUMP@ahncI^>p!J4PRy0{SL2x{FoCM1KN{WH z0!u^7_Kb_uzVoZS-l^?>4x7nQ!trZD^l`vaNNCvV2K7q!diUPbEwc%3 zRB#{rFQG=b#uV^Dr(0fLo`L*;xddA~1{9pMH-8T5j`R8*;>voceIN<})GROj^K&I^ zUTH^Bh3hU$z%ojB-1e#kVdMZn`smRkfMI@_89?Ws{`)pAR@cSz@JoL(WL3gOoQVQ# z_Kefr5Ttj|3PRIj!$xr@nvu$dU(W<-PLBfeugyAUw|UkA^SOx#f|>ROJ8lSauvmpO z$$jIye;v7WC(b%5H_WVo-?{lWuJZ5Z`aIuf`j3vd#vW$(OXj6{z9ezsY;2>GsF#W3 zOz(k=@e=CjYF88=zbla?T7-twwH`+ZMy|sW%VrZFnyrF0Mahvbv@zXS6PrUKKu9jB z<)%sNP!`PxavjFJ#Vy5hMZP+%LOOqXW~JUwGZ#9Q$ulkJejo|Ac!0xciI>e_nqK;cI+1tfT!r`nMx7LjF6VM<4CFC8X-GqG|kh*gZ+LCQo zUr+ZQxCJtxz_$#Ho_1LR1<`o&9abX$Z!jnG3A!$GokImRpGp0B-J&>z(R;s>a<#L_ z3>%EP9oUQY}4=N=V8l0sff_H<^|RQ4-vEmne~;C1<(SkJ)ZNXEMAw5fvZHg`+9a z?Xc+|+6cG^Ngr!Ilb`>w*t&lO*>h_>ebx_!|0H<|yloMDpBK#N-MyLIf27h%Q0Wya z&DOlIvimx2d2SM@&Fivo;JH*S9XPqqIdsZ7@J>H%=R)+*%`R&dfIu_wMMS1u8A4cI z#@2?vUd-OpH#=ukpQN0trvtlUl%*Bi1_XeJD@ap$ z`EvfT7PZ0_WPoT1gFuS%rjl10M5+~QW_2ZVwWC3hvmf;OEFGWqV9`%*lS}TLaBtoR zZ>VguiULBpAsX_|+?9`NTU5aiU!GFQJ1-6`+2a~U%Enc}UeM1KjTO?tg)id|uo^NT z2hNibkodpa{`wnx?mz4yWg)XCN^j!k>e_XFE?lV1JeNnRbT{#w{9O)D_X;S*#v~-q z!%Q()OEWM8x!rFr7?cyngn&dAZIkV%h_!w+HfZ$oN?6D>QD@IbZC27MkpZAjXbBzz z1A`F#0~k30t9VioeU{CnJ?xtiH8S&D(DWbHxwL29%}Y%~SChqS_%nzQ}@7E9b zQM5feIpg)F*a_6Vai?f|Lv+bjWEcHEUxx9FiOV+th#WfthnXs(2W;w3F-z^4$5+Rb zTEJT!L(qb`Bs3(RvQM3w(&rmuBT@tv2bkoUhMZlA#B~~S-*B!ePZ;9IuhB|tB#DYf zgIL4#Ua!&-FcN%1$2R%9nj@e!=Q!6`@gjO&ZJGpYR8YYCVwbK2h5BBv!3P`GPo>b` zc!Ag==+r}G^ov-+EXX5{ZAIh9#?IHbbpubf$lA{qFlXtY{P{A+5zFTBm{8SClKuc> zp8VT9u!KrVERV0S=2LHPMFEW_$cJ6o9v}dvj0zf^V{Gi=3rzLBI17RKN1ee#<59^+ z?txw|9h1)KjZSNLcas6h)_-)^^D84B>c~ZgL=b=9pIMNIpvy&`Ewb^^M})cA8~S*R zJCs%^a||{?IVAK$FLlvl5JOC|ou5~OY5G?RIcOslJ98Y=KaLt&LfH)pFNYkaTD?k1!!yrx~HS@RpX03QUL3iwT22BOxF&U^Y zwabsx0|aNP5nj1*H=&uBt7~g8L}NcRWm^V0cY1#OV{NTFWAE3q={Y@nC(62%SK}99 z=0tXze%@k)jSNZ*xsfuG&n20+!+r`?a;kC`+V#I#wTAM9o9*?)B3aQ_!CDCcP+8d4 z*pTW4uR$Fwhc}js$(;+LDdP6?9UE?PVz+y~R$imOFJiSvW4fN{2Yk*#;+=#{ThBXx z2#mCkr=C%m)^@s8OVg5A0AgcGSNB3sgJdx@2roVg2stW$I-gNHr*dse5GtW3D z&HF{xs@-4f+`%hzvkt&U+a|XbD(FjQA{}Kt42e9F3+ZB~xK?Xt9!y-F? zZC)=x3*}cQLP!#er17RyVad)T+Bych6$>&8UN$)_WjGH8$zx8z~SCry4qRnG538Lyg4ROde6odnazLy2+(Le zqDVB{#gs=$+ot~zr)FL;4Ub!mS*h`9n&5{u9{DYHf)8$u>U9PvzSKPSYL!AVJS`IM zi{s#xH^+5=G}k7$hWsbkW!Sox-IvWv^Y*(Vu%9QNDxC>TZRoE!IVM}*OSGtX#dLK4 zo(OP@o?e2W9)N8D45+E8so{?mOI-15INOh|cgL>z{=5URYaD@pYx55ZnXjTswDJ!d zKvxHjOrT_pFelx36~#Yf0siUfqF;hd?Er}0ZofXMmXwg_I1Nyr1ZJ3zRGsdo!Gcm_ z&(nNfEi01&3}%4H$`an)pt2q8rct?Ko*Q<9DtnU9)#*gQV*&HR7ySP4W^g|N^ zhoIDd&J9CrTv`$m5@zDLpzbnw#czKi)i3fM4SWKyz2+yeY@CxkYXcB=7=H}H+Imhh zLJ?eKLPY;f@p5zr&+Bk%OpaAJ#|(Hw-%wt^^;DAAPvP^egIH643wOYSa}p_X)<1Y! zsPoC7^kaV}M>LZAO>{_ESxie8RahB!Mp-oQE7!z+vg589-tZo-1}OqJ2Xjkq(>vM| zo`74q0jT=ybDO7lc>Ue1H@XZ*qO8BDupI1ubKKS5BUaSZrDXM{NBuKgJ{x5W+<$#d z7<7J=4>DL#{^D!Q*DK6tK{w1dcsKhAH~1a)CEcNyry%-He&(dN!{|7F=<1E|3Tb0@ z3=2Nc&(S3?$+E>;0zwx`8q?D44@CpcXq~?Z&*8x4lkmB=>$&cj-`7a!8gJSqLQrjJ z+M+!(9Mj1_p1@#zt3QKqpxk$eT~ zj^}?Pj|~hBKmgBx$KZxF$r4im{zE0~B0Kez;GGhQyr%}d^Rs>(S~m+?aJbz8xYr|s zuF*j~4R~UYuuSkK!X(JTJ}9UYD3xoMH@q})vdqH0TFIDPvy?%TYq$xgV6$3ZhlRdi zEqXR!KM7y@IR1IS(JINw z`$HNL6vo#cLu#`ZB0s8|RWi)4h%ios+(h%;Q0(k5EnP19&9{Q{5`6?j6d{o$H>r>h zKA6`0TY)(MUCupKBlYfaRi}+OYkN+M+vBQ=u#&DQ;ASHV0F@rhVuG@OyT{XM^N7He z!7{5D$DA=jTR;nmDYjFi3=cDZ3ee@Q3Ti7e64J~lZ5w{|kQ6Uk`DH-Ib79Dqh#&XN z)#16sjG^7{lud!;Gm7pXhxLhx)*K{Zgn8%*%xe)^qEZ9ie*^jd2!c)wgL)H!jtg_F zWI} z%}GF+y{VhkHuNqR$QGudVaUrr`Zl*Qu+5X8r70mHyRp0*%VldT1BGsmkCTxk#Gf6n zhfSI$fFb!GpEL6)<_L9$$lA`}&NpH7N-6F$)M&fGWEt6!$z-t&*DSXbq{n1RNr$DG z4ZLFa&)3!lmD*WIJe#H_Xly~=8)$67gCT0xt?}Q40fa68Y2aL18;#-V5j~$0*s#iM)LZ%97*N@#MI#5FeNf<~+d_uh(ZiY7eTR{vQ8H!4DF!B=Op{9C z_nXRse`p_z7!+#`@Ff)yRIQO-2sAP1@E7axe@?Y7OH(c_GN-3T!h63BelAlZM`5?A z&^Kq;ztb2&d`__U+a!%wrZsNl1d zDsGu6(x}p63&2iTGN& zT<7m)Iu(%yrxGg_XO?y+Z{}zF`iB;ca*q}}%S$D^>W1B}W7=N##*DTScqec8O zm|WwEtNDha<|m#IbsJi2OVbho-}&e0u8Rv_Wd(aFd0IEu@wHvY9>+ACDD_D?J70AM zIaUo4*rXz2Js9GnfRN#Ynz`*#ft z+{MFVcK1F5SkD9rnSYRm`GB4QKtmhNoA?7QoD1md?IlZ3M*-nR-x?ujGSkudotaz& zgh`$b)z9 z>KkjDhZe5|q~~7gIZ&-iWLK*p!tAmLnLFZ?lbgLxzk7Kl{HfHKo|f{MHkkcwZ0q2V z8+YPq@Y{)~8-kC8+s&GOK5eO_{W)dmWg1Q*`~vFJ)tP)RqA(aT`xNn)OhILm^P3S{ z!|`KNBb7&OCC}IQJ+0e#O?zeAcv=Q(4ygMZVjE!7f-xAkg*M{F>32Ny(%yOCwy|<@ zdeAKdI3gg5X!Gu~QxH$vIRw7|W_pllMxmS*&nsmw62Zw1LETNKPD1{G&|uRni!$D_g8Ba)Th(+=uG*_M1*^Xk9QZt6 zid#uEpcTP_l801TkN;i-V}rB1mwtW#wP2@t#>B+5+ISlX0*2fbLFpNm@22of6!BL9O-hAN?+wOVkB9+AH)8@ zSF_D@GJ9Y*VDMAeCHduyaE@aqiy-y# z)q~Oap6dwZLe9cc5ir^>WCz)>JZ)QF>P}_L>TpSdZlvRh1BcY6)Q3I%ao%oe6AciazgWvi$h(k{Ox;J&M;mk# zD&)*2mI>Rw<+pwds#m?w`eI>;k`H_Q5BXMSfhg`E?U+#mfo zH8^_|wryyJ3W}^mF;vKjjgoLKk6a4d z;jE+032Vd2$QALu9$1%^$O_;UMUpUs`J8;6(FSO8p+N_rZBz+5Rsq?(w~yh=_8aNj znAJR#Rv&h!H22_XB7}L!ckt>`={Bh_PkRuhkwLQ2c97Nk_bk2953b zOH;l9lV=6*nw=euplzC{pz(9-qM4T$*XvwzpNuLG4^Yg4d;vHARbT(N!50utdd_Q2 z9bA&Z04CL7mhj!<4KU9PS>FK>Cr2^`^FeDG)a^4Ht@y9&|7AxA)*RQ}eN5t;^8Y1O zHPvdwhi!ene45pxv^MJ*rRU_Ejl}@Nz^z|iQCVfNCADyLG~9JMTkB|DHNHHWD-CSW zlLL%jXp(JVh!=zngrF0&dy^Fb!LC9BdJK3<0nQ`P`b#m=k8mpB@1Eyudw^2R5}GZ1 zi+RolbQ{Y#U@rcZtL@)Z6_b3r)P|u5Ggrn&3A<8~aS_z;S<))Er3&V!e4d3U>QAX_ zny5^D3vY==e)`N?Q^v+~A5ma6vvpx%E)>fKqK!4li6*UzCqK2e#m_n?Pw#}CNSW8E zy(nGQ=lxzx?mpQgPUP5a27G9b<9N(a!ltB&_ut!PWlDbt%wM|x+mMr!XOYDTLSV$` zaOizg^QN4{_6Aeq4~ntO)kV0qv5pQg02gxC+ZalU4c_RURy4eyL zUv)fv-m2`c$R^`8bNuz@4mseHeX=W@pJ@13%pXuK-Nu8_U(RmAly_wdynf%HG+OTcY9kQtKeKZx z{*e|_74OrVAxTRFE(@DlPzK7USp)hqz&(NczPBlzV4c%#qJhOZCbfH29wa)l;B%*{ zf%^{~1=43e6!0~@5oY0A8){Si-+}dN;uZtzyxa&D6Ij33ga^!mH=6okT@bDS6a8L3 z0K(z@9^aiD8ajr*WDq0m=eLXb!+*V_fIra{qGZ+PMUybdAIJe?8@;4Jx#IgXYm9L| z$lIBal|utw&f41ASTTs5L{DsQ^Vr(jg7$Iz&pUvZK(DTbfTT&>D`;pZ7&Z1-iNK$Y z{|yL|r@RcLj)D+%hht754NRs#+0#wdDQ>#7zN9d#Xg?#xhQJg`^}t3s<_Z`~=NFMv z+b=-RSZ5^W)s~zP=z@R&lSu9eb=q*D|6{(qo|yJV#(>o*+-uN7ss#R_vAxp8OSe#4 zo10GL#3rr}czh~ljYfDQC`GokB%q8GX95pL!tmzqKYT=Qty^HMQ!1V%_GItJuU`cV zB0`k;+uq8j7HZ406^||cKC%9-PdAjvok~Ri=uyS4b8XF6wN_-(Gw9ku?6`M4ug`8J zD4pI^G>Gx(B8}?aRC>N#nEx~h|KvFMJ$_{1JZz;OY4L3Lt?F)N={I}w4TM6m5ps6c z%Gx2)phT^XFA6LDD28q!hAb)nLWHE5?8S`dHVd+zRh}!M@w5(eR|V z&bHzw+uqw!M^BIcti$blKh8Rj8!dhMTHmSG-St8NNs(Px%M-uzr4don{^`rK-eF0cO?A>+Vn-aZJ+s2*egB6?p4+njWCr{mQw2C^2ht6p49CDT z8@byM{zSj;3B1aBVy6C2c$IEqTby#}yXGq$VaxlJ8_{1IT`+e*n01nMX6>YI9$4=& z5DOFKj@rjmj!;cMp|AK>UQso@T_Fx1(Y37u(MuL}w|!Yb?Qf6~s8gDztsf52_^^r( zM||ZYoE*M>68_jRNn{*b!QWWw%z;Q+j41rq+gaM6h2cVvBudj!J(cEn^oTMZOofM`|a6M#vS;JS`RMU06$ne7zY8qRq&ef5syYpSLEI<*I9(Vs{TQ8j!O_~iLb`Mc z0*49Ho55!^tyalG=5qy5DV-}!G(0t5NJVa2EE9B-f<$>Sdcq)C9AVOk6${euRcqHG zXS_2lJpa#(Yan@j7GzX&9Oph^IKm%ri3ls?R?Wh-Q)^!W&`MgAr(NCL{KAqgSPP`S zdpIffY3mp0d7D7R#YgN{$%lF;^$N< zF?|3d8sRe+pApJ(c#}r(zI&rLu`pS<3iEX$W!6Z)RG*TPfKsVIuhOipL3`&RH$rW9 znD#H~_`_riM%v7L z&WfIWqiwqIw0dfMgZ8cXr=@(MyzjI|gUYXvCWb;{1TbpQ3Kx&}**mg4q{0u_tfdRR z&e@*t)w2|zyI)xri@Z;%)F%Y8V;nM0h?`1T37P#wR<>87*7j1=c&scrjxNi2$fJ2J z4a#5sA@;2c=PSpKANee2A(2CY%B-HBiU`q1Xj>C3y!iUxgNi;WqsBN+g8B25qZk6b zNVv3D7I`i*+FI>%bcYy`+7xwhpOk%Nd{wXNcx++eU9AM?>Ha^?yQo7!$Tk)g_p}gWs7hLo{~2z%Jv{tr>iILUn9}2Ye@y6kh z_jsdVS33WV(~3sgkp`kNvG~hvrl3_W!evxWP!fU-8}J z2g0GZVB%lD=hycZWcPNhk5&xIHOi-qVh<_}vcXfkaIwg=T(@%SvjMLjFBR{`p#@D! zffaR2gt?V7FST^En~4SMif!`sScNBh2+J^5r4pGrB^l*jeTdm72VhJ^`w&|EI&JCI ztn6xuK-U>Xfnkgg1qJ15i;Fv1BhOUt#ln@5sa$gdzt7Sut7 z<6G`U)&ACmj^+oC!a9yxa1LI5_vQ8%Xsn8HvekatOSO8s)57u^{rfFpK6`*Q0ExZ* zUlinn$nQ2-&Hd@O3aWa_{z?R-sDqaFIk|QguxMTdGB2IlHO$SodnL5C%7XZ-h$M|7 z8t8u$DdB|;*6Tghq-;lDZL;(pQ5k^;W<|vh`v$vpt2ec?!<<~bwn^R$3!=#>*<)f$!Z9p*njkuqp(ak*zjk2__=-|ST=-)c~305}*^pJ(jF{`36LC$QDD zM7w&@zHY0cs^)9s!1N}kL$2mtxu)W1Xh>P(B|iTWJ@<2R7(;T8a5SG~s+!)QwVnoh zyrut=ZC>+tTB1mzi-5jl0(nxiEKV?dA`IF#o_s5CvpCcql!Xj4?|&m^gzff-Gpx7g z&=be4Il9~vtSr0Joh~g3Ui+i7{ETICjDDsn9KekY&>=i2)sYApT;FD&`4eZ`nf4uiq*7_%A@=tst?e{bJ=uZT^90Vp; zyIGFqRna-mdPcErW!8^9x>g2q8X0xI*05OBJgYx3OwxYLsGpRymgoX4UV7-}4?L;s z>_acA`?t!TB5EK*ZhLY%oCh+y15pFsgWj7Wwf9sh%9O6$8?E391|Ha?KJN3|8H>G9 zk$AeA0t|y3^5vX`s#q{ARst&}#WczZiN1W>FwFUR*mZw5p{|&|!VHHVjAekXv7(GO zx`GWQ+$L)-#^B~Exi*_QGXJ2|kfSUu776Po?~e{a{`a{iHYyPP zIQ_f%aLWa>eQSPbx!m~339QeKG?FS43t0jfQSg@^XsAnoD#xbwL)DI;EEn%yHiQ7E z5&`%Os1jE_L^6TfI}!5XUGvD_zv5uI_bp4qV}S(>{dBB&YK3 z_|Kh_UIlMUa6fH@Ja2k!x&6Ja;t@64-L18?rmOElYVOsminorSwgLOSi6FA-72Kjm zj0p@u{RgN?%}IrFN47|CL3RUjjUaD_Oo{VL4C$+<|LW$Y=Z`g)Qf{R7ouHR7Kx*-h zaZ%i-y4&j$@oU_mHzxt``Ja&j4Kq_BhG@bz%p~TN=KVbIRSIh)4_GNw&#=P~N-R$r zjViC>oaVp?2&kp0)7lc4<=Vz48iG@_$omUX8YQ|iV?zeG?vHrkX<)^6?ypIT9W`ID zV~w|)y)WMObOzDZkan9e^NNradbBOgm_TqDV4tVL4oeI(yJL*xF{NDDhdl1EDD<*=ZrQc3H~B#)0v}CY=5;dShISalW!c4t3NN?+t2U z(hXKTZBMdBhNrNXeVMe|J~lKp4H}gsr!Mzy71%BAuQlVWdDJ0lq9dG5RM03IfbAkBX{ynlk5NR!CWU(0l>u0T-r>U)t@A zD^dKWf~H!RF~fNEqFe;%4Etw|kQa{$SP7mcA_=}?$c8z+lUdg~qb83e(f9I&)VS%e zOh#07v>k=xbrBsLo{nANxb*Qz0;Nic8{a$0$)I5qfpW zy-~^ke1{h@YIrO&Meq+lsw672CW&%oJjhH?_88-mk@fseiyPd*JJ7F_HNe?l{~Ygb zn{00$aYlS!ERwho-r-G8rc4ka@IwDRDIGkElfZS0Vp!$Z8f)!%nDf}?!PlX4mXA7! z&v2j|g%uTn743S{*`)AaV6~N4*_vO0&)AA+iDD_@C=K+#1kAP%{tttoR^p7pbYryf z0mNgnHt$qtQ+TpPvJiD&NfRNgU!S0|iN8cD_7&1a+7i=*-qD~+c}^Za$X`RKs*vBG zxjcqD%*o!=Dyc%lbmdAEY^}Ow6b-?)V^ZhYm;HC1aqb-j1UrN9v5m7XF3nFSy5sr; z&#o^^gk6`(hgHCLdUTLd8$+kn%qqI{(b(CHcC%n_=xWui$+xOw>ZUJd6o`Ox9bVHc zTG1_G&)x^DwwJr3aL$03M(b8nl-S4^9os_K>a9$2?1(l2rGD7E)Wx4#Rt6AWRd#@4 zZVl&-j&_>Mj0wAqg(3qQSqr)%yQUu+2!$sD53+#{#;DRJW^05q&Pe%KJWaRoD~m_W znnk*!`K%)??xR2A;F`~5N*BzSv42d|%`$>cfo188tSN($Az;ambV*6hoTQy}`L=4- z0mVr<1f3ZsTbw)oQ#`Vg4`-~}AG(4UP?^Kxn^mLT!O=H4qCGCATiy)EAWgh|8 z98xa*FmD=G@w+q{4}aJnE;xoeL5$n&h~=UVx4v(c%cFrIofwg)@L zFvhj=%WZEBB4uBcPi3fc0#r!>_eJb+<&=HUnhz65XOWyZEi1-&773m!5(c%03v!Mf z2@zQezu}G!S(6})-0ywd8+@(5uU~yBexOuT&0gc=(@Wy>zxau_;?Jt)1zufD%OvYWK|NB88mViABWEa%% zO^$NeXj3jA+Jct78UF}^nFPwaN|9XprSG#|#0!Xg&Qruwa;`JyVA+PRPLV4=*Venx zXKZGhef{L0qwh>OYE|`DG+>Q&yKWBKxZ|>?cZ(+2+Cp`u6?CyV6cx0wVZ5pne2A8v zCW*fcsCTp)0r8`<#A85C6>Zm_5}Wgp><*7M?Q%0nP+OJ|X#F%cS*3LTpA zbML+|rZ4_j8~A{N3$jG(402}JYo2Yk$Dn;Ajw(hNW=iCUeZ05aJoeaOb~f7;Rv7gx z8)pZFrS3W>W0bWNi3E6KH+s<%zQL*3V}8?vg4ZosRe6tGPTGN};x#TPVQ0dh!26!a z*DFM1^97j9D5Y)m3S@QtK8@*{S!D#kSJawt8zvdaqqax?NJH1A(V6@VTEF8K=-S!Y zc~?b6#rW@~w*E;aP*ppB%Cg1tZ3ICrfGKqv~6Y}@_o#GPv;?!GTOhiR}SAm0Pl zWTf3~V8r(JHfRFQR!D*}{(^^e9sRA0YBV9}-z^_)aOwKb(jiC)gTM-mDhqZl+(}+V z^utI5y^dJI%z&uGYbhWd*qQGz$pz*pY060gdI7L&q*?Rxg;yJF#(-jg!MW|kFkk)b zRK6qMp8kE+kyeLX80$S)*9-mi7TKBUY44k>Q<|Rhq5AQ;U*e!<$PS~qnu&I|YYTV) z>pBh4%!l^+oh4R|M=c2T(1ZltyXMyzkm-M4W(TsgOZ5gTl&WN&TRky-`42I2P7^rc zd|@kye$D06WEGwSpX0}gdUQdU1h=8Q2TQ!PYH)gZM_(T-#Q?GzB64Wah-pB(8;{*f zI3~4|D6egQOpqV<}9IavooD; z9i>1E9!5%7zB?0MegZm(UeCeVsWrB@je?C??f!fA2nZ0A(~;nLkK#OV@A4Hz8X?B@ zJ|I4s(a(BmkoDhJ-y}$gXJ>*EX8a}_u^;r+Eig^`qJGO^H6EP5pYmL!2Kx#&{1$q0 zep13zPg6Ud_G?Mc&On6uJDqD*qrT9$SYx^|3aI4$^Q+q60g!N>IBP9S0P7o{E8&J?L*V+QK+~ z*&-ulVMw)?5wYg|TfA?gpC)mH;E$^8Z>hS7M+sZ!PbojEiv+xL@0hgE{P=ilG2V{f4kg2TKp_~O6et&jDQ zMMEj-NmtNl7EN<8*!fh+U1r8GRW=KEr7w#q+;ky`B)D3-22XWzSmN?GnnS^MFCP!e zkEJ}9JNI|^=@t@&eyTH>P<&R~jAx5o`A%~%6iIimR8_qDc0f-JfOK`mt$5R7t)p^0 z(!R)gdr{hs?8d;WUtb9HEab{G12(^Z|)FU?fz${E1PdEo8^`K@~Dky zd_n@4o=5suFcU`twwIX3ZQ3dx2@_DR1BjYr%p_>iZ4Zc*wR^x|YPQxEJYx8aMS0Yy znOR@aj(U7hYm7C|BJ|Se70@55`Jx8j#XjTf>#`*gNXb-aAEY*4K9YKhjJI~~5DEA{ zrrt6v%D3;@{!1vO0@5Nmbax8U3@Ho)4k;aiNQZQiDIXT6n@=osT&@?RWZ*m*ClOjWfT` zj(^f{>~kK4*ykeAj8g^QzKyS!T<-|Y&%#4W>G+6 zg1s``%oC%ocanT|$s_LJmXUCrNe&v}m11bX5xU9afJXZED5hA4-RJtK|9XwQ18^U2 zfrJ~LeRsVbntk)fdIz*f7TO-yf{(#z^F6RO4u5*^ z7~IN0(L7j}Zll1IHIPupg;o(+Xtcnpcht~9s0UrSV`K5X6H~wF7qw!ACK6z>ne5$` zG6*wcXoj*a^I%s6R$jRfC*-YGOSt8@i-Z*VJZbvW~yeU z&pDi@W~v>So7_|>g7tXr`b?2lTZ*~b2+?{EY8J-y6u;pvb`S8MTwY!tB*Z;)nlyZ& zP2(UzAO|ELaAyM3N34YgP=IDdqgKJ%($O*g`!_r1m91Y?qkf8kPfKI5G-6*heB6@B z@xoKg@~$QDV$n)!SMHt6Xez2ic21jDhf*Q#<;mx_(NCSGBE*^#e~s&O@%Bz(=sqQS z!xiR@kcC+isOOror#1)DGH0B^XlR&0w_Gj>ax=7Cx5v)tCHtUFO=QK#VoNeFn8>CX zs}sKt<%mTWU2a*6eFKJg)SufDc79py6lg+X@iP~1+;MOAw(1vM*7amn%fb3k+2LJN zo3tP&lJ0lN!!mqLp4VG1=0Xw0SYs`(WQ%A}C4;HW0VP&%h186gq1wT+O9u6-0qj>ci$OnNtr)`(d%u;hIHpFKZzL&+*4}vBo3JDAb-j;(=&{P-*3A zfA!HPO(~&-O<^}@O?Q8<{owL87UWAsTVqp$)Jx9X=F7#A$&_}nXo2CIjfm)oR!g7B zqm<5>roV1rPd!;kbVxh(1@@=$tmnghhoTG6pI$Uzw%^`9b_2}Wv$LPIi#eIwXU?jJ zZ!Gzwp947!*sy`t)*B#P_3BD^lo+6Hw2SKwm!z}DYnN0j0pv()$oEW_ozzwRQ-=4b zyTM#!|C~l89ZrSuZF1*}Wa$_1JEY(e1S(Z%cpG|!E&xdhX-z}vnB5957`Wlk%yzux2KQGq=@qC86P-fkRxAJ#i)#G$1YQhke$g!j!2~5R%-h`C+v@Y=W#E>I^y{T!#q8C`Nfi|^#)3x(|! z*Ts$HRRU3L40!MN+HZ_b)c%&}N5h@~Q3}G93G_WD`_H;MKLl@AXF(E!USk-t;#HIc zK(wGR;ScJsg)}){Jhj1qzrf5jY~sOu_WC)`43@rXaa;;p>jL89xx~fug$s0&WXHqr z_n105l2@!9puxJgh{1789EF^==AjVaH~IH(@kB3q%q=XeFSot7cXWg`a0ak;RXbeM zy%^f9VS@fqC2px1DTGuf742QuGGYZ2p4+M7Xs7!>VB}>v5Xd1BFKkMFQLE|Ka z?9PF&~R8hi6}z6D%aJ8W*T-yP$cuJi1PmBxrC&E!*NYBZKLBc@1k9u=5_}{;*o%Na_#S70z3|8>;{&hSVIE2Uu*K6zKzO2ONqMa#q@Vd*S8pb5PQW__lX`|F zQu~^>#fm?si8?sLxbKvi-`G%*OGry7f-e4DTPj`XVK)Vfa%vN9o^8(s-C85dFLfb? zVwtw4a9iZapV*7V{v(D(!@@NrYB=^<GoEJVb;|PhYu4mZBg~OO3F%(JyeXUu~N& zB`-E%I5aOJ@pcgcpMRA^3hDjk#Ap>Av)%706PY|LfEtzdb7hx+>!Q>Mmwe`P8-0(T~JJ>DxYN;{0 z`AB%+qS=x&YrC1d&De2O>Fc}Ux>ir*R?9Q8&hN*c*LVU3wvj<^ItA4aZ$f#$e`Jog zh%KbA*AGIk_-FsitqG=&j^w((-Hw<;%#GL8$bRC`&as3$KjavxFNeQooT;lYuEqH7 z%*DO%>_z*Fr5)GvpEu}>$;bW!SHOe4)Z)?PkLH$WX#sPrT-I+54goLd>Tv7_^3%J2;YUXH38gMjr`|SzHV01)SR{a60q#9xV`HJ~ z=K|U7*gK`R^D`jMMfD1Q?dzc&Nk1RJg=rLlmO3mM;a6T+K#YudV=Vu+eNTEVLV2Zv zuqXIq&Tc+bmLm97a_?_DVJA1zE?&G)}QoB5^4gr8rdD!^i zn&`0K>*DdrI4y?sK(>=(80YeTeu*o0558S_rFpoR&0|T@zatIOr}?nIw8#bU>_%9$ zxB;(MPq?tGb|HnzpwW^Jfut)1}hV@eDa9H>R z^80Uxl4+{&l2vb908#)+Lq$F(yov|iwknPcsh;L^`(VkPX@7UjJzyCDgEn>iZiZSEK&NKh1u=n*^SABCU7>j&X+Q zar0(I$_?>RD-iXMHvJVyq_%Z$JuPHA1mg79z&MgY!<+3U4Nv8hO1VP4zJjifJ&Oge zd)k<9uIPgB+ZI0Zyiks)+9sUchFLMt5_mO#Rz+(@mEVI1%@ zrC508mn+ysBe$1VN)JxSpMlFmqqpJD!t0dYnYLTEpSpeHFI!ol@jpc$wHe1mMBsPv z<{NTb`)ZPKks-Qy4M(1mDj*^^hu8m3_h=tFp#*seKJi9Y?~!Sp`f2=n^kv$0`Tc>> z-@kvgNP++=t@Cx{LBbm~>HD{WGj)E6Z(b=}npU{&ZK0oB2;Dt8cllYQibUjO`r`cA zmUqHwd=Z&D0Pd}z3+DO$ivPF1K5sG>mdEil{XinM+#mlMSa_4Dy|bOV<=@47i_v{f z8GIxlxTiGW{8TwEpTGImoO8G1CHsvuZ{!_z_jxwC<-_%HzDQ|VEq5YW8Jvgkwg}r> zo>xEakSi^$cjOTVC70asItnKL@_}Pt3xv#! z13T$N9lur`-Xe>-`cz#+q0xjL6sYxcFGAPeamYVG>slEaPbg31r*XLqC3d$+ z+XyeHWQWRo^;|eN`TXCYlQ*!jZ^|R=v@H=b_7GG0F-gtu=4*(0Q~H9(cNdI?G=Jgb z;|7=GGe0Q@8)New14F%W2j%^Zo}<30i!KJwi`%osWeW+OjwXwpg^}^nCZRnhUKpHx zl=;eIzp8D6Oa>005(GZOcVHy-Wkt!wXkV4sKiBp;EcdX2vz8r;i~dy{L>Ce^Em`j} zyp&A1m;muBDY=%{YGfR)gX}cD3Dy#-epGmF{zw@C*pkWtl5?hkmi=Xdl8qD*c_ZB$j1b!?~A=y_iUS9%b&Hr41D%`o$r`i8OFgw51CST=!OEhE4V4p>*a}_~#5Kd0o?*68KLbkOKJv{Q?Ur z5Q25JS{H?>=nq%{H|f&r@&fbURM3*23gWWdrbx2p3L*tQ)I|pCyD$2<61(gzyW)Ew zW21cj@f*G+1RMmDCUr8@KuQFqF5ei0Ur+?gYt_9n z-hIX-zJITZq+S=uRKx?fOR+-cTrb z#TsCF#BL^C*EU))ecku{Pz!{u#w*pQkolPv{>H`#+l~Y14m>yJ62@+Trq3AgYFI0;c>^9;M zUU4D(j$>OS<}2ba&nM|bZ@(v=OAdOGCc~_KBLSypzWziTuEOa~etu&%g0jh*JW7{T z!5?#3L*MAmRHz2=2A#c)hu}x70RED0X`Mb7kbS}#sQWw7zVQ&RXx{dnc1Jn(#5mUQ zLtomJlXuT!=m;hQKC}s1DrqTOqzU{FW@Eec`JX#96qLpkp_=|zu-1D4?GyG zX8$NQ_rF)hwi~H;quka+3s;+4(+lfT4EaN^TjHOT7i44@4T}&ZpIqlgwk9SnV#`^E z{nv2ZnyZ$>P7WO$CV{*Y`wB(07h#=$`%e^;Ib61B^0mKX%xKiZeMjq$D*@&Sh;Pjcck3^n+clh$uuG_=s^PJRMfa zCPaM56phA}e6W)FIbT5c4H3G5L~2%~sYB1K)O_m87n>3+Sq2$~@J_TOhJSF*tbO^n zE02d#vpm%u2($O27Ucz~GP}f)NQ+asj|UI(bJV9@8(Y}UJ24Z`e>wj)5D4qvFGs2j z65_-W_Xf}@A3aZjWK7i=E-%3^Ix!<29~dkyuvJg9Y4MbP372QE0Q5D_doJ%4snijr zA+7I~)<4}4dwf-Im{I>RPn8s(fjI6X_JiPbr?R@nO^KDO479X}(=OIGwjj*f%3+nP zub6P*aT5g<-J3dvC7FuK5|^)+!A z=d`6i?6A`)0ljbG(9n|nycC?yAs?SY1LNe0m4NeX)U~OF{`~e6G3|0~W5=87db_DF zB^2I21XzmI2$^SVDRV?}L6LM-`C+GD7#hvBzn3^?Tx%oZ3L=c8cupn6ia(o#8M12h zAA42h9k<;OKXzC7Qgc&R}B5qr5Dr9+o@e7P&6kSkZELV-UHMwe8D&(H^pECDZ@l6MX_ymZbO_;V)Ap zF$YH}MKGwt26Q^$08}GnZBh-XH#9%~DE9NezuL~nE*p@yVlyQH1vMI3i$1o`$Lb~7 zI;?4-HMWAk!8`$QC!#G8_?e9AFy;hs=2ptc2AN)}lFLWPzfxdDk`_U21x^4?PK#e- zNXb5fJmU;8{@zP(4a~%RXAO*l9M-FWV=7t0qA>Ogfyj(k%Q9MjokfVEn8cE^6XGEt zsR)DYu$73jsKb3aWRT04Dj<0R1}A}p*(W>iQ4(P3(d#U2oe}v6*p(QL5gCB&kbE<| z_PDrWg{*?{T?fvLQNmf+I8au1-So@JkcE7i8lWjzIsTM#r=`99Rz!gp307C2SOOco zq~EnC(NZ0SMJ}uNjt;>{?@#JrFsaBGw5Xx;dm05C@nNj!uwPy@PSiZ?+60HgJHK6w zt()1Es*in)@B;o@K|2|d_d5Z)&wOfB-4%bGA`6P}@GF-NmYTv!M$A>+?a&GPX(?Sg@*ZTH-ejz>R!ny@0|+_ zoqQ4@8HH@tUdaCj_)9p~FQm3X0ep7$y4a(9wk*QP55l7_<*RQ z05~Ipt8o2@VxggWudCAJZ_93TbfN8d8~*-`lwy1`9Y;{MZ2b`N*G#@Yq5f`WuXqVDSccN?o- z!a}I^8>2AVWs#<@=2dgmv&(kxw#UF%J{5dQEDdO?=mXS^hsl`z}sHtn>9uu4A`o{KHyQ*>&?=_j01*M zJr?5qS;i9;jXs`w+npL=d+<;7@QCs=@r87uANtHlB!BI5?#o9vlUUg4OVf$}f;=lz zbNk>&l{s0rj4r=TI3vY`YfxzTdSk}$4s3W=WrBoGlE;4P;v8#r?X-o**Y17y%<1HC z=8%!CdnuR?`m?l!?DrizuRI9$9n${T>&`B{t$trL`a?HJ5v~}^n{Cbg%*R8eW$o~_ z!QohRU*^M4<@NjqPu&`X4eZNO_yj4`B>_D&de?l1X7^$C&BNK1m$OD9bIWB_wM%33 z_Vt05`Hj)zHdIfT*_#XVSkvwWGzKrWA;YgX(e9LbG5Cv#cWy`OOKA9-ddg9HY^A43 zyyWaWSeZnr0mlxh`-gg>_E~~VRcc6(X=uuW$Fwp1BGEg;7YR^dBJLrHXw`VnSX*3H zJdhIGzA!X4p4v6y&0grKeu0BYPaK#^Cr?g4_Ewo3hwnuwpKK)I8ZrG?HU}e={LAnE z0BLmi$!kSq^hfS@!Gw>_ya7-98357#b9K}9$>IRBLw`RwlW%)w1(z(#!B=GBkxh!{ zX^JUC9ZZnHAt41@s6eZRTq04IeEg8IHZJ{lLjgl>y+o!ClfMM?oPn%aT8SK4_Xrap zDUiXujMMrQ<&_o?dLcJD*xY}{K7Mh!iS#&(h&I7)4}0Hk*ZPp3dmZ3q#C^zrAJ&}9 zmF#}SjyuW59&AzjYs};qQPZ2c<3&>rYQC5L(dnbtfqx=wJ+-Z^ZKTMtcJ3NTQy?uY zrn2!O5attWydCh@8XC|93(n zlr^OHeej6~;!B&)R?d`k{##nduzJk5J^fBH+*!e!vh`P1kfDamC0?hzR+XKQFmQH> zr008mOX4K}X}#K7fk!dXzf6qR&ZYd2{P&x~yW(t}KbJ^GnoB*xpiv}>Sx?nG(ed%8 z1_j45>_h%k{nR~t{m?EtbpeSqNT-vL;-`BlDJdh&ApB)GyQX%JCHm-?&qgwhcaX)m z7wa9UoTrK+>pAe0v=Yx;6CKN?<+*R9p!L5hIOX_2(SwDL2C6FtKI??Cgu(Q+7u4f4 zW^4itdc^MH>>LmO*SlH&%l>VZn{`5YJr$FXl~ldh!6;~-ZYHf=(3&+u*-QgHQ{_!T z-;c~^Klb;lrnvJ|?xBtnPlj=`M2g;G|8c}||Di)g`Y%s1_qMj-eahHA*=Vx{wPyx; zD+9ePA2;rezdQ$@+q0NS9A+M^chg1hPs|$?!1ZvSr=OhgS(C#^h^jicojg zRSi6$kGhYj;j@2SUwg>q8Nt51q8hb|8XXl97SVfUn;eS+;JRXxY5Zw(f*iPLVYoYx zG{R-u>?ROzsvFwmtlxi9?0%sE?k|)|)=zxKO=~Hg9bs1Q;ROh|%@5T&@n^aoe}CDR z*UZPV+1_g#rc^3|P0gCWXWG1N=<_znL%?hW*~{KKOw29i*DvSm#uMg@z7yR1VJDSvTahe}-~z8bgiQ0eY<^h$CjF$_ z0=0eLv?ZAS+^TcOLaEQw@q<4hNgjl+Xne?J)CK-w{w`fu= z7R)!K$+Pv1-v|(uVp7-qNN-DJiev_^1o!vYMlXPHBPl^?-D$bfp0(iAxEx3V_Bc3%6!J3Dim3gi3|z^t)iVA$7)Z z2GB^1_LYCDP-r{frQY*3Q(j}A#6jDBHz|doi#vs<;YuZ59$aaiG9G))6=uo2ofB3j zZWSC29%Jf&)q7_qB_;h|+>Y!h_b!%DB%z&sqT}Of+wWylF{56$?MfQ*bG1yMS5GGv zUyY|QviKqxe8&S{a}Iwl0Y>NDW^jyf@$pduRly)1-Q6wMyu^(f45W*8`dCdEsh)sn z8hHs9i(t_3%|2mWBW_fKaibEKlXNy#62tiu1bcZ3shjX~=6s^pMdh2umnaT{=#*hy zi%OL#E$>*RA{=rQUQe1$@`Ey2jij|FHDBfnZMho^$jrgeTsdAIa3{}psANbJl)*-`CSrjHdvuj<8<>ecJ4p1`8RGQ@6f z6Nhuui9=kG{7*U93%Orfe5`nr2#8LZZX5cPa^Y1N;dG2tM+WACV9ScDPplJ^!E4;Bx`k{gkgDH1?*!jL{ z_LDyo`+H=lhH9=A+b?8bAsYy0AoqmN6dxMt;Yg+o-=>~?l#A~v6DDoYCHdz;$5Vsl zEq{R_c0P+xvGU9RjU2S;o17z=IeF!f?@Gnb1e1*u@USiLjyN8ovMUcL2pp*H-kD3y z+b1znpWXlBs!vtOol}fgPy?-BDx5Nwsa9b<_pIe>TMpdcj79lg%khhDpK&f0C3*HT6*;;b@4iDX~IgBZ%JyCQ@2IMf;6q~tMe7Rvx<90QZ^bIlW2CaBve6g2QU`)%tmxb~;k(-L`Th^V z{BlX6H7(!qqvP>%B?ic;4SS|rwBfD*BTW_m_4#_`G|$0*F!t+FoOA-9-T71Qs?ybzqYP`A}exE!WL6uJktr z-dJFs6&4nL1)70SJDkzrIuj3B-Q1@ZRPW}k(P*m*hr^S++r}<0#naQ%!Pf%8>Xng^ zrJGxlMp5ngt$LyCJ}_hf=UYjMRdQpKf*yEf{qldenQ&+vNYDT`+ixnYPBT%pW78#7 zSb*ToHB|{@(q4GSRs7$<|LqZ1UTUJ@jWi&fOnp7VPHsQg$gk91Tvuu4K2Wf2x*j&X zQm_oqvYMOn_^(l`$Mt`yG~i*$lJx3Y?~m~U=r(XnIB$>P97a?T87WP?3I@ip%SkrYqO^9jhi0+en*)60RkKBfHX{HI}Mj&kRc$_%8V{@VT}>Imo$}h7j1u6y%g)MkgJ_MDaf{KVo9k0i-qH| z{SEuW_Sa$vmB(kD!X5l?*wdRBR}gvnLx)lh9*f;0^Fbn=zm?j>d4J+5cC2h0r&dGF zB|cPE3*>w5X=98t9T)brcW+Jl~=mj zfP4xTJqrXg4u%C{jLa4}y1iA{>rLBzWEeqq|N9 zhg6gHZQ&c~gw5XkgQcgwmsYnu((g@@SJF!xxews$Zj$TppAK=g-xin;$X8o*v%xXI zuB`(P_{PXb$sNVF|0P!s#isOT&aih3UR^GC_|sN5HKma;cluxV`;Va8(a#?}etc1R zch&*;Bb@*v`7b^~^>BbzKQ>E5CZ2zDPEZ}x3kdIw(Ff8E`M!|yy-YmvY^BpARp@l0 z2J%nZ_%;yM|FQD+_qhvgrK}y`i@VINzo>a^T6Y2K(iX{|sX_kf;5F|HEfTOQ6lmMZ z;20_pVnr%40*SlI;c4=5(4l@bP>XptR^a37&C*e|H`;Z z&6`}vWIsSwucV3|JP>_MTO&esBtv2LAMG&zoj$ifnGRqCw5`V8f&0V2)kljQVxFbD zp(AqBrfatu>iWL+1Dv&wuSjw(Rni~PYs+rCO0lBOYTO^|6 z{=J&q*HrgQN{F`;%Jn9BxK>vCT5vyZd(HhAJMuJ*;&3K@6%PR`+5b6@s+id}4E9|< z(Y*$4d=Jh+tRdE;$~ZRb6Ws!B6ov4+?dz zx2H({G>zK$$gCMx>ATG#P&P9{?3QUYY%q@9?{CPGYQijs)C##fd<8eMrKw!R9({q6 z=PuAwQ{XAFK zTh&p7#0JlztguUxRSL>1#)}b&9Ly!`>rMB_Q-rf6Gmnl?VlEP9^=!3MFN>2wWW7W@ z3aL~2rb#qVmiz({otkp!@9C6?e_6d3;~)>Td9!h}Q9MkTYTH33*T zW0_Sp?z2{s?X(3IJjz?4V48E%E|G19fP~Fd-2LKoF0$jUh#n+dBueolp5EV6RNVN% zP8FpX=A9R5S)vT>v9Lc`xjl>bKOe=qSN2tC-VK|>vDOLz`dDG5NfWB(~w0}};M0AD< z)!Is?>=JE6xTr{|u0w&_NEW4GPbURtP(ywE`@tgWZ zCEm*EjS1;LQK-U4p+j$x1wdcB=^xWdZLD^O%r-=US*UrgNZmt zzJD~CpZv3lgVRETgS5yk{yjjGS?sv`;NB9%Io#vqTwsjwH}L5QFZ(s^{;GZJMERuG zN*N>xIzMZY87P4So>uvf|G<3h-$BFOJ^0}23vh~A^0~)Dn4xAb=!RP;`b^A`cO2oz z|Iv?d__^X=$Y)Iz8~{kS@yD)t_a^6IbUUtsSD0WFPWQ@_VE+ityX-`j=WC5U7S1W6 zw=wy)36KlDq0JS5dhEHmdLx)#AG8WX8n1li1n6A7u$iMsy?+c33}BxFL3({(V>16Q zXSWO_>nPyJJ}5jfOmj*2>Vzx=eqNgA-v5ldVCA^}r6irJT?`8UXiqfnPBqWl$4zcG zO^#>$Z)OI-IM5$vZ+qsO4OjuS)__$m)``9eDgf1#QEo;argN?^yyWY7G6S247oxpb zb8dhx#|NZ;q1r#@ z{%!c(smW*6pyUuAp>~@xqm1&wKMo)pz$JyogT4Oi$HjjHaSB-EP-SA-Rs1w_nWBM|}yo+&LP#6j0yz69MMA_LUXv88j8 z;#9iu0y+VW{dEzXfZj*ZxsEe}QTpYIMJQm5lSXHV-21xTxe%1TOiPbtO7oux9<>@* zuKfoF{CGho1%@yf^^CQ6ecFO>Uh(izQ$MQqL#Lo3F)=b8alwF#g^=EAk-A;%OPf2r z|L8#xw~`V0Ow6Rv$70-?dZK#_OwkNJ3*BjBY3k3oL!@FfeY3K#Mg3%_<5|;*oif-Y ziPgweVL;O(R8E3@U3p!8~SppvXSJGTaayudFRYqBtAf@4C;M zDWO&fzE^6z8CyOF7wOEJR~) z*=^PyfA+i@3pen*tw^7EIYSX&yw|Kuk=MK4-P3=wXjtRLpYG3ux%~4xDkXP!R81gI#UWMG$)ieahPn+9qcM(cl882+A0;e zp2-CxJa3(D3!Qs8dFa(6PH;^reu}zmolwdUezs*VhzIw^9;d{9atgDBN48XXtjA`Z zOi;5mQN|Re;^;zOTKEm``4Ki6LvNhyx-r6;7<;>(Xj@M`| zly2W!6TKbTlWBieU4{3kI~Gha$+{K%j;fo;YroJ8S6IUd5NODg5t@1;s`pg#hQ#GT0Z8&?lEEFeE zkZEwxj2B4MgIG6QlJ{w9BTXkE0xxIuiR1~yzTV*Lx^s9Ode%_Fo(-AuP z1cdFNmQb@!uECr8)*p>p$ItrXA(L;v&aM0p^8pTvJYrVu8jI1mm9!#ln8Uu%qoy*R z{BZYknyhw1(Ix@~1k znXB8_ZDt^xus(hP{MC3i7+-C9-FA1NiU04Do$3dWvagmu4EU4cU%-<7h|6{HLkNr* z)Umg_LsujpdAk02(vQ1P8n71~SusSR|Fr_dRhhy7h~H0mPgt?dg5FBqHKjZfZ0!4Q)~6 zpVVN|8vPiyG=3(dzV8Bh=-2w;_GD zDRagk0UHpX^}L@Sq9zBzezpvr&SZxXG0z^g&`%Tpaby)NB}O9aWd;*@>oR6j=)++e z?}_e?C)Rfm@_86&5lcH=_bAH&9ZoRC?Gj{CQ)j?=;^$9stoax_;ayN>xq1ow4Id%r zZhJ3!Q)h#`aUT7DdTVLluJ_nX?*iTS@Q|mG$t%P3LN)-T0)wl68y#TV90{2>6xwFH ztPpPB`g|(*TpHB*a<%e~Jur4c@X2t==8rBuTdEBtg1!jj*M}s3shISV8y}&{{l7#n zCbO+OTTmnrsA4Dy;!)T-WralV33aZ0+fI`;>C@0a0QAZRwlPPryKm<6G+hS?*Ku0V z&%#ggF!YB1{dTwPBZ4+-vE&0+Sprw@SiYVi0-}FPCjc|^17e`# zzh}AYkGCVkA!KRuSdjs*jT+4fV* zBZqnvc&3edowqrAW*B`dHLx3V{W=ILIQ+pmCVh-+TlJ!@=<)UabETiSu{bga9D)HW zI@|F7Ea+KrWnjq&@_=ZF7EzkxAS;W1lAuD<$3@a!omHkQ4z?5k;*$(Bmr^J0yEtIj zMhU`j>}2V-1Ksp+eBcVnkb!<`wyzsW&Z_js?fR&?AQN|#Ao(^v`D1WJ5dOq3py{TxIilq3kFtO5 z3Kcexdw~Nl1cq4}99V68&d%2!SXZ-?4oTxPhxzh*I^agMhOa6CL(u|<0q0GKTvns?I@3|7JCR;b%$R z=pO;cxKZglf2ljyE4aprt!D^N`#Hu!!v(>aPiNKe@dXODLIX7**`FIg43tKDvY+jf8~<~$*{Bsw8i zS0m$-Wee9nHu(7z1*zk_Wnj7^4t6c@**Ybat!io-Ofm5r1nIA0o!?1rergW&7K24Y zf6y;O!%#CP#?F_8WTL)kYqQHSabN3>COfOEHAkPWT*-*P>S718t{uoUOnwYMtXuM* zIfP}q;vIqn?m`b8jv0Rlc09BQ`bEG_|Jbv1>))qtNo#6s z8U+@DP286dgzRfJ1v!=5^okQ-VSEk_9hCy>8c>N_>)R~+B*CvabfL~MvmHp0kF-cS zM?TFq>{&+){qiREIDq^94Y9ar_4Ma#er$CRBR=$o<--{#I9)x&4nv?2XRQw zm*k4^tFNYLKRqoZQluOmAqu|Nz96HAse*tuebySVnJo;~2@`04K0$e3%&a_8hgpkP zF~!LFN2(7u!y6V2uf%UBOqfcOPjUOS?k`omp0~}N=%oEx6{srqU^75QJSp(vP>!OB zN^YD1qR9?pGARQcwavHJ{bdLV-({=pm8|`AOTYI+URNs*XlwR|os%pLzmsbaCGh1X z68Ad+I4(g*w>3Hr+0!*@!Jm)XNy!?m*ag}%K7C4|+SOQo#zuLjcN-MkAx_%wHL!>& zC7^jWd#04{1%-8sNSOO(RQ3;Z4oPsC|NcImC8l61>U18xU3lN-Nr)q@XK0}t@2JDn z&fSxp)4RY5#~hvO0F+1d6A!=l$qE-m8DIUA=Okfm(_#X?ceE+)z$Y249cCZ#w)|xO zYfMm#QM-bnLRyib^Z zR_p&aUE+Cwug18w&ximlL=vKIyJ2ez~%H_EBA&cgT>`#?J53QcNvfXRdCN(?9n7vW;kuX>? z_L!M*%b8i^Fe=rlL%hviO`?GQI4=T>lY}T@bqjcj$tc!Q6Z1VG*BvV1u973Qt zDKfuIrpH!wO%b6o4F{={L91HN3Z+^^(aiJislM-c0fxGAD#Y6bdGrd=Y8A3g+Vnz* z)%K;gd{lMH22vbMQHj`2`8H@qWLxOVg4yM{^>oMf9BuBCnU33}e_a{$ryQTo` ziML;W)oBpbXo?jL6bNC3uZk85V6jvUSPX-B1B{dGAC9$Dp=jxa9%G4@LP6}vJk9$j zEMAH92_`uWH7ddholboKfXYuJtej@9f>7WtXJm?`hL%biRFx^~1-Q_S)1>ZjZf zUqnSsWW;5cSC2(sB@!WOICAsyJ&jXFplYgRgQog%Ai;fKkiQuD^uU&STk~667)*pR zHYUREich}#zMDY?$Qc9I@}RDyphK5a94mrvpbPmW`Q-KgERY6v}45MIo;av zEX2ZXD|p1VQHhA61CU|eac=oy`93~B0x9e9jCK?YMRNss_&~$`xGo>L zELeFid~WA{7pFisozwocZQSY=?07kP-zr$PMV-3r4vspXJtSuCrP4X96@x{wXv$f)4_34^_-vNJ^%g4Rwg)cU+dD*v;sk?lTe=>%|(UaQGU;?M&V5~9p_xF+7uBYq1F5-s+oA`Zkqlu&in zPW#*AMkF1#=2kyEUpb$>R8c4U6R%uAC@Y70NFdkE`%M_TsZOKd^-GSq#A7yoHhj66 zNx^5RI^_cG+2o)K7L8t46-I~Me9n&!0erZRzinLQ?dTuNIwuC+^1DSvmcs^qCq^*$ z%7&{k2U(>m3zB?(BhIz}$>fg@`uRt`!ffobwje7h!4w;&(G2*ZBCebZ%t_)+pbTZu4Q!;u{lesSA{o(Xlq(HA!2vZ!jc1hsic zHs4^ro3^{Q=Ddd4{QOqFVNQ1+FM-cI*CvWeO3% z`fHdSzQkkiX*Vd+TfgM~P2w!xkYtE1##F`mX!|Q)`+h^YX&PkPLU70{N?U_5wc+vj+ zYv}3VmC6ofDT6?@XCeKwT*^}WWqyQ3!C`nS4BzJOo#0&=XRR%J!kM-a)I?yt^D zvfXjZ_9$)@TE-Lpv0*l5>EK{fwV{e&eeToAT=!wd-AOjNgNdaSlaZ7z z+1#Vr;DtX&7dH{{gwPCQfYEw%pWxCc@K3Ei&Mbno4F2#K@kHIGF>9@?RCL#X;D*3B zgFciF@0l83F4AqTOj(!Xes61-?Jt7N)V#utFP0N%XI8Ed!f;j!hOS5Tt>gQ_gNoCT zbUs34-$YQ6^dS|R==Kglgl2~ z?_?fLMWZ<;DC-av_2%oq=JDd)>Osb{y8%Tp3xA1GyJ?8S?q{s^7gN9cGThxRjme-i zVbPI_uX6kC=q=jtI4l!<`d+X_RbVQqE3}#a3`G1n`2y! zTfoc7KPu4%0anBOq}f5Nzc0OdG34V9JhG%|LFS4 zpf7~DI78eG$?a7B{#{X*cuSrP&Ka%*;0Pj+^8&lK>A zVOyITKWj}W_1F=~pfnwG5AHE^$CWgr8VE^aXm&5W;`Qe$#9g~{T&fUUsL})_k!%9D z+RvP40p3L~`9p%bUjrNvfNVkp*Yag!KU^j~ z47DOdZ$@{509P=8Is%}~fOM^YJtG&yW%Fa~#OtIN*DY{KVN~cG#S=2s&QEo+4I{$1 zI7C+i;R@s!J$A=P@BEt$zKFXJS7$dH3XIiP^*o%bK4@-GKA z=YiVG_A&7g$i%`pGqjai4jY`;gYN$AEuMu991BbR-|qse827k8n^I#lu8ai=9x24# zhiD=nF7gl;hD4D1ob|#l`)?GK*d%E2(gpXk3!2o=o?)i{(#9&Zm1*m$NXbqIl!cF} z%D34EjM$v%DmB38jh47^;vU!cNMs4(LU-R@TEANg-sd{RW2CnR})rxevI5$|tmh^CH(R(|#f((rh@Mex? zC`|DDvF{Z7*@?OORh-Vx3Zv0aU>&!s;0N~~NSY1_3H?)Q9J#p4O;cY3E4LA_6X%$! zs9aT^k^=@bfj8{VV+c=4Y^@C|M+0*FR86q0+s6G{nq4z21<5{&h zd@dnE5O1o$M)a+fz{`H#GN=#SoL6Enmdds6&hT@sk09*I^@m@Q97PBvR5#1>$JyD3 zdO$T)S6-#Blm13jXO`o0$~pupcYm^$MT7h5ED?y!{3@DcGQ-%~s!>-9s;S?stRrsV zXH(StaTgVGFa034Y7v?&@SFl9Zf!rdHYny?pX03O@_ve=T!+N0wX)UhejX`)+j=-C z;R1>`B{FOKN|DsWz3-qw->4iQvxEdO9dApobk5AB8a-jf}dr71PNCq4mj@b8Fc zpOVK--ZrqswftN1ct4?xBh{CG?YfnFfF+OOK}Kmlz%LrCixC%EKS+rnhs3-M;j1xy zt)dOzV$*0tbVr<*RIO6oc&=i^qytD*O^*=WIRVg-LXjY}m-Lm^Mz&bU!;C28@FBdS zRx7Wki8myBXoa>s4j)677~lH6-PmT(JxCR=JoR!=)^_6is+z$@T&8nJF?e!M>qcG5 zLa3|nO49;Z-`%l=UI#a;>_Z}O{qz8W!r0etzqV>_ZY;&Ci z`?w-|0HAuI7qDUIxk(gBEEElCro}86cpJO6p3%)_eG&+mngU6sc7ZQpJn^KfW||Ys zZw4VcRWNJ7`U+R=OR%PTkhu*7`^u{(_pKO~z6ke5&T&{Zb6?~!W4*EE3A`|9>D;?8 zi~KcCMhOGcc-h{LGxEwS3)`*)X6;r1?4EJ;V<-tN0rE(ke|zWM)|3~h0t2oP3E$)Q zgr(wr%o;t#c}!EL#75NEUD5#JXWj7OW0T=6wDz)3=0VSv#Lv+R)n}ZTQyB(b5A{W<%dutjx>weqOke{+v1pChTv`llv2IQ;H z73p!g?+@a(_H@9wdlxqiFHTG?;#4T*)D{0=@y2P+bb3a^?dN|58xjhy(3HA~n3uJp6_k3HV z=F7@lj!L=1yCJf4EJvaGTCbat+}rb+=eH|{lnb*{zt@--H|f}1(zhF}bw(X+`NgJG z3l_UI4nhKc)Q2|1-NLljHd_?u$YlRAIkne(AeF8a7?og4M zD5}Xk!c&H!P>7Xy@{f?=-}Y{3zMI=|XV~ABTyaPCS>BKHiV7j@v(aabSscUm0Dv>Q zU~2y`E`_{5VSZR?rMx{D5vYEaG<0Yg%S$PkFs7fTYF?Tw$ZCk?d|g_(_VbWm?ovxTsYx+OceCCp!K9L2VYp53Q&NkD@~Wy1uQ1b zZO_|sM5ValaT{H1q>E~WkI}dVOovJE_C;y?MQP>=r3TKbOu%v=+tjdh;k#+UN(Y++f~drBgxT@U+ixp z{9&-+eh(AkPdODSh3nbl+M?X#ZxsWk$C~M*hrUm?0C3Z7Wrzj!*_)RklioJx<@m&dx8b z`!iOM33K(p2OY7ZRNeGOiV9q-%A+SL9M8ze<$W_IhcPREJ5C7ru zPT)zu{Ky!OldesxZ)%60W4Bn~@UWk;EXPl2kUeo!;L6|LzafMhKAn`GefC{8LAF-0 zIj1-;j;$hOcs9#Z(~|rXPikAJI-?Ak<6dyW@HNK#44^|AZP57xGa_JPU@TNwU3$;> zvQ5Q1JcG{c>o;H|k^B6W>%I}O^K+!56{Qp~xf|A8^%>-4UHmb$sZw9_Zq{^bPBWsL zGZ%&WG?r5acwHkXWRt&pltuGSj6OE7%4k0G!Sx-cGHt4|Wtgl`YNut}^!Ao)=aQ+6 zE`Q;h5^NXYG%x77ZwKNW%o4HXEcATc$d+LAiBz=t|w0wn61+(va*>MS753D8);3L}m#1gcCTOq2<&giLzs^q;Z;e4w{3LhHYrt#w)jn*?BbB*g^} zv_eVG^um_`pM6meF8(SY1lHziubo2qg$GBB-3OgV*S3*HWNcmUR<5z^_YnKo4#FL6 zeRN-5>x{o)f2L4g$+}5gQH&`Yfr7S~tA}k*IQo#!n8eRN(Ie9y-mNiE2gcwrzs{I! zgadV5LTV1DZlWT z#zl%|sDHcqlHp)rzDm}5qXSl^3Zk_R$B?PP{MAplFY>zk{sR!h4anaWd9S_?Dk^BL zl0tmS2>n0)!eLA#oj@h+2#8~&T=K?~MG{qJVa`svNfXg8*2s{V@!ioAB4tSGiQ3j3 zVe#++H7=$q;-YkY;zq3bPez|mYKpF-mfI697dv?^WWpWs&v7d%jB3eh3^f|r@{QQL zXjN=1_`HL~79Zt4d z-u%J^u0S&F9cFFEWJL{LBhZ>C7?TMMHFFlIjjaQmm#j7~+bd?vxLm-;tsv2H$sHzh zGx+?**M&IreR%Z`{q!sP&9f_F??CEJpF%A)^TI2kE@yGBVHPlnpcygmTgn8*a4Ti9 zNPx@=WV7}DOY#N4>-Wq{E2)J^S^YJ79h!!iuh|CDfXOE4hceM>M#axmCN(J+lI;CV ztM{(G!!hD+UwR2%z7|p+7Gjo*zk-gf@)wNoPhE5c>-Lv9d+E$jqQ2R}UNMp5YbErp zhv73P&Noo=weyaRC-*_M zM6U0-sUA+B*t6gS$`vYqs$=o#FdzLLLmrU&Q>1Kh*W={qQ55nnq4ar@gFqmF1JUXz zS0y*lV=^U@uKCCCHwH|uA01wZ-i{zErTyjW5zv>6gfbk@%!it8ZL}u$gQ#ex0&e^8 zB(Cm*D4>wbgRJj35qV!mv2d9Z-;f8zHvB85N}AW`(;FK9>$@l8i4@pMTvmCvmx;J7(h?G_B=UFhDhJ%vq%TS zVcPkwNc%lG;x{GlG`&?$X~Z7o8zfzqLaFZV4uR(|)FMc1W(xaXgoO5XcVwD3uIpM) z@3zGc99I}j|M}Eeb9PZOAdKP&nUD_x70j-|rQrnv&6`}+X!^~YxGT8Yzd`C%(Xvx_Q?8U?_eULERZx zIl|~lLW?8r0g`^(`zskDTU4%7<`T4HYtgt)w4%`qLxWH3w9`P%^emKpzd*)W0De!O+mrHY1Ai?$!L0J{x3F>rOB@F;t2;YU;p7* zkI%kNLzcuV3KXi_u)CJ;+L^(=>YV8T)jJa7O^xOQ0Y0N9I%txq>YgbP?0);9K_%1}hr6R&M~?7XZc z))Gm~?Nd3iyJT1ClEutZ#pNp%qZrzF@^%$T01^%Z9%60Wovg$ng_kIo^8|Y2E(*p6?34bFO*(dD-neMf1sB6rbZX87b_BRV`KLnO1*5i^EWWzAnl zso#-hvyD#l{SO4c3Z!s!A%R}hqizseF-b}cHi24G1SpyB}zyqC%oYYdZS4Q zl|&y{n$<%zK_32Ro*?1%sS<{ylFjn4ZG~I%ceEw(B&7m;u0|5m(N{K+k~hje^_-SD zT90s*F7MA;I54ld(;od1BkCF$eBnOYx0d|TiO`1sT%ORkFM&)wNV)`&55yhQm&x@WOP}nK);u1Tro8&-4%~|pd z#}(A6VO!-R-`WahM=oPc)@nVcu1kR_d-|+U&-=zgXqP5>$IIIM;_6iNg#FGQn&Wnk z9h4Y2aBX=#V-a=-2f`+S6h#R6_7@B{oup-0Ac%QNV&#O}yw&D4bdlGfRyW0b>Eu!&Zs*kA)p>X*SaiK{>xj3I7@p z5%5qE_=5S%Lp(zWLEMEDlIAfETLA4-$c}(wxop)tf4b?3+a|~Rhw3)xp6Wq>fue7d zBFWP&)SzyCaxDXhPq!l(+nb4=TMERW=XMn&q!I$WD<&xB+CCk^S&9gbvQ%aA zVkfMRval^rJVgSfvFIpBEFv+6vVsR2M^|n0h($8~t*Qd?8wra^a(RiWn+54~3D<3G z2}(L_Z#sg0_?+@r+f64=%~22j&F@Yt+HzX@M_puo5{g7Bxoj!leW=)|xHoHvj<*>% zt;~K?{Jm_5u=?PlnAY)@Jwe1EoB-&G2=EU)ttX)ppTGb092_yU~!iZVu z1b6eJqgYb=Din}ot5D{K^dd$Itf#C``7}x9Q8+eS@qruRR}>C&DW9HT;#IUPAa0v)dbggUK z%Usc&IovuCZH3e47CyUyxUAnu?fX*B_yAQ6$Ze8U%N?4Hfq67hBwbKY&_Y)L(H$T( zc#t5&Cd0sQGh*`N zpcje1Jth(&T%l>9(U*0;$80;3_Cs8 z=9_JBYzifCj9V3XObE3D8uhLcRY8P=NXj2^j1<;Xm_BbU2ZL~hwEOYrXjV-UZ=f-b zi_ziX0`-`lhcoG55aC=Gu7`0~4ELuhN|q;xzsp)-typ>y5KMS-smF^wl5ThbGF!9; z0xZNF*Wx9;HSpiKv+$>A&C>xz{W>3TPEUDL_pS;E&`$XIyAMzH7wnUVoNx($yol7x z%VVBZ$38~Pj4$KWl&3dZt{3xyGoj=tkx-jTNAS zN2C<2M{j1tlFLm91VYppQe0DsP9Wx_pvntw!74uFObXo3Lx?}3`;^rq{V zz6g{A>@Ibx+<~8ceSLrX08Vn>TykZL--_jj0@H-M4ZTtFfUO2rxWZK!*J0yc;fTO* zPv$hbd`%YqFyv#Jrg30O6^nVisfnD;?tu^I&fmXNx63Nr5l5X)Jv#exlLxxn+N$o- zd=MHF6-QjSh=udwQm%dcX@gak*%3)*^yAgRl44QMX1vYf?^zE_{ zvJKI{Yi+d~3grBjc|D1?TslcT7qCV5F%@Mf-sl#a)@id66Xe98-dvY{m=LjCAHeHdK{;RMS-rP+plbpuo}oNBZPRR(X3pt? zSO4v&tK2PL*CyMbH`q8C4}H}A@$=esJe1(Gm2x$y=VYviE4x@?Lcv#U6Oetm@j#=C zgvF}JO4B>C{7~gmBKJ8gQ54F*@hy9+YLy;cF02UuGBkcoQN*=G%@6JXM68cN8 zU`HJZa8^*Gry;LMckTi=dTuWXf8>szhV*1DIMbkcj~a$NKy^7?TgO$~9v~^<5F`nW zZfCwd0`8eD(WGvF9ax~$ZVI-udsa@&!omWm&;T~*^74mVVYd?M*RLm0XURGxD6wqy zNI)SDP@YuEQw_~X1|qW_f(w$Z`9uf`VgFq_-;5n( zLO%VKtr5fk!v6J-iBhJ^<~%aUgRR5%*?0i#*S9&X8JVvUCu_if&6G$7$_Wu@0P7&QDv zYBu?C4g`+T=L-=i(q~#_Wp%{pQOI!o7EI9jxjq*d+g&Tri4uClL^Q8TB8CW0tjkR! zYh%sU7fLQ~Yu z-DM1XXq?l%FW@Bja-OiCmsJ4!ZsXYk$zmM(;vn}#XxJaw#kq)NF;rMma1npN?%m@B zC-yq#}>-Uul4-#CjdYC)%hnrct&4QUGjjxpT1&|B??B6p2`-Kp3bp&I+W*U6! zMPG7gZ;}-mG1jxjS&r++eM#A>6l3diRM0B*@& z25~3;!8_SKU4l&_Ry{R_<>;)}FTuaWPg(dwdb1+^{J&h4q%P$JfGjHQWb&Kj(sm|h z@^{4NdwagthfE!7CBQY1Nf8P{vW;zA;o9L_9hXb^Ch>pk6oLaRkN0b_kqcZ$8!{hj zn*}P$2?icld0UsS-8@=qi(-y)p0W=0;2(4AVLgePe|E4m)X<(vpTYOeL0*R;ULcs8 z_EgDMJ@XFdr*JDGIEV!*?1#0`Q&4Kqg8z8cf$++ZJs8e#`h2~X;Kt;Wo2%XBre;As z?g^E%J*yqgTl_cF5i(SA%LyqmNvh*(OFnTH$T9SOzJvu8ahnIHT)9JuyA=pMJ77hDci@%H>|F2CtF>lkL9e-TzKAI zn}4)ed~-4&^KonnJNx@1SvmH6)=A9AG=6^u-kH_a4+MMh4{#r@UtJq~6rb^`sr_qI zp-3giA!z+#tszSIKv%tep$7xGqxT|SU0Xg0>uC6y=W*I|h#{=D*GqDW)< zH2^#;&(DKG16qe>TCo-u#u^MX(i#P=%Lx=Q%!|m9qN3aSj3C+&bPUs1#0a> zew-HMwcbHOww*1+a{YSn0`$W=9gzAf?8Bk2>^22ANa$nU+^ni2%`7+aj&SQ&`2vru95QsnwRRRoctY=T7%hzg0B#j$y5~#huNLZ7fO$249{`*&ya+#+$ukAUbCft>W$(oudJPtcj{&;cr-MeOY zcMG=ovDc()I5Q0T0S0%ZGaf_64Cg}DJ%5|px^p)N4Zd_QVg=0@{x#)J9U}|ZmUml6 zpgQqIozo=__I_mEIJzEyfkKAgyC&RG0%m0BGR}ksl-D!`VqbLo{U}fs2Z_sZ-NZL0 zj=rrYduA3Zg9)$dTsGNA)x9-%&WZQOKWL)T-@6XgA5>fp)JnAaHBp&zd)b5~FY|@^aC$sc>P}mojD^3-M?+2nd^dKUA2+#o zC)VrM5jL?EcW(Bou4K}g!nhdOTPO+oh0hdMF(BU<=@?L}hGo1u%Sp zUnD&*r;W7^_ERT2g=OKQ%Cn|o6@{4aS<}H*uV#!oeKIZXcGT|sXj z!@9;un?sbv(N=SYvGoq{tjk>O%(_=xmtF0DCNnuMXnSL&l0*=B3mcO^o6UVwKK?WljY-)0? zO|}L?vt3?M7We14VfXtK7KbvTQwq;CJgazxg|J%;)En8T&4iUVu+oM7+V3|=Zx8(Z zF7LPv+jyy;qlkPJso{+p-m%h2Vf)a_#-7qxOwh-DeIttvOFN^_M;;N zW-twE_lOGWy^fWB2#uWoE zLiR!+cJp6%4BkF3nC9*mh|j$Lg$JG+%M}Lh{+M*%IO2d_*kIJplbY!Vq0US{0XZY% z{4=TZjd;?QyS%5eXSQ@FoPaqj4W3&_8#*%Np^GY01?|HrAyv5ac%;W4cC$=xqRJLs z5>4x_o$}e`sjY626Mi_>Oq|D)BYlcJimE9eUq7|I8dN%;6llIQOMji8A_xESqxGK3 z!dnJn&`@1!wd=QLb6dYY+fqW^;iv191hc?l%621QCey!%KH8aHTh`Wq71)~Q#~RIJR48p?caq=B zFxWWt>#W`oxc*nCPM5%E>>FWS^=n!+CtqKob~{bzi2$AUd(XR&7*Z@*KFcW{tZF^O zuQl~$Y_{~+R63DbT%0`KGWg0}Z&#u)ctyXxFZ-(R+xpTJiC^}epwx;6evzW8dGIsy zaBN4ibNerR#U=AXs{MUqZyf64m~RUGY{V)upxRnHkY;p7v=EH$#zk*Q+b?ON5)SGAbRP)OrCv@|d`uDP&g>ajB6)Fa(2jHAuKJ}hJVWOP{ zfUtq?1dn@jCEyRoTL5k(k}wp^gNi_bjt^Hxb=7F~CI;=hr$Pm_S#Vs-r!FI)_txHxY{ zc|Mjkk-VKC5kMZTBju9JtD#4G=|%cbzo;@7*ydN*plaj0+}KxjENNb*F!y!-T|ajd zhhJ3LSRmWe&aAx8}(v>#YWAD-lOvqHfv^br!@KGKCtI8Y+aidrTm%9Y) zhl(wwDBaz`htSfGNQs~O%@b=MR5c-42%b~hMj$S2#um6p=YLo!ikp{WaxK93?ZC&d zb*?0N`i4nnPVchVRv?i)fVZ?5{%y+6sGdk@x9v_!n>a-i^=FtlB3_f~O{5KMNMo)@ z+MWEnSbQ$a(=o9W@v}6|;-U2O4%BD-ioY*;CTYb-M@bs0JNCKlR!OAWDwD7S)sjfzz7msE?tM4-1{H6*SLG=HYGUl=YrMk>lSlA1fT6 zu9UsDnDmn!H>OUc34`z4Jt(3lLh^i{+eSOGd&|+jim{ncqsqEV(W+i5x?ffr551(U zg=o1eF<@&XkCA188$V>N4aB5*Qy~uGMMFJD(`fF;fv@4634+p;&vh^O*q$JM0~`nb zL=OOMr9$i`R?57?MhI0Dc2^jdPEMwwX${X@{rp2Y`raqg*&`WV8 z>D!E^KFIeVT=V~cjCT-*&T&oRJ#E?Sp4a8&i=?p2ms(-BZ}OXT!%tyzmK2|8Rl(un zt~y!(eEi<0Qu-=pjpjl2?jBD-%h=yF+|JQ&HAaa>L3krIA*q9jSh>>I(KS*3CYyF2|g$uDkz1gSoQ(&d0?=;6W6X?KbEw3ucXx0BUow zuq8`6kgY2_67{8b-C7FcX4%VfZk(HJ^>KJbsUt=Xm^NIohHz`ul4_DhW$x}nT414) z)~-ab&epf3ts0`=e#D6ovVa&HACbguaR5bNR)oh1KIUu_zXo-CBah(4oJdSxNfYUqtYDGDIfY@?bgmL{d=; z2L%2t3Y{*NJ|YX?wXs-Z&HBi&gTn;isE#&2N+CF}B}CVDR5DBXqtYUkI!RZa z#Tv2SzU8A*!&sBgL^N?}6;6waz1MNW-o}-^#V?zDVU5H6-%K2NJpJ|OEb#osQRP^l zlD$OeiQ$33)?YOljIwhPW18#b( zS2@hd9-c0dYti?#%Qd2_JKAYbEx3r-{%?oQ(@0(ht(EJ?!tY?%TgUz6ix(m;rvaXS zmM7Bq6SX?Oci04e#`mLQu;Y6&gJX?RD&GR0*{EDvSxFO1D#$xNErYI9sdW?ao~ON_ z_Vk4RQ_%8&z8xHu^_4<=3aFYZg{c7GvH?QCMqRvv37}E_RlWHO5Z#+Ivs1jLGgCp} z^nso^x7jT5m_qTofx2P?oHorW1{jghOx<6>&KTf9yll3M5x1$v}j;%t}Opniv; zeYRU>b-(_XJAj*Vo$eiEwmEM#%j}kUC6wmex2=6Ai9ro%^(03Z}V0!%P6 z?294hMJcJzJlm0te7aeUdorY*VNR#n+rJez?XNjQ@hzZiYb zDhiRt-obsG1DMrRjHKyiMzKgKjU}5)t6J}2#2$-Si(u{G$OV4>pb3X8o%s9j9anNW z;$ScQ_=SE}N6Nx)n4kQUC(uACXc@76o#*qI!c3*<`C{m*-r18Flp?kw9RT~fkxjmS z`aMtzijzr0lXP`)!zE1};f=8zK`0&X(XAp`ntVW8HiajG@|UOzq7t%R;Q5@v#*gb9 z$Pgjy@eMaG`GBX4AA9>W#MrD5EL=37R1l#KB$YUEUmLZlS81cJ5{@%To5IetYy?Kq4);^_v z$%Dxdamd2o8l|c3Mh3vcKz%m{2d^G4Ggb%+B-2OKQ94-fmR-4zhSAg!8tR|V{@hNj z{?+-d63p4Hu;Y^@y|ph=x|Pph7*oxPPOn}#sJAw6)zXH}*!6IdlrwR+AnXDbZ`B8% zHEAM!I}_GKxJNIi!X5xsE*b&Ra%}x~Tk^O#F!OAu`sX4_L&FeWWW3%lt5Miv-llmi88@JjAmcMdgZI* zJ0e(Gqy|!L#XnXgkylXAk^gGX7$e16AhD{I;Wgk_=&~E%Mclxr&H9c|L${xAf(84P zjm0GOpoA38)iLI7w?Koc4H*{GlW=A5$NmQTJX0;Wp+FD-$%P!y{G=U98Pf-_YO=`t zJzz>@0aw!{apArW1}&}UB_&jU?Srykcz+8n2$sbJ3_!KzS@RcXLQz+~UFXzvOGZy&pMg-@a-wYMB|}pnC2=p~?o3rVu;J zH&TGa6~F*EM*aoi?05q}AYkAM1bmx&U)R;txT7~UbgGE83Z*uH$X^WvD@mMf6UbXK zpxNmQh`}}Gk#JD36kNen2>I`j7t9)dd7IaB;hpb9DiXrIE_ib`Gjd1e;(73A=Ozt5 zuPYKTvrxGC(lykWu!IvAFA1JA`D!~+-$1$IuTAJSlOa-Pz1%O*!JC94P}0aRE*$u# zmgW1!2nzsSZs=T-IYxziUwzgd1+lC;%6Tp{kSQtbIcKQ1eE4O#ei@%fD+<+(U$l*M z=zfL%#wB`Cc4SK~t~>Z$=klZN`Xzi_4NR{#*BuQ>_`u?$;FZNx7N5;UCy+~OxOrb5`D@m|4?Oork8U#)GAE?UqE1aCB~Y} zpt6qUTTes8nuxcU%b=@>BRA}K2C!j5I13?X(R~$)Gh@Z0gdN!osT4Nc{b&XW3+A}l zgjaVd>ospgoqsvb{7QQkTkhj|^|+1{K?$%5X&s*!@|`d)%8X8cetcfbDoxa=F$As- zm}B(V0LMI(4A@46C`D4i!mCFd>&Mw8yy^swb~WD5iXlWHN>YiGE7!-iKiji>r8g^i zJ7w8j+9;9 zHqFOLS$q)|-LhHxx_i_x5>gOs%}2R=q6tjSt0ayeLk>By*DP00^YbNdcwun)0%4Eq z3iB*)-~WHLQ9K_N3z^>+szd2iPyL;pE!JaS5y1s00_DX2O9<+}-Ynk0&|6Ws}w)U*4b8Yl-i8A#_*!;121AT<2^%&@% z;{dq6f^L0ob3c#w8V@D*aWrKt18_RuHu!iiGNAngK)V1_gi=x<_x1P=5lxfy)P+Tk z+0e$PT(MlI@0-(~AR(mWivag^uesRDR{Gb}ELOCk{)GHwkfBX>Kqc7F+#Zur2b|d@ zMAc4wF?bVP`?Zu_KLjGzZdiDZ8rQM@VS=m~)&2BJ-4OIP<7y?r$ey zG_k?~&rX1{8h&VKy@PTgqwJw+}TCQR8wG@j{c)?FI9=8Jpb+1&9o|HoRv(I=rzqM&8tT{i_}+p$acC zt`UI;J2(M(#hO2GS}1LO)oiy<>W}bcG9Nc%)SpS!7HF<#wcukWGV5 zgHgS4x+g`&mvHf94{L;DPGTL(Atw=^qM}NBC>h6HeZ2*ubc+MU zZEj5qHfNE5G|}^0DMLJmL-`sssjVJxGoPqQ$TD&6%nCco6SV1b_vb9cWVCfwVMop@ zh2tMNt|va=p)&oXT%zfUJA(~y}J z>hbyBe}wP(v$QhZb?g4DhS zj25rUuB^GC3Ht)M)ocdCB<=B!9NLN%@l{t@-zqSiutj}#qOd>?d%AIt0C;rn8^_^i zFXx5Qvbui#z+)VDP^LPsT7P^l^z6Afo2zq0TT21BIj2PD+mIcHz!+`(QyDS_8@x1v z>Vf3>P7;f~t-r|eZ`~>Xq^_JfWx5!?Vj5y5PhTWl6C506)f$h^JO>>t47uCzk4?Xi z%!t*HvH>R4s$}x2eO{8me#*n%vvmN}p>f`d*fSgX)?ENRNPF1C=!*;)<W+u-vRILWNj9L&C+bXfbj*pl zwH`7%uj=K(Ire^_A`8LG`7xa$98%Y9^t}OgOo1jJJ~Nn zJf=3JleF~ff%6mUq0YZ>KQKiy479ml>FEC)dGWs_H^49iaG+jWvjI}bNS0(C`e^Qt zI4ayu<_w?Bw>UN%1O6+;jua8HRy(I=|K?$$;r=(87NQ;ogY{Vn;)^tzk^hY|_6CfL zA_5tC3fA#hwzj>t)TJ$^cf`l))9Ihys?1t{?fZ0Zraq4INQ>^Ww~{t-7X2b+3@lp;{BCh3@9zad4!m z>WYX|!a14!dNQx#f)UQufJR*XIf4jcA7k>|X<^R7susK!_2{fZt^SaXAya2?!&3Jo zE|=urOD>MO+q4efFHs2B0k-<;>G3;w5;@dS4T#co-?nasOHcUbrf<&T7{@=n?&AHP z^HvqHPg(r-X>jBdQaLZso9o~P2>PrRyVaL(&uy!%xpg?-1Z9dGPaE0s4;{5+B*%ru zV?gxyLVA{(E-l@_RL_Ok7dqMpYwy@|tC=fE zepCAAHB;qjCv{1#Pm=@(^no9YV<~4&_7*EW(bHHrGio&kIJQ)i+iiJ5*Ovz$rn;c} zEM=E5A2tHdaNX$VY6NsNB&x%j1c#pT0SFi-fJX z2JInU8K@b)bw1wGi7a4F6OU7qo=_Yn@v_~lf1c|wq%r+&k#1>dN4$A>{f}(6XiIcJ z)K0(^#HTW*SrDTO7I3}Zb=4J@*CR~Yf2wi9g4CxeA7stoShzHe0!1BqfznU3-+#~< zFOzyg7w_f$UotDx$DPN4TN_06-q*4e<(kr!h)h4z^P5%O-&S9=Gzx{SW-$&UFBa?K z_O3_xoWdG@{7d9Zbj$$?+#@jX`V+!1m-B~!25>vx@uuL7Srp{{yQjCR5QF#I|KHrs zb4q1&-_~J8eKbteCYD=PBN7wxgQ&>wg}n#%ca|$uTHq215GnI9QO7&Ox`uf7@pG=X zMZ{0a9o{Htv6Z~EwJ&L^Ce|ny$<7;DIC6lcYLP$cZ|L_j3wHZZn*e$<;5;p0pei;{ zqgbA2bz_+s)O(X7eV=4;g1ig(%#DZbxrtx$`YIi6pkY%DemCw!>?KxR9pLMB(D+cz zeRbU`d^JGIWHCyY9@2ocoWi;#i_Oy+WhFt;x90DiwPCwGvRh~!ca~l+}=kU|8 zX{ylWd|&g2&lEYoTy3$mLUZq)guH+MxgM88NYzGtqK`0Snw__kk~vrZv#Pdz`4^j; zL9XY>(1F&l+fG$Oiy$ zL>wB3{7uLAbyuS1tAIAY`q`}iIU`l3*14%&-wx0N`-Pr81N;W0dT&d9oHhUVi+@4; zv+VoP^RJdpQ)R|~sxow)UF*Eomb%yHZvPKeXBie%`^9SmknZk=p}RwxAtj_crMpW~ zy1PbNq(izvx>1nslFlKAvwi>Pd^p$THwWG9>)Fpdd#!cfziqLX>ql&UzBk3|vf-jy zF<1)=yjo+#X&n-+$)?*62>d}VUz}jx8RD=_;_enpbrrPpJOr2Nl3bKOmAwkOW$Lqw zIumu>-4hGl55U#IP+%7Th;4SZGRV*GG5ZkN>nx2aebv0BZys2A;pVfv+o`%Yq^TW+LHF2R;Wp2d1Jev7*RWGo zVd3_w&podAkkWy+f%!=}w}(JCC`00_W^5|WRtuOEf;IFuHF^qZhJy@>5xGrMcmPsw zGA?My)=7g&@0I@+66I3{2gDpb-|6fux7hk43ypKtOqCTb9C6eREun!*I#h{h7eGoy=s#HFfa+ z`XLobICJ-0Qt*2b@H4cmf5Ra;Yg5+HNue(Ud2YH^V^*^C7svp8a8=q6AXSn~)z2)8 zuod3Q1kTBW_>DYT*u3koAMSL(#T?=cKBMmEY{H`DwB(h*4W^Lq)1j z{OD+}`%8f{TuGh|qogC)@<6(dI?@8$Z+?^-nRoU#F)-5oyIxjsKc&HdODoJFSaK7d zGmtWhBeDEVs`fX>D%UfuEOw~Uyn2=%wYgW?(K6Be zC@1`KNh(q~E{{4NP!U0-RR}RH0cSAd@@XwDF*P)2Z9QZcW?zkDis|YDNjj8De-TR} z8tXq)n$}diI+Tsxu*Sr4noNuL_7tT4=0M96NQMiRWFiwy>u1saX0O%iQqc%DVE zDaouKxxb26#!_E$%gf!3e2q@s=j+~=Yo}G@AAZ;5$@(cQ8YE^MB5&lIGjk{tkm6^0 zQ5OTg6kFKuKd*)yo`w35RevYH8b7)&Kkxuf%P$7Yn^K(XdjvloEw4I3>Iwj7*=d9^ zWc@enEhq{D!f_+*8YAULo-Z zH4h$+OmY$fQCo5W&h*(eVQwC_`_wrCjWlTzSVFLjE?&oQ13o&50chAdV@}(79r8~) zw>xb0(4=7hQlJNhQOZ%aqF^+tT%<$S4{Uj!DQer9UzdB5&9$zgC7EcE4dJN1I=>wm z{?|`bn=+mLwDtR_<%BQn_3y&T35T3eJHE7#V9gB#>;>SL_#Ei|mBv!hkn-^D5`PrTV_ljRAf*CUJJU+?>`-qX$nF57O`2NE_Ijwm>w) zb(zRfI`?S?{0iO~OPjd=8u4G=M_LqYTlDS`bRyPVg8tX(g?`RPWr%Iv{OO}KVf&Wx zH7{il(v`muDM;n7&qvsTM9#p1vMTAqRZJ&qA9hGGre^q8Mz{#|8j+#gMpg9M43%{{ z2;NBaUv-7z*d%rApUC2oE*+UkKt~)1D9e0EkwSmDBp?oYj-Q|7&J9+?1&4i*FFBVA z2U@ak=VEM{zK8^zXO_Ld&Vb&pD8TZzy~SYH9C%L%(AEG<_m9y8xP+qqi)RV}K(guL z`NDuOfo@9JXR)Wvf|pOR-2(oL;ZHn-7T@)lKNs|Z68DI4_~TK!Kza^78#8QXz~>M= z*k!@tDkv(peTX6MdcBIllW-(JgAv}t0hf)SR(vip@<$uMhE$ve4pSINpKsO$@0Y#Y z(N&2O49q~LvqN{LxdMKMBLLFwcq0H@Y*BZ>&>{xL0pv$y->WfP3w5DvdPK>neFmEY zIG)KRQt`W}{xjI^NMSJ|^ zCct)6He}@Ms%$RK!es9|3s~g;Rhv6V8lzgp4f(b|T6Jgp_^9R-Yu`*?&=&Xk6i}|= z@+Q&b`xQ82*;;)wK?$5DVjT;IeBRG!lKY2lauoh1c6I%;vonJMu!yzc#y3%q^u zn*f^b$r1etYM^ptsFKh3iIo4aDS}5>Y-r-MEI+{gN+~M+tT&H_1h2*Ss*ecHYIBY*rTS)77x_KZJj-T6$c7! zeA{%HjV6l4b*ZERpd##dSI)SzC$WOUhpqqse5eM{z3Z>HzHq6i!``BreaqpR_seU22LyZ;n#X)l*nAwxvAFTgQz?_egk$72G;y0eB$2`D-^2vITDQG#r~fTS79Z7) z>U)ui7oq(Q&M3V6;)RXEm4-|9aVWwK>;1XG;0Ik-!F-ZHM@s-TVH(%ZaXyG=MDqs%Zn=aqrOMT&3$Ywc81YNv3WM-sFd z%Dpk!_)LLZNN8X@LlF)p3#AQm!gZUExjX!`4>KLs)!v9ekf6mLs)vMO9-!m2`l?*E zt|19J$TT8MlOgH71$*Icz2nTM5O>3ceu?rAU?^V25OkCk5lRD#2FDX10uGNBtY-=N zW^r(TqNn(l&FVJLm@~0AFNtPUdIHC8CILXXY!q96g!v0XE)92UZPe_t0U3$tzxpe^un*%&4FP-7gdnA%MrQF?&%ZvaEf8>A} zOTfv=3>Y!P+*nhad&2Rjr*;bKtrI577F;kH#uY z`uz3B%cjD8I@xo?Xy+aP=|C7}OZ6Dg6#h@y3@kz*qn`a~xcp@kBj|3wOcgTg*bFY4 z+i?{P!vwOMiQ^CegV5AI;1)h;i_fg2xjInkfLySKrK6hNXNi^ zZJs9JAqsA%aQl-?+Jq|9k6X|s9? zY&j(v&5%q{>%ssqU5ImNBJ&GFGy_`be2>X7Gmn`)3Q&vwi8+)avsyT?xOA*Q{+lF$Zv%|I7P3%vQD_TB)3hH)2-i@Wh+K_!~BxB?^|L+Zp~Fw90E zepAPFDcaka^s3QxV}vCO(7|dQzzwM7{=$+VrK=0}E6VI|NQA`G$vK1u+tKiZrqb12 z?-*sx;|y-xM;DN|t)Sj*f(J~OY1N6;o1)mZq}kw;at&S>b-&ikC3qVo_&9%`fx23_ zeMAl?EiD&!+ZtcGGqc*@46+}6+P`{;xVr6TUGumas_`jTvj7_5#xrD$g4B}6Y# zi&ClF$FDThY?dsBUwS#vgDD^ziQksr%i(BB_zvg~+omZ``uJ4B+;lmfg3#b7lj_6O zp)-wNGWQTmSp2&~QXk&~wVb+2gx}x4YKyar27PDoWbt|KTyHK_WGs2HGf(B+)Yb4mySmMmz@Ud=2qjSK zxxKv=5D@S_y#}mYKx-Jt6!~sWpFnmyo;$Yw{MRT( zMXYmw=sFkhn+SZoaW(E~ToZh!`wbVM&cE6ymYuz`T8wZS?C2F(W#CC#8Iq(y#4B=% zhuDF-C0 z8?rLv?(k!yn1dVMR1%nlskG6!)ewP53JBbjGokA*9z^LZrRoq%YnRSU*z`{*4XzNE zidB*40~SE3W2kI2Lg)))^Vo`rJhv*z9tY~A%LgQmTatlo3y8z%*Z730E-gzWY$%Bs zlZ3-X-Y%^sErO&!?PUhIRmDYIT7z&m!|1f;ood_Kz%^%=cQ)7!I!eCVl&P_~nOdrs z#cV!H9E5j1PX*@xfbKU>0sd`Z*($Hek^Nh?V~^>E1_#$a ztXYUT!4xPOK5VWJd-RvaxH?C!EZ)Z*@B?-N0aO2vB){nkk!}bW2e6%y2O$%BaEWItsO#*y&n14l3s zAIXZ}g?d%4&d%>k)C*P}HEI^H#ODsja=%{rSm|p1;}+BXEB{+<*NBfq&(Iu*V7WHp zFD1b#E8wlKivoVk(?0+o0A{T`jHr01L4GrL3o$lO8gvp)Io4pDz1lfT zy0{ZiaP!BZ&V zD%T{QN;te0MTq}GA?+Qg7NyF0ufnn(|&plfjIq%56F5O5qMZH9X*9+Ou6fk_+i zol9f40arS0eT>kOve-6ZG`^`jpr*)i-B9ex%algklsAY%yP7{VeSy>`ow3QdRY~IV zNAs5-0H@PAXt5L>;!80Uu3j9Vy)_W;Ly?qK8PH|O0Nwp02Vp zZg+p+6VMcaiK?!hZ$xv1nE<=4=MidFCr^q3z`wnWnF2B9gGShaTaB!Orw6DVELz_s z0jxF!TwpUo9dZTT^3N9M%L#(5$mBM^7+!Q8`Dq(iimqiGZ#6hSbXlZ{ZsUo4@ru2( z&+8%ppx9o9(lN_>QjfspjyS&D=CrBp`unvo8Oy1&oYj0GpQ$l!KYKCbOBO$|Iex@9 zOlin~y&c%sfN027M}iVue#e!Ky@{MD^9N)qfIJ0qRRLHg!c?TfCPZtmg4^LB2J30KOvfawLvKTH(XnuDl1l`F{zL1=6Na9 zT}=}fUrIz+D#MgE*yNxj)z8N}3yO&S`JoUqah$u_lPi-LPNDXspe9JkG}=y0U9DUU91GOD1=~l7q{(c#xG@fJZWD@<1TB^0;pI!6P ziReammwNiFYbZw7$!2D>FeP)=JmTMOm@39|b=l#V&2Fx?c;!>Z0+I)g0ggKUsVs2r&BB(yRE4 zIhNIx;S1Mo^#b#4HMa7Fxt5*{jns>>PjyHP6+h!*hEbgkn~MB3NBlCOFrH_a(!QXv z(w4uw=a|1*Mfx*0EldOvUg8^KA0koQNH*ItGnQ9P&Psd!-kxK6XddY@2@x{UTTqJ5 z51>rpOPGwz*ORo7V0atSXglSbTFkuI8s|qGX;({6xvr-=7EmwL>@_$L3<97ttHCXteOJm z&o|(7*n9m80ho$kCX;NB0m z)cX{HCJCs!xR0BMvjMbl<1`Kz!0uW+M5lTk^Cy0w0)l=Pk3JoS=pN|cH(=&s-_2Rs z*?M8?)-gM0;p*7Y@>@D998l~-l9V829*hqaK2?{GTU?*LcV+Ki?Ih**agtsf>YKz|1qDiY#F+ShIDBPGeEovU(Ppx^=B6d(ALsw@wuWe*R=h=jbUxdfp(V%wk0T=Ku!E1 z46=P4P=2g^ebFGtK6!6jy3Y=LVX=?#brNzKRyvriOf+Sb+6?P##T(|TVcGFy8u!&#Ix zWx_K^f-y`e^$!FUs|8x66BDZ<&FJ4(y4nX%9fMtszFNowj>k{Hi;*wxF&2L_?fRP1 z#jZs9GJo_+duLx>yPdt${;>7?pJM&Nip~3&2m!YDSkP*eKo{nTRT4qRbw=m?c_zO{ zUablndqYfFfPs!3ihakBI`_6~iTnQdDk=2Vb)`#p^EQw77FtnRQ(6}ndGV)c@%G>r z0TcS?yRX07++L18(dh|Zi`R#})4Y$j7^47ruPz&V2JG2vt81^MJX*W(FMbD4WIXCR z8c(_HYsCqBE&N#psSW@2c9d^FnmLUlN_e!Gcntuf#-~Ipo%wsWM!~|(+O2NZ;j4Cn zmx9h@5Ub^qWfGilsqcbc^q0?XB(iIq_U^Z=qS~9a{w&wj)#cNfOB}qrju6YsmmAD@FfB1FD*Wu}1q@D0G;pwTyL63@lVfqCv`nm(T`kWA zG(e_~-U3!AvAaZ%Zini6wlf7!Dme8)?|jwv)si_*F`}{>%+`^Fu#Bcrqu!%E@tU>$ z2LF_B*|JTtbd_}(mK$KB{I@VWRC3s|1vsZHFO^yu&kD^6PV}#XU{_CFz-cSfaJWjn zytcDbSqx&$LFnx3tMH%IL=@wULq9Urb(3f7zZ%_TU}iQnBdC(D0}NXb-LLN`?hkpl zp1Rm!#^o*`?$xKb%dYGCGgGLGEoCpt9K7*3{HG)n>5oz932&Y&6_Q}$Na;66huY_u zW&g)7%TK2Z!2G}rI7dpdCT`?2fHRg9s}0i@kSUaq9`SqrxxYmTd(Z_+C07xZ`S@Ukb!F zDE7-=Yt=2E=f#=>-7P?0i{#eM=nX6KcFRJCbckT?1x4ay@Gs zxd9{=lav{J-+n6`AD_AIYa5>riLdWqbTiepZ(+x)A`PNxDPr;SDf8LU-^a?bS!+~k z>9*HR0lIgoGn*|%&aH^tSP}f{>B6)*a8bG`m=+PJt*SEEf-n2jZKQ9XdA@gm$ z;?He8R&!1A=Gz|0`7hB}NeZD_;N&s+sa9nQY#Eg596o2b?i02UG%N=>BA>t`#(9B;mX?D@-%zxenEv zd`?>d7!Pl$R>Xv3$7}l!Hd|xWz|m2GL9E55F55(J>;nZg7MAJr=eDV>Jo4AOBNs2( zh^+vpB#62Mh{o-kmifl1EBrO{V`eEOBH)Cz;z7_Z`B0Ah^9Jn}YPZs5+MtG3z9TX! z2xhAa4!IBj9%V*EP$&#m58R->`S^aU zy1u@3a&?)`@$A#DOLChrx}4~dVmOy#!Z>qT^C*%mX<4MsTCk-osLl?MRjS@9xyUjX=hL+~MilDMuG>T=!VQU`sbQN6CpkwSdQilMVXZY4% zV!y|aw_eJ#vJkZwRKqyHs^D(8fxt<_FnH0cK_vfMUVDCVrLs1N_2*ifSp{~T7Jlj_ zLbiOLkwE@Hj1sll`>{%mYKPh;y~5mu=^ey8suF)Cs*Lfln>SJ0pp_o#uaW8c1x+7! zCne%1^upM_WM<1zy#?uG%7V$ZG5zg}KMt}~HMDgNo{+#ho&XYCGh|&bRYel`@G~%` zmd}p3`vU2FG4P~aoNeeG62;}=tNoDw|<7h6IV zaLdq)j$8 z10C}q>nOnD(a@c(UW*{mSEhxqfHhFwhk>>|Og(GK$fB;Rzk8cUPeIXIsVU{oMXS8^ z)ZGr{Za&M!h>B{y5mM@iSzOS*km^j1A{fB1hqftobS{|AW*k1$5Leh>oPNC78_(jl zo>bla_~yLv6Dqr`jQG{cEKdnw9|iVGAZZz#Y>j8_C1gFBE9kZ+LW~$}1bYQ}$k%_D zvVinjJm_OXT4`p>UJBo8)^-OHDhF4DVwH`Zve|fpCO-cJWAdA~8wz%MdN{~N^6&chku26n9MeGddCmTuU^~6PZ+y_A1U%O7WDAE8%Q{C>a43ak zp(`e6P(q=Gzc9cP{Bj82e?-m{2l7UknvusXSBI;!!u=#$U?reJXaGHg7+~ibaCIDW z6U!1-F%rQ2s1LUUhj2LIFt+t4&VS6991(<#&~;8G(kg@SXn!a7;Ac%QQ&?`D467bO zrIs=rs#VzzjGCgJUM;8#a8aqeNmT)BNMYJOz2`*6_RYhpnzLI{ojS~GJn;07)#9;B zPK$`!mLCi@6VQqckokvGqPB`TaSC;P4Vt04F0!X?EgWdV)hh|BGb05>1eidz3UrBYVZlI-hh=5N|`bPJ!sYBuRLKBS)_0{gy-7y1Q)U+!Yh z+v4PlH3uw9>%m#Pv$sI zo(nWkt5lr58)m&XaZ&K+Mr`p`Dw8Y}!-F&sWHt0qdXc{h`;NG>h`6f#yR*DQCBQpB z%Crf_QsY}V`T&rccN7q@fYF~n;eiR+@NjtK@Hd@C3ICGcQh(Y0@KIS8bOYA4gvC^A zS6yzG3)_I$tNd3I`IeJoF9kVW9WhUQ1MgI>_y~MDxlB}l=W(?DICwqeQ@sZF?M-<3 za>5~OeDAu`TGw74%(ovf=>BYHey0xV`jsvA0YBtBCi_a4lJqK1%}coYQ>c0_Nhl4R z2UTs^1Gi04aMQeh5$^h@iL`mz&$w^oBF|#g%uq81&rd|k&h#hz{ktLi=Bb^7U?sv> zj*-md9CcPOshpT%owkm)hmRg}=_ed^K_@ zM%<@{ZUL*?*Q5J|d6;y>QxSy>&i28W*kP2)=7lq#zb=^l=_*}5y}ENB2;Ke+qb$%# z)l|@h`;MD6UAdKM@rU8`2ceg~fx)%6JJ;5ekc5py0c9O)t%X(dd;~2jQUsaoD)YlC zdS`DYjKhjU0jv?Xk4&n#(@!%kM0XMJWb{R?{b%j+{-y1B^N?7^Nv5r?I=jAn(ylz; z8>QsrSlO^u{36eoNso*&YjbkQm+J^*bGW9w8^t|>GTL~gA*m1;Z_z1lKMbJ#_U@Q0 zI21qxEDFE?Fn;T+LU2){^Dpz2HY?}V(gkuCfAZ;523&)0`0!0>%JU3xU(uLQquv=7 z8s)gujQ#u_DOdJ7;{>+^br-lvh?P%aOjVI@DT1c0ezjT&9L)Sh@PD=#FG|7u{1g7& z!bD+W3CE|j!VoIc8s%vR&eL0gcGAlm7bUw5b;fxY(KBH|f9tWGE1^yiT`!!5BkPPH z<+akT4x}|pzctNui0aP#1cU4HTP?@oL&ejQ*w4%xoX&ZEG(T6yvO5pwTR$z2i8frV zj4{1ARazb4>|62kOKgt%tiA?$lDrz%@b^$zudEK-vn0YPyaCxf5TyEk=9CI!q?A64 z=#q~GtNjh(c_uCC8puRf$G;4ilD!)+z9w#*Z1hSo?t3QFCG{5!8gT3$kcQB_%dwqreUn%$SOgU z`FKq;sOdCpWZ%S>x*6~Q13Yq*gPVuPPar_tCR=Rm@KNdd#9iqB@__gR1b|}2z{rR? zEHkF--{a)=RMv=Tn92fr8x`qd$`?BA48z;IfJeUYA6&tKkjO~9|jJ>mE7 zSgITEH=|&DetGSmx879wI;{hk%4Ek1aLOKb*oue7Zc?OfiI;$a%5Z7Eg6I?_H})3p zz|T>g7>#@43`j3vP}lG5LQLQZq$7xCc&@wDFO$=|f~0K$e~HmZj|WUzu*l!{v65Ux zt*n9e%>B97J*d$nku|?ZN>b#-gM2`{g*}r_v1q!;k&LlOPoEhP2~lZOYN+GcX$HFn zJ>c@6pQZ?3Ggg5MBWY0)LqDHV?p$+i?QofqG8*WAxa5*hN9iHNE{W`mq$I?7!0HSf z*q)1g$AIufs#Fd2u%c=ml{px4S)m)QD)ZT{YTS^-CnQvwt7j;f2oWmVbRHGlfFBvmn4 z!PM0fmk>_RrjUdB`YpA1F+Zx0JO0H_bBLDe=ARICDx3g@?Mh7r+z4sY_n6_Ra-10_ zVR8&qQsR~cwrXr(7AlTWi*mu_Fb-5;Y^E0=Z&d`lVp~5kAbvd%;^@}+NG1X3{vCAd-Dx@~mtu6E)pv4;da(`Dhg8fPrFrF0FsC^awB z#hU|NA;6Z&7%dIG3uuVA^hJTAOpIUn4AX0{7s^XrO5Bvv|PK z)s2fnkUdTrTPP>$AShH#p&1hA(hiyaJ7nMqY&XCk-sifQL^}JSPR>!U^o%OV?#D;( zWpBu-^4gcjs)ZBi(Y#-hjVj%UXdMSsptLhLUvSTDYN|B zrYYMShSm&jzgS)R6Q_D^9M@jSBgCqj=?6G-As=yW5?81V6I!UZmT|dU@gdS+Z0V9n z4ZkiQy>vNB>O(fqGjbLC)kT|6W#EBLR+seCuc?#A^ZOvYv-W@$lkG>_eGE%aC9%ef ztRFs0w>IMDOJ^-3Wj=G3w=L`1j%H1hF?0l?Em|_U!d%I_Q$UJZy8+y6h z8NsQpsnONd-JHx52JGJ|Dk{LkA%!T>Y4oy46JYK0yXoJl%!dSXYeM=#MIl@KPub+rLO#7(F*UuU)&y@yHPgsN}e?xs&8SZdsJ7)XP13Y!F;cVh+kr%sBl|s zp`l<)9pjqQ#hNAe&?P)Rba#{G&z{OP;G%#@UknbxMW8|*e4lOGH^70;yO5VEXN{!F zRv>yEIZHjN^Qp)r*$-=)ctn3HjYdY(GnNx%u!gH-)^vM9{!LKXT&Vy)3esr04cM1BC0zenk#!Hy807c zoY;(B{7_!^!jjw)L>`)td=e>T63&pYAdJOTWyk3fhO zMw6(tfnt;A5BmF5wh-gGM5EdMV5-i64^#4gIy~C5&#DZ%ZOyxnpJG$>UVLZ{ZfAZ@Ez4W+-T>0~T+Sj%hu zO=l*$If!bx^_)jRe2uyk;m(P!ub0<)>zfkmefzm>r)UCLj~;0P2nZ_H!Dk$s9#b#S zi_+VxrpvBn{!OX6tD#G(gal565?mm$Xh;`42UqpGVWmYk-ymF_eQ)BULV>2%St(cN ziRF#eky5&~vC~rEoL}Bl)z_=_dZQXC(jh=vhmVhsDG8*4A3`7HL4ra;TL%Y-7ZkAm zAdZ*a?k9q-r%Oe#hXWeeo)auWuWB&px#lJ9W6 zb8T2UZ`eo5r~u7h1qWd)sc31*19cJ#!3t>G(;VllvMx!f$g(ZPE}vD}coe8&Jc5sQ z0#Xv`IZ%0Nj6XLFj6OIp{eX{$hN;V>VH`p{w(z5o@5%iqK*xN*V9)S3NGu z#jEAmZy9(u$=>PelEamTnL4o{Z;FaT@IR{K;oLh!-zLg_)L>RLP^4sijnGHfMkC>L zoER_?uHR+^evpkKmem>PeU;)~PJr|GBcm_E6vFak@>#5uR0aFt1AL`qij`YdxG&jQ zeZC=o+4Ixk?&1xORrh+s7>-dB%wf~vY&~H&pw900K`;(8u@nh-7q96K1X5Y?iQ-^F zFEIhs^<29$-&PcnuDTcUR{FE^;Z3unGPtsgwD*k^cnNgNL`%T|Y0Vc`+U#dz17pEq3iH!oJmKv8gdWZBaI+B4b3krN)K7UdV>-#TnDsqvFm-=^OJ^9@!H@O%4;%G~M@8M>%Wj%cEb+_QNnH)*yZDTv`naH@cwhT<@H+&5ynJBCXS~XPva0;J4Yt$$)%q%P{Cb6c!&bY{pXl_0iW^4_!2vNtQ zncAkP4}$m@wecBoqZvyG(I4T8_?igSZ`F}i@H?|hL(~|fG+#H@$~%}QnUyPH!Zj{u z0H1(oE;Qh2Ngug%9z+nYpRNzlwpFj|y=&u2{F&W-*1XP9jZJ|C)dtxVF3_yVGRxH=$v5CNh&EIWqSlRe zm{^$dfeFzbMIB9zX}UB_ij16$!VvPyFCHa~B*8Nod zq2l)k8-;d{aPf}PgF8)RgsIn|RFS5nh#|%A>*~j6FPU~-iPoWqo@|jJ=+);SAzh0f z^O(%wLW9~0?-H5sKj@nrxJl6uylQ5^kPx!%R#0`Cp>2xh1_N7E`9>NsPj<|&?|24K zAC)OxilF&*NI+gd(A;HMMpaObH^IZ5xi`gec4weU4UXhj&Fq*ij4rwByjU5}?Yy*O z)uc1)B@!R9n&Z-E)fX+L+be@>ye$+1XzKydgD5)cZMRD0t2 zrJ=#Cx>_jAG=vjhJ;XBW$tE(&0q2q4Ym``WTuqmG`~W~bf4Rt7k&(=h*a z&b#o1;O^|S7}jzXy@T6^QjC^n1oNy=+Qsq`wS zPowevEduzSf>(*|XOEU2&YH{;vvjMYN2x2*r|IL$xj3#=6TkzCpy051hnhglM}V z%5J?SttJ{EeSzw5OF^CV7zbg2{1`gJ#w0=(k*QL(vG}=4L0m%;OE@*;C3%*yUKdi* zAsIb*`{g2acb!V%N|rXBBXpSsNy@~&z9z26lGI}2kby+T;3FL#JGKhtgtUl$$)6=U z=gy=A%PtoklJtYf1SJl|?%&cK(n;9H&@yZ}aTNRhBz`~U-u}ZkTMv#KNI)&0O+`sA zfzQ~DkZV1lEEzD_Zj*@)r>=h7vQVH9;(tcE53y0_U6_Wot+)py~8!PQf1=eY7*Qc#@T*(IqnA3{(zYO#9d zpsh__lq8iMZM|UVBGpsHNe`6B?k>#P*x7A(h>y?C02TI{4v=PXGuL4+y58XMUyB5R zi*t8k=Q_`FDGeq@e`4G$w%+hE0y7#uWTrb(cQsbva~hrjo<7I6@k=H@r|&Ohj&}i> zj{J*4Q-_a^R|N^}m{SzYIk7(*-6~S4qb1)2$rGb75>VeRz*A79-yPJB!mQFt!r$bl zAeERDpaO1cLeyHKx+RX4RaWPhuj8wo*mA;_eA>~Pqv(?I*bZ0>A&k-BLMnJ+6YHPn z7pB1p=+P@{tPOQ~g<3AP4#AdS`{iCjKuF6-x-r739}|&)9^>!XJ}UR_<83oD%dZZl z&p%~NlS;E0Q8^A1eL1(B=k|*FsiEi!q$iqx3_zA|8d8h(u**Vt)M#BIr_J#zK+R1C zr>6uhof>;46@8;-uct<&x6JCKtBi&|_5QpE^`FAW@J{5dazUYE|E{E6t+;hV&6%~a z?!%xgQH&<2&^dAcUI`Vs#hu+j)2> z)bR8aueDR#)T=?V=yXdE9}$I$EEPLe&b;ue>f+U+VMA-P@z$I^o|w>gSKyy1(H-!p z>QLoVKq{P;E0fCJjSOY|^7troN|7q~hpnN>pm@X5M!2{U80`Aa1M`;0hJyS!g)8uZ zdw+jdJ6P4pbTlS|T#Y9AwXW74V|h{xBc*1iGC|z1RJ>+xeRJ{b3aGjM+n;?19-o9d zoIH`J>dO#}n9I&8PqsWnQ$cH*&)^^2@P}Uv0`)~piy!$bvzC(nVssI#bMFM9dEaXD0Z76P`P~auSm+hfRj};jD z7Zw%U(Y7N;?A0yjAqZIKTpMx;$};9*KAm)g$nGRm-J1@3XYN`VR?4od z;rqLP*qC%o>a1dVTt>U~%x;Z*XPP>F^5y!S6e*{fpWtp$K!)EV9ePtV>B)SHpIwyj zJ4JgI0v9qFCrszxm%w1H_k3jDZdd|_{plHj;0lvVOGD2|za{!DzK)@5IWE=5h)qU% zL|M%aPC1h7sdy1FAx0O!n$f&|8OL=!QzyUR@bAjmrPl~pCzFXhKhD=}VtGMLa zl^w}|hH*oW>s)Ug9*y{JTT01a9mY7|%I}Z0l7QQFO8ToMsBz$=$kMa zzKd6PU*5Bj7kWm^kM7|?h2Z2eAl7N^Lkc6xf=9lgaGCf}!>;ABb(RtIyYCtHneU77 zV7u7@?cXEIXFPH6|5iy^WEcDPm-!&kE|NQjz{UgSu<@dx?wsVKg&l{3e)gYVERI+x z>QBT8oP3q6ivmh;i)|#Emp7j}WNNJ6dR=sM_qmA4Ad(0SW@0sUhC96AknUsX%sa5y zVT>(J7#l;MuiFl9gx58iuDEp9Ml<-6Tv0&HmmWDjS@!Ec>zgpmURMpJD$rR9{VpmKc%S}P!37T9b4lujja27v-voVT2=Gkj^=dL~(h_B& z%TaF~9-Snnc-cgM*;`-RHGa9mb96v4K%h0vs3)oWVOH0ETCbCm8XdAEnHBde>iB%B z0b8UvyBZR&boN^QP~Vo>qWtq}Q7?GHGDi<+z3T9gpxvIWYoX-g$oh*X&}u#?MDmfl#$9D;uR9V;aqbxSy&0 ztY6~ja%@KmwLtYvt%D~CPFG)oJTroBAC*tf{CJ|pgicMesKz{gJ`@%k;p2*eOVrIQ z%|jm{tGnw@HvzD{n&$gevUdxOpNz$2{=W7*LT~m`tYjDb#l&~xo0iwfD)xBd0NXVt zyBHE@adjfVM}8~4RI&wUUn9?#!J7S1qH3$Qqhok&h6?&}4tn`pb9%1bZEO*wOf}2L z*E*IJAxOpgdi9=X<{&vwuLrIh~=ZXr#4*$8o1MbM(0~?UFS$= z!r8@8&;{1Z1H{qAY2oLr)Wa&iI>w?d-^!xrqLGX4x3*vS05X5#{MhjLbOB5?oxyCw z6FgCA$(HiOOmoKral11XN$UPv1pih8BxfJ?MH=xP)#!b3s-lh-87M~nkXjmkOs}t= z&sEJsov*7|{(j33#Gg2iFsnCIKcD3KeE-m)<4QKBC#7e1f9p|fFg8B|9{Fo=qg7W4 z^dx|vl^FdPHvAWCd&APBAQ#$?*h=?C2`|-cUXdBpf zHc&wh=g(SSn?UgBEag}xc*d6UtxAyH47IqN8vU)NNTG?0W;8G|LYTKZulz&W7cV+a zCYyx$*kNJOpzd3S+4t~F-^CA?^_9zNg^}jUW-PH9p-Vz(CA{<6N5_?d_B?oTKm_gA zZ~up+vyO_Y>%zDq-OWgM4oH^>(v2{5NJ{6>jnWO$4U*E`(lOEwNOyxsclur4@1J3@ z7HgP$*STlTe)jYHHX$P9J;=6KCy^9opASlw39hWJZPNpQ|2n-e;+M&R#TxO()GC-z zgosm4!&mb z)XycNU>Uu+8Qhdgq?nv$!&S6q@xg;Wf-zQHkwn+Zcy`vBqq?qHcXK>;%$dE=IIfgX zJu;^Uq@2W@Kp1@zAHt~O7n0m4jqNd9A;4$Ol&7Y~YfFqYAyGP-!c0V;j~XIfaYs5< z#Na*?`L}|Ubo<5ZS#3yfLZAlOhDM?d#=a)=bn5fJXlv^}3kzlSz3MfVBWJRDyjz70U0+MyVRuzGb=vNpemppy7-?vB zQ@@2OwHpg0<9yk-UAlkr%51#!Yxc;{Z5QLc55Ts7ofSJ1ywU$>!`M@ixDP8~`l|t& zl07IX+)wQv%8l5VK+6|fM+u-s{U=5Em`;BSw3E?`I&)Zz66lR^Wda6q#9%m2!BoB~ zgnr@)z7des(WGkm$?AxWH)wrtPJ$|=;M)B|v&@(53h#c7^jRKnZoO1{z%scY62D;( zKS2Us<z;04Voxw28tClj7h|r=T*NuZNsbNhp~Ahq z!=9IED;|x%j&~SU!qoNbr^(!7<>U7$8|x)!iPxM{VME6-3?aYWeTo&`olUWrZcunH z?&`5wg6OpAsDviu#LiC96z!{Wq{SktIrYVB*ER#gi&m?9t4izN;6LVMFk&5uu|7V` z-H9R$A<}iJz-M`9!{(`UC@z9M5KEV&mEbGucpKjYz3*m=~JMqUu~jK zVSrMg!0O%?751b-1KyXs&3_v%2T|@JNWvgS(%#U)hc!7Z+l^8X5yag`-@ild!(&Pn zr4uvx##*XnXCO-(++E8q0_$n)R`A_W3sdWYZElK^%)Z>tH@5D{*BGr*n;eFX;tR^q zZF0f$BoE3)=953P=TakrHRu6pvgs55cvA=i!(P%)D!h9?+JK+tw*BUByKAw^qp*>* zsli_kTK z&@_5hwaB=$PvI}!AE$E6nLTw~Y&*KxyEs>pn+WJ+x}vW40S^AdH|SrBJoc_p42(Qp zVYB1PW_hAye_3QCb;ooF3>{ZDRg0KshEy?w%UDJUpt zSLiu$)jK^+6uYSkCI+O4TO8B3neSTl{21@Xb3RA#2(XBKdMv-`Jp8}(M%Qm}@i=}s z5ck9VUyGIi&-|>xgUht{L#{v=O)NpaQ?MG{u%SXF{b`ocNn*;2$_QVou35{a zgULfo!FE!Y`EkV2I;K-};*{iw&JTRrFC8j}=uyx%^jP^zhBJMLxjXJ>l@P*_YGlJQYQ3;e@O znkC6GBND?$RQl&N!}&vtkWf6%((w?K0?}oIgRuR(`tk&}9&AQ8&x8F{^K>dUiO|#2 zeEW8N@6JW7ira@u$GfEUWmW60s$EaU{hH1>nPH6l?@`({y88NNE-q2drAl3^CDS%0(YHrZ4%*t< z6%`dH`z5(`ot-MxvzupU_%w)@-j6Ynoz;B@Gnvmv86ymuo&00AV>JIQw+C3G_jutW zu%Nrra#3CcB_R~?n&9Rof0$SPo`RUdUJu)NQOReL#YuV}{TQzT1V+vK6VKCnZ8_QwrBc1g7fBkTRMd!vzd{4x~2SLWzdaWc!a6 z)W^$~;Q4pF9u zSagfs^E_t3&d3M?%S8KNIQ*HUqV7z~=|R5<7WDt!0a0)qASEa;^IV74(8!3N#Zbcm0hnvV*6c!k`V zuiN-N1q|3}WIl7zm-b#u$~-$_@@GHX>x@3!J=}Y$uQSX}%^x3^H+4Avh`Z{D;*YpixUk>OKaPvL z@T<>1x~35Y!97wuN~^_CsM;RuM`4@0vOv!&i4z+ol%$QTd z;aPh-!_yE<^XXctdXmz#H*=M#ujlc@C7Gb1`g)mQ#->y>cVMiq*;Bry;-=diP zS#`kdTK!uhEfR(*vv~YMvrR$ZoK(|7Ex6!*)Axw~@2r{cY1@(X;mJQHn=j1yB^V?6 z#YnA_K_*142B?D<+;0AAMNAxN-0bFZoWMSiw6tsI8-`M={K|jt;ss13P*xbg2T(#$ zQBfp}&OS69eSk0R`e?ae-cB_0Qh7AIb4%gwLfWHE3Fm5LrVG1T-|@K|^JjmyrB(Y) zoX}P6i~W!uEL?tGw>R+iPYu7xYN7)LVuA?&Ls+K7sp; z&vKT= zmx)vn+gX<%&7}$@H!8BCD$6C_ zuLPg`Ej>?IBw(6PyOd>trJ@;$LnX=Vt*0b9!l4gyNUxf}Up zJ-;u3yPZyzKT4IFl)J_|b3f)RUv-{!F5FhlF^E#buTzO!jZjW|B7v&juVx)~@@?T! zcoepCpB}rH{Udkm6*#BJBQ5q~xxWq@H~$Nl-{ggDgTGRv$d1+G=Jj7P88L;(~5wZK0UZcn2aVk;W^`{Ojht-8OtFED* zV%S1HKyj9eK$jrY(Q|68*WbGK7=M!zK?|*D3TM{)eF>92oqy0|Nt9)?^($z20*XfYqCtn)+-Rc$bzqyU*S6XntT6Hd`t1tQ~X_O8}1|m8;Py zUG;%j!*t;juvEove@(#ppEOuD1Zg778dUK1L`-=ydD$beED;ck1;b~0b6 zr_o`Po*bxX@)>C5dSn~@zy7f07%)?QMECZ8SwY)mP7{Ls4fm6M!teOPP-tH3UwRP{ zMK5QTy%-97d$Z_XeBn6k@jtBQa_T3N1DbEPhlsM8KO(EWjES-Ra`3tTlZ0JHhfM7^ z0gbF?^&A7MaUX1e_8^RnA3{hvg3p{C(QIvVLZ|E)@4NyrxH6EK>UKT|HVIep6M0KE z5%E21OJd)+LeSAxWg+3Jn@-Naj^&g*$lhE4RLY&+Qq~>EJNs^+Zz{?01K(p__%`9W zO9HJF@t=w$8np~TzGzA1@)A#9G)w!`s$*_+!ofm9gP6^5r5_*8Hw#16fQu=fSisz* zO)*z@!ir7)_*FCGl3gClBx~MzCeq9GtMf|J2^|g%xNuaICqC+cz(j}-({I3wZF?x> z+krMzQBk9uh@1(Bsk3Jy_v+fx$Z~bHL1f+hV3lM=``%x<-O$6C>Xb67Q*{WoemS@? zc3!uc!Sd2u@4_#7Ga9~dYb?F0yCR!u7=|-_L^T{HtWxdcmn-vwesZ?1?gCryS}x&D zO(jzW@2jJ~ZA;5+wOwi!KJ`O8<1`~a&81cB>XjH}T2lA}7qXGHxsVIqd$NSLZ;`H= zS?!K0YMMNLZmi%H$t3EDtJTrw>p=Y}ct9}l4B|Z*HcBSP;sH@p)6AT(;3sR_+z{{d zHWIZn%wbgIPFiJr;^CThi~8oH+FCsm7t8GI#b%ww`9g?$c<<-14|iFOc^bMF=9fNF ztIw60o11i_p1F%lA~3qHWXdaLWnEci>#7Xrukz({V{O-7I=pr7zAqrs#lPR}DXd8@ zdYzlZZ(=w9*k@qTU&TK<@s8W!EQ)btAzr%v4$5Dfo{KL_Wn2rWVAc8LTd=hm=3ZNy>C1u7JxL_=^)8Gh2Lk$gOwNrEj{MZjuk&9GXOF zU4~i!z?aZWW2pX&ruZ|ib^j1k}j;85}mq=;Zt!W6dxMe|iU^y(@?=NmzT<~77M+1#Y!35CJ6z%V57 zBxxxezX3OG7vCZwd-*%@2nlPTDJ?;0ja+~#Cg)5eT3_;3BkLnfq*XP`!+_RCt^~bT zf=xOflCOgooIyR@zcNm}{i9N_Gq7Czs}y*Rn7GC+j$RFDlv0)bu#)M_&y3nCnGK!! zB|U;{X#Fxcgo^&gADC5=FK21CBWY>H-P6vn7dxV}LOZ>Hkh3e=Zihh=&z+g&P$%*z zl>PrsFmLt3mFOw3_ckpstf<3h>pIEy?{{uX@5csL(&pt>shI9M?QZ?n{2KOQoK#^{ zgJHskiOj2+bbdT=4^CXGcj-SXzs!7$z72$|vt*8N0ZZ>dk)yFpwh{7eM(5k4j#L{y z5jHg_Zxe_E?6eV3OAQa^4>nb=Q~sMon|YcPl@Rfr2InWvXisN;3t%*qhH)c{ z^$gf18R619#@i*rv^7?qPGfGMkVY+w6C@pMA~!z;76235@=w>vR;rcM{!l7{NQAwd zC7}@g*ETk#$efg_qi5LDRB8E|tP?n)dz}Ft__5((bdRH@Z@^@Fz_AVlKf2%00@wEV ztjzwXMCBs!3;1LIq8o?kb654%r+e4JV_=W{=czw2l8NtYWU>0`Uy8YjF0lf}U0<5Y ztR*?U7Xv7|{x=#d^%o90YCGWDDo_RI#~AN1U4-G8q?uyUqqjXK!FIWrW&@#+VpSVR zIr~~0Qm@X`q3i_;9<)%El|Zc8howY2@;sJW6&T8{MDMyx>;JvwD0&}D0#b?J#O`LV`E!xjtRQ%J~p zZ>B}fpsvQ%7D+m|`F4r(%=(`f=KqOypimJq*fp-^}A;SS-=CkI_B#e{}RE5q{ z!l{Bmm0a&}ZL1d0;>Jy5tEuHi2-%_lhnmPNT_j(SN!om_O~cUNF1r$uRym<#9r|LK zq9(aUJxSEeC+eNHO&-u7eW{E!2t}9vR9A6|)Dz_M<*t{z(VK!Da8)P;;Nr>r(cL=x zHJjB4iLJ&wgyQY=bPAcfCR!F{kjz{YCXZlQjgk$Myf*xe2{8w#28)CB?~Cz< zYGir;uHT!rJ*G{Kd{&EBWl^*tiZT`7SRihqs|^u{y?pvU;`EClnzonOe2WQDJkVUc zm6@5?j(oAVmLZli4RE3zI=RoUM>*bSH6a?d$~S5;$>Wc|8E^gZqaW0_zE0EZu)Hy^ z`MVAbKsPWjFjnzA|8HveZ~0O@PZo`^U(`tn0vezg&*lRPQogXjR19)pC&DZx)@cgb z^%8V==_IF)hB_=A6_TM`%d9Fq(nwZ#4KDB{s1Gp|A{Em6>OTlu7n{wZe+P zFC3JSBY{Do1~|cr`CwjEmPOtJj_m}gaBz%77HV{DEtmx6UQ0nn$DY*mA~SGTwOsf& z8$ZJb+M@TybnGaLQ0PaL5%-$HAb7f>KC9mEVSa`Q3G`pXc|@ozP;!!4_D}HI1zI@5 zZ2fm_a#GE=nXbw^`&{^Fb91pln{%ipzWdT-|@^9|~Bzc2hAA+EwQDSUk& zs$ssn;znV&NHdKu7R<~w*Cbu^Nu5J)F6BLoxDCBbi;jxBgWZTYDoJ6`?z{#Yc4LAv zoXD70twJ911&?1sg%SEBB>nBVeDIG{72XZwdACB9g3AY-(wb3Trhi~Tv_=;%d>yO4 zu*_9bVkvUF!xr0h8(N;oh8z^nJj57NBQ3fc7|J{o{gC8t+dZ^tmg9R##DqiT>^E&9 zs1TkAKGf;?sza+dwH+3^!ntu)21}HMi9GC(#pY5@{fP!et0zd&y<8mkU7+WReg$+X zpQG=1yw%$RDznSHN3xh6f_+>+mV904ko zkf`X+#RVa>Ps%~*(Tv&<4}M>FDR5eM`JY79MB5)mwr|_9zgS3hLv~WOkZ6A{w~7k<5+-TNZ(nzR=6dZ{iWn?pmpz%^z|%dyLps5$fn)=-7DvW<&ZR@v<-of-6n4 zhc{-^yB<1L2{d_P5Z>BSvMB=A8UywXdFk--n;Qsd0G zLa}2f!)~^Vb&ve(m@=owRyTR_osc3pslSwd8ovNvNjOgnbxxlEIL`rXUQ^4CpM!NU zg{dM#w^|C6!YG*y6g+v}Pu~b#YOJA_&E0S<;F4*}l~Zj1UC(M{Fx+l5f%*L&4MtJ; zBVSK;HbR&_Lz7k=R~2AEUZmB;FJgdJpgz#BzOGwRCAWXOw7^lvbF$jN!3d*AbQJ<5Df;3)D zARE#tgze=?ipP|pptjM9e`*&S0|LNO5AlLSlNldf@pBC6U6Ijg5M$YM#-M=aLOyUK z{vHItq#~)9tJ>xXY*Mp>>8EvFtCO4M)i;=FfULS;zCg5>=Z4FCVp5qRn1jPipLkg% zOr<0UH?Xg-k7Vj?gC2WIYHF+70ddYQ)Yo@(e*X0a6Hr~6J;&Sr+dmJVzy4RPehkJv zP^8)O>E=XrQXwDT-Es~7Fz#z0c&nSkA%WV!A8hh7Zxu$>_?<*%X|ew1uY1HIut@mX z$SF9SkH_(SG8q(dXd)rN;K?UWsXZ>+%iy>xdr=`)C!dmjI8Z?zRXYDW;x1Q|wzEck z4DaEj#tb7oW^@xxv~wV0tmnMzch#KT1lQR zzLgdgWp-)!xO25mdwpE#ysMQsZI1KBgj31t5&`h}bby?F$V7!)0q2ZyK>cWC5Mwho z$^6oHRSvXC!EduT`#7(nN$U-;TAFQ7;U*D5l?d&bM6p+yORbGRkF?Tl<{9NN3sgC; zB)zj;{)D`wqG(xq%Us)~`^lqxczuO7%m)jw1+_Gp`~^-Rc-coUOl0idl=wQYIseP+ zhO4COqhJ7&F-9vN8Ox(6C+S`9ULG1skt6*^mvpda7JcX38lbxKpAA9zXjylAd>v?T zQOc7}9Wep(ro=f5H)4f!w0&LIpqB03mCK~EvWiQc^`kw8*)q!bp?Il>@$LfDF<})m zzDGyhOtO?7*NRIErBK&jTYJV|D`U8Gie6*-0<+9ksHr1Df1{hlHZfdZqc5#+KpV6- z0TtkARo5b*UaBY$gypyJnA9)mnGy>D<1Cz1_Vt zZG}eOeZ>Xt;eI=I+Q52q4B{}S_Nx(aHB zYV1FBSa3psV-+AK?zEMwl8VTR5coB6F=sDhFlS3MinXFzU(zQpaF>@KBq>PC6s+^W zuh-Vr+VhWrld!C;EFpS`0LPumY_5OyvgotQaE$(}GPLf!-JRW$d@BY)Ptf3^#5$Gz z80p-9l(BQw1lx&J>@bBJRMfgP^c`h?Y-;}VTV%|WcjvF^`(}Z{ztVpzd)A9b?(zYX zp}BqI(Du_yHy6&mFgZGq&#Ps;PC4 zWFgJrQMkKfwA#x9<$>jQnH^ZEMQze%-})EEk@Y>|blw`6S;J*y&Bn*?L`3&-VP-(@ zwz-*|Kc)URvf>!}Tc3l!S|xgYE4 zJyWtuHCZvZw9q6p=cH@ss$)Yuzo2JvU>w)j!Jd+VL@p5;M8lLy*K0g#!LxpqGB%z8 zI}03ht{?p~gPbWFR&nH@S}7Rp-+gMG8&yt&ExM=}sh^oJW*)YU6jroNdVr#Dx4dFz z?-8;cDlG^)J1pQ}vh*|9cWJP0X*93z+^YM1qNA^C>=Te!*=Mh%MZzr1vToHsa2bA3 z^`R$La1NK;Zq5!Y4CX_xsC-lF*8&@_q~M=n;~hbJ^6Wn~mvXdIXH9lme$G$=Y2M=W zkN!krg-7k9_jbFB#+fZ@7t3QVcf`fEDqi@GuYhj@5d7L+{J|K04FjT9Koawqn<_A- zdc+s(PkOv@(@{3DQIzkPGDQ+vxc@52JCKkL-7rO9wh0mW7bzq;nVCQ9qwK@2$!PO# zf|`MZ4#%VwxU}|D%8KH)eNyPRYQfI%+q-_J75CT3*lHCoW7x%q|0-nL1VQ3i`{uL_ znuu^zuwb(Z(7)85W|&wok%NY&1V6D%=ZVRAdgH-g>k*D9+f1|Sl{Dzt3uj|@*GDYn z7)ifO4r45_7VfxXIxkD)eqPZoD}F0K?L)aHMsSRsGtc|<#=ML?r@J1H>42yfYd&+- zoV^CVR;Dhtb`doyQ1ef+ZfFvhR#m&yH2*wmQ!Kz&W4i-u5fHN)D*{7SbDQCHh>Z-y z0O-i=*P7F!xw*JH8>2=P>BKngYne`3Ow3w$)ezzFN%Tb&wuh%pKldQ&4d`R7(OU^D=l={oM&+687H9vaq zyq`T^QEtpFcvVE|PCQJ)k-Tb2+dq<3zM3`;ZJcqp>tngkQW(se{d(6&WR}4faexZs z(Q13FFkozIx>j|awWI4R2(pVDnNb(Ty;Hc427DxKd>qTt;qiHHqteT>%+ra~Wm!IgKrS&! zr5WHOGXcWy+Wk{K7tf#wi64{A&Gt3}ka*!$&W&gkGmjror2P^wEc*ZYZV`52Klq`u zcOrmp=i!AYeQqyE)jAS_7l^9Jh)NhS8C=U4jbFFS*?;>!oE$SHdx!B;d1s3rz z0&JKOd`=fc1q>JG%8p{|t>@`gNfnHK8F8@SSrtNEPKBroq*)CE+%}{K*I`!2J~0cKgkO&?dAX&A(mo-$V4xpTN9#)G-kBs&Bff;AsXVZQSigblNdv z!pXKjlfvzV9@7k0l3+y$``h0m{7BlLztE4}ldvHd#E)x+jSnI9xszn34P<=VU??n& z!$X6RPD&BLLa8B`$fA|y{FNiXsSPKYo|l#QK9d85jPkN9h6{Hv5}@`!1UvZ1-Jd1E zw}t$>!cZ7b_!BQQ^(a1Z^e8J+wn$?7+G(idv@%~zAH64!e(y;OU>-PDWf zMLCN4Cac;Gn<=m@c{(+`P=SDP9va$*_c+546y?(EP7**E#6|cQQJc|N!(BJ0P2h&1 z%fs<#@OT*z%FgWt!zPa9bN!OUbVjixj+D1|*LM86xNy#pip4y-b}Wxm*CgvM-O?!H zZqpt-EnuhS+o={;H>>Phfp2(bLD`iF{IwjDq{N=iG@ednp01?lhtSd8Rpf_jF_eex zT6$ir6m5a|rKP;u7VIPZ)IJ293BJ{wXVL!VN^pm&3+*=7s9)qqjhge9!3}iV16y8V zZFv5;=EM#>eFv^PoOqt(7-#l$H1hf1Rccc8gn=pC7;2rIoUA!K%@EiyY(28Gp1w2W z)teX%Ue#%CPAl5cTp(UOwgxTcq6mT*DrT8)y>W!VZ1+uBOW#wRfc2i-&aJI| zq#%>elgKQHKuiHN_@x&IlgeC;3y4@Ku+6DlnjT7-Tf(-&L>#)4RGt$9)@14t*NsDjV-5a@bUn&YP1Vi z(-hh1#VL!C-xJa5!?wPPN;N2&u3=o73K5$Bde=VGe^}F}yD#j+?Z!FqCL>{4Mh&lT z@npyfM`7UK(UKulukQ-d&PTB0fJ9rLnBRC}HI~IgVT(*zdz8#(-17z1`wsZvzJ4E93LLt#vd& zoA_rLC~)ysj2*S(*jKKplVi$^n<`Ua;_qB%#e?1_*o0xo3mBA{m~BoagxL)c9(x3P za3m&q5#BFI+cgL75;>8-CDD#TRijn`%lHo)KhyU8^FT0sX|!sc0t0})3ygDRol{BY zWdB7eQBKAK8UP;sZ`G)#U(88ZQdyJO%!jw}2b|HWRi#il{Y+-!5O7 zKXzGDtWNDPB2tHV^1BOMegS|%I2p~CSsiGy0tC1Cg9UhMyv1vU8j-ir-NNnPRFVgA zOl4l>p8hNmLltaC7wQb($HW?xBbPPS4F!M9|H+t(YL@T|Ms;h@wd3cx1r-67CoafD;&Rz7%VvXhh*xGMc!B^2$dH(yY%hflv%Q zBy+N4vkXh<6;#1;25(&kv1kbz3xd~~jd{s=$U`r}&?*h3N^-~Y{|X@}ZkYyhG6RT` zw9`3DLG--~T9r3ul-VU0uuwf+$XR#V1FI>uJQ2XyCcTZ8ec55sKQ$7M@@&0`#GMZ& zZDNl{#hiz3aMEAB3WUy7N4fV05#+8je>1FQCZUH|SkpvLeix*{lnDG9Cx*R;W7_FS zcyOkkm?chsAM>6}Iiav$T#ok&?IApIO^tH=(^>iLzk;-4sULv07>o-#H&s~`T)EH5>P)<@J}@7f37}|av@eeH znYzk!zFUzP2A4Qq-E-fQo%`K)M(C0mhROSJZBnQhBIUvF2C_N_m3A-K{T)?S(ZqIl zw|AGW7yE&X=LZu}8Aqxq|BJu(@_2wYh8ENluT?(b#LQ{&YGgrn7!6SEN}? zsWwt3TY~-~G1c+gB{AkRxJh~(&Iq|(>i1*{QwCI1N!;R_C*14{-Kv456@1@@W1zn7 z?+D^%;5S2tCYzbC_!cOcBY}b*TBDn1PJvMhQ+8~(_PV>&JpQi>`0;(X zz?(b0&+j&cyU@Al()^7ctqmC)RA!b1zb>*#twyk)F+2v$*KQ?^RR;MKyOlq#um-RoGgt&WyI)vOTT>s`N}E} z&^8B96KBU_zctV(^Sv^HdS{>Qf!8aF!qp{PshLT*b#;rpYa@skW9%*^lk5R!%&Tjex}t(PcR$MrIzs+9BJDdgm>k>Vn|@>_>N<~bYeWJcS5tNu&vxOHU$kb5?cojNbPsG!E>XIt$|_gLYn83s`R$Cq zbQ0LUrM1tu6N8yZ(9JNnP5Ie)KlM5ie@%$-%CbQ=8|$kF&Y4o=MiwqkU=1ETvO1_1 zX@W*#VTR!Jh3x!hWPm@&WuB`gN4dk!WXKNL+DyrvB6|+s_RefzY9wisO}@}pqYR@Z zC+huH+#t~IFv_@|pE2s?r$HS2PA0WF4QLz0%uLnX*P~llD}%~PBBZu9V$~8#6qb;H zD^Q}aKvd_O+@JQ74wL`(FRM)_b5p${wqWTi{6d4}c7Tz_k$@^PTP)-j)R7r(`5fCZ zir_j>&p&=qM45M~j_#1#XffRJE^~F6D7;B_KoLlNxWCC4 zIEe@p<@4x=hwcb8V>~=1O8Wz`9nr@_;_G9Mx>hTi$#^Y~em7XjELDEKXpD1rCHvU% zQPNExEDCr>W&i7W;@lN`rxuI#`N;YMytr7O3!itvq%{yM!37|~qv-4H7_s`#A!;?C zZG7E5J)wLb7^m7}m+yw| z$)t@~<_QrLKYjHlXg1n>TN?B~DJ?1vosL8qciz%}~b%V=V;zY3f${w=xJD&{?%IpZE_Hd>!a#eNKL z$t(*@`xj|j|FDc)5Frc1q7(zyYs<#wnZ%JN*sdkgn$&>IQ3{2EgKW(SLf=JQOo#Fg zo!#d&0~8K0zLb0wRBBztZ-PZ0G4_c`1tKx%bw}t)0E(s3!IGsab279iwCXnK+;VokUp14R@4+ccVAjJCi@KHGnIKY8c0sF3d>JSmD3 zfrb|$BtE^SOo_1!2NoI?yRn|WakF3n@&?gEtb%_Td#*J9P!1>*YE=Pdi3r*2MoZwZ zu!}4jE5-~3rl<|Z7l$%W0?wlnZ8V$@L?jepN%%Rtv_;6@zS6!G;Qpt?fOa91kyxH2 zOY-ZqTqlHaTLB5y^rhmdP>IKnB76C%uFWw}ptZU=`+9jMcTu#mUY>LDa7C%GT7W^r zV)b}*6V)Zo`r@78d@%iW9dc`GQCCR}a{dp8cpv9cPm9E-A60wzKmH8{dy!Geo!Jh5 zx>sh7KK=4?5c3NmP4T@L+YSOfXbP6bKN$T2ccWGHNd$DfjO$l&TNHcVbn^sPe6=t9DEG^ibLk(E6-B=3UJETXwbpOe*6W#?b@U8A zQL>?+{7%i(cqa)*hs!GkfTj-bbr_8+zt*+0&g-4d%{kOTZQ~jX-q}R)MRE_S3<8u7 z;E`ITQm>)o&j>x_3p<>~*ViAhqdKf_YnSZM%KQ9*^p-pq$Lpq{&q<{4k936qmaQW3 zh$6#`KYnEj_as$!O+zx`05E!3pZK6x2 z{4k{?kcZyjTE%I?9F+TltU9}Thq#UJNe)&E9`xKs-V%0sM+2!5(k6Y{=`F7w$S+cR zYE-^QwP9(11tmQURpgE=n+rj%(x=r7h2j!0p;uzbmz_#Vx96U1(QagxaDAv|_)@k; zXrq%N3-D@9q80dL$ncZ5;fI(!Gpu?g3(f7CW0N?<)40iH`)hlx!V|!4*KN7Z z&dE-`<}H*&a!M2==t+W^Dn^K4KE}IPRYxeggrU8Zp zaUGd9EO5CTE{n`&xS+~87Z1>DY)^IeO+VQKZx@>5m9aW}i*0xp2-I0<^7-|@iJ`=J zOF+RXQ?&}hoMGJWgl>wc3C{xG=UD@OhCG5;9c zH3JQ>2i)HQ=;vJb=4n?^OyS4jowRWm?|w=dEz6cypJtiOWZ?N^VrQ~ZV(_IZG%DF! zM?eZ9w&h$odsSrR@I(ET+7E&vbZvVfeZ__@-)Go3yX*yLdiA4~2LNb9oJQHPfT zIb`cvo#xWycYiQ6Z4{N!r%4%O&`{p>Qs)Vv?q5hw(icI3w?hXcrt=NXzR%i%+1b@N zh~!`89vpBOiR?K!V-*c^+a8c5jhG_`y{yza(De=jJ}4U-8!*@5sVx6ESlj+Tdw26o znw981HW%$qEMFu5(1_~v*)7k%?=WP@Og{EY7fR07W$NdJ-%m`J(j4g5F5qe>7(oU@Cb+XD^M}DR?2Fq zGr_V8>4n3KZsB^^OLAY@D!-m-tM=9~2`*7bdJz*B>pj=pV^sMi>}*&>C8 z2}3gzGZl$F_a88DCETU3iW9+C?_uLmt(>m4#}S(Z0={%&7UYGlp=jF6X(KAbnke4l zFU_V|v`LeO%`u7n( z`E7`SazF&az(bv*J>%*?zjsXuU<5CUylwAtmPsHUAdrq{vJ!R_xGSMngNCdz1U>O23rrZ0;wTV%{{3k*%P z9ycdnFYgGLuU_?1y5-c98@-ZOmdyh}&18U#O6(%l8dp5*8YKm9W+IwfG-_nQ7sY%y4G#g@@5a_6`sZ zt{w=pyZ>M=>Mka>Ap`fIBk{{*p6*MlW+771`53KZ{{#Cwz9R1H7cR|O2=mG!?1u54_LMP5) z1M*|mUm~9qUO1a?HVC(bR;KPCgZ zjHN6pG3y$(!d{VKQoaHmlJ4#C@ABsOc69GB_%2^;+hqdH6ck1^%vEXXq7eJ`x!ABI z?9!Beu72y3SpfxJ6R=!TNIZYj=2{zNta^77`e)?|M3aQ=E=|$~JZAfE(D;(6)>Tua>+fQFG<){>fPs&f(|qXiH9^rie>h_Qn-VQGQHHppD)bMkynJOv$JnlCLcONHg;mKT!9 zL|;=qf8s-P>9F%%@o-)PH6MO(jQOHxRnTs$U@VGYp#pvh?k)^2LmeDqZr&sqn@Tj> z*Ypm&D2@?DPb38;Rx3|>PZmK;EFq|q5@Rkj7aMgz^ws^VhW^ALn9Pz>20>+pvFqi} z@fgK22Kj{SRQjPani4$(;*`TRLCPwgeItH<3BB*WOB?j{Oe&Zr2(?YP&#aa7y|a|j zACJO64j=D@JXh_X+(bRRho{mC95w`621BYAenK^sc`#nKXOWCO6ymbeO= zapYt{AGQU^10Fw*)Y~HWg$qtwB@-37rbw^>mRz*i*;lbtIcIz4jzYJ8xDLxHH>qjV zNP}o{he?tpK<4&URrex(p2G!iy0*9AT$NX!p=rkQ%p~l?(h+8sPo-4WcYWa_akhPU}J5C*uqb`!^iz$;qqIxT$nN6=n2#de z5Aa}`0m6iRijF=jgmuA~`*ap0U&dRo{vsPw#|Q!dSU%NrP$jk28=3fY$Tfa4)8&V> zQB(AC4pLz=3yJ#J2DY~Y%9Fy9*mtJ}b(2!v?mBuk1U-Eay&9nzB-e(<*k#Dcc4T`+ zJUuSXhWtoN$GQ#mrMR73a>z_z_jN=NHUPToY1>-yk7!3RcR19VGi0k%b*o{=pfLzO zkxODk9n6yf&p@tb?T`L`x>}kq7k&Frja4qGzv-ia0Exeg!=0$|{bS;_W6;Dum!o_8 zEyBpPGt!7~Z=E|{e$MWG_T@0+B?F{tP0*vJLNZwAJt~SB zN(l+$w}}-{&%0&d988wO{+z#_^S-ZI?fq z=%>Y1`g5r7Z7%v7eX%H9grZ52u^tbXJJw}bnj>?oC0K)w*d_r2m(W9qmVvRzgYZ#a z%4g*@eiKvhaw0!nqi2lfo6Zk8PzdH?E|SkrV@8cAcu}dIF9n@e_VguonO5xiyo+wY zbRLtWMGsACj*v?r6q9SUf{AoQ?_<|(3di<0)m;ecvE(z;MoZ)~TX05tdH#8JR~@ib zR8?+zaa3wW0ZK^{gq)n*>0qW3a99D7df7B<5hMa`1t3}elLLuRPc5WY<)P;&cQa>; zk@5>6c7L*Vl~xv8seh!cMph_opLgtAaz){d)Q#|ciTUx7?|7oYb{vP$Pr5TMpNlud zc(G29FmMc@*j!YOU$93fmISnz>-yU0lFQ5Ubv=DJ_BmT22sEdeOHF8pI5RUS3MheL zfQrR4aPN-Y$R(vFwT7b#O_dqiz2XOOMBOFL!@!SmOAqZ9j~c*Tm7iovPLC2X~K zuvZ34JTiTq144p2gn^k4Dny&mUMq6^FuKUA@MX<`5?PL}){k2P9)BLL5{X0~JwqPH z(1~yo=)Z|YTVMJw1J1jUEhn} zY2Gwz#;*Vu*~w5PdhU|mn!t5xtH<0Y^V-h`a!H|rMA0zOj;q(2jz3{xlwGar>ji!2 zRI7Sx1&bp~3zhYqdk`l=leAw)As5JWQVW)qc&9_(V2;OHz*P!&bu|BU6gAu#uS717 z;;1SL&re&Swh$!W{AKAOv|UdZwz|Y}I$tT=_2?P#K~&GgvRnpEgecx`ob-!K1~beL zxYCZ~qz_jVqOBh{uBrHL`YdgMJCYuhjgCPC5TQo2q)G%Bm65?(GD{1=9~_f!4zpQv zm+epXmjPZ9-@GBank$xkR?&-nECg4PJY8&|B|_gH$EQPw=v}R|Tg|yu&i`g;qcAua zl>zqp18U*+nxKRKp)ZuSd0yhlFSDb#^UvFxGJCP)T$fcxI;y}9v4KIr;xdo~)5thW z<9CTWVEp1aIhc92#eZI?FEG|%Tul2JPP2xgCv__=pkAyp@0UGNtkSFX&C|S5GYX_09$M zyx_KvSIogs_0Zx%wNW6>{FT6vFL{9QM*7};UBnWy)#HmEmYf517BmIv2!{H3r)2&@ zy+qa+y!pcJxCv9y0Nm3Hg#x@JbOhKCllTmB5$3QJFKJQzP~?XyUK|xdlEWnWX-^Kq zcVCk7T(#b4&i^2_w%NnR@AM`=8QA=+^*@@)CQSr<#ESsCUWPF44Ui-w3-_^H$_z6kY%ffGJB$-Xs$0rcd z#J90MvaPA>8ojf~3HjMy^m49(B29~gY0=UF$jz!7HSA9O@cdQD1%ts&+#P~OV+2Nj zQeX^bf5rgCg#O|N3k!d3Zun&8I%!_4V`gd^GTK3+=?=6URjStFAU(cRQNFwB39m4R zbdW@KybXgOhFKK$j|}MYSY(wm%@XTV>gG@kZoe*Y?El8Oc~m)i^|Q~p&(yql>~4vs zJgn`PeN}bMnn{^IV5gwn{;1EmaumC)Rq*a>-rl-0~aj0*4Io9*L1{pT|y{VR(Pr#Gql|EnmB*@CJKM?3Its zM%4}Jh)sA*{#K0D)R zv=i}4K9P7b`fo1E3OP`jyZ-E1x}Wvn24GNE=0o}q4W;%{7IBZJ*tfx7mNYI-lzcLY z$4Bp@?PpK2OhmTh7MSN7@=up*tRBMutr+$Wq zNE;Oh0_1xMCd4qR+K8rV$k-Epo=^LbOc)PiLwEtMDhmUHRKV;Ca^$2or?NiS65GZz zs-Z&(fKY4?^+AtW=BEPWb zw~zzz6($N ztj@x+#kEmJVa_X={pz}3I)=#^vNn2D5j}ALl{evrs{<>jY46$64cZKiGDXxjk?bh= zp@|?a4x)d+3al4p$imCoKOn0-y;mV5wv`=GmMs3Etsk!;&!OU;|2|}e5p$C}0S(X= zRk(OHjV+R7ZMOEh04UV=9dmJoWGfu%Nw;_9jbg4oVTFaR!Y!XnZH86r2k>&jA(=(G zwehd&1GU?zYD0fi)HHWNEc1SyOy*1U5KbOmq6EG$wQtQF&o~$6VE(S`rBrpMNMmNQ zsT+&g4%Im4Xx<8H(#7;jz1>%&Wa>KRwVp}V;yWDmBPm$w!-mds!EM0%R zPT*Apx!0@TnAC>}qch#Bt+Xvi^eh6Xv;-8G0L%7Fv*PEQaWCx^bnpV4t>7K{j+J^UOPtrm> zYT`(k>=n6~22Ip2%5rTfA&Xp51R=`5T70h|#$@cAP4g%ygqi6Kh5_Lrzcq zz<$5W^S0;al$!jm&gF98v%r6cMd``9@mkvACosL7d0;ZTym^w8WDQLup7GDqpm^mv zsqKfrn>(b08;!F>NTgAjg(~BF?ND-H6jFrxLCRrey0eplPtfvDGYxU{iGI&sBA`X> z#bQIp3rgt4d{;PkR(RdyX2b)APgUL zi8@tool1VbMmdoABZhdyG3{5u*O^*c=&WP!^_#+bQO2Q?dhm%M4U52U-%mb3=wx>q zyz0kUcXIPM4_vEbf^}b}GE{>(={#^x4OYVvd&rAiX9|40drme2?F%Jvs5PRH7+s?R zEpezb1M+~dwiw;+wU59kPtE}K<37>aP^pH0ai$o1X%3b0- zY_3WQj34;lbtmSf1Htt29V3h%p=P$qoT!8uzw$`?0L-JyLHOcHL{m7#_gSCpn9xOJ zT(aGpsLC|cP}dLaTlsYOcdlL&XTQkB}b>n7a1zs53CG!5%VHx8F zP|XQHk3VmoA4W(~?lG}c)HF7Z^{xqM+K0p%uED6iz3@)+M51p*p3BFd*O%`e{GQ%B zc^^{7AmXQL0)&N$s}Ip?r6UY_dRVh@tRnIBWY+(DXW#vNYbfz_IG9}LMoQb%JFvwA zhr^keDuOyBUDDTYi`T-E!u^ELZ<~&Np8S?W9?rF$W=3fokgA!82A!&Zw@* z-&4-jEuu9j$sI8zFkstE?d6kX^yRyX{g>P0c}{?4+~x5gFC(4*jT#}70y%=DQ{%H* z(atSB?;HtBuHQJ!{kaEHM`r!G0fd2D0GiSur|z#Ah~t>7QkSZhpvBBM#AHMb*sZVvvF0T2T)7Y)Z)9`K6Gvsm@n|D+= zCts2wz{ZkO1=*Ac^D#MV*8jW^SGnt%mBsf#a5T#0m55|g#Nrnr3!1_A(UiIs7%_#G z*?#wVz;6=X?YSZ+@iHSddfj}@1MK_d0)Cj-jyu=%wz1rtn#w&a?lgT zDCtVd+q-tQ55nyG_1O|yI(m^~rWrisP01!(ES3!V{?<8GjKUJ=T94q0P)F{ zOIthvkf8Wfp~*~?Z}PP11)5T^7Ly|G_m4qd<7!F0%XX7_Xs)BWT^|aU!O}IYkgSk_rXGDj7m&0khR4Q0bZ_)6Ylw<+R*T)z}ZZBcK8_a zaytqan5M`{GEovit=F7!xY4?wuhGc7XTSv=^s~eDi=?3HR<%xMCKVyfJ(eO2`7!k|Q;{jhM`=tbUAoK9$OSel+~~aLDS~*=}g; z0}~^Ee+!UruaE+}X(I&wpnVYTqP>BqNj*6{gSY*jZF2k`Yk%J*Mol3ZGlk=dfuwHw zNQ->-Lei1p_|NN6l+X8Fvo~Jy4ngkN2y^b#&`|pDw~8r}vXfx9JI(BN8Lq%bgAm+UedB#t@E2XUi%% zr2kj+Qy0FzkMLF_lBP0e)b^LF#cTRHo!B5?mszaez?EhfWo%_ixH1nzAdbGT8sOxi zEu)O2&I#DqpD+dZPeLs&+&h^dKk~?>mUm4Gr+`3-fS15+RRZ$i!RZI?a zk)Q-*W)q~0(&flXIna4JC7c|p^|~SpjTh;146Zr5W&iSZd+L*&Bn7^4DxSX9SR)*p z&Tj`B{5WVVk6&9rzjnY5?bj3#B8I7GgL9cwqoQ(}oKlp_FD7<_@ zpI7jQJ(ttON9_a_3)Kqa@AOvPI<;gZ7M?e9C>`5Jp;)*zem$3dcbb;90Kk@W&g1=i+kJL2#Ma+Nlrbb3$1Hk+Jv3o-G(Mt< zu((#gV$}Y3(sX#KWu)|6PaG3YnX0@AC`0?#3wjDUZZRrrS9RqoDw(%dU+sG5#lx9Y zs?5!nzg2LRjl{kS&IE3$QD_jSX%o;U8#5*xKRvV`i;x|kpg8a!B1uo1hN`0cZd)lz zZ6{tfUl>y_$-T|yq=b=QHn7J@Jog|oPkVb*3|=s z&0prYI|7YuqJVy;CeCgst&T+jQ7&_{-|iX*XwQhPIX7fn}!)eCPTIX%?dQ%;?uPAAc#C7?TZ*SWp*f!xNd!4UNuKZD( zK_<5Qr|EL)O6hc&Iq9XP*volz0=}rMhNap}MmiQeS0w|RpRZpwo$_42aIbide;^%v z(7xfk^f`QJTv>qY_4jB=$+I?-2Uj%>GCtw?sfF^tXVQ@`6mTt99jIpSfZBm)dY`v(qJ(%~6tU`IN|a zNdzAP$=5cVyCJRwgP@uga+n@(OOu+BjZMa74#`Nl9-cEs?3LKSMHoeevMdN;BPaM$ zYV;}+vf!NC5JTN~<*vP~pP$T|AlJPsl_#yIO2=HoL49YSuODBsyI*iTJd6qe&CV^q zfgo&)eynCW1Yn=VQRItpLc}S&5bU=QA_cQM;QzTr#?MN||J$ycivEK?8S6W>d{>XV z&C37fWbJOX$4@_x*{9xhi`s}K(?~i6D!!9Tl#06wOI%_rUR2=kRP=B{Jv1i$O<0md zD~BisN-CiT(+8?f5>eHAp=#wMpW6&eK??}*CoSGjkL1_WMB)@sQOo1?0imYiugLU3 zlU!7ZSm8FieEGb=yK(3Te>&xe^<+Bv_ehCLbj33&EzNSCz7#D7d{i2aF7McEdu6ey z6S-0(<>dX6Eqr!qxEAQQnS*B8RG7ZZ8zA=8qxPk<0op?#*@7-j|CnC{Q{wMX=IaRb z)yUUaF7B)!Fv6R*bqim^qObFA4w5rmNUY0Bo-X;mzunFB;n&}J3wpIgoB@t3Gdrr? zx755O!&0~`Z5TmC!8v-}PA*h6RFOD|T-4EAL2CG}@LgK{2B7cm9qk-a=Px5KZY%(F zrem%<$B_f5TTa>LBdY9W;Fp!|>oi%dns$bg&G?c;z=H9Ce{7#|_evdTo{LIdI~dro zNSD!ZeFFiSm6D3AQ&TDB-XkVHsZPD8$r!D~$;|<}C_R}R0(2`v_gllu*gdoIM0`km zrni$e;mtOW4v=G?hvg^{y)K6~w@4M`i^sCJ!gx*TUO*ZA-BarYMjdd1QD0R*e$GmV zZ>?|n*w`r%2G&1HYp){9QK~r(;RBUSPAG8 za{Y9&z<$D3y|&S+8r)Q-sW0E~CM4utwh0MEPxM{79GM@U^H=uoGbC#CRSDym2@QX!MM_E-XwCL*8*ZrM@$`KgSBS)J~ks*`Bx%x0S< ziEVOc^J&|<5UxH#wed@2F@4|lT+@=CVduQ;Q!xPAN~JB z-Q(SZ-(nqbSzZKFuqomirx)Qos7{j6pogLct!nF&PyuH~<*Ix#ylNK(4I2eO@lzD| z#Ad?x2DKoVJozGwo_8_*oJg+Fjx8E3-98805}F20kO!etwmycdrDH)dS7oQHAlb`w znL&!}qwnzPQhs_=^2XM`-Mx~K_6;N}F-_oFU6XcRbdR@)aW+qoZ-_PbLdZAai$nvcS z$V{wN6Ngv)0@f2p=n*(d>1OcyHu{*xg2_;^^gib=$s^p(x0K-b=nv^nbS@8lsvIZp zDhztf6`W@pP|UEanngPr{kJz!Oq9yEr;m`(rBNje*-@wfpVzI@h!p&v%BY<2&z?fF zeJo!{e%{JGcA?tu34q_C(z25)EVjb11*=gQ>KB2_j0IVY)2E0eFJyFBmek0tufc?) z!F5DR(D8uJrMiX%VEV!aIX|q)Lu5ew^xBZ)ggVDpzA(kv?ZZyAvvvz2Q0GUF9|e+5 z6`J; zEw50E;8M$<(F>&++P>$h4&~J~f@&JA)EHqNl@aC{>7fDfi5`C}=dJT4$f)bcfoG5C z*~k#!9cHxFLemJ?LXaq9WsyEl8E5f?CrOWqxH{hY)U?LvuuSK5-q(VQr zCwUM{qlyg2g|Nw@_9FBRZG9i|!cVm`g46WAcz=i9`jMqd!27y>&=a@4_n&f1*QT

FzB=0X6(TbZOaUw7~E#h4sFCjp81&DJQ>_Xr8(V4l>h4|(HiL)$eu z#BZY5@U0M13Ltxdea@r#D;ZYCiFCNe0>GJ^C2g?LrGJD%8Qr$esO(E!o-Asg zDlW~dX$~i1qQ3eF^`IqHc92}Ku_~h=(;Uf&6f(3HD>Ue^xj8p%+q1bc%6Oru>}5LM zxlUFp0r7|w8e0T7Nm6e_FQZ6d~ffeo-B_|m<(WpQBE5 zsbR`uo4N>%)R17etkHG_?jrhwqV#BG#OP3ns|(&4Khq%f3_e(=S<@}3THobI3ZRA} zPrxdEbzVH`3`&(*=C!`&ir}JqXREmfbLuw2^Z$bpn^dYj^pBD0?ZHG)V{#-;(uEk; z?1AZ9=NlD%lB_wwo%u1xp#i%IUy!Hk=?LoFm^F#d_|eLMY#Eqvz9eS9)o4DNQmn1& zz0^u8KH8_S;>;-1tl$W8O`g~++O%^vTGT?3mrK{z7jeC%3&%~{aa*1b3+Md1$K%(x zGZUU|cVvN>r$qNMYwAb$UWaFyI{Zc-~72Y84EQe#lJnP?(fT?BvK%~Z4Vj{w?-~gOyr`7ke^I#W6^ zz|>PHTtZsYKy_m~zFvM0b2?gPtZm##IOoV0kh3e|2~P^{J{A`+Ybo()&)d$Dp?*56 z?(K2Qb)J+q`d%Ml&2MGA5+{8VgrCcAX8D%%c=L9xdZ@4iN6?0PZ-%(i^c4*buzCv? z3s*$?6NQc0#M~+TyV0{+&&;bsw8iW-7A+DOPFIZjAe_HkvV#N^LwG-a@1DaP@ov(X01rPg)HKeSB-}rmiTlB7jtq(7;V{ zf1-wmC4()Oxh4~tRk>E5P^_!4q5XoVE`mzqr!3HJ^(JC1*jHo=BGvtAkV@-^dc_~C zSAZ$w(2Z#2lk#%c{bXo@_*ST2hs3r);k(eHec?6O{E_- zd|AK$hX`pt5Ququo*;-d}zcCFQ(C!8>^s9#K5)ruf=)65*KWwO;wi6dr+x97^q z=A%#;+!Mp?rx2h1mSfY{Uf^4S68Kx|YwtBj(P1KIKhDi6%#WQyOpbm{Lw!Mrq<;-p zjY>zvA(Tqsg${pNGeh?f|h{QLrt!vNqdE7r~-xxz?Gg${W`p2JJI6m5!E-znxY_oYh?iu`(x|}Iux(s2(@u3y}B5^Yj6T=nKV@6U#G+Y`-H6OhQ)+M8mzQn(r9oj5%x0KpuQ_tD4V10Ybb(J zqXR!Tg>xdqH;K`x(2cP5V5mhPI5O}9@KG)pRfld2?fwjKC>%_q;PnXtrq&IYZ{>?A zgL?ce>TWqY`V=OwZ*J1FvWS2Ht;fv?g`S=sf|zI>O!D{t9lR@0PM;(VO}+Idkz)hK zZt(j_<{rx~k`i7CPY`txisFx73nZ=gck`c^=r6x^ULP;nKf@?EE_!62n9XFYcyAq3 z%$eVRNcnDo(9R5BK@y{Vq-?V9{};(7JD;o zKk!V>v7skQa)8iLq)8&j{2yH*eO^3pMiw;cBUg_io7925vjo zcTB13q=)H3^KLq2uw*oznPLI7>#HA2a8v92KuhThFCRiJl84FoZ}eL|jXkTWqs}uW zU}qg|U1PiBm(Y7JrYgL6xuIM0v?%F3swtOw3`XnPJ9|g2grnf_UrhMy5zW?D zd>uQ90}_pCIGyu#p4Er7ojcXqtg40vrpEX2kPc>^IpRp=p-a~|g3^zP9^GR<*UDd~ z<#4BkL@HGn7+bqU%`B3n@}}ujW`TC&2R@}uLrIH-y%;0I;Q0GdGz~wYz2F>3g%^`8 zS#4{JIDhFbFoOKJ=jb|}Z-8>+J-2c0lXmzi4e~8^VT}?k%ZusYu*S2h*O}V<&;IF! zhitinSs54ceiDw)EeiZx@62+)&BJoioiY<+5OC1|`f*N!K{KL(Tvbbbkrzzeoq#j$)u%2*UXo4glftb`BnFQC1c8rOBTOVg;LI%L-`9ieBDrE-@x?5hZ$q~I_sw(x^HR~D z?})}s{B4o9Pkfv8*6h*xcr>9eOUl@TrC);s!13}@enot3%AU?8{2lD?rCv=BO^MH^ zB7MO1Nf`kv6w!Z01}@8&hxu~FjlEl2_?ylV_Du?flcMss;0taVX&iCDCDH8nEDSUa zuD6@4(Z+$6++P2$5z3c;x`|+SzW6xK;q{+C+jVfo`1RJO8#mLl+pjspwXa&cXKwhW zov7;uJ@gEYyqK=s8Z7wmogA{F?e4V8W^YU<-HR?OoLHt06&gya@R*BttdxFTLR*vI zIksY541i1c16g@jVS_ZIQeugunrJ4v)9>{5a%?2YG8#oZa0ZkXV1|pP8^2N zEI#;OgVy^aBb!mv7AE@nmz(Vc z1~o%%ND+S!@~Kh-|NNHlegBh)iHW(oE!WgfsYahLp-CF=9H&nsIDznb`9&Lg@u#zL zB4XmolN+{i@kv^Y%#Foh=gF}tRjNd)Nm-_PJ+b7pAHSID+CHy3LM>d>>h#r^G|_(+ z+t$Wve8DMGD?w`!z?-A{qcyM3c)L4=5vi$=YoemF> zDCO#0%5pgHH1lhFwq_~)Jd@X-q~(_g$-j#B%%HFz`Mr@BG}0t*iqix{w^*pUks_8evnT1l`#C|H-TN6*Tq~^Caqe%AQ;r;mW;*dTd zJ3^I5v?3{J5Vv7xsnat%oD`5QdnxZa(0f_*Sa*nRI>~#kpVBI}FRVLWl791eTR-SD zaBzLA-U!J`ZWfoIk zA&lF>^K^o@S&~d(o%s?~9ol&FOrIoZ>c)crFnr!BN;tXT+7R(YziA@FiM1DU)co;> zm;J9fl2b%hu2Nw|vu%m~U#e__LE_K-4<3mp>lr;tUH*s6;EJmCjkMFGR}}%@XHxXt zaR?8C&$EKpq3Hw!z;ZH~ZkDxb+dXDF?O7lRoJfVLhL=#Xn?nSIi2x$!Ix5%s6xuDZ z(sT1j^~&QaQb>nqAfXkln?W@*QY2%^qSp2`yj!dLsrl(*+ns~CVC+FyalrwtClTv_ z2OB40(h1@vW|T zQg#STu?MjFJi*@YZn`8c@jLN}qbQAW(r+QeqH;{$K1s+Qul_lmV`U4BnY4V+Nkk>tyn1F5e%(u+ewC zbgtRL;}&qjND{l95)bbMLYx=-UATZ@t}Ht`UZ0wL6K}z^0Bx&hXgj~u+qU@rB5TEL zgn5fdw%X2UZ^(oS=<6Li%NWXd9Et;r+G==UxsGA|yn*YFl+m=f+uIroKKUIU!3)B{ ze%|qjwzo_qHQGF7@7^j>$W8zHTqw#8q6Y?T_N5VikLWOu&@YRk!~hgwFw2z6%}=mK z1-AeJM5gd#7Sa>MWn+dN=u^t1nlBM6Hd12(q2LBeaz@rYUF-U^#<)D}(+*vs5yLDk zVJkf!qly{6tFLhhJ_i94=K=I@A45lR6QQL)pe}1mD7aFS>K<;0?6PmO8|H~wV=+DEBjQ|@q_>wNc}4IQ~p8^U&lWn%mQvxA#iv!WMY@z3b~Y4Q+`6{ZrMh!8-Hz+|i+?=utHT1CE%GYKM#t@~)O(ga)R9Gp6%nOETY#-E$rS9hLV$Ox&c3-%L=yLpnFCQfmRD*sh!SKZnu zSJNsl8WPpWX|L4_`!rH_p5E1kavN8QDx(!i{`(tEe>rWL#fMqL$WD2B?|*ellgHNy z;HB1{e~PT%p3gg{c48RX@6noX^+?IHAs~@t?Uz5*G3mzaId3EC{H0b%tGPOJZl&yR zlU+DrO{mF(y$+wl)2u&OTtDuW^5Eu2iK}7!)r?n4BwE(QcN@s(J0nIFT8 z084kAfVtWKoJkat2Db_^BEHR{djOcRL}T*Zi%|WX9LLM`<=w{{lErhR!hpD0fvkB& zQ9MTJPBT1Nq|S{J*!BSo+j99a2fD3dwFtPZ;@aCU=(pblK#!lJ zwkA1yeF^krg^+Fa3KJx)XG`)~ci&Q05@}_NJm&5nKM#+dqCOtIpjKDpjOq->fpm}W zVR}o|?YcO99XoA$-q>mLbM$qVF6E_6(JA@~&ScX-mxl2?CKSO)9i5zeVVKX~PdB_O zRS3T>ycA0>ND)VoN|cw*|Bav{QVctAI$cXoI_}(FgK_HeGWYWJ)RG^gDt^HAmhrXN z{->9pTsweqD*N5g#w2Y6f;ccecIeAqa^*S{OOfBU6Nkw9+?CB9Pp`E^o|5(pO&54S z7RZ|DyqX)XtPm>#-DQx}x_I{N#)&vS^)>0|X}J$*frptkW(DP_E6rMlZhBX@HU(z>i^3H&jzew2Vs8BmmBn>P<$ZITF9=t zzSF_GZS|~iL%j?a!6h>VE-A0yHI|yM_ta5qjs$X1@djM?O;wctQsSs!nX1rjE@^Z$0Aola`V)+|6^i+Ih;0$E{nj0PVNyASM zd(6lj+7^T1dn#9#y2^$(*2>Fz&X>yPCF2$uhl8ILk6mWn>h_ZWBNzX1!1DRSxPV-YA^%PfP=kMAf7-VrAedW>N+FiV`DTc)gS%piohk$ov>#PUsOU3i2Pzmdf~2 zKu3e_E#2pzr8zVyHDvvv9}}!k@_aA7XI=$&I+)>|B2sb1=!xrR8c%1TPh2^A03n!dw;Q zrSPb4eEpA+zlKo|%wZ1_P~?b3z91%2#N}YLe2MFXhmEN7EV#&*)w~YWoynJ_DarG{ zp2nh6;_;M8#0kw94eg89WL<45^e?5wuE7J=1a{^=FDK-2Y3aqV*6xKn$tIg4Jig&O z`{EWP7wo+EzzDa+z^R|5auO#FM(_oM-1_b&Pp4cPFr8>?!e31D^QmH2RBk+FvUmg0 z-g)FG?_(*)DqnSeRJOYM{02hUqJOkmq>L-_ygd>OyLR`fI?{b+Ka=^gAGQ)V>vuZF z@8hsuI=9i*VnbHfzU!gr^E92^S>g%cLGbinUHdL2v7Dy^OZ8pWG?z@KpVj{fyrQXi)~$(v6CNk)sb;< zOgeYeJPpw97PkhO>Peyk5UoL#0$#l~9g@&IzPD@o`l$hr&grUV?C8JJ<9-fiusPeA zX=o6O_?0$G2(c{+>A?oy-;uhm1WO-49M4u``bC+*XOOo_hgz# zQ!|`ZkWS_2!7=;jOOFENVonNHIdy@KWJ>~6l`*Qh&sd<1j{$Zuh=v&{8gfM%(XWO# zm3pU>(x(}vvfnsY%8hYjNz+K~4YMcHDAj2oZ1d6s(-LAs`CiZ)ezCF9g$lf#Qzi)b z3phiH){u)@MFm(Ea+h(@Z6V&)_-Dw=e$wQX!!xF{cEiohm40}%1|YLH;;PIU9?CqJ zi0c_i#TrFmY)GVq$m$K)*%??OG7+mMx!|R8*UXjQl_eu26wN68V{;zFr;e-@q2E`` z3s}wErqv%Ydy}OZR|luVX@NHBiCCfyfj9Sg=@7(bJdo{CXSJl9v#U z|3?8S_Ty2Pg|{*nM@qkwyhHlg8s+A0NJO3{kk4q&6-*uS+k0OAMDx3|YhTcH?k+Av z#3XB()-U-f>YDQIi4>sRY$TTrd)tooukNZSC-CIC14`4k&f_#MayFZUX2}9tJ}$Ak zcs(V)Go;OxGVPURUv&=tR5JbYQ{@&n(f&J}>7GaPG*cpYq5j2}m?6Q4X6poQJD z9-GvQ&%b#h z_mZ?FB@4Ma3gxg8FJKVaY|gj7`dq)dvXJB!12N7RMad~46ankIQ;m<|0L)KoKv1#~ zA$?^TXTG-OC2ko2WRS;m>C&?jW}Y`HkVBg*WyU5^DAaV#Cnp(jPy4hdiR6`uutlUT|Y=DC3V{ft> zuw4lJzZp+GX?%%_KkiTSypFd)VS+4e`}{m}K;rGFy2icxYzwedi}k!Y2@_6J%5(Gb zhlGzx4fljXj-+E_MyY4ruC4# z#^IxnW}~~#k4-xGOB(DP-)_i^e@}IsCDJcux<)M~9?dgbTb4SPW@tx;D?S4a!&?uh z+4Xi*d*6VNJDf(->3z4|uL7XZo~I{aAnsTN92tnLfur!h1Gi=EM3dI0*2#WjF6VY1 z8)NSisZ|BLQj$I(TvZi|( zmR+FgQQG^Sazi9H_`&h#u8dUn1CG5fkRVvuk$3qUZe0)jL&?&dTe~{lUC(T5&v(LW zo_&=0@-3}_X;ssh9|`T6_e$yq4_obJhXB7I0Fv3OSwm0Y=P~ z>M)>a5zxU5Jb2)ok%^$+j+03cl7xwy=fsTqptp^|Vn|D6!>wI8vOe?R=H^H}oa+E{ zxy&qIjJ~qo=LkvWP!DeeLgh#yVN2m;9+%(J7u^A5ON(=fE)LfKv%a1#ppaH^4kU z)Fgxbe9N?Xvml-6I@0h-(IKHRZ@r+n#a1q}H`vq`jx{3%xGPjheP@TKT%+fP8&3aKLro`9TY*x0+oE6L@B= zrQc*u{SqiITw-*Ox0oK7Wui+MB5h2rh-NH+(%0i^i(if}SdOFzrEd_IWKUB)!E4Mw zG^e9xQ$&V-Wk)9hm13w9N`N+8JOPJ&iag@@Ih)dXv7N!zg&j{80MNTQI{I&WX6NQQ zJ2?g1#C-6vZ2Dg^_#MSoLZ`!~b9Q;#F0qwNV}_eVOaKj>^{s#&kX|Njf^f6@ryA4T@a7p+S-~9I?$xV6Hfe@` z=-^^7K^ki5T%cO=;MGq&g?F}a>Tq!DV|+eYQh%g|Id0qH*E(46xE_r!m_|##09>~2 z!7m+90K3|~3~~}c*@**>Rhs5z1GMT}%1<=F(vcyBT`5ZqYx)owJrvt1JFy=|Wf!A4 z0*fj6SU)M9;oP8H4DMn3jN~g`zK@H*ZWW#+S8v}xJsmEsx3Zzx+~k#)GV^I+5qmtg z%BsOAnQ52HXL~imG2Nemb0`&m|%u&9{ zgJZ0!<*W(g1g9S$o1@|OBG8WVWQz?;Yx3N@((Jz|f%TF#t-Zayl)&>03f`!_s2>Pd zDpArRsh&F%V2c4Rk1}7erd_s0U)*$R7m-U)xKJ&zxL5X{?d_AGg3Y9rC8d*{Q;Kd! z>@3ouy)5t|AoPSvAo_)lWB`{=Via>(p~xoOl+W=N&XY=4i5c9r+5VdGnfLQ^beqjU6|PYL2;CPAfYqq0>kgO>@xf&ec_CqqiHa!#a%Zxp-FQn$}Kbio8iEA)jTM8TB+A%^9UG)Las^7 z*-ry}D;4K>m0rVZYe+EB)7W_4-|GAfVbcUk7T>&6V^UohsMjZ+qXr-xds1rsJ7KOi zoiQzuKhffHe(@X^+%;V6ibR8rY|OaE+QAXu1!V!nmNZZS2|cYrD7(P2Acc>vZ=N-` z2&E={NZ}Df5&Ho!Us%??szA#k8&a8)B6Q$fs9m4bcncIlQ(+)q3S`r`3st8{WLtxh z>99d1QTv-GLYRY2qVqQ|FOU-R^F}R*2i}Nip!*Ed;mO+S^UAPjbzi=rJH(L;Wcz32 z2hAP6rRqqaR7B{luO=aybE;+v(}1CS2QA5RqTm6a-&Bs+2FLn)aMy&Hqj(KD}{V6kHP>YQ!p&IHEO zXlI;G1J=387xOFHy~hzrmQ0|GrI06Aqv<_6X@7OA|8j0LXcb#7$~ZaZy$BeghBA^O z0ZF$RWPt&7vw!!wd0pDY?e`LoZ>eYAa$#JECza;FU!<;gNq#+ke0nN>o8cq(t9{q2 z670{NT^ZK*zYi~r(;JR$GtLEc(SnVQ^nh#-xj)@3`eU%0sf0~WG zpU}xV)7|(3OF=9@=#@@}-i@(*lg6UX^$!coP^k4jOG%xPXWEl#J}I*vn| z`si>enJC zlMkg&njwh}L+vy^k*KES2TaEWW&o{e+^A7)81|$oG0taG1Ai})-o=uLp=-m=Qb6*` zBGt3uZ`2gh{DR>*j$mvu&o5t0m}>6g>e};T&ZvRLvpLDsVcOIMV(K$&+G3&3psk&X z+AD1wPnF0;xC(m7$PK6iAb>pp=&$K;jhjIvm}^iSp^|%xhcAbN6D`oZ6+cQ1=?R67 zg$@E>03b1BL}Qt$RS-TlhVz51*$6|5{(Z5vEhugKJZ<|d@N)iC8DG^kv}F@_kpR+{iZG4`^>#u zf%aIY8De56)h8qrWVjoa$(iiN{<zM->!Z4?p|hld^XRpchKbZX=%*G z;xi(@kDpCH(FdR6zsNKRopXLaWS6alXWe{kfwwXItPH02kI~_Yy{Mr0MCKvA^wBaO z4_pEA$c><=m2+IDqPTedKu#WMQltT_8^_1QfrFD|O+k8HEoyQIxe~P#F)rduF;L=2 zGTr|W^_Edl{qOhp2Nk5d8>G8Cq#LDEy1S$Yq+2?Nj-k7xhVBxOE@_a?q5U7fzjd$s z36BnN&RXxhuDxG75jZu{bfKc~p@sl4y!E>NK1Effh2KhZI`m2%Jpl~2%Ku@wD^sr*(jf^lko_PG0rJL%$Z^LbNW# z>QE_oi4tAvF~@yOHKG=S?De)0+n6lv{@FCAwp({^d^-I{Yid6y-iy(QmT>Vpkr}6n zpWyzn6rh&#^Y`fyNo^nB_?T)BBI>kHiah3{t4i=ilQIywGR=Om=bOt&99VNj=PW zROyRAurUP@Oqcb^zn=MiKXI5pKHC4bY-%yD_G13Pp7cy}L^#W6^PmE6_9Qm4AHCT> zj~#~ky1Mfw(C71hWyDwLtA7sg7fl1lKb%XWhQqA~(ap29Seg;m8WU`4S`D9^wA;R( zEUt>1=LH~jw~`G2P7b8j=K+7pf$G9qbw1M!U{DDu=?WQ=5J)T_}OYEw8!2 zOnJKGTz%yezn`?Yo#Qd(IGnS)4WtHWR8r+YfrIG5P`;ef56#@QR%FZ}&clt2pq@JW{zG{&;XeNTy z@QbG0w+-~K@*)B@D4J1CRF9{BH#HTn4nT5z{bSVCN;f zuXwi=x+6pu(QKwy~y#-wt^5nBD9DN-mmAieIHO}0t?|-@ZgwTTyu!j zpNRKW4)bNWIKf7FJIQ?f{D8Y>x|dF*^Pr(^x6TlzyJ=*$vjsN}!_d`Z+#(bG*oEoc5GT@f)eG>*+soliOLv^G7}&E+KcFWobm{5ZOOX@i6Sn7z(TbQ)gGJiB4JUZU$pHh8CTsB@L-Ed#hz0)=D&@--X0M!i-0e5VjN?b$4aNuA7cdAMN z<`ZUSgtdzDrGH5y5LswRYlAI|kzlj}h?mbU>g}g+?BD%7+ClhANO|kr%c~x`26cJ{ zY;}ztQ~O(DFW3Rj!vEy- z!qjKDAbLR%eIm3q3flO*VSe=G{zLOE%mNU&#fyVa2LVwi2cdEl^f+VxpU>$KPF+lH zOoK&F!&?o1!!=_ zazZ?JP^9hKLJwsk@#-&c^b81L>F|s^tvnv8TvE(&28AkiuCZNs0eproq=$z$eB@QB zbaYmO#+HsHkB@rgEe^YJ|M(u_Ggf*!TGk}pe}KXT=x}z89oc8m4Voq1-hRa=AgFgF zacbMT7aVr{Ujd|(3V{mw<9-7O+gj$D~7R>H>T4X zi}oi;@eL@fmel1>K7 z`bt^CpMt7^W0^|Xtql5_IvK}R$*VGWU5;mlKlXcx0~v2!BCA35gQcRfT(q-JDI!%322|2e1^Nsi~z&)#mn0VDw{h?y~n!1_pb-nvqW^VoXB;Jj`HdzJQ_k29fXnt_q zn+~Bv5#x%)IYi}IB(u4N2O0kcUWd~5R{cAh`s*vt?q*=5KzmE#0yc^gjA5}gvdnd^ z(WZ&_6k5oi;_=9Nu!!~7oBSdWL~*&>ClO7{)QslfoG7vDx*p`I!+C?(p9qwmyaVtr zyP+8Oe*;JWu*J77>4K)wbn5U~E^m>@H{S4kw4oa#JbdDv-`OH4BJ| zZ9P9{_nLf5!5(@|>%Es|F5y6Z3&1NJNjOtGj;H^G{tpR|?X`2de(m*JX9M(n#doVtEk2WM@A)|pa^(_#Oe}j4`H%5PHaSm6v>{%u~|~v484tb&0Hzh-?U@(QGldedti|BGkoVE>H^{+?s$GvJfDgP3~Zu^zUH( zzM46hYg`;(NjAs~mjR(D(x_44?{-R=&(v>-6h|~C+MwC8Nt;!oJ6a*f>QFrHOMv`! zh0N1#_LfYDsr5I&Ds8XENGO0VUEYU*8^F-$c`MgcQ*)S|``eiJXZjB(fpjI^PN(qd$SkQwvqHgT-@u90j6nTH6hc4Y>rw zEq^&+g<;j0e`X@|{ys!Wt1x>)D4__rjBJ|^vnjZR#OeMtGx{*v$7BswKz;{(#QE;t zf!3e0)l=&3qjC-e9X|*bSz!$Sbg1<^ao#UWbHGL_lIM!0J8+Xf*p7ZZ;@sL3RNyF9 z=h8LcDaYqQKvJM4l1Y-tKa9t2&k;KvBI=JJkk--h=zx=`s#A$Fr$bCFOvxhq?rh=q z_u`=4^!e+Ab+Uzp78f_>CRzAg zGQlZyY#@d_KxvaYK;-+w8O>f{H6jjXc#x%D z1|*D7e7>-Ghyzu5&L6OGH!fQM0KJfqw<6O!H#aFj*De%OlPUE<+6gE)XL5&No~LaT z2&taeaj2f>KODUc#ZkMJ!gUhdU`h5Rj{G*I>>D8$-utKt z3cDgfj@FIjAKr1PM1{YVJ;i^qN?uUOBe4FEn*)Cn&M5fl^8~o3n1YyyR{90bv}3St zc}!h6`_Zrky9poH4IvFFoDpc^{YMpL{u~f*S}D`LUb0F8U{wICwW8hCh6My5Sr;_X zpuUmKVSBNP2`H5-65KwDnzm$yWbl{aswl%j5|<@LQ(Sg;yxLK-ScN9p(ig=D_&MJi ze~{)YYOu77->0HfQ4iiqFfEm7T?VY}(|amCjT(7Q?2rVUU6HrrxXWJ^?2~Bd4q=2F zO@utFH!GlAaty(CR5%GS>urH$L4H&?#bjwLiSAE7ZMpw={D2aKGD9|RvD zD}V57;?Rm_K)moHLzpCaHcHp_8-TIPK6`H$6;Dy;u)dA^Sd^%sC~}1)#QLlqBhfLg zXFh^K&siN50%qP3I2+e1IA>aO{gOjA-Wq4WQTIAMQu=E#Z>O~(Y{4yiqrhFsH^S@c z^G4F@<1O1TmWHKJSO26ySAVav?|Yw^#aq9$qX3H|vZkq7VS%ajBeFkVCAy3TuGb7| zNxJ8~A4DH;AMdF$R!?^hK)FSuP)L~dB{%RmpwGn#6`)Q3RIVMf>*kY^QtX=a&@w7u zLdDn1ki@Rdh|3Isu>m;^>g7u$N3{GLZqS?RD2=z`^}Q9Jt60>_wJ+VsB@vToXi-1( z2}q9NR^z%>5Xw-7$(3ssY1eA7;MKE`pvh+@L9n=@hT%|_U3n1Hm_-5ym{3{@Czp;TK{MJG+)2+7cUj%J zFrtYihn0BFO&V(0N}Zc6Eg~FG6c(+%k)JTGP93W?t!#+iubgrt<+MUIOn`MTnWBm8 zmK){B80*>rXdG-6aji~?0=VfZv;P1!@bH`JkFgjj#f+|mmaFho zz%IN=_K^LB-lm$709AzE&)?ZCn`#H7V)FshR|Ti} zx6V0pu*Ea$S^UfJY^Wci+7B6<7s8)Af9qrF4_AFrW?<32Wcc{V;-g1?tIc~F+Zv~^ zT1%%^aq&5|p>1oKO*nN?xpHm?d-;<5W!>O@m6&NyQu0$g?2$b z&QP^zabhg^FJndn8(F^r?K?XLihsc8?Jc9IR2dBEG$v|Q)NL9n)Noz99(BQO7mW*> z>78Du#3x~R^0VFL)e#NG4hH35d7MxzuFFf7yk@(Jvn-RqxgD2|5x_(TTM)gWFE=M$Yvb)^b8GmFD@?lu|UqN)Bozz&xx4Zo-nLi*yWv93+h&5fPm`0`1p^c^<{XX>Mu_W zX0WDJpKX?`0EFf|ip5SNMAL%%4SlqLwzL{hQg-uA1%|OLtg&o98!fF|Ez6m=z)_HvK3y(y5e z1Nb3UC7Lu*X1hvkU`DyP6j>!|JY2Wmf|MRZhU*3oQ5gRBa#vP($yoY7-!yG!HF1WcDN~yKk`4|4 z<^I5$hr04xMV}zcxK@S6<#c)wlGMKz_n^$cOxQp?Dll)3i3!a_7$b?Nm$I1(@3l65 zlv)FkOL}8Kg{Fq~r;xC!r)ndQgSuj$w5w3Xx@%&NqUa%drxtZEEe!)jj30?%!c zn@61ft6%SGI0F_RKLveiJNI%;SJ}AZ++OxoWGu9ye$=NKy(0qx=5|k4UWLq99`Ax0 z8E0T7vNxbsT$b)|{=DG4mCyEZ>eT?-q)YFn3q;+zI@IrYEQTkx2kmCx=X*BiCffoh zK2UhYK(+q$z^5g$6Wsl(SeeMz8t<>SUnxpM+FX7hcjC+d;99Qt z`ibKyYOn7b0ojK3l0L2gWE+Cx>Us*eRPm+omEeRcvm#(_B0^Qydk0|u%VlI?dqG2u z*+2Ila&Lwi-yp0zMaULOw5|@qK(joS0;JIBn=U96qFq_v(Ghdpo}HKH?&A}M8SK^u zJYa;%-f1;C5rBmgmx+^5QK4ID0 zMScuI>+9p_`H5|+?2vHFtBTkEan?rZJ4q(tDh3PBo8hDkNh<<{2BPnK0)L%K^IQUO zuD8|QJdjRXSdyfvEwoElO3Ajeh5+L@|2YW&FNhQ+7y`pZxg=aBF^Nrv-pAIR!Bs$|7>6 z(LBOY=2+s#-G%F)1fa3^mjZiLwpfb%G02NtF`a%sue{}&()AE-dBF}ChT_Ew^L59uo6*@ zRqPG~9~wv<&Dz~P z)9*b@cYVE3|8iujC@Dd9?qJtV{_s6+c4V{N*kf?-A2$F*Y>9q$-q6-4JIEa+nO77; zp3WL(idXLl#A6@Ao=_3q22=LHKtP4>xX`&--_p+ypdJ3`>;wY5{++z95kkEi51;rF zq20Hs=Lg^US+k%DH(kuLFh07k0cT6%!(WE6H451^h86ya=)@Vr2@mGygE`azAs~Zl zf;u(rePS`cGH^YfssXSi!Qdb{Nus%_f+ot?hS5$@doP64wdv@LSwA@T%~4hRzn*?kJ30;BLLrzsd5Dku~Bqa4s^F6 z)y=;|z}DlHG>W-=#85TGDpO9Lk6|QAt@e zGw)0n{(?Q}u=9)*9!;Ypzk=;RnH!Y9A0bk>gOu;eCt8lsY58-MX~f%tTF$AexSVPP z&H|OQAbO9n^-HT1@__rJNOtU~SwWo5tX&Ir4(KvIQ&OumwhSupJ-G!^w%L_tulMN9 z0f9oC*~sBXSoD}Oj0#MN>R3a|G~U35F{lDehEct7x>+r^L3oz&UW}s4HZ>Y`EZHn5 zJ0Si0&IV{zm1=z1mH_kyHt}0=`N(aa7)^3#Zc+t((5Rlq)i!dCpJ0<#={?sNZ2tDD zhU>xq@$21-<2Vi=2`%Sr?LY3s@QWmxft}r;v$tk^xfG@vvZQ{Tf_UBY7M?raZZ>Z= zK4mm{d9H({H}m#ya$=z8Q-CobqK)kDQY63Z+VmlvP0}^mZwqDuD6r6@3R5c6 zrcD5>s*N^*G9c*K3zjNZFX(KF)Tqdx&G@lNi|8-(l3>M887=8nQ*0D$H(LC8M6Enc zPgf73J5p2#Sc}KKriDnq(Aj+W1pVCC!s@?i%|VDcd^ukKtgpH`I4I-kEpX zIhz3%5TbaX(o4yT+M0%LYs^0LOQ;nk>D5*YYsMu0t6ZoI@Acx7YXG?u*>{+ zfMx^;h*qTDTWT_-9PgsY^jQ2Z1-igRz`H8jd^N8z;3SyLhykbE{p=fKuWxG}-rh!D zKTAlCA@K#}F@*Ip6wIr%+0pZ?Bpuh7VEPiOW-6AbmKe&~C2vq#91a7G>J7?*WrZJ3 z^oA|Cao-gZ@JI2|%e0RLzAND>08#g*2^?2ALNB3K^pF{4nHNweI8#ZCS0v3%5miiyM!(3#({gIyhCIZn%7#b@tDaWdGLwR46 zvfdK(SJ&75kZv=SW$GP?C;INlcL9Kn01As_5v{F%7fja9an6sN3rA_V;XPH(yu(siW_DcI#pr z*j5-obzQnq^Lujvs$mXya3{sS=d(F==*O|p-uzFM^G{KS%vuDK@wlDb+J89hWAK}! zLk{$p(f~$GzJ_|>$E8zBGYXG=7_ZZtV&!rj)G>IJ#g;l2lyR;Z1<9pTtgpfYMc)88 zWhW6QGPa^mvfB*JLDmu^$TAaADq#St$S6XsREh8(Nb+YqQqjA)&pSv#jh@SCQMQP) z48@zLvp>$R)*HzIseQN@1DXQ%+&Qc<1syW__$UM@_I4r;LVGw+Nr$QOrgwan;qO{V zw!LIJB@X|mFBK=Ai$_sSkBSg+b#^wtYaG40d0hV5xaXL$Zq;5^&h@l>@>-f+D!!;+ zvh`ynB`~G3CAp@oNd~h7cxUqZWaMzxg$hg1Ma_VaJLW?1V~;y@;LIl784&a zk+Cy#J`KBapyF^oSodqPZ3lg9B-TV7sRcGmJ#X<&f=jq@j7Q0dB@BMv1 zuI=(AeSThxT+IKTX|@&6^*1#1^+#X;3Z8RE;9{|`IrQcDf6+MRGLf0`o7ypd1ZEe> zelvHHzeCOa49y2jl(J@pp<<>^@6r3KbHj zR2&L(xSJHzJ7P+BqXh5S0(~?AlnZv=X)4em1RA*2Dk+```j|3Qray=A-nQuA8k2ha zf7RU`f1)Kn!PomTwzWj3NW!38j6I+h8XE!NWJ1tF*fB()0lM4c{n$^dGRR>p2aXji zJbE=t0_aGxqAcNS26+Bo#etrD%s-l{J*}LWbwSuiTa?ha*=R~c`F^+(!--zVAi?@)cepo&`9D_jpseD4m>-mQF(ch?F?7TCfuurR`Oh2rx;`$TAXJq%?|cmla9d1q~?$%czDx!uc=TV>((=mxz-RPtCS5vtz} zDn19-u(}6~t;wFpx3P)`gklySf#-y6UQ)9CRGSIanKSEW2e`Sc6%G*4w(v>z(S+G|@;Pz`?^UE<7r{d)trqn%AvC7a zhwr7;l#?K#iNH;!&4#YYmr*^)5s5`AKUH=tMIe?dPb8Ko7TYNMJ?R(eSV`VH13cR- z>Jn+hpdA5*5~EM&b)B6Py}fWX$FKW^?BdtU{|Ybu4v3ddh0Y+BN4B-ZIatkVoNc=Q z!d|XFUx!GCQ@iqA;38l0V$`9Z5bdKF2kJs-&kyD^%WSKy7B0Vuqn+0^>$$wm7U03h zbeqY4Ni)~s}=1whFdD9;{kPh_MyNxaD9jGxR5v1Jq6S|rKbc+ih4heE5*SX)X#Dm_u zrB+vC=h|b3GXf``?sJ?lK;iPOA8oW-Wa6?mPwW2J`s?eXcjhpRK-YM5DV;6!0 zI2Fq*MEF3peg|*aI!)k*{`X1` z_{uckD^dJkTr%GaYiSdZIcRVpo2}$bSV2zev{CN2d$wvZVgTAxShtwb_VKiix65Y$ zEVHG@sAzS><@r))XjdASo;Pggt57FILv z6rBv{o%H^?@6~|diZzbN^a;ZwY|qRIJAhicRcqiXm#OE=7fW@ov}GZjU7$Lzp?r|rZBk_}6{|n%zkMT~B+?yu|I`>cXCI~++-YuN@C|c$u0}DLDMxfBmC3h1CAkuS zwIao}P$wd^SpL?1BW02NXjhV`Ux38`|B|>nYC9sH`yL=@7Qb7>&7iZ%X=aTPAdrU- zQ5yqf^;+-U9^ml_(i!lT2xaMfBTVIM7mH?WI?0bZfC(1z9^*nJes1nx+6-2A!J+Wv z>cam-N*r1>9CLN&tW;vMHm$cV#X zt^=1RoOI)JiBOPPQSNXC58R4W}xQ(^>TBzJ5>=3HRZx z?b=ObAC^5z^t2}npzjy1(@#`6lP$ea(Ru0drkZ3%uZH5A1r6nU* zMgo?KRP1B$an;FbU0atjuQPxuBhB-&X^BY54qgBH4~p|gl21amMBVJGdoobwSlXy+ z(B=EdwGH9j{R4?9?;x$RCqMD7Vi8C|LBI8li#tVIIIpgUYC|&S?u&q^C{ctEa_0FI z_PZ~4d{-=*j(;rUQ&VXd&Q9xES_kiB!+a&MQ6sVqZwTXL9a!Ya&zDZBmVYtx?YZTy zj1kB9`Bj}dHPj~+?JDZ?gL28dfG*+F@kMRkp1|6bZk3=W{uqmcV?$l*_{tqJSP9+g zkGtE-*0L*!(mVe)=c|W+g>=PH(*kW%OHjysj^hF!e%U_Wmd(hePyCQdUNBBWH-g_W z-?$doZMg*cqvr}pr$RrkZ__I<`i1KI_WM-C4M;*PefaCiqW{w}m#8WLhT_Y0HW(&Z zm>$hoJ#c~qE|vyv8V9lim(~L^X=Bm5hFx!;JJJbXK1R&{X1v#%(zy)i$}V5$%5UfL zVhr3KeKYnqNTfHB+)fzjxSw)alW*rH&fHsGn-+y^pPE2)U*Yr5B5$e-ev#|@iDQA> z1!44m9kK#IBpW4Xr4wta-DvK@e?^?z#XDkjRuNHm_-4Eu6>rEW=dsE~q#~&Vb&|bQ z9E6pOORx@ISNb&VfI|A;SMv$+_yTUX8QqNVp?aS0@O>j!M-as#+;;`L+jQHbwtcp9 z#RLNKsYuY=>bk0`9L4zIU<7ex!LI0se-6ci=+_a-#5V@{CbC0^rOC0z$w4$#tH&=$yU^;Om2X zY!vUi#$^(ET>MEmn4eX-^=gOSIJtMOOJ~2(MrN+QE$4le9w3Wpm{Z=QL!_L~YFX&~ znRTEzGn=J(Z1BXP_dO_R!K0au@BllvTHMco3p63<{(5MGW}B2W09On`c9L%UuZ6XN zw55PXo{|rx><{(cDK9$4@rl#YFJw-5{sR3_x84CBv_f4R=_Gk1v&MH1Xo^a!Cl2hF z-t5C0S%qnzTw*;)bN&PTujUF#`o)C^z+vh5cul;jvD;B&Qf^81V{huN=vzi!QwAhS zl@uWcceu*Vsk<}ecnp-2T4wDrEI?rGJK9tK4{Bdeqez7%=n)XMwv7*7trS^fb`Kmk zE6+~w`oILQGKICwmq@MfZkEL?Ev>Aks;;$T1Sq&C2~!jEjxJNBHUwEB0nLlSXlWvw z(nt;C!cOSeidAaP*N86O!F_ebF4SE6H<>v_x*wWD>g*!>5K10}Vo+k*?@Tr|_4t z*1G)ovOCk#>njTRJ){fiBmDG-U@%KpPrfpgjctdus=lsiz>M|Nlp@DXLN=Z{<2Of% zDqOX~s1g+>#lto1wp(8G^F_fmJ{Orh)lxZ3*;?ZZam?$dcz$jyzQ8kzboUK<-xJXN zjt(eBj`P$K-kkl|>||z?-&-H&oSoRut-t=8?`rq1v-!emCL6N+Pv1~=vZk}RRq?L% z6DSv!&at=QY+fw)iyJYDXpOLRxqXUVECo1V-96`5VDq~3`18}1=R_@QH(H}-E~~qk z2!KXJO~HPb2;s+Wil93U%3s$pZ2d4*`{Dc4o7Y#7-OT`Hfp$-|g*PRCgwk52#hHE* z>jIHaPV9RXWUc{tt}BQ@x9RFeWUIk3M6bl~OW)*tWQyD({~;>wsZo~7tRK$hHudlG zPj3O+T;DC_CuoywTT2jM1y2O}qNs#d@gb_ZQ#;U*O&}mp^NfX1tG+cGqqR0~ zzvlLN0#Hu?bzuX}r?(<{xt-Alv%)Ns#a7rTO=4Q(|(Aj3*xj_fEL%Rwn{u|ncLu>bJm{&V% zvM)|C<~ws&VX{_wai_WBYW7v0R~7kh-p>qiAKp!Qzj&Z?p z?)=n9X1S1uUw-YMas0N$kGQ#VqSIm|L1T2l1G){dsXe@|X)lUds` z^P@z+b>xYvI(TiiPAJliQZLKk$DEWbt!r6mjVtrf9aSpFWCrw3U}WS`XIjuDJK*2S z)5Q((*l9J-&o>(5P*bIdCXGlaQ*xhtm-wgrre{< zoo{x{f{NIm&_n1^J6PAOXzd}WtfX$yYMyzfsCP$f$}Gu(D#Bal|aYT z<~`nxswTblXrwn8j-kr)yrvn9FKUDqBwX~IFAIDx3kW%Pd@;k7sjdU!O*tbd&fWBE*uAp z-Tqw;q);eA0tD#=`elR4OrUeG>Zn7h$dzW2w@_elq{?$A!oA<%RQBz>A((k~!eQ;y z5X}8?bS89y0U&y|euFo`D>+f#6#gbW=SAPi$vW$SX5Mloq<*=5ch1?9p%Sb2R{w!_ zIcsNdYiF&jGl-Lwa%7R9WwWu3IKKmOKIac{ww+eqRJY|co@*7;pX28y?0|`82?gT1 zX3NoRilHW*?<|jk*SSGD+I-o2WM_r9q(ctLH1Rng3vx~dfBI|*+B2OW!fV|$KUEt^w9aptxaL)?Yj)eB2{hwb%# zJb0dOZ6X2L@%YKX7d6qR9UZpyFczPF5|UQG9JJbR?dpazS>qo?^P27Z@kErzpxB!C^OU58uY*N2aIkW^d-pcoxUr>9 zQsnR;+<+k+b*T8p4{6;u`!hegAPRv#hA{7tARwubnksStart2aay{ zZ`NI%5E&e=0!-&}WR7>JJ12TVFDa_*rl6wnz8H8T|6BN4=tFQbe=X5Xhvs;+tY|T= z?d$*tVew7G&r^B_hh{zBhv;-;(cQtKVHT(!qhE`{VvIpkSIO26H7)WQQe71p6_WW4dcRJc#ysgw zK_Kwn0lUxj9svvJ)V%L^(PHR7U_r}w0d5D2v7^jWSQHtkYiW8$bSYH^5 zi1)>}%uJ$%g$3{HeKbYJtjYbFOsK$uhZo28>m$e1x@ETT`+KBd%ItTV5Cv=D(3)Cq zLp>tL-Tj)Q)$zrfuhx(F)kjKaMpJ0u88)10qbh^nlUH9~qFuy^S&*$?5PA?Uxbh6# z)a=DJtW}J&slHJFIcX6Xbe|e|anqaWMi%c>x-P;U=<3I6U9CSpZ8 zxKpR&(XUUWyX=U=F%q_e9IA(2tHrKhSwX9cnn-GRgp|?15eoF9TVK>i(H8x;Mr?Q9 zuU0*t!nDzveC9fq=Rw*Dbm68V7E9+dr+2#Rq!<1L)7Si>`lq4lh_}(y$Ri8FZ)a(M zD&o)MuScG~4o#0DeseoVeF@dy3$)S7bV3}$)5h>WC?$n%<$tPOEL}Zn{E-HNwA2X= z5ZY++nwr!(mve@10j{m%xzjrvN0_eu@ip*2`V{j_QRA{8hGY64bND~HW*g5P&m37b zGLd%^*Zm;ZsC}nA|E5XX=4nR_SV6*X{Pb2@S{e@m?>na-Jrx->e=%bs#MUSzr)vM`KIz)@RuCc%t@(FC%p}Q4tR)N6bMqv1ZKAv2^e?&Ws2m@wvnByP zoug7?PDVrsT$fS2QSJ1seqL$q6H663hpl2!MVTgGgctcD<@*47e4#ovkb(xx>@9=g zg0u~@M^X1p7GpYdyFB7ZrQ)Do^u3Xe)8_;Jj`Mju^2!MkGZX4{oe6bjNTpjHah&DK3oVL07NLId;!1EE`8*}-#LNudP~KdA zMv0UwK5}o`fEaNaW~;4r;9s6T@k0Mr+d^U0g3u?mxcuSQ6`FoD z2(zP;QRrsJ>hGt#iOVh^BN`@~_D~xdh>2LIR ztDP#*i&GEZ_z&T&OsjbQ=DI8`>pmoWt&99}G)HLDhoip~243q_hu}gSyq;+2ikGvC zIC*7AhpVcp>Yz|n%y<4*qxAlNpEjpsq0@Qd){c&GZEb?Uv<2Wp=C-!BO`RbfiC}Q% zT{m-0x2Tu+@I`;DxwyClW+*J`ufj5$f01+T@L+J<+P43P*7#V{789TMI;`mKfibmq z1YgVzls5y#FVCHK8ZVdj$c%K0YSsO#c+;;-NM^tE$Gjcj1mbsVTxZ3&I%7HSSIm)f z7q=tGB&}}dvt%|84L(Dhy0v%DD9#V)5R=^!(D$BfQ;BW?k*V!k5jJ|cj=E$H>x+pp zv_LwWix6`2$T@D$8E=f?^C)Cm=8{uNb}gg zm@rpFLQXQV35S-ec6G+z2EZ(O7TL|VrMdaZKG!axiBqYl1jL8Fa+p?w>j<~j2z)#dmjuLVC<)!aF zkC5quPvOgK)0UGC8aN2Z$<8!p%4ycFNk5P2tDJ^Je0g^-gCXgv+`OVvCD}wKYjo3W zTfY~LVcmhxY@}Ku2SL|qIzy`?)iyrw%}=h-!Bbc03x@{9czSAW_h&oOdqbZL5TM<7 z**{+LcB{K-rxu^TxbV4k#4NPBn;&lwHD^ljO$^$lA;@3*yPliJH}f|Nq8O9%q2v)o zuOGb+Slfje4rWXo8ZbEuleFm-odbX0-Tih()~N+CzR{6!TVO}*Sbif%WXdcv)*gqe z=p3J!*`P^Z!s(`KhvobNv7fsll~3nj3+5!_4+{u8e=$lv5AY59)a{D^X0dG!asYf322CZ;1DN36?W8z!E)1M3FGo%=r#W1OkFe^s4hz)(mEy{#5w zK3*MoQQ0RJB{z&X7c0By`m}$);Z<{;^QqSLuT{>2Z?Hmml{2cI{`riP*H2sUgKVC# z&Q`LpmROhg@Ohwbs&k-!B3Nhq*BFvPwU~ip-1mxsC{vIq_(w-`vrM_>c5ldg|C<>( zVcE^SJxd^k%9Jn(!U%*3R@KxLHV2euLS?or+j5M`P*jr zU1Oo{7It0sYfV4?xF(LBw4c&H)f*Eou20>N5rLZbi`EX01AZTF`iU!yqW0Vw$#6{_ z+D{LYt-AY({FnBPUZTgWT2?@K{N@+#RtB`{@Nd8{+s+#NRvqq05lW{5InI>Su=WDj8?fh)}RGEC?%e!?Hy!D{oqg@-Soy8&l zQ;7ES?nBagDL*jG$*+6`w{Cr-jD$XEW||5RauIqUYGx4?Uz24;C@6ZF*fg8tFPa&# z^j}%Y2bOW;d{;zzb4AqbLq716thmYnBJxGv=UKmgqXtu2tHKh|Bbp1z)+}=F`N6#3 zQ+R*!PeN@T%%`2OZlmTW8AGfBVlx$DgWo_suSuzg9}+W|a~dCwJ*|PPKm#XVE0`KS z*=z)L$<^Ty=kz3wgvyv)xBwy2NL_x)g;kyRV4M?4&iZ{M}cnmYG zp37IyD;4CGNzqG#{nF@$lK=V+MUlgs_Sxy&2ire~IjC#`+wIB<9SO-6mDOXre)OOn z^8%&a^%9zaN6vUafOmjG&)T!WyrEj6#>1nWc@@+s0vm>9h8SgBsxvMYY0n z{3zJ;1$QNmKC7g9+IKlZ;$dUk^{^*D^-)@L`9a$|lb`wMsYLPxg$w zQ@3;3k>OS*ksoS%caua;8(o8Yt6rIHT*DKn%J5_@2CJREb7nrx@JD<)))ByV=yEQb zD(uDPveWCxw%#^)5=Q6VoKaq;)V+By2 zM71E2?~_rNmd+ZR7D^URKgT>8g8A1vaow&JbHM!UZm?cQr;T>y#u|l)FGf(?ODnYC zGKDt74=Ok-r+n*9=^UtSK+H4jd~?(BCay4&0~M)1LA8`S3!g>S7}(UvPAL0YG@^2j zI$B!%eBM`c7df|O6M_6h$puK-9=>E$(mh@CM*lja{_BOu&*}*|AAb(%Q_7uLtl>K_ zI?szw_1`}SbnGX@Z^muMG^%|xs#PT@quS{HB)NdxJ?^gNh@vr4kjTpCzJbh_HtN z@$aNH%xcl083$kcQA97&rvFEJS#vQLl zmYSns`cx2{V8m}QMyaAP8+s!4k}|2?*EgGB9TOgBlw97nI$`{y@6Hpvx2N^^;-C;E zFuBm`=+(HMxLHNlX*B2HVPEV^?2+1w>D&L#yn)EgHCt4v5?t4i=vZ-SM{Ch&OkXjf zR?RDKCe=Duv`F!jV&!Xu3Lmqt=h zBv|whD?`irVV4$ALVP0QG}hH39VJLL#rN#=LW(4G6uj- zjC6)q;Gt=i;vbjQGpb+w9LFx7f9{6Ycu=%OR@8>_=(TLWZ5Zh zSF3jlpW9}<>?;TVRM_8irIUd8TzEY-K;PA{);U*%qMYekwV#=8<`{Y)eeQu3Ya6(!ovTlxU!yTG^Z{CaAm4wDL!d=f%%}pY6 zH*!xHwJDpoO9-9Rw>0z;#4mW)Ta@v!xt%_u?eAMPqV4Czt(~oRv^GM{fegjQ#>Tbo zz`XHv_Bp3kugMz#YH2RM6@G*&o-Nhk;4v5 z8IN(xZ5q>-MH1HYeB^zF*ksrKxFXHzb&sW;LSE3h{pA0n>OG*@eBbc#4pnW@MWxhe zt7h$4v#KbH+FPiPJ!95LC`xPAsy%9}5j$q6Su=L*)E+_Xh#>#B-{1NF&iS3gah#L$ z=1tz`dG7nZuIs+;7m3gZI}FgC{2uBTCXd(6SVMawJ4H*aC|)vD&u&PHe7PMIYf)bW z-zSzpEKVfpLW1|ODV~SGWiA~Q?vZiif79+VFWNY%PN~?*{D}_JqkE)hX~x5CbU~j# zBQ+-EK91GSegkaDCWf(HwxgTFo4@-M`wsI%^h>Vp{HBrfiC#N;S!sOO_O|31fyAJ78OP#zPvxQsaY*LxGSbeHa zJ+M}*o`C1}yN?&H73neLk$DiONAIR^7%_H?`u1TH6S|@ToweA14GBmK$v2q%L~9sg zfQLL?IIp%rO_PEASe95-0bGSAn?bBt&D7l5_eYV5HNUVUBCMuKVE*dS&G0hOX2X6; zY((0NGwW29SbpcD2o>$HTg& zTN#-@C0S5H9a!B#YlNasL)-b62;nr(Yc16q--lv$>f6fFb|JB|nvn7R3_5iOk_v4juF0M`J%*>-M>saItxI_HUV3KnCHaoAE8s>|sA~@f9Rr2|%(KKu+Y&IW4 zYx~jgatArt(j9|JdU?@ZdL`yo&^i$EGB(q8Y%KsZAqq)o+{^tWcv8|&Brv|f0fR)Y z_V$l;DRc78b~pKk%~%~BqmMG4D~krg?R3CyR!G!w#8daduPU!*u`IXcg#{os5xd=7 z1+7$&K5Iz}J80mu)us1;Jf7#7+@=WPF#hN?Y*SG?RMFC1F>948@^@7$X=gXkT3!fE zX{S>y&C$;P+D<`gV`Sc_u&L#Dwk!*i^_dG?jFqw)%4=U6s8#eW=lg5zjFQ09~f)W#;oSyX6QL5ANWk&KAJA;Qm|V+LjELM#-`pSRef*J=;M$YBCW68b<@mz0TGSdkvzQ;Ev}^D#<%IAluLmY z^aAhtYKSk4S_Cg@)IVhAFC_mQNe0F-K2&`1;eH1ztNL>auFe7#zVeC@YD0`3<=AaH zUbD4@`KI?8s48S#Z-l<8qxVM-1NQv%d?$kfq0-nQw)`zAV8I{z2CLM*p-Ghc(#giE ztV$X8pvbDX&G|0#O3({no%R;(|A_ykn1bHaa~i3Q|&$ zZASx?fDXW|A(Hw{Xz@^HPr~B@tHut&GP9JSis)N5MH+(uV69eb^iwCCpNmCZeu?$J z)Y6O*aLz0Ihq-pln$NtsYc7!=y*-? zrXBB*KUIl^-%YLcoZM^Deu1w`QE*o3((Lgz6OSOQ0lMkZR;_8rF*BF8v&N?84u9t( zPm$YT#Y$S+r!L%j@(0V1c~hX;b8*Xsn=ULqQqh%uzAY!nJ16daI~poRn!~FKSo4UT zAvu5r6E*jvC2G!Vh92RxA0^GJ;N0_@*og9ZeQ@9b8SU#AJKuF&sf%TR!XJ|1M|?9` z=U?yB9&f4g4jffq(AhuL%+}1ygT&NVtGl=-?oK{pdkj?iFUzQWYR7HF-`h?eN^^VE zz0{v^5wa~CnK!W3SJ*{=p_B}{9pJrLS(9&;)iv2z->^WHeN zY`g#pIY(5Ou*lok92zUt$P{tw;pu|vT6G79h}BvrBy4i+m}|U%*KKvnbE}r#!NCD= znE}N5yQ_?u(th0s2ZGw#+9s{A4Ex$X;Gc+yh$|nDXvr4mmQjKb$t2e3>6s}OGh@pF z^fDlS?@$4<@6R?z(#}8IoMgSxD0(P(y(5w^x8VnQlM82rJ+TG}>fMe+BBwlz=TeZ* zb6tKbIy=kij=RtvDI1{4Zxtc!F@ zfJu0h;rx`GiTyfUA~`C-EWhN3SvbUsMb?rtGp&BTQfM%FHcVe8Vga~X=@m^BepR80 zEdQtzm7QQJBqAc8YsMb_>{>>CnGTnB`8Y=s16e+O@vrP4^3|D_QEO6TW7Xpho+|nL zem_{Ub$B()b-wj^wikVlrZ38tD;89>N&Ip)A(v)y? z_xH%id}Q>wX!`)0Ah+v5^xuBnODHJPSVapLa3pS9+k|uUjp~(&aOf6LsA_N6hF3_b zmC2~O`$iXYI(*e7`<$c0rCB`5p^r^ai7I`ms{MB2G5z2x>jIlY{~^lC&yxxL1Q8#@ z)a=mcf}(L7J9V(RuWu?agg4=?Xb|ubxwL=ja4L8PX$7;SKI0H69$&W|8W0i^QY|y< zk3t*zHGrE8R`gfAo;5I?h+63h-k0hMUEgaI6p$g0>;&(ZODjaRe4~heI_;RnM(U~%}ZVzJ%h3!+d5~9}PSuMmj#;_in$HY$M_wcN$ z`pISNy0o6?$M)Bnymmn>t!(O`Bceys^h|58_|I7lLlee>72TLG6IX+$K7?0MB}56Z zLI$#B&4+Pg%#>4eOnWOF33S(TCaRRe;ng)Y zAre6;$dkFkKZ8|SfU>k*`E{|Q{)u?B-KrvH(>aMCtgtQwrE|}9Nri9vp+MFLSZMAB zlWQD66%;@cF)#<~& z&sVH9D%2~{F45ymYR`~}ul|46dZx={1$I&sJ={bD{Ye7OQt9QY=2cXFeFKq7x7_*uMZLU6a|9%rkm)|0hhjWz^DI zMH_A%|4gfD(0!RKRCzMo^NbVqDD-?yGph$~30RA%C~HR3aD09Eb$L4=&Kiwq1i(^C zpHu%oqvT|A@AC7$<&)sv|z`Be@VX5f+P8*7cs zA+t}J*CYUBA^y!sU^`O`IW6(qEcLEtKKn!PJg-*m-RJRUSXtHEtldvqXr$5-KMhDP zUqf^zx7)PgB5wCV9z5=0wIH*2P>K}!_IF(R$#ctEtsJdy6!IA^nek3Cp|foHC-e$5 zH+1^$uWG&fjt#Ke1x6%ReF}Q5UGk$C{Y!^CWpOk)VO%+a$_0L_BhaYqO^e=7nlFL9 zVp*z+7LKRFY9lK5zofcx0tDXuSD_Ag>1y-~<#4UY%zQ0sB_U%T=Y{EiSA*-P*pC0kvrzF#-XjbpbbT&bVy#UIrE0HBx4XNU}8hWICG^?20WlC%XicheZI zs_EsqTa|*5PIoymtXl)IxBD(L6Lu~*r{PaM%I8nYa4nB~6Au-*XYu}RA>43Q-OwrD z(YuN&u&E_vM}0t1a`yEns6$(Pl9LRvu^$bmNo$cPgC*uuO--q9oCw<0J_L%rI>bZ0 zNpk$j{=SK!dSb{*(%K5z7LorHwUGDH`NsyO!%^Jr z`OgJne@@~zjvg3gB;qHh&4g6&RF`>;;t!nYbhq{}CrnqTm=#Ca81rNAF88YX-yw7hmZ z8dJxp$JXDUv+k>(AjFN13K$ix)X2+$iu(nL*4!3D$v-4pZ9C50V<+N84X54JpEBfBd2)BRanbyOH@M##ggtVZx z)>xWQY1FNL&4aSghAuS9&(V?LOulQ)s~wv!!d zo&Hk1YpVg~o1j$`w5MP<-(c->`pjq6=44E4$R*t`mL$I4L4cgPeV9L7g+6mSoP}r| zlop+z%Xp`sLK}C`y|Q8R5OB->?YFl|k!`U0V;ssh{3aJlE3@mqMp@f*?yOsmsG19({x()EjIPKBs=0 zm~Z?^@?0vhSz7i~LW%7;#zgqumD^>42s6ILRjn`#thx)%?0nc2&l_WQa(bo!=sxC_ zNcg)9=V7gw74%&(AA)_9*q#2Dyyg4$kTjP}Pr`w6n2XuTr}L`*bxaKb3wAjmU4&6x zo`~@v{i}Kp>H3>}Yda(K9sG~~ivUYI9+reTG0E&4oLa@eGrb9gVLX>x`f2CKvSo(^ zbV;*+%K#SLFXdO+*U^)9z6(hULp4c3=hq2uV)(LZ2tAnEQ;k-q{o@FxGB3*R54=gX z!t}!Qz>L=iGnJdWmAifYPbq?2YyRt?D(TyOgvP9NkID@9$FSeRT%UQ(F3TLICNF!p zHG{cnzmWrnycv7#y5*E;D39gF8u=`w9yN=axPYMSl+*XTD3;brTGa2UKMxa$Zc+Q> zH)sL(^vUvFs`rQ%;y7FXkyEk!Dak*T_1t3A?H=v{3L?9%4AS=BAZ975ziD86(LJ?l ze{ORy@4wT*)I&f`2MeU8a~Bu(cHP)NTU-v>JE8Z>@N3qVNm(4HMGspkLKw!nK>A}` z?FBa9!*Cx5_5y8&Mzg-`MD8vYSbF>V@8fVy8O$eZNv=AlKHLQVlPS~k?HRDe&BQd> zYA;@g5|tz3SgG^S!M0jFM!M{vB<#iIX8(6^fbQDjm;_wk+SYSz^Tc}NIQ58FUDHHo zV3M|&mP7?RR7(L%QxcBRS6c}ZOC^P#kA4(R+YgRzif(!oYt-Rva*bVwkdna{q&lipUv%-A=j zwZ-y#q{KquQH!+#M|VbDP}-IT%G${H+OVX8=02(}P&GD{1@lqix^ed& zhZ?~t^*~(BP%Jc4phzk*O$MJSWNn?G=*#7SPE?m~# zIL1=NweSh&D$kyui4KN$KZ`XK=;cuv^5kbZ=wYt#9cH2TYaaNieRr^SAV_Ck#Wtid zFQDzc-WIpT-^*|Jm&S|4t#I0%fh-ry-*aa*V0#>vcVi1w^ERK49x+7vL1w1LBy`(Fvs^$b`=Sh0e zeIEImL|H}xF`f%ZoIc*`ISM0%M@_bxTdcA-8(ltMD7_4+oz&PoBHe%-z(1UUe;&8a zz1ZX1ybEhTn~X6uAlwJ8*xA?3JhFQk(s}Nl(DxRbm2<2icadOmZdq3s&^bCIYiV(O z*T9T)-*|^v=sk>L?)nx7?L7s#x?2@u{d6p+n;HRUV3~l+bfAZGZEdX{b|GN_IygIa zc=YJe2bG&{&B|~iY;&NWEQsw**&Z=zd?2|;oSQ{^bINwHpMeOOlgFBi|&6lWonW`O&52bM9DG1elu|C9PZh{ zpqLb@xxTQM8i%Kt6^O`ZH<;sk^%UuZiN{=z)7fD#Qj0RMTp;%q=XLXWP$qpxQup2{ zD3o9}FwOr_Y5U-AhSd3b_uk8x67shyR?Ngzl6D(Fsb4|9(`G8*Wbm2#rS;Y)D|V`8 zT_~C^YO!JRrEbaF>gx9zYF1D~vmfzC%FZO+@g&*|obo>rJ%_l(jk=|Za}w6>og)_) zHB-fr=?-c$X;u!c5mmBXt$%RCe?%Cb@C?Dch5v|DDm8cuxBzKxSntc%Evw2xJeN4 zZXw}6Z1Hx_Jzr%CCALW?AXT$**-CDbNyZ#dhWg+|>d>aP>j}s^CY{MLwKJmn<-!ea ztAeX;Uw|bKtn#nR$v|au@b`BxW9~m%2SB_A1_yuES_4jpVlE5W02kI`#!lgd3re@z za(*%5?TwlzT~?xCupkx+cJiJL$)iYQiON13ndx3>*6JaiYQcxon*x zCQeKEu!1jYF;SqDL7qE?2yt$MCbO2VD9#yhPDU-N@!sjK_>YpiP0M-vq8s#RG~sG? zVop4<>~V*pOo)+KsIKLwtN!r?QtA&T5B8o84_)XVo`Tmm{j4c95;rJuz)sz zJ*}Faz-sz~`|+%7S740@3cin6RsrRuy(Vo%%J950#6FTeU%Z==mbMIhf6FeHYc3#4^0^kjB!D5WnFN%U zO5Pj`FYdB6c+CkJ`v;KbY|=@ryUXp7%N2_;Y4JZY$2uWv^Y!@bcXz9MPJnq47o?6w zKw;6bw#HUcQu30Qx3VGSUeDocX>_*A5T$XV1EX<6tml=1)l{`tcm#da1vNM1(SrRd z#?183G6JJ9?cm5XGmT^*Toh<-JyRK6!S`CPSjRAkldtduWd~`YM|_#y9Ugmg6FfeH z@?d%^c400p&NoxYEz2wRVcLz|itA3cK?z^x(Q~O%-yUa2x`mJZQ7qF~=3phzcmP9}a<>J8S zUEsYgI`H1#-`6j@T2JW)P^iN+%z0?QSz&-LcP^YnV)_52V=8l3L!`Z9yuAVzZb{zG zrMdVW_{9dbA>-RQco{y=!|@gCl&x`1W|)GnN#AcpRX9cg(oAwJmgg%FTlA(}2- zCr>p?Wy>DPK#HEBD%N^yq)mNIMecR>_`5HNk0>YKcK>&Ij`-a1av~LQ@Uv3Jg$OHu zJ$Q`Dmr56q#4#z@5rm06%94u3{{b%(R327 z_g}q@aGC$;<>jSQSI4KVvU)u-<8vqMdUy_U5RetHybpp8H_~vnI76$uqjMzcv$CvO z{oLDX#qq8XOun$15(H%FMWC(3T+G!p$}ZF^V$#H1q9c*5gh7{oz#_|3{95qAUsF!5 zn_^T7J|D+)R@Mc1HF{Q;>wQ4B z-p8VWz#<1OX<+baaBE9q-Z_SK1Y}wJGLeMGJSGY??F$P=^j&PXL{`rQdzC9Q$_KfX zSIaK4lYM?h!4+b0qfE%@PxAY;-psBwzelZF^!cSfB?ddJ@dM$u?@~{`F3Ttjvi1X} z=!pl!LP9`VLFaTc&!3JPKK@?6f~Z`^WLSI?~#&3fs4^*1zf(eOWhSYgArDf{ISNErFB`- ztFf`M^Lfk#BWdq0$u)ZfEq3y2?#lLZ=>B~86iVYB*9D6x;w6wziM|LIG&59_pZks9i;rO)XD~C zS!(ss`QL4KEtEz@Rp0(#gbSBd9~&`O<-Va3OCPMP4e0U3LHr;}VBEuZRx$$R!sD{y zb-UX6B(>Zjn~O6$<|GX-P*<{4RgsBSc6`ChLQbc$lqUZa7bw=)q`@tJcmqoxoXWdu8Zb*`78|8oRyW& z@jiA|ZX3O@(__&QK%p*Aufe61=&gYJ+%?hi^78IJ^6FG5B*0*1YHDS3j9tp_Bxq;H zOoljFzUtrx$?B0t7!8X@b+k@0-ZYkncXeB~wrpLKuMq{N%H(Sm=!8>rTq*PZp*wIy z<0kL&BXfYKKSEgTfDucpVzn3#XobIS;%`!tLqj`+(V1HfdPeTnyKBYTTpF!ue0DNp z^%M?Y2P5xEF7E8Bi}S0`>`<;hQZZ$tfKkt}sI}&deEO3i=^HbGuEYK^uSzkdyYTfH zCK#?rNiEyG?pxg8+~V^W%coI(P>n?X?oWdIoE=edbIUK)#|G36-_#SZmL$oQNSu9k zINyjz>~V7i$j_tmH67>vQNS|7*L&lbuEa1&FOB3Xg;>pNq70z~k*2tWZy{;?4i{H)QNrNj?@7n1F5wXSk9S=pJR5&KM2Vc)F^^$~ z(a+(32}5UaMGOdX8JVc zf1G5g87}4BXGZiergqKa#=-~ zJdS@5`c#wVKAA-(X8MpiOdinjcjuAGg=hg=ZUgV<(PY{<&vYTfs(iyz1*CnA_vbM} zbL-dPT%V*@R)?(D#YF|piuR9%NLDc2DR(5{LH@O%zu+r~cVq0r^k9HEyz^eeMiDbG zCB3jA*%)DS#cMty8g*xVKShH}>k&>-GhnfCr-m5%+q`XZ@{y{lDxj7b!CI-VPMrX= z{*2Osyc&~p{EScb=~cZ3cWZD_?C!1|GZN#tyHhjv0PROOU5>$nptY0dX#s~WfYdzT zGK;jn#z#8f^EusH{IfCZC`!P#USca1!E^|SC&SXSfULuXD|dqS79WdcyIw?eiklC<%dM%X4Yq zi63=2$t~V_+J{?LD+7WU&P0@WF#u00Ww5^nEIuae1)fkV^|JsBg2aw!hcn$gZtR^l zFQ_A3p!9m0%DytqepX`tISZvyN%Laf7%$!&RrdE>>;{&l>GIL?N?2diLTi-eCA6?3 z;N62If{zU01knIF-YSxE0_(EI4;(R~vy?GxPeh2E8NBhKe#}!f^|%h$rsV%wYFj{R zcjwL>;N5rU8iWMT0LZm<>3^9cb=dmQ2DS?Pgh-%;0H9D}^}!S7AjF^~sRe@Csl-{M zb|THNyF*8{&?bR@@^&fMIS_C3Hjk|Xao4lfWc%YCDD|#C4g;o0D47I^nQUE*I~`;& zAp6jDQT*2b+S_UK^)!E}(Qpz$1Xo2YHgKQwQ4z z>MV7e*YKMi7S}C#KMqLQ&2%M;wFw)MUa4G&XAvAYfw1Vp1J?IPK&EWi!PH9t3Psy} z9_9~SV!n=09xMrHm+E2Ihc~7HDA(zv0{@w9q7Kx`i?}%}b1)l;SvVi&Hl z*bh`S1mG|$*5UF$p3pyE6Kj2{`~Y@Rowa|MMQT1}S1bMxsH>HN`}8S(t;$D)9%}yN z^F*OCn|6r=;$n{f53D_M+Bg~IM9^;wu%;x%3$NniLyCU~0>T-rgM(wO5hZ!>$ZIa; zxDiNd0bm@Ss1joF$DH3B>K>msq3fHL_TE#%pF5hcEWAqBz4!31%)M2HW@sYo%XNmD zL5{9;t${Vt-8i58`VW7RSp(}Nn}nQOItkA{OvljxHx~3)Tgv<)+i&Xk7gx6+AA3qK zp{C1OX$b>71L)eao!nSjJEKdVky>OcL;1F?m4nYrdG_($k+bzSzIS8HLQl*J$371Z zs)>qDZg@#H{CwueR1!l#r4?}Q{Hu_4C(*`^PIis+#;m=qWS4~;$2J_tL?5HNKy-sw z(IHQ;&7l>u^}%w-pGfcdbavS3rWB(W+o9QVui43uW8yCx8!FkmFZeW=VAJnjwJ^%v z`v4b9PD@)K&F400^!O4%$0r~g@Hr&~&`;T=4n9sDG%P5zNl8t8mG{22T9{s8^oeqd zOs%R3UaL5r#19BiRDhO7U+@EUrzdY-%3S%@l*nB7pI(M zIqDjA3wK5fjXe=vS|(`$ZMUpqo&dEN!M3P16bIfK^ji_S38Io||xS7XSc#_pNS zIk5lkcBvJNXE0)3O`X1zu=P{tMm}(2v)f*T*}2l zX!!U7Y-VN+3=9Wi3efgx00vt+IK->lw7DAeBojG~QGuA7&e`LRNW$#1?(-Yn=d%23 z%mUUT;D9N#(Y$-s#lg@q1A9|zDZk=s+~Yu}5vv|+bg6i2WKXa9oZoy%%PE1K;3>G@ z|0k(>K+ZP|zCIEjs=tgfyZ(Avs_8Y}HS)ilEW)AVpiszwIe+%-P8HA|UNVt0VILKZKp_5g zjE2cCi8JjanD(tZnGE4&oz@P;wv~jic75^UYYK70Ku*g42WsXAqY>nd0s4C|2Bw~K zfs5k%H=mckZ@1{U({9+we17(1r8OANN%IY^`y|ha<{K5=`pgvDuvd*>BAiF+u8BhN z9S+?u{yZ2~Wxyo4W_Bl5y$<-=&tj1i_V;@PjDRT_PosBM%D9YBx#Rv!tuJm&DvyQb zZLWi=lTokh>>lGw!)Rrh!`}kwx9$EhD?($ht4lU3#9wB#+!F1?KKE(f3Gdl-EQKDj zUv(NS?tOT5ItOQLYz(}W^dn1vv0n7}zV^#QTCCk{P`2?-`bCZ;!O{Gn!V9SixOR9s zM;jxY_eYeN|NTO63(`RF=*W=tuJ^<9dgB&WPj+U3ZTZcklDwESLY8 z*H+nIR-e}T_g-)6_&m`jC~sSk`k!;YR_FHiOEetwccI1apIjZlI(J8VWiC%GWe6(| zoiRwY>Q_;d68cUi%k~Aaj~}iRbb3WPi5#9(K)*G)4Rvk`qfnicldo4KOFOXxicmlC zV>39f;(7p(^}d67K6P1|N!Liq?OeEiVKj3gPfe(KKzEg2jOI%thrU%opzImP9yfRV zVH(y*Nc#@?b{`PsdI=$YFcBrcn%wTC7-fCQxVn3l`KtcGbp5>lo!B0g{kNQYfj?V# zfp%@Kxkk?j=8Fxb^VtBR3!z$;CG!R3Fob%L+rr{QjKRVn81!qdIuF8ouaFAP}$F^I&Ay^cq{tf%NFirtH0vwYizZ_h$<7gtu7 zku$%IwP<1{BDn~ty}N-7AZ^q|8F`|ef{Im7cODROzTBbv=b_NgOxkBY1Y%pdEU{X4 znXdKp#{cfwo3xABUk|ukKVM;&U0i5s0z&&M%hUON>j&$0&Cy!2^4Ng*!k&UIp0taD zuFz^BUugK!{%XbcI3rl!9XsAz47N#w5ctxm&kwlc;)htzzQUW*b%7C@&WL^>TbZ4m z?S!0M&r(OU;x!}d`E&vn8esF|$ z|JvnZHY01JDkIof{OOTvOVHwQc^4?eTIsf7A?JL~^KtZ(dPL51io#pqSJIJBZpL>y z%zjpGKYQYR*0Ybqc{9acF4`t{R=9pAZT_|wkwa!r<^whxz59HM*g!?ga0W5wz0JZb zVCDYrPCA$`zyG_FxhC%zAe!ZHkiF*g({MO?A&Z*a8T|46lv)KEqy;mgE`z?BA{sfG z0Edd%^@Ot9%Uhk(#e9YFEvqk9Av62^Zt3Yd)T4k$_F>I8;uE{nQTtj0ARIxVP!y9> zQ^5v+%H_v7oqBULegF z!mC{}(XG;8!bT~7tqgy3r`*UTjDFR>NL$-}e1FR0OrA_NwiT!6G0!?>LmN^(6gEDu zeirzbWr1ynnG5~}-(v?yv!{$@!I5=u${|zBZ6keb4%|Vnfd-#Ml`nuMuf>dc^W#aq z$&i;ThS5gzT$C~2fBvtEoRR-d-x;l)*(G6isT)eOuhzW@cVTDl1#nJ7E=x>>?|4qF z;=1KJ#Ld9ux_=xit8$f1vDpsY=?EA?M`-jI5cLMWIHf4w6ohU)5xBQT;+GSVFrV~V#}H@w@v^Uf-p zFWuTd{{Z^O?zaXa4W?c3ts4eJUgpB)J1`R^TAZQczkQT_=_xDQeKdBni{Na))7f1n zC5F$uY=Bo1F8wM|v8=Z`7G2Sbjlj^^o_$-Yo|Y-K3I9}zPr8H>M$_!X0K*d7E)_v9V=d_Yf>|*Q=$u$&&3n=gqXl+ zQh7C^t@S7HR9)R%Cq8AM_-@s(QC_oG(d=LUL}Sscu(*@m9_|LKl%gmZBFf*EyEnwE z5MB(VJA`($FB>|SPsFVYC~R&PGc!}C;cyb4E?pzX z(gh=XMLql^9h-Marm2R}qmJZ5|V-S{&tZ#e#_lTWXqEn%p|hxiLQyWl(^5s(5L?zT9#vE*VYEpfXJx{+7oqi5T>82hXeyk zS)lmyx6HzJ1N;HG4Q{9Gf@6#RdjEo`)^Y zI4mg=+7nb9^|_jVp$VqBzg@OGk-%$;N=kI1>2`|WZY8vR4JiqMEvx_4{8P&_=>(sVo%yY{szPD28RnjkPspJrsaEwauk3 zZUL;J*+n|H{$*;3J}J`y{ixnoS3q*MQ0MH4BxHVNoBT;zGRHF;REpZ}VV`6FfU6T_deKU4^Gs=83yvoRrB^tFX zxG6qm7Bs6on6XodV{pcIt{zEx6`6-oO%ATyUMRO>@V`&CIQ-}J=Js=|^TqG}vdPt^ zvNvDfDMNE4)^R3TawR6%*J_ka=IG7lnBhJYv%bB*!feiXJIzxr`<6UM;#opJAVzm7 z($Oo{^V8u9<=FZ;eyzK?J$cZ-H*7p+$TIIUf$2>&h*9Ga%;#MS1$!dIb6n)?5y4nT z=2i(1+c8JXtOA2d>hwSR{QVgB0ek$R>{0%p8n2V1tG9dnY+9PI zsOVQj9l%P0Y!zC=6k*|?6OoYMro5j($$H+@kUNuP8=Jl&Gn@^IAK4`Vy;bn z%EAOAX~(=B#vJesF5O@>p0@;cSoXXaoogWz7O?*DJ6olRDlIKMDvFDN!Mzj?{?Dnb zcg^}8qDd$Mtw)O(GMFC-FVj9TT%H=<>~wUdwyPztnU$|YXp4$zi;9waNQ8=KUMKmCB4=Y@A?;teJHp5B^>B z5U4N29iAu}zWg{YZo`hm^O}f=y?R|$CM~#s(XFg$5GdhFTc5z1FzaMsoQtgK%viNT zwpO)!CnYsBR|FcFzSuo}H<~aG0u)SWz*WctK&}9dtV9Mi&{*&Muhj(bNV8EiXHk1F zY8EqkCXw{x$EUqDs;rkh5F5L1GfU6*x42f-!?4}1+!V=uhfcO)K0?-Lcj8B}&kWxL zo>ztfh2;h)K3)p543UqxE-?*AE?g(?U*NW%2hZJ&Sh+BZE<_DRer*!FRZXsh4#YZW zDe=7c`@qgO-q|}kD(V3@H|>ufywx>FelK~T?W3bSdU{)qF7k2zkx#IG1J1}JU+KpA z-FNKn`LjC`mfl_F=2J6*rR8x9vD`<#o}!{EFZl``oqoH1x>|q3d5rdt-sh~(Ep#Fn@!)2_?jDR>3!Y6`#-0FO1={#k}ujt zD9O{D8@%xJ27jnO|Rk;Vv%syl@4)h;Oqg!RqLp%!|iFI zOc`ijwGB$KMV<UnO;*^zF1fzZeSRf z1fo34+Z0uCTHQ3c`pDg9`Oh2POF~nhTPA5bsmm|w0Jx`iiwZAeho<>eJWv6p0$wRb zWse4}<4g46fCR$O)HHwISx8nEHJB!rl9BO(vLoZp9Ti_ z@(h?b4%p zt$PiHIPF@Ri|DVHwBbL?UfW&6Unu=0-8iSWks6#6Ww*HjU3N*Aq?3NDO~vua>Al0Ku#?$W4v2uP%n4oT|uQ z+9Hi^dh&(ee%OtZxwYVD#$4^vxU29`dDtt^Ueu%xA9xKfLkQtc;>$hg!p#GVY}Tg{ zn#sJdgtxkEU`bb0H(Isqd#=O+r9Oz2tn|so^16+gET5ydva3r)A^|YDvNJ)N0C5@#uB!T z-FVv3{G~*36Q=>s!+EEskIT-hMuy>9fB-Fw=$_G&B;YOg_@_SU2OP~#hNW^5; zijQVba@z>geDjCsCR~^|mAYB<^X_ENNCN%o?fgF53zWbbTesBDk<;*51g-b3iMh2k zFkONr>lo|uk!)Mg)j*8Ysv0}1yvSOp_Wj`g0uwsa9CZN**QD|K;9(=nNDDv_qZGq5 zVqg1@aKw-<@c6kuNO3`SCj7&*z$c8dWaMYySz*G~w+1YpsWI)qC5I z=Ygg+zG!84`7JnLGN#?|blhoko3SiS(h(HK+%1jhDR0bJQ`Cs(JKhUxm>cRie{hQX z3|@8rpaar^>nMU@^*hZ?Ug@USa?m)-(S(RLd|&|3ZdlSVf_HK6z^Ud_B!1L$uvfW% z#p|cUjqLQ=Shc!W>0F=8=A;Iqnp8L=g|_*lN0QFv+r8o{tSLKGzNn(hA*A2ec?VE2 z?Ne2r#8HUD&tk3RH^=@Db{Ho@04 zt^yteIYwnb_n`ZA+fbM7T|Cf%=(Hq0Sjb@V276~5OPf0Qzz97x_0G07GioW+F8gZ8 z(BWUezKAIGs}0k*ys|1jY9h2i=F8!Ey(}P90L{O~Kpz!0*HTjNzCOy7%co9sMs;m) zH}mEBhakG@CUzT07+^QY(?eVFEw~X|;nRbDKEf(rfHL4m6GJ+UA$0MDRF;d(44Fy)svBx(g$yO;{)lJpdHqVQ!HH6q9G1WUy?S};G~jxx zW_aD?cbBTr_(^G0I;N)uf5Y;$T;QDMl%@z^J4p!)uq^Jb57?2M1GX6XtKF!F@sFW+ z`O3qAZV}LWz1<7cOCPEak!}G=r9>nYX&Abtk?xQfU}zX>zGu$+{{QuT*O?2?IhV}d``J(2_qx|wcYy6B zT}|z)raw%k$3{D4HxCg*{id86oPr~SUm=YHbRhILXrSrD1wyvXvQx0UeDo> zro~);ng-|RK`tQW1@S2;O%Q|zOZ)7*p6{Ch56W+Ap8Ut)O{T(nVXaMkXC#9?0* z`=9o|or;tHO{iD(rR54`lNd=cLen{}c`Temfszr05qM8Iy@Yq?RO-w53sp4eBywp< z=2H?ORyeo)d~WeyvU$Axk?rLz_cG$Q?H5xOak`X&4PB{fy_sJm&R2+nR}Qi6Q=INO z#xvIg!;pCq_V`i;<$P%QjN`t^`eISrnVB>5zpHv@SR;ZP-!5`ouXazGS7oVo^LNUr z@rKN1OGOw}ef|FaPX|yg#|y&*_`Rc}f3Ce;yOBYom{<$A>d85YA1`yC|asW;Gj<#7|_(R=Sw}>lIAUg+^FvP!yW{g zV!^jYl>DHCy^qreR<%?uqO0dH7xp1BNO2m`c;OaWS_onhNQ=GVhkuPuZHg6}B^*f) z*WHB6o};dM&I0B05o0?P&Eyk1|!`|1%#1q?qd^RhDWVU3LrG zEPv4plzY^*d}L!QV49~}L7}2sq(UEN$T$AkiB83*k4N*>;=x?aG+!g|(HBrij97SA zJ7Z#Q`_AS);?)1W$`bdfR4ja|J`gEV{f;Vm_>-;%sL=pu56zfRaK@iB8D}IhHa4-k z*rD?r}%aDHm%FXg6AEknBfN47TKLB$B)9*4?7_eiWW%%gUuTU3` zcz6E{vv4+X04+ET`0io1ZQVjGY;$M!Is)aV23K>gpdiYY*!G%-m0F!Z#I^k;bMc0( zX+eEZLOh`98R{?wHaSQm#Kozu2i|(i61P^e?#pZ!X!2m-VEMp=XqY_wQDZmIWt+{N zt+Q&~m(!}cvc3HPhVkf#>)A8Ry)*FMSWSYW^fE@p)o(-Hk<4}84*`h7FGe?Busu`& zU@Vo)GF9F1N&h$4kGU+pyt0idKzY!JD1!4VKIr+HPTR0R*!@FDy75meebt|#p?lbL z)*`ZfJIaRhfxmA9>@N4q@+X5ebGU1WM68t+Yv*i05Fzg3|D)WTGY&=7{m1GLx_s|` zUjA>f`6d=7eJ&-hvgDT4WhWJ{CYkf_8uq<+_y!SCy9omJ=76sMLx z?DoeuK=Zk0blAU@$TR} zN*PR?+W(z6+G5Bb(R+hh9~>Ms&;9=t^RiZce^i=E=UquplF{mU;_%zsTw%>lpWd;l?(rlTzkDg{@#mhSwxWF3C_so&;%Dj{H~gVdy6?~9&*e#Kv)YugeknA=WpbQW2F zyDiWNY?O95VF?DmXL+KfBN~JhsS*`6=(phmS^$}w{J={4;c>)2^_hQ>$wtm{pPm&y z&{pM31=8R58UkoaPuttzLAfu>jQ}O5}6hXW3CI?9D%Kslhh((L`{|G_=MH~3k zfMJ?=S3OVj^A#_(w?y9tgel=VH|DF41pTJ1AI%3$%UdkhddJf+Ak=^FUN171D`M7h zyOhYoC&#?)*?7%xGql6;m(K*xvk|MN_RCT(Zh8KMa!Qm?(R<-xp49v4xuo^pdm_ij z)Fp-vekMMvC5ELL=AYmJr~cXQl2!nsd3U1))v=>9)3n`S`CxgK{h#?~Z_BpNt7!YOhbZm+`B5s4 ziF)?$XNk7^(KP2JA;oZmngTdhv9EQSxfh%EvaI*483eU?ODskArybgQvr z{qpkGg558l%y?41l<=@RkCGhq3EiqwGO#pKWFg_BBfRSknmMM1VJU%QyJ%W+Z z9^r9;T$8Ur{2REqZO>`^e7txxrxO@;9W_5BbsNB$1=QT9$`V%>8;--(lLmZLbg{Ux zD%K}!v%M`~?eL|hgD@S8DZ2IrGK{<<9sgvGs}nT-*0jnW32T=WarXZ(Y)0uh)WbQm z=S?3HZGf5CJwbzeBbS+(`2YqT9H18TJzd4AFo(@Q6f{4OLZMc>?wZR4bi>mGLHGxt zy*C3QzHS{uRdY7n*O{GiEf^AA?%L zY^FGK42yy>P0rPlSREW7Fr;ntu)M|E@$l5eYP1*Z+Px!D3;fI$Y%$%87Mw&bASRJV z16t-y)3y$!It-*Cj(Zj#-AAVfXMTxE^SWuD6$J#fbllDHetX>ZDS6FHjYQvNhypst ziu61?81Tu$=MFnJ=E^5k=#}^TnfsO@e9pDyuIjxA@P-;i%C+W7zxl6{(8yGKrCm%l z+p$`dvkv`rlxFu!r-wXKZ{KiE3!tYD{0$olGu?M`eDe!71nIfSuYM$FZnn|ZjSB)g z4RuA*Eh&{Hu@;M@r6rJ`d1IWFhgH^6q40E;mM-fpu@!oKnKepXMSLy7C|6z&DH>lu zb}bLDHaq$i{CgB`BPh-{4>+cheoSJI^rSl_aTG4)+Bv`l92}T`!fx-;(=-44dH9+m zGm~_B{`%vf7X17k`1iN9EhOAT_h9*?R zO>ObdDhA7!Ug13`Z>*$VqVc^yOR@6$`bdC$<;1e7!g8U;C2jN^!X8`6f?ted(KpZj z1eSfgk0hj^lZn?44OFNL(R0Ln^<;_Gz2FH}-Q8=^<#>1Gq1+M0axPX7z6?guQ5}%A zvsM!Pz?3axjT%u%D6~(yzQ$PfPI{+_5*soDbyLe=axT^Wt94hy{?$sz5Bf#KIO!28 zA*sX$>otY-Wblw$fF5YljOj{o5oS7fXpJn9@C8>vS9=49;i32MlWq=1}*3|M%>Ti&9f&*fogphX|8v+%)$7>~{fZc-NHV#RwFp zGpbOdUrlnSL80}lo($IgY~y;mZq=?jt?bK^ zBfP2j12!7Eza0p^bo6pe#q_T~Ec^&yOqQ4)BaQb}u_@<$U*10Vz3+Op?RN8Psko)A zPBg%5VePH$wQa!N@Go3ZxY%vv2iVp*SoAcae+C2jXxLvLU`pEyOtJI~`{cfoK{l%xyU_tQD`@rss{s>&J*L3LvHcWNPy-nOzeitS% zy{tR_tqveB5`E2zp9d-&rT5;iOpCRYUJUOx#7dtP_6`erD3EcjnWY%5oUm>{eo2&W z=uQYSbKq?gCigd*+Yy9*d{?d5%=BEq<;w(6^i-&iy95)bXoO(!p!H*eUnfkRvAs7G z1!mle>Ic`CoKroV#BdGO$5m;LU#0kKQdaY%VQCb86q*{i-sE#X=EM(8ynrT0($PYy z3K`Gd4mb7>^qd|mOUV6j+cAqZXrF1XpuUQ9J>eM(ViJu@#FH=IX!!es|lA!`JU+bxR{_Ri}Pc@``Ce>=xwQwSrrnB zrF%L55ZABJxW&%xU$#cKx&#MdFga*RJ?ff&fA`POA5f+9E?f$#aT~VmxNzRDR^C6O z`IYhLYGM}OZqJI7^Yh@|$595{@f07Omw zwmIeR57VEs8T^L&RkNA&kK59Lv`N_hlJ)>6wV$r4iqqCEGeyHDhE`sdFome}SJFa7Ucu1^pNBrK>bZqDx^7?;P*J zemPRD-n7T)=iR`CKJeg!sP*6M-_k#3EkmyUpH&rF1w&kdy>Cae7JMHL*dxPRKf^{P2A_g0tp98lY=5s>YZscT@yGd`Ui#$}(9hOtD7O-e1cA2p>jd@0 zI|tL|+2yl6I3SzxieJW6(BD>3$ws4MuGEFgKQuLFU0Zn|B-Jwr5qLrOH;o-V!t=W- z`0@JuY{u+rR~!Qvl%*})UY@v00G_}76Es?YU)tXA@Vhkr0Uow;&srxcYv}yE>y0?D zRcXRTrdl=Ve+Xk1o+{}tnhyVAdRVY1(gpH&%z#JVHLxwrT=>p zsLb7tyNS!^VX--x`Ha|(n;+&Zuu#m)Edv`%SSWGT-TE8U5(Wz5%wTR1WlpJ3tzivv zEr6`T|7qFU*^Qcj+?~tf`EA{LUP#wmf4K?=(A9)jn93zWl)mXDc)7&5I$1iZs<0rd@h+PdcS76X~dF-9J@_G zH6XE%7)L!RMhX|rnp}8po#MFY9{kI~H`-|!vk2U*Ma$42U`Bec;Q}*Cj;vfeKkrW# z;&j(qC7trT$~9@8XDfNBhA`c^ijWo(rP~s7J2&r7TVtF8U{&scUj&ZVXvaN(020BV zbT=LrQ3o9tZ20Yc8xQ!G6@~2}%U+)o-7^@_Ybw6>UY5u=!%{T)a<10J19fW#n3-MG z4@YwaJmc(fD0|DnH1Uj!&m7s_N^3k+Ig^e|Fq|JS4?^nmq% z%IMs5qvczneg9cE?ji6XUJytdPa&`TO8G36@Xyd#R(7FznQcoeV}2+k;UyrWnOBI) zi}w^pQ`h@$lMf>Oz8L_$dp$H4QN#;?SnNRR3z7qtCF+LaM@2JlE-$^xLI0YKQlonJ zQ$CQvzG;|YIHj#QJ{kX#Wx>9;8MF^NlMw4uswM`Eo&x(+ zjzQGqUR_a&6UW%z-~yHw*hD}n%Nh#(ghbL76|s$peOanVE^3L;lUIbxKZP^rog(s2 zi(7G0=$@B))%jNg(dtya+ZrL7JJX*EHb)EKBuDXKMZh{ZlgmPNR!5NE{dkQ>YO(%4 zs9o%xJ*KAzCJvb#eULodrZ1-Ewor3n`K3IQ>a&RKz~gb~%f)|mE1<~1ne}4+ECvDv z1{h8k(3auuZd{;5wXN#=^M}Zdp{lN~7pNgI&~p#Ct*bV|6&vpH6#}a?^9oe}&j`^Z z6m7o}uLQeSamM-R88Ws74qLkEsgLitbu}t8%*e_4*WMLv3UhzYZ&WTMDy~|lXKBd0 ze+NZ$A$*{UL^1#SuyprHvmScvfZ3qy_82(iY~N16!4c-^{NzxYZ@_Xq@FfX73X0^% zZyyyYHzOOu)4S!4uM!%tCtobeT@thiy!j(5nB>M&2-y5!Yyoxy3{ux;tNBfvd_z>M z3Crw+|2^&wq%OQAD};tl28B<`(gC-BirEg>2SXSbhwKlKR9C@m=g(ebIJ+BeF3 z&vA-^?Ny78>S^wgNlm@iO&RWV^S>i3pMbVc22mqY7>apWK@J=5YzF7cu(6-&s?{?9YZq*V;C^ne zjKE02fZgI1rQSs|A#aCa=*7Ym6Y&E+Y9oWXsJX9h!MZc17u9#Z}!)$e+QpRW>1- ze4%a#)JT6|x^T^pvd|_=!25Z6*mX~z^U2$tPNs_UaF=k&?Nc;$0;>0^-MFJ-mP zVJa~GRt%tEV6qt8=I81WI*rjJb3vJftz5z$pwCkr$17)yuYPa)^!(HO8h+4iE>l;H|7|bkE!8uMgzBTTDMzq=fhO1l_WZ zCjNtL?q)2QF?X(YR!PX`>aKc{dnICCgvUsZq+dPetIZ{mUjSz~Z3J_<+W$PcX_P0n zMUcchH{#aGz1OCDf0z(|4o-i5=x1Y3%gqUUL(Vv8C*Pv3`>wXl6kK9D+KD|Doqhq) zpL)_RVZPv>za@t4H+aZ1ie-ot8HlfsJJkpV=Tr+PV7R(QZV+nD$>x8RQ&;DV)ti2q zFpB~6Eu#@`&7(kAV~`w)_)ysXis=4qN2f5!HU-YgR>HYDnKl!*ACZK*#r`=Gdcjt^ z_I1nr7P4?X^d_xV)fT&ECyRdap(rXRS)}jL+EI0{i)meQoCR0=cBh~!@3WQP)-kO3 zcK`_%jlv%v9|H*2%<0EtK5>(83oWc@wx?>alXVXdETXFgI_7(JxAcczg-Ay=9Wg6c zPq%Q%ZbJ==76ThBIhZFd)YY@M(G!~3cG0&flQsywj0iFh@3$MgZ`LC3QkvaNe|x-t zzPonKWsP{0fqAt~x265AA-rQP%_b4QJz_bp!46)GHj-?gn4ko3B|0D3wZM>Nm{wj@ zErai;gLe=f`SV?UDU4~7RU9c1r;bj^+_*X=2@d6e5DTZ?#pXk2m>4Z-kZ zDDsjGU$x5;{;5Bsi-cbeyis@Bm<=UXe55xI_rih@Ve6H^H^J3A-C&Ax zNKTJhTOfjwo{1Z#?Mibb=J-H6cRnz~F!2n*mcTIxYbIVH5YHGk9UF2lZYvG&SCbN zM31Oj=qr2enZh9d?b6o|cu6khoU2 zHN|^?-#*8VNPV5?@7f@th1ic6myIGSfbEiy5?TkY?X1ugS5bjMq^_MOb<0>@rt{Js zkmhKTzrI|FcDS{R<(<_C!guT|O^ znCYmQ10n~+IS}8S_&3XL{3xaeZZogb^3lV4#3QmEr#~iH@U`5arZxU3{wCOZT9ENd zS8T*L(^va0lp8!;<9`pQu(!QdkY<#aBSIp+3$>E`Y$q>aCIB_L-kcrQk;s_#)|r*J zB)`5C(GDM1`dkDUbdMiDo<>K8-iVnZeQTky|FmYbiw*gcQWT_97!MZSqvsy@em4oT zLE_RZC$A+fev15ZG|1d@t7bQ4bACn5$c>xaA8d}^Y-4d1^N0-Gc`=WidK+0=^6s{e zf;(GgJ6-W~`(&5KxKLGM%Fi$P*;sO_pNqaNI`KSkd9psgB%s}|vqI@AvQj>IeO$q^ z8%b|HVge3Iz%9(5aRj+ad8L%PZ>9|c82E6}7)+}?yx+mGrn67jEZp5+NITf%vpy$wFf_bsxvMz$#17%K942XmjI+e32Qt zK*!xqlDr<POHV;|9oSCS_{WZN2=qv`xi@R)cSZRD$6>rJ-Hztih~{`Hp$Mfqc7l2e?xxk^ zj^L4TYrwe;S-9K9%{mgY6n_5RVv6hDiYw)fL^0U^{yAKt2d_Bd#@0+z#G^x*_q`!u zb`VcpIVIpqX^eh1cs|IbUfs`MhGaaD$t7-DI#86n+kbcMULm>+bi=Z@x4q6mhr#u? z>RRZGHqa+`*Y5@F+a%oqO(ByWUvm-QmzSN5UQ+Fj^@q-@HHlZ8HBRZWeE3`yX7Ncc zr#+&xB03`RI2`g%Oba6D>5`Ddft&xP;S(4K`*pL_i_ung`S_5BuW8>p40iDwUEJu% zZ!GT3eS4Rx^}2)R(r@$YFEww~|8_R+lw%1N8UCzXg=WSqQej()mu&cx^?`n$(H0qK zg`L0b9eR+jVQ^YZAHARlzT?Gk&0fchL**0id3iNe;H2QB3vLD=@cP#qsy(xI+>vQ(0j3tZsx&cDt$2|ShGl>K3+V=Z* z=j<%q4K&C|VqyFABZcu2`V?rYD$W0uovIg(bk5VI{Bj)9Z`l~bW?fa8c3#MVUd;iC zk1srr5QGCeIHsnji4{bdb>@FIJ)j1JasXK_ZP(_;Khd$R#;vmAs zT9ufp z!{`GIv?kWseQRdo0EqlV9X^kzVenJ*T#x9u{r0uGX!Nb?N;?k7EA>C;2m~Mgh63{Vg!Qw1Kb5 z1jQe!4zvl|)jPy5&6|7^0c`a(P*UY;Xj|nW8&MUBHA3SF`pSMi>zNSBJzRe>k!Vm` z2VYjzuh9L&$4rG)^!DE~2VMZh>XmpdYc4^O&_kBRigry`qeQ1Adhd){3P|O{(ObyS zo&6_o=34=%1ZE1Tomqii4R+$s_DVK72pirQ}~{3hYj?NU;}Itg_;q0qe?#v?LY^nb|` zE^lDO&HN1Yqm;uukcr_ygEzQ~*<$fZF(I22E%SvW_?uKf*b5J0==slnLjY*X(pt1B zl-?0u0aX_`N*?*%JKgD6@c|VK22zo9$}=S*61#|Ty*O!62r z;RoI}EVxhFDzNmJ!;ixmbBpaYipN1$x;YyIlXy{(VA#pl%UR%O=ytfI-NY;>av18+tW@#}hSi0IUi?`4P+1`pR<@hW_81ME|k6Rr1?F zBn}7{Wv9JrKLWzSIUdn)K9fPuNtHP3#}PM-x(pgAmtHJM=&!^vDD zU&DC&;~}&4*(7sn#oWt^spbq=yYBL4Eepf_QPS`miT=x#ZmJKgfQDk)=pBnhBC|JW zj$JRuOfxhjTMPU)SD*g3>F6JYsQnkE0mlM#lon?#UvLm2sez*i&YwgdopkJ1ItArf zvS*^RaUB(R@wyUegM5a@ClWaNrfJRE8&_ZzNiFSjt5f-G_lB612ZSiy3^rWWBENnF z^ij~a$;au){^sLlsBfqOA@hr2DU2b{k-~X)IE4 z#NEA@F<>PqJ~nlIL0m`)*!5^~^WkcjY;H7qiGYu4Bx8o1Nrtffzwazjir@bdZn*pX z_5keqP}j3`Pm$Eg&+he_WrqCo>-s&BtXfrVjZ&TeWR9MJ#HGE@vc=+K&tU6m2fg+4 z?!0VmE_HpAxw!`wKPv0%`-Ie65&VR!_vMJ7Ga#0~O*q$)Akwo4XA^0DFh~p|EWDG_ zk9z=&rD|$`rXb$$bcb`nxu?a{SLedNwrRLscXg@^3_I8URF(4DIa`Vz+7Bz~hK1(C z%9cqXrBo!b{bVuv5jGi!v4a;#XA5{i7vxQs+cOvB>VP1vql-QcorbO+pD0;?8$R=W zU0fW1eIjyPY2IK6?#AEI;CX~27P%G-@Zdwh6n-X$y7s84@qZ$F9{-g6s`aC;wr}pm z=^*t=pq{~*|69OQJIze5EqfVJ{TiK6_Jn~%__q?qMDX3#7t7y;0Vj+)W`%vZu~jSY z`^34+9-Diw z=VxHn2jgWae2(XZIucAAI^YekA@UAt)LMMq{p2^S+?AXYQ1iFE8aG6Rw~QJ$cM73* z=KJiB-({{zTuP-cFIVO|Ss6&q&4T2;o{=q*wB(@g%6G0wv>WX_b?Oh!Z{YJ!6=81d zFb*g75&sv_geKLvHq6+hJo-TP4MXcSm-wAwjMzW{f`AYAyDqwcImgKIN@aGot58yL z=z*mly|Ovk0~cu;%DA+1eF45Dr!PD9wu9GpzIzrdS(x(;r_h}Y7KVq=h3uz8?1-aX zzFf^z4gx?#0Khq@9cqn_PQMZqbQ#?y@zFYgG|r)`uEn3{$sDdQusCq6&QpM14!sA& z(XYL=fEn68hpn_ypgusXG;z9IAcO|n;R zbFT%rdX)OH_S^V+qyIL(s$M_cIP4V;j~rY+u!)^+$p|9_WO^)#BzG>t^VekPTCv-@ zg78S#xfAm>(zNwN71DlEX8_ng)OASMR+S_&P{Cb9qAx0rC5wCAj501XSuU80#I^z| zrrk^2lAKyS5q%utGLa<3g1>gSlKmVn8o6HBC%ar`I8B?&to3u5*xGv5Ozi!2^W>{JEZhzQ!(#xjqX!3k!hN2^*nkPse)W|1z3>MyE%hX9 z>pyRQl|$?6y1eazLD`_|0DZE6XfI=i6`va~!-=^HF5D1tB&ma>f`V~|4t}A!`W3*EJp3L`o(PA=f)hJ5?+QM-N)&`jAGN3Pv z>^#H!`XEQ3AO?Ho4J)DmJW_68;Dxni!2XVs^1f>_bDwtTd7NsySb>e-dgLo7B@#Ri9o64D4`I*G#rw0mX+U;a*1J$vj!6a zq!?lg&4Cr(#0|G>ZDYddMzh()Q|i9n#xW(I1yr9us?aL zl96=JI)dr}unz(=;Oy)%C=Xx92ygi}YtwuXYRj|u-&g#Cp-F68^$JfY+h$@U0q(|Q81fhEl7w!5*P z!nY2W<`@r^n8I&v`*)sonanEK)a5QIFi_UAG!X|Z#V)Yn7r(#hu7TxJ1x1?G4mF}{ zXK4fA9looceXCcYTx;?WH%wt=5F9Vy-lE8CUXf`F96dUP_-pMFB4$wfAh{2pT3~c=8I~G(?S)gn~!1)fVDkDu}qUfHquHTl+f+*X<+HY%`VO#bJZz%vH_E6>Pg`)SYu*^yZlG=Q!B21MpAKV`}c#TK*2_DQy z`Rc(G7G%}(=LsGCgP*3jQDHByeiBhMSd4?;f1!xE#)AL6#K1K$FvjX9{_yb-qmMsQ z11}mp8u*s~>yHyW6W(1vCK3{o15oF7baBZC-f7bN2OFK&iKep^E*@XcUj6qAegQwl zlOKM9hdEW;ADC0EEv4Xvh;MQ!_eUeo;U@Q=36AbfTB$3FyZsf)gG(Ghm^a*M-z#F; z+1VXHP^L2H?E#Ei(XRjZrC%<}e2|oQ)cMsIJl6(zu(CP1j^PgxR1!S?_40uWQy`UMEkF4K>g`Sh;VJyl zB&r2DUGwgzDYIih^+~3<6T?*LPkXyfjZ-+W7%u!bgZlXCXYDt?U*bmYGi%C&hiUZv z4%O7~TVc4VOCc|ZTe-WF1GhxbkWobnbe9`_^7Cb9TN`GUUP(B(x=GoGg$1+ggAPFT zoPSz16GPdJ9#oh6X=kUxjOiPDT_ld^amtP{DUYg^y!fIsD@`x1J94fyVSFUgJa}g9 z=P|PP5@q~Aoh5)E6hnaw=)S%J(8hodXc{S&pjQvGIy#iV;SMUFGC{WtXhjUb7l7Js z4|@?zO@f>K`4jEmxl=pTHwQg|tM~prw1F2Z@I$r8pYBDh(B1Gv*j&`dISpC)W&U)U zbWb(iJT&IsT4RPdo?O-ilchlGx;1V61UojurY~twPL-SkneooUx?m(?Q^;WmFaz?Q z^DM8gWB+|e1>Pc%PIl#@uI(haxw!$Y2oO(rC<&(*7P@Y4<$wqRRODZ8<)K$;gSE6q z^v1iNPD3I@)51>fy9Hw+_&*kiNsi)OG@fs3@kVr!M>ua542&|6m1bpSp>F~NEIFHIb(zH|3EiuQ_oF!p{PDb~qfSOLf$|8erM_TemB6;vVSh z$8ZWCtc&h{y4Ky6diYLdsF?`?mJ-gwnU?%B)W@-Exgf;)rUXei87MtV$jEd8BT}$9 zz*+()%-FUV7dN-Cygcb4ax-@T;GVm?yRGMt0zF=g;7vcZLcNzSU$z1Y!F_6KVL3UH zDKA0%f%p62+T*0s1~4XAboyy_T%D34L>V2A`n`zlra%vM}JCs$ga6cw&XBWEKbmT3a1L#p@Go6B%f3RFJu`!}BV3-HoJ#C9+uj>3za^SuawTm&8RJ+6Y zO)DDWw0bnJ9=PE8Lo}#Ts$TkIo(dKh7lRBMbOq@GYH+m08embZt#m|Oz%5X~+B6c# z9V~5aam(R!@>Y(*)^iSG{_=0PWsORVAPj~1h-eKLy0p=noFOkpHC|mF24F=+Pfu@b zP>P~z66SX!?ZK25 zBR(sOO`c)y-tzW1#qRFBLi244>xLG_(rc^LS4~&tosEbgaA+uYF$wIHEqUt8QRtpJ z(-NH(xwve^gyS@hjvt(gP?d$#XVYv$d7*L%`!2@PY2@lE8*`8Hm)dNH2G$c2BOMt*JVrXCU7; z#^YtL;Qr#ZWz;70mwLP1jiErVWMpOh_1ovawx!xRZG<;eO}UoL^J)qFXC$hrlLG@S zOlJh5b9}}yL&)wtS6C{u8-5Ns!RLQ}2U;EqTaqTZg{wZa9bTWVT_T@Pc`)E<4R=f~GR5V{v|SI8$hkZ|jAY%q^1-l>B00N%U#Pj} z!>j1%GWri@xRBGMF>h148>x=a@+HdzCmNg3q-e*d_?RV3x@;fRwXJC0(EYnyC(;vW z@CHLOG(-~B#Rf|3ZlG7<`mrByu_gj0A?T@XY}|HoBF4}ojH^~2WmzTqyJTFUpN4xX zZv-CyX3Xi`^qVfS{*b~7$&x`}{@%BEvMiO{?I|G@xMS?qF zt9;@x>MjBX2Pz!N`8r|D9yd&-Tdor^F4`pb3=cmu={O2pYqj`>EF2)JSd5o{B{%GHn;LbnQEaQ@hj>cGg6sztA2q9x zvS<1zq(S&E>4H}>^R{+cNph1WbotkoJMpjE-eJSi7YxkIYhYi0j~ec9=&MFYH3rPXoijYIaozt0->s;<=Yy5})Khl)gvLY6|JO>B;}%3dkW zOLltpjHDR6IYtCd0r#T7Dcs#+cBo>^O*Y$p&J7zQ(&~^hGqY!UAzQKe7NeEhHS1Vm z3AE{HWd_rT!e_tWXQk`i{QP_j6LEbn->Z#{(Sh!jo?i2llP1!SV`*S@f^egw<4#R& zw1m{>r}>Ah!EPAC2?NcoV|s)>`+rz+UAmZfP!gWfQNNsW;suzw_g_l)GOO`|EjDq` zHnHzb`ypSSYlcHYPX0)IGW6@RcVddSUwcA@0QHhidYli|hPwB%9}~#2zy~0)@CHb> zQs-Y$goU~)SHK>1h(GBMbUdO`ljfb?Kgl~`&d@_(EjDON0X~W!J?@_2hP;6h|H+x{ z>vU=`lMZ2Rh7BgZ3)FpZ-_6(eBM2h0va{a{3r=}~+Yy$Qjt1qXk6=49u9(s-GwgVp za*tOI1m2F0L>Zb~KVFKAXpiFTQ#QL1(a{mrmCyM%8B0?AFhj2FTYc#b`AELxNz*hg(;q7${zB zCY!1QF78Bn`Y?J{>y%yHt1s25JSUM|c`F;NL83(NA*VPCBwLpu1{R;aCdR2t3Q5$Z4KjrPNE1=PFo-C`%Ds_Tq`bmOk zSmITb3Z*btJ-9h->(R*HF!ZSD}lOT}&JzQGzO>rB?`H*c`@ zaKw1G9HQ=-K5Du#Eh_hv9$D#G8`IdZ2Js-MTYwK;tje@SY+RgGV4zHlUu0L8vN5nj z6G&6)+M$jXc?yvxaBJrdSzjUceV)Kzsw#wCiyI>J`#9X$z-8)4aKp zgWBb3SUZPN#VCESb_&xSFP}&$Ik35bjrO%8`H4*aGI4l$AKr?(ltSF|c-rTUER0)6 zo!jq6=quuK>?n0NGg+=;zSFlGaD^hKDztj;+@Bzx;D4@HVzBk+RA$TnBwDG(mQrs) z(ou}=VMGrT=hTfcgK5q;DT=SvJJgC4CRIrnAEdh|4~^}cRj^*+wL z7&$+r=RFL@ffN#6LQ4%Vp_>h5sej7nEq4<}VupZ&>*r zr~Q%7qhibFR{hJ_5Nd}g`Nz`=n~B->0~x^+`wY#ZHO#m2{q5Ucw`mIJ zg_mcQ3VUCtI2aKLWsfokJ@{2Yrrh4DthW$}4e=M>@{)Y}^?F*!-PkzaQ`?VgFmoMz z4MVPy)=Okoo*DF}M)>Q(BhBqoh|qL7FEIxGC|$?nOj_SXSv90&um#4;5He#bhBBr8 zT#ReXxZ|Ki4Mz6tp(h`=gq<7R6Xy5EwzoFkk;Wo9P1yfTT@_-R%w%{eJw=L{ohvT9 zEcy%f`B={8sP~~kx?TziWt_bE1~=8ODQP1hG>`} zLqk)M^+;Ui*yp_CS5E)D7?TAf`y+;;?_zkEafkfF`knOYUk-=rSkv2by5d^rL=&=~ zX?{LRS=zV{>F?R(--MT|KklvIH%9fX)q+`7Z%Tp;WLjHWQ!5@P0}VAQ(6dTgM+X>B zf)ozeF}8mIotvWz3ps(Nn=Ngi?L|B%S@!+9@sp`Ys93KhcX`NX7F7^=e#p*dbaZwG zn!Od^Wckv}tgGx*ZGs`fTT_wTO#d@T4f6BZLH4)2wZ+501Hd()llNd0^68!M4W|=| ziH~0c?$|F)O`i&+g&Bk>FOp$(EXdI83whXOQa0asxeHkBKmMGst6g2n6fzoC=r2d< z3tqW|lvF#&ssopL07D<0L`q5$wBc&5S8m-?V|pI1*iu9=<+b9+bar-jaI&pWNr)2@ z8@n=;Ee*oXCvv$F9dh#&cF(TUONM;?*ur3DPo~uL%yU%eaJh?`I4n4qB>>+MaQ#~Q zx^eHHpC8uy7gsQy+(-w@OOn*s+6mV;MAakx3aVitLffG2U=7zz<{7JPkf8AP2zH-d znc?Bl#xoheBW#dO0j_c#aNhvwFbK-nxVf@CyeIH3GtO;RYAzlpP7JkjaRBs3{^o=N z5I(>Pt^D;XG&MCfZt+>b-v-zYeEX@He93+VVN&vXR8k|o6mRL81jx8S?%AEM8t)3i z7eGmALfu}~L9V6x-YTOS{y&|;>hj*6yZ6~|0(I*@!MnnZI3-ZkG(JFoaZ^`b?yLgLUJKTEwR6@39`Vd7cO90yI_^2F7Q=)xaHnk ztg$t1tDeYZ@N0HfgqFE5)mN;xAzO`b3Mn8h?ekH$A#!rk_mD_QbMuZ9Qc}<6s&4Ht zeB$TKCrJAXS4gaFRPCi1lE6)|2+prcwcC4Z&C{QHeiS4C7ZG3pc9aAnXF!aA3|0WM zl+}-<@!D?8HS-+2d{Nx%;>mMj36u5Hw5~2-|C+Yie?)0JyC`(&)=^$47 zH&W$6&#|`~X#K+k>X?W6LXW>BFJ;L!JY0qiZveuOq<#fo1v)xv-}Gj2+^?}CZUET% zo|v`2O*++^`Q%+rmW{w;U0T!PqB1BJDQRd(4yZjW%Uw34xVo()Cnu+>rWWa>%)0_~ zv#m5w3IfM+5(Gy;r5msyux{-55S)=%nXngZLg3}aXf#OH$_#>92ID3Z|I}f%x)ekg zlICn?kYH&yHjp}@Oc4_k6QEYz=$?gs&KB!BAtAxf6|rzF*Y&&!*@X(88c?s&+FA)p z7YM6kWRQ_T_LgPh%XWI8S+03!sUaDZtbmi&WS1QG>T|EbMx^wZPo7mQ{t-22#Nnu} zKX5>5%sxupP-YzNp|3wnOJHqy8r)FEt}y!k0b@)~faqxn#MyexpIktvU4S_EoJ))m z=&5lXhHIIfC6rDjvJfT&19bUNGf=8o>9a>n_7&R${enhbnb3%xt)*%`y$`>$fgS>^ zj?^25d9kr6kB?8tfqEV*S!` zb;GV80CCJ&eDZQJV)qazgKu?HpFG{~2LDjD6&>~meLHZ>$(quQOc4-OG|47 zNTTuR?=c}@Im1>G5I=!HuP>TlNPbr@W&dfenBqc`Nc~Ix>}#tn+WyGC_$KJ*ngRnG zn?blGy8=|ZGmZ|5^mea>Fj ziST`09a_`VC2}lHe_s*^v^?u|5qxm(JF1^5;2&O_=@I7mdz9D_O%Y@Q*3&FgQnGv^D~u(i_jefrSVS82+g@4=oux@~N6W-+(y8kU zzNW8*=}M-KgqvmH{Dm-=WhvW#PR%bY-1YZAA}p=*1w4fKX4ZnYpwGPJfF;0MV1onQ zvnWuY0r+BeA;mKQg^CAzy`&V%69Sx}t0pFJgtneu($++{OjhVBKWJlA2~FUipIWsU z5FQjg34uVw-TJF9*IBxV(bjB813-;z9WI`|qTc!LiRv5Gw#7?ApuVQ6s%p7YYvSkf zvIuCdqDJ!yv&E}6{F?@>b2WQ;zvvE4@lKF zA)SFzFh>@8Qvy_=(3H4y0^SmvMskuUw7@;&Su1(d$;ba0W_KtbZFv}S(u?=6iDwLo z=8y0G0)!ivw;J}PtN|RN(-nY+1xf~0_4Shr3k#R~)PI4`$j6M>&>2wPp>p>SP`Cz| zPmIVUL&uTck!W${^PJWpJ6L8coryF!Ij(CgxL;?~l8-enaE1+2=j_emz;WE&MYFR7 zPReY76gC_B`#1Ci5c@TNRpL(b`GG%`Ox6VkDumHJh$4q6^A(L;O-F2!gMJ6pK0aWnI(8*Z)`}Nrz*59^fFj39-rn5ofEr>#clUL-^4m4T z=Hr&?J$ke?BlN%=5)4gry?>tvR8@%>WjlV#wJrSOnhcE~2d57Ut29VE&G_vcZI?J` z7{Pv2Z||Q>=B3YeR)~kegJZXLLxn2cDOm`F{=oI%+3ix_?;QmENVw&t6JsBLvKw@U~cYp6h6#H0BC?wY6M+(Aif9MRf+7Y@!q{N6~=`J{bKZHA*alTgKZq0 z!>_+>SHG4wiL^Cs|FOu-7AZG1{>LAMnOA^-GAB2zPWWP$)W&b>__(yRe%MQ+oCq-T z88jF@!}OoqZ#5kz(j8R1eZJ44{!GX9d{u=n#4>b>BtIvnd&BkZ32Bu7qu>@m1VQ$d zngH^^Eun#}l-E`6MHoUr;2~`HdV}iAhS?fV{ayzLGel=+Fc=&PuVfvGNS>?}7H4E6r3YGXZv)Cuet~UhLHeQEcdQXWeK21fc(9GeS1TH^w;VB) zjn9R|VLI1;!cQ3VXW*glxx4ck@v#@^$rB0Sf)@G6cc^srUkY3xz{o<^;QBp2=`hO) z6=uk3Q{1~BuLM{9dFXO{oRX4~Kffi214GIe#Q}CR+a?0(cz_U%o{AzuSDr!d1B(lM z%{yqc6i~3rftVF;`32zdyM=Fq^D5v)H&X$~25+N>y?2SWS9M0!90;S7rC39FEIRJ< zDTvg;)|w0k*#uS4m~)U(E*x;g?d9jmC=Cu+@{iXzf1LkRzpS&o#P5`z)Onu8D9g}7 z3gLPk1N|O2)R`#zcsH3ruQHXhj9^E+pR+h{SU~JHiMv8j54e2^YONyT#p6)xbhXyW zO0gO*{b{l|6j=cGn~`(iXzr$*k>ECDdSFPDep8rgpT;0=2#Qmgo+ePey%=d4-(BpE zAeEAAqDfFn()`=?K4mmXjcz<4KJX#xv_bx@=-{B|>1I4jW^u+4{pijZ1GW9L>rhW)tplE)`QD31jVTKzPn1LWY%d%2oYxB*oJ3bf$J`DCr zx?E!@O=!sH$KIPzN3A6z899QBhmi49c5+~$$@h|Vh9eHv%WWTlg>-@{C zDZ+~G5M*1?Lu;SEtUO3BF}v1X3!NCY;Ol{<&Q%Sf%vR<<#@Tvu8kXda&pZ`1a7=8^ zcohJ;WnWl4;M&$v)f;#0{qo>P#4S)wwsSsv50m*fA?mM8Vr?K);KQ7&a+yZqAJj$n zf_2rZ1IeC7h&n6lL_Yr`4Ia3n;X2*u_H$2 zF0{2q`xlI=`;Dp3=i_#?eT&^50<;c-Fm|2>^tng3wVBq;3ktHeUS9>O_j={agfS0* zfs==1f@3F%Z)~{IePI?QM){r_u9^aqm-tfKxjAAFS*0u}6CWQ3dE_FLLVc57pkdNp zogvQm(=7q*aY8`ri){(@f#8`H=_IuMw zsauki42Q>($CvX11S)e=-^1RLziU|B{~6Shj3I6aWihn)qL#bVQ^N>8a+Q@(DU~5r zOGnG#mroN-tzQf!*SHpu0g~&DH*PiKxvY?K(@vm~*XE#&80Q%*Tr=wtd9T_3j+5=t zZfT317d5V920m|gD$eDJeL35znRk-owKe4FbXdfX*Mhd0HdP9P+!64gk*{UhC`jzR_!3dvTQL z5o%0{W;};fawzxlv>hoa)mf1L7mu2zw;eq9D9J?=JI3w7o7h8q`W_(~7CY<&lyVim7g9O@e8oZi+Se!5@IYh$y9!|ksPKsVx@gJ>yEFW5w(Ndi{ av(KwBwaeepj#}dYFH>WyD@3C^QU3!`T@dX6 literal 0 HcmV?d00001 diff --git a/src/main/java/frc/robot/subsystems/drive/heatmap/main.py b/src/main/java/frc/robot/subsystems/drive/heatmap/main.py new file mode 100644 index 0000000..db1f837 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/heatmap/main.py @@ -0,0 +1,565 @@ +# app.py +# Interactive FRC robot cycle-time heatmap with Plotly Dash + +import numpy as np +from heapq import heappush, heappop +from dataclasses import dataclass +from typing import List, Tuple, Optional +import os +import base64 +import json +from datetime import datetime + +import dash +from dash import dcc, html, Output, Input, State, ctx +import plotly.graph_objects as go + +#Fetch values from Drive.getinstance().getCtreDrive().getState().Pose? + +import ntcore + +inst = ntcore.NetworkTableInstance.getDefault() +table = inst.getTable("SmartDashboard") +pose_topic = table.getDoubleArrayTopic("RobotPose") +subscriber = pose_topic.subscribe([0.0, 0.0, 0.0]) + +inst.startClient4("Python-Client") #Python-Monitor +inst.setServerTeam(1458) + +print("Waiting for connection...") +while not inst.isConnected(): + x = 0 + +print("Connected! Streaming Pose:") + +try: + for i in range(10): #limit infinite loop for testing? + pose = subscriber.get() + #print(pose[0], pose[1], pose[2]) +except KeyboardInterrupt: + pass +#============================================ + +def add_paper_background(fig: go.Figure, img_or_data_uri: str, opacity: float = 1.0): + """ + Add a full-plot (paper-aligned) background image. + This image does NOT move/scale with axes; it fills the plotting region. + """ + if not img_or_data_uri: + return + fig.add_layout_image({ + "source": img_or_data_uri, + "xref": "paper", "yref": "paper", + "x": 0, "y": 1, + "sizex": 1, "sizey": 1, + "xanchor": "left", "yanchor": "top", + "sizing": "stretch", + "layer": "below", + "opacity": opacity, + }) + +# ----------------------------- +# Robot Kinematics Constants +# ----------------------------- +@dataclass +class RobotKinematics: + max_velocity: float = 19.685 # feet per second (6 m/s converted to feet/s) + acceleration: float = 13.1234 # feet per second^2 (4 m/s^2 converted to feet/s^2) + deceleration: float = 13.1234 # feet per second^2 (4 m/s^2 converted to feet/s^2) + turn_time: float = 0.2 # seconds per 90-degree turn (for path changes) + +def distance_to_time(distance_feet: float, kinematics: RobotKinematics = RobotKinematics()) -> float: + """ + Convert distance in feet to time in seconds using robot kinematics. + Uses trapezoidal velocity profile: accelerate -> cruise -> decelerate + """ + if distance_feet <= 0: + return 0.0 + + # Distance needed to reach max velocity + accel_distance = (kinematics.max_velocity ** 2) / (2 * kinematics.acceleration) + decel_distance = (kinematics.max_velocity ** 2) / (2 * kinematics.deceleration) + + # Total distance needed for full acceleration/deceleration + min_distance_for_max_speed = accel_distance + decel_distance + + if distance_feet <= min_distance_for_max_speed: + # Triangular profile - never reach max speed + # v_max_achieved^2 = 2 * a * d_accel = 2 * a * (distance / 2) = a * distance + # Assuming symmetric accel/decel for simplicity + avg_accel = (kinematics.acceleration + kinematics.deceleration) / 2 + v_max_achieved = np.sqrt(avg_accel * distance_feet) + time_accel = v_max_achieved / kinematics.acceleration + time_decel = v_max_achieved / kinematics.deceleration + return time_accel + time_decel + else: + # Trapezoidal profile - reach max speed + time_accel = kinematics.max_velocity / kinematics.acceleration + time_decel = kinematics.max_velocity / kinematics.deceleration + cruise_distance = distance_feet - accel_distance - decel_distance + time_cruise = cruise_distance / kinematics.max_velocity + return time_accel + time_cruise + time_decel + +# ----------------------------- +# Grid model +# ----------------------------- +@dataclass +class GridModel: + w: int = 58 # 57 feet 6 7/8 inches ≈ 58 feet + h: int = 29 # 26 feet 5 inches ≈ 27 feet + robot: Tuple[int, int] = (13, 29) # row, col (roughly center of field) + blocked: List[Tuple[int, int]] = None + + def __post_init__(self): + if self.blocked is None: + # Example obstacles - you can modify these for actual field elements + self.blocked = [] + + def is_blocked(self, r: int, c: int) -> bool: + return (r, c) in set(self.blocked) + + def toggle_blocked(self, r: int, c: int): + if (r, c) == self.robot: + return # do not block the robot cell + if (r, c) in self.blocked: + self.blocked.remove((r, c)) + else: + self.blocked.append((r, c)) + + def set_robot(self, r: int, c: int): + if (r, c) in self.blocked: + # if user puts robot on blocked cell, un-block it first + self.blocked.remove((r, c)) + self.robot = (r, c) + +# ----------------------------- +# Pathfinding (Dijkstra 4-neighbor) +# ----------------------------- +def dijkstra_distances(w: int, h: int, start: Tuple[int, int], blocked: List[Tuple[int, int]]) -> np.ndarray: + blocked_set = set(blocked) + sr, sc = start + dist = np.full((h, w), np.inf, dtype=float) + dist[sr, sc] = 0.0 + pq = [(0.0, sr, sc)] + while pq: + d, r, c = heappop(pq) + if d > dist[r, c]: + continue + for dr, dc in [(-1,0),(1,0),(0,-1),(0,1)]: + nr, nc = r + dr, c + dc + if 0 <= nr < h and 0 <= nc < w and (nr, nc) not in blocked_set: + nd = d + 1.0 + if nd < dist[nr, nc]: + dist[nr, nc] = nd + heappush(pq, (nd, nr, nc)) + return dist + +# ----------------------------- +# Figure builder +# ----------------------------- +def build_figure(model: GridModel, kinematics: RobotKinematics = None) -> go.Figure: + if kinematics is None: + kinematics = RobotKinematics() + dist = dijkstra_distances(model.w, model.h, model.robot, model.blocked) + time_matrix = np.zeros_like(dist, dtype=float) + for r in range(model.h): + for c in range(model.w): + time_matrix[r, c] = distance_to_time(dist[r, c], kinematics) if not np.isinf(dist[r, c]) else np.nan + + viz = time_matrix.copy() + for r, c in model.blocked: + viz[r, c] = np.nan + rr, cc = model.robot + viz[rr, cc] = 0.0 + + x_vals = list(range(model.w)) + y_vals = list(range(model.h)) + zmin = np.nanmin(viz) + zmax = np.nanmax(viz) + + heat = go.Heatmap( + z=viz, + x=x_vals, + y=y_vals, + colorscale="Viridis", + zmin=zmin, + zmax=zmax, + colorbar=dict(title="Time (s)", thickness=16, len=0.85, x=1.02), + hovertemplate="r %{y} • c %{x}
%{z:.2f}s", + opacity=0.58, # transparency so field underneath is visible + showscale=True, + ) + + # Blocked cells (black) drawn above the heat layer + mask = np.full_like(viz, np.nan, dtype=float) + for r, c in model.blocked: + mask[r, c] = 1.0 + blocked_layer = go.Heatmap( + z=mask, + x=x_vals, + y=y_vals, + colorscale=[[0, "black"], [1, "black"]], + showscale=False, + hoverinfo="skip", + zmin=1, + zmax=1, + opacity=1.0, + ) + + robot = go.Scatter( + x=[cc], + y=[rr], + mode="markers", + marker=dict(symbol="star", size=22, line=dict(color="white", width=2)), + name="Robot", + hovertemplate="Robot
r %{y} c %{x}", + showlegend=False, + ) + + fig = go.Figure(data=[heat, blocked_layer, robot]) + + # Axis ranges (cell edges) and square scaling + fig.update_xaxes(visible=False, range=[-0.5, model.w - 0.5], constrain="domain") + fig.update_yaxes(visible=False, range=[model.h - 0.5, -0.5], scaleanchor="x", constrain="domain") + + # Paper-aligned subtle background (does not move with pan/zoom) + if FIELD_IMAGE_DATA_URI: + add_paper_background(fig, FIELD_IMAGE_DATA_URI, opacity=1.0) + + # keep same aspect ratio but scale down + orig_w, orig_h = 1405, 652 + scale = 0.9 # adjust (0.5 = half size, 0.8 = 80%, etc.) + fig.update_layout( + width=int(orig_w * scale), + height=int(orig_h * scale), + margin=dict(l=0, r=0, t=0, b=0), + paper_bgcolor="#1c2128", + plot_bgcolor="#1c2128", + font=dict(color="#e6edf3"), + showlegend=False, + autosize=False, + ) + return fig + +# ----------------------------- +# Dash app +# ----------------------------- +# Explicitly point to the assets folder (resolves path issues when launching from elsewhere) +ASSETS_PATH = os.path.join(os.path.dirname(__file__), "assets") + +app = dash.Dash( + __name__, + assets_folder=ASSETS_PATH, + serve_locally=True, +) + +# Optional: force no cache for dev (uncomment if needed) +# app.config.update({ +# "assets_ignore": r"^$", +# "serve_locally": True, +# }) + +print("Serving assets from:", ASSETS_PATH, "Exists:", os.path.isdir(ASSETS_PATH)) + +app.title = "FRC Heatmap" + +# Path to persist layout +LAYOUT_SAVE_PATH = os.path.join(os.path.dirname(__file__), "saved_layout.json") + +def load_saved_layout() -> Optional[dict]: + if os.path.isfile(LAYOUT_SAVE_PATH): + try: + with open(LAYOUT_SAVE_PATH, "r", encoding="utf-8") as f: + data = json.load(f) + # minimal validation + if all(k in data for k in ("w", "h", "robot", "blocked")): + return data + except Exception as e: + print("Failed to load saved layout:", e) + return None + +# Stores +# - model_state: the whole model (robot + blocked) +# - ui_mode: "robot" or "blocked" +_saved = load_saved_layout() +if _saved: + try: + default_model = GridModel( + w=_saved["w"], + h=_saved["h"], + robot=tuple(_saved["robot"]), + blocked=[tuple(x) for x in _saved["blocked"]], + ) + print("Loaded saved layout from disk.") + except Exception as e: + print("Invalid saved layout, using defaults:", e) + default_model = GridModel() +else: + default_model = GridModel() + +# Encode field.png once (shown below heatmap) +FIELD_IMAGE_DATA_URI = None +_field_path = os.path.join(os.path.dirname(__file__), "field.png") +if os.path.isfile(_field_path): + with open(_field_path, "rb") as _f: + FIELD_IMAGE_DATA_URI = "data:image/png;base64," + base64.b64encode(_f.read()).decode() +else: + print("field.png not found; field image below heatmap will be hidden.") + +app.layout = html.Div( + className="container", + children=[ + html.Link(rel="stylesheet", href="/assets/style.css"), + html.H2("FRC Robot Cycle Time Heatmap"), + html.P("Interactive visualization of travel time based on drivetrain kinematics and field obstacles."), + html.Div( + className="toolbar", + children=[ + html.Button("Robot mode", id="btn-robot", n_clicks=0, className="btn"), + html.Button("Blocked mode", id="btn-blocked", n_clicks=0, className="btn"), + html.Button("Clear obstacles", id="btn-clear", n_clicks=0, className="btn outline"), + html.Button("Reset", id="btn-reset", n_clicks=0, className="btn outline"), + html.Button("Save layout", id="btn-save", n_clicks=0, className="btn outline"), + html.Span(id="mode-label", className="status-label inline-badge"), + html.Span(id="save-status", className="status-label", style={"marginLeft": "10px"}), + ] + ), + html.Div( + className="kinematics-panel", + children=[ + html.H4("Robot Kinematics", className="kinematics-header"), + html.Div( + className="kinematics-inputs", + children=[ + html.Div(className="input-group", children=[ + html.Label("Max Velocity", htmlFor="input-max-velocity"), + dcc.Input( + id="input-max-velocity", + type="number", + value=19.685, + min=0.1, + max=50, + step=0.1, + className="kinematics-input", + ), + html.Span("ft/s", className="input-unit"), + ]), + html.Div(className="input-group", children=[ + html.Label("Acceleration", htmlFor="input-acceleration"), + dcc.Input( + id="input-acceleration", + type="number", + value=13.1234, + min=0.1, + max=50, + step=0.1, + className="kinematics-input", + ), + html.Span("ft/s²", className="input-unit"), + ]), + html.Div(className="input-group", children=[ + html.Label("Deceleration", htmlFor="input-deceleration"), + dcc.Input( + id="input-deceleration", + type="number", + value=13.1234, + min=0.1, + max=50, + step=0.1, + className="kinematics-input", + ), + html.Span("ft/s²", className="input-unit"), + ]), + html.Div(className="input-group", children=[ + html.Label("Turn Time (90°)", htmlFor="input-turn-time"), + dcc.Input( + id="input-turn-time", + type="number", + value=0.2, + min=0.01, + max=5, + step=0.01, + className="kinematics-input", + ), + html.Span("sec", className="input-unit"), + ]), + ] + ), + ] + ), + html.Div( + className="panel-card", + children=[ + html.H4("Field Heatmap", style={"marginTop": 0, "marginBottom": "8px"}), + dcc.Graph( + id="heatmap", + className="graph-wrapper", + style={"height": "640px", "width": "100%"}, + config={"displaylogo": False, "modeBarButtonsToRemove": ["zoom2d","pan2d","lasso2d","select2d"]} + ), + # Removed standalone field since we now overlay it under the heatmap. + ] + ), + html.Div( + className="footer", + children=[ + html.Span("Made for FRC strategy & path planning • "), + html.Span("Adjust robot/obstacles by clicking the heatmap. Code uses Dijkstra + kinematic time model."), + ] + ), + dcc.Store(id="model_state", data={ + "w": default_model.w, + "h": default_model.h, + "robot": list(default_model.robot), + "blocked": [list(p) for p in default_model.blocked], + }), + dcc.Store(id="ui_mode", data="robot"), + dcc.Store(id="kinematics_state", data={ + "max_velocity": 19.685, + "acceleration": 13.1234, + "deceleration": 13.1234, + "turn_time": 0.2, + }), + ] +) + +# Update mode label when mode changes +@app.callback( + Output("mode-label", "children"), + Input("ui_mode", "data"), +) +def show_mode(mode): + return f"Current mode: {mode.capitalize()}" + +# Mode switching and clear/reset buttons +@app.callback( + Output("ui_mode", "data", allow_duplicate=True), + Output("model_state", "data", allow_duplicate=True), + Input("btn-robot", "n_clicks"), + Input("btn-blocked", "n_clicks"), + Input("btn-clear", "n_clicks"), + Input("btn-reset", "n_clicks"), + State("ui_mode", "data"), + State("model_state", "data"), + prevent_initial_call=True, +) +def on_toolbar(robot_clicks, blocked_clicks, clear_clicks, reset_clicks, mode, data): + trigger = ctx.triggered_id + model = GridModel( + w=data["w"], h=data["h"], + robot=tuple(data["robot"]), + blocked=[tuple(x) for x in data["blocked"]] + ) + if trigger == "btn-robot": + return "robot", data + if trigger == "btn-blocked": + return "blocked", data + if trigger == "btn-clear": + model.blocked = [] + return mode, {"w": model.w, "h": model.h, "robot": list(model.robot), "blocked": [list(p) for p in model.blocked]} + if trigger == "btn-reset": + model = GridModel() # back to defaults + return "robot", {"w": model.w, "h": model.h, "robot": list(model.robot), "blocked": [list(p) for p in model.blocked]} + return dash.no_update, dash.no_update + +# Handle clicks on the heatmap: place robot or toggle block +@app.callback( + Output("model_state", "data"), + Input("heatmap", "clickData"), + State("ui_mode", "data"), + State("model_state", "data"), + prevent_initial_call=True, +) + +def on_click(click_data, mode, data): + if not click_data or "points" not in click_data or not click_data["points"]: + return dash.no_update + pt = click_data["points"][0] + r = int(pose[1]) #int(pt["y"]) + c = int(pose[0]) #int(pt["x"]) + + model = GridModel( + w=data["w"], h=data["h"], + robot=tuple(data["robot"]), + blocked=[tuple(x) for x in data["blocked"]] + ) + + if mode == "robot": + model.set_robot(r, c) + else: + model.toggle_blocked(r, c) + + return {"w": model.w, "h": model.h, "robot": list(model.robot), "blocked": [list(p) for p in model.blocked]} + +# Sync kinematics inputs to state store +@app.callback( + Output("kinematics_state", "data"), + Input("input-max-velocity", "value"), + Input("input-acceleration", "value"), + Input("input-deceleration", "value"), + Input("input-turn-time", "value"), + prevent_initial_call=True, +) +def sync_kinematics(max_vel, accel, decel, turn): + def safe_float(val, default): + try: + v = float(val) + return v if v > 0 else default + except (TypeError, ValueError): + return default + + return { + "max_velocity": safe_float(max_vel, 19.685), + "acceleration": safe_float(accel, 13.1234), + "deceleration": safe_float(decel, 13.1234), + "turn_time": safe_float(turn, 0.2), + } + +# Redraw figure whenever the model or kinematics changes +@app.callback( + Output("heatmap", "figure"), + Input("model_state", "data"), + Input("kinematics_state", "data"), +) +def redraw(data, kin_data): + model = GridModel( + w=data["w"], h=data["h"], + robot=tuple(data["robot"]), + blocked=[tuple(x) for x in data["blocked"]] + ) + kinematics = RobotKinematics( + max_velocity=kin_data["max_velocity"], + acceleration=kin_data["acceleration"], + deceleration=kin_data["deceleration"], + turn_time=kin_data["turn_time"], + ) + return build_figure(model, kinematics) + +# Highlight active mode button +@app.callback( + Output("btn-robot", "className"), + Output("btn-blocked", "className"), + Input("ui_mode", "data"), +) +def highlight_active(mode): + base = "btn" + robot_cls = base + (" active" if mode == "robot" else "") + blocked_cls = base + (" active" if mode == "blocked" else "") + return robot_cls, blocked_cls + +@app.callback( + Output("save-status", "children"), + Input("btn-save", "n_clicks"), + State("model_state", "data"), + prevent_initial_call=True, +) +def save_layout(n, data): + try: + with open(LAYOUT_SAVE_PATH, "w", encoding="utf-8") as f: + json.dump(data, f, indent=2) + return f"Saved {datetime.now().strftime('%H:%M:%S')}" + except Exception as e: + return f"Save failed: {e}" + +if __name__ == "__main__": + app.run(debug=True, host='0.0.0.0', port=8050) diff --git a/src/main/java/frc/robot/subsystems/drive/heatmap/saved_layout.json b/src/main/java/frc/robot/subsystems/drive/heatmap/saved_layout.json new file mode 100644 index 0000000..1d65da8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/heatmap/saved_layout.json @@ -0,0 +1,282 @@ +{ + "w": 58, + "h": 29, + "robot": [ + 8, + 13 + ], + "blocked": [ + [ + 23, + 14 + ], + [ + 23, + 15 + ], + [ + 23, + 16 + ], + [ + 23, + 17 + ], + [ + 23, + 39 + ], + [ + 23, + 40 + ], + [ + 23, + 43 + ], + [ + 23, + 42 + ], + [ + 23, + 41 + ], + [ + 23, + 18 + ], + [ + 5, + 14 + ], + [ + 5, + 15 + ], + [ + 5, + 16 + ], + [ + 5, + 17 + ], + [ + 5, + 18 + ], + [ + 5, + 40 + ], + [ + 5, + 41 + ], + [ + 5, + 42 + ], + [ + 5, + 43 + ], + [ + 5, + 39 + ], + [ + 16, + 14 + ], + [ + 15, + 14 + ], + [ + 14, + 14 + ], + [ + 13, + 14 + ], + [ + 12, + 14 + ], + [ + 12, + 15 + ], + [ + 12, + 16 + ], + [ + 12, + 18 + ], + [ + 12, + 17 + ], + [ + 16, + 15 + ], + [ + 16, + 16 + ], + [ + 16, + 17 + ], + [ + 16, + 18 + ], + [ + 15, + 18 + ], + [ + 14, + 18 + ], + [ + 13, + 18 + ], + [ + 16, + 39 + ], + [ + 15, + 39 + ], + [ + 14, + 39 + ], + [ + 12, + 39 + ], + [ + 13, + 39 + ], + [ + 12, + 40 + ], + [ + 12, + 41 + ], + [ + 12, + 42 + ], + [ + 12, + 43 + ], + [ + 13, + 43 + ], + [ + 14, + 43 + ], + [ + 15, + 43 + ], + [ + 16, + 43 + ], + [ + 16, + 42 + ], + [ + 16, + 40 + ], + [ + 16, + 41 + ], + [ + 15, + 55 + ], + [ + 15, + 56 + ], + [ + 15, + 57 + ], + [ + 15, + 54 + ], + [ + 12, + 54 + ], + [ + 12, + 55 + ], + [ + 12, + 56 + ], + [ + 12, + 57 + ], + [ + 14, + 3 + ], + [ + 14, + 0 + ], + [ + 14, + 1 + ], + [ + 14, + 2 + ], + [ + 17, + 0 + ], + [ + 17, + 1 + ], + [ + 17, + 2 + ], + [ + 17, + 3 + ] + ] +} \ No newline at end of file From a3a1abda5693b8794808393c98ea02cb7b158a5a Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Tue, 10 Feb 2026 20:10:39 -0800 Subject: [PATCH 075/118] fixed annoying sim issues --- simgui-ds.json | 9 +- src/main/java/frc/robot/ControlsMapping.java | 16 +-- src/main/java/frc/robot/Robot.java | 2 +- .../robot/subsystems/TelemetryManager.java | 5 + .../drive/ctre/CtreDriveConstants.java | 2 +- .../frc/robot/subsystems/intake/Intake.java | 97 ++++++++++--------- .../subsystems/intake/IntakeConstants.java | 21 ++-- 7 files changed, 82 insertions(+), 70 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 5f7890c..543cd0e 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -16,10 +16,13 @@ "incKey": 68 }, { - "decayRate": 0.0, - "keyRate": 0.009999999776482582 + "decKey": 49, + "incKey": 50 + }, + { + "decKey": 51, + "incKey": 52 }, - {}, { "decKey": 74, "incKey": 76 diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index 8d0fd0f..0fad08a 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -29,21 +29,21 @@ public static void mapTeleopCommand() { Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER); controller.a().onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); - controller.leftBumper().whileTrue(Drive.getInstance().autoAlign(true)); - controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false)); - controller.x().whileTrue(Drive.getInstance().autopilotAlign(true)); - controller.y().whileTrue(Drive.getInstance().autopilotAlign(false)); + // controller.leftBumper().whileTrue(Drive.getInstance().autoAlign(true)); + // controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false)); + // controller.x().whileTrue(Drive.getInstance().autopilotAlign(true)); + // controller.y().whileTrue(Drive.getInstance().autopilotAlign(false)); controller.b().whileTrue(hangCommand()); // TODO: Implement hang from armaaan - controller.leftTrigger().whileTrue(intakeCommand()); - controller.rightTrigger().whileTrue(shooterCommand()); + controller.leftBumper().whileTrue(intakeCommand()); + controller.rightBumper().whileTrue(Intake.getInstance().stow()); } public static Command intakeCommand() { - return Intake.getInstance().setBarDown().andThen(Intake.getInstance().setWheelIntaking()); + return Intake.getInstance().intake(); } public static Command outtakeCommand() { - return Intake.getInstance().setBarDown().andThen(Intake.getInstance().setWheelOutaking()); + return Intake.getInstance().outtake(); } public static Command hangCommand() { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e7bfded..6549b04 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -143,7 +143,7 @@ public void testInit() { CommandScheduler.getInstance().cancelAll(); //map test commands - ControlsMapping.mapSysId(); + // ControlsMapping.mapSysId(); } /** This function is called periodically during test mode. */ diff --git a/src/main/java/frc/robot/subsystems/TelemetryManager.java b/src/main/java/frc/robot/subsystems/TelemetryManager.java index 6a1047d..6bfe4d7 100644 --- a/src/main/java/frc/robot/subsystems/TelemetryManager.java +++ b/src/main/java/frc/robot/subsystems/TelemetryManager.java @@ -119,6 +119,11 @@ public static void makeSendableTalonFX(String name, TalonFX motor, SendableBuild .getValue() .in(Units.Celsius), null); + builder.addStringProperty(name + "/Request", + () -> motor + .getAppliedControl() + .getName(), + null); } public void addSendable(Sendable sendable) { diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java index 7ac2351..2cccc46 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java @@ -65,7 +65,7 @@ public class CtreDriveConstants { private static final Slot0Configs driveGains = new Slot0Configs() .withKP(kP_ctre_drive) .withKI(0) - .withKD(kD_ctre_drive) + .withKD(0) .withKS(kS_ctre_drive) .withKV(kV_ctre_drive); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index c6e1692..d541ca8 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -25,6 +25,7 @@ import frc.robot.Constants; import frc.robot.Robot; import frc.robot.subsystems.TelemetryManager; +import frc.robot.subsystems.intake.IntakeConstants.Motors; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.simulation.BatterySim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; @@ -76,24 +77,24 @@ private Intake() { if (Robot.isSimulation()) { sim = new SingleJointedArmSim( - LinearSystemId.createSingleJointedArmSystem(DCMotor.getKrakenX60(1),1, 2), - DCMotor.getKrakenX60(1), - 2.0, - INTAKE_LENGTH, - BAR_POS_MIN, - BAR_POS_MAX, - true, - 0.0, - 0.0, 0.0 + DCMotor.getKrakenX60(1), + BAR_GEAR_RATIO, + 0.1756163, + INTAKE_LENGTH, + BAR_POS_MIN, + BAR_POS_MAX, + true, + BAR_POSITION_UP, + 0.0, 0.0 ); + barMotor.getSimState() + .setRawRotorPosition(sim.getAngleRads() * (1 / Constants.TAU)); } TelemetryManager.getInstance().addSendable(this); } - - @Override public void periodic(){ wheelSpeed = wheelMotor.getVelocity().getValueAsDouble(); @@ -102,6 +103,33 @@ public void periodic(){ wheelMotor.setControl(wheelRequest); //ligament.setAngle(barPosition); } + + @Override + public void simulationPeriodic() { + barMotor.getSimState().setSupplyVoltage(12.0); + sim.setInput(barMotor.getSimState().getMotorVoltage()); + sim.update(0.020); + barMotor.getSimState() + .setRotorVelocity(sim.getVelocityRadPerSec() * (1 / Constants.TAU) * BAR_GEAR_RATIO); + barMotor.getSimState() + .setRawRotorPosition(sim.getAngleRads() * (1 / Constants.TAU) * BAR_GEAR_RATIO); + } + + + public Command intake() { + return setSetpoint(INTAKE_SPEED, BAR_POSITION_DOWN) + .andThen(waitUntilBarIsAtPosition(BAR_POSITION_DOWN)); + } + + public Command outtake() { + return setSetpoint(-INTAKE_SPEED, BAR_POSITION_DOWN) + .andThen(waitUntilBarIsAtPosition(BAR_POSITION_DOWN)); + } + + public Command stow() { + return setSetpoint(0.0, BAR_POSITION_UP) + .andThen(waitUntilBarIsAtPosition(BAR_POSITION_UP)); + } //----------------set request--------------- private void setRequestWheel(ControlRequest request) { @@ -116,9 +144,8 @@ private void setRequestBar(ControlRequest request) { public Command setSetpoint(double wheelSpeed, double barPosition) { return setWheelSpeed(wheelSpeed) - .andThen(setBarPosition(barPosition), - Commands.print("something happen")) - .withName("Setpoint reached probably"); + .andThen(setBarPosition(barPosition)) + .withName("Setpoint: " + wheelSpeed + "rps, " + barPosition + "rot"); } @@ -147,15 +174,17 @@ public Command setBarUp() { */ public Command setBarPosition(double position) { double checkedPos = MathUtil.clamp(position, BAR_POS_MIN, BAR_POS_MAX); + + var req = new PositionVoltage(checkedPos); return runOnce(() -> - setRequestBar( - new PositionVoltage(checkedPos)) + setRequestBar(req) ).withName("bar pos set" + (checkedPos)); } public Command setWheelSpeed(double speed) { - return runOnce(() -> setRequestWheel( - new VelocityVoltage(speed)) + var req = new VelocityVoltage(speed); + return runOnce( + () -> setRequestWheel(req) ).withName("wheel speed set "+ (speed)); } @@ -165,39 +194,13 @@ public Command waitUntilBarIsAtPosition(double target) { public Command waitUntilWheelIsAtSpeed(double target) { return Commands.waitUntil(() -> Math.abs(target - wheelSpeed) < WHEEL_EPSILON); - } - @Override - public void simulationPeriodic() { - barMotor.getSimState().setSupplyVoltage(12.0); - sim.setInput(barMotor.getSimState().getMotorVoltage()); - sim.update(0.020); - barMotor.getSimState() - .setRawRotorPosition(sim.getAngleRads() * (1 / Constants.TAU)); - - - } + @Override public void initSendable(SendableBuilder builder){ super.initSendable(builder); builder.addDoubleProperty("Position", () -> barPosition, null); - TelemetryManager.makeSendableTalonFX("Intake123", barMotor, builder); - - } - - - public Command intake() { - return setSetpoint(INTAKE_SPEED, BAR_POSITION_DOWN) - .andThen(waitUntilBarIsAtPosition(BAR_POSITION_DOWN)); - } - - public Command outtake() { - return setSetpoint(-INTAKE_SPEED, BAR_POSITION_DOWN) - .andThen(waitUntilBarIsAtPosition(BAR_POSITION_DOWN)); - } - - public Command stow() { - return setSetpoint(0.0, BAR_POSITION_UP) - .andThen(waitUntilBarIsAtPosition(BAR_POSITION_UP)); + TelemetryManager.makeSendableTalonFX("Bar Motor", barMotor, builder); + TelemetryManager.makeSendableTalonFX("Wheel Motor", wheelMotor, builder); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 8fc8c03..a51fca6 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -8,24 +8,25 @@ import com.ctre.phoenix6.signals.GravityTypeValue; import edu.wpi.first.units.Units; +import frc.robot.Constants; public class IntakeConstants { public static final double BAR_EPSILON = Units.Degrees.of(5).in(Units.Rotations); public static final double WHEEL_EPSILON = 0.5; // rotations per second public static final double INTAKE_SPEED = 20; //rotations per second? // public static final double BAR_VOLTAGE = 4.0; //? - public static final double BAR_POSITION_DOWN = 0.33; - public static final double BAR_POSITION_UP = 0.0; - public static final double BAR_GEAR_RATIO = 30.0; + public static final double BAR_POSITION_DOWN = 0.00; + public static final double BAR_POSITION_UP = Constants.TAU / 4; + public static final double BAR_GEAR_RATIO = 50.0; public static final double BAR_POS_MIN = 0.0; - public static final double BAR_POS_MAX = 0.30; - public static final double INTAKE_MASS = 10.0; // kg, ideally - public static final double INTAKE_LENGTH = 0.5; //m, hopefully + public static final double BAR_POS_MAX = Constants.TAU / 4; + public static final double INTAKE_MASS = 3.656684786; // kg, ideally + public static final double INTAKE_LENGTH = 0.1746631508; //m, hopefully //etc public static enum Motors { //TODO: set motor ids; use separate file for ports? - WHEEL(-1), - BAR(-1); + WHEEL(35), + BAR(36); public final int id; private Motors(int id) { this.id = id; @@ -62,10 +63,10 @@ public static TalonFXConfiguration getBarConfig() { //TODO: values return new TalonFXConfiguration() .withSlot0(new Slot0Configs() .withKV(0.0) - .withKP(1.0) + .withKP(25.0) .withKI(0.0) .withKD(0.0) - .withKG(0.0).withGravityType(GravityTypeValue.Arm_Cosine)) + .withKG(0.1).withGravityType(GravityTypeValue.Arm_Cosine)) .withCurrentLimits(new CurrentLimitsConfigs() .withStatorCurrentLimit(30) .withSupplyCurrentLimit(30)) From 7cf548bb837858e38c8b9e843bc6c486f937adcc Mon Sep 17 00:00:00 2001 From: Arvind Venkatesh Date: Wed, 11 Feb 2026 15:58:40 -0800 Subject: [PATCH 076/118] fix: use ChassisAccels --- .../subsystems/shooter/ShotCalculator.java | 4 +- .../houndlib/ChassisAccelerations.java | 48 ------------------- .../houndlib/ShootOnTheFlyCalculator.java | 9 ++-- 3 files changed, 7 insertions(+), 54 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/shooter/houndlib/ChassisAccelerations.java diff --git a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java index 8fcd951..9385f32 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.shooter; -import frc.robot.subsystems.shooter.houndlib.ChassisAccelerations; import frc.robot.subsystems.shooter.houndlib.ShootOnTheFlyCalculator; import frc.robot.subsystems.shooter.houndlib.ShootOnTheFlyCalculator.InterceptSolution; import edu.wpi.first.math.geometry.Pose2d; @@ -8,6 +7,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +import frc.robot.lib.trajectory.RedTrajectory.State.ChassisAccels; import frc.robot.subsystems.drive.Drive; // stores current target and actively computes effective target @@ -40,7 +40,7 @@ public void periodic() { Pose3d shooterPose = new Pose3d(drivetrainPose).plus(ShooterConstants.OFFSET); ChassisSpeeds drivetrainSpeeds = drivetrain.getFieldSpeeds(); - ChassisAccelerations drivetrainAccelerations = new ChassisAccelerations(drivetrainSpeeds, drivetrain.getPrevFieldSpeeds(), Constants.DT); + ChassisAccels drivetrainAccelerations = ChassisAccels.estimate(drivetrainSpeeds, drivetrain.getPrevFieldSpeeds(), Constants.DT); currentInterceptSolution = ShootOnTheFlyCalculator.solveShootOnTheFly(shooterPose, targetLocation, drivetrainSpeeds, drivetrainAccelerations, targetSpeedRps, diff --git a/src/main/java/frc/robot/subsystems/shooter/houndlib/ChassisAccelerations.java b/src/main/java/frc/robot/subsystems/shooter/houndlib/ChassisAccelerations.java deleted file mode 100644 index 1cc18ec..0000000 --- a/src/main/java/frc/robot/subsystems/shooter/houndlib/ChassisAccelerations.java +++ /dev/null @@ -1,48 +0,0 @@ -package frc.robot.subsystems.shooter.houndlib; - -import edu.wpi.first.math.kinematics.ChassisSpeeds; - -/** - * Represents the complete acceleration data of a chassis. Counterpart to - * {@link ChassisSpeeds}. Used when acceleration data is needed for some - * computation (in 2024, shooting on the move). - */ -public class ChassisAccelerations { - public double axMetersPerSecondSquared; - public double ayMetersPerSecondSquared; - public double omegaRadiansPerSecondSquared; - - /** - * Creates a {@link ChassisAccelerations} object based on given acceleration - * data. - * - * @param axMetersPerSecondSquared the acceleration in the x direction, in - * m/s^2 - * @param ayMetersPerSecondSquared the acceleration in the y direction, in - * m/s^2 - * @param omegaRadiansPerSecondSquared the rotational acceleration of the - * chassis (CCW+), in rad/s^2 - */ - public ChassisAccelerations(double axMetersPerSecondSquared, double ayMetersPerSecondSquared, - double omegaRadiansPerSecondSquared) { - this.axMetersPerSecondSquared = axMetersPerSecondSquared; - this.ayMetersPerSecondSquared = ayMetersPerSecondSquared; - this.omegaRadiansPerSecondSquared = omegaRadiansPerSecondSquared; - } - - /** - * Creates a {@link ChassisAccelerations} object based on the current - * {@link ChassisSpeeds}, the {@link ChassisSpeeds} from the previous iteration, - * and the time between iterations. - * - * @param speed the current speed of the chassis - * @param previousSpeed the previous speed of the chassis - * @param dt the time elapsed between measurements (your loop time), - * in seconds - */ - public ChassisAccelerations(ChassisSpeeds speed, ChassisSpeeds previousSpeed, double dt) { - this.axMetersPerSecondSquared = (speed.vxMetersPerSecond - previousSpeed.vxMetersPerSecond) / dt; - this.ayMetersPerSecondSquared = (speed.vyMetersPerSecond - previousSpeed.vyMetersPerSecond) / dt; - this.omegaRadiansPerSecondSquared = (speed.omegaRadiansPerSecond - previousSpeed.omegaRadiansPerSecond) / dt; - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/houndlib/ShootOnTheFlyCalculator.java b/src/main/java/frc/robot/subsystems/shooter/houndlib/ShootOnTheFlyCalculator.java index 76b4ef2..cfe07f9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/houndlib/ShootOnTheFlyCalculator.java +++ b/src/main/java/frc/robot/subsystems/shooter/houndlib/ShootOnTheFlyCalculator.java @@ -2,6 +2,7 @@ import java.util.function.Function; +import frc.robot.lib.trajectory.RedTrajectory.State.ChassisAccels; import frc.robot.subsystems.shooter.houndlib.BallPhysics.ShotSolution; import edu.wpi.first.math.geometry.Pose2d; @@ -161,7 +162,7 @@ public static double getTimeToShoot( public static Pose3d calculateEffectiveTargetLocation( Pose2d robotPose, Pose3d targetPose, ChassisSpeeds fieldRelRobotVelocity, - ChassisAccelerations fieldRelRobotAcceleration, + ChassisAccels fieldRelRobotAcceleration, Function xyDistanceToProjectileVelocity, double goalPositionIterations, double accelerationCompensationFactor) { @@ -172,11 +173,11 @@ public static Pose3d calculateEffectiveTargetLocation( for (int i = 0; i < goalPositionIterations; i++) { double virtualGoalX = targetPose.getX() - shotTime * (fieldRelRobotVelocity.vxMetersPerSecond - + fieldRelRobotAcceleration.axMetersPerSecondSquared + + fieldRelRobotAcceleration.ax * accelerationCompensationFactor); double virtualGoalY = targetPose.getY() - shotTime * (fieldRelRobotVelocity.vyMetersPerSecond - + fieldRelRobotAcceleration.ayMetersPerSecondSquared + + fieldRelRobotAcceleration.ay * accelerationCompensationFactor); correctedTargetPose = new Pose3d(virtualGoalX, virtualGoalY, targetPose.getZ(), @@ -205,7 +206,7 @@ public static InterceptSolution solveShootOnTheFly( Pose3d shooterPose, Pose3d targetPose, ChassisSpeeds fieldRelRobotVelocity, - ChassisAccelerations fieldRelRobotAcceleration, + ChassisAccels fieldRelRobotAcceleration, double targetSpeedRps, int maxIterations, double timeTolerance) { From 0f57de3b3f2e2f878b24f817b6f9563d32dffa1c Mon Sep 17 00:00:00 2001 From: shakability Date: Wed, 11 Feb 2026 17:44:47 -0800 Subject: [PATCH 077/118] there's two now --- .../frc/robot/subsystems/shooter/Shooter.java | 36 ++++++++++++++----- .../subsystems/shooter/ShooterConstants.java | 6 ++-- 2 files changed, 32 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 414d0cc..26aa81b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -21,14 +21,22 @@ import frc.robot.subsystems.TelemetryManager; public class Shooter extends SubsystemBase { - private static Shooter ShooterInstance; - public static Shooter getInstance() { - if (ShooterInstance == null) { - ShooterInstance = new Shooter(); + private static Shooter shooterLeftInstance; + private static Shooter shooterRightInstance; + public static Shooter getLeftInstance() { + if (shooterLeftInstance == null) { + shooterLeftInstance = new Shooter(true); } - return ShooterInstance; + return shooterLeftInstance; } + + public static Shooter getRightInstance() { + if (shooterRightInstance == null) { + shooterRightInstance = new Shooter(false); + } + return shooterRightInstance; + } private final TalonFX topMotor; private final TalonFX bottomMotor; private double lastReadSpeedTop; @@ -39,14 +47,26 @@ public static Shooter getInstance() { private FlywheelSim topSim; private FlywheelSim bottomSim; - private Shooter() { + private Shooter(boolean left) { super(); - bottomMotor = new TalonFX(ShooterConstants.Motors.BOTTOM.id); + int bottomID; + int topID; + + if (left) { + bottomID = ShooterConstants.Motors.BOTTOMLEFT.id; + topID = ShooterConstants.Motors.TOPLEFT.id; + } + else { + bottomID = ShooterConstants.Motors.BOTTOMRIGHT.id; + topID = ShooterConstants.Motors.TOPRIGHT.id; + } + + bottomMotor = new TalonFX(bottomID); bottomMotor.getConfigurator().apply(ShooterConstants.getConfig()); bottomMotor.setNeutralMode(NeutralModeValue.Coast); - topMotor = new TalonFX(ShooterConstants.Motors.TOP.id); + topMotor = new TalonFX(topID); topMotor.getConfigurator().apply(ShooterConstants.getConfig()); topMotor.setNeutralMode(NeutralModeValue.Coast); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 32d9c8f..9bca810 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -10,8 +10,10 @@ public final class ShooterConstants { /** Motor ids */ public static enum Motors { - TOP(12), - BOTTOM(13); + TOPLEFT(12), + BOTTOMLEFT(13), + TOPRIGHT(12), + BOTTOMRIGHT(13); public final int id; private Motors(int id) { this.id = id; From f5b45d09372fb508185174423ba7342408fed3c3 Mon Sep 17 00:00:00 2001 From: audreywtu Date: Wed, 11 Feb 2026 17:53:16 -0800 Subject: [PATCH 078/118] small fixes --- .../frc/robot/subsystems/drive/heatmap/main.py | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/heatmap/main.py b/src/main/java/frc/robot/subsystems/drive/heatmap/main.py index db1f837..ad50975 100644 --- a/src/main/java/frc/robot/subsystems/drive/heatmap/main.py +++ b/src/main/java/frc/robot/subsystems/drive/heatmap/main.py @@ -20,22 +20,23 @@ inst = ntcore.NetworkTableInstance.getDefault() table = inst.getTable("SmartDashboard") -pose_topic = table.getDoubleArrayTopic("RobotPose") -subscriber = pose_topic.subscribe([0.0, 0.0, 0.0]) +pose_topic = table.getDoubleArrayTopic("Field/Robot") +subscriber = pose_topic.subscribe([0.0, 0.0, 0.0]) # may debatably swap current x, y order inst.startClient4("Python-Client") #Python-Monitor inst.setServerTeam(1458) print("Waiting for connection...") -while not inst.isConnected(): - x = 0 +#while not inst.isConnected(): + # x = 0 print("Connected! Streaming Pose:") try: - for i in range(10): #limit infinite loop for testing? + for i in range(10): # limit infinite loop for testing? pose = subscriber.get() - #print(pose[0], pose[1], pose[2]) + #print(pose) + #print(pose[0], pose[1], pose[2]) # cleaner except KeyboardInterrupt: pass #============================================ @@ -475,8 +476,9 @@ def on_click(click_data, mode, data): if not click_data or "points" not in click_data or not click_data["points"]: return dash.no_update pt = click_data["points"][0] - r = int(pose[1]) #int(pt["y"]) - c = int(pose[0]) #int(pt["x"]) + r = int(pose[1]) # non-updating: r = int(pt["y"]) | c = int(pt["x"]) + c = int(pose[0]) # int casting optional (still runs without error); float otherwise + #uselessRot = int(pose[2]) model = GridModel( w=data["w"], h=data["h"], From 12740a1023183515a53845fb9a0e88b3fbd95c8f Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Wed, 11 Feb 2026 18:39:08 -0800 Subject: [PATCH 079/118] Fixed indexer --- .../frc/robot/subsystems/indexer/Indexer.java | 137 ++++++++++-------- .../subsystems/indexer/IndexerConstants.java | 36 ++++- 2 files changed, 110 insertions(+), 63 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index dc63914..2a094e8 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -1,6 +1,9 @@ package frc.robot.subsystems.indexer; + +import static frc.robot.subsystems.indexer.IndexerConstants.*; import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.sim.TalonFXSimState; import edu.wpi.first.util.sendable.SendableBuilder; @@ -15,51 +18,64 @@ // TODO (ethan): ask tommy setControl(request) public class Indexer extends SubsystemBase { /** getInstance of indexer */ - private static Indexer instance; - public static Indexer getInstance() { - if (instance == null) { - instance = new Indexer(); + private static Indexer leftInstance; + public static Indexer getLeftInstance() { + if (leftInstance == null) { + leftInstance = new Indexer(true); + } + return leftInstance; + } + private static Indexer rightInstance; + public static Indexer getrightInstance() { + if (rightInstance == null) { + rightInstance = new Indexer(false); } - return instance; + return rightInstance; } private ControlRequest request; private TalonFX motor; private TalonFXSimState motorSim; - private LaserCan lc, lcTwo; + private LaserCan lc; /** boolean that's modified by checkForBall() */ private boolean hasBall; - private boolean shooterReady = false; + // private boolean shooterReady = false; // COMMENTED OUT /** boolean that's modified by checkForBallTwo() */ // private boolean hasBallTwo; /** boolean that's modified by shooter */ /** setup, adding motor and laser */ - private Indexer() { + private Indexer(boolean isLeft) { super(); - motor = new TalonFX(IndexerConstants.MOTOR_ID); + setName("Indexer " + (isLeft ? "Left" : "Right")); + motor = new TalonFX(isLeft ? L_MOTOR_ID : R_MOTOR_ID); + motor.getConfigurator().apply(getConfig()); motorSim = motor.getSimState(); - lc = new LaserCan(IndexerConstants.LASER_ID); - lcTwo = new LaserCan(IndexerConstants.LASER_ID_2); + lc = new LaserCan(isLeft ? L_LASER_ID : R_LASER_ID); + // lcTwo = new LaserCan(LASER_ID_2); /* new laser configs */ - try { - lc.setRangingMode(LaserCan.RangingMode.SHORT); - lc.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16)); - lc.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); - } catch (ConfigurationFailedException e) { - System.out.println("Configuration failed! " + e); - } - try { - lcTwo.setRangingMode(LaserCan.RangingMode.SHORT); - lcTwo.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16)); - lcTwo.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); - } catch (ConfigurationFailedException e) { - System.out.println("Configuration failed! " + e); + for (int i = 0; i < 20; i++) { + try { + lc.setRangingMode(LaserCan.RangingMode.SHORT); + lc.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16)); + lc.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); + break; + } catch (ConfigurationFailedException e) { + System.out.println("Configuration failed! " + e); + } } + // try { + // lcTwo.setRangingMode(LaserCan.RangingMode.SHORT); + // lcTwo.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16)); + // lcTwo.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); + // } catch (ConfigurationFailedException e) { + // System.out.println("Configuration failed! " + e); + // } + setDefaultCommand(loadIndexer()); } @Override @@ -69,9 +85,14 @@ public void periodic() { checkForBall(); // checkForBallTwo(); - if (shooterReady = true) { - activateIndexer(); - } + // if (shooterReady == true) { + // activateIndexer(); + // } + } + + @Override + public void simulationPeriodic() { + } /** type conversion/abstraction */ @@ -80,22 +101,26 @@ public void setRequest(ControlRequest request) { } /** sets speed (duh) */ - public Command setSpeed(double speed) { + public Command setSpeed(double speed) { return runOnce(() -> setRequest(new VelocityVoltage(speed))); } /** moves motor to speed if sense ball */ - public Command activateIndexer() { - return Commands.either ( - setSpeed(IndexerConstants.ActiveSpeed), - setSpeed(IndexerConstants.InactiveSpeed), + public Command loadIndexer() { + return Commands.either( + deactivateIndexer(), + activateIndexer(), this::hasBall - ); + ).repeatedly(); + } + + public Command activateIndexer() { + return setSpeed(ROLLING_SPEED); } /** turn motor down to zero */ public Command deactivateIndexer() { - return setSpeed(IndexerConstants.InactiveSpeed); + return setSpeed(0); } /** command to sense distance from LaserCAN; used to sense if bol */ @@ -104,18 +129,17 @@ private double getDistanceMm() { if (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) { return measurement.distance_mm; } else { - return -1; + return Double.POSITIVE_INFINITY; + } + } + + /** command that modifies hasBall, uses getDistanceMm() */ + private void checkForBall() { + double x = getDistanceMm(); + if (x <= MAXIMUM_LASER_DIST) { + hasBall = false; } } - // COMMENTED OUT /** command to sense if bol going into shooter */ - // private double getDistanceMmTwo() { - // LaserCan.Measurement measurement = lc.getMeasurement(); - // if (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) { - // return measurement.distance_mm; - // } else { - // return -1; - // } - // } /** setup formatting for boolean hasBall so it can be used in activateIndexer().either */ private boolean hasBall() { @@ -125,32 +149,31 @@ private boolean hasBall() { // private boolean hasBallTwo() { // return hasBallTwo; // } - - /** command that modifies hasBall, uses getDistanceMm() */ - private void checkForBall() { - double x = getDistanceMm(); - if (x >= IndexerConstants.LaserCan_DefaultMeasurement) { - hasBall = false; - } else if (x != -1) { /* (x != -1) is checking that the camera isn't just returning an error as ball sensed */ - hasBall = true; - } - } + // COMMENTED OUT /** command to sense if bol going into shooter */ + // private double getDistanceMmTwo() { + // LaserCan.Measurement measurement = lc.getMeasurement(); + // if (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) { + // return measurement.distance_mm; + // } else { + // return -1; + // } + // } // COMMENTED OUT //** command that checks if bol is boutta be shot */ // private void checkForBallTwo() { // double x = getDistanceMmTwo(); - // if (x >= IndexerConstants.LaserCan_DefaultMeasurement) { + // if (x >= LaserCan_DefaultMeasurement) { // hasBallTwo = false; // } else if (x != -1) { /* (x != -1) is checking that the camera isn't just returning an error as ball sensed */ // hasBallTwo = true; // } // } - @Override // TODO: AdvantageKit! /** ????????? */ + @Override public void initSendable(SendableBuilder builder) { super.initSendable(builder); builder.addBooleanProperty("Has Ball", () -> hasBall, null); - TelemetryManager.makeSendableTalonFX("Indexer motor", motor, builder); + TelemetryManager.makeSendableTalonFX("Indexer Motor", motor, builder); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java index 5529e6f..b3e1e4a 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java @@ -1,14 +1,38 @@ package frc.robot.subsystems.indexer; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.VoltageConfigs; + public class IndexerConstants { // TODO: Set IDs // TODO (Ethan): Set LaserCan_DefaultMeasurement - public static final int MOTOR_ID = 0; - public static final int LASER_ID = 0; - public static final int LASER_ID_2 = 0; - public static final double ActiveSpeed = 0; - public static final double InactiveSpeed = 0; - public static final double LaserCan_DefaultMeasurement = 1; + public static final int L_MOTOR_ID = 32; + public static final int L_LASER_ID = 36; + public static final int R_MOTOR_ID = 32; + public static final int R_LASER_ID = 36; + // public static final int LASER_ID_2 = 0; + public static final double ROLLING_SPEED = 10; // rps + public static final double MAXIMUM_LASER_DIST = 100; + + public static TalonFXConfiguration getConfig() { + return new TalonFXConfiguration() + .withSlot0(new Slot0Configs() + .withKV(0.0) + .withKP(0.3) + .withKI(0.001) + .withKD(0.0) + .withKA(0.1) + .withKS(0.1) + .withKV(0.1)) // placeholder values + .withCurrentLimits(new CurrentLimitsConfigs() + .withStatorCurrentLimit(40) + .withSupplyCurrentLimit(40)) + .withVoltage(new VoltageConfigs() + .withPeakForwardVoltage(12.0) + .withPeakReverseVoltage(-12.0)); + } } \ No newline at end of file From b09c15b9d3cdab183a1bd8797a49fbed2dbb4a59 Mon Sep 17 00:00:00 2001 From: Arvind Venkatesh Date: Wed, 11 Feb 2026 19:19:22 -0800 Subject: [PATCH 080/118] feat: lock --- .../java/frc/robot/subsystems/shooter/Shooter.java | 9 ++++++--- .../frc/robot/subsystems/shooter/ShotCalculator.java | 11 ++++++++++- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 692c6c3..42bfa97 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -6,11 +6,9 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; - import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.simulation.BatterySim; import edu.wpi.first.wpilibj.simulation.FlywheelSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; @@ -19,6 +17,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.subsystems.TelemetryManager; +import frc.robot.subsystems.drive.Drive; public class Shooter extends SubsystemBase { private static Shooter ShooterInstance; @@ -39,6 +38,8 @@ public static Shooter getInstance() { private FlywheelSim topSim; private FlywheelSim bottomSim; + private ShotCalculator shotCalculator = ShotCalculator.getInstance(); + private Shooter() { super(); @@ -128,7 +129,9 @@ public Command shoot(double topSpeed, double bottomSpeed) { } public Command shoot() { - return shoot(3000 / 60.0, 3000 / 60.0); + return Drive.getInstance() + .headingLockToPose(shotCalculator.getCurrentEffectiveTargetPose().toPose2d()) + .andThen(shoot(3000 / 60.0, 3000 / 60.0)); } @Override diff --git a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java index 9385f32..030a985 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java @@ -12,6 +12,15 @@ // stores current target and actively computes effective target public class ShotCalculator extends SubsystemBase { + private static ShotCalculator calcInstance; + + public static ShotCalculator getInstance() { + if (calcInstance == null) { + calcInstance = new ShotCalculator(); + } + return calcInstance; + } + private final Drive drivetrain; private Pose3d currentEffectiveTargetPose = Pose3d.kZero; @@ -26,7 +35,7 @@ public class ShotCalculator extends SubsystemBase { private double targetSpeedRps = 8; - public ShotCalculator() { + private ShotCalculator() { this.drivetrain = Drive.getInstance(); } From 492978f37768f6459d57bd8e92ccf665f91764c9 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Sat, 14 Feb 2026 23:12:27 -0800 Subject: [PATCH 081/118] Robot ready hopefully, not can ids tho --- src/main/java/frc/robot/ControlsMapping.java | 46 +++++--- src/main/java/frc/robot/Robot.java | 9 +- .../frc/robot/subsystems/climb/Climb.java | 71 ++++++------ .../subsystems/climb/ClimbConstants.java | 29 ++--- .../frc/robot/subsystems/drive/Drive.java | 10 +- .../frc/robot/subsystems/indexer/Indexer.java | 34 +++++- .../subsystems/indexer/IndexerConstants.java | 8 +- .../frc/robot/subsystems/intake/Intake.java | 37 ++++-- .../subsystems/intake/IntakeConstants.java | 6 +- .../frc/robot/subsystems/roller/Roller.java | 108 ++++++++++++++++++ .../subsystems/roller/RollerConstants.java | 26 +++++ .../frc/robot/subsystems/shooter/Shooter.java | 9 +- .../subsystems/shooter/ShooterConstants.java | 8 +- .../robot/subsystems/vision/VisionDevice.java | 2 - .../vision/VisionDeviceManager.java | 4 +- 15 files changed, 298 insertions(+), 109 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/roller/Roller.java create mode 100644 src/main/java/frc/robot/subsystems/roller/RollerConstants.java diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index 9d1c838..9df6921 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -4,34 +4,52 @@ import com.ctre.phoenix6.swerve.SwerveRequest; -import com.ctre.phoenix6.swerve.SwerveRequest; - import edu.wpi.first.math.geometry.*; -import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import frc.robot.lib.field.FieldUtil; +import frc.robot.subsystems.climb.Climb; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveConstants; import frc.robot.subsystems.drive.ctre.CtreDrive.SysIdRoutineType; +import frc.robot.subsystems.indexer.Indexer; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.vision.VisionDeviceManager; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; public class ControlsMapping { public static void mapTeleopCommand() { Drive.getInstance().setDefaultCommand((Drive.getInstance().openLoopControl())); - // run sysID functions - Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER); + Intake.getInstance().setDefaultCommand(Intake.getInstance().intake()); - controller.a().onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); + controller.back().and(controller.a()).onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); + controller.back().and(controller.b()).onTrue(VisionDeviceManager.getInstance().bootUp()); + + controller.leftTrigger().debounce(0.1).whileTrue(Intake.getInstance().outtake()); + controller.b().whileTrue(Intake.getInstance().outtake()); + controller.x().onTrue(Climb.getInstance().hangCommand()); + + controller.rightTrigger().debounce(0.1).onTrue( + Commands.parallel( + Shooter.getLeftInstance().shoot(), + Shooter.getRightInstance().shoot()) + ).onFalse( + Commands.parallel( + Shooter.getLeftInstance().stop(), + Shooter.getRightInstance().stop()) + ).whileTrue( + Commands.parallel( + Indexer.getLeftInstance().activateIndexer(), + Indexer.getRightInstance().activateIndexer()) + ); - controller.b().whileTrue(Drive.getInstance().headingLockToPose(DriveConstants.FieldPoses.TAG.pose)); - controller.x().onTrue(Drive.getInstance().pathFindToThisRandomPlaceIdk()); - // controller.leftBumper().whileTrue(Drive.getInstance().autoAlign(true)); - // controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false)); - // controller.x().whileTrue(Drive.getInstance().autopilotAlign(true)); - // controller.y().whileTrue(Drive.getInstance().autopilotAlign(false)); - controller.y().onTrue(VisionDeviceManager.getInstance().bootUp()); + controller.leftBumper().whileTrue(Drive.getInstance().headingLockToPose( + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? + DriveConstants.FieldPoses.HUB.pose : + FieldUtil.flipPose(DriveConstants.FieldPoses.HUB.pose))); } public static void mapSysId() { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d783862..31a5327 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -23,8 +23,11 @@ import frc.robot.Constants.Controllers; import frc.robot.auto.AutoSelector; import frc.robot.subsystems.TelemetryManager; +import frc.robot.subsystems.climb.Climb; import frc.robot.subsystems.drive.*; +import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.roller.Roller; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.vision.VisionDeviceManager; @@ -55,8 +58,11 @@ public Robot() { Drive.getInstance(); Shooter.getLeftInstance(); Shooter.getRightInstance(); - + Indexer.getLeftInstance(); + Indexer.getRightInstance(); Intake.getInstance(); + Climb.getInstance(); + Roller.getInstance(); TelemetryManager.getInstance(); commandScheduler.schedule(FollowPathCommand.warmupCommand()); @@ -126,6 +132,7 @@ public void autonomousExit() { @Override public void teleopInit() { + ControlsMapping.mapTeleopCommand(); // This makes sure that the autonomous stops running when teleop starts running. if (autoCommand != null) { autoCommand.cancel(); diff --git a/src/main/java/frc/robot/subsystems/climb/Climb.java b/src/main/java/frc/robot/subsystems/climb/Climb.java index 046ae92..45f8032 100644 --- a/src/main/java/frc/robot/subsystems/climb/Climb.java +++ b/src/main/java/frc/robot/subsystems/climb/Climb.java @@ -1,18 +1,11 @@ package frc.robot.subsystems.climb; import static edu.wpi.first.units.Units.*; -import static frc.robot.subsystems.climb.ClimbConstants.CLIMB_MOTOR_ID; -import static frc.robot.subsystems.climb.ClimbConstants.END_EFFECTOR_HEIGHT; -import static frc.robot.subsystems.climb.ClimbConstants.EPSILON; -import static frc.robot.subsystems.climb.ClimbConstants.GEAR_RATIO; -import static frc.robot.subsystems.climb.ClimbConstants.getConfig; -import static frc.robot.subsystems.climb.ClimbConstants.metersToRotations; -import static frc.robot.subsystems.climb.ClimbConstants.rotationsToMeters; +import static frc.robot.subsystems.climb.ClimbConstants.*; import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.NeutralOut; -import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -24,6 +17,8 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants; +import frc.robot.Robot; import frc.robot.subsystems.TelemetryManager; import frc.robot.subsystems.climb.ClimbConstants.Setpoint; @@ -46,38 +41,50 @@ public static Climb getInstance() { private double targetHeight = END_EFFECTOR_HEIGHT; private ControlRequest request = new NeutralOut(); - private final ElevatorSim sim = new edu.wpi.first.wpilibj.simulation.ElevatorSim( - DCMotor.getKrakenX60(1), - GEAR_RATIO, - 1.13, - 0.05, - Setpoint.BASE.height, - Setpoint.UP.height, - true, - 0.0, - 0.0,0.0 - ); + private ElevatorSim sim; private Climb() { super(); climbMotor = new TalonFX(CLIMB_MOTOR_ID); climbMotor.getConfigurator().apply(getConfig()); climbMotor.setNeutralMode(NeutralModeValue.Brake); + if (Robot.isSimulation()) { + sim = new ElevatorSim( + DCMotor.getKrakenX60(1), + GEAR_RATIO, + 1.13, + 0.05, + 0.0, + 1, + true, + 0.0, + 0.0, 0.0 + ); + } TelemetryManager.getInstance().addSendable(this); - setDefaultCommand(stop()); } @Override public void periodic() { // Read the height from the motor encoder - lastReadHeight = rotationsToMeters( - climbMotor.getPosition().getValueAsDouble()); - lastReadSpeed = rotationsToMeters( - climbMotor.getVelocity().getValueAsDouble()); + lastReadHeight = + climbMotor.getPosition().getValueAsDouble(); + lastReadSpeed = + climbMotor.getVelocity().getValueAsDouble(); // updates the motor climbMotor.setControl(request); } + @Override + public void simulationPeriodic() { + climbMotor.getSimState().setSupplyVoltage(12); + sim.setInput(climbMotor.getSimState().getMotorVoltage()); + sim.update(0.020); + climbMotor.getSimState().setRawRotorPosition(sim.getPositionMeters() * CONVERSION_FACTOR * 1 / Constants.TAU); + climbMotor.getSimState().setRotorVelocity( + sim.getVelocityMetersPerSecond() * CONVERSION_FACTOR * 1 / Constants.TAU); + } + /** Swaps the control request */ private void setRequest(ControlRequest request) { this.request = request; @@ -93,8 +100,7 @@ private Command moveToTarget(double targetHeight) { () -> { this.targetHeight = targetHeight; setRequest(new MotionMagicVoltage( - metersToRotations( - targetHeight))); + targetHeight)); } ).andThen( Commands.waitUntil(() -> isNearTarget()) @@ -105,8 +111,7 @@ private Command moveToTarget(double targetHeight) { public Command stop() { return runOnce( () -> setRequest( - new PositionVoltage( - metersToRotations(targetHeight)))) + new NeutralOut())) .withName("Stopped"); } @@ -129,11 +134,10 @@ public Command sysId(boolean dynamic, SysIdRoutine.Direction direction) { .voltage(Volts.of(request.Output)) .linearPosition( Meters.of( - rotationsToMeters( - climbMotor.getPosition().getValueAsDouble()))) + climbMotor.getPosition().getValueAsDouble())) .linearVelocity( MetersPerSecond.of( - rotationsToMeters(climbMotor.getVelocity().getValueAsDouble()))); + climbMotor.getVelocity().getValueAsDouble())); }, this ) @@ -174,7 +178,8 @@ public void initSendable(SendableBuilder builder) { // command - public static Command HangCommand() { - return Climb.getInstance().moveToScoringHeight(ClimbConstants.Setpoint.UP).andThen(Climb.getInstance().moveToScoringHeight(ClimbConstants.Setpoint.BASE)); + public Command hangCommand() { + return moveToScoringHeight(Setpoint.UP) + .andThen(Climb.getInstance().moveToScoringHeight(Setpoint.BASE)); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java b/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java index 5ba54ec..b7ddf65 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java @@ -1,12 +1,12 @@ package frc.robot.subsystems.climb; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.VoltageConfigs; -import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; import frc.robot.Constants; @@ -16,8 +16,10 @@ public class ClimbConstants { public static final double SPROCKET_RADIUS = 0.0412; // Effective pitch radius public static final double GEAR_RATIO = 9; + public static final double CONVERSION_FACTOR = 67; + public static final double SPROCKET_CIRCUMFERENCE = SPROCKET_RADIUS * Constants.TAU; - public static final double END_EFFECTOR_HEIGHT = 0.54; // Meters // CHANGE THIS + public static final double END_EFFECTOR_HEIGHT = 0.0; // Meters // CHANGE THIS public static final double METERS_PER_ROTATION = 0.028776; // Approximated using measurement public static final double CARRIAGE_WEIGHT = 7.55; // kg @@ -26,11 +28,11 @@ public class ClimbConstants { public static final double MAX_ACCEL = 1.5; public static final double MAX_SPEED = 1.0; // m/s - public static final int CLIMB_MOTOR_ID = 67; // change this + public static final int CLIMB_MOTOR_ID = 25; // change this public static enum Setpoint { BASE(0.003), // small offset to prevent stalling (allegedly) - UP(Units.inchesToMeters(27.0)); + UP(Units.inchesToMeters(5.0)); public final double height; private Setpoint(double height) { this.height = height; @@ -47,25 +49,16 @@ public static TalonFXConfiguration getConfig() { .withKD(0.05) .withKG(0.375)) .withMotionMagic(new MotionMagicConfigs() - .withMotionMagicAcceleration(metersToRotations(MAX_ACCEL)) // 1.0 m/s^2 - .withMotionMagicCruiseVelocity(metersToRotations(MAX_SPEED)) + .withMotionMagicAcceleration(MAX_ACCEL) // 1.0 m/s^2 + .withMotionMagicCruiseVelocity(MAX_SPEED) .withMotionMagicJerk(320)) .withCurrentLimits(new CurrentLimitsConfigs() .withStatorCurrentLimit(60) .withSupplyCurrentLimit(60)) .withVoltage(new VoltageConfigs() .withPeakForwardVoltage(12.0) - .withPeakReverseVoltage(-12.0)); - } - - /** Conversion utility */ - public static double rotationsToMeters(double rotations) { - return rotations * METERS_PER_ROTATION; + .withPeakReverseVoltage(-12.0)) + .withFeedback(new FeedbackConfigs() + .withSensorToMechanismRatio(CONVERSION_FACTOR)); } - - /** Conversion utility */ - public static double metersToRotations(double meters) { - return meters / METERS_PER_ROTATION; - } - } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 9685396..69d0791 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -4,7 +4,6 @@ import java.util.function.Consumer; import java.util.function.Function; -import java.util.function.Supplier; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; @@ -16,7 +15,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; +// import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -32,7 +31,6 @@ import frc.robot.Robot; import frc.robot.lib.field.FieldLayout; import frc.robot.lib.trajectory.LocalADStarWrapper; -import frc.robot.lib.util.TunableNumber; import frc.robot.lib.util.Util; import frc.robot.subsystems.TelemetryManager; import frc.robot.subsystems.drive.ctre.CtreDriveConstants; @@ -41,7 +39,7 @@ import frc.robot.subsystems.drive.commands.TrajectoryCommand; import frc.robot.subsystems.drive.ctre.CtreDrive; import frc.robot.subsystems.drive.ctre.CtreDriveTelemetry; -import frc.robot.subsystems.vision.VisionConstants; +// import frc.robot.subsystems.vision.VisionConstants; public class Drive extends SubsystemBase { private static Drive driveInstance; @@ -65,8 +63,6 @@ public static Drive getInstance() { private Drive() { drivetrain = CtreDriveConstants.createDrivetrain(); - drivetrain.setVisionMeasurementStdDevs(VisionConstants.LOCAL_MEASUREMENT_STD_DEVS); - drivetrain.setStateStdDevs(VisionConstants.STATE_STD_DEVS); telemetry = new CtreDriveTelemetry(MAX_SPEED); teleopRequest = new SwerveRequest.FieldCentric(); driveRequest = teleopRequest; @@ -264,7 +260,7 @@ public Command headingLockToPoseWithTOFAdjustment(Pose2d pose, Function setRequest(req) + ).withName(speed + ":Speed"); + } + + public Command roll() { + return setSpeed(ROLL_SPEED); + } + + public Command stop() { + return runOnce(() -> setRequest(new NeutralOut())) + .withName("Stop"); + } + + @Override + public void initSendable(SendableBuilder builder){ + super.initSendable(builder); + builder.addDoubleProperty("Speed", () -> speed, null); + TelemetryManager.makeSendableTalonFX("Roller Motor", motor, builder); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/roller/RollerConstants.java b/src/main/java/frc/robot/subsystems/roller/RollerConstants.java new file mode 100644 index 0000000..75c594a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/roller/RollerConstants.java @@ -0,0 +1,26 @@ +package frc.robot.subsystems.roller; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.VoltageConfigs; + +public class RollerConstants { + public static final int MOTOR_ID = 54; + public static final double ROLL_SPEED = 20; + + public static TalonFXConfiguration getConfig() { + return new TalonFXConfiguration() + .withSlot0(new Slot0Configs() + .withKV(0.0) + .withKP(1.0) + .withKI(0.0) + .withKD(0.0)) + .withCurrentLimits(new CurrentLimitsConfigs() + .withStatorCurrentLimit(30) + .withSupplyCurrentLimit(30)) + .withVoltage(new VoltageConfigs() + .withPeakForwardVoltage(12.0) + .withPeakReverseVoltage(-12.0)); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 26aa81b..e149822 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -10,12 +10,10 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.simulation.BatterySim; import edu.wpi.first.wpilibj.simulation.FlywheelSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.subsystems.TelemetryManager; @@ -50,6 +48,8 @@ public static Shooter getRightInstance() { private Shooter(boolean left) { super(); + setName(this.getClass().getSimpleName() + (left ? "left" : "right")); + int bottomID; int topID; @@ -85,7 +85,6 @@ private Shooter(boolean left) { ), DCMotor.getKrakenX60(1), 0.0); } TelemetryManager.getInstance().addSendable(this); - setDefaultCommand(stop()); } @Override @@ -144,11 +143,11 @@ public Command shoot(double topSpeed, double bottomSpeed) { return runOnce(() -> { setTopRequest(new VelocityVoltage(topSpeed)); setBottomRequest(new VelocityVoltage(bottomSpeed)); - }).andThen(Commands.repeatingSequence(Commands.none())).withName("Shooting"); + }).withName("Shooting"); } public Command shoot() { - return shoot(512, -512); + return shoot(50, -50); } @Override diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 9bca810..54852ab 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -10,10 +10,10 @@ public final class ShooterConstants { /** Motor ids */ public static enum Motors { - TOPLEFT(12), - BOTTOMLEFT(13), - TOPRIGHT(12), - BOTTOMRIGHT(13); + TOPLEFT(58), + BOTTOMLEFT(57), + TOPRIGHT(56), + BOTTOMRIGHT(55); public final int id; private Motors(int id) { this.id = id; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionDevice.java b/src/main/java/frc/robot/subsystems/vision/VisionDevice.java index 037bb4a..1c16690 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionDevice.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionDevice.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.vision; -import frc.robot.subsystems.TelemetryManager; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.vision.VisionConstants.VisionDeviceConstants; import edu.wpi.first.math.Matrix; @@ -14,7 +13,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.lib.field.FieldLayout; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java b/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java index 535ec67..477e132 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java @@ -15,8 +15,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.List; -import java.util.stream.Collectors; - import org.photonvision.simulation.VisionSystemSim; public class VisionDeviceManager extends SubsystemBase { @@ -58,6 +56,8 @@ public VisionDeviceManager() { visionSim.addAprilTags(FieldLayout.APRILTAG_MAP); cameras.forEach((camera) -> visionSim.addCamera(camera.getSimulation(), camera.getConstants().robotToCamera)); } + Drive.getInstance().getCtreDrive().setVisionMeasurementStdDevs(LOCAL_MEASUREMENT_STD_DEVS); + Drive.getInstance().getCtreDrive().setStateStdDevs(STATE_STD_DEVS); TelemetryManager.getInstance().addSendable(this); } From 89d72e1279ac032e4755dc336a4f8f45817034e1 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Sun, 15 Feb 2026 14:24:27 -0800 Subject: [PATCH 082/118] no more laser can >:( --- .../frc/robot/subsystems/indexer/Indexer.java | 82 +++++++++---------- 1 file changed, 41 insertions(+), 41 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 0d399ed..09d24a0 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -38,10 +38,10 @@ public static Indexer getRightInstance() { private ControlRequest request; private TalonFX motor; - private LaserCan lc; + // private LaserCan lc; /** boolean that's modified by checkForBall() */ - private boolean hasBall; + // private boolean hasBall; private FlywheelSim wheelSim; @@ -57,21 +57,21 @@ private Indexer(boolean isLeft) { setName("Indexer " + (isLeft ? "Left" : "Right")); motor = new TalonFX(isLeft ? L_MOTOR_ID : R_MOTOR_ID); motor.getConfigurator().apply(getConfig()); - lc = new LaserCan(isLeft ? L_LASER_ID : R_LASER_ID); + // lc = new LaserCan(isLeft ? L_LASER_ID : R_LASER_ID); // lcTwo = new LaserCan(LASER_ID_2); /* new laser configs */ - if (Robot.isReal()) { - for (int i = 0; i < 20; i++) { - try { - lc.setRangingMode(LaserCan.RangingMode.SHORT); - lc.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16)); - lc.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); - break; - } catch (ConfigurationFailedException e) { - System.out.println("Configuration failed! " + e); - } - }} + // if (Robot.isReal()) { + // for (int i = 0; i < 20; i++) { + // try { + // lc.setRangingMode(LaserCan.RangingMode.SHORT); + // lc.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16)); + // lc.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); + // break; + // } catch (ConfigurationFailedException e) { + // System.out.println("Configuration failed! " + e); + // } + // }} if (Robot.isSimulation()) { wheelSim = new FlywheelSim( @@ -91,7 +91,7 @@ private Indexer(boolean isLeft) { // } TelemetryManager.getInstance().addSendable(this); - setDefaultCommand(loadIndexer()); + setDefaultCommand(deactivateIndexer()); } @Override @@ -130,13 +130,13 @@ public Command setSpeed(double speed) { } /** moves motor to speed if sense ball */ - public Command loadIndexer() { - return Commands.either( - deactivateIndexer(), - activateIndexer(), - this::hasBall - ).repeatedly(); - } + // public Command loadIndexer() { + // return Commands.either( + // deactivateIndexer(), + // activateIndexer(), + // this::hasBall + // ).repeatedly(); + // } public Command activateIndexer() { return setSpeed(ROLLING_SPEED); @@ -148,27 +148,27 @@ public Command deactivateIndexer() { } /** command to sense distance from LaserCAN; used to sense if bol */ - private double getDistanceMm() { - LaserCan.Measurement measurement = lc.getMeasurement(); - if (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) { - return measurement.distance_mm; - } else { - return Double.POSITIVE_INFINITY; - } - } + // private double getDistanceMm() { + // LaserCan.Measurement measurement = lc.getMeasurement(); + // if (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) { + // return measurement.distance_mm; + // } else { + // return Double.POSITIVE_INFINITY; + // } + // } /** command that modifies hasBall, uses getDistanceMm() */ - private void checkForBall() { - double x = getDistanceMm(); - if (x <= MAXIMUM_LASER_DIST) { - hasBall = false; - } - } + // private void checkForBall() { + // double x = getDistanceMm(); + // if (x <= MAXIMUM_LASER_DIST) { + // hasBall = false; + // } + // } - /** setup formatting for boolean hasBall so it can be used in activateIndexer().either */ - private boolean hasBall() { - return hasBall; - } + // /** setup formatting for boolean hasBall so it can be used in activateIndexer().either */ + // private boolean hasBall() { + // return hasBall; + // } // COMMENTED OUT /** setup formatting for boolean hasBall so that it can be used to tell if shooter ready */ // private boolean hasBallTwo() { // return hasBallTwo; @@ -197,7 +197,7 @@ private boolean hasBall() { @Override public void initSendable(SendableBuilder builder) { super.initSendable(builder); - builder.addBooleanProperty("Has Ball", () -> hasBall, null); + // builder.addBooleanProperty("Has Ball", () -> hasBall, null); TelemetryManager.makeSendableTalonFX("Indexer Motor", motor, builder); } } \ No newline at end of file From a17f0d2e3ee34733364081d418a69168a2dadd65 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Sun, 15 Feb 2026 14:25:27 -0800 Subject: [PATCH 083/118] i forgor to pop the stash --- .../java/frc/robot/lib/field/FieldUtil.java | 1 + .../frc/robot/subsystems/drive/Drive.java | 42 ++++++++++++++----- .../subsystems/vision/VisionConstants.java | 4 +- 3 files changed, 35 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/lib/field/FieldUtil.java b/src/main/java/frc/robot/lib/field/FieldUtil.java index c8666d4..9a7cc70 100644 --- a/src/main/java/frc/robot/lib/field/FieldUtil.java +++ b/src/main/java/frc/robot/lib/field/FieldUtil.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; import frc.robot.lib.trajectory.RedTrajectory.State.ChassisAccels; public class FieldUtil { diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 69d0791..a3081e9 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -19,6 +19,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.units.BaseUnits; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Time; @@ -29,6 +30,10 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Robot; +import frc.robot.lib.control.ControlConstants.PIDVConstants; +import frc.robot.lib.control.ControlConstants.ProfiledPIDVConstants; +import frc.robot.lib.control.PIDVController; +import frc.robot.lib.control.ProfiledPIDVController; import frc.robot.lib.field.FieldLayout; import frc.robot.lib.trajectory.LocalADStarWrapper; import frc.robot.lib.util.Util; @@ -184,14 +189,25 @@ public Command openLoopControl() { * Utilizes feedforwards derived from the current chassis speeds */ public Command headingLockToPose(Pose2d pose) { - SwerveRequest.FieldCentricFacingAngle request = - new SwerveRequest.FieldCentricFacingAngle() - .withHeadingPID(8, 0, 0.00) - .withMaxAbsRotationalRate(MAX_ROTATION_SPEED); + SwerveRequest.FieldCentric request = + new SwerveRequest.FieldCentric(); + + ProfiledPIDVController thetaController = + new ProfiledPIDVController( + new ProfiledPIDVConstants( + new PIDVConstants(10.0, 0.0, 1), + new TrapezoidProfile.Constraints(Math.PI * 16, Math.PI * 5)) + ); + thetaController.enableContinuousInput(-Math.PI, Math.PI); return runOnce(() -> { - request.withVelocityX(0).withVelocityY(0).withTargetDirection(getPose().getRotation()); + request.withVelocityX(0).withVelocityY(0) + .withRotationalRate(0); setSwerveRequest(request); + + thetaController.setInitialSetpoint( + getPose().getRotation().getRadians(), + getState().Speeds.omegaRadiansPerSecond); }).andThen( run(() -> { double xDesiredRaw = -Robot.controller.getLeftY(); @@ -204,23 +220,29 @@ public Command headingLockToPose(Pose2d pose) { var state = getState(); var delta = pose.getTranslation().minus(getPose().getTranslation()); var targetDirection = delta.getAngle(); + + var normSq = delta.getNorm() * delta.getNorm(); var fieldSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(state.Speeds, getPose().getRotation()); var rotationalRate = normSq > 1e-4 ? (-delta.getX() * fieldSpeeds.vyMetersPerSecond + delta.getY() * fieldSpeeds.vxMetersPerSecond) / (normSq) : 0.0; + + var rotation = thetaController + .setTarget(targetDirection.getRadians(), rotationalRate) + .setMeasurement(state.Pose.getRotation().getRadians(), state.Speeds.omegaRadiansPerSecond) + .getOutput(); - // SmartDashboard.putNumber("error tracking", - // MathUtil.inputModulus(state.Pose.getRotation().minus(targetDirection).getDegrees(), -180, 180 - // )); + SmartDashboard.putNumber("error tracking", + MathUtil.inputModulus(state.Pose.getRotation().minus(targetDirection).getDegrees(), -180, 180 + )); request // .withHeadingPID(p.get(), i.get(), d.get()) .withVelocityX(xFancy * MAX_SPEED) .withVelocityY(yFancy * MAX_SPEED) - .withTargetDirection(targetDirection) - .withTargetRateFeedforward(rotationalRate * 1.0); + .withRotationalRate(rotation); }).handleInterrupt(() -> setSwerveRequest(new SwerveRequest.FieldCentric()))).withName("Heading Lock"); } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index f4d64f6..24320ae 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -18,8 +18,8 @@ public class VisionConstants { Math.pow(0.02, 1)); // drive public static final Matrix LOCAL_MEASUREMENT_STD_DEVS = VecBuilder.fill( - Math.pow(0.2, 1), // vision - Math.pow(0.2, 1), + Math.pow(0.35, 1), // vision + Math.pow(0.35, 1), Math.pow(Double.POSITIVE_INFINITY, 1)); public static enum VisionDeviceConstants { From 9be5a5d570c4ba62c18739b7b6a4e4d80869ba71 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Mon, 16 Feb 2026 18:49:27 -0800 Subject: [PATCH 084/118] Actually added advantagekit correctly for once and for all --- build.gradle | 47 ++++ src/main/java/frc/robot/BuildConstants.java | 19 ++ src/main/java/frc/robot/Robot.java | 55 ++++- .../java/frc/robot/lib/field/FieldUtil.java | 1 - .../java/frc/robot/lib/io/CancoderIO.java | 54 +++++ src/main/java/frc/robot/lib/io/TalonFXIO.java | 89 ++++++++ .../frc/robot/subsystems/climb/Climb.java | 10 +- .../frc/robot/subsystems/climb/ClimbIO.java | 44 ++++ .../frc/robot/subsystems/drive/Drive.java | 125 +++++------ .../frc/robot/subsystems/drive/DriveIO.java | 164 ++++++++++++++ .../drive/ctre/CtreDriveTelemetry.java | 206 +++++++++--------- .../frc/robot/subsystems/indexer/Indexer.java | 26 ++- .../robot/subsystems/indexer/IndexerIO.java | 41 ++++ .../frc/robot/subsystems/intake/Intake.java | 31 ++- .../frc/robot/subsystems/intake/IntakeIO.java | 47 ++++ .../frc/robot/subsystems/roller/Roller.java | 30 ++- .../frc/robot/subsystems/roller/RollerIO.java | 41 ++++ .../frc/robot/subsystems/shooter/Shooter.java | 46 ++-- .../robot/subsystems/shooter/ShooterIO.java | 47 ++++ .../frc/robot/subsystems/vision/VisionIO.java | 0 20 files changed, 876 insertions(+), 247 deletions(-) create mode 100644 src/main/java/frc/robot/BuildConstants.java create mode 100644 src/main/java/frc/robot/lib/io/CancoderIO.java create mode 100644 src/main/java/frc/robot/lib/io/TalonFXIO.java create mode 100644 src/main/java/frc/robot/subsystems/climb/ClimbIO.java create mode 100644 src/main/java/frc/robot/subsystems/drive/DriveIO.java create mode 100644 src/main/java/frc/robot/subsystems/indexer/IndexerIO.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeIO.java create mode 100644 src/main/java/frc/robot/subsystems/roller/RollerIO.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShooterIO.java create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionIO.java diff --git a/build.gradle b/build.gradle index fff5f8f..3b981de 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,17 @@ plugins { id "java" id "edu.wpi.first.GradleRIO" version "2026.2.1" + id "com.peterabeles.gversion" version "1.10" +} + +project.compileJava.dependsOn(createVersionFile) +gversion { + srcDir = "src/main/java/" + classPackage = "frc.robot" + className = "BuildConstants" + dateFormat = "yyyy-MM-dd HH:mm:ss z" + timeZone = "America/New_York" // Use preferred time zone + indent = " " } java { @@ -57,6 +68,7 @@ wpi.java.debugJni = false // Set this to true to enable desktop support. def includeDesktopSupport = true + // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. @@ -92,6 +104,41 @@ dependencies { testRuntimeOnly 'org.junit.platform:junit-platform-launcher' } + + +// Create commit with working changes on event branches +task(eventDeploy) { + doLast { + if (project.gradle.startParameter.taskNames.any({ it.toLowerCase().contains("deploy") })) { + def branchPrefix = "event" + def branch = 'git branch --show-current'.execute().text.trim() + def commitMessage = "Update at '${new Date().toString()}'" + + if (branch.startsWith(branchPrefix)) { + exec { + workingDir(projectDir) + executable 'git' + args 'add', '-A' + } + exec { + workingDir(projectDir) + executable 'git' + args 'commit', '-m', commitMessage + ignoreExitValue = true + } + + println "Committed to branch: '$branch'" + println "Commit message: '$commitMessage'" + } else { + println "Not on an event branch, skipping commit" + } + } else { + println "Not running deploy task, skipping commit" + } + } +} +createVersionFile.dependsOn(eventDeploy) + test { useJUnitPlatform() systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java new file mode 100644 index 0000000..293c407 --- /dev/null +++ b/src/main/java/frc/robot/BuildConstants.java @@ -0,0 +1,19 @@ +package frc.robot; + +/** + * Automatically generated file containing build version information. + */ +public final class BuildConstants { + public static final String MAVEN_GROUP = ""; + public static final String MAVEN_NAME = "Robot2026"; + public static final String VERSION = "unspecified"; + public static final int GIT_REVISION = 145; + public static final String GIT_SHA = "a17f0d2e3ee34733364081d418a69168a2dadd65"; + public static final String GIT_DATE = "2026-02-15 17:25:27 EST"; + public static final String GIT_BRANCH = "Akit"; + public static final String BUILD_DATE = "2026-02-16 21:47:52 EST"; + public static final long BUILD_UNIX_TIME = 1771296472495L; + public static final int DIRTY = 1; + + private BuildConstants(){} +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 31a5327..b9fad63 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,10 +1,13 @@ package frc.robot; import frc.robot.auto.AutoSelector; +import frc.robot.lib.util.MovingAverageDouble; import java.util.Optional; import org.littletonrobotics.junction.LogFileUtil; +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; @@ -14,7 +17,10 @@ import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -37,7 +43,7 @@ * this project, you must also update the Main.java file in the project. */ @SuppressWarnings("unused") -public class Robot extends TimedRobot { +public class Robot extends LoggedRobot { private static final CommandScheduler commandScheduler = CommandScheduler.getInstance(); private AutoSelector autoChooser; private Command autoCommand; @@ -47,12 +53,54 @@ public class Robot extends TimedRobot { public static final CommandXboxController controller = new CommandXboxController(Controllers.DRIVER_CONTROLLER_PORT); + + public double lastTime = -1.0; + public MovingAverageDouble fpsTracker = new MovingAverageDouble(60); /** * This function is run when the robot is first started up and should be used for any * initialization code. */ public Robot() { + boolean replay = false; + + Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); + Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); + Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); + Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + switch (BuildConstants.DIRTY) { + case 0: + Logger.recordMetadata("GitDirty", "All changes committed"); + break; + case 1: + Logger.recordMetadata("GitDirty", "Uncomitted changes"); + break; + default: + Logger.recordMetadata("GitDirty", "Unknown"); + break; + } + + if (RobotBase.isReal()) { + Logger.addDataReceiver(new WPILOGWriter()); + if (!DriverStation.isFMSAttached()) { + Logger.addDataReceiver(new NT4Publisher()); + } + } else if (replay) { + setUseTiming(false); + String logPath = LogFileUtil.findReplayLog(); + Logger.setReplaySource(new WPILOGReader(logPath)); + Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); + } else if (RobotBase.isSimulation()) { + Logger.addDataReceiver(new NT4Publisher()); + Logger.addDataReceiver(new WPILOGWriter()); + } + + Logger.start(); + if (!Logger.hasReplaySource()) { + RobotController.setTimeSource(RobotController::getFPGATime); + } + VisionDeviceManager.getInstance(); Drive.getInstance(); @@ -95,7 +143,12 @@ public void robotPeriodic() { // commands, running already-scheduled commands, removing finished or interrupted commands, // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. + commandScheduler.run(); + double now = Timer.getFPGATimestamp(); + fpsTracker.add(1.0 / (now - lastTime)); + Logger.recordOutput("FPS", fpsTracker.getAverage()); + lastTime = now; } /** This function is called once each time the robot enters Disabled mode. */ diff --git a/src/main/java/frc/robot/lib/field/FieldUtil.java b/src/main/java/frc/robot/lib/field/FieldUtil.java index 9a7cc70..c8666d4 100644 --- a/src/main/java/frc/robot/lib/field/FieldUtil.java +++ b/src/main/java/frc/robot/lib/field/FieldUtil.java @@ -4,7 +4,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.DriverStation; import frc.robot.lib.trajectory.RedTrajectory.State.ChassisAccels; public class FieldUtil { diff --git a/src/main/java/frc/robot/lib/io/CancoderIO.java b/src/main/java/frc/robot/lib/io/CancoderIO.java new file mode 100644 index 0000000..4c73181 --- /dev/null +++ b/src/main/java/frc/robot/lib/io/CancoderIO.java @@ -0,0 +1,54 @@ +package frc.robot.lib.io; + +import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.CANcoder; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; + +public class CancoderIO { + @AutoLog + public static class CancoderIOInputs { + public int id = 0; + public boolean connected = false; + public double positionRotations = 0.0; + public double velocityRPS = 0.0; + } + + public final String name; + public final CANcoder motor; + public final CancoderIOInputsAutoLogged inputs; + + public StatusSignal position; + public StatusSignal velocity; + + private final Debouncer connectedDebouncer = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + + public CancoderIO(String name, CANcoder motor) { + this.name = name; + this.motor = motor; + inputs = new CancoderIOInputsAutoLogged(); + inputs.id = motor.getDeviceID(); + position = motor.getPosition(); + velocity = motor.getVelocity(); + } + + public void updateInputs() { + var status = BaseStatusSignal.refreshAll( + position, + velocity); + inputs.connected = connectedDebouncer.calculate(status.isOK()); + inputs.positionRotations = position.getValueAsDouble(); + inputs.velocityRPS = velocity.getValueAsDouble(); + } + + public void process() { + Logger.processInputs(name, inputs); + } +} diff --git a/src/main/java/frc/robot/lib/io/TalonFXIO.java b/src/main/java/frc/robot/lib/io/TalonFXIO.java new file mode 100644 index 0000000..99d5a67 --- /dev/null +++ b/src/main/java/frc/robot/lib/io/TalonFXIO.java @@ -0,0 +1,89 @@ +package frc.robot.lib.io; + +import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularAcceleration; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; +import edu.wpi.first.units.measure.Voltage; + +public class TalonFXIO { + @AutoLog + public static class TalonFXIOInputs { + public int id = 0; + public boolean connected = false; + public double positionRotations = 0.0; + public double velocityRPS = 0.0; + public double accelerationRPSS = 0.0; + public double appliedVolts = 0.0; + public double supplyVolts = 0.0; + public double statorCurrentAmps = 0.0; + public double supplyCurrentAmps = 0.0; + public double temperatureCelsius = 0.0; + public String controlRequest = ""; + } + + public final String name; + public final TalonFX motor; + public final TalonFXIOInputsAutoLogged inputs; + + public StatusSignal position; + public StatusSignal velocity; + public StatusSignal acceleration; + public StatusSignal appliedVoltage; + public StatusSignal supplyVoltage; + public StatusSignal statorCurrent; + public StatusSignal supplyCurrent; + public StatusSignal temperature; + + private final Debouncer connectedDebouncer = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + + public TalonFXIO(String name, TalonFX motor) { + this.name = name; + this.motor = motor; + inputs = new TalonFXIOInputsAutoLogged(); + inputs.id = motor.getDeviceID(); + position = motor.getPosition(); + velocity = motor.getVelocity(); + acceleration = motor.getAcceleration(); + appliedVoltage = motor.getMotorVoltage(); + supplyVoltage = motor.getSupplyVoltage(); + statorCurrent = motor.getStatorCurrent(); + supplyCurrent = motor.getSupplyCurrent(); + temperature = motor.getDeviceTemp(); + } + + public void updateInputs() { + var status = BaseStatusSignal.refreshAll( + position, + velocity, + appliedVoltage, + supplyVoltage, + statorCurrent, + supplyCurrent, + temperature); + inputs.connected = connectedDebouncer.calculate(status.isOK()); + inputs.positionRotations = position.getValueAsDouble(); + inputs.velocityRPS = velocity.getValueAsDouble(); + inputs.accelerationRPSS = acceleration.getValueAsDouble(); + inputs.appliedVolts = appliedVoltage.getValueAsDouble(); + inputs.supplyVolts = supplyVoltage.getValueAsDouble(); + inputs.statorCurrentAmps = statorCurrent.getValueAsDouble(); + inputs.supplyCurrentAmps = supplyCurrent.getValueAsDouble(); + inputs.temperatureCelsius = temperature.getValueAsDouble(); + inputs.controlRequest = motor.getAppliedControl().getName(); + } + + public void process() { + Logger.processInputs(name, inputs); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/climb/Climb.java b/src/main/java/frc/robot/subsystems/climb/Climb.java index 45f8032..b548594 100644 --- a/src/main/java/frc/robot/subsystems/climb/Climb.java +++ b/src/main/java/frc/robot/subsystems/climb/Climb.java @@ -20,8 +20,8 @@ import frc.robot.Constants; import frc.robot.Robot; import frc.robot.subsystems.TelemetryManager; +import frc.robot.lib.io.TalonFXIO; import frc.robot.subsystems.climb.ClimbConstants.Setpoint; - import edu.wpi.first.wpilibj.simulation.ElevatorSim; public class Climb extends SubsystemBase { @@ -43,6 +43,8 @@ public static Climb getInstance() { private ElevatorSim sim; + private ClimbIO io; + private Climb() { super(); climbMotor = new TalonFX(CLIMB_MOTOR_ID); @@ -61,7 +63,8 @@ private Climb() { 0.0, 0.0 ); } - TelemetryManager.getInstance().addSendable(this); + io = new ClimbIO(getName(), climbMotor); + // TelemetryManager.getInstance().addSendable(this); } @Override @@ -73,6 +76,9 @@ public void periodic() { climbMotor.getVelocity().getValueAsDouble(); // updates the motor climbMotor.setControl(request); + + io.updateInputs(lastReadHeight, lastReadSpeed, getCurrentCommand(), getDefaultCommand()); + io.process(); } @Override diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIO.java b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java new file mode 100644 index 0000000..0daee10 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java @@ -0,0 +1,44 @@ + +package frc.robot.subsystems.climb; + +import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.lib.io.TalonFXIO; + +public class ClimbIO { + @AutoLog + public static class ClimbIOInputs { + public double positionMeters = 0.0; + public double velocityMetersPerSecond = 0.0; + public String currentCommand = ""; + public String defaultCommand = ""; + } + + private final String name; + private final TalonFXIO motorIO; + private final ClimbIOInputsAutoLogged inputs; + + public ClimbIO(String name, TalonFX motor) { + this.name = name; + motorIO = new TalonFXIO(name + "/Motor", motor); + inputs = new ClimbIOInputsAutoLogged(); + } + + public void updateInputs(double positionMeters, double velocityMetersPerSecond, Command currentCommand, Command defaultCommand) { + motorIO.updateInputs(); + + inputs.positionMeters = positionMeters; + inputs.velocityMetersPerSecond = velocityMetersPerSecond; + inputs.currentCommand = currentCommand != null ? currentCommand.getName() : "None"; + inputs.defaultCommand = defaultCommand != null ? defaultCommand.getName() : "None"; + } + + public void process() { + motorIO.process(); + Logger.processInputs(name, inputs); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index a3081e9..90885be 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -23,7 +23,6 @@ import edu.wpi.first.units.BaseUnits; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Time; -import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -32,7 +31,6 @@ import frc.robot.Robot; import frc.robot.lib.control.ControlConstants.PIDVConstants; import frc.robot.lib.control.ControlConstants.ProfiledPIDVConstants; -import frc.robot.lib.control.PIDVController; import frc.robot.lib.control.ProfiledPIDVController; import frc.robot.lib.field.FieldLayout; import frc.robot.lib.trajectory.LocalADStarWrapper; @@ -43,8 +41,7 @@ import frc.robot.subsystems.drive.commands.PIDToPoseCommand; import frc.robot.subsystems.drive.commands.TrajectoryCommand; import frc.robot.subsystems.drive.ctre.CtreDrive; -import frc.robot.subsystems.drive.ctre.CtreDriveTelemetry; -// import frc.robot.subsystems.vision.VisionConstants; +// import frc.robot.subsystems.drive.ctre.CtreDriveTelemetry; public class Drive extends SubsystemBase { private static Drive driveInstance; @@ -60,15 +57,17 @@ public static Drive getInstance() { public SwerveRequest driveRequest; private final CtreDrive drivetrain; - private final CtreDriveTelemetry telemetry; + // private final CtreDriveTelemetry telemetry; @SuppressWarnings("unused") private Time lastPoseResetTime = BaseUnits.TimeUnit.of(0.0); // Citrus what are you doing private final LocalADStarWrapper pathfinder; + private final DriveIO io; + private Drive() { drivetrain = CtreDriveConstants.createDrivetrain(); - telemetry = new CtreDriveTelemetry(MAX_SPEED); + // telemetry = new CtreDriveTelemetry(MAX_SPEED); teleopRequest = new SwerveRequest.FieldCentric(); driveRequest = teleopRequest; lastReadState = drivetrain.getState(); @@ -80,30 +79,8 @@ private Drive() { drivetrain.getOdometryThread().setThreadPriority(31); TelemetryManager.getInstance().addStructPublisher("Mechanisms/Drive", Pose3d.struct, () -> new Pose3d(getPose())); - // TelemetryManager.getInstance().addStructPublisher("Drive/TargetSpeeds", ChassisSpeeds.struct, - // () -> { - // try { - // if (driveRequest instanceof SwerveRequest.ApplyFieldSpeeds) { - // return ChassisSpeeds.fromFieldRelativeSpeeds( - // ((SwerveRequest.ApplyFieldSpeeds) driveRequest).Speeds, - // lastReadState.Pose.getRotation()); - // } else if (driveRequest instanceof SwerveRequest.ApplyRobotSpeeds) { - // return ((SwerveRequest.ApplyRobotSpeeds) driveRequest).Speeds; - // } else if (driveRequest instanceof SwerveRequest.FieldCentric) { - // var req = ((SwerveRequest.FieldCentric) driveRequest); - // return ChassisSpeeds.fromFieldRelativeSpeeds( - // req.VelocityX, - // req.VelocityY, - // req.RotationalRate, - // lastReadState.Pose.getRotation()); - // } else if (driveRequest instanceof SwerveRequest.RobotCentric) { - // var req = ((SwerveRequest.RobotCentric) driveRequest); - // return new ChassisSpeeds(req.VelocityX, req.VelocityY, req.RotationalRate); - // } - // } finally {} - // return lastReadState.Speeds; - // }); - TelemetryManager.getInstance().addSendable(this); + io = new DriveIO(getName(), drivetrain); + // TelemetryManager.getInstance().addSendable(this); } /** @return the ctre generated drivetrain */ @@ -118,8 +95,10 @@ public void periodic() { } public void outputTelemetry() { - telemetry.telemeterize(lastReadState); + // telemetry.telemeterize(lastReadState); FieldLayout.field.setRobotPose(getPose()); + io.updateInputs(driveRequest, lastReadState, getCurrentCommand(), getDefaultCommand()); + io.process(); } /** @@ -383,46 +362,46 @@ && isRollStable() && Units.RadiansPerSecond.of(speeds.omegaRadiansPerSecond).lte(MAX_ROTATION_SPEED_SCORING); } - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.addDoubleProperty( - "Pitch Velocity Degrees Per Second", - () -> drivetrain - .getPigeon2() - .getAngularVelocityYDevice() - .getValue() - .in(Units.DegreesPerSecond), - null); - builder.addDoubleProperty( - "Pitch Degrees", - () -> drivetrain.getPigeon2().getPitch().getValue().in(Units.Degrees), - null); - - builder.addDoubleProperty( - "Roll Velocity Degrees Per Second", - () -> drivetrain - .getPigeon2() - .getAngularVelocityXDevice() - .getValue() - .in(Units.DegreesPerSecond), - null); - builder.addDoubleProperty( - "Roll Degrees", - () -> drivetrain.getPigeon2().getRoll().getValue().in(Units.Degrees), - null); - - addModuleToBuilder(builder, 0); - addModuleToBuilder(builder, 1); - addModuleToBuilder(builder, 2); - addModuleToBuilder(builder, 3); - } - - /** Telemeterizes a module */ - private void addModuleToBuilder(SendableBuilder builder, int module) { - TelemetryManager.makeSendableTalonFX("Modules/" + module + "/Drive", - drivetrain.getModules()[module].getDriveMotor(), builder); - TelemetryManager.makeSendableTalonFX("Modules/" + module + "/Angle", - drivetrain.getModules()[module].getSteerMotor(), builder); - } + // @Override + // public void initSendable(SendableBuilder builder) { + // super.initSendable(builder); + // builder.addDoubleProperty( + // "Pitch Velocity Degrees Per Second", + // () -> drivetrain + // .getPigeon2() + // .getAngularVelocityYDevice() + // .getValue() + // .in(Units.DegreesPerSecond), + // null); + // builder.addDoubleProperty( + // "Pitch Degrees", + // () -> drivetrain.getPigeon2().getPitch().getValue().in(Units.Degrees), + // null); + + // builder.addDoubleProperty( + // "Roll Velocity Degrees Per Second", + // () -> drivetrain + // .getPigeon2() + // .getAngularVelocityXDevice() + // .getValue() + // .in(Units.DegreesPerSecond), + // null); + // builder.addDoubleProperty( + // "Roll Degrees", + // () -> drivetrain.getPigeon2().getRoll().getValue().in(Units.Degrees), + // null); + + // addModuleToBuilder(builder, 0); + // addModuleToBuilder(builder, 1); + // addModuleToBuilder(builder, 2); + // addModuleToBuilder(builder, 3); + // } + + // /** Telemeterizes a module */ + // private void addModuleToBuilder(SendableBuilder builder, int module) { + // TelemetryManager.makeSendableTalonFX("Modules/" + module + "/Drive", + // drivetrain.getModules()[module].getDriveMotor(), builder); + // TelemetryManager.makeSendableTalonFX("Modules/" + module + "/Angle", + // drivetrain.getModules()[module].getSteerMotor(), builder); + // } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/DriveIO.java b/src/main/java/frc/robot/subsystems/drive/DriveIO.java new file mode 100644 index 0000000..72862c8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveIO.java @@ -0,0 +1,164 @@ +package frc.robot.subsystems.drive; + +import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.Pigeon2; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; +import com.ctre.phoenix6.swerve.SwerveModule; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.lib.io.CancoderIO; +import frc.robot.lib.io.TalonFXIO; +import frc.robot.subsystems.drive.ctre.CtreDrive; + +public class DriveIO { + @AutoLog + public static class DriveIOInputs { + public Pose2d fieldPose = new Pose2d(); + public ChassisSpeeds fieldSpeeds = new ChassisSpeeds(); + public String driveRequest = ""; + + public SwerveModuleState[] moduleStates = new SwerveModuleState[4]; + public SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; + public SwerveModuleState[] moduleTargets = new SwerveModuleState[4]; + + public String currentCommand = ""; + public String defaultCommand = ""; + } + + public class GyroIO { + @AutoLog + public static class GyroIOInputs { + public boolean connected = false; + public Rotation2d yawPosition = Rotation2d.kZero; + public double yawVelocityRadPerSec = 0.0; + } + + private final String name; + private final GyroIOInputsAutoLogged inputs; + private final StatusSignal yaw ; + private final StatusSignal yawVelocity; + + public GyroIO(String name, Pigeon2 pigeon) { + this.name = name; + yaw = pigeon.getYaw(); + yawVelocity = pigeon.getAngularVelocityZWorld(); + inputs = new GyroIOInputsAutoLogged(); + } + + public void updateInputs() { + inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + } + + public void process() { + Logger.processInputs(name, inputs); + } + } + + public static class ModuleIO { + @AutoLog + public static class ModuleIOInputs { + public SwerveModuleState state; + public SwerveModulePosition position; + public SwerveModuleState target; + } + + private final String name; + private final TalonFXIO driveMotorIO; + private final TalonFXIO azimuthMotorIO; + private final CancoderIO cancoderIO; + private final ModuleIOInputsAutoLogged inputs; + + public ModuleIO(String name, SwerveModule module) { + this.name = name; + driveMotorIO = new TalonFXIO(name + "/DriveMotor", module.getDriveMotor()); + azimuthMotorIO = new TalonFXIO(name + "/AzimuthMotor", module.getSteerMotor()); + cancoderIO = new CancoderIO(name + "/Cancoder", module.getEncoder()); + inputs = new ModuleIOInputsAutoLogged(); + } + + /** Updates the set of loggable inputs. */ + public void updateInputs(SwerveModuleState state, SwerveModulePosition position, SwerveModuleState target) { + inputs.state = state; + inputs.position = position; + inputs.target = target; + + driveMotorIO.updateInputs(); + azimuthMotorIO.updateInputs(); + cancoderIO.updateInputs(); + } + + public void process() { + driveMotorIO.process(); + azimuthMotorIO.process(); + cancoderIO.process(); + Logger.processInputs(name, inputs); + } + } + + private final String name; + private final ModuleIO[] moduleIOs; + private final GyroIO gyroIO; + private final CtreDrive drivetrain; + private final DriveIOInputsAutoLogged inputs; + + public DriveIO(String name, CtreDrive drivetrain) { + this.name = name; + this.drivetrain = drivetrain; + inputs = new DriveIOInputsAutoLogged(); + + //FrontLeft, FrontRight, BackLeft, BackRight + moduleIOs = new ModuleIO[] { + new ModuleIO(name + "/Modules/" + "FL", drivetrain.getModule(0)), + new ModuleIO(name + "/Modules/" + "FR", drivetrain.getModule(1)), + new ModuleIO(name + "/Modules/" + "BL", drivetrain.getModule(2)), + new ModuleIO(name + "/Modules/" + "BR", drivetrain.getModule(3))}; + + gyroIO = new GyroIO(name + "/Gyro", drivetrain.getPigeon2()); + } + + public void updateInputs(SwerveRequest request, SwerveDriveState state, Command currentCommand, Command defaultCommand) { + inputs.driveRequest = request.getClass().getSimpleName(); + inputs.fieldPose = state.Pose; + inputs.fieldSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds( + state.Speeds, state.Pose.getRotation()); + + inputs.moduleStates = state.ModuleStates; + inputs.modulePositions = state.ModulePositions; + inputs.moduleTargets = state.ModuleTargets; + + inputs.currentCommand = currentCommand != null ? currentCommand.getName() : "None"; + inputs.defaultCommand = defaultCommand != null ? defaultCommand.getName() : "None"; + for (int i = 0; i < 4; i++) { + moduleIOs[i].updateInputs(state.ModuleStates[i], state.ModulePositions[i], state.ModuleTargets[i]); + } + + gyroIO.updateInputs(); + } + + public void process() { + for (int i = 0; i < 4; i++) { + moduleIOs[i].process(); + } + + gyroIO.process(); + Logger.processInputs(name, inputs); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveTelemetry.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveTelemetry.java index 34a5403..12cdcb8 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveTelemetry.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveTelemetry.java @@ -1,115 +1,115 @@ -package frc.robot.subsystems.drive.ctre; +// package frc.robot.subsystems.drive.ctre; -import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.networktables.DoublePublisher; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StructArrayPublisher; -import edu.wpi.first.networktables.StructPublisher; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj.util.Color8Bit; +// import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.kinematics.ChassisSpeeds; +// import edu.wpi.first.math.kinematics.SwerveModulePosition; +// import edu.wpi.first.math.kinematics.SwerveModuleState; +// import edu.wpi.first.networktables.DoublePublisher; +// import edu.wpi.first.networktables.NetworkTable; +// import edu.wpi.first.networktables.NetworkTableInstance; +// import edu.wpi.first.networktables.StructArrayPublisher; +// import edu.wpi.first.networktables.StructPublisher; +// import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +// import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import edu.wpi.first.wpilibj.util.Color; +// import edu.wpi.first.wpilibj.util.Color8Bit; -public class CtreDriveTelemetry { - private final double MaxSpeed; +// public class CtreDriveTelemetry { +// private final double MaxSpeed; - /** - * Construct a telemetry object, with the specified max speed of the robot - * - * @param maxSpeed Maximum speed in meters per second - */ - public CtreDriveTelemetry(double maxSpeed) { - MaxSpeed = maxSpeed; - } +// /** +// * Construct a telemetry object, with the specified max speed of the robot +// * +// * @param maxSpeed Maximum speed in meters per second +// */ +// public CtreDriveTelemetry(double maxSpeed) { +// MaxSpeed = maxSpeed; +// } - /* What to publish over networktables for telemetry */ - private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); +// /* What to publish over networktables for telemetry */ +// private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); - /* Robot swerve drive state */ - private final NetworkTable driveStateTable = inst.getTable("SmartDashboard/Drive"); - private final StructPublisher drivePose = - driveStateTable.getStructTopic("Pose", Pose2d.struct).publish(); - private final StructPublisher driveSpeeds = - driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish(); - private final StructArrayPublisher driveModuleStates = driveStateTable - .getStructArrayTopic("ModuleStates", SwerveModuleState.struct) - .publish(); - private final StructArrayPublisher driveModuleTargets = driveStateTable - .getStructArrayTopic("ModuleTargets", SwerveModuleState.struct) - .publish(); - private final StructArrayPublisher driveModulePositions = driveStateTable - .getStructArrayTopic("ModulePositions", SwerveModulePosition.struct) - .publish(); - private final DoublePublisher driveTimestamp = - driveStateTable.getDoubleTopic("Timestamp").publish(); - private final DoublePublisher driveOdometryFrequency = - driveStateTable.getDoubleTopic("OdometryFrequency").publish(); +// /* Robot swerve drive state */ +// private final NetworkTable driveStateTable = inst.getTable("SmartDashboard/Drive"); +// private final StructPublisher drivePose = +// driveStateTable.getStructTopic("Pose", Pose2d.struct).publish(); +// private final StructPublisher driveSpeeds = +// driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish(); +// private final StructArrayPublisher driveModuleStates = driveStateTable +// .getStructArrayTopic("ModuleStates", SwerveModuleState.struct) +// .publish(); +// private final StructArrayPublisher driveModuleTargets = driveStateTable +// .getStructArrayTopic("ModuleTargets", SwerveModuleState.struct) +// .publish(); +// private final StructArrayPublisher driveModulePositions = driveStateTable +// .getStructArrayTopic("ModulePositions", SwerveModulePosition.struct) +// .publish(); +// private final DoublePublisher driveTimestamp = +// driveStateTable.getDoubleTopic("Timestamp").publish(); +// private final DoublePublisher driveOdometryFrequency = +// driveStateTable.getDoubleTopic("OdometryFrequency").publish(); - /* Mechanisms to represent the swerve module states */ - private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { - new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), - }; - /* A direction and length changing ligament for speed representation */ - private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { - m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - }; - /* A direction changing and length constant ligament for module direction */ - private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { - m_moduleMechanisms[0] - .getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[1] - .getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[2] - .getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[3] - .getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - }; +// /* Mechanisms to represent the swerve module states */ +// private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { +// new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), +// }; +// /* A direction and length changing ligament for speed representation */ +// private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { +// m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), +// m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), +// m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), +// m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), +// }; +// /* A direction changing and length constant ligament for module direction */ +// private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { +// m_moduleMechanisms[0] +// .getRoot("RootDirection", 0.5, 0.5) +// .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), +// m_moduleMechanisms[1] +// .getRoot("RootDirection", 0.5, 0.5) +// .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), +// m_moduleMechanisms[2] +// .getRoot("RootDirection", 0.5, 0.5) +// .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), +// m_moduleMechanisms[3] +// .getRoot("RootDirection", 0.5, 0.5) +// .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), +// }; - private final double[] m_moduleStatesArray = new double[8]; - private final double[] m_moduleTargetsArray = new double[8]; +// private final double[] m_moduleStatesArray = new double[8]; +// private final double[] m_moduleTargetsArray = new double[8]; - /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */ - public void telemeterize(SwerveDriveState state) { - /* Telemeterize the swerve drive state */ - drivePose.set(state.Pose); - driveSpeeds.set(state.Speeds); - driveModuleStates.set(state.ModuleStates); - driveModuleTargets.set(state.ModuleTargets); - driveModulePositions.set(state.ModulePositions); - driveTimestamp.set(state.Timestamp); - driveOdometryFrequency.set(1.0 / state.OdometryPeriod); +// /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */ +// public void telemeterize(SwerveDriveState state) { +// /* Telemeterize the swerve drive state */ +// drivePose.set(state.Pose); +// driveSpeeds.set(state.Speeds); +// driveModuleStates.set(state.ModuleStates); +// driveModuleTargets.set(state.ModuleTargets); +// driveModulePositions.set(state.ModulePositions); +// driveTimestamp.set(state.Timestamp); +// driveOdometryFrequency.set(1.0 / state.OdometryPeriod); - for (int i = 0; i < 4; ++i) { - m_moduleStatesArray[i * 2 + 0] = state.ModuleStates[i].angle.getRadians(); - m_moduleStatesArray[i * 2 + 1] = state.ModuleStates[i].speedMetersPerSecond; - m_moduleTargetsArray[i * 2 + 0] = state.ModuleTargets[i].angle.getRadians(); - m_moduleTargetsArray[i * 2 + 1] = state.ModuleTargets[i].speedMetersPerSecond; - } +// for (int i = 0; i < 4; ++i) { +// m_moduleStatesArray[i * 2 + 0] = state.ModuleStates[i].angle.getRadians(); +// m_moduleStatesArray[i * 2 + 1] = state.ModuleStates[i].speedMetersPerSecond; +// m_moduleTargetsArray[i * 2 + 0] = state.ModuleTargets[i].angle.getRadians(); +// m_moduleTargetsArray[i * 2 + 1] = state.ModuleTargets[i].speedMetersPerSecond; +// } - SmartDashboard.putNumberArray("Drive/ModuleStates", m_moduleStatesArray); - SmartDashboard.putNumberArray("Drive/ModuleTargets", m_moduleTargetsArray); - SmartDashboard.putNumber("Drive/OdometryPeriod", state.OdometryPeriod); +// SmartDashboard.putNumberArray("Drive/ModuleStates", m_moduleStatesArray); +// SmartDashboard.putNumberArray("Drive/ModuleTargets", m_moduleTargetsArray); +// SmartDashboard.putNumber("Drive/OdometryPeriod", state.OdometryPeriod); - /* Telemeterize the module states to a Mechanism2d */ - for (int i = 0; i < 4; ++i) { - m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); - m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); - m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); +// /* Telemeterize the module states to a Mechanism2d */ +// for (int i = 0; i < 4; ++i) { +// m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); +// m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); +// m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); - SmartDashboard.putData("Drive/Module " + i, m_moduleMechanisms[i]); - } - } -} \ No newline at end of file +// SmartDashboard.putData("Drive/Module " + i, m_moduleMechanisms[i]); +// } +// } +// } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 09d24a0..2e9d84f 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -1,21 +1,17 @@ package frc.robot.subsystems.indexer; import static frc.robot.subsystems.indexer.IndexerConstants.*; + import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.simulation.FlywheelSim; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; -import frc.robot.subsystems.TelemetryManager; -import au.grapplerobotics.LaserCan; -import au.grapplerobotics.ConfigurationFailedException; // TODO (ethan): only activate if shooter ready // TODO (ethan): ask tommy setControl(request) @@ -51,6 +47,8 @@ public static Indexer getRightInstance() { /** boolean that's modified by shooter */ + private IndexerIO io; + /** setup, adding motor and laser */ private Indexer(boolean isLeft) { super(); @@ -90,7 +88,8 @@ private Indexer(boolean isLeft) { // System.out.println("Configuration failed! " + e); // } - TelemetryManager.getInstance().addSendable(this); + io = new IndexerIO(getName(), motor); + // TelemetryManager.getInstance().addSendable(this); setDefaultCommand(deactivateIndexer()); } @@ -104,6 +103,8 @@ public void periodic() { // if (shooterReady == true) { // activateIndexer(); // } + io.updateInputs(motor.getVelocity().getValueAsDouble(), getCurrentCommand(), getDefaultCommand()); + io.process(); } @Override @@ -194,10 +195,11 @@ public Command deactivateIndexer() { // TODO: AdvantageKit! /** ????????? */ - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - // builder.addBooleanProperty("Has Ball", () -> hasBall, null); - TelemetryManager.makeSendableTalonFX("Indexer Motor", motor, builder); - } + // @Override + // public void initSendable(SendableBuilder builder) { + // super.initSendable(builder); + // // builder.addBooleanProperty("Has Ball", () -> hasBall, null); + // TelemetryManager.makeSendableTalonFX("Indexer Motor", motor, builder); + // } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java new file mode 100644 index 0000000..081878e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java @@ -0,0 +1,41 @@ +package frc.robot.subsystems.indexer; + +import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.lib.io.TalonFXIO; + +public class IndexerIO { + @AutoLog + public static class IndexerIOInputs { + public double velocityRPS = 0.0; + public String currentCommand = ""; + public String defaultCommand = ""; + } + + private final String name; + private final TalonFXIO motorIO; + private final IndexerIOInputsAutoLogged inputs; + + public IndexerIO(String name, TalonFX motor) { + this.name = name; + motorIO = new TalonFXIO(name + "/Motor", motor); + inputs = new IndexerIOInputsAutoLogged(); + } + + public void updateInputs(double velocityRPS, Command currentCommand, Command defaultCommand) { + motorIO.updateInputs(); + + inputs.velocityRPS = velocityRPS; + inputs.currentCommand = currentCommand != null ? currentCommand.getName() : "None"; + inputs.defaultCommand = defaultCommand != null ? defaultCommand.getName() : "None"; + } + + public void process() { + motorIO.process(); + Logger.processInputs(name, inputs); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index e00ae2c..73c94e9 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -17,9 +17,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Robot; -import frc.robot.subsystems.TelemetryManager; import frc.robot.subsystems.intake.IntakeConstants.Motors; -import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.simulation.BatterySim; import edu.wpi.first.wpilibj.simulation.FlywheelSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; @@ -45,14 +43,9 @@ public static Intake getInstance() { private ControlRequest barRequest = new NeutralOut(); private SingleJointedArmSim sim; - private FlywheelSim wheelSim; - /* - private MechanismLigament2d ligament; - private final Mechanism2d mech2d = new Mechanism2d(20, 20); - private final MechanismRoot2d mech2droot = mech2d.getRoot("Bar Root", 10, 1); - */ + private IntakeIO io; private Intake() { super(); @@ -93,8 +86,8 @@ private Intake() { 0.0); } - TelemetryManager.getInstance().addSendable(this); - + // TelemetryManager.getInstance().addSendable(this); + io = new IntakeIO(getName(), wheelMotor, barMotor); } @Override @@ -103,7 +96,8 @@ public void periodic(){ barPosition = barMotor.getPosition().getValueAsDouble(); barMotor.setControl(barRequest); wheelMotor.setControl(wheelRequest); - //ligament.setAngle(barPosition); + io.updateInputs(wheelSpeed, barPosition, getCurrentCommand(), getDefaultCommand()); + io.process(); } @Override @@ -211,11 +205,12 @@ public Command waitUntilWheelIsAtSpeed(double target) { return Commands.waitUntil(() -> Math.abs(target - wheelSpeed) < WHEEL_EPSILON); } - @Override - public void initSendable(SendableBuilder builder){ - super.initSendable(builder); - builder.addDoubleProperty("Position", () -> barPosition, null); - TelemetryManager.makeSendableTalonFX("Bar Motor", barMotor, builder); - TelemetryManager.makeSendableTalonFX("Wheel Motor", wheelMotor, builder); - } + // @Override + // public void initSendable(SendableBuilder builder){ + // super.initSendable(builder); + // builder.addDoubleProperty("Position", () -> barPosition, null); + // TelemetryManager.makeSendableTalonFX("Bar Motor", barMotor, builder); + // TelemetryManager.makeSendableTalonFX("Wheel Motor", wheelMotor, builder); + // } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java new file mode 100644 index 0000000..0d86905 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -0,0 +1,47 @@ +package frc.robot.subsystems.intake; + +import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.lib.io.TalonFXIO; + +public class IntakeIO { + @AutoLog + public static class IntakeIOInputs { + public double wheelVelocityRPS = 0.0; + public double barPositionDeg = 0.0; + public String currentCommand = ""; + public String defaultCommand = ""; + } + + private final String name; + private final TalonFXIO wheelMotorIO; + private final TalonFXIO barMotorIO; + private final IntakeIOInputsAutoLogged inputs; + + public IntakeIO(String name, TalonFX wheelMotor, TalonFX barMotor) { + this.name = name; + wheelMotorIO = new TalonFXIO(name + "/WheelMotor", wheelMotor); + barMotorIO = new TalonFXIO(name + "/BarMotor", barMotor); + inputs = new IntakeIOInputsAutoLogged(); + } + + public void updateInputs(double wheelVelocityRPS, double barPositionDeg, Command currentCommand, Command defaultCommand) { + wheelMotorIO.updateInputs(); + barMotorIO.updateInputs(); + + inputs.wheelVelocityRPS = wheelVelocityRPS; + inputs.barPositionDeg = barPositionDeg; + inputs.currentCommand = currentCommand != null ? currentCommand.getName() : "None"; + inputs.defaultCommand = defaultCommand != null ? defaultCommand.getName() : "None"; + } + + public void process() { + wheelMotorIO.process(); + barMotorIO.process(); + Logger.processInputs(name, inputs); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/roller/Roller.java b/src/main/java/frc/robot/subsystems/roller/Roller.java index 16880dc..3fd0a47 100644 --- a/src/main/java/frc/robot/subsystems/roller/Roller.java +++ b/src/main/java/frc/robot/subsystems/roller/Roller.java @@ -12,8 +12,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; -import frc.robot.subsystems.TelemetryManager; -import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.simulation.BatterySim; import edu.wpi.first.wpilibj.simulation.FlywheelSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; @@ -34,14 +32,8 @@ public static Roller getInstance() { private ControlRequest request = new NeutralOut(); - private FlywheelSim sim; - /* - private MechanismLigament2d ligament; - - private final Mechanism2d mech2d = new Mechanism2d(20, 20); - private final MechanismRoot2d mech2droot = mech2d.getRoot("Bar Root", 10, 1); - */ + private RollerIO io; private Roller() { super(); @@ -58,13 +50,17 @@ private Roller() { DCMotor.getKrakenX44(1), 0.0); } - TelemetryManager.getInstance().addSendable(this); + + io = new RollerIO(getName(), motor); + // TelemetryManager.getInstance().addSendable(this); } @Override public void periodic(){ speed = motor.getVelocity().getValueAsDouble(); motor.setControl(request); + io.updateInputs(speed, getCurrentCommand(), getDefaultCommand()); + io.process(); } @Override @@ -99,10 +95,12 @@ public Command stop() { .withName("Stop"); } - @Override - public void initSendable(SendableBuilder builder){ - super.initSendable(builder); - builder.addDoubleProperty("Speed", () -> speed, null); - TelemetryManager.makeSendableTalonFX("Roller Motor", motor, builder); - } + // @Override + // public void initSendable(SendableBuilder builder){ + // super.initSendable(builder); + // builder.addDoubleProperty("Speed", () -> speed, null); + // TelemetryManager.makeSendableTalonFX("Roller Motor", motor, builder); + // } + + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/roller/RollerIO.java b/src/main/java/frc/robot/subsystems/roller/RollerIO.java new file mode 100644 index 0000000..c115fbe --- /dev/null +++ b/src/main/java/frc/robot/subsystems/roller/RollerIO.java @@ -0,0 +1,41 @@ +package frc.robot.subsystems.roller; + +import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.lib.io.TalonFXIO; + +public class RollerIO { + @AutoLog + public static class RollerIOInputs { + public double velocityRPS = 0.0; + public String currentCommand = ""; + public String defaultCommand = ""; + } + + private final String name; + private final TalonFXIO motorIO; + private final RollerIOInputsAutoLogged inputs; + + public RollerIO(String name, TalonFX motor) { + this.name = name; + motorIO = new TalonFXIO(name + "/Motor", motor); + inputs = new RollerIOInputsAutoLogged(); + } + + public void updateInputs(double velocityRPS, Command currentCommand, Command defaultCommand) { + motorIO.updateInputs(); + + inputs.velocityRPS = velocityRPS; + inputs.currentCommand = currentCommand != null ? currentCommand.getName() : "None"; + inputs.defaultCommand = defaultCommand != null ? defaultCommand.getName() : "None"; + } + + public void process() { + motorIO.process(); + Logger.processInputs(name, inputs); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index e149822..91f6f2c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -9,14 +9,12 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.simulation.BatterySim; import edu.wpi.first.wpilibj.simulation.FlywheelSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; -import frc.robot.subsystems.TelemetryManager; public class Shooter extends SubsystemBase { private static Shooter shooterLeftInstance; @@ -28,7 +26,6 @@ public static Shooter getLeftInstance() { return shooterLeftInstance; } - public static Shooter getRightInstance() { if (shooterRightInstance == null) { shooterRightInstance = new Shooter(false); @@ -45,10 +42,12 @@ public static Shooter getRightInstance() { private FlywheelSim topSim; private FlywheelSim bottomSim; + private ShooterIO io; + private Shooter(boolean left) { super(); - setName(this.getClass().getSimpleName() + (left ? "left" : "right")); + setName(this.getClass().getSimpleName() + (left ? "Left" : "Right")); int bottomID; int topID; @@ -84,7 +83,9 @@ private Shooter(boolean left) { 1 ), DCMotor.getKrakenX60(1), 0.0); } - TelemetryManager.getInstance().addSendable(this); + + io = new ShooterIO(getName(), topMotor, bottomMotor); + // TelemetryManager.getInstance().addSendable(this); } @Override @@ -94,6 +95,8 @@ public void periodic() { lastReadSpeedBottom = bottomMotor.getVelocity().getValueAsDouble(); topMotor.setControl(topRequest); bottomMotor.setControl(bottomRequest); + io.updateInputs(lastReadSpeedTop, lastReadSpeedBottom, getCurrentCommand(), getDefaultCommand()); + io.process(); } @Override @@ -150,20 +153,21 @@ public Command shoot() { return shoot(50, -50); } - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - - builder.addDoubleProperty( - "TopSpeed", - () -> lastReadSpeedTop, - null); - TelemetryManager.makeSendableTalonFX("Top", topMotor, builder); - - builder.addDoubleProperty( - "BottomSpeed", - () -> lastReadSpeedBottom, - null); - TelemetryManager.makeSendableTalonFX("Bottom", bottomMotor, builder); - } + // @Override + // public void initSendable(SendableBuilder builder) { + // super.initSendable(builder); + + // builder.addDoubleProperty( + // "TopSpeed", + // () -> lastReadSpeedTop, + // null); + // TelemetryManager.makeSendableTalonFX("Top", topMotor, builder); + + // builder.addDoubleProperty( + // "BottomSpeed", + // () -> lastReadSpeedBottom, + // null); + // TelemetryManager.makeSendableTalonFX("Bottom", bottomMotor, builder); + // } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java new file mode 100644 index 0000000..1bb5f5f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -0,0 +1,47 @@ +package frc.robot.subsystems.shooter; + +import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.lib.io.TalonFXIO; + +public class ShooterIO { + @AutoLog + public static class ShooterIOInputs { + public double topWheelVelocityRPS = 0.0; + public double bottomWheelVelocityRPS = 0.0; + public String currentCommand = ""; + public String defaultCommand = ""; + } + + private final String name; + private final TalonFXIO topMotorIO; + private final TalonFXIO bottomMotorIO; + private final ShooterIOInputsAutoLogged inputs; + + public ShooterIO(String name, TalonFX topMotor, TalonFX bottomMotor) { + this.name = name; + topMotorIO = new TalonFXIO(name + "/TopMotor", topMotor); + bottomMotorIO = new TalonFXIO(name + "/BottomMotor", bottomMotor); + inputs = new ShooterIOInputsAutoLogged(); + } + + public void updateInputs(double topWheelVelocityRPS, double bottomWheelVelocityRPS, Command currentCommand, Command defaultCommand) { + topMotorIO.updateInputs(); + bottomMotorIO.updateInputs(); + + inputs.topWheelVelocityRPS = topWheelVelocityRPS; + inputs.bottomWheelVelocityRPS = bottomWheelVelocityRPS; + inputs.currentCommand = currentCommand != null ? currentCommand.getName() : "None"; + inputs.defaultCommand = defaultCommand != null ? defaultCommand.getName() : "None"; + } + + public void process() { + topMotorIO.process(); + bottomMotorIO.process(); + Logger.processInputs(name, inputs); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java new file mode 100644 index 0000000..e69de29 From ed36554821f0bd66188e0493792fdd3952e2a129 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Mon, 16 Feb 2026 20:13:02 -0800 Subject: [PATCH 085/118] fixed a few things --- src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/Robot.java | 20 +- .../frc/robot/subsystems/drive/DriveIO.java | 6 + .../drive/ctre/CtreDriveTelemetry.java | 206 +++++++++--------- 4 files changed, 124 insertions(+), 118 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 293c407..0a4388f 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Robot2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 145; - public static final String GIT_SHA = "a17f0d2e3ee34733364081d418a69168a2dadd65"; - public static final String GIT_DATE = "2026-02-15 17:25:27 EST"; + public static final int GIT_REVISION = 146; + public static final String GIT_SHA = "9be5a5d570c4ba62c18739b7b6a4e4d80869ba71"; + public static final String GIT_DATE = "2026-02-16 21:49:27 EST"; public static final String GIT_BRANCH = "Akit"; - public static final String BUILD_DATE = "2026-02-16 21:47:52 EST"; - public static final long BUILD_UNIX_TIME = 1771296472495L; + public static final String BUILD_DATE = "2026-02-16 23:12:34 EST"; + public static final long BUILD_UNIX_TIME = 1771301554512L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b9fad63..13baacb 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -62,7 +62,16 @@ public class Robot extends LoggedRobot { * initialization code. */ public Robot() { - boolean replay = false; + boolean replay = Logger.hasReplaySource(); + //robot data loggers + // boolean usbPresent = new java.io.File("/u").exists(); + // if (usbPresent) { + // DataLogManager.start("/u/logs"); // USB stick + // System.out.println("Log/USB mounts OK"); + // } else { + // DataLogManager.start(); // falls back to /home/lvuser/logs + // System.out.println("Log/USB mounts NOT OK"); + // } Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); @@ -117,15 +126,6 @@ public Robot() { commandScheduler.schedule(VisionDeviceManager.getInstance().bootUp()); autoChooser = new AutoSelector(); - //robot data loggers - boolean usbPresent = new java.io.File("/u").exists(); - if (usbPresent) { - DataLogManager.start("/u/logs"); // USB stick - System.out.println("Log/USB mounts OK"); - } else { - DataLogManager.start(); // falls back to /home/lvuser/logs - System.out.println("Log/USB mounts NOT OK"); - } DriverStation.startDataLog(DataLogManager.getLog()); } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveIO.java b/src/main/java/frc/robot/subsystems/drive/DriveIO.java index 72862c8..745d824 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveIO.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveIO.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.drive; import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.BaseStatusSignal; @@ -33,8 +34,13 @@ public static class DriveIOInputs { public ChassisSpeeds fieldSpeeds = new ChassisSpeeds(); public String driveRequest = ""; + @AutoLogOutput public SwerveModuleState[] moduleStates = new SwerveModuleState[4]; + + @AutoLogOutput public SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; + + @AutoLogOutput public SwerveModuleState[] moduleTargets = new SwerveModuleState[4]; public String currentCommand = ""; diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveTelemetry.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveTelemetry.java index 12cdcb8..34a5403 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveTelemetry.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveTelemetry.java @@ -1,115 +1,115 @@ -// package frc.robot.subsystems.drive.ctre; +package frc.robot.subsystems.drive.ctre; -// import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; -// import edu.wpi.first.math.geometry.Pose2d; -// import edu.wpi.first.math.kinematics.ChassisSpeeds; -// import edu.wpi.first.math.kinematics.SwerveModulePosition; -// import edu.wpi.first.math.kinematics.SwerveModuleState; -// import edu.wpi.first.networktables.DoublePublisher; -// import edu.wpi.first.networktables.NetworkTable; -// import edu.wpi.first.networktables.NetworkTableInstance; -// import edu.wpi.first.networktables.StructArrayPublisher; -// import edu.wpi.first.networktables.StructPublisher; -// import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -// import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -// import edu.wpi.first.wpilibj.util.Color; -// import edu.wpi.first.wpilibj.util.Color8Bit; +import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; -// public class CtreDriveTelemetry { -// private final double MaxSpeed; +public class CtreDriveTelemetry { + private final double MaxSpeed; -// /** -// * Construct a telemetry object, with the specified max speed of the robot -// * -// * @param maxSpeed Maximum speed in meters per second -// */ -// public CtreDriveTelemetry(double maxSpeed) { -// MaxSpeed = maxSpeed; -// } + /** + * Construct a telemetry object, with the specified max speed of the robot + * + * @param maxSpeed Maximum speed in meters per second + */ + public CtreDriveTelemetry(double maxSpeed) { + MaxSpeed = maxSpeed; + } -// /* What to publish over networktables for telemetry */ -// private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); + /* What to publish over networktables for telemetry */ + private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); -// /* Robot swerve drive state */ -// private final NetworkTable driveStateTable = inst.getTable("SmartDashboard/Drive"); -// private final StructPublisher drivePose = -// driveStateTable.getStructTopic("Pose", Pose2d.struct).publish(); -// private final StructPublisher driveSpeeds = -// driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish(); -// private final StructArrayPublisher driveModuleStates = driveStateTable -// .getStructArrayTopic("ModuleStates", SwerveModuleState.struct) -// .publish(); -// private final StructArrayPublisher driveModuleTargets = driveStateTable -// .getStructArrayTopic("ModuleTargets", SwerveModuleState.struct) -// .publish(); -// private final StructArrayPublisher driveModulePositions = driveStateTable -// .getStructArrayTopic("ModulePositions", SwerveModulePosition.struct) -// .publish(); -// private final DoublePublisher driveTimestamp = -// driveStateTable.getDoubleTopic("Timestamp").publish(); -// private final DoublePublisher driveOdometryFrequency = -// driveStateTable.getDoubleTopic("OdometryFrequency").publish(); + /* Robot swerve drive state */ + private final NetworkTable driveStateTable = inst.getTable("SmartDashboard/Drive"); + private final StructPublisher drivePose = + driveStateTable.getStructTopic("Pose", Pose2d.struct).publish(); + private final StructPublisher driveSpeeds = + driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish(); + private final StructArrayPublisher driveModuleStates = driveStateTable + .getStructArrayTopic("ModuleStates", SwerveModuleState.struct) + .publish(); + private final StructArrayPublisher driveModuleTargets = driveStateTable + .getStructArrayTopic("ModuleTargets", SwerveModuleState.struct) + .publish(); + private final StructArrayPublisher driveModulePositions = driveStateTable + .getStructArrayTopic("ModulePositions", SwerveModulePosition.struct) + .publish(); + private final DoublePublisher driveTimestamp = + driveStateTable.getDoubleTopic("Timestamp").publish(); + private final DoublePublisher driveOdometryFrequency = + driveStateTable.getDoubleTopic("OdometryFrequency").publish(); -// /* Mechanisms to represent the swerve module states */ -// private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { -// new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), -// }; -// /* A direction and length changing ligament for speed representation */ -// private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { -// m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), -// m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), -// m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), -// m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), -// }; -// /* A direction changing and length constant ligament for module direction */ -// private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { -// m_moduleMechanisms[0] -// .getRoot("RootDirection", 0.5, 0.5) -// .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), -// m_moduleMechanisms[1] -// .getRoot("RootDirection", 0.5, 0.5) -// .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), -// m_moduleMechanisms[2] -// .getRoot("RootDirection", 0.5, 0.5) -// .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), -// m_moduleMechanisms[3] -// .getRoot("RootDirection", 0.5, 0.5) -// .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), -// }; + /* Mechanisms to represent the swerve module states */ + private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { + new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), + }; + /* A direction and length changing ligament for speed representation */ + private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { + m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + }; + /* A direction changing and length constant ligament for module direction */ + private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { + m_moduleMechanisms[0] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[1] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[2] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[3] + .getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + }; -// private final double[] m_moduleStatesArray = new double[8]; -// private final double[] m_moduleTargetsArray = new double[8]; + private final double[] m_moduleStatesArray = new double[8]; + private final double[] m_moduleTargetsArray = new double[8]; -// /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */ -// public void telemeterize(SwerveDriveState state) { -// /* Telemeterize the swerve drive state */ -// drivePose.set(state.Pose); -// driveSpeeds.set(state.Speeds); -// driveModuleStates.set(state.ModuleStates); -// driveModuleTargets.set(state.ModuleTargets); -// driveModulePositions.set(state.ModulePositions); -// driveTimestamp.set(state.Timestamp); -// driveOdometryFrequency.set(1.0 / state.OdometryPeriod); + /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */ + public void telemeterize(SwerveDriveState state) { + /* Telemeterize the swerve drive state */ + drivePose.set(state.Pose); + driveSpeeds.set(state.Speeds); + driveModuleStates.set(state.ModuleStates); + driveModuleTargets.set(state.ModuleTargets); + driveModulePositions.set(state.ModulePositions); + driveTimestamp.set(state.Timestamp); + driveOdometryFrequency.set(1.0 / state.OdometryPeriod); -// for (int i = 0; i < 4; ++i) { -// m_moduleStatesArray[i * 2 + 0] = state.ModuleStates[i].angle.getRadians(); -// m_moduleStatesArray[i * 2 + 1] = state.ModuleStates[i].speedMetersPerSecond; -// m_moduleTargetsArray[i * 2 + 0] = state.ModuleTargets[i].angle.getRadians(); -// m_moduleTargetsArray[i * 2 + 1] = state.ModuleTargets[i].speedMetersPerSecond; -// } + for (int i = 0; i < 4; ++i) { + m_moduleStatesArray[i * 2 + 0] = state.ModuleStates[i].angle.getRadians(); + m_moduleStatesArray[i * 2 + 1] = state.ModuleStates[i].speedMetersPerSecond; + m_moduleTargetsArray[i * 2 + 0] = state.ModuleTargets[i].angle.getRadians(); + m_moduleTargetsArray[i * 2 + 1] = state.ModuleTargets[i].speedMetersPerSecond; + } -// SmartDashboard.putNumberArray("Drive/ModuleStates", m_moduleStatesArray); -// SmartDashboard.putNumberArray("Drive/ModuleTargets", m_moduleTargetsArray); -// SmartDashboard.putNumber("Drive/OdometryPeriod", state.OdometryPeriod); + SmartDashboard.putNumberArray("Drive/ModuleStates", m_moduleStatesArray); + SmartDashboard.putNumberArray("Drive/ModuleTargets", m_moduleTargetsArray); + SmartDashboard.putNumber("Drive/OdometryPeriod", state.OdometryPeriod); -// /* Telemeterize the module states to a Mechanism2d */ -// for (int i = 0; i < 4; ++i) { -// m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); -// m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); -// m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); + /* Telemeterize the module states to a Mechanism2d */ + for (int i = 0; i < 4; ++i) { + m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); + m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); + m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); -// SmartDashboard.putData("Drive/Module " + i, m_moduleMechanisms[i]); -// } -// } -// } \ No newline at end of file + SmartDashboard.putData("Drive/Module " + i, m_moduleMechanisms[i]); + } + } +} \ No newline at end of file From fb2105c8c79f104d3ef4312e398a0db9e498f2c1 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Mon, 16 Feb 2026 20:57:40 -0800 Subject: [PATCH 086/118] added vision akit maybe --- src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/Robot.java | 1 + .../robot/subsystems/vision/VisionDevice.java | 11 ++- .../vision/VisionDeviceManager.java | 14 ++- .../frc/robot/subsystems/vision/VisionIO.java | 95 +++++++++++++++++++ 5 files changed, 118 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 0a4388f..ad66ab3 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Robot2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 146; - public static final String GIT_SHA = "9be5a5d570c4ba62c18739b7b6a4e4d80869ba71"; - public static final String GIT_DATE = "2026-02-16 21:49:27 EST"; + public static final int GIT_REVISION = 147; + public static final String GIT_SHA = "ed36554821f0bd66188e0493792fdd3952e2a129"; + public static final String GIT_DATE = "2026-02-16 23:13:02 EST"; public static final String GIT_BRANCH = "Akit"; - public static final String BUILD_DATE = "2026-02-16 23:12:34 EST"; - public static final long BUILD_UNIX_TIME = 1771301554512L; + public static final String BUILD_DATE = "2026-02-16 23:51:04 EST"; + public static final long BUILD_UNIX_TIME = 1771303864303L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 13baacb..a70273d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -148,6 +148,7 @@ public void robotPeriodic() { double now = Timer.getFPGATimestamp(); fpsTracker.add(1.0 / (now - lastTime)); Logger.recordOutput("FPS", fpsTracker.getAverage()); + Logger.recordOutput("rawDtMs", 1000 * (now - lastTime)); lastTime = now; } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionDevice.java b/src/main/java/frc/robot/subsystems/vision/VisionDevice.java index 1c16690..0bb5daa 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionDevice.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionDevice.java @@ -80,6 +80,9 @@ private void processFrames() { // } // } + // if (Robot.isSimulation()) { + // botPose = sim.process(0.01, constants.robotToCamera); + // } var result = camera.getLatestResult(); if (result.hasTargets()) { @@ -173,10 +176,10 @@ public void periodic() { processFrames(); - SmartDashboard.putNumber( - "Vision " + constants.tableName + "/Last Update Timestamp Timestamp", latestTimestamp); - // SmartDashboard.putNumber("Vision " + mConstants.tableName + "/N Queued Updates", frames.size()); - SmartDashboard.putBoolean("Vision " + constants.tableName + "/is Connnected", isConnected); + // SmartDashboard.putNumber( + // "Vision " + constants.tableName + "/Last Update Timestamp Timestamp", latestTimestamp); + // // SmartDashboard.putNumber("Vision " + mConstants.tableName + "/N Queued Updates", frames.size()); + // SmartDashboard.putBoolean("Vision " + constants.tableName + "/is Connnected", isConnected); } public boolean isConnected() { diff --git a/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java b/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java index 477e132..ec1f071 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java @@ -33,7 +33,7 @@ public static VisionDeviceManager getInstance() { private VisionDevice frontrCamera; private VisionDevice frontlCamera; - private List cameras; + public List cameras; private static TunableNumber timestampOffset = new TunableNumber("VisionTimestampOffset", (0.1), false); @@ -44,6 +44,8 @@ public static VisionDeviceManager getInstance() { public VisionSystemSim visionSim; + public VisionIO io; + public VisionDeviceManager() { // leftCamera = new VisionDevice(Constants.Limelight.VisionDeviceConstants.L_CONSTANTS); // rightCamera = new VisionDevice(Constants.Limelight.VisionDeviceConstants.R_CONSTANTS); @@ -58,7 +60,8 @@ public VisionDeviceManager() { } Drive.getInstance().getCtreDrive().setVisionMeasurementStdDevs(LOCAL_MEASUREMENT_STD_DEVS); Drive.getInstance().getCtreDrive().setStateStdDevs(STATE_STD_DEVS); - TelemetryManager.getInstance().addSendable(this); + io = new VisionIO(getName(), this); + // TelemetryManager.getInstance().addSendable(this); } @Override @@ -68,8 +71,11 @@ public void periodic() { } cameras.forEach(VisionDevice::periodic); movingAvgRead = headingAvg.getAverage(); - SmartDashboard.putNumber("Vision heading moving avg", getMovingAvgRead()); - SmartDashboard.putBoolean("vision disabled", getVisionDisabled()); + + io.updateInputs(getCurrentCommand(), getDefaultCommand()); + io.process(); + // SmartDashboard.putNumber("Vision heading moving avg", getMovingAvgRead()); + // SmartDashboard.putBoolean("vision disabled", getVisionDisabled()); } public double getMovingAvgRead() { diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index e69de29..916e2b4 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -0,0 +1,95 @@ +package frc.robot.subsystems.vision; + +import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; + +import java.util.List; + +public class VisionIO { + @AutoLog + public static class VisionIOInputs { + public boolean enabled = true; + public boolean fullyConnected = false; + + public double timestampOffset = 0.0; + public double headingMovingAverage = 0.0; + + public String currentCommand = ""; + public String defaultCommand = ""; + } + + public static class VisionDeviceIO { + @AutoLog + public static class VisionDeviceIOInputs { + public boolean connected = false; + public boolean hasTarget = false; + public Pose2d estimatedPose = Pose2d.kZero; + public double lastTimestamp = 0.0; + } + + private final String name; + private final VisionDevice device; + private final VisionDeviceIOInputsAutoLogged inputs; + + public VisionDeviceIO(String name, VisionDevice device) { + this.name = name; + this.device = device; + this.inputs = new VisionDeviceIOInputsAutoLogged(); + } + + public void updateInputs() { + inputs.connected = device.isConnected(); + inputs.hasTarget = device.hasTarget(); + inputs.estimatedPose = device.botPose != null ? device.botPose : Pose2d.kZero; + } + + public void process() { + Logger.processInputs(name, inputs); + } + } + + private final String name; + private final VisionDeviceManager manager; + private final VisionDeviceIO[] cameraIOs; + private final VisionIOInputsAutoLogged inputs; + + public VisionIO(String name, VisionDeviceManager manager) { + this.name = name; + this.manager = manager; + this.inputs = new VisionIOInputsAutoLogged(); + + List devices = manager.cameras; + + cameraIOs = new VisionDeviceIO[devices.size()]; + for (int i = 0; i < devices.size(); i++) { + cameraIOs[i] = new VisionDeviceIO( + name + "/Cameras/" + devices.get(i).getConstants().tableName, + devices.get(i)); + } + } + + public void updateInputs(Command currentCommand, Command defaultCommand) { + inputs.enabled = !VisionDeviceManager.getVisionDisabled(); + inputs.timestampOffset = VisionDeviceManager.getTimestampOffset(); + inputs.fullyConnected = manager.isFullyConnected(); + inputs.headingMovingAverage = manager.getMovingAvgRead(); + + inputs.currentCommand = currentCommand != null ? currentCommand.getName() : "None"; + inputs.defaultCommand = defaultCommand != null ? defaultCommand.getName() : "None"; + + for (VisionDeviceIO cameraIO : cameraIOs) { + cameraIO.updateInputs(); + } + } + + public void process() { + for (VisionDeviceIO cameraIO : cameraIOs) { + cameraIO.process(); + } + + Logger.processInputs(name, inputs); + } +} From 4d8860542751f77cebfbe5320421f48745e13b2d Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Mon, 16 Feb 2026 23:06:02 -0800 Subject: [PATCH 087/118] Works(?) in sim --- src/main/java/frc/robot/BuildConstants.java | 12 ++-- src/main/java/frc/robot/ControlsMapping.java | 28 ++++---- .../houndlib/BallConstants.java | 2 +- .../shooter => lib}/houndlib/BallPhysics.java | 2 +- .../shooter => lib}/houndlib/BallState.java | 2 +- .../houndlib/ShootOnTheFlyCalculator.java | 21 +++--- .../frc/robot/subsystems/drive/Drive.java | 69 ++++++++++++++----- .../frc/robot/subsystems/shooter/Shooter.java | 20 +++--- .../subsystems/shooter/ShooterConstants.java | 2 + .../subsystems/shooter/ShotCalculator.java | 27 +++++--- 10 files changed, 116 insertions(+), 69 deletions(-) rename src/main/java/frc/robot/{subsystems/shooter => lib}/houndlib/BallConstants.java (94%) rename src/main/java/frc/robot/{subsystems/shooter => lib}/houndlib/BallPhysics.java (99%) rename src/main/java/frc/robot/{subsystems/shooter => lib}/houndlib/BallState.java (90%) rename src/main/java/frc/robot/{subsystems/shooter => lib}/houndlib/ShootOnTheFlyCalculator.java (96%) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index ad66ab3..f18d6f1 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Robot2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 147; - public static final String GIT_SHA = "ed36554821f0bd66188e0493792fdd3952e2a129"; - public static final String GIT_DATE = "2026-02-16 23:13:02 EST"; - public static final String GIT_BRANCH = "Akit"; - public static final String BUILD_DATE = "2026-02-16 23:51:04 EST"; - public static final long BUILD_UNIX_TIME = 1771303864303L; + public static final int GIT_REVISION = 154; + public static final String GIT_SHA = "5ef7a9e03334a932a2f95144cd7174b2ca62aa78"; + public static final String GIT_DATE = "2026-02-17 01:15:37 EST"; + public static final String GIT_BRANCH = "sotm"; + public static final String BUILD_DATE = "2026-02-17 01:34:08 EST"; + public static final long BUILD_UNIX_TIME = 1771310048772L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index 9df6921..fa70475 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -16,6 +16,7 @@ import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.ShotCalculator; import frc.robot.subsystems.vision.VisionDeviceManager; import edu.wpi.first.wpilibj2.command.Commands; @@ -32,24 +33,23 @@ public static void mapTeleopCommand() { controller.b().whileTrue(Intake.getInstance().outtake()); controller.x().onTrue(Climb.getInstance().hangCommand()); - controller.rightTrigger().debounce(0.1).onTrue( - Commands.parallel( - Shooter.getLeftInstance().shoot(), - Shooter.getRightInstance().shoot()) - ).onFalse( - Commands.parallel( - Shooter.getLeftInstance().stop(), - Shooter.getRightInstance().stop()) + controller.rightTrigger().debounce(0.1 + // ).onTrue( + // Commands.parallel( + // Shooter.getLeftInstance().shoot(50, -50), + // Shooter.getRightInstance().shoot(50, -50)) + // ).onFalse( + // Commands.parallel( + // Shooter.getLeftInstance().stop(), + // Shooter.getRightInstance().stop()) ).whileTrue( Commands.parallel( - Indexer.getLeftInstance().activateIndexer(), - Indexer.getRightInstance().activateIndexer()) + Indexer.getLeftInstance().activateIndexer(), + Indexer.getRightInstance().activateIndexer()) ); - controller.leftBumper().whileTrue(Drive.getInstance().headingLockToPose( - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? - DriveConstants.FieldPoses.HUB.pose : - FieldUtil.flipPose(DriveConstants.FieldPoses.HUB.pose))); + controller.leftBumper().whileTrue(Drive.getInstance().headingLockToHub() + .alongWith(Shooter.getLeftInstance().shoot(), Shooter.getRightInstance().shoot())); } public static void mapSysId() { diff --git a/src/main/java/frc/robot/subsystems/shooter/houndlib/BallConstants.java b/src/main/java/frc/robot/lib/houndlib/BallConstants.java similarity index 94% rename from src/main/java/frc/robot/subsystems/shooter/houndlib/BallConstants.java rename to src/main/java/frc/robot/lib/houndlib/BallConstants.java index db9135e..5f703fc 100644 --- a/src/main/java/frc/robot/subsystems/shooter/houndlib/BallConstants.java +++ b/src/main/java/frc/robot/lib/houndlib/BallConstants.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.shooter.houndlib; +package frc.robot.lib.houndlib; public class BallConstants { public final double mass; diff --git a/src/main/java/frc/robot/subsystems/shooter/houndlib/BallPhysics.java b/src/main/java/frc/robot/lib/houndlib/BallPhysics.java similarity index 99% rename from src/main/java/frc/robot/subsystems/shooter/houndlib/BallPhysics.java rename to src/main/java/frc/robot/lib/houndlib/BallPhysics.java index 9d9a9b8..5f3b746 100644 --- a/src/main/java/frc/robot/subsystems/shooter/houndlib/BallPhysics.java +++ b/src/main/java/frc/robot/lib/houndlib/BallPhysics.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.shooter.houndlib; +package frc.robot.lib.houndlib; import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose3d; diff --git a/src/main/java/frc/robot/subsystems/shooter/houndlib/BallState.java b/src/main/java/frc/robot/lib/houndlib/BallState.java similarity index 90% rename from src/main/java/frc/robot/subsystems/shooter/houndlib/BallState.java rename to src/main/java/frc/robot/lib/houndlib/BallState.java index 3f8fcc4..191ffde 100644 --- a/src/main/java/frc/robot/subsystems/shooter/houndlib/BallState.java +++ b/src/main/java/frc/robot/lib/houndlib/BallState.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.shooter.houndlib; +package frc.robot.lib.houndlib; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Translation3d; diff --git a/src/main/java/frc/robot/subsystems/shooter/houndlib/ShootOnTheFlyCalculator.java b/src/main/java/frc/robot/lib/houndlib/ShootOnTheFlyCalculator.java similarity index 96% rename from src/main/java/frc/robot/subsystems/shooter/houndlib/ShootOnTheFlyCalculator.java rename to src/main/java/frc/robot/lib/houndlib/ShootOnTheFlyCalculator.java index cfe07f9..ba4fdfe 100644 --- a/src/main/java/frc/robot/subsystems/shooter/houndlib/ShootOnTheFlyCalculator.java +++ b/src/main/java/frc/robot/lib/houndlib/ShootOnTheFlyCalculator.java @@ -1,10 +1,9 @@ -package frc.robot.subsystems.shooter.houndlib; +package frc.robot.lib.houndlib; import java.util.function.Function; +import frc.robot.lib.houndlib.BallPhysics.ShotSolution; import frc.robot.lib.trajectory.RedTrajectory.State.ChassisAccels; -import frc.robot.subsystems.shooter.houndlib.BallPhysics.ShotSolution; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform3d; @@ -207,25 +206,25 @@ public static InterceptSolution solveShootOnTheFly( Pose3d targetPose, ChassisSpeeds fieldRelRobotVelocity, ChassisAccels fieldRelRobotAcceleration, - double targetSpeedRps, + double incomingAngle, int maxIterations, double timeTolerance) { ShotSolution sol = BallPhysics.solveBallisticWithSpeed( shooterPose, targetPose, - targetSpeedRps); + incomingAngle); double t = sol.flightTimeSeconds(); Pose3d effectiveTarget = targetPose; for (int i = 0; i < maxIterations; i++) { - double dx = fieldRelRobotVelocity.vxMetersPerSecond * t; - // + 0.5 * fieldRelRobotAcceleration.axMetersPerSecondSquared * t * t; + double dx = fieldRelRobotVelocity.vxMetersPerSecond * t + + 0.5 * fieldRelRobotAcceleration.ax * t * t; - double dy = fieldRelRobotVelocity.vyMetersPerSecond * t; - // + 0.5 * fieldRelRobotAcceleration.ayMetersPerSecondSquared * t * t; + double dy = fieldRelRobotVelocity.vyMetersPerSecond * t + + 0.5 * fieldRelRobotAcceleration.ay * t * t; effectiveTarget = new Pose3d( targetPose.getX() - dx, @@ -233,10 +232,10 @@ public static InterceptSolution solveShootOnTheFly( targetPose.getZ(), targetPose.getRotation()); - ShotSolution newSol = BallPhysics.solveBallisticWithSpeed( + ShotSolution newSol = BallPhysics.solveBallisticWithIncomingAngle( shooterPose, effectiveTarget, - targetSpeedRps); + incomingAngle); if (Math.abs(newSol.flightTimeSeconds() - t) < timeTolerance) { return new InterceptSolution( diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 8a42c21..553fe81 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -4,6 +4,9 @@ import java.util.function.Consumer; import java.util.function.Function; +import java.util.function.Supplier; + +import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; @@ -15,6 +18,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; // import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N1; @@ -23,6 +27,8 @@ import edu.wpi.first.units.BaseUnits; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Time; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -33,10 +39,12 @@ import frc.robot.lib.control.ControlConstants.ProfiledPIDVConstants; import frc.robot.lib.control.ProfiledPIDVController; import frc.robot.lib.field.FieldLayout; +import frc.robot.lib.field.FieldUtil; import frc.robot.lib.trajectory.LocalADStarWrapper; import frc.robot.lib.util.Util; import frc.robot.subsystems.TelemetryManager; import frc.robot.subsystems.drive.ctre.CtreDriveConstants; +import frc.robot.subsystems.shooter.ShotCalculator; import frc.robot.subsystems.drive.commands.AutopilotCommand; import frc.robot.subsystems.drive.commands.PIDToPoseCommand; import frc.robot.subsystems.drive.commands.TrajectoryCommand; @@ -233,18 +241,29 @@ public Command headingLockToPose(Pose2d pose) { } /** - * Locks the robot onto a pose, with TOF Adjustment + * Locks the robot onto a pose. * Utilizes feedforwards derived from the current chassis speeds */ - public Command headingLockToPoseWithTOFAdjustment(Pose2d pose, Function tof, Consumer tofAcceptor) { - SwerveRequest.FieldCentricFacingAngle request = - new SwerveRequest.FieldCentricFacingAngle() - .withHeadingPID(25, 0, 0.01) - .withMaxAbsRotationalRate(MAX_ROTATION_SPEED); + public Command headingLockToPose(Supplier pose) { + SwerveRequest.FieldCentric request = + new SwerveRequest.FieldCentric(); + + ProfiledPIDVController thetaController = + new ProfiledPIDVController( + new ProfiledPIDVConstants( + new PIDVConstants(10.0, 0.0, 1), + new TrapezoidProfile.Constraints(Math.PI * 16, Math.PI * 5)) + ); + thetaController.enableContinuousInput(-Math.PI, Math.PI); return runOnce(() -> { - request.withVelocityX(0).withVelocityY(0).withTargetDirection(getPose().getRotation()); + request.withVelocityX(0).withVelocityY(0) + .withRotationalRate(0); setSwerveRequest(request); + + thetaController.setInitialSetpoint( + getPose().getRotation().getRadians(), + getState().Speeds.omegaRadiansPerSecond); }).andThen( run(() -> { double xDesiredRaw = -Robot.controller.getLeftY(); @@ -255,29 +274,47 @@ public Command headingLockToPoseWithTOFAdjustment(Pose2d pose, Function 1e-4 ? (-delta.getX() * fieldSpeeds.vyMetersPerSecond + delta.getY() * fieldSpeeds.vxMetersPerSecond) / (normSq) : 0.0; + + var rotation = thetaController + .setTarget(targetDirection.getRadians(), rotationalRate) + .setMeasurement(state.Pose.getRotation().getRadians(), state.Speeds.omegaRadiansPerSecond) + .getOutput(); - SmartDashboard.putNumber("error tracking", - MathUtil.inputModulus(state.Pose.getRotation().minus(targetDirection).getDegrees(), -180, 180 - )); + Logger.recordOutput("Tracking Error", + MathUtil.inputModulus(state.Pose.getRotation().minus(targetDirection).getDegrees(), -180, 180)); - // Translation2d speedVector = new Translation2d(fieldSpeeds.vxMetersPerSecond, fieldSpeeds.vyMetersPerSecond); - request + // .withHeadingPID(p.get(), i.get(), d.get()) .withVelocityX(xFancy * MAX_SPEED) .withVelocityY(yFancy * MAX_SPEED) - .withTargetDirection(targetDirection) - .withTargetRateFeedforward(rotationalRate * 1.5); + .withRotationalRate(rotation); }).handleInterrupt(() -> setSwerveRequest(new SwerveRequest.FieldCentric()))).withName("Heading Lock"); } + /** + * Locks the robot onto a pose, with TOF Adjustment + * Utilizes feedforwards derived from the current chassis speeds + */ + public Command headingLockToHub() { + return Commands.runOnce(() -> ShotCalculator.getInstance().setTarget( + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? + DriveConstants.FieldPoses.HUB.pose3d : + new Pose3d( + FieldUtil.flipPose(DriveConstants.FieldPoses.HUB.pose).getX(), + FieldUtil.flipPose(DriveConstants.FieldPoses.HUB.pose).getY(), + DriveConstants.FieldPoses.HUB.pose3d.getZ(), Rotation3d.kZero))) + .andThen(headingLockToPose(() -> ShotCalculator.getInstance().getCurrentEffectiveTargetPose().toPose2d())); + } + /** * Auto aligns to the nearest reef face * @param left chooses the left or right face diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index a52a1e6..cc218dc 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.shooter; +import static frc.robot.subsystems.shooter.ShooterConstants.*; + import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.controls.NeutralOut; @@ -14,7 +16,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; -import frc.robot.subsystems.drive.Drive; public class Shooter extends SubsystemBase { private static Shooter shooterLeftInstance; @@ -55,20 +56,20 @@ private Shooter(boolean left) { int topID; if (left) { - bottomID = ShooterConstants.Motors.BOTTOMLEFT.id; - topID = ShooterConstants.Motors.TOPLEFT.id; + bottomID = Motors.BOTTOMLEFT.id; + topID = Motors.TOPLEFT.id; } else { - bottomID = ShooterConstants.Motors.BOTTOMRIGHT.id; - topID = ShooterConstants.Motors.TOPRIGHT.id; + bottomID = Motors.BOTTOMRIGHT.id; + topID = Motors.TOPRIGHT.id; } bottomMotor = new TalonFX(bottomID); - bottomMotor.getConfigurator().apply(ShooterConstants.getConfig()); + bottomMotor.getConfigurator().apply(getConfig()); bottomMotor.setNeutralMode(NeutralModeValue.Coast); topMotor = new TalonFX(topID); - topMotor.getConfigurator().apply(ShooterConstants.getConfig()); + topMotor.getConfigurator().apply(getConfig()); topMotor.setNeutralMode(NeutralModeValue.Coast); if (Robot.isSimulation()) { @@ -152,9 +153,8 @@ public Command shoot(double topSpeed, double bottomSpeed) { } public Command shoot() { - return Drive.getInstance() - .headingLockToPose(shotCalculator.getCurrentEffectiveTargetPose().toPose2d()) - .andThen(shoot(3000 / 60.0, 3000 / 60.0)); + return defer(() -> shoot(shotCalculator.getInterceptSolution().launchSpeed() - TOPSPIN_FACTOR, + -shotCalculator.getInterceptSolution().launchSpeed() - TOPSPIN_FACTOR)); } // @Override diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 3eaa625..b42dc0c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -11,6 +11,8 @@ public final class ShooterConstants { public static final double GEAR_RATIO = 1; + public static final double TOPSPIN_FACTOR = 0; + public static final Transform3d OFFSET = new Transform3d(); public static final InterpolatingDoubleTreeMap DISTANCE_TO_SHOT_SPEED = new InterpolatingDoubleTreeMap(); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java index 030a985..bde87a6 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java @@ -1,12 +1,16 @@ package frc.robot.subsystems.shooter; -import frc.robot.subsystems.shooter.houndlib.ShootOnTheFlyCalculator; -import frc.robot.subsystems.shooter.houndlib.ShootOnTheFlyCalculator.InterceptSolution; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.AutoLogOutputManager; +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +import frc.robot.lib.houndlib.ShootOnTheFlyCalculator; +import frc.robot.lib.houndlib.ShootOnTheFlyCalculator.InterceptSolution; import frc.robot.lib.trajectory.RedTrajectory.State.ChassisAccels; import frc.robot.subsystems.drive.Drive; @@ -23,20 +27,23 @@ public static ShotCalculator getInstance() { private final Drive drivetrain; + @AutoLogOutput private Pose3d currentEffectiveTargetPose = Pose3d.kZero; private double currentEffectiveYaw; + @AutoLogOutput private InterceptSolution currentInterceptSolution; private Pose3d targetLocation = new Pose3d(); private double targetDistance = 0.0; - private double targetSpeedRps = 8; + private double shooterAngle = 75 * Constants.TAU / 360; private ShotCalculator() { this.drivetrain = Drive.getInstance(); + AutoLogOutputManager.addObject(this); } @Override @@ -44,24 +51,26 @@ public void periodic() { Pose2d drivetrainPose = drivetrain.getPose(); targetDistance = drivetrainPose.getTranslation().getDistance(targetLocation.toPose2d().getTranslation()); - targetSpeedRps = ShooterConstants.DISTANCE_TO_SHOT_SPEED.get(targetDistance); Pose3d shooterPose = new Pose3d(drivetrainPose).plus(ShooterConstants.OFFSET); ChassisSpeeds drivetrainSpeeds = drivetrain.getFieldSpeeds(); ChassisAccels drivetrainAccelerations = ChassisAccels.estimate(drivetrainSpeeds, drivetrain.getPrevFieldSpeeds(), Constants.DT); - currentInterceptSolution = ShootOnTheFlyCalculator.solveShootOnTheFly(shooterPose, targetLocation, - drivetrainSpeeds, drivetrainAccelerations, targetSpeedRps, - 5, 0.01); + currentInterceptSolution = ShootOnTheFlyCalculator.solveShootOnTheFly( + shooterPose, + targetLocation, + drivetrainSpeeds, + new ChassisAccels(), + -shooterAngle, + 5, 0.01); currentEffectiveTargetPose = currentInterceptSolution.effectiveTargetPose(); currentEffectiveYaw = currentInterceptSolution.requiredYaw(); } - public void setTarget(Pose3d targetLocation, double targetSpeedRps) { + public void setTarget(Pose3d targetLocation) { this.targetLocation = targetLocation; - this.targetSpeedRps = targetSpeedRps; } public Pose3d getCurrentEffectiveTargetPose() { From a0460dbdb6be3d01e1532a98022679c1703c53a1 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Tue, 17 Feb 2026 18:42:31 -0800 Subject: [PATCH 088/118] added fuel simulation Sotm dosn't work as well as it shooud though --- src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/ControlsMapping.java | 13 + src/main/java/frc/robot/Robot.java | 21 + src/main/java/frc/robot/lib/sim/FuelSim.java | 861 ++++++++++++++++++ .../frc/robot/subsystems/shooter/Shooter.java | 27 +- .../subsystems/shooter/ShotCalculator.java | 23 +- 6 files changed, 934 insertions(+), 21 deletions(-) create mode 100644 src/main/java/frc/robot/lib/sim/FuelSim.java diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index f18d6f1..62cb309 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Robot2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 154; - public static final String GIT_SHA = "5ef7a9e03334a932a2f95144cd7174b2ca62aa78"; - public static final String GIT_DATE = "2026-02-17 01:15:37 EST"; + public static final int GIT_REVISION = 155; + public static final String GIT_SHA = "4d8860542751f77cebfbe5320421f48745e13b2d"; + public static final String GIT_DATE = "2026-02-17 02:06:02 EST"; public static final String GIT_BRANCH = "sotm"; - public static final String BUILD_DATE = "2026-02-17 01:34:08 EST"; - public static final long BUILD_UNIX_TIME = 1771310048772L; + public static final String BUILD_DATE = "2026-02-17 21:38:32 EST"; + public static final long BUILD_UNIX_TIME = 1771382312237L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index fa70475..ac2c7d6 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -1,5 +1,8 @@ package frc.robot; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.MetersPerSecond; import static frc.robot.Robot.controller; import com.ctre.phoenix6.swerve.SwerveRequest; @@ -46,6 +49,16 @@ public static void mapTeleopCommand() { Commands.parallel( Indexer.getLeftInstance().activateIndexer(), Indexer.getRightInstance().activateIndexer()) + ).whileTrue( + Commands.repeatingSequence( + Commands.runOnce(() -> Robot.fuelSim.launchFuel( + MetersPerSecond.of( + Shooter.getLeftInstance().getTopSpeed() * Constants.TAU * 0.0508), + Degrees.of(75), + Degrees.of(0), + Inches.of(19) + )).andThen(Commands.waitSeconds(0.1)) + ) ); controller.leftBumper().whileTrue(Drive.getInstance().headingLockToHub() diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a70273d..2775da2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,8 +1,11 @@ package frc.robot; import frc.robot.auto.AutoSelector; +import frc.robot.lib.sim.FuelSim; import frc.robot.lib.util.MovingAverageDouble; +import static edu.wpi.first.units.Units.Inches; + import java.util.Optional; import org.littletonrobotics.junction.LogFileUtil; @@ -25,6 +28,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.Controllers; import frc.robot.auto.AutoSelector; @@ -212,13 +216,30 @@ public void testInit() { public void testPeriodic() { } + public static FuelSim fuelSim = new FuelSim(); + /** This function is called once when the robot is first started up. */ @Override public void simulationInit() { + // fuelSim.spawnStartingFuel(); + fuelSim.registerRobot( + Inches.of(27), + Inches.of(27), + Inches.of(0.5), + () -> Drive.getInstance().getPose(), + () -> Drive.getInstance().getFieldSpeeds()); + fuelSim.registerIntake( + Inches.of(-13), Inches.of(13), Inches.of(-21.5), Inches.of(-17.5)); + fuelSim.start(); + fuelSim.enableAirResistance(); + } /** This function is called periodically whilst in simulation. */ @Override public void simulationPeriodic() { + fuelSim.updateSim(); + Logger.recordOutput("Blue Score", FuelSim.Hub.BLUE_HUB.getScore()); + Logger.recordOutput("Red Score", FuelSim.Hub.RED_HUB.getScore()); } } diff --git a/src/main/java/frc/robot/lib/sim/FuelSim.java b/src/main/java/frc/robot/lib/sim/FuelSim.java new file mode 100644 index 0000000..28ce7bc --- /dev/null +++ b/src/main/java/frc/robot/lib/sim/FuelSim.java @@ -0,0 +1,861 @@ +// https://github.com/hammerheads5000/FuelSim +package frc.robot.lib.sim; + +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Radians; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; +import java.util.ArrayList; +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; + +public class FuelSim { + protected static final double PERIOD = 0.02; // sec + protected static final Translation3d GRAVITY = new Translation3d(0, 0, -9.81); // m/s^2 + // Room temperature dry air density: https://en.wikipedia.org/wiki/Density_of_air#Dry_air + protected static final double AIR_DENSITY = 1.2041; // kg/m^3 + protected static final double FIELD_COR = Math.sqrt(22 / 51.5); // coefficient of restitution with the field + protected static final double FUEL_COR = 0.5; // coefficient of restitution with another fuel + protected static final double NET_COR = 0.2; // coefficient of restitution with the net + protected static final double ROBOT_COR = 0.1; // coefficient of restitution with a robot + protected static final double FUEL_RADIUS = 0.075; + protected static final double FIELD_LENGTH = 16.51; + protected static final double FIELD_WIDTH = 8.04; + protected static final double TRENCH_WIDTH = 1.265; + protected static final double TRENCH_BLOCK_WIDTH = 0.305; + protected static final double TRENCH_HEIGHT = 0.565; + protected static final double TRENCH_BAR_HEIGHT = 0.102; + protected static final double TRENCH_BAR_WIDTH = 0.152; + protected static final double FRICTION = 0.1; // proportion of horizontal vel to lose per sec while on ground + protected static final double FUEL_MASS = 0.448 * 0.45392; // kgs + protected static final double FUEL_CROSS_AREA = Math.PI * FUEL_RADIUS * FUEL_RADIUS; + // Drag coefficient of smooth sphere: https://en.wikipedia.org/wiki/Drag_coefficient#/media/File:14ilf1l.svg + protected static final double DRAG_COF = 0.47; // dimensionless + protected static final double DRAG_FORCE_FACTOR = 0.5 * AIR_DENSITY * DRAG_COF * FUEL_CROSS_AREA; + + protected static final Translation3d[] FIELD_XZ_LINE_STARTS = { + new Translation3d(0, 0, 0), + new Translation3d(3.96, 1.57, 0), + new Translation3d(3.96, FIELD_WIDTH / 2 + 0.60, 0), + new Translation3d(4.61, 1.57, 0.165), + new Translation3d(4.61, FIELD_WIDTH / 2 + 0.60, 0.165), + new Translation3d(FIELD_LENGTH - 5.18, 1.57, 0), + new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH / 2 + 0.60, 0), + new Translation3d(FIELD_LENGTH - 4.61, 1.57, 0.165), + new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2 + 0.60, 0.165), + new Translation3d(3.96, TRENCH_WIDTH, TRENCH_HEIGHT), + new Translation3d(3.96, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), + new Translation3d(FIELD_LENGTH - 5.18, TRENCH_WIDTH, TRENCH_HEIGHT), + new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), + new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d(FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + }; + + protected static final Translation3d[] FIELD_XZ_LINE_ENDS = { + new Translation3d(FIELD_LENGTH, FIELD_WIDTH, 0), + new Translation3d(4.61, FIELD_WIDTH / 2 - 0.60, 0.165), + new Translation3d(4.61, FIELD_WIDTH - 1.57, 0.165), + new Translation3d(5.18, FIELD_WIDTH / 2 - 0.60, 0), + new Translation3d(5.18, FIELD_WIDTH - 1.57, 0), + new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2 - 0.60, 0.165), + new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH - 1.57, 0.165), + new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH / 2 - 0.60, 0), + new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57, 0), + new Translation3d(5.18, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), + new Translation3d(5.18, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), + new Translation3d(FIELD_LENGTH - 3.96, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), + new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), + new Translation3d( + 4.61 + TRENCH_BAR_WIDTH / 2, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d(4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, + TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d(FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + }; + + protected static class Fuel { + protected Translation3d pos; + protected Translation3d vel; + + protected Fuel(Translation3d pos, Translation3d vel) { + this.pos = pos; + this.vel = vel; + } + + protected Fuel(Translation3d pos) { + this(pos, new Translation3d()); + } + + protected void update(boolean simulateAirResistance, int subticks) { + pos = pos.plus(vel.times(PERIOD / subticks)); + if (pos.getZ() > FUEL_RADIUS) { + Translation3d Fg = GRAVITY.times(FUEL_MASS); + Translation3d Fd = new Translation3d(); + + if (simulateAirResistance) { + double speed = vel.getNorm(); + if (speed > 1e-6) { + Fd = vel.times(-DRAG_FORCE_FACTOR * speed); + } + } + + Translation3d accel = Fg.plus(Fd).div(FUEL_MASS); + vel = vel.plus(accel.times(PERIOD / subticks)); + } + if (Math.abs(vel.getZ()) < 0.05 && pos.getZ() <= FUEL_RADIUS + 0.03) { + vel = new Translation3d(vel.getX(), vel.getY(), 0); + vel = vel.times(1 - FRICTION * PERIOD / subticks); + // pos = new Translation3d(pos.getX(), pos.getY(), FUEL_RADIUS); + } + handleFieldCollisions(subticks); + } + + protected void handleXZLineCollision(Translation3d lineStart, Translation3d lineEnd) { + if (pos.getY() < lineStart.getY() || pos.getY() > lineEnd.getY()) return; // not within y range + // Convert into 2D + Translation2d start2d = new Translation2d(lineStart.getX(), lineStart.getZ()); + Translation2d end2d = new Translation2d(lineEnd.getX(), lineEnd.getZ()); + Translation2d pos2d = new Translation2d(pos.getX(), pos.getZ()); + Translation2d lineVec = end2d.minus(start2d); + + // Get closest point on line + Translation2d projected = + start2d.plus(lineVec.times(pos2d.minus(start2d).dot(lineVec) / lineVec.getSquaredNorm())); + + if (projected.getDistance(start2d) + projected.getDistance(end2d) > lineVec.getNorm()) + return; // projected point not on line + double dist = pos2d.getDistance(projected); + if (dist > FUEL_RADIUS) return; // not intersecting line + // Back into 3D + Translation3d normal = new Translation3d(-lineVec.getY(), 0, lineVec.getX()).div(lineVec.getNorm()); + + // Apply collision response + pos = pos.plus(normal.times(FUEL_RADIUS - dist)); + if (vel.dot(normal) > 0) return; // already moving away from line + vel = vel.minus(normal.times((1 + FIELD_COR) * vel.dot(normal))); + } + + protected void handleFieldCollisions(int subticks) { + // floor and bumps + for (int i = 0; i < FIELD_XZ_LINE_STARTS.length; i++) { + handleXZLineCollision(FIELD_XZ_LINE_STARTS[i], FIELD_XZ_LINE_ENDS[i]); + } + + // edges + if (pos.getX() < FUEL_RADIUS && vel.getX() < 0) { + pos = pos.plus(new Translation3d(FUEL_RADIUS - pos.getX(), 0, 0)); + vel = vel.plus(new Translation3d(-(1 + FIELD_COR) * vel.getX(), 0, 0)); + } else if (pos.getX() > FIELD_LENGTH - FUEL_RADIUS && vel.getX() > 0) { + pos = pos.plus(new Translation3d(FIELD_LENGTH - FUEL_RADIUS - pos.getX(), 0, 0)); + vel = vel.plus(new Translation3d(-(1 + FIELD_COR) * vel.getX(), 0, 0)); + } + + if (pos.getY() < FUEL_RADIUS && vel.getY() < 0) { + pos = pos.plus(new Translation3d(0, FUEL_RADIUS - pos.getY(), 0)); + vel = vel.plus(new Translation3d(0, -(1 + FIELD_COR) * vel.getY(), 0)); + } else if (pos.getY() > FIELD_WIDTH - FUEL_RADIUS && vel.getY() > 0) { + pos = pos.plus(new Translation3d(0, FIELD_WIDTH - FUEL_RADIUS - pos.getY(), 0)); + vel = vel.plus(new Translation3d(0, -(1 + FIELD_COR) * vel.getY(), 0)); + } + + // hubs + handleHubCollisions(Hub.BLUE_HUB, subticks); + handleHubCollisions(Hub.RED_HUB, subticks); + + handleTrenchCollisions(); + } + + protected void handleHubCollisions(Hub hub, int subticks) { + hub.handleHubInteraction(this, subticks); + hub.fuelCollideSide(this); + + double netCollision = hub.fuelHitNet(this); + if (netCollision != 0) { + pos = pos.plus(new Translation3d(netCollision, 0, 0)); + vel = new Translation3d(-vel.getX() * NET_COR, vel.getY() * NET_COR, vel.getZ()); + } + } + + protected void handleTrenchCollisions() { + fuelCollideRectangle( + this, + new Translation3d(3.96, TRENCH_WIDTH, 0), + new Translation3d(5.18, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(3.96, FIELD_WIDTH - 1.57, 0), + new Translation3d(5.18, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(FIELD_LENGTH - 5.18, TRENCH_WIDTH, 0), + new Translation3d(FIELD_LENGTH - 3.96, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH - 1.57, 0), + new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT), + new Translation3d( + 4.61 + TRENCH_BAR_WIDTH / 2, + TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), + new Translation3d(4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, + TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, + FIELD_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); + } + + protected void addImpulse(Translation3d impulse) { + vel = vel.plus(impulse); + } + } + + protected static void handleFuelCollision(Fuel a, Fuel b) { + Translation3d normal = a.pos.minus(b.pos); + double distance = normal.getNorm(); + if (distance == 0) { + normal = new Translation3d(1, 0, 0); + distance = 1; + } + normal = normal.div(distance); + double impulse = 0.5 * (1 + FUEL_COR) * (b.vel.minus(a.vel).dot(normal)); + double intersection = FUEL_RADIUS * 2 - distance; + a.pos = a.pos.plus(normal.times(intersection / 2)); + b.pos = b.pos.minus(normal.times(intersection / 2)); + a.addImpulse(normal.times(impulse)); + b.addImpulse(normal.times(-impulse)); + } + + protected static final double CELL_SIZE = 0.25; + protected static final int GRID_COLS = (int) Math.ceil(FIELD_LENGTH / CELL_SIZE); + protected static final int GRID_ROWS = (int) Math.ceil(FIELD_WIDTH / CELL_SIZE); + + @SuppressWarnings("unchecked") + protected final ArrayList[][] grid = new ArrayList[GRID_COLS][GRID_ROWS]; + private final ArrayList> activeCells = new ArrayList<>(); + + protected void handleFuelCollisions(ArrayList fuels) { + // Clear grid + for (ArrayList cell : activeCells) { + cell.clear(); + } + activeCells.clear(); + + // Populate grid + for (Fuel fuel : fuels) { + int col = (int) (fuel.pos.getX() / CELL_SIZE); + int row = (int) (fuel.pos.getY() / CELL_SIZE); + + if (col >= 0 && col < GRID_COLS && row >= 0 && row < GRID_ROWS) { + grid[col][row].add(fuel); + if (grid[col][row].size() == 1) { + activeCells.add(grid[col][row]); + } + } + } + + // Check collisions + for (Fuel fuel : fuels) { + int col = (int) (fuel.pos.getX() / CELL_SIZE); + int row = (int) (fuel.pos.getY() / CELL_SIZE); + + // Check 3x3 neighbor cells + for (int i = col - 1; i <= col + 1; i++) { + for (int j = row - 1; j <= row + 1; j++) { + if (i >= 0 && i < GRID_COLS && j >= 0 && j < GRID_ROWS) { + for (Fuel other : grid[i][j]) { + if (fuel != other && fuel.pos.getDistance(other.pos) < FUEL_RADIUS * 2) { + if (fuel.hashCode() < other.hashCode()) { + handleFuelCollision(fuel, other); + } + } + } + } + } + } + } + } + + protected ArrayList fuels = new ArrayList<>(); + protected boolean running = false; + protected boolean simulateAirResistance = false; + protected Supplier robotPoseSupplier = null; + protected Supplier robotFieldSpeedsSupplier = null; + protected double robotWidth; // size along the robot's y axis + protected double robotLength; // size along the robot's x axis + protected double bumperHeight; + protected ArrayList intakes = new ArrayList<>(); + protected int subticks = 5; + + /** + * Creates a new instance of FuelSim + * @param tableKey NetworkTable to log fuel positions to as an array of {@link Translation3d} structs. + */ + public FuelSim(String tableKey) { + // Initialize grid + for (int i = 0; i < GRID_COLS; i++) { + for (int j = 0; j < GRID_ROWS; j++) { + grid[i][j] = new ArrayList(); + } + } + + fuelPublisher = NetworkTableInstance.getDefault() + .getStructArrayTopic(tableKey + "/Fuels", Translation3d.struct) + .publish(); + } + + /** + * Creates a new instance of FuelSim with log path "/Fuel Simulation" + */ + public FuelSim() { + this("/Fuel Simulation"); + } + + /** + * Clears the field of fuel + */ + public void clearFuel() { + fuels.clear(); + } + + /** + * Spawns fuel in the neutral zone and depots + */ + public void spawnStartingFuel() { + // Center fuel + Translation3d center = new Translation3d(FIELD_LENGTH / 2, FIELD_WIDTH / 2, FUEL_RADIUS); + for (int i = 0; i < 15; i++) { + for (int j = 0; j < 6; j++) { + fuels.add(new Fuel(center.plus(new Translation3d(0.076 + 0.152 * j, 0.0254 + 0.076 + 0.152 * i, 0)))); + fuels.add(new Fuel(center.plus(new Translation3d(-0.076 - 0.152 * j, 0.0254 + 0.076 + 0.152 * i, 0)))); + fuels.add(new Fuel(center.plus(new Translation3d(0.076 + 0.152 * j, -0.0254 - 0.076 - 0.152 * i, 0)))); + fuels.add(new Fuel(center.plus(new Translation3d(-0.076 - 0.152 * j, -0.0254 - 0.076 - 0.152 * i, 0)))); + } + } + + // Depots + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 4; j++) { + fuels.add(new Fuel(new Translation3d(0.076 + 0.152 * j, 5.95 + 0.076 + 0.152 * i, FUEL_RADIUS))); + fuels.add(new Fuel(new Translation3d(0.076 + 0.152 * j, 5.95 - 0.076 - 0.152 * i, FUEL_RADIUS))); + fuels.add(new Fuel( + new Translation3d(FIELD_LENGTH - 0.076 - 0.152 * j, 2.09 + 0.076 + 0.152 * i, FUEL_RADIUS))); + fuels.add(new Fuel( + new Translation3d(FIELD_LENGTH - 0.076 - 0.152 * j, 2.09 - 0.076 - 0.152 * i, FUEL_RADIUS))); + } + } + + // DEBUG: Log XZ lines + // Translation3d[][] lines = new Translation3d[FIELD_XZ_LINE_STARTS.length][2]; + // for (int i = 0; i < FIELD_XZ_LINE_STARTS.length; i++) { + // lines[i][0] = FIELD_XZ_LINE_STARTS[i]; + // lines[i][1] = FIELD_XZ_LINE_ENDS[i]; + // } + + // Logger.recordOutput("Fuel Simulation/Lines (debug)", lines); + } + + protected StructArrayPublisher fuelPublisher; + + /** + * Adds array of `Translation3d`'s to NetworkTables at tableKey + "/Fuels" + */ + public void logFuels() { + fuelPublisher.set(fuels.stream().map((fuel) -> fuel.pos).toArray(Translation3d[]::new)); + } + + /** + * Start the simulation. `updateSim` must still be called every loop + */ + public void start() { + running = true; + } + + /** + * Pause the simulation. + */ + public void stop() { + running = false; + } + + /** Enables accounting for drag force in physics step **/ + public void enableAirResistance() { + simulateAirResistance = true; + } + + /** + * Sets the number of physics iterations per loop (0.02s) + * @param subticks + */ + public void setSubticks(int subticks) { + this.subticks = subticks; + } + + /** + * Registers a robot with the fuel simulator + * @param width from left to right (y-axis) + * @param length from front to back (x-axis) + * @param bumperHeight + * @param poseSupplier + * @param fieldSpeedsSupplier field-relative `ChassisSpeeds` supplier + */ + public void registerRobot( + double width, + double length, + double bumperHeight, + Supplier poseSupplier, + Supplier fieldSpeedsSupplier) { + this.robotPoseSupplier = poseSupplier; + this.robotFieldSpeedsSupplier = fieldSpeedsSupplier; + this.robotWidth = width; + this.robotLength = length; + this.bumperHeight = bumperHeight; + } + + /** + * Registers a robot with the fuel simulator + * @param width from left to right (y-axis) + * @param length from front to back (x-axis) + * @param bumperHeight from the ground + * @param poseSupplier + * @param fieldSpeedsSupplier field-relative `ChassisSpeeds` supplier + */ + public void registerRobot( + Distance width, + Distance length, + Distance bumperHeight, + Supplier poseSupplier, + Supplier fieldSpeedsSupplier) { + this.robotPoseSupplier = poseSupplier; + this.robotFieldSpeedsSupplier = fieldSpeedsSupplier; + this.robotWidth = width.in(Meters); + this.robotLength = length.in(Meters); + this.bumperHeight = bumperHeight.in(Meters); + } + + /** + * To be called periodically + * Will do nothing if sim is not running + */ + public void updateSim() { + if (!running) return; + + stepSim(); + } + + /** + * Run the simulation forward 1 time step (0.02s) + */ + public void stepSim() { + for (int i = 0; i < subticks; i++) { + for (Fuel fuel : fuels) { + fuel.update(this.simulateAirResistance, this.subticks); + } + + handleFuelCollisions(fuels); + + if (robotPoseSupplier != null) { + handleRobotCollisions(fuels); + handleIntakes(fuels); + } + } + + logFuels(); + } + + /** + * Adds a fuel onto the field + * @param pos Position to spawn at + * @param vel Initial velocity vector + */ + public void spawnFuel(Translation3d pos, Translation3d vel) { + fuels.add(new Fuel(pos, vel)); + } + + /** + * Spawns a fuel onto the field with a specified launch velocity and angles, accounting for robot movement + * @param launchVelocity Initial launch velocity + * @param hoodAngle Hood angle where 0 is launching horizontally and 90 degrees is launching straight up + * @param turretYaw Robot-relative turret yaw + * @param launchHeight Height of the fuel to launch at. Make sure this is higher than your robot's bumper height, or else it will collide with your robot immediately. + * @throws IllegalStateException if robot is not registered + */ + public void launchFuel(LinearVelocity launchVelocity, Angle hoodAngle, Angle turretYaw, Distance launchHeight) { + if (robotPoseSupplier == null || robotFieldSpeedsSupplier == null) { + throw new IllegalStateException("Robot must be registered before launching fuel."); + } + + Pose3d launchPose = new Pose3d(this.robotPoseSupplier.get()) + .plus(new Transform3d(new Translation3d(Meters.zero(), Meters.zero(), launchHeight), Rotation3d.kZero)); + ChassisSpeeds fieldSpeeds = this.robotFieldSpeedsSupplier.get(); + + double horizontalVel = Math.cos(hoodAngle.in(Radians)) * launchVelocity.in(MetersPerSecond); + double verticalVel = Math.sin(hoodAngle.in(Radians)) * launchVelocity.in(MetersPerSecond); + double xVel = horizontalVel + * Math.cos( + turretYaw.plus(launchPose.getRotation().getMeasureZ()).in(Radians)); + double yVel = horizontalVel + * Math.sin( + turretYaw.plus(launchPose.getRotation().getMeasureZ()).in(Radians)); + + xVel += fieldSpeeds.vxMetersPerSecond; + yVel += fieldSpeeds.vyMetersPerSecond; + + spawnFuel(launchPose.getTranslation(), new Translation3d(xVel, yVel, verticalVel)); + } + + protected void handleRobotCollision(Fuel fuel, Pose2d robot, Translation2d robotVel) { + Translation2d relativePos = new Pose2d(fuel.pos.toTranslation2d(), Rotation2d.kZero) + .relativeTo(robot) + .getTranslation(); + + if (fuel.pos.getZ() > bumperHeight) return; // above bumpers + double distanceToBottom = -FUEL_RADIUS - robotLength / 2 - relativePos.getX(); + double distanceToTop = -FUEL_RADIUS - robotLength / 2 + relativePos.getX(); + double distanceToRight = -FUEL_RADIUS - robotWidth / 2 - relativePos.getY(); + double distanceToLeft = -FUEL_RADIUS - robotWidth / 2 + relativePos.getY(); + + // not inside robot + if (distanceToBottom > 0 || distanceToTop > 0 || distanceToRight > 0 || distanceToLeft > 0) return; + + Translation2d posOffset; + // find minimum distance to side and send corresponding collision response + if ((distanceToBottom >= distanceToTop + && distanceToBottom >= distanceToRight + && distanceToBottom >= distanceToLeft)) { + posOffset = new Translation2d(distanceToBottom, 0); + } else if ((distanceToTop >= distanceToBottom + && distanceToTop >= distanceToRight + && distanceToTop >= distanceToLeft)) { + posOffset = new Translation2d(-distanceToTop, 0); + } else if ((distanceToRight >= distanceToBottom + && distanceToRight >= distanceToTop + && distanceToRight >= distanceToLeft)) { + posOffset = new Translation2d(0, distanceToRight); + } else { + posOffset = new Translation2d(0, -distanceToLeft); + } + + posOffset = posOffset.rotateBy(robot.getRotation()); + fuel.pos = fuel.pos.plus(new Translation3d(posOffset)); + Translation2d normal = posOffset.div(posOffset.getNorm()); + if (fuel.vel.toTranslation2d().dot(normal) < 0) + fuel.addImpulse( + new Translation3d(normal.times(-fuel.vel.toTranslation2d().dot(normal) * (1 + ROBOT_COR)))); + if (robotVel.dot(normal) > 0) fuel.addImpulse(new Translation3d(normal.times(robotVel.dot(normal)))); + } + + protected void handleRobotCollisions(ArrayList fuels) { + Pose2d robot = robotPoseSupplier.get(); + ChassisSpeeds speeds = robotFieldSpeedsSupplier.get(); + Translation2d robotVel = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); + + for (Fuel fuel : fuels) { + handleRobotCollision(fuel, robot, robotVel); + } + } + + protected void handleIntakes(ArrayList fuels) { + Pose2d robot = robotPoseSupplier.get(); + for (SimIntake intake : intakes) { + for (int i = 0; i < fuels.size(); i++) { + if (intake.shouldIntake(fuels.get(i), robot)) { + fuels.remove(i); + i--; + } + } + } + } + + protected static void fuelCollideRectangle(Fuel fuel, Translation3d start, Translation3d end) { + if (fuel.pos.getZ() > end.getZ() + FUEL_RADIUS || fuel.pos.getZ() < start.getZ() - FUEL_RADIUS) + return; // above rectangle + double distanceToLeft = start.getX() - FUEL_RADIUS - fuel.pos.getX(); + double distanceToRight = fuel.pos.getX() - end.getX() - FUEL_RADIUS; + double distanceToTop = fuel.pos.getY() - end.getY() - FUEL_RADIUS; + double distanceToBottom = start.getY() - FUEL_RADIUS - fuel.pos.getY(); + + // not inside hub + if (distanceToLeft > 0 || distanceToRight > 0 || distanceToTop > 0 || distanceToBottom > 0) return; + + Translation2d collision; + // find minimum distance to side and send corresponding collision response + if (fuel.pos.getX() < start.getX() + || (distanceToLeft >= distanceToRight + && distanceToLeft >= distanceToTop + && distanceToLeft >= distanceToBottom)) { + collision = new Translation2d(distanceToLeft, 0); + } else if (fuel.pos.getX() >= end.getX() + || (distanceToRight >= distanceToLeft + && distanceToRight >= distanceToTop + && distanceToRight >= distanceToBottom)) { + collision = new Translation2d(-distanceToRight, 0); + } else if (fuel.pos.getY() > end.getY() + || (distanceToTop >= distanceToLeft + && distanceToTop >= distanceToRight + && distanceToTop >= distanceToBottom)) { + collision = new Translation2d(0, -distanceToTop); + } else { + collision = new Translation2d(0, distanceToBottom); + } + + if (collision.getX() != 0) { + fuel.pos = fuel.pos.plus(new Translation3d(collision)); + fuel.vel = fuel.vel.plus(new Translation3d(-(1 + FIELD_COR) * fuel.vel.getX(), 0, 0)); + } else if (collision.getY() != 0) { + fuel.pos = fuel.pos.plus(new Translation3d(collision)); + fuel.vel = fuel.vel.plus(new Translation3d(0, -(1 + FIELD_COR) * fuel.vel.getY(), 0)); + } + } + + /** + * Registers an intake with the fuel simulator. This intake will remove fuel from the field based on the `ableToIntake` parameter. + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param ableToIntake Should a return a boolean whether the intake is active + * @param intakeCallback Function to call when a fuel is intaked + */ + public void registerIntake( + double xMin, double xMax, double yMin, double yMax, BooleanSupplier ableToIntake, Runnable intakeCallback) { + intakes.add(new SimIntake(xMin, xMax, yMin, yMax, ableToIntake, intakeCallback)); + } + + /** + * Registers an intake with the fuel simulator. This intake will remove fuel from the field based on the `ableToIntake` parameter. + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param ableToIntake Should a return a boolean whether the intake is active + */ + public void registerIntake(double xMin, double xMax, double yMin, double yMax, BooleanSupplier ableToIntake) { + registerIntake(xMin, xMax, yMin, yMax, ableToIntake, () -> {}); + } + + /** + * Registers an intake with the fuel simulator. This intake will always remove fuel from the field. + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param intakeCallback Function to call when a fuel is intaked + */ + public void registerIntake(double xMin, double xMax, double yMin, double yMax, Runnable intakeCallback) { + registerIntake(xMin, xMax, yMin, yMax, () -> true, intakeCallback); + } + + /** + * Registers an intake with the fuel simulator. This intake will always remove fuel from the field. + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + */ + public void registerIntake(double xMin, double xMax, double yMin, double yMax) { + registerIntake(xMin, xMax, yMin, yMax, () -> true, () -> {}); + } + + /** + * Registers an intake with the fuel simulator. This intake will remove fuel from the field based on the `ableToIntake` parameter. + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param ableToIntake Should a return a boolean whether the intake is active + * @param intakeCallback Function to call when a fuel is intaked + */ + public void registerIntake( + Distance xMin, Distance xMax, Distance yMin, Distance yMax, BooleanSupplier ableToIntake, Runnable intakeCallback) { + registerIntake(xMin.in(Meters), xMax.in(Meters), yMin.in(Meters), yMax.in(Meters), ableToIntake, intakeCallback); + } + + /** + * Registers an intake with the fuel simulator. This intake will remove fuel from the field based on the `ableToIntake` parameter. + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param ableToIntake Should a return a boolean whether the intake is active + */ + public void registerIntake(Distance xMin, Distance xMax, Distance yMin, Distance yMax, BooleanSupplier ableToIntake) { + registerIntake(xMin.in(Meters), xMax.in(Meters), yMin.in(Meters), yMax.in(Meters), ableToIntake); + } + + /** + * Registers an intake with the fuel simulator. This intake will always remove fuel from the field. + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param intakeCallback Function to call when a fuel is intaked + */ + public void registerIntake(Distance xMin, Distance xMax, Distance yMin, Distance yMax, Runnable intakeCallback) { + registerIntake(xMin.in(Meters), xMax.in(Meters), yMin.in(Meters), yMax.in(Meters), intakeCallback); + } + + /** + * Registers an intake with the fuel simulator. This intake will always remove fuel from the field. + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + */ + public void registerIntake(Distance xMin, Distance xMax, Distance yMin, Distance yMax) { + registerIntake(xMin.in(Meters), xMax.in(Meters), yMin.in(Meters), yMax.in(Meters)); + } + + public static class Hub { + public static final Hub BLUE_HUB = + new Hub(new Translation2d(4.61, FIELD_WIDTH / 2), new Translation3d(5.3, FIELD_WIDTH / 2, 0.89), 1); + public static final Hub RED_HUB = new Hub( + new Translation2d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2), + new Translation3d(FIELD_LENGTH - 5.3, FIELD_WIDTH / 2, 0.89), + -1); + + protected static final double ENTRY_HEIGHT = 1.83; + protected static final double ENTRY_RADIUS = 0.56; + + protected static final double SIDE = 1.2; + + protected static final double NET_HEIGHT_MAX = 3.057; + protected static final double NET_HEIGHT_MIN = 1.5; + protected static final double NET_OFFSET = SIDE / 2 + 0.261; + protected static final double NET_WIDTH = 1.484; + + protected final Translation2d center; + protected final Translation3d exit; + protected final int exitVelXMult; + + protected int score = 0; + + protected Hub(Translation2d center, Translation3d exit, int exitVelXMult) { + this.center = center; + this.exit = exit; + this.exitVelXMult = exitVelXMult; + } + + protected void handleHubInteraction(Fuel fuel, int subticks) { + if (didFuelScore(fuel, subticks)) { + fuel.pos = exit; + fuel.vel = getDispersalVelocity(); + score++; + } + } + + protected boolean didFuelScore(Fuel fuel, int subticks) { + return fuel.pos.toTranslation2d().getDistance(center) <= ENTRY_RADIUS + && fuel.pos.getZ() <= ENTRY_HEIGHT + && fuel.pos.minus(fuel.vel.times(PERIOD / subticks)).getZ() > ENTRY_HEIGHT; + } + + protected Translation3d getDispersalVelocity() { + return new Translation3d(exitVelXMult * (Math.random() + 0.1) * 1.5, Math.random() * 2 - 1, 0); + } + + /** + * Reset this hub's score to 0 + */ + public void resetScore() { + score = 0; + } + + /** + * Get the current count of fuel scored in this hub + * @return + */ + public int getScore() { + return score; + } + + protected void fuelCollideSide(Fuel fuel) { + fuelCollideRectangle( + fuel, + new Translation3d(center.getX() - SIDE / 2, center.getY() - SIDE / 2, 0), + new Translation3d(center.getX() + SIDE / 2, center.getY() + SIDE / 2, ENTRY_HEIGHT - 0.1)); + } + + protected double fuelHitNet(Fuel fuel) { + if (fuel.pos.getZ() > NET_HEIGHT_MAX || fuel.pos.getZ() < NET_HEIGHT_MIN) return 0; + if (fuel.pos.getY() > center.getY() + NET_WIDTH / 2 || fuel.pos.getY() < center.getY() - NET_WIDTH / 2) + return 0; + if (fuel.pos.getX() > center.getX() + NET_OFFSET * exitVelXMult) { + return Math.max(0, center.getX() + NET_OFFSET * exitVelXMult - (fuel.pos.getX() - FUEL_RADIUS)); + } else { + return Math.min(0, center.getX() + NET_OFFSET * exitVelXMult - (fuel.pos.getX() + FUEL_RADIUS)); + } + } + } + + protected class SimIntake { + double xMin, xMax, yMin, yMax; + BooleanSupplier ableToIntake; + Runnable callback; + + protected SimIntake( + double xMin, + double xMax, + double yMin, + double yMax, + BooleanSupplier ableToIntake, + Runnable intakeCallback) { + this.xMin = xMin; + this.xMax = xMax; + this.yMin = yMin; + this.yMax = yMax; + this.ableToIntake = ableToIntake; + this.callback = intakeCallback; + } + + protected boolean shouldIntake(Fuel fuel, Pose2d robotPose) { + if (!ableToIntake.getAsBoolean() || fuel.pos.getZ() > bumperHeight) return false; + + Translation2d fuelRelativePos = new Pose2d(fuel.pos.toTranslation2d(), Rotation2d.kZero) + .relativeTo(robotPose) + .getTranslation(); + + boolean result = fuelRelativePos.getX() >= xMin + && fuelRelativePos.getX() <= xMax + && fuelRelativePos.getY() >= yMin + && fuelRelativePos.getY() <= yMax; + if (result) { + callback.run(); + } + return result; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index cc218dc..a6a8785 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -2,6 +2,8 @@ import static frc.robot.subsystems.shooter.ShooterConstants.*; +import java.util.function.DoubleSupplier; + import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.controls.NeutralOut; @@ -15,7 +17,9 @@ import edu.wpi.first.wpilibj.simulation.RoboRioSim; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; import frc.robot.Robot; +import frc.robot.subsystems.shooter.ShooterConstants.Motors; public class Shooter extends SubsystemBase { private static Shooter shooterLeftInstance; @@ -102,6 +106,10 @@ public void periodic() { io.process(); } + public double getTopSpeed() { + return lastReadSpeedTop; + } + @Override public void simulationPeriodic() { topMotor.getSimState().setSupplyVoltage(12); @@ -152,9 +160,24 @@ public Command shoot(double topSpeed, double bottomSpeed) { }).withName("Shooting"); } + public Command shoot(DoubleSupplier topSpeed, DoubleSupplier bottomSpeed) { + var topReq = new VelocityVoltage(0.0); + var bottomReq = new VelocityVoltage(0.0); + return runOnce(() -> { + setTopRequest(topReq); + setBottomRequest(bottomReq); + }).andThen( + runOnce(() -> { + topReq.withVelocity(topSpeed.getAsDouble()); + bottomReq.withVelocity(bottomSpeed.getAsDouble()); + }) + ).withName("Shooting"); + } + + public Command shoot() { - return defer(() -> shoot(shotCalculator.getInterceptSolution().launchSpeed() - TOPSPIN_FACTOR, - -shotCalculator.getInterceptSolution().launchSpeed() - TOPSPIN_FACTOR)); + return shoot(() -> shotCalculator.getInterceptSolution().launchSpeed() / Constants.TAU / 0.0508 - TOPSPIN_FACTOR, + () -> -shotCalculator.getInterceptSolution().launchSpeed() / Constants.TAU / 0.0508 - TOPSPIN_FACTOR); } // @Override diff --git a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java index bde87a6..e0b64a0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java @@ -17,7 +17,6 @@ // stores current target and actively computes effective target public class ShotCalculator extends SubsystemBase { private static ShotCalculator calcInstance; - public static ShotCalculator getInstance() { if (calcInstance == null) { calcInstance = new ShotCalculator(); @@ -25,43 +24,39 @@ public static ShotCalculator getInstance() { return calcInstance; } - private final Drive drivetrain; + private final Drive drive; @AutoLogOutput private Pose3d currentEffectiveTargetPose = Pose3d.kZero; - private double currentEffectiveYaw; @AutoLogOutput private InterceptSolution currentInterceptSolution; - private Pose3d targetLocation = new Pose3d(); - private double targetDistance = 0.0; - private double shooterAngle = 75 * Constants.TAU / 360; private ShotCalculator() { - this.drivetrain = Drive.getInstance(); + this.drive = Drive.getInstance(); AutoLogOutputManager.addObject(this); } @Override public void periodic() { - Pose2d drivetrainPose = drivetrain.getPose(); + Pose2d drivePose = drive.getPose(); - targetDistance = drivetrainPose.getTranslation().getDistance(targetLocation.toPose2d().getTranslation()); + targetDistance = drivePose.getTranslation().getDistance(targetLocation.toPose2d().getTranslation()); - Pose3d shooterPose = new Pose3d(drivetrainPose).plus(ShooterConstants.OFFSET); + Pose3d shooterPose = new Pose3d(drivePose).plus(ShooterConstants.OFFSET); - ChassisSpeeds drivetrainSpeeds = drivetrain.getFieldSpeeds(); - ChassisAccels drivetrainAccelerations = ChassisAccels.estimate(drivetrainSpeeds, drivetrain.getPrevFieldSpeeds(), Constants.DT); + ChassisSpeeds driveSpeeds = drive.getFieldSpeeds(); + ChassisAccels driveAccelerations = ChassisAccels.estimate(driveSpeeds, drive.getPrevFieldSpeeds(), Constants.DT); currentInterceptSolution = ShootOnTheFlyCalculator.solveShootOnTheFly( shooterPose, targetLocation, - drivetrainSpeeds, - new ChassisAccels(), + driveSpeeds, + driveAccelerations, -shooterAngle, 5, 0.01); From 40f2a4fe4bec42c1f6582769290a06859b69c101 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Tue, 17 Feb 2026 18:53:27 -0800 Subject: [PATCH 089/118] fixed the shooter implementation Now for increasing the accuracy via adding 6 more krakens --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/Robot.java | 7 +++++++ .../java/frc/robot/subsystems/shooter/Shooter.java | 2 +- 3 files changed, 13 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 62cb309..cd15c5d 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Robot2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 155; - public static final String GIT_SHA = "4d8860542751f77cebfbe5320421f48745e13b2d"; - public static final String GIT_DATE = "2026-02-17 02:06:02 EST"; + public static final int GIT_REVISION = 156; + public static final String GIT_SHA = "a0460dbdb6be3d01e1532a98022679c1703c53a1"; + public static final String GIT_DATE = "2026-02-17 21:42:31 EST"; public static final String GIT_BRANCH = "sotm"; - public static final String BUILD_DATE = "2026-02-17 21:38:32 EST"; - public static final long BUILD_UNIX_TIME = 1771382312237L; + public static final String BUILD_DATE = "2026-02-17 21:45:08 EST"; + public static final long BUILD_UNIX_TIME = 1771382708262L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2775da2..c2a4e1e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -232,14 +232,21 @@ public void simulationInit() { Inches.of(-13), Inches.of(13), Inches.of(-21.5), Inches.of(-17.5)); fuelSim.start(); fuelSim.enableAirResistance(); + timer.start();; } + Timer timer = new Timer(); /** This function is called periodically whilst in simulation. */ @Override public void simulationPeriodic() { fuelSim.updateSim(); Logger.recordOutput("Blue Score", FuelSim.Hub.BLUE_HUB.getScore()); Logger.recordOutput("Red Score", FuelSim.Hub.RED_HUB.getScore()); + + if (timer.hasElapsed(5)) { + fuelSim.clearFuel(); + timer.restart(); + } } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index a6a8785..1f1a61e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -167,7 +167,7 @@ public Command shoot(DoubleSupplier topSpeed, DoubleSupplier bottomSpeed) { setTopRequest(topReq); setBottomRequest(bottomReq); }).andThen( - runOnce(() -> { + run(() -> { topReq.withVelocity(topSpeed.getAsDouble()); bottomReq.withVelocity(bottomSpeed.getAsDouble()); }) From 52167db8f4af9e19a176121a3ee2837f1ce658f1 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Sun, 22 Feb 2026 22:30:28 -0800 Subject: [PATCH 090/118] constant changes --- src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/ControlsMapping.java | 195 +++++++----- src/main/java/frc/robot/Robot.java | 7 +- .../subsystems/drive/ctre/CtreDrive.java | 2 +- .../drive/ctre/CtreDriveConstants2.java | 285 ++++++++++++++++++ .../frc/robot/subsystems/indexer/Indexer.java | 16 +- .../subsystems/indexer/IndexerConstants.java | 23 +- .../subsystems/intake/IntakeConstants.java | 4 +- .../frc/robot/subsystems/roller/Roller.java | 5 +- .../subsystems/roller/RollerConstants.java | 12 +- .../frc/robot/subsystems/shooter/Shooter.java | 15 +- .../subsystems/shooter/ShooterConstants.java | 15 +- .../vision/VisionDeviceManager.java | 16 +- 13 files changed, 489 insertions(+), 116 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants2.java diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index cd15c5d..d2272ac 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Robot2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 156; - public static final String GIT_SHA = "a0460dbdb6be3d01e1532a98022679c1703c53a1"; - public static final String GIT_DATE = "2026-02-17 21:42:31 EST"; + public static final int GIT_REVISION = 157; + public static final String GIT_SHA = "40f2a4fe4bec42c1f6582769290a06859b69c101"; + public static final String GIT_DATE = "2026-02-17 21:53:27 EST"; public static final String GIT_BRANCH = "sotm"; - public static final String BUILD_DATE = "2026-02-17 21:45:08 EST"; - public static final long BUILD_UNIX_TIME = 1771382708262L; + public static final String BUILD_DATE = "2026-02-21 21:32:44 EST"; + public static final long BUILD_UNIX_TIME = 1771727564987L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index ac2c7d6..0d15a78 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -18,93 +18,150 @@ import frc.robot.subsystems.drive.ctre.CtreDrive.SysIdRoutineType; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.roller.Roller; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShotCalculator; import frc.robot.subsystems.vision.VisionDeviceManager; import edu.wpi.first.wpilibj2.command.Commands; public class ControlsMapping { - + public static void mapTeleopCommand() { - Drive.getInstance().setDefaultCommand((Drive.getInstance().openLoopControl())); - Intake.getInstance().setDefaultCommand(Intake.getInstance().intake()); - - controller.back().and(controller.a()).onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); - controller.back().and(controller.b()).onTrue(VisionDeviceManager.getInstance().bootUp()); - - controller.leftTrigger().debounce(0.1).whileTrue(Intake.getInstance().outtake()); - controller.b().whileTrue(Intake.getInstance().outtake()); - controller.x().onTrue(Climb.getInstance().hangCommand()); - - controller.rightTrigger().debounce(0.1 - // ).onTrue( - // Commands.parallel( - // Shooter.getLeftInstance().shoot(50, -50), - // Shooter.getRightInstance().shoot(50, -50)) - // ).onFalse( - // Commands.parallel( - // Shooter.getLeftInstance().stop(), - // Shooter.getRightInstance().stop()) - ).whileTrue( - Commands.parallel( - Indexer.getLeftInstance().activateIndexer(), - Indexer.getRightInstance().activateIndexer()) - ).whileTrue( - Commands.repeatingSequence( - Commands.runOnce(() -> Robot.fuelSim.launchFuel( - MetersPerSecond.of( - Shooter.getLeftInstance().getTopSpeed() * Constants.TAU * 0.0508), - Degrees.of(75), - Degrees.of(0), - Inches.of(19) - )).andThen(Commands.waitSeconds(0.1)) - ) - ); - - controller.leftBumper().whileTrue(Drive.getInstance().headingLockToHub() - .alongWith(Shooter.getLeftInstance().shoot(), Shooter.getRightInstance().shoot())); + // Drive.getInstance().setDefaultCommand((Drive.getInstance().openLoopControl())); + // Intake.getInstance().setDefaultCommand(Intake.getInstance().intake()); + + // controller.back().and(controller.a()).onTrue(Drive.getInstance().resetPoseCommand(new + // Pose2d())); + // controller.back().and(controller.b()).onTrue(VisionDeviceManager.getInstance().bootUp()); + + // controller.leftTrigger().debounce(0.1).whileTrue(Intake.getInstance().outtake()); + // controller.b().whileTrue(Intake.getInstance().outtake()); + // controller.x().onTrue(Climb.getInstance().hangCommand()); + + // controller.rightTrigger().debounce(0.1 + // // ).onTrue( + // // Commands.parallel( + // // Shooter.getLeftInstance().shoot(50, -50), + // // Shooter.getRightInstance().shoot(50, -50)) + // // ).onFalse( + // // Commands.parallel( + // // Shooter.getLeftInstance().stop(), + // // Shooter.getRightInstance().stop()) + // ).whileTrue( + // Commands.parallel( + // Indexer.getLeftInstance().activateIndexer(), + // Indexer.getRightInstance().activateIndexer()) + // ).whileTrue( + // Commands.repeatingSequence( + // Commands.runOnce(() -> Robot.fuelSim.launchFuel( + // MetersPerSecond.of( + // Shooter.getLeftInstance().getTopSpeed() * Constants.TAU * 0.0508), + // Degrees.of(75), + // Degrees.of(0), + // Inches.of(19) + // )).andThen(Commands.waitSeconds(0.1)) + // ) + // ); + + controller.x().whileTrue( + Commands.parallel( + // Shooter.getRightInstance().shoot(50, -10), + // Indexer.getRightInstance().activateIndexer() + // , + Shooter.getLeftInstance().shoot(50, -10), + Indexer.getLeftInstance().activateIndexer())) + .onFalse( + Commands.parallel( + // Shooter.getRightInstance().stop(), + // Indexer.getRightInstance().deactivateIndexer() + // , + Shooter.getLeftInstance().stop(), + Indexer.getLeftInstance().deactivateIndexer() + + )); + + controller.y().whileTrue( + Commands.parallel( + // Shooter.getRightInstance().shoot(-10, 50), + // Indexer.getRightInstance().activateIndexer() + // , + Shooter.getLeftInstance().shoot(-10, 50), + Indexer.getLeftInstance().activateIndexer())) + .onFalse( + Commands.parallel( + // Shooter.getRightInstance().stop(), + // Indexer.getRightInstance().deactivateIndexer() + // , + Shooter.getLeftInstance().stop(), + Indexer.getLeftInstance().deactivateIndexer() + + )); + + controller.a().whileTrue( + Commands.parallel( + Shooter.getRightInstance().shoot(40, 40), + Indexer.getRightInstance().activateIndexer(), + Shooter.getLeftInstance().shoot(40, 40), + Indexer.getLeftInstance().activateIndexer(), + Roller.getInstance().roll())) + .onFalse( + Commands.parallel( + Shooter.getRightInstance().stop(), + Indexer.getRightInstance().deactivateIndexer() + , + Shooter.getLeftInstance().stop(), + Indexer.getLeftInstance().deactivateIndexer(), + Roller.getInstance().stop() + )); + + controller.b().whileTrue( + Roller.getInstance().roll()).onFalse(Roller.getInstance().stop()); + + // controller.leftBumper().whileTrue(Drive.getInstance().headingLockToHub() + // .alongWith(Shooter.getLeftInstance().shoot(), + // Shooter.getRightInstance().shoot())); } public static void mapSysId() { // set up sysID routine type controller.a().onTrue(Commands.runOnce( - () -> Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.TRANSLATION))); + () -> Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.TRANSLATION))); controller.b().onTrue(Commands.runOnce( - () -> Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.ROTATION))); + () -> Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.ROTATION))); controller.back().onTrue(Commands.runOnce( - () -> Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER))); + () -> Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER))); // map the sysid routine movement directions controller.leftBumper().and(controller.x()).whileTrue( - Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kForward) - .finallyDo(( - boolean interrupted) -> { - if (interrupted) { - Drive.getInstance().setSwerveRequest(new SwerveRequest.Idle()); - } - })); + Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kForward) + .finallyDo(( + boolean interrupted) -> { + if (interrupted) { + Drive.getInstance().setSwerveRequest(new SwerveRequest.Idle()); + } + })); controller.leftBumper().and(controller.x()).whileTrue( - Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kReverse) - .finallyDo(( - boolean interrupted) -> { - if (interrupted) { - Drive.getInstance().setSwerveRequest(new SwerveRequest.Idle()); - } - })); + Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kReverse) + .finallyDo(( + boolean interrupted) -> { + if (interrupted) { + Drive.getInstance().setSwerveRequest(new SwerveRequest.Idle()); + } + })); controller.rightBumper().and(controller.x()).whileTrue( - Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kForward) - .finallyDo(( - boolean interrupted) -> { - if (interrupted) { - Drive.getInstance().setSwerveRequest(new SwerveRequest.Idle()); - } - })); + Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kForward) + .finallyDo(( + boolean interrupted) -> { + if (interrupted) { + Drive.getInstance().setSwerveRequest(new SwerveRequest.Idle()); + } + })); controller.rightBumper().and(controller.y()).whileTrue( - Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kReverse) - .finallyDo(( - boolean interrupted) -> { - if (interrupted) { - Drive.getInstance().setSwerveRequest(new SwerveRequest.Idle()); - } - })); + Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kReverse) + .finallyDo(( + boolean interrupted) -> { + if (interrupted) { + Drive.getInstance().setSwerveRequest(new SwerveRequest.Idle()); + } + })); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c2a4e1e..ad6be19 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -121,8 +121,8 @@ public Robot() { Shooter.getRightInstance(); Indexer.getLeftInstance(); Indexer.getRightInstance(); - Intake.getInstance(); - Climb.getInstance(); + // Intake.getInstance(); + // Climb.getInstance(); Roller.getInstance(); TelemetryManager.getInstance(); @@ -216,11 +216,12 @@ public void testInit() { public void testPeriodic() { } - public static FuelSim fuelSim = new FuelSim(); + public static FuelSim fuelSim; /** This function is called once when the robot is first started up. */ @Override public void simulationInit() { + fuelSim = new FuelSim(); // fuelSim.spawnStartingFuel(); fuelSim.registerRobot( Inches.of(27), diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java index fc2392f..e9d9681 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java @@ -23,7 +23,7 @@ import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.subsystems.drive.ctre.CtreDriveConstants.TunerSwerveDrivetrain; +import frc.robot.subsystems.drive.ctre.CtreDriveConstants2.TunerSwerveDrivetrain; /** * Class that extends the Phoenix 6 SwerveDrivetrain class and implements diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants2.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants2.java new file mode 100644 index 0000000..45cb196 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants2.java @@ -0,0 +1,285 @@ +package frc.robot.subsystems.drive.ctre; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.*; +import com.ctre.phoenix6.hardware.*; +import com.ctre.phoenix6.signals.*; +import com.ctre.phoenix6.swerve.*; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.units.measure.*; + + +// Generated by the 2026 Tuner X Swerve Project Generator +// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html +public class CtreDriveConstants2 { + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs steerGains = new Slot0Configs() + .withKP(100).withKI(0).withKD(0.5) + .withKS(0.1).withKV(2.66).withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs driveGains = new Slot0Configs() + .withKP(0.1).withKI(0).withKD(0) + .withKS(0).withKV(0.124); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final Current kSlipCurrent = Amps.of(120); + + // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true) + ); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("CV", "./logs/example.hoot"); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.04); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = 3.5714285714285716; + + private static final double kDriveGearRatio = 6.122448979591837; + private static final double kSteerGearRatio = 21.428571428571427; + private static final Distance kWheelRadius = Inches.of(2); + + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final int kPigeonId = 60; + + // These are only used for simulation + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); + // Simulated voltage necessary to overcome friction + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + + // Front Left + private static final int kFrontLeftDriveMotorId = 8; + private static final int kFrontLeftSteerMotorId = 10; + private static final int kFrontLeftEncoderId = 7; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.2236328125); + private static final boolean kFrontLeftSteerMotorInverted = true; + private static final boolean kFrontLeftEncoderInverted = false; + + private static final Distance kFrontLeftXPos = Inches.of(10.875); + private static final Distance kFrontLeftYPos = Inches.of(10.875); + + // Front Right + private static final int kFrontRightDriveMotorId = 9; + private static final int kFrontRightSteerMotorId = 11; + private static final int kFrontRightEncoderId = 6; + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.431640625); + private static final boolean kFrontRightSteerMotorInverted = true; + private static final boolean kFrontRightEncoderInverted = false; + + private static final Distance kFrontRightXPos = Inches.of(10.875); + private static final Distance kFrontRightYPos = Inches.of(-10.875); + + // Back Left + private static final int kBackLeftDriveMotorId = 3; + private static final int kBackLeftSteerMotorId = 5; + private static final int kBackLeftEncoderId = 14; + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.210205078125); + private static final boolean kBackLeftSteerMotorInverted = true; + private static final boolean kBackLeftEncoderInverted = false; + + private static final Distance kBackLeftXPos = Inches.of(-10.875); + private static final Distance kBackLeftYPos = Inches.of(10.875); + + // Back Right + private static final int kBackRightDriveMotorId = 4; + private static final int kBackRightSteerMotorId = 2; + private static final int kBackRightEncoderId = 1; + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.10693359375); + private static final boolean kBackRightSteerMotorInverted = true; + private static final boolean kBackRightEncoderInverted = false; + + private static final Distance kBackRightXPos = Inches.of(-10.875); + private static final Distance kBackRightYPos = Inches.of(-10.875); + + + public static final SwerveModuleConstants FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, + kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted + ); + public static final SwerveModuleConstants FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, + kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted + ); + public static final SwerveModuleConstants BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, + kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted + ); + public static final SwerveModuleConstants BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, + kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted + ); + + /** + * Creates a CommandSwerveDrivetrain instance. + * This should only be called once in your robot program,. + */ + public static CtreDrive createDrivetrain() { + return new CtreDrive( + DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight + ); + } + + + /** + * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. + */ + public static class TunerSwerveDrivetrain extends SwerveDrivetrain { + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation + * in the form [x, y, theta]áµ€, with units in meters + * and radians + * @param visionStandardDeviation The standard deviation for vision calculation + * in the form [x, y, theta]áµ€, with units in meters + * and radians + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, + odometryStandardDeviation, visionStandardDeviation, modules + ); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 2e9d84f..44ca7ce 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -2,9 +2,12 @@ import static frc.robot.subsystems.indexer.IndexerConstants.*; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; @@ -54,7 +57,15 @@ private Indexer(boolean isLeft) { super(); setName("Indexer " + (isLeft ? "Left" : "Right")); motor = new TalonFX(isLeft ? L_MOTOR_ID : R_MOTOR_ID); - motor.getConfigurator().apply(getConfig()); + + var config = getConfig(); + + if (isLeft) { + config = config.clone().withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive)); + } + motor.getConfigurator().apply(config); // lc = new LaserCan(isLeft ? L_LASER_ID : R_LASER_ID); // lcTwo = new LaserCan(LASER_ID_2); @@ -90,7 +101,6 @@ private Indexer(boolean isLeft) { io = new IndexerIO(getName(), motor); // TelemetryManager.getInstance().addSendable(this); - setDefaultCommand(deactivateIndexer()); } @Override @@ -145,7 +155,7 @@ public Command activateIndexer() { /** turn motor down to zero */ public Command deactivateIndexer() { - return setSpeed(0); + return runOnce(() -> setRequest(new CoastOut())); } /** command to sense distance from LaserCAN; used to sense if bol */ diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java index 3b61421..7d74bcb 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java @@ -9,12 +9,12 @@ public class IndexerConstants { // TODO: Set IDs // TODO (Ethan): Set LaserCan_DefaultMeasurement - public static final int L_MOTOR_ID = 51; - public static final int L_LASER_ID = 50; - public static final int R_MOTOR_ID = 49; - public static final int R_LASER_ID = 48; + public static final int L_MOTOR_ID = 26; + // public static final int L_LASER_ID = 50; + public static final int R_MOTOR_ID = 21; + // public static final int R_LASER_ID = 48; // public static final int LASER_ID_2 = 0; - public static final double ROLLING_SPEED = 10; // rps + public static final double ROLLING_SPEED = 30; // rps public static final double MAXIMUM_LASER_DIST = 100; @@ -23,14 +23,15 @@ public static TalonFXConfiguration getConfig() { .withSlot0(new Slot0Configs() .withKV(0.0) .withKP(0.3) - .withKI(0.001) + .withKI(0.0) .withKD(0.0) - .withKA(0.1) - .withKS(0.1) - .withKV(0.1)) // placeholder values + .withKA(0.0) + .withKS(0.0) + .withKV(0.0)) // placeholder values .withCurrentLimits(new CurrentLimitsConfigs() - .withStatorCurrentLimit(40) - .withSupplyCurrentLimit(40)) + .withStatorCurrentLimit(80) + // .withSupplyCurrentLimit(120) + ) .withVoltage(new VoltageConfigs() .withPeakForwardVoltage(12.0) .withPeakReverseVoltage(-12.0)); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index f2cb2d8..51fd3a6 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -25,8 +25,8 @@ public class IntakeConstants { //etc public static enum Motors { //TODO: set motor ids; use separate file for ports? - WHEEL(53), - BAR(52); + WHEEL(31), + BAR(32); public final int id; private Motors(int id) { this.id = id; diff --git a/src/main/java/frc/robot/subsystems/roller/Roller.java b/src/main/java/frc/robot/subsystems/roller/Roller.java index 3fd0a47..50d5b07 100644 --- a/src/main/java/frc/robot/subsystems/roller/Roller.java +++ b/src/main/java/frc/robot/subsystems/roller/Roller.java @@ -2,6 +2,7 @@ import static frc.robot.subsystems.roller.RollerConstants.*; +import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.VelocityVoltage; @@ -46,7 +47,7 @@ private Roller() { LinearSystemId.createFlywheelSystem( DCMotor.getKrakenX44(1), 0.000189000861, - 2), + 1), DCMotor.getKrakenX44(1), 0.0); } @@ -91,7 +92,7 @@ public Command roll() { } public Command stop() { - return runOnce(() -> setRequest(new NeutralOut())) + return runOnce(() -> setRequest(new CoastOut())) .withName("Stop"); } diff --git a/src/main/java/frc/robot/subsystems/roller/RollerConstants.java b/src/main/java/frc/robot/subsystems/roller/RollerConstants.java index 75c594a..10bcbdf 100644 --- a/src/main/java/frc/robot/subsystems/roller/RollerConstants.java +++ b/src/main/java/frc/robot/subsystems/roller/RollerConstants.java @@ -1,12 +1,14 @@ package frc.robot.subsystems.roller; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.VoltageConfigs; +import com.ctre.phoenix6.signals.InvertedValue; public class RollerConstants { - public static final int MOTOR_ID = 54; + public static final int MOTOR_ID = 33; public static final double ROLL_SPEED = 20; public static TalonFXConfiguration getConfig() { @@ -17,10 +19,12 @@ public static TalonFXConfiguration getConfig() { .withKI(0.0) .withKD(0.0)) .withCurrentLimits(new CurrentLimitsConfigs() - .withStatorCurrentLimit(30) - .withSupplyCurrentLimit(30)) + .withStatorCurrentLimit(110) + // .withSupplyCurrentLimit(120) + ) .withVoltage(new VoltageConfigs() .withPeakForwardVoltage(12.0) - .withPeakReverseVoltage(-12.0)); + .withPeakReverseVoltage(-12.0)) + .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 1f1a61e..5a15011 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -4,11 +4,14 @@ import java.util.function.DoubleSupplier; +import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; @@ -68,12 +71,20 @@ private Shooter(boolean left) { topID = Motors.TOPRIGHT.id; } + var config = getConfig(); + + if (!left) { + config = config.clone().withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive)); + } + bottomMotor = new TalonFX(bottomID); - bottomMotor.getConfigurator().apply(getConfig()); + bottomMotor.getConfigurator().apply(config); bottomMotor.setNeutralMode(NeutralModeValue.Coast); topMotor = new TalonFX(topID); - topMotor.getConfigurator().apply(getConfig()); + topMotor.getConfigurator().apply(config); topMotor.setNeutralMode(NeutralModeValue.Coast); if (Robot.isSimulation()) { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index b42dc0c..df0d97c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.shooter; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.VoltageConfigs; @@ -18,10 +19,10 @@ public final class ShooterConstants { /** Motor ids */ public static enum Motors { - TOPLEFT(58), - BOTTOMLEFT(57), - TOPRIGHT(56), - BOTTOMRIGHT(55); + TOPLEFT(24), + BOTTOMLEFT(25), + TOPRIGHT(23), + BOTTOMRIGHT(22); public final int id; private Motors(int id) { this.id = id; @@ -33,15 +34,15 @@ public static TalonFXConfiguration getConfig() { return new TalonFXConfiguration() .withSlot0(new Slot0Configs() .withKV(0.0) - .withKP(0.3 ) + .withKP(0.3) .withKI(0.001) .withKD(0.0) .withKA(0.1) .withKS(0.1) .withKV(0.1)) // placeholder values .withCurrentLimits(new CurrentLimitsConfigs() - .withStatorCurrentLimit(60) - .withSupplyCurrentLimit(60)) + .withStatorCurrentLimit(30) + .withSupplyCurrentLimit(30)) .withVoltage(new VoltageConfigs() .withPeakForwardVoltage(12.0) .withPeakReverseVoltage(-12.0)); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java b/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java index ec1f071..37e08ac 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java @@ -49,9 +49,9 @@ public static VisionDeviceManager getInstance() { public VisionDeviceManager() { // leftCamera = new VisionDevice(Constants.Limelight.VisionDeviceConstants.L_CONSTANTS); // rightCamera = new VisionDevice(Constants.Limelight.VisionDeviceConstants.R_CONSTANTS); - frontrCamera = new VisionDevice(VisionDeviceConstants.FR_CONSTANTS); - frontlCamera = new VisionDevice(VisionDeviceConstants.FL_CONSTANTS); - cameras = List.of(frontrCamera, frontlCamera); + // frontrCamera = new VisionDevice(VisionDeviceConstants.FR_CONSTANTS); + // frontlCamera = new VisionDevice(VisionDeviceConstants.FL_CONSTANTS); + cameras = List.of(); // cameras = List.of(rightCamera); if (Robot.isSimulation()) { visionSim = new VisionSystemSim(getName()); @@ -87,16 +87,18 @@ public synchronized MovingAverageDouble getMovingAverage() { } public synchronized boolean isFullyConnected() { - return frontlCamera.isConnected() - && frontrCamera.isConnected(); + return true; + // frontlCamera.isConnected() + // && frontrCamera.isConnected(); // && rightCamera.isConnected(); // && backCamera.isConnected(); } public Command bootUp() { return Commands.parallel( - frontlCamera.bootUpSequence(), - frontrCamera.bootUpSequence()) + // frontlCamera.bootUpSequence(), + // frontrCamera.bootUpSequence() + ) .withTimeout(4) .andThen(Commands.print("Finished vision bootup")); } From 732c52a558218dd276880459e4a6d226304ede3b Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Mon, 23 Feb 2026 20:47:17 -0800 Subject: [PATCH 091/118] lots of stuff works --- src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/ControlsMapping.java | 60 +--- src/main/java/frc/robot/Robot.java | 4 +- .../frc/robot/subsystems/climb/Climb.java | 23 +- .../subsystems/climb/ClimbConstants.java | 2 - .../frc/robot/subsystems/drive/Drive.java | 6 +- .../subsystems/drive/DriveConstants.java | 4 +- .../drive/ctre/CompCtreDriveConstants.java | 311 ++++++++++++++++++ .../subsystems/drive/ctre/CtreDrive.java | 7 +- .../drive/ctre/CtreDriveConstants.java | 10 +- .../drive/ctre/CtreDriveConstants2.java | 8 +- .../frc/robot/subsystems/intake/Intake.java | 68 ++-- .../subsystems/intake/IntakeConstants.java | 48 ++- .../subsystems/roller/RollerConstants.java | 2 +- .../frc/robot/subsystems/shooter/Shooter.java | 20 -- .../subsystems/shooter/ShooterConstants.java | 12 +- .../subsystems/shooter/ShotCalculator.java | 32 +- 17 files changed, 442 insertions(+), 185 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/drive/ctre/CompCtreDriveConstants.java diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index d2272ac..b73d075 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Robot2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 157; - public static final String GIT_SHA = "40f2a4fe4bec42c1f6582769290a06859b69c101"; - public static final String GIT_DATE = "2026-02-17 21:53:27 EST"; + public static final int GIT_REVISION = 158; + public static final String GIT_SHA = "52167db8f4af9e19a176121a3ee2837f1ce658f1"; + public static final String GIT_DATE = "2026-02-23 01:30:28 EST"; public static final String GIT_BRANCH = "sotm"; - public static final String BUILD_DATE = "2026-02-21 21:32:44 EST"; - public static final long BUILD_UNIX_TIME = 1771727564987L; + public static final String BUILD_DATE = "2026-02-23 23:41:50 EST"; + public static final long BUILD_UNIX_TIME = 1771908110729L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index 0d15a78..847b187 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -27,11 +27,11 @@ public class ControlsMapping { public static void mapTeleopCommand() { - // Drive.getInstance().setDefaultCommand((Drive.getInstance().openLoopControl())); + Drive.getInstance().setDefaultCommand((Drive.getInstance().openLoopControl())); // Intake.getInstance().setDefaultCommand(Intake.getInstance().intake()); - // controller.back().and(controller.a()).onTrue(Drive.getInstance().resetPoseCommand(new - // Pose2d())); + controller.back().onTrue(Drive.getInstance().resetPoseCommand(new + Pose2d())); // controller.back().and(controller.b()).onTrue(VisionDeviceManager.getInstance().bootUp()); // controller.leftTrigger().debounce(0.1).whileTrue(Intake.getInstance().outtake()); @@ -62,61 +62,29 @@ public static void mapTeleopCommand() { // )).andThen(Commands.waitSeconds(0.1)) // ) // ); - - controller.x().whileTrue( - Commands.parallel( - // Shooter.getRightInstance().shoot(50, -10), - // Indexer.getRightInstance().activateIndexer() - // , - Shooter.getLeftInstance().shoot(50, -10), - Indexer.getLeftInstance().activateIndexer())) - .onFalse( - Commands.parallel( - // Shooter.getRightInstance().stop(), - // Indexer.getRightInstance().deactivateIndexer() - // , - Shooter.getLeftInstance().stop(), - Indexer.getLeftInstance().deactivateIndexer() - - )); - - controller.y().whileTrue( - Commands.parallel( - // Shooter.getRightInstance().shoot(-10, 50), - // Indexer.getRightInstance().activateIndexer() - // , - Shooter.getLeftInstance().shoot(-10, 50), - Indexer.getLeftInstance().activateIndexer())) - .onFalse( - Commands.parallel( - // Shooter.getRightInstance().stop(), - // Indexer.getRightInstance().deactivateIndexer() - // , - Shooter.getLeftInstance().stop(), - Indexer.getLeftInstance().deactivateIndexer() - - )); - controller.a().whileTrue( Commands.parallel( - Shooter.getRightInstance().shoot(40, 40), + Shooter.getRightInstance().shoot(60, 60), Indexer.getRightInstance().activateIndexer(), - Shooter.getLeftInstance().shoot(40, 40), - Indexer.getLeftInstance().activateIndexer(), - Roller.getInstance().roll())) + Shooter.getLeftInstance().shoot(60, 60), + Indexer.getLeftInstance().activateIndexer() + , + Roller.getInstance().roll() + )) .onFalse( Commands.parallel( Shooter.getRightInstance().stop(), Indexer.getRightInstance().deactivateIndexer() , Shooter.getLeftInstance().stop(), - Indexer.getLeftInstance().deactivateIndexer(), - Roller.getInstance().stop() + Indexer.getLeftInstance().deactivateIndexer() + , + Roller.getInstance().stop() )); controller.b().whileTrue( - Roller.getInstance().roll()).onFalse(Roller.getInstance().stop()); - + Intake.getInstance().intake()).onFalse(Intake.getInstance().stow()); + // controller.y().onTrue(Intake.getInstance().) // controller.leftBumper().whileTrue(Drive.getInstance().headingLockToHub() // .alongWith(Shooter.getLeftInstance().shoot(), // Shooter.getRightInstance().shoot())); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ad6be19..f56b4f5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -37,7 +37,6 @@ import frc.robot.subsystems.drive.*; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.roller.Roller; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.vision.VisionDeviceManager; @@ -121,9 +120,8 @@ public Robot() { Shooter.getRightInstance(); Indexer.getLeftInstance(); Indexer.getRightInstance(); - // Intake.getInstance(); + Intake.getInstance(); // Climb.getInstance(); - Roller.getInstance(); TelemetryManager.getInstance(); commandScheduler.schedule(FollowPathCommand.warmupCommand()); diff --git a/src/main/java/frc/robot/subsystems/climb/Climb.java b/src/main/java/frc/robot/subsystems/climb/Climb.java index b548594..a52e279 100644 --- a/src/main/java/frc/robot/subsystems/climb/Climb.java +++ b/src/main/java/frc/robot/subsystems/climb/Climb.java @@ -20,7 +20,6 @@ import frc.robot.Constants; import frc.robot.Robot; import frc.robot.subsystems.TelemetryManager; -import frc.robot.lib.io.TalonFXIO; import frc.robot.subsystems.climb.ClimbConstants.Setpoint; import edu.wpi.first.wpilibj.simulation.ElevatorSim; @@ -64,7 +63,6 @@ private Climb() { ); } io = new ClimbIO(getName(), climbMotor); - // TelemetryManager.getInstance().addSendable(this); } @Override @@ -163,27 +161,8 @@ public boolean isNearTarget() { targetHeight, EPSILON); } - - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.addDoubleProperty( - "Height", - () -> lastReadHeight, - null); - builder.addDoubleProperty( - "Speed", - () -> lastReadSpeed, - null); - builder.addDoubleProperty( - "Target Height", - () -> targetHeight, - null); - TelemetryManager.makeSendableTalonFX("ClimbMotor", climbMotor, builder); - } - // command - + public Command hangCommand() { return moveToScoringHeight(Setpoint.UP) .andThen(Climb.getInstance().moveToScoringHeight(Setpoint.BASE)); diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java b/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java index b7ddf65..35c85dc 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java @@ -23,8 +23,6 @@ public class ClimbConstants { public static final double METERS_PER_ROTATION = 0.028776; // Approximated using measurement public static final double CARRIAGE_WEIGHT = 7.55; // kg - // Yo uu hav eto tTune all of this above me - public static final double MAX_ACCEL = 1.5; public static final double MAX_SPEED = 1.0; // m/s diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 553fe81..5327cc6 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -2,8 +2,6 @@ import static frc.robot.subsystems.drive.DriveConstants.*; -import java.util.function.Consumer; -import java.util.function.Function; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -43,7 +41,7 @@ import frc.robot.lib.trajectory.LocalADStarWrapper; import frc.robot.lib.util.Util; import frc.robot.subsystems.TelemetryManager; -import frc.robot.subsystems.drive.ctre.CtreDriveConstants; +import frc.robot.subsystems.drive.ctre.CompCtreDriveConstants; import frc.robot.subsystems.shooter.ShotCalculator; import frc.robot.subsystems.drive.commands.AutopilotCommand; import frc.robot.subsystems.drive.commands.PIDToPoseCommand; @@ -75,7 +73,7 @@ public static Drive getInstance() { private final DriveIO io; private Drive() { - drivetrain = CtreDriveConstants.createDrivetrain(); + drivetrain = CompCtreDriveConstants.createDrivetrain(); // telemetry = new CtreDriveTelemetry(MAX_SPEED); teleopRequest = new SwerveRequest.FieldCentric(); driveRequest = teleopRequest; diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index f9d6c81..60cc6d8 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -10,7 +10,7 @@ import edu.wpi.first.units.measure.Time; import frc.robot.lib.control.ControlConstants.*; import frc.robot.lib.field.FieldLayout; -import frc.robot.subsystems.drive.ctre.CtreDriveConstants; +import frc.robot.subsystems.drive.ctre.CompCtreDriveConstants; public final class DriveConstants { public static final double EPSILON_TRANSLATION = 0.015; // cm @@ -27,7 +27,7 @@ public final class DriveConstants { // Swerve dimensions public static final double TRACK_WIDTH = Units.Inches.of(24).in(Units.Meters); public static final double WHEEL_BASE = Units.Inches.of(24).in(Units.Meters); - public static final double WHEEL_DIAMETER = 2 * CtreDriveConstants.kWheelRadius.in(Units.Meters); + public static final double WHEEL_DIAMETER = 2 * CompCtreDriveConstants.kWheelRadius.in(Units.Meters); public static final double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER * Math.PI; // Stability constants diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CompCtreDriveConstants.java b/src/main/java/frc/robot/subsystems/drive/ctre/CompCtreDriveConstants.java new file mode 100644 index 0000000..8d07ba4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CompCtreDriveConstants.java @@ -0,0 +1,311 @@ +package frc.robot.subsystems.drive.ctre; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.*; +import com.ctre.phoenix6.hardware.*; +import com.ctre.phoenix6.signals.*; +import com.ctre.phoenix6.swerve.*; +import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.units.measure.*; +import frc.robot.Constants; + +// Generated by the 2026 Tuner X Swerve Project Generator +// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html +public class CompCtreDriveConstants { + // Both sets of gains need to be tuned to your individual robot. + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + public static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + public static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + public static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + public static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* + public static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + public static final Current kSlipCurrent = Amps.of(120); + + // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. + public static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + public static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true) + ); + public static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + public static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("CV", "./logs/example.hoot"); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.04); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + public static final double kCoupleRatio = 3.5714285714285716; + + public static final double kDriveGearRatio = 6.122448979591837; + public static final double kSteerGearRatio = 21.428571428571427; + public static final Distance kWheelRadius = Inches.of(2); + + public static final boolean kInvertLeftSide = false; + public static final boolean kInvertRightSide = true; + + public static final int kPigeonId = 60; + + // These are only used for simulation + public static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); + public static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); + // Simulated voltage necessary to overcome friction + public static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + public static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + + // Both sets of gains need to be tuned to your individual robot via sysid. + + // translation sysid results + public static final double kS_sysid_drive = 0.12903; + public static final double kV_sysid_drive = 2.3293; + public static final double kA_sysid_drive = 0.41181; + public static final double kP_sysid_drive = 2.2622; + public static final double kD_sysid_drive = 0.0; + + // convert sysid gains into CTRE (rotation/s) units + public static final double kS_ctre_drive = kS_sysid_drive; + public static final double sysIdToCTRE = Constants.TAU * kWheelRadius.in(Meters) / kDriveGearRatio; + public static final double kV_ctre_drive = kV_sysid_drive * sysIdToCTRE; + public static final double kA_ctre_drive = kA_sysid_drive * sysIdToCTRE; + public static final double kP_ctre_drive = kP_sysid_drive * sysIdToCTRE; + public static final double kD_ctre_drive = kD_sysid_drive * sysIdToCTRE; + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + public static final Slot0Configs steerGains = new Slot0Configs() + .withKP(100) + .withKI(0) + .withKD(0.5) + .withKS(0.1) + .withKV(2.66) + .withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + public static final Slot0Configs driveGains = new Slot0Configs() + .withKP(kP_ctre_drive) + .withKI(0) + .withKD(0) + .withKS(kS_ctre_drive) + .withKV(kV_ctre_drive); + + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + public static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + + // Front Left + public static final int kFrontLeftDriveMotorId = 8; + public static final int kFrontLeftSteerMotorId = 10; + public static final int kFrontLeftEncoderId = 7; + public static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.224365234375); + public static final boolean kFrontLeftSteerMotorInverted = true; + public static final boolean kFrontLeftEncoderInverted = false; + + public static final Distance kFrontLeftXPos = Inches.of(10.875); + public static final Distance kFrontLeftYPos = Inches.of(10.875); + + // Front Right + public static final int kFrontRightDriveMotorId = 9; + public static final int kFrontRightSteerMotorId = 11; + public static final int kFrontRightEncoderId = 6; + public static final Angle kFrontRightEncoderOffset = Rotations.of(0.431640625); + public static final boolean kFrontRightSteerMotorInverted = true; + public static final boolean kFrontRightEncoderInverted = false; + + public static final Distance kFrontRightXPos = Inches.of(10.875); + public static final Distance kFrontRightYPos = Inches.of(-10.875); + + // Back Left + public static final int kBackLeftDriveMotorId = 3; + public static final int kBackLeftSteerMotorId = 5; + public static final int kBackLeftEncoderId = 14; + public static final Angle kBackLeftEncoderOffset = Rotations.of(-0.2119140625); + public static final boolean kBackLeftSteerMotorInverted = true; + public static final boolean kBackLeftEncoderInverted = false; + + public static final Distance kBackLeftXPos = Inches.of(-10.875); + public static final Distance kBackLeftYPos = Inches.of(10.875); + + // Back Right + public static final int kBackRightDriveMotorId = 4; + public static final int kBackRightSteerMotorId = 2; + public static final int kBackRightEncoderId = 1; + public static final Angle kBackRightEncoderOffset = Rotations.of(-0.1064453125); + public static final boolean kBackRightSteerMotorInverted = true; + public static final boolean kBackRightEncoderInverted = false; + + public static final Distance kBackRightXPos = Inches.of(-10.875); + public static final Distance kBackRightYPos = Inches.of(-10.875); + + + public static final SwerveModuleConstants FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, + kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted + ); + public static final SwerveModuleConstants FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, + kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted + ); + public static final SwerveModuleConstants BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, + kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted + ); + public static final SwerveModuleConstants BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, + kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted + ); + + /** + * Creates a CommandSwerveDrivetrain instance. + * This should only be called once in your robot program,. + */ + public static CtreDrive createDrivetrain() { + return new CtreDrive( + DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight + ); + } + + + /** + * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. + */ + public static class TunerSwerveDrivetrain extends SwerveDrivetrain { + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation + * in the form [x, y, theta]áµ€, with units in meters + * and radians + * @param visionStandardDeviation The standard deviation for vision calculation + * in the form [x, y, theta]áµ€, with units in meters + * and radians + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, + odometryStandardDeviation, visionStandardDeviation, modules + ); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java index e9d9681..cd4475a 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java @@ -23,8 +23,13 @@ import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.subsystems.drive.ctre.CtreDriveConstants2.TunerSwerveDrivetrain; +import frc.robot.subsystems.drive.ctre.CompCtreDriveConstants.TunerSwerveDrivetrain; + +// FL: -0.419922 +// FR: 0.141113 +// BL: -0.460693 +// BR: 0.193604 /** * Class that extends the Phoenix 6 SwerveDrivetrain class and implements * Subsystem so it can easily be used in command-based projects. diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java index f2abcb8..14c9439 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java @@ -26,7 +26,7 @@ // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class CtreDriveConstants { // which set of robot constants we are deploy - private static final boolean kIs2ndBot = false; //true for the 2nd bot we built for 2026; false for 2025 bot + private static final boolean kCompBot = true; //true for the 2nd bot we built for 2026; false for 2025 bot // mechanical and geometric parameters of drive train public static final double kDriveGearRatio = 6.122448979591837; @@ -161,7 +161,7 @@ public class CtreDriveConstants { private static final int kFrontLeftDriveMotorId = 8; private static final int kFrontLeftSteerMotorId = 10; private static final int kFrontLeftEncoderId = 7; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(kIs2ndBot ? -0.462890625: 0.355224609375); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(kCompBot ? -0.419922 : 0.355224609375); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; @@ -172,7 +172,7 @@ public class CtreDriveConstants { private static final int kFrontRightDriveMotorId = 9; private static final int kFrontRightSteerMotorId = 11; private static final int kFrontRightEncoderId = 6; - private static final Angle kFrontRightEncoderOffset = Rotations.of(kIs2ndBot ? 0.025390625: -0.4296875); + private static final Angle kFrontRightEncoderOffset = Rotations.of(kCompBot ? 0.141113 : -0.4296875); private static final boolean kFrontRightSteerMotorInverted = true; private static final boolean kFrontRightEncoderInverted = false; @@ -183,7 +183,7 @@ public class CtreDriveConstants { private static final int kBackLeftDriveMotorId = 3; private static final int kBackLeftSteerMotorId = 5; private static final int kBackLeftEncoderId = 14; - private static final Angle kBackLeftEncoderOffset = Rotations.of(kIs2ndBot ? 0.1435546875: 0.326416015625); + private static final Angle kBackLeftEncoderOffset = Rotations.of(kCompBot ? -0.460693 : 0.326416015625); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; @@ -194,7 +194,7 @@ public class CtreDriveConstants { private static final int kBackRightDriveMotorId = 4; private static final int kBackRightSteerMotorId = 2; private static final int kBackRightEncoderId = 1; - private static final Angle kBackRightEncoderOffset = Rotations.of(kIs2ndBot ? 0.183837890625: 0.0869140625); + private static final Angle kBackRightEncoderOffset = Rotations.of(kCompBot ? 0.193604 : 0.0869140625); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants2.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants2.java index 45cb196..b464e8d 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants2.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants2.java @@ -23,7 +23,7 @@ public class CtreDriveConstants2 { // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(100).withKI(0).withKD(0.5) + .withKP(60).withKI(0).withKD(0.1) .withKS(0.1).withKV(2.66).withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control @@ -77,10 +77,12 @@ public class CtreDriveConstants2 { // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.5714285714285716; - + private static final double kDriveGearRatio = 6.122448979591837; private static final double kSteerGearRatio = 21.428571428571427; + private static final double kCoupleRatio = kDriveGearRatio /(150./7.); + // 3.5714285714285716; + private static final Distance kWheelRadius = Inches.of(2); private static final boolean kInvertLeftSide = false; diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 73c94e9..4b4fd06 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -1,11 +1,15 @@ package frc.robot.subsystems.intake; import static frc.robot.subsystems.intake.IntakeConstants.*; +import static edu.wpi.first.units.Units.*; +import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.MathUtil; @@ -15,6 +19,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants; import frc.robot.Robot; import frc.robot.subsystems.intake.IntakeConstants.Motors; @@ -53,12 +58,10 @@ private Intake() { barMotor = new TalonFX(Motors.BAR.id); wheelMotor.getConfigurator().apply(getWheelConfig()); barMotor.getConfigurator().apply(getBarConfig()); - wheelMotor.setNeutralMode(NeutralModeValue.Brake); - barMotor.setNeutralMode(NeutralModeValue.Brake); //? + wheelMotor.setNeutralMode(NeutralModeValue.Coast); + barMotor.setNeutralMode(NeutralModeValue.Brake); - // SmartDashboard.putData("123123", mech2d); - // //TelemetryManager.getInstance().addSendable(this); - // mech2droot.append(mech2dpivot); + barMotor.setPosition(BAR_POSITION_UP); if (Robot.isSimulation()) { sim = new SingleJointedArmSim( @@ -75,7 +78,6 @@ private Intake() { barMotor.getSimState() .setRawRotorPosition(sim.getAngleRads() * (1 / Constants.TAU)); - wheelSim = new FlywheelSim( LinearSystemId.createFlywheelSystem( @@ -86,7 +88,6 @@ private Intake() { 0.0); } - // TelemetryManager.getInstance().addSendable(this); io = new IntakeIO(getName(), wheelMotor, barMotor); } @@ -149,15 +150,19 @@ private void setRequestBar(ControlRequest request) { this.barRequest = request; } - public Command setSetpoint(double wheelSpeed, double barPosition) { + if (Math.abs(wheelSpeed) <= 0.1) { + return stopWheel() + .andThen(setBarPosition(barPosition)) + .withName("Setpoint: " + wheelSpeed + "rps, " + barPosition + "rot"); + } + return setWheelSpeed(wheelSpeed) .andThen(setBarPosition(barPosition)) .withName("Setpoint: " + wheelSpeed + "rps, " + barPosition + "rot"); } - //---------bar----------- /** @@ -197,20 +202,41 @@ public Command setWheelSpeed(double speed) { ).withName("wheel speed set "+ (speed)); } + public Command stopWheel() { + return runOnce(() -> setRequestWheel(new CoastOut())); + } + public Command waitUntilBarIsAtPosition(double target) { return Commands.waitUntil(() -> Math.abs(target - barPosition) < BAR_EPSILON); } - - public Command waitUntilWheelIsAtSpeed(double target) { - return Commands.waitUntil(() -> Math.abs(target - wheelSpeed) < WHEEL_EPSILON); - } - - // @Override - // public void initSendable(SendableBuilder builder){ - // super.initSendable(builder); - // builder.addDoubleProperty("Position", () -> barPosition, null); - // TelemetryManager.makeSendableTalonFX("Bar Motor", barMotor, builder); - // TelemetryManager.makeSendableTalonFX("Wheel Motor", wheelMotor, builder); - // } + /** + * Recalibrates the elevator zero point. This slowly drives the elevator + * down until we see a drop in velocity and a spike in stator current, + * indicating that we've hit a hard stop. + * + * @return Command to run + */ + public Command calibrateZero() { + VoltageOut calibrationRequest = new VoltageOut(-1) + .withIgnoreHardwareLimits(true) + .withIgnoreSoftwareLimits(true); + + /** Trigger to detect when the elevator drives into a hard stop. */ + Trigger isHardStop = new Trigger(() -> { + return barMotor.getVelocity().getValue().abs(RotationsPerSecond) < 1 && + barMotor.getTorqueCurrent().getValue().abs(Amps) > 10; + }).debounce(0.1); + + return run(() -> { + barMotor.setControl(calibrationRequest); + }) + .until(isHardStop) + .andThen( + runOnce(() -> setRequestBar(new NeutralOut())).withTimeout(0.25) + .finallyDo(() -> { + barMotor.setPosition(Rotations.of(0)); + }) + ); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 51fd3a6..07cc2a9 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -1,29 +1,30 @@ package frc.robot.subsystems.intake; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Rotations; + import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.VoltageConfigs; import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; import edu.wpi.first.units.Units; -import frc.robot.Constants; public class IntakeConstants { public static final double BAR_EPSILON = Units.Degrees.of(5).in(Units.Rotations); - public static final double WHEEL_EPSILON = 0.5; // rotations per second - public static final double INTAKE_SPEED = 20; //rotations per second? - // public static final double BAR_VOLTAGE = 4.0; //? + public static final double INTAKE_SPEED = 50; public static final double BAR_POSITION_DOWN = 0.00; - public static final double BAR_POSITION_UP = Constants.TAU / 4; - public static final double BAR_GEAR_RATIO = 50.0; + public static final double BAR_POSITION_UP = Degrees.of(126).in(Rotations); + public static final double BAR_GEAR_RATIO = 44.0 / 18.0 * 5.0 * 4.0; public static final double BAR_POS_MIN = 0.0; - public static final double BAR_POS_MAX = Constants.TAU / 4; + public static final double BAR_POS_MAX = Degrees.of(126).in(Rotations); public static final double INTAKE_MASS = 3.656684786; // kg, ideally public static final double INTAKE_LENGTH = 0.1746631508; //m, hopefully - //etc - + public static enum Motors { //TODO: set motor ids; use separate file for ports? WHEEL(31), BAR(32); @@ -33,17 +34,7 @@ private Motors(int id) { } } - // public static enum Lasers { //TODO: set laser ids - // LEFT(-1), - // RIGHT(-1); - // public final int id; - // private Lasers(int id) { - // this.id = id; - // } - // } - - - public static TalonFXConfiguration getWheelConfig() { //TODO: values + public static TalonFXConfiguration getWheelConfig() { return new TalonFXConfiguration() .withSlot0(new Slot0Configs() .withKV(0.0) @@ -51,25 +42,24 @@ public static TalonFXConfiguration getWheelConfig() { //TODO: values .withKI(0.0) .withKD(0.0)) .withCurrentLimits(new CurrentLimitsConfigs() - .withStatorCurrentLimit(30) - .withSupplyCurrentLimit(30)) + .withStatorCurrentLimit(78)) .withVoltage(new VoltageConfigs() .withPeakForwardVoltage(12.0) - .withPeakReverseVoltage(-12.0)); + .withPeakReverseVoltage(-12.0)) + .withMotorOutput(new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive)); } - - public static TalonFXConfiguration getBarConfig() { //TODO: values + public static TalonFXConfiguration getBarConfig() { return new TalonFXConfiguration() .withSlot0(new Slot0Configs() .withKV(0.0) - .withKP(1.0) + .withKP(0.0) .withKI(0.0) .withKD(0.0) - .withKG(0.1).withGravityType(GravityTypeValue.Arm_Cosine)) + .withKG(0.0).withGravityType(GravityTypeValue.Arm_Cosine)) .withCurrentLimits(new CurrentLimitsConfigs() - .withStatorCurrentLimit(30) - .withSupplyCurrentLimit(30)) + .withStatorCurrentLimit(40)) .withVoltage(new VoltageConfigs() .withPeakForwardVoltage(12.0) .withPeakReverseVoltage(-12.0)) diff --git a/src/main/java/frc/robot/subsystems/roller/RollerConstants.java b/src/main/java/frc/robot/subsystems/roller/RollerConstants.java index 10bcbdf..ce7287c 100644 --- a/src/main/java/frc/robot/subsystems/roller/RollerConstants.java +++ b/src/main/java/frc/robot/subsystems/roller/RollerConstants.java @@ -19,7 +19,7 @@ public static TalonFXConfiguration getConfig() { .withKI(0.0) .withKD(0.0)) .withCurrentLimits(new CurrentLimitsConfigs() - .withStatorCurrentLimit(110) + .withStatorCurrentLimit(90) // .withSupplyCurrentLimit(120) ) .withVoltage(new VoltageConfigs() diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 5a15011..c31bda0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -4,7 +4,6 @@ import java.util.function.DoubleSupplier; -import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.ControlRequest; @@ -103,7 +102,6 @@ private Shooter(boolean left) { } io = new ShooterIO(getName(), topMotor, bottomMotor); - // TelemetryManager.getInstance().addSendable(this); } @Override @@ -190,22 +188,4 @@ public Command shoot() { return shoot(() -> shotCalculator.getInterceptSolution().launchSpeed() / Constants.TAU / 0.0508 - TOPSPIN_FACTOR, () -> -shotCalculator.getInterceptSolution().launchSpeed() / Constants.TAU / 0.0508 - TOPSPIN_FACTOR); } - - // @Override - // public void initSendable(SendableBuilder builder) { - // super.initSendable(builder); - - // builder.addDoubleProperty( - // "TopSpeed", - // () -> lastReadSpeedTop, - // null); - // TelemetryManager.makeSendableTalonFX("Top", topMotor, builder); - - // builder.addDoubleProperty( - // "BottomSpeed", - // () -> lastReadSpeedBottom, - // null); - // TelemetryManager.makeSendableTalonFX("Bottom", bottomMotor, builder); - // } - } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index df0d97c..cb0085c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.shooter; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.VoltageConfigs; @@ -35,14 +34,13 @@ public static TalonFXConfiguration getConfig() { .withSlot0(new Slot0Configs() .withKV(0.0) .withKP(0.3) - .withKI(0.001) + .withKI(0.0) .withKD(0.0) - .withKA(0.1) - .withKS(0.1) - .withKV(0.1)) // placeholder values + .withKA(0.0) + .withKS(0.0) + .withKV(0.0)) // placeholder values .withCurrentLimits(new CurrentLimitsConfigs() - .withStatorCurrentLimit(30) - .withSupplyCurrentLimit(30)) + .withStatorCurrentLimit(40)) .withVoltage(new VoltageConfigs() .withPeakForwardVoltage(12.0) .withPeakReverseVoltage(-12.0)); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java index e0b64a0..4b23c41 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java @@ -36,6 +36,9 @@ public static ShotCalculator getInstance() { private double targetDistance = 0.0; private double shooterAngle = 75 * Constants.TAU / 360; + private ChassisSpeeds zero = new ChassisSpeeds(); + private ChassisAccels zero1 = new ChassisAccels(); + private ShotCalculator() { this.drive = Drive.getInstance(); AutoLogOutputManager.addObject(this); @@ -43,25 +46,26 @@ private ShotCalculator() { @Override public void periodic() { - Pose2d drivePose = drive.getPose(); + // Pose2d drivePose = drive.getPose(); - targetDistance = drivePose.getTranslation().getDistance(targetLocation.toPose2d().getTranslation()); + // targetDistance = drivePose.getTranslation().getDistance(targetLocation.toPose2d().getTranslation()); - Pose3d shooterPose = new Pose3d(drivePose).plus(ShooterConstants.OFFSET); + // Pose3d shooterPose = new Pose3d(drivePose).plus(ShooterConstants.OFFSET); - ChassisSpeeds driveSpeeds = drive.getFieldSpeeds(); - ChassisAccels driveAccelerations = ChassisAccels.estimate(driveSpeeds, drive.getPrevFieldSpeeds(), Constants.DT); + + // ChassisSpeeds driveSpeeds = drive.getFieldSpeeds(); + // ChassisAccels driveAccelerations = ChassisAccels.estimate(driveSpeeds, drive.getPrevFieldSpeeds(), Constants.DT); - currentInterceptSolution = ShootOnTheFlyCalculator.solveShootOnTheFly( - shooterPose, - targetLocation, - driveSpeeds, - driveAccelerations, - -shooterAngle, - 5, 0.01); + // currentInterceptSolution = ShootOnTheFlyCalculator.solveShootOnTheFly( + // shooterPose, + // targetLocation, + // zero, + // zero1, + // -shooterAngle, + // 5, 0.01); - currentEffectiveTargetPose = currentInterceptSolution.effectiveTargetPose(); - currentEffectiveYaw = currentInterceptSolution.requiredYaw(); + // currentEffectiveTargetPose = currentInterceptSolution.effectiveTargetPose(); + // currentEffectiveYaw = currentInterceptSolution.requiredYaw(); } public void setTarget(Pose3d targetLocation) { From 9c4a6dd46ee231882ff4a41d63eebec04055e099 Mon Sep 17 00:00:00 2001 From: Arvind Venkatesh Date: Tue, 24 Feb 2026 15:40:05 -0800 Subject: [PATCH 092/118] feat: camera rotations --- .../java/frc/robot/subsystems/vision/VisionConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 24320ae..1ac1cbd 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -27,14 +27,14 @@ public static enum VisionDeviceConstants { "frontr", new Transform3d( new Translation3d(0.2822, 0.1087, 0.1984), - new Rotation3d(0.5 * Constants.TAU, 14.0 * Constants.TAU / 360.0, -26.0 * Constants.TAU/360.0)), + new Rotation3d(0, 26 * Constants.TAU / 360.0, -32.5 * Constants.TAU/360.0)), 1, 1280, 800), FL_CONSTANTS ( "frontl", new Transform3d( new Translation3d(0.2822, -0.1087, 0.1984), - new Rotation3d(0.5 * Constants.TAU, 14.0 * Constants.TAU / 360.0, 26.0 * Constants.TAU/360.0)), + new Rotation3d(0, 24 * Constants.TAU / 360.0, 27.5 * Constants.TAU/360.0)), 2, 1280, 800); public final String tableName; From ce09e076753a57e0f8ab453607d72e1433288e4a Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Tue, 24 Feb 2026 17:40:50 -0800 Subject: [PATCH 093/118] vision is quite off for some reasons, some stuff may work --- src/main/java/frc/robot/BuildConstants.java | 10 ++-- src/main/java/frc/robot/ControlsMapping.java | 48 +++++++++++-------- src/main/java/frc/robot/Robot.java | 14 +++--- .../frc/robot/subsystems/shooter/Shooter.java | 9 ++-- .../subsystems/shooter/ShooterConstants.java | 13 +++-- .../subsystems/shooter/ShotCalculator.java | 32 ++++++------- .../subsystems/vision/VisionConstants.java | 16 +++++-- .../vision/VisionDeviceManager.java | 6 +-- 8 files changed, 84 insertions(+), 64 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index b73d075..d42598d 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Robot2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 158; - public static final String GIT_SHA = "52167db8f4af9e19a176121a3ee2837f1ce658f1"; - public static final String GIT_DATE = "2026-02-23 01:30:28 EST"; + public static final int GIT_REVISION = 160; + public static final String GIT_SHA = "9c4a6dd46ee231882ff4a41d63eebec04055e099"; + public static final String GIT_DATE = "2026-02-24 18:40:05 EST"; public static final String GIT_BRANCH = "sotm"; - public static final String BUILD_DATE = "2026-02-23 23:41:50 EST"; - public static final long BUILD_UNIX_TIME = 1771908110729L; + public static final String BUILD_DATE = "2026-02-24 20:38:28 EST"; + public static final long BUILD_UNIX_TIME = 1771983508642L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index 847b187..1376f48 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -32,7 +32,7 @@ public static void mapTeleopCommand() { controller.back().onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); - // controller.back().and(controller.b()).onTrue(VisionDeviceManager.getInstance().bootUp()); + controller.y().onTrue(VisionDeviceManager.getInstance().bootUp()); // controller.leftTrigger().debounce(0.1).whileTrue(Intake.getInstance().outtake()); // controller.b().whileTrue(Intake.getInstance().outtake()); @@ -63,30 +63,36 @@ public static void mapTeleopCommand() { // ) // ); controller.a().whileTrue( - Commands.parallel( - Shooter.getRightInstance().shoot(60, 60), - Indexer.getRightInstance().activateIndexer(), - Shooter.getLeftInstance().shoot(60, 60), - Indexer.getLeftInstance().activateIndexer() - , - Roller.getInstance().roll() - )) - .onFalse( - Commands.parallel( - Shooter.getRightInstance().stop(), - Indexer.getRightInstance().deactivateIndexer() - , - Shooter.getLeftInstance().stop(), - Indexer.getLeftInstance().deactivateIndexer() - , - Roller.getInstance().stop() - )); + Commands.parallel( + Shooter.getRightInstance().shoot(60, 60), + Shooter.getLeftInstance().shoot(60, 60) + ) + ).onFalse( + Commands.parallel( + Shooter.getRightInstance().stop(), + Shooter.getLeftInstance().stop() + ) + ); + + controller.x().whileTrue( + Commands.parallel( + Indexer.getRightInstance().activateIndexer(), + Indexer.getLeftInstance().activateIndexer(), + Roller.getInstance().roll() + ) + ).onFalse( + Commands.parallel( + Indexer.getRightInstance().deactivateIndexer(), + Indexer.getLeftInstance().deactivateIndexer(), + Roller.getInstance().stop() + ) + ); + controller.b().whileTrue( Intake.getInstance().intake()).onFalse(Intake.getInstance().stow()); // controller.y().onTrue(Intake.getInstance().) - // controller.leftBumper().whileTrue(Drive.getInstance().headingLockToHub() - // .alongWith(Shooter.getLeftInstance().shoot(), + controller.leftBumper().whileTrue(Drive.getInstance().headingLockToHub()); // Shooter.getRightInstance().shoot())); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f56b4f5..0d835f9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -67,14 +67,14 @@ public class Robot extends LoggedRobot { public Robot() { boolean replay = Logger.hasReplaySource(); //robot data loggers - // boolean usbPresent = new java.io.File("/u").exists(); - // if (usbPresent) { + boolean usbPresent = new java.io.File("/u").exists(); + if (usbPresent) { // DataLogManager.start("/u/logs"); // USB stick - // System.out.println("Log/USB mounts OK"); - // } else { + System.out.println("Log/USB mounts OK"); + } else { // DataLogManager.start(); // falls back to /home/lvuser/logs - // System.out.println("Log/USB mounts NOT OK"); - // } + System.out.println("Log/USB mounts NOT OK"); + } Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); @@ -108,7 +108,7 @@ public Robot() { Logger.addDataReceiver(new WPILOGWriter()); } - Logger.start(); + // Logger.start(); if (!Logger.hasReplaySource()) { RobotController.setTimeSource(RobotController::getFPGATime); } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index c31bda0..fcfd8c3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -7,6 +7,7 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; @@ -164,14 +165,14 @@ public Command stop() { public Command shoot(double topSpeed, double bottomSpeed) { return runOnce(() -> { - setTopRequest(new VelocityVoltage(topSpeed)); - setBottomRequest(new VelocityVoltage(bottomSpeed)); + setTopRequest(new MotionMagicVelocityVoltage(topSpeed)); + setBottomRequest(new MotionMagicVelocityVoltage(bottomSpeed)); }).withName("Shooting"); } public Command shoot(DoubleSupplier topSpeed, DoubleSupplier bottomSpeed) { - var topReq = new VelocityVoltage(0.0); - var bottomReq = new VelocityVoltage(0.0); + var topReq = new MotionMagicVelocityVoltage(0.0); + var bottomReq = new MotionMagicVelocityVoltage(0.0); return runOnce(() -> { setTopRequest(topReq); setBottomRequest(bottomReq); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index cb0085c..8f37c60 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.shooter; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.VoltageConfigs; @@ -33,16 +34,20 @@ public static TalonFXConfiguration getConfig() { return new TalonFXConfiguration() .withSlot0(new Slot0Configs() .withKV(0.0) - .withKP(0.3) + .withKP(0.7) .withKI(0.0) .withKD(0.0) .withKA(0.0) - .withKS(0.0) + .withKS(5.0) .withKV(0.0)) // placeholder values .withCurrentLimits(new CurrentLimitsConfigs() - .withStatorCurrentLimit(40)) + .withStatorCurrentLimit(60)) .withVoltage(new VoltageConfigs() .withPeakForwardVoltage(12.0) - .withPeakReverseVoltage(-12.0)); + .withPeakReverseVoltage(-12.0)) + .withMotionMagic(new MotionMagicConfigs() + .withMotionMagicAcceleration(120) // 1.0 m/s^2 + .withMotionMagicCruiseVelocity(120) + .withMotionMagicJerk(120)); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java index 4b23c41..f1994be 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java @@ -46,26 +46,26 @@ private ShotCalculator() { @Override public void periodic() { - // Pose2d drivePose = drive.getPose(); + Pose2d drivePose = drive.getPose(); - // targetDistance = drivePose.getTranslation().getDistance(targetLocation.toPose2d().getTranslation()); + targetDistance = drivePose.getTranslation().getDistance(targetLocation.toPose2d().getTranslation()); - // Pose3d shooterPose = new Pose3d(drivePose).plus(ShooterConstants.OFFSET); + Pose3d shooterPose = new Pose3d(drivePose).plus(ShooterConstants.OFFSET); - // ChassisSpeeds driveSpeeds = drive.getFieldSpeeds(); - // ChassisAccels driveAccelerations = ChassisAccels.estimate(driveSpeeds, drive.getPrevFieldSpeeds(), Constants.DT); - - // currentInterceptSolution = ShootOnTheFlyCalculator.solveShootOnTheFly( - // shooterPose, - // targetLocation, - // zero, - // zero1, - // -shooterAngle, - // 5, 0.01); - - // currentEffectiveTargetPose = currentInterceptSolution.effectiveTargetPose(); - // currentEffectiveYaw = currentInterceptSolution.requiredYaw(); + ChassisSpeeds driveSpeeds = drive.getFieldSpeeds(); + ChassisAccels driveAccelerations = ChassisAccels.estimate(driveSpeeds, drive.getPrevFieldSpeeds(), Constants.DT); + + currentInterceptSolution = ShootOnTheFlyCalculator.solveShootOnTheFly( + shooterPose, + targetLocation, + zero, + zero1, + -shooterAngle, + 5, 0.01); + + currentEffectiveTargetPose = currentInterceptSolution.effectiveTargetPose(); + currentEffectiveYaw = currentInterceptSolution.requiredYaw(); } public void setTarget(Pose3d targetLocation) { diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 1ac1cbd..c2ca43f 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.vision; +import static edu.wpi.first.units.Units.Inches; + import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Rotation3d; @@ -24,16 +26,22 @@ public class VisionConstants { public static enum VisionDeviceConstants { FR_CONSTANTS ( - "frontr", + "orangelight", new Transform3d( - new Translation3d(0.2822, 0.1087, 0.1984), + new Translation3d( + Inches.of(4.52790), + Inches.of(13.124114), + Inches.of(14.365654)), new Rotation3d(0, 26 * Constants.TAU / 360.0, -32.5 * Constants.TAU/360.0)), 1, 1280, 800), FL_CONSTANTS ( - "frontl", + "lemonlight", new Transform3d( - new Translation3d(0.2822, -0.1087, 0.1984), + new Translation3d( + Inches.of(-3.030256), + Inches.of(13.262586), + Inches.of(14.325391)), new Rotation3d(0, 24 * Constants.TAU / 360.0, 27.5 * Constants.TAU/360.0)), 2, 1280, 800); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java b/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java index 37e08ac..539966a 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java @@ -49,9 +49,9 @@ public static VisionDeviceManager getInstance() { public VisionDeviceManager() { // leftCamera = new VisionDevice(Constants.Limelight.VisionDeviceConstants.L_CONSTANTS); // rightCamera = new VisionDevice(Constants.Limelight.VisionDeviceConstants.R_CONSTANTS); - // frontrCamera = new VisionDevice(VisionDeviceConstants.FR_CONSTANTS); - // frontlCamera = new VisionDevice(VisionDeviceConstants.FL_CONSTANTS); - cameras = List.of(); + frontrCamera = new VisionDevice(VisionDeviceConstants.FR_CONSTANTS); + frontlCamera = new VisionDevice(VisionDeviceConstants.FL_CONSTANTS); + cameras = List.of(frontrCamera, frontlCamera); // cameras = List.of(rightCamera); if (Robot.isSimulation()) { visionSim = new VisionSystemSim(getName()); From bba633add7f6a412e9e277d2ce1cb83903b3e7b2 Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Wed, 25 Feb 2026 17:48:31 -0800 Subject: [PATCH 094/118] feat: front cameras calib-ed and working. make sure PV tag pipeline setting to 3D instead of 2D. --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- .../robot/subsystems/vision/VisionConstants.java | 14 +++++++------- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index d42598d..0ce49d7 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Robot2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 160; - public static final String GIT_SHA = "9c4a6dd46ee231882ff4a41d63eebec04055e099"; - public static final String GIT_DATE = "2026-02-24 18:40:05 EST"; + public static final int GIT_REVISION = 161; + public static final String GIT_SHA = "ce09e076753a57e0f8ab453607d72e1433288e4a"; + public static final String GIT_DATE = "2026-02-24 20:40:50 EST"; public static final String GIT_BRANCH = "sotm"; - public static final String BUILD_DATE = "2026-02-24 20:38:28 EST"; - public static final long BUILD_UNIX_TIME = 1771983508642L; + public static final String BUILD_DATE = "2026-02-25 18:28:39 EST"; + public static final long BUILD_UNIX_TIME = 1772062119568L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index c2ca43f..c3c1afa 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -26,21 +26,21 @@ public class VisionConstants { public static enum VisionDeviceConstants { FR_CONSTANTS ( - "orangelight", + "orangelight", //right camera new Transform3d( new Translation3d( - Inches.of(4.52790), - Inches.of(13.124114), + Inches.of(13.124114), //wpi x-axis positive is forward direction + Inches.of(-3.030256), //wpi y-axis positive is strafe left, so right camera shall have negative offset Inches.of(14.365654)), - new Rotation3d(0, 26 * Constants.TAU / 360.0, -32.5 * Constants.TAU/360.0)), + new Rotation3d(0, 26 * Constants.TAU / 360.0, -32.5 * Constants.TAU/360.0)), //(roll: x, pitch: y, yaw: z) 1, 1280, 800), FL_CONSTANTS ( - "lemonlight", + "lemonlight", //left camera new Transform3d( new Translation3d( - Inches.of(-3.030256), - Inches.of(13.262586), + Inches.of(13.262586), //wpi x-axis positive is forward direction + Inches.of(4.52790), //wpi y-axis positive is strafe left, so left camera shall have positive offset Inches.of(14.325391)), new Rotation3d(0, 24 * Constants.TAU / 360.0, 27.5 * Constants.TAU/360.0)), 2, 1280, 800); From 4cef7ab6228497f5481ccd8c3f953114b793bc02 Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Wed, 25 Feb 2026 18:25:09 -0800 Subject: [PATCH 095/118] feat: sysid for drivetrain and user AdvantageKit for logging --- src/main/java/frc/robot/BuildConstants.java | 10 ++++---- .../subsystems/drive/ctre/CtreDrive.java | 25 +++++-------------- 2 files changed, 11 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 0ce49d7..d4414c0 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Robot2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 161; - public static final String GIT_SHA = "ce09e076753a57e0f8ab453607d72e1433288e4a"; - public static final String GIT_DATE = "2026-02-24 20:40:50 EST"; + public static final int GIT_REVISION = 162; + public static final String GIT_SHA = "bba633add7f6a412e9e277d2ce1cb83903b3e7b2"; + public static final String GIT_DATE = "2026-02-25 20:48:31 EST"; public static final String GIT_BRANCH = "sotm"; - public static final String BUILD_DATE = "2026-02-25 18:28:39 EST"; - public static final long BUILD_UNIX_TIME = 1772062119568L; + public static final String BUILD_DATE = "2026-02-25 21:22:08 EST"; + public static final long BUILD_UNIX_TIME = 1772072528929L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java index cd4475a..7b805fd 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java @@ -5,6 +5,7 @@ import java.util.function.Supplier; import com.ctre.phoenix6.SignalLogger; +import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants; @@ -68,21 +69,14 @@ public static enum SysIdRoutineType { null, // Use default timeout (10 s) // Log state with SignalLogger class // state -> SignalLogger.writeString("SysIdTranslation_State", state.toString()) - null + (state) -> Logger.recordOutput("SysIdDrive", state.toString()) ), new SysIdRoutine.Mechanism( output -> { m_lastAppliedVolts = output.in(Volts); setControl(m_translationCharacterization.withVolts(output)); }, - log -> { - var s = getStateCopy(); - log.motor("drive") - .voltage(Volts.of(m_lastAppliedVolts)) - .linearPosition(Meters.of(s.Pose.getTranslation().getX())) // or avg wheel distance - .linearVelocity(MetersPerSecond.of(getKinematics() - .toChassisSpeeds(s.ModuleStates).vxMetersPerSecond)); - }, + null , this ) ); @@ -94,7 +88,7 @@ public static enum SysIdRoutineType { Volts.of(7), // Use dynamic voltage of 7 V null, // Use default timeout (10 s) // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdSteer_State", state.toString()) + (state) -> Logger.recordOutput("SysIdSteer", state.toString()) ), new SysIdRoutine.Mechanism( volts -> setControl(m_steerCharacterization.withVolts(volts)), @@ -117,7 +111,7 @@ public static enum SysIdRoutineType { null, // Use default timeout (10 s) // Log state with SignalLogger class //state -> SignalLogger.writeString("SysIdRotation_State", state.toString()) - null + (state) -> Logger.recordOutput("SysIdRotation", state.toString()) ), new SysIdRoutine.Mechanism( output -> { @@ -125,14 +119,7 @@ public static enum SysIdRoutineType { m_lastAppliedVolts = output.in(Volts); setControl(m_rotationCharacterization.withRotationalRate(m_lastAppliedVolts)); }, - log -> { - var s = getStateCopy(); - log.motor("yaw") - .voltage(Volts.of(m_lastAppliedVolts)) - .angularPosition(Radians.of(s.Pose.getRotation().getRadians())) // get rotation position - .angularVelocity(RadiansPerSecond.of(getKinematics() - .toChassisSpeeds(s.ModuleStates).omegaRadiansPerSecond)); - }, + null, this // output -> { From 74aab5b41831805b7f70087f5720a09ecea4a60d Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Wed, 25 Feb 2026 18:52:46 -0800 Subject: [PATCH 096/118] feat: mapping sysid cmds --- src/main/java/frc/robot/BuildConstants.java | 10 +-- src/main/java/frc/robot/ControlsMapping.java | 73 ++++++++++---------- 2 files changed, 40 insertions(+), 43 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index d4414c0..ae0e97e 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Robot2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 162; - public static final String GIT_SHA = "bba633add7f6a412e9e277d2ce1cb83903b3e7b2"; - public static final String GIT_DATE = "2026-02-25 20:48:31 EST"; + public static final int GIT_REVISION = 163; + public static final String GIT_SHA = "4cef7ab6228497f5481ccd8c3f953114b793bc02"; + public static final String GIT_DATE = "2026-02-25 21:25:09 EST"; public static final String GIT_BRANCH = "sotm"; - public static final String BUILD_DATE = "2026-02-25 21:22:08 EST"; - public static final long BUILD_UNIX_TIME = 1772072528929L; + public static final String BUILD_DATE = "2026-02-25 21:47:17 EST"; + public static final long BUILD_UNIX_TIME = 1772074037013L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index 1376f48..a0f8b0c 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -23,6 +23,9 @@ import frc.robot.subsystems.shooter.ShotCalculator; import frc.robot.subsystems.vision.VisionDeviceManager; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ProxyCommand; + public class ControlsMapping { @@ -98,44 +101,38 @@ public static void mapTeleopCommand() { public static void mapSysId() { // set up sysID routine type - controller.a().onTrue(Commands.runOnce( - () -> Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.TRANSLATION))); - controller.b().onTrue(Commands.runOnce( - () -> Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.ROTATION))); - controller.back().onTrue(Commands.runOnce( - () -> Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER))); + controller.a().onTrue(new InstantCommand(()->Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.TRANSLATION))); + controller.b().onTrue(new InstantCommand(()->Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.ROTATION))); + controller.back().onTrue(new InstantCommand(()->Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER))); // map the sysid routine movement directions - controller.leftBumper().and(controller.x()).whileTrue( - Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kForward) - .finallyDo(( - boolean interrupted) -> { - if (interrupted) { - Drive.getInstance().setSwerveRequest(new SwerveRequest.Idle()); - } - })); - controller.leftBumper().and(controller.x()).whileTrue( - Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kReverse) - .finallyDo(( - boolean interrupted) -> { - if (interrupted) { - Drive.getInstance().setSwerveRequest(new SwerveRequest.Idle()); - } - })); - controller.rightBumper().and(controller.x()).whileTrue( - Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kForward) - .finallyDo(( - boolean interrupted) -> { - if (interrupted) { - Drive.getInstance().setSwerveRequest(new SwerveRequest.Idle()); - } - })); - controller.rightBumper().and(controller.y()).whileTrue( - Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kReverse) - .finallyDo(( - boolean interrupted) -> { - if (interrupted) { - Drive.getInstance().setSwerveRequest(new SwerveRequest.Idle()); - } - })); + controller.leftBumper().and(controller.x()) + .whileTrue( + new ProxyCommand( + ()->Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kForward) + .finallyDo(interrupted->Drive.getInstance().getCtreDrive().setControl(new SwerveRequest.Idle())) + ) + ); + controller.leftBumper().and(controller.y()) + .whileTrue( + new ProxyCommand( + ()->Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kReverse) + .finallyDo(interrupted->Drive.getInstance().getCtreDrive().setControl(new SwerveRequest.Idle())) + ) + ); + controller.rightBumper().and(controller.x()) + .whileTrue( + new ProxyCommand( + ()->Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kForward) + .finallyDo(interrupted->Drive.getInstance().getCtreDrive().setControl(new SwerveRequest.Idle())) + ) + ); + controller.rightBumper().and(controller.y()) + .whileTrue( + new ProxyCommand( + ()->Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kReverse) + .finallyDo(interrupted->Drive.getInstance().getCtreDrive().setControl(new SwerveRequest.Idle())) + ) + ); } + } \ No newline at end of file From 986c106dadfc41918ea683820e1a80283efc8a18 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Wed, 25 Feb 2026 19:01:37 -0800 Subject: [PATCH 097/118] leds --- src/main/java/frc/robot/Robot.java | 32 +-- .../java/frc/robot/subsystems/led/Led.java | 200 ++++++++++++++++++ .../robot/subsystems/led/LedConstants.java | 9 + 3 files changed, 228 insertions(+), 13 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/led/Led.java create mode 100644 src/main/java/frc/robot/subsystems/led/LedConstants.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0d835f9..0099662 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -26,6 +26,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; @@ -37,6 +38,7 @@ import frc.robot.subsystems.drive.*; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.led.Led; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.vision.VisionDeviceManager; @@ -113,20 +115,24 @@ public Robot() { RobotController.setTimeSource(RobotController::getFPGATime); } - VisionDeviceManager.getInstance(); + // VisionDeviceManager.getInstance(); - Drive.getInstance(); - Shooter.getLeftInstance(); - Shooter.getRightInstance(); - Indexer.getLeftInstance(); - Indexer.getRightInstance(); - Intake.getInstance(); + // Drive.getInstance(); + // Shooter.getLeftInstance(); + // Shooter.getRightInstance(); + // Indexer.getLeftInstance(); + // Indexer.getRightInstance(); + // Intake.getInstance(); // Climb.getInstance(); - TelemetryManager.getInstance(); - commandScheduler.schedule(FollowPathCommand.warmupCommand()); - commandScheduler.schedule(VisionDeviceManager.getInstance().bootUp()); - autoChooser = new AutoSelector(); + // TelemetryManager.getInstance(); + // commandScheduler.schedule(FollowPathCommand.warmupCommand()); + // commandScheduler.schedule(VisionDeviceManager.getInstance().bootUp()); + // autoChooser = new AutoSelector(); + + Led.getInstance(); + commandScheduler.schedule( + Led.getInstance().setRainbowCommand()); DriverStation.startDataLog(DataLogManager.getLog()); } @@ -188,7 +194,7 @@ public void autonomousExit() { @Override public void teleopInit() { - ControlsMapping.mapTeleopCommand(); + // ControlsMapping.mapTeleopCommand(); // This makes sure that the autonomous stops running when teleop starts running. if (autoCommand != null) { autoCommand.cancel(); @@ -206,7 +212,7 @@ public void testInit() { CommandScheduler.getInstance().cancelAll(); //map test commands - ControlsMapping.mapSysId(); + // ControlsMapping.mapSysId(); } /** This function is called periodically during test mode. */ diff --git a/src/main/java/frc/robot/subsystems/led/Led.java b/src/main/java/frc/robot/subsystems/led/Led.java new file mode 100644 index 0000000..aca4f3c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/led/Led.java @@ -0,0 +1,200 @@ +package frc.robot.subsystems.led; + +import java.util.Arrays; +import java.util.function.BiFunction; +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.AddressableLEDSim; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; + +public class Led extends SubsystemBase { + private static Led ledInstance; + public static Led getInstance() { + if (ledInstance == null) { + ledInstance = new Led(); + } + return ledInstance; + } + + /** A helper to optimize LEDs state */ + private static class Colorer { + /** The state of the LEDs */ + private static enum State { + SOLID, + PATTERN, + TIMED_PATTERN; + } + + private final Object lock = new Object(); + private State state = State.SOLID; + private Color solidColor = Color.kGreen; + private Color[] pattern; + private BiFunction timedPatternSupplier; + private Color[] colors = new Color[LedConstants.LED_LENGTH]; + private boolean hasUpdated = true; + + public void setSolidColor(Color color) { + synchronized (lock) { + hasUpdated = true; + solidColor = color; + state = State.SOLID; + } + } + + public void setPattern(Color[] colors) { + synchronized (lock) { + hasUpdated = true; + pattern = Arrays.stream(colors) + .map((Color c) -> new Color(c.red, c.green, c.blue)) + .toArray(Color[]::new); + state = State.PATTERN; + } + } + + public void setTimedPattern(BiFunction timedPatternSupplier) { + synchronized (lock) { + hasUpdated = true; + this.timedPatternSupplier = timedPatternSupplier; + state = State.TIMED_PATTERN; + } + } + + /** Gets the colors based on the state */ + public Color[] get(double time) { + synchronized (lock) { + switch (state) { + case SOLID: + if (hasUpdated) { + Arrays.fill(colors, solidColor); + } + break; + case PATTERN: + if (hasUpdated) { + colors = pattern; + } + break; + case TIMED_PATTERN: + for (int i = 0; i < colors.length; i++) { + colors[i] = timedPatternSupplier.apply(i, time); + } + break; + default: + break; + }; + hasUpdated = false; + return colors; + } + } + } + + private final Notifier ledNotifier; + private final AddressableLED led; + private final AddressableLEDBuffer ledBuffer; + private final Timer timer; + private final Colorer colorer; + + public Led() { + ledNotifier = new Notifier(this::update); + timer = new Timer(); + led = new AddressableLED(LedConstants.LED_PORT); + ledBuffer = new AddressableLEDBuffer(LedConstants.LED_LENGTH); + led.setLength(ledBuffer.getLength()); + led.start(); + timer.start(); + colorer = new Colorer(); + if (Robot.isSimulation()) { + new AddressableLEDSim(led); + } + ledNotifier.startPeriodic(LedConstants.UPDATE_DT); + } + + /** Updates the LEDs */ + private void update() { + if (colorer.state != Colorer.State.TIMED_PATTERN) { + if (colorer.hasUpdated) { + var colors = colorer.get(timer.get()); + for(int i = LedConstants.LED_START; i < LedConstants.LED_LENGTH; i++) { + ledBuffer.setLED(i, colors[i]); + } + led.setData(ledBuffer); + } + } else { + var colors = colorer.get(timer.get()); + for(int i = LedConstants.LED_START; i < LedConstants.LED_LENGTH; i++) { + ledBuffer.setLED(i, colors[i]); + } + led.setData(ledBuffer); + } + } + + /** Sets the LEDs to a color indefinitely */ + public Command setSolidColorCommand(Color color) { + return setSolidColorCommand(color, Double.POSITIVE_INFINITY); + } + + /** Sets the LEDs to a color for a set time */ + public Command setSolidColorCommand(Color color, double holdTime) { + return runOnce(() -> colorer.setSolidColor(color)) + .andThen( + Commands.waitSeconds(holdTime) + ).ignoringDisable(true) + .withName(color.toString() + ": Solid Color"); + } + + /** Animates the LEDs with a rainbow animation */ + public Command setRainbowCommand() { + return setRainbowCommand(Double.POSITIVE_INFINITY); + } + + /** Sets the LEDs to an animated rainbow for a set time */ + public Command setRainbowCommand(double holdTime) { + return runOnce(() -> colorer.setTimedPattern( + (Integer index, Double time) -> + Color.fromHSV(index * 5 + (int) (time * 50), 255, 255) + )).andThen( + Commands.waitSeconds(holdTime) + ).ignoringDisable(true).withName("Rainbow"); + } + + /** Holds the LEDs at a random state */ + public Command setRandomCommand() { + return setRandomCommand(Double.POSITIVE_INFINITY); + } + + /** Sets the LEDs to a random state for a set time */ + public Command setRandomCommand(double holdTime) { + return defer(() -> { + Color[] colors = new Color[LedConstants.LED_LENGTH]; + for (int i = 0; i < colors.length; i++) { + colors[i] = new Color(Math.random(), Math.random(), Math.random()); + } + return runOnce( + () -> colorer.setPattern( + colors)); + } + ).andThen( + Commands.waitSeconds(holdTime) + ).ignoringDisable(true).withName("Random"); + } + + /** Blinks the LEDs indefinitely */ + public Command blinkCommand(Color color1, Color color2, double delta) { + return blinkCommand(color1, color2, delta, Double.POSITIVE_INFINITY); + } + + /** Blinks the LEDs for a set time */ + public Command blinkCommand(Color color1, Color color2, double delta, double holdTime) { + return Commands.repeatingSequence( + setSolidColorCommand(color1, delta), + setSolidColorCommand(color2, delta) + ).raceWith( + Commands.waitSeconds(holdTime) + ).withName(color1.toString() + ":" + color2.toString() + ":" + (int) (delta * 1000) + " ms Blink"); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/led/LedConstants.java b/src/main/java/frc/robot/subsystems/led/LedConstants.java new file mode 100644 index 0000000..e9793ac --- /dev/null +++ b/src/main/java/frc/robot/subsystems/led/LedConstants.java @@ -0,0 +1,9 @@ +package frc.robot.subsystems.led; + +public class LedConstants { + public static final int LED_START = 0; + public static final int LED_LENGTH = 19; + public static final int LED_LENGTH_2 = 17; + public static final int LED_PORT = 0; + public static final double UPDATE_DT = 0.06; +} \ No newline at end of file From 8bbf7e7efdafca6ad60f7c482e04866db018194c Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Sat, 28 Feb 2026 17:22:09 -0800 Subject: [PATCH 098/118] at least half the stuff works --- simgui-ds.json | 4 +- src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/Constants.java | 312 ++++++++++++++++++ src/main/java/frc/robot/ControlsMapping.java | 31 +- src/main/java/frc/robot/Robot.java | 37 ++- .../java/frc/robot/auto/AutoRoutines.java | 4 +- .../java/frc/robot/auto/AutoSelector.java | 2 +- .../frc/robot/lib/houndlib/BallPhysics.java | 25 +- .../lib/houndlib/ShootOnTheFlyCalculator.java | 47 +-- .../frc/robot/subsystems/drive/Drive.java | 25 +- .../subsystems/drive/DriveConstants.java | 22 -- .../frc/robot/subsystems/intake/Intake.java | 7 +- .../subsystems/intake/IntakeConstants.java | 9 +- .../subsystems/roller/RollerConstants.java | 4 +- .../frc/robot/subsystems/shooter/Shooter.java | 23 +- .../subsystems/shooter/ShooterConstants.java | 8 +- .../subsystems/shooter/ShotCalculator.java | 41 +-- .../subsystems/vision/VisionConstants.java | 2 +- .../robot/subsystems/vision/VisionDevice.java | 4 + .../vision/VisionDeviceManager.java | 4 +- 20 files changed, 489 insertions(+), 132 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 543cd0e..aaec342 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -21,7 +21,9 @@ }, { "decKey": 51, - "incKey": 52 + "decayRate": 1.0, + "incKey": 52, + "keyRate": 1.0 }, { "decKey": 74, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index ae0e97e..8f8d2c8 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Robot2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 163; - public static final String GIT_SHA = "4cef7ab6228497f5481ccd8c3f953114b793bc02"; - public static final String GIT_DATE = "2026-02-25 21:25:09 EST"; + public static final int GIT_REVISION = 166; + public static final String GIT_SHA = "81cd114328c916e94e7701a2cf77ec819cffbd8d"; + public static final String GIT_DATE = "2026-02-25 22:02:00 EST"; public static final String GIT_BRANCH = "sotm"; - public static final String BUILD_DATE = "2026-02-25 21:47:17 EST"; - public static final long BUILD_UNIX_TIME = 1772074037013L; + public static final String BUILD_DATE = "2026-02-28 20:11:56 EST"; + public static final long BUILD_UNIX_TIME = 1772327516533L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f9fac0a..445fa02 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -3,7 +3,12 @@ import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.path.PathConstraints; +import edu.wpi.first.math.geometry.*; + import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.robot.lib.field.FieldLayout; +import frc.robot.lib.field.FieldUtil; import frc.robot.subsystems.drive.DriveConstants; /** @@ -41,4 +46,311 @@ public static final class Pathplanner { DriveConstants.MAX_ROTATION_ACCEL * 0.85); public static final double GENERATION_WAIT_TIME = 5; } + + // Copyright (c) 2025-2026 Littleton Robotics + // http://github.com/Mechanical-Advantage + // + // Use of this source code is governed by an MIT-style + // license that can be found in the LICENSE file at + // the root directory of this project. + /** + * Contains information for location of field element and other useful reference points. + * + *

NOTE: All constants are defined relative to the field coordinate system, and from the + * perspective of the blue alliance station + */ + public static class FieldConstants { + public static Translation3d allianceCorrected(Translation3d t) { + Alliance a = DriverStation.getAlliance().orElseThrow(); + if (a == Alliance.Red) { + var t2d = + FieldUtil.flipTranslation(t.toTranslation2d()); + return new Translation3d(t2d.getX(), t2d.getY(), t.getZ()); + } else { + return t; + } + } + + // AprilTag related constants + public static final int aprilTagCount = FieldLayout.APRILTAG_MAP.getTags().size(); + public static final double aprilTagWidth = edu.wpi.first.math.util.Units.inchesToMeters(6.5); + + // Field dimensions + public static final double fieldLength = FieldLayout.APRILTAG_MAP.getFieldLength(); + public static final double fieldWidth = FieldLayout.APRILTAG_MAP.getFieldWidth(); + + // Fuel dimensions + public static final double fuelDiameter = edu.wpi.first.math.util.Units.inchesToMeters(5.91); + + /** + * Officially defined and relevant vertical lines found on the field (defined by X-axis offset) + */ + public static class LinesVertical { + public static final double center = fieldLength / 2.0; + public static final double starting = + FieldLayout.APRILTAG_MAP.getTagPose(26).get().getX(); + public static final double allianceZone = starting; + public static final double hubCenter = + FieldLayout.APRILTAG_MAP.getTagPose(26).get().getX() + Hub.width / 2.0; + public static final double neutralZoneNear = center - edu.wpi.first.math.util.Units.inchesToMeters(120); + public static final double neutralZoneFar = center + edu.wpi.first.math.util.Units.inchesToMeters(120); + public static final double oppHubCenter = + FieldLayout.APRILTAG_MAP.getTagPose(4).get().getX() + Hub.width / 2.0; + public static final double oppAllianceZone = + FieldLayout.APRILTAG_MAP.getTagPose(10).get().getX(); + } + + /** + * Officially defined and relevant horizontal lines found on the field (defined by Y-axis offset) + * + *

NOTE: The field element start and end are always left to right from the perspective of the + * alliance station + */ + public static class LinesHorizontal { + + public static final double center = fieldWidth / 2.0; + + // Right of hub + public static final double rightBumpStart = Hub.nearRightCorner.getY(); + public static final double rightBumpEnd = rightBumpStart - RightBump.width; + public static final double rightBumpMiddle = (rightBumpStart + rightBumpEnd) / 2.0; + public static final double rightTrenchOpenStart = rightBumpEnd - edu.wpi.first.math.util.Units.inchesToMeters(12.0); + public static final double rightTrenchOpenEnd = 0; + + // Left of hub + public static final double leftBumpEnd = Hub.nearLeftCorner.getY(); + public static final double leftBumpStart = leftBumpEnd + LeftBump.width; + public static final double leftBumpMiddle = (leftBumpStart + leftBumpEnd) / 2.0; + public static final double leftTrenchOpenEnd = leftBumpStart + edu.wpi.first.math.util.Units.inchesToMeters(12.0); + public static final double leftTrenchOpenStart = fieldWidth; + } + + /** Hub related constants */ + public static class Hub { + // Dimensions + public static final double width = edu.wpi.first.math.util.Units.inchesToMeters(47.0); + public static final double height = + edu.wpi.first.math.util.Units.inchesToMeters(72.0); // includes the catcher at the top + public static final double innerWidth = edu.wpi.first.math.util.Units.inchesToMeters(41.7); + public static final double innerHeight = edu.wpi.first.math.util.Units.inchesToMeters(56.5); + + // Relevant reference points on alliance side + public static final Translation3d topCenterPoint = + new Translation3d( + FieldLayout.APRILTAG_MAP.getTagPose(26).get().getX() + width / 2.0, + fieldWidth / 2.0, + height); + public static final Translation3d innerCenterPoint = + new Translation3d( + FieldLayout.APRILTAG_MAP.getTagPose(26).get().getX() + width / 2.0, + fieldWidth / 2.0, + innerHeight); + + public static final Translation2d nearLeftCorner = + new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d nearRightCorner = + new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0); + public static final Translation2d farLeftCorner = + new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d farRightCorner = + new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0); + + // Relevant reference points on the opposite side + public static final Translation3d oppTopCenterPoint = + new Translation3d( + FieldLayout.APRILTAG_MAP.getTagPose(4).get().getX() + width / 2.0, + fieldWidth / 2.0, + height); + public static final Translation2d oppNearLeftCorner = + new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d oppNearRightCorner = + new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0); + public static final Translation2d oppFarLeftCorner = + new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d oppFarRightCorner = + new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0); + + // Hub faces + public static final Pose2d nearFace = + FieldLayout.APRILTAG_MAP.getTagPose(26).get().toPose2d(); + public static final Pose2d farFace = + FieldLayout.APRILTAG_MAP.getTagPose(20).get().toPose2d(); + public static final Pose2d rightFace = + FieldLayout.APRILTAG_MAP.getTagPose(18).get().toPose2d(); + public static final Pose2d leftFace = + FieldLayout.APRILTAG_MAP.getTagPose(21).get().toPose2d(); + } + + /** Left Bump related constants */ + public static class LeftBump { + + // Dimensions + public static final double width = edu.wpi.first.math.util.Units.inchesToMeters(73.0); + public static final double height = edu.wpi.first.math.util.Units.inchesToMeters(6.513); + public static final double depth = edu.wpi.first.math.util.Units.inchesToMeters(44.4); + + // Relevant reference points on alliance side + public static final Translation2d nearLeftCorner = + new Translation2d(LinesVertical.hubCenter - width / 2, edu.wpi.first.math.util.Units.inchesToMeters(255)); + public static final Translation2d nearRightCorner = Hub.nearLeftCorner; + public static final Translation2d farLeftCorner = + new Translation2d(LinesVertical.hubCenter + width / 2, edu.wpi.first.math.util.Units.inchesToMeters(255)); + public static final Translation2d farRightCorner = Hub.farLeftCorner; + + // Relevant reference points on opposing side + public static final Translation2d oppNearLeftCorner = + new Translation2d(LinesVertical.hubCenter - width / 2, edu.wpi.first.math.util.Units.inchesToMeters(255)); + public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner; + public static final Translation2d oppFarLeftCorner = + new Translation2d(LinesVertical.hubCenter + width / 2, edu.wpi.first.math.util.Units.inchesToMeters(255)); + public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner; + } + + /** Right Bump related constants */ + public static class RightBump { + // Dimensions + public static final double width = edu.wpi.first.math.util.Units.inchesToMeters(73.0); + public static final double height = edu.wpi.first.math.util.Units.inchesToMeters(6.513); + public static final double depth = edu.wpi.first.math.util.Units.inchesToMeters(44.4); + + // Relevant reference points on alliance side + public static final Translation2d nearLeftCorner = + new Translation2d(LinesVertical.hubCenter + width / 2, edu.wpi.first.math.util.Units.inchesToMeters(255)); + public static final Translation2d nearRightCorner = Hub.nearLeftCorner; + public static final Translation2d farLeftCorner = + new Translation2d(LinesVertical.hubCenter - width / 2, edu.wpi.first.math.util.Units.inchesToMeters(255)); + public static final Translation2d farRightCorner = Hub.farLeftCorner; + + // Relevant reference points on opposing side + public static final Translation2d oppNearLeftCorner = + new Translation2d(LinesVertical.hubCenter + width / 2, edu.wpi.first.math.util.Units.inchesToMeters(255)); + public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner; + public static final Translation2d oppFarLeftCorner = + new Translation2d(LinesVertical.hubCenter - width / 2, edu.wpi.first.math.util.Units.inchesToMeters(255)); + public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner; + } + + /** Left Trench related constants */ + public static class LeftTrench { + // Dimensions + public static final double width = edu.wpi.first.math.util.Units.inchesToMeters(65.65); + public static final double depth = edu.wpi.first.math.util.Units.inchesToMeters(47.0); + public static final double height = edu.wpi.first.math.util.Units.inchesToMeters(40.25); + public static final double openingWidth = edu.wpi.first.math.util.Units.inchesToMeters(50.34); + public static final double openingHeight = edu.wpi.first.math.util.Units.inchesToMeters(22.25); + + // Relevant reference points on alliance side + public static final Translation3d openingTopLeft = + new Translation3d(LinesVertical.hubCenter, fieldWidth, openingHeight); + public static final Translation3d openingTopRight = + new Translation3d(LinesVertical.hubCenter, fieldWidth - openingWidth, openingHeight); + + // Relevant reference points on opposing side + public static final Translation3d oppOpeningTopLeft = + new Translation3d(LinesVertical.oppHubCenter, fieldWidth, openingHeight); + public static final Translation3d oppOpeningTopRight = + new Translation3d(LinesVertical.oppHubCenter, fieldWidth - openingWidth, openingHeight); + } + + public static class RightTrench { + + // Dimensions + public static final double width = edu.wpi.first.math.util.Units.inchesToMeters(65.65); + public static final double depth = edu.wpi.first.math.util.Units.inchesToMeters(47.0); + public static final double height = edu.wpi.first.math.util.Units.inchesToMeters(40.25); + public static final double openingWidth = edu.wpi.first.math.util.Units.inchesToMeters(50.34); + public static final double openingHeight = edu.wpi.first.math.util.Units.inchesToMeters(22.25); + + // Relevant reference points on alliance side + public static final Translation3d openingTopLeft = + new Translation3d(LinesVertical.hubCenter, openingWidth, openingHeight); + public static final Translation3d openingTopRight = + new Translation3d(LinesVertical.hubCenter, 0, openingHeight); + + // Relevant reference points on opposing side + public static final Translation3d oppOpeningTopLeft = + new Translation3d(LinesVertical.oppHubCenter, openingWidth, openingHeight); + public static final Translation3d oppOpeningTopRight = + new Translation3d(LinesVertical.oppHubCenter, 0, openingHeight); + } + + /** Tower related constants */ + public static class Tower { + // Dimensions + public static final double width = edu.wpi.first.math.util.Units.inchesToMeters(49.25); + public static final double depth = edu.wpi.first.math.util.Units.inchesToMeters(45.0); + public static final double height = edu.wpi.first.math.util.Units.inchesToMeters(78.25); + public static final double innerOpeningWidth = edu.wpi.first.math.util.Units.inchesToMeters(32.250); + public static final double frontFaceX = edu.wpi.first.math.util.Units.inchesToMeters(43.51); + + public static final double uprightHeight = edu.wpi.first.math.util.Units.inchesToMeters(72.1); + + // Rung heights from the floor + public static final double lowRungHeight = edu.wpi.first.math.util.Units.inchesToMeters(27.0); + public static final double midRungHeight = edu.wpi.first.math.util.Units.inchesToMeters(45.0); + public static final double highRungHeight = edu.wpi.first.math.util.Units.inchesToMeters(63.0); + + // Relevant reference points on alliance side + public static final Translation2d centerPoint = + new Translation2d( + frontFaceX, FieldLayout.APRILTAG_MAP.getTagPose(31).get().getY()); + public static final Translation2d leftUpright = + new Translation2d( + frontFaceX, + (FieldLayout.APRILTAG_MAP.getTagPose(31).get().getY()) + + innerOpeningWidth / 2 + + edu.wpi.first.math.util.Units.inchesToMeters(0.75)); + public static final Translation2d rightUpright = + new Translation2d( + frontFaceX, + (FieldLayout.APRILTAG_MAP.getTagPose(31).get().getY()) + - innerOpeningWidth / 2 + - edu.wpi.first.math.util.Units.inchesToMeters(0.75)); + + // Relevant reference points on opposing side + public static final Translation2d oppCenterPoint = + new Translation2d( + fieldLength - frontFaceX, + FieldLayout.APRILTAG_MAP.getTagPose(15).get().getY()); + public static final Translation2d oppLeftUpright = + new Translation2d( + fieldLength - frontFaceX, + (FieldLayout.APRILTAG_MAP.getTagPose(15).get().getY()) + + innerOpeningWidth / 2 + + edu.wpi.first.math.util.Units.inchesToMeters(0.75)); + public static final Translation2d oppRightUpright = + new Translation2d( + fieldLength - frontFaceX, + (FieldLayout.APRILTAG_MAP.getTagPose(15).get().getY()) + - innerOpeningWidth / 2 + - edu.wpi.first.math.util.Units.inchesToMeters(0.75)); + } + + public static class Depot { + // Dimensions + public static final double width = edu.wpi.first.math.util.Units.inchesToMeters(42.0); + public static final double depth = edu.wpi.first.math.util.Units.inchesToMeters(27.0); + public static final double height = edu.wpi.first.math.util.Units.inchesToMeters(1.125); + public static final double distanceFromCenterY = edu.wpi.first.math.util.Units.inchesToMeters(75.93); + + // Relevant reference points on alliance side + public static final Translation3d depotCenter = + new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY, height); + public static final Translation3d leftCorner = + new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY + (width / 2), height); + public static final Translation3d rightCorner = + new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY - (width / 2), height); + } + + public static class Outpost { + // Dimensions + public static final double width = edu.wpi.first.math.util.Units.inchesToMeters(31.8); + public static final double openingDistanceFromFloor = edu.wpi.first.math.util.Units.inchesToMeters(28.1); + public static final double height = edu.wpi.first.math.util.Units.inchesToMeters(7.0); + + // Relevant reference points on alliance side + public static final Translation2d centerPoint = + new Translation2d(0, FieldLayout.APRILTAG_MAP.getTagPose(29).get().getY()); + } + } } diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index a0f8b0c..0fbe1cf 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -10,6 +10,7 @@ import edu.wpi.first.math.geometry.*; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.lib.field.FieldUtil; import frc.robot.subsystems.climb.Climb; @@ -30,12 +31,15 @@ public class ControlsMapping { public static void mapTeleopCommand() { + Drive.getInstance().setDefaultCommand((Drive.getInstance().openLoopControl())); // Intake.getInstance().setDefaultCommand(Intake.getInstance().intake()); - controller.back().onTrue(Drive.getInstance().resetPoseCommand(new + controller.back().and(controller.a()).onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); - controller.y().onTrue(VisionDeviceManager.getInstance().bootUp()); + controller.back().and(controller.b()).onTrue(VisionDeviceManager.getInstance().bootUp()); + controller.back().and(controller.y()).onTrue( + Commands.runOnce(() -> Drive.getInstance().getCtreDrive().getPigeon2().reset())); // controller.leftTrigger().debounce(0.1).whileTrue(Intake.getInstance().outtake()); // controller.b().whileTrue(Intake.getInstance().outtake()); @@ -65,10 +69,16 @@ public static void mapTeleopCommand() { // )).andThen(Commands.waitSeconds(0.1)) // ) // ); - controller.a().whileTrue( + controller.rightBumper().whileTrue( Commands.parallel( - Shooter.getRightInstance().shoot(60, 60), - Shooter.getLeftInstance().shoot(60, 60) + Shooter.getRightInstance().shoot( + () -> SmartDashboard.getNumber("topVel", 30), + () -> SmartDashboard.getNumber("botVel", 30) + ), + Shooter.getLeftInstance().shoot( + () -> SmartDashboard.getNumber("topVel", 30), + () -> SmartDashboard.getNumber("botVel", 30) + ) ) ).onFalse( Commands.parallel( @@ -77,7 +87,7 @@ public static void mapTeleopCommand() { ) ); - controller.x().whileTrue( + controller.leftBumper().whileTrue( Commands.parallel( Indexer.getRightInstance().activateIndexer(), Indexer.getLeftInstance().activateIndexer(), @@ -91,12 +101,13 @@ public static void mapTeleopCommand() { ) ); - - controller.b().whileTrue( - Intake.getInstance().intake()).onFalse(Intake.getInstance().stow()); + controller.leftTrigger().whileTrue( + Intake.getInstance().intake()).onFalse(Intake.getInstance().stopWheel()); // controller.y().onTrue(Intake.getInstance().) - controller.leftBumper().whileTrue(Drive.getInstance().headingLockToHub()); + controller.rightTrigger().whileTrue(Drive.getInstance().headingLockToHub()); // Shooter.getRightInstance().shoot())); + controller.povDown().onTrue(Intake.getInstance().calibrateZero()); + controller.povUp().onTrue(Intake.getInstance().stow()); } public static void mapSysId() { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0099662..b782e2b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -39,6 +39,7 @@ import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.led.Led; +import frc.robot.subsystems.roller.Roller; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.vision.VisionDeviceManager; @@ -110,25 +111,29 @@ public Robot() { Logger.addDataReceiver(new WPILOGWriter()); } - // Logger.start(); + Logger.start(); if (!Logger.hasReplaySource()) { RobotController.setTimeSource(RobotController::getFPGATime); } - // VisionDeviceManager.getInstance(); + VisionDeviceManager.getInstance(); - // Drive.getInstance(); - // Shooter.getLeftInstance(); - // Shooter.getRightInstance(); - // Indexer.getLeftInstance(); - // Indexer.getRightInstance(); + Drive.getInstance(); + Shooter.getLeftInstance(); + Shooter.getRightInstance(); + Indexer.getLeftInstance(); + Indexer.getRightInstance(); + Roller.getInstance(); // Intake.getInstance(); // Climb.getInstance(); - // TelemetryManager.getInstance(); - // commandScheduler.schedule(FollowPathCommand.warmupCommand()); - // commandScheduler.schedule(VisionDeviceManager.getInstance().bootUp()); - // autoChooser = new AutoSelector(); + SmartDashboard.putNumber("botVel", 30); + SmartDashboard.putNumber("topVel", 30); + + TelemetryManager.getInstance(); + commandScheduler.schedule(FollowPathCommand.warmupCommand()); + commandScheduler.schedule(VisionDeviceManager.getInstance().bootUp()); + autoChooser = new AutoSelector(); Led.getInstance(); commandScheduler.schedule( @@ -163,6 +168,8 @@ public void robotPeriodic() { /** This function is called once each time the robot enters Disabled mode. */ @Override public void disabledInit() { + commandScheduler.schedule( + Led.getInstance().setSolidColorCommand(Color.kGreen)); } /** This function is called periodically during disabled. */ @@ -174,6 +181,8 @@ public void disabledPeriodic() { /** This autonomous runs the autonomous command selected. */ @Override public void autonomousInit() { + commandScheduler.schedule( + Led.getInstance().setSolidColorCommand(Color.kBlue)); autoCommand = autoChooser.getAuto(); if (autoCommand != null) { commandScheduler.schedule(autoCommand); @@ -194,7 +203,9 @@ public void autonomousExit() { @Override public void teleopInit() { - // ControlsMapping.mapTeleopCommand(); + commandScheduler.schedule( + Led.getInstance().setSolidColorCommand(Color.kRed)); + ControlsMapping.mapTeleopCommand(); // This makes sure that the autonomous stops running when teleop starts running. if (autoCommand != null) { autoCommand.cancel(); @@ -208,6 +219,8 @@ public void teleopPeriodic() { @Override public void testInit() { + commandScheduler.schedule( + Led.getInstance().setSolidColorCommand(Color.kGray)); // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); diff --git a/src/main/java/frc/robot/auto/AutoRoutines.java b/src/main/java/frc/robot/auto/AutoRoutines.java index fcdc417..d4688d2 100644 --- a/src/main/java/frc/robot/auto/AutoRoutines.java +++ b/src/main/java/frc/robot/auto/AutoRoutines.java @@ -11,13 +11,13 @@ import edu.wpi.first.wpilibj2.command.Command; public final class AutoRoutines { - @Auto(name = "Pid Test") + // @Auto(name = "Pid Test") public static Command testPidToPose() { return new PIDToPoseCommand( new Pose2d(2.0, 1.0, Rotation2d.fromDegrees(120))); } - @Auto(name = "Trajectory Test") + // @Auto(name = "Trajectory Test") public static Command testTrajectoryAuto() { RedTrajectory traj = TrajectoryLoader.loadAutoTrajectory( TrajectoryType.CHOREO, diff --git a/src/main/java/frc/robot/auto/AutoSelector.java b/src/main/java/frc/robot/auto/AutoSelector.java index 4cb4551..c98705c 100644 --- a/src/main/java/frc/robot/auto/AutoSelector.java +++ b/src/main/java/frc/robot/auto/AutoSelector.java @@ -51,7 +51,7 @@ public AutoSelector() { } chooser.setDefaultOption("None", () -> null); - SmartDashboard.putData(chooser); + SmartDashboard.putData("Auto Selector", chooser); } /** Gets the auto selected from the SmartDashboard */ diff --git a/src/main/java/frc/robot/lib/houndlib/BallPhysics.java b/src/main/java/frc/robot/lib/houndlib/BallPhysics.java index 5f3b746..dce904e 100644 --- a/src/main/java/frc/robot/lib/houndlib/BallPhysics.java +++ b/src/main/java/frc/robot/lib/houndlib/BallPhysics.java @@ -1,6 +1,7 @@ package frc.robot.lib.houndlib; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; @@ -84,12 +85,12 @@ public static void step( } public static ShotSolution solveBallisticWithIncomingAngle( - Pose3d shooterPose, - Pose3d targetPose, + Translation3d shooterPose, + Translation3d targetPose, double incomingPitchRad) { - Translation3d s = shooterPose.getTranslation(); - Translation3d t = targetPose.getTranslation(); + Translation3d s = shooterPose; + Translation3d t = targetPose; double dx = t.getX() - s.getX(); double dy = t.getY() - s.getY(); @@ -120,12 +121,12 @@ public static ShotSolution solveBallisticWithIncomingAngle( } public static ShotSolution solveBallisticWithSpeed( - Pose3d shooterPose, - Pose3d targetPose, + Translation3d shooterPose, + Translation3d targetPose, double launchSpeed) { - Translation3d s = shooterPose.getTranslation(); - Translation3d t = targetPose.getTranslation(); + Translation3d s = shooterPose; + Translation3d t = targetPose; double dx = t.getX() - s.getX(); double dy = t.getY() - s.getY(); @@ -156,11 +157,11 @@ public static ShotSolution solveBallisticWithSpeed( } public static double minSpeedForAnyArc( - Pose3d shooterPose, - Pose3d targetPose) { + Translation3d shooterPose, + Translation3d targetPose) { - Translation3d s = shooterPose.getTranslation(); - Translation3d t = targetPose.getTranslation(); + Translation3d s = shooterPose; + Translation3d t = targetPose; double dx = t.getX() - s.getX(); double dy = t.getY() - s.getY(); diff --git a/src/main/java/frc/robot/lib/houndlib/ShootOnTheFlyCalculator.java b/src/main/java/frc/robot/lib/houndlib/ShootOnTheFlyCalculator.java index ba4fdfe..6afa352 100644 --- a/src/main/java/frc/robot/lib/houndlib/ShootOnTheFlyCalculator.java +++ b/src/main/java/frc/robot/lib/houndlib/ShootOnTheFlyCalculator.java @@ -6,6 +6,8 @@ import frc.robot.lib.trajectory.RedTrajectory.State.ChassisAccels; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; @@ -23,7 +25,7 @@ public class ShootOnTheFlyCalculator { * projectile's velocity. This allows you to have a shooter that may shoot a * projectile at varying speeds given varying distances from the targe. * - * @see #calculateEffectiveTargetLocation(Pose2d, Pose3d, ChassisSpeeds, + * @see #calculateEffectiveTargetLocation(Pose2d, Translation3d, ChassisSpeeds, * ChassisAccelerations, Function, double, double) * * @param robotPose the current pose of the robot @@ -41,9 +43,18 @@ public class ShootOnTheFlyCalculator { * projectile in m/s. * @return the time it will take for the projectile to reach the target */ - public static double getTimeToShoot(Pose2d robotPose, Pose3d targetPose, + public static double getTimeToShoot(Pose2d robotPose, Translation3d targetPose, Function xyDistanceToProjectileVelocity) { - Transform3d diff = new Pose3d(robotPose).minus(targetPose); + Transform3d diff = + new Pose3d( + new Translation3d( + robotPose.getTranslation() + ), + Rotation3d.kZero) + .minus( + new Pose3d( + targetPose, + Rotation3d.kZero)); double xyDistance = new Translation2d(diff.getX(), diff.getY()).getNorm(); double distance = diff.getTranslation().getNorm(); double projectileVelocity = xyDistanceToProjectileVelocity.apply(xyDistance); @@ -52,12 +63,12 @@ public static double getTimeToShoot(Pose2d robotPose, Pose3d targetPose, } public static double getTimeToShoot( - Pose3d shooterPose, - Pose3d targetPose, + Translation3d shooterPose, + Translation3d targetPose, double launchSpeed, double launchPitchRad) { - Translation3d s = shooterPose.getTranslation(); - Translation3d t = targetPose.getTranslation(); + Translation3d s = shooterPose; + Translation3d t = targetPose; double dx = t.getX() - s.getX(); double dy = t.getY() - s.getY(); @@ -158,8 +169,8 @@ public static double getTimeToShoot( * @param accelerationCompensationFactor the value to multiply the acceleration * @return */ - public static Pose3d calculateEffectiveTargetLocation( - Pose2d robotPose, Pose3d targetPose, + public static Translation3d calculateEffectiveTargetLocation( + Pose2d robotPose, Translation3d targetPose, ChassisSpeeds fieldRelRobotVelocity, ChassisAccels fieldRelRobotAcceleration, Function xyDistanceToProjectileVelocity, @@ -168,7 +179,7 @@ public static Pose3d calculateEffectiveTargetLocation( double shotTime = getTimeToShoot(robotPose, targetPose, xyDistanceToProjectileVelocity); - Pose3d correctedTargetPose = new Pose3d(); + Translation3d correctedTargetPose = new Translation3d(); for (int i = 0; i < goalPositionIterations; i++) { double virtualGoalX = targetPose.getX() - shotTime * (fieldRelRobotVelocity.vxMetersPerSecond @@ -179,8 +190,7 @@ public static Pose3d calculateEffectiveTargetLocation( + fieldRelRobotAcceleration.ay * accelerationCompensationFactor); - correctedTargetPose = new Pose3d(virtualGoalX, virtualGoalY, targetPose.getZ(), - targetPose.getRotation()); + correctedTargetPose = new Translation3d(virtualGoalX, virtualGoalY, targetPose.getZ()); double newShotTime = getTimeToShoot(robotPose, correctedTargetPose, xyDistanceToProjectileVelocity); @@ -194,7 +204,7 @@ public static Pose3d calculateEffectiveTargetLocation( } public record InterceptSolution( - Pose3d effectiveTargetPose, + Translation3d effectiveTargetPose, double launchPitchRad, double launchSpeed, double flightTime, @@ -202,8 +212,8 @@ public record InterceptSolution( } public static InterceptSolution solveShootOnTheFly( - Pose3d shooterPose, - Pose3d targetPose, + Translation3d shooterPose, + Translation3d targetPose, ChassisSpeeds fieldRelRobotVelocity, ChassisAccels fieldRelRobotAcceleration, double incomingAngle, @@ -216,7 +226,7 @@ public static InterceptSolution solveShootOnTheFly( incomingAngle); double t = sol.flightTimeSeconds(); - Pose3d effectiveTarget = targetPose; + Translation3d effectiveTarget = targetPose; for (int i = 0; i < maxIterations; i++) { @@ -226,11 +236,10 @@ public static InterceptSolution solveShootOnTheFly( double dy = fieldRelRobotVelocity.vyMetersPerSecond * t + 0.5 * fieldRelRobotAcceleration.ay * t * t; - effectiveTarget = new Pose3d( + effectiveTarget = new Translation3d( targetPose.getX() - dx, targetPose.getY() - dy, - targetPose.getZ(), - targetPose.getRotation()); + targetPose.getZ()); ShotSolution newSol = BallPhysics.solveBallisticWithIncomingAngle( shooterPose, diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 5327cc6..503bced 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -2,6 +2,7 @@ import static frc.robot.subsystems.drive.DriveConstants.*; +import java.lang.reflect.Field; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -17,7 +18,9 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; -// import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -108,6 +111,8 @@ public void outputTelemetry() { FieldLayout.field.setRobotPose(getPose()); io.updateInputs(driveRequest, lastReadState, getCurrentCommand(), getDefaultCommand()); io.process(); + + SmartDashboard.putNumber("To Shooter", FieldLayout.APRILTAG_MAP.getTagPose(10).get().toPose2d().getTranslation().getDistance(getPose().getTranslation())); } /** @@ -180,7 +185,7 @@ public Command openLoopControl() { * Locks the robot onto a pose. * Utilizes feedforwards derived from the current chassis speeds */ - public Command headingLockToPose(Pose2d pose) { + public Command headingLockToPose(Translation2d pose) { SwerveRequest.FieldCentric request = new SwerveRequest.FieldCentric(); @@ -210,7 +215,7 @@ public Command headingLockToPose(Pose2d pose) { double yFancy = xy[1]; var state = getState(); - var delta = pose.getTranslation().minus(getPose().getTranslation()); + var delta = pose.minus(getPose().getTranslation()); var targetDirection = delta.getAngle(); @@ -304,13 +309,13 @@ public Command headingLockToPose(Supplier pose) { */ public Command headingLockToHub() { return Commands.runOnce(() -> ShotCalculator.getInstance().setTarget( - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? - DriveConstants.FieldPoses.HUB.pose3d : - new Pose3d( - FieldUtil.flipPose(DriveConstants.FieldPoses.HUB.pose).getX(), - FieldUtil.flipPose(DriveConstants.FieldPoses.HUB.pose).getY(), - DriveConstants.FieldPoses.HUB.pose3d.getZ(), Rotation3d.kZero))) - .andThen(headingLockToPose(() -> ShotCalculator.getInstance().getCurrentEffectiveTargetPose().toPose2d())); + Constants.FieldConstants.allianceCorrected(Constants.FieldConstants.Hub.topCenterPoint))) + .andThen( + headingLockToPose( + Constants.FieldConstants + .allianceCorrected( + Constants.FieldConstants.Hub.topCenterPoint) + .toTranslation2d())); } /** diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 60cc6d8..85e3499 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -52,26 +52,4 @@ public final class DriveConstants { public static final double ACCELERATION_CONSTANT = 0.1; public static final double AUTO_ALIGN_TIMEOUT = 0.5; - - public static enum FieldPoses { - HUB( - new Pose3d( - Units.Inches.of(182.11), - Units.Inches.of(158.84), - Units.Inches.of(72), Rotation3d.kZero)), - TRENCH( - FieldLayout.APRILTAG_MAP.getTagPose(12).orElse(Pose3d.kZero)), - TAG( - FieldLayout.APRILTAG_MAP.getTagPose(12).orElse(Pose3d.kZero)); - public Pose2d pose; - public Pose3d pose3d; - private FieldPoses(Pose2d pose) { - this.pose = pose; - } - - private FieldPoses(Pose3d pose3d) { - this.pose = pose3d.toPose2d(); - this.pose3d = pose3d; - } - } } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 4b4fd06..5457ef5 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; @@ -189,7 +190,7 @@ public Command setBarUp() { public Command setBarPosition(double position) { double checkedPos = MathUtil.clamp(position, BAR_POS_MIN, BAR_POS_MAX); - var req = new PositionVoltage(checkedPos); + var req = new MotionMagicVoltage(checkedPos); return runOnce(() -> setRequestBar(req) ).withName("bar pos set" + (checkedPos)); @@ -218,14 +219,14 @@ public Command waitUntilBarIsAtPosition(double target) { * @return Command to run */ public Command calibrateZero() { - VoltageOut calibrationRequest = new VoltageOut(-1) + VoltageOut calibrationRequest = new VoltageOut(-2) .withIgnoreHardwareLimits(true) .withIgnoreSoftwareLimits(true); /** Trigger to detect when the elevator drives into a hard stop. */ Trigger isHardStop = new Trigger(() -> { return barMotor.getVelocity().getValue().abs(RotationsPerSecond) < 1 && - barMotor.getTorqueCurrent().getValue().abs(Amps) > 10; + barMotor.getTorqueCurrent().getValue().abs(Amps) > 25; }).debounce(0.1); return run(() -> { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 07cc2a9..9cd9922 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -5,6 +5,7 @@ import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -54,12 +55,16 @@ public static TalonFXConfiguration getBarConfig() { return new TalonFXConfiguration() .withSlot0(new Slot0Configs() .withKV(0.0) - .withKP(0.0) + .withKP(10.0) .withKI(0.0) .withKD(0.0) .withKG(0.0).withGravityType(GravityTypeValue.Arm_Cosine)) .withCurrentLimits(new CurrentLimitsConfigs() - .withStatorCurrentLimit(40)) + .withStatorCurrentLimit(60)) + .withMotionMagic(new MotionMagicConfigs() + .withMotionMagicAcceleration(2) // 1.0 m/s^2 + .withMotionMagicCruiseVelocity(3) + .withMotionMagicJerk(16)) .withVoltage(new VoltageConfigs() .withPeakForwardVoltage(12.0) .withPeakReverseVoltage(-12.0)) diff --git a/src/main/java/frc/robot/subsystems/roller/RollerConstants.java b/src/main/java/frc/robot/subsystems/roller/RollerConstants.java index ce7287c..71881e4 100644 --- a/src/main/java/frc/robot/subsystems/roller/RollerConstants.java +++ b/src/main/java/frc/robot/subsystems/roller/RollerConstants.java @@ -9,7 +9,7 @@ public class RollerConstants { public static final int MOTOR_ID = 33; - public static final double ROLL_SPEED = 20; + public static final double ROLL_SPEED = 15; public static TalonFXConfiguration getConfig() { return new TalonFXConfiguration() @@ -19,7 +19,7 @@ public static TalonFXConfiguration getConfig() { .withKI(0.0) .withKD(0.0)) .withCurrentLimits(new CurrentLimitsConfigs() - .withStatorCurrentLimit(90) + .withStatorCurrentLimit(70) // .withSupplyCurrentLimit(120) ) .withVoltage(new VoltageConfigs() diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index fcfd8c3..23ef178 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -7,12 +7,17 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.ControlRequest; -import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.math.InterpolatingMatrixTreeMap; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.simulation.BatterySim; @@ -52,6 +57,9 @@ public static Shooter getRightInstance() { private ShotCalculator shotCalculator = ShotCalculator.getInstance(); + private InterpolatingMatrixTreeMap distance_to_shooter_left = new InterpolatingMatrixTreeMap(); + private InterpolatingMatrixTreeMap distance_to_shooter_right = new InterpolatingMatrixTreeMap(); + private ShooterIO io; private Shooter(boolean left) { @@ -103,6 +111,11 @@ private Shooter(boolean left) { } io = new ShooterIO(getName(), topMotor, bottomMotor); + + distance_to_shooter_left.put(1.67, VecBuilder.fill(28.59375, 28.59375)); + distance_to_shooter_left.put(2.12, VecBuilder.fill(55, 20)); + distance_to_shooter_right.put(1.67, VecBuilder.fill(28.59375, 28.59375)); + distance_to_shooter_right.put(2.12, VecBuilder.fill(55, 20)); } @Override @@ -165,14 +178,14 @@ public Command stop() { public Command shoot(double topSpeed, double bottomSpeed) { return runOnce(() -> { - setTopRequest(new MotionMagicVelocityVoltage(topSpeed)); - setBottomRequest(new MotionMagicVelocityVoltage(bottomSpeed)); + setTopRequest(new VelocityVoltage(topSpeed)); + setBottomRequest(new VelocityVoltage(bottomSpeed)); }).withName("Shooting"); } public Command shoot(DoubleSupplier topSpeed, DoubleSupplier bottomSpeed) { - var topReq = new MotionMagicVelocityVoltage(0.0); - var bottomReq = new MotionMagicVelocityVoltage(0.0); + var topReq = new VelocityVoltage(0.0); + var bottomReq = new VelocityVoltage(0.0); return runOnce(() -> { setTopRequest(topReq); setBottomRequest(bottomReq); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 8f37c60..3534fe3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -6,8 +6,11 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.VoltageConfigs; +import edu.wpi.first.math.InterpolatingMatrixTreeMap; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; public final class ShooterConstants { public static final double GEAR_RATIO = 1; @@ -15,8 +18,7 @@ public final class ShooterConstants { public static final double TOPSPIN_FACTOR = 0; public static final Transform3d OFFSET = new Transform3d(); - public static final InterpolatingDoubleTreeMap DISTANCE_TO_SHOT_SPEED = new InterpolatingDoubleTreeMap(); - + /** Motor ids */ public static enum Motors { TOPLEFT(24), @@ -34,7 +36,7 @@ public static TalonFXConfiguration getConfig() { return new TalonFXConfiguration() .withSlot0(new Slot0Configs() .withKV(0.0) - .withKP(0.7) + .withKP(0.700) .withKI(0.0) .withKD(0.0) .withKA(0.0) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java index f1994be..275d8ea 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -27,12 +28,12 @@ public static ShotCalculator getInstance() { private final Drive drive; @AutoLogOutput - private Pose3d currentEffectiveTargetPose = Pose3d.kZero; + private Translation3d currentEffectiveTargetPose = Translation3d.kZero; private double currentEffectiveYaw; @AutoLogOutput private InterceptSolution currentInterceptSolution; - private Pose3d targetLocation = new Pose3d(); + private Translation3d targetLocation = new Translation3d(); private double targetDistance = 0.0; private double shooterAngle = 75 * Constants.TAU / 360; @@ -46,33 +47,33 @@ private ShotCalculator() { @Override public void periodic() { - Pose2d drivePose = drive.getPose(); + // Pose2d drivePose = drive.getPose(); - targetDistance = drivePose.getTranslation().getDistance(targetLocation.toPose2d().getTranslation()); + // targetDistance = drivePose.getTranslation().getDistance(targetLocation.toTranslation2d()); - Pose3d shooterPose = new Pose3d(drivePose).plus(ShooterConstants.OFFSET); + // var shooterPose = new Pose3d(drivePose).plus(ShooterConstants.OFFSET).getTranslation(); - ChassisSpeeds driveSpeeds = drive.getFieldSpeeds(); - ChassisAccels driveAccelerations = ChassisAccels.estimate(driveSpeeds, drive.getPrevFieldSpeeds(), Constants.DT); - - currentInterceptSolution = ShootOnTheFlyCalculator.solveShootOnTheFly( - shooterPose, - targetLocation, - zero, - zero1, - -shooterAngle, - 5, 0.01); - - currentEffectiveTargetPose = currentInterceptSolution.effectiveTargetPose(); - currentEffectiveYaw = currentInterceptSolution.requiredYaw(); + // ChassisSpeeds driveSpeeds = drive.getFieldSpeeds(); + // ChassisAccels driveAccelerations = ChassisAccels.estimate(driveSpeeds, drive.getPrevFieldSpeeds(), Constants.DT); + + // currentInterceptSolution = ShootOnTheFlyCalculator.solveShootOnTheFly( + // shooterPose, + // targetLocation, + // zero, + // zero1, + // -shooterAngle, + // 5, 0.01); + + // currentEffectiveTargetPose = currentInterceptSolution.effectiveTargetPose(); + // currentEffectiveYaw = currentInterceptSolution.requiredYaw(); } - public void setTarget(Pose3d targetLocation) { + public void setTarget(Translation3d targetLocation) { this.targetLocation = targetLocation; } - public Pose3d getCurrentEffectiveTargetPose() { + public Translation3d getCurrentEffectiveTargetPose() { return currentEffectiveTargetPose; } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index c3c1afa..0c73a90 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -30,7 +30,7 @@ public static enum VisionDeviceConstants { new Transform3d( new Translation3d( Inches.of(13.124114), //wpi x-axis positive is forward direction - Inches.of(-3.030256), //wpi y-axis positive is strafe left, so right camera shall have negative offset + Inches.of(-4.527904), //wpi y-axis positive is strafe left, so right camera shall have negative offset Inches.of(14.365654)), new Rotation3d(0, 26 * Constants.TAU / 360.0, -32.5 * Constants.TAU/360.0)), //(roll: x, pitch: y, yaw: z) 1, 1280, 800), diff --git a/src/main/java/frc/robot/subsystems/vision/VisionDevice.java b/src/main/java/frc/robot/subsystems/vision/VisionDevice.java index 0bb5daa..a269579 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionDevice.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionDevice.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.vision; +import frc.robot.subsystems.TelemetryManager; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.vision.VisionConstants.VisionDeviceConstants; import edu.wpi.first.math.Matrix; @@ -66,6 +67,9 @@ public VisionDevice(VisionDeviceConstants constants) { // poseEstimator = new PhotonPoseEstimator(FieldLayout.APRILTAG_MAP, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, constants.robotToCamera); this.constants = constants; + TelemetryManager.getInstance().addStructPublisher( + constants.name() + "Pose", Pose2d.struct, + () -> botPose); hasTarget = false; } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java b/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java index 539966a..3a7c74c 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java @@ -96,8 +96,8 @@ public synchronized boolean isFullyConnected() { public Command bootUp() { return Commands.parallel( - // frontlCamera.bootUpSequence(), - // frontrCamera.bootUpSequence() + frontlCamera.bootUpSequence(), + frontrCamera.bootUpSequence() ) .withTimeout(4) .andThen(Commands.print("Finished vision bootup")); From cce467fb6ddaf429f215f4a9165a5e9fa798b49c Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Mon, 2 Mar 2026 09:38:02 -0800 Subject: [PATCH 099/118] Feat: camera offsets calib-ed, left and right cameras are now in sync. But these offsets DONOT make sense. TODO: check pitch value if they are correct. --- .../frc/robot/subsystems/vision/VisionConstants.java | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 0c73a90..3647bf8 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -30,9 +30,11 @@ public static enum VisionDeviceConstants { new Transform3d( new Translation3d( Inches.of(13.124114), //wpi x-axis positive is forward direction - Inches.of(-4.527904), //wpi y-axis positive is strafe left, so right camera shall have negative offset + // Inches.of(12.624114), + Inches.of(-9.527904), //wpi y-axis positive is strafe left, so right camera shall have negative offset Inches.of(14.365654)), - new Rotation3d(0, 26 * Constants.TAU / 360.0, -32.5 * Constants.TAU/360.0)), //(roll: x, pitch: y, yaw: z) + // new Rotation3d(0, 26 * Constants.TAU / 360.0, -24 * Constants.TAU / 360.0)), //(roll: x, pitch: y, yaw: z) + new Rotation3d(0, 24 * Constants.TAU / 360.0, -24 * Constants.TAU / 360.0)), 1, 1280, 800), FL_CONSTANTS ( @@ -40,9 +42,9 @@ public static enum VisionDeviceConstants { new Transform3d( new Translation3d( Inches.of(13.262586), //wpi x-axis positive is forward direction - Inches.of(4.52790), //wpi y-axis positive is strafe left, so left camera shall have positive offset + Inches.of(7.030256), //wpi y-axis positive is strafe left, so left camera shall have positive offset Inches.of(14.325391)), - new Rotation3d(0, 24 * Constants.TAU / 360.0, 27.5 * Constants.TAU/360.0)), + new Rotation3d(0, 26 * Constants.TAU / 360.0, 18 * Constants.TAU / 360.0)), 2, 1280, 800); public final String tableName; From fb467e5adf2771a23a59551780aef6548356bc60 Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Mon, 2 Mar 2026 11:43:07 -0800 Subject: [PATCH 100/118] Bugfix: add half hub width to calc target angle for heading lock --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 445fa02..9d91712 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -128,7 +128,7 @@ public static class LinesHorizontal { /** Hub related constants */ public static class Hub { // Dimensions - public static final double width = edu.wpi.first.math.util.Units.inchesToMeters(47.0); + public static final double width = edu.wpi.first.math.util.Units.inchesToMeters(46.0); public static final double height = edu.wpi.first.math.util.Units.inchesToMeters(72.0); // includes the catcher at the top public static final double innerWidth = edu.wpi.first.math.util.Units.inchesToMeters(41.7); From bc33723c69538d583b9c442b7d0f5e3742666539 Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Mon, 2 Mar 2026 11:44:05 -0800 Subject: [PATCH 101/118] work-in-progress: calib projectile launch speeds --- src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/ControlsMapping.java | 120 +++++++++--------- src/main/java/frc/robot/Robot.java | 6 +- .../frc/robot/subsystems/drive/Drive.java | 2 +- .../subsystems/drive/DriveConstants.java | 4 +- .../frc/robot/subsystems/shooter/Shooter.java | 14 +- .../subsystems/shooter/ShooterConstants.java | 2 +- 7 files changed, 81 insertions(+), 77 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 8f8d2c8..2f58cf4 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Robot2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 166; - public static final String GIT_SHA = "81cd114328c916e94e7701a2cf77ec819cffbd8d"; - public static final String GIT_DATE = "2026-02-25 22:02:00 EST"; + public static final int GIT_REVISION = 167; + public static final String GIT_SHA = "8bbf7e7efdafca6ad60f7c482e04866db018194c"; + public static final String GIT_DATE = "2026-02-28 20:22:09 EST"; public static final String GIT_BRANCH = "sotm"; - public static final String BUILD_DATE = "2026-02-28 20:11:56 EST"; - public static final long BUILD_UNIX_TIME = 1772327516533L; + public static final String BUILD_DATE = "2026-03-01 21:20:24 EST"; + public static final long BUILD_UNIX_TIME = 1772418024795L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index 0fbe1cf..f0c38c6 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -27,19 +27,17 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ProxyCommand; - public class ControlsMapping { public static void mapTeleopCommand() { - + Drive.getInstance().setDefaultCommand((Drive.getInstance().openLoopControl())); // Intake.getInstance().setDefaultCommand(Intake.getInstance().intake()); - controller.back().and(controller.a()).onTrue(Drive.getInstance().resetPoseCommand(new - Pose2d())); + controller.back().and(controller.a()).onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); controller.back().and(controller.b()).onTrue(VisionDeviceManager.getInstance().bootUp()); controller.back().and(controller.y()).onTrue( - Commands.runOnce(() -> Drive.getInstance().getCtreDrive().getPigeon2().reset())); + Commands.runOnce(() -> Drive.getInstance().getCtreDrive().getPigeon2().reset())); // controller.leftTrigger().debounce(0.1).whileTrue(Intake.getInstance().outtake()); // controller.b().whileTrue(Intake.getInstance().outtake()); @@ -69,40 +67,45 @@ public static void mapTeleopCommand() { // )).andThen(Commands.waitSeconds(0.1)) // ) // ); + // controller.rightBumper().whileTrue( + // Commands.parallel( + // Shooter.getRightInstance().shoot( + // () -> SmartDashboard.getNumber("rightV", 30), + // () -> SmartDashboard.getNumber("rightV", 30) - 15 + // ), + // Shooter.getLeftInstance().shoot( + // () -> SmartDashboard.getNumber("leftV", 30), + // () -> SmartDashboard.getNumber("leftV", 30) - 15 + // ) + // ) + // ).onFalse( + // Commands.parallel( + // Shooter.getRightInstance().stop(), + // Shooter.getLeftInstance().stop() + // ) + // ); controller.rightBumper().whileTrue( - Commands.parallel( - Shooter.getRightInstance().shoot( - () -> SmartDashboard.getNumber("topVel", 30), - () -> SmartDashboard.getNumber("botVel", 30) - ), - Shooter.getLeftInstance().shoot( - () -> SmartDashboard.getNumber("topVel", 30), - () -> SmartDashboard.getNumber("botVel", 30) - ) - ) - ).onFalse( - Commands.parallel( - Shooter.getRightInstance().stop(), - Shooter.getLeftInstance().stop() - ) - ); + Commands.parallel( + Shooter.getRightInstance().shoot(() -> SmartDashboard.getNumber("toShooter", 1)), + Shooter.getLeftInstance().shoot(() -> SmartDashboard.getNumber("toShooter", 1)))) + .onFalse( + Commands.parallel( + Shooter.getRightInstance().stop(), + Shooter.getLeftInstance().stop())); controller.leftBumper().whileTrue( - Commands.parallel( - Indexer.getRightInstance().activateIndexer(), - Indexer.getLeftInstance().activateIndexer(), - Roller.getInstance().roll() - ) - ).onFalse( - Commands.parallel( - Indexer.getRightInstance().deactivateIndexer(), - Indexer.getLeftInstance().deactivateIndexer(), - Roller.getInstance().stop() - ) - ); + Commands.parallel( + Indexer.getRightInstance().activateIndexer(), + Indexer.getLeftInstance().activateIndexer(), + Roller.getInstance().roll())) + .onFalse( + Commands.parallel( + Indexer.getRightInstance().deactivateIndexer(), + Indexer.getLeftInstance().deactivateIndexer(), + Roller.getInstance().stop())); controller.leftTrigger().whileTrue( - Intake.getInstance().intake()).onFalse(Intake.getInstance().stopWheel()); + Intake.getInstance().intake()).onFalse(Intake.getInstance().stopWheel()); // controller.y().onTrue(Intake.getInstance().) controller.rightTrigger().whileTrue(Drive.getInstance().headingLockToHub()); // Shooter.getRightInstance().shoot())); @@ -112,38 +115,37 @@ public static void mapTeleopCommand() { public static void mapSysId() { // set up sysID routine type - controller.a().onTrue(new InstantCommand(()->Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.TRANSLATION))); - controller.b().onTrue(new InstantCommand(()->Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.ROTATION))); - controller.back().onTrue(new InstantCommand(()->Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER))); + controller.a().onTrue(new InstantCommand( + () -> Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.TRANSLATION))); + controller.b().onTrue(new InstantCommand( + () -> Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.ROTATION))); + controller.back().onTrue( + new InstantCommand(() -> Drive.getInstance().getCtreDrive().setSysIdRoutine(SysIdRoutineType.STEER))); // map the sysid routine movement directions controller.leftBumper().and(controller.x()) - .whileTrue( - new ProxyCommand( - ()->Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kForward) - .finallyDo(interrupted->Drive.getInstance().getCtreDrive().setControl(new SwerveRequest.Idle())) - ) - ); + .whileTrue( + new ProxyCommand( + () -> Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kForward) + .finallyDo(interrupted -> Drive.getInstance().getCtreDrive() + .setControl(new SwerveRequest.Idle())))); controller.leftBumper().and(controller.y()) - .whileTrue( - new ProxyCommand( - ()->Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kReverse) - .finallyDo(interrupted->Drive.getInstance().getCtreDrive().setControl(new SwerveRequest.Idle())) - ) - ); + .whileTrue( + new ProxyCommand( + () -> Drive.getInstance().getCtreDrive().sysIdDynamic(Direction.kReverse) + .finallyDo(interrupted -> Drive.getInstance().getCtreDrive() + .setControl(new SwerveRequest.Idle())))); controller.rightBumper().and(controller.x()) .whileTrue( - new ProxyCommand( - ()->Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kForward) - .finallyDo(interrupted->Drive.getInstance().getCtreDrive().setControl(new SwerveRequest.Idle())) - ) - ); + new ProxyCommand( + () -> Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kForward) + .finallyDo(interrupted -> Drive.getInstance().getCtreDrive() + .setControl(new SwerveRequest.Idle())))); controller.rightBumper().and(controller.y()) .whileTrue( - new ProxyCommand( - ()->Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kReverse) - .finallyDo(interrupted->Drive.getInstance().getCtreDrive().setControl(new SwerveRequest.Idle())) - ) - ); + new ProxyCommand( + () -> Drive.getInstance().getCtreDrive().sysIdQuasistatic(Direction.kReverse) + .finallyDo(interrupted -> Drive.getInstance().getCtreDrive() + .setControl(new SwerveRequest.Idle())))); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b782e2b..fa0ca81 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -127,8 +127,8 @@ public Robot() { // Intake.getInstance(); // Climb.getInstance(); - SmartDashboard.putNumber("botVel", 30); - SmartDashboard.putNumber("topVel", 30); + SmartDashboard.putNumber("rightV", 30); + SmartDashboard.putNumber("leftV", 30); TelemetryManager.getInstance(); commandScheduler.schedule(FollowPathCommand.warmupCommand()); @@ -169,7 +169,7 @@ public void robotPeriodic() { @Override public void disabledInit() { commandScheduler.schedule( - Led.getInstance().setSolidColorCommand(Color.kGreen)); + Led.getInstance().setRainbowCommand()); } /** This function is called periodically during disabled. */ diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 503bced..b035e54 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -112,7 +112,7 @@ public void outputTelemetry() { io.updateInputs(driveRequest, lastReadState, getCurrentCommand(), getDefaultCommand()); io.process(); - SmartDashboard.putNumber("To Shooter", FieldLayout.APRILTAG_MAP.getTagPose(10).get().toPose2d().getTranslation().getDistance(getPose().getTranslation())); + SmartDashboard.putNumber("toShooter", FieldLayout.APRILTAG_MAP.getTagPose(10).get().toPose2d().getTranslation().getDistance(getPose().getTranslation())); } /** diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 85e3499..4bfcee0 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -17,10 +17,10 @@ public final class DriveConstants { public static final double EPSILON_ROTATION = Units.Degrees.of(1.5).in(Units.Radians); // Maximums - public static final double MAX_SPEED = Units.MetersPerSecond.of(4.5).in(Units.MetersPerSecond); + public static final double MAX_SPEED = Units.MetersPerSecond.of(2.5).in(Units.MetersPerSecond); public static final double MAX_ACCEL = Units.MetersPerSecondPerSecond.of(9.0).in(Units.MetersPerSecondPerSecond); public static final double MAX_ROTATION_SPEED = - Units.RotationsPerSecond.of(2.0).in(Units.RadiansPerSecond); + Units.RotationsPerSecond.of(1.0).in(Units.RadiansPerSecond); public static final double MAX_ROTATION_ACCEL = Units.RotationsPerSecondPerSecond.of(4.0).in(Units.RadiansPerSecondPerSecond); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 23ef178..6324331 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -18,6 +18,7 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.simulation.BatterySim; @@ -57,8 +58,7 @@ public static Shooter getRightInstance() { private ShotCalculator shotCalculator = ShotCalculator.getInstance(); - private InterpolatingMatrixTreeMap distance_to_shooter_left = new InterpolatingMatrixTreeMap(); - private InterpolatingMatrixTreeMap distance_to_shooter_right = new InterpolatingMatrixTreeMap(); + private InterpolatingMatrixTreeMap distance_to_shooter = new InterpolatingMatrixTreeMap(); private ShooterIO io; @@ -112,10 +112,8 @@ private Shooter(boolean left) { io = new ShooterIO(getName(), topMotor, bottomMotor); - distance_to_shooter_left.put(1.67, VecBuilder.fill(28.59375, 28.59375)); - distance_to_shooter_left.put(2.12, VecBuilder.fill(55, 20)); - distance_to_shooter_right.put(1.67, VecBuilder.fill(28.59375, 28.59375)); - distance_to_shooter_right.put(2.12, VecBuilder.fill(55, 20)); + distance_to_shooter.put(1.6015, VecBuilder.fill(37.5, 31.875, 10)); + distance_to_shooter.put(2.49,VecBuilder.fill(44.0625, 39.140625, 15)); } @Override @@ -183,6 +181,10 @@ public Command shoot(double topSpeed, double bottomSpeed) { }).withName("Shooting"); } + public Command shoot(DoubleSupplier distance) { + return shoot(distance_to_shooter.get(distance.getAsDouble()).get(0, 0), distance_to_shooter.get(distance.getAsDouble()).get(1, 0) - 10); + } + public Command shoot(DoubleSupplier topSpeed, DoubleSupplier bottomSpeed) { var topReq = new VelocityVoltage(0.0); var bottomReq = new VelocityVoltage(0.0); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 3534fe3..95bd865 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -36,7 +36,7 @@ public static TalonFXConfiguration getConfig() { return new TalonFXConfiguration() .withSlot0(new Slot0Configs() .withKV(0.0) - .withKP(0.700) + .withKP(0.600) .withKI(0.0) .withKD(0.0) .withKA(0.0) From 1ff89ec6d556676bc22c6fb6c0e21e0912debd0d Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Mon, 2 Mar 2026 19:17:23 -0800 Subject: [PATCH 102/118] feat: 3/2 changes --- src/main/java/frc/robot/BuildConstants.java | 10 ++-- src/main/java/frc/robot/ControlsMapping.java | 48 +++++++++---------- src/main/java/frc/robot/Robot.java | 2 +- .../java/frc/robot/lib/field/FieldLayout.java | 7 ++- .../frc/robot/subsystems/drive/Drive.java | 15 +++++- .../subsystems/drive/DriveConstants.java | 23 +++++++++ .../subsystems/vision/VisionConstants.java | 1 + 7 files changed, 72 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 2f58cf4..e1c79db 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Robot2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 167; - public static final String GIT_SHA = "8bbf7e7efdafca6ad60f7c482e04866db018194c"; - public static final String GIT_DATE = "2026-02-28 20:22:09 EST"; + public static final int GIT_REVISION = 170; + public static final String GIT_SHA = "bc33723c69538d583b9c442b7d0f5e3742666539"; + public static final String GIT_DATE = "2026-03-02 14:44:05 EST"; public static final String GIT_BRANCH = "sotm"; - public static final String BUILD_DATE = "2026-03-01 21:20:24 EST"; - public static final long BUILD_UNIX_TIME = 1772418024795L; + public static final String BUILD_DATE = "2026-03-02 21:29:03 EST"; + public static final long BUILD_UNIX_TIME = 1772504943383L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index f0c38c6..0d245c1 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -67,31 +67,31 @@ public static void mapTeleopCommand() { // )).andThen(Commands.waitSeconds(0.1)) // ) // ); - // controller.rightBumper().whileTrue( - // Commands.parallel( - // Shooter.getRightInstance().shoot( - // () -> SmartDashboard.getNumber("rightV", 30), - // () -> SmartDashboard.getNumber("rightV", 30) - 15 - // ), - // Shooter.getLeftInstance().shoot( - // () -> SmartDashboard.getNumber("leftV", 30), - // () -> SmartDashboard.getNumber("leftV", 30) - 15 - // ) - // ) - // ).onFalse( - // Commands.parallel( - // Shooter.getRightInstance().stop(), - // Shooter.getLeftInstance().stop() - // ) - // ); controller.rightBumper().whileTrue( - Commands.parallel( - Shooter.getRightInstance().shoot(() -> SmartDashboard.getNumber("toShooter", 1)), - Shooter.getLeftInstance().shoot(() -> SmartDashboard.getNumber("toShooter", 1)))) - .onFalse( - Commands.parallel( - Shooter.getRightInstance().stop(), - Shooter.getLeftInstance().stop())); + Commands.parallel( + Shooter.getRightInstance().shoot( + () -> SmartDashboard.getNumber("rightV", 30) - 15, + () -> SmartDashboard.getNumber("rightV", 30) + 15 + ), + Shooter.getLeftInstance().shoot( + () -> SmartDashboard.getNumber("leftV", 30) - 15, + () -> SmartDashboard.getNumber("leftV", 30) + 15 + ) + ) + ).onFalse( + Commands.parallel( + Shooter.getRightInstance().stop(), + Shooter.getLeftInstance().stop() + ) + ); + // controller.rightBumper().whileTrue( + // Commands.parallel( + // Shooter.getRightInstance().shoot(() -> SmartDashboard.getNumber("toShooter", 1)), + // Shooter.getLeftInstance().shoot(() -> SmartDashboard.getNumber("toShooter", 1)))) + // .onFalse( + // Commands.parallel( + // Shooter.getRightInstance().stop(), + // Shooter.getLeftInstance().stop())); controller.leftBumper().whileTrue( Commands.parallel( diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fa0ca81..7ef15c4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -204,7 +204,7 @@ public void autonomousExit() { @Override public void teleopInit() { commandScheduler.schedule( - Led.getInstance().setSolidColorCommand(Color.kRed)); + Led.getInstance().blinkCommand(Color.kRed, Color.kYellow, 0.25)); ControlsMapping.mapTeleopCommand(); // This makes sure that the autonomous stops running when teleop starts running. if (autoCommand != null) { diff --git a/src/main/java/frc/robot/lib/field/FieldLayout.java b/src/main/java/frc/robot/lib/field/FieldLayout.java index 4eb487e..ef6fa3e 100644 --- a/src/main/java/frc/robot/lib/field/FieldLayout.java +++ b/src/main/java/frc/robot/lib/field/FieldLayout.java @@ -36,8 +36,8 @@ public class FieldLayout { //TODO: this must be tuned to the specific year's field public static Field2d field; - public static final double FIELD_LENGTH = Units.inchesToMeters(651.223); - public static final double FIELD_WIDTH = Units.inchesToMeters(323.277); + public static final double FIELD_LENGTH; + public static final double FIELD_WIDTH; public static final double APRITAG_WIDTH = Units.inchesToMeters(6.50); public static AprilTagFieldLayout APRILTAG_MAP; @@ -54,6 +54,9 @@ public class FieldLayout { DriverStation.reportError(e.getMessage(), false); APRILTAG_MAP = AprilTagLayoutGenerated.getLayout(); } + + FIELD_LENGTH = APRILTAG_MAP.getFieldLength(); + FIELD_WIDTH = APRILTAG_MAP.getFieldWidth(); // APRILTAG_MAP = AprilTagLayoutGenerated.getLayout(); field = new Field2d(); SmartDashboard.putData(field); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index b035e54..c9f48e5 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -308,13 +308,24 @@ public Command headingLockToPose(Supplier pose) { * Utilizes feedforwards derived from the current chassis speeds */ public Command headingLockToHub() { + TelemetryManager.getInstance().addStructPublisher( + "thing", Pose2d.struct, () -> new Pose2d(Constants.FieldConstants.allianceCorrected( + FieldPoses.HUB.pose3d.getTranslation() + // Constants.FieldConstants.Hub.topCenterPoint + ).toTranslation2d(), Rotation2d.kZero)); + return Commands.runOnce(() -> ShotCalculator.getInstance().setTarget( - Constants.FieldConstants.allianceCorrected(Constants.FieldConstants.Hub.topCenterPoint))) + Constants.FieldConstants.allianceCorrected( + FieldPoses.HUB.pose3d.getTranslation() + // Constants.FieldConstants.Hub.topCenterPoint + ))) .andThen( headingLockToPose( Constants.FieldConstants .allianceCorrected( - Constants.FieldConstants.Hub.topCenterPoint) + FieldPoses.HUB.pose3d.getTranslation() + // Constants.FieldConstants.Hub.topCenterPoint + ) .toTranslation2d())); } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 4bfcee0..7989e67 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -3,11 +3,13 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Time; +import frc.robot.Constants; import frc.robot.lib.control.ControlConstants.*; import frc.robot.lib.field.FieldLayout; import frc.robot.subsystems.drive.ctre.CompCtreDriveConstants; @@ -52,4 +54,25 @@ public final class DriveConstants { public static final double ACCELERATION_CONSTANT = 0.1; public static final double AUTO_ALIGN_TIMEOUT = 0.5; + public static enum FieldPoses { + HUB( + + new Pose3d(Constants.FieldConstants.Hub.topCenterPoint, Rotation3d.kZero) + ), + TRENCH( + FieldLayout.APRILTAG_MAP.getTagPose(12).orElse(Pose3d.kZero)), + TAG( + FieldLayout.APRILTAG_MAP.getTagPose(12).orElse(Pose3d.kZero)); + public Pose2d pose; + public Pose3d pose3d; + private FieldPoses(Pose2d pose) { + this.pose = pose; + } + + private FieldPoses(Pose3d pose3d) { + this.pose = pose3d.toPose2d(); + this.pose3d = pose3d; + } + } + } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 3647bf8..85a5e13 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -57,6 +57,7 @@ private VisionDeviceConstants( Transform3d robotToCamera, int cameraId, int cameraResolutionWidth, + int cameraResolutionHeight ) { this.tableName = tableName; From 695bc91a4d8a133e53361806bb43aa32164c82a3 Mon Sep 17 00:00:00 2001 From: Arvind Venkatesh Date: Thu, 5 Mar 2026 19:09:47 -0800 Subject: [PATCH 103/118] feat: shooter interpolation --- .../frc/robot/subsystems/shooter/Shooter.java | 21 +++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 6324331..65a96d1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -24,6 +24,7 @@ import edu.wpi.first.wpilibj.simulation.BatterySim; import edu.wpi.first.wpilibj.simulation.FlywheelSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -58,7 +59,7 @@ public static Shooter getRightInstance() { private ShotCalculator shotCalculator = ShotCalculator.getInstance(); - private InterpolatingMatrixTreeMap distance_to_shooter = new InterpolatingMatrixTreeMap(); + private InterpolatingMatrixTreeMap distance_to_shooter = new InterpolatingMatrixTreeMap(); private ShooterIO io; @@ -112,8 +113,14 @@ private Shooter(boolean left) { io = new ShooterIO(getName(), topMotor, bottomMotor); - distance_to_shooter.put(1.6015, VecBuilder.fill(37.5, 31.875, 10)); - distance_to_shooter.put(2.49,VecBuilder.fill(44.0625, 39.140625, 15)); + distance_to_shooter.put(0.0, VecBuilder.fill(0,0)); + distance_to_shooter.put(0.641, VecBuilder.fill(25.0, 25.0)); + distance_to_shooter.put(1.06, VecBuilder.fill(31.25, 31.25)); + distance_to_shooter.put(1.56, VecBuilder.fill(34.375, 34.375)); + distance_to_shooter.put(2.04, VecBuilder.fill(37.5, 37.5)); + distance_to_shooter.put(2.54, VecBuilder.fill(49.21875, 49.21875)); + distance_to_shooter.put(3.0, VecBuilder.fill(60.9375, 60.9375)); + distance_to_shooter.put(3.4, VecBuilder.fill(81.25, 81.265)); } @Override @@ -123,6 +130,12 @@ public void periodic() { lastReadSpeedBottom = bottomMotor.getVelocity().getValueAsDouble(); topMotor.setControl(topRequest); bottomMotor.setControl(bottomRequest); + + SmartDashboard.putNumber( + "ShooterV", + distance_to_shooter.get( + SmartDashboard.getNumber("toShooter", 1.5)).get(0, 0)); + io.updateInputs(lastReadSpeedTop, lastReadSpeedBottom, getCurrentCommand(), getDefaultCommand()); io.process(); } @@ -182,7 +195,7 @@ public Command shoot(double topSpeed, double bottomSpeed) { } public Command shoot(DoubleSupplier distance) { - return shoot(distance_to_shooter.get(distance.getAsDouble()).get(0, 0), distance_to_shooter.get(distance.getAsDouble()).get(1, 0) - 10); + return shoot(distance_to_shooter.get(distance.getAsDouble()).get(0, 0) - 15, distance_to_shooter.get(distance.getAsDouble()).get(1, 0) + 15); } public Command shoot(DoubleSupplier topSpeed, DoubleSupplier bottomSpeed) { From 177df4b78996e981f8083f7e2def65bf915f6b97 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Thu, 5 Mar 2026 22:14:21 -0800 Subject: [PATCH 104/118] feat: 3/5 definitely not last minute changes --- src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/ControlsMapping.java | 152 ++++++++-------- src/main/java/frc/robot/Robot.java | 27 ++- .../frc/robot/subsystems/drive/Drive.java | 42 ++--- .../frc/robot/subsystems/indexer/Indexer.java | 27 +-- .../frc/robot/subsystems/intake/Intake.java | 16 +- .../subsystems/intake/IntakeConstants.java | 9 +- .../frc/robot/subsystems/roller/Roller.java | 4 + .../subsystems/roller/RollerConstants.java | 3 + .../frc/robot/subsystems/shooter/Shooter.java | 42 ++++- .../subsystems/shooter/ShooterConstants.java | 2 +- .../subsystems/shooter/ShotCalculator.java | 170 +++++++++--------- .../subsystems/vision/VisionConstants.java | 5 + .../vision/VisionDeviceManager.java | 2 - 14 files changed, 271 insertions(+), 240 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index e1c79db..806b516 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Robot2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 170; - public static final String GIT_SHA = "bc33723c69538d583b9c442b7d0f5e3742666539"; - public static final String GIT_DATE = "2026-03-02 14:44:05 EST"; + public static final int GIT_REVISION = 172; + public static final String GIT_SHA = "695bc91a4d8a133e53361806bb43aa32164c82a3"; + public static final String GIT_DATE = "2026-03-05 22:09:47 EST"; public static final String GIT_BRANCH = "sotm"; - public static final String BUILD_DATE = "2026-03-02 21:29:03 EST"; - public static final long BUILD_UNIX_TIME = 1772504943383L; + public static final String BUILD_DATE = "2026-03-06 01:04:17 EST"; + public static final long BUILD_UNIX_TIME = 1772777057559L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index 0d245c1..e0501ab 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -1,28 +1,20 @@ package frc.robot; -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.MetersPerSecond; import static frc.robot.Robot.controller; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.geometry.*; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; -import frc.robot.lib.field.FieldUtil; -import frc.robot.subsystems.climb.Climb; import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.drive.DriveConstants; import frc.robot.subsystems.drive.ctre.CtreDrive.SysIdRoutineType; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.roller.Roller; import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.ShotCalculator; import frc.robot.subsystems.vision.VisionDeviceManager; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ProxyCommand; @@ -39,78 +31,80 @@ public static void mapTeleopCommand() { controller.back().and(controller.y()).onTrue( Commands.runOnce(() -> Drive.getInstance().getCtreDrive().getPigeon2().reset())); - // controller.leftTrigger().debounce(0.1).whileTrue(Intake.getInstance().outtake()); - // controller.b().whileTrue(Intake.getInstance().outtake()); - // controller.x().onTrue(Climb.getInstance().hangCommand()); - - // controller.rightTrigger().debounce(0.1 - // // ).onTrue( - // // Commands.parallel( - // // Shooter.getLeftInstance().shoot(50, -50), - // // Shooter.getRightInstance().shoot(50, -50)) - // // ).onFalse( - // // Commands.parallel( - // // Shooter.getLeftInstance().stop(), - // // Shooter.getRightInstance().stop()) - // ).whileTrue( - // Commands.parallel( - // Indexer.getLeftInstance().activateIndexer(), - // Indexer.getRightInstance().activateIndexer()) - // ).whileTrue( - // Commands.repeatingSequence( - // Commands.runOnce(() -> Robot.fuelSim.launchFuel( - // MetersPerSecond.of( - // Shooter.getLeftInstance().getTopSpeed() * Constants.TAU * 0.0508), - // Degrees.of(75), - // Degrees.of(0), - // Inches.of(19) - // )).andThen(Commands.waitSeconds(0.1)) - // ) - // ); - controller.rightBumper().whileTrue( - Commands.parallel( - Shooter.getRightInstance().shoot( - () -> SmartDashboard.getNumber("rightV", 30) - 15, - () -> SmartDashboard.getNumber("rightV", 30) + 15 - ), - Shooter.getLeftInstance().shoot( - () -> SmartDashboard.getNumber("leftV", 30) - 15, - () -> SmartDashboard.getNumber("leftV", 30) + 15 - ) - ) - ).onFalse( - Commands.parallel( - Shooter.getRightInstance().stop(), - Shooter.getLeftInstance().stop() - ) - ); - // controller.rightBumper().whileTrue( - // Commands.parallel( - // Shooter.getRightInstance().shoot(() -> SmartDashboard.getNumber("toShooter", 1)), - // Shooter.getLeftInstance().shoot(() -> SmartDashboard.getNumber("toShooter", 1)))) - // .onFalse( - // Commands.parallel( - // Shooter.getRightInstance().stop(), - // Shooter.getLeftInstance().stop())); - - controller.leftBumper().whileTrue( - Commands.parallel( - Indexer.getRightInstance().activateIndexer(), - Indexer.getLeftInstance().activateIndexer(), - Roller.getInstance().roll())) - .onFalse( - Commands.parallel( - Indexer.getRightInstance().deactivateIndexer(), - Indexer.getLeftInstance().deactivateIndexer(), - Roller.getInstance().stop())); - - controller.leftTrigger().whileTrue( - Intake.getInstance().intake()).onFalse(Intake.getInstance().stopWheel()); - // controller.y().onTrue(Intake.getInstance().) + controller.x().whileTrue(Intake.getInstance().outtake()); + + controller.rightBumper() + .whileTrue(shootAll()) + .onFalse(stopShoot()); + controller.leftBumper() + .whileTrue(indexAll()) + .onFalse(stopIndex()); + + controller.leftTrigger() + .whileTrue(Intake.getInstance().intake()) + .onFalse(Intake.getInstance().stopWheel()); + + controller.y().and(controller.back().negate()) + .whileTrue(Intake.getInstance().stow()) + .onFalse(Intake.getInstance().lower()); + controller.rightTrigger().whileTrue(Drive.getInstance().headingLockToHub()); - // Shooter.getRightInstance().shoot())); controller.povDown().onTrue(Intake.getInstance().calibrateZero()); - controller.povUp().onTrue(Intake.getInstance().stow()); + controller.b().and(controller.back().negate()).whileTrue(backIndex()).onFalse(stopIndex()); + + // controller.a().whileTrue( + // Intake.getInstance().agitate() + // ).onFalse(Intake.getInstance().lower()); + + // controller.rightBumper().whileTrue( + // Commands.parallel( + // Shooter.getRightInstance().shoot( + // () -> SmartDashboard.getNumberArray("shootervel", new Double[] {30.0, 30.0})[0] - 15, + // () -> SmartDashboard.getNumberArray("shootervel", new Double[] {30.0, 30.0})[0] + 15 + // ), + // Shooter.getLeftInstance().shoot( + // () -> SmartDashboard.getNumberArray("shootervel", new Double[] {30.0, 30.0})[1] - 15, + // () -> SmartDashboard.getNumberArray("shootervel", new Double[] {30.0, 30.0})[1] + 15 + // ) + // ) + // ).onFalse( + // stopShoot() + // ) + // ); + + } + + public static Command shootAll() { + return Commands.parallel( + Shooter.getRightInstance().shoot(), + Shooter.getLeftInstance().shoot()); + } + + public static Command stopShoot() { + return Commands.parallel( + Shooter.getRightInstance().stop(), + Shooter.getLeftInstance().stop()); + } + + public static Command indexAll() { + return Commands.parallel( + Indexer.getRightInstance().activateIndexer(), + Indexer.getLeftInstance().activateIndexer(), + Roller.getInstance().roll()); + } + + public static Command stopIndex() { + return Commands.parallel( + Indexer.getRightInstance().deactivateIndexer(), + Indexer.getLeftInstance().deactivateIndexer(), + Roller.getInstance().stop()); + } + + public static Command backIndex() { + return Commands.parallel( + Indexer.getRightInstance().back(), + Indexer.getLeftInstance().back(), + Roller.getInstance().antiRoll()); } public static void mapSysId() { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7ef15c4..49fb9a1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -41,6 +41,7 @@ import frc.robot.subsystems.led.Led; import frc.robot.subsystems.roller.Roller; import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.vision.VisionConstants; import frc.robot.subsystems.vision.VisionDeviceManager; /** @@ -97,18 +98,18 @@ public Robot() { } if (RobotBase.isReal()) { - Logger.addDataReceiver(new WPILOGWriter()); + // Logger.addDataReceiver(new WPILOGWriter()); if (!DriverStation.isFMSAttached()) { Logger.addDataReceiver(new NT4Publisher()); } } else if (replay) { setUseTiming(false); String logPath = LogFileUtil.findReplayLog(); - Logger.setReplaySource(new WPILOGReader(logPath)); - Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); + // Logger.setReplaySource(new WPILOGReader(logPath)); + // Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); } else if (RobotBase.isSimulation()) { Logger.addDataReceiver(new NT4Publisher()); - Logger.addDataReceiver(new WPILOGWriter()); + // Logger.addDataReceiver(new WPILOGWriter()); } Logger.start(); @@ -138,8 +139,8 @@ public Robot() { Led.getInstance(); commandScheduler.schedule( Led.getInstance().setRainbowCommand()); - - DriverStation.startDataLog(DataLogManager.getLog()); + Drive.getInstance().getCtreDrive().setStateStdDevs(VisionConstants.STATE_STD_DEVS); + // DriverStation.startDataLog(DataLogManager.getLog()); } @@ -170,6 +171,9 @@ public void robotPeriodic() { public void disabledInit() { commandScheduler.schedule( Led.getInstance().setRainbowCommand()); + + Drive.getInstance().getCtreDrive().setVisionMeasurementStdDevs(VisionConstants.ROTATION_STD_DEVS); + } /** This function is called periodically during disabled. */ @@ -178,9 +182,16 @@ public void disabledPeriodic() { } + @Override + public void disabledExit() { + Drive.getInstance().getCtreDrive().setVisionMeasurementStdDevs(VisionConstants.LOCAL_MEASUREMENT_STD_DEVS); + } + /** This autonomous runs the autonomous command selected. */ @Override public void autonomousInit() { + Drive.getInstance().getCtreDrive().setVisionMeasurementStdDevs(VisionConstants.LOCAL_MEASUREMENT_STD_DEVS); + commandScheduler.schedule( Led.getInstance().setSolidColorCommand(Color.kBlue)); autoCommand = autoChooser.getAuto(); @@ -203,6 +214,8 @@ public void autonomousExit() { @Override public void teleopInit() { + Drive.getInstance().getCtreDrive().setVisionMeasurementStdDevs(VisionConstants.LOCAL_MEASUREMENT_STD_DEVS); + commandScheduler.schedule( Led.getInstance().blinkCommand(Color.kRed, Color.kYellow, 0.25)); ControlsMapping.mapTeleopCommand(); @@ -219,6 +232,8 @@ public void teleopPeriodic() { @Override public void testInit() { + Drive.getInstance().getCtreDrive().setVisionMeasurementStdDevs(VisionConstants.LOCAL_MEASUREMENT_STD_DEVS); + commandScheduler.schedule( Led.getInstance().setSolidColorCommand(Color.kGray)); // Cancels all running commands at the start of test mode. diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index c9f48e5..fd7c2b3 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -45,7 +45,7 @@ import frc.robot.lib.util.Util; import frc.robot.subsystems.TelemetryManager; import frc.robot.subsystems.drive.ctre.CompCtreDriveConstants; -import frc.robot.subsystems.shooter.ShotCalculator; +// import frc.robot.subsystems.shooter.ShotCalculator; import frc.robot.subsystems.drive.commands.AutopilotCommand; import frc.robot.subsystems.drive.commands.PIDToPoseCommand; import frc.robot.subsystems.drive.commands.TrajectoryCommand; @@ -112,7 +112,7 @@ public void outputTelemetry() { io.updateInputs(driveRequest, lastReadState, getCurrentCommand(), getDefaultCommand()); io.process(); - SmartDashboard.putNumber("toShooter", FieldLayout.APRILTAG_MAP.getTagPose(10).get().toPose2d().getTranslation().getDistance(getPose().getTranslation())); + // SmartDashboard.putNumber("toShooter", FieldLayout.APRILTAG_MAP.getTagPose(10).get().toPose2d().getTranslation().getDistance(getPose().getTranslation())); } /** @@ -170,9 +170,9 @@ public Command openLoopControl() { double yFancy = xy[1]; double rotFancy = Util.applyJoystickDeadband(rotDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); - SmartDashboard.putNumber("Sticks/vX", xDesiredRaw); - SmartDashboard.putNumber("Sticks/vY", yDesiredRaw); - SmartDashboard.putNumber("Sticks/vW", rotDesiredRaw); + // SmartDashboard.putNumber("Sticks/vX", xDesiredRaw); + // SmartDashboard.putNumber("Sticks/vY", yDesiredRaw); + // SmartDashboard.putNumber("Sticks/vW", rotDesiredRaw); teleopRequest .withVelocityX(xFancy * MAX_SPEED) @@ -308,25 +308,19 @@ public Command headingLockToPose(Supplier pose) { * Utilizes feedforwards derived from the current chassis speeds */ public Command headingLockToHub() { - TelemetryManager.getInstance().addStructPublisher( - "thing", Pose2d.struct, () -> new Pose2d(Constants.FieldConstants.allianceCorrected( - FieldPoses.HUB.pose3d.getTranslation() - // Constants.FieldConstants.Hub.topCenterPoint - ).toTranslation2d(), Rotation2d.kZero)); - - return Commands.runOnce(() -> ShotCalculator.getInstance().setTarget( - Constants.FieldConstants.allianceCorrected( - FieldPoses.HUB.pose3d.getTranslation() - // Constants.FieldConstants.Hub.topCenterPoint - ))) - .andThen( - headingLockToPose( - Constants.FieldConstants - .allianceCorrected( - FieldPoses.HUB.pose3d.getTranslation() - // Constants.FieldConstants.Hub.topCenterPoint - ) - .toTranslation2d())); + // TelemetryManager.getInstance().addStructPublisher( + // "thing", Pose2d.struct, () -> new Pose2d(Constants.FieldConstants.allianceCorrected( + // FieldPoses.HUB.pose3d.getTranslation() + // // Constants.FieldConstants.Hub.topCenterPoint + // ).toTranslation2d(), Rotation2d.kZero)); + + return headingLockToPose( + Constants.FieldConstants + .allianceCorrected( + FieldPoses.HUB.pose3d.getTranslation() + // Constants.FieldConstants.Hub.topCenterPoint + ) + .toTranslation2d()); } /** diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 44ca7ce..4b21154 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -66,21 +66,6 @@ private Indexer(boolean isLeft) { .withInverted(InvertedValue.Clockwise_Positive)); } motor.getConfigurator().apply(config); - // lc = new LaserCan(isLeft ? L_LASER_ID : R_LASER_ID); - // lcTwo = new LaserCan(LASER_ID_2); - - /* new laser configs */ - // if (Robot.isReal()) { - // for (int i = 0; i < 20; i++) { - // try { - // lc.setRangingMode(LaserCan.RangingMode.SHORT); - // lc.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16)); - // lc.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); - // break; - // } catch (ConfigurationFailedException e) { - // System.out.println("Configuration failed! " + e); - // } - // }} if (Robot.isSimulation()) { wheelSim = new FlywheelSim( @@ -91,14 +76,6 @@ private Indexer(boolean isLeft) { DCMotor.getKrakenX44(1), 0.0); } - // try { - // lcTwo.setRangingMode(LaserCan.RangingMode.SHORT); - // lcTwo.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16)); - // lcTwo.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); - // } catch (ConfigurationFailedException e) { - // System.out.println("Configuration failed! " + e); - // } - io = new IndexerIO(getName(), motor); // TelemetryManager.getInstance().addSendable(this); } @@ -153,6 +130,10 @@ public Command activateIndexer() { return setSpeed(ROLLING_SPEED); } + public Command back() { + return setSpeed(-ROLLING_SPEED); + } + /** turn motor down to zero */ public Command deactivateIndexer() { return runOnce(() -> setRequest(new CoastOut())); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 5457ef5..33e6cd8 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -141,7 +141,17 @@ public Command stow() { return setSetpoint(0.0, BAR_POSITION_UP) .andThen(waitUntilBarIsAtPosition(BAR_POSITION_UP)); } + + public Command agitate() { + return setSetpoint(0.0, BAR_POSITION_DOWN) + .andThen(setSetpoint(0.0, BAR_POSITION_MID)) + .repeatedly(); + } + public Command lower() { + return setSetpoint(0.0, BAR_POSITION_DOWN); + } + //----------------set request--------------- private void setRequestWheel(ControlRequest request) { this.wheelRequest = request; @@ -219,15 +229,15 @@ public Command waitUntilBarIsAtPosition(double target) { * @return Command to run */ public Command calibrateZero() { - VoltageOut calibrationRequest = new VoltageOut(-2) + VoltageOut calibrationRequest = new VoltageOut(-0.5) .withIgnoreHardwareLimits(true) .withIgnoreSoftwareLimits(true); /** Trigger to detect when the elevator drives into a hard stop. */ Trigger isHardStop = new Trigger(() -> { return barMotor.getVelocity().getValue().abs(RotationsPerSecond) < 1 && - barMotor.getTorqueCurrent().getValue().abs(Amps) > 25; - }).debounce(0.1); + barMotor.getTorqueCurrent().getValue().abs(Amps) > 10; + }).debounce(0.05); return run(() -> { barMotor.setControl(calibrationRequest); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 9cd9922..ceeac9a 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -16,9 +16,10 @@ import edu.wpi.first.units.Units; public class IntakeConstants { - public static final double BAR_EPSILON = Units.Degrees.of(5).in(Units.Rotations); + public static final double BAR_EPSILON = Units.Degrees.of(10).in(Units.Rotations); public static final double INTAKE_SPEED = 50; public static final double BAR_POSITION_DOWN = 0.00; + public static final double BAR_POSITION_MID = Degrees.of(67).in(Rotations); public static final double BAR_POSITION_UP = Degrees.of(126).in(Rotations); public static final double BAR_GEAR_RATIO = 44.0 / 18.0 * 5.0 * 4.0; public static final double BAR_POS_MIN = 0.0; @@ -55,15 +56,15 @@ public static TalonFXConfiguration getBarConfig() { return new TalonFXConfiguration() .withSlot0(new Slot0Configs() .withKV(0.0) - .withKP(10.0) + .withKP(20.0) .withKI(0.0) .withKD(0.0) .withKG(0.0).withGravityType(GravityTypeValue.Arm_Cosine)) .withCurrentLimits(new CurrentLimitsConfigs() .withStatorCurrentLimit(60)) .withMotionMagic(new MotionMagicConfigs() - .withMotionMagicAcceleration(2) // 1.0 m/s^2 - .withMotionMagicCruiseVelocity(3) + .withMotionMagicAcceleration(10) // 1.0 m/s^2 + .withMotionMagicCruiseVelocity(10) .withMotionMagicJerk(16)) .withVoltage(new VoltageConfigs() .withPeakForwardVoltage(12.0) diff --git a/src/main/java/frc/robot/subsystems/roller/Roller.java b/src/main/java/frc/robot/subsystems/roller/Roller.java index 50d5b07..d8987d0 100644 --- a/src/main/java/frc/robot/subsystems/roller/Roller.java +++ b/src/main/java/frc/robot/subsystems/roller/Roller.java @@ -91,6 +91,10 @@ public Command roll() { return setSpeed(ROLL_SPEED); } + public Command antiRoll() { + return setSpeed(-ROLL_SPEED); + } + public Command stop() { return runOnce(() -> setRequest(new CoastOut())) .withName("Stop"); diff --git a/src/main/java/frc/robot/subsystems/roller/RollerConstants.java b/src/main/java/frc/robot/subsystems/roller/RollerConstants.java index 71881e4..5f0a7f3 100644 --- a/src/main/java/frc/robot/subsystems/roller/RollerConstants.java +++ b/src/main/java/frc/robot/subsystems/roller/RollerConstants.java @@ -4,6 +4,7 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.TorqueCurrentConfigs; import com.ctre.phoenix6.configs.VoltageConfigs; import com.ctre.phoenix6.signals.InvertedValue; @@ -22,6 +23,8 @@ public static TalonFXConfiguration getConfig() { .withStatorCurrentLimit(70) // .withSupplyCurrentLimit(120) ) + // .withTorqueCurrent(new TorqueCurrentConfigs() + // .withPeakForwardTorqueCurrent(null)) .withVoltage(new VoltageConfigs() .withPeakForwardVoltage(12.0) .withPeakReverseVoltage(-12.0)) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 65a96d1..491a667 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -4,6 +4,8 @@ import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; + import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.ControlRequest; @@ -29,6 +31,9 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Robot; +import frc.robot.lib.util.Util; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.DriveConstants.FieldPoses; import frc.robot.subsystems.shooter.ShooterConstants.Motors; public class Shooter extends SubsystemBase { @@ -57,7 +62,7 @@ public static Shooter getRightInstance() { private FlywheelSim topSim; private FlywheelSim bottomSim; - private ShotCalculator shotCalculator = ShotCalculator.getInstance(); + // private ShotCalculator shotCalculator = ShotCalculator.getInstance(); private InterpolatingMatrixTreeMap distance_to_shooter = new InterpolatingMatrixTreeMap(); @@ -131,13 +136,26 @@ public void periodic() { topMotor.setControl(topRequest); bottomMotor.setControl(bottomRequest); - SmartDashboard.putNumber( - "ShooterV", - distance_to_shooter.get( - SmartDashboard.getNumber("toShooter", 1.5)).get(0, 0)); + // SmartDashboard.putNumber( + // "ShooterV", + // distance_to_shooter.get( + // SmartDashboard.getNumber("toShooter", 1.5)).get(0, 0)); io.updateInputs(lastReadSpeedTop, lastReadSpeedBottom, getCurrentCommand(), getDefaultCommand()); io.process(); + + SmartDashboard.putNumber("toShooter", Drive.getInstance().getPose().getTranslation() + .getDistance( + Constants.FieldConstants.allianceCorrected( + FieldPoses.HUB.pose3d.getTranslation() + ).toTranslation2d())); + SmartDashboard.putNumber("ShooterVel", + distance_to_shooter.get( + Drive.getInstance().getPose().getTranslation() + .getDistance( + Constants.FieldConstants.allianceCorrected( + FieldPoses.HUB.pose3d.getTranslation() + ).toTranslation2d())).get(0, 0)); } public double getTopSpeed() { @@ -195,7 +213,11 @@ public Command shoot(double topSpeed, double bottomSpeed) { } public Command shoot(DoubleSupplier distance) { - return shoot(distance_to_shooter.get(distance.getAsDouble()).get(0, 0) - 15, distance_to_shooter.get(distance.getAsDouble()).get(1, 0) + 15); + return shoot( + distance_to_shooter.get(distance.getAsDouble()) + .get(0, 0) - 15, + distance_to_shooter.get(distance.getAsDouble()) + .get(1, 0) + 15); } public Command shoot(DoubleSupplier topSpeed, DoubleSupplier bottomSpeed) { @@ -214,7 +236,11 @@ public Command shoot(DoubleSupplier topSpeed, DoubleSupplier bottomSpeed) { public Command shoot() { - return shoot(() -> shotCalculator.getInterceptSolution().launchSpeed() / Constants.TAU / 0.0508 - TOPSPIN_FACTOR, - () -> -shotCalculator.getInterceptSolution().launchSpeed() / Constants.TAU / 0.0508 - TOPSPIN_FACTOR); + return shoot(() -> + Drive.getInstance().getPose().getTranslation() + .getDistance( + Constants.FieldConstants.allianceCorrected( + FieldPoses.HUB.pose3d.getTranslation() + ).toTranslation2d())); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 95bd865..8101990 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -36,7 +36,7 @@ public static TalonFXConfiguration getConfig() { return new TalonFXConfiguration() .withSlot0(new Slot0Configs() .withKV(0.0) - .withKP(0.600) + .withKP(0.6) .withKI(0.0) .withKD(0.0) .withKA(0.0) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java index 275d8ea..908ce31 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShotCalculator.java @@ -1,87 +1,87 @@ -package frc.robot.subsystems.shooter; - -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.AutoLogOutputManager; -import org.littletonrobotics.junction.Logger; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import frc.robot.lib.houndlib.ShootOnTheFlyCalculator; -import frc.robot.lib.houndlib.ShootOnTheFlyCalculator.InterceptSolution; -import frc.robot.lib.trajectory.RedTrajectory.State.ChassisAccels; -import frc.robot.subsystems.drive.Drive; - -// stores current target and actively computes effective target -public class ShotCalculator extends SubsystemBase { - private static ShotCalculator calcInstance; - public static ShotCalculator getInstance() { - if (calcInstance == null) { - calcInstance = new ShotCalculator(); - } - return calcInstance; - } - - private final Drive drive; - - @AutoLogOutput - private Translation3d currentEffectiveTargetPose = Translation3d.kZero; - private double currentEffectiveYaw; - - @AutoLogOutput - private InterceptSolution currentInterceptSolution; - private Translation3d targetLocation = new Translation3d(); - private double targetDistance = 0.0; - private double shooterAngle = 75 * Constants.TAU / 360; - - private ChassisSpeeds zero = new ChassisSpeeds(); - private ChassisAccels zero1 = new ChassisAccels(); - - private ShotCalculator() { - this.drive = Drive.getInstance(); - AutoLogOutputManager.addObject(this); - } - - @Override - public void periodic() { - // Pose2d drivePose = drive.getPose(); - - // targetDistance = drivePose.getTranslation().getDistance(targetLocation.toTranslation2d()); - - // var shooterPose = new Pose3d(drivePose).plus(ShooterConstants.OFFSET).getTranslation(); +// package frc.robot.subsystems.shooter; + +// import org.littletonrobotics.junction.AutoLogOutput; +// import org.littletonrobotics.junction.AutoLogOutputManager; +// import org.littletonrobotics.junction.Logger; + +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Pose3d; +// import edu.wpi.first.math.geometry.Translation3d; +// import edu.wpi.first.math.kinematics.ChassisSpeeds; +// import edu.wpi.first.wpilibj2.command.SubsystemBase; +// import frc.robot.Constants; +// import frc.robot.lib.houndlib.ShootOnTheFlyCalculator; +// import frc.robot.lib.houndlib.ShootOnTheFlyCalculator.InterceptSolution; +// import frc.robot.lib.trajectory.RedTrajectory.State.ChassisAccels; +// import frc.robot.subsystems.drive.Drive; + +// // stores current target and actively computes effective target +// public class ShotCalculator extends SubsystemBase { +// private static ShotCalculator calcInstance; +// public static ShotCalculator getInstance() { +// if (calcInstance == null) { +// calcInstance = new ShotCalculator(); +// } +// return calcInstance; +// } + +// private final Drive drive; + +// @AutoLogOutput +// private Translation3d currentEffectiveTargetPose = Translation3d.kZero; +// private double currentEffectiveYaw; + +// @AutoLogOutput +// private InterceptSolution currentInterceptSolution; +// private Translation3d targetLocation = new Translation3d(); +// private double targetDistance = 0.0; +// private double shooterAngle = 75 * Constants.TAU / 360; + +// private ChassisSpeeds zero = new ChassisSpeeds(); +// private ChassisAccels zero1 = new ChassisAccels(); + +// private ShotCalculator() { +// this.drive = Drive.getInstance(); +// AutoLogOutputManager.addObject(this); +// } + +// @Override +// public void periodic() { +// // Pose2d drivePose = drive.getPose(); + +// // targetDistance = drivePose.getTranslation().getDistance(targetLocation.toTranslation2d()); + +// // var shooterPose = new Pose3d(drivePose).plus(ShooterConstants.OFFSET).getTranslation(); - // ChassisSpeeds driveSpeeds = drive.getFieldSpeeds(); - // ChassisAccels driveAccelerations = ChassisAccels.estimate(driveSpeeds, drive.getPrevFieldSpeeds(), Constants.DT); - - // currentInterceptSolution = ShootOnTheFlyCalculator.solveShootOnTheFly( - // shooterPose, - // targetLocation, - // zero, - // zero1, - // -shooterAngle, - // 5, 0.01); - - // currentEffectiveTargetPose = currentInterceptSolution.effectiveTargetPose(); - // currentEffectiveYaw = currentInterceptSolution.requiredYaw(); - } - - public void setTarget(Translation3d targetLocation) { - this.targetLocation = targetLocation; - } - - public Translation3d getCurrentEffectiveTargetPose() { - return currentEffectiveTargetPose; - } - - public double getCurrentEffectiveYaw() { - return currentEffectiveYaw; - } - - public InterceptSolution getInterceptSolution() { - return currentInterceptSolution; - } -} +// // ChassisSpeeds driveSpeeds = drive.getFieldSpeeds(); +// // ChassisAccels driveAccelerations = ChassisAccels.estimate(driveSpeeds, drive.getPrevFieldSpeeds(), Constants.DT); + +// // currentInterceptSolution = ShootOnTheFlyCalculator.solveShootOnTheFly( +// // shooterPose, +// // targetLocation, +// // zero, +// // zero1, +// // -shooterAngle, +// // 5, 0.01); + +// // currentEffectiveTargetPose = currentInterceptSolution.effectiveTargetPose(); +// // currentEffectiveYaw = currentInterceptSolution.requiredYaw(); +// } + +// public void setTarget(Translation3d targetLocation) { +// this.targetLocation = targetLocation; +// } + +// public Translation3d getCurrentEffectiveTargetPose() { +// return currentEffectiveTargetPose; +// } + +// public double getCurrentEffectiveYaw() { +// return currentEffectiveYaw; +// } + +// public InterceptSolution getInterceptSolution() { +// return currentInterceptSolution; +// } +// } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 85a5e13..bc8b5a6 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -23,6 +23,11 @@ public class VisionConstants { Math.pow(0.35, 1), // vision Math.pow(0.35, 1), Math.pow(Double.POSITIVE_INFINITY, 1)); + public static final Matrix ROTATION_STD_DEVS = + VecBuilder.fill( + Math.pow(0.35, 1), // vision + Math.pow(0.35, 1), + Math.pow(0.35, 1)); public static enum VisionDeviceConstants { FR_CONSTANTS ( diff --git a/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java b/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java index 3a7c74c..2d4d6d8 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java @@ -58,8 +58,6 @@ public VisionDeviceManager() { visionSim.addAprilTags(FieldLayout.APRILTAG_MAP); cameras.forEach((camera) -> visionSim.addCamera(camera.getSimulation(), camera.getConstants().robotToCamera)); } - Drive.getInstance().getCtreDrive().setVisionMeasurementStdDevs(LOCAL_MEASUREMENT_STD_DEVS); - Drive.getInstance().getCtreDrive().setStateStdDevs(STATE_STD_DEVS); io = new VisionIO(getName(), this); // TelemetryManager.getInstance().addSendable(this); } From a6a2e94e0245a0e6844a0a6e728b9de77987aad7 Mon Sep 17 00:00:00 2001 From: Arvind Venkatesh Date: Fri, 6 Mar 2026 18:17:36 -0800 Subject: [PATCH 105/118] feat: auto left --- simgui-ds.json | 5 - .../deploy/pathplanner/autos/AutoLeft.auto | 55 +++++ src/main/deploy/pathplanner/navgrid.json | 2 +- .../deploy/pathplanner/paths/HangLeft1.path | 54 ++++ .../deploy/pathplanner/paths/HangLeft2.path | 54 ++++ .../deploy/pathplanner/paths/PreloadLeft.path | 54 ++++ .../deploy/pathplanner/paths/testPath.path | 91 ------- .../deploy/pathplanner/paths/testPath2.path | 203 ---------------- src/main/deploy/pathplanner/settings.json | 6 +- src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/Constants.java | 230 +++++++++--------- src/main/java/frc/robot/ControlsMapping.java | 18 +- src/main/java/frc/robot/Robot.java | 13 +- 13 files changed, 365 insertions(+), 430 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/AutoLeft.auto create mode 100644 src/main/deploy/pathplanner/paths/HangLeft1.path create mode 100644 src/main/deploy/pathplanner/paths/HangLeft2.path create mode 100644 src/main/deploy/pathplanner/paths/PreloadLeft.path delete mode 100644 src/main/deploy/pathplanner/paths/testPath.path delete mode 100644 src/main/deploy/pathplanner/paths/testPath2.path diff --git a/simgui-ds.json b/simgui-ds.json index aaec342..cae181e 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,9 +1,4 @@ { - "Keyboard 0 Settings": { - "window": { - "visible": true - } - }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/deploy/pathplanner/autos/AutoLeft.auto b/src/main/deploy/pathplanner/autos/AutoLeft.auto new file mode 100644 index 0000000..8524c50 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/AutoLeft.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "PreloadLeft" + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "stopShoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "path", + "data": { + "pathName": "HangLeft1" + } + }, + { + "type": "path", + "data": { + "pathName": "HangLeft2" + } + }, + { + "type": "named", + "data": { + "name": "hang" + } + } + ] + } + }, + "resetOdom": false, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json index 7a1e1ce..ac5f521 100644 --- a/src/main/deploy/pathplanner/navgrid.json +++ b/src/main/deploy/pathplanner/navgrid.json @@ -1 +1 @@ -{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file +{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HangLeft1.path b/src/main/deploy/pathplanner/paths/HangLeft1.path new file mode 100644 index 0000000..acd2a64 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HangLeft1.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.29, + "y": 2.017 + }, + "prevControl": null, + "nextControl": { + "x": 2.924538197409069, + "y": 2.3929658255457746 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.06, + "y": 2.017 + }, + "prevControl": { + "x": 1.0830664619292123, + "y": 1.7680663977401438 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 45.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HangLeft2.path b/src/main/deploy/pathplanner/paths/HangLeft2.path new file mode 100644 index 0000000..d91123e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HangLeft2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.06, + "y": 2.017 + }, + "prevControl": null, + "nextControl": { + "x": 1.0662045517242758, + "y": 2.772060322299513 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.06, + "y": 2.95 + }, + "prevControl": { + "x": 1.0482405717754884, + "y": 2.700276721453861 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/PreloadLeft.path b/src/main/deploy/pathplanner/paths/PreloadLeft.path new file mode 100644 index 0000000..96137bd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/PreloadLeft.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.7217407078598486, + "y": 0.36528959517045334 + }, + "prevControl": null, + "nextControl": { + "x": 4.491448554596527, + "y": 0.055810175445162036 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.29, + "y": 2.017 + }, + "prevControl": { + "x": 2.1843002308162807, + "y": 1.76913764224765 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 45.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/testPath.path b/src/main/deploy/pathplanner/paths/testPath.path deleted file mode 100644 index 7ecb284..0000000 --- a/src/main/deploy/pathplanner/paths/testPath.path +++ /dev/null @@ -1,91 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 0.0, - "y": 0.0 - }, - "prevControl": null, - "nextControl": { - "x": 1.6543032786885246, - "y": 0.15896516393442653 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.870081967215316, - "y": 0.7583504098420167 - }, - "prevControl": { - "x": 2.025922131149742, - "y": 0.386731557383 - }, - "nextControl": { - "x": 1.5354564647707203, - "y": 1.5563035310560531 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.4265368852459013, - "y": 1.6094774590163934 - }, - "prevControl": { - "x": 1.798155737707119, - "y": 1.6574282786944752 - }, - "nextControl": { - "x": 0.3915104179993174, - "y": 1.4759256567752452 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.0, - "y": 0.0 - }, - "prevControl": { - "x": 0.45553278688744697, - "y": 0.9501536885305403 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.57, - "rotationDegrees": 180.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/testPath2.path b/src/main/deploy/pathplanner/paths/testPath2.path deleted file mode 100644 index 3f70b5f..0000000 --- a/src/main/deploy/pathplanner/paths/testPath2.path +++ /dev/null @@ -1,203 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 0.0, - "y": 0.0 - }, - "prevControl": null, - "nextControl": { - "x": 1.6543032786885246, - "y": 0.15896516393442653 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.485761730491173, - "y": 4.037301172765461 - }, - "prevControl": { - "x": 5.509565134948722, - "y": 3.226444756275118 - }, - "nextControl": { - "x": 5.460371714516559, - "y": 4.902205073571968 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.735055812607463, - "y": 6.133043272882445 - }, - "prevControl": { - "x": 5.106674665068683, - "y": 6.1809940925605265 - }, - "nextControl": { - "x": 3.7000293453608792, - "y": 5.999491470641297 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.780072232928514, - "y": 6.011002645344107 - }, - "prevControl": { - "x": 3.5326180679689743, - "y": 5.975415621807755 - }, - "nextControl": { - "x": 4.027526397888053, - "y": 6.0465896688804595 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.7878323651372563, - "y": 6.008715381856753 - }, - "prevControl": { - "x": 3.539200549866918, - "y": 5.9825960320299245 - }, - "nextControl": { - "x": 4.036464180407595, - "y": 6.034834731683581 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.045238038417188, - "y": 7.650092201562578 - }, - "prevControl": { - "x": 1.7991783016715992, - "y": 7.6058812758462855 - }, - "nextControl": { - "x": 2.2912977751627763, - "y": 7.694303127278871 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.9744459885853738, - "y": 4.6167229448664475 - }, - "prevControl": { - "x": 0.7248159400374581, - "y": 4.630318490532778 - }, - "nextControl": { - "x": 1.2240760371332904, - "y": 4.603127399200117 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.833662245012324, - "y": 2.853709352937069 - }, - "prevControl": { - "x": 5.814051418158163, - "y": 3.102938996980663 - }, - "nextControl": { - "x": 5.853273071866484, - "y": 2.604479708893476 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.345091538394257, - "y": 5.195327906283456 - }, - "prevControl": { - "x": 3.7842404867504196, - "y": 5.213024584543108 - }, - "nextControl": { - "x": 2.35508229182681, - "y": 5.155432842942953 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.967506839071151, - "y": 2.853709352937069 - }, - "prevControl": { - "x": 3.742412623667063, - "y": 4.167081416357846 - }, - "nextControl": { - "x": 2.192601054475239, - "y": 1.5403372895162928 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.0, - "y": 0.0 - }, - "prevControl": { - "x": 0.45553278688744697, - "y": 0.9501536885305403 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.57, - "rotationDegrees": 180.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 5bf438c..746e71b 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.9, - "robotLength": 0.9, + "robotWidth": 0.711, + "robotLength": 0.7112, "holonomicMode": true, "pathFolders": [], "autoFolders": [], @@ -9,7 +9,7 @@ "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 74.088, + "robotMass": 52.1631, "robotMOI": 6.883, "robotTrackwidth": 0.546, "driveWheelRadius": 0.048, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 806b516..175c056 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Robot2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 172; - public static final String GIT_SHA = "695bc91a4d8a133e53361806bb43aa32164c82a3"; - public static final String GIT_DATE = "2026-03-05 22:09:47 EST"; + public static final int GIT_REVISION = 173; + public static final String GIT_SHA = "177df4b78996e981f8083f7e2def65bf915f6b97"; + public static final String GIT_DATE = "2026-03-06 01:14:21 EST"; public static final String GIT_BRANCH = "sotm"; - public static final String BUILD_DATE = "2026-03-06 01:04:17 EST"; - public static final long BUILD_UNIX_TIME = 1772777057559L; + public static final String BUILD_DATE = "2026-03-06 21:07:09 EST"; + public static final long BUILD_UNIX_TIME = 1772849229825L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9d91712..bb94335 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -28,7 +28,6 @@ public static final class Controllers { public static final class Odometry { } - public static final class Pathplanner { public static RobotConfig config; @@ -39,10 +38,9 @@ public static final class Pathplanner { DriverStation.reportError("Pathplanner configs failed to load ", e.getStackTrace()); } } - public static final PathConstraints GLOBAL_CONSTRAINTS = - new PathConstraints(DriveConstants.MAX_SPEED * 0.85, - DriveConstants.MAX_ACCEL * 0.85, - DriveConstants.MAX_ROTATION_SPEED * 0.85, + public static final PathConstraints GLOBAL_CONSTRAINTS = new PathConstraints(DriveConstants.MAX_SPEED * 0.85, + DriveConstants.MAX_ACCEL * 0.85, + DriveConstants.MAX_ROTATION_SPEED * 0.85, DriveConstants.MAX_ROTATION_ACCEL * 0.85); public static final double GENERATION_WAIT_TIME = 5; } @@ -54,18 +52,24 @@ public static final class Pathplanner { // license that can be found in the LICENSE file at // the root directory of this project. /** - * Contains information for location of field element and other useful reference points. + * Contains information for location of field element and other useful reference + * points. * - *

NOTE: All constants are defined relative to the field coordinate system, and from the + *

+ * NOTE: All constants are defined relative to the field coordinate system, and + * from the * perspective of the blue alliance station */ public static class FieldConstants { public static Translation3d allianceCorrected(Translation3d t) { - Alliance a = DriverStation.getAlliance().orElseThrow(); - if (a == Alliance.Red) { - var t2d = - FieldUtil.flipTranslation(t.toTranslation2d()); - return new Translation3d(t2d.getX(), t2d.getY(), t.getZ()); + if (DriverStation.getAlliance().isPresent()) { + Alliance a = DriverStation.getAlliance().orElseThrow(); + if (a == Alliance.Red) { + var t2d = FieldUtil.flipTranslation(t.toTranslation2d()); + return new Translation3d(t2d.getX(), t2d.getY(), t.getZ()); + } else { + return t; + } } else { return t; } @@ -83,27 +87,29 @@ public static Translation3d allianceCorrected(Translation3d t) { public static final double fuelDiameter = edu.wpi.first.math.util.Units.inchesToMeters(5.91); /** - * Officially defined and relevant vertical lines found on the field (defined by X-axis offset) + * Officially defined and relevant vertical lines found on the field (defined by + * X-axis offset) */ public static class LinesVertical { public static final double center = fieldLength / 2.0; - public static final double starting = - FieldLayout.APRILTAG_MAP.getTagPose(26).get().getX(); + public static final double starting = FieldLayout.APRILTAG_MAP.getTagPose(26).get().getX(); public static final double allianceZone = starting; - public static final double hubCenter = - FieldLayout.APRILTAG_MAP.getTagPose(26).get().getX() + Hub.width / 2.0; + public static final double hubCenter = FieldLayout.APRILTAG_MAP.getTagPose(26).get().getX() + + Hub.width / 2.0; public static final double neutralZoneNear = center - edu.wpi.first.math.util.Units.inchesToMeters(120); public static final double neutralZoneFar = center + edu.wpi.first.math.util.Units.inchesToMeters(120); - public static final double oppHubCenter = - FieldLayout.APRILTAG_MAP.getTagPose(4).get().getX() + Hub.width / 2.0; - public static final double oppAllianceZone = - FieldLayout.APRILTAG_MAP.getTagPose(10).get().getX(); + public static final double oppHubCenter = FieldLayout.APRILTAG_MAP.getTagPose(4).get().getX() + + Hub.width / 2.0; + public static final double oppAllianceZone = FieldLayout.APRILTAG_MAP.getTagPose(10).get().getX(); } /** - * Officially defined and relevant horizontal lines found on the field (defined by Y-axis offset) + * Officially defined and relevant horizontal lines found on the field (defined + * by Y-axis offset) * - *

NOTE: The field element start and end are always left to right from the perspective of the + *

+ * NOTE: The field element start and end are always left to right from the + * perspective of the * alliance station */ public static class LinesHorizontal { @@ -114,14 +120,16 @@ public static class LinesHorizontal { public static final double rightBumpStart = Hub.nearRightCorner.getY(); public static final double rightBumpEnd = rightBumpStart - RightBump.width; public static final double rightBumpMiddle = (rightBumpStart + rightBumpEnd) / 2.0; - public static final double rightTrenchOpenStart = rightBumpEnd - edu.wpi.first.math.util.Units.inchesToMeters(12.0); + public static final double rightTrenchOpenStart = rightBumpEnd + - edu.wpi.first.math.util.Units.inchesToMeters(12.0); public static final double rightTrenchOpenEnd = 0; // Left of hub public static final double leftBumpEnd = Hub.nearLeftCorner.getY(); public static final double leftBumpStart = leftBumpEnd + LeftBump.width; public static final double leftBumpMiddle = (leftBumpStart + leftBumpEnd) / 2.0; - public static final double leftTrenchOpenEnd = leftBumpStart + edu.wpi.first.math.util.Units.inchesToMeters(12.0); + public static final double leftTrenchOpenEnd = leftBumpStart + + edu.wpi.first.math.util.Units.inchesToMeters(12.0); public static final double leftTrenchOpenStart = fieldWidth; } @@ -129,56 +137,50 @@ public static class LinesHorizontal { public static class Hub { // Dimensions public static final double width = edu.wpi.first.math.util.Units.inchesToMeters(46.0); - public static final double height = - edu.wpi.first.math.util.Units.inchesToMeters(72.0); // includes the catcher at the top + public static final double height = edu.wpi.first.math.util.Units.inchesToMeters(72.0); // includes the + // catcher at the + // top public static final double innerWidth = edu.wpi.first.math.util.Units.inchesToMeters(41.7); public static final double innerHeight = edu.wpi.first.math.util.Units.inchesToMeters(56.5); // Relevant reference points on alliance side - public static final Translation3d topCenterPoint = - new Translation3d( + public static final Translation3d topCenterPoint = new Translation3d( FieldLayout.APRILTAG_MAP.getTagPose(26).get().getX() + width / 2.0, fieldWidth / 2.0, height); - public static final Translation3d innerCenterPoint = - new Translation3d( + public static final Translation3d innerCenterPoint = new Translation3d( FieldLayout.APRILTAG_MAP.getTagPose(26).get().getX() + width / 2.0, fieldWidth / 2.0, innerHeight); - public static final Translation2d nearLeftCorner = - new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0); - public static final Translation2d nearRightCorner = - new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0); - public static final Translation2d farLeftCorner = - new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0); - public static final Translation2d farRightCorner = - new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0); + public static final Translation2d nearLeftCorner = new Translation2d(topCenterPoint.getX() - width / 2.0, + fieldWidth / 2.0 + width / 2.0); + public static final Translation2d nearRightCorner = new Translation2d(topCenterPoint.getX() - width / 2.0, + fieldWidth / 2.0 - width / 2.0); + public static final Translation2d farLeftCorner = new Translation2d(topCenterPoint.getX() + width / 2.0, + fieldWidth / 2.0 + width / 2.0); + public static final Translation2d farRightCorner = new Translation2d(topCenterPoint.getX() + width / 2.0, + fieldWidth / 2.0 - width / 2.0); // Relevant reference points on the opposite side - public static final Translation3d oppTopCenterPoint = - new Translation3d( + public static final Translation3d oppTopCenterPoint = new Translation3d( FieldLayout.APRILTAG_MAP.getTagPose(4).get().getX() + width / 2.0, fieldWidth / 2.0, height); - public static final Translation2d oppNearLeftCorner = - new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0); - public static final Translation2d oppNearRightCorner = - new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0); - public static final Translation2d oppFarLeftCorner = - new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0); - public static final Translation2d oppFarRightCorner = - new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0); + public static final Translation2d oppNearLeftCorner = new Translation2d( + oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d oppNearRightCorner = new Translation2d( + oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0); + public static final Translation2d oppFarLeftCorner = new Translation2d( + oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d oppFarRightCorner = new Translation2d( + oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0); // Hub faces - public static final Pose2d nearFace = - FieldLayout.APRILTAG_MAP.getTagPose(26).get().toPose2d(); - public static final Pose2d farFace = - FieldLayout.APRILTAG_MAP.getTagPose(20).get().toPose2d(); - public static final Pose2d rightFace = - FieldLayout.APRILTAG_MAP.getTagPose(18).get().toPose2d(); - public static final Pose2d leftFace = - FieldLayout.APRILTAG_MAP.getTagPose(21).get().toPose2d(); + public static final Pose2d nearFace = FieldLayout.APRILTAG_MAP.getTagPose(26).get().toPose2d(); + public static final Pose2d farFace = FieldLayout.APRILTAG_MAP.getTagPose(20).get().toPose2d(); + public static final Pose2d rightFace = FieldLayout.APRILTAG_MAP.getTagPose(18).get().toPose2d(); + public static final Pose2d leftFace = FieldLayout.APRILTAG_MAP.getTagPose(21).get().toPose2d(); } /** Left Bump related constants */ @@ -190,19 +192,19 @@ public static class LeftBump { public static final double depth = edu.wpi.first.math.util.Units.inchesToMeters(44.4); // Relevant reference points on alliance side - public static final Translation2d nearLeftCorner = - new Translation2d(LinesVertical.hubCenter - width / 2, edu.wpi.first.math.util.Units.inchesToMeters(255)); + public static final Translation2d nearLeftCorner = new Translation2d(LinesVertical.hubCenter - width / 2, + edu.wpi.first.math.util.Units.inchesToMeters(255)); public static final Translation2d nearRightCorner = Hub.nearLeftCorner; - public static final Translation2d farLeftCorner = - new Translation2d(LinesVertical.hubCenter + width / 2, edu.wpi.first.math.util.Units.inchesToMeters(255)); + public static final Translation2d farLeftCorner = new Translation2d(LinesVertical.hubCenter + width / 2, + edu.wpi.first.math.util.Units.inchesToMeters(255)); public static final Translation2d farRightCorner = Hub.farLeftCorner; // Relevant reference points on opposing side - public static final Translation2d oppNearLeftCorner = - new Translation2d(LinesVertical.hubCenter - width / 2, edu.wpi.first.math.util.Units.inchesToMeters(255)); + public static final Translation2d oppNearLeftCorner = new Translation2d(LinesVertical.hubCenter - width / 2, + edu.wpi.first.math.util.Units.inchesToMeters(255)); public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner; - public static final Translation2d oppFarLeftCorner = - new Translation2d(LinesVertical.hubCenter + width / 2, edu.wpi.first.math.util.Units.inchesToMeters(255)); + public static final Translation2d oppFarLeftCorner = new Translation2d(LinesVertical.hubCenter + width / 2, + edu.wpi.first.math.util.Units.inchesToMeters(255)); public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner; } @@ -214,19 +216,19 @@ public static class RightBump { public static final double depth = edu.wpi.first.math.util.Units.inchesToMeters(44.4); // Relevant reference points on alliance side - public static final Translation2d nearLeftCorner = - new Translation2d(LinesVertical.hubCenter + width / 2, edu.wpi.first.math.util.Units.inchesToMeters(255)); + public static final Translation2d nearLeftCorner = new Translation2d(LinesVertical.hubCenter + width / 2, + edu.wpi.first.math.util.Units.inchesToMeters(255)); public static final Translation2d nearRightCorner = Hub.nearLeftCorner; - public static final Translation2d farLeftCorner = - new Translation2d(LinesVertical.hubCenter - width / 2, edu.wpi.first.math.util.Units.inchesToMeters(255)); + public static final Translation2d farLeftCorner = new Translation2d(LinesVertical.hubCenter - width / 2, + edu.wpi.first.math.util.Units.inchesToMeters(255)); public static final Translation2d farRightCorner = Hub.farLeftCorner; // Relevant reference points on opposing side - public static final Translation2d oppNearLeftCorner = - new Translation2d(LinesVertical.hubCenter + width / 2, edu.wpi.first.math.util.Units.inchesToMeters(255)); + public static final Translation2d oppNearLeftCorner = new Translation2d(LinesVertical.hubCenter + width / 2, + edu.wpi.first.math.util.Units.inchesToMeters(255)); public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner; - public static final Translation2d oppFarLeftCorner = - new Translation2d(LinesVertical.hubCenter - width / 2, edu.wpi.first.math.util.Units.inchesToMeters(255)); + public static final Translation2d oppFarLeftCorner = new Translation2d(LinesVertical.hubCenter - width / 2, + edu.wpi.first.math.util.Units.inchesToMeters(255)); public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner; } @@ -240,16 +242,16 @@ public static class LeftTrench { public static final double openingHeight = edu.wpi.first.math.util.Units.inchesToMeters(22.25); // Relevant reference points on alliance side - public static final Translation3d openingTopLeft = - new Translation3d(LinesVertical.hubCenter, fieldWidth, openingHeight); - public static final Translation3d openingTopRight = - new Translation3d(LinesVertical.hubCenter, fieldWidth - openingWidth, openingHeight); + public static final Translation3d openingTopLeft = new Translation3d(LinesVertical.hubCenter, fieldWidth, + openingHeight); + public static final Translation3d openingTopRight = new Translation3d(LinesVertical.hubCenter, + fieldWidth - openingWidth, openingHeight); // Relevant reference points on opposing side - public static final Translation3d oppOpeningTopLeft = - new Translation3d(LinesVertical.oppHubCenter, fieldWidth, openingHeight); - public static final Translation3d oppOpeningTopRight = - new Translation3d(LinesVertical.oppHubCenter, fieldWidth - openingWidth, openingHeight); + public static final Translation3d oppOpeningTopLeft = new Translation3d(LinesVertical.oppHubCenter, + fieldWidth, openingHeight); + public static final Translation3d oppOpeningTopRight = new Translation3d(LinesVertical.oppHubCenter, + fieldWidth - openingWidth, openingHeight); } public static class RightTrench { @@ -262,16 +264,16 @@ public static class RightTrench { public static final double openingHeight = edu.wpi.first.math.util.Units.inchesToMeters(22.25); // Relevant reference points on alliance side - public static final Translation3d openingTopLeft = - new Translation3d(LinesVertical.hubCenter, openingWidth, openingHeight); - public static final Translation3d openingTopRight = - new Translation3d(LinesVertical.hubCenter, 0, openingHeight); + public static final Translation3d openingTopLeft = new Translation3d(LinesVertical.hubCenter, openingWidth, + openingHeight); + public static final Translation3d openingTopRight = new Translation3d(LinesVertical.hubCenter, 0, + openingHeight); // Relevant reference points on opposing side - public static final Translation3d oppOpeningTopLeft = - new Translation3d(LinesVertical.oppHubCenter, openingWidth, openingHeight); - public static final Translation3d oppOpeningTopRight = - new Translation3d(LinesVertical.oppHubCenter, 0, openingHeight); + public static final Translation3d oppOpeningTopLeft = new Translation3d(LinesVertical.oppHubCenter, + openingWidth, openingHeight); + public static final Translation3d oppOpeningTopRight = new Translation3d(LinesVertical.oppHubCenter, 0, + openingHeight); } /** Tower related constants */ @@ -291,39 +293,33 @@ public static class Tower { public static final double highRungHeight = edu.wpi.first.math.util.Units.inchesToMeters(63.0); // Relevant reference points on alliance side - public static final Translation2d centerPoint = - new Translation2d( + public static final Translation2d centerPoint = new Translation2d( frontFaceX, FieldLayout.APRILTAG_MAP.getTagPose(31).get().getY()); - public static final Translation2d leftUpright = - new Translation2d( + public static final Translation2d leftUpright = new Translation2d( frontFaceX, (FieldLayout.APRILTAG_MAP.getTagPose(31).get().getY()) - + innerOpeningWidth / 2 - + edu.wpi.first.math.util.Units.inchesToMeters(0.75)); - public static final Translation2d rightUpright = - new Translation2d( + + innerOpeningWidth / 2 + + edu.wpi.first.math.util.Units.inchesToMeters(0.75)); + public static final Translation2d rightUpright = new Translation2d( frontFaceX, (FieldLayout.APRILTAG_MAP.getTagPose(31).get().getY()) - - innerOpeningWidth / 2 - - edu.wpi.first.math.util.Units.inchesToMeters(0.75)); + - innerOpeningWidth / 2 + - edu.wpi.first.math.util.Units.inchesToMeters(0.75)); // Relevant reference points on opposing side - public static final Translation2d oppCenterPoint = - new Translation2d( + public static final Translation2d oppCenterPoint = new Translation2d( fieldLength - frontFaceX, FieldLayout.APRILTAG_MAP.getTagPose(15).get().getY()); - public static final Translation2d oppLeftUpright = - new Translation2d( + public static final Translation2d oppLeftUpright = new Translation2d( fieldLength - frontFaceX, (FieldLayout.APRILTAG_MAP.getTagPose(15).get().getY()) - + innerOpeningWidth / 2 - + edu.wpi.first.math.util.Units.inchesToMeters(0.75)); - public static final Translation2d oppRightUpright = - new Translation2d( + + innerOpeningWidth / 2 + + edu.wpi.first.math.util.Units.inchesToMeters(0.75)); + public static final Translation2d oppRightUpright = new Translation2d( fieldLength - frontFaceX, (FieldLayout.APRILTAG_MAP.getTagPose(15).get().getY()) - - innerOpeningWidth / 2 - - edu.wpi.first.math.util.Units.inchesToMeters(0.75)); + - innerOpeningWidth / 2 + - edu.wpi.first.math.util.Units.inchesToMeters(0.75)); } public static class Depot { @@ -334,12 +330,12 @@ public static class Depot { public static final double distanceFromCenterY = edu.wpi.first.math.util.Units.inchesToMeters(75.93); // Relevant reference points on alliance side - public static final Translation3d depotCenter = - new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY, height); - public static final Translation3d leftCorner = - new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY + (width / 2), height); - public static final Translation3d rightCorner = - new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY - (width / 2), height); + public static final Translation3d depotCenter = new Translation3d(depth, + (fieldWidth / 2) + distanceFromCenterY, height); + public static final Translation3d leftCorner = new Translation3d(depth, + (fieldWidth / 2) + distanceFromCenterY + (width / 2), height); + public static final Translation3d rightCorner = new Translation3d(depth, + (fieldWidth / 2) + distanceFromCenterY - (width / 2), height); } public static class Outpost { @@ -349,8 +345,8 @@ public static class Outpost { public static final double height = edu.wpi.first.math.util.Units.inchesToMeters(7.0); // Relevant reference points on alliance side - public static final Translation2d centerPoint = - new Translation2d(0, FieldLayout.APRILTAG_MAP.getTagPose(29).get().getY()); + public static final Translation2d centerPoint = new Translation2d(0, + FieldLayout.APRILTAG_MAP.getTagPose(29).get().getY()); } } } diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index e0501ab..899055c 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -69,8 +69,22 @@ public static void mapTeleopCommand() { // ) // ).onFalse( // stopShoot() - // ) - // ); + // ); + + controller.rightBumper().whileTrue( + Commands.parallel( + Shooter.getRightInstance().shoot( + () -> SmartDashboard.getNumber("rightv", 30.0) - 15, + () -> SmartDashboard.getNumber("rightv", 30.0) + 15 + ), + Shooter.getLeftInstance().shoot( + () -> SmartDashboard.getNumber("leftv", 30.0) - 15, + () -> SmartDashboard.getNumber("leftv", 30.0) + 15 + ) + ) + ).onFalse( + stopShoot() + ); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 49fb9a1..144d8dd 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -15,7 +15,7 @@ import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; - +import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.FollowPathCommand; import edu.wpi.first.wpilibj.DataLogManager; @@ -128,8 +128,14 @@ public Robot() { // Intake.getInstance(); // Climb.getInstance(); - SmartDashboard.putNumber("rightV", 30); - SmartDashboard.putNumber("leftV", 30); + SmartDashboard.putNumber("rightv", 30); + SmartDashboard.putNumber("leftv", 30); + + NamedCommands.registerCommand("shoot", Commands.parallel(Shooter.getLeftInstance().shoot(), Shooter.getRightInstance().shoot())); + NamedCommands.registerCommand("hang", Commands.parallel(Climb.getInstance().hangCommand())); + NamedCommands.registerCommand("stopShoot", Commands.parallel( + Shooter.getRightInstance().stop(), + Shooter.getLeftInstance().stop())); TelemetryManager.getInstance(); commandScheduler.schedule(FollowPathCommand.warmupCommand()); @@ -194,6 +200,7 @@ public void autonomousInit() { commandScheduler.schedule( Led.getInstance().setSolidColorCommand(Color.kBlue)); + autoCommand = autoChooser.getAuto(); if (autoCommand != null) { commandScheduler.schedule(autoCommand); From 479473ffcc67b97a8d36cca6c117c2b1adff9bfa Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Fri, 6 Mar 2026 19:56:55 -0800 Subject: [PATCH 106/118] Emergency changes trust also right auto --- src/main/deploy/choreo/rightTrench.traj | 27 ++ src/main/deploy/choreo/testPath3.traj | 403 ------------------ .../pathplanner/paths/ReturnTrenchRight.path | 122 ++++++ .../deploy/pathplanner/paths/SwipeRight.path | 54 +++ .../deploy/pathplanner/paths/TrenchRight.path | 75 ++++ src/main/deploy/pathplanner/settings.json | 28 +- src/main/java/frc/robot/BuildConstants.java | 10 +- .../java/frc/robot/auto/AutoRoutines.java | 62 +++ .../java/frc/robot/auto/AutoSelector.java | 47 +- src/main/java/frc/robot/auto/Automation.java | 39 +- .../drive/commands/HeadingLockToHub2.java | 131 ++++++ .../drive/commands/TrajectoryCommand.java | 1 + 12 files changed, 555 insertions(+), 444 deletions(-) create mode 100644 src/main/deploy/choreo/rightTrench.traj delete mode 100644 src/main/deploy/choreo/testPath3.traj create mode 100644 src/main/deploy/pathplanner/paths/ReturnTrenchRight.path create mode 100644 src/main/deploy/pathplanner/paths/SwipeRight.path create mode 100644 src/main/deploy/pathplanner/paths/TrenchRight.path create mode 100644 src/main/java/frc/robot/subsystems/drive/commands/HeadingLockToHub2.java diff --git a/src/main/deploy/choreo/rightTrench.traj b/src/main/deploy/choreo/rightTrench.traj new file mode 100644 index 0000000..6d56648 --- /dev/null +++ b/src/main/deploy/choreo/rightTrench.traj @@ -0,0 +1,27 @@ +{ + "name":"rightTrench", + "version":1, + "snapshot":{ + "waypoints":[], + "constraints":[], + "targetDt":0.05 + }, + "params":{ + "waypoints":[], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":null, + "waypoints":[], + "samples":[], + "splits":[] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/testPath3.traj b/src/main/deploy/choreo/testPath3.traj deleted file mode 100644 index 5f72622..0000000 --- a/src/main/deploy/choreo/testPath3.traj +++ /dev/null @@ -1,403 +0,0 @@ -{ - "name":"testPath3", - "version":1, - "snapshot":{ - "waypoints":[ - {"x":7.125288963317871, "y":1.5220201015472412, "heading":2.297438401528901, "intervals":58, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.6629953384399414, "y":3.0058600902557373, "heading":1.0010398833246623, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.1074931621551514, "y":0.9861889481544496, "heading":0.8709035133255352, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.951519966125488, "y":2.7791624069213867, "heading":1.0427221780766034, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.0868842601776123, "y":1.0067977905273438, "heading":0.8818720101391697, "intervals":54, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.147773265838623, "y":3.87143349647522, "heading":0.0, "intervals":55, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.010371446609497, "y":6.991826057434082, "heading":-0.9272954961807792, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.684423923492432, "y":5.05485200881958, "heading":-1.0240075818295364, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], - "constraints":[ - {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":4, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":1, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":5, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":7, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":6, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}], - "targetDt":0.05 - }, - "params":{ - "waypoints":[ - {"x":{"exp":"7.125288963317871 m", "val":7.125288963317871}, "y":{"exp":"1.5220201015472412 m", "val":1.5220201015472412}, "heading":{"exp":"2.297438401528901 rad", "val":2.297438401528901}, "intervals":58, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.6629953384399414 m", "val":3.6629953384399414}, "y":{"exp":"3.0058600902557373 m", "val":3.0058600902557373}, "heading":{"exp":"1.0010398833246623 rad", "val":1.0010398833246623}, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.1074931621551514 m", "val":1.1074931621551514}, "y":{"exp":"0.9861889481544495 m", "val":0.9861889481544496}, "heading":{"exp":"0.8709035133255352 rad", "val":0.8709035133255352}, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.9515199661254883 m", "val":3.951519966125488}, "y":{"exp":"2.7791624069213867 m", "val":2.7791624069213867}, "heading":{"exp":"1.0427221780766034 rad", "val":1.0427221780766034}, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.0868842601776123 m", "val":1.0868842601776123}, "y":{"exp":"1.0067977905273438 m", "val":1.0067977905273438}, "heading":{"exp":"0.8818720101391697 rad", "val":0.8818720101391697}, "intervals":54, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.147773265838623 m", "val":3.147773265838623}, "y":{"exp":"3.8714334964752197 m", "val":3.87143349647522}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":55, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.010371446609497 m", "val":1.010371446609497}, "y":{"exp":"6.991826057434082 m", "val":6.991826057434082}, "heading":{"exp":"-0.9272954961807793 rad", "val":-0.9272954961807792}, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.6844239234924316 m", "val":3.684423923492432}, "y":{"exp":"5.05485200881958 m", "val":5.05485200881958}, "heading":{"exp":"-1.0240075818295364 rad", "val":-1.0240075818295364}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], - "constraints":[ - {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":3, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":4, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":1, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":5, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":7, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":6, "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}], - "targetDt":{ - "exp":"0.05 s", - "val":0.05 - } - }, - "trajectory":{ - "sampleType":"Swerve", - "waypoints":[0.0,2.07522,3.9942,5.94381,7.89686,9.90077,11.97542,13.90891], - "samples":[ - {"t":0.0, "x":7.12529, "y":1.52202, "heading":2.29744, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.21938, "ay":1.38054, "alpha":-1.19736, "fx":[-40.35714,-45.25012,-47.08586,-42.54132], "fy":[26.24058,16.43546,9.97559,22.49251]}, - {"t":0.03578, "x":7.12323, "y":1.5229, "heading":2.29744, "vx":-0.11519, "vy":0.0494, "omega":-0.04284, "ax":-3.21923, "ay":1.38047, "alpha":-1.19772, "fx":[-40.35437,-45.24878,-47.08442,-42.5385], "fy":[26.24145,16.4342,9.97221,22.49258]}, - {"t":0.07156, "x":7.11705, "y":1.52555, "heading":2.29591, "vx":-0.23037, "vy":0.09879, "omega":-0.0857, "ax":-3.21907, "ay":1.3804, "alpha":-1.1979, "fx":[-40.3479,-45.24331,-47.08328,-42.54309], "fy":[26.24771,16.44395,9.9667,22.4782]}, - {"t":0.10734, "x":7.10674, "y":1.52997, "heading":2.29284, "vx":-0.34555, "vy":0.14818, "omega":-0.12856, "ax":-3.21891, "ay":1.38032, "alpha":-1.19789, "fx":[-40.33775,-45.23368,-47.08242,-42.5551], "fy":[26.25932,16.46473,9.95913,22.44929]}, - {"t":0.14312, "x":7.09232, "y":1.53616, "heading":2.28824, "vx":-0.46072, "vy":0.19757, "omega":-0.17142, "ax":-3.21875, "ay":1.38024, "alpha":-1.19769, "fx":[-40.32392,-45.21984,-47.08179,-42.57453], "fy":[26.2762,16.49654,9.94965,22.40572]}, - {"t":0.1789, "x":7.07377, "y":1.54411, "heading":2.28211, "vx":-0.57589, "vy":0.24695, "omega":-0.21427, "ax":-3.21858, "ay":1.38016, "alpha":-1.19731, "fx":[-40.30648,-45.20175,-47.08131,-42.60137], "fy":[26.29821,16.5394,9.93846,22.34733]}, - {"t":0.21468, "x":7.05111, "y":1.55383, "heading":2.27444, "vx":-0.69104, "vy":0.29633, "omega":-0.25711, "ax":-3.21841, "ay":1.38006, "alpha":-1.19676, "fx":[-40.28547,-45.17933,-47.0809,-42.63562], "fy":[26.3252,16.59335,9.92584,22.2739]}, - {"t":0.25046, "x":7.02432, "y":1.56532, "heading":2.26524, "vx":-0.8062, "vy":0.34571, "omega":-0.29993, "ax":-3.21822, "ay":1.37996, "alpha":-1.19602, "fx":[-40.26099,-45.1525,-47.08046,-42.67727], "fy":[26.35695,16.65839,9.91213,22.18516]}, - {"t":0.28624, "x":6.99342, "y":1.57857, "heading":2.25451, "vx":-0.92134, "vy":0.39508, "omega":-0.34272, "ax":-3.21802, "ay":1.37984, "alpha":-1.19513, "fx":[-40.23313,-45.12116,-47.07986,-42.72631], "fy":[26.39319,16.73454,9.89776,22.08084]}, - {"t":0.32202, "x":6.95839, "y":1.59359, "heading":2.24225, "vx":-1.03648, "vy":0.44445, "omega":-0.38548, "ax":-3.21781, "ay":1.37971, "alpha":-1.19407, "fx":[-40.20202,-45.08521,-47.07894,-42.78268], "fy":[26.43361,16.82181,9.88319,21.96058]}, - {"t":0.3578, "x":6.91925, "y":1.61037, "heading":2.22845, "vx":-1.15162, "vy":0.49382, "omega":-0.42821, "ax":-3.21758, "ay":1.37956, "alpha":-1.19288, "fx":[-40.16781,-45.04449,-47.07755,-42.84634], "fy":[26.47787,16.92022,9.86896,21.82404]}, - {"t":0.39358, "x":6.87599, "y":1.62892, "heading":2.21313, "vx":-1.26674, "vy":0.54318, "omega":-0.47089, "ax":-3.21732, "ay":1.37939, "alpha":-1.19156, "fx":[-40.13065,-44.99888,-47.07549,-42.91717], "fy":[26.52554,17.02977,9.85565,21.67083]}, - {"t":0.42936, "x":6.8286, "y":1.64924, "heading":2.19629, "vx":-1.38185, "vy":0.59253, "omega":-0.51352, "ax":-3.21703, "ay":1.37919, "alpha":-1.19014, "fx":[-40.09072,-44.94819,-47.07254,-42.99505], "fy":[26.57617,17.15043,9.8439,21.50055]}, - {"t":0.46513, "x":6.7771, "y":1.67133, "heading":2.17791, "vx":-1.49696, "vy":0.64188, "omega":-0.5561, "ax":-3.2167, "ay":1.37897, "alpha":-1.18866, "fx":[-40.04821,-44.89223,-47.06845,-43.07978], "fy":[26.62922,17.28217,9.8344,21.31279]}, - {"t":0.50091, "x":6.72148, "y":1.69517, "heading":2.15802, "vx":-1.61205, "vy":0.69122, "omega":-0.59863, "ax":-3.21633, "ay":1.3787, "alpha":-1.18714, "fx":[-40.00332,-44.83077,-47.06295,-43.17109], "fy":[26.6841,17.42495,9.82784,21.10713]}, - {"t":0.53669, "x":6.66174, "y":1.72079, "heading":2.1366, "vx":-1.72713, "vy":0.74055, "omega":-0.64111, "ax":-3.21589, "ay":1.37838, "alpha":-1.18564, "fx":[-39.95625,-44.76355,-47.05568,-43.26863], "fy":[26.74015,17.57867,9.82498,20.88317]}, - {"t":0.57247, "x":6.59789, "y":1.74817, "heading":2.11366, "vx":-1.84219, "vy":0.78987, "omega":-0.68353, "ax":-3.21536, "ay":1.37802, "alpha":-1.1842, "fx":[-39.90717,-44.69025,-47.04627,-43.37192], "fy":[26.79663,17.74323,9.82656,20.64049]}, - {"t":0.60825, "x":6.52992, "y":1.77731, "heading":2.0892, "vx":-1.95724, "vy":0.83917, "omega":-0.7259, "ax":-3.21473, "ay":1.37758, "alpha":-1.1829, "fx":[-39.85623,-44.61049,-47.03424,-43.48034], "fy":[26.8527,17.91845,9.83332,20.37871]}, - {"t":0.64403, "x":6.45783, "y":1.80822, "heading":2.06323, "vx":-2.07226, "vy":0.88846, "omega":-0.76822, "ax":-3.21396, "ay":1.37706, "alpha":-1.18179, "fx":[-39.80347,-44.52378,-47.01899,-43.59305], "fy":[26.90742,18.1041,9.84596,20.09744]}, - {"t":0.67981, "x":6.38163, "y":1.84089, "heading":2.03574, "vx":-2.18725, "vy":0.93773, "omega":-0.81051, "ax":-3.213, "ay":1.37644, "alpha":-1.18099, "fx":[-39.74883,-44.42951,-46.99975,-43.70895], "fy":[26.95971,18.29985,9.8651,19.79628]}, - {"t":0.71559, "x":6.30131, "y":1.87532, "heading":2.00674, "vx":-2.30221, "vy":0.98698, "omega":-0.85276, "ax":-3.21178, "ay":1.37568, "alpha":-1.18058, "fx":[-39.69194,-44.32679,-46.97551,-43.8265], "fy":[27.00833,18.50527,9.89124,19.47477]}, - {"t":0.75137, "x":6.21689, "y":1.91151, "heading":1.97623, "vx":-2.41713, "vy":1.0362, "omega":-0.895, "ax":-3.2102, "ay":1.37474, "alpha":-1.1807, "fx":[-39.632,-44.21438,-46.94478,-43.94352], "fy":[27.05179,18.71969,9.9246,19.13231]}, - {"t":0.78715, "x":6.12835, "y":1.94947, "heading":1.94421, "vx":-2.53199, "vy":1.08539, "omega":-0.93725, "ax":-3.20809, "ay":1.37354, "alpha":-1.18153, "fx":[-39.56729,-44.09033,-46.90532,-44.05676], "fy":[27.08825,18.94213,9.96492,18.76792]}, - {"t":0.82293, "x":6.0357, "y":1.98918, "heading":1.91067, "vx":-2.64677, "vy":1.13453, "omega":-0.97952, "ax":-3.20516, "ay":1.37196, "alpha":-1.18331, "fx":[-39.49437,-43.95138,-46.85344,-44.16097], "fy":[27.11533,19.17104,10.01099,18.37985]}, - {"t":0.85871, "x":5.93895, "y":2.03065, "heading":1.87563, "vx":-2.76145, "vy":1.18362, "omega":-1.02186, "ax":-3.20087, "ay":1.36975, "alpha":-1.18649, "fx":[-39.40597,-43.79147,-46.78242,-44.24681], "fy":[27.12952,19.40363,10.05955,17.96442]}, - {"t":0.89449, "x":5.8381, "y":2.07388, "heading":1.83907, "vx":-2.87598, "vy":1.23263, "omega":-1.06431, "ax":-3.19407, "ay":1.3664, "alpha":-1.19193, "fx":[-39.28549,-43.59781,-46.67828,-44.29513], "fy":[27.12502,19.63434,10.10232,17.51303]}, - {"t":0.93027, "x":5.73315, "y":2.11886, "heading":1.80098, "vx":-2.99026, "vy":1.28152, "omega":-1.10696, "ax":-3.1818, "ay":1.36056, "alpha":-1.20194, "fx":[-39.08806,-43.33745,-46.50547,-44.25777], "fy":[27.08958,19.84946,10.11573,17.00176]}, - {"t":0.96605, "x":5.62412, "y":2.16558, "heading":1.76138, "vx":-3.1041, "vy":1.3302, "omega":-1.14996, "ax":-3.1533, "ay":1.34731, "alpha":-1.22519, "fx":[-38.64694,-42.89164,-46.13689,-43.96215], "fy":[26.98489,20.00125,10.01028,16.33933]}, - {"t":1.00183, "x":5.51104, "y":2.21404, "heading":1.72023, "vx":-3.21693, "vy":1.37841, "omega":-1.1938, "ax":-3.01623, "ay":1.2846, "alpha":-1.33997, "fx":[-36.51767,-41.25951,-44.49323,-41.90641], "fy":[26.49866,19.69498,9.01479,14.71364]}, - {"t":1.03761, "x":5.39401, "y":2.26418, "heading":1.67752, "vx":-3.32485, "vy":1.42437, "omega":-1.24175, "ax":3.02045, "ay":-1.29842, "alpha":0.96586, "fx":[37.95041,41.05616,43.6823,41.71769], "fy":[-24.08183,-19.49802,-11.61026,-15.48454]}, - {"t":1.07339, "x":5.27698, "y":2.31431, "heading":1.63309, "vx":-3.21678, "vy":1.37791, "omega":-1.20719, "ax":3.15676, "ay":-1.35264, "alpha":1.11091, "fx":[39.19593,42.50431,45.82833,44.29711], "fy":[-26.10139,-20.73387,-11.32007,-15.47007]}, - {"t":1.10917, "x":5.16391, "y":2.36275, "heading":1.5899, "vx":-3.10383, "vy":1.32952, "omega":-1.16744, "ax":3.18388, "ay":-1.36332, "alpha":1.14507, "fx":[39.4596,42.66887,46.22256,44.95127], "fy":[-26.48576,-21.21531,-11.38601,-15.12]}, - {"t":1.14495, "x":5.05489, "y":2.40944, "heading":1.54813, "vx":-2.98991, "vy":1.28074, "omega":-1.12647, "ax":3.19536, "ay":-1.36784, "alpha":1.16314, "fx":[39.59666,42.65828,46.36596,45.30622], "fy":[-26.61502,-21.57875,-11.51526,-14.74402]}, - {"t":1.18073, "x":4.94996, "y":2.45439, "heading":1.50782, "vx":-2.87558, "vy":1.2318, "omega":-1.08485, "ax":3.20163, "ay":-1.37034, "alpha":1.17546, "fx":[39.69837,42.59303,46.42468,45.55226], "fy":[-26.64901,-21.89374,-11.66932,-14.37695]}, - {"t":1.21651, "x":4.84912, "y":2.49759, "heading":1.46901, "vx":-2.76103, "vy":1.18276, "omega":-1.04279, "ax":3.20554, "ay":-1.37194, "alpha":1.18484, "fx":[39.78819,42.50589,46.44431,45.74257], "fy":[-26.63332,-22.17924,-11.83567,-14.02804]}, - {"t":1.25229, "x":4.75238, "y":2.53903, "heading":1.4317, "vx":-2.64634, "vy":1.13368, "omega":-1.0004, "ax":3.20818, "ay":-1.37308, "alpha":1.19236, "fx":[39.87429,42.40956,46.44263,45.89828], "fy":[-26.58668,-22.44266,-12.00836,-13.70048]}, - {"t":1.28807, "x":4.65975, "y":2.57871, "heading":1.3959, "vx":-2.53155, "vy":1.08455, "omega":-0.95774, "ax":3.21007, "ay":-1.37395, "alpha":1.19853, "fx":[39.9598,42.31001,46.42812,46.0297], "fy":[-26.51874,-22.68756,-12.18387,-13.39531]}, - {"t":1.32385, "x":4.57123, "y":2.61664, "heading":1.36163, "vx":-2.41669, "vy":1.03539, "omega":-0.91486, "ax":3.21148, "ay":-1.37465, "alpha":1.20363, "fx":[40.04584,42.21046,46.40543,46.14269], "fy":[-26.43536,-22.91595,-12.35978,-13.1126]}, - {"t":1.35962, "x":4.48682, "y":2.6528, "heading":1.3289, "vx":-2.30179, "vy":0.98621, "omega":-0.87179, "ax":3.21257, "ay":-1.37524, "alpha":1.20781, "fx":[40.13263,42.11277,46.37736,46.24094], "fy":[-26.34056,-23.12913,-12.53427,-12.8519]}, - {"t":1.3954, "x":4.40652, "y":2.68721, "heading":1.29771, "vx":-2.18684, "vy":0.937, "omega":-0.82858, "ax":3.21344, "ay":-1.37575, "alpha":1.21121, "fx":[40.21992,42.01812,46.34579,46.32698], "fy":[-26.23737,-23.32802,-12.70593,-12.61248]}, - {"t":1.43118, "x":4.33033, "y":2.71985, "heading":1.26806, "vx":-2.07187, "vy":0.88778, "omega":-0.78524, "ax":3.21414, "ay":-1.37621, "alpha":1.21394, "fx":[40.30724,41.92727,46.31199,46.4027], "fy":[-26.12824,-23.51336,-12.87356,-12.3934]}, - {"t":1.46696, "x":4.25826, "y":2.75074, "heading":1.23997, "vx":-1.95687, "vy":0.83854, "omega":-0.74181, "ax":3.21473, "ay":-1.37662, "alpha":1.21607, "fx":[40.39402,41.84072,46.27693,46.46951], "fy":[-26.01525,-23.68576,-13.03619,-12.19366]}, - {"t":1.50274, "x":4.1903, "y":2.77986, "heading":1.21343, "vx":-1.84185, "vy":0.78928, "omega":-0.69829, "ax":3.21523, "ay":-1.37699, "alpha":1.21768, "fx":[40.47963,41.75878,46.24137,46.52859], "fy":[-25.9002,-23.84576,-13.19299,-12.01217]}, - {"t":1.53852, "x":4.12646, "y":2.80722, "heading":1.18844, "vx":-1.72681, "vy":0.74001, "omega":-0.65473, "ax":3.21566, "ay":-1.37733, "alpha":1.21887, "fx":[40.56346,41.68168,46.20588,46.58088], "fy":[-25.78471,-23.99386,-13.34322,-11.84781]}, - {"t":1.5743, "x":4.06673, "y":2.83281, "heading":1.16501, "vx":-1.61175, "vy":0.69073, "omega":-0.61112, "ax":3.21604, "ay":-1.37764, "alpha":1.21968, "fx":[40.64488,41.60955,46.17096,46.62717], "fy":[-25.67022,-24.1305,-13.48629,-11.6995]}, - {"t":1.61008, "x":4.01112, "y":2.85665, "heading":1.14315, "vx":-1.49668, "vy":0.64144, "omega":-0.56748, "ax":3.21638, "ay":-1.37793, "alpha":1.22019, "fx":[40.72333,41.54245,46.13703,46.66814], "fy":[-25.55806,-24.25614,-13.62163,-11.56615]}, - {"t":1.64586, "x":3.95963, "y":2.87871, "heading":1.12285, "vx":-1.3816, "vy":0.59214, "omega":-0.52382, "ax":3.21668, "ay":-1.37819, "alpha":1.22047, "fx":[40.79828,41.48042,46.10442,46.70439], "fy":[-25.44941,-24.37116,-13.74881,-11.44673]}, - {"t":1.68164, "x":3.91225, "y":2.89902, "heading":1.1041, "vx":-1.26651, "vy":0.54283, "omega":-0.48015, "ax":3.21696, "ay":-1.37842, "alpha":1.22056, "fx":[40.86922,41.42345,46.07346,46.7364], "fy":[-25.34538,-24.47597,-13.86741,-11.34024]}, - {"t":1.71742, "x":3.869, "y":2.91756, "heading":1.08692, "vx":-1.15141, "vy":0.49351, "omega":-0.43648, "ax":3.21721, "ay":-1.37864, "alpha":1.22051, "fx":[40.93572,41.37152,46.04439,46.76461], "fy":[-25.24695,-24.5709,-13.97709,-11.24577]}, - {"t":1.7532, "x":3.82986, "y":2.93433, "heading":1.07131, "vx":-1.0363, "vy":0.44418, "omega":-0.39281, "ax":3.21744, "ay":-1.37883, "alpha":1.22038, "fx":[40.99738,41.32458,46.01745,46.78941], "fy":[-25.155,-24.65631,-14.07755,-11.16246]}, - {"t":1.78898, "x":3.79484, "y":2.94934, "heading":1.05725, "vx":-0.92118, "vy":0.39485, "omega":-0.34915, "ax":3.21765, "ay":-1.37901, "alpha":1.22019, "fx":[41.05386,41.28258,45.99284,46.81112], "fy":[-25.07033,-24.7325,-14.16854,-11.0895]}, - {"t":1.82476, "x":3.76394, "y":2.96259, "heading":1.04476, "vx":-0.80605, "vy":0.34551, "omega":-0.30549, "ax":3.21785, "ay":-1.37916, "alpha":1.21999, "fx":[41.10484,41.24547,45.97074,46.83002], "fy":[-24.99364,-24.79975,-14.24984,-11.02619]}, - {"t":1.86054, "x":3.73716, "y":2.97407, "heading":1.03383, "vx":-0.69092, "vy":0.29616, "omega":-0.26184, "ax":3.21803, "ay":-1.3793, "alpha":1.21981, "fx":[41.15006,41.21319,45.95129,46.84637], "fy":[-24.92557,-24.85832,-14.32127,-10.97189]}, - {"t":1.89632, "x":3.7145, "y":2.98378, "heading":1.02446, "vx":-0.57578, "vy":0.24681, "omega":-0.21819, "ax":3.2182, "ay":-1.37943, "alpha":1.21966, "fx":[41.18931,41.18568,45.93463,46.86036], "fy":[-24.86665,-24.90844,-14.38266,-10.92603]}, - {"t":1.9321, "x":3.69596, "y":2.99173, "heading":1.01665, "vx":-0.46063, "vy":0.19746, "omega":-0.17455, "ax":3.21835, "ay":-1.37954, "alpha":1.21957, "fx":[41.22239,41.16289,45.92088,46.87217], "fy":[-24.81734,-24.9503,-14.43389,-10.88815]}, - {"t":1.96788, "x":3.68154, "y":2.99791, "heading":1.01041, "vx":-0.34548, "vy":0.1481, "omega":-0.13092, "ax":3.21849, "ay":-1.37963, "alpha":1.21956, "fx":[41.24915,41.14479,45.91012,46.88195], "fy":[-24.77802,-24.98406,-14.47486,-10.85783]}, - {"t":2.00366, "x":3.67124, "y":3.00233, "heading":1.00572, "vx":-0.23033, "vy":0.09873, "omega":-0.08728, "ax":3.21862, "ay":-1.37971, "alpha":1.21964, "fx":[41.26947,41.13133,45.90242,46.88982], "fy":[-24.749,-25.00985,-14.50547,-10.83476]}, - {"t":2.03944, "x":3.66506, "y":3.00498, "heading":1.0026, "vx":-0.11517, "vy":0.04937, "omega":-0.04364, "ax":3.21874, "ay":-1.37978, "alpha":1.21981, "fx":[41.28326,41.12249,45.89786,46.89586], "fy":[-24.73053,-25.02779,-14.52567,-10.81871]}, - {"t":2.07522, "x":3.663, "y":3.00586, "heading":1.00104, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.87023, "ay":-2.06751, "alpha":-0.16069, "fx":[-38.7583,-39.7416,-39.35153,-38.37825], "fy":[-28.55723,-27.17332,-27.73786,-29.06862]}, - {"t":2.11883, "x":3.66027, "y":3.00389, "heading":1.00104, "vx":-0.12518, "vy":-0.09017, "omega":-0.00701, "ax":-2.86958, "ay":-2.06814, "alpha":-0.16058, "fx":[-38.74934,-39.73237,-39.34275,-38.36977], "fy":[-28.56558,-27.18287,-27.74657,-29.07621]}, - {"t":2.16244, "x":3.65208, "y":2.99799, "heading":1.00073, "vx":-0.25033, "vy":-0.18037, "omega":-0.01401, "ax":-2.86886, "ay":-2.06884, "alpha":-0.16045, "fx":[-38.73924,-39.7221,-39.33327,-38.36051], "fy":[-28.57508,-27.19353,-27.75589,-29.08444]}, - {"t":2.20606, "x":3.63843, "y":2.98816, "heading":1.00012, "vx":-0.37545, "vy":-0.2706, "omega":-0.02101, "ax":-2.86806, "ay":-2.0696, "alpha":-0.16031, "fx":[-38.72785,-39.71065,-39.32294,-38.35033], "fy":[-28.58587,-27.20544,-27.76597,-29.09346]}, - {"t":2.24967, "x":3.61933, "y":2.97439, "heading":0.99921, "vx":-0.50054, "vy":-0.36086, "omega":-0.028, "ax":-2.86718, "ay":-2.07046, "alpha":-0.16015, "fx":[-38.715,-39.69781,-39.31158,-38.33907], "fy":[-28.59811,-27.2188,-27.77697,-29.1034]}, - {"t":2.29328, "x":3.59477, "y":2.95668, "heading":0.99799, "vx":-0.62558, "vy":-0.45116, "omega":-0.03499, "ax":-2.86618, "ay":-2.07142, "alpha":-0.15998, "fx":[-38.70045,-39.68336,-39.29896,-38.32649], "fy":[-28.612,-27.23385,-27.78913,-29.11446]}, - {"t":2.3369, "x":3.56476, "y":2.93504, "heading":0.99646, "vx":-0.75059, "vy":-0.5415, "omega":-0.04196, "ax":-2.86506, "ay":-2.0725, "alpha":-0.15978, "fx":[-38.68391,-39.667,-39.2848,-38.31234], "fy":[-28.62782,-27.2509,-27.80272,-29.12688]}, - {"t":2.38051, "x":3.5293, "y":2.90945, "heading":0.99463, "vx":-0.87554, "vy":-0.63189, "omega":-0.04893, "ax":-2.86377, "ay":-2.07373, "alpha":-0.15956, "fx":[-38.66503,-39.64835,-39.26874,-38.29626], "fy":[-28.64589,-27.27033,-27.8181,-29.14096]}, - {"t":2.42412, "x":3.48839, "y":2.87992, "heading":0.9925, "vx":-1.00044, "vy":-0.72233, "omega":-0.05589, "ax":-2.8623, "ay":-2.07514, "alpha":-0.1593, "fx":[-38.64332,-39.6269,-39.2503,-38.2778], "fy":[-28.66665,-27.29264,-27.83573,-29.15712]}, - {"t":2.46774, "x":3.44204, "y":2.84644, "heading":0.99006, "vx":-1.12527, "vy":-0.81283, "omega":-0.06284, "ax":-2.8606, "ay":-2.07678, "alpha":-0.159, "fx":[-38.61815,-39.60201,-39.22886,-38.25636], "fy":[-28.69067,-27.31849,-27.85622,-29.17586]}, - {"t":2.51135, "x":3.39024, "y":2.80902, "heading":0.98732, "vx":-1.25003, "vy":-0.90341, "omega":-0.06977, "ax":-2.85859, "ay":-2.0787, "alpha":-0.15865, "fx":[-38.58869,-39.57281,-39.20359,-38.23114], "fy":[-28.71871,-27.34877,-27.88038,-29.19789]}, - {"t":2.55496, "x":3.333, "y":2.76764, "heading":0.98427, "vx":-1.37471, "vy":-0.99407, "omega":-0.07669, "ax":-2.8562, "ay":-2.08098, "alpha":-0.15823, "fx":[-38.55376,-39.53806,-39.17332,-38.20104], "fy":[-28.75183,-27.3847,-27.90934,-29.2242]}, - {"t":2.59857, "x":3.27033, "y":2.7223, "heading":0.98093, "vx":-1.49927, "vy":-1.08483, "omega":-0.08359, "ax":-2.85331, "ay":-2.08374, "alpha":-0.15773, "fx":[-38.5117,-39.49605,-39.13641,-38.16445], "fy":[-28.79153,-27.428,-27.9447,-29.25615]}, - {"t":2.64219, "x":3.20223, "y":2.67301, "heading":0.97728, "vx":-1.62372, "vy":-1.1757, "omega":-0.09047, "ax":-2.84973, "ay":-2.08715, "alpha":-0.15711, "fx":[-38.46004,-39.44422,-39.09042,-38.11905], "fy":[-28.84001,-27.48123,-27.98881,-29.29579]}, - {"t":2.6858, "x":3.12871, "y":2.61975, "heading":0.97334, "vx":-1.748, "vy":-1.26673, "omega":-0.09732, "ax":-2.84519, "ay":-2.09146, "alpha":-0.15632, "fx":[-38.39503,-39.37865,-39.0316,-38.06125], "fy":[-28.90064,-27.54824,-28.04525,-29.34622]}, - {"t":2.72941, "x":3.04976, "y":2.56251, "heading":0.96909, "vx":-1.87209, "vy":-1.35795, "omega":-0.10414, "ax":-2.83924, "ay":-2.09708, "alpha":-0.1553, "fx":[-38.31056,-39.293,-38.95392,-37.98524], "fy":[-28.97882,-27.63529,-28.11978,-29.41242]}, - {"t":2.77303, "x":2.96542, "y":2.5013, "heading":0.96455, "vx":-1.99592, "vy":-1.44941, "omega":-0.11091, "ax":-2.83111, "ay":-2.10472, "alpha":-0.1539, "fx":[-38.19607,-39.17629,-38.84685,-37.88095], "fy":[-29.08385,-27.75306,-28.22233,-29.50301]}, - {"t":2.81664, "x":2.87568, "y":2.43608, "heading":0.95971, "vx":-2.11939, "vy":-1.5412, "omega":-0.11763, "ax":-2.81932, "ay":-2.11571, "alpha":-0.15189, "fx":[-38.03153,-39.00762,-38.69046,-37.72928], "fy":[-29.23314,-27.92163,-28.37151,-29.6341]}, - {"t":2.86025, "x":2.78056, "y":2.36685, "heading":0.95458, "vx":-2.24235, "vy":-1.63347, "omega":-0.12425, "ax":-2.80073, "ay":-2.13286, "alpha":-0.14873, "fx":[-37.77387,-38.742,-38.44167,-37.4891], "fy":[-29.46352,-28.18349,-28.60682,-29.83989]}, - {"t":2.90387, "x":2.6801, "y":2.29358, "heading":0.94917, "vx":-2.3645, "vy":-1.72649, "omega":-0.13074, "ax":-2.76701, "ay":-2.16334, "alpha":-0.14307, "fx":[-37.31029,-38.26116,-37.98722,-37.05256], "fy":[-29.86885,-28.64704,-29.02932,-30.20775]}, - {"t":2.94748, "x":2.57435, "y":2.21623, "heading":0.94346, "vx":-2.48518, "vy":-1.82084, "omega":-0.13698, "ax":-2.68721, "ay":-2.23248, "alpha":-0.12983, "fx":[-36.2229,-37.12355,-36.90295,-36.01859], "fy":[-30.77869,-29.69415,-29.9972,-31.04651]}, - {"t":2.99109, "x":2.4634, "y":2.13469, "heading":0.93749, "vx":-2.60237, "vy":-1.91821, "omega":-0.14264, "ax":-2.27323, "ay":-2.53307, "alpha":-0.06343, "fx":[-30.69378,-31.20535,-31.1703,-30.66523], "fy":[-34.66508,-34.20896,-34.27729,-34.7264]}, - {"t":3.03471, "x":2.34774, "y":2.04862, "heading":0.93127, "vx":-2.70152, "vy":-2.02868, "omega":-0.14541, "ax":3.13325, "ay":1.32981, "alpha":0.25193, "fx":[42.38604,43.33765,42.90072,41.92186], "fy":[18.53827,16.35411,17.69976,19.79099]}, - {"t":3.07832, "x":2.2329, "y":1.96141, "heading":0.92493, "vx":-2.56487, "vy":-1.97069, "omega":-0.13442, "ax":2.98697, "ay":1.81136, "alpha":0.19407, "fx":[40.317,41.3638,40.9708,39.93256], "fy":[25.17017,23.43314,24.16494,25.82614]}, - {"t":3.12193, "x":2.12388, "y":1.87718, "heading":0.91906, "vx":-2.43459, "vy":-1.89169, "omega":-0.12595, "ax":2.94809, "ay":1.9088, "alpha":0.1817, "fx":[39.77322,40.81774,40.45409,39.42288], "fy":[26.4981,24.87022,25.48558,27.04423]}, - {"t":3.16555, "x":2.0205, "y":1.7965, "heading":0.91357, "vx":-2.30602, "vy":-1.80844, "omega":-0.11803, "ax":2.93043, "ay":1.9505, "alpha":0.17636, "fx":[39.52533,40.56726,40.22037,39.1937], "fy":[27.06738,25.48648,26.04979,27.56439]}, - {"t":3.20916, "x":1.92272, "y":1.71948, "heading":0.90842, "vx":-2.17821, "vy":-1.72337, "omega":-0.11034, "ax":2.92036, "ay":1.97365, "alpha":0.1734, "fx":[39.38289,40.42342,40.08816,39.06407], "fy":[27.38476,25.82936,26.36141,27.85212]}, - {"t":3.25277, "x":1.8305, "y":1.6462, "heading":0.90361, "vx":-2.05085, "vy":-1.63729, "omega":-0.10278, "ax":2.91386, "ay":1.98836, "alpha":0.17151, "fx":[39.28997,40.3299,40.00363,38.981], "fy":[27.58782,26.04809,26.55833,28.03436]}, - {"t":3.29639, "x":1.74382, "y":1.57668, "heading":0.89913, "vx":-1.92376, "vy":-1.55058, "omega":-0.0953, "ax":2.90931, "ay":1.99854, "alpha":0.17021, "fx":[39.2243,40.26408,39.9452,38.9234], "fy":[27.7293,26.19998,26.69361,28.15989]}, - {"t":3.34, "x":1.66269, "y":1.51095, "heading":0.89497, "vx":-1.79688, "vy":-1.46341, "omega":-0.08787, "ax":2.90595, "ay":2.00601, "alpha":0.16925, "fx":[39.17527,40.21517,39.90257,38.88122], "fy":[27.83376,26.31175,26.79204,28.25148]}, - {"t":3.38361, "x":1.58709, "y":1.44904, "heading":0.89114, "vx":-1.67014, "vy":-1.37592, "omega":-0.08049, "ax":2.90337, "ay":2.01171, "alpha":0.16853, "fx":[39.13716,40.17733,39.87018,38.84906], "fy":[27.91418,26.39752,26.86672,28.32117]}, - {"t":3.42723, "x":1.51701, "y":1.39094, "heading":0.88763, "vx":-1.54352, "vy":-1.28819, "omega":-0.07314, "ax":2.90133, "ay":2.01621, "alpha":0.16795, "fx":[39.10666,40.14716,39.8448,38.82377], "fy":[27.97808,26.46547,26.92526,28.37592]}, - {"t":3.47084, "x":1.45245, "y":1.33668, "heading":0.88444, "vx":-1.41698, "vy":-1.20025, "omega":-0.06582, "ax":2.89967, "ay":2.01986, "alpha":0.16749, "fx":[39.08166,40.12253,39.8244,38.80336], "fy":[28.03009,26.52065,26.97234,28.42006]}, - {"t":3.51445, "x":1.39341, "y":1.28625, "heading":0.88157, "vx":-1.29052, "vy":-1.11216, "omega":-0.05851, "ax":2.89829, "ay":2.02287, "alpha":0.16711, "fx":[39.06082,40.10204,39.80763,38.78656], "fy":[28.07325,26.56634,27.01104,28.45638]}, - {"t":3.55806, "x":1.33988, "y":1.23967, "heading":0.87902, "vx":-1.16411, "vy":-1.02394, "omega":-0.05122, "ax":2.89713, "ay":2.0254, "alpha":0.16679, "fx":[39.04319,40.08474,39.79359,38.77248], "fy":[28.10961,26.6048,27.04342,28.48682]}, - {"t":3.60168, "x":1.29186, "y":1.19694, "heading":0.87678, "vx":-1.03776, "vy":-0.9356, "omega":-0.04395, "ax":2.89614, "ay":2.02755, "alpha":0.16651, "fx":[39.02811,40.06995,39.78164,38.76048], "fy":[28.14062,26.63759,27.07097,28.51271]}, - {"t":3.64529, "x":1.24936, "y":1.15806, "heading":0.87487, "vx":-0.91145, "vy":-0.84718, "omega":-0.03669, "ax":2.89529, "ay":2.02941, "alpha":0.16628, "fx":[39.0151,40.05718,39.77131,38.75012], "fy":[28.16733,26.66585,27.09473,28.53504]}, - {"t":3.6889, "x":1.21236, "y":1.12305, "heading":0.87327, "vx":-0.78518, "vy":-0.75867, "omega":-0.02943, "ax":2.89454, "ay":2.03103, "alpha":0.16607, "fx":[39.00381,40.04606,39.76226,38.74106], "fy":[28.19052,26.69043,27.1155,28.55452]}, - {"t":3.73252, "x":1.18087, "y":1.09189, "heading":0.87198, "vx":-0.65894, "vy":-0.67009, "omega":-0.02219, "ax":2.89389, "ay":2.03245, "alpha":0.16589, "fx":[38.99396,40.03632,39.75421,38.73304], "fy":[28.21078,26.71196,27.13387,28.5717]}, - {"t":3.77613, "x":1.15488, "y":1.0646, "heading":0.87102, "vx":-0.53273, "vy":-0.58145, "omega":-0.01496, "ax":2.89331, "ay":2.03371, "alpha":0.16574, "fx":[38.98534,40.02775,39.74696,38.72586], "fy":[28.22856,26.73094,27.1503,28.58701]}, - {"t":3.81974, "x":1.1344, "y":1.04117, "heading":0.87036, "vx":-0.40654, "vy":-0.49275, "omega":-0.00773, "ax":2.89279, "ay":2.03483, "alpha":0.16559, "fx":[38.97779,40.02016,39.74036,38.71937], "fy":[28.24421,26.74776,27.16515,28.60078]}, - {"t":3.86336, "x":1.11942, "y":1.02162, "heading":0.87003, "vx":-0.28038, "vy":-0.404, "omega":-0.00051, "ax":2.89232, "ay":2.03584, "alpha":0.16547, "fx":[38.97118,40.01344,39.73427,38.71344], "fy":[28.25803,26.76272,27.1787,28.61328]}, - {"t":3.90697, "x":1.10995, "y":1.00593, "heading":0.87, "vx":-0.15423, "vy":-0.31521, "omega":0.00671, "ax":2.8919, "ay":2.03674, "alpha":0.16535, "fx":[38.96541,40.00748,39.7286,38.70797], "fy":[28.27023,26.77606,27.19119,28.6247]}, - {"t":3.95058, "x":1.10597, "y":0.99412, "heading":0.8703, "vx":-0.02811, "vy":-0.22639, "omega":0.01392, "ax":2.89152, "ay":2.03757, "alpha":0.16525, "fx":[38.96037,40.00217,39.72325,38.70288], "fy":[28.28101,26.788,27.20281,28.63523]}, - {"t":3.9942, "x":1.10749, "y":0.98619, "heading":0.8709, "vx":0.098, "vy":-0.13752, "omega":0.02113, "ax":2.8912, "ay":2.03809, "alpha":0.15792, "fx":[38.97352,39.96884,39.7022,38.72642], "fy":[28.26249,26.83725,27.23297,28.60288]}, - {"t":4.03752, "x":1.11445, "y":0.98214, "heading":0.87182, "vx":0.22326, "vy":-0.04922, "omega":0.02797, "ax":2.89082, "ay":2.03835, "alpha":0.15787, "fx":[38.96894,39.96385,39.69658,38.72114], "fy":[28.26503,26.84075,27.23743,28.60644]}, - {"t":4.08085, "x":1.12684, "y":0.98192, "heading":0.87303, "vx":0.34851, "vy":0.03909, "omega":0.03481, "ax":2.89041, "ay":2.03863, "alpha":0.15782, "fx":[38.96404,39.95843,39.69026,38.71525], "fy":[28.26764,26.8445,27.24251,28.61045]}, - {"t":4.12417, "x":1.14465, "y":0.98553, "heading":0.87454, "vx":0.47373, "vy":0.12741, "omega":0.04165, "ax":2.88995, "ay":2.03895, "alpha":0.15775, "fx":[38.95873,39.9525,39.68319,38.70868], "fy":[28.27037,26.84856,27.24828,28.61497]}, - {"t":4.1675, "x":1.16789, "y":0.99296, "heading":0.87634, "vx":0.59894, "vy":0.21575, "omega":0.04848, "ax":2.88944, "ay":2.0393, "alpha":0.15769, "fx":[38.95292,39.94594,39.67524,38.70133], "fy":[28.27329,26.85301,27.25479,28.62006]}, - {"t":4.21082, "x":1.19655, "y":1.00423, "heading":0.87844, "vx":0.72412, "vy":0.3041, "omega":0.05531, "ax":2.88887, "ay":2.03969, "alpha":0.15761, "fx":[38.94647,39.93865,39.66632,38.69309], "fy":[28.27648,26.85793,27.26215,28.62578]}, - {"t":4.25415, "x":1.23063, "y":1.01932, "heading":0.88084, "vx":0.84928, "vy":0.39247, "omega":0.06214, "ax":2.88823, "ay":2.04012, "alpha":0.15752, "fx":[38.93925,39.93045,39.65625,38.6838], "fy":[28.28004,26.86346,27.27045,28.63223]}, - {"t":4.29747, "x":1.27014, "y":1.03823, "heading":0.88353, "vx":0.97442, "vy":0.48086, "omega":0.06897, "ax":2.88751, "ay":2.04062, "alpha":0.15743, "fx":[38.93104,39.92115,39.64486,38.67329], "fy":[28.28409,26.86974,27.27982,28.63951]}, - {"t":4.34079, "x":1.31506, "y":1.06098, "heading":0.88652, "vx":1.09952, "vy":0.56927, "omega":0.07579, "ax":2.88668, "ay":2.04119, "alpha":0.15732, "fx":[38.9216,39.91048,39.6319,38.66132], "fy":[28.28879,26.87696,27.29045,28.64777]}, - {"t":4.38412, "x":1.36541, "y":1.08756, "heading":0.8898, "vx":1.22458, "vy":0.6577, "omega":0.0826, "ax":2.88573, "ay":2.04184, "alpha":0.15719, "fx":[38.91061,39.89812,39.61704,38.64757], "fy":[28.29435,26.88537,27.30256,28.65721]}, - {"t":4.42744, "x":1.42117, "y":1.11797, "heading":0.89338, "vx":1.3496, "vy":0.74616, "omega":0.08941, "ax":2.88461, "ay":2.0426, "alpha":0.15704, "fx":[38.89763,39.88361,39.59985,38.63162], "fy":[28.30106,26.89531,27.31646,28.66808]}, - {"t":4.47077, "x":1.48235, "y":1.15222, "heading":0.89726, "vx":1.47458, "vy":0.83466, "omega":0.09622, "ax":2.8833, "ay":2.0435, "alpha":0.15687, "fx":[38.88205,39.86634,39.57976,38.61292], "fy":[28.3093,26.90723,27.33254,28.68071]}, - {"t":4.51409, "x":1.54894, "y":1.1903, "heading":0.90143, "vx":1.5995, "vy":0.92319, "omega":0.10301, "ax":2.88172, "ay":2.04457, "alpha":0.15666, "fx":[38.86305,39.84545,39.55594,38.59067], "fy":[28.31961,26.92175,27.35139,28.69561]}, - {"t":4.55742, "x":1.62094, "y":1.23221, "heading":0.90589, "vx":1.72435, "vy":1.01177, "omega":0.1098, "ax":2.87979, "ay":2.04589, "alpha":0.15641, "fx":[38.83941,39.81971,39.52721,38.5637], "fy":[28.33275,26.93979,27.37384,28.71347]}, - {"t":4.60074, "x":1.69835, "y":1.27797, "heading":0.91065, "vx":1.84911, "vy":1.10041, "omega":0.11658, "ax":2.87738, "ay":2.04753, "alpha":0.15609, "fx":[38.80933,39.78726,39.4918,38.5303], "fy":[28.34989,26.96269,27.40111,28.73535]}, - {"t":4.64407, "x":1.78117, "y":1.32756, "heading":0.9157, "vx":1.97378, "vy":1.18912, "omega":0.12334, "ax":2.87427, "ay":2.04963, "alpha":0.1557, "fx":[38.76995,39.7452,39.44692,38.48773], "fy":[28.37286,26.99259,27.43515,28.76289]}, - {"t":4.68739, "x":1.86938, "y":1.38101, "heading":0.92104, "vx":2.0983, "vy":1.27792, "omega":0.13009, "ax":2.87014, "ay":2.05242, "alpha":0.15517, "fx":[38.71652,39.68867,39.38794,38.43147], "fy":[28.40465,27.03298,27.47917,28.79885]}, - {"t":4.73072, "x":1.96298, "y":1.4383, "heading":0.92668, "vx":2.22265, "vy":1.36684, "omega":0.13681, "ax":2.86434, "ay":2.05632, "alpha":0.15444, "fx":[38.64048,39.60894,39.30647,38.35334], "fy":[28.45069,27.09017,27.53892,28.84817]}, - {"t":4.77404, "x":2.06196, "y":1.49945, "heading":0.9326, "vx":2.34675, "vy":1.45593, "omega":0.1435, "ax":2.85565, "ay":2.06215, "alpha":0.15337, "fx":[38.52471,39.48852,39.18576,38.23694], "fy":[28.52173,27.17664,27.62585,28.92063]}, - {"t":4.81737, "x":2.16632, "y":1.56446, "heading":0.93882, "vx":2.47047, "vy":1.54527, "omega":0.15014, "ax":2.84114, "ay":2.07178, "alpha":0.1516, "fx":[38.32922,39.28657,38.98662,38.04403], "fy":[28.64255,27.3212,27.76642,29.03893]}, - {"t":4.86069, "x":2.27602, "y":1.63335, "heading":0.94532, "vx":2.59356, "vy":1.63503, "omega":0.15671, "ax":2.81208, "ay":2.09078, "alpha":0.14813, "fx":[37.93398,38.88017,38.59107,37.65959], "fy":[28.8861,27.60871,28.03866,29.26987]}, - {"t":4.90402, "x":2.39102, "y":1.70615, "heading":0.95211, "vx":2.71539, "vy":1.72562, "omega":0.16313, "ax":2.72458, "ay":2.14573, "alpha":0.13779, "fx":[36.73867,37.65139,37.40473,36.50711], "fy":[29.59953,28.44481,28.81713,29.93286]}, - {"t":4.94734, "x":2.51122, "y":1.78293, "heading":0.95918, "vx":2.83343, "vy":1.81858, "omega":0.1691, "ax":-1.43393, "ay":1.9915, "alpha":-0.33918, "fx":[-17.85593,-19.20235,-21.11804,-19.87426], "fy":[27.91503,28.10094,26.31646,26.06687]}, - {"t":4.99067, "x":2.63263, "y":1.86359, "heading":0.96651, "vx":2.77131, "vy":1.90486, "omega":0.1544, "ax":-2.94554, "ay":-1.83028, "alpha":-0.17785, "fx":[-39.79114,-40.75222,-40.37074,-39.41457], "fy":[-25.33672,-23.79398,-24.50549,-25.9878]}, - {"t":5.03399, "x":2.74994, "y":1.9444, "heading":0.9732, "vx":2.6437, "vy":1.82556, "omega":0.1467, "ax":-2.92388, "ay":-1.93111, "alpha":-0.16823, "fx":[-39.49439,-40.45558,-40.07691,-39.12332], "fy":[-26.71006,-25.2452,-25.87445,-27.28273]}, - {"t":5.07732, "x":2.86173, "y":2.02168, "heading":0.97955, "vx":2.51702, "vy":1.7419, "omega":0.13941, "ax":-2.91599, "ay":-1.96487, "alpha":-0.16503, "fx":[-39.38913,-40.34771,-39.9671,-39.01653], "fy":[-27.16507,-25.72918,-26.33747,-27.71835]}, - {"t":5.12064, "x":2.96804, "y":2.0953, "heading":0.98559, "vx":2.39068, "vy":1.65677, "omega":0.13226, "ax":-2.91191, "ay":-1.98177, "alpha":-0.16345, "fx":[-39.33673,-40.2927,-39.90865,-38.96061], "fy":[-27.39001,-25.97018,-26.57204,-27.93778]}, - {"t":5.16397, "x":3.06888, "y":2.16522, "heading":0.99132, "vx":2.26453, "vy":1.57091, "omega":0.12518, "ax":-2.90943, "ay":-1.99192, "alpha":-0.16252, "fx":[-39.30617,-40.25969,-39.8717,-38.92587], "fy":[-27.52301,-26.11396,-26.71485,-28.07046]}, - {"t":5.20729, "x":3.16426, "y":2.23141, "heading":0.99675, "vx":2.13848, "vy":1.48461, "omega":0.11814, "ax":-2.90776, "ay":-1.99868, "alpha":-0.1619, "fx":[-39.28667,-40.23786,-39.84581,-38.90199], "fy":[-27.61015,-26.20917,-26.81157,-28.15963]}, - {"t":5.25062, "x":3.25418, "y":2.29385, "heading":1.00187, "vx":2.0125, "vy":1.39802, "omega":0.11112, "ax":-2.90655, "ay":-2.00352, "alpha":-0.16147, "fx":[-39.27348,-40.22248,-39.82641,-38.88445], "fy":[-27.6712,-26.27669,-26.88183,-28.22385]}, - {"t":5.29394, "x":3.33865, "y":2.35254, "heading":1.00668, "vx":1.88657, "vy":1.31122, "omega":0.10413, "ax":-2.90565, "ay":-2.00714, "alpha":-0.16115, "fx":[-39.26421,-40.21112,-39.81117,-38.87095], "fy":[-27.71605,-26.32695,-26.93545,-28.27241]}, - {"t":5.33726, "x":3.41766, "y":2.40747, "heading":1.01119, "vx":1.76069, "vy":1.22426, "omega":0.09715, "ax":-2.90494, "ay":-2.00996, "alpha":-0.1609, "fx":[-39.2575,-40.20245,-39.79878,-38.8602], "fy":[-27.75019,-26.36576,-26.97787,-28.3105]}, - {"t":5.38059, "x":3.49121, "y":2.45862, "heading":1.0154, "vx":1.63483, "vy":1.13718, "omega":0.09018, "ax":-2.90437, "ay":-2.01222, "alpha":-0.1607, "fx":[-39.25252,-40.19562,-39.78846,-38.8514], "fy":[-27.77692,-26.39659,-27.01237,-28.34119]}, - {"t":5.42391, "x":3.55931, "y":2.506, "heading":1.01931, "vx":1.509, "vy":1.05, "omega":0.08321, "ax":-2.9039, "ay":-2.01406, "alpha":-0.16054, "fx":[-39.24876,-40.19013,-39.7797,-38.84406], "fy":[-27.79834,-26.42165,-27.04104,-28.36648]}, - {"t":5.46724, "x":3.62196, "y":2.5496, "heading":1.02291, "vx":1.38319, "vy":0.96274, "omega":0.07626, "ax":-2.90352, "ay":-2.0156, "alpha":-0.1604, "fx":[-39.24586,-40.18563,-39.77217,-38.83784], "fy":[-27.81585,-26.44241,-27.06526,-28.38769]}, - {"t":5.51056, "x":3.67917, "y":2.58942, "heading":1.02622, "vx":1.25739, "vy":0.87541, "omega":0.06931, "ax":-2.90319, "ay":-2.0169, "alpha":-0.16029, "fx":[-39.24358,-40.18187,-39.76561,-38.8325], "fy":[-27.83041,-26.4599,-27.08599,-28.40572]}, - {"t":5.55389, "x":3.73092, "y":2.62545, "heading":1.02922, "vx":1.13161, "vy":0.78803, "omega":0.06236, "ax":-2.9029, "ay":-2.01801, "alpha":-0.1602, "fx":[-39.24174,-40.17868,-39.75988,-38.82787], "fy":[-27.84273,-26.47482,-27.10391,-28.42124]}, - {"t":5.59721, "x":3.77722, "y":2.6577, "heading":1.03192, "vx":1.00585, "vy":0.7006, "omega":0.05542, "ax":-2.90266, "ay":-2.01898, "alpha":-0.16011, "fx":[-39.24022,-40.17592,-39.75484,-38.82384], "fy":[-27.85331,-26.48773,-27.11955,-28.43472]}, - {"t":5.64054, "x":3.81807, "y":2.68616, "heading":1.03432, "vx":0.88009, "vy":0.61313, "omega":0.04849, "ax":-2.90244, "ay":-2.01983, "alpha":-0.16004, "fx":[-39.23891,-40.17352,-39.7504,-38.8203], "fy":[-27.86253,-26.49901,-27.13326,-28.44653]}, - {"t":5.68386, "x":3.85348, "y":2.71083, "heading":1.03642, "vx":0.75434, "vy":0.52562, "omega":0.04155, "ax":-2.90225, "ay":-2.02057, "alpha":-0.15998, "fx":[-39.23773,-40.17139,-39.7465,-38.81718], "fy":[-27.87068,-26.50898,-27.14534,-28.45693]}, - {"t":5.72719, "x":3.88344, "y":2.7317, "heading":1.03822, "vx":0.6286, "vy":0.43808, "omega":0.03462, "ax":-2.90208, "ay":-2.02124, "alpha":-0.15992, "fx":[-39.23665,-40.16948,-39.74307,-38.81442], "fy":[-27.87799,-26.51786,-27.15601,-28.46616]}, - {"t":5.77051, "x":3.90795, "y":2.74879, "heading":1.03972, "vx":0.50287, "vy":0.35051, "omega":0.02769, "ax":-2.90193, "ay":-2.02183, "alpha":-0.15987, "fx":[-39.23559,-40.16774,-39.74007,-38.81199], "fy":[-27.88464,-26.52585,-27.16545,-28.47436]}, - {"t":5.81384, "x":3.92701, "y":2.76207, "heading":1.04092, "vx":0.37714, "vy":0.26292, "omega":0.02077, "ax":-2.9018, "ay":-2.02236, "alpha":-0.15982, "fx":[-39.23454,-40.16614,-39.73747,-38.80984], "fy":[-27.89078,-26.53309,-27.1738,-28.48169]}, - {"t":5.85716, "x":3.94063, "y":2.77157, "heading":1.04182, "vx":0.25142, "vy":0.1753, "omega":0.01384, "ax":-2.90167, "ay":-2.02285, "alpha":-0.15978, "fx":[-39.23346,-40.16465,-39.73524,-38.80795], "fy":[-27.89651,-26.5397,-27.18118,-28.48825]}, - {"t":5.90049, "x":3.9488, "y":2.77726, "heading":1.04242, "vx":0.12571, "vy":0.08766, "omega":0.00692, "ax":-2.90156, "ay":-2.02328, "alpha":-0.15975, "fx":[-39.23232,-40.16324,-39.73335,-38.80629], "fy":[-27.90194,-26.54579,-27.18767,-28.49414]}, - {"t":5.94381, "x":3.95152, "y":2.77916, "heading":1.04272, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.00808, "ay":-1.86111, "alpha":-0.16883, "fx":[-40.72739,-41.60415,-41.14124,-40.26022], "fy":[-25.67046,-24.22545,-25.00647,-26.40006]}, - {"t":5.98721, "x":3.94869, "y":2.77741, "heading":1.04272, "vx":-0.13055, "vy":-0.08077, "omega":-0.00733, "ax":-3.00794, "ay":-1.86103, "alpha":-0.16882, "fx":[-40.72553,-41.60225,-41.13943,-40.25845], "fy":[-25.66926,-24.2244,-25.00539,-26.39885]}, - {"t":6.03061, "x":3.94019, "y":2.77215, "heading":1.0424, "vx":-0.2611, "vy":-0.16155, "omega":-0.01465, "ax":-3.0078, "ay":-1.86094, "alpha":-0.16882, "fx":[-40.72325,-41.60009,-41.13765,-40.25659], "fy":[-25.6683,-24.22334,-25.00386,-26.3974]}, - {"t":6.07402, "x":3.92602, "y":2.76339, "heading":1.04177, "vx":-0.39164, "vy":-0.24231, "omega":-0.02198, "ax":-3.00763, "ay":-1.86084, "alpha":-0.16881, "fx":[-40.72054,-41.59766,-41.13587,-40.25459], "fy":[-25.66755,-24.22226,-25.00187,-26.3957]}, - {"t":6.11742, "x":3.90619, "y":2.75112, "heading":1.04081, "vx":-0.52218, "vy":-0.32307, "omega":-0.02931, "ax":-3.00745, "ay":-1.86072, "alpha":-0.1688, "fx":[-40.71735,-41.59491,-41.13405,-40.25243], "fy":[-25.66699,-24.22113,-24.99939,-26.39372]}, - {"t":6.16082, "x":3.8807, "y":2.73534, "heading":1.03954, "vx":-0.65271, "vy":-0.40383, "omega":-0.03663, "ax":-3.00725, "ay":-1.8606, "alpha":-0.16879, "fx":[-40.71364,-41.5918,-41.13215,-40.25007], "fy":[-25.66659,-24.21994,-24.99641,-26.39144]}, - {"t":6.20422, "x":3.84954, "y":2.71606, "heading":1.03795, "vx":-0.78322, "vy":-0.48458, "omega":-0.04396, "ax":-3.00702, "ay":-1.86046, "alpha":-0.16878, "fx":[-40.70936,-41.58827,-41.13011,-40.24744], "fy":[-25.66632,-24.21865,-24.99288,-26.38882]}, - {"t":6.24762, "x":3.81271, "y":2.69328, "heading":1.03604, "vx":-0.91373, "vy":-0.56533, "omega":-0.05128, "ax":-3.00676, "ay":-1.8603, "alpha":-0.16877, "fx":[-40.70444,-41.58425,-41.12787,-40.2445], "fy":[-25.66613,-24.21723,-24.98877,-26.3858]}, - {"t":6.29102, "x":3.77022, "y":2.66699, "heading":1.03382, "vx":-1.04423, "vy":-0.64607, "omega":-0.05861, "ax":-3.00646, "ay":-1.86011, "alpha":-0.16876, "fx":[-40.69878,-41.57965,-41.12534,-40.24114], "fy":[-25.66596,-24.21561,-24.98402,-26.38234]}, - {"t":6.33442, "x":3.72207, "y":2.6372, "heading":1.03127, "vx":-1.17471, "vy":-0.7268, "omega":-0.06593, "ax":-3.00612, "ay":-1.8599, "alpha":-0.16874, "fx":[-40.69227,-41.57433,-41.1224,-40.23727], "fy":[-25.66573,-24.21375,-24.97858,-26.37835]}, - {"t":6.37782, "x":3.66825, "y":2.6039, "heading":1.02841, "vx":-1.30518, "vy":-0.80752, "omega":-0.07326, "ax":-3.00572, "ay":-1.85965, "alpha":-0.16873, "fx":[-40.68476,-41.56815,-41.1189,-40.23273], "fy":[-25.66535,-24.21155,-24.97234,-26.37373]}, - {"t":6.42122, "x":3.60878, "y":2.56711, "heading":1.02523, "vx":-1.43563, "vy":-0.88823, "omega":-0.08058, "ax":-3.00525, "ay":-1.85936, "alpha":-0.16871, "fx":[-40.67602,-41.56089,-41.11464,-40.22732], "fy":[-25.66466,-24.20888,-24.96518,-26.36834]}, - {"t":6.46463, "x":3.54364, "y":2.5268, "heading":1.02174, "vx":-1.56606, "vy":-0.96893, "omega":-0.0879, "ax":-3.00468, "ay":-1.85901, "alpha":-0.16869, "fx":[-40.66576,-41.55222,-41.10931,-40.22075], "fy":[-25.66349,-24.20558,-24.95694,-26.36199]}, - {"t":6.50803, "x":3.47284, "y":2.483, "heading":1.01792, "vx":-1.69647, "vy":-1.04962, "omega":-0.09522, "ax":-3.00399, "ay":-1.85858, "alpha":-0.16866, "fx":[-40.65354,-41.54171,-41.1025,-40.21262], "fy":[-25.66155,-24.20141,-24.94735,-26.35439]}, - {"t":6.55143, "x":3.39638, "y":2.4357, "heading":1.01379, "vx":-1.82685, "vy":-1.13028, "omega":-0.10254, "ax":-3.00312, "ay":-1.85805, "alpha":-0.16863, "fx":[-40.63871,-41.52871,-41.09358,-40.2023], "fy":[-25.65842,-24.19597,-24.93605,-26.34513]}, - {"t":6.59483, "x":3.31427, "y":2.38489, "heading":1.00934, "vx":-1.95719, "vy":-1.21092, "omega":-0.10986, "ax":-3.00201, "ay":-1.85736, "alpha":-0.1686, "fx":[-40.62025,-41.51216,-41.08155,-40.18883], "fy":[-25.65345,-24.1887,-24.92243,-26.33355]}, - {"t":6.63823, "x":3.22649, "y":2.33059, "heading":1.00457, "vx":-2.08748, "vy":-1.29153, "omega":-0.11718, "ax":-3.00053, "ay":-1.85644, "alpha":-0.16856, "fx":[-40.59645,-41.49034,-41.06478,-40.17057], "fy":[-25.64555,-24.17859,-24.90551,-26.31858]}, - {"t":6.68163, "x":3.13307, "y":2.27278, "heading":0.99948, "vx":-2.2177, "vy":-1.3721, "omega":-0.1245, "ax":-2.99846, "ay":-1.85516, "alpha":-0.16851, "fx":[-40.56424,-41.46015,-41.04029,-40.1446], "fy":[-25.63276,-24.16386,-24.88351,-26.29828]}, - {"t":6.72503, "x":3.03399, "y":2.21149, "heading":0.99408, "vx":-2.34784, "vy":-1.45262, "omega":-0.13181, "ax":-2.99535, "ay":-1.85324, "alpha":-0.16845, "fx":[-40.51749,-41.4154,-41.00217,-40.10507], "fy":[-25.6112,-24.14092,-24.85285,-26.26878]}, - {"t":6.76843, "x":2.92927, "y":2.14669, "heading":0.98836, "vx":-2.47784, "vy":-1.53305, "omega":-0.13912, "ax":-2.99018, "ay":-1.85004, "alpha":-0.16837, "fx":[-40.44189,-41.34167,-40.93666,-40.03833], "fy":[-25.57178,-24.10143,-24.80522,-26.22111]}, - {"t":6.81183, "x":2.81892, "y":2.07842, "heading":0.98232, "vx":-2.60762, "vy":-1.61335, "omega":-0.14643, "ax":-2.97986, "ay":-1.84365, "alpha":-0.16827, "fx":[-40.29472,-41.19594,-40.80261,-39.90356], "fy":[-25.48743,-24.02032,-24.71574,-26.12851]}, - {"t":6.85524, "x":2.70294, "y":2.00666, "heading":0.97597, "vx":-2.73695, "vy":-1.69336, "omega":-0.15373, "ax":-2.94911, "ay":-1.82463, "alpha":-0.16812, "fx":[-39.86404,-40.76496,-40.39596,-39.49825], "fy":[-25.22399,-23.77328,-24.46091,-25.85835]}, - {"t":6.89864, "x":2.58137, "y":1.93145, "heading":0.96929, "vx":-2.86494, "vy":-1.77255, "omega":-0.16103, "ax":0.0, "ay":0.0, "alpha":-0.00063, "fx":[0.00251,-0.00042,-0.00242,0.0005], "fy":[0.00048,0.00249,-0.00044,-0.00245]}, - {"t":6.94204, "x":2.45703, "y":1.85451, "heading":0.96231, "vx":-2.86494, "vy":-1.77255, "omega":-0.16105, "ax":2.94911, "ay":1.82463, "alpha":0.1681, "fx":[39.85513,40.7615,40.40424,39.50236], "fy":[25.23835,23.77852,24.44699,25.85268]}, - {"t":6.98544, "x":2.33547, "y":1.7793, "heading":0.95532, "vx":-2.73695, "vy":-1.69336, "omega":-0.15376, "ax":2.97986, "ay":1.84365, "alpha":0.16826, "fx":[40.2767,41.18946,40.81932,39.91135], "fy":[25.51617,24.03073,24.68789,26.11722]}, - {"t":7.02884, "x":2.21949, "y":1.70754, "heading":0.94864, "vx":-2.60762, "vy":-1.61335, "omega":-0.14646, "ax":2.99018, "ay":1.85004, "alpha":0.16837, "fx":[40.41521,41.33228,40.9614,40.04966], "fy":[25.61419,24.11683,24.76411,26.20441]}, - {"t":7.07224, "x":2.10913, "y":1.63927, "heading":0.94229, "vx":-2.47784, "vy":-1.53305, "omega":-0.13915, "ax":2.99535, "ay":1.85324, "alpha":0.16846, "fx":[40.48257,41.40323,41.03454,40.11978], "fy":[25.66661,24.1611,24.79914,26.2469]}, - {"t":7.11564, "x":2.00441, "y":1.57448, "heading":0.93625, "vx":-2.34784, "vy":-1.45262, "omega":-0.13184, "ax":2.99846, "ay":1.85516, "alpha":0.16852, "fx":[40.52153,41.44535,41.07988,40.16252], "fy":[25.70049,24.18857,24.81787,26.27148]}, - {"t":7.15904, "x":1.90533, "y":1.51318, "heading":0.93053, "vx":-2.2177, "vy":-1.3721, "omega":-0.12452, "ax":3.00053, "ay":1.85644, "alpha":0.16858, "fx":[40.54637,41.47304,41.11119,40.19153], "fy":[25.72491,24.2076,24.8286,26.28713]}, - {"t":7.20245, "x":1.81191, "y":1.45537, "heading":0.92512, "vx":-2.08748, "vy":-1.29153, "omega":-0.11721, "ax":3.00201, "ay":1.85736, "alpha":0.16862, "fx":[40.56323,41.49251,41.13439,40.21265], "fy":[25.74375,24.22177,24.8349,26.29771]}, - {"t":7.24585, "x":1.72414, "y":1.40107, "heading":0.92003, "vx":-1.95719, "vy":-1.21092, "omega":-0.10989, "ax":3.00312, "ay":1.85805, "alpha":0.16866, "fx":[40.5752,41.50684,41.15244,40.22882], "fy":[25.75898,24.23286,24.83858,26.30516]}, - {"t":7.28925, "x":1.64202, "y":1.35026, "heading":0.91526, "vx":-1.82685, "vy":-1.13028, "omega":-0.10257, "ax":3.00399, "ay":1.85858, "alpha":0.16869, "fx":[40.58396,41.51777,41.16698,40.24165], "fy":[25.77168,24.24186,24.8406,26.31055]}, - {"t":7.33265, "x":1.56556, "y":1.30296, "heading":0.91081, "vx":-1.69647, "vy":-1.04961, "omega":-0.09525, "ax":3.00468, "ay":1.85901, "alpha":0.16872, "fx":[40.59055,41.52635,41.179,40.25213], "fy":[25.7825,24.24936,24.84157,26.31456]}, - {"t":7.37605, "x":1.49477, "y":1.25916, "heading":0.90668, "vx":-1.56606, "vy":-0.96893, "omega":-0.08792, "ax":3.00525, "ay":1.85936, "alpha":0.16874, "fx":[40.59562,41.53323,41.18914,40.26086], "fy":[25.79187,24.25573,24.84187,26.31759]}, - {"t":7.41945, "x":1.42963, "y":1.21885, "heading":0.90286, "vx":-1.43563, "vy":-0.88823, "omega":-0.0806, "ax":3.00572, "ay":1.85965, "alpha":0.16876, "fx":[40.5996,41.53886,41.19782,40.26826], "fy":[25.80006,24.26123,24.84174,26.31992]}, - {"t":7.46285, "x":1.37015, "y":1.18206, "heading":0.89937, "vx":-1.30518, "vy":-0.80752, "omega":-0.07328, "ax":3.00612, "ay":1.8599, "alpha":0.16878, "fx":[40.60279,41.54354,41.20533,40.27462], "fy":[25.80727,24.26602,24.84136,26.32175]}, - {"t":7.50625, "x":1.31633, "y":1.14876, "heading":0.89618, "vx":-1.17471, "vy":-0.7268, "omega":-0.06595, "ax":3.00646, "ay":1.86011, "alpha":0.1688, "fx":[40.6054,41.5475,41.21187,40.28013], "fy":[25.81364,24.27021,24.84085,26.32323]}, - {"t":7.54965, "x":1.26818, "y":1.11897, "heading":0.89332, "vx":-1.04423, "vy":-0.64607, "omega":-0.05863, "ax":3.00676, "ay":1.8603, "alpha":0.16881, "fx":[40.6076,41.5509,41.21761,40.28494], "fy":[25.81926,24.27391,24.8403,26.32444]}, - {"t":7.59306, "x":1.22569, "y":1.09268, "heading":0.89078, "vx":-0.91373, "vy":-0.56533, "omega":-0.0513, "ax":3.00702, "ay":1.86046, "alpha":0.16882, "fx":[40.6095,41.55386,41.22265,40.28917], "fy":[25.82422,24.27717,24.83979,26.32548]}, - {"t":7.63646, "x":1.18887, "y":1.0699, "heading":0.88855, "vx":-0.78322, "vy":-0.48458, "omega":-0.04397, "ax":3.00725, "ay":1.8606, "alpha":0.16883, "fx":[40.61119,41.55647,41.22709,40.2929], "fy":[25.82858,24.28005,24.83935,26.3264]}, - {"t":7.67986, "x":1.15771, "y":1.05062, "heading":0.88664, "vx":-0.65271, "vy":-0.40383, "omega":-0.03664, "ax":3.00745, "ay":1.86072, "alpha":0.16884, "fx":[40.61274,41.55881,41.23099,40.2962], "fy":[25.83237,24.28257,24.83904,26.32725]}, - {"t":7.72326, "x":1.13221, "y":1.03484, "heading":0.88505, "vx":-0.52218, "vy":-0.32307, "omega":-0.02932, "ax":3.00763, "ay":1.86084, "alpha":0.16885, "fx":[40.6142,41.56094,41.23441,40.2991], "fy":[25.83565,24.28478,24.83888,26.32807]}, - {"t":7.76666, "x":1.11238, "y":1.02257, "heading":0.88378, "vx":-0.39164, "vy":-0.24231, "omega":-0.02199, "ax":3.0078, "ay":1.86094, "alpha":0.16886, "fx":[40.61562,41.5629,41.23738,40.30167], "fy":[25.83843,24.28669,24.83889,26.32888]}, - {"t":7.81006, "x":1.09822, "y":1.01381, "heading":0.88283, "vx":-0.2611, "vy":-0.16155, "omega":-0.01466, "ax":3.00794, "ay":1.86103, "alpha":0.16887, "fx":[40.61703,41.56472,41.23996,40.30393], "fy":[25.84075,24.28832,24.8391,26.32972]}, - {"t":7.85346, "x":1.08972, "y":1.00855, "heading":0.88219, "vx":-0.13055, "vy":-0.08077, "omega":-0.00733, "ax":3.00808, "ay":1.86111, "alpha":0.16888, "fx":[40.61847,41.56645,41.24216,40.3059], "fy":[25.84261,24.28969,24.83953,26.3306]}, - {"t":7.89686, "x":1.08688, "y":1.0068, "heading":0.88187, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.05492, "ay":2.85624, "alpha":-0.88813, "fx":[31.82003,24.67623,23.27118,32.08398], "fy":[36.125,41.33598,42.131,35.87611]}, - {"t":7.93397, "x":1.0883, "y":1.00876, "heading":0.88187, "vx":0.07626, "vy":0.10599, "omega":-0.03296, "ax":2.05483, "ay":2.85612, "alpha":-0.88807, "fx":[31.81855,24.67547,23.27043,32.08214], "fy":[36.12377,41.33425,42.1288,35.87464]}, - {"t":7.97108, "x":1.09254, "y":1.01466, "heading":0.88065, "vx":0.15251, "vy":0.21198, "omega":-0.06591, "ax":2.05474, "ay":2.85598, "alpha":-0.88798, "fx":[31.82115,24.67993,23.26505,32.07534], "fy":[36.11874,41.32923,42.12896,35.87733]}, - {"t":8.00819, "x":1.09962, "y":1.0245, "heading":0.8782, "vx":0.22876, "vy":0.31797, "omega":-0.09887, "ax":2.05464, "ay":2.85584, "alpha":-0.88787, "fx":[31.82781,24.68962,23.25506,32.06356], "fy":[36.1099,41.3209,42.13144,35.88418]}, - {"t":8.0453, "x":1.10952, "y":1.03826, "heading":0.87453, "vx":0.30501, "vy":0.42395, "omega":-0.13182, "ax":2.05453, "ay":2.85568, "alpha":-0.88774, "fx":[31.83846,24.70454,23.24052,32.04671], "fy":[36.09726,41.30921,42.13616,35.89521]}, - {"t":8.08241, "x":1.12226, "y":1.05596, "heading":0.86964, "vx":0.38125, "vy":0.52992, "omega":-0.16476, "ax":2.05441, "ay":2.85551, "alpha":-0.88757, "fx":[31.85304,24.7247,23.22151,32.0247], "fy":[36.08082,41.29412,42.14305,35.91045]}, - {"t":8.11952, "x":1.13782, "y":1.07759, "heading":0.86353, "vx":0.45749, "vy":0.63588, "omega":-0.1977, "ax":2.05429, "ay":2.85532, "alpha":-0.88736, "fx":[31.87147,24.75014,23.19813,31.99738], "fy":[36.06062,41.27556,42.152,35.92994]}, - {"t":8.15663, "x":1.15621, "y":1.10316, "heading":0.85619, "vx":0.53372, "vy":0.74184, "omega":-0.23063, "ax":2.05415, "ay":2.85511, "alpha":-0.88712, "fx":[31.89364,24.78088,23.17054,31.96461], "fy":[36.03667,41.25347,42.16287,35.95374]}, - {"t":8.19374, "x":1.17743, "y":1.13265, "heading":0.84763, "vx":0.60995, "vy":0.8478, "omega":-0.26355, "ax":2.054, "ay":2.85488, "alpha":-0.88682, "fx":[31.91942,24.81696,23.13889,31.92619], "fy":[36.00901,41.22773,42.17548,35.98192]}, - {"t":8.23085, "x":1.20148, "y":1.16608, "heading":0.83785, "vx":0.68617, "vy":0.95374, "omega":-0.29646, "ax":2.05383, "ay":2.85462, "alpha":-0.88647, "fx":[31.94866,24.85841,23.10339,31.88189], "fy":[35.97767,41.19825,42.18966,36.01454]}, - {"t":8.26796, "x":1.22836, "y":1.20344, "heading":0.82685, "vx":0.76239, "vy":1.05967, "omega":-0.32935, "ax":2.05365, "ay":2.85433, "alpha":-0.88605, "fx":[31.98117,24.90527,23.06424,31.83146], "fy":[35.94271,41.1649,42.20517,36.05169]}, - {"t":8.30507, "x":1.25806, "y":1.24473, "heading":0.81463, "vx":0.8386, "vy":1.16559, "omega":-0.36223, "ax":2.05343, "ay":2.85401, "alpha":-0.88556, "fx":[32.01674,24.95757,23.02171,31.77458], "fy":[35.90415,41.12753,42.22175,36.09343]}, - {"t":8.34218, "x":1.2906, "y":1.28995, "heading":0.80119, "vx":0.9148, "vy":1.27151, "omega":-0.3951, "ax":2.05319, "ay":2.85364, "alpha":-0.88497, "fx":[32.05511,25.01533,22.97606,31.71091], "fy":[35.86206,41.08595,42.23908,36.13981]}, - {"t":8.37929, "x":1.32596, "y":1.3391, "heading":0.78653, "vx":0.99099, "vy":1.3774, "omega":-0.42794, "ax":2.05291, "ay":2.85322, "alpha":-0.88428, "fx":[32.09597,25.07857,22.92761,31.64006], "fy":[35.81646,41.03994,42.25678,36.19086]}, - {"t":8.4164, "x":1.36415, "y":1.39218, "heading":0.77065, "vx":1.06718, "vy":1.48328, "omega":-0.46075, "ax":2.05259, "ay":2.85274, "alpha":-0.88348, "fx":[32.13895,25.14727,22.87666,31.56157], "fy":[35.76739,40.98926,42.27442,36.2466]}, - {"t":8.45351, "x":1.40516, "y":1.44918, "heading":0.75355, "vx":1.14335, "vy":1.58915, "omega":-0.49354, "ax":2.0522, "ay":2.85217, "alpha":-0.88253, "fx":[32.18358,25.22139,22.82355,31.47491], "fy":[35.71486,40.93356,42.29145,36.30695]}, - {"t":8.49062, "x":1.449, "y":1.51012, "heading":0.73523, "vx":1.2195, "vy":1.69499, "omega":-0.52629, "ax":2.05174, "ay":2.8515, "alpha":-0.88143, "fx":[32.22931,25.30083,22.76863,31.37942], "fy":[35.65882,40.87243,42.30719,36.37175]}, - {"t":8.52772, "x":1.49567, "y":1.57498, "heading":0.7157, "vx":1.29564, "vy":1.80081, "omega":-0.559, "ax":2.05117, "ay":2.85069, "alpha":-0.88014, "fx":[32.27538,25.38542,22.71223,31.27431], "fy":[35.59917,40.80532,42.32078,36.44071]}, - {"t":8.56483, "x":1.54517, "y":1.64377, "heading":0.69496, "vx":1.37176, "vy":1.9066, "omega":-0.59166, "ax":2.05046, "ay":2.84969, "alpha":-0.87863, "fx":[32.32081,25.47485,22.65465,31.15856], "fy":[35.53569,40.73145,42.33103,36.51329]}, - {"t":8.60194, "x":1.59748, "y":1.71649, "heading":0.673, "vx":1.44785, "vy":2.01235, "omega":-0.62426, "ax":2.04956, "ay":2.84842, "alpha":-0.87685, "fx":[32.36421,25.56861,22.5961,31.03075], "fy":[35.46792,40.6497,42.33629,36.58856]}, - {"t":8.63905, "x":1.65262, "y":1.79313, "heading":0.64984, "vx":1.52391, "vy":2.11805, "omega":-0.6568, "ax":2.04837, "ay":2.84676, "alpha":-0.87472, "fx":[32.40349,25.66582,22.53663,30.88884], "fy":[35.395,40.55833,42.33402,36.66489]}, - {"t":8.67616, "x":1.71058, "y":1.87369, "heading":0.62546, "vx":1.59992, "vy":2.22369, "omega":-0.68926, "ax":2.04673, "ay":2.8445, "alpha":-0.87212, "fx":[32.43526,25.76487,22.4759,30.72952], "fy":[35.31526,40.45439,42.32009,36.73937]}, - {"t":8.71327, "x":1.77137, "y":1.95816, "heading":0.59988, "vx":1.67588, "vy":2.32925, "omega":-0.72163, "ax":2.04434, "ay":2.84123, "alpha":-0.86883, "fx":[32.45344,25.86271,22.41276,30.5469], "fy":[35.22527,40.33245,42.28698,36.80629]}, - {"t":8.75038, "x":1.83496, "y":2.04656, "heading":0.57311, "vx":1.75174, "vy":2.43468, "omega":-0.75387, "ax":2.04058, "ay":2.83607, "alpha":-0.86438, "fx":[32.44549,25.95269,22.34394,30.32877], "fy":[35.11727,40.18098,42.21898,36.85333]}, - {"t":8.78749, "x":1.90138, "y":2.13886, "heading":0.54513, "vx":1.82746, "vy":2.53993, "omega":-0.78595, "ax":2.03378, "ay":2.82679, "alpha":-0.85756, "fx":[32.37959,26.01745,22.25972,30.04412], "fy":[34.97042,39.97022,42.07605,36.8484]}, - {"t":8.8246, "x":1.97059, "y":2.23506, "heading":0.51596, "vx":1.90294, "vy":2.64483, "omega":-0.81777, "ax":2.01793, "ay":2.80513, "alpha":-0.84403, "fx":[32.14129,25.99327,22.122,29.58134], "fy":[34.70682,39.59148,41.71351,36.67452]}, - {"t":8.86171, "x":2.0426, "y":2.33514, "heading":0.48562, "vx":1.97782, "vy":2.74893, "omega":-0.84909, "ax":1.9398, "ay":2.69832, "alpha":-0.78409, "fx":[30.72315,25.30781,21.57836,27.97636], "fy":[33.62238,38.07319,39.86384,35.31286]}, - {"t":8.89882, "x":2.11733, "y":2.43901, "heading":0.45411, "vx":2.04981, "vy":2.84906, "omega":-0.87819, "ax":-1.94046, "ay":-2.6941, "alpha":0.94772, "fx":[-31.63338,-25.29383,-20.49064,-28.20334], "fy":[-32.86045,-38.20492,-40.49223,-35.08495]}, - {"t":8.93593, "x":2.19206, "y":2.54288, "heading":0.42152, "vx":1.9778, "vy":2.74908, "omega":-0.84302, "ax":-2.01772, "ay":-2.80313, "alpha":0.89526, "fx":[-32.60602,-26.37956,-21.65892,-29.18192], "fy":[-34.26869,-39.35202,-41.97582,-36.98113]}, - {"t":8.97304, "x":2.26407, "y":2.64297, "heading":0.39023, "vx":1.90292, "vy":2.64506, "omega":-0.8098, "ax":-2.03362, "ay":-2.82563, "alpha":0.8844, "fx":[-32.85261,-26.72476,-21.8815,-29.23315], "fy":[-34.51685,-39.50887,-42.29058,-37.48606]}, - {"t":9.01015, "x":2.33328, "y":2.73918, "heading":0.36018, "vx":1.82745, "vy":2.5402, "omega":-0.77698, "ax":-2.04046, "ay":-2.83536, "alpha":0.87956, "fx":[-32.98647,-26.95532,-21.97059,-29.15228], "fy":[-34.59805,-39.52095,-42.42993,-37.78285]}, - {"t":9.04726, "x":2.39969, "y":2.83149, "heading":0.33135, "vx":1.75173, "vy":2.43498, "omega":-0.74434, "ax":-2.04426, "ay":-2.8408, "alpha":0.87675, "fx":[-33.0785,-27.14359,-22.01903,-29.03043], "fy":[-34.62674,-39.48632,-42.50814,-38.00635]}, - {"t":9.08437, "x":2.46329, "y":2.9199, "heading":0.30373, "vx":1.67587, "vy":2.32956, "omega":-0.7118, "ax":-2.04667, "ay":-2.84427, "alpha":0.8749, "fx":[-33.14857,-27.3094,-22.05174,-28.89302], "fy":[-34.63413,-39.43216,-42.557,-38.1934]}, - {"t":9.12148, "x":2.52407, "y":3.00439, "heading":0.27731, "vx":1.59992, "vy":2.22401, "omega":-0.67933, "ax":-2.04833, "ay":-2.84669, "alpha":0.8736, "fx":[-33.20468,-27.46035,-22.07785,-28.75014], "fy":[-34.63203,-39.36908,-42.58904,-38.35808]}, - {"t":9.15859, "x":2.58204, "y":3.08496, "heading":0.2521, "vx":1.52391, "vy":2.11838, "omega":-0.64691, "ax":-2.04954, "ay":-2.84847, "alpha":0.87266, "fx":[-33.25078,-27.59999,-22.10126,-28.60671], "fy":[-34.62577,-39.30212,-42.6103,-38.5069]}, - {"t":9.1957, "x":2.63718, "y":3.16161, "heading":0.2281, "vx":1.44785, "vy":2.01267, "omega":-0.61453, "ax":-2.05046, "ay":-2.84983, "alpha":0.87199, "fx":[-33.28914,-27.73014,-22.12375,-28.46548], "fy":[-34.61801,-39.234,-42.62414,-38.64329]}, - {"t":9.2328, "x":2.68949, "y":3.23434, "heading":0.20529, "vx":1.37176, "vy":1.90692, "omega":-0.58217, "ax":-2.05117, "ay":-2.85091, "alpha":0.87152, "fx":[-33.32126,-27.85189,-22.14614,-28.32813], "fy":[-34.61013,-39.16635,-42.63264,-38.76922]}, - {"t":9.26991, "x":2.73899, "y":3.30314, "heading":0.18369, "vx":1.29564, "vy":1.80112, "omega":-0.54983, "ax":-2.05174, "ay":-2.85179, "alpha":0.87121, "fx":[-33.34817,-27.96588,-22.16874,-28.19578], "fy":[-34.60289,-39.10018,-42.63717,-38.88589]}, - {"t":9.30702, "x":2.78565, "y":3.36802, "heading":0.16328, "vx":1.2195, "vy":1.69529, "omega":-0.5175, "ax":-2.05221, "ay":-2.85252, "alpha":0.87102, "fx":[-33.37067,-28.07254,-22.19158,-28.0692], "fy":[-34.59667,-39.03619,-42.63871,-38.99412]}, - {"t":9.34413, "x":2.8295, "y":3.42896, "heading":0.14408, "vx":1.14335, "vy":1.58944, "omega":-0.48518, "ax":-2.0526, "ay":-2.85313, "alpha":0.87093, "fx":[-33.3894,-28.17219,-22.2146,-27.94893], "fy":[-34.59162,-38.97488,-42.63798,-39.09447]}, - {"t":9.38124, "x":2.87051, "y":3.48598, "heading":0.12608, "vx":1.06718, "vy":1.48356, "omega":-0.45286, "ax":-2.05292, "ay":-2.85365, "alpha":0.87092, "fx":[-33.40489,-28.26503,-22.2376,-27.83538], "fy":[-34.58778,-38.9166,-42.63555,-39.18736]}, - {"t":9.41835, "x":2.9087, "y":3.53907, "heading":0.10927, "vx":0.99099, "vy":1.37766, "omega":-0.42054, "ax":-2.0532, "ay":-2.8541, "alpha":0.87098, "fx":[-33.41761,-28.35122,-22.2604,-27.72886], "fy":[-34.5851,-38.86161,-42.63188,-39.2731]}, - {"t":9.45546, "x":2.94406, "y":3.58823, "heading":0.09366, "vx":0.9148, "vy":1.27175, "omega":-0.38822, "ax":-2.05344, "ay":-2.85449, "alpha":0.87107, "fx":[-33.42795,-28.43091,-22.28277,-27.62959], "fy":[-34.58351,-38.81012,-42.62735,-39.35197]}, - {"t":9.49257, "x":2.9766, "y":3.63346, "heading":0.07926, "vx":0.8386, "vy":1.16582, "omega":-0.35589, "ax":-2.05365, "ay":-2.85483, "alpha":0.8712, "fx":[-33.43626,-28.50419,-22.30447,-27.53773], "fy":[-34.58286,-38.76228,-42.62227,-39.42416]}, - {"t":9.52968, "x":3.0063, "y":3.67476, "heading":0.06605, "vx":0.76239, "vy":1.05988, "omega":-0.32356, "ax":-2.05384, "ay":-2.85513, "alpha":0.87135, "fx":[-33.44288,-28.57115,-22.32529,-27.45342], "fy":[-34.58304,-38.71823,-42.61691,-39.48987]}, - {"t":9.56679, "x":3.03318, "y":3.71212, "heading":0.05404, "vx":0.68617, "vy":0.95392, "omega":-0.29123, "ax":-2.05401, "ay":-2.8554, "alpha":0.8715, "fx":[-33.44808,-28.63187,-22.34503,-27.37675], "fy":[-34.58389,-38.67806,-42.61149,-39.54927]}, - {"t":9.6039, "x":3.05723, "y":3.74555, "heading":0.04324, "vx":0.60995, "vy":0.84796, "omega":-0.25888, "ax":-2.05415, "ay":-2.85564, "alpha":0.87166, "fx":[-33.45211,-28.68642,-22.36348,-27.30777], "fy":[-34.58528,-38.64185,-42.60622,-39.60248]}, - {"t":9.64101, "x":3.07845, "y":3.77506, "heading":0.03363, "vx":0.53372, "vy":0.74199, "omega":-0.22654, "ax":-2.05429, "ay":-2.85586, "alpha":0.87181, "fx":[-33.45521,-28.73486,-22.38047,-27.24654], "fy":[-34.58708,-38.60968,-42.60127,-39.64964]}, - {"t":9.67812, "x":3.09684, "y":3.80062, "heading":0.02522, "vx":0.45749, "vy":0.63601, "omega":-0.19419, "ax":-2.05441, "ay":-2.85606, "alpha":0.87194, "fx":[-33.45758,-28.77723,-22.39584,-27.19308], "fy":[-34.58915,-38.58158,-42.59678,-39.69086]}, - {"t":9.71523, "x":3.1124, "y":3.82226, "heading":0.01802, "vx":0.38125, "vy":0.53002, "omega":-0.16183, "ax":-2.05452, "ay":-2.85624, "alpha":0.87205, "fx":[-33.45939,-28.81358,-22.40945,-27.14742], "fy":[-34.59139,-38.5576,-42.59288,-39.72623]}, - {"t":9.75234, "x":3.12514, "y":3.83996, "heading":0.01201, "vx":0.30501, "vy":0.42403, "omega":-0.12947, "ax":-2.05463, "ay":-2.8564, "alpha":0.87214, "fx":[-33.46079,-28.84396,-22.42119,-27.10957], "fy":[-34.5937,-38.53779,-42.58967,-39.75584]}, - {"t":9.78945, "x":3.13504, "y":3.85373, "heading":0.00721, "vx":0.22876, "vy":0.31803, "omega":-0.0971, "ax":-2.05472, "ay":-2.85655, "alpha":0.8722, "fx":[-33.46192,-28.86838,-22.43095,-27.07954], "fy":[-34.59599,-38.52216,-42.58725,-39.77976]}, - {"t":9.82656, "x":3.14211, "y":3.86357, "heading":0.0036, "vx":0.15251, "vy":0.21203, "omega":-0.06474, "ax":-2.05481, "ay":-2.85669, "alpha":0.87223, "fx":[-33.46287,-28.88689,-22.43866,-27.05733], "fy":[-34.5982,-38.51074,-42.58568,-39.79806]}, - {"t":9.86367, "x":3.14636, "y":3.86947, "heading":0.0012, "vx":0.07626, "vy":0.10601, "omega":-0.03237, "ax":-2.0549, "ay":-2.85682, "alpha":0.87222, "fx":[-33.46373,-28.89951,-22.44426,-27.04294], "fy":[-34.60027,-38.50355,-42.58502,-39.81078]}, - {"t":9.90077, "x":3.14777, "y":3.87143, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-1.98888, "ay":2.90408, "alpha":-0.8559, "fx":[-25.98491,-21.61386,-28.10717,-32.55114], "fy":[40.51082,43.01354,39.08649,35.46115]}, - {"t":9.9385, "x":3.14636, "y":3.8735, "heading":0.0, "vx":-0.07502, "vy":0.10954, "omega":-0.03229, "ax":-1.9888, "ay":2.90395, "alpha":-0.85592, "fx":[-25.98364,-21.61292,-28.10615,-32.54984], "fy":[40.50886,43.01184,39.08505,35.45957]}, - {"t":9.97622, "x":3.14211, "y":3.8797, "heading":-0.00122, "vx":-0.15004, "vy":0.21908, "omega":-0.06457, "ax":-1.98871, "ay":2.90382, "alpha":-0.85589, "fx":[-25.99021,-21.6097,-28.09921,-32.54866], "fy":[40.50165,43.01111,39.0877,35.45768]}, - {"t":10.01394, "x":3.13504, "y":3.89003, "heading":-0.00365, "vx":-0.22506, "vy":0.32862, "omega":-0.09686, "ax":-1.98862, "ay":2.90368, "alpha":-0.85582, "fx":[-26.00462,-21.60422,-28.08633,-32.54755], "fy":[40.48915,43.0113,39.09442,35.45548]}, - {"t":10.05166, "x":3.12513, "y":3.90449, "heading":-0.00731, "vx":-0.30007, "vy":0.43815, "omega":-0.12914, "ax":-1.98852, "ay":2.90352, "alpha":-0.8557, "fx":[-26.02686,-21.59652,-28.06747,-32.54645], "fy":[40.47133,43.01237,39.10521,35.45301]}, - {"t":10.08938, "x":3.1124, "y":3.92308, "heading":-0.01218, "vx":-0.37508, "vy":0.54767, "omega":-0.16142, "ax":-1.98841, "ay":2.90335, "alpha":-0.85555, "fx":[-26.05692,-21.58667,-28.04263,-32.54527], "fy":[40.44812,43.01426,39.12002,35.45031]}, - {"t":10.1271, "x":3.09684, "y":3.94581, "heading":-0.01827, "vx":-0.45008, "vy":0.65719, "omega":-0.19369, "ax":-1.9883, "ay":2.90317, "alpha":-0.85536, "fx":[-26.09479,-21.57473,-28.01177,-32.5439], "fy":[40.41945,43.01689,39.13884,35.44744]}, - {"t":10.16482, "x":3.07845, "y":3.97266, "heading":-0.02557, "vx":-0.52508, "vy":0.7667, "omega":-0.22595, "ax":-1.98817, "ay":2.90296, "alpha":-0.85514, "fx":[-26.14048,-21.56083,-27.97485,-32.5422], "fy":[40.38526,43.02015,39.16163,35.44449]}, - {"t":10.20254, "x":3.05722, "y":4.00365, "heading":-0.0341, "vx":-0.60008, "vy":0.8762, "omega":-0.25821, "ax":-1.98803, "ay":2.90274, "alpha":-0.85488, "fx":[-26.19395,-21.54507,-27.93182,-32.53999], "fy":[40.34543,43.02393,39.18834,35.44155]}, - {"t":10.24026, "x":3.03318, "y":4.03876, "heading":-0.04384, "vx":-0.67507, "vy":0.9857, "omega":-0.29046, "ax":-1.98788, "ay":2.90249, "alpha":-0.85459, "fx":[-26.25519,-21.5276,-27.88264,-32.53707], "fy":[40.29986,43.02809,39.21892,35.43872]}, - {"t":10.27798, "x":3.0063, "y":4.07801, "heading":-0.05479, "vx":-0.75006, "vy":1.09518, "omega":-0.32269, "ax":-1.98771, "ay":2.90221, "alpha":-0.85428, "fx":[-26.32416,-21.50859,-27.82725,-32.53321], "fy":[40.24841,43.03246,39.25331,35.43612]}, - {"t":10.3157, "x":2.97659, "y":4.12139, "heading":-0.06696, "vx":-0.82503, "vy":1.20465, "omega":-0.35492, "ax":-1.98752, "ay":2.90189, "alpha":-0.85395, "fx":[-26.4008,-21.48823,-27.76558,-32.52816], "fy":[40.19093,43.03685,39.2914,35.43387]}, - {"t":10.35343, "x":2.94405, "y":4.16889, "heading":-0.08035, "vx":-0.9, "vy":1.31412, "omega":-0.38713, "ax":-1.9873, "ay":2.90153, "alpha":-0.85361, "fx":[-26.48502,-21.46671,-27.69754,-32.52161], "fy":[40.12726,43.04101,39.33311,35.4321]}, - {"t":10.39115, "x":2.90869, "y":4.22052, "heading":-0.09496, "vx":-0.97497, "vy":1.42356, "omega":-0.41933, "ax":-1.98705, "ay":2.90112, "alpha":-0.85326, "fx":[-26.57672,-21.44424,-27.62305,-32.51322], "fy":[40.05718,43.04466,39.37828,35.43093]}, - {"t":10.42887, "x":2.8705, "y":4.27629, "heading":-0.11077, "vx":-1.04992, "vy":1.533, "omega":-0.45151, "ax":-1.98676, "ay":2.90064, "alpha":-0.85291, "fx":[-26.67572,-21.42107,-27.542,-32.5026], "fy":[39.98045,43.04749,39.42675,35.43046]}, - {"t":10.46659, "x":2.82948, "y":4.33618, "heading":-0.1278, "vx":-1.12486, "vy":1.64241, "omega":-0.48369, "ax":-1.98642, "ay":2.90009, "alpha":-0.85258, "fx":[-26.7818,-21.39741,-27.45423,-32.4893], "fy":[39.89677,43.04906,39.47828,35.43077]}, - {"t":10.50431, "x":2.78564, "y":4.40019, "heading":-0.14605, "vx":-1.19979, "vy":1.75181, "omega":-0.51585, "ax":-1.98601, "ay":2.89943, "alpha":-0.85227, "fx":[-26.89464,-21.3735,-27.35958,-32.47279], "fy":[39.80577,43.04888,39.53256,35.43189]}, - {"t":10.54203, "x":2.73897, "y":4.46834, "heading":-0.16551, "vx":-1.27471, "vy":1.86118, "omega":-0.54799, "ax":-1.98551, "ay":2.89864, "alpha":-0.85198, "fx":[-27.01381,-21.34952,-27.2578,-32.45244], "fy":[39.70694,43.04627,39.58918,35.43375]}, - {"t":10.57975, "x":2.68947, "y":4.5406, "heading":-0.18618, "vx":-1.3496, "vy":1.97051, "omega":-0.58013, "ax":-1.9849, "ay":2.89768, "alpha":-0.85173, "fx":[-27.13868,-21.32561,-27.14857,-32.42745], "fy":[39.5996,43.04037,39.64754,35.43615]}, - {"t":10.61747, "x":2.63715, "y":4.61699, "heading":-0.20806, "vx":-1.42447, "vy":2.07982, "omega":-0.61226, "ax":-1.98413, "ay":2.89647, "alpha":-0.85153, "fx":[-27.26836,-21.30178,-27.03138,-32.39679], "fy":[39.48276,43.02995,39.70677,35.43866]}, - {"t":10.65519, "x":2.58201, "y":4.69751, "heading":-0.23116, "vx":-1.49932, "vy":2.18907, "omega":-0.64438, "ax":-1.98313, "ay":2.89493, "alpha":-0.85138, "fx":[-27.40149,-21.27782,-26.90547,-32.35901], "fy":[39.35488,43.01321,39.76555,35.44039]}, - {"t":10.69291, "x":2.52404, "y":4.78214, "heading":-0.25546, "vx":-1.57412, "vy":2.29827, "omega":-0.67649, "ax":-1.98178, "ay":2.89287, "alpha":-0.8513, "fx":[-27.5359,-21.25302,-26.76957,-32.31197], "fy":[39.21341,42.98731,39.8217,35.43969]}, - {"t":10.73063, "x":2.46326, "y":4.87089, "heading":-0.28098, "vx":-1.64888, "vy":2.4074, "omega":-0.70861, "ax":-1.97988, "ay":2.89, "alpha":-0.8513, "fx":[-27.66786,-21.22569,-26.62135,-32.2521], "fy":[39.05379,42.94737,39.87141,35.43328]}, - {"t":10.76836, "x":2.39965, "y":4.96376, "heading":-0.30771, "vx":-1.72356, "vy":2.51641, "omega":-0.74072, "ax":-1.97701, "ay":2.88571, "alpha":-0.8514, "fx":[-27.79017,-21.1918,-26.45615,-32.17276], "fy":[38.86679,42.88395,39.90711,35.41423]}, - {"t":10.80608, "x":2.33323, "y":5.06073, "heading":-0.33565, "vx":-1.79813, "vy":2.62526, "omega":-0.77283, "ax":-1.97221, "ay":2.87857, "alpha":-0.85171, "fx":[-27.88659,-21.14093,-26.26298,-32.05912], "fy":[38.63088,42.77561,39.91144,35.36581]}, - {"t":10.8438, "x":2.264, "y":5.16181, "heading":-0.3648, "vx":-1.87253, "vy":2.73384, "omega":-0.80496, "ax":-1.9626, "ay":2.86436, "alpha":-0.85255, "fx":[-27.90958,-21.04,-26.00872,-31.86822], "fy":[38.28126,42.55929,39.83285,35.23698]}, - {"t":10.88152, "x":2.19197, "y":5.26697, "heading":-0.39517, "vx":-1.94656, "vy":2.84189, "omega":-0.83712, "ax":-1.93395, "ay":2.82215, "alpha":-0.85596, "fx":[-27.62721,-20.71996,-25.52856,-31.39122], "fy":[37.49652,41.92633,39.41755,34.77248]}, - {"t":10.91924, "x":2.11717, "y":5.37617, "heading":-0.42674, "vx":-2.01951, "vy":2.94834, "omega":-0.86941, "ax":-0.00042, "ay":-0.01101, "alpha":-0.31897, "fx":[0.43792,1.17801,-0.44951,-1.18931], "fy":[-1.33339,0.2938,1.03388,-0.59361]}, - {"t":10.95696, "x":2.04099, "y":5.48738, "heading":-0.45954, "vx":-2.01953, "vy":2.94793, "omega":-0.88144, "ax":1.93378, "ay":-2.82317, "alpha":0.83891, "fx":[27.96018,20.88017,25.23542,31.18187], "fy":[-37.26208,-41.83335,-39.59994,-34.97304]}, - {"t":10.99468, "x":1.96619, "y":5.59657, "heading":-0.49279, "vx":-1.94658, "vy":2.84144, "omega":-0.8498, "ax":1.96253, "ay":-2.86475, "alpha":0.8471, "fx":[28.64647,21.18008,25.39759,31.59839], "fy":[-37.74076,-42.47541,-40.22193,-35.4931]}, - {"t":11.0324, "x":1.89416, "y":5.70171, "heading":-0.52484, "vx":-1.87255, "vy":2.73337, "omega":-0.81784, "ax":1.97218, "ay":-2.87869, "alpha":0.85145, "fx":[28.99969,21.3016,25.34931,31.69736], "fy":[-37.80916,-42.68147,-40.49508,-35.70441]}, - {"t":11.07012, "x":1.82493, "y":5.80277, "heading":-0.55569, "vx":-1.79816, "vy":2.62479, "omega":-0.78572, "ax":1.97701, "ay":-2.88566, "alpha":0.85462, "fx":[29.26019,21.38338,25.2531,31.71392], "fy":[-37.77882,-42.77454,-40.67644,-35.83955]}, - {"t":11.10784, "x":1.7585, "y":5.89973, "heading":-0.58533, "vx":-1.72359, "vy":2.51594, "omega":-0.75349, "ax":1.97989, "ay":-2.88983, "alpha":0.85718, "fx":[29.47706,21.45225,25.14177,31.69656], "fy":[-37.71299,-42.82066,-40.81803,-35.94471]}, - {"t":11.14556, "x":1.6949, "y":5.99257, "heading":-0.61375, "vx":-1.6489, "vy":2.40693, "omega":-0.72115, "ax":1.9818, "ay":-2.8926, "alpha":0.85936, "fx":[29.66698,21.51648,25.02627,31.66203], "fy":[-37.63287,-42.8423,-40.93732,-36.03488]}, - {"t":11.18329, "x":1.63411, "y":6.08131, "heading":-0.64096, "vx":-1.57415, "vy":2.29782, "omega":-0.68874, "ax":1.98316, "ay":-2.89458, "alpha":0.86124, "fx":[29.83731,21.57911,24.91127,31.61787], "fy":[-37.54753,-42.84936,-41.04189,-36.11621]}, - {"t":11.22101, "x":1.57614, "y":6.16593, "heading":-0.66693, "vx":-1.49934, "vy":2.18863, "omega":-0.65625, "ax":1.98417, "ay":-2.89606, "alpha":0.86289, "fx":[29.99192,21.64129,24.79911,31.56809], "fy":[-37.46145,-42.84698,-41.13558,-36.19158]}, - {"t":11.25873, "x":1.521, "y":6.24642, "heading":-0.69169, "vx":-1.4245, "vy":2.07939, "omega":-0.6237, "ax":1.98494, "ay":-2.89721, "alpha":0.86434, "fx":[30.13314,21.70335,24.69103,31.51512], "fy":[-37.37706,-42.83819,-41.22059,-36.26242]}, - {"t":11.29645, "x":1.46868, "y":6.3228, "heading":-0.71522, "vx":-1.34962, "vy":1.97011, "omega":-0.5911, "ax":1.98556, "ay":-2.89813, "alpha":0.86562, "fx":[30.26249,21.76524,24.58777,31.46054], "fy":[-37.29575,-42.82491,-41.29829,-36.32947]}, - {"t":11.33417, "x":1.41918, "y":6.39505, "heading":-0.73751, "vx":-1.27473, "vy":1.86079, "omega":-0.55845, "ax":1.98605, "ay":-2.89889, "alpha":0.86673, "fx":[30.38109,21.8267,24.48975,31.4055], "fy":[-37.21835,-42.80849,-41.36957,-36.39313]}, - {"t":11.37189, "x":1.37251, "y":6.46318, "heading":-0.75858, "vx":-1.19981, "vy":1.75144, "omega":-0.52575, "ax":1.98646, "ay":-2.89952, "alpha":0.86771, "fx":[30.4898,21.88741,24.39722,31.35085], "fy":[-37.14533,-42.78992,-41.4351,-36.45357]}, - {"t":11.40961, "x":1.32866, "y":6.52718, "heading":-0.77841, "vx":-1.12488, "vy":1.64207, "omega":-0.49302, "ax":1.9868, "ay":-2.90005, "alpha":0.86855, "fx":[30.58934,21.94698,24.31033,31.29724], "fy":[-37.07695,-42.76995,-41.49535,-36.51086]}, - {"t":11.44733, "x":1.28765, "y":6.58706, "heading":-0.79701, "vx":-1.04993, "vy":1.53267, "omega":-0.46026, "ax":1.98709, "ay":-2.90052, "alpha":0.86928, "fx":[30.68029,22.00501,24.22916,31.24521], "fy":[-37.01335,-42.74919,-41.55069,-36.56501]}, - {"t":11.48505, "x":1.24945, "y":6.64281, "heading":-0.81437, "vx":-0.97498, "vy":1.42326, "omega":-0.42747, "ax":1.98734, "ay":-2.90092, "alpha":0.8699, "fx":[30.76317,22.06112,24.15371,31.19521], "fy":[-36.95457,-42.72816,-41.60142,-36.61598]}, - {"t":11.52277, "x":1.21409, "y":6.69443, "heading":-0.83049, "vx":-0.90001, "vy":1.31384, "omega":-0.39466, "ax":1.98756, "ay":-2.90127, "alpha":0.87043, "fx":[30.83844,22.11491,24.084,31.14762], "fy":[-36.9006,-42.70726,-41.6478,-36.66372]}, - {"t":11.56049, "x":1.18156, "y":6.74193, "heading":-0.84538, "vx":-0.82504, "vy":1.2044, "omega":-0.36182, "ax":1.98775, "ay":-2.90159, "alpha":0.87087, "fx":[30.90652,22.16604,24.01998,31.10274], "fy":[-36.85138,-42.68688,-41.69002,-36.70817]}, - {"t":11.59822, "x":1.15185, "y":6.78529, "heading":-0.85903, "vx":-0.75006, "vy":1.09495, "omega":-0.32897, "ax":1.98792, "ay":-2.90187, "alpha":0.87124, "fx":[30.96778,22.21416,23.96162,31.06085], "fy":[-36.80683,-42.66733,-41.72828,-36.74926]}, - {"t":11.63594, "x":1.12497, "y":6.82453, "heading":-0.87144, "vx":-0.67508, "vy":0.98549, "omega":-0.29611, "ax":1.98807, "ay":-2.90212, "alpha":0.87155, "fx":[31.02255,22.25896,23.90887,31.0222], "fy":[-36.76686,-42.64889,-41.76273,-36.78693]}, - {"t":11.67366, "x":1.10092, "y":6.85964, "heading":-0.88261, "vx":-0.60009, "vy":0.87602, "omega":-0.26323, "ax":1.9882, "ay":-2.90235, "alpha":0.87181, "fx":[31.07112,22.30016,23.86166,30.98698], "fy":[-36.73138,-42.6318,-41.7935,-36.82112]}, - {"t":11.71138, "x":1.0797, "y":6.89062, "heading":-0.89254, "vx":-0.52509, "vy":0.76654, "omega":-0.23035, "ax":1.98832, "ay":-2.90255, "alpha":0.87201, "fx":[31.11377,22.33751,23.81994,30.95537], "fy":[-36.70027,-42.61628,-41.82071,-36.85176]}, - {"t":11.7491, "x":1.06131, "y":6.91747, "heading":-0.90122, "vx":-0.45009, "vy":0.65705, "omega":-0.19745, "ax":1.98844, "ay":-2.90274, "alpha":0.87218, "fx":[31.15074,22.37077,23.78367,30.92753], "fy":[-36.67346,-42.60251,-41.84447,-36.87882]}, - {"t":11.78682, "x":1.04574, "y":6.94019, "heading":-0.90867, "vx":-0.37508, "vy":0.54756, "omega":-0.16455, "ax":1.98854, "ay":-2.90291, "alpha":0.87231, "fx":[31.18223,22.39974,23.75277,30.90358], "fy":[-36.65084,-42.59066,-41.86487,-36.90225]}, - {"t":11.82454, "x":1.03301, "y":6.95878, "heading":-0.91488, "vx":-0.30007, "vy":0.43806, "omega":-0.13165, "ax":1.98863, "ay":-2.90307, "alpha":0.87242, "fx":[31.20841,22.42427,23.72721,30.88363], "fy":[-36.63233,-42.58085,-41.88197,-36.92202]}, - {"t":11.86226, "x":1.02311, "y":6.97324, "heading":-0.91985, "vx":-0.22506, "vy":0.32855, "omega":-0.09874, "ax":1.98872, "ay":-2.90321, "alpha":0.87251, "fx":[31.22945,22.44421,23.70695,30.86778], "fy":[-36.61788,-42.57322,-41.89586,-36.93809]}, - {"t":11.89998, "x":1.01603, "y":6.98356, "heading":-0.92357, "vx":-0.15004, "vy":0.21904, "omega":-0.06583, "ax":1.98881, "ay":-2.90335, "alpha":0.87257, "fx":[31.24545,22.45946,23.69196,30.85609], "fy":[-36.60743,-42.56783,-41.90657,-36.95044]}, - {"t":11.9377, "x":1.01179, "y":6.98976, "heading":-0.92605, "vx":-0.07502, "vy":0.10952, "omega":-0.03292, "ax":1.98889, "ay":-2.90347, "alpha":0.87262, "fx":[31.25652,22.46992,23.68221,30.84863], "fy":[-36.60094,-42.56479,-41.91415,-36.95906]}, - {"t":11.97542, "x":1.01037, "y":6.99183, "heading":-0.9273, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.86512, "ay":-2.07538, "alpha":-0.10359, "fx":[38.56437,39.21104,39.41498,38.76117], "fy":[-28.8233,-27.93772,-27.64753,-28.55638]}, - {"t":12.02039, "x":1.01327, "y":6.98973, "heading":-0.9273, "vx":0.12883, "vy":-0.09332, "omega":-0.00466, "ax":2.86499, "ay":-2.07528, "alpha":-0.10359, "fx":[38.56265,39.20928,39.41316,38.75939], "fy":[-28.82198,-27.93648,-27.64628,-28.55504]}, - {"t":12.06535, "x":1.02196, "y":6.98343, "heading":-0.9275, "vx":0.25765, "vy":-0.18663, "omega":-0.00932, "ax":2.86485, "ay":-2.07518, "alpha":-0.10358, "fx":[38.56069,39.20724,39.41119,38.75752], "fy":[-28.82058,-27.93523,-27.64484,-28.55345]}, - {"t":12.11032, "x":1.03644, "y":6.97294, "heading":-0.92792, "vx":0.38647, "vy":-0.27994, "omega":-0.01397, "ax":2.86469, "ay":-2.07506, "alpha":-0.10358, "fx":[38.55848,39.2049,39.40904,38.75551], "fy":[-28.81909,-27.93395,-27.64318,-28.55157]}, - {"t":12.15528, "x":1.05671, "y":6.95826, "heading":-0.92855, "vx":0.51528, "vy":-0.37325, "omega":-0.01863, "ax":2.86451, "ay":-2.07493, "alpha":-0.10357, "fx":[38.55596,39.20222,39.40667,38.75334], "fy":[-28.81747,-27.93262,-27.64128,-28.54937]}, - {"t":12.20025, "x":1.08278, "y":6.93938, "heading":-0.92939, "vx":0.64408, "vy":-0.46655, "omega":-0.02329, "ax":2.86431, "ay":-2.07479, "alpha":-0.10357, "fx":[38.5531,39.19914,39.40404,38.75095], "fy":[-28.81569,-27.93119,-27.6391,-28.54682]}, - {"t":12.24521, "x":1.11463, "y":6.9163, "heading":-0.93044, "vx":0.77287, "vy":-0.55984, "omega":-0.02794, "ax":2.86408, "ay":-2.07462, "alpha":-0.10356, "fx":[38.54984,39.19562,39.40108,38.74829], "fy":[-28.81371,-27.92963,-27.6366,-28.54388]}, - {"t":12.29018, "x":1.15228, "y":6.88903, "heading":-0.93169, "vx":0.90166, "vy":-0.65312, "omega":-0.0326, "ax":2.86382, "ay":-2.07443, "alpha":-0.10355, "fx":[38.54609,39.19157,39.3977,38.74527], "fy":[-28.81146,-27.92788,-27.63373,-28.54048]}, - {"t":12.33514, "x":1.19572, "y":6.85757, "heading":-0.93316, "vx":1.03043, "vy":-0.7464, "omega":-0.03726, "ax":2.86352, "ay":-2.07421, "alpha":-0.10355, "fx":[38.54177,39.18689,39.39382,38.7418], "fy":[-28.80886,-27.92588,-27.63041,-28.53654]}, - {"t":12.38011, "x":1.24495, "y":6.82191, "heading":-0.93483, "vx":1.15918, "vy":-0.83967, "omega":-0.04191, "ax":2.86317, "ay":-2.07396, "alpha":-0.10354, "fx":[38.53674,39.18144,39.38927,38.73773], "fy":[-28.80583,-27.92351,-27.62654,-28.53197]}, - {"t":12.42507, "x":1.29996, "y":6.78206, "heading":-0.93672, "vx":1.28793, "vy":-0.93292, "omega":-0.04657, "ax":2.86275, "ay":-2.07366, "alpha":-0.10353, "fx":[38.53081,39.17504,39.38388,38.73288], "fy":[-28.80221,-27.92067,-27.62201,-28.52663]}, - {"t":12.47004, "x":1.36077, "y":6.73801, "heading":-0.93881, "vx":1.41665, "vy":-1.02616, "omega":-0.05122, "ax":2.86225, "ay":-2.0733, "alpha":-0.10351, "fx":[38.52374,39.16744,39.37738,38.72698], "fy":[-28.79782,-27.91715,-27.61661,-28.52032]}, - {"t":12.515, "x":1.42736, "y":6.68978, "heading":-0.94112, "vx":1.54535, "vy":-1.11939, "omega":-0.05588, "ax":2.86165, "ay":-2.07286, "alpha":-0.1035, "fx":[38.51515,39.15826,39.36938,38.71967], "fy":[-28.79237,-27.9127,-27.6101,-28.51276]}, - {"t":12.55997, "x":1.49974, "y":6.63735, "heading":-0.94363, "vx":1.67402, "vy":-1.21259, "omega":-0.06053, "ax":2.86089, "ay":-2.07231, "alpha":-0.10348, "fx":[38.5045,39.14693,39.35931,38.71036], "fy":[-28.78546,-27.90693,-27.60207,-28.50353]}, - {"t":12.60493, "x":1.5779, "y":6.58073, "heading":-0.94635, "vx":1.80266, "vy":-1.30577, "omega":-0.06519, "ax":2.85991, "ay":-2.0716, "alpha":-0.10346, "fx":[38.49092,39.1326,39.34626,38.69818], "fy":[-28.77642,-27.8992,-27.59189,-28.49198]}, - {"t":12.6499, "x":1.66185, "y":6.51992, "heading":-0.94928, "vx":1.93126, "vy":-1.39892, "omega":-0.06984, "ax":2.85861, "ay":-2.07066, "alpha":-0.10344, "fx":[38.47297,39.11378,39.32873,38.68163], "fy":[-28.76418,-27.88848,-27.57851,-28.477]}, - {"t":12.69486, "x":1.75158, "y":6.45493, "heading":-0.95242, "vx":2.05979, "vy":-1.49203, "omega":-0.07449, "ax":2.85679, "ay":-2.06934, "alpha":-0.10341, "fx":[38.44805,39.08787,39.30402,38.65803], "fy":[-28.74677,-27.8729,-27.56006,-28.45662]}, - {"t":12.73983, "x":1.84709, "y":6.38574, "heading":-0.95577, "vx":2.18825, "vy":-1.58508, "omega":-0.07914, "ax":2.85406, "ay":-2.06736, "alpha":-0.10338, "fx":[38.41098,39.0496,39.26673,38.62206], "fy":[-28.7203,-27.84874,-27.53277,-28.42691]}, - {"t":12.78479, "x":1.94836, "y":6.31238, "heading":-0.95933, "vx":2.31658, "vy":-1.67804, "omega":-0.08379, "ax":2.84951, "ay":-2.06407, "alpha":-0.10334, "fx":[38.34966,38.98679,39.20429,38.56125], "fy":[-28.67571,-27.80727,-27.48784,-28.37871]}, - {"t":12.82975, "x":2.05541, "y":6.23484, "heading":-0.9631, "vx":2.44471, "vy":-1.77085, "omega":-0.08843, "ax":2.84044, "ay":-2.0575, "alpha":-0.10328, "fx":[38.22792,38.86291,39.07916,38.43839], "fy":[-28.58591,-27.72248,-27.39899,-28.2846]}, - {"t":12.87472, "x":2.16821, "y":6.15314, "heading":-0.96707, "vx":2.57243, "vy":-1.86336, "omega":-0.09308, "ax":2.81341, "ay":-2.03792, "alpha":-0.10319, "fx":[37.86622,38.49686,38.70509,38.06881], "fy":[-28.31659,-27.46514,-27.13571,-28.00872]}, - {"t":12.91968, "x":2.28672, "y":6.06729, "heading":-0.97126, "vx":2.69893, "vy":-1.95499, "omega":-0.09772, "ax":0.0, "ay":0.0, "alpha":0.00008, "fx":[0.00006,-0.00033,-0.00006,0.00033], "fy":[0.00033,0.00006,-0.00033,-0.00007]}, - {"t":12.96465, "x":2.40808, "y":5.97939, "heading":-0.97565, "vx":2.69893, "vy":-1.95499, "omega":-0.09771, "ax":-2.81341, "ay":2.03792, "alpha":0.1032, "fx":[-37.86431,-38.49334,-38.70685,-38.07249], "fy":[28.31895,27.47017,27.13342,28.00362]}, - {"t":13.00961, "x":2.52659, "y":5.89354, "heading":-0.98005, "vx":2.57243, "vy":-1.86336, "omega":-0.09307, "ax":-2.84044, "ay":2.0575, "alpha":0.10328, "fx":[-38.22422,-38.85583,-39.08255,-38.4458], "fy":[28.59066,27.7325,27.39438,28.27444]}, - {"t":13.05458, "x":2.63939, "y":5.81183, "heading":-0.98423, "vx":2.44471, "vy":-1.77085, "omega":-0.08843, "ax":-2.84951, "ay":2.06407, "alpha":0.10334, "fx":[-38.34426,-38.97631,-39.20923,-38.57219], "fy":[28.68274,27.82204,27.48101,28.36373]}, - {"t":13.09954, "x":2.74643, "y":5.7343, "heading":-0.98821, "vx":2.31658, "vy":-1.67804, "omega":-0.08378, "ax":-2.85406, "ay":2.06736, "alpha":0.10338, "fx":[-38.40396,-39.03592,-39.27313,-38.63636], "fy":[28.72949,27.86801,27.52384,28.40738]}, - {"t":13.14451, "x":2.84771, "y":5.66093, "heading":-0.99197, "vx":2.18825, "vy":-1.58508, "omega":-0.07913, "ax":-2.85679, "ay":2.06934, "alpha":0.10341, "fx":[-38.43951,-39.07114,-39.31181,-38.67551], "fy":[28.75799,27.89643,27.54915,28.43278]}, - {"t":13.18947, "x":2.94322, "y":5.59175, "heading":-0.99553, "vx":2.05979, "vy":-1.49203, "omega":-0.07448, "ax":-2.85861, "ay":2.07066, "alpha":0.10344, "fx":[-38.463,-39.0942,-39.33782,-38.70209], "fy":[28.77732,27.91601,27.56574,28.44909]}, - {"t":13.23444, "x":3.03294, "y":5.52676, "heading":-0.99888, "vx":1.93126, "vy":-1.39892, "omega":-0.06983, "ax":-2.85991, "ay":2.0716, "alpha":0.10346, "fx":[-38.47961,-39.11033,-39.35657,-38.72144], "fy":[28.79137,27.93049,27.57737,28.46027]}, - {"t":13.2794, "x":3.11689, "y":5.46595, "heading":-1.00202, "vx":1.80266, "vy":-1.30577, "omega":-0.06518, "ax":-2.86089, "ay":2.07231, "alpha":0.10348, "fx":[-38.49194,-39.12216,-39.37076,-38.73625], "fy":[28.80208,27.94172,27.58592,28.46826]}, - {"t":13.32437, "x":3.19506, "y":5.40933, "heading":-1.00495, "vx":1.67402, "vy":-1.21259, "omega":-0.06053, "ax":-2.86165, "ay":2.07286, "alpha":0.1035, "fx":[-38.50143,-39.13116,-39.38189,-38.74798], "fy":[28.81055,27.95076,27.59244,28.47419]}, - {"t":13.36933, "x":3.26743, "y":5.3569, "heading":-1.00768, "vx":1.54535, "vy":-1.11939, "omega":-0.05588, "ax":-2.86225, "ay":2.0733, "alpha":0.10351, "fx":[-38.50895,-39.13819,-39.39086,-38.75754], "fy":[28.81743,27.95822,27.59756,28.47869]}, - {"t":13.4143, "x":3.33403, "y":5.30867, "heading":-1.01019, "vx":1.41665, "vy":-1.02616, "omega":-0.05122, "ax":-2.86275, "ay":2.07366, "alpha":0.10352, "fx":[-38.51504,-39.14382,-39.39825,-38.7655], "fy":[28.82314,27.96449,27.60168,28.48221]}, - {"t":13.45926, "x":3.39483, "y":5.26462, "heading":-1.01249, "vx":1.28793, "vy":-0.93292, "omega":-0.04657, "ax":-2.86317, "ay":2.07396, "alpha":0.10353, "fx":[-38.52008,-39.14843,-39.40445,-38.77222], "fy":[28.82795,27.96985,27.60506,28.485]}, - {"t":13.50423, "x":3.44985, "y":5.22477, "heading":-1.01458, "vx":1.15918, "vy":-0.83967, "omega":-0.04191, "ax":-2.86352, "ay":2.07421, "alpha":0.10354, "fx":[-38.52432,-39.15226,-39.40972,-38.77798], "fy":[28.83206,27.97447,27.60788,28.48728]}, - {"t":13.54919, "x":3.49908, "y":5.18911, "heading":-1.01647, "vx":1.03043, "vy":-0.7464, "omega":-0.03726, "ax":-2.86382, "ay":2.07443, "alpha":0.10355, "fx":[-38.52793,-39.1555,-39.41425,-38.78295], "fy":[28.8356,27.97849,27.61028,28.48918]}, - {"t":13.59416, "x":3.54251, "y":5.15765, "heading":-1.01814, "vx":0.90166, "vy":-0.65312, "omega":-0.0326, "ax":-2.86408, "ay":2.07462, "alpha":0.10355, "fx":[-38.53106,-39.1583,-39.41818,-38.78729], "fy":[28.83868,27.982,27.61235,28.49079]}, - {"t":13.63912, "x":3.58016, "y":5.13038, "heading":-1.01961, "vx":0.77287, "vy":-0.55984, "omega":-0.02794, "ax":-2.86431, "ay":2.07479, "alpha":0.10356, "fx":[-38.53379,-39.16074,-39.42163,-38.79108], "fy":[28.84138,27.98507,27.61416,28.49221]}, - {"t":13.68408, "x":3.61202, "y":5.1073, "heading":-1.02087, "vx":0.64408, "vy":-0.46655, "omega":-0.02329, "ax":-2.86451, "ay":2.07493, "alpha":0.10357, "fx":[-38.53621,-39.16291,-39.42466,-38.79441], "fy":[28.84375,27.98776,27.61576,28.49348]}, - {"t":13.72905, "x":3.63808, "y":5.08842, "heading":-1.02191, "vx":0.51528, "vy":-0.37325, "omega":-0.01863, "ax":-2.86469, "ay":2.07506, "alpha":0.10357, "fx":[-38.53838,-39.16487,-39.42734,-38.79733], "fy":[28.84584,27.9901,27.61721,28.49465]}, - {"t":13.77401, "x":3.65836, "y":5.07373, "heading":-1.02275, "vx":0.38647, "vy":-0.27994, "omega":-0.01397, "ax":-2.86485, "ay":2.07518, "alpha":0.10358, "fx":[-38.54034,-39.16667,-39.42972,-38.79991], "fy":[28.84768,27.99214,27.61853,28.49576]}, - {"t":13.81898, "x":3.67284, "y":5.06324, "heading":-1.02338, "vx":0.25765, "vy":-0.18663, "omega":-0.00932, "ax":-2.86499, "ay":2.07528, "alpha":0.10358, "fx":[-38.54212,-39.16834,-39.43184,-38.80217], "fy":[28.84931,27.9939,27.61974,28.49684]}, - {"t":13.86394, "x":3.68153, "y":5.05695, "heading":-1.0238, "vx":0.12883, "vy":-0.09332, "omega":-0.00466, "ax":-2.86512, "ay":2.07538, "alpha":0.10358, "fx":[-38.54377,-39.16992,-39.43374,-38.80415], "fy":[28.85074,27.9954,27.62088,28.4979]}, - {"t":13.90891, "x":3.68442, "y":5.05485, "heading":-1.02401, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], - "splits":[0] - }, - "events":[] -} diff --git a/src/main/deploy/pathplanner/paths/ReturnTrenchRight.path b/src/main/deploy/pathplanner/paths/ReturnTrenchRight.path new file mode 100644 index 0000000..8d47638 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ReturnTrenchRight.path @@ -0,0 +1,122 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.700699001426534, + "y": 3.3723777777777775 + }, + "prevControl": null, + "nextControl": { + "x": 7.290308373485518, + "y": 2.020871045563947 + }, + "isLocked": false, + "linkedName": "SwipeRightEnd" + }, + { + "anchor": { + "x": 5.931333333333333, + "y": 0.5269999999999992 + }, + "prevControl": { + "x": 6.840653196016428, + "y": 0.5642121011497352 + }, + "nextControl": { + "x": 5.13998205576381, + "y": 0.494615527830878 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.3880333333333335, + "y": 0.5269999999999992 + }, + "prevControl": { + "x": 4.3724540864184664, + "y": 0.49061197954017965 + }, + "nextControl": { + "x": 2.6308950776424673, + "y": 0.5549867752205017 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.978766666666667, + "y": 1.8522444444444432 + }, + "prevControl": { + "x": 2.8894665564826085, + "y": 1.618737505958421 + }, + "nextControl": { + "x": 3.3699884727203506, + "y": 2.875233173263234 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.1931444444444446, + "y": 2.9825999999999997 + }, + "prevControl": { + "x": 2.9828901003305504, + "y": 2.847347973097494 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9792531120331951, + "rotationDegrees": 177.87659887200687 + }, + { + "waypointRelativePos": 2.5, + "rotationDegrees": 179.39144110888557 + } + ], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.63, + "y": 4.03 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 3.0, + "maxWaypointRelativePos": 4.0, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 34.835830364154894 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -115.11483488614444 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SwipeRight.path b/src/main/deploy/pathplanner/paths/SwipeRight.path new file mode 100644 index 0000000..d6d1c1e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SwipeRight.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.700699001426534, + "y": 1.1302710413694728 + }, + "prevControl": null, + "nextControl": { + "x": 7.700699001426534, + "y": 2.1302710413694728 + }, + "isLocked": false, + "linkedName": "RightTrenchEnd" + }, + { + "anchor": { + "x": 7.700699001426534, + "y": 3.3723777777777775 + }, + "prevControl": { + "x": 7.700699001426534, + "y": 2.3723777777777775 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "SwipeRightEnd" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -115.11483488614444 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 2.0, + "rotation": -104.036 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TrenchRight.path b/src/main/deploy/pathplanner/paths/TrenchRight.path new file mode 100644 index 0000000..c5964a8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TrenchRight.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.8449786019971466, + "y": 0.5221540656205423 + }, + "prevControl": null, + "nextControl": { + "x": 6.329201141226819, + "y": 0.5092154065620549 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.290385164051354, + "y": 0.5221540656205423 + }, + "prevControl": { + "x": 5.977957318609738, + "y": 0.5221540656205423 + }, + "nextControl": { + "x": 6.602813009492971, + "y": 0.5221540656205423 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.700699001426534, + "y": 1.1302710413694728 + }, + "prevControl": { + "x": 7.7265763195435095, + "y": 0.40570613409415135 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "RightTrenchEnd" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 2.0, + "rotation": -104.03624346792651 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -32.90524292298786 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 746e71b..a6531c7 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,5 +1,5 @@ { - "robotWidth": 0.711, + "robotWidth": 0.7, "robotLength": 0.7112, "holonomicMode": true, "pathFolders": [], @@ -9,24 +9,26 @@ "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 52.1631, - "robotMOI": 6.883, + "robotMass": 51.94, + "robotMOI": 8.0, "robotTrackwidth": 0.546, "driveWheelRadius": 0.048, - "driveGearing": 5.143, + "driveGearing": 6.122448979591837, "maxDriveSpeed": 5.45, "driveMotorType": "krakenX60", "driveCurrentLimit": 60.0, "wheelCOF": 1.2, - "flModuleX": 0.273, - "flModuleY": 0.273, - "frModuleX": 0.273, - "frModuleY": -0.273, - "blModuleX": -0.273, - "blModuleY": 0.273, - "brModuleX": -0.273, - "brModuleY": -0.273, + "flModuleX": 0.276225, + "flModuleY": 0.276225, + "frModuleX": 0.276225, + "frModuleY": -0.276225, + "blModuleX": -0.276225, + "blModuleY": 0.276225, + "brModuleX": -0.276225, + "brModuleY": -0.276225, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, - "robotFeatures": [] + "robotFeatures": [ + "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":-0.45,\"y\":0.0},\"size\":{\"width\":0.7,\"length\":0.3},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}" + ] } \ No newline at end of file diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 175c056..78272a3 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Robot2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 173; - public static final String GIT_SHA = "177df4b78996e981f8083f7e2def65bf915f6b97"; - public static final String GIT_DATE = "2026-03-06 01:14:21 EST"; + public static final int GIT_REVISION = 174; + public static final String GIT_SHA = "a6a2e94e0245a0e6844a0a6e728b9de77987aad7"; + public static final String GIT_DATE = "2026-03-06 21:17:36 EST"; public static final String GIT_BRANCH = "sotm"; - public static final String BUILD_DATE = "2026-03-06 21:07:09 EST"; - public static final long BUILD_UNIX_TIME = 1772849229825L; + public static final String BUILD_DATE = "2026-03-06 22:53:20 EST"; + public static final long BUILD_UNIX_TIME = 1772855600747L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/auto/AutoRoutines.java b/src/main/java/frc/robot/auto/AutoRoutines.java index d4688d2..9158913 100644 --- a/src/main/java/frc/robot/auto/AutoRoutines.java +++ b/src/main/java/frc/robot/auto/AutoRoutines.java @@ -4,11 +4,16 @@ import frc.robot.lib.trajectory.RedTrajectory; import frc.robot.lib.trajectory.TrajectoryLoader; import frc.robot.lib.trajectory.RedTrajectory.TrajectoryType; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.commands.HeadingLockToHub2; import frc.robot.subsystems.drive.commands.PIDToPoseCommand; import frc.robot.subsystems.drive.commands.TrajectoryCommand; +import frc.robot.subsystems.intake.Intake; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; public final class AutoRoutines { // @Auto(name = "Pid Test") @@ -24,5 +29,62 @@ public static Command testTrajectoryAuto() { "testPath3").get(); return new TrajectoryCommand(traj); } + + // @Auto(name = "right neutral auto") + public static Command rightAutoNeutral() { + var tTrenchRight = TrajectoryLoader.loadAutoTrajectory( + TrajectoryType.PATHPLANNER, + "TrenchRight"); + + if (tTrenchRight.isEmpty()) { + DriverStation.reportWarning( + "Something happened", true); + return Commands.none(); + } + + var tSwipeRight = TrajectoryLoader.loadAutoTrajectory( + TrajectoryType.PATHPLANNER, + "SwipeRight"); + + if (tSwipeRight.isEmpty()) { + DriverStation.reportWarning( + "Something happened", true); + return Commands.none(); + } + + var tReturnTrenchRight = TrajectoryLoader.loadAutoTrajectory( + TrajectoryType.PATHPLANNER, + "ReturnTrenchRight"); + + if (tReturnTrenchRight.isEmpty()) { + DriverStation.reportWarning( + "Something happened", true); + return Commands.none(); + } + + var crossTrench = tTrenchRight.get(); + var swipe = tSwipeRight.get(); + var back = tReturnTrenchRight.get(); + return + Intake.getInstance().calibrateZero() + .alongWith( + new TrajectoryCommand(crossTrench)) + .andThen( + Intake.getInstance().intake()) + .andThen( + new TrajectoryCommand(swipe)) + .andThen( + Intake.getInstance().lower() + .alongWith( + new TrajectoryCommand(back))) + .andThen( + Drive.getInstance().headingLockToHub()) + .alongWith( + Automation.shootAll() + .andThen( + Commands.waitSeconds(0.5)) + .andThen( + Automation.indexAll())); + } } diff --git a/src/main/java/frc/robot/auto/AutoSelector.java b/src/main/java/frc/robot/auto/AutoSelector.java index c98705c..760a088 100644 --- a/src/main/java/frc/robot/auto/AutoSelector.java +++ b/src/main/java/frc/robot/auto/AutoSelector.java @@ -26,29 +26,32 @@ public class AutoSelector { private final SendableChooser> chooser = new SendableChooser<>(); public AutoSelector() { - Method[] autos = AutoRoutines.class.getMethods(); - // Warning: dark and evil magic below - for (Method auto : autos) { - if (auto.isAnnotationPresent(Auto.class)) { - String name = auto.getAnnotation(Auto.class).name(); - if (name.equals("")) { - name = auto.getName(); - } + // Method[] autos = AutoRoutines.class.getMethods(); + // // Warning: dark and evil magic below + // for (Method auto : autos) { + // if (auto.isAnnotationPresent(Auto.class)) { + // String name = auto.getAnnotation(Auto.class).name(); + // if (name.equals("")) { + // name = auto.getName(); + // } - if (auto.getReturnType() == Command.class) { - chooser.addOption(name, () -> { - try { - return (Command) auto.invoke(null); - } catch (Exception e) { - return null; - } - }); - } else { - DriverStation.reportWarning( - "@Auto annotation for this element is not supported", true); - } - } - } + // if (auto.getReturnType() == Command.class) { + // chooser.addOption(name, () -> { + // try { + // return (Command) auto.invoke(null); + // } catch (Exception e) { + // DriverStation.reportWarning( + // "something really bad happened", true); + // return null; + // } + // }); + // } else { + // DriverStation.reportWarning( + // "@Auto annotation for this element is not supported", true); + // } + // } + // } + chooser.addOption("right", () -> AutoRoutines.rightAutoNeutral()); chooser.setDefaultOption("None", () -> null); SmartDashboard.putData("Auto Selector", chooser); diff --git a/src/main/java/frc/robot/auto/Automation.java b/src/main/java/frc/robot/auto/Automation.java index 8de0739..9a97149 100644 --- a/src/main/java/frc/robot/auto/Automation.java +++ b/src/main/java/frc/robot/auto/Automation.java @@ -1,5 +1,42 @@ package frc.robot.auto; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.indexer.Indexer; +import frc.robot.subsystems.roller.Roller; +import frc.robot.subsystems.shooter.Shooter; + public class Automation { - + public static Command shootAll() { + return Commands.parallel( + Shooter.getRightInstance().shoot(), + Shooter.getLeftInstance().shoot()); + } + + public static Command stopShoot() { + return Commands.parallel( + Shooter.getRightInstance().stop(), + Shooter.getLeftInstance().stop()); + } + + public static Command indexAll() { + return Commands.parallel( + Indexer.getRightInstance().activateIndexer(), + Indexer.getLeftInstance().activateIndexer(), + Roller.getInstance().roll()); + } + + public static Command stopIndex() { + return Commands.parallel( + Indexer.getRightInstance().deactivateIndexer(), + Indexer.getLeftInstance().deactivateIndexer(), + Roller.getInstance().stop()); + } + + public static Command backIndex() { + return Commands.parallel( + Indexer.getRightInstance().back(), + Indexer.getLeftInstance().back(), + Roller.getInstance().antiRoll()); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/commands/HeadingLockToHub2.java b/src/main/java/frc/robot/subsystems/drive/commands/HeadingLockToHub2.java new file mode 100644 index 0000000..ebdbb24 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/commands/HeadingLockToHub2.java @@ -0,0 +1,131 @@ +package frc.robot.subsystems.drive.commands; + +import static frc.robot.subsystems.drive.DriveConstants.*; + +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.lib.control.ControlConstants.PIDVConstants; +import frc.robot.lib.control.ControlConstants.ProfiledPIDVConstants; +import frc.robot.lib.control.ProfiledPIDVController; +import frc.robot.lib.util.Util; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.DriveConstants.FieldPoses; + +public class HeadingLockToHub2 extends Command { + private final Drive drive; + + private final SwerveRequest.FieldCentric request; + private final ProfiledPIDVController thetaController; + + private final Translation2d pose; + + private final Debouncer finishDebouncer; + + public HeadingLockToHub2() { + this( + Constants.FieldConstants.allianceCorrected( + FieldPoses.HUB.pose3d.getTranslation()) + .toTranslation2d()); + } + + /** + * Locks the robot onto a pose. + * Utilizes feedforwards derived from the current chassis speeds + */ + public HeadingLockToHub2(Translation2d pose) { + this(Drive.getInstance(), pose); + } + + /** + * Locks the robot onto a pose. + * Utilizes feedforwards derived from the current chassis speeds + */ + public HeadingLockToHub2(Drive drive, Translation2d pose) { + this.pose = pose; + this.drive = drive; + request = + new SwerveRequest.FieldCentric(); + + thetaController = + new ProfiledPIDVController( + new ProfiledPIDVConstants( + new PIDVConstants(10.0, 0.0, 1), + new TrapezoidProfile.Constraints(Math.PI * 16, Math.PI * 5)) + ); + thetaController.enableContinuousInput(-Math.PI, Math.PI); + + finishDebouncer = new Debouncer(0.040, DebounceType.kRising); + addRequirements(drive); + } + + @Override + public void initialize() { + request.withVelocityX(0).withVelocityY(0) + .withRotationalRate(0); + drive.setSwerveRequest(request); + + thetaController.setInitialSetpoint( + drive.getPose().getRotation().getRadians(), + drive.getState().Speeds.omegaRadiansPerSecond); + } + + @Override + public void execute() { + double xDesiredRaw = -Robot.controller.getLeftY(); + double yDesiredRaw = -Robot.controller.getLeftX(); + + double[] xy = Util.applyRadialDeadband(xDesiredRaw, yDesiredRaw, Constants.Controllers.DRIVER_DEADBAND); + double xFancy = xy[0]; + double yFancy = xy[1]; + + var state = drive.getState(); + var delta = pose.minus(drive.getPose().getTranslation()); + var targetDirection = delta.getAngle(); + + var normSq = delta.getNorm() * delta.getNorm(); + var fieldSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(state.Speeds, drive.getPose().getRotation()); + var rotationalRate = normSq > 1e-4 ? + (-delta.getX() * fieldSpeeds.vyMetersPerSecond + + delta.getY() * fieldSpeeds.vxMetersPerSecond) + / (normSq) : 0.0; + + var rotation = thetaController + .setTarget(targetDirection.getRadians(), rotationalRate) + .setMeasurement(state.Pose.getRotation().getRadians(), state.Speeds.omegaRadiansPerSecond) + .getOutput(); + + SmartDashboard.putNumber("error tracking", + MathUtil.inputModulus(state.Pose.getRotation().minus(targetDirection).getDegrees(), -180, 180 + )); + + request + // .withHeadingPID(p.get(), i.get(), d.get()) + .withVelocityX(xFancy * MAX_SPEED) + .withVelocityY(yFancy * MAX_SPEED) + .withRotationalRate(rotation); + } + + @Override + public boolean isFinished() { + return finishDebouncer.calculate( + MathUtil.isNear(thetaController.getError(), 0, EPSILON_ROTATION) + ); + } + + @Override + public void end(boolean interrupted) { + if (interrupted) { + drive.setSwerveRequest(new SwerveRequest.ApplyRobotSpeeds()); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java index 1cf8f9d..d8cca92 100644 --- a/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java +++ b/src/main/java/frc/robot/subsystems/drive/commands/TrajectoryCommand.java @@ -81,6 +81,7 @@ public Command withPIDToPoseAtEnd() { public void initialize() { timer.start(); // actually starts the timer drive.setSwerveRequest(request); + thetaController.setInitialSetpoint(drive.getPose().getRotation().getRadians(), drive.getFieldSpeeds().omegaRadiansPerSecond); } @Override From 0547c1a7b653d946d3068e238282f76452ee03d5 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Fri, 6 Mar 2026 20:02:05 -0800 Subject: [PATCH 107/118] Fixed auto selector shenanigans --- src/main/java/frc/robot/BuildConstants.java | 12 ++--- .../java/frc/robot/auto/AutoRoutines.java | 2 +- .../java/frc/robot/auto/AutoSelector.java | 54 ++++++++++--------- 3 files changed, 35 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 78272a3..7755f0b 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,13 +7,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Robot2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 174; - public static final String GIT_SHA = "a6a2e94e0245a0e6844a0a6e728b9de77987aad7"; - public static final String GIT_DATE = "2026-03-06 21:17:36 EST"; + public static final int GIT_REVISION = 175; + public static final String GIT_SHA = "479473ffcc67b97a8d36cca6c117c2b1adff9bfa"; + public static final String GIT_DATE = "2026-03-06 22:56:55 EST"; public static final String GIT_BRANCH = "sotm"; - public static final String BUILD_DATE = "2026-03-06 22:53:20 EST"; - public static final long BUILD_UNIX_TIME = 1772855600747L; - public static final int DIRTY = 1; + public static final String BUILD_DATE = "2026-03-06 22:58:42 EST"; + public static final long BUILD_UNIX_TIME = 1772855922105L; + public static final int DIRTY = 0; private BuildConstants(){} } diff --git a/src/main/java/frc/robot/auto/AutoRoutines.java b/src/main/java/frc/robot/auto/AutoRoutines.java index 9158913..302bceb 100644 --- a/src/main/java/frc/robot/auto/AutoRoutines.java +++ b/src/main/java/frc/robot/auto/AutoRoutines.java @@ -30,7 +30,7 @@ public static Command testTrajectoryAuto() { return new TrajectoryCommand(traj); } - // @Auto(name = "right neutral auto") + @Auto(name = "right neutral auto") public static Command rightAutoNeutral() { var tTrenchRight = TrajectoryLoader.loadAutoTrajectory( TrajectoryType.PATHPLANNER, diff --git a/src/main/java/frc/robot/auto/AutoSelector.java b/src/main/java/frc/robot/auto/AutoSelector.java index 760a088..5a0dec3 100644 --- a/src/main/java/frc/robot/auto/AutoSelector.java +++ b/src/main/java/frc/robot/auto/AutoSelector.java @@ -5,12 +5,14 @@ import java.lang.annotation.RetentionPolicy; import java.lang.annotation.Target; import java.lang.reflect.Method; +import java.util.Optional; import java.util.function.Supplier; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; /** * A class to select autos @@ -26,32 +28,32 @@ public class AutoSelector { private final SendableChooser> chooser = new SendableChooser<>(); public AutoSelector() { - // Method[] autos = AutoRoutines.class.getMethods(); - // // Warning: dark and evil magic below - // for (Method auto : autos) { - // if (auto.isAnnotationPresent(Auto.class)) { - // String name = auto.getAnnotation(Auto.class).name(); - // if (name.equals("")) { - // name = auto.getName(); - // } + Method[] autos = AutoRoutines.class.getMethods(); + // Warning: dark and evil magic below + for (Method auto : autos) { + if (auto.isAnnotationPresent(Auto.class)) { + String name = auto.getAnnotation(Auto.class).name(); + if (name.equals("")) { + name = auto.getName(); + } - // if (auto.getReturnType() == Command.class) { - // chooser.addOption(name, () -> { - // try { - // return (Command) auto.invoke(null); - // } catch (Exception e) { - // DriverStation.reportWarning( - // "something really bad happened", true); - // return null; - // } - // }); - // } else { - // DriverStation.reportWarning( - // "@Auto annotation for this element is not supported", true); - // } - // } - // } - chooser.addOption("right", () -> AutoRoutines.rightAutoNeutral()); + if (auto.getReturnType() == Command.class) { + chooser.addOption(name, () -> { + try { + return (Command) auto.invoke(null); + } catch (Exception e) { + DriverStation.reportWarning( + "something really bad happened", true); + return null; + } + }); + } else { + DriverStation.reportWarning( + "@Auto annotation for this element is not supported", true); + } + } + } + // chooser.addOption("right", () -> AutoRoutines.rightAutoNeutral()); chooser.setDefaultOption("None", () -> null); SmartDashboard.putData("Auto Selector", chooser); @@ -59,6 +61,6 @@ public AutoSelector() { /** Gets the auto selected from the SmartDashboard */ public Command getAuto() { - return chooser.getSelected().get(); + return Optional.of(chooser.getSelected()).orElse(() -> Commands.none()).get(); } } From 78307cdb71e36e3aea91ffe4817f72e976082c12 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Fri, 6 Mar 2026 20:59:50 -0800 Subject: [PATCH 108/118] Left neutral auto --- .../deploy/pathplanner/paths/HangLeft1.path | 2 +- .../deploy/pathplanner/paths/HangLeft2.path | 2 +- .../deploy/pathplanner/paths/PreloadLeft.path | 2 +- .../pathplanner/paths/ReturnTrenchLeft.path | 106 ++++++++++++++++++ .../pathplanner/paths/ReturnTrenchRight.path | 2 +- .../deploy/pathplanner/paths/SwipeLeft.path | 54 +++++++++ .../deploy/pathplanner/paths/SwipeRight.path | 6 +- .../deploy/pathplanner/paths/TrenchLeft.path | 75 +++++++++++++ .../deploy/pathplanner/paths/TrenchRight.path | 4 +- src/main/deploy/pathplanner/settings.json | 5 +- src/main/java/frc/robot/BuildConstants.java | 12 +- .../java/frc/robot/auto/AutoRoutines.java | 57 ++++++++++ 12 files changed, 311 insertions(+), 16 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/ReturnTrenchLeft.path create mode 100644 src/main/deploy/pathplanner/paths/SwipeLeft.path create mode 100644 src/main/deploy/pathplanner/paths/TrenchLeft.path diff --git a/src/main/deploy/pathplanner/paths/HangLeft1.path b/src/main/deploy/pathplanner/paths/HangLeft1.path index acd2a64..b93dc3a 100644 --- a/src/main/deploy/pathplanner/paths/HangLeft1.path +++ b/src/main/deploy/pathplanner/paths/HangLeft1.path @@ -45,7 +45,7 @@ "rotation": 90.0 }, "reversed": false, - "folder": null, + "folder": "Hang Left", "idealStartingState": { "velocity": 0, "rotation": 45.0 diff --git a/src/main/deploy/pathplanner/paths/HangLeft2.path b/src/main/deploy/pathplanner/paths/HangLeft2.path index d91123e..82d224e 100644 --- a/src/main/deploy/pathplanner/paths/HangLeft2.path +++ b/src/main/deploy/pathplanner/paths/HangLeft2.path @@ -45,7 +45,7 @@ "rotation": 90.0 }, "reversed": false, - "folder": null, + "folder": "Hang Left", "idealStartingState": { "velocity": 0, "rotation": 90.0 diff --git a/src/main/deploy/pathplanner/paths/PreloadLeft.path b/src/main/deploy/pathplanner/paths/PreloadLeft.path index 96137bd..c14fff5 100644 --- a/src/main/deploy/pathplanner/paths/PreloadLeft.path +++ b/src/main/deploy/pathplanner/paths/PreloadLeft.path @@ -45,7 +45,7 @@ "rotation": 45.0 }, "reversed": false, - "folder": null, + "folder": "Hang Left", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/ReturnTrenchLeft.path b/src/main/deploy/pathplanner/paths/ReturnTrenchLeft.path new file mode 100644 index 0000000..e835170 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ReturnTrenchLeft.path @@ -0,0 +1,106 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.665844444444444, + "y": 4.765833333333332 + }, + "prevControl": null, + "nextControl": { + "x": 7.295555555555556, + "y": 5.866955555555555 + }, + "isLocked": false, + "linkedName": "SwipeLeftEnd" + }, + { + "anchor": { + "x": 5.931333333333333, + "y": 7.416322222222221 + }, + "prevControl": { + "x": 6.759611111111111, + "y": 7.435811111111111 + }, + "nextControl": { + "x": 5.681402508763545, + "y": 7.410441496938226 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.3393111111111113, + "y": 7.416322222222221 + }, + "prevControl": { + "x": 3.836277777777778, + "y": 7.474788888888888 + }, + "nextControl": { + "x": 2.876194950316932, + "y": 7.361837968011141 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.154166666666667, + "y": 5.067911111111112 + }, + "prevControl": { + "x": 3.173655555555556, + "y": 5.876700000000001 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2.0, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.62, + "y": 4.039 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 2.3, + "maxWaypointRelativePos": 3.0, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -27.28121112254839 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 122.27564431457749 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReturnTrenchRight.path b/src/main/deploy/pathplanner/paths/ReturnTrenchRight.path index 8d47638..7a85d5a 100644 --- a/src/main/deploy/pathplanner/paths/ReturnTrenchRight.path +++ b/src/main/deploy/pathplanner/paths/ReturnTrenchRight.path @@ -113,7 +113,7 @@ "rotation": 34.835830364154894 }, "reversed": false, - "folder": null, + "folder": "Right", "idealStartingState": { "velocity": 0, "rotation": -115.11483488614444 diff --git a/src/main/deploy/pathplanner/paths/SwipeLeft.path b/src/main/deploy/pathplanner/paths/SwipeLeft.path new file mode 100644 index 0000000..08d4281 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SwipeLeft.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.617122222222221, + "y": 6.714722222222222 + }, + "prevControl": null, + "nextControl": { + "x": 7.617122222222221, + "y": 5.714722222222221 + }, + "isLocked": false, + "linkedName": "TrenchLeftEnd" + }, + { + "anchor": { + "x": 7.665844444444444, + "y": 4.765833333333332 + }, + "prevControl": { + "x": 7.665844444444444, + "y": 5.765833333333332 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "SwipeLeftEnd" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 122.27564431457749 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 2.0, + "rotation": 116.56505117707796 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SwipeRight.path b/src/main/deploy/pathplanner/paths/SwipeRight.path index d6d1c1e..a723312 100644 --- a/src/main/deploy/pathplanner/paths/SwipeRight.path +++ b/src/main/deploy/pathplanner/paths/SwipeRight.path @@ -12,7 +12,7 @@ "y": 2.1302710413694728 }, "isLocked": false, - "linkedName": "RightTrenchEnd" + "linkedName": "TrenchRightEnd" }, { "anchor": { @@ -45,10 +45,10 @@ "rotation": -115.11483488614444 }, "reversed": false, - "folder": null, + "folder": "Right", "idealStartingState": { "velocity": 2.0, - "rotation": -104.036 + "rotation": -104.03624346792651 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TrenchLeft.path b/src/main/deploy/pathplanner/paths/TrenchLeft.path new file mode 100644 index 0000000..e070f86 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TrenchLeft.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6219, + "y": 7.562488888888888 + }, + "prevControl": null, + "nextControl": { + "x": 4.791381184057424, + "y": 7.511295674470193 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.2236666666666665, + "y": 7.562488888888888 + }, + "prevControl": { + "x": 5.754164134386084, + "y": 7.5540188679051194 + }, + "nextControl": { + "x": 6.5900361678797, + "y": 7.569098347091411 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.617122222222221, + "y": 6.714722222222222 + }, + "prevControl": { + "x": 7.617122222222221, + "y": 7.377416090897831 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "TrenchLeftEnd" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 2.0, + "rotation": 116.56505117707796 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 23.025492008528023 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TrenchRight.path b/src/main/deploy/pathplanner/paths/TrenchRight.path index c5964a8..1f4bcce 100644 --- a/src/main/deploy/pathplanner/paths/TrenchRight.path +++ b/src/main/deploy/pathplanner/paths/TrenchRight.path @@ -41,7 +41,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "RightTrenchEnd" + "linkedName": "TrenchRightEnd" } ], "rotationTargets": [ @@ -66,7 +66,7 @@ "rotation": -104.03624346792651 }, "reversed": false, - "folder": null, + "folder": "Right", "idealStartingState": { "velocity": 0, "rotation": -32.90524292298786 diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index a6531c7..8c12284 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -2,7 +2,10 @@ "robotWidth": 0.7, "robotLength": 0.7112, "holonomicMode": true, - "pathFolders": [], + "pathFolders": [ + "Hang Left", + "Right" + ], "autoFolders": [], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 7755f0b..6043d08 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,13 +7,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Robot2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 175; - public static final String GIT_SHA = "479473ffcc67b97a8d36cca6c117c2b1adff9bfa"; - public static final String GIT_DATE = "2026-03-06 22:56:55 EST"; + public static final int GIT_REVISION = 176; + public static final String GIT_SHA = "0547c1a7b653d946d3068e238282f76452ee03d5"; + public static final String GIT_DATE = "2026-03-06 23:02:05 EST"; public static final String GIT_BRANCH = "sotm"; - public static final String BUILD_DATE = "2026-03-06 22:58:42 EST"; - public static final long BUILD_UNIX_TIME = 1772855922105L; - public static final int DIRTY = 0; + public static final String BUILD_DATE = "2026-03-06 23:23:02 EST"; + public static final long BUILD_UNIX_TIME = 1772857382884L; + public static final int DIRTY = 1; private BuildConstants(){} } diff --git a/src/main/java/frc/robot/auto/AutoRoutines.java b/src/main/java/frc/robot/auto/AutoRoutines.java index 302bceb..4747e96 100644 --- a/src/main/java/frc/robot/auto/AutoRoutines.java +++ b/src/main/java/frc/robot/auto/AutoRoutines.java @@ -86,5 +86,62 @@ public static Command rightAutoNeutral() { .andThen( Automation.indexAll())); } + + @Auto(name = "left neutral auto") + public static Command leftAutoNeutral() { + var tTrenchLeft = TrajectoryLoader.loadAutoTrajectory( + TrajectoryType.PATHPLANNER, + "TrenchLeft"); + + if (tTrenchLeft.isEmpty()) { + DriverStation.reportWarning( + "Something happened", true); + return Commands.none(); + } + + var tSwipeLeft = TrajectoryLoader.loadAutoTrajectory( + TrajectoryType.PATHPLANNER, + "SwipeLeft"); + + if (tSwipeLeft.isEmpty()) { + DriverStation.reportWarning( + "Something happened", true); + return Commands.none(); + } + + var tReturnTrenchLeft = TrajectoryLoader.loadAutoTrajectory( + TrajectoryType.PATHPLANNER, + "ReturnTrenchLeft"); + + if (tReturnTrenchLeft.isEmpty()) { + DriverStation.reportWarning( + "Something happened", true); + return Commands.none(); + } + + var crossTrench = tTrenchLeft.get(); + var swipe = tSwipeLeft.get(); + var back = tReturnTrenchLeft.get(); + return + Intake.getInstance().calibrateZero() + .alongWith( + new TrajectoryCommand(crossTrench)) + .andThen( + Intake.getInstance().intake()) + .andThen( + new TrajectoryCommand(swipe)) + .andThen( + Intake.getInstance().lower() + .alongWith( + new TrajectoryCommand(back))) + .andThen( + Drive.getInstance().headingLockToHub()) + .alongWith( + Automation.shootAll() + .andThen( + Commands.waitSeconds(0.5)) + .andThen( + Automation.indexAll())); + } } From 3de8d1c1fab502907f0472bc5aad86c892c0129a Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Sat, 7 Mar 2026 09:46:15 -0800 Subject: [PATCH 109/118] hotifxes for a llot of things --- .../deploy/pathplanner/paths/DepotCenter.path | 68 +++++++ .../pathplanner/paths/DepotShootCenter.path | 65 +++++++ .../deploy/pathplanner/paths/HangLeft.path | 54 ++++++ .../deploy/pathplanner/paths/PreloadLeft.path | 4 +- .../pathplanner/paths/ReturnTrenchLeft.path | 54 ++++-- .../pathplanner/paths/ReturnTrenchRight.path | 48 ++--- .../pathplanner/paths/StationCenter.path | 54 ++++++ .../pathplanner/paths/StationShootCenter.path | 65 +++++++ .../deploy/pathplanner/paths/SwipeLeft.path | 6 +- .../deploy/pathplanner/paths/SwipeRight.path | 6 +- .../deploy/pathplanner/paths/TrenchLeft.path | 6 +- .../deploy/pathplanner/paths/TrenchRight.path | 18 +- src/main/deploy/pathplanner/settings.json | 20 +- src/main/java/frc/robot/BuildConstants.java | 10 +- .../java/frc/robot/auto/AutoRoutines.java | 182 ++++++++++++++++-- .../java/frc/robot/auto/AutoSelector.java | 4 +- .../subsystems/drive/DriveConstants.java | 4 +- .../frc/robot/subsystems/intake/Intake.java | 5 +- .../subsystems/intake/IntakeConstants.java | 2 +- .../subsystems/vision/VisionConstants.java | 6 +- .../vision/VisionDeviceManager.java | 12 +- 21 files changed, 583 insertions(+), 110 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/DepotCenter.path create mode 100644 src/main/deploy/pathplanner/paths/DepotShootCenter.path create mode 100644 src/main/deploy/pathplanner/paths/HangLeft.path create mode 100644 src/main/deploy/pathplanner/paths/StationCenter.path create mode 100644 src/main/deploy/pathplanner/paths/StationShootCenter.path diff --git a/src/main/deploy/pathplanner/paths/DepotCenter.path b/src/main/deploy/pathplanner/paths/DepotCenter.path new file mode 100644 index 0000000..399259c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/DepotCenter.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.45681883024251, + "y": 4.403751783166904 + }, + "prevControl": null, + "nextControl": { + "x": 2.9263338088445074, + "y": 4.597831669044223 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7914550641940081, + "y": 6.072838801711842 + }, + "prevControl": { + "x": 2.564051355206847, + "y": 6.085777460770328 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "DepotCenterEnd" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.8669817690749491, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Alliance Center", + "idealStartingState": { + "velocity": 0, + "rotation": -36.02737338510356 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/DepotShootCenter.path b/src/main/deploy/pathplanner/paths/DepotShootCenter.path new file mode 100644 index 0000000..cab67a4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/DepotShootCenter.path @@ -0,0 +1,65 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.7914550641940081, + "y": 6.072838801711842 + }, + "prevControl": null, + "nextControl": { + "x": 1.7914550641940081, + "y": 6.072838801711842 + }, + "isLocked": false, + "linkedName": "DepotCenterEnd" + }, + { + "anchor": { + "x": 2.8487018544935796, + "y": 4.015592011412268 + }, + "prevControl": { + "x": 2.7840085592011405, + "y": 5.1800713266761775 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "DepotShootCenterEnd" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.623, + "y": 4.05 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.25, + "maxWaypointRelativePos": 1.0, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Alliance Center", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HangLeft.path b/src/main/deploy/pathplanner/paths/HangLeft.path new file mode 100644 index 0000000..b826858 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HangLeft.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.154166666666667, + "y": 5.067911111111112 + }, + "prevControl": null, + "nextControl": { + "x": 2.325888888888889, + "y": 5.564877777777777 + }, + "isLocked": false, + "linkedName": "ReturnTrenchLeftEnd" + }, + { + "anchor": { + "x": 1.6632666666666664, + "y": 4.678133333333332 + }, + "prevControl": { + "x": 2.4428222222222193, + "y": 4.668388888888886 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -88.51854282911293 + }, + "reversed": false, + "folder": "Neutral Left", + "idealStartingState": { + "velocity": 0, + "rotation": -27.28121112254839 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/PreloadLeft.path b/src/main/deploy/pathplanner/paths/PreloadLeft.path index c14fff5..971e07d 100644 --- a/src/main/deploy/pathplanner/paths/PreloadLeft.path +++ b/src/main/deploy/pathplanner/paths/PreloadLeft.path @@ -35,8 +35,8 @@ "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/ReturnTrenchLeft.path b/src/main/deploy/pathplanner/paths/ReturnTrenchLeft.path index e835170..20d568d 100644 --- a/src/main/deploy/pathplanner/paths/ReturnTrenchLeft.path +++ b/src/main/deploy/pathplanner/paths/ReturnTrenchLeft.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 7.295555555555556, - "y": 5.866955555555555 + "x": 7.207357766605293, + "y": 5.841058521048764 }, "isLocked": false, "linkedName": "SwipeLeftEnd" @@ -20,12 +20,12 @@ "y": 7.416322222222221 }, "prevControl": { - "x": 6.759611111111111, - "y": 7.435811111111111 + "x": 6.7162227377693515, + "y": 7.202279252827075 }, "nextControl": { - "x": 5.681402508763545, - "y": 7.410441496938226 + "x": 5.615054102478352, + "y": 7.502573030740725 }, "isLocked": false, "linkedName": null @@ -36,12 +36,28 @@ "y": 7.416322222222221 }, "prevControl": { - "x": 3.836277777777778, - "y": 7.474788888888888 + "x": 3.687662243109644, + "y": 7.421700229689098 }, "nextControl": { - "x": 2.876194950316932, - "y": 7.361837968011141 + "x": 2.7760581588108626, + "y": 7.407626460087526 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.382910128388017, + "y": 6.512753209700428 + }, + "prevControl": { + "x": 2.402438844725988, + "y": 6.872100722346741 + }, + "nextControl": { + "x": 2.3580322232939324, + "y": 6.0549753834114455 }, "isLocked": false, "linkedName": null @@ -52,12 +68,12 @@ "y": 5.067911111111112 }, "prevControl": { - "x": 3.173655555555556, - "y": 5.876700000000001 + "x": 2.9486968238238505, + "y": 5.376805917860191 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "ReturnTrenchLeftEnd" } ], "rotationTargets": [ @@ -66,7 +82,7 @@ "rotationDegrees": 180.0 }, { - "waypointRelativePos": 2.0, + "waypointRelativePos": 3.0, "rotationDegrees": 180.0 } ], @@ -78,8 +94,8 @@ "y": 4.039 }, "rotationOffset": 0.0, - "minWaypointRelativePos": 2.3, - "maxWaypointRelativePos": 3.0, + "minWaypointRelativePos": 3.2, + "maxWaypointRelativePos": 4.0, "name": "Point Towards Zone" } ], @@ -87,8 +103,8 @@ "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, "nominalVoltage": 12.0, "unlimited": false }, @@ -97,7 +113,7 @@ "rotation": -27.28121112254839 }, "reversed": false, - "folder": null, + "folder": "Neutral Left", "idealStartingState": { "velocity": 0, "rotation": 122.27564431457749 diff --git a/src/main/deploy/pathplanner/paths/ReturnTrenchRight.path b/src/main/deploy/pathplanner/paths/ReturnTrenchRight.path index 7a85d5a..89cf7a0 100644 --- a/src/main/deploy/pathplanner/paths/ReturnTrenchRight.path +++ b/src/main/deploy/pathplanner/paths/ReturnTrenchRight.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 7.290308373485518, - "y": 2.020871045563947 + "x": 7.033137820589351, + "y": 1.984221330339962 }, "isLocked": false, "linkedName": "SwipeRightEnd" @@ -20,44 +20,44 @@ "y": 0.5269999999999992 }, "prevControl": { - "x": 6.840653196016428, - "y": 0.5642121011497352 + "x": 6.989072753209699, + "y": 0.4962767475035662 }, "nextControl": { - "x": 5.13998205576381, - "y": 0.494615527830878 + "x": 5.383155793497732, + "y": 0.5429224442738556 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.3880333333333335, + "x": 3.379186875891583, "y": 0.5269999999999992 }, "prevControl": { - "x": 4.3724540864184664, - "y": 0.49061197954017965 + "x": 4.2590156918687585, + "y": 0.5221540656205423 }, "nextControl": { - "x": 2.6308950776424673, - "y": 0.5549867752205017 + "x": 3.129190667808615, + "y": 0.5283769328731714 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.978766666666667, - "y": 1.8522444444444432 + "x": 2.9522111269614832, + "y": 1.013823109843081 }, "prevControl": { - "x": 2.8894665564826085, - "y": 1.618737505958421 + "x": 2.9723228063699474, + "y": 0.6953053819320217 }, "nextControl": { - "x": 3.3699884727203506, - "y": 2.875233173263234 + "x": 2.9026908586211704, + "y": 1.7980979104181296 }, "isLocked": false, "linkedName": null @@ -68,8 +68,8 @@ "y": 2.9825999999999997 }, "prevControl": { - "x": 2.9828901003305504, - "y": 2.847347973097494 + "x": 2.971754978667799, + "y": 0.7916137458990562 }, "nextControl": null, "isLocked": false, @@ -82,7 +82,7 @@ "rotationDegrees": 177.87659887200687 }, { - "waypointRelativePos": 2.5, + "waypointRelativePos": 3.0021321961620533, "rotationDegrees": 179.39144110888557 } ], @@ -94,7 +94,7 @@ "y": 4.03 }, "rotationOffset": 0.0, - "minWaypointRelativePos": 3.0, + "minWaypointRelativePos": 3.41, "maxWaypointRelativePos": 4.0, "name": "Point Towards Zone" } @@ -103,8 +103,8 @@ "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, "nominalVoltage": 12.0, "unlimited": false }, @@ -113,7 +113,7 @@ "rotation": 34.835830364154894 }, "reversed": false, - "folder": "Right", + "folder": "Neutral Right", "idealStartingState": { "velocity": 0, "rotation": -115.11483488614444 diff --git a/src/main/deploy/pathplanner/paths/StationCenter.path b/src/main/deploy/pathplanner/paths/StationCenter.path new file mode 100644 index 0000000..5d62864 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/StationCenter.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.8487018544935796, + "y": 4.015592011412268 + }, + "prevControl": null, + "nextControl": { + "x": 2.2016666534554235, + "y": 3.3233789905366047 + }, + "isLocked": false, + "linkedName": "DepotShootCenterEnd" + }, + { + "anchor": { + "x": 0.5973751783166898, + "y": 0.8973751783166914 + }, + "prevControl": { + "x": 1.0680613214397714, + "y": 1.5780987777774225 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "StationCenterEnd" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Alliance Center", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/StationShootCenter.path b/src/main/deploy/pathplanner/paths/StationShootCenter.path new file mode 100644 index 0000000..655635a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/StationShootCenter.path @@ -0,0 +1,65 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.5973751783166898, + "y": 0.8973751783166914 + }, + "prevControl": null, + "nextControl": { + "x": 1.4786361957748682, + "y": 1.4318016192691616 + }, + "isLocked": false, + "linkedName": "StationCenterEnd" + }, + { + "anchor": { + "x": 3.2368616262482157, + "y": 4.028530670470756 + }, + "prevControl": { + "x": 2.242255154397988, + "y": 4.030653058607221 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.623, + "y": 4.05 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.49155975692099935, + "maxWaypointRelativePos": 1.0, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Alliance Center", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SwipeLeft.path b/src/main/deploy/pathplanner/paths/SwipeLeft.path index 08d4281..0e85076 100644 --- a/src/main/deploy/pathplanner/paths/SwipeLeft.path +++ b/src/main/deploy/pathplanner/paths/SwipeLeft.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 1.5, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -45,10 +45,10 @@ "rotation": 122.27564431457749 }, "reversed": false, - "folder": null, + "folder": "Neutral Left", "idealStartingState": { "velocity": 2.0, "rotation": 116.56505117707796 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SwipeRight.path b/src/main/deploy/pathplanner/paths/SwipeRight.path index a723312..9bdbfc7 100644 --- a/src/main/deploy/pathplanner/paths/SwipeRight.path +++ b/src/main/deploy/pathplanner/paths/SwipeRight.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 1.5, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -45,10 +45,10 @@ "rotation": -115.11483488614444 }, "reversed": false, - "folder": "Right", + "folder": "Neutral Right", "idealStartingState": { "velocity": 2.0, "rotation": -104.03624346792651 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TrenchLeft.path b/src/main/deploy/pathplanner/paths/TrenchLeft.path index e070f86..a6d7534 100644 --- a/src/main/deploy/pathplanner/paths/TrenchLeft.path +++ b/src/main/deploy/pathplanner/paths/TrenchLeft.path @@ -56,8 +56,8 @@ "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, "nominalVoltage": 12.0, "unlimited": false }, @@ -66,7 +66,7 @@ "rotation": 116.56505117707796 }, "reversed": false, - "folder": null, + "folder": "Neutral Left", "idealStartingState": { "velocity": 0, "rotation": 23.025492008528023 diff --git a/src/main/deploy/pathplanner/paths/TrenchRight.path b/src/main/deploy/pathplanner/paths/TrenchRight.path index 1f4bcce..7e4a6b1 100644 --- a/src/main/deploy/pathplanner/paths/TrenchRight.path +++ b/src/main/deploy/pathplanner/paths/TrenchRight.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.8449786019971466, - "y": 0.5221540656205423 + "x": 3.7026533523537797, + "y": 0.6256633380884444 }, "prevControl": null, "nextControl": { - "x": 6.329201141226819, - "y": 0.5092154065620549 + "x": 6.186875891583452, + "y": 0.612724679029957 }, "isLocked": false, "linkedName": null @@ -45,6 +45,10 @@ } ], "rotationTargets": [ + { + "waypointRelativePos": 0.06396588486140724, + "rotationDegrees": 0.0 + }, { "waypointRelativePos": 1.0, "rotationDegrees": 0.0 @@ -56,8 +60,8 @@ "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, "nominalVoltage": 12.0, "unlimited": false }, @@ -66,7 +70,7 @@ "rotation": -104.03624346792651 }, "reversed": false, - "folder": "Right", + "folder": "Neutral Right", "idealStartingState": { "velocity": 0, "rotation": -32.90524292298786 diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 8c12284..b586433 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,23 +1,25 @@ { - "robotWidth": 0.7, - "robotLength": 0.7112, + "robotWidth": 0.826, + "robotLength": 0.8255, "holonomicMode": true, "pathFolders": [ "Hang Left", - "Right" + "Neutral Left", + "Neutral Right", + "Alliance Center" ], "autoFolders": [], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, - "defaultMaxAngVel": 540.0, - "defaultMaxAngAccel": 720.0, + "defaultMaxAngVel": 360.0, + "defaultMaxAngAccel": 540.0, "defaultNominalVoltage": 12.0, "robotMass": 51.94, - "robotMOI": 8.0, + "robotMOI": 6.779055, "robotTrackwidth": 0.546, "driveWheelRadius": 0.048, "driveGearing": 6.122448979591837, - "maxDriveSpeed": 5.45, + "maxDriveSpeed": 5.04, "driveMotorType": "krakenX60", "driveCurrentLimit": 60.0, "wheelCOF": 1.2, @@ -32,6 +34,8 @@ "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [ - "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":-0.45,\"y\":0.0},\"size\":{\"width\":0.7,\"length\":0.3},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}" + "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":-0.57,\"y\":0.0},\"size\":{\"width\":0.8,\"length\":0.3},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}", + "{\"name\":\"Line\",\"type\":\"line\",\"data\":{\"start\":{\"x\":0.343,\"y\":0.0},\"end\":{\"x\":1.53,\"y\":2.77},\"strokeWidth\":0.02}}", + "{\"name\":\"Line\",\"type\":\"line\",\"data\":{\"start\":{\"x\":0.343,\"y\":0.0},\"end\":{\"x\":1.53,\"y\":-2.77},\"strokeWidth\":0.02}}" ] } \ No newline at end of file diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 6043d08..bc9cbd8 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Robot2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 176; - public static final String GIT_SHA = "0547c1a7b653d946d3068e238282f76452ee03d5"; - public static final String GIT_DATE = "2026-03-06 23:02:05 EST"; + public static final int GIT_REVISION = 177; + public static final String GIT_SHA = "78307cdb71e36e3aea91ffe4817f72e976082c12"; + public static final String GIT_DATE = "2026-03-06 23:59:50 EST"; public static final String GIT_BRANCH = "sotm"; - public static final String BUILD_DATE = "2026-03-06 23:23:02 EST"; - public static final long BUILD_UNIX_TIME = 1772857382884L; + public static final String BUILD_DATE = "2026-03-07 12:31:11 EST"; + public static final long BUILD_UNIX_TIME = 1772904671629L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/auto/AutoRoutines.java b/src/main/java/frc/robot/auto/AutoRoutines.java index 4747e96..e39c6d3 100644 --- a/src/main/java/frc/robot/auto/AutoRoutines.java +++ b/src/main/java/frc/robot/auto/AutoRoutines.java @@ -1,18 +1,25 @@ package frc.robot.auto; +import frc.robot.Constants; import frc.robot.auto.AutoSelector.Auto; import frc.robot.lib.trajectory.RedTrajectory; import frc.robot.lib.trajectory.TrajectoryLoader; import frc.robot.lib.trajectory.RedTrajectory.TrajectoryType; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.commands.AutopilotCommand; import frc.robot.subsystems.drive.commands.HeadingLockToHub2; import frc.robot.subsystems.drive.commands.PIDToPoseCommand; import frc.robot.subsystems.drive.commands.TrajectoryCommand; import frc.robot.subsystems.intake.Intake; + +import com.therekrab.autopilot.APTarget; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; public final class AutoRoutines { @@ -65,8 +72,8 @@ public static Command rightAutoNeutral() { var crossTrench = tTrenchRight.get(); var swipe = tSwipeRight.get(); var back = tReturnTrenchRight.get(); - return - Intake.getInstance().calibrateZero() + return Commands.print(Timer.getFPGATimestamp() + ": Time start") + .andThen(Intake.getInstance().calibrateZero()) .alongWith( new TrajectoryCommand(crossTrench)) .andThen( @@ -78,13 +85,35 @@ public static Command rightAutoNeutral() { .alongWith( new TrajectoryCommand(back))) .andThen( - Drive.getInstance().headingLockToHub()) - .alongWith( - Automation.shootAll() - .andThen( - Commands.waitSeconds(0.5)) - .andThen( - Automation.indexAll())); + Drive.getInstance().headingLockToHub() + .raceWith( + Automation.shootAll() + .andThen( + Commands.waitSeconds(0.5)) + .andThen( + Automation.indexAll()) + .andThen( + Commands.waitSeconds(3)) + .andThen( + Intake.getInstance().stow()) + .andThen( + Commands.waitSeconds(3)))) + // .andThen( + // new AutopilotCommand( + // new APTarget( + // new Pose2d(Constants.FieldConstants.Tower.rightUpright, Rotation2d.kCCW_90deg)) + // .withEntryAngle(Rotation2d.kZero))) + .andThen( + Commands.print(Timer.getFPGATimestamp() + ": Time end"), + Commands.idle()) + .finallyDo( + () -> { + CommandScheduler.getInstance().schedule( + Drive.getInstance().openLoopControl(), + Intake.getInstance().lower(), + Automation.stopShoot(), + Automation.stopIndex()); + }); } @Auto(name = "left neutral auto") @@ -122,10 +151,10 @@ public static Command leftAutoNeutral() { var crossTrench = tTrenchLeft.get(); var swipe = tSwipeLeft.get(); var back = tReturnTrenchLeft.get(); - return - Intake.getInstance().calibrateZero() - .alongWith( - new TrajectoryCommand(crossTrench)) + return Commands.print(Timer.getFPGATimestamp() + ": Time start") + .andThen(Intake.getInstance().calibrateZero()) + .alongWith( + new TrajectoryCommand(crossTrench)) .andThen( Intake.getInstance().intake()) .andThen( @@ -135,13 +164,126 @@ public static Command leftAutoNeutral() { .alongWith( new TrajectoryCommand(back))) .andThen( - Drive.getInstance().headingLockToHub()) - .alongWith( - Automation.shootAll() - .andThen( - Commands.waitSeconds(0.5)) - .andThen( - Automation.indexAll())); + Drive.getInstance().headingLockToHub() + .alongWith( + Automation.shootAll() + .andThen( + Commands.waitSeconds(0.5)) + .andThen( + Automation.indexAll() + .andThen( + Commands.waitSeconds(3)) + .andThen( + Intake.getInstance().stow())))) + .andThen( + Commands.print(Timer.getFPGATimestamp() + ": Time end"), + Commands.idle()) + .finallyDo( + () -> { + CommandScheduler.getInstance().schedule( + Drive.getInstance().openLoopControl(), + Intake.getInstance().lower(), + Automation.stopShoot(), + Automation.stopIndex()); + }); + } + + @Auto(name = "center auto") + public static Command centerAuto() { + var tDepotCenter = TrajectoryLoader.loadAutoTrajectory( + TrajectoryType.PATHPLANNER, + "DepotCenter"); + + if (tDepotCenter.isEmpty()) { + DriverStation.reportWarning( + "Something happened", true); + return Commands.none(); + } + + var tDepotShootCenter = TrajectoryLoader.loadAutoTrajectory( + TrajectoryType.PATHPLANNER, + "DepotShootCenter"); + + if (tDepotShootCenter.isEmpty()) { + DriverStation.reportWarning( + "Something happened", true); + return Commands.none(); + } + + var tStationCenter = TrajectoryLoader.loadAutoTrajectory( + TrajectoryType.PATHPLANNER, + "StationCenter"); + + if (tStationCenter.isEmpty()) { + DriverStation.reportWarning( + "Something happened", true); + return Commands.none(); + } + + var tStationShootCenter = TrajectoryLoader.loadAutoTrajectory( + TrajectoryType.PATHPLANNER, + "StationShootCenter"); + + if (tStationCenter.isEmpty()) { + DriverStation.reportWarning( + "Something happened", true); + return Commands.none(); + } + + var depotCenter = tDepotCenter.get(); + var depotShootCenter = tDepotShootCenter.get(); + var stationCenter = tStationCenter.get(); + var stationShootCenter = tStationShootCenter.get(); + + return Commands.print(Timer.getFPGATimestamp() + ": Time start") + .andThen(Intake.getInstance().calibrateZero()) + .andThen(Intake.getInstance().intake()) + .alongWith(new TrajectoryCommand(depotCenter)) + .andThen( + new TrajectoryCommand(depotShootCenter)) + .andThen( + Drive.getInstance().headingLockToHub() + .raceWith( + Automation.shootAll() + .andThen( + Commands.waitSeconds(0.5)) + .andThen( + Automation.indexAll()) + .andThen( + Commands.waitSeconds(3)))) + .andThen( + Intake.getInstance().lower(), + Automation.stopShoot(), + Automation.stopIndex()) + .andThen( + new TrajectoryCommand(stationCenter)) + .andThen( + Commands.waitSeconds(3)) + .andThen( + new TrajectoryCommand(stationShootCenter)) + .andThen( + Drive.getInstance().headingLockToHub() + .alongWith( + Automation.shootAll() + .andThen( + Commands.waitSeconds(0.5)) + .andThen( + Automation.indexAll()) + .andThen( + Commands.waitSeconds(3)) + .andThen( + Intake.getInstance().stow()))) + .andThen( + Commands.print(Timer.getFPGATimestamp() + ": Time end"), + Commands.idle()) + .finallyDo( + () -> { + CommandScheduler.getInstance().schedule( + Drive.getInstance().openLoopControl(), + Intake.getInstance().lower(), + Automation.stopShoot(), + Automation.stopIndex()); + }); } } diff --git a/src/main/java/frc/robot/auto/AutoSelector.java b/src/main/java/frc/robot/auto/AutoSelector.java index 5a0dec3..f2c2d16 100644 --- a/src/main/java/frc/robot/auto/AutoSelector.java +++ b/src/main/java/frc/robot/auto/AutoSelector.java @@ -43,7 +43,7 @@ public AutoSelector() { return (Command) auto.invoke(null); } catch (Exception e) { DriverStation.reportWarning( - "something really bad happened", true); + "something really bad happened " + e.getMessage(), true); return null; } }); @@ -53,7 +53,7 @@ public AutoSelector() { } } } - // chooser.addOption("right", () -> AutoRoutines.rightAutoNeutral()); + // chooser.addOption("right", () -> AutoRoutines.leftAutoNeutral()); chooser.setDefaultOption("None", () -> null); SmartDashboard.putData("Auto Selector", chooser); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 7989e67..83688fb 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -19,10 +19,10 @@ public final class DriveConstants { public static final double EPSILON_ROTATION = Units.Degrees.of(1.5).in(Units.Radians); // Maximums - public static final double MAX_SPEED = Units.MetersPerSecond.of(2.5).in(Units.MetersPerSecond); + public static final double MAX_SPEED = Units.MetersPerSecond.of(4.0).in(Units.MetersPerSecond); public static final double MAX_ACCEL = Units.MetersPerSecondPerSecond.of(9.0).in(Units.MetersPerSecondPerSecond); public static final double MAX_ROTATION_SPEED = - Units.RotationsPerSecond.of(1.0).in(Units.RadiansPerSecond); + Units.RotationsPerSecond.of(1.5).in(Units.RadiansPerSecond); public static final double MAX_ROTATION_ACCEL = Units.RotationsPerSecondPerSecond.of(4.0).in(Units.RadiansPerSecondPerSecond); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 33e6cd8..abcb168 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -74,8 +74,7 @@ private Intake() { BAR_POS_MAX, true, BAR_POSITION_UP, - 0.0, 0.0 - ); + 0.0, 0.0); barMotor.getSimState() .setRawRotorPosition(sim.getAngleRads() * (1 / Constants.TAU)); @@ -95,7 +94,7 @@ private Intake() { @Override public void periodic(){ wheelSpeed = wheelMotor.getVelocity().getValueAsDouble(); - barPosition = barMotor.getPosition().getValueAsDouble(); + barPosition = barMotor.getPosition().getValue().in(Degrees); barMotor.setControl(barRequest); wheelMotor.setControl(wheelRequest); io.updateInputs(wheelSpeed, barPosition, getCurrentCommand(), getDefaultCommand()); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index ceeac9a..d1b5ee2 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -23,7 +23,7 @@ public class IntakeConstants { public static final double BAR_POSITION_UP = Degrees.of(126).in(Rotations); public static final double BAR_GEAR_RATIO = 44.0 / 18.0 * 5.0 * 4.0; public static final double BAR_POS_MIN = 0.0; - public static final double BAR_POS_MAX = Degrees.of(126).in(Rotations); + public static final double BAR_POS_MAX = Degrees.of(127).in(Rotations); public static final double INTAKE_MASS = 3.656684786; // kg, ideally public static final double INTAKE_LENGTH = 0.1746631508; //m, hopefully diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index bc8b5a6..51ec01d 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -31,7 +31,7 @@ public class VisionConstants { public static enum VisionDeviceConstants { FR_CONSTANTS ( - "orangelight", //right camera + "right", //right camera new Transform3d( new Translation3d( Inches.of(13.124114), //wpi x-axis positive is forward direction @@ -39,11 +39,11 @@ public static enum VisionDeviceConstants { Inches.of(-9.527904), //wpi y-axis positive is strafe left, so right camera shall have negative offset Inches.of(14.365654)), // new Rotation3d(0, 26 * Constants.TAU / 360.0, -24 * Constants.TAU / 360.0)), //(roll: x, pitch: y, yaw: z) - new Rotation3d(0, 24 * Constants.TAU / 360.0, -24 * Constants.TAU / 360.0)), + new Rotation3d(0, 26 * Constants.TAU / 360.0, -24 * Constants.TAU / 360.0)), 1, 1280, 800), FL_CONSTANTS ( - "lemonlight", //left camera + "left", //left camera new Transform3d( new Translation3d( Inches.of(13.262586), //wpi x-axis positive is forward direction diff --git a/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java b/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java index 2d4d6d8..6034c31 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java @@ -64,18 +64,20 @@ public VisionDeviceManager() { @Override public void periodic() { - if (Robot.isSimulation()) { - visionSim.update(Drive.getInstance().getPose()); - } cameras.forEach(VisionDevice::periodic); movingAvgRead = headingAvg.getAverage(); - + io.updateInputs(getCurrentCommand(), getDefaultCommand()); io.process(); // SmartDashboard.putNumber("Vision heading moving avg", getMovingAvgRead()); // SmartDashboard.putBoolean("vision disabled", getVisionDisabled()); } - + + @Override + public void simulationPeriodic() { + visionSim.update(Drive.getInstance().getPose()); + } + public double getMovingAvgRead() { return movingAvgRead; } From 71f9fdd7738d2e49fb75fdf7096ec0bea2145358 Mon Sep 17 00:00:00 2001 From: Dibavi Date: Wed, 11 Mar 2026 18:32:57 -0700 Subject: [PATCH 110/118] added more paths --- .../pathplanner/autos/DepotCollect.auto | 25 +++++ .../deploy/pathplanner/autos/LeftCollect.auto | 31 +++++ .../pathplanner/autos/RightCollect.auto | 31 +++++ .../paths/NeutralReturnTrenchLeft.path | 102 +++++++++++++++++ .../paths/NeutralReturnTrenchRight.path | 106 ++++++++++++++++++ .../pathplanner/paths/ReturnTrenchRight.path | 4 +- .../paths/ShootLeftToTrenchLeft.path | 54 +++++++++ .../paths/ShootLeftToTrenchRight.path | 54 +++++++++ .../paths/ShootRightToTrenchLeft.path | 54 +++++++++ .../paths/ShootRightToTrenchRight.path | 54 +++++++++ .../deploy/pathplanner/paths/TrenchLeft.path | 6 +- .../deploy/pathplanner/paths/TrenchRight.path | 2 +- 12 files changed, 517 insertions(+), 6 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/DepotCollect.auto create mode 100644 src/main/deploy/pathplanner/autos/LeftCollect.auto create mode 100644 src/main/deploy/pathplanner/autos/RightCollect.auto create mode 100644 src/main/deploy/pathplanner/paths/NeutralReturnTrenchLeft.path create mode 100644 src/main/deploy/pathplanner/paths/NeutralReturnTrenchRight.path create mode 100644 src/main/deploy/pathplanner/paths/ShootLeftToTrenchLeft.path create mode 100644 src/main/deploy/pathplanner/paths/ShootLeftToTrenchRight.path create mode 100644 src/main/deploy/pathplanner/paths/ShootRightToTrenchLeft.path create mode 100644 src/main/deploy/pathplanner/paths/ShootRightToTrenchRight.path diff --git a/src/main/deploy/pathplanner/autos/DepotCollect.auto b/src/main/deploy/pathplanner/autos/DepotCollect.auto new file mode 100644 index 0000000..90723c6 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/DepotCollect.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "DepotCenter" + } + }, + { + "type": "path", + "data": { + "pathName": "DepotShootCenter" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/LeftCollect.auto b/src/main/deploy/pathplanner/autos/LeftCollect.auto new file mode 100644 index 0000000..dee60e2 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/LeftCollect.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TrenchLeft" + } + }, + { + "type": "path", + "data": { + "pathName": "SwipeLeft" + } + }, + { + "type": "path", + "data": { + "pathName": "ReturnTrenchLeft" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/RightCollect.auto b/src/main/deploy/pathplanner/autos/RightCollect.auto new file mode 100644 index 0000000..82e7fe3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/RightCollect.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TrenchRight" + } + }, + { + "type": "path", + "data": { + "pathName": "SwipeRight" + } + }, + { + "type": "path", + "data": { + "pathName": "ReturnTrenchRight" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/NeutralReturnTrenchLeft.path b/src/main/deploy/pathplanner/paths/NeutralReturnTrenchLeft.path new file mode 100644 index 0000000..4d1df3f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NeutralReturnTrenchLeft.path @@ -0,0 +1,102 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.1775421472937, + "y": 7.416322222222221 + }, + "prevControl": null, + "nextControl": { + "x": 5.1796913576946055, + "y": 7.314602720089652 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.3393111111111113, + "y": 7.416322222222221 + }, + "prevControl": { + "x": 3.55850891816058, + "y": 7.536540030323617 + }, + "nextControl": { + "x": 3.120113304061643, + "y": 7.296104414120825 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.382910128388017, + "y": 6.512753209700428 + }, + "prevControl": { + "x": 2.220629382465464, + "y": 7.249190195604299 + }, + "nextControl": { + "x": 2.4557888794833342, + "y": 6.182026311152107 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.154166666666667, + "y": 5.067911111111112 + }, + "prevControl": { + "x": 2.9444535373164356, + "y": 5.212507580753187 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "ReturnTrenchLeftEnd" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.1816976127320966, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.62, + "y": 4.039 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 2.2, + "maxWaypointRelativePos": 3.0, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -27.28121112254839 + }, + "reversed": false, + "folder": "Neutral Left", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/NeutralReturnTrenchRight.path b/src/main/deploy/pathplanner/paths/NeutralReturnTrenchRight.path new file mode 100644 index 0000000..c3856be --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NeutralReturnTrenchRight.path @@ -0,0 +1,106 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.931333333333333, + "y": 0.5269999999999992 + }, + "prevControl": null, + "nextControl": { + "x": 5.382924599438744, + "y": 0.5269999999999994 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.379186875891583, + "y": 0.5269999999999992 + }, + "prevControl": { + "x": 4.2590156918687585, + "y": 0.5221540656205423 + }, + "nextControl": { + "x": 3.129190667808615, + "y": 0.5283769328731714 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9522111269614832, + "y": 1.013823109843081 + }, + "prevControl": { + "x": 2.9723228063699474, + "y": 0.6953053819320217 + }, + "nextControl": { + "x": 2.9026908586211704, + "y": 1.7980979104181296 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.1931444444444446, + "y": 2.9825999999999997 + }, + "prevControl": { + "x": 2.971754978667799, + "y": 0.7916137458990562 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.0, + "rotationDegrees": 177.87659887200687 + }, + { + "waypointRelativePos": 2.0021321961620533, + "rotationDegrees": 179.39144110888557 + } + ], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.63, + "y": 4.03 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 2.41, + "maxWaypointRelativePos": 3.0, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 34.835830364154894 + }, + "reversed": false, + "folder": "Neutral Right", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReturnTrenchRight.path b/src/main/deploy/pathplanner/paths/ReturnTrenchRight.path index 89cf7a0..1eb63f1 100644 --- a/src/main/deploy/pathplanner/paths/ReturnTrenchRight.path +++ b/src/main/deploy/pathplanner/paths/ReturnTrenchRight.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 7.033137820589351, - "y": 1.984221330339962 + "y": 1.9842213303399618 }, "isLocked": false, "linkedName": "SwipeRightEnd" @@ -73,7 +73,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "ReturnTrenchRightEnd" } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/paths/ShootLeftToTrenchLeft.path b/src/main/deploy/pathplanner/paths/ShootLeftToTrenchLeft.path new file mode 100644 index 0000000..2a5e8b8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ShootLeftToTrenchLeft.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.154166666666667, + "y": 5.067911111111112 + }, + "prevControl": null, + "nextControl": { + "x": 3.5732665782842257, + "y": 5.966236275955364 + }, + "isLocked": false, + "linkedName": "ReturnTrenchLeftEnd" + }, + { + "anchor": { + "x": 3.622, + "y": 7.562488888888888 + }, + "prevControl": { + "x": 3.4931492650157048, + "y": 7.303448730289524 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "TrenchLeftStart" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 23.025492008528023 + }, + "reversed": false, + "folder": "Neutral Left", + "idealStartingState": { + "velocity": 0, + "rotation": -27.28121112254839 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ShootLeftToTrenchRight.path b/src/main/deploy/pathplanner/paths/ShootLeftToTrenchRight.path new file mode 100644 index 0000000..8c452fd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ShootLeftToTrenchRight.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.154166666666667, + "y": 5.067911111111112 + }, + "prevControl": null, + "nextControl": { + "x": 3.0790949423247556, + "y": 4.505803016858917 + }, + "isLocked": false, + "linkedName": "ReturnTrenchLeftEnd" + }, + { + "anchor": { + "x": 3.7026533523537797, + "y": 0.6256633380884444 + }, + "prevControl": { + "x": 2.7026533523537797, + "y": 0.6256633380884447 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "TrenchRightStart" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -32.90524292298786 + }, + "reversed": false, + "folder": "Neutral Right", + "idealStartingState": { + "velocity": 0, + "rotation": -27.28121112254839 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ShootRightToTrenchLeft.path b/src/main/deploy/pathplanner/paths/ShootRightToTrenchLeft.path new file mode 100644 index 0000000..90f284c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ShootRightToTrenchLeft.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.1931444444444446, + "y": 2.9825999999999997 + }, + "prevControl": null, + "nextControl": { + "x": 2.8215616681455185, + "y": 3.4837178349600704 + }, + "isLocked": false, + "linkedName": "ReturnTrenchRightEnd" + }, + { + "anchor": { + "x": 3.622, + "y": 7.562488888888888 + }, + "prevControl": { + "x": 3.24810115350488, + "y": 7.346716947648624 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "TrenchLeftStart" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 23.025492008528023 + }, + "reversed": false, + "folder": "Neutral Left", + "idealStartingState": { + "velocity": 0, + "rotation": 34.835830364154894 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ShootRightToTrenchRight.path b/src/main/deploy/pathplanner/paths/ShootRightToTrenchRight.path new file mode 100644 index 0000000..35a4f77 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ShootRightToTrenchRight.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.1931444444444446, + "y": 2.9825999999999997 + }, + "prevControl": null, + "nextControl": { + "x": 3.2320053238686772, + "y": 1.8821827861579403 + }, + "isLocked": false, + "linkedName": "ReturnTrenchRightEnd" + }, + { + "anchor": { + "x": 3.7026533523537797, + "y": 0.6256633380884444 + }, + "prevControl": { + "x": 3.030807453416149, + "y": 0.6106122448979577 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "TrenchRightStart" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -32.90524292298786 + }, + "reversed": false, + "folder": "Neutral Right", + "idealStartingState": { + "velocity": 0, + "rotation": 34.835830364154894 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TrenchLeft.path b/src/main/deploy/pathplanner/paths/TrenchLeft.path index a6d7534..69f13a3 100644 --- a/src/main/deploy/pathplanner/paths/TrenchLeft.path +++ b/src/main/deploy/pathplanner/paths/TrenchLeft.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.6219, + "x": 3.622, "y": 7.562488888888888 }, "prevControl": null, "nextControl": { - "x": 4.791381184057424, + "x": 4.791481184057424, "y": 7.511295674470193 }, "isLocked": false, - "linkedName": null + "linkedName": "TrenchLeftStart" }, { "anchor": { diff --git a/src/main/deploy/pathplanner/paths/TrenchRight.path b/src/main/deploy/pathplanner/paths/TrenchRight.path index 7e4a6b1..8359288 100644 --- a/src/main/deploy/pathplanner/paths/TrenchRight.path +++ b/src/main/deploy/pathplanner/paths/TrenchRight.path @@ -12,7 +12,7 @@ "y": 0.612724679029957 }, "isLocked": false, - "linkedName": null + "linkedName": "TrenchRightStart" }, { "anchor": { From 58944db230fff02478f960d510ae3ff78eda5ab2 Mon Sep 17 00:00:00 2001 From: Arvind Venkatesh Date: Sat, 14 Mar 2026 18:03:55 -0700 Subject: [PATCH 111/118] chore: cleanup --- src/main/java/frc/robot/Constants.java | 3 -- src/main/java/frc/robot/ControlsMapping.java | 29 +++--------- src/main/java/frc/robot/Robot.java | 44 +++++++++---------- .../java/frc/robot/auto/AutoRoutines.java | 5 --- .../frc/robot/lib/houndlib/BallPhysics.java | 1 - .../lib/houndlib/ShootOnTheFlyCalculator.java | 1 - .../frc/robot/subsystems/climb/Climb.java | 2 - .../frc/robot/subsystems/drive/Drive.java | 7 --- .../subsystems/drive/DriveConstants.java | 1 - .../frc/robot/subsystems/drive/DriveIO.java | 2 - .../subsystems/drive/ctre/CtreDrive.java | 1 - .../frc/robot/subsystems/intake/Intake.java | 2 - .../subsystems/roller/RollerConstants.java | 1 - .../frc/robot/subsystems/shooter/Shooter.java | 5 --- .../subsystems/shooter/ShooterConstants.java | 4 -- .../robot/subsystems/vision/VisionDevice.java | 2 + .../vision/VisionDeviceManager.java | 4 -- 17 files changed, 28 insertions(+), 86 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bb94335..e61b2af 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -26,9 +26,6 @@ public static final class Controllers { public static final double DRIVER_DEADBAND = 0.07; } - public static final class Odometry { - } - public static final class Pathplanner { public static RobotConfig config; static { diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index 899055c..9d99fb7 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -5,7 +5,6 @@ import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.geometry.*; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.ctre.CtreDrive.SysIdRoutineType; @@ -24,7 +23,6 @@ public class ControlsMapping { public static void mapTeleopCommand() { Drive.getInstance().setDefaultCommand((Drive.getInstance().openLoopControl())); - // Intake.getInstance().setDefaultCommand(Intake.getInstance().intake()); controller.back().and(controller.a()).onTrue(Drive.getInstance().resetPoseCommand(new Pose2d())); controller.back().and(controller.b()).onTrue(VisionDeviceManager.getInstance().bootUp()); @@ -55,37 +53,22 @@ public static void mapTeleopCommand() { // controller.a().whileTrue( // Intake.getInstance().agitate() // ).onFalse(Intake.getInstance().lower()); - + // controller.rightBumper().whileTrue( // Commands.parallel( // Shooter.getRightInstance().shoot( - // () -> SmartDashboard.getNumberArray("shootervel", new Double[] {30.0, 30.0})[0] - 15, - // () -> SmartDashboard.getNumberArray("shootervel", new Double[] {30.0, 30.0})[0] + 15 + // () -> SmartDashboard.getNumber("rightv", 30.0) - 15, + // () -> SmartDashboard.getNumber("rightv", 30.0) + 15 // ), // Shooter.getLeftInstance().shoot( - // () -> SmartDashboard.getNumberArray("shootervel", new Double[] {30.0, 30.0})[1] - 15, - // () -> SmartDashboard.getNumberArray("shootervel", new Double[] {30.0, 30.0})[1] + 15 + // () -> SmartDashboard.getNumber("leftv", 30.0) - 15, + // () -> SmartDashboard.getNumber("leftv", 30.0) + 15 // ) // ) // ).onFalse( // stopShoot() // ); - controller.rightBumper().whileTrue( - Commands.parallel( - Shooter.getRightInstance().shoot( - () -> SmartDashboard.getNumber("rightv", 30.0) - 15, - () -> SmartDashboard.getNumber("rightv", 30.0) + 15 - ), - Shooter.getLeftInstance().shoot( - () -> SmartDashboard.getNumber("leftv", 30.0) - 15, - () -> SmartDashboard.getNumber("leftv", 30.0) + 15 - ) - ) - ).onFalse( - stopShoot() - ); - } public static Command shootAll() { @@ -121,6 +104,7 @@ public static Command backIndex() { Roller.getInstance().antiRoll()); } + @SuppressWarnings("removal") public static void mapSysId() { // set up sysID routine type controller.a().onTrue(new InstantCommand( @@ -155,5 +139,4 @@ public static void mapSysId() { .finallyDo(interrupted -> Drive.getInstance().getCtreDrive() .setControl(new SwerveRequest.Idle())))); } - } \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 144d8dd..ca6d0e2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -54,8 +54,6 @@ public class Robot extends LoggedRobot { private static final CommandScheduler commandScheduler = CommandScheduler.getInstance(); private AutoSelector autoChooser; private Command autoCommand; - private static final String standardMap = "standard"; - private static final String mapTwo = "mapTwo"; private final SendableChooser mapChooser = new SendableChooser<>(); public static final CommandXboxController controller = @@ -73,10 +71,8 @@ public Robot() { //robot data loggers boolean usbPresent = new java.io.File("/u").exists(); if (usbPresent) { - // DataLogManager.start("/u/logs"); // USB stick System.out.println("Log/USB mounts OK"); } else { - // DataLogManager.start(); // falls back to /home/lvuser/logs System.out.println("Log/USB mounts NOT OK"); } @@ -85,6 +81,7 @@ public Robot() { Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + switch (BuildConstants.DIRTY) { case 0: Logger.recordMetadata("GitDirty", "All changes committed"); @@ -98,26 +95,20 @@ public Robot() { } if (RobotBase.isReal()) { - // Logger.addDataReceiver(new WPILOGWriter()); if (!DriverStation.isFMSAttached()) { Logger.addDataReceiver(new NT4Publisher()); } } else if (replay) { setUseTiming(false); String logPath = LogFileUtil.findReplayLog(); - // Logger.setReplaySource(new WPILOGReader(logPath)); - // Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); } else if (RobotBase.isSimulation()) { Logger.addDataReceiver(new NT4Publisher()); - // Logger.addDataReceiver(new WPILOGWriter()); } Logger.start(); if (!Logger.hasReplaySource()) { RobotController.setTimeSource(RobotController::getFPGATime); } - - VisionDeviceManager.getInstance(); Drive.getInstance(); Shooter.getLeftInstance(); @@ -125,28 +116,33 @@ public Robot() { Indexer.getLeftInstance(); Indexer.getRightInstance(); Roller.getInstance(); - // Intake.getInstance(); - // Climb.getInstance(); + Intake.getInstance(); + Climb.getInstance(); - SmartDashboard.putNumber("rightv", 30); - SmartDashboard.putNumber("leftv", 30); - - NamedCommands.registerCommand("shoot", Commands.parallel(Shooter.getLeftInstance().shoot(), Shooter.getRightInstance().shoot())); - NamedCommands.registerCommand("hang", Commands.parallel(Climb.getInstance().hangCommand())); - NamedCommands.registerCommand("stopShoot", Commands.parallel( - Shooter.getRightInstance().stop(), - Shooter.getLeftInstance().stop())); + VisionDeviceManager.getInstance(); + + /** TODO: 1 LED? */ + Led.getInstance(); + + /** TODO: Interpolate */ + // SmartDashboard.putNumber("rightv", 30); + // SmartDashboard.putNumber("leftv", 30); + + + /** TODO: Why this break robot? */ + // NamedCommands.registerCommand("shoot", Commands.parallel(Shooter.getLeftInstance().shoot(), Shooter.getRightInstance().shoot())); + // NamedCommands.registerCommand("hang", Commands.parallel(Climb.getInstance().hangCommand())); + // NamedCommands.registerCommand("stopShoot", Commands.parallel( + // Shooter.getRightInstance().stop(), + // Shooter.getLeftInstance().stop())); TelemetryManager.getInstance(); commandScheduler.schedule(FollowPathCommand.warmupCommand()); commandScheduler.schedule(VisionDeviceManager.getInstance().bootUp()); autoChooser = new AutoSelector(); - Led.getInstance(); - commandScheduler.schedule( - Led.getInstance().setRainbowCommand()); + commandScheduler.schedule(Led.getInstance().setRainbowCommand()); Drive.getInstance().getCtreDrive().setStateStdDevs(VisionConstants.STATE_STD_DEVS); - // DriverStation.startDataLog(DataLogManager.getLog()); } diff --git a/src/main/java/frc/robot/auto/AutoRoutines.java b/src/main/java/frc/robot/auto/AutoRoutines.java index e39c6d3..4f5a343 100644 --- a/src/main/java/frc/robot/auto/AutoRoutines.java +++ b/src/main/java/frc/robot/auto/AutoRoutines.java @@ -1,19 +1,14 @@ package frc.robot.auto; -import frc.robot.Constants; import frc.robot.auto.AutoSelector.Auto; import frc.robot.lib.trajectory.RedTrajectory; import frc.robot.lib.trajectory.TrajectoryLoader; import frc.robot.lib.trajectory.RedTrajectory.TrajectoryType; import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.drive.commands.AutopilotCommand; -import frc.robot.subsystems.drive.commands.HeadingLockToHub2; import frc.robot.subsystems.drive.commands.PIDToPoseCommand; import frc.robot.subsystems.drive.commands.TrajectoryCommand; import frc.robot.subsystems.intake.Intake; -import com.therekrab.autopilot.APTarget; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; diff --git a/src/main/java/frc/robot/lib/houndlib/BallPhysics.java b/src/main/java/frc/robot/lib/houndlib/BallPhysics.java index dce904e..d29d843 100644 --- a/src/main/java/frc/robot/lib/houndlib/BallPhysics.java +++ b/src/main/java/frc/robot/lib/houndlib/BallPhysics.java @@ -4,7 +4,6 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; public final class BallPhysics { public static final double GRAVITY = 9.81; diff --git a/src/main/java/frc/robot/lib/houndlib/ShootOnTheFlyCalculator.java b/src/main/java/frc/robot/lib/houndlib/ShootOnTheFlyCalculator.java index 6afa352..739668d 100644 --- a/src/main/java/frc/robot/lib/houndlib/ShootOnTheFlyCalculator.java +++ b/src/main/java/frc/robot/lib/houndlib/ShootOnTheFlyCalculator.java @@ -10,7 +10,6 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.interpolation.InterpolatingTreeMap; import edu.wpi.first.math.kinematics.ChassisSpeeds; diff --git a/src/main/java/frc/robot/subsystems/climb/Climb.java b/src/main/java/frc/robot/subsystems/climb/Climb.java index a52e279..5c428a6 100644 --- a/src/main/java/frc/robot/subsystems/climb/Climb.java +++ b/src/main/java/frc/robot/subsystems/climb/Climb.java @@ -12,14 +12,12 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.Robot; -import frc.robot.subsystems.TelemetryManager; import frc.robot.subsystems.climb.ClimbConstants.Setpoint; import edu.wpi.first.wpilibj.simulation.ElevatorSim; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index fd7c2b3..4ea51c1 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -2,7 +2,6 @@ import static frc.robot.subsystems.drive.DriveConstants.*; -import java.lang.reflect.Field; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -17,10 +16,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -28,8 +24,6 @@ import edu.wpi.first.units.BaseUnits; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Time; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -40,7 +34,6 @@ import frc.robot.lib.control.ControlConstants.ProfiledPIDVConstants; import frc.robot.lib.control.ProfiledPIDVController; import frc.robot.lib.field.FieldLayout; -import frc.robot.lib.field.FieldUtil; import frc.robot.lib.trajectory.LocalADStarWrapper; import frc.robot.lib.util.Util; import frc.robot.subsystems.TelemetryManager; diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 83688fb..d00b8e3 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -3,7 +3,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.AngularVelocity; diff --git a/src/main/java/frc/robot/subsystems/drive/DriveIO.java b/src/main/java/frc/robot/subsystems/drive/DriveIO.java index 745d824..bf27287 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveIO.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveIO.java @@ -122,12 +122,10 @@ public void process() { private final String name; private final ModuleIO[] moduleIOs; private final GyroIO gyroIO; - private final CtreDrive drivetrain; private final DriveIOInputsAutoLogged inputs; public DriveIO(String name, CtreDrive drivetrain) { this.name = name; - this.drivetrain = drivetrain; inputs = new DriveIOInputsAutoLogged(); //FrontLeft, FrontRight, BackLeft, BackRight diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java index 7b805fd..7e8ba0d 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDrive.java @@ -4,7 +4,6 @@ import java.util.function.Supplier; -import com.ctre.phoenix6.SignalLogger; import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index abcb168..7d2d6c8 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -5,10 +5,8 @@ import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.ControlRequest; -import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.NeutralOut; -import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; diff --git a/src/main/java/frc/robot/subsystems/roller/RollerConstants.java b/src/main/java/frc/robot/subsystems/roller/RollerConstants.java index 5f0a7f3..984457e 100644 --- a/src/main/java/frc/robot/subsystems/roller/RollerConstants.java +++ b/src/main/java/frc/robot/subsystems/roller/RollerConstants.java @@ -4,7 +4,6 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.configs.TorqueCurrentConfigs; import com.ctre.phoenix6.configs.VoltageConfigs; import com.ctre.phoenix6.signals.InvertedValue; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 491a667..d480a32 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -4,14 +4,11 @@ import java.util.function.DoubleSupplier; -import org.littletonrobotics.junction.Logger; - import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.NeutralOut; -import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -20,7 +17,6 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N2; -import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.simulation.BatterySim; @@ -31,7 +27,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Robot; -import frc.robot.lib.util.Util; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveConstants.FieldPoses; import frc.robot.subsystems.shooter.ShooterConstants.Motors; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 8101990..e4a8317 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -6,11 +6,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.VoltageConfigs; -import edu.wpi.first.math.InterpolatingMatrixTreeMap; import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N2; public final class ShooterConstants { public static final double GEAR_RATIO = 1; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionDevice.java b/src/main/java/frc/robot/subsystems/vision/VisionDevice.java index a269579..cd83589 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionDevice.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionDevice.java @@ -74,6 +74,7 @@ public VisionDevice(VisionDeviceConstants constants) { hasTarget = false; } + @SuppressWarnings("removal") private void processFrames() { // var results = camera.getAllUnreadResults(); // for (var result : results) { @@ -132,6 +133,7 @@ private void processFrames() { // } } + @SuppressWarnings("removal") private void processFramesRigged(Matrix riggedness) { var result = camera.getLatestResult(); if (result.hasTargets()) { diff --git a/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java b/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java index 6034c31..fd384be 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionDeviceManager.java @@ -1,15 +1,11 @@ package frc.robot.subsystems.vision; -import static frc.robot.subsystems.vision.VisionConstants.*; - import frc.robot.lib.util.TunableNumber; -import frc.robot.subsystems.TelemetryManager; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.vision.VisionConstants.VisionDeviceConstants; import frc.robot.Robot; import frc.robot.lib.field.FieldLayout; import frc.robot.lib.util.MovingAverageDouble; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; From 4661338ae4e12d010d1a73792395fd56053fb4e8 Mon Sep 17 00:00:00 2001 From: Arvind Venkatesh Date: Sun, 15 Mar 2026 11:39:36 -0700 Subject: [PATCH 112/118] feat: limits + memory saving disable advantagekit and add current limits and memory profiling plus heap dump on OOM errors --- build.gradle | 2 ++ src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/Robot.java | 2 +- .../drive/ctre/CompCtreDriveConstants.java | 12 +++++++++--- .../subsystems/drive/ctre/CtreDriveConstants.java | 9 +++++++-- .../subsystems/drive/ctre/CtreDriveConstants2.java | 10 ++++++++-- .../java/frc/robot/subsystems/shooter/Shooter.java | 13 ------------- 7 files changed, 32 insertions(+), 26 deletions(-) diff --git a/build.gradle b/build.gradle index 3b981de..f91cefd 100644 --- a/build.gradle +++ b/build.gradle @@ -45,6 +45,8 @@ deploy { jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") jvmArgs.add("-Djava.rmi.server.hostname=10.14.58.2") // Replace TE.AM with team number + jvmArgs.add("-XX:+HeapDumpOnOutOfMemoryError") + jvmArgs.add("-XX:HeapDumpPath=/home/lvuser/dumps/") } // Static files artifact diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index bc9cbd8..e4083a0 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Robot2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 177; - public static final String GIT_SHA = "78307cdb71e36e3aea91ffe4817f72e976082c12"; - public static final String GIT_DATE = "2026-03-06 23:59:50 EST"; + public static final int GIT_REVISION = 180; + public static final String GIT_SHA = "58944db230fff02478f960d510ae3ff78eda5ab2"; + public static final String GIT_DATE = "2026-03-14 21:03:55 EDT"; public static final String GIT_BRANCH = "sotm"; - public static final String BUILD_DATE = "2026-03-07 12:31:11 EST"; - public static final long BUILD_UNIX_TIME = 1772904671629L; + public static final String BUILD_DATE = "2026-03-15 14:19:22 EDT"; + public static final long BUILD_UNIX_TIME = 1773598762502L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ca6d0e2..92b9047 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -105,7 +105,7 @@ public Robot() { Logger.addDataReceiver(new NT4Publisher()); } - Logger.start(); + // Logger.start(); if (!Logger.hasReplaySource()) { RobotController.setTimeSource(RobotController::getFPGATime); } diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CompCtreDriveConstants.java b/src/main/java/frc/robot/subsystems/drive/ctre/CompCtreDriveConstants.java index 8d07ba4..e571544 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CompCtreDriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CompCtreDriveConstants.java @@ -42,15 +42,21 @@ public class CompCtreDriveConstants { // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - public static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - public static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true) + ); + private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() // Swerve azimuth does not require much torque output, so we can set a relatively low // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimit(Amps.of(40)) .withStatorCurrentLimitEnable(true) ); + public static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs public static final Pigeon2Configuration pigeonConfigs = null; diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java index 14c9439..d5c108a 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants.java @@ -91,13 +91,18 @@ public class CtreDriveConstants { // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true) + ); private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() // Swerve azimuth does not require much torque output, so we can set a relatively low // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimit(Amps.of(40)) .withStatorCurrentLimitEnable(true) ); private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants2.java b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants2.java index b464e8d..b369780 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants2.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CtreDriveConstants2.java @@ -54,15 +54,21 @@ public class CtreDriveConstants2 { // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true) + ); private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() // Swerve azimuth does not require much torque output, so we can set a relatively low // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimit(Amps.of(40)) .withStatorCurrentLimitEnable(true) ); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs private static final Pigeon2Configuration pigeonConfigs = null; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index d480a32..55b9bda 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -138,19 +138,6 @@ public void periodic() { io.updateInputs(lastReadSpeedTop, lastReadSpeedBottom, getCurrentCommand(), getDefaultCommand()); io.process(); - - SmartDashboard.putNumber("toShooter", Drive.getInstance().getPose().getTranslation() - .getDistance( - Constants.FieldConstants.allianceCorrected( - FieldPoses.HUB.pose3d.getTranslation() - ).toTranslation2d())); - SmartDashboard.putNumber("ShooterVel", - distance_to_shooter.get( - Drive.getInstance().getPose().getTranslation() - .getDistance( - Constants.FieldConstants.allianceCorrected( - FieldPoses.HUB.pose3d.getTranslation() - ).toTranslation2d())).get(0, 0)); } public double getTopSpeed() { From 237cfa1c94554327f075771a5cf853cc676ed696 Mon Sep 17 00:00:00 2001 From: Arvind Venkatesh Date: Sun, 15 Mar 2026 12:51:04 -0700 Subject: [PATCH 113/118] feat: sysid --- src/main/java/frc/robot/BuildConstants.java | 10 +- src/main/java/frc/robot/ControlsMapping.java | 21 ++- src/main/java/frc/robot/Robot.java | 2 +- .../frc/robot/subsystems/shooter/Shooter.java | 156 ++++++++++-------- 4 files changed, 106 insertions(+), 83 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index e4083a0..24f8614 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Robot2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 180; - public static final String GIT_SHA = "58944db230fff02478f960d510ae3ff78eda5ab2"; - public static final String GIT_DATE = "2026-03-14 21:03:55 EDT"; + public static final int GIT_REVISION = 181; + public static final String GIT_SHA = "4661338ae4e12d010d1a73792395fd56053fb4e8"; + public static final String GIT_DATE = "2026-03-15 14:39:36 EDT"; public static final String GIT_BRANCH = "sotm"; - public static final String BUILD_DATE = "2026-03-15 14:19:22 EDT"; - public static final long BUILD_UNIX_TIME = 1773598762502L; + public static final String BUILD_DATE = "2026-03-15 15:43:08 EDT"; + public static final long BUILD_UNIX_TIME = 1773603788356L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index 9d99fb7..f78dfae 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -5,6 +5,7 @@ import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.geometry.*; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.ctre.CtreDrive.SysIdRoutineType; @@ -38,18 +39,22 @@ public static void mapTeleopCommand() { .whileTrue(indexAll()) .onFalse(stopIndex()); - controller.leftTrigger() - .whileTrue(Intake.getInstance().intake()) - .onFalse(Intake.getInstance().stopWheel()); + // controller.leftTrigger() + // .whileTrue(Intake.getInstance().intake()) + // .onFalse(Intake.getInstance().stopWheel()); - controller.y().and(controller.back().negate()) - .whileTrue(Intake.getInstance().stow()) - .onFalse(Intake.getInstance().lower()); + // controller.y().and(controller.back().negate()) + // .whileTrue(Intake.getInstance().stow()) + // .onFalse(Intake.getInstance().lower()); controller.rightTrigger().whileTrue(Drive.getInstance().headingLockToHub()); - controller.povDown().onTrue(Intake.getInstance().calibrateZero()); - controller.b().and(controller.back().negate()).whileTrue(backIndex()).onFalse(stopIndex()); + // controller.povDown().onTrue(Intake.getInstance().calibrateZero()); + // controller.b().and(controller.back().negate()).whileTrue(backIndex()).onFalse(stopIndex()); + controller.y().and(controller.back().negate()).whileTrue(Shooter.getLeftInstance().sysId().dynamic(SysIdRoutine.Direction.kForward)); + controller.x().and(controller.back().negate()).whileTrue(Shooter.getLeftInstance().sysId().dynamic(SysIdRoutine.Direction.kReverse)); + controller.b().and(controller.back().negate()).whileTrue(Shooter.getLeftInstance().sysId().quasistatic(SysIdRoutine.Direction.kForward)); + controller.a().and(controller.back().negate()).whileTrue(Shooter.getLeftInstance().sysId().quasistatic(SysIdRoutine.Direction.kReverse)); // controller.a().whileTrue( // Intake.getInstance().agitate() // ).onFalse(Intake.getInstance().lower()); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 92b9047..ca6d0e2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -105,7 +105,7 @@ public Robot() { Logger.addDataReceiver(new NT4Publisher()); } - // Logger.start(); + Logger.start(); if (!Logger.hasReplaySource()) { RobotController.setTimeSource(RobotController::getFPGATime); } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 55b9bda..06c310e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,13 +1,17 @@ package frc.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.Volts; import static frc.robot.subsystems.shooter.ShooterConstants.*; import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; + import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; @@ -19,12 +23,15 @@ import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.simulation.BatterySim; import edu.wpi.first.wpilibj.simulation.FlywheelSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.Constants; import frc.robot.Robot; import frc.robot.subsystems.drive.Drive; @@ -34,25 +41,27 @@ public class Shooter extends SubsystemBase { private static Shooter shooterLeftInstance; private static Shooter shooterRightInstance; - public static Shooter getLeftInstance() { - if (shooterLeftInstance == null) { - shooterLeftInstance = new Shooter(true); - } - return shooterLeftInstance; - } - - public static Shooter getRightInstance() { - if (shooterRightInstance == null) { - shooterRightInstance = new Shooter(false); - } - return shooterRightInstance; - } + + public static Shooter getLeftInstance() { + if (shooterLeftInstance == null) { + shooterLeftInstance = new Shooter(true); + } + return shooterLeftInstance; + } + + public static Shooter getRightInstance() { + if (shooterRightInstance == null) { + shooterRightInstance = new Shooter(false); + } + return shooterRightInstance; + } + private final TalonFX topMotor; private final TalonFX bottomMotor; - private double lastReadSpeedTop; + private double lastReadSpeedTop; private double lastReadSpeedBottom; - private ControlRequest topRequest = new NeutralOut(); - private ControlRequest bottomRequest = new NeutralOut(); + private ControlRequest topRequest = new NeutralOut(); + private ControlRequest bottomRequest = new NeutralOut(); private FlywheelSim topSim; private FlywheelSim bottomSim; @@ -74,8 +83,7 @@ private Shooter(boolean left) { if (left) { bottomID = Motors.BOTTOMLEFT.id; topID = Motors.TOPLEFT.id; - } - else { + } else { bottomID = Motors.BOTTOMRIGHT.id; topID = Motors.TOPRIGHT.id; } @@ -84,36 +92,36 @@ private Shooter(boolean left) { if (!left) { config = config.clone().withMotorOutput( - new MotorOutputConfigs() - .withInverted(InvertedValue.Clockwise_Positive)); + new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive)); } - bottomMotor = new TalonFX(bottomID); - bottomMotor.getConfigurator().apply(config); - bottomMotor.setNeutralMode(NeutralModeValue.Coast); + bottomMotor = new TalonFX(bottomID); + bottomMotor.getConfigurator().apply(config); + bottomMotor.setNeutralMode(NeutralModeValue.Coast); topMotor = new TalonFX(topID); - topMotor.getConfigurator().apply(config); - topMotor.setNeutralMode(NeutralModeValue.Coast); - + topMotor.getConfigurator().apply(config); + topMotor.setNeutralMode(NeutralModeValue.Coast); + if (Robot.isSimulation()) { topSim = new FlywheelSim( - LinearSystemId.createFlywheelSystem( - DCMotor.getKrakenX60(1), - 0.000489000861, - 1 - ), DCMotor.getKrakenX60(1), 0.0); + LinearSystemId.createFlywheelSystem( + DCMotor.getKrakenX60(1), + 0.000489000861, + 1), + DCMotor.getKrakenX60(1), 0.0); bottomSim = new FlywheelSim( - LinearSystemId.createFlywheelSystem( - DCMotor.getKrakenX60(1), - 0.000489000861, - 1 - ), DCMotor.getKrakenX60(1), 0.0); + LinearSystemId.createFlywheelSystem( + DCMotor.getKrakenX60(1), + 0.000489000861, + 1), + DCMotor.getKrakenX60(1), 0.0); } io = new ShooterIO(getName(), topMotor, bottomMotor); - distance_to_shooter.put(0.0, VecBuilder.fill(0,0)); + distance_to_shooter.put(0.0, VecBuilder.fill(0, 0)); distance_to_shooter.put(0.641, VecBuilder.fill(25.0, 25.0)); distance_to_shooter.put(1.06, VecBuilder.fill(31.25, 31.25)); distance_to_shooter.put(1.56, VecBuilder.fill(34.375, 34.375)); @@ -132,9 +140,9 @@ public void periodic() { bottomMotor.setControl(bottomRequest); // SmartDashboard.putNumber( - // "ShooterV", - // distance_to_shooter.get( - // SmartDashboard.getNumber("toShooter", 1.5)).get(0, 0)); + // "ShooterV", + // distance_to_shooter.get( + // SmartDashboard.getNumber("toShooter", 1.5)).get(0, 0)); io.updateInputs(lastReadSpeedTop, lastReadSpeedBottom, getCurrentCommand(), getDefaultCommand()); io.process(); @@ -151,18 +159,18 @@ public void simulationPeriodic() { topSim.setInput(topMotor.getSimState().getMotorVoltage()); topSim.update(0.020); - topMotor.getSimState() - .setRotorVelocity(topSim.getAngularVelocityRPM() / 60.0); + topMotor.getSimState() + .setRotorVelocity(topSim.getAngularVelocityRPM() / 60.0); topMotor.getSimState().addRotorPosition(topSim.getAngularVelocityRPM() / 60.0 * 0.020); RoboRioSim.setVInVoltage( - BatterySim.calculateDefaultBatteryLoadedVoltage(topSim.getCurrentDrawAmps())); - + BatterySim.calculateDefaultBatteryLoadedVoltage(topSim.getCurrentDrawAmps())); + bottomSim.setInput(bottomMotor.getSimState().getMotorVoltage()); bottomSim.update(0.020); RoboRioSim.setVInVoltage( - BatterySim.calculateDefaultBatteryLoadedVoltage(bottomSim.getCurrentDrawAmps())); - bottomMotor.getSimState() - .setRotorVelocity(topSim.getAngularVelocityRPM() / 60.0); + BatterySim.calculateDefaultBatteryLoadedVoltage(bottomSim.getCurrentDrawAmps())); + bottomMotor.getSimState() + .setRotorVelocity(topSim.getAngularVelocityRPM() / 60.0); bottomMotor.getSimState().addRotorPosition(topSim.getAngularVelocityRPM() / 60.0 * 0.020); } @@ -170,8 +178,8 @@ public void simulationPeriodic() { /** Replaces the request */ private void setTopRequest(ControlRequest request) { this.topRequest = request; - } - + } + /** Replaces the request */ private void setBottomRequest(ControlRequest request) { this.bottomRequest = request; @@ -180,11 +188,10 @@ private void setBottomRequest(ControlRequest request) { /** Stops the shooter */ public Command stop() { return runOnce( - () -> { - setTopRequest(new CoastOut()); - setBottomRequest(new CoastOut()); - } - ).withName("Stopped"); + () -> { + setTopRequest(new CoastOut()); + setBottomRequest(new CoastOut()); + }).withName("Stopped"); } public Command shoot(double topSpeed, double bottomSpeed) { @@ -196,10 +203,10 @@ public Command shoot(double topSpeed, double bottomSpeed) { public Command shoot(DoubleSupplier distance) { return shoot( - distance_to_shooter.get(distance.getAsDouble()) - .get(0, 0) - 15, - distance_to_shooter.get(distance.getAsDouble()) - .get(1, 0) + 15); + distance_to_shooter.get(distance.getAsDouble()) + .get(0, 0) - 15, + distance_to_shooter.get(distance.getAsDouble()) + .get(1, 0) + 15); } public Command shoot(DoubleSupplier topSpeed, DoubleSupplier bottomSpeed) { @@ -209,20 +216,31 @@ public Command shoot(DoubleSupplier topSpeed, DoubleSupplier bottomSpeed) { setTopRequest(topReq); setBottomRequest(bottomReq); }).andThen( - run(() -> { - topReq.withVelocity(topSpeed.getAsDouble()); - bottomReq.withVelocity(bottomSpeed.getAsDouble()); - }) - ).withName("Shooting"); + run(() -> { + topReq.withVelocity(topSpeed.getAsDouble()); + bottomReq.withVelocity(bottomSpeed.getAsDouble()); + })).withName("Shooting"); } - public Command shoot() { - return shoot(() -> - Drive.getInstance().getPose().getTranslation() + return shoot(() -> Drive.getInstance().getPose().getTranslation() .getDistance( - Constants.FieldConstants.allianceCorrected( - FieldPoses.HUB.pose3d.getTranslation() - ).toTranslation2d())); + Constants.FieldConstants.allianceCorrected( + FieldPoses.HUB.pose3d.getTranslation()).toTranslation2d())); + } + + public Command runVolts(double voltage) { + return runOnce(() -> setTopRequest(new VoltageOut(voltage))); + } + + public SysIdRoutine sysId() { + return new SysIdRoutine( + new SysIdRoutine.Config( + null, null, null, // Use default config + (state) -> Logger.recordOutput("SysIdTestState", state.toString())), + new SysIdRoutine.Mechanism( + (voltage) -> runVolts(voltage.in(Volts)), + null, // No log consumer, since data is recorded by AdvantageKit + this)); } } \ No newline at end of file From 521f4fd4a7783e9a45f4c8d356e09f15070d7cb4 Mon Sep 17 00:00:00 2001 From: github-0-0 Date: Sun, 15 Mar 2026 15:01:08 -0700 Subject: [PATCH 114/118] current limits and tuning --- src/main/java/frc/robot/BuildConstants.java | 10 ++--- src/main/java/frc/robot/ControlsMapping.java | 11 +++--- src/main/java/frc/robot/Robot.java | 1 + .../subsystems/climb/ClimbConstants.java | 6 ++- .../drive/ctre/CompCtreDriveConstants.java | 6 ++- .../subsystems/indexer/IndexerConstants.java | 5 ++- .../subsystems/intake/IntakeConstants.java | 10 ++++- .../robot/subsystems/led/LedConstants.java | 2 +- .../subsystems/roller/RollerConstants.java | 5 ++- .../frc/robot/subsystems/shooter/Shooter.java | 31 +++++++++------ .../subsystems/shooter/ShooterConstants.java | 39 +++++++++++++++---- 11 files changed, 90 insertions(+), 36 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 24f8614..627a997 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Robot2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 181; - public static final String GIT_SHA = "4661338ae4e12d010d1a73792395fd56053fb4e8"; - public static final String GIT_DATE = "2026-03-15 14:39:36 EDT"; + public static final int GIT_REVISION = 182; + public static final String GIT_SHA = "237cfa1c94554327f075771a5cf853cc676ed696"; + public static final String GIT_DATE = "2026-03-15 15:51:04 EDT"; public static final String GIT_BRANCH = "sotm"; - public static final String BUILD_DATE = "2026-03-15 15:43:08 EDT"; - public static final long BUILD_UNIX_TIME = 1773603788356L; + public static final String BUILD_DATE = "2026-03-15 17:58:44 EDT"; + public static final long BUILD_UNIX_TIME = 1773611924443L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index f78dfae..b7f2a05 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -30,8 +30,9 @@ public static void mapTeleopCommand() { controller.back().and(controller.y()).onTrue( Commands.runOnce(() -> Drive.getInstance().getCtreDrive().getPigeon2().reset())); - controller.x().whileTrue(Intake.getInstance().outtake()); + // controller.x().whileTrue(Intake.getInstance().outtake()); + controller.rightBumper() .whileTrue(shootAll()) .onFalse(stopShoot()); @@ -51,10 +52,10 @@ public static void mapTeleopCommand() { // controller.povDown().onTrue(Intake.getInstance().calibrateZero()); // controller.b().and(controller.back().negate()).whileTrue(backIndex()).onFalse(stopIndex()); - controller.y().and(controller.back().negate()).whileTrue(Shooter.getLeftInstance().sysId().dynamic(SysIdRoutine.Direction.kForward)); - controller.x().and(controller.back().negate()).whileTrue(Shooter.getLeftInstance().sysId().dynamic(SysIdRoutine.Direction.kReverse)); - controller.b().and(controller.back().negate()).whileTrue(Shooter.getLeftInstance().sysId().quasistatic(SysIdRoutine.Direction.kForward)); - controller.a().and(controller.back().negate()).whileTrue(Shooter.getLeftInstance().sysId().quasistatic(SysIdRoutine.Direction.kReverse)); + controller.y().and(controller.back().negate()).whileTrue(Shooter.getRightInstance().sysId().dynamic(SysIdRoutine.Direction.kForward)); + controller.x().and(controller.back().negate()).whileTrue(Shooter.getRightInstance().sysId().dynamic(SysIdRoutine.Direction.kReverse)); + controller.b().and(controller.back().negate()).whileTrue(Shooter.getRightInstance().sysId().quasistatic(SysIdRoutine.Direction.kForward)); + controller.a().and(controller.back().negate()).whileTrue(Shooter.getRightInstance().sysId().quasistatic(SysIdRoutine.Direction.kReverse)); // controller.a().whileTrue( // Intake.getInstance().agitate() // ).onFalse(Intake.getInstance().lower()); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ca6d0e2..4f95acf 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -105,6 +105,7 @@ public Robot() { Logger.addDataReceiver(new NT4Publisher()); } + Logger.addDataReceiver(new WPILOGWriter("/home/lvuser/logs")); Logger.start(); if (!Logger.hasReplaySource()) { RobotController.setTimeSource(RobotController::getFPGATime); diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java b/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java index 35c85dc..79a60b7 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java @@ -26,7 +26,7 @@ public class ClimbConstants { public static final double MAX_ACCEL = 1.5; public static final double MAX_SPEED = 1.0; // m/s - public static final int CLIMB_MOTOR_ID = 25; // change this + public static final int CLIMB_MOTOR_ID = 35; // change this public static enum Setpoint { BASE(0.003), // small offset to prevent stalling (allegedly) @@ -52,7 +52,9 @@ public static TalonFXConfiguration getConfig() { .withMotionMagicJerk(320)) .withCurrentLimits(new CurrentLimitsConfigs() .withStatorCurrentLimit(60) - .withSupplyCurrentLimit(60)) + .withSupplyCurrentLimit(40) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimitEnable(true)) .withVoltage(new VoltageConfigs() .withPeakForwardVoltage(12.0) .withPeakReverseVoltage(-12.0)) diff --git a/src/main/java/frc/robot/subsystems/drive/ctre/CompCtreDriveConstants.java b/src/main/java/frc/robot/subsystems/drive/ctre/CompCtreDriveConstants.java index e571544..e4f3d89 100644 --- a/src/main/java/frc/robot/subsystems/drive/ctre/CompCtreDriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/ctre/CompCtreDriveConstants.java @@ -46,7 +46,9 @@ public class CompCtreDriveConstants { .withCurrentLimits( new CurrentLimitsConfigs() .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(40) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimitEnable(true) ); private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() .withCurrentLimits( @@ -54,7 +56,9 @@ public class CompCtreDriveConstants { // Swerve azimuth does not require much torque output, so we can set a relatively low // stator current limit to help avoid brownouts without impacting performance. .withStatorCurrentLimit(Amps.of(40)) + .withSupplyCurrentLimit(40) .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimitEnable(true) ); public static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java index 7d74bcb..f321a88 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java @@ -29,7 +29,10 @@ public static TalonFXConfiguration getConfig() { .withKS(0.0) .withKV(0.0)) // placeholder values .withCurrentLimits(new CurrentLimitsConfigs() - .withStatorCurrentLimit(80) + .withStatorCurrentLimit(60) + .withSupplyCurrentLimit(40) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimitEnable(true) // .withSupplyCurrentLimit(120) ) .withVoltage(new VoltageConfigs() diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index d1b5ee2..bbcba46 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -44,7 +44,10 @@ public static TalonFXConfiguration getWheelConfig() { .withKI(0.0) .withKD(0.0)) .withCurrentLimits(new CurrentLimitsConfigs() - .withStatorCurrentLimit(78)) + .withStatorCurrentLimit(60) + .withSupplyCurrentLimit(40) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimitEnable(true)) .withVoltage(new VoltageConfigs() .withPeakForwardVoltage(12.0) .withPeakReverseVoltage(-12.0)) @@ -61,7 +64,10 @@ public static TalonFXConfiguration getBarConfig() { .withKD(0.0) .withKG(0.0).withGravityType(GravityTypeValue.Arm_Cosine)) .withCurrentLimits(new CurrentLimitsConfigs() - .withStatorCurrentLimit(60)) + .withStatorCurrentLimit(60) + .withSupplyCurrentLimit(40) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimitEnable(true)) .withMotionMagic(new MotionMagicConfigs() .withMotionMagicAcceleration(10) // 1.0 m/s^2 .withMotionMagicCruiseVelocity(10) diff --git a/src/main/java/frc/robot/subsystems/led/LedConstants.java b/src/main/java/frc/robot/subsystems/led/LedConstants.java index e9793ac..37fd9a8 100644 --- a/src/main/java/frc/robot/subsystems/led/LedConstants.java +++ b/src/main/java/frc/robot/subsystems/led/LedConstants.java @@ -4,6 +4,6 @@ public class LedConstants { public static final int LED_START = 0; public static final int LED_LENGTH = 19; public static final int LED_LENGTH_2 = 17; - public static final int LED_PORT = 0; + public static final int LED_PORT = 7; public static final double UPDATE_DT = 0.06; } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/roller/RollerConstants.java b/src/main/java/frc/robot/subsystems/roller/RollerConstants.java index 984457e..67fb697 100644 --- a/src/main/java/frc/robot/subsystems/roller/RollerConstants.java +++ b/src/main/java/frc/robot/subsystems/roller/RollerConstants.java @@ -19,7 +19,10 @@ public static TalonFXConfiguration getConfig() { .withKI(0.0) .withKD(0.0)) .withCurrentLimits(new CurrentLimitsConfigs() - .withStatorCurrentLimit(70) + .withStatorCurrentLimit(60) + .withSupplyCurrentLimit(40) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimitEnable(true) // .withSupplyCurrentLimit(120) ) // .withTorqueCurrent(new TorqueCurrentConfigs() diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 06c310e..7d77a55 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -88,20 +88,28 @@ private Shooter(boolean left) { topID = Motors.TOPRIGHT.id; } - var config = getConfig(); + var tconfig = getTopConfig(); if (!left) { - config = config.clone().withMotorOutput( + tconfig = tconfig.clone().withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive)); + } + + var bconfig = getBottomConfig(); + + if (!left) { + bconfig = bconfig.clone().withMotorOutput( new MotorOutputConfigs() .withInverted(InvertedValue.Clockwise_Positive)); } bottomMotor = new TalonFX(bottomID); - bottomMotor.getConfigurator().apply(config); + bottomMotor.getConfigurator().apply(bconfig); bottomMotor.setNeutralMode(NeutralModeValue.Coast); topMotor = new TalonFX(topID); - topMotor.getConfigurator().apply(config); + topMotor.getConfigurator().apply(tconfig); topMotor.setNeutralMode(NeutralModeValue.Coast); if (Robot.isSimulation()) { @@ -223,14 +231,15 @@ public Command shoot(DoubleSupplier topSpeed, DoubleSupplier bottomSpeed) { } public Command shoot() { - return shoot(() -> Drive.getInstance().getPose().getTranslation() - .getDistance( - Constants.FieldConstants.allianceCorrected( - FieldPoses.HUB.pose3d.getTranslation()).toTranslation2d())); + return shoot(30, 30); + // return shoot(() -> Drive.getInstance().getPose().getTranslation() + // .getDistance( + // Constants.FieldConstants.allianceCorrected( + // FieldPoses.HUB.pose3d.getTranslation()).toTranslation2d())); } - public Command runVolts(double voltage) { - return runOnce(() -> setTopRequest(new VoltageOut(voltage))); + public void runVolts(Voltage voltage) { + setTopRequest(new VoltageOut(voltage)); } public SysIdRoutine sysId() { @@ -239,7 +248,7 @@ public SysIdRoutine sysId() { null, null, null, // Use default config (state) -> Logger.recordOutput("SysIdTestState", state.toString())), new SysIdRoutine.Mechanism( - (voltage) -> runVolts(voltage.in(Volts)), + this::runVolts, null, // No log consumer, since data is recorded by AdvantageKit this)); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index e4a8317..2afe60e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -28,18 +28,43 @@ private Motors(int id) { } /** Config for shooter motors */ - public static TalonFXConfiguration getConfig() { + public static TalonFXConfiguration getTopConfig() { return new TalonFXConfiguration() .withSlot0(new Slot0Configs() - .withKV(0.0) - .withKP(0.6) + .withKP(0.3) .withKI(0.0) .withKD(0.0) - .withKA(0.0) - .withKS(5.0) - .withKV(0.0)) // placeholder values + .withKA(0.012289) + .withKS(0.12018) + .withKV(0.12347)) // placeholder values .withCurrentLimits(new CurrentLimitsConfigs() - .withStatorCurrentLimit(60)) + .withStatorCurrentLimit(60) + .withSupplyCurrentLimit(40) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimitEnable(true)) + .withVoltage(new VoltageConfigs() + .withPeakForwardVoltage(12.0) + .withPeakReverseVoltage(-12.0)) + .withMotionMagic(new MotionMagicConfigs() + .withMotionMagicAcceleration(120) // 1.0 m/s^2 + .withMotionMagicCruiseVelocity(120) + .withMotionMagicJerk(120)); + } + /** Config for shooter motors */ + public static TalonFXConfiguration getBottomConfig() { + return new TalonFXConfiguration() + .withSlot0(new Slot0Configs() + .withKP(0.3) + .withKI(0.0) + .withKD(0.0) + .withKA(0.0092851) + .withKS(0.068015) + .withKV(0.11781)) // placeholder values + .withCurrentLimits(new CurrentLimitsConfigs() + .withStatorCurrentLimit(60) + .withSupplyCurrentLimit(40) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimitEnable(true)) .withVoltage(new VoltageConfigs() .withPeakForwardVoltage(12.0) .withPeakReverseVoltage(-12.0)) From 80d73638a95ef8d42e2e46c6d04b67b1725ed703 Mon Sep 17 00:00:00 2001 From: seaspace1250 <117596196+seaspace1250@users.noreply.github.com> Date: Fri, 20 Mar 2026 00:13:32 -0700 Subject: [PATCH 115/118] changes made on 3/18 --- src/main/java/frc/robot/BuildConstants.java | 10 +++--- src/main/java/frc/robot/ControlsMapping.java | 26 +++++++++----- src/main/java/frc/robot/Robot.java | 6 +++- .../frc/robot/subsystems/shooter/Shooter.java | 34 ++++++++++++++----- .../robot/subsystems/vision/VisionDevice.java | 3 ++ 5 files changed, 56 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 627a997..2c34a72 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Robot2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 182; - public static final String GIT_SHA = "237cfa1c94554327f075771a5cf853cc676ed696"; - public static final String GIT_DATE = "2026-03-15 15:51:04 EDT"; + public static final int GIT_REVISION = 183; + public static final String GIT_SHA = "521f4fd4a7783e9a45f4c8d356e09f15070d7cb4"; + public static final String GIT_DATE = "2026-03-15 18:01:08 EDT"; public static final String GIT_BRANCH = "sotm"; - public static final String BUILD_DATE = "2026-03-15 17:58:44 EDT"; - public static final long BUILD_UNIX_TIME = 1773611924443L; + public static final String BUILD_DATE = "2026-03-18 20:25:52 EDT"; + public static final long BUILD_UNIX_TIME = 1773879952982L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index b7f2a05..f31a7b7 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -2,6 +2,8 @@ import static frc.robot.Robot.controller; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; + import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.geometry.*; @@ -21,6 +23,9 @@ public class ControlsMapping { + public static LoggedNetworkNumber speed = new LoggedNetworkNumber("speed", 30); + public static LoggedNetworkNumber bottom = new LoggedNetworkNumber("bottom", 30); + public static void mapTeleopCommand() { Drive.getInstance().setDefaultCommand((Drive.getInstance().openLoopControl())); @@ -40,17 +45,17 @@ public static void mapTeleopCommand() { .whileTrue(indexAll()) .onFalse(stopIndex()); - // controller.leftTrigger() - // .whileTrue(Intake.getInstance().intake()) - // .onFalse(Intake.getInstance().stopWheel()); + controller.leftTrigger() + .whileTrue(Intake.getInstance().intake()) + .onFalse(Intake.getInstance().stopWheel()); - // controller.y().and(controller.back().negate()) - // .whileTrue(Intake.getInstance().stow()) - // .onFalse(Intake.getInstance().lower()); + controller.y().and(controller.back().negate()) + .whileTrue(Intake.getInstance().stow()) + .onFalse(Intake.getInstance().lower()); controller.rightTrigger().whileTrue(Drive.getInstance().headingLockToHub()); - // controller.povDown().onTrue(Intake.getInstance().calibrateZero()); - // controller.b().and(controller.back().negate()).whileTrue(backIndex()).onFalse(stopIndex()); + controller.povDown().onTrue(Intake.getInstance().calibrateZero()); + controller.b().and(controller.back().negate()).whileTrue(backIndex()).onFalse(stopIndex()); controller.y().and(controller.back().negate()).whileTrue(Shooter.getRightInstance().sysId().dynamic(SysIdRoutine.Direction.kForward)); controller.x().and(controller.back().negate()).whileTrue(Shooter.getRightInstance().sysId().dynamic(SysIdRoutine.Direction.kReverse)); @@ -82,6 +87,11 @@ public static Command shootAll() { Shooter.getRightInstance().shoot(), Shooter.getLeftInstance().shoot()); } + public static Command shootAllTable() { + return Commands.parallel( + Shooter.getRightInstance().shoot(speed::getAsDouble, speed::getAsDouble), + Shooter.getLeftInstance().shoot(speed::getAsDouble, speed::getAsDouble)); + } public static Command stopShoot() { return Commands.parallel( diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4f95acf..d3c6018 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -36,6 +36,7 @@ import frc.robot.subsystems.TelemetryManager; import frc.robot.subsystems.climb.Climb; import frc.robot.subsystems.drive.*; +import frc.robot.subsystems.drive.DriveConstants.FieldPoses; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.led.Led; @@ -160,7 +161,10 @@ public void robotPeriodic() { // commands, running already-scheduled commands, removing finished or interrupted commands, // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. - + SmartDashboard.putNumber("toShooter", Drive.getInstance().getPose().getTranslation() + .getDistance( + Constants.FieldConstants.allianceCorrected( + FieldPoses.HUB.pose3d.getTranslation()).toTranslation2d())); commandScheduler.run(); double now = Timer.getFPGATimestamp(); fpsTracker.add(1.0 / (now - lastTime)); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 7d77a55..0cd2677 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -19,6 +19,7 @@ import edu.wpi.first.math.InterpolatingMatrixTreeMap; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.plant.DCMotor; @@ -69,6 +70,7 @@ public static Shooter getRightInstance() { // private ShotCalculator shotCalculator = ShotCalculator.getInstance(); private InterpolatingMatrixTreeMap distance_to_shooter = new InterpolatingMatrixTreeMap(); + private InterpolatingDoubleTreeMap tree = new InterpolatingDoubleTreeMap(); private ShooterIO io; @@ -137,6 +139,18 @@ private Shooter(boolean left) { distance_to_shooter.put(2.54, VecBuilder.fill(49.21875, 49.21875)); distance_to_shooter.put(3.0, VecBuilder.fill(60.9375, 60.9375)); distance_to_shooter.put(3.4, VecBuilder.fill(81.25, 81.265)); + + tree.put(1.24, 27.0); + tree.put(1.56, 28.0); + tree.put(1.752, 29.8); + tree.put(2.005, 32.0); + tree.put(2.77, 36.0); + tree.put(3.09, 44.0); + tree.put(2.45, 34.5); + tree.put(2.3, 33.3); + tree.put(1.87, 32.0); + tree.put(2.094, 32.6); + tree.put(2.91, 39.8); } @Override @@ -211,10 +225,12 @@ public Command shoot(double topSpeed, double bottomSpeed) { public Command shoot(DoubleSupplier distance) { return shoot( - distance_to_shooter.get(distance.getAsDouble()) - .get(0, 0) - 15, - distance_to_shooter.get(distance.getAsDouble()) - .get(1, 0) + 15); + tree.get(distance.getAsDouble()) - 15, + tree.get(distance.getAsDouble()) + 15); + // distance_to_shooter.get(distance.getAsDouble()) + // .get(0, 0) - 15, + // distance_to_shooter.get(distance.getAsDouble()) + // .get(1, 0) + 15); } public Command shoot(DoubleSupplier topSpeed, DoubleSupplier bottomSpeed) { @@ -231,11 +247,11 @@ public Command shoot(DoubleSupplier topSpeed, DoubleSupplier bottomSpeed) { } public Command shoot() { - return shoot(30, 30); - // return shoot(() -> Drive.getInstance().getPose().getTranslation() - // .getDistance( - // Constants.FieldConstants.allianceCorrected( - // FieldPoses.HUB.pose3d.getTranslation()).toTranslation2d())); + // return shoot(30, 30); + return shoot(() -> Drive.getInstance().getPose().getTranslation() + .getDistance( + Constants.FieldConstants.allianceCorrected( + FieldPoses.HUB.pose3d.getTranslation()).toTranslation2d())); } public void runVolts(Voltage voltage) { diff --git a/src/main/java/frc/robot/subsystems/vision/VisionDevice.java b/src/main/java/frc/robot/subsystems/vision/VisionDevice.java index cd83589..d7d613c 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionDevice.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionDevice.java @@ -92,6 +92,9 @@ private void processFrames() { var result = camera.getLatestResult(); if (result.hasTargets()) { var target = result.getBestTarget(); + if (target.getPoseAmbiguity() > 0.2) { + return; + } var initBotPose = PhotonUtils.estimateFieldToRobotAprilTag( target.getBestCameraToTarget(), From 51ba8578054f3953debad8e5fd11fbee3e6944da Mon Sep 17 00:00:00 2001 From: dcao6668 Date: Sun, 22 Mar 2026 11:06:15 -0700 Subject: [PATCH 116/118] wk -2 feats and fixes --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/ControlsMapping.java | 14 +++++++------- .../java/frc/robot/subsystems/climb/Climb.java | 2 +- .../frc/robot/subsystems/climb/ClimbConstants.java | 2 +- .../robot/subsystems/intake/IntakeConstants.java | 6 +++--- .../robot/subsystems/roller/RollerConstants.java | 2 +- 6 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 2c34a72..fccf68e 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Robot2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 183; - public static final String GIT_SHA = "521f4fd4a7783e9a45f4c8d356e09f15070d7cb4"; - public static final String GIT_DATE = "2026-03-15 18:01:08 EDT"; + public static final int GIT_REVISION = 184; + public static final String GIT_SHA = "80d73638a95ef8d42e2e46c6d04b67b1725ed703"; + public static final String GIT_DATE = "2026-03-20 03:13:32 EDT"; public static final String GIT_BRANCH = "sotm"; - public static final String BUILD_DATE = "2026-03-18 20:25:52 EDT"; - public static final long BUILD_UNIX_TIME = 1773879952982L; + public static final String BUILD_DATE = "2026-03-22 13:47:41 EDT"; + public static final long BUILD_UNIX_TIME = 1774201661432L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/ControlsMapping.java b/src/main/java/frc/robot/ControlsMapping.java index f31a7b7..11faa41 100644 --- a/src/main/java/frc/robot/ControlsMapping.java +++ b/src/main/java/frc/robot/ControlsMapping.java @@ -45,7 +45,7 @@ public static void mapTeleopCommand() { .whileTrue(indexAll()) .onFalse(stopIndex()); - controller.leftTrigger() + controller.rightTrigger() .whileTrue(Intake.getInstance().intake()) .onFalse(Intake.getInstance().stopWheel()); @@ -53,15 +53,15 @@ public static void mapTeleopCommand() { .whileTrue(Intake.getInstance().stow()) .onFalse(Intake.getInstance().lower()); - controller.rightTrigger().whileTrue(Drive.getInstance().headingLockToHub()); + controller.leftTrigger().whileTrue(Drive.getInstance().headingLockToHub()); controller.povDown().onTrue(Intake.getInstance().calibrateZero()); controller.b().and(controller.back().negate()).whileTrue(backIndex()).onFalse(stopIndex()); - controller.y().and(controller.back().negate()).whileTrue(Shooter.getRightInstance().sysId().dynamic(SysIdRoutine.Direction.kForward)); - controller.x().and(controller.back().negate()).whileTrue(Shooter.getRightInstance().sysId().dynamic(SysIdRoutine.Direction.kReverse)); - controller.b().and(controller.back().negate()).whileTrue(Shooter.getRightInstance().sysId().quasistatic(SysIdRoutine.Direction.kForward)); - controller.a().and(controller.back().negate()).whileTrue(Shooter.getRightInstance().sysId().quasistatic(SysIdRoutine.Direction.kReverse)); - // controller.a().whileTrue( + // controller.y().and(controller.back().negate()).whileTrue(Shooter.getRightInstance().sysId().dynamic(SysIdRoutine.Direction.kForward)); + // controller.x().and(controller.back().negate()).whileTrue(Shooter.getRightInstance().sysId().dynamic(SysIdRoutine.Direction.kReverse)); + // controller.b().and(controller.back().negate()).whileTrue(Shooter.getRightInstance().sysId().quasistatic(SysIdRoutine.Direction.kForward)); + // controller.a().and(controller.back().negate()).whileTrue(Shooter.getRightInstance().sysId().quasistatic(SysIdRoutine.Direction.kReverse)); + // // controller.a().whileTrue( // Intake.getInstance().agitate() // ).onFalse(Intake.getInstance().lower()); diff --git a/src/main/java/frc/robot/subsystems/climb/Climb.java b/src/main/java/frc/robot/subsystems/climb/Climb.java index 5c428a6..faa6504 100644 --- a/src/main/java/frc/robot/subsystems/climb/Climb.java +++ b/src/main/java/frc/robot/subsystems/climb/Climb.java @@ -44,7 +44,7 @@ public static Climb getInstance() { private Climb() { super(); - climbMotor = new TalonFX(CLIMB_MOTOR_ID); + climbMotor = new TalonFX(CLIMB_MOTOR_ID,"CV"); climbMotor.getConfigurator().apply(getConfig()); climbMotor.setNeutralMode(NeutralModeValue.Brake); if (Robot.isSimulation()) { diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java b/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java index 79a60b7..530b3ab 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java @@ -26,7 +26,7 @@ public class ClimbConstants { public static final double MAX_ACCEL = 1.5; public static final double MAX_SPEED = 1.0; // m/s - public static final int CLIMB_MOTOR_ID = 35; // change this + public static final int CLIMB_MOTOR_ID = 41; // change this public static enum Setpoint { BASE(0.003), // small offset to prevent stalling (allegedly) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index bbcba46..e5338c9 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -17,10 +17,10 @@ public class IntakeConstants { public static final double BAR_EPSILON = Units.Degrees.of(10).in(Units.Rotations); - public static final double INTAKE_SPEED = 50; + public static final double INTAKE_SPEED = 70; public static final double BAR_POSITION_DOWN = 0.00; - public static final double BAR_POSITION_MID = Degrees.of(67).in(Rotations); - public static final double BAR_POSITION_UP = Degrees.of(126).in(Rotations); + public static final double BAR_POSITION_MID = Degrees.of(60).in(Rotations); + public static final double BAR_POSITION_UP = Degrees.of(80).in(Rotations); public static final double BAR_GEAR_RATIO = 44.0 / 18.0 * 5.0 * 4.0; public static final double BAR_POS_MIN = 0.0; public static final double BAR_POS_MAX = Degrees.of(127).in(Rotations); diff --git a/src/main/java/frc/robot/subsystems/roller/RollerConstants.java b/src/main/java/frc/robot/subsystems/roller/RollerConstants.java index 67fb697..9f968cd 100644 --- a/src/main/java/frc/robot/subsystems/roller/RollerConstants.java +++ b/src/main/java/frc/robot/subsystems/roller/RollerConstants.java @@ -9,7 +9,7 @@ public class RollerConstants { public static final int MOTOR_ID = 33; - public static final double ROLL_SPEED = 15; + public static final double ROLL_SPEED = -10; public static TalonFXConfiguration getConfig() { return new TalonFXConfiguration() From c8d0bf90ed27abe1da8251028426d7db4ca1bcf9 Mon Sep 17 00:00:00 2001 From: Arvind Venkatesh Date: Sun, 22 Mar 2026 11:12:38 -0700 Subject: [PATCH 117/118] feat: sysid values --- .../frc/robot/subsystems/indexer/Indexer.java | 20 +++++++++++++++++++ .../subsystems/indexer/IndexerConstants.java | 6 +++--- .../frc/robot/subsystems/roller/Roller.java | 2 +- 3 files changed, 24 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 4b21154..274027f 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -2,18 +2,23 @@ import static frc.robot.subsystems.indexer.IndexerConstants.*; +import org.littletonrobotics.junction.Logger; + import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.simulation.FlywheelSim; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Robot; // TODO (ethan): only activate if shooter ready @@ -193,4 +198,19 @@ public Command deactivateIndexer() { // TelemetryManager.makeSendableTalonFX("Indexer Motor", motor, builder); // } + public void runVolts(Voltage voltage) { + setRequest(new VoltageOut(voltage)); + } + + public SysIdRoutine sysId() { + return new SysIdRoutine( + new SysIdRoutine.Config( + null, null, null, // Use default config + (state) -> Logger.recordOutput("SysIdTestState", state.toString())), + new SysIdRoutine.Mechanism( + this::runVolts, + null, // No log consumer, since data is recorded by AdvantageKit + this)); + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java index f321a88..dc33909 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java @@ -25,9 +25,9 @@ public static TalonFXConfiguration getConfig() { .withKP(0.3) .withKI(0.0) .withKD(0.0) - .withKA(0.0) - .withKS(0.0) - .withKV(0.0)) // placeholder values + .withKA(0.021119) + .withKS(0.69736) + .withKV(0.10261)) // placeholder values .withCurrentLimits(new CurrentLimitsConfigs() .withStatorCurrentLimit(60) .withSupplyCurrentLimit(40) diff --git a/src/main/java/frc/robot/subsystems/roller/Roller.java b/src/main/java/frc/robot/subsystems/roller/Roller.java index d8987d0..3a8e9a1 100644 --- a/src/main/java/frc/robot/subsystems/roller/Roller.java +++ b/src/main/java/frc/robot/subsystems/roller/Roller.java @@ -92,7 +92,7 @@ public Command roll() { } public Command antiRoll() { - return setSpeed(-ROLL_SPEED); + return setSpeed(-ROLL_SPEED/2); } public Command stop() { From e5b13223d05ac23239d0333d3f7b3d21012ff24a Mon Sep 17 00:00:00 2001 From: audreywtu Date: Mon, 23 Mar 2026 19:48:22 -0700 Subject: [PATCH 118/118] merging --- build.gradle | 3 --- .../robot/subsystems/drive/heatmap/README.md | 18 +----------------- 2 files changed, 1 insertion(+), 20 deletions(-) diff --git a/build.gradle b/build.gradle index 9dd9992..f91cefd 100644 --- a/build.gradle +++ b/build.gradle @@ -38,8 +38,6 @@ deploy { // getTargetTypeClass is a shortcut to get the class type using a string frcJava(getArtifactTypeClass('FRCJavaArtifact')) { -<<<<<<< HEAD -======= // Enable VisualVM connection jvmArgs.add("-Dcom.sun.management.jmxremote=true") jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") @@ -49,7 +47,6 @@ deploy { jvmArgs.add("-Djava.rmi.server.hostname=10.14.58.2") // Replace TE.AM with team number jvmArgs.add("-XX:+HeapDumpOnOutOfMemoryError") jvmArgs.add("-XX:HeapDumpPath=/home/lvuser/dumps/") ->>>>>>> origin/sotm } // Static files artifact diff --git a/src/main/java/frc/robot/subsystems/drive/heatmap/README.md b/src/main/java/frc/robot/subsystems/drive/heatmap/README.md index 885f690..eb65a70 100644 --- a/src/main/java/frc/robot/subsystems/drive/heatmap/README.md +++ b/src/main/java/frc/robot/subsystems/drive/heatmap/README.md @@ -1,18 +1,2 @@ -<<<<<<< HEAD # 1458Framework2026_2 -A framework for team 1458's 2026 code. Will be using the command-based framework from WPILIB -======= -# Cycle Time Heatmap - -Welcome to the Cycle Time Heatmap project! - -This project is designed to help visualize and analyze cycle times using a heatmap -To run the code, follow these steps: -1. Clone the repository to your local machine. -2. Run the main python script -3. Install any missing dependencies using pip. -4. Enjoy visualizing your cycle times! - -Note: this project is currently under development, so documentation may be incomplete. Stay tuned for updates! - ->>>>>>> other_repo/main +A framework for team 1458's 2026 code. Will be using the command-based framework from WPILIB \ No newline at end of file