From df8250005a1ade59fb5ff0f8fa55adcc3575fb37 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Wed, 23 Oct 2024 23:39:05 -0400 Subject: [PATCH 1/7] clean up some unused constants and smartnumbers, make robot align to back of wall for speaker shot (arm still aims for point on slant) --- .../commands/auton/BCA/FourPieceBCA.java | 1 - .../auton/BCA/RightAngleFourPieceBCA.java | 1 - .../swerve/SwerveDriveAlignToSpeaker.java | 2 +- .../driveAligned/SwerveDriveDriveAligned.java | 3 +- .../com/stuypulse/robot/constants/Field.java | 7 +- .../stuypulse/robot/constants/Settings.java | 44 ++--- .../robot/subsystems/arm/ArmImpl.java | 32 ++-- .../util/FollowPathPointSpeakerCommand.java | 3 +- .../util/SpeakerAngleElinInterpolation.java | 165 ++++++++++++------ 9 files changed, 139 insertions(+), 119 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java b/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java index 576303e3..1a60a59e 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java @@ -9,7 +9,6 @@ import com.stuypulse.robot.commands.intake.IntakeSetAcquire; import com.stuypulse.robot.commands.shooter.ShooterFeederShoot; import com.stuypulse.robot.commands.shooter.ShooterFeederStop; -import com.stuypulse.robot.constants.Settings.Alignment.Shoot; import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BCA/RightAngleFourPieceBCA.java b/src/main/java/com/stuypulse/robot/commands/auton/BCA/RightAngleFourPieceBCA.java index 51b3509a..e08d1cd6 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BCA/RightAngleFourPieceBCA.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BCA/RightAngleFourPieceBCA.java @@ -9,7 +9,6 @@ import com.stuypulse.robot.commands.intake.IntakeSetAcquire; import com.stuypulse.robot.commands.shooter.ShooterFeederShoot; import com.stuypulse.robot.commands.shooter.ShooterFeederStop; -import com.stuypulse.robot.constants.Settings.Alignment.Shoot; import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignToSpeaker.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignToSpeaker.java index 2c33f898..5ff36818 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignToSpeaker.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignToSpeaker.java @@ -37,7 +37,7 @@ public SwerveDriveAlignToSpeaker() { .withRotationalDeadband(Settings.Swerve.MAX_ANGULAR_VELOCITY * Settings.Driver.Turn.DEADBAND.get()) .withDriveRequestType(DriveRequestType.OpenLoopVoltage); - controller = new AnglePIDController(Settings.Swerve.Assist.kP, Settings.Swerve.Assist.kI, Settings.Swerve.Assist.kD) + controller = new AnglePIDController(Settings.Swerve.Motion.THETA.kP, Settings.Swerve.Motion.THETA.kI, Settings.Swerve.Motion.THETA.kD) .setOutputFilter(x -> -x); AngleVelocity derivative = new AngleVelocity(); diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java index d69e511e..75ac1535 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java @@ -2,6 +2,7 @@ import com.stuypulse.robot.constants.Settings.Driver.Drive; import com.stuypulse.robot.constants.Settings.Swerve.Assist; +import com.stuypulse.robot.constants.Settings.Swerve.Motion; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.stuylib.control.angle.AngleController; import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; @@ -38,7 +39,7 @@ public SwerveDriveDriveAligned(Gamepad driver) { new VRateLimit(Drive.MAX_TELEOP_ACCEL.get()), new VLowPassFilter(Drive.RC.get())); - controller = new AnglePIDController(Assist.kP, Assist.kI, Assist.kD) + controller = new AnglePIDController(Motion.THETA.kP, Motion.THETA.kI, Motion.THETA.kD) .setOutputFilter(x -> -x); AngleVelocity derivative = new AngleVelocity(); diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 13d3b51c..2ac16a6a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -155,12 +155,7 @@ public static AprilTag getTag(int id) { public static Pose2d getAllianceSpeakerPose() { return (Robot.isBlue() ? NamedTags.BLUE_SPEAKER : NamedTags.RED_SPEAKER) - .getLocation().toPose2d().transformBy(new Transform2d(SPEAKER_OPENING_X, 0, new Rotation2d())); - } - - public static Pose2d getSpeakerPathFindPose() { - return getAllianceSpeakerPose().transformBy( - new Transform2d(0, Units.inchesToMeters(200), new Rotation2d())); + .getLocation().toPose2d(); } /*** AMP ***/ diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 90c24f94..a5ee8456 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -39,18 +39,18 @@ public interface Arm { SmartNumber MAX_VELOCITY = new SmartNumber("Arm/Max Velocity (deg/s)", SAFE_MODE_ENABLED ? 200 : 750); SmartNumber MAX_ACCELERATION = new SmartNumber("Arm/Max Acceleration (deg/s^2)", SAFE_MODE_ENABLED ? 200 : 700); - SmartNumber MAX_ANGLE = new SmartNumber("Arm/Max Angle (deg)", 85); - SmartNumber MIN_ANGLE = new SmartNumber("Arm/Min Angle (deg)", -90 + 12.25); + double MAX_ANGLE = 85; + double MIN_ANGLE = -90 + 12.25; SmartNumber MAX_ANGLE_ERROR = new SmartNumber("Arm/Max Angle Error", 2.5); SmartNumber AMP_ANGLE = new SmartNumber("Arm/Amp Angle", 49); - SmartNumber LOW_FERRY_ANGLE = new SmartNumber("Arm/Low Ferry Angle", MIN_ANGLE.get()); + SmartNumber LOW_FERRY_ANGLE = new SmartNumber("Arm/Low Ferry Angle", MIN_ANGLE); SmartNumber LOB_FERRY_ANGLE = new SmartNumber("Arm/Lob Ferry Angle", -50); - SmartNumber PRE_CLIMB_ANGLE = new SmartNumber("Arm/Pre climb angle", 90); - SmartNumber POST_CLIMB_ANGLE = new SmartNumber("Arm/Post Climb Angle", MIN_ANGLE.get()); + SmartNumber PRE_CLIMB_ANGLE = new SmartNumber("Arm/Pre climb angle", 85); + SmartNumber POST_CLIMB_ANGLE = new SmartNumber("Arm/Post Climb Angle", MIN_ANGLE); - SmartNumber FEED_ANGLE = new SmartNumber("Arm/Feed Angle", MIN_ANGLE.get() + 0); + SmartNumber FEED_ANGLE = new SmartNumber("Arm/Feed Angle", MIN_ANGLE + 0); SmartNumber MAX_ACCEPTABLE_FEED_ANGLE = new SmartNumber("Arm/Max Acceptable Feed Angle", FEED_ANGLE.get() + 4); SmartNumber SUBWOOFER_SHOT_ANGLE = new SmartNumber("Arm/Subwoofer Shot Angle", -33); @@ -85,7 +85,7 @@ public interface Intake { double INTAKE_FEED_SPEED = 0.4; - double MAX_ARM_ANGLE_FOR_INTAKE_SHOOT = Arm.MIN_ANGLE.get() + 20; + double MAX_ARM_ANGLE_FOR_INTAKE_SHOOT = Arm.MIN_ANGLE + 20; double ARM_SPEED_THRESHOLD_TO_FEED = 1.75; // degrees per second double INTAKE_SHOOT_SPEED = 0.9; @@ -112,11 +112,8 @@ public interface Shooter { new SmartNumber("Shooter/Speaker RPM differential", 500) ); - // TODO: Find velocity - double SPEAKER_SHOT_VELOCITY = 10.0; // m/s - - SmartNumber HAS_NOTE_FALLING_DEBOUNCE = new SmartNumber("Shooter/Has Note Falling Debounce", 0.0); - SmartNumber HAS_NOTE_RISING_DEBOUNCE = new SmartNumber("Shooter/Has Note Rising Debounce", 0.0); + double HAS_NOTE_FALLING_DEBOUNCE = 0.0; + double HAS_NOTE_RISING_DEBOUNCE = 0.0; // left runs faster than right public interface LEFT { @@ -168,15 +165,8 @@ public interface Swerve { double SPEED_AT_12_VOLTS = 5.21; public interface Assist { - SmartNumber ALIGN_MIN_SPEAKER_DIST = new SmartNumber("SwerveAssist/Minimum Distance to Speaker", 4); - double AMP_WALL_SCORE_DISTANCE = (Settings.LENGTH / 2) + Units.inchesToMeters(2.5); - // angle PID - SmartNumber kP = new SmartNumber("SwerveAssist/kP", 5.0); - SmartNumber kI = new SmartNumber("SwerveAssist/kI", 0.0); - SmartNumber kD = new SmartNumber("SwerveAssist/kD", 0.2); - double ANGLE_DERIV_RC = 0.05; double REDUCED_FF_DIST = 0.75; } @@ -273,7 +263,7 @@ public interface Alignment { SmartNumber X_TOLERANCE = new SmartNumber("Alignment/X Tolerance", 0.1); SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance", 0.1); - SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 6); + SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 3); SmartNumber CLIMB_SETUP_DISTANCE = new SmartNumber("Alignment/Climb/Setup Distance", Units.inchesToMeters(21.0)); SmartNumber INTO_CHAIN_SPEED = new SmartNumber("Alignment/Climb/Into Chain Speed", 0.25); @@ -293,20 +283,6 @@ public interface Rotation { SmartNumber ALIGN_OMEGA_DEADBAND = new SmartNumber("Alignment/Rotation/Omega Deadband", 0.05); } - - public interface Shoot { - public interface Translation { - SmartNumber kP = new SmartNumber("ShootAlign/Translation/kP", 7.5); - SmartNumber kI = new SmartNumber("ShootAlign/Translation/kI", 0.0); - SmartNumber kD = new SmartNumber("ShootAlign/Translation/kD", 0.7); - } - - public interface Rotation { - SmartNumber kP = new SmartNumber("ShootAlign/Rotation/kP", 6.0); - SmartNumber kI = new SmartNumber("ShootAlign/Rotation/kI", 0.0); - SmartNumber kD = new SmartNumber("ShootAlign/Rotation/kD", 0.4); - } - } } public interface LED { diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index 3f0e3ac2..18fc0a57 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -1,11 +1,11 @@ package com.stuypulse.robot.subsystems.arm; -import org.ejml.dense.row.mult.SubmatrixOps_FDRM; - +import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; - -import com.revrobotics.CANSparkLowLevel.MotorType; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Motors; +import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; @@ -15,22 +15,16 @@ import com.stuypulse.stuylib.control.Controller; import com.stuypulse.stuylib.control.feedback.PIDController; import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; -import com.stuypulse.stuylib.math.Angle; import com.stuypulse.stuylib.math.SLMath; import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; import com.stuypulse.stuylib.streams.numbers.filters.MotionProfile; -import com.stuypulse.robot.Robot; -import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.constants.Motors; -import com.stuypulse.robot.constants.Ports; 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.util.Units; import edu.wpi.first.wpilibj.DigitalInput; @@ -113,19 +107,19 @@ private double getTargetDegrees() { case FEED: return Settings.Arm.FEED_ANGLE.get(); case STOW: - return Settings.Arm.MIN_ANGLE.get(); + return Settings.Arm.MIN_ANGLE; case PRE_CLIMB: return Settings.Arm.PRE_CLIMB_ANGLE.get(); case CLIMBING: return Settings.Arm.POST_CLIMB_ANGLE.get(); default: - return Settings.Arm.MIN_ANGLE.get(); + return Settings.Arm.MIN_ANGLE; } } private double getSpeakerAngleElin() { try { - Pose2d speakerPose = Field.getAllianceSpeakerPose(); + Pose2d speakerPose = Field.getAllianceSpeakerPose().transformBy(new Transform2d(Field.SPEAKER_OPENING_X, 0, new Rotation2d())); double distanceToSpeaker = Units.metersToInches(SwerveDrive.getInstance().getPose().minus(speakerPose).getTranslation().getNorm()) - Units.metersToInches(Settings.WIDTH / 2); SmartDashboard.putNumber("harry", distanceToSpeaker); return SpeakerAngleElinInterpolation.getAngleInDegrees(distanceToSpeaker); @@ -304,7 +298,7 @@ public void periodic() { super.periodic(); if (bumpSwitchTriggered.get()) { - armEncoder.setPosition(Settings.Arm.MIN_ANGLE.get()/360); + armEncoder.setPosition(Settings.Arm.MIN_ANGLE/360); if (state == State.RESETTING) { state = State.FEED; } @@ -312,15 +306,15 @@ public void periodic() { if (state == State.RESETTING) { setVoltage(-1.5); - controller.update(Settings.Arm.MIN_ANGLE.get(), Settings.Arm.MIN_ANGLE.get()); + controller.update(Settings.Arm.MIN_ANGLE, Settings.Arm.MIN_ANGLE); } - else if (getTargetDegrees() == Settings.Arm.MIN_ANGLE.get() && bumpSwitchTriggered.get()) { + else if (getTargetDegrees() == Settings.Arm.MIN_ANGLE && bumpSwitchTriggered.get() && state != State.CLIMBING) { setVoltage(0); - controller.update(Settings.Arm.MIN_ANGLE.get(), Settings.Arm.MIN_ANGLE.get()); + controller.update(Settings.Arm.MIN_ANGLE, Settings.Arm.MIN_ANGLE); } else { - controller.update(SLMath.clamp(getTargetDegrees(), Settings.Arm.MIN_ANGLE.get(), Settings.Arm.MAX_ANGLE.get()), getDegrees()); - if (Shooter.getInstance().getFeederState() == Shooter.FeederState.SHOOTING && getDegrees() < Settings.Arm.MAX_ANGLE.get()) { + controller.update(SLMath.clamp(getTargetDegrees(), Settings.Arm.MIN_ANGLE, Settings.Arm.MAX_ANGLE), getDegrees()); + if (Shooter.getInstance().getFeederState() == Shooter.FeederState.SHOOTING && getDegrees() < Settings.Arm.MAX_ANGLE) { setVoltage(controller.getOutput() + 0.31); } else { diff --git a/src/main/java/com/stuypulse/robot/util/FollowPathPointSpeakerCommand.java b/src/main/java/com/stuypulse/robot/util/FollowPathPointSpeakerCommand.java index f0cdc24d..f2b7ae90 100644 --- a/src/main/java/com/stuypulse/robot/util/FollowPathPointSpeakerCommand.java +++ b/src/main/java/com/stuypulse/robot/util/FollowPathPointSpeakerCommand.java @@ -9,6 +9,7 @@ import com.pathplanner.lib.util.ReplanningConfig; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings.Swerve.Assist; +import com.stuypulse.robot.constants.Settings.Swerve.Motion; import com.stuypulse.stuylib.control.angle.AngleController; import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; import com.stuypulse.stuylib.math.Angle; @@ -84,7 +85,7 @@ public FollowPathPointSpeakerCommand( this.shouldFlipPath = shouldFlipPath; - headingController = new AnglePIDController(Assist.kP, Assist.kI, Assist.kD) + headingController = new AnglePIDController(Motion.THETA.kP, Motion.THETA.kI, Motion.THETA.kD) .setOutputFilter(x -> -x); AngleVelocity derivative = new AngleVelocity(); diff --git a/src/main/java/com/stuypulse/robot/util/SpeakerAngleElinInterpolation.java b/src/main/java/com/stuypulse/robot/util/SpeakerAngleElinInterpolation.java index c8c92d5a..40597cd6 100644 --- a/src/main/java/com/stuypulse/robot/util/SpeakerAngleElinInterpolation.java +++ b/src/main/java/com/stuypulse/robot/util/SpeakerAngleElinInterpolation.java @@ -10,61 +10,116 @@ public class SpeakerAngleElinInterpolation { // distance (angle), angle (radians) private static final double[][] distanceAndAngle = { - {45.1854245,-0.57751}, - {47.5, -0.60548}, - {50, -0.63458}, - {52.5, -0.66255}, - {55, -0.68943}, - {57.5,-0.71524}, - {60, -0.74002}, - {62.5, -0.76382}, - {65, -0.78668}, - {67.5, -0.80862}, - {70, -0.82969}, - {72.5, -0.84993}, - {75, -0.86938}, - {77.5, -0.88806}, - {80, -0.90602}, - {82.5, -0.92328}, - {85, -0.93989}, - {87.5, -0.95587}, - {90, -0.97125}, - {92.5, -0.98606}, - {95, -1.00032}, - {97.5, -1.01407}, - {100, -1.02732}, - {102.5, -1.0401}, - {105, -1.05243}, - {107.5, -1.06434}, - {110, -1.07583}, - {112.5, -1.08693}, - {115, -1.09767}, - {117.5, -1.10804}, - {120, -1.11808}, - {122.5, -1.12779}, - {125, -1.13719}, - {127.5, -1.14629}, - {130, -1.15512}, - {132.5, -1.16366}, - {135, -1.17195}, - {137.5, -1.17999}, - {140, -1.18779}, - {142.5, -1.19536}, - {145, -1.20271}, - {147.5, -1.20986}, - {150, -1.21679}, - {152.5, -1.22354}, - {155, -1.2301}, - {157.5, -1.23647}, - {160, -1.24268}, - {162.5, -1.24872}, - {165, -1.2546}, - {167.5, -1.26033}, - {170, -1.26591}, - {172.5, -1.27134}, - {175, -1.27664}, - {177.5, -1.28181}, - {180, -1.28685}, + // {45.1854245,-0.57751}, + // {47.5, -0.60548}, + // {50, -0.63458}, + // {52.5, -0.66255}, + // {55, -0.68943}, + // {57.5,-0.71524}, + // {60, -0.74002}, + // {62.5, -0.76382}, + // {65, -0.78668}, + // {67.5, -0.80862}, + // {70, -0.82969}, + // {72.5, -0.84993}, + // {75, -0.86938}, + // {77.5, -0.88806}, + // {80, -0.90602}, + // {82.5, -0.92328}, + // {85, -0.93989}, + // {87.5, -0.95587}, + // {90, -0.97125}, + // {92.5, -0.98606}, + // {95, -1.00032}, + // {97.5, -1.01407}, + // {100, -1.02732}, + // {102.5, -1.0401}, + // {105, -1.05243}, + // {107.5, -1.06434}, + // {110, -1.07583}, + // {112.5, -1.08693}, + // {115, -1.09767}, + // {117.5, -1.10804}, + // {120, -1.11808}, + // {122.5, -1.12779}, + // {125, -1.13719}, + // {127.5, -1.14629}, + // {130, -1.15512}, + // {132.5, -1.16366}, + // {135, -1.17195}, + // {137.5, -1.17999}, + // {140, -1.18779}, + // {142.5, -1.19536}, + // {145, -1.20271}, + // {147.5, -1.20986}, + // {150, -1.21679}, + // {152.5, -1.22354}, + // {155, -1.2301}, + // {157.5, -1.23647}, + // {160, -1.24268}, + // {162.5, -1.24872}, + // {165, -1.2546}, + // {167.5, -1.26033}, + // {170, -1.26591}, + // {172.5, -1.27134}, + // {175, -1.27664}, + // {177.5, -1.28181}, + // {180, -1.28685}, + {45, -0.53055}, + {47.5, -0.56248}, + {50, -0.59322}, + {52.5, -0.62279}, + {55, -0.65122}, + {57.5, -0.67854}, + {60, -0.70478}, + {62.5, -0.72998}, + {65, -0.75418}, + {67.5, -0.77742}, + {70, -0.79973}, + {72.5, -0.82116}, + {75, -0.84173}, + {77.5, -0.8615}, + {80, -0.88049}, + {82.5, -0.89874}, + {85, -0.91629}, + {87.5, -0.93316}, + {90, -0.94939}, + {92.5, -0.96502}, + {95, -0.98005}, + {97.5, -0.99454}, + {100, -1.00849}, + {102.5, -1.02195}, + {105, -1.03492}, + {107.5, -1.04743}, + {110, -1.05951}, + {112.5, -1.07117}, + {115, -1.08243}, + {117.5, -1.09331}, + {120, -1.10383}, + {122.5, -1.114}, + {125, -1.12385}, + {127.5, -1.13337}, + {130, -1.1426}, + {132.5, -1.15153}, + {135, -1.16019}, + {137.5, -1.16858}, + {140, -1.17672}, + {142.5, -1.18462}, + {145, -1.19229}, + {147.5, -1.19973}, + {150, -1.20695}, + {152.5, -1.21397}, + {155, -1.2208}, + {157.5, -1.22743}, + {160, -1.23388}, + {162.5, -1.24016}, + {165, -1.24626}, + {167.5, -1.25221}, + {170, -1.258}, + {172.5, -1.26364}, + {175, -1.26913}, + {177.5, -1.27449}, + {180, -1.27971}, }; static { From 923679231f13775d2e1659a5d139732e45e39b60 Mon Sep 17 00:00:00 2001 From: jopy-man Date: Thu, 24 Oct 2024 17:12:15 -0400 Subject: [PATCH 2/7] paths and stuff --- logs/FRC_TBD_0b09622a29ea64b8.wpilog | Bin 0 -> 61331 bytes logs/FRC_TBD_25d35e33ade850f9.wpilog | Bin 111340 -> 0 bytes .../deploy/pathplanner/autos/BCA BLUE.auto | 8 +--- .../deploy/pathplanner/autos/BCA RED.auto | 8 +--- .../paths/Blue 90 Deg B Shoot to C.path | 12 +++--- .../pathplanner/paths/Blue A to Center.path | 8 ++-- .../deploy/pathplanner/paths/Blue A to D.path | 12 +++--- .../pathplanner/paths/Blue ABC Waypoints.path | 40 +++++++++--------- .../pathplanner/paths/Blue Amp to A.path | 8 ++-- .../deploy/pathplanner/paths/Blue B to C.path | 20 ++++----- .../pathplanner/paths/Blue B to Center.path | 16 +++---- .../deploy/pathplanner/paths/Blue B to D.path | 8 ++-- .../deploy/pathplanner/paths/Blue B to F.path | 8 ++-- .../paths/Blue C to Shoot Before A.path | 16 +++---- .../pathplanner/paths/Blue Center to A.path | 16 +++---- .../pathplanner/paths/Blue Center to B.path | 14 +++--- .../pathplanner/paths/Blue Center to C.path | 19 +++------ .../pathplanner/paths/Blue F Shoot to A.path | 8 ++-- .../pathplanner/paths/Blue FC Shoot to C.path | 8 ++-- .../paths/Blue Midline Waypoints.path | 14 +++--- .../deploy/pathplanner/paths/Red B to C.path | 20 ++++----- .../paths/Red C to Shoot Before A.path | 2 +- .../pathplanner/paths/Red Center to B.path | 6 +-- .../com/stuypulse/robot/RobotContainer.java | 12 +++--- .../commands/auton/BCA/FourPieceBCA.java | 3 +- .../stuypulse/robot/constants/Settings.java | 4 +- .../robot/subsystems/arm/ArmImpl.java | 21 ++++++++- .../robot/subsystems/swerve/SwerveDrive.java | 3 +- .../util/SpeakerAngleElinInterpolation.java | 20 +++++++++ 29 files changed, 177 insertions(+), 157 deletions(-) create mode 100644 logs/FRC_TBD_0b09622a29ea64b8.wpilog delete mode 100644 logs/FRC_TBD_25d35e33ade850f9.wpilog diff --git a/logs/FRC_TBD_0b09622a29ea64b8.wpilog b/logs/FRC_TBD_0b09622a29ea64b8.wpilog new file mode 100644 index 0000000000000000000000000000000000000000..3f983fd8911af6bcd647580d6291771401ec50ca GIT binary patch literal 61331 zcmb`Q2YeLO8pSt)a4A+m=`7WNK!AWKMQRclI-x@XA=yA6$%Yhq??rl(-g_4vn)Ir4 z6i}p!fPmN$pHJnT`DQOUA_R7M`+oc6pUFAjy?5r`xosvb8`rJhD8%9Cz{lw*P|g2{yR?MkIsCgAz73mI3J8shNKCFBk<_`PJ0dYE zphilvJKi1VPDu(#a>cqLlU-4%7Nw!-Qz#+Po#0ALj&UUws??`YqANKfCf*fQs8Vub zimPAALP_qF#7I}6N`)FWE7Z^F2(bdDgYXqkz~BapMm0i~0BCAd<(3;f&rUuNkt zMwTt$(K{W@E$8%*SyC%2+Bqa5&J~*Aij0YliELn(gLiS#JLUxza60N)0iK6>j)wvS zb$7)lH$)WgY@c`Rux#sFwwChshin@}Bu92`m=f2~m1sqciBEp3{L>Um2K7o4>-Xg2 zbTqL1GXiqr%jMG4l@J@z%ZC6N)hrHRSOKhy?BQ~e3^c=O^(y&Fea!BSE%!`t&-GC9 z>n1gIMb(InjfselbXoc9=yu1tBI2K>h%))D>Q%tc>1b$~XMTivXmUhyj62?k!pW?9 zA1q*9;MT|KXkM+Se{g?)W$H-#&$sB0Ia{BBX6k z{S~VK~Ju%vY}N_I!OW7~IkB_<)gg({UTQ?3H; zJB{0JEaAw8R9m$@saI06E3R2gTx!cy#?FwwSy>lYeRrn@5%IWdCL$5haxowU$xyXV zZl985-OVBws?v_^ke0;Ps@(}VvF`ZpF;?t=WOol&VyVc8xPYicRLR5!?kHDNSGN>b zQnI%fZ&YB`($c;Lq#YW*Tz;ojB=--A%2=>F_$_5fB6)u?sT*Xe!hm>N1ifTUGJjhR@LF}`5@YF zpgh2*j8@ybXnEAn9IbVx3^g2i5zTXdNuwl#g9EyTdKWCO$_RV;oT!GISsqZnrw-P_ zyI}cLP94nQK(OMV{7)UMhIhg8t2ZcEF7;t%Pd!kW7cJt z<_WEBHB|vVlyL!dTF17^xXo`M=hH{?vXGtXmw!Y{m!*dEf1WaLUY2q}<(H*KpX_Tx zuJj&?yoEiN%WxDz-iqOCwUG~9t-&#_*r z9pNuK4M$NNPO~V36H%1T`mX5YQstt&O+isd4yWOCv@eE8RutWBdgMv7BQ=fb)Y+F% z#T>=0P+{*NloiIx*kjLH&4|dZzUJu-M-?kjcnAXdKdJO2kgr+tcNmCN$S-GM!w=Wv z9e=}790Ak3P)f&pJs2+j{5Ssg_2_hTOsHT;FWu8oc<4lV?y3#kR);j8K}1hy3v{X@ zW0HG$*L0Rpoe@Vq>8m>((Gx*dzt-vs;-oz>YK_Q940aM_py*xXH`Pdv++``q%4_+j zjU3-8B{m|_$9$Jmiz^tX3I-17`=!;Zi2o4LOCj>xUPkvl|E1Ii6^xvvYB&3D!Qx0k z+hI%VIjqSfPjz{?=N5M67qQ15_nMlh^|gyZy_n>d6IW)uG47wpu!8vxvk_u4M1C zTSjF?5bHB@JwkRU1u6?Ff$!r(O8ek*%(CiDPt5vjte82^)^k|J`{8 zu1*0-#l0)qaw;pLSf7Luh#6f26bO2B{nat~s`bPTFXv_CKvmTf`SLJCZrc=O)!j#5 zzYikEcZzj6gAx8=Iuch5Y96~d#Jit;cCPsIBQtpybDx8g?R!E ztYhV?8|bYx^Hty7!x`*KOzh=LxVMzs6D}ci7s3q%ReVCYj=qPhsQP=tIqF*BW`L|V z@9}4?aiYr^852r0=ZkudcMz@^zW=U<)>&)pPD*xS1_beZ zIC<}=R}sPbNNKi+0967x@qKi>V3#Y(mq?XV7)Npq2USKC&v5$@*Q0flwklpiveg&H zDfBIHWi?m_Zi+M5VLqq|0zbZ$2~FwP!|g`f?F_}tj4zq3qULyF4y=xtuFasTX=b)& z3Tj_yj~L9}J8NTtGzIh1PHVEsyUJKqeegv+d(1HfV~&P1f34?u7lEybjYpqPZ%4)N zs)_LQd=SBXx1OU~+7Y}QrBzdL@Jx*`5L7+w2sOMbrRu8xKO&3;)kr%+h<6ccsL^-p z?Z1BYT;sBPtn=_^>H3bEX-DvKny04vwRC-Zoc@UwA!k5+N3FCYcsaFGOXYy4=Yt5v zK(*73;N^5qZB-2x<7@~WHyNQDA*|IC^CmRz@4k(O8>tC8@1>Cd*PYw-9gPwE(cOuMi(Dqx zeY?IItLwMv`f8uyyZVkM2>j@=*&_pcdwDfc>As`Ot7CSAY>JT9=>N%uZ7(Eve|0rg zuOg29K?x@n0)+yte#}!wwJt6zynTf^{S&c>>Ft0mR29_8$b)30lmRtE60AOlx5RMXkBE#+ zNsNf}a=OY9S`V%*;c6|)Je*88TJsY=-CUctf^nM03h6-T>l?ZO=G5b`l|)ttkacJ^e-Oh&Y zg z#9}TxD}G>w(`*HiUnp-y&*V8OVEw8b@+2*b=b#549Dd7LvvyE$4Sm>yd^osPjoNfr zqnKB4)(j4*VdHX6R8H$ znHtF=XZwfB-`U1*-PABYRL@S+!W?<43Dcfe%)}9TBLye@CT*Z6l)vOX%Fb>-H5>vHV`4A zy$a!FkHe)#A%uJ`S93MnGhHDyR$o z>1}CaFPyhma+i$bwucoCs`>Uif#s3wMTp$hhg|n?S$8<^F6ypoq0V)Y>ZrMAK(UDZ zq?&78W3^+jOH`JGy{$W;9&!SZoB5}Zgu;xm+jFSRaF#~I;Qaz~gSItIb`VG@2? z5Z|;;h!49xl2ol7Vfr434o?S!N=BHpJ3M8dsl$`(7lkOu*B1OI<%#)1Fz-7&DOo;7 zzW&kS*@tM|5zUG3<4dt@fG-`M?k^vNyL|r9;W>$)_P~$RZZ^vXJX43KN5;=^;Xdf_ zTtv*Ch?#bWr))rKcfi|bI_x?^^ztf%mrZm}^{p;XG||1(O(I99pFq74KJ8mD=g{X4 zPw(`pWT+4i9c?w9eEoz&qeh{*kY$h9v`Z-3>~xt0!3Nzeeqep1c-XE(sxniLU;Fax~~ z<7I6cpepFRp*9UvZxK1%7x#ua1|bP)cX;e;E47=G`Y#_^=RxVe+3TrQ?%xo8@H2$> zp>_>czaXgfk=m7zabK8Y2q^6i&*MwLms&PNJ-?4TJg*`RLy-nw&w>xNZD__<_Hl;? zDH!$)rQl1g8>aFgs^{ZO_A9e5%nu!d>iFSs{InpxY2B)Q=)z<)+QF<%JgeTQdcmMG-wA03`JL>q-@PJAC1GdBIAeY(T58ezxz5Y~q#d7N5_s*hNXwpFX6 z!($@!cu?9Mo`2`qm!^2U`cRj)%<$|1P4FSxJ~Y7-GVa2a{9uOXC?ZevA=kb%y%W_D zoongvd;yw-*iWjt&)DI)j4+eE4&!BQnxwwgc|&cQ{10-tf5WjUNJ84TTKn2c?dGKZ z%ZJu^O8WokTIKfJALf|)4B>sKT~n1~Kh>^;tVqH%Bq8k%&*MwLms&PWW!cXio;*my z^k*mrA8On5jJftxho>k~@cuKDf-kl1eN`AyJs;-?zmhmF=oq|%AI`u}3*wvBt=fkU z&kR*@zwYpqLx(2>p=KgX+8v(2XX^0G^fM3z`I>(q%rWbU`9d)7J3O9iG5v z>F^vwh#S4`K(L5i_@Z4*oR#E()2D?@911hhbI`c1hJn~bDy!p(*R+XdL72g+O$M9(s@H|TKW%ixLf1c zG9)4G4v&3prFL^t|K&sLyexgYgL;N13gMSOLwFx**K!qUmxo=u68eEwAPH%AcphH@ zzSOc6c>fT0ct#-&E1#hle5h?JGmb!Qd$6Q66)9Nt45i>pty`rg+GhfB9nP74GY^LO zp<}QFKU|HU6vQ{JTeS}zp4Dp6LEYgA#Dmg34uv_^AWZ&8hmvRMoz`onynJie8kKw~ ztajbd8a3+&)pi;xbGq;&M~;Nw>T2yA3VZjJaK~CiEcWQAE$kQ5)QL#Kqfot^KDvWY84N6e1Hh0 z(u^Wc{syPueM_;_ow6W>Q%6rS^C$MWuceVBfkjuQ5Lxp2ovPoE|4pPczfeTxvD56Uo4 zY`DXGazHtEE!?rmcxRM%C(TqZB7}VWtK+o}Z9$uW{`fws96cw^erDUJh?`U^j@af* zLbziyqCI94rM^sjtOT3=j>8{&HC4B8$Cf9Dk&^WKIsYvx1MIDjJow%wH)t!U2)<7& zMX#f6RXIJ;tViBWH9g6527esj+@xj_IohuPZ9^&^Gy2t~>#2l3ozQJ+nGR{yzp!nf zj}h_-^QeP7*o|OCA01G#{dqk=H&Em;bp-6s-gr$47ovtfDTxI@8dEm4>)+*GVdcEP(d8ALo3ihL@V+5h}@Pr z{UhQMynR6JplYFGT5V`V6zCA*J!Y(K6{~#<_OvZN1wEuXa?ln@pidCg`S^??urIGN z_(Ua0q`UXs*84VE4qFoL`1A>p^lNCmtP!934O|lLhiCk*BF16FsP^dGJb0Ir^+ZEb zKCumxeoz zB6>YFAvUbPQPA+eH0eQgcjYh4Lw?k3f-|Mr{Z;f@nX!DFUu9)G>cVgkqr2d`^N-rSO?5 z!b#{}9`reqP#xb#+~yux3UWty=H}~jRgP2Q4g#I_vM}tF_}qS-Ry93Iko{_dM}p2E z3H5y~jQ>lMaYhZ}g6T6AbQUQJ@;X)jm*kvPlQ}u=<)AN+6mRMcpC=!hurJh7PZENH z(1dl{4*C*FsO)u#JVP7yrP{{j(CrN9+%qJ?r|Nr7ea?wUxC#0SiSVW|_f%i&RR=z` zVqd8n|42igRpE~FzNf*bayYMYBC6-(bR@q4x&V9^-zPU?58g}q%#GLuRg}{a7YMqD zG*t959X>Z<7ggC+;eLo_@9!E|9SOJuGQ3Q{|FZ{nNxjR3(yJcmGSXAa%QXF8ny<^M zE+;0g4d@CI;@cVWq4Bz+nmsr}yPOVp91`#~67Y2GXX>T;3{BV9swW##6xi#?gLZ?yMGBs7RKlEWHnd zZX!`nH*%yz{a;dZQ+>jzi9Z9n<@-tUq2;=zKDSeVlZ2Kl;Rfh7Qt))04JwakXu58z z>s$&6_dwtIo(7-l?RV-fr@@_Zb-3dW(%?l4{;YkjJL-7^^?Yy>c^&k<&q?s96uwtE zI0>!G9t?N| zJ5aCUsM9;86s!rSVmZJ5*HNo2wI4rvBj>jdj#ks!2R$YKoOk|pRJ76{8S2`FaQsn; z+b{LO3WA~Lmu~-Elg;OjtpSHQyS3$Ec*`+0ssUkIr zi;Kuqj!UfbBQjM+O`=morkeOQ>!uQ!YL+I^%_c&QZm@0#k*S>DdX9;%5Rs{t|H8WU zM5fxTNpxF@a5n#B-EbmPy{AcZV~I>P&e4I|o{2=Jnx;u~(}_&gGac)C6Pap&CeaNf zGL$oShfdMaAC9x@^tId_9P*Kt?-r4%8szgF6PHFrrs`Udb#X+dO4KB}BqCFdFTuKr zM5dalNp!P_Om!lVb!Ul8by<_>z9ur&yOmj2gUD3DnnYKJ$W#a4W!+&Sq(zhHP7;~w z)!MAfPGqWFnnahI$W)y|Sl5-vR0*0y*NwO`=;&WU3XdS+|DBRHi1; zZ6Y$%oX#D7^KUn!=W1ySCMUAXG$K>Y(IhT&iA=S$7wc9LnQDV3(QPC`3*DD>9}$^q zt0vKHBf^z6fOT_;Otn~(=wb$XeiLM>LnB%DDG^#>O`n0PKYKA7!%_Ks5 zHkoyEh)lImljs%^q5Yi3x-3MddPS4yUL`VBnc1u>Ph_epnnYKX2x(csx+O%W`b(4O z91A_a2{P68wXEAqWU5a!iS96wp}zf~1Kz0IzvEiD>N0%DE{e!hFK7~%7m3g+Zeraa zB2x|5B)SnqrmDYxfL% z=osso5TSH6iS7uIsdk-W-CiP79oA&)h)i|u9P4fnnd&=DqPs(6sxNP_?gEjizR@JQ zYec5X{2l9FAi`CyNp!CgnQGqmtXoKAs%4r)x10!f*q>O}i^x>{HHmHjk*Qw(m37&O zOqElU=yDO^Jltd5ZX#11&?LHpM5apqgLU1BOw~t|==u_w>fE2K`-;d^mo$m)GLfmO z-e+A6B2xuv5?wHnp*E(A=$n0QsrKuo77t4w;gFB?JVp_j>a-?tIYVTs9a&kon+T<( zNpuH^OjRfw>xvMWs)Q!dF`>OK$GUk$rYaTaIVQR?M1~q#Kcea8{427pliapxz%JoL zrs}LoTw;k#)uJ`)S`nElOq1xsiO^GR$GWXV7|Uo9-7X?i6=~19qC_YqO`>~~2>qo9 z)*T`;)lp5NJ4R%xTQ1h!A;QR8ljwdXLhqt8>*f*Rj;={`ONh{Z4rAR$B2#VCB)X4@ zOqF*W>+%y}bgfBrMTkt*YBKBE5t*u^CecL_;a)X^b%Tg-uhJyC5k$C_=CZCEk*QKN ziLM8cp^7evz^u&BSi4_0cq6+EC&E2klep|8GS!G3tQ$>)p1&s1O(ZhaZ@XCcJCUjW z)FisUiA?qWUe?VZGSys7qGN*Y0PAKFnQFc!(JdlEzwR*WCJ>owswUA*Bf=g180(f2 znQEmb(XApfRg;sfYf5CQ-kL<$hsad*uClHf5oRl{d5(#02@ziDdnW>~^j#8aj}1Rf z-;rAA<3t#DJnuOsE}syYYEl-~O(jBWs!4P+i7$Vb^>PJnY``; zB_fP(HHoeok*VSfvo4XyRNXa+t_Km?&tj}wMuhfLljznFnW{@5>)b?$r^${-gjS;> z>qZfw#%U7Wcp}_a-eKKwB2#^)NpznR8ERCej<_?|7_waM%z3M@OFkk~71Sgyg@`cH zsKL6IiO|c_B)Z&0=!ev0T~Q+3VKs^FO(Ijh)rfVKh)h*Yljy1wnW|)S)|DnQRe4RK zt3YI`9&K3HhX`|BnnX8*2(v~JtZPJMs!&a$Yes}Qfhg83CotcuO`v;BQn(4Q5`WSaQ=g(Qj1?2%`UGJp*Cs~ms~^`JH5xcoMK0#iLjcRnRR7|Ockg}bmfUm<)4*x#fdP7 zq)Bw8i7=+j!@3zn81-lp-2x(0?at4-y+oL)(ImPSU zt{V~NN;HYCI}xtBH(6Jg2v?mZ(KRH(8eb{a#So$9Xc8ThsTv2ct_hK;nrjjrlc_3| zWnE<=v<#X=SDgqWhCtSpCNfo7O`pmgE*jJP2juByfC5Uxj5n*0mljtrJVg98h>+TVuPozn7e-Rn#R!Zc) zyZ-%`+2iT$BiUsSk*Pk>Brb=E(4I|U-6A5);%E}xaw5z+ywAFLB2x|3B)VZlm<3MMMQ0aF^1_lP+F1AGOaoKjW%WFhf zE6^k^`G~MixtDdX5}7K8Cegh{WU8O{vF;ZlQ{B@fIws5=9%Nl5BCO+S65YE*SO@xq zb;XEq57#8R5=0oyA7))L5!O>RiLN&hYR6I5#So!(XcAo<5%v!rXI&K{+~YKft~!ya zTAyTH8zQW$X%byH5$=tju`YnfROK{@E|3T_E~ipmtzPfnBQb`xPX?FQ@qCc^0RTTbV5L>N!sWZgX?jFvP>JSN=7ZnN$f z5#}c}iS85;YVjS`eMy8-q$bf_Br?^QA6Yk+2s;fmiEa{+sV4u-x@kn%U8G5LvxqSF zbeDCxiO^@zB)S4bSS$F0b=!!r0OY~B8&s-y68M46QQ51NpyXQ4E0G}*U=+K{FenuxfFiig}Kr1 zKC%eCnK|s)oCv)HP2w3wWGKfq7v`+i{9apJGTmaA7l|;hp-Eg`Co)v=veDQ*{9>k> z;*#`sH07^55yo+v#HAmRq0aS-F41oMA5}(9wQ>|Zie2gxVGrGC&oObCO@tjNhoh@y zK0D{au_=~Iy(8?>kO=#_G>J=dBCM6?>x6RIQtlIRxl^JO<>*Huw2GR<<#!@O9gpwS zIXe3D5qrg@{2+FDhX`XJP2w_`$WZ+^b(%F}hTki@#l^jiT@s0~o})=zdJ-AxO1jQj z&n{YQZWEV{SvymXJ|e<>MU%K}BQn(1W}T6imiIP`%U|K_;%J{r=(}kWmkdOPN?2f} z<&W7L#bv_sRNW>unFwpin#5%$k)f)6-}&p_*}o3|KwNVF$u9YbuzN$3xD+8W)TC-L zb+7dPW8nsIsn;Zi%B2wzt`|+>(wqqU=iiH27jos*8|%eo&_s3_PK15pn#5%+k)eXW zh}nDB|80lua_?Jq`IE?0>2G?DiOcguhB{Nc3tT>Jz0S^2i7uQYBJ_|niAx(IL#^o1 z<%>lLW&gBYW({PQxkOk?)+8=VhzvDlUl%`z<6=p>bXOi@mo-GD`aqMoY$U=M@8zzP zhv7uH+iMctdqjpBl%p%IkY=^4@-);79lLJHKdkCpJ4Xk*u;(X4*!`_ZJWmlBD#QA& zYE)S8R2HuQ4vnU~7fEC?PCo9I?^>3GF+A2xQ_Z8zPNB4-Z^Y(4eF*`>@ z*oiQVb$y61@2yF6Lx~JEcSKzFwIR9wS}AFnb21Jo%hYkXq^0*|_UuoD^;AvbIfBSg zL51Vt^7^FS;_}v;@sy)?h%lDXBres740Y2Te|k|u(LMFW`Ro#Bh`1Cxz%C_-uxnhCxRfI@R8m&=klRgu-&0FmhGcV7j^eVX63&Arap_59 zsIXS<-FN*LzEN9T_D7`ZHmOgEP#ZOg%Sj?by|dh%ZEeWwM}oy=#|P}PhX`vZn#AQW zk)gWZb)z=!{M2^2kv@TPbejnK6*Y;=&qSC#2ui>!k9(IfBrO@6uuE1V>^#yWF4>3- zwPRv}f4lKDyVVny?E*p14i6A}8fn#9FTWT=UKySb}oz7`rPE{%q>ODGZUN1DW?4N*3A zb#%8S=^N&+QZ$1D?L@;9-EKA6+-g#D@htK=d-@Y$#X*yJ1`rvlPwvDuAy;1R(O6t& z6-lK0%_qXn6;0x@jL1;`?V8wRbN;DA?6mYwV3&bJSohT=E~AMImApOCx{~j;mb9GQ z$u6gfOm$w9xLhPck26!!xIK5L{HKGsTz)x;a&(;tyU{d>%TGjxy3{=B@R1|&8{3P^ zsW5gqON13$P2zHu$WWn6laQmcyTipLaVxuYCqnO6lei2fGE~nyNpG|pKRczZxE%VK zU5*l&>T^xva+=6cdn+g78a!RPow%eAN~UtjL}aR$G>OYAMChYTX5AJd?9u(v{!xC|vSRPE%H%e}L|SkNxrImzrYp9nQTlejP$D$nkecH{SacF1;Vb%0&k z5n)zZlelywGE|NiyW^Shl7HDQ{c~A=o){nL%?u>ME(}fLGMvazmpgRFQ{PSd+AhyU zvr7gd?55BpE-w%nD(BknIo5{s`mbHO^*?5p#zfe?uSr~55@Gi49_uO-nW}~+(bXiv zx&5R2l7y-?ms`;dRXDgu!C|+{er~r6O@ez+xilxjNMDn9h7lR6_xn9&&zMorVY^gX zz%EsZOckO@TgQ`cunu%&y6sZ^4!hJQ!Yq|0aj8dSs4nGt{_fwdc|*H&U#!-X za`Xxj=GHWcOD-bJLl0nGcOuL)XcApNBHSAX_I$J5_>1YSE5yK4Q#~;j-P*)%85VrW zo=b_at3{J|t|vnKlCRhB+f8nDvfDam@m`dp;zXvZs7YMjAu`mM*j^~zQ~4sKWhkD^ zE&)VX_thjWm5B^>YHP1MO*Vfsx}&(9-^MPNiA?paCULn%WT*?7dcU?dveol}juUcEe~Amqa2%jd-sQ_9qA3vR$T+XP22oSTofmE(?ha)%Yt*_i?GtlA~-F z*(DDV%0-i1E<}bp;@=m~g_P?SBQ9f0^`#t5C&Kd*~jf%g>*%%RM5DMKy`beIi4x%-s)f5}#1jPRr#&{U}FQ ziA;4zlem0OWT=X+ewgj8`DzzQ%N`fI>?bnS5l!N9jL1-bZ0Lu1+F!5R=ct9rF0F}7 z)n1dhbRaU+SAX|Ixy;FFpQG%V`csZxC&J34CUMC}ggM;$tjk4&H)?1Sos-B=@7M3| zijEF8tmuY1Frz<4v_JH*)3RkQdu}H()qYLld639Z#&`YE_YNM?NlJIkPwcXR2{-)P?hw)AF~?L?;9r%7D)6XE&B?Uu`tVfHwn*;#gJO@w}(CUNOVgteC! z1}#dc+P{N+ht2=eAj(leBJ2~;Brg6$hWfDGAk5{}-frCyOm!-XUCt6=e56TSE)Zdk zWd-Yki0}-!Cebw{GE~%xLGNZhTlHtFJPp?Xnp z<;q~JkM)?Jqr*`<3b&q&JA86xZt(IhSxiLg(g9qV=z zVfUgY(H$nj9Vl$%%8)B(YT0+@I_ue`ArY>5P2$pm$WZTX7+JL4`1i6{y#%bi+#eYk z9i4u+ouj_bjiMY4Ai|u7Ch;6jgjqjhRGGlQ%DL^7y;6@|auDGO1Wn?Smk2Aw(?+3A z9}luysYVOfr701fc-JH@t%(d3a%0rr_wR>Jx5tgwe`1%LM5g*dleqjuWT@Ekqj4vh zyVrJES9LU%%SIyf4mF7j6W(MrX!O}d2{(VRT{?|omo7xuC$33cx)EXAe~fh>5Mixc zljuGsGSs1Cqp@G(z;ZkKkOE^U52J{%D@l{MOd`VmmqKIkHl~)d?Vd-^_%YapAAHEZ zt1XCU&&5P|K1Y*yt|Y>Jdg~ah?N0o|?$=e`$1YWgOjT2pxYQ!T%t?m#uyVfq3wzF^ zK&JO7e}#z9XV4@r{zRr4-I8^ah_L%xlj!CU8ER_l_eR`qlC`fD9cfwm-mxP`YFDsx zlztg|Dk9A9Y7)-W9wvgA*8Sx$s!q&11lIwFkKs*lAk(o8*U7e}M9 zR4(a=Fms|wT(S@us`@y~W$#CJ=^mKIE}szLDOXM6a-0b7==q9ue-UAa#(B>%(J3NB zWjj9>y{F01cIn$iLest8oxRJu)V+9^OzsTvFBzY?B~-YE<1?~Rpo>6m_5(7#qKpHZfBS7 zM3~joBrg4kumkgN)@8V#N_eV4ljw318Or~@6}{yZD?09R^(SEO$T+_+5a#@wOrTO) zON6&`Y7)#9lei?3hA1dXWfc zT9dfsAj0}|&=jOh6)>b+zHZ1a*NM>1YZ8|`L`cizDcFN`xmq1@nK+qUrV(L&SChER zBf`AYwJDg9kNM4ZdHou@R3bv3L6f)y6Jb;yI2Ac6dBo1q-gl=`xf~$EoT?^qIYwlt z@A_N1U$fe~Vag6=mx@F<@0!G=1`$@Fj!Z?%x2-_~DVI9u*rg#6&a@_RX+eavJXIc%|Kgl+imSpGSul_Gce9M^`>1LKk3b$$B3{Rs7X9e6B%mfp&2+wlRMkB zar{wsnMQ=&-=qZuB2)-;dHYW6z94*cG8kJYOKfs>1wP8uZkw|TuOu;kwa!<{iy7l_VsdM2)kS*!tARiak)i=-I=FmuMWBLeP`Qc%2{@K zp9t?X)Fdu*i43*CIR`WOjVjq~UCZKgC`WCHOckX`TwFwWQn%|ItU^tkW~U{o54-dx zGSxs$;xdQ`Pl4`WT^Azs*)@qSkqCL%G3WgmGkz*>TT?3x`n?0W=!t<$`#4`&KMwqqdMMp=E4zb4por334xpX1I zyNfi5O9~NYh9=HijM?)NcDXd2$S%!^@WviZ;?j-?`%N#*Lr=BL8aqeLFS1K(B8;as ziAxj_+TG&w!^iIlE!7OAgx$mC=TnZ#6Jh6yCUL1uWT^9f=3~BYa$);=S=f(VmJ;C| ze44~%4UwT-2j-&(zv+zK*3CP?E{lk8|I;KcD~Sx%F6RO)W_8$Wm+mR&0?N@BM7U3D z5|=APcxy)F0_+W~xXI4Zxkz@oM1&{CG>OY~B9zNV3$U|zSA;#vYQLLZqKI&nYZ4bG zLz#|+n8j%_&OXy^vn-?>btJ+YMm32`43VjR31!`1M5fBn%yUe18Hv!_ZnY3=J%9JI z=NXRAx6;z-54&`~S;3ySh_FvplX(6_gy&`MEX02H+fD2oz4;rvlp(@jK+z;FZxLaC z-8+kduJj(a!@gebR$WBp@*g5RbMUU`n7A+*D*Kp4ZN~3ue!#8)J14NqULx#7*CZ~7 ziST^Y7mIMcoQ|{4bmmL!@*)w&jhe*eH6lY@FSr=jOZF(cbYtFJOgV}p!ryVwBrZur z*f*NUx-mp}|EDI=O(rtb`ozWf<3G*X*e&K~yB1@P?8}SxIcj&5Jv$It&xd=CiDzdb zL*;v633BwsD%+*#OG_w6#fk7_sU~qLLxlClwo9-!aqkyTE7gfz$`hHYvLUE)|I2qDfq;5E*LL?@Mqe$sA$3G|RA*a@3LtyE-+AOBj)%TGd&K zb&_+>+cS`p8?wuEBJ?~oiOXCfL*1FY6mzP{)9m@Wfa&Z~mI%8SHHpjHMA+|reQA}< zXEPf1+~J_x>@t+dRHHPB%V;8;qrhd@9ew3f+a>2a%cxxP5@GFAlejP$s`B7v`G(!z zdDC`TF`Qji6X9(zn#AQpBCKH^U4}L9MM<97&@=3^oCs?~n#5%T5q9b2T8?LT?q9Rd z(dvB5DMuTK@J=U9;=_B*M(BCh?p?WT4+~dd7h8b{J4YFRSb;q!&V%+j+VKN> z?jgdvwKR$6AtKCfRbPo5{S|DN%e)RNsa%#2q5q;uT-Fd_H`;qEQI9g+w9BQ@Bz6fU z!t*kk#H9@pX7VqtL?3hbK6}mP>Lqr$L4-Y!n#ARMB0TL_VO8;VYs?Pi&T@iII??w6NDa1jnzvNs`{^AA}+fU*ySh@b~0-cmup0ZD!Xv?hL9`! zUO6K!##VM|PK0--XcCtpM27n4KdZ0x&K?`|o4DL~ZVlz=4w0$uY7&>^X;W=7O;?j@^cl17M zC+)errri(2od|D!9OgMDF6oF2HTLthc=Nz7Q|)uq>JqzjCBmD_G>OYHB0K?G zVBOW;+0!T3<#N8{I?B-vBJ7{gBrZP^;d$TQtjk4&_i|_wT`?lGii_5v=8XC4Ie7;| zg%U=US-_D!+?T z9ZG~J%`{2$SR%Y9Y6R;l5aA7@nnV{&gr}H-ccVNTpZ^Kbu@0MQPo}er@<)C+&q~Yo zm_3x1BSiR{HJZfp0ukPpIgE8nh_J6qljyb*p(j^yFLtmm@7!d*6@8?8FGZhDgp_F# zmt91L`t#0S^pl?p{a9Su-)EOtBJ6L~Brg4k@K?7A?xW}i5#IJz)N@R9$wa2Q*o1Z8 z6QMRXW!()TL!~s|hbQ6&+$pxe%KL~>>@t-I^U0bd`UgaK)A9t?{Z53Q<3x`B0TJHo zw~Tdrh|sUoB=Ig11?3!kU|);L2Y0_!v6tuiw#&@=e`j4~9@zJ1aQfFLl|5X|8KUwX z)U*>ob{ zpQpwWgj*3?PqAvDP-sDoaO@z12-0~cgQW#Ez zYy7{gGl+1tXcApG5oXz6-fyQ(=JD?lp{0Apb4+xKC@5$3*Y}@THT`C~LlfkL|X ztg9NozQ5OjNC|d zj{fDuqt1Hke;s{LZfGOcHRhb|Cc+yGn{ZBZ64h1jtxL^mh?-7>`Hpq_@pp#HU5%4g z=)ii8aDoWEX-#sTuM=U<+5y(RM1&`wG>I-B5uRy1$+`wa*v+a*bZv<6_oBXH-EJb> z%{7VcI1%1i@eS*~BEmJUNp!WZd43aw_gTz5io4|71LvWX@aBYV$1uNH;DTeJ)pFO# zdz|VPsK5uOW- zIgXj^u|tN*St*#pF2#wkw^ft4j3dG`J%d=+ng}h8Ced{v!kWbx)~zQ(Z(5V+wh-aj zjtQ(gMTAvXO``jf2xoda>%JgDzf_axE)!u-^K8~lA;KBcB)VBdczfzv*5x9?^Msm2 zcaR9Xt=At%&H1Bv=0#RrS+tp5mJy*oXcCvTMA*5%lXYu}&@?KH7ko?-luK5xamUFM7#RLGsW@Cas{<$PlSPiMUTKMGHR&NF6OZd`C&9%ZOu?MSnOaM}|Koux* p(h4YjvgN8QQC$@kF2}6n@Qf#_qufMz>t6Ut>;d_!T7^Z`{s)tYlH>pY literal 0 HcmV?d00001 diff --git a/logs/FRC_TBD_25d35e33ade850f9.wpilog b/logs/FRC_TBD_25d35e33ade850f9.wpilog deleted file mode 100644 index 536a67e3eebb08df4797cd349b8bdc3764e556c4..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 111340 zcmb@P2Y6J)*Ty#jG8Cl>h>8mWf*_$NC>kMj2wZxvA=!jLNWx7*3q^X7rXWZaL7Ma? zT_E({1?e3WQKX5L@4aVs%&ULeF5mj(kzY3FJu`D>=AJ2cZsR)DYu2vn3UJ}$cD))h zrJ(#<0Xur)<3l4o@z3Jl@d8r3Wiy{4y)9vUp*!wHn3Piw>VFV_N)J^{Xb%< z(&np_-D~f5HFT6y!^|;tvSO60hIaJSkMo2@MMQg^*nLWq22s&kXE_ckb@eI1E-Sxw_GFx4RBd~O1@GVS-p;(V;Oh9y_pFY(hOxbZlsNU_0|K z=WBmlINj$Hx^Vh%A6?X}^;JPvR=9AgmDK+5^t9% z^KSVPP%?iLhrcONvP`k!_>X=c7i%ZvD_gq6J0;2#b#6upv0<^%tvh?{c%(O9*%HM| zzK#1%ozBY(y0RhFPHT_v7N6kh*dVH7O3zeWXUN>FoC;PmHnLV|4DOmX5)mPl0Vzm; zRqKS-iSf?eEUfVBgRAxa(o9S#CGx6Z-#|-3=Fr?B<)(U;huV) zPKloQ1Yav}_~b%`rGE{4w4=DFI8QY0ztv;HJzc%NeCk_V4Q3WA?8*Tz<~I7TETIFT zT==FORw%Hr+~giuShyD@c>bRXD+rF9yWIl|E5*^%7gn?731#HQlwV(#|3pvqas4}c z0vjaSF@bfeR|@P<$7zS*v5Dx2pQKxSQO$`R42uHQ6|GB7mkdL3y6oS+H-;@~9c)>_s&t$y=y5%DG=sbCSwT81H;mA=k~8 z;5(<%DCB9ko(EK`B=zqwfSG(_Ji_1xi=q6Ij`EAR3<6= zf~x1b_eb7ZZrL2<$_H-+@#A#+|EaB2qCC;zfyHpQbK<*bC+_mG!_OB`Wor zLq2X-tLJwHxeDOxM;2uj8%61^>4`{qvt+ohDJbB|yfet{YMmRN94{tpdf-LVp6W$K zw)4kTLD$QUtCp|9mE*?A*h8;ch0w4L{^sd*R{_USt733e=y9bdj{MD1Ay*(goqtm41*n828zCUE~0*9&+YGn zoV>pEq@H6U6Qe_IKl5Ew{e3#llS$s@^52qg7mG219f&!c&<-BkU9V0p--^4Knhgier^j-*YqtaB zgoY*Y^MDKezwX@OQ4tXy8zYmb&}iR$2dZse-{pJ5chpIcQ^86-(s#Acc(;X2yQ_p^ zf+HrE^~aA ze5RZ<;|Kc;wmbf-!*Ul2_e2K97xwM4zNHGoi}Q&a1fLO0L2rW|+;JJ7++4kWTb1cmlH)4#Lt7TTAkSV z1UDwO;7{s`Z~lUm3m%-0lxCxhAg9A|nFDD5*v$Kvb#Pe-2BOGdG)H-=ZEFt z(^Ci({K(8!NJQ(a-z5r*GXLC;-Oe1MZ`U(ey`HL^9!icW=XP1WbCh#cfMaLA>cQ93 z*U63wsxvgbA9xrGs`zLRJ`TPss!#vrVG5|yqdf%sR!WuBq{ zRPE6oe4M4Lrgm2I*46~%p@#i&IahUfaMlJ;TLaOzc>--`UcmX6uOo9;rQy)40WD|g z=N~;kmV#Y1V&|1}g+RrbjdzsvJeC<5x|{e`J6tv1o?p&Y^Wl0;eblR50eZEdm+4>W z!(&LpfJ%h(0)o6c^}i zJqZaI62yB;w~{9`ylrTVkG^WFkAEn)GS7f(byrS7oocfHp6kH#gS-0&d#+c}xAL!} z=KbJpi*w*JY$bfwg->VP`@g;JgMHR+T_0|p_}LfN7&iDf$x&ZzHF+>Z3H}byx3HueM|0vJqT9cmnHzF8!!ojnruP z(I1p>{O6#?Kqnl1{3VtWb}E0-%`#KzLtR|Xdyo6*V&IUC>5}bP?2vqthm8rb$Q?5)wDLeR)??B*Gc%{BJ_^G-7akd{lfwV6CW_sE&yp-IV~&eiC;= ztWzY;e3Nf2zNs1uuky)^G&J7Sp}eaZFfV={lLlW`2b-x45SLGO$JJg?bKsM>@-fh8 zuJ#yL&d8(jQBaE~bme2j(L#OO!P}Ias3~Os`e0W}xN_F+QGkKGpQ5uT+UJm?&J_r@ zf|%Km!-7#Up8I6$qmJxjLse> z13ni+z5CD&eTZ&?Ou+e--nLn4hq&58*O^Io3gW-R#*__(PqRMOHsD?O!Q!fo|AoPo zciH$+?tEV)3wyqI?U4GiUD90<)57r>aJpJ#Y!c!Mhs)O=pTnpa-JPVtpFZ*ay>Rsg zJa|8t(8$y-#N`2cr=cE^#c)qVXkxU_TbADJ8sdt0A_uW?&YDZSQ`__|(kEL3bTtd^ z7UGJ8m3Qg+5qZ#c+Uaz)hgb2h|7!TY@zoj;eaOS>yns$3=5eCN;>a%|A2f|3ST)_%&~I*?fjMZi>lR9zNlKwaqRUJjC7`W za>yO|pF(!G4d~uGBmiwN(}y9hcCdYdHt74pV7v6`KMYCPhKwc1eq%yhQE-!jJ|?76_4>gTYF4gAoYj)xm~L)Awj{O> zxWB;Z#2Q{)9^&c%FHT*5K#r=%Bp?t8^dn0hQlDKOQolj%I#}Yr2`ABT;C`?ZQI8MQ zzIZ$NTkr7gzuk$q zz+^G0N+R|yM!PHOd$5zSCm$8Zs7N^Seo!NR1H}R#((_|3QWh$mc~EC+()TJDoBG$) z=6a=G6XJ@4GdF%7P?Fy3rG5E!z2Z_ov&QSC<Z+)NSp%20}J8jLFMSZXnGmYFOTu+1bb|jeq)F$ z0p1?cADU|7XCp`mP|!!W`x5?__;@!`l0L7QC#n}A?|ft}wOIjBXV61#;3iMH6n*yA zS-s}<=B%RHszb8(3f}1Lj#sfnuGU>aU67K(_<6+j^fp4jE@&6k*|>DtU(3Fru5kC5 zb}uFES4Y!TeaH@54Fz?B!zZeN`lLm@$-NS z=`1k$(S;_Zo}L_1rLuDeuRk9idO-b^2Yc{#p>X6||Jft;T;rnott}z0o{)c7aPDNn zlMok+=-;0e@Sf@yIB-6)`q$*Sts$;npoiREoidY^Xn%b5QW>ExpRM1xUE#Svy@3z8 z$vVDxO{{?(6^Xs_p73%$-qm{t21C0hqXe* z#M^uM?y;|`z#iLJpnmZ7#=|{w@8vEO+A+@8dBA?Ert#_YnxPFr{o$|3!+q-8g||2H z%V~erfSopK4H^K4?uR?Y_4H@Ocz|jlp6=X_b#{j|?gbk77*A#|laDQ8U_g(pApw|5 zodh3);G^7wbMwzm$neByPkdm?F4;iu7An8D!VFRqCA0ABlx56J&|v)2*$(r7rup!M z_??2m>Pv45Hr7RP_gMrQ0`E=_j0Me)x{U?HrM;o%@R9$Iz#TE&N`g)c;FrhN=~unwTq~kB|~SFOnZym_AaMy(!pOw>)Ot+ieSR4MPgNGw%N#mp#0i z$dZVEgS25P0$#lzq+tkXxc_PJD_|V125}lkA)OIeIr2U}LNKs{kz(VVy33K1C9o9g9!ebafJ*q7VC2o#%4slx};7YwS}b!ms8#R;7ho?*|=e zcF;JWFP*u!`TE_9jZ@kFm4=d_&-_n=U*+(bD$Z$0C<_{oG<<-c$JdB|p6Yq(PHen- zm($TP7&HNCDC1)~{O-agsPfxG0^rTnJP1wW!x~7yL{JSM6Y&2TflXA^xlp>*0Zl@B zD*BkF|CjD-lB&&#>DU}J842<43i;7_O;%0+xk9+f#6}_kQ;>is>pxSf>Qi)GQ&a?( zL2M_`=YA)_uLk?MisK~2^a4#q5}qs$PALW12;*1qHC6TeR|*D$zVJ5%ewD!&Y7nO& z@l((=q~OU$jww$FK6S4(O^xP+#7qKB_pK=Sj_y1lpADLUL_OKakrMU)lA0N67N;iW7-**d7s-#FYoJ$H3w<%p$C83m}`y- zfK%@WH<65>ul!DeU#0Ms%D_ozI(ScrYc7)Do#y_}9wP6`dvz?l%LXVPXV>Ni4A>JA z;I5JO?E@j^3HSoJS{?{-9`Db-VBh*1v$lqm8FFO36p1tYR4`Y&|M}MGSTSzG*zWaQ z9sl{ZoUsjXzIDQ;Lb>Ap=UcIr-l=-74-foP_KpdE+_hBcga3T%4pHTZYO3-?ma1?t zV{X0jn>ZtKKU-Dg=r5Q3VE$KJ|&A|RifXi5Z5C3 zd;Kv!>tWM@><f1uOtL3L~vDhaUKJJTLq^81!e9o40FILluT#*^hg}9dB%M$o` zRJ}>bq0Xlt&xb5YUF=+lb1)J%;n#)RrGB^#_5ESNrK&odIUkwCZWj$&1}cdkUveDn ziHX$bggyD@m1U~E*EgIY+XFz$@lRh%8>eacT9cQj?SC%hwaOLVdAHPCc1e;TCP*aDn;R(IXP6q*OnA|4{lca?8e8|v_k!E z@`l#5QvE^XihbsMh-(#+@Q}#ipI-RiIZes4AFXOt>eT1Gtt$2X=Q6v{_`OzXDr1AAq{Jh27j-DAFXX|+PBV!yjDd%h@?Z1 zf^|<(3jVaZb*hHLk0Ut{sV4Uf$iocSK`Y{)Pb`TAGD^EJF}gf};S9)8m(5$I3AvoZ5p zsLSVH0Z%f1n~YySy4x%f_*4N;a@x(Pxc>w^2jFuPd_FqhDG`_w4*1$lw>$$En|*TO zV;8+ioj2u)E_$=NK;(*i=0b>T3*0~Y7R)*HJK))p`q>L6;K>ZPTm5kBN0+=+Wr8#3 zBdcERa)7ph9v$%fZ;t)xinpm3yuRTO0Z$RocKp-V()ObZ-kvu91qygd!}AV5a_vvo zyFusj8a1p*8LL7r9~wT?}#UMiL%< ztJQTYCCo|rmmj_J?$kprnt>?)_+8d(_8p>U^Yi#Z3n71wA_8d3Xu<)5`X$ zNf$Zb`4VZ^hcx(m75r#z`_fLoNCD4Wq+tJ3l!8C4Zom4ioW^gD;MYYSD& z@LYkGPZU*w9sjqi@Z{iAQeoI}3@o@1%+ znyxwWPq_Wg54V1F$=|6z(DdO;uaUQV=2D33IOx#<&;RDwpRV}03V=872LVrd&Af#;Kc6Xeq76a%7Et;xH;{!8y{QKDV4|M z4Xx?4dX>l(TL5$hNqF?FR@beRFel|-e)P^~Qonx51UyCI{_In@_oH>4RYfjQ>xwIh zB%DJM9v$#JyafDdW#?3hOC0dLk2IWriem7iwVhA<9$MQ!GdvZLf(uVk3jVaZ3o001 zy&w0%fd05H2nM$pkzl5%5fet4nb6=zyoxQw2Pi0w%)? z@-+?ryZqRE!I|#?&*cnXB47UscxJ=f6?k*w=i#MT%CCUuO6ISiE}wq|Jd5z#tN8V! zyUkKhH^Xx^?P65ie*&Hr@cBJ_K04qj^)vxbGF<%NlM5fa=s$ z*Wmuqw_whp-vQ6H)O$_W9C;XSfAqtxA6@c~>X6Q{-d)q~Jm@-HKRV#~-yHkX6<=59 zv~T!BV~P9$^phXi_M;2_DeYAbcy7Y;&wk|EpRV_3^^3{11U$ciZov2BTJBQ@Jb%H> zFFw2Ru{GUL_f6i=ntu5gxniHa9OAl(Bs@Cc(RC{&%t`r|AHDO<)M+o9faf{5zx5RE z{b*gcR7N<(N4KuHY@pk~M+ZC)F9CmA*=?2eG6y^_Aq{t)q8R*WZFkbXc$os8yhy>_ zrziz~THRgc)>i_4UG7T(B`${qATS8VFMq{v9v$!?Fz|gz>sM9&vI%%f;UV}Og`8(a z@WC^q@#-ZaTfJsT9*{3UWUF5S+3psRt?n5TyWfZ`RqgGX&+JR;AUlX`)uJ4$v?j7u zgdtIhB(l|*>TEZT$W{{#iQOb3Th$C_yZS`7YHCRAni1J*Yb@LCBC^#!Lt?j|$W|fo zY*(AeR`m^uT>~Op-A`n@fX*pot2Bnhj>%TPewgx?@#+sETe5{Ycp&5)>cC$d$k5p4Gkk*(f0Bz8eWwi+^u?S>NBYJ?%NW3ts7quH(q zk*!J^5<4ba{V~0a^+U{YyF+{eSU`XsH5?QMCk(x1AU$(uq^~#^Da)Su%!H}rjBeK=%fLhe^B@^Lr z7eivVgUD6`)3M!9B3q3zBz7MY+3I#yw)>sPR?lSfekXRRh%8k)cdfXcZ4$k?4SShY zJVdr?Z%9-+5TSnNWxFy&Xq$$_j>%R<3$R^rB4`>CySIsK^+#d0`%43B0|(`NbH&sS?cIaYxa;o`+8gd z$l0tiiU?QJkf<=(>ifBD_X8248AD>nWUDF**sdxOqAo*XSChzA`4_QW0U}!!HY9dT zw)%20+sz=d)oepzH-`xKjumW|hR9Yu4T)VZB3o5h&2|-uY*p2e*fH7a!*y)ehsaj_ z4T;?VBJ@(5+0L~kh0xm=61!)KY!$aRJrU~Xakl$`2=&vD*!@gotDUFVZV!>I4jB@= z!$h|F`5fEbB0_{@NbK$rSt|Nht8tNtf!@~mazJg0bY3O0RRKey!epy+&$8VGB3oTC zBz9McY&9c=c>cby3RpdqpQg~(RRbFCJ0@G%ud!Vsk*&HL61yZK+&2rd-DgCIlMRU-ldXEc!*+v+aGy3LcEgD*wYGfi zi|u9&@z#P-!L0HLk*!{>;QdZi+(d|ws<7QYBGg?&Vt1GbwV)c?%_p+eB12-gn8;S6 zYp~rIB3msmBz6mlY~`-YcCQg(WMfF|-XucA63%vRB3o5ABz7T0wwfQ!c1wwDwbGE- zts=r)S1j9gAhK1AA+d`kvenx*+m#`*)q94-?tLO#?MPs|y+pP;Xh`gsY!%y??c#`R z6>muF5{NJ(*p=;mCqndONbLS5veoonY&Vn0R&xxA-B(1mD%*$c>J!sQYnnQAXYNI0s{w|@ZXl7ZPARrKM`WvuhQ#g?5#qrN zY}c0v{tSs7ldUp6&vsdeY?a-R*u6kxt0`I8ZWh0bAX1#ZVi#G+7x2DC?fQAhQuy`2(yzV>&z+NWwf`pcKv`=dJ@^Hk0DX% zOJu9OmD#QUk*$gt61$>AxGHtnE<2H}avKu6mx$2w)n~g{BD7mWVi!+@7_2ed?IN<( z2}5FciU_6aVLJ~I?s$g8E|$nv{W`GSU?N+MG$eLRwi?%w?IsY}YO*1*W5RekmhI{g zVNS=8*fH7aLLA#&Cc^rIA+ftggw-pX?Isb~>T^S4$7HJp32fJp2)&yjv1>|XtBT#& zt}>CWsu>cy>O_{B)1%Jqg=@!pYx#t}tTKfNZQ78i%ptPX?$6ln0FkYZ8xlJv+(oCc z-8~}26Nbd@Zz5a0G>h$CCBnGhkk}OEqHX^J(8WOvlL>S|3VY`Jyh_eid-BKb; z?b%kRbj+}O@e_AD2masM!78_jFmqx^RPGUBw6u%ub`sfYk0G&RvegTF*e(Z=t#TO> zyBCQt0^7@WhlsEWWk~Ff5@C14ezvPcgx?2;(C|V%LKR*C9RIH6X$c8$)8p zgb_n-w!1`x7{!p-T_dv8iZXR~@7}#P`u3;g(5hm~vq}OH#xjOPg$Xl>mD%n*5ymTq z#O@N2tzNInc7=!#85n+UzehirF&2fJ5W zqIXH1mR@P=Q;zx(p(Yy=m7zqIT2-n(UhM5I6)I_I(~wncB3pGaBr08rEY)m)V|Ti9 zYf;%Vja3d1A?h+DDomEzxW9gjk;#FVn~O^4pID_k5$-aEM1{#xw_k1WW8IZ&k2e*S z7lRv6xwwfiDzD)EPE?u^S*l6b20gMbsJ^eUsOSLLM;}5 zrPC4=!z$&8uq(ols8k`c)E6@wW?xXXK*!pWmM=E5$}A$R;208>1w@vbe#cSix2dwI z+%haU!`Fer1)HiLfSYNK^_ES!(8+jbB($^^0*w zMCH@q##Am7iEK5^kf=;2vQ$vF#&5J3x#Y8BqB3C|t4t-r{lSo^%qFr_mtBqB1Fofc z?Ubl&Kfx+{h%m}BBr1o9ELAjDlbLZr1K-ywnTj=`9Azc4RZc^ql8eYv#XL>+?cV+T z2CXu9G^>mtvej5aqB4%iQd^fc`K?fkaTQNXj(*?4D)))d8h3iX6BQ;)9dNdU9J^TBq&xy*RFjhH6g#N{lsGKIURN5&`+l)+} z(M#uO!V*@QLWH>lL!!cDsk>L3c8G|`_xTA)OSb2mQI2vGVV`(r?{}i|2a%=bmu;5m z=yX%-HZ^6H?L?S&YsMOfm5|yDuxKCedd9P55H>Wib zl~?{|m3%}PzZ()2Cba3|txzs+<*zF$XREcMa=AcctE+}Y<$EGal}l&l0;;yzJ^4F32WCcw=SRlXpl>%Wl-7H zl%wH9w)(`7sEj7ERLgd)CyyV$rfO|TOXnV}(t`+ViiUJrh%A+6b?bzn^gX*a(4~8v zRW1`@_Q#N@TqCkn<1}q9cF+FIU-d<$W1cpYqc|e$m@p(Norx^GBh+^dLgrH6$wih%8m6P}?O{&$-rWl@c}DQn{2S z!aA2BQK?2`saLx>DmCI_Bu4{2W|fbKuzqVuR7MfuxxuY%TaQeh{A{eKq&m(jiU>2o zhD0S35$=sy!_Z@vFV;y^(v}XR96d*bT4zX9@)KF=ov^U@p!6*-bQG10iL8>12($c# zM1{#xBNl}%s(P+M<@Tb|brY-fB*Ok4L!vT($Wn#=4x2cB{N%ckqLMpLIOQlm5uRQ% zBr1i8EY&6?9OHm5M|ec#Y)e+TK!iCKL!xq($WniP9*%j2)3bDad1f)Iq#?pgz9CUz zveeBV!&l}Rkhi^7S(C~`Ia*JIy$6OwWebs|g5UP!8E~!RdpbvF8nMbHBJAupBq~1= zS?aAJo&neDuFlw2$|d!5R!L8UnNmZdl9?!*dTWN~_4}tHa~F8l<+fDGW1do_O4Z$^ zHP78-&5K01#~Bh$CiKFuMqqyF>X0x=*_00=sFc1S!uq=*QDL%FrY;d1?-Xi#EL2q9 z9mOi;h%mY~Bq~*iEH!d-M2S+Reml}iRA!!Jm3c(AT4qR8mJ?ZO+;fp%#08Byr0)n5 zOGHwRrV=4XhD2o!k)`@Jja+}H&>~B##3iyyJQ3C=42cSprCQ94Jl#F}{vGLtq5|v{_mKr;z-L$x%!BJY}(o9ylMugqbhD3!4{luAe!;|m4JW{JXdy7>v5ZNlT zAyLUfg!Q90qJF5m@gl6e7&C z8xoZnM3y?aBkKPB`)L-p5S14$vPy0u%=Q`*6*m#mlB0cGQ2OQtboBd0;r5iHSwz?Y zZAet+6Ip6#MEgQ5Movo9r8}c5tIQ^XiXp8+WT{C@+GF0$)vBT7=;TgTIYWd!N`^$` zB9Wz*xjLXtpE}h@RGR1RKskye!hC@tQHdwA)H8KD{C)ras+PK3W<;{eTq5lLHY6&G zi7b^ksl(3QyT_i=SNfNgta6vg*~{VmPE`IRvQ&+09Z+{Wx7D>SM?f^?=tUyj`wfW- zlcm0XFWOp}r|}i7QnneZ1QB7sjUiE~NMxz_!O>Y3R4o&&uXLx`tkQ)DYuAQEr6-Z4 zdLNCxS*XSEoJq zioHyf7e{H?kSc|6_J$!**+zufyOMFqcM83eu7h0ZVRcjN;?)=;Ta7m)Dies%@AiwE zU-ewkF*=^lJDODr5IHAKc)t^sB1D#2zAx_80oUft*5jOzo2=512=i`+M1{#xZ@kiSartxi-f?-QYaF(fJ#i7?ySp%d;$=?ZF`6cKj!84{I?M2I|AcETMt z{Q(_gX4%UsIf$^Y(vYYySt=o&jTm!GlD^VbE}L>xm&jI)42eo(B1=U!vJ18tS$d9E z`MCqD+$6$IE<>Vnj|g}BY4)(>JIB7&Dl?X|%4{O60~r#P1w@wGd(-ZBt?rV>x^zFw z5KlS!lE_wH84{JbM3#CsIKE`5QfqE&l_8<5@-Y!s2Mmb{6HacyqW;P}zs}Pwpu~Ju zDMN%)9t?>Jlck0qkKeU>_vX!dEPCJ;s~jRijA=+zjuBbPRW#x2JOj4;s#RR&6R2EL z6Je#okf@|5!j$z4Tx;j+>ofWAhOhZ+Y``^%7*Frf>%zkNezzBkCX4zv~*fZZ)TOUL^$ulkf@X=vQ&0gXQT^=V(b=R#{Diolb^CC7H-l`^R_AaCG|3_jFpC%x9IBM79bwBr0u* zEOqev&R93RT2f!>pHvsh(JdnE)HWn4zY|%?F5Lwy=dpFP%G?^PvVaJyvW7%uDG^q1 z26VB5(!cwQE%&3dGg##k5oR3>iOP>eh`kSVLFwMhsLSQ{PptAg5zdJ+Br2|-y?+U^ z)DQW(Uci2h`FgJF$MRjNTz)3P?r1}z!epuZFLDz9~9l{biNRosxMlpwOy?611D9GP6z-9=Q^u49$;M3|*ABq~gn+H$vB zjs;aiKk6L!y$C$WmXP?T)>n{nI2#T8{q3Dkq5$c^DFv3q+QhRw60g z(dpHm?BPz3hV3qkq=ot)&%2FarUCq@4vzkeP`WnqG+JkbmhzL8m42jALB1_eY=+P-C z{mAEaeR;JPtK=iXUCofFFj?x(${y%3r(M@|cfdYY8AgPs(+!ErCq(F_QuoBJk0CvD zJDQoRC*|lXB8=?~i3*dYlIr)wm?SAdt1Jm;l~qL8Pish2))QH($n2hYJzAD$b9AIrf1Op9 z5@9E^AyL^!gnMJb-l%oYP1Gt!EA^&wIZcGU0ER^65|O12jOvZK^fxN$E8XN1R%uOy z)oMec(uv4Y9g2U5-BTA9kCW2f|4biB%W)#iRu~eM^F)@axUx@dQ2J|uX=L}*PdixU z4iRRQ42cR8_OR|{yQxIBnqf%nW)fjse3;@6x z~=ckFXL4*k*&5E61%NLIQPfh7vt%yuO&$7W-Qv5u2FU( zoX=oLRPqwx^wBD87f6IXkcPxAgb1Vgp?$G$JF4mg@!n+&tMnkk+NU8=VY1Zo$NJV; znI|^*8&TPPfmMzXAx<_VDrbnWGUD!sk>b}e>qMnzseV)vy@{~KXGm0-utOudALj0p zbF2}SpN6x_Z6efGL!$C0k)<}R>$l-fp}pU%6qVZFu}VWCjH?WZN(&;)Ija6=x@SKg z@wKQVO+JYX-HIxo4>3Um1fgf#Y2R%-wcULHzMpwT*G#65@8RI zA+ck!)QpS$F;1PoOJCb8Sq4xZau8wnw;@rDd>DpQCsYjk43D+8{rTXaoSqVBLtG!dSXG$bla z7;51rD=QA9a#=%!XDSSd3X`Q$^&E)td+!rBMdj3RR=G%oT_%P^)Gdyo$f!TQ6# zm;M%&SMIP%J|gVNFeEC4iLf8~wV_ylxO((gotAfpQjV??;WQCLIxR$&Y8yLr?D+9V zcit0~dmpjN-$b@bJO;lzaW|dh)SeY~=D)WiZOJ({9t20qVgG$rQZGHBj>q-t^s46{qtd2hf$8Ai13u8AyJ7V!p_tx!_e=p zN%f4Vw2ojE4-ro3FeEB5M3y=`dRSycMCPu#1-!A0Rf-eYsWnVkKN^gn zatUadLYUh&Br55NEY&?`#K3EHtF*o^?Won@6uWp8L4?vZBr35)mKyT)2#iy6cGfCq z533Ey&ZQegIl4lG zJuZesr(pH;FF;rt~-qVf_EPRe$TM%`^teSxT~&o!EIbeIVD=@-4< ziAs7RtfmTlFvN@P`5X-|Yxstk!rJdvf| z?l%UrY2}~&T+;I4Oja34glD7;iONVKtQ_nc^H!-+!BvNfO1U3dr4kYLtr-#(ChS2j z@F`X?S{&&sDlY|pO68KD$X0I}5|v^^*oPGJDRSgW(lg|d!&#*h5q1z65|y4rSc6{o zDejGXKGXBbOHQ!LDkAK{FeED5h%i=5Hx~ECUqzCvogAvytJ3damFI|XMx7y1VY1Zbn`05z zwZ5lk$g|}bM>%?#2&VuU5|si(mReY89Cn1PTCYp@a0^!XjtFy=hD7B&k)`^N9ET`q z;?H{ab>AXZIZTA})(nZtNg_PcduAMVgls#cXJ5nrW|j6t*w1H3RBR&b6)W-?#;^m{ z>e8KE{WB_;xkOkCF(fKWi7b^d=`*a$q%NcD%az5fa*YUQIU5p{TSS&Rz5O%npSX0} zyE1u-RbC*%ERG>jd4&jbmD$H*nqgIXotD^=<0(gpL^wahkf`(|vQ%}?`1D7ok8tZb z{|-G^r4tdJM>ixYU5OCaEgp|&S3OFr3^>FpgNU%(z>uggLFJhV^;hP(Hc;1>z`PSE zN2Q3I_kMW46O|w$+|@!REUbENNk(0|rK4EoJtCYSXGl~k5n((%VFFh8-`N}|eO=Ic zR;fsYvx^LgN-ZKhFZ#m-?3h1#PwzJ9nPDR3Xdn^F#gM3sAVTb2b|OZ6$N!3!wEWqC zRZ=%hA?HLCFNsPB!{fm9)y# z(X29y2Ak2CU0EfX2r+{pQAr@O)K_yRcMMA3v}#MK zFFkj%N`E4p_hd*^Mi5~={Ex}Y^9)FvQ>$#sHHC7tod{=Y8xj>J%mr1M^4IXjo|r5X_;4@06-n+WT8jyYIJNKP}hp zKC8qMLB)`$BoX2HzY?EgP2>Gf^xoy{RX(S3d5H*TDHsx!qC|+`dwjkk&w$`0y}!K0 z6jli(!jqncL?wy{`^$HIj#E}zU27&+I>&WZ$xVc301SzWn+WThFHfzvGEY)-eWi<) znMye-NrXKMhD7CEBAk!XZYpA_R8#d6IXwrkN?#&d4K*Yx9}yw0TR9ayX6r+`1$@7Y zRjw1^WJ^P$a+e6F=czAd#0BmDUGFV?EB6;J`4n>A6j+uBC+HXwm0%)EUG{u|-EZ%F zr1wPS@4_mt6XDr-L!uH$g#9*Oe1Y-N)R*+`l+RYO%0wbtePKvcrV&|c(5)|eW?!)5 zCB2s;A=5O=dQ3y2l1yZ&-5*UWm;UIw zzjTbbaT=>^C&C*B4T;KrBJAZjJ?+@^xc5fs`*exFSmj+JJi%p1RH_hRRlU%3#0)vV z*Rgl1;OSH@X^3!dG$bk+iLgVl%XG{$G`pzpuv5pf$_ygxt~Deo3y3V0b@Oz@3|6AP z+s{14D)Wf&G?^h$Sx#iB%sIYnJ~Fw}B3&+NUjLGElz|9u1TrKl*@zIognfw^v%^ds z9ro_ZDt(D?T9F}9VX{<(gU7Dw(p* zpd4i-!inXEL?t(orP7DYScQOhz0OhDa8@Ztg!*DgRH_i+u03%EjToji3$_;M8(a*b70SYt6M;q;jEHGgflP= ziAo-z-td$!Q6(;O5+5J_w>(JI#tv&s!3yraXAsQgNVT}c7+zQtj=D#tv^ zQ793jW<#P9MP#Xgb>^W3^th>GhUX($B?}SG$TB1z$(*;a91-VDqj&{e)s5nT%*!wb#(asT~_&# z2yA~_-N^JoulRD7f`u;LxgjNg1z5~N@*gDgcBBEG_kyyj=d+2WtC|} zwpwgRRF)E9G_iTXp543Ccl}U$hFhmuDp5x zhE-}4;r*$GM5Qqi=G~?*MKm)yl~zgKz$#mbu!d8Szk(t`-|ONK<{BO*M#9k?9p?+ZTFeOCI$%c)$7H%%d1 z1-J2%sB~_dLYC^*WjXE$%Vy}b^d6C77q9Y-Od+gn84{I~L@3>@%dwi&y_PPQ_)95v z@v18k-bi9dRC*Ix>UfT?k(T_|blojq^lQpdWg_eaFeEB9i4Z|}zQzi1n)bREzS*5s zeka22OhcmLO7i|C2yyF@uhEWb+|;G}{B~B!N`%PDkf`J$!dTR`0^{O+C3GCJIL8Xg z(F!8$v@j$p>xeKnT5|I>{q z$Wm6*)rilkWYGQY#%NaALWFnm8WNRVM3|5KYBl!vn0|Ng238qKg#CktL}e@yuF>7q zn0apetnQ_@=UhWM+E0WhJq?Mfh7(!p*JbN)w{LS>&!c4hmQ`{RVMJ?4RPqqvoY^$rVAn_edU^)r z>}%gpj;;`4SD7JExlV*rdFp->g!PWCIxUl;SY;{^o}Du!Dl>^JHTjEg@I=SSvARu% ztY?)vM2HRziArN4=b3|Va3)E?QCcPK^Xn-`&kuh9C$d!d2kWt;z-zHZMOL{(WUJo|iOL^DmI}?h5#zs79rXAwJl{sjQ52D_IvNs{ z7$QrpYqJs0(Y}6H*OyHnvdUH>+(`_H$}S>H#m?J^z0$`^>$Du-%_^sfu)D#Ks9Yqn z)bD?7jEaZ|Kc}yB-K#qYWtAi%oNQu9RQeKG%6&2U!0z3%E9icAsoF$2T0w-9a}9~g8X}wp z{MII%Zak=vZjCeQvdU~C%%d0*mH9-Lda?f|oHKhOmyW%MO=gubL^x5(kf=-`vecM; zPFjB3s8#k{W0eC$xZ4{Nm19JfdLiFttTPX;uJ88Wm)uO{a)St~_=ZH~E)k;U=*>A7 zR1GSuBagi!S>-ShPKh%lDkq36HF~v^mQkzpJZ+wRtdft&Rs{`-%Iidyic7l%djY-* z(<*4N3-yjz@|Vi&IxiLhtSkf`(^a?XO; z^>q|9Vl%6ZA;LS`42jAFB8>QMZowV4eY{o~nsqDXXc!UR#BE4aJ|@C>qZPMeJUw-^ zR;kv4RYHjH)-6M#QillhZX>s1E@=0cy1rbP$12|wVeQ3`sQf~Nvna)t9M0jgO1MhdDlA6dl;}eIL?yi(c&kz)z&MGB{ z@I0O&Q7J=&li7~!#60Hup<1QH9aedV2)lU=iOL5=7#|hbg}Cls8NKhRX}MigF0F}d z1H~mOE=APdniZgh!DXW5|zwE zP^q#9ceNJ^>a+~^u*w)B#4mU!xN9S*0`)W^oLO%KJpvmvG)m z%b`iSH4grRRfZDb(agR@_x$EA4Iltz2f~&R04<)H87mFh%T5i=wz77_MHe6b&GdgpGPme6&q;vuqCdqbkqfyg-vYCpz}*&67cp=PE7 zl%qOCm}f8~DvgQo{O0=ypc3?oj>vwf%_=_;q3#+Im0Lu3mUQR=%#mHnr0dIj3t1(Y z28^*gLmgb3%}84{I}M2N`pAN+25T%S_<8jY%ckjiBY z5#AhaNK_^gVHaZJK|JYlDVlc=B*N)dhD4z5?Ly&^`XR|^mo42^WLozSS5_eR*{B8 zC5i~=r7t|RHqU^bWp&-nvXfP^6XDGwhD7B>B1^6O{Sd~m-GX(kyYa9&CJBN)Rrwe&U0o$Uza=v5+|31moA3KC(gR_X|9-RsA63;3xHtK1^ONh5|t z)A-TrQ<9;RFsU*IDHt5oYoYiOO*z zOYM90TcoA`c0G&pewlBnT!M+vGZ+$;szjLGiu)EP)=em;X9)5QVU^d3@GOQQQ7KA< zJ)Ud7Ma-~xjIMRh9b=WuMA$oGNK|qVVQnJaQS3w?`Jq;sllLg)Xg(31Ff=48ONr1+ zH9d-$VSgsAQlkT_)FQ&$F%5}I10qXJopTg>jaGc6XOwR5V3psAu$pT~R9ri~e+jZw zzuQN#KD{-ot}hp}9itpwBf@j@hD7BTBJ8#cK8DjdyI$AzC4CE4d7cO})`mnSI}zTZ zH1Zh6u*I(Gw5<4&RaO(>+#f@tvYrSr=GkMo()IW1`r`SURicP+>WLvyi6g?ZJ8yi4 zHy3qTrsobb*8GmjB|8zGn>QpXFA?G7h@|iET*&)3bdFX}V3iF-II+%zn<=%7{9X;#TVg!h^l5|u1O7?tNZjb+$OSAmdN9H ziegejtx{|NtCS?d{EHz`d6x)t@QaRPuDMBso;hi@k5yU|;Ver-qT(UK4vl9|pqJ`B zQ}>u_bDy9bB@@|dyCG58L4*-s-4p0{Z`INF#-xs{(vt{J-53&;eni-bKJ5g~6BwcN zJs{{CRtY9TUuQ^EsuJNo{o@JDR-{_0Rr04hNjWM?gdLHFM5Qzl_7J>#5-KOM>T+37 zlU0@xVa45$sH`Kxywpc0vn{Ba<#$~!nI^GHb|O5{VMtVR6XDI#ha8m-hxA;~^`BYg z77@TxT$g^XxY8~5X_Yj~Q|#hZ1|rN>7!sAtM7THJJA-}xIn_YP(WF0EWhxQY zgbj(xOd_1|QtvF4LL5+*m-nW0oD*K5LgBcQ)<3!jUz2_{> z_&k|&mgFJJWmd^SgtufE5|vkoFoJ&R9G=bH({GcgoGx{aa&&fqOy$$ccA*`@yyQ;?`#m2#6(uGthSJMNRfOAe> z+Ot}6RN^+Plp(_GvLR6kBEs`(Z(PJ`#O+#ZM5Rc%i&QQph;VwBAyFw!gmvxC7ctWv z(tL%e^#6!e1{2{KF+-v3uJNE;HBeni;Yl=%|&pB0*> z%O&vLOO&Ixi7@|ZNL1b@!favaB}Ch0_v>1hzB{XACc=owkf`J!!n*e2OW1pmt*&kX zv-Ysc0wRov42jB8BAiM2%w>$L8qU(Ku}tpEl%pUboabRkR4NhSDW=+&u?k$QknS1& zXu~Scgr<O1~zw-Ri>BZM* zm5N`oN);mP9yTN@HHk1karg>GA#wIL$x-&3tdf%mcM?N7M?^U5>eZ`wJME<$J4K~< z@KwrDX(F^~L!uHyggbNG)n3^b^xnHiRDK`9D)))-+{8%lccPMp$WqPLTt&Iu7_dWB z{yNMmsg9%&W|s|#N;)Duua@rn#Z}K;UbFT8sygd9uZr%E??=xqf@A-Y^`S1PUzF&9ddt>Iz znKNRk9(jdYRFeohe8VJ`dPF#Dqw^JHV9qS~omkSvvn3-Dp2jeVB|8!NWz7}byQOB@ zdw1{wwuFdq?}kY%qlp54;QcEmo^{GTM=R1_r53Ft!cO@xiDfep;-8JKBF=YZhP__~ z^>PUUYAM9Cdf9$?wt+3lHvT^$CnHQ^Nlka=+w1%a$TUI2A5TVkt?4^JsHk$8R*S*Fmu? zD0iJ&w3sO5zaJ*CEGI%%(tzve-KhOPiDmffY#B|2Jq}?K%OoOXgMNA)@0Vs5?K6Gv zSGGJO!ZZEb|Nc!ZDTt6&o$Lnke&Zh>l50s@@CLOg6A>~y!X%cQMCg}RH}HPRQODl9 zQ3Kghj0jcm!X%cmM2MGva0BPRA3AE^b#?Z!r9Kfd7{Vl$CPaaMpLZiE#dVn8b2{2&eGWyovX2y@s}T7e}*YIZ?=86(+H)Cc>$cuir#0wpk~8EonBg zB@>Z3h39|&CYBsTsQ+^2CL%8XWw-ZBp1W)*K!kXCn8Z?qDDYbpzlEMimfyB0zM3KY zPjYxdM5xjqCb5hnLfx$qSVoBSG8~orC1xI5+7Y3iYM8{*l?bz}Z*F0vTCu);j=G;` zOK&2aqZuZ##1Ub?MTXnRL5|tJUaqA}>D$zzSR&*>gh?#@iLf`f`)x#YH;tPumLwzD zl9DLorwx->(h(sqZ_{nF_kZj=VoA53Eg6YWsW41p$wq|d=<#jL__mFiBbF~S+@Tij zCPFQmFo|U^k;w+VQ?%7zNKb~?@GVaY|EB9L^ww? zOk!z3gwqu#-bMb|rV+MxuPU>{`)fxmOzJ?yu?{PAM3Jm1HbWZ(WjVH6Z5v7{!# ze*1s#Mem3_Q!i001v1~K78NDJN?Mr2Qi=$t{nowTyI;S^@$ZS{Ne{NXAi{S#_J98- zmgGd(wK4lXB1#9|w9iqBO>9X;6!O!DNh}$O@C5vRALm`~-eRACxsTbhkO=3Kgh?## z6QLGOi3bfXj@|OBz3Xx|dO-b>mk7I*!z7l1M5x3u{K5W1he~~J`z6m}wiF~n4sw{p zQj`d%An$*0bk(pt+ii<-TxLsdB2;?|lUVW-1%71Khp6k@1t(v}Fj%)=y>u0)6~Z+?iouF$CIVww39TiziG`SZgh zmW4#f2K63Iy%uw*+uLHvT;LJ4Cli__}?L?l=37`sFDR>Q*%W-@l0^2@#&7*Nr9OytOn*WJ@$r$Zs1av9u#X zO@yQWA)=JNi0zl9581MU2=VeTiDeB@;1?f)|wtaOMdD6ud^kJ2=VzaiKPS)^2B#PMlOH-jn)uiCb2vx!s)G@p5P0+yO{0WGDF!?fe0D9VG>JKBJ2wL_zCK? zc`?>fc@JBv5h4B(Cb85eLLS6_Pmobp%C{{_n&&CCC>0UbSHdKgOhkcyr@>R?LA3ws z4f%~4bY@EvB4pHsNh|?T;7^(R6nPLE+uCa>u$e7IiSShmlUPa-Ap`T`Q`Dw^TEecj z4EdKW!-%lAEKFh*@~cZrZs6eh9!M}++O9M2I8`Y5kyQ5OG1 zrRP7+9vQAJ@@wVi)V|Y1A^$>{#Cee@@K^MIj;yQkN9|j9=v20hBEk-tFo|V65q`1H zo@1YIl~(qJwph9VW3fAwu=$Tj391f~ID;?EdMpW}x6x9!mI8pm#goiZ5wG;4xfa=&2M|F}B zTV@nRb)GTsvlQ{XdZ?UJ5>Fab1?zj@ zrDH^)J4I1tX2c-@dp)nf^lSaU$2|#E>OUP-dd85SMNw2)84(M}oWykduD!N_J&Gpb z1`c3EX?;aeB{1S~>z>3kaP=p+$8l8?M>(o#jHoxND5{x^I3sII5)VOLKib}+4SsS| z%@}d$i=wDv7*Q7DNfHm0rS5FQHw0IeByUn~U~)$Eq@t+OG9pIYBB_V6Ta$jVeU&24 zQKe=?I)$RBGBaYBxiG1R@4%gV$UneUrTolMrD23h(W)3xLFr0T4=M1Ud^bZ>OOhw! z27bU8^4BPeY8@jsF;!0H;oEr^M(pNR6jgpk?B$!9%)^jo>}b4v(ZF;o z9aTm~1c($xm5mYU{hN6v6m;~9|`rYNdujL7ualiWke;9)ngMFgsb zXB|~jMij786jd}M3JIo4Vfrfl6=X0$wW?SOZr~b5)Q3?N)doiF|LU5;LouK(CG3L} zG0IUTXGB>-MNy??MDdVyDLm|%+FE<6Tvgefj;azPR-_e0Rhu#JYduWiAuyKxS9@R0 zOOcWrn8=7F2}Mz@V8rIB<|#eYsm*o4Ue)9-j_Nf=R5n%=)htHDAKy>up@hxWyY_{= zWsRfylo7=`6-D(WBL>cwQ+k*?_MUBDGI#Ggsz;2d9H%I%=Zt~Bt)%ZQOicVLg?(@; zHSxKxsxyZCmlZ`-j}Zq2PVl|sh&oI{9vSYdNAn%kGe*2)7li*+RLK~T+kDXX);GS` z>l#w^pvrb7{2v#qivdPnMtpD;MOBCqN89I4<>92tuhQDb`TdfqxPdDf@#<3))mp~D zf7&;dhmsm4{t08ZmkZUZUXE%#BkEu(ifSt( zBL7QLdk9%}tR528!QGDP2qQ*cilREsh!E=4)aGtKc4L^RS|mxs4Q$1Tol1(LYR?$> zKUPa)x;>(fRmFF3RKppOY^5lw$&7(taaI}+t2Og`TUDC1jw%Zya$6Nem76i}zdxSF z!}3v<^+*#(xBv6Eqq@n6O;n1a`j-(&V1?3p2T^C@8M3^gno}n&_tiW`?DErMts^7MKzi+@K5bc>tO{aQ_K)iH9h61S~4Owq$sL(jL4u$ zpU&IZ_~L7)`->`f{&d{HNJcE4D2l2uBW}^|={&4XrTNre)v0lg>H;IqtXCA(-;Bu6 zU!TrH8LnML2gy}kIP9qYWW?{QD5@)rSW0}D&cjOW^y33XwJ1Y+Zr~C|*c6AF^2r^ilT~TL>ORRdNUT?SiX;_wr_D%yBM(wrzonujHr)s zDZPgcJzbyl71frA4BWtNj3|bpD5_nIf#0`W1`m~zwxsH5RWXk0Ge&$;6-D(WBPyUx z$>1TWZu7EUqMAP6QN6*4BbXFLHJcIT!4GBdu)24$bT?5Q`^!y{v9l~+ zMl+Hr_=ml!WHmE#U!`V5kW5ijnHZ7qIv}HYa84Gmebsb^ql#w4GpH!4c8oZE>x+yY z5~E9Gq@Xj_M>M79kZyb&fIc|6Y~JL&cK;ee6qS-fl;=m@(w9P!!clMpWXzoykLx zv-@KE;1tZ5nHyM)5#`?$MOBs&)9{*^y|gD*&HK`-#Jd1}-SKEf# zo8jGBS-7tjF=E`VD5~X*D0MnIi|O{!b?mFJ@nT2Sk`eJMMNze3#EF>uvY07A!YSMB z9sY7uof)xIqbRCaMl>*URudeM)2Z-v-cunfH}D%qRM}D#)j>u?;CpBFuwgn!Py3lT zI^0qH%7~&zilREph|R&9vU;dkRB4g@4h}o&s75g2tTshajbp@BMP&1EKvbj7*ph_? zPRg5&8~7R{9%n^S&0@r6ffm_3MC9XEV9yU!=Lb2eKN#_KRut9WjM)D_Kb!e@onJCo zRAYBIs)>vkw=0V3HO9cNb19n%4*XNi_Eo0mjw(ANR&x|ZRe%vEL{`Y|;jrPuW#Z(j zPPfX=eRY8`N;6_Ry`rcpGGf_aXAW~;6`F3x?OT6yR67{)ger>aYeozp)8sUL^)$J?syxMV zasvx6Vv$8rR7DvP3hS2BLk9eeF7~RjjCWKy7_m03D5^-t!0)s!r-uXBC)UNrJ#_n} zy^iV%BMSE_is}|4=7ay{G=0^)u$>R)PLqop7|DpiwxXz_7}4zwbD6MV?}7Gl?$g^* z4PgxV35uc`!5H}Y=jJjEoI2WGRi4d`svsk3Eh~zu7$Z)iJD1Dr->=`~Q?{>uyyK`2 zF`|^IqNt8BqVzNBFk(**h3(Ic#?-yBCZlo8LM zqNqkOVln7oZZk3YevN%0zkc0Oy~&6QHHxB|%ZM7}IrEs0SJUt8)UssdJlw$Yj95HY z6jfD53?K*O@lYLR+e!Q2Y@gz&zGTE%F^Z!4h7tLj+wz!~%q!p9I77A*jw*@~DS3*b zs?CVSmx#RPCoea!IwLab6h+mD5pU7fdCh~f?VMFz9q6cTGh#cIqNpA+ zVr$H@yk;z#_OAWVfBvv_#HxO;~EOmvON#|`Ykh=oc;QT1iS z2JYthJY+`1=d%s`b&#Vv!-zT_ap8Xz)qTbQhq&gGxaeVfi#Gfq{2xoKuPGx+11gFt zhB5G$p2}xt^o{Di zUjLGZebHrpu)~k{_Bg6UMoa+|MYWs}Rg|BK>h=hGRV&lw=LW7}#4?AXsMa%L#iU7o z5Boev{A2s-LFRhd>es%(tdi=81mODifSDrGSojT5xI_vqB_8cQvmBkc__pgUFv_`-qBHQXGFIvifT7w;Qu}+%6y$Oma{j*+_jEs zA!Ep2swk@W88KZt6=iOQpDWr2=gp^%Y7S$_U#KXmMT~e~6))^z4@iURHgsRIeqrvb za*X(RDT=BJBk~zW7xu6TyX;LnR6DTHQT@z_LlYH6^&2Br;=V8JVFi0ij2)`2KJTd3 zGlu-lilW-Wh_GRfA|8_d!s|Gb$`#=TPGb!DGZaPj1|!N#^(taU$m{ahZm%)bQPp9@ zvb&8)| zs3JZM|HnY=>A6Jd zimECjcEv;$H=oqka@hIc@~XwTuU0c+8;PQ*K4J{~97BqmSl`q}cJBP`Y)ADYW61wW zQB*%O2LAD##m#r{l^Ism=A5JI!id>~qNw^YqUcMS5+;z6`hZnc%3p#TSe+4jYZXOR zhY|JUyOl7jOS3*g!68IHj*W3trx|fTv7)H{U<~|;>q>a2#*}&%YEnX#Y>%Vz8IioC zD5~^~*pqj^gn0({b+v2su~|!U1N$DUE$LVG7Hv?vG&is*BN7)C zMb(-Sr_m26?X}wxIi{Qa1@lgIR0SBZtx-`_B^dEZ-CEi-@a7Yny>sC=M|GJo?HCax?^wpeKKEef#3vA+-RtE3-0 zW}gxJ;S|N3kr4~)zm@fnTz@^sOi|sw>ZtB9qU4&QsGcxl_))Z+NfGQ+)ZXnSUn$3Z zRgMv3QAJTzVZ=_dgmPx6mSUm(4t_YtQEgzvFQ_Q0PZ$G#@OR}r)bly{lYJqty6C9Z zGUCmkD5?#N*i)9Ny!X=dYw_RP&qUJl<+*`r7?H)QD5`9X_~P^`Z{iGd=i9ev;R%ka zBqNqN6h&2uG4NmBRNiDd?dW8$s_Ah@)shhtUPV#0VZ^qCr{z7I;J3Dg?W^axDsTgn zs9Y2HIS$652AqxzT;_m!fkK4HX+zDh+8M|nc0hl<=+ zl^L-&N>Nm`84;72TG7L1#do{gt6KPhqgujb8sA@6d1gtMAdDxQK|B+QCJ?*G`M!fnIMU{au@N=iCZ0`1P$!zrFLhj1k zz{`wydn=0SIwLBNb*OA^(e!ieRh=E=s4g;^CG7COit2C1z%R49vUzaE9kjF5vR{P% zV+oR1WyA~$|y)%&kl*IS2r$Ss=KvMTr0`-~WiDvIhuM#N;si0W8Fn>6)&mZM7Y zCL!LUilR!*h!6dbqWXN7y%}DB1#tpp7 zi24tTqI$@Pe9_+3ycQS7-ppWC%O^Rim5fNVRut7bMx=6Wu4Z;r9jj)a!6JtpRdGfn zO(}}1EF)qv5!JmyD8v7W4I3_wtj-Nw!H5mTilSP}h|J8E)y-Ai>14x(Cx$qxGmKb# zQ54lhMl66XtM1Lj*4GX8X6UrTQN=Q1?yM-PzKjSPUajuMzO%Q$X8Yi5c;=|KFk%v? zD5~v@rfOpi(^qRd+rCQPss{H}T1MoVD~c*BBNkt#)$njuz?JQ`feV&9s-=t}{{ux) ztz^U&jN>)D%VV?dxMlD5V}CoUQ;bNbR20>DMuc|?*7Sb3Uo6>Fdy9Tuqb4`-dq$kP zt0<~Nj0mI*ujx(By6MJ38^0UPbkr5fcilT~T4E&e+*7DxPnKbL|yL~~(QEg;I zRYpZoea46&`TANOw*NNYX1_QSe{fW=hV?mXa|1Us zVl7qCUKJyz;f-s1*dYDoY^xg6(@{-e4Ea+OMKz5PTYKi!_D~hQRS)}S7_iY%4Q34a z!xTjoVhsF6=WCm=;hi}9@jC3);RYUM#Ni8yqB_Zl^zVvwOqNBi1s&z%wJh3Et!6~n zP*GGLGa`^Ov5tup{W3X5RP&cPszgRC%PNX$IV0ljhw7NMiO;hIqRRGiE1o8jKpr{-p_8lvVss>}=x9tD2S;vV;Zez%YW;&{07;&DZ zqNq+XqT>0Em(5VEcR4#TIeOetonXWWSy5EKGh%aA%DNt^Vek6Q#*oVwsmnJ*Wkw{o zDB1=x;^P%l*F*91nBDfOGQ>Notc=*MqA04|jL5BBRo5){cRpn2I4O2Gs`QN5x}_+p z?2I^*;8tBT*sgxfs$w(O;|BI+#8F0yq8iL-#-jDi@>!EkHaO6xwWI39h*zJYsCqDB zujTA|X18pIAFOK53P<%GBhp9|MYV(xWhGA4GjWD~mF&_$Z`k0##<%Ei6a1}=A^&qlQSD$v z)uX-j&EqTwS|RB4%PB{7k`Y<=ilRElh}ETZ4NQ8{r#J0Y6)4w$8yLli@1UZnN-$#G zxpxB(2ZznQVEZc9L`RjE5&H)fMOBCqF`3N`yv>a-j$UaWoIXb!RU9MU?TVrbF(PE) zH8l6ti`Q{b5c+Cqu7=#e)r`nUQ54mBMohz-H8jipCAZiIXY?RPHHk6gzosav>5Ryr zOKfOWfbt)o#X+zfVzA-!h_H&ZUMX>;B`Fc2M+1vPRs%U5v<8Qxw%cMm)}y z8=1GL9J`FZ8ra@Z#WNxYQc+YR8IiCtrIClq=uP_CK+3iSj_ONBWE(1q>KjIsdibT0 zS;Eeg!oC^4yyd9&G9n246a|5a3#81YHn`HIQ?sQ%FYf2h8R(CSJ7S-BThz4 z+t?&*yp!4vw&xUY%nh8+h_|SssFpCI)Muy0CQ?-Tm>sJ18RDo0GKT!2ilT~V4Ezlr zHug3&zWC!3I~HBD&rz*oL}6k@QEg&G3El^dP1rE@o>l#wsR=jm8YABAilVy982DN1 zH!;b0*`HWdr&veTn-L9E6jcHvLKd@|m~6wPm#nJCYDd+V5ua2=QN=N0zw@~!X4__5 zydBm~c;Tp~GGfD#qNrvtB7Rk}sTYq^cA8k#!G=w_uZ}PxMNm;xzcL2?pJSSOICQrC zKs(`mzQj=_UrLDLq>7?S&xq;L4^2(Hef=W4cd787^tTCq2}Ya|uPCbWjHug}qnU?8 z9BP!ZW6=i{n{fl5FoyhyO5uMMRT4&QQtsc(?05doVS{LDNd}~8e?Byy_2@!1`c2h`9l;%HIxy10Bf}HP++p+78?x7 z7C5S$jM&_vD5^+C6eXS2!pn%N>JMyR%~;{6-ekln(u$&bml1nverw^qx%d9Nd8{he zO-B{Uh|L{}qAJXYUFQW`dL7@{`%N;dYEZ2u_f=Cy6em#>RWu`3(-K;mo8k5i+gAy1 zII1y>C?BLKs!5C}1N&7=uMw)7-?dj&^Q@z)!-x|h6h+mD5l?s8z{Jc)gzQzVEgNtH zH!va?qA03Q8Bwyddtg35i|*P7=k@W9>P<%M$WRp3T*km3^l@O4rFVtwo3Q$ij;anL z8mK6$hKx91>q%fX8khUazMltXiRK0lXT%;rMNy4qL^!Kyv{@9WQ`j~zroW@=z=-40 z6-CvZG4Q`x5bfbCju!pw`|7|3NA(jUQhgLfb(|43Vg86Vxx{;#*+~BWCywf8M$F0; zMRkr5@2d)}OoGa#2liEeDQLxgb(JyX-%=FS9Y%ynC$}8SEEVpo)+sERQn`7n2knLVXjWmOaE#Bc+rF(O{4D5|#@ zaa>?rj2Y#&h_yGvgsF~dDkJKBDT-IJOPCKeJj3|etD5}hiIB+Rx zYqKGAN)Nk3ze?`b+`w9lIB!EyRP`B=7u2S;xvwHl+jw2R1V>eX5rya#MOB0m2iUJ} zZL%vby=I@m>3ba2EJjrJP!!c%Mx0A^r?p8E`?`l6Fwe-)h8y@cBk~v(MKzxh=~%Ve znB>C&tL!g0yt|_s$B0{0QB+eIvGVwK81W;%Nre4BSw@QR1{TnMl2VeYGWc8 zSvT5eu*OYCRhto6yNaS}#2EN{i?ua@qt_?dtNOfoTkfk}j3_FoD60L8NF*51)9D|F)%tUe>J!G0zg$r{E&T& zy{g(}+i?RMGGe(S+Rh}X+-z$< zsY~`bs%4BJf2E?RRx#p*?6o&nHEXaPs&#v*JvXp7BO(ckqKae0mXq%7O%1DG^4K_V z-GPqk6-Ml&RuolBMr_Yn-rhrr^Xtzg85{&susEu1-7-3E^8 zr0l>Atj~!3M~b3q!ie!z0V7h3(sc5+M^686qgAaf)QKDT86(13ilW-bh$%qVPTu*kS^Z14 zucAgcsuGNt|0;^A93#?ZKI-JS=Zol8jOaW?Swl_ob zbe*|@(TvzMq$sMkjHv4KN@uf+bAQOj>z?;=R7rXhqQIu2sC-6juzIJniHvnPW}}ET z*3jQ3_%Ac!%t%F1HDSbb>G#fNUNRyH^xNHIot5ILmY~-@!G*>^_k% zn|9&8+QW#5tD>mBWyF~QW4d^o8eiOA%|6Z%OC428Mx?tdiYh&0;Qw~8iz)1RdaG4U zKkumCV8ls>ilTa(5qp-icQxVn3%l$K`9kTg+`vB>QCUk-R96^r=t}>trb1Ng!s(SW))vd3iie*GriK3|bGMWUHZe}F&$9I$E)z@{Gqw2|s zBr!!%^=HKCgEzXFMSOiZ3`im3h#9Myk} z$emUc)pJJV63^^zW~t$;dU?5{s?Uf|s-mcxFe0S9wr++T)DKxTaKzNBQ}I8imD4E zR{!_)FombKoUq;g-UUaM$cWW{MNuth!~imVtcMflW*?X&ef4|6SZ?4&Mua*QMRk=i z@JGhRdN}NGNCA7dPaE&3W-#IpA^)hNsE#q>c#exbO$JVzYWAwiC-21#tj36fv5KOq$B4eF z(98Soez8OO?Mr5V6G!z6Bf4EtR96`T|JZB2OkMG@>!-_o_0J8ow^8rl%ZL8%Det6h+mA5li-| z`gk}#A)>eK_B^@!a0Bx*BKn~ys-ld@b!pefyhV%Ew+;MgprhKvh|DfUQGLdUQwrDi zF&jdcPk%!iIDe<3TEvJWofJj&J|hO(cl(&d@iZsx-JUOHU#EeLXrQ8~iZCL6Rj;oZ z^*#K>-t9~JII5M5NXJqX)yIqoX3p+wW)pWB*&P{8KXg>lj2Ou%imDxB;Q#e|U$cn0 zqm7+S+<54y{$&jL4;4lAh!KaBmFQ<8+Z&48Oo^9Y>BoK5fDs=rMNu_l#4k9mpZ7VA zp8CL!8R8QiRfrKYFGW#}W<*-@5B*Hh=Jpxw7iZUBj%qJs$UmScs)LL;-zal`GXIx%*xXsD?4(c%7;P%}ZuUF{{emZXownK1Kvm6h&2#5gV-D9%!ZjyXM-y+O*nH zea47-6pEtS&4`c4E$45)VZ^VmD1Uo^G4Sg@9%%BfzG`k>P?)8b9Av6eAG>6q!RRW3 zxPe_5QNUbL%zYS9qC9?(Nl=;iik&X)d(TnrXAJp=6h(EI5f`~Jn|{OMXYbpLd(qe`}q5c@JcLQjAW9=o0BZkMB4@?kBa98 zrennJ6-7~HVMMuxcJW@#)F&GCvMVN4;~Z5TMl7r=imD+a?yC>tJsf{IdRnaX)#c9| zRqkzs$jws}Rb|G&FMc!LYj|<&(6RQanr9lu4UAz#q)1Uz9T@{ZrtUBk+5Y{Y?W@XN z9aRlREPyJCsvcwDubMf`G;sKwF4DlBD;(7TM%L?=`m?>n|QjeTzE8X6xbch?+ zhB4$1Rut7xMr@uP6fz(BQ-eE+s{SZP)r1ifUPV#0VnijPz0Tj3Vhs7^73FU$G6sJ7 z??PtGf9!?%hT?eC=OHsfKK$-@xvDF<6S#r588IMNz%a82BH~8D&x% zc6OB4YDeVEskVXNzHn3r7*SbEQB+45Q5d20Xb*?RUyilEV7j`axvw%ahWs3gqRPdH zs~SDpWH?`YZXf6O<~yndjEFNRifSn%ip+mM+I%MJ4Y7Co*)xvn0wW^ZilVyA82DXs zj4>gLPiNUV&YqHExPjj>hWx{dwyziie^&1?W{y*Mot;fA9Pg->F=C^!qNr9fB60E4 zF=mc4@0z{a>mPSiO&IYVR1{S-BR&%^#+W!m+H*G6*E0WDZeR>!$ZxMGst$~(RMTp# zH+S#-3LR_&vilH6)sqn`af+hq&lvd6mW?%uiwpbNtNLb_qxyjn-L5FABaGtoFe zIevy+d}$mpjvLsL5o`2{qH4#8IZn-SWIOj7W%86xAF?Y!#n2&P29T zq_iQ6f=eA$5k{QjtSG8djEMCeA7{Ef_2AcJ_)+?fqbkRUQJOQH@~?{Ee@U_l_JobUflMxvE8PJF2CONSaa<)hb40@9Z6Kb_f67 zeU7NUKJBQ!XGBa!QB;Q+1OG(o31-b}>_6{{YH*@IVy@OiV^9QmnWKa=l7a_B&zqkN!-ANjQC6_it2qvJcA`Bnc+u~Udu#v zwwa^4$cWE`qNpx2VyDl0*E6GlWo6h-CL4FAVK)XI(%TvXTg)v=l3 zf0gLR2}bOEQ54l#M&u#ym~39i`JS7r!daXtrbwy(cP+-Zn_|9_uRdNP|C)T|6gjx0 I9q#A<11A#(ng9R* diff --git a/src/main/deploy/pathplanner/autos/BCA BLUE.auto b/src/main/deploy/pathplanner/autos/BCA BLUE.auto index 24d9090d..e38c6155 100644 --- a/src/main/deploy/pathplanner/autos/BCA BLUE.auto +++ b/src/main/deploy/pathplanner/autos/BCA BLUE.auto @@ -19,7 +19,7 @@ { "type": "path", "data": { - "pathName": "Blue Center to C" + "pathName": "Blue 90 Deg B Shoot to C" } }, { @@ -33,12 +33,6 @@ "data": { "pathName": "Blue Center to A" } - }, - { - "type": "path", - "data": { - "pathName": "Blue A to Center" - } } ] } diff --git a/src/main/deploy/pathplanner/autos/BCA RED.auto b/src/main/deploy/pathplanner/autos/BCA RED.auto index 9f95d67a..a83277ff 100644 --- a/src/main/deploy/pathplanner/autos/BCA RED.auto +++ b/src/main/deploy/pathplanner/autos/BCA RED.auto @@ -19,7 +19,7 @@ { "type": "path", "data": { - "pathName": "Red Center to C" + "pathName": "Red 90 Deg B Shoot to C" } }, { @@ -33,12 +33,6 @@ "data": { "pathName": "Red Center to A" } - }, - { - "type": "path", - "data": { - "pathName": "Red A to Center" - } } ] } diff --git a/src/main/deploy/pathplanner/paths/Blue 90 Deg B Shoot to C.path b/src/main/deploy/pathplanner/paths/Blue 90 Deg B Shoot to C.path index 77a92caf..0406c942 100644 --- a/src/main/deploy/pathplanner/paths/Blue 90 Deg B Shoot to C.path +++ b/src/main/deploy/pathplanner/paths/Blue 90 Deg B Shoot to C.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 1.8887518932445275, - "y": 4.32662701436514 + "x": 1.843395339506453, + "y": 4.143491955138672 }, "isLocked": false, "linkedName": "Blue BCA B Shoot Pose" }, { "anchor": { - "x": 2.6238277281250206, - "y": 4.378717757621604 + "x": 3.1137973538967914, + "y": 4.11421492392836 }, "prevControl": { - "x": 1.5581750644122865, - "y": 4.3785396044875196 + "x": 2.048144690184057, + "y": 4.114036770794276 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue A to Center.path b/src/main/deploy/pathplanner/paths/Blue A to Center.path index 18646e91..56138fbc 100644 --- a/src/main/deploy/pathplanner/paths/Blue A to Center.path +++ b/src/main/deploy/pathplanner/paths/Blue A to Center.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.101440276954628, - "y": 7.133434209888363 + "x": 3.3013338050485626, + "y": 7.2749805200131235 }, "prevControl": null, "nextControl": { - "x": 2.9106477302293583, - "y": 6.9718848238973115 + "x": 3.110541258323293, + "y": 7.113431134022072 }, "isLocked": false, "linkedName": "Blue BCA A" diff --git a/src/main/deploy/pathplanner/paths/Blue A to D.path b/src/main/deploy/pathplanner/paths/Blue A to D.path index 8038c033..078edc50 100644 --- a/src/main/deploy/pathplanner/paths/Blue A to D.path +++ b/src/main/deploy/pathplanner/paths/Blue A to D.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.954672602983599, - "y": 7.029418819812343 + "x": 3.2143199684464845, + "y": 7.113452071956227 }, "prevControl": null, "nextControl": { - "x": 8.522793824407191, - "y": 7.437045490202704 + "x": 8.786290798324133, + "y": 7.476231693120632 }, "isLocked": false, "linkedName": "Blue ADEF A" @@ -20,8 +20,8 @@ "y": 7.429163844438801 }, "prevControl": { - "x": 3.438956606648067, - "y": 7.060221406032563 + "x": 3.2657732528510195, + "y": 7.0862756529426 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue ABC Waypoints.path b/src/main/deploy/pathplanner/paths/Blue ABC Waypoints.path index 2c3f5073..602ea2d8 100644 --- a/src/main/deploy/pathplanner/paths/Blue ABC Waypoints.path +++ b/src/main/deploy/pathplanner/paths/Blue ABC Waypoints.path @@ -3,45 +3,45 @@ "waypoints": [ { "anchor": { - "x": 3.101440276954628, - "y": 7.133434209888363 + "x": 3.3013338050485626, + "y": 7.2749805200131235 }, "prevControl": null, "nextControl": { - "x": 2.8769730857742335, - "y": 7.243500008770597 + "x": 3.076866613868168, + "y": 7.385046318895357 }, "isLocked": false, "linkedName": "Blue BCA A" }, { "anchor": { - "x": 3.1, - "y": 5.55 + "x": 3.090096900059872, + "y": 5.657628859900576 }, "prevControl": { - "x": 2.8749498626139145, - "y": 5.658868892078977 + "x": 2.8650467626737863, + "y": 5.7664977519795535 }, "nextControl": { - "x": 3.3250501373860857, - "y": 5.441131107921023 + "x": 3.3151470374459575, + "y": 5.548759967821599 }, "isLocked": false, "linkedName": "Blue BCA B" }, { "anchor": { - "x": 2.6238277281250206, - "y": 4.378717757621604 + "x": 3.1137973538967914, + "y": 4.11421492392836 }, "prevControl": { - "x": 2.3975266746789083, - "y": 4.484961972566135 + "x": 2.887496300450679, + "y": 4.220459138872892 }, "nextControl": { - "x": 2.850128781571133, - "y": 4.272473542677072 + "x": 3.3400984073429036, + "y": 4.007970708983828 }, "isLocked": false, "linkedName": "Blue BCA C" @@ -96,12 +96,12 @@ }, { "anchor": { - "x": 2.4, - "y": 6.43 + "x": 2.2940885539509175, + "y": 6.475991589445976 }, "prevControl": { - "x": 2.1642311849658524, - "y": 6.346855151347819 + "x": 2.05831973891677, + "y": 6.392846740793796 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue Amp to A.path b/src/main/deploy/pathplanner/paths/Blue Amp to A.path index 2b873f2c..8d952245 100644 --- a/src/main/deploy/pathplanner/paths/Blue Amp to A.path +++ b/src/main/deploy/pathplanner/paths/Blue Amp to A.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.954672602983599, - "y": 7.029418819812343 + "x": 3.2143199684464845, + "y": 7.113452071956227 }, "prevControl": { - "x": 1.5093951667101788, - "y": 6.588627448229724 + "x": 1.7690425321730643, + "y": 6.6726607003736085 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue B to C.path b/src/main/deploy/pathplanner/paths/Blue B to C.path index dafe6f9a..5ae1beb1 100644 --- a/src/main/deploy/pathplanner/paths/Blue B to C.path +++ b/src/main/deploy/pathplanner/paths/Blue B to C.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 2.6938714628917357, - "y": 5.631284339205584 + "x": 3.090096900059872, + "y": 5.657628859900576 }, "prevControl": null, "nextControl": { - "x": 2.0322297871158246, - "y": 5.373357077845108 + "x": 1.7912352437800116, + "y": 5.494793717191601 }, "isLocked": false, - "linkedName": null + "linkedName": "Blue BCA B" }, { "anchor": { - "x": 2.8299207212253714, - "y": 4.58 + "x": 3.1137973538967914, + "y": 4.11421492392836 }, "prevControl": { - "x": 2.3071646664126666, - "y": 4.837775838126081 + "x": 2.395957238900271, + "y": 4.417946095006697 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Blue BCA C" } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/Blue B to Center.path b/src/main/deploy/pathplanner/paths/Blue B to Center.path index 8e828743..2e56bcde 100644 --- a/src/main/deploy/pathplanner/paths/Blue B to Center.path +++ b/src/main/deploy/pathplanner/paths/Blue B to Center.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.1, - "y": 5.55 + "x": 3.090096900059872, + "y": 5.657628859900576 }, "prevControl": null, "nextControl": { - "x": 2.85, - "y": 5.55 + "x": 2.8433488902081527, + "y": 5.617436576633564 }, "isLocked": false, "linkedName": "Blue BCA B" }, { "anchor": { - "x": 1.9507925278871394, - "y": 5.55 + "x": 2.138713570581903, + "y": 5.514320436192957 }, "prevControl": { - "x": 2.200792527887139, - "y": 5.55 + "x": 2.3857289673852144, + "y": 5.552835287122212 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue B to D.path b/src/main/deploy/pathplanner/paths/Blue B to D.path index ba11d6d7..b3752ee9 100644 --- a/src/main/deploy/pathplanner/paths/Blue B to D.path +++ b/src/main/deploy/pathplanner/paths/Blue B to D.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.1, - "y": 5.55 + "x": 3.090096900059872, + "y": 5.657628859900576 }, "prevControl": null, "nextControl": { - "x": 4.09653456185508, - "y": 5.917887071242202 + "x": 4.086631461914951, + "y": 6.025515931142778 }, "isLocked": false, "linkedName": "Blue BCA B" diff --git a/src/main/deploy/pathplanner/paths/Blue B to F.path b/src/main/deploy/pathplanner/paths/Blue B to F.path index 36dbdbb7..23bf1ad3 100644 --- a/src/main/deploy/pathplanner/paths/Blue B to F.path +++ b/src/main/deploy/pathplanner/paths/Blue B to F.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.1, - "y": 5.55 + "x": 3.090096900059872, + "y": 5.657628859900576 }, "prevControl": null, "nextControl": { - "x": 5.044398825776819, - "y": 4.782298508049397 + "x": 5.03449572583669, + "y": 4.889927367949974 }, "isLocked": false, "linkedName": "Blue BCA B" diff --git a/src/main/deploy/pathplanner/paths/Blue C to Shoot Before A.path b/src/main/deploy/pathplanner/paths/Blue C to Shoot Before A.path index 94d21d45..eea1e153 100644 --- a/src/main/deploy/pathplanner/paths/Blue C to Shoot Before A.path +++ b/src/main/deploy/pathplanner/paths/Blue C to Shoot Before A.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.6238277281250206, - "y": 4.378717757621604 + "x": 3.1137973538967914, + "y": 4.11421492392836 }, "prevControl": null, "nextControl": { - "x": 2.1014736029324417, - "y": 4.3270624847953885 + "x": 2.5914432287042124, + "y": 4.062559651102145 }, "isLocked": false, "linkedName": "Blue BCA C" }, { "anchor": { - "x": 2.4, - "y": 6.43 + "x": 2.2940885539509175, + "y": 6.475991589445976 }, "prevControl": { - "x": 2.4487602821359506, - "y": 6.028702057404115 + "x": 2.3428488360868682, + "y": 6.074693646850092 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue Center to A.path b/src/main/deploy/pathplanner/paths/Blue Center to A.path index 4b24808f..727b3008 100644 --- a/src/main/deploy/pathplanner/paths/Blue Center to A.path +++ b/src/main/deploy/pathplanner/paths/Blue Center to A.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.4, - "y": 6.43 + "x": 2.2940885539509175, + "y": 6.475991589445976 }, "prevControl": null, "nextControl": { - "x": 2.8901573716465885, - "y": 6.887841058048188 + "x": 3.176206283465587, + "y": 7.200482584899087 }, "isLocked": false, "linkedName": "Blue BCA C Shoot Pose" }, { "anchor": { - "x": 3.101440276954628, - "y": 7.133434209888363 + "x": 3.3013338050485626, + "y": 7.2749805200131235 }, "prevControl": { - "x": 2.6374369114478973, - "y": 6.684084632617781 + "x": 2.616694679797016, + "y": 6.77157523067328 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue Center to B.path b/src/main/deploy/pathplanner/paths/Blue Center to B.path index b22a760e..02cf07e7 100644 --- a/src/main/deploy/pathplanner/paths/Blue Center to B.path +++ b/src/main/deploy/pathplanner/paths/Blue Center to B.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.359711113629195, - "y": 5.530775383617674 + "x": 2.267677335262537, + "y": 5.592946872771572 }, "isLocked": false, "linkedName": "Blue BCA Starting Pose" }, { "anchor": { - "x": 3.1, - "y": 5.55 + "x": 3.090096900059872, + "y": 5.657628859900576 }, "prevControl": { - "x": 2.393853468522427, - "y": 5.551255318985296 + "x": 2.1856013617617, + "y": 5.5787055761019975 }, "nextControl": null, "isLocked": false, @@ -34,7 +34,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 1.5, - "maxAcceleration": 3.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Blue Center to C.path b/src/main/deploy/pathplanner/paths/Blue Center to C.path index cbc05c12..45d871ce 100644 --- a/src/main/deploy/pathplanner/paths/Blue Center to C.path +++ b/src/main/deploy/pathplanner/paths/Blue Center to C.path @@ -8,32 +8,27 @@ }, "prevControl": null, "nextControl": { - "x": 2.0924424154876817, - "y": 5.333196225699311 + "x": 2.28067836224651, + "y": 5.0083681416224035 }, "isLocked": false, "linkedName": "Blue BCA B Shoot Pose" }, { "anchor": { - "x": 2.6238277281250206, - "y": 4.378717757621604 + "x": 3.1137973538967914, + "y": 4.11421492392836 }, "prevControl": { - "x": 1.758976899669116, - "y": 5.811554088964557 + "x": 2.53011686068628, + "y": 5.129772001745606 }, "nextControl": null, "isLocked": false, "linkedName": "Blue BCA C" } ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": -52.33367496345772 - } - ], + "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], diff --git a/src/main/deploy/pathplanner/paths/Blue F Shoot to A.path b/src/main/deploy/pathplanner/paths/Blue F Shoot to A.path index a8da4d1c..f454a004 100644 --- a/src/main/deploy/pathplanner/paths/Blue F Shoot to A.path +++ b/src/main/deploy/pathplanner/paths/Blue F Shoot to A.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.4, - "y": 6.43 + "x": 2.2940885539509175, + "y": 6.475991589445976 }, "prevControl": { - "x": 3.9604601919295908, - "y": 5.356198902739307 + "x": 3.8545487458805083, + "y": 5.402190492185284 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue FC Shoot to C.path b/src/main/deploy/pathplanner/paths/Blue FC Shoot to C.path index be2ff57e..dc8244ee 100644 --- a/src/main/deploy/pathplanner/paths/Blue FC Shoot to C.path +++ b/src/main/deploy/pathplanner/paths/Blue FC Shoot to C.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.6238277281250206, - "y": 4.378717757621604 + "x": 3.1137973538967914, + "y": 4.11421492392836 }, "prevControl": { - "x": 2.315502052772566, - "y": 4.530128331558909 + "x": 2.8054716785443365, + "y": 4.265625497865665 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Blue Midline Waypoints.path b/src/main/deploy/pathplanner/paths/Blue Midline Waypoints.path index 0c2f87e9..0b9d594b 100644 --- a/src/main/deploy/pathplanner/paths/Blue Midline Waypoints.path +++ b/src/main/deploy/pathplanner/paths/Blue Midline Waypoints.path @@ -69,7 +69,7 @@ }, "prevControl": { "x": 7.862682673242908, - "y": 0.976362387364147 + "y": 0.9763623873641469 }, "nextControl": { "x": 7.414861727999737, @@ -112,16 +112,16 @@ }, { "anchor": { - "x": 2.954672602983599, - "y": 7.029418819812343 + "x": 3.2143199684464845, + "y": 7.113452071956227 }, "prevControl": { - "x": 3.9529715982039386, - "y": 7.541832769355557 + "x": 4.212618963666824, + "y": 7.625866021499442 }, "nextControl": { - "x": 1.956373607763259, - "y": 6.517004870269129 + "x": 2.2160209732261444, + "y": 6.601038122413013 }, "isLocked": false, "linkedName": "Blue ADEF A" diff --git a/src/main/deploy/pathplanner/paths/Red B to C.path b/src/main/deploy/pathplanner/paths/Red B to C.path index 748ef4ff..44769eb8 100644 --- a/src/main/deploy/pathplanner/paths/Red B to C.path +++ b/src/main/deploy/pathplanner/paths/Red B to C.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 2.6938714628917357, - "y": 5.631284339205584 + "x": 3.1, + "y": 5.57 }, "prevControl": null, "nextControl": { - "x": 2.0322297871158246, - "y": 5.373357077845108 + "x": 1.8919980957521443, + "y": 5.607295441711419 }, "isLocked": false, - "linkedName": null + "linkedName": "Red BCA B" }, { "anchor": { - "x": 2.8299207212253714, - "y": 4.58 + "x": 2.6088286579474946, + "y": 4.298674642736191 }, "prevControl": { - "x": 2.0304029772866095, - "y": 5.007284560700074 + "x": 1.6421097406918816, + "y": 4.538467469792007 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Red BCA C" } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/Red C to Shoot Before A.path b/src/main/deploy/pathplanner/paths/Red C to Shoot Before A.path index 867bdb6b..2d8c2412 100644 --- a/src/main/deploy/pathplanner/paths/Red C to Shoot Before A.path +++ b/src/main/deploy/pathplanner/paths/Red C to Shoot Before A.path @@ -25,7 +25,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "Red BCA C Shoot Pose" + "linkedName": null } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/Red Center to B.path b/src/main/deploy/pathplanner/paths/Red Center to B.path index 2c905a8d..e9ba4363 100644 --- a/src/main/deploy/pathplanner/paths/Red Center to B.path +++ b/src/main/deploy/pathplanner/paths/Red Center to B.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.5, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -48,5 +48,5 @@ "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index cb66e368..9e6a03fe 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -298,10 +298,10 @@ public void configureAutons() { AutonConfig MOBILITY_RED = new AutonConfig("Mobility", Mobility::new, "Mobility"); // BCA - //AutonConfig BCA_BLUE = new AutonConfig("4 BCA", FourPieceBCA::new, - //"Blue Center to B", "Blue B to Center", "Blue Center to C", "Blue C to Shoot Before A", "Blue Center to A", "Blue A to Center"); - //AutonConfig BCA_RED = new AutonConfig("4 BCA", FourPieceBCA::new, - //"Red Center to B", "Red B to Center", "Red Center to C", "Red C to Shoot Before A", "Red Center to A", "Red A to Center"); + AutonConfig BCA_BLUE = new AutonConfig("4 BCA", FourPieceBCA::new, + "Blue Center to B", "Blue B to Center","Blue 90 Deg B Shoot to C", "Blue C to Shoot Before A", "Blue Center to A"); + AutonConfig BCA_RED = new AutonConfig("4 BCA", FourPieceBCA::new, + "Red Center to B", "Red B to Center", "Red 90 Deg B Shoot to C", "Red C to Shoot Before A", "Red Center to A"); // HGF AutonConfig HGF_BLUE = new AutonConfig("4 HGF", FourPieceHGF::new, @@ -377,8 +377,8 @@ public void configureAutons() { MOBILITY_BLUE.registerBlue(autonChooser); MOBILITY_RED.registerRed(autonChooser); - //BCA_BLUE.registerDefaultBlue(autonChooser); - //BCA_RED.registerDefaultRed(autonChooser); + BCA_BLUE.registerDefaultBlue(autonChooser); + BCA_RED.registerDefaultRed(autonChooser); New_BCA_Blue.registerDefaultBlue(autonChooser); New_BCA_Red.registerDefaultRed(autonChooser); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java b/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java index 1a60a59e..cfdc89e1 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java @@ -38,8 +38,7 @@ public FourPieceBCA(PathPlannerPath... paths) { // Drive to A + Shoot A new IntakeSetAcquire(), - SwerveDrive.getInstance().followPathCommand(paths[4]), - new FollowPathThenShoot(paths[5], true), + new FollowPathThenShoot(paths[4], true), new ArmToFeed() ); } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index a5ee8456..e94dea23 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -104,7 +104,7 @@ public interface Shooter { double FEEDER_DEAQUIRE_SPEED = 0.5; double FEEDER_SHOOT_SPEED = 1.0; - double TARGET_RPM_THRESHOLD = 250; + double TARGET_RPM_THRESHOLD = 1000; double MAX_WAIT_TO_REACH_TARGET = 2.0; ShooterSpeeds SPEAKER = new ShooterSpeeds( @@ -263,7 +263,7 @@ public interface Alignment { SmartNumber X_TOLERANCE = new SmartNumber("Alignment/X Tolerance", 0.1); SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance", 0.1); - SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 3); + SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 6); SmartNumber CLIMB_SETUP_DISTANCE = new SmartNumber("Alignment/Climb/Setup Distance", Units.inchesToMeters(21.0)); SmartNumber INTO_CHAIN_SPEED = new SmartNumber("Alignment/Climb/Into Chain Speed", 0.25); diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index 18fc0a57..762f7035 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -120,9 +120,26 @@ private double getTargetDegrees() { private double getSpeakerAngleElin() { try { Pose2d speakerPose = Field.getAllianceSpeakerPose().transformBy(new Transform2d(Field.SPEAKER_OPENING_X, 0, new Rotation2d())); + Pose2d robotPose = SwerveDrive.getInstance().getPose(); + double distanceToSpeaker = Units.metersToInches(SwerveDrive.getInstance().getPose().minus(speakerPose).getTranslation().getNorm()) - Units.metersToInches(Settings.WIDTH / 2); - SmartDashboard.putNumber("harry", distanceToSpeaker); - return SpeakerAngleElinInterpolation.getAngleInDegrees(distanceToSpeaker); + double angleFromSpeakerBaseToRobot = Math.abs(Units.radiansToDegrees(Math.atan((speakerPose.getY() - robotPose.getY())/(speakerPose.getX() - robotPose.getX())))); + + double targetAngle = SpeakerAngleElinInterpolation.getAngleInDegrees(distanceToSpeaker); + + if (distanceToSpeaker > 120) { + targetAngle += 2; + } + + SmartDashboard.putNumber("distance to speaker", distanceToSpeaker); + SmartDashboard.putNumber("Angle to speaker base", angleFromSpeakerBaseToRobot); + + // if the robot is more than 30 degrees off to the side from the perspective of the speaker + // this is intended to help with shooting from the sides + if (angleFromSpeakerBaseToRobot > 30) { + targetAngle += 2; + } + return targetAngle; } catch (Exception e) { e.printStackTrace(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index bfa84f33..e7707991 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -174,7 +174,7 @@ public void drive(Vector2D velocity, double rotation) { twistVel.dy / Settings.DT, twistVel.dtheta / Settings.DT )); - }/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + } private void startSimThread() { m_lastSimTime = Utils.getCurrentTimeSeconds(); @@ -238,6 +238,7 @@ public boolean isAlignedToSpeaker() { Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); // Rotate by 180 because the shooter is on the back of the robot Rotation2d targetAngle = speakerPose.minus(currentPose).getAngle().rotateBy(Rotation2d.fromDegrees(180)); + SmartDashboard.putNumber("harry and jphy", getPose().getRotation().minus(targetAngle).getDegrees()); return Math.abs(getPose().getRotation().minus(targetAngle).getDegrees()) < Settings.Alignment.ANGLE_TOLERANCE.get(); } diff --git a/src/main/java/com/stuypulse/robot/util/SpeakerAngleElinInterpolation.java b/src/main/java/com/stuypulse/robot/util/SpeakerAngleElinInterpolation.java index 40597cd6..347c7e79 100644 --- a/src/main/java/com/stuypulse/robot/util/SpeakerAngleElinInterpolation.java +++ b/src/main/java/com/stuypulse/robot/util/SpeakerAngleElinInterpolation.java @@ -120,6 +120,26 @@ public class SpeakerAngleElinInterpolation { {175, -1.26913}, {177.5, -1.27449}, {180, -1.27971}, + {182.5, -1.2848}, + {185, -1.28976}, + {187.5, -1.29461}, + {190, -1.29934}, + {192.5, -1.30396}, + {195, -1.30846}, + {197.5, -1.31287}, + {200, -1.31717}, + {202.5, -1.32137}, + {205, -1.32548}, + {207.5, -1.3295}, + {210, -1.33343}, + {212.5, -1.33727}, + {215, -1.34103}, + {217.5, -1.34471}, + {220, -1.34831}, + {222.5, -1.35184}, + {225, -1.35529}, + {227.5, -1.35867}, + {230, -1.36198}, }; static { From 2d390903dcf0a9af16b57f548e6367393df18508 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Fri, 25 Oct 2024 16:28:51 -0400 Subject: [PATCH 3/7] change speaker shot speed based on distance, intake outtake thingy, arm math --- .../SwerveDriveDriveAlignedSpeaker.java | 23 +++++++++++++++++-- .../stuypulse/robot/constants/Cameras.java | 4 ++-- .../com/stuypulse/robot/constants/Field.java | 2 ++ .../stuypulse/robot/constants/Settings.java | 10 ++++---- .../robot/subsystems/arm/ArmImpl.java | 23 +++++++++++++------ .../robot/subsystems/intake/Intake.java | 6 +++++ .../robot/subsystems/intake/IntakeImpl.java | 17 +++++++++++--- .../robot/subsystems/shooter/ShooterImpl.java | 18 ++++++++++++--- .../robot/subsystems/swerve/SwerveDrive.java | 1 - .../util/SpeakerAngleElinInterpolation.java | 2 +- 10 files changed, 82 insertions(+), 24 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeaker.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeaker.java index 28dbc8d8..a2abbc2f 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeaker.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeaker.java @@ -4,8 +4,11 @@ import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.stuylib.input.Gamepad; +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; public class SwerveDriveDriveAlignedSpeaker extends SwerveDriveDriveAligned{ @@ -13,17 +16,33 @@ public SwerveDriveDriveAlignedSpeaker(Gamepad driver) { super(driver); } + private Pose2d getTargetSpeakerPose() { + Pose2d speakerPose = Field.getAllianceSpeakerPose().transformBy(new Transform2d(-Units.inchesToMeters(1.5), 0, new Rotation2d())); + Pose2d robotPose = SwerveDrive.getInstance().getPose(); + + double angleFromSpeakerBaseToRobot = Units.radiansToDegrees(Math.atan((speakerPose.getY() - robotPose.getY())/(speakerPose.getX() - robotPose.getX()))); + + // aim at the side of the speaker if youre on the side + // if (angleFromSpeakerBaseToRobot > 30) { + // speakerPose = speakerPose.transformBy(new Transform2d(0, Field.SPEAKER_OPENING_WIDTH / 2, new Rotation2d())); + // } + // if (angleFromSpeakerBaseToRobot < 30) { + // speakerPose = speakerPose.transformBy(new Transform2d(0, -Field.SPEAKER_OPENING_WIDTH / 2, new Rotation2d())); + // } + return speakerPose; + } + @Override protected Rotation2d getTargetAngle() { Translation2d currentPose = SwerveDrive.getInstance().getPose().getTranslation(); - Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); + Translation2d speakerPose = getTargetSpeakerPose().getTranslation(); return currentPose.minus(speakerPose).getAngle(); } @Override protected double getDistanceToTarget() { Translation2d currentPose = SwerveDrive.getInstance().getPose().getTranslation(); - Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); + Translation2d speakerPose = getTargetSpeakerPose().getTranslation(); return currentPose.getDistance(speakerPose); } } diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 3bc76a29..5de7d52a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -27,7 +27,7 @@ public interface Limelight { new CameraConfig( "tower-cam", new Pose3d( - new Translation3d(Units.inchesToMeters(-11.25), Units.inchesToMeters(+3.333797), Units.inchesToMeters(23.929362)), + new Translation3d(Units.inchesToMeters(-11.25), Units.inchesToMeters(3.333797 - 1.4375), Units.inchesToMeters(23.929362)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(-15), Units.degreesToRadians(180)) ), "11", @@ -37,7 +37,7 @@ public interface Limelight { new CameraConfig( "plate-cam", new Pose3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(-4.863591), Units.inchesToMeters(19.216471)), + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(-4.863591 - 1.4375), Units.inchesToMeters(19.216471)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(-10), Units.degreesToRadians(0)) ), "96", diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 2ac16a6a..56809d2f 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -30,6 +30,8 @@ public interface Field { double SPEAKER_MAX_HEIGHT = 2.11; // represents the top of the speaker opening double SPEAKER_MIN_HEIGHT = 1.98; // represents the bottom of the speaker opening + double SPEAKER_OPENING_WIDTH = Units.inchesToMeters(41); + public static Pose3d transformToOppositeAlliance(Pose3d pose) { Pose3d rotated = pose.rotateBy(new Rotation3d(0, 0, Math.PI)); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index e94dea23..2daf0e1a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -104,13 +104,13 @@ public interface Shooter { double FEEDER_DEAQUIRE_SPEED = 0.5; double FEEDER_SHOOT_SPEED = 1.0; - double TARGET_RPM_THRESHOLD = 1000; + double TARGET_RPM_THRESHOLD = 250; double MAX_WAIT_TO_REACH_TARGET = 2.0; - ShooterSpeeds SPEAKER = new ShooterSpeeds( - new SmartNumber("Shooter/Speaker RPM", 5500), - new SmartNumber("Shooter/Speaker RPM differential", 500) - ); + // ShooterSpeeds SPEAKER = new ShooterSpeeds( + // new SmartNumber("Shooter/Speaker RPM", 5500), + // new SmartNumber("Shooter/Speaker RPM differential", 500) + // ); double HAS_NOTE_FALLING_DEBOUNCE = 0.0; double HAS_NOTE_RISING_DEBOUNCE = 0.0; diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index 762f7035..71a6a073 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -119,24 +119,33 @@ private double getTargetDegrees() { private double getSpeakerAngleElin() { try { - Pose2d speakerPose = Field.getAllianceSpeakerPose().transformBy(new Transform2d(Field.SPEAKER_OPENING_X, 0, new Rotation2d())); + Pose2d speakerPose = Field.getAllianceSpeakerPose().transformBy(new Transform2d(Field.SPEAKER_OPENING_X - Units.inchesToMeters(1.5), 0, new Rotation2d())); Pose2d robotPose = SwerveDrive.getInstance().getPose(); - double distanceToSpeaker = Units.metersToInches(SwerveDrive.getInstance().getPose().minus(speakerPose).getTranslation().getNorm()) - Units.metersToInches(Settings.WIDTH / 2); - double angleFromSpeakerBaseToRobot = Math.abs(Units.radiansToDegrees(Math.atan((speakerPose.getY() - robotPose.getY())/(speakerPose.getX() - robotPose.getX())))); + double angleFromSpeakerToRobot = Units.radiansToDegrees(Math.atan((speakerPose.getY() - robotPose.getY())/(speakerPose.getX() - robotPose.getX()))); + + // aim at the side of the speaker if youre on the side + // if (angleFromSpeakerToRobot > 30) { + // speakerPose = speakerPose.transformBy(new Transform2d(0, Field.SPEAKER_OPENING_WIDTH / 2, new Rotation2d())); + // } + // if (angleFromSpeakerToRobot < 30) { + // speakerPose = speakerPose.transformBy(new Transform2d(0, -Field.SPEAKER_OPENING_WIDTH / 2, new Rotation2d())); + // } + + double distanceToSpeaker = Units.metersToInches(SwerveDrive.getInstance().getPose().minus(speakerPose).getTranslation().getNorm()) - Units.metersToInches(Settings.LENGTH / 2); double targetAngle = SpeakerAngleElinInterpolation.getAngleInDegrees(distanceToSpeaker); if (distanceToSpeaker > 120) { - targetAngle += 2; + targetAngle += (distanceToSpeaker - 120) * (1.34 / 80) * (1.05); } - SmartDashboard.putNumber("distance to speaker", distanceToSpeaker); - SmartDashboard.putNumber("Angle to speaker base", angleFromSpeakerBaseToRobot); + SmartDashboard.putNumber("Distance to speaker", distanceToSpeaker); + SmartDashboard.putNumber("Angle to speaker", angleFromSpeakerToRobot); // if the robot is more than 30 degrees off to the side from the perspective of the speaker // this is intended to help with shooting from the sides - if (angleFromSpeakerBaseToRobot > 30) { + if (Math.abs(angleFromSpeakerToRobot) > 30) { targetAngle += 2; } return targetAngle; diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java index 8f0eb833..8cb12012 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -4,6 +4,7 @@ import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.shooter.Shooter.FeederState; +import com.stuypulse.stuylib.util.StopWatch; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -30,12 +31,17 @@ public enum State { } private State state; + protected final StopWatch feedingTimer; protected Intake() { this.state = State.STOP; + feedingTimer = new StopWatch(); } public void setState(State state) { + if (state == State.FEEDING && this.state != State.FEEDING) { + feedingTimer.reset(); + } this.state = state; } diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index 6582df43..8c76bdf5 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -6,6 +6,7 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import com.stuypulse.stuylib.util.StopWatch; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -63,9 +64,19 @@ private void stop() { } private void feed() { - intakeMotor.set(Settings.Intake.INTAKE_FEED_SPEED); - funnelMotorLeft.stopMotor(); - funnelMotorRight.stopMotor(); + if (feedingTimer.getTime() < 1.5) { + intakeMotor.set(Settings.Intake.INTAKE_FEED_SPEED); + funnelMotorLeft.stopMotor(); + funnelMotorRight.stopMotor(); + } + else if (feedingTimer.getTime() > 3.0) { + feedingTimer.reset(); + } + else { + intakeMotor.set(Settings.Intake.INTAKE_DEACQUIRE_SPEED); + funnelMotorLeft.stopMotor(); + funnelMotorRight.stopMotor(); + } } @Override diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java index af4ed8ed..2c9825ea 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java @@ -17,10 +17,12 @@ import com.stuypulse.robot.util.ShooterLobFerryInterpolation; import com.stuypulse.robot.util.ShooterLowFerryInterpolation; import com.stuypulse.robot.util.ShooterSpeeds; +import com.stuypulse.stuylib.math.SLMath; import com.stuypulse.stuylib.network.SmartNumber; import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DigitalInput; @@ -83,8 +85,8 @@ protected ShooterImpl() { Motors.Shooter.RIGHT_SHOOTER.configure(rightMotor); Motors.Shooter.FEEDER_MOTOR.configure(feederMotor); - leftTargetRPM = new SmartNumber("Shooter/Left Target RPM", Settings.Shooter.SPEAKER.getLeftRPM()); - rightTargetRPM = new SmartNumber("Shooter/Right Target RPM", Settings.Shooter.SPEAKER.getRightRPM()); + leftTargetRPM = new SmartNumber("Shooter/Left Target RPM", getSpeakerShotSpeeds().getLeftRPM()); + rightTargetRPM = new SmartNumber("Shooter/Right Target RPM", getSpeakerShotSpeeds().getRightRPM()); } private double getLeftShooterRPM() { @@ -138,7 +140,7 @@ private void setFlywheelsBasedOnState() { double manualFerryDistance = Units.metersToInches(Field.getManualFerryPosition().getDistance(Field.getAmpCornerPose())); switch (getFlywheelState()) { case SPEAKER: - setTargetSpeeds(Settings.Shooter.SPEAKER); + setTargetSpeeds(getSpeakerShotSpeeds()); break; case LOW_FERRY: setTargetSpeeds(getLowFerrySpeeds()); @@ -175,6 +177,16 @@ private void setFlywheelsBasedOnState() { } } + private ShooterSpeeds getSpeakerShotSpeeds() { + Pose2d speakerPose = Field.getAllianceSpeakerPose(); + Pose2d robotPose = SwerveDrive.getInstance().getPose(); + double distanceToSpeaker = robotPose.minus(speakerPose).getTranslation().getNorm() - Settings.LENGTH / 2; + return new ShooterSpeeds( + 4000 + SLMath.clamp(distanceToSpeaker - 1.5, 0, Double.MAX_VALUE) * 430, + 500 + ); + } + @Override public boolean hasNote() { return hasNote.get(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index e7707991..1a1e1b06 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -238,7 +238,6 @@ public boolean isAlignedToSpeaker() { Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); // Rotate by 180 because the shooter is on the back of the robot Rotation2d targetAngle = speakerPose.minus(currentPose).getAngle().rotateBy(Rotation2d.fromDegrees(180)); - SmartDashboard.putNumber("harry and jphy", getPose().getRotation().minus(targetAngle).getDegrees()); return Math.abs(getPose().getRotation().minus(targetAngle).getDegrees()) < Settings.Alignment.ANGLE_TOLERANCE.get(); } diff --git a/src/main/java/com/stuypulse/robot/util/SpeakerAngleElinInterpolation.java b/src/main/java/com/stuypulse/robot/util/SpeakerAngleElinInterpolation.java index 347c7e79..3ef236d6 100644 --- a/src/main/java/com/stuypulse/robot/util/SpeakerAngleElinInterpolation.java +++ b/src/main/java/com/stuypulse/robot/util/SpeakerAngleElinInterpolation.java @@ -145,7 +145,7 @@ public class SpeakerAngleElinInterpolation { static { interpolatingDoubleTreeMap = new InterpolatingDoubleTreeMap(); for (double[] data: distanceAndAngle) { - interpolatingDoubleTreeMap.put(data[0], data[1]); + interpolatingDoubleTreeMap.put(data[0] - 1.4375, data[1]); } } From 98788f21d492358efc4c69fd035114b0b258459f Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Fri, 25 Oct 2024 18:20:14 -0400 Subject: [PATCH 4/7] characterize --- src/main/java/com/stuypulse/robot/constants/Settings.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 2daf0e1a..d88b602d 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -207,9 +207,9 @@ public interface Drive { SmartNumber kI = new SmartNumber("Swerve/Turn/PID/kI", Robot.isReal() ? 0.0 : 0.0); SmartNumber kD = new SmartNumber("Swerve/Turn/PID/kD", Robot.isReal() ? 0.0 : 0.1); - SmartNumber kS = new SmartNumber("Swerve/Drive/FF/kS", Robot.isReal() ? 0.31007 : Simulation.DRIVE_FRICTION_VOLTAGE); - SmartNumber kV = new SmartNumber("Swerve/Drive/FF/kV", Robot.isReal() ? 1.62153 : 0.25); - SmartNumber kA = new SmartNumber("Swerve/Drive/FF/kA", Robot.isReal() ? 0.0048373 : 0.01); + SmartNumber kS = new SmartNumber("Swerve/Drive/FF/kS", Robot.isReal() ? 0.099001 : Simulation.DRIVE_FRICTION_VOLTAGE); + SmartNumber kV = new SmartNumber("Swerve/Drive/FF/kV", Robot.isReal() ? 0.11964 : 0.25); + SmartNumber kA = new SmartNumber("Swerve/Drive/FF/kA", Robot.isReal() ? 0.030009 : 0.01); double L2 = ((50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0)); // 6.74607175 double L3 = ((50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0)); // 6.12244898 From 638b2e65a1c0964dcf2ff069cce562d27525057c Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Fri, 25 Oct 2024 18:20:21 -0400 Subject: [PATCH 5/7] 10/25 --- .../stuypulse/robot/subsystems/arm/ArmImpl.java | 2 +- .../robot/subsystems/intake/IntakeImpl.java | 4 ++-- .../robot/subsystems/shooter/ShooterImpl.java | 14 ++++++++++---- 3 files changed, 13 insertions(+), 7 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index 71a6a073..7284fa62 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -137,7 +137,7 @@ private double getSpeakerAngleElin() { double targetAngle = SpeakerAngleElinInterpolation.getAngleInDegrees(distanceToSpeaker); if (distanceToSpeaker > 120) { - targetAngle += (distanceToSpeaker - 120) * (1.34 / 80) * (1.05); + targetAngle += (distanceToSpeaker - 120) * (1.34 / 80) * (1.2); } SmartDashboard.putNumber("Distance to speaker", distanceToSpeaker); diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index 8c76bdf5..8daec2d0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -64,12 +64,12 @@ private void stop() { } private void feed() { - if (feedingTimer.getTime() < 1.5) { + if (feedingTimer.getTime() < 0.75) { intakeMotor.set(Settings.Intake.INTAKE_FEED_SPEED); funnelMotorLeft.stopMotor(); funnelMotorRight.stopMotor(); } - else if (feedingTimer.getTime() > 3.0) { + else if (feedingTimer.getTime() > 1.25) { feedingTimer.reset(); } else { diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java index 2c9825ea..3310b767 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java @@ -181,10 +181,16 @@ private ShooterSpeeds getSpeakerShotSpeeds() { Pose2d speakerPose = Field.getAllianceSpeakerPose(); Pose2d robotPose = SwerveDrive.getInstance().getPose(); double distanceToSpeaker = robotPose.minus(speakerPose).getTranslation().getNorm() - Settings.LENGTH / 2; - return new ShooterSpeeds( - 4000 + SLMath.clamp(distanceToSpeaker - 1.5, 0, Double.MAX_VALUE) * 430, - 500 - ); + // return new ShooterSpeeds( + // 4000 + SLMath.clamp(distanceToSpeaker - 1.5, 0, Double.MAX_VALUE) * 600, + // 500 + // ); + if (distanceToSpeaker <= 1.5) { + return new ShooterSpeeds(4000, 500); + } + else { + return new ShooterSpeeds(5500, 500); + } } @Override From 37c34466b348e4354ff3048904e2df9c7559cd8d Mon Sep 17 00:00:00 2001 From: jopy-man Date: Fri, 25 Oct 2024 19:09:24 -0400 Subject: [PATCH 6/7] Changed feedingTimer to always deacquire and timer constants --- .../com/stuypulse/robot/subsystems/intake/IntakeImpl.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index 8daec2d0..984423d8 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -65,11 +65,11 @@ private void stop() { private void feed() { if (feedingTimer.getTime() < 0.75) { - intakeMotor.set(Settings.Intake.INTAKE_FEED_SPEED); + intakeMotor.set(Settings.Intake.INTAKE_DEACQUIRE_SPEED); funnelMotorLeft.stopMotor(); funnelMotorRight.stopMotor(); } - else if (feedingTimer.getTime() > 1.25) { + else if (feedingTimer.getTime() > 1.5) { feedingTimer.reset(); } else { From 0e53dcb3818999a2dcd6534452fe6e5810b098e8 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Fri, 25 Oct 2024 20:42:40 -0400 Subject: [PATCH 7/7] fix intake changes --- .../com/stuypulse/robot/subsystems/intake/IntakeImpl.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index 984423d8..9b25d72e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -65,7 +65,7 @@ private void stop() { private void feed() { if (feedingTimer.getTime() < 0.75) { - intakeMotor.set(Settings.Intake.INTAKE_DEACQUIRE_SPEED); + intakeMotor.set(Settings.Intake.INTAKE_FEED_SPEED); funnelMotorLeft.stopMotor(); funnelMotorRight.stopMotor(); } @@ -73,7 +73,7 @@ else if (feedingTimer.getTime() > 1.5) { feedingTimer.reset(); } else { - intakeMotor.set(Settings.Intake.INTAKE_DEACQUIRE_SPEED); + intakeMotor.set(1.0); funnelMotorLeft.stopMotor(); funnelMotorRight.stopMotor(); }