From 4cfd05b93d85720dd6280dd658bcb03c8e4ad7ad Mon Sep 17 00:00:00 2001 From: AlexLC202 <71470671+AlexLC202@users.noreply.github.com> Date: Mon, 13 Sep 2021 15:37:34 -0700 Subject: [PATCH 1/4] Add files via upload --- c2021 v2/src/main/cpp/Drivetrain.cpp | 21 + c2021 v2/src/main/cpp/Robot.cpp | 100 +++- c2021 v2/src/main/cpp/Shooter.cpp | 2 + c2021 v2/src/main/cpp/config.cpp | 506 ++++++++++++++++++ c2021 v2/src/main/cpp/controls.cpp | 136 +++++ c2021 v2/src/main/cpp/robot_state.cpp | 109 ++++ c2021 v2/src/main/cpp/shims/minimal_phoenix.h | 23 + c2021 v2/src/main/cpp/shims/navx_ahrs.h | 138 +++++ c2021 v2/src/main/cpp/subsystems/drive.cpp | 370 +++++++++++++ c2021 v2/src/main/cpp/subsystems/drive.h | 96 ++++ .../src/main/cpp/subsystems/limelight.cpp | 90 ++++ c2021 v2/src/main/cpp/subsystems/limelight.h | 97 ++++ c2021 v2/src/main/include/Drivetrain.h | 25 + c2021 v2/src/main/include/Robot.h | 61 ++- c2021 v2/src/main/include/Shooter.h | 2 + c2021 v2/src/main/include/config.h | 172 ++++++ c2021 v2/src/main/include/controls.h | 91 ++++ c2021 v2/src/main/include/robot_state.h | 51 ++ c2021 v2/src/main/include/subsystem.h | 59 ++ .../src/main/include/util/caching_types.h | 34 ++ .../main/include/util/construcotr_macros.h | 8 + c2021 v2/src/main/include/util/interp_map.h | 145 +++++ c2021 v2/src/main/include/util/number_util.h | 43 ++ c2021 v2/src/main/include/util/sdb_types.h | 83 +++ 24 files changed, 2416 insertions(+), 46 deletions(-) create mode 100644 c2021 v2/src/main/cpp/Drivetrain.cpp create mode 100644 c2021 v2/src/main/cpp/Shooter.cpp create mode 100644 c2021 v2/src/main/cpp/config.cpp create mode 100644 c2021 v2/src/main/cpp/controls.cpp create mode 100644 c2021 v2/src/main/cpp/robot_state.cpp create mode 100644 c2021 v2/src/main/cpp/shims/minimal_phoenix.h create mode 100644 c2021 v2/src/main/cpp/shims/navx_ahrs.h create mode 100644 c2021 v2/src/main/cpp/subsystems/drive.cpp create mode 100644 c2021 v2/src/main/cpp/subsystems/drive.h create mode 100644 c2021 v2/src/main/cpp/subsystems/limelight.cpp create mode 100644 c2021 v2/src/main/cpp/subsystems/limelight.h create mode 100644 c2021 v2/src/main/include/Drivetrain.h create mode 100644 c2021 v2/src/main/include/Shooter.h create mode 100644 c2021 v2/src/main/include/config.h create mode 100644 c2021 v2/src/main/include/controls.h create mode 100644 c2021 v2/src/main/include/robot_state.h create mode 100644 c2021 v2/src/main/include/subsystem.h create mode 100644 c2021 v2/src/main/include/util/caching_types.h create mode 100644 c2021 v2/src/main/include/util/construcotr_macros.h create mode 100644 c2021 v2/src/main/include/util/interp_map.h create mode 100644 c2021 v2/src/main/include/util/number_util.h create mode 100644 c2021 v2/src/main/include/util/sdb_types.h diff --git a/c2021 v2/src/main/cpp/Drivetrain.cpp b/c2021 v2/src/main/cpp/Drivetrain.cpp new file mode 100644 index 00000000..d2139459 --- /dev/null +++ b/c2021 v2/src/main/cpp/Drivetrain.cpp @@ -0,0 +1,21 @@ + +#include "Drivetrain.h" + +Drivetrain::Drivetrain(){ + left_master.SetInverted(false); + right_master.SetInverted(true); + left_slave.Follow(left_master); + right_slave.Follow(right_master); + left_slave.SetInverted(InvertType::FollowMaster); + right_slave.SetInverted(InvertType::FollowMaster); +} + +void +Drivetrain::Drive(){ + +} + +void +Drivetrain::UpdateOdometry(){ + +} \ No newline at end of file diff --git a/c2021 v2/src/main/cpp/Robot.cpp b/c2021 v2/src/main/cpp/Robot.cpp index 31e6c680..6a25f6ae 100644 --- a/c2021 v2/src/main/cpp/Robot.cpp +++ b/c2021 v2/src/main/cpp/Robot.cpp @@ -1,32 +1,102 @@ #include "Robot.h" +#include + +#include + +namespace team114 { +namespace c2020 { + +/** + * Instentiates structs for future use. +**/ +Robot::Robot() + : frc::TimedRobot{Robot::kPeriod}, + controls_{}, + drive_{Drive::GetInstance()}, + robot_state_{RobotState::GetInstance()}, + ljoy_{0}, + rjoy_{1}, + ojoy_{2}, + cfg{conf::GetConfig()} {} + +/** + * Nothing. +**/ void Robot::RobotInit() { - m_chooser.SetDefaultOption(kAutoNameDefault, kAutoNameDefault); - m_chooser.AddOption(kAutoNameCustom, kAutoNameCustom); - frc::SmartDashboard::PutData("Auto Modes", &m_chooser); } -void Robot::RobotPeriodic() {} -void Robot::AutonomousInit() {} +/** + * Calls period function of select classes. Possibly unfinished? +**/ +void Robot::RobotPeriodic() { + + drive_.Periodic(); + +} + +/** + * Zeroing sensors, selecting auto mode. +**/ +void Robot::AutonomousInit() { + +} + +/** + * Calls periodic function of select structs. +**/ void Robot::AutonomousPeriodic() { - Drivetrain.Auto(); + } -void Robot::TeleopInit() {} +/** + * Finishes initialition (stows hood?). +**/ +void Robot::TeleopInit() { + +} + +/** + * Calls remaining periodic funtions. Checks if robot is shooting, climbing or doing the control panel and calls functions accordingly. +**/ void Robot::TeleopPeriodic() { - Drivetrain.Periodic(l_joy, r_joy); - Shooter.Periodic(xbox); + // if (!controls_.Shoot()) { + drive_.SetWantCheesyDrive(controls_.Throttle(), controls_.Wheel(), + controls_.QuickTurn()); + // } + + //check if different contol buttons have been pressed, act accordingly + } -void Robot::DisabledInit() {} -void Robot::DisabledPeriodic() {} +/** + * Nothing. +**/ void Robot::TestInit() {} -void Robot::TestPeriodic() {} +/** + * Simulates the climbing portion of periodic action. +**/ +void Robot::TestPeriodic() { + +} + +/** + * Resets a couple things (most zeroing happens in AutonomousInit()). +**/ +void Robot::DisabledInit() { + +} + +/** + * Updates auto selector. +**/ +void Robot::DisabledPeriodic() { } + +} // namespace c2020 +} // namespace team114 #ifndef RUNNING_FRC_TESTS -int main() { - return frc::StartRobot(); -} +int main() { return frc::StartRobot(); } #endif diff --git a/c2021 v2/src/main/cpp/Shooter.cpp b/c2021 v2/src/main/cpp/Shooter.cpp new file mode 100644 index 00000000..78004ce8 --- /dev/null +++ b/c2021 v2/src/main/cpp/Shooter.cpp @@ -0,0 +1,2 @@ +#include "Shooter.h" + diff --git a/c2021 v2/src/main/cpp/config.cpp b/c2021 v2/src/main/cpp/config.cpp new file mode 100644 index 00000000..60ba0c69 --- /dev/null +++ b/c2021 v2/src/main/cpp/config.cpp @@ -0,0 +1,506 @@ +#include "config.h" + +#include +#include +#include +#include + +//#include "util/number_util.h" + +namespace team114 { +namespace c2020 { +namespace conf { + +// WPILib chose GCC 7, so no C++20 designated initializers here + +/** +* This function is declaring all the constants that will be needed for the robot through obj c +* Includes PID constants, ticks, etc +**/ +const RobotConfig MakeDefaultRobotConfig() { + RobotConfig c; + // /sys/class/net//address is newline terminated, match here + c.mac_address = "aa:bb:cc:dd:ee:ff\n"; + + c.drive.left_master_id = 0; + c.drive.left_slave_id = 1; + c.drive.right_master_id = 2; + c.drive.right_slave_id = 3; + c.drive.track_width = 0.661924_m; + c.drive.meters_per_falcon_tick = + 1.0 / 2048.0 * 10.0 / 62.0 * 18.0 / 30.0 * 6 * M_PI * 0.0254_m; + c.drive.traj_max_vel = 2.5_mps; + c.drive.traj_max_accel = units::meters_per_second_squared_t{5.0}; + // currently this is g with 2x FOS + c.drive.traj_max_centrip_accel = units::meters_per_second_squared_t{4.9}; + c.drive.orient_kp = 0.45; + c.drive.orient_ki = 0.0; + c.drive.orient_kd = 0.0; + c.drive.orient_vel = 0.0; + c.drive.orient_acc = 0.0; + + c.ctrl_panel.talon_id = 31; + c.ctrl_panel.current_limit = 22; + c.ctrl_panel.kP = 1.5; + c.ctrl_panel.kI = 0.0; + c.ctrl_panel.kD = 5.0; + c.ctrl_panel.ticks_per_inch = 4096.0 / (3.5 * M_PI); + c.ctrl_panel.ticks_per_color_slice = + 12.56 * 0.96 * c.ctrl_panel.ticks_per_inch; + c.ctrl_panel.rot_control_ticks = + 32 * M_PI * 3.5 * c.ctrl_panel.ticks_per_inch; + c.ctrl_panel.scoot_cmd = 0.5; + c.ctrl_panel.solenoid_channel = 4; + + // TODO the rest + c.intake.rot_talon_id = 41; //needs to be updated? + c.intake.roller_talon_id = 20; + c.intake.intake_cmd = 0.80; + c.intake.rot_current_limit = 10; + c.intake.zeroing_kp = 0.001; + c.intake.zeroing_vel = 8; + c.intake.roller_current_limit = 20; + c.intake.abs_enc_tick_offset = 2945; + c.intake.abs_ticks_per_rot = 4096.0 * 36.0 / 22.0; + c.intake.rel_ticks_per_abs_tick = 1.0; + c.intake.rads_per_rel_tick = + 2 * M_PI / c.intake.abs_ticks_per_rot / c.intake.rel_ticks_per_abs_tick; + c.intake.zeroed_rad_from_vertical = 15.4 * M_PI / 180.0; + c.intake.SinekF = -0.063; + c.intake.kP = 1.0; + c.intake.kI = 0.005; + c.intake.kD = 0.0; + c.intake.profile_acc = 3000; + c.intake.profile_vel = 2000; + c.intake.stowed_pos_ticks = 1222; + c.intake.intaking_pos_ticks = 2350; + // c.intake.trench_driving_pos_ticks = 20.0 / 36.0 * 4096.0; + + c.hood.talon_id = 52; + c.hood.ticks_per_degree = 4096.0 * 350.0 / 28.0 / 360.0; + c.hood.min_degrees = 20; // TODO + c.hood.max_degrees = 64; + c.hood.current_limit = 20; + c.hood.zeroing_current = 6; + c.hood.zeroing_kp = .001; + c.hood.zeroing_vel = 15; + c.hood.profile_acc = 20000; // TODO + c.hood.profile_vel = 9000; // TODO + c.hood.kP = 2.0; + c.hood.kI = 0.01; + c.hood.kD = 10.0; + c.hood.ctre_curve_smoothing = 2; + + c.shooter.master_id = 5; //left side of shooter + c.shooter.slave_id = 6; //right side of shooter + c.shooter.turret_id = 4; + c.shooter.shooter_current_limit = 40; + c.shooter.kF = 0.0160; + c.shooter.kP = 0.25; + c.shooter.kP = 0.0; + c.shooter.kI = 0.0; + c.shooter.kD = 0.0; + c.shooter.meas_period = VelocityMeasPeriod::Period_2Ms; + c.shooter.meas_filter_width = 32; + c.shooter.shootable_err_pct = 0.04; + c.shooter.kicker_id = 51; + c.shooter.kicker_current_limit = 20; + c.shooter.kicker_cmd = 0.7; //IIF YOU CHANGE THIS U GOTTA REDO AUTO SHOOT DATA + + c.ball_channel.serializer_id = 43; + c.ball_channel.channel_id = 44; + c.ball_channel.current_limit = 25; + c.ball_channel.serializer_cmd = 1.00; + c.ball_channel.channel_cmd = 0.9; + c.ball_channel.s0_port = 0; + c.ball_channel.s1_port = 1; + c.ball_channel.s2_port = 2; + c.ball_channel.s3_port = 3; + + c.climber.master_id = 19; + c.climber.slave_id = 20; + c.climber.current_limit = 35; + c.climber.release_solenoid_id = 7; + c.climber.brake_solenoid_id = 6; + c.climber.avg_ticks_per_inch = 4096.0 * 9.0 / ((.969 + 1.25) * M_PI); + c.climber.initial_step_ticks = 10.0 * c.climber.avg_ticks_per_inch; + c.climber.forward_soft_limit_ticks = 122000; + c.climber.ascend_command = 1.00; + c.climber.descend_command = -1.00; + + /* c.limelight.name = "limelight"; + c.limelight.table_name = "limelight"; + c.limelight.diff_height = 2.496_m - 0.55_m; + c.limelight.angle_above_horizontal = DegToRad(8.0); */ + + return c; +} + +/** +* Basic getter, returning the configuration to uphold encapsulation in oop +**/ +RobotConfig& GetConfig() { + static std::optional CONFIG; + if (CONFIG.has_value()) { + return CONFIG.value(); + } + // TODO(josh): log the chosen config + std::ifstream ifs("/sys/class/net/eth0/address"); + if (!ifs.good() || !ifs.is_open()) { + CONFIG = MakeDefaultRobotConfig(); + return GetConfig(); + } + std::string rio_mac((std::istreambuf_iterator(ifs)), + (std::istreambuf_iterator())); + // TODO(josh) fill in with real RIOs + RobotConfig config_a = MakeDefaultRobotConfig(); + RobotConfig config_b = MakeDefaultRobotConfig(); + if (config_a.mac_address == rio_mac) { + CONFIG = std::move(config_a); + } else if (config_a.mac_address == rio_mac) { + CONFIG = std::move(config_b); + } else { + CONFIG = MakeDefaultRobotConfig(); + } + return GetConfig(); +} + +/** +* Defining constants for Talon configurations +* This includes current limit, nominal outputs, etc regarding talon motors +*/ +void DriveFalconCommonConfig(TalonFX& falcon) { + TalonFXConfiguration c; + // ====== Talon FX CFG + c.supplyCurrLimit = + SupplyCurrentLimitConfiguration{true, 50.0, 55.0, 0.100}; + // StatorCurrentLimitConfiguration statorCurrLimit + + // TODO(josh) enable FOC on arrival + c.motorCommutation = MotorCommutation::Trapezoidal; + // absoluteSensorRange = AbsoluteSensorRange::Unsigned_0_to_360 double + // integratedSensorOffsetDegrees = 0 + c.initializationStrategy = SensorInitializationStrategy::BootToZero; + // ===== Base Talon CFG + c.primaryPID.selectedFeedbackSensor = FeedbackDevice::IntegratedSensor; + c.primaryPID.selectedFeedbackCoefficient = 1.0; + // ======= BaseMotor Param cfg + c.openloopRamp = 0.25; // time from neutral to full + c.closedloopRamp = 0.0; // disable ramping, should be running a profile + c.peakOutputForward = 1.0; + c.peakOutputReverse = -1.0; + c.nominalOutputForward = 0.0; + c.nominalOutputReverse = 0.0; + // double neutralDeadband + c.voltageCompSaturation = 12.0; + // int voltageMeasurementFilter + // TODO(josh) tune these + // https://phoenix-documentation.readthedocs.io/en/latest/ch14_MCSensor.html#velocity-measurement-filter + c.velocityMeasurementPeriod = VelocityMeasPeriod::Period_5Ms; + c.velocityMeasurementWindow = 2; + // int forwardSoftLimitThreshold + // int reverseSoftLimitThreshold + c.forwardSoftLimitEnable = false; + c.reverseSoftLimitEnable = false; + // TODO(Josh) tune for Ramsete + c.slot0.allowableClosedloopError = 0; + c.slot0.closedLoopPeakOutput = 1.0; + c.slot0.closedLoopPeriod = 1; + // TODO(josh) tune + // c.slot0.integralZone = + // c.slot0.maxIntegralAccumulator = + // https://phoenix-documentation.readthedocs.io/en/latest/ch16_ClosedLoop.html#calculating-velocity-feed-forward-gain-kf + c.slot0.kF = 0.0; + c.slot0.kP = 0.0; + c.slot0.kI = 0.0; + c.slot0.kD = 0.0; + // SlotConfiguration slot1 + // SlotConfiguration slot2 + // SlotConfiguration slot3 + // bool auxPIDPolarity + // FilterConfiguration remoteFilter0 + // FilterConfiguration remoteFilter1 + // int motionCruiseVelocity + // int motionAcceleration + // int motionCurveStrength + // int motionProfileTrajectoryPeriod + // bool feedbackNotContinuous + // bool remoteSensorClosedLoopDisableNeutralOnLOS + // bool clearPositionOnLimitF + // bool clearPositionOnLimitR + // bool clearPositionOnQuadIdx + // bool limitSwitchDisableNeutralOnLOS + // bool softLimitDisableNeutralOnLOS + // int pulseWidthPeriod_EdgesPerRot + // int pulseWidthPeriod_FilterWindowSz + // bool trajectoryInterpolationEnable + // ======= Custom Param CFG + // int customParam0 + // int customParam1 + // bool enableOptimizations + // TODO(josh) logs everywhere + for (int i = 0; i < 10; i++) { + auto err = falcon.ConfigAllSettings(c, 100); + if (err == ErrorCode::OKAY) { + break; + } + } + falcon.EnableVoltageCompensation(true); + // TODO(josh) falcon's dont have enable current limit? + falcon.SelectProfileSlot(0, 0); + falcon.SetNeutralMode(NeutralMode::Brake); +} + +// CAN metrics docs +// https://phoenix-documentation.readthedocs.io/en/latest/ch18_CommonAPI.html#can-bus-utilization-error-metrics + +//Basically finding and storing multiple error codes for Talonfx +//Explains why you need error handling +//https:github.com/CrossTheRoadElec/Phoenix-Documentation/blob/master/Legacy/README.md#error-handling +inline static ErrorCode SetDriveCommonFramePeriods(TalonFX& falcon) { + constexpr int lng = 255; + ErrorCollection err; + err.NewError(falcon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_3_Quadrature, lng)); + err.NewError(falcon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_4_AinTempVbat, lng)); + err.NewError(falcon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_8_PulseWidth, lng)); + err.NewError(falcon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_10_MotionMagic, lng)); + err.NewError(falcon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_12_Feedback1, lng)); + err.NewError(falcon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_14_Turn_PIDF1, lng)); + return err.GetFirstNonZeroError(); +} +constexpr int kStatusFrameAttempts = 3; + +// TODO(josh) log here +//Similiar to the function above, it just adds error to collection +//Main difference is that you don't return it. Also this is master meaning it controls slave +void SetDriveMasterFramePeriods(TalonFX& falcon) { + for (int i = 0; i < kStatusFrameAttempts; i++) { + ErrorCollection err; + err.NewError(SetDriveCommonFramePeriods(falcon)); + err.NewError(falcon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_1_General, 5)); + err.NewError(falcon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_2_Feedback0, 5)); + err.NewError(falcon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_13_Base_PIDF0, 50)); + if (err.GetFirstNonZeroError() == ErrorCode::OK) { + return; + } + } +} + +/** +* Collects error codes, however master controls +**/ +void SetDriveSlaveFramePeriods(TalonFX& falcon) { + for (int i = 0; i < kStatusFrameAttempts; i++) { + ErrorCollection err; + err.NewError(SetDriveCommonFramePeriods(falcon)); + err.NewError(falcon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_1_General, 255)); + err.NewError(falcon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_2_Feedback0, 255)); + err.NewError(falcon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_13_Base_PIDF0, 255)); + if (err.GetFirstNonZeroError() == ErrorCode::OK) { + return; + } + } +} + +/** +* Setting values and configurations for Talons +* Also collecting error from functions +**/ +void SetFramePeriodsForPidTalon(TalonSRX& talon, FeedbackType feedback_type) { + constexpr int lng = 255; + constexpr int shrt = 10; + int quad = lng; + int pulsewidth = lng; + switch (feedback_type) { + case FeedbackType::Both: + quad = shrt; + pulsewidth = shrt; + break; + case FeedbackType::Quadrature: + quad = shrt; + break; + case FeedbackType::PulseWidth: + pulsewidth = shrt; + break; + case FeedbackType::None: + break; + } + for (int i = 0; i < kStatusFrameAttempts; i++) { + ErrorCollection err; + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_1_General, shrt)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_2_Feedback0, shrt)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_3_Quadrature, quad)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_4_AinTempVbat, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_8_PulseWidth, pulsewidth)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_10_MotionMagic, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_12_Feedback1, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_14_Turn_PIDF1, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_13_Base_PIDF0, 50)); + if (err.GetFirstNonZeroError() == ErrorCode::OK) { + return; + } + } +} + +void SetFramePeriodsForPidTalonFX(TalonFX& talon, FeedbackType feedback_type) { + constexpr int lng = 255; + constexpr int shrt = 10; + int quad = lng; + int pulsewidth = lng; + switch (feedback_type) { + case FeedbackType::Both: + quad = shrt; + pulsewidth = shrt; + break; + case FeedbackType::Quadrature: + quad = shrt; + break; + case FeedbackType::PulseWidth: + pulsewidth = shrt; + break; + case FeedbackType::None: + break; + } + for (int i = 0; i < kStatusFrameAttempts; i++) { + ErrorCollection err; + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_1_General, shrt)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_2_Feedback0, shrt)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_3_Quadrature, quad)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_4_AinTempVbat, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_8_PulseWidth, pulsewidth)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_10_MotionMagic, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_12_Feedback1, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_14_Turn_PIDF1, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_13_Base_PIDF0, 50)); + if (err.GetFirstNonZeroError() == ErrorCode::OK) { + return; + } + } +} + + +/** +* Error collecting for Open loop TalonSRX's +**/ +void SetFramePeriodsForOpenLoopTalon(TalonSRX& talon) { + constexpr int lng = 255; + constexpr int shrt = 20; + for (int i = 0; i < kStatusFrameAttempts; i++) { + ErrorCollection err; + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_1_General, shrt)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_2_Feedback0, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_3_Quadrature, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_4_AinTempVbat, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_8_PulseWidth, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_10_MotionMagic, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_12_Feedback1, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_14_Turn_PIDF1, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_13_Base_PIDF0, lng)); + if (err.GetFirstNonZeroError() == ErrorCode::OK) { + return; + } + } +} + +/** +* Another Slave function which means controlled by master +* collect error +**/ +void SetFramePeriodsForSlaveTalon(TalonSRX& talon) { + constexpr int lng = 255; + for (int i = 0; i < kStatusFrameAttempts; i++) { + ErrorCollection err; + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_1_General, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_2_Feedback0, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_3_Quadrature, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_4_AinTempVbat, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_8_PulseWidth, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_10_MotionMagic, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_12_Feedback1, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_14_Turn_PIDF1, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_13_Base_PIDF0, lng)); + if (err.GetFirstNonZeroError() == ErrorCode::OK) { + return; + } + } +} + +void SetFramePeriodsForSlaveTalonFX(TalonFX& talon) { + constexpr int lng = 255; + for (int i = 0; i < kStatusFrameAttempts; i++) { + ErrorCollection err; + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_1_General, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_2_Feedback0, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_3_Quadrature, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_4_AinTempVbat, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_8_PulseWidth, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_10_MotionMagic, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_12_Feedback1, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_14_Turn_PIDF1, lng)); + err.NewError(talon.SetStatusFramePeriod( + StatusFrameEnhanced::Status_13_Base_PIDF0, lng)); + if (err.GetFirstNonZeroError() == ErrorCode::OK) { + return; + } + } +} + +} // namespace conf +} // namespace c2020 +} // namespace team114 diff --git a/c2021 v2/src/main/cpp/controls.cpp b/c2021 v2/src/main/cpp/controls.cpp new file mode 100644 index 00000000..674c781a --- /dev/null +++ b/c2021 v2/src/main/cpp/controls.cpp @@ -0,0 +1,136 @@ +#include "controls.h" +#include + +namespace team114 { +namespace c2020 { + +/** +* Constructor, initializes left, right, and other joystick with respective ports. +**/ +Controls::Controls() : ljoy_{0}, rjoy_{1}, ojoy_{2} {} +/** + * returns value for joystick in axis 1 to determine throttle amount. + **/ + +void Controls::OPrints() { + /* if (ojoy_.GetRawButton(1)) std::cout << 1 << std::endl; + if (ojoy_.GetRawButton(2)) std::cout << 2 << std::endl; + if (ojoy_.GetRawButton(3)) std::cout << 3 << std::endl; + if (ojoy_.GetRawButton(4)) std::cout << 4 << std::endl; + if (ojoy_.GetRawButton(5)) std::cout << 5 << std::endl; + if (ojoy_.GetRawButton(6)) std::cout << 6 << std::endl; + if (ojoy_.GetRawButton(7)) std::cout << 7 << std::endl; */ +} + +double Controls::Throttle() { return -ljoy_.GetRawAxis(1); } +/** + * returns value for joystick in axis 0. +**/ +double Controls::Wheel() { return rjoy_.GetRawAxis(0); } +/** + * Checks if button has been pressed to indicate a quick turn. +**/ +bool Controls::QuickTurn() { return rjoy_.GetRawButton(1); } +/** + * Checks if joystick has been moved enough to indicate climbing up should start. +**/ +bool Controls::ClimbUp() { return ojoy_.GetRawAxis(1) < -kAnalogJoyThreshold; } +/** + * Checks if joystick has been moved enough to indicate climbing down should start. +**/ +bool Controls::ClimbDown() { return ojoy_.GetRawAxis(1) > kAnalogJoyThreshold; } +/** + * check if ball is ready to shoot ball short distance if +**/ +bool Controls::ShotShortPressed() { + int pov = ojoy_.GetPOV(); + return shot_short_.Pressed(pov == 0 || pov == 315 || pov == 45); +} +/** +* Check if ball is ready to shoot ball medium distance +**/ +bool Controls::ShotMedPressed() { + int pov = ojoy_.GetPOV(); + return shot_short_.Pressed(pov == 90 || pov == 135 || pov == 270 || + pov == 225); +} +/** + * Check if ready to shoot ball long distance +**/ +bool Controls::ShotLongPressed() { + int pov = ojoy_.GetPOV(); + return shot_short_.Pressed(pov == 180); +} +/** + * Checking if intake should begin. +**/ +bool Controls::Intake() { return ojoy_.GetRawAxis(3) > kTriggerThreshold; } +/** + * Check if button for unjamming has been pressed. +**/ +bool Controls::Unjam() { return ojoy_.GetRawButton(7); } +/** + * Checks if button to shoot has been pressed +**/ +bool Controls::Shoot() { + // return ljoy_.GetRawButton(1); + return ojoy_.GetRawButton(5); +} + +bool Controls::ShortShot() { + return ojoy_.GetRawButton(4); //y +} + +bool Controls::LongShot() { + return ojoy_.GetRawButton(1); //the button below y i can't remember which letter it is +} + +/** + * Checks if joystick has been moved enough to start deploying panel. +**/ +bool Controls::PanelDeploy() { return ojoy_.GetRawAxis(2) > kTriggerThreshold; } +/** +* Pass on message that scoot left has been pressed +**/ +bool Controls::ScootLeftPressed() { + return scoot_left_.Pressed(scoot_tracker_.PassThroughFeed( + ojoy_.GetRawAxis(4) < -kAnalogJoyThreshold)); +} +/** + * Pass on message that scoot right has been pressed +**/ +bool Controls::ScootRightPressed() { + return scoot_right_.Pressed(scoot_tracker_.PassThroughFeed( + ojoy_.GetRawAxis(4) > kAnalogJoyThreshold)); +} +/** + * Pass on message that scoot has been released +**/ +bool Controls::ScootReleased() { + return scoot_released_.Pressed(scoot_tracker_.WasNotHeld()); +} +/** + * Check if Rot Control button has been pressed +**/ +bool Controls::RotControlPressed() { return ojoy_.GetRawButtonPressed(8); } +/** + * Check if Pos Control Red button has been pressed +**/ +bool Controls::PosControlRedPressed() { return ojoy_.GetRawButtonPressed(2); } +/** +* Check if Pos Control Yellow button has been pressed +**/ +bool Controls::PosControlYellowPressed() { + return ojoy_.GetRawButtonPressed(4); +} +/** + * Check if Pos Control Green button has been pressed +**/ +bool Controls::PosControlGreenPressed() { return ojoy_.GetRawButtonPressed(1); } +/** + * Check if pos control blue button has been pressed +**/ +bool Controls::PosControlBluePressed() { return ojoy_.GetRawButtonPressed(3); } + +} // namespace c2020 +} // namespace team114 diff --git a/c2021 v2/src/main/cpp/robot_state.cpp b/c2021 v2/src/main/cpp/robot_state.cpp new file mode 100644 index 00000000..ad691dbd --- /dev/null +++ b/c2021 v2/src/main/cpp/robot_state.cpp @@ -0,0 +1,109 @@ +#include "robot_state.h" + +#include + +namespace team114 { +namespace c2020 { + + +/** + * default construtor of robot state, calls the other constructor(?) + */ +RobotState::RobotState() : RobotState{conf::GetConfig()} {} + +/** + * configures the robot state w/ field position, limelight configuration, and + * vision systems + */ +RobotState::RobotState(conf::RobotConfig cfg) + : field_to_robot_{800}, + ll_cfg_{cfg.limelight}, + debounced_vision_{}, + vision_null_ct_{0} {} + +/** + * gets the latest known field position of the robot + * @returns latest field to robot/position value + */ +std::pair RobotState::GetLatestFieldToRobot() { + return field_to_robot_.Latest(); +} + +/** + * gets the field positon of the robot at a given timestamp + * @returns field to robot/position and a given timestamp + */ +frc::Pose2d RobotState::GetFieldToRobot(units::second_t timestamp) { + return field_to_robot_.InterpAt(timestamp).second; +} + +/** + * Reads the stance/position of the robot at a given time and sets it to an + * array(timeline like?) + */ +void RobotState::ObserveFieldToRobot(units::second_t timestamp, + const frc::Pose2d& pose) { + field_to_robot_[timestamp] = pose; +} + +/** + * resets the robot's field positioning + */ +void RobotState::ResetFieldToRobot() { field_to_robot_.Inner().clear(); } + +/** + * Checks the robot's vision, resets if it can't find the target too many times + */ +void RobotState::ObserveVision(units::second_t timestamp, + std::optional target) { + if (!target.has_value()) { + if (vision_null_ct_ > kDroppableFrames) { + debounced_vision_.reset(); + } + vision_null_ct_++; + return; + } + vision_null_ct_ = 0; + debounced_vision_ = std::make_pair(timestamp, target.value()); +} + +/** + * calculates the cotangent of a given value + * @returns cotangent of given value + */ +inline units::scalar_t FastCotangent(units::radian_t r) { + auto const pi2 = units::radian_t{M_PI_2}; + return units::math::tan(pi2 - r); +} + +std::optional> +/** + * Gets the latest recorded distance (to the outer port?) from the robot vision if possible + * @returns the distance + */ +RobotState::GetLatestDistanceToOuterPort() { + if (!debounced_vision_.has_value()) { + return {}; + } + auto target = *debounced_vision_; + auto dist = + ll_cfg_.diff_height * + FastCotangent(target.second.vertical + ll_cfg_.angle_above_horizontal); + return std::make_pair(target.first, dist); +} + +std::optional> +/** + * Gets/calculates the angle at which the robot's vision is viewing an object(the outer port?) + * @returns the angle + */ +RobotState::GetLatestAngleToOuterPort() { + if (!debounced_vision_.has_value()) { + return {}; + } + auto target = *debounced_vision_; + return std::make_pair(target.first, target.second.horizontal); +} + +} // namespace c2020 +} // namespace team114 diff --git a/c2021 v2/src/main/cpp/shims/minimal_phoenix.h b/c2021 v2/src/main/cpp/shims/minimal_phoenix.h new file mode 100644 index 00000000..29fa0d48 --- /dev/null +++ b/c2021 v2/src/main/cpp/shims/minimal_phoenix.h @@ -0,0 +1,23 @@ +#pragma once + +//listen to josh rant lmao + +// this is a comproimise +// CTRE uses back-asswards namespace conventions, +// like we're in Java land, basically mandating we use +// using namespace declarations. +// The Phoenix.h header does this, but also brings in the whole library +// In reality, we need like two classes and associated data types. + +// Since basically every translation unit will have ctre stuff, +// this should reduce compilation times + +#include +#include + +using namespace ctre; +using namespace ctre::phoenix; +using namespace ctre::phoenix::motion; +using namespace ctre::phoenix::motorcontrol; +using namespace ctre::phoenix::motorcontrol::can; +using namespace ctre::phoenix::sensors; \ No newline at end of file diff --git a/c2021 v2/src/main/cpp/shims/navx_ahrs.h b/c2021 v2/src/main/cpp/shims/navx_ahrs.h new file mode 100644 index 00000000..0d3065c8 --- /dev/null +++ b/c2021 v2/src/main/cpp/shims/navx_ahrs.h @@ -0,0 +1,138 @@ +#pragma once + +// Kuailabs doesnt supply x86 binaries in their package. +// This header enables building on x86 by replacing the arm-only class +// with an empty interface + +#ifdef __arm__ + +#include "AHRS.h" + +#else + +#include "frc/I2C.h" +#include "frc/PIDSource.h" +#include "frc/SPI.h" +#include "frc/SerialPort.h" +#include "frc/smartdashboard/SendableBase.h" +#include "frc/smartdashboard/SendableBuilder.h" + +class AHRS /*: public frc::SendableBase, + public frc::ErrorBase, + public frc::PIDSource*/ +{ + public: + enum BoardAxis { + kBoardAxisX = 0, + kBoardAxisY = 1, + kBoardAxisZ = 2, + }; + + struct BoardYawAxis { + /* Identifies one of the board axes */ + BoardAxis board_axis; + /* true if axis is pointing up (with respect to gravity); false if + * pointing down. */ + bool up; + }; + + enum SerialDataType { + /** + * (default): 6 and 9-axis processed data + */ + kProcessedData = 0, + /** + * unprocessed data from each individual sensor + */ + kRawData = 1 + }; + + public: + AHRS(frc::SPI::Port spi_port_id) {} + AHRS(frc::I2C::Port i2c_port_id) {} + AHRS(frc::SerialPort::Port serial_port_id) {} + + AHRS(frc::SPI::Port spi_port_id, uint8_t update_rate_hz) {} + AHRS(frc::SPI::Port spi_port_id, uint32_t spi_bitrate, + uint8_t update_rate_hz) {} + + AHRS(frc::I2C::Port i2c_port_id, uint8_t update_rate_hz) {} + + AHRS(frc::SerialPort::Port serial_port_id, AHRS::SerialDataType data_type, + uint8_t update_rate_hz) {} + + float GetPitch() { return 0.0; } + float GetRoll() { return 0.0; } + float GetYaw() { return 0.0; } + float GetCompassHeading() { return 0.0; } + void ZeroYaw() {} + bool IsCalibrating() { return false; } + bool IsConnected() { return false; } + double GetByteCount() { return 0.0; } + double GetUpdateCount() { return 0.0; } + long GetLastSensorTimestamp() { return 0; } + float GetWorldLinearAccelX() { return 0.0; } + float GetWorldLinearAccelY() { return 0.0; } + float GetWorldLinearAccelZ() { return 0.0; } + bool IsMoving() { return false; } + bool IsRotating() { return false; } + float GetBarometricPressure() { return 0.0; } + float GetAltitude() { return 0.0; } + bool IsAltitudeValid() { return false; } + float GetFusedHeading() { return 0.0; } + bool IsMagneticDisturbance() { return false; } + bool IsMagnetometerCalibrated() { return false; } + float GetQuaternionW() { return 0.0; } + float GetQuaternionX() { return 0.0; } + float GetQuaternionY() { return 0.0; } + float GetQuaternionZ() { return 0.0; } + void ResetDisplacement() {} + void UpdateDisplacement(float accel_x_g, float accel_y_g, + int update_rate_hz, bool is_moving); + float GetVelocityX() { return 0.0; } + float GetVelocityY() { return 0.0; } + float GetVelocityZ() { return 0.0; } + float GetDisplacementX() { return 0.0; } + float GetDisplacementY() { return 0.0; } + float GetDisplacementZ() { return 0.0; } + double GetAngle() { return 0.0; } + double GetRate() { return 0.0; } + void SetAngleAdjustment(double angle) {} + double GetAngleAdjustment() { return 0.0; }; + void Reset() {} + float GetRawGyroX() { return 0.0; }; + float GetRawGyroY() { return 0.0; }; + float GetRawGyroZ() { return 0.0; }; + float GetRawAccelX() { return 0.0; }; + float GetRawAccelY() { return 0.0; }; + float GetRawAccelZ() { return 0.0; }; + float GetRawMagX() { return 0.0; }; + float GetRawMagY() { return 0.0; }; + float GetRawMagZ() { return 0.0; }; + float GetPressure() { return 0.0; }; + float GetTempC() { return 0.0; }; + AHRS::BoardYawAxis GetBoardYawAxis() { + return {BoardAxis::kBoardAxisX, true}; + } + std::string GetFirmwareVersion() { return ""; }; + + // bool RegisterCallback(ITimestampedDataSubscriber *callback, + // void *callback_context) { + // return false; + // } + // bool DeregisterCallback(ITimestampedDataSubscriber *callback) { + // return false; + // } + + int GetActualUpdateRate() { return 0; } + int GetRequestedUpdateRate() { return 0; } + + void EnableLogging(bool enable) {} + void EnableBoardlevelYawReset(bool enable) {} + bool IsBoardlevelYawResetEnabled() { return false; } + + int16_t GetGyroFullScaleRangeDPS() { return 0; } + int16_t GetAccelFullScaleRangeG() { return 0; } +}; + +#endif \ No newline at end of file diff --git a/c2021 v2/src/main/cpp/subsystems/drive.cpp b/c2021 v2/src/main/cpp/subsystems/drive.cpp new file mode 100644 index 00000000..6b60184a --- /dev/null +++ b/c2021 v2/src/main/cpp/subsystems/drive.cpp @@ -0,0 +1,370 @@ +#include "drive.h" + +#include + +#include +#include +#include + +#include + +#include "number_util.h" + +namespace team114 { +namespace c2020 { + +/** + * The constructor called if no config is passed in, + * in which case it calls the second constructor anyways by explicitly getting the drive config and passing it in +**/ +Drive::Drive() : Drive{conf::GetConfig().drive} {} + +/** + * The constructor that takes in a driveconfig to initialize all the drive's members, + * and the values of the members' variables +**/ +Drive::Drive(const conf::DriveConfig& cfg) + : left_master_{cfg.left_master_id}, + right_master_{cfg.right_master_id}, + left_slave_{cfg.left_slave_id}, + right_slave_{cfg.right_slave_id}, + cfg_{cfg}, + robot_state_{RobotState::GetInstance()}, + kinematics_{cfg.track_width}, + odometry_{{}}, + ramsete_{}, + vision_rot_{cfg_.orient_kp, + cfg_.orient_ki, + cfg_.orient_kd, + {2.0_rad / 1_s, 6.0_rad / 1.0_s / 1.0_s}, + 10_ms}, + has_vision_target_{false} { + conf::DriveFalconCommonConfig(left_master_); + conf::DriveFalconCommonConfig(right_master_); + conf::DriveFalconCommonConfig(left_slave_); + conf::DriveFalconCommonConfig(right_slave_); + left_master_.SetInverted(false); + right_master_.SetInverted(true); + left_slave_.Follow(left_master_); + right_slave_.Follow(right_master_); + left_slave_.SetInverted(InvertType::FollowMaster); + right_slave_.SetInverted(InvertType::FollowMaster); + + conf::SetDriveMasterFramePeriods(left_master_); + conf::SetDriveMasterFramePeriods(right_master_); + conf::SetDriveSlaveFramePeriods(left_slave_); + conf::SetDriveSlaveFramePeriods(right_slave_); + CheckFalconFramePeriods(); + + vision_rot_.SetIntegratorRange(-0.05, 0.05); + vision_rot_.SetTolerance(0.02_rad, 0.2_rad / 1.0_s); +} + +/** + * The first method called in Periodic(), in which the slaves are checked whether a reset has occured + * If a slave was reset, its frame period will once again be set to the correct value like in the constructor + * A counter will also be ticked to show how many times the falcons have been reset + * + * https://phoenix-documentation.readthedocs.io/en/latest/ch18_CommonAPI.html#can-bus-utilization-error-metrics +**/ +void Drive::CheckFalconFramePeriods() { + if (left_master_.HasResetOccurred()) { + conf::SetDriveMasterFramePeriods(left_master_); + falcon_reset_count_++; + } + if (right_master_.HasResetOccurred()) { + conf::SetDriveMasterFramePeriods(right_master_); + falcon_reset_count_++; + } + if (left_slave_.HasResetOccurred()) { + conf::SetDriveSlaveFramePeriods(left_slave_); + falcon_reset_count_++; + } + if (right_slave_.HasResetOccurred()) { + conf::SetDriveSlaveFramePeriods(right_slave_); + falcon_reset_count_++; + } +} + +/** + * This method is the most important as it is periodically called, and is what allows the robot to move + * After checking for motor resets, the current state will be updated + * Depending on the state, different cotnrollers will be updated such that the robot can perform the movement necessary + * to follow a path or orient the shooter. + * Outputs will also be written for analysis +**/ +void Drive::Periodic() { + CheckFalconFramePeriods(); + UpdateRobotState(); + switch (state_) { + case DriveState::OPEN_LOOP: + // TODO(josh) + break; + case DriveState::FOLLOW_PATH: + UpdatePathController(); + break; + case DriveState::SHOOT_ORIENT: + UpdateOrientController(); + default: + // TODO(josh) log here + break; + } + WriteOuts(); +} + +/** + * Empty method, presumably was supposed to be used to stop the drive +**/ +void Drive::Stop() {} + +/** + * Resets the position of sensors on the drive +**/ +void Drive::ZeroSensors() { + robot_state_.ResetFieldToRobot(); + WaitForNavxCalibration(0.5); + navx_.ZeroYaw(); + left_master_.SetSelectedSensorPosition(0); + right_master_.SetSelectedSensorPosition(0); + odometry_.ResetPosition({}, GetYaw()); +} + +/** + * Gives the NavX (navigation sensor for field oriented, auto balancing, collision detection, etc...) a time interval to calibrate + * Calibration retries, failures, and successes are supposed to be log, but atm nothing has been written to log it +**/ + void Drive::WaitForNavxCalibration(double timeout_sec) { + frc::Timer time; + time.Start(); + while (navx_.IsCalibrating() && time.Get() < timeout_sec) { + // LOG calib retry + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + if (navx_.IsCalibrating()) { + // LOG failed to calibrate + } else { + // LOG success + } +} + +/** + * Empty method, presumably to ... output telemetry +**/ +void Drive::OutputTelemetry() {} + +/** + * Adds the drive trajectory it wants to move at, and changes the current state to follow that path +**/ +void Drive::SetWantDriveTraj(frc::Trajectory&& traj) { + curr_traj_.emplace(std::move(traj)); + state_ = DriveState::FOLLOW_PATH; + traj_timer.Reset(); + traj_timer.Start(); +} + +/** + * Updates the odometry of the drive, such that the robot's relative position and pose can be predicted +**/ +void Drive::UpdateRobotState() { + odometry_.Update(GetYaw(), GetEncoder(left_master_), + GetEncoder(right_master_)); + robot_state_.ObserveFieldToRobot(frc2::Timer::GetFPGATimestamp(), + odometry_.GetPose()); +} + +/** + * Updates the path controller such that it'll correctly follow the trajectory the driver has set for it +**/ +void Drive::UpdatePathController() { + if (!curr_traj_.has_value()) { + // LOG + } + if (FinishedTraj()) { + curr_traj_.reset(); + SetWantRawOpenLoop({0.0_mps, 0.0_mps}); + } + auto time_along = traj_timer.Get(); + auto desired = curr_traj_.value().Sample(units::second_t{time_along}); + auto chassis_v = ramsete_.Calculate( + robot_state_.GetLatestFieldToRobot().second, desired); + auto wheel_v = kinematics_.ToWheelSpeeds(chassis_v); + // convert into falcon 500 internal encoder ticks + auto MetersPerSecToTicksPerDecisec = + [this](units::meters_per_second_t meters_per_second) -> double { + return meters_per_second / cfg_.meters_per_falcon_tick * 1_s / 10.0; + }; + // TODO(josh) consider using WPILib + // SimpleMotorFeedForward to add kA term + pout_.control_mode = ControlMode::Velocity; + pout_.left_demand = MetersPerSecToTicksPerDecisec(wheel_v.left); + pout_.right_demand = MetersPerSecToTicksPerDecisec(wheel_v.right); +} + +bool Drive::BackUp(double dist) { //units are meters + //do pid but until desired # of ticks works + double wheel_diameter = 5.75; //inches + double circumference = wheel_diameter*M_PI; + double rotations = dist/circumference; + double ticks = -rotations*22000; + + double ticks_gone = left_master_.GetSelectedSensorPosition(); + double Kp = -0.00003; + double Ki = -0.00002; + double error = abs(ticks) - abs(ticks_gone); + double correction = Kp*error + Ki; + + pout_.control_mode = ControlMode::PercentOutput; + pout_.left_demand = correction; + pout_.right_demand = correction; + + std::cout << "ticks: " << ticks << std::endl; + std::cout << "ticks gone: " << ticks_gone << std::endl; + std::cout << "correction: " << correction << std::endl; + + return abs(correction) < 0.1; + +} + + +/** + * Changes the current state of the robot to orient its vision sensor for a shot, and sets the angle at which the sensor should be rotated to +**/ +/*void Drive::SetWantOrientForShot(Limelight& limelight, double Kp, double Ki, double Kd) { + state_ = DriveState::SHOOT_ORIENT; + vision_rot_.SetGoal(0.0_rad); + + Kp = 0.017; + Ki = 0.015; + + double x_off = limelight.GetNetworkTable()->GetNumber("tx", 0.0); + + //auto position robot so x_off is reasonable + //https://docs.limelightvision.io/en/latest/cs_aiming.html + double heading_error = -x_off; + double steering_adjust = 0.0; + steering_adjust = Kp*heading_error + Ki; + if (steering_adjust < -1) steering_adjust = 1; + if (steering_adjust > 1) steering_adjust = 1; + // std::cout <<"x offset: " << x_off << std::endl; + std::cout << "steering adjust: " << steering_adjust << std::endl; + pout_.control_mode = ControlMode::PercentOutput; + pout_.left_demand = steering_adjust; + pout_.right_demand = -steering_adjust; +}*/ + +/** + * Returns true if the vision sensor has correctly rotated to the position set +**/ +/*bool Drive::OrientedForShot(Limelight& limelight) { + double x_off = limelight.GetNetworkTable()->GetNumber("tx", 0.0); + std::cout << x_off << std::endl; + return (x_off < 2); +}*/ + +/** + * Called if the current drive state is to orient for a shot + * It updates the vision controller, which determines the movement the vision sensor takes towards the goal +**/ +void Drive::UpdateOrientController() { + /* auto latest = robot_state_.GetLatestAngleToOuterPort(); + if (!latest.has_value()) { + return; + has_vision_target_ = false; + } + READING_SDB_NUMERIC(double, RotKp) kp; + READING_SDB_NUMERIC(double, RotKi) ki; + READING_SDB_NUMERIC(double, RotKd) kd; + vision_rot_.SetPID(kp, ki, kd); + READING_SDB_NUMERIC(double, RotVel) vel; + READING_SDB_NUMERIC(double, RotAcc) acc; + vision_rot_.SetConstraints({static_cast(vel) * 1_rad / 1_s, + static_cast(acc) * 1_rad / 1_s / 1_s}); + // std::cout << "read gains " << kp << " " << ki << " " << kd << " " << vel << " " << acc << std::endl; + has_vision_target_ = true; + auto err = latest.value().second; + double demand = vision_rot_.Calculate(err); + pout_.control_mode = ControlMode::PercentOutput; + pout_.left_demand = -demand; + pout_.right_demand = demand; + // std::cout << "orient w/ tgt, dmd " << err << " " << demand << std::endl; */ +} + +/** + * Returns true if the current trajectory variable does not have a value, meaning it is not currently moving +**/ +bool Drive::FinishedTraj() { + if (!curr_traj_.has_value()) { + // not running one at the moment + return true; + } + // TODO(josh) find a better way? + return traj_timer.Get() > curr_traj_.value().TotalTime(); +} + +/** + * Sets the drive's state to an open loop, meaning nothing gets updated everytime Periodic() is called +**/ +void Drive::SetWantRawOpenLoop( + const frc::DifferentialDriveWheelSpeeds& openloop) { + state_ = DriveState::OPEN_LOOP; + pout_.control_mode = ControlMode::PercentOutput; + pout_.left_demand = openloop.left.to(); + pout_.right_demand = openloop.right.to(); +} + +/** + * Taken from 254, it changes the controls and feel of manipulating the drive to that of a curvature drive + * https://www.reddit.com/r/FRC/comments/80679m/what_is_curvature_drive_cheesy_drive/ +**/ +void Drive::SetWantCheesyDrive(double throttle, double wheel, bool quick_turn) { + throttle = Deadband(throttle, 0.05); + wheel = Deadband(wheel, 0.035); + + constexpr double kWheelGain = 1.97; + constexpr double kWheelNonlinearity = 0.05; + const double denominator = std::sin(M_PI / 2.0 * kWheelNonlinearity); + // Apply a sin function that's scaled to make it feel better. + if (!quick_turn) { + wheel = std::sin(M_PI / 2.0 * kWheelNonlinearity * wheel); + wheel = std::sin(M_PI / 2.0 * kWheelNonlinearity * wheel); + wheel = wheel / (denominator * denominator) * std::abs(throttle); + } + wheel *= kWheelGain; + frc::DifferentialDriveWheelSpeeds signal = kinematics_.ToWheelSpeeds( + frc::ChassisSpeeds{units::meters_per_second_t{throttle}, 0.0_mps, + units::radians_per_second_t{wheel}}); + signal.Normalize(1.0_mps); + SetWantRawOpenLoop(signal); +} + +/** + * Writes the relevant outputs +**/ +void Drive::WriteOuts() { + // SDB_NUMERIC(double, LeftDriveTalonDemand){pout_.left_demand}; + // SDB_NUMERIC(double, RightDriveTalonDemand){pout_.right_demand}; + left_master_.Set(pout_.control_mode, pout_.left_demand); + right_master_.Set(pout_.control_mode, pout_.right_demand); + // if (pout_.left_demand == 0 && pout_.right_demand == 0) return; + // std::cout << "write out left demand: " << pout_.left_demand << std::endl; + // std::cout << "write out right demand: " << pout_.right_demand << std::endl; +} + +/** + * Gets the rotation of the drive in terms of radians +**/ +constexpr auto RAD_PER_DEGREE = units::constants::pi * 1_rad / 180.0; +frc::Rotation2d Drive::GetYaw() { + return frc::Rotation2d(navx_.GetYaw() * RAD_PER_DEGREE); +} + +/** + * Simple getter method to get the encoder object of the Talon motor controller +**/ +units::meter_t Drive::GetEncoder(TalonFX& master_talon) { + return (static_cast(master_talon.GetSelectedSensorPosition())) * + cfg_.meters_per_falcon_tick; +} + +} // namespace c2020 +} // namespace team114 diff --git a/c2021 v2/src/main/cpp/subsystems/drive.h b/c2021 v2/src/main/cpp/subsystems/drive.h new file mode 100644 index 00000000..3b888220 --- /dev/null +++ b/c2021 v2/src/main/cpp/subsystems/drive.h @@ -0,0 +1,96 @@ +#pragma once + +#include + +#include "minimal_phoenix.h" + +#include +#include +#include +#include +#include + +#include + +#include "config.h" +#include "robot_state.h" +#include "navx_ahrs.h" +#include "subsystem.h" +//#include "util/sdb_types.h" + +namespace team114 { +namespace c2020 { + +class Drive : public Subsystem { + DISALLOW_COPY_ASSIGN(Drive) + CREATE_SINGLETON(Drive) + public: + Drive(); + Drive(const conf::DriveConfig& cfg); + void Periodic() override; + void Stop() override; + void ZeroSensors() override; + void OutputTelemetry() override; + + void SetWantDriveTraj(frc::Trajectory&& traj); + bool FinishedTraj(); + + // Assumes range of -1 to 1 mps max speed + void SetWantRawOpenLoop(const frc::DifferentialDriveWheelSpeeds& openloop); + void SetWantCheesyDrive(double throttle, double wheel, bool quick_turn); + + // void SetWantOrientForShot(Limelight& limelight, double Kp, double Ki, double Kd); + // bool OrientedForShot(Limelight& limelight); + + bool BackUp(double dist); + + TalonFX left_master_, right_master_; //this is my code, and I do what I want + AHRS navx_{frc::SPI::Port::kMXP}; + + struct PeriodicOut { + ControlMode control_mode; + double left_demand; + double right_demand; + }; + PeriodicOut pout_{}; + + private: + enum class DriveState { + OPEN_LOOP, + FOLLOW_PATH, + SHOOT_ORIENT, + }; + void CheckFalconFramePeriods(); + + void UpdateRobotState(); + void UpdatePathController(); + void UpdateOrientController(); + + frc::Rotation2d GetYaw(); + units::meter_t GetEncoder(TalonFX& master_talon); + void WaitForNavxCalibration(double timeout_sec); + + // SDB_NUMERIC(unsigned int, DriveFalconResetCount) falcon_reset_count_{0}; + unsigned int falcon_reset_count_ = 0; //temporary? can't tell what line above is supposed to do + + //TalonFX left_master_, right_master_; + TalonFX left_slave_, right_slave_; + + void WriteOuts(); + + const conf::DriveConfig cfg_; + DriveState state_{DriveState::OPEN_LOOP}; + RobotState& robot_state_; + + frc::DifferentialDriveKinematics kinematics_; + frc::DifferentialDriveOdometry odometry_; + frc::RamseteController ramsete_; + std::optional curr_traj_; + frc2::Timer traj_timer{}; + + frc::ProfiledPIDController vision_rot_; + bool has_vision_target_; +}; + +} // namespace c2020 +} // namespace team114 \ No newline at end of file diff --git a/c2021 v2/src/main/cpp/subsystems/limelight.cpp b/c2021 v2/src/main/cpp/subsystems/limelight.cpp new file mode 100644 index 00000000..a52b71e7 --- /dev/null +++ b/c2021 v2/src/main/cpp/subsystems/limelight.cpp @@ -0,0 +1,90 @@ +#include "limelight.h" + +#include "robot_state.h" +#include "number_util.h" + +namespace team114 { +namespace c2020 { + +Limelight::Limelight() : Limelight(conf::GetConfig().limelight) {} + +Limelight::Limelight(conf::LimelightConfig& cfg) : cfg_{std::move(cfg)} { + network_table_ = + nt::NetworkTableInstance::GetDefault().GetTable(cfg_.table_name); +} + +void Limelight::Periodic() { + ReadPeriodicIn(); + WritePeriodicOut(); + RobotState::GetInstance().ObserveVision(frc2::Timer::GetFPGATimestamp(), + GetTarget()); +} + +void Limelight::ReadPeriodicIn() { + per_in_.latency = + network_table_->GetNumber("tl", 0) * 1_ms + kImageCaptureLatency; + per_in_.led_mode = (int)network_table_->GetNumber("ledMode", 1.0); + per_in_.pipeline = (int)network_table_->GetNumber("pipeline", 0); + per_in_.x_off = network_table_->GetNumber("tx", 1000.0); + per_in_.y_off = network_table_->GetNumber("ty", 1000.0); + per_in_.area = network_table_->GetNumber("ta", 1000.0); + // paranoid about double equality... + sees_target_ = network_table_->GetNumber("tv", 0) > 0.5; +} + +void Limelight::WritePeriodicOut() { + if (per_out_.led_mode != per_in_.led_mode || + per_out_.pipeline != per_in_.pipeline) { + // LOG + per_out_dirty_ = true; + } + if (per_out_dirty_) { + network_table_->PutNumber("ledMode", per_out_.led_mode); + network_table_->PutNumber("camMode", per_out_.cam_mode); + network_table_->PutNumber("pipeline", per_out_.pipeline); + network_table_->PutNumber("stream", per_out_.stream); + network_table_->PutNumber("snapshot", per_out_.snapshot); + per_out_dirty_ = false; + } +} + +void Limelight::SetLedMode(LedMode mode) { + int mode_int = static_cast(mode); + if (mode_int != per_out_.led_mode) { + per_out_.led_mode = mode_int; + per_out_dirty_ = true; + } +} + +void Limelight::SetPipeline(int pipeline) { + if (pipeline != per_out_.pipeline) { + // TODO reset veision here + // RobotState::GetInstance().ResetVision(); + per_out_.pipeline = pipeline; + // LOG + per_out_dirty_ = true; + } +} + +int Limelight::GetPipeline() { return per_out_.pipeline; } + +void Limelight::ForceFlushOuts() { per_out_dirty_ = true; } + +bool Limelight::SeesTarget() { return sees_target_; } + +units::second_t Limelight::GetLatency() { return per_in_.latency; } + +std::shared_ptr Limelight::GetNetworkTable() { return network_table_; } + +std::optional Limelight::GetTarget() { + if (SeesTarget()) { + TargetInfo t; + t.horizontal = DegToRad(-per_in_.x_off); + t.vertical = DegToRad(per_in_.y_off); + return t; + } + return {}; +} + +} // namespace c2020 +} // namespace team114 diff --git a/c2021 v2/src/main/cpp/subsystems/limelight.h b/c2021 v2/src/main/cpp/subsystems/limelight.h new file mode 100644 index 00000000..6d046125 --- /dev/null +++ b/c2021 v2/src/main/cpp/subsystems/limelight.h @@ -0,0 +1,97 @@ +#pragma once + +#include +#include +#include + +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Translation2d.h" +#include "frc/smartdashboard/SmartDashboard.h" +#include "frc2/Timer.h" + +#include "networktables/NetworkTable.h" +#include "networktables/NetworkTableInstance.h" + +#include + +#include "config.h" +#include "subsystem.h" + +namespace team114 { +namespace c2020 { + +class Limelight : public Subsystem { + public: + // SUBSYSTEM_PRELUDE(Limelight); + const units::second_t kImageCaptureLatency = 11_ms; + + struct RawTargetInfo { + double x = 1.0; + double y; + double z; + }; + struct TargetInfo { + units::radian_t horizontal; + units::radian_t vertical; + }; + const conf::LimelightConfig cfg_; + + enum class LedMode : int { + PIPELINE = 0, + OFF = 1, + BLINK = 2, + ON = 3, + }; + + Limelight(); + Limelight(conf::LimelightConfig& cfg); + + void Periodic() final override; + + void SetLedMode(LedMode mode); + + void SetPipeline(int pipeline); + + int GetPipeline(); + + void ForceFlushOuts(); + + bool SeesTarget(); + + units::second_t GetLatency(); + + std::shared_ptr GetNetworkTable(); + + std::optional GetTarget(); + + private: + void ReadPeriodicIn(); + + void WritePeriodicOut(); + + std::shared_ptr network_table_; + + struct PeriodicIn { + units::second_t latency; + int led_mode; + int pipeline; + double x_off; + double y_off; + double area; + }; + struct PeriodicOut { + int led_mode = 1; // 0 - use pipeline mode, 1 - off, 2 - blink, 3 - on + int cam_mode = 0; // 0 - vision processing, 1 - driver camera + int pipeline = 0; // 0 - 9 + int stream = 2; // sets stream layout if another webcam is attached + int snapshot = 0; // 0 - stop snapshots, 1 - 2 Hz + }; + PeriodicIn per_in_; + PeriodicOut per_out_; + bool per_out_dirty_ = true; + bool sees_target_ = false; +}; + +} // namespace c2020 +} // namespace team114 \ No newline at end of file diff --git a/c2021 v2/src/main/include/Drivetrain.h b/c2021 v2/src/main/include/Drivetrain.h new file mode 100644 index 00000000..55281f50 --- /dev/null +++ b/c2021 v2/src/main/include/Drivetrain.h @@ -0,0 +1,25 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "ctre/Phoenix.h" + +class Drivetrain { + public: + Drivetrain(); + void Drive(); + void UpdateOdometry(); + + private: + TalonFX left_master = {0}; + TalonFX left_slave = {1}; + TalonFX right_master = {2}; + TalonFX right_slave = {3}; + +}; + + diff --git a/c2021 v2/src/main/include/Robot.h b/c2021 v2/src/main/include/Robot.h index 9b87d518..554437db 100644 --- a/c2021 v2/src/main/include/Robot.h +++ b/c2021 v2/src/main/include/Robot.h @@ -1,51 +1,50 @@ #pragma once -#include - -#include -#include -#include #include -#include -#include +#include +#include + +#include "robot_state.h" +#include "drive.h" -#include "ctre/Phoenix.h" -#include "frc/WPILib.h" -#include +#include -#include -#include -#include -#include -#include +#include "controls.h" -#include "Drive.h" -#include "Shoot.h" -//Drive -frc::Joystick l_joy{0}; -frc::Joystick r_joy{1}; -frc::XboxController xbox{2}; +namespace team114 { +namespace c2020 { class Robot : public frc::TimedRobot { - public: + public: + inline static const auto kPeriod = 10_ms; + Robot(); void RobotInit() override; void RobotPeriodic() override; + void AutonomousInit() override; void AutonomousPeriodic() override; + void TeleopInit() override; void TeleopPeriodic() override; - void DisabledInit() override; - void DisabledPeriodic() override; + void TestInit() override; void TestPeriodic() override; - private: - frc::SendableChooser m_chooser; - const std::string kAutoNameDefault = "Default"; - const std::string kAutoNameCustom = "My Auto"; - std::string m_autoSelected; + void DisabledInit() override; + void DisabledPeriodic() override; + + private: + Controls controls_; + Drive& drive_; + RobotState& robot_state_; + frc::Joystick ljoy_; + frc::Joystick rjoy_; + frc::Joystick ojoy_; + conf::RobotConfig cfg; - Drive Drivetrain; - Shoot Shooter; + // CachingSolenoid brake_{frc::Solenoid{6}}; }; + +} // namespace c2020 +} // namespace team114 \ No newline at end of file diff --git a/c2021 v2/src/main/include/Shooter.h b/c2021 v2/src/main/include/Shooter.h new file mode 100644 index 00000000..77e81455 --- /dev/null +++ b/c2021 v2/src/main/include/Shooter.h @@ -0,0 +1,2 @@ +#pragma once + diff --git a/c2021 v2/src/main/include/config.h b/c2021 v2/src/main/include/config.h new file mode 100644 index 00000000..fd8a4c55 --- /dev/null +++ b/c2021 v2/src/main/include/config.h @@ -0,0 +1,172 @@ +#pragma once + +#include + +#include +#include "minimal_phoenix.h" //need to take care of + +namespace team114 { +namespace c2020 { +namespace conf { + +struct DriveConfig { + int left_master_id; /** < talon id of master drive motor on left side **/ + int left_slave_id; /** < talon id of slave drive motor on left side **/ + int right_master_id; /** < talon id of master drive motor on right side **/ + int right_slave_id; /** < talon id of slave drive motor on right side **/ + units::meter_t track_width; + units::meter_t meters_per_falcon_tick; /**< self explanitory, look up what a frc falcon tick is **/ + units::meters_per_second_t traj_max_vel; + units::meters_per_second_squared_t traj_max_accel; + units::meters_per_second_squared_t traj_max_centrip_accel; + double orient_kp; + double orient_ki; + double orient_kd; + double orient_vel; + double orient_acc; +}; + +struct ControlPanelConfig { + int talon_id; /** < talon id for control panel motor **/ + double current_limit; /** < current limit (amps) for said motor **/ + double ticks_per_inch; /**< self explanitory, look up what a frc falcon tick is **/ + double kP; /**< P constant for this motor's pid **/ + double kI; /**< I constant for this motor's pid **/ + double kD; /**< D constant for this motor's pid **/ + double ticks_per_color_slice; /**< # of ticks to rotate the color wheel one "slice" (which is a specific angle) **/ + double rot_control_ticks; + double scoot_cmd; + int solenoid_channel; /**< I think the wheel is lifted with a pneumatic, and this is likely its solenoid **/ + std::string sdb_key; +}; + +struct IntakeConfig { + int rot_talon_id; + int roller_talon_id; + double intake_cmd; /**< percent output for when intake is running **/ + double rot_current_limit; /**< current limit (amps) for rotator motor **/ + double roller_current_limit; /**< current limit (amps) for roller motor **/ + double zeroing_kp; + double zeroing_vel; + double abs_enc_tick_offset; + double abs_ticks_per_rot; + double rel_ticks_per_abs_tick; + double rads_per_rel_tick; + double zeroed_rad_from_vertical; + double SinekF; + double kP; + double kI; + double kD; + double profile_vel; + double profile_acc; + int stowed_pos_ticks; + int intaking_pos_ticks; + // int trench_driving_pos_ticks; +}; + +struct HoodConfig { + int talon_id; /**< talon id of motor that moves the shooter hood **/ + double ticks_per_degree; /**< self explanitory, look up what a frc falcon tick is **/ + double min_degrees; /** < smallest angle hood can move to **/ + double max_degrees; /** < largest angle hood can move to **/ + double current_limit; /** < current limit (amps) for hood motor **/ + double zeroing_current; + double zeroing_kp; + double zeroing_vel; + double profile_acc; + double profile_vel; + double kP; + double kI; + double kD; + int ctre_curve_smoothing; +}; + + +struct ShooterConifg { + int master_id; /**< The shooter flywheel takes 2 motors to spin, this is the master's id. left of shooter **/ + int slave_id; /**< The respective slave ID. Right of shooter. **/ + int turret_id; + double shooter_current_limit; /**< The current limit (amps) of the talons **/ + double kF; /**< F (feedforward) term in shooter PID **/ + double kP; /**< P term in shooter PID **/ + double kI; /**< I term in shooter PID **/ + double kD; /**< D term in shooter PID **/ + VelocityMeasPeriod meas_period; /**< Talons can return "velocity". Velocity is measured every period and averaged. */ + int meas_filter_width; /**< Helps with velocity measurement. **/ + double shootable_err_pct; /**< Perhaps ammount of error acceptable when trying to make a shot? idk josh wrote this and we now have a new system*/ + int kicker_id; /**< Motor/wheel that spins weels into shooter wheel. **/ + double kicker_current_limit; /**< Current limit (amps) for kicker talon (small green wheel below flywheel, helps feed balls into shooter) */ + double kicker_cmd; /**< Percent output for kicker talon when engaged **/ +}; + +struct BallChannelConfig { + int serializer_id; /**< Talon ID of serializing talon motor. Serializer is in front of robot, it feeds balls into channel after they are intaked**/ + int channel_id; /**< Talon id of the channel motor **/ + double current_limit; /**< The current limit (amps) of the talons **/ + double serializer_cmd; /**< percent output to serializing talon when it is turned on **/ + double channel_cmd; /**< percent output to channel talon when it is turned on **/ + int s0_port; /**< we use sensors to track the balls in the channel. this is one of them **/ + int s1_port; /**< we use sensors to track the balls in the channel. this is one of them **/ + int s2_port; /**< antiquated, we don't use this sensor **/ + int s3_port; /**< antiquated, we don't use this sensor **/ +}; + +struct ClimberConfig { + int master_id; /**< ID of master talon motor **/ + int slave_id; /**< ID of slave talon motor **/ + double current_limit; /**< The current limit (amps) of the talons **/ + // spool winding makes this weird + double avg_ticks_per_inch; /**< Probably number of ticks on motors that pass for climber to raise one inch. **/ + int release_solenoid_id; /**< ID of solenoid that controls the release pneumatic (to raise/lower climber) **/ + int brake_solenoid_id; /**< ID of brake solenoid, I think it's purpose is to hold the climber in position? **/ + int initial_step_ticks; /**< Probably the amount of ticks the motors should turn initially to raise the climber. **/ + int forward_soft_limit_ticks; /**< Probably a soft limit for how many ticks the motors should turn. **/ + double ascend_command; /**< Percent output to talons to raise climber (positive value) **/ + double descend_command; /**< Percent output to talons to lower climber (negative value) **/ +}; + +struct LimelightConfig { + std::string name; /**< Unused? Name of limelight**/ + std::string table_name; /**< Name used for limelight NetworkTable**/ + units::meter_t diff_height; /**< Not sure, unused **/ + units::radian_t angle_above_horizontal; /**< limelight isn't exaclty level; this is the offset angle **/ +}; + +struct RobotConfig { + std::string mac_address; /**< For identifying roboRIO **/ + DriveConfig drive; /**< For the drive base, go to DriveConfig for more info **/ + ControlPanelConfig ctrl_panel; /**< For control panel, go to ControlPanelConfig for more info **/ + IntakeConfig intake; /**< For the intake, go to IntakeConfig for more info **/ + HoodConfig hood; /**< For the shooter hood, go to HoodConfig for more info **/ + ShooterConifg shooter; /**< Perhaps scrapped, but for entire shooter. Go to ShooterConifg for more info **/ + BallChannelConfig ball_channel; /**< For the ball channel, go to BallChannelConfig for more info **/ + ClimberConfig climber; /**< For the climber, go to ClimberConfig for more info **/ + LimelightConfig limelight; /**< For the limelight, go to LimelightConfig for more info **/ +}; + +RobotConfig& GetConfig(); + +void DriveFalconCommonConfig(TalonFX& falcon); + +void SetDriveMasterFramePeriods(TalonFX& falcon); + +void SetDriveSlaveFramePeriods(TalonFX& falcon); + +enum class FeedbackType { + Quadrature, + PulseWidth, + Both, + None, +}; + +void SetFramePeriodsForPidTalon(TalonSRX& talon, FeedbackType feedback_type = FeedbackType::None); +void SetFramePeriodsForPidTalonFX(TalonFX& talon, FeedbackType feedback_type = FeedbackType::None); + +void SetFramePeriodsForOpenLoopTalon(TalonSRX& talon); + +void SetFramePeriodsForSlaveTalon(TalonSRX& talon); +void SetFramePeriodsForSlaveTalonFX(TalonFX& talon); + +} // namespace conf +} // namespace c2020 +} // namespace team114 \ No newline at end of file diff --git a/c2021 v2/src/main/include/controls.h b/c2021 v2/src/main/include/controls.h new file mode 100644 index 00000000..23eb012b --- /dev/null +++ b/c2021 v2/src/main/include/controls.h @@ -0,0 +1,91 @@ +#pragma once + +#include + +namespace team114 { +namespace c2020 { + +class EdgeDetector { + public: + bool Pressed(bool observed) { + bool to_ret = observed && !last_; + last_ = observed; + return to_ret; + } + + private: + bool last_ = false; +}; + +class ScootStateTracker { + public: + bool PassThroughFeed(bool val) { + if (val) { + been_fed_ = true; + } + return val; + } + bool WasNotHeld() { + bool to_ret = !been_fed_; + been_fed_ = false; + return to_ret; + } + + private: + bool been_fed_ = true; +}; + +class Controls { + public: + Controls(); + + static constexpr double kTriggerThreshold = 0.3; + static constexpr double kAnalogJoyThreshold = 0.3; + + void OPrints(); + + double Throttle(); + double Wheel(); + bool QuickTurn(); + + bool ClimbUp(); + bool ClimbDown(); + + bool ShotShortPressed(); + bool ShotMedPressed(); + bool ShotLongPressed(); + + bool Intake(); + bool Unjam(); + bool Shoot(); + + bool ShortShot(); + bool LongShot(); + + bool PanelDeploy(); + bool ScootLeftPressed(); + bool ScootRightPressed(); + bool ScootReleased(); + bool RotControlPressed(); + bool PosControlRedPressed(); + bool PosControlYellowPressed(); + bool PosControlGreenPressed(); + bool PosControlBluePressed(); + + private: + frc::Joystick ljoy_; + frc::Joystick rjoy_; + frc::Joystick ojoy_; + + EdgeDetector shot_short_; + EdgeDetector shot_med_; + EdgeDetector shot_long_; + + EdgeDetector scoot_left_; + EdgeDetector scoot_right_; + ScootStateTracker scoot_tracker_; + EdgeDetector scoot_released_; +}; + +} // namespace c2020 +} // namespace team114 \ No newline at end of file diff --git a/c2021 v2/src/main/include/robot_state.h b/c2021 v2/src/main/include/robot_state.h new file mode 100644 index 00000000..efd48faf --- /dev/null +++ b/c2021 v2/src/main/include/robot_state.h @@ -0,0 +1,51 @@ +#pragma once + +#include + +#include + +#include "config.h" +#include "subsystem.h" +#include "limelight.h" +#include "util/construcotr_macros.h" +#include "util/interp_map.h" + +namespace team114 { +namespace c2020 { + +// Saying frame1_to_frame2 represents the transform applied to the frame1 origin +// that will bring it to the frame2 origin. +class RobotState { + DISALLOW_COPY_ASSIGN(RobotState) + CREATE_SINGLETON(RobotState) + public: + RobotState(); + RobotState(conf::RobotConfig cfg); + + std::pair GetLatestFieldToRobot(); + frc::Pose2d GetFieldToRobot(units::second_t); + void ObserveFieldToRobot(units::second_t timestamp, + const frc::Pose2d& pose); + void ResetFieldToRobot(); + + void ObserveVision(units::second_t timestamp, + std::optional target); + std::optional> + GetLatestDistanceToOuterPort(); + std::optional> + GetLatestAngleToOuterPort(); + + private: + InterpolatingMap, Pose2dInterp> + field_to_robot_; + + const conf::LimelightConfig ll_cfg_; + std::optional> + debounced_vision_; + const unsigned int kDroppableFrames = 3; + unsigned int vision_null_ct_; +}; + +} // namespace c2020 +} // namespace team114 \ No newline at end of file diff --git a/c2021 v2/src/main/include/subsystem.h b/c2021 v2/src/main/include/subsystem.h new file mode 100644 index 00000000..41222813 --- /dev/null +++ b/c2021 v2/src/main/include/subsystem.h @@ -0,0 +1,59 @@ +#pragma once + +//#include "util/constructor_macros.h" weird josh stuff + +namespace team114 { +namespace c2020 { + +class Subsystem { + public: + /** + * Subsystem is an abstract class (and all following functions are abstract as well). An empty constructor. + **/ + virtual ~Subsystem() = default; + + /** + * Called in Robot::TeleopPeriodic() if it makes sense (hood and intake yes, control panel(unfinished?) and climber no) + **/ + virtual void Periodic(){}; + /** + * Resets things (like setting motors back to initial position) + **/ + virtual void Stop(){}; + /** + * Presumably to reset sensors, although it currently isn't used for much in the subclasses. + **/ + virtual void ZeroSensors(){}; + /** + * Currently undefined in subclasses, but it could presumably be used to output general positional or sensory data. + **/ + virtual void OutputTelemetry(){}; +}; + +// static member is inline so as to be single across translation units +// https://stackoverflow.com/a/53705993 +#define CREATE_SINGLETON(Classname) \ + private: \ + inline static Classname* __singleton_instance_; \ + \ + public: \ + static Classname& GetInstance() { \ + if (__singleton_instance_ == nullptr) { \ + __singleton_instance_ = new Classname(); \ + } \ + return *__singleton_instance_; \ + } \ + static void DestroyInstance() { \ + delete __singleton_instance_; \ + __singleton_instance_ = nullptr; \ + } + +#define SUBSYSTEM_PRELUDE(Classname) \ + private: \ + Classname(); \ + CREATE_SINGLETON(Classname) \ + public: /* better diagnostics if public */ \ + DISALLOW_COPY_ASSIGN(Classname) + +} // namespace c2020 +} // namespace team114 \ No newline at end of file diff --git a/c2021 v2/src/main/include/util/caching_types.h b/c2021 v2/src/main/include/util/caching_types.h new file mode 100644 index 00000000..dd91e894 --- /dev/null +++ b/c2021 v2/src/main/include/util/caching_types.h @@ -0,0 +1,34 @@ +#pragma once + +#include + +#include + +namespace team114 { +namespace c2020 { + +class CachingSolenoid { + public: + CachingSolenoid(frc::Solenoid&& sol) : cache_{false}, sol_{std::move(sol)} { + sol_.Set(false); + } + + void Set(bool actuated) { + // For some reason actually caching breaks things + // TODO fix this + sol_.Set(actuated); + // if (cache_ != actuated) { + // cache_ = actuated; + // sol_.Set(actuated); + // std::cout << "set solenoid " << sol_.GetName() << sol_.Get() + // << std::endl; + // } + } + + private: + bool cache_; + frc::Solenoid sol_; +}; + +} // namespace c2020 +} // namespace team114 \ No newline at end of file diff --git a/c2021 v2/src/main/include/util/construcotr_macros.h b/c2021 v2/src/main/include/util/construcotr_macros.h new file mode 100644 index 00000000..0fdb4460 --- /dev/null +++ b/c2021 v2/src/main/include/util/construcotr_macros.h @@ -0,0 +1,8 @@ +#pragma once + +#define DISALLOW_COPY(Type) Type(const Type&) = delete; +#define DISALLOW_ASSIGN(Type) Type& operator=(const Type&) = delete; + +#define DISALLOW_COPY_ASSIGN(Type) \ + DISALLOW_COPY(Type) \ + DISALLOW_ASSIGN(Type) \ No newline at end of file diff --git a/c2021 v2/src/main/include/util/interp_map.h b/c2021 v2/src/main/include/util/interp_map.h new file mode 100644 index 00000000..bf36ac44 --- /dev/null +++ b/c2021 v2/src/main/include/util/interp_map.h @@ -0,0 +1,145 @@ +#pragma once + +#include + +#include + +namespace team114 { +namespace c2020 { + +template double [0, + // 1) + typename TInterpolateFunctor, // (lower_val, upper_val, double [0, + // 1)) -> T + class Compare = std::less, // map::key_compare + class Alloc = + std::allocator > // map::allocator_type + > +class InterpolatingMap { + public: + typedef std::map InnerMap; + InterpolatingMap(typename InnerMap::size_type max_size) + : inverse_interp_{}, interp_{}, map_{}, max_size_{max_size} {} + InnerMap& Inner() { return map_; } + typename InnerMap::value_type InterpAt(Key key) { + // gets prev item, if key exists return that item + auto below_iter = GetLowerOrEqual(key); + // gets next item, if key exists returns next item + auto above_iter = map_.upper_bound(key); + auto end = map_.end(); + if (below_iter == end && above_iter == end) { + // map is empty, we'd rather not return an past-the-end iter as STL + // would, and this is a degenerate case, so throw + throw std::out_of_range{"interp_map is empty"}; + } + if (above_iter == end) { + // return map end + return *below_iter; + } + if (below_iter == end) { + // return map beginning + return *above_iter; + } + // both iterators are valid, interpolate: + double interp = + inverse_interp_(below_iter->first, above_iter->first, key); + const T value = interp_(below_iter->second, above_iter->second, interp); + typename InnerMap::value_type ret{key, value}; + return ret; + } + + // naming assumes time-based and that comparator is less + typename InnerMap::value_type Latest() { + auto it = map_.rbegin(); + if (it == map_.rend()) { + throw std::out_of_range{"interp_map is empty"}; + } + return *it; + } + + // DOES NOT INTERPOLATE, for insertion only + T& operator[](const Key&& key) { + if (map_.size() >= max_size_) { + map_.erase(map_.begin()); + } + return map_[key]; + } + T& operator[](const Key& key) { + if (map_.size() >= max_size_) { + map_.erase(map_.begin()); + } + return map_[key]; + } + + void CheckSize() { + if (map_.size() > max_size_) { + map_.erase(map_.begin()); + } + } + + private: + auto GetLowerOrEqual(Key key) { + auto it = map_.upper_bound(key); + if (it == map_.begin()) { + // if the map is empty, we find nothing and end == begin, in which + // case return end, thats covered + // if our key is below all other keys, our result will be the first + // element, which is begin in that case there is no lower or equal, + // so return end + return map_.end(); + } + // now we either have: + // 1. our key was past all other keys, we got one-past-end, want the + // last element + // 2. our key was not in the map, we got one-past what we want + // 3. our key was in the map, we got one-past what we want + --it; + return it; + } + + KeyInverseInterpolateFunctor inverse_interp_; + TInterpolateFunctor interp_; + InnerMap map_; + const typename InnerMap::size_type max_size_; +}; + +template +struct ArithmeticInverseInterp { + double operator()(T low, T high, T key) { + T total_diff = high - low; + T diff = key - low; + return diff / total_diff; + } +}; + +template +struct ArithmeticInterp { + T operator()(T low, T high, double pct) { + T total_diff = high - low; + return low + pct * total_diff; + } +}; + +struct Pose2dInterp { + frc::Pose2d operator()(const frc::Pose2d& low, const frc::Pose2d& high, + double pct) { + if (pct <= 0) { + return low; + } + if (pct >= 1) { + return high; + } + frc::Twist2d twist = low.Log(high); + frc::Twist2d interped_twist; + interped_twist.dx = pct * twist.dx; + interped_twist.dy = pct * twist.dy; + interped_twist.dtheta = pct * twist.dtheta; + return low.Exp(interped_twist); + } +}; + +} // namespace c2020 +} // namespace team114 \ No newline at end of file diff --git a/c2021 v2/src/main/include/util/number_util.h b/c2021 v2/src/main/include/util/number_util.h new file mode 100644 index 00000000..93a6a842 --- /dev/null +++ b/c2021 v2/src/main/include/util/number_util.h @@ -0,0 +1,43 @@ +#pragma once + +#include +#include + +#include + +namespace team114 { +namespace c2020 { + +template +constexpr inline bool EpsilonEq(T one, T two, T epsilon) noexcept { + return std::abs(one - two) < epsilon; +} + +template +constexpr inline T Deadband(T val, T deadband) noexcept { + constexpr T kZero = T(0.0); + if (EpsilonEq(val, kZero, deadband)) { + return kZero; + } + return val; +} + +template +constexpr inline T Clamp(T val, T min, T max) noexcept { + if (val < min) { + return min; + } else if (val > max) { + return max; + } else { + return val; + } +} + +template +constexpr inline units::radian_t DegToRad(T deg) { + constexpr T conv = M_PI / 180.0; + return units::radian_t{deg * conv}; +} + +} // namespace c2020 +} // namespace team114 \ No newline at end of file diff --git a/c2021 v2/src/main/include/util/sdb_types.h b/c2021 v2/src/main/include/util/sdb_types.h new file mode 100644 index 00000000..a9f6ae96 --- /dev/null +++ b/c2021 v2/src/main/include/util/sdb_types.h @@ -0,0 +1,83 @@ +#pragma once + +#include +#include + +#include + +namespace team114 { +namespace c2020 { + +// no string literals in templates yet, so have to resort to an ugly macro +// defining an anonymous type. When/if those come, we can unify all types +// into one beautiful SFINAE dance, without any macros + +#define SDB_NUMERIC(type, key_ident) \ + struct __SdbKey_##key_ident { \ + static const std::string GetName() { return #key_ident; }; \ + }; \ + team114::c2020::SdbNumeric + +template +struct SdbNumeric { + SdbNumeric() : value() { Update(); }; + SdbNumeric(const NumericTy& val) : value(val) { Update(); } + const NumericTy& operator=(const NumericTy& val) { + value = val; + Update(); + return val; + } + operator NumericTy() const { return value; } + NumericTy operator++(int) { + value++; + Update(); + return value; + } + + private: + NumericTy value; + void Update() { frc::SmartDashboard::PutNumber(KeyTy::GetName(), value); } +}; + +#define SDB_BOOL(key_ident) \ + struct __SdbKey_##key_ident { \ + static const std::string GetName() { return #key_ident; }; \ + }; \ + team114::c2020::SdbBool<__SdbKey_##key_ident> + +template +struct SdbBool { + SdbBool() : value() { Update(); }; + SdbBool(const bool& val) : value(val) { Update(); } + const bool& operator=(const bool& val) { + value = val; + Update(); + return val; + } + operator bool() const { return value; } + + private: + bool value; + void Update() { frc::SmartDashboard::PutBoolean(KeyTy::GetName(), value); } +}; + +#define READING_SDB_NUMERIC(type, key_ident) \ + struct __SdbKey_##key_ident { \ + static const std::string GetName() { return #key_ident; }; \ + }; \ + team114::c2020::ReadingSdbNumeric + +template +struct ReadingSdbNumeric { + ReadingSdbNumeric() { + if (!frc::SmartDashboard::GetEntry(KeyTy::GetName()).Exists()) { + frc::SmartDashboard::PutNumber(KeyTy::GetName(), NumericTy()); + } + } + operator NumericTy() const { + return frc::SmartDashboard::GetNumber(KeyTy::GetName(), NumericTy()); + } +}; + +} // namespace c2020 +} // namespace team114 \ No newline at end of file From 9b4e07a616eaf891ec68918729d769f736547132 Mon Sep 17 00:00:00 2001 From: Alex Chang Date: Sat, 2 Oct 2021 22:11:30 -0700 Subject: [PATCH 2/4] Revert "Add files via upload" This reverts commit 4cfd05b93d85720dd6280dd658bcb03c8e4ad7ad. --- c2021 v2/src/main/cpp/Drivetrain.cpp | 21 - c2021 v2/src/main/cpp/Robot.cpp | 100 +--- c2021 v2/src/main/cpp/Shooter.cpp | 2 - c2021 v2/src/main/cpp/config.cpp | 506 ------------------ c2021 v2/src/main/cpp/controls.cpp | 136 ----- c2021 v2/src/main/cpp/robot_state.cpp | 109 ---- c2021 v2/src/main/cpp/shims/minimal_phoenix.h | 23 - c2021 v2/src/main/cpp/shims/navx_ahrs.h | 138 ----- c2021 v2/src/main/cpp/subsystems/drive.cpp | 370 ------------- c2021 v2/src/main/cpp/subsystems/drive.h | 96 ---- .../src/main/cpp/subsystems/limelight.cpp | 90 ---- c2021 v2/src/main/cpp/subsystems/limelight.h | 97 ---- c2021 v2/src/main/include/Drivetrain.h | 25 - c2021 v2/src/main/include/Robot.h | 61 +-- c2021 v2/src/main/include/Shooter.h | 2 - c2021 v2/src/main/include/config.h | 172 ------ c2021 v2/src/main/include/controls.h | 91 ---- c2021 v2/src/main/include/robot_state.h | 51 -- c2021 v2/src/main/include/subsystem.h | 59 -- .../src/main/include/util/caching_types.h | 34 -- .../main/include/util/construcotr_macros.h | 8 - c2021 v2/src/main/include/util/interp_map.h | 145 ----- c2021 v2/src/main/include/util/number_util.h | 43 -- c2021 v2/src/main/include/util/sdb_types.h | 83 --- 24 files changed, 46 insertions(+), 2416 deletions(-) delete mode 100644 c2021 v2/src/main/cpp/Drivetrain.cpp delete mode 100644 c2021 v2/src/main/cpp/Shooter.cpp delete mode 100644 c2021 v2/src/main/cpp/config.cpp delete mode 100644 c2021 v2/src/main/cpp/controls.cpp delete mode 100644 c2021 v2/src/main/cpp/robot_state.cpp delete mode 100644 c2021 v2/src/main/cpp/shims/minimal_phoenix.h delete mode 100644 c2021 v2/src/main/cpp/shims/navx_ahrs.h delete mode 100644 c2021 v2/src/main/cpp/subsystems/drive.cpp delete mode 100644 c2021 v2/src/main/cpp/subsystems/drive.h delete mode 100644 c2021 v2/src/main/cpp/subsystems/limelight.cpp delete mode 100644 c2021 v2/src/main/cpp/subsystems/limelight.h delete mode 100644 c2021 v2/src/main/include/Drivetrain.h delete mode 100644 c2021 v2/src/main/include/Shooter.h delete mode 100644 c2021 v2/src/main/include/config.h delete mode 100644 c2021 v2/src/main/include/controls.h delete mode 100644 c2021 v2/src/main/include/robot_state.h delete mode 100644 c2021 v2/src/main/include/subsystem.h delete mode 100644 c2021 v2/src/main/include/util/caching_types.h delete mode 100644 c2021 v2/src/main/include/util/construcotr_macros.h delete mode 100644 c2021 v2/src/main/include/util/interp_map.h delete mode 100644 c2021 v2/src/main/include/util/number_util.h delete mode 100644 c2021 v2/src/main/include/util/sdb_types.h diff --git a/c2021 v2/src/main/cpp/Drivetrain.cpp b/c2021 v2/src/main/cpp/Drivetrain.cpp deleted file mode 100644 index d2139459..00000000 --- a/c2021 v2/src/main/cpp/Drivetrain.cpp +++ /dev/null @@ -1,21 +0,0 @@ - -#include "Drivetrain.h" - -Drivetrain::Drivetrain(){ - left_master.SetInverted(false); - right_master.SetInverted(true); - left_slave.Follow(left_master); - right_slave.Follow(right_master); - left_slave.SetInverted(InvertType::FollowMaster); - right_slave.SetInverted(InvertType::FollowMaster); -} - -void -Drivetrain::Drive(){ - -} - -void -Drivetrain::UpdateOdometry(){ - -} \ No newline at end of file diff --git a/c2021 v2/src/main/cpp/Robot.cpp b/c2021 v2/src/main/cpp/Robot.cpp index 6a25f6ae..31e6c680 100644 --- a/c2021 v2/src/main/cpp/Robot.cpp +++ b/c2021 v2/src/main/cpp/Robot.cpp @@ -1,102 +1,32 @@ #include "Robot.h" -#include - -#include - -namespace team114 { -namespace c2020 { - -/** - * Instentiates structs for future use. -**/ -Robot::Robot() - : frc::TimedRobot{Robot::kPeriod}, - controls_{}, - drive_{Drive::GetInstance()}, - robot_state_{RobotState::GetInstance()}, - ljoy_{0}, - rjoy_{1}, - ojoy_{2}, - cfg{conf::GetConfig()} {} - -/** - * Nothing. -**/ void Robot::RobotInit() { + m_chooser.SetDefaultOption(kAutoNameDefault, kAutoNameDefault); + m_chooser.AddOption(kAutoNameCustom, kAutoNameCustom); + frc::SmartDashboard::PutData("Auto Modes", &m_chooser); } +void Robot::RobotPeriodic() {} +void Robot::AutonomousInit() {} -/** - * Calls period function of select classes. Possibly unfinished? -**/ -void Robot::RobotPeriodic() { - - drive_.Periodic(); - -} - -/** - * Zeroing sensors, selecting auto mode. -**/ -void Robot::AutonomousInit() { - -} - -/** - * Calls periodic function of select structs. -**/ void Robot::AutonomousPeriodic() { - + Drivetrain.Auto(); } +void Robot::TeleopInit() {} -/** - * Finishes initialition (stows hood?). -**/ -void Robot::TeleopInit() { - -} - -/** - * Calls remaining periodic funtions. Checks if robot is shooting, climbing or doing the control panel and calls functions accordingly. -**/ void Robot::TeleopPeriodic() { - // if (!controls_.Shoot()) { - drive_.SetWantCheesyDrive(controls_.Throttle(), controls_.Wheel(), - controls_.QuickTurn()); - // } - - //check if different contol buttons have been pressed, act accordingly - + Drivetrain.Periodic(l_joy, r_joy); + Shooter.Periodic(xbox); } -/** - * Nothing. -**/ +void Robot::DisabledInit() {} +void Robot::DisabledPeriodic() {} void Robot::TestInit() {} +void Robot::TestPeriodic() {} -/** - * Simulates the climbing portion of periodic action. -**/ -void Robot::TestPeriodic() { - -} - -/** - * Resets a couple things (most zeroing happens in AutonomousInit()). -**/ -void Robot::DisabledInit() { - -} - -/** - * Updates auto selector. -**/ -void Robot::DisabledPeriodic() { } - -} // namespace c2020 -} // namespace team114 #ifndef RUNNING_FRC_TESTS -int main() { return frc::StartRobot(); } +int main() { + return frc::StartRobot(); +} #endif diff --git a/c2021 v2/src/main/cpp/Shooter.cpp b/c2021 v2/src/main/cpp/Shooter.cpp deleted file mode 100644 index 78004ce8..00000000 --- a/c2021 v2/src/main/cpp/Shooter.cpp +++ /dev/null @@ -1,2 +0,0 @@ -#include "Shooter.h" - diff --git a/c2021 v2/src/main/cpp/config.cpp b/c2021 v2/src/main/cpp/config.cpp deleted file mode 100644 index 60ba0c69..00000000 --- a/c2021 v2/src/main/cpp/config.cpp +++ /dev/null @@ -1,506 +0,0 @@ -#include "config.h" - -#include -#include -#include -#include - -//#include "util/number_util.h" - -namespace team114 { -namespace c2020 { -namespace conf { - -// WPILib chose GCC 7, so no C++20 designated initializers here - -/** -* This function is declaring all the constants that will be needed for the robot through obj c -* Includes PID constants, ticks, etc -**/ -const RobotConfig MakeDefaultRobotConfig() { - RobotConfig c; - // /sys/class/net//address is newline terminated, match here - c.mac_address = "aa:bb:cc:dd:ee:ff\n"; - - c.drive.left_master_id = 0; - c.drive.left_slave_id = 1; - c.drive.right_master_id = 2; - c.drive.right_slave_id = 3; - c.drive.track_width = 0.661924_m; - c.drive.meters_per_falcon_tick = - 1.0 / 2048.0 * 10.0 / 62.0 * 18.0 / 30.0 * 6 * M_PI * 0.0254_m; - c.drive.traj_max_vel = 2.5_mps; - c.drive.traj_max_accel = units::meters_per_second_squared_t{5.0}; - // currently this is g with 2x FOS - c.drive.traj_max_centrip_accel = units::meters_per_second_squared_t{4.9}; - c.drive.orient_kp = 0.45; - c.drive.orient_ki = 0.0; - c.drive.orient_kd = 0.0; - c.drive.orient_vel = 0.0; - c.drive.orient_acc = 0.0; - - c.ctrl_panel.talon_id = 31; - c.ctrl_panel.current_limit = 22; - c.ctrl_panel.kP = 1.5; - c.ctrl_panel.kI = 0.0; - c.ctrl_panel.kD = 5.0; - c.ctrl_panel.ticks_per_inch = 4096.0 / (3.5 * M_PI); - c.ctrl_panel.ticks_per_color_slice = - 12.56 * 0.96 * c.ctrl_panel.ticks_per_inch; - c.ctrl_panel.rot_control_ticks = - 32 * M_PI * 3.5 * c.ctrl_panel.ticks_per_inch; - c.ctrl_panel.scoot_cmd = 0.5; - c.ctrl_panel.solenoid_channel = 4; - - // TODO the rest - c.intake.rot_talon_id = 41; //needs to be updated? - c.intake.roller_talon_id = 20; - c.intake.intake_cmd = 0.80; - c.intake.rot_current_limit = 10; - c.intake.zeroing_kp = 0.001; - c.intake.zeroing_vel = 8; - c.intake.roller_current_limit = 20; - c.intake.abs_enc_tick_offset = 2945; - c.intake.abs_ticks_per_rot = 4096.0 * 36.0 / 22.0; - c.intake.rel_ticks_per_abs_tick = 1.0; - c.intake.rads_per_rel_tick = - 2 * M_PI / c.intake.abs_ticks_per_rot / c.intake.rel_ticks_per_abs_tick; - c.intake.zeroed_rad_from_vertical = 15.4 * M_PI / 180.0; - c.intake.SinekF = -0.063; - c.intake.kP = 1.0; - c.intake.kI = 0.005; - c.intake.kD = 0.0; - c.intake.profile_acc = 3000; - c.intake.profile_vel = 2000; - c.intake.stowed_pos_ticks = 1222; - c.intake.intaking_pos_ticks = 2350; - // c.intake.trench_driving_pos_ticks = 20.0 / 36.0 * 4096.0; - - c.hood.talon_id = 52; - c.hood.ticks_per_degree = 4096.0 * 350.0 / 28.0 / 360.0; - c.hood.min_degrees = 20; // TODO - c.hood.max_degrees = 64; - c.hood.current_limit = 20; - c.hood.zeroing_current = 6; - c.hood.zeroing_kp = .001; - c.hood.zeroing_vel = 15; - c.hood.profile_acc = 20000; // TODO - c.hood.profile_vel = 9000; // TODO - c.hood.kP = 2.0; - c.hood.kI = 0.01; - c.hood.kD = 10.0; - c.hood.ctre_curve_smoothing = 2; - - c.shooter.master_id = 5; //left side of shooter - c.shooter.slave_id = 6; //right side of shooter - c.shooter.turret_id = 4; - c.shooter.shooter_current_limit = 40; - c.shooter.kF = 0.0160; - c.shooter.kP = 0.25; - c.shooter.kP = 0.0; - c.shooter.kI = 0.0; - c.shooter.kD = 0.0; - c.shooter.meas_period = VelocityMeasPeriod::Period_2Ms; - c.shooter.meas_filter_width = 32; - c.shooter.shootable_err_pct = 0.04; - c.shooter.kicker_id = 51; - c.shooter.kicker_current_limit = 20; - c.shooter.kicker_cmd = 0.7; //IIF YOU CHANGE THIS U GOTTA REDO AUTO SHOOT DATA - - c.ball_channel.serializer_id = 43; - c.ball_channel.channel_id = 44; - c.ball_channel.current_limit = 25; - c.ball_channel.serializer_cmd = 1.00; - c.ball_channel.channel_cmd = 0.9; - c.ball_channel.s0_port = 0; - c.ball_channel.s1_port = 1; - c.ball_channel.s2_port = 2; - c.ball_channel.s3_port = 3; - - c.climber.master_id = 19; - c.climber.slave_id = 20; - c.climber.current_limit = 35; - c.climber.release_solenoid_id = 7; - c.climber.brake_solenoid_id = 6; - c.climber.avg_ticks_per_inch = 4096.0 * 9.0 / ((.969 + 1.25) * M_PI); - c.climber.initial_step_ticks = 10.0 * c.climber.avg_ticks_per_inch; - c.climber.forward_soft_limit_ticks = 122000; - c.climber.ascend_command = 1.00; - c.climber.descend_command = -1.00; - - /* c.limelight.name = "limelight"; - c.limelight.table_name = "limelight"; - c.limelight.diff_height = 2.496_m - 0.55_m; - c.limelight.angle_above_horizontal = DegToRad(8.0); */ - - return c; -} - -/** -* Basic getter, returning the configuration to uphold encapsulation in oop -**/ -RobotConfig& GetConfig() { - static std::optional CONFIG; - if (CONFIG.has_value()) { - return CONFIG.value(); - } - // TODO(josh): log the chosen config - std::ifstream ifs("/sys/class/net/eth0/address"); - if (!ifs.good() || !ifs.is_open()) { - CONFIG = MakeDefaultRobotConfig(); - return GetConfig(); - } - std::string rio_mac((std::istreambuf_iterator(ifs)), - (std::istreambuf_iterator())); - // TODO(josh) fill in with real RIOs - RobotConfig config_a = MakeDefaultRobotConfig(); - RobotConfig config_b = MakeDefaultRobotConfig(); - if (config_a.mac_address == rio_mac) { - CONFIG = std::move(config_a); - } else if (config_a.mac_address == rio_mac) { - CONFIG = std::move(config_b); - } else { - CONFIG = MakeDefaultRobotConfig(); - } - return GetConfig(); -} - -/** -* Defining constants for Talon configurations -* This includes current limit, nominal outputs, etc regarding talon motors -*/ -void DriveFalconCommonConfig(TalonFX& falcon) { - TalonFXConfiguration c; - // ====== Talon FX CFG - c.supplyCurrLimit = - SupplyCurrentLimitConfiguration{true, 50.0, 55.0, 0.100}; - // StatorCurrentLimitConfiguration statorCurrLimit - - // TODO(josh) enable FOC on arrival - c.motorCommutation = MotorCommutation::Trapezoidal; - // absoluteSensorRange = AbsoluteSensorRange::Unsigned_0_to_360 double - // integratedSensorOffsetDegrees = 0 - c.initializationStrategy = SensorInitializationStrategy::BootToZero; - // ===== Base Talon CFG - c.primaryPID.selectedFeedbackSensor = FeedbackDevice::IntegratedSensor; - c.primaryPID.selectedFeedbackCoefficient = 1.0; - // ======= BaseMotor Param cfg - c.openloopRamp = 0.25; // time from neutral to full - c.closedloopRamp = 0.0; // disable ramping, should be running a profile - c.peakOutputForward = 1.0; - c.peakOutputReverse = -1.0; - c.nominalOutputForward = 0.0; - c.nominalOutputReverse = 0.0; - // double neutralDeadband - c.voltageCompSaturation = 12.0; - // int voltageMeasurementFilter - // TODO(josh) tune these - // https://phoenix-documentation.readthedocs.io/en/latest/ch14_MCSensor.html#velocity-measurement-filter - c.velocityMeasurementPeriod = VelocityMeasPeriod::Period_5Ms; - c.velocityMeasurementWindow = 2; - // int forwardSoftLimitThreshold - // int reverseSoftLimitThreshold - c.forwardSoftLimitEnable = false; - c.reverseSoftLimitEnable = false; - // TODO(Josh) tune for Ramsete - c.slot0.allowableClosedloopError = 0; - c.slot0.closedLoopPeakOutput = 1.0; - c.slot0.closedLoopPeriod = 1; - // TODO(josh) tune - // c.slot0.integralZone = - // c.slot0.maxIntegralAccumulator = - // https://phoenix-documentation.readthedocs.io/en/latest/ch16_ClosedLoop.html#calculating-velocity-feed-forward-gain-kf - c.slot0.kF = 0.0; - c.slot0.kP = 0.0; - c.slot0.kI = 0.0; - c.slot0.kD = 0.0; - // SlotConfiguration slot1 - // SlotConfiguration slot2 - // SlotConfiguration slot3 - // bool auxPIDPolarity - // FilterConfiguration remoteFilter0 - // FilterConfiguration remoteFilter1 - // int motionCruiseVelocity - // int motionAcceleration - // int motionCurveStrength - // int motionProfileTrajectoryPeriod - // bool feedbackNotContinuous - // bool remoteSensorClosedLoopDisableNeutralOnLOS - // bool clearPositionOnLimitF - // bool clearPositionOnLimitR - // bool clearPositionOnQuadIdx - // bool limitSwitchDisableNeutralOnLOS - // bool softLimitDisableNeutralOnLOS - // int pulseWidthPeriod_EdgesPerRot - // int pulseWidthPeriod_FilterWindowSz - // bool trajectoryInterpolationEnable - // ======= Custom Param CFG - // int customParam0 - // int customParam1 - // bool enableOptimizations - // TODO(josh) logs everywhere - for (int i = 0; i < 10; i++) { - auto err = falcon.ConfigAllSettings(c, 100); - if (err == ErrorCode::OKAY) { - break; - } - } - falcon.EnableVoltageCompensation(true); - // TODO(josh) falcon's dont have enable current limit? - falcon.SelectProfileSlot(0, 0); - falcon.SetNeutralMode(NeutralMode::Brake); -} - -// CAN metrics docs -// https://phoenix-documentation.readthedocs.io/en/latest/ch18_CommonAPI.html#can-bus-utilization-error-metrics - -//Basically finding and storing multiple error codes for Talonfx -//Explains why you need error handling -//https:github.com/CrossTheRoadElec/Phoenix-Documentation/blob/master/Legacy/README.md#error-handling -inline static ErrorCode SetDriveCommonFramePeriods(TalonFX& falcon) { - constexpr int lng = 255; - ErrorCollection err; - err.NewError(falcon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_3_Quadrature, lng)); - err.NewError(falcon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_4_AinTempVbat, lng)); - err.NewError(falcon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_8_PulseWidth, lng)); - err.NewError(falcon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_10_MotionMagic, lng)); - err.NewError(falcon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_12_Feedback1, lng)); - err.NewError(falcon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_14_Turn_PIDF1, lng)); - return err.GetFirstNonZeroError(); -} -constexpr int kStatusFrameAttempts = 3; - -// TODO(josh) log here -//Similiar to the function above, it just adds error to collection -//Main difference is that you don't return it. Also this is master meaning it controls slave -void SetDriveMasterFramePeriods(TalonFX& falcon) { - for (int i = 0; i < kStatusFrameAttempts; i++) { - ErrorCollection err; - err.NewError(SetDriveCommonFramePeriods(falcon)); - err.NewError(falcon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_1_General, 5)); - err.NewError(falcon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_2_Feedback0, 5)); - err.NewError(falcon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_13_Base_PIDF0, 50)); - if (err.GetFirstNonZeroError() == ErrorCode::OK) { - return; - } - } -} - -/** -* Collects error codes, however master controls -**/ -void SetDriveSlaveFramePeriods(TalonFX& falcon) { - for (int i = 0; i < kStatusFrameAttempts; i++) { - ErrorCollection err; - err.NewError(SetDriveCommonFramePeriods(falcon)); - err.NewError(falcon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_1_General, 255)); - err.NewError(falcon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_2_Feedback0, 255)); - err.NewError(falcon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_13_Base_PIDF0, 255)); - if (err.GetFirstNonZeroError() == ErrorCode::OK) { - return; - } - } -} - -/** -* Setting values and configurations for Talons -* Also collecting error from functions -**/ -void SetFramePeriodsForPidTalon(TalonSRX& talon, FeedbackType feedback_type) { - constexpr int lng = 255; - constexpr int shrt = 10; - int quad = lng; - int pulsewidth = lng; - switch (feedback_type) { - case FeedbackType::Both: - quad = shrt; - pulsewidth = shrt; - break; - case FeedbackType::Quadrature: - quad = shrt; - break; - case FeedbackType::PulseWidth: - pulsewidth = shrt; - break; - case FeedbackType::None: - break; - } - for (int i = 0; i < kStatusFrameAttempts; i++) { - ErrorCollection err; - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_1_General, shrt)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_2_Feedback0, shrt)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_3_Quadrature, quad)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_4_AinTempVbat, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_8_PulseWidth, pulsewidth)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_10_MotionMagic, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_12_Feedback1, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_14_Turn_PIDF1, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_13_Base_PIDF0, 50)); - if (err.GetFirstNonZeroError() == ErrorCode::OK) { - return; - } - } -} - -void SetFramePeriodsForPidTalonFX(TalonFX& talon, FeedbackType feedback_type) { - constexpr int lng = 255; - constexpr int shrt = 10; - int quad = lng; - int pulsewidth = lng; - switch (feedback_type) { - case FeedbackType::Both: - quad = shrt; - pulsewidth = shrt; - break; - case FeedbackType::Quadrature: - quad = shrt; - break; - case FeedbackType::PulseWidth: - pulsewidth = shrt; - break; - case FeedbackType::None: - break; - } - for (int i = 0; i < kStatusFrameAttempts; i++) { - ErrorCollection err; - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_1_General, shrt)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_2_Feedback0, shrt)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_3_Quadrature, quad)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_4_AinTempVbat, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_8_PulseWidth, pulsewidth)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_10_MotionMagic, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_12_Feedback1, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_14_Turn_PIDF1, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_13_Base_PIDF0, 50)); - if (err.GetFirstNonZeroError() == ErrorCode::OK) { - return; - } - } -} - - -/** -* Error collecting for Open loop TalonSRX's -**/ -void SetFramePeriodsForOpenLoopTalon(TalonSRX& talon) { - constexpr int lng = 255; - constexpr int shrt = 20; - for (int i = 0; i < kStatusFrameAttempts; i++) { - ErrorCollection err; - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_1_General, shrt)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_2_Feedback0, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_3_Quadrature, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_4_AinTempVbat, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_8_PulseWidth, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_10_MotionMagic, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_12_Feedback1, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_14_Turn_PIDF1, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_13_Base_PIDF0, lng)); - if (err.GetFirstNonZeroError() == ErrorCode::OK) { - return; - } - } -} - -/** -* Another Slave function which means controlled by master -* collect error -**/ -void SetFramePeriodsForSlaveTalon(TalonSRX& talon) { - constexpr int lng = 255; - for (int i = 0; i < kStatusFrameAttempts; i++) { - ErrorCollection err; - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_1_General, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_2_Feedback0, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_3_Quadrature, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_4_AinTempVbat, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_8_PulseWidth, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_10_MotionMagic, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_12_Feedback1, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_14_Turn_PIDF1, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_13_Base_PIDF0, lng)); - if (err.GetFirstNonZeroError() == ErrorCode::OK) { - return; - } - } -} - -void SetFramePeriodsForSlaveTalonFX(TalonFX& talon) { - constexpr int lng = 255; - for (int i = 0; i < kStatusFrameAttempts; i++) { - ErrorCollection err; - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_1_General, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_2_Feedback0, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_3_Quadrature, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_4_AinTempVbat, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_8_PulseWidth, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_10_MotionMagic, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_12_Feedback1, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_14_Turn_PIDF1, lng)); - err.NewError(talon.SetStatusFramePeriod( - StatusFrameEnhanced::Status_13_Base_PIDF0, lng)); - if (err.GetFirstNonZeroError() == ErrorCode::OK) { - return; - } - } -} - -} // namespace conf -} // namespace c2020 -} // namespace team114 diff --git a/c2021 v2/src/main/cpp/controls.cpp b/c2021 v2/src/main/cpp/controls.cpp deleted file mode 100644 index 674c781a..00000000 --- a/c2021 v2/src/main/cpp/controls.cpp +++ /dev/null @@ -1,136 +0,0 @@ -#include "controls.h" -#include - -namespace team114 { -namespace c2020 { - -/** -* Constructor, initializes left, right, and other joystick with respective ports. -**/ -Controls::Controls() : ljoy_{0}, rjoy_{1}, ojoy_{2} {} -/** - * returns value for joystick in axis 1 to determine throttle amount. - **/ - -void Controls::OPrints() { - /* if (ojoy_.GetRawButton(1)) std::cout << 1 << std::endl; - if (ojoy_.GetRawButton(2)) std::cout << 2 << std::endl; - if (ojoy_.GetRawButton(3)) std::cout << 3 << std::endl; - if (ojoy_.GetRawButton(4)) std::cout << 4 << std::endl; - if (ojoy_.GetRawButton(5)) std::cout << 5 << std::endl; - if (ojoy_.GetRawButton(6)) std::cout << 6 << std::endl; - if (ojoy_.GetRawButton(7)) std::cout << 7 << std::endl; */ -} - -double Controls::Throttle() { return -ljoy_.GetRawAxis(1); } -/** - * returns value for joystick in axis 0. -**/ -double Controls::Wheel() { return rjoy_.GetRawAxis(0); } -/** - * Checks if button has been pressed to indicate a quick turn. -**/ -bool Controls::QuickTurn() { return rjoy_.GetRawButton(1); } -/** - * Checks if joystick has been moved enough to indicate climbing up should start. -**/ -bool Controls::ClimbUp() { return ojoy_.GetRawAxis(1) < -kAnalogJoyThreshold; } -/** - * Checks if joystick has been moved enough to indicate climbing down should start. -**/ -bool Controls::ClimbDown() { return ojoy_.GetRawAxis(1) > kAnalogJoyThreshold; } -/** - * check if ball is ready to shoot ball short distance if -**/ -bool Controls::ShotShortPressed() { - int pov = ojoy_.GetPOV(); - return shot_short_.Pressed(pov == 0 || pov == 315 || pov == 45); -} -/** -* Check if ball is ready to shoot ball medium distance -**/ -bool Controls::ShotMedPressed() { - int pov = ojoy_.GetPOV(); - return shot_short_.Pressed(pov == 90 || pov == 135 || pov == 270 || - pov == 225); -} -/** - * Check if ready to shoot ball long distance -**/ -bool Controls::ShotLongPressed() { - int pov = ojoy_.GetPOV(); - return shot_short_.Pressed(pov == 180); -} -/** - * Checking if intake should begin. -**/ -bool Controls::Intake() { return ojoy_.GetRawAxis(3) > kTriggerThreshold; } -/** - * Check if button for unjamming has been pressed. -**/ -bool Controls::Unjam() { return ojoy_.GetRawButton(7); } -/** - * Checks if button to shoot has been pressed -**/ -bool Controls::Shoot() { - // return ljoy_.GetRawButton(1); - return ojoy_.GetRawButton(5); -} - -bool Controls::ShortShot() { - return ojoy_.GetRawButton(4); //y -} - -bool Controls::LongShot() { - return ojoy_.GetRawButton(1); //the button below y i can't remember which letter it is -} - -/** - * Checks if joystick has been moved enough to start deploying panel. -**/ -bool Controls::PanelDeploy() { return ojoy_.GetRawAxis(2) > kTriggerThreshold; } -/** -* Pass on message that scoot left has been pressed -**/ -bool Controls::ScootLeftPressed() { - return scoot_left_.Pressed(scoot_tracker_.PassThroughFeed( - ojoy_.GetRawAxis(4) < -kAnalogJoyThreshold)); -} -/** - * Pass on message that scoot right has been pressed -**/ -bool Controls::ScootRightPressed() { - return scoot_right_.Pressed(scoot_tracker_.PassThroughFeed( - ojoy_.GetRawAxis(4) > kAnalogJoyThreshold)); -} -/** - * Pass on message that scoot has been released -**/ -bool Controls::ScootReleased() { - return scoot_released_.Pressed(scoot_tracker_.WasNotHeld()); -} -/** - * Check if Rot Control button has been pressed -**/ -bool Controls::RotControlPressed() { return ojoy_.GetRawButtonPressed(8); } -/** - * Check if Pos Control Red button has been pressed -**/ -bool Controls::PosControlRedPressed() { return ojoy_.GetRawButtonPressed(2); } -/** -* Check if Pos Control Yellow button has been pressed -**/ -bool Controls::PosControlYellowPressed() { - return ojoy_.GetRawButtonPressed(4); -} -/** - * Check if Pos Control Green button has been pressed -**/ -bool Controls::PosControlGreenPressed() { return ojoy_.GetRawButtonPressed(1); } -/** - * Check if pos control blue button has been pressed -**/ -bool Controls::PosControlBluePressed() { return ojoy_.GetRawButtonPressed(3); } - -} // namespace c2020 -} // namespace team114 diff --git a/c2021 v2/src/main/cpp/robot_state.cpp b/c2021 v2/src/main/cpp/robot_state.cpp deleted file mode 100644 index ad691dbd..00000000 --- a/c2021 v2/src/main/cpp/robot_state.cpp +++ /dev/null @@ -1,109 +0,0 @@ -#include "robot_state.h" - -#include - -namespace team114 { -namespace c2020 { - - -/** - * default construtor of robot state, calls the other constructor(?) - */ -RobotState::RobotState() : RobotState{conf::GetConfig()} {} - -/** - * configures the robot state w/ field position, limelight configuration, and - * vision systems - */ -RobotState::RobotState(conf::RobotConfig cfg) - : field_to_robot_{800}, - ll_cfg_{cfg.limelight}, - debounced_vision_{}, - vision_null_ct_{0} {} - -/** - * gets the latest known field position of the robot - * @returns latest field to robot/position value - */ -std::pair RobotState::GetLatestFieldToRobot() { - return field_to_robot_.Latest(); -} - -/** - * gets the field positon of the robot at a given timestamp - * @returns field to robot/position and a given timestamp - */ -frc::Pose2d RobotState::GetFieldToRobot(units::second_t timestamp) { - return field_to_robot_.InterpAt(timestamp).second; -} - -/** - * Reads the stance/position of the robot at a given time and sets it to an - * array(timeline like?) - */ -void RobotState::ObserveFieldToRobot(units::second_t timestamp, - const frc::Pose2d& pose) { - field_to_robot_[timestamp] = pose; -} - -/** - * resets the robot's field positioning - */ -void RobotState::ResetFieldToRobot() { field_to_robot_.Inner().clear(); } - -/** - * Checks the robot's vision, resets if it can't find the target too many times - */ -void RobotState::ObserveVision(units::second_t timestamp, - std::optional target) { - if (!target.has_value()) { - if (vision_null_ct_ > kDroppableFrames) { - debounced_vision_.reset(); - } - vision_null_ct_++; - return; - } - vision_null_ct_ = 0; - debounced_vision_ = std::make_pair(timestamp, target.value()); -} - -/** - * calculates the cotangent of a given value - * @returns cotangent of given value - */ -inline units::scalar_t FastCotangent(units::radian_t r) { - auto const pi2 = units::radian_t{M_PI_2}; - return units::math::tan(pi2 - r); -} - -std::optional> -/** - * Gets the latest recorded distance (to the outer port?) from the robot vision if possible - * @returns the distance - */ -RobotState::GetLatestDistanceToOuterPort() { - if (!debounced_vision_.has_value()) { - return {}; - } - auto target = *debounced_vision_; - auto dist = - ll_cfg_.diff_height * - FastCotangent(target.second.vertical + ll_cfg_.angle_above_horizontal); - return std::make_pair(target.first, dist); -} - -std::optional> -/** - * Gets/calculates the angle at which the robot's vision is viewing an object(the outer port?) - * @returns the angle - */ -RobotState::GetLatestAngleToOuterPort() { - if (!debounced_vision_.has_value()) { - return {}; - } - auto target = *debounced_vision_; - return std::make_pair(target.first, target.second.horizontal); -} - -} // namespace c2020 -} // namespace team114 diff --git a/c2021 v2/src/main/cpp/shims/minimal_phoenix.h b/c2021 v2/src/main/cpp/shims/minimal_phoenix.h deleted file mode 100644 index 29fa0d48..00000000 --- a/c2021 v2/src/main/cpp/shims/minimal_phoenix.h +++ /dev/null @@ -1,23 +0,0 @@ -#pragma once - -//listen to josh rant lmao - -// this is a comproimise -// CTRE uses back-asswards namespace conventions, -// like we're in Java land, basically mandating we use -// using namespace declarations. -// The Phoenix.h header does this, but also brings in the whole library -// In reality, we need like two classes and associated data types. - -// Since basically every translation unit will have ctre stuff, -// this should reduce compilation times - -#include -#include - -using namespace ctre; -using namespace ctre::phoenix; -using namespace ctre::phoenix::motion; -using namespace ctre::phoenix::motorcontrol; -using namespace ctre::phoenix::motorcontrol::can; -using namespace ctre::phoenix::sensors; \ No newline at end of file diff --git a/c2021 v2/src/main/cpp/shims/navx_ahrs.h b/c2021 v2/src/main/cpp/shims/navx_ahrs.h deleted file mode 100644 index 0d3065c8..00000000 --- a/c2021 v2/src/main/cpp/shims/navx_ahrs.h +++ /dev/null @@ -1,138 +0,0 @@ -#pragma once - -// Kuailabs doesnt supply x86 binaries in their package. -// This header enables building on x86 by replacing the arm-only class -// with an empty interface - -#ifdef __arm__ - -#include "AHRS.h" - -#else - -#include "frc/I2C.h" -#include "frc/PIDSource.h" -#include "frc/SPI.h" -#include "frc/SerialPort.h" -#include "frc/smartdashboard/SendableBase.h" -#include "frc/smartdashboard/SendableBuilder.h" - -class AHRS /*: public frc::SendableBase, - public frc::ErrorBase, - public frc::PIDSource*/ -{ - public: - enum BoardAxis { - kBoardAxisX = 0, - kBoardAxisY = 1, - kBoardAxisZ = 2, - }; - - struct BoardYawAxis { - /* Identifies one of the board axes */ - BoardAxis board_axis; - /* true if axis is pointing up (with respect to gravity); false if - * pointing down. */ - bool up; - }; - - enum SerialDataType { - /** - * (default): 6 and 9-axis processed data - */ - kProcessedData = 0, - /** - * unprocessed data from each individual sensor - */ - kRawData = 1 - }; - - public: - AHRS(frc::SPI::Port spi_port_id) {} - AHRS(frc::I2C::Port i2c_port_id) {} - AHRS(frc::SerialPort::Port serial_port_id) {} - - AHRS(frc::SPI::Port spi_port_id, uint8_t update_rate_hz) {} - AHRS(frc::SPI::Port spi_port_id, uint32_t spi_bitrate, - uint8_t update_rate_hz) {} - - AHRS(frc::I2C::Port i2c_port_id, uint8_t update_rate_hz) {} - - AHRS(frc::SerialPort::Port serial_port_id, AHRS::SerialDataType data_type, - uint8_t update_rate_hz) {} - - float GetPitch() { return 0.0; } - float GetRoll() { return 0.0; } - float GetYaw() { return 0.0; } - float GetCompassHeading() { return 0.0; } - void ZeroYaw() {} - bool IsCalibrating() { return false; } - bool IsConnected() { return false; } - double GetByteCount() { return 0.0; } - double GetUpdateCount() { return 0.0; } - long GetLastSensorTimestamp() { return 0; } - float GetWorldLinearAccelX() { return 0.0; } - float GetWorldLinearAccelY() { return 0.0; } - float GetWorldLinearAccelZ() { return 0.0; } - bool IsMoving() { return false; } - bool IsRotating() { return false; } - float GetBarometricPressure() { return 0.0; } - float GetAltitude() { return 0.0; } - bool IsAltitudeValid() { return false; } - float GetFusedHeading() { return 0.0; } - bool IsMagneticDisturbance() { return false; } - bool IsMagnetometerCalibrated() { return false; } - float GetQuaternionW() { return 0.0; } - float GetQuaternionX() { return 0.0; } - float GetQuaternionY() { return 0.0; } - float GetQuaternionZ() { return 0.0; } - void ResetDisplacement() {} - void UpdateDisplacement(float accel_x_g, float accel_y_g, - int update_rate_hz, bool is_moving); - float GetVelocityX() { return 0.0; } - float GetVelocityY() { return 0.0; } - float GetVelocityZ() { return 0.0; } - float GetDisplacementX() { return 0.0; } - float GetDisplacementY() { return 0.0; } - float GetDisplacementZ() { return 0.0; } - double GetAngle() { return 0.0; } - double GetRate() { return 0.0; } - void SetAngleAdjustment(double angle) {} - double GetAngleAdjustment() { return 0.0; }; - void Reset() {} - float GetRawGyroX() { return 0.0; }; - float GetRawGyroY() { return 0.0; }; - float GetRawGyroZ() { return 0.0; }; - float GetRawAccelX() { return 0.0; }; - float GetRawAccelY() { return 0.0; }; - float GetRawAccelZ() { return 0.0; }; - float GetRawMagX() { return 0.0; }; - float GetRawMagY() { return 0.0; }; - float GetRawMagZ() { return 0.0; }; - float GetPressure() { return 0.0; }; - float GetTempC() { return 0.0; }; - AHRS::BoardYawAxis GetBoardYawAxis() { - return {BoardAxis::kBoardAxisX, true}; - } - std::string GetFirmwareVersion() { return ""; }; - - // bool RegisterCallback(ITimestampedDataSubscriber *callback, - // void *callback_context) { - // return false; - // } - // bool DeregisterCallback(ITimestampedDataSubscriber *callback) { - // return false; - // } - - int GetActualUpdateRate() { return 0; } - int GetRequestedUpdateRate() { return 0; } - - void EnableLogging(bool enable) {} - void EnableBoardlevelYawReset(bool enable) {} - bool IsBoardlevelYawResetEnabled() { return false; } - - int16_t GetGyroFullScaleRangeDPS() { return 0; } - int16_t GetAccelFullScaleRangeG() { return 0; } -}; - -#endif \ No newline at end of file diff --git a/c2021 v2/src/main/cpp/subsystems/drive.cpp b/c2021 v2/src/main/cpp/subsystems/drive.cpp deleted file mode 100644 index 6b60184a..00000000 --- a/c2021 v2/src/main/cpp/subsystems/drive.cpp +++ /dev/null @@ -1,370 +0,0 @@ -#include "drive.h" - -#include - -#include -#include -#include - -#include - -#include "number_util.h" - -namespace team114 { -namespace c2020 { - -/** - * The constructor called if no config is passed in, - * in which case it calls the second constructor anyways by explicitly getting the drive config and passing it in -**/ -Drive::Drive() : Drive{conf::GetConfig().drive} {} - -/** - * The constructor that takes in a driveconfig to initialize all the drive's members, - * and the values of the members' variables -**/ -Drive::Drive(const conf::DriveConfig& cfg) - : left_master_{cfg.left_master_id}, - right_master_{cfg.right_master_id}, - left_slave_{cfg.left_slave_id}, - right_slave_{cfg.right_slave_id}, - cfg_{cfg}, - robot_state_{RobotState::GetInstance()}, - kinematics_{cfg.track_width}, - odometry_{{}}, - ramsete_{}, - vision_rot_{cfg_.orient_kp, - cfg_.orient_ki, - cfg_.orient_kd, - {2.0_rad / 1_s, 6.0_rad / 1.0_s / 1.0_s}, - 10_ms}, - has_vision_target_{false} { - conf::DriveFalconCommonConfig(left_master_); - conf::DriveFalconCommonConfig(right_master_); - conf::DriveFalconCommonConfig(left_slave_); - conf::DriveFalconCommonConfig(right_slave_); - left_master_.SetInverted(false); - right_master_.SetInverted(true); - left_slave_.Follow(left_master_); - right_slave_.Follow(right_master_); - left_slave_.SetInverted(InvertType::FollowMaster); - right_slave_.SetInverted(InvertType::FollowMaster); - - conf::SetDriveMasterFramePeriods(left_master_); - conf::SetDriveMasterFramePeriods(right_master_); - conf::SetDriveSlaveFramePeriods(left_slave_); - conf::SetDriveSlaveFramePeriods(right_slave_); - CheckFalconFramePeriods(); - - vision_rot_.SetIntegratorRange(-0.05, 0.05); - vision_rot_.SetTolerance(0.02_rad, 0.2_rad / 1.0_s); -} - -/** - * The first method called in Periodic(), in which the slaves are checked whether a reset has occured - * If a slave was reset, its frame period will once again be set to the correct value like in the constructor - * A counter will also be ticked to show how many times the falcons have been reset - * - * https://phoenix-documentation.readthedocs.io/en/latest/ch18_CommonAPI.html#can-bus-utilization-error-metrics -**/ -void Drive::CheckFalconFramePeriods() { - if (left_master_.HasResetOccurred()) { - conf::SetDriveMasterFramePeriods(left_master_); - falcon_reset_count_++; - } - if (right_master_.HasResetOccurred()) { - conf::SetDriveMasterFramePeriods(right_master_); - falcon_reset_count_++; - } - if (left_slave_.HasResetOccurred()) { - conf::SetDriveSlaveFramePeriods(left_slave_); - falcon_reset_count_++; - } - if (right_slave_.HasResetOccurred()) { - conf::SetDriveSlaveFramePeriods(right_slave_); - falcon_reset_count_++; - } -} - -/** - * This method is the most important as it is periodically called, and is what allows the robot to move - * After checking for motor resets, the current state will be updated - * Depending on the state, different cotnrollers will be updated such that the robot can perform the movement necessary - * to follow a path or orient the shooter. - * Outputs will also be written for analysis -**/ -void Drive::Periodic() { - CheckFalconFramePeriods(); - UpdateRobotState(); - switch (state_) { - case DriveState::OPEN_LOOP: - // TODO(josh) - break; - case DriveState::FOLLOW_PATH: - UpdatePathController(); - break; - case DriveState::SHOOT_ORIENT: - UpdateOrientController(); - default: - // TODO(josh) log here - break; - } - WriteOuts(); -} - -/** - * Empty method, presumably was supposed to be used to stop the drive -**/ -void Drive::Stop() {} - -/** - * Resets the position of sensors on the drive -**/ -void Drive::ZeroSensors() { - robot_state_.ResetFieldToRobot(); - WaitForNavxCalibration(0.5); - navx_.ZeroYaw(); - left_master_.SetSelectedSensorPosition(0); - right_master_.SetSelectedSensorPosition(0); - odometry_.ResetPosition({}, GetYaw()); -} - -/** - * Gives the NavX (navigation sensor for field oriented, auto balancing, collision detection, etc...) a time interval to calibrate - * Calibration retries, failures, and successes are supposed to be log, but atm nothing has been written to log it -**/ - void Drive::WaitForNavxCalibration(double timeout_sec) { - frc::Timer time; - time.Start(); - while (navx_.IsCalibrating() && time.Get() < timeout_sec) { - // LOG calib retry - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - } - if (navx_.IsCalibrating()) { - // LOG failed to calibrate - } else { - // LOG success - } -} - -/** - * Empty method, presumably to ... output telemetry -**/ -void Drive::OutputTelemetry() {} - -/** - * Adds the drive trajectory it wants to move at, and changes the current state to follow that path -**/ -void Drive::SetWantDriveTraj(frc::Trajectory&& traj) { - curr_traj_.emplace(std::move(traj)); - state_ = DriveState::FOLLOW_PATH; - traj_timer.Reset(); - traj_timer.Start(); -} - -/** - * Updates the odometry of the drive, such that the robot's relative position and pose can be predicted -**/ -void Drive::UpdateRobotState() { - odometry_.Update(GetYaw(), GetEncoder(left_master_), - GetEncoder(right_master_)); - robot_state_.ObserveFieldToRobot(frc2::Timer::GetFPGATimestamp(), - odometry_.GetPose()); -} - -/** - * Updates the path controller such that it'll correctly follow the trajectory the driver has set for it -**/ -void Drive::UpdatePathController() { - if (!curr_traj_.has_value()) { - // LOG - } - if (FinishedTraj()) { - curr_traj_.reset(); - SetWantRawOpenLoop({0.0_mps, 0.0_mps}); - } - auto time_along = traj_timer.Get(); - auto desired = curr_traj_.value().Sample(units::second_t{time_along}); - auto chassis_v = ramsete_.Calculate( - robot_state_.GetLatestFieldToRobot().second, desired); - auto wheel_v = kinematics_.ToWheelSpeeds(chassis_v); - // convert into falcon 500 internal encoder ticks - auto MetersPerSecToTicksPerDecisec = - [this](units::meters_per_second_t meters_per_second) -> double { - return meters_per_second / cfg_.meters_per_falcon_tick * 1_s / 10.0; - }; - // TODO(josh) consider using WPILib - // SimpleMotorFeedForward to add kA term - pout_.control_mode = ControlMode::Velocity; - pout_.left_demand = MetersPerSecToTicksPerDecisec(wheel_v.left); - pout_.right_demand = MetersPerSecToTicksPerDecisec(wheel_v.right); -} - -bool Drive::BackUp(double dist) { //units are meters - //do pid but until desired # of ticks works - double wheel_diameter = 5.75; //inches - double circumference = wheel_diameter*M_PI; - double rotations = dist/circumference; - double ticks = -rotations*22000; - - double ticks_gone = left_master_.GetSelectedSensorPosition(); - double Kp = -0.00003; - double Ki = -0.00002; - double error = abs(ticks) - abs(ticks_gone); - double correction = Kp*error + Ki; - - pout_.control_mode = ControlMode::PercentOutput; - pout_.left_demand = correction; - pout_.right_demand = correction; - - std::cout << "ticks: " << ticks << std::endl; - std::cout << "ticks gone: " << ticks_gone << std::endl; - std::cout << "correction: " << correction << std::endl; - - return abs(correction) < 0.1; - -} - - -/** - * Changes the current state of the robot to orient its vision sensor for a shot, and sets the angle at which the sensor should be rotated to -**/ -/*void Drive::SetWantOrientForShot(Limelight& limelight, double Kp, double Ki, double Kd) { - state_ = DriveState::SHOOT_ORIENT; - vision_rot_.SetGoal(0.0_rad); - - Kp = 0.017; - Ki = 0.015; - - double x_off = limelight.GetNetworkTable()->GetNumber("tx", 0.0); - - //auto position robot so x_off is reasonable - //https://docs.limelightvision.io/en/latest/cs_aiming.html - double heading_error = -x_off; - double steering_adjust = 0.0; - steering_adjust = Kp*heading_error + Ki; - if (steering_adjust < -1) steering_adjust = 1; - if (steering_adjust > 1) steering_adjust = 1; - // std::cout <<"x offset: " << x_off << std::endl; - std::cout << "steering adjust: " << steering_adjust << std::endl; - pout_.control_mode = ControlMode::PercentOutput; - pout_.left_demand = steering_adjust; - pout_.right_demand = -steering_adjust; -}*/ - -/** - * Returns true if the vision sensor has correctly rotated to the position set -**/ -/*bool Drive::OrientedForShot(Limelight& limelight) { - double x_off = limelight.GetNetworkTable()->GetNumber("tx", 0.0); - std::cout << x_off << std::endl; - return (x_off < 2); -}*/ - -/** - * Called if the current drive state is to orient for a shot - * It updates the vision controller, which determines the movement the vision sensor takes towards the goal -**/ -void Drive::UpdateOrientController() { - /* auto latest = robot_state_.GetLatestAngleToOuterPort(); - if (!latest.has_value()) { - return; - has_vision_target_ = false; - } - READING_SDB_NUMERIC(double, RotKp) kp; - READING_SDB_NUMERIC(double, RotKi) ki; - READING_SDB_NUMERIC(double, RotKd) kd; - vision_rot_.SetPID(kp, ki, kd); - READING_SDB_NUMERIC(double, RotVel) vel; - READING_SDB_NUMERIC(double, RotAcc) acc; - vision_rot_.SetConstraints({static_cast(vel) * 1_rad / 1_s, - static_cast(acc) * 1_rad / 1_s / 1_s}); - // std::cout << "read gains " << kp << " " << ki << " " << kd << " " << vel << " " << acc << std::endl; - has_vision_target_ = true; - auto err = latest.value().second; - double demand = vision_rot_.Calculate(err); - pout_.control_mode = ControlMode::PercentOutput; - pout_.left_demand = -demand; - pout_.right_demand = demand; - // std::cout << "orient w/ tgt, dmd " << err << " " << demand << std::endl; */ -} - -/** - * Returns true if the current trajectory variable does not have a value, meaning it is not currently moving -**/ -bool Drive::FinishedTraj() { - if (!curr_traj_.has_value()) { - // not running one at the moment - return true; - } - // TODO(josh) find a better way? - return traj_timer.Get() > curr_traj_.value().TotalTime(); -} - -/** - * Sets the drive's state to an open loop, meaning nothing gets updated everytime Periodic() is called -**/ -void Drive::SetWantRawOpenLoop( - const frc::DifferentialDriveWheelSpeeds& openloop) { - state_ = DriveState::OPEN_LOOP; - pout_.control_mode = ControlMode::PercentOutput; - pout_.left_demand = openloop.left.to(); - pout_.right_demand = openloop.right.to(); -} - -/** - * Taken from 254, it changes the controls and feel of manipulating the drive to that of a curvature drive - * https://www.reddit.com/r/FRC/comments/80679m/what_is_curvature_drive_cheesy_drive/ -**/ -void Drive::SetWantCheesyDrive(double throttle, double wheel, bool quick_turn) { - throttle = Deadband(throttle, 0.05); - wheel = Deadband(wheel, 0.035); - - constexpr double kWheelGain = 1.97; - constexpr double kWheelNonlinearity = 0.05; - const double denominator = std::sin(M_PI / 2.0 * kWheelNonlinearity); - // Apply a sin function that's scaled to make it feel better. - if (!quick_turn) { - wheel = std::sin(M_PI / 2.0 * kWheelNonlinearity * wheel); - wheel = std::sin(M_PI / 2.0 * kWheelNonlinearity * wheel); - wheel = wheel / (denominator * denominator) * std::abs(throttle); - } - wheel *= kWheelGain; - frc::DifferentialDriveWheelSpeeds signal = kinematics_.ToWheelSpeeds( - frc::ChassisSpeeds{units::meters_per_second_t{throttle}, 0.0_mps, - units::radians_per_second_t{wheel}}); - signal.Normalize(1.0_mps); - SetWantRawOpenLoop(signal); -} - -/** - * Writes the relevant outputs -**/ -void Drive::WriteOuts() { - // SDB_NUMERIC(double, LeftDriveTalonDemand){pout_.left_demand}; - // SDB_NUMERIC(double, RightDriveTalonDemand){pout_.right_demand}; - left_master_.Set(pout_.control_mode, pout_.left_demand); - right_master_.Set(pout_.control_mode, pout_.right_demand); - // if (pout_.left_demand == 0 && pout_.right_demand == 0) return; - // std::cout << "write out left demand: " << pout_.left_demand << std::endl; - // std::cout << "write out right demand: " << pout_.right_demand << std::endl; -} - -/** - * Gets the rotation of the drive in terms of radians -**/ -constexpr auto RAD_PER_DEGREE = units::constants::pi * 1_rad / 180.0; -frc::Rotation2d Drive::GetYaw() { - return frc::Rotation2d(navx_.GetYaw() * RAD_PER_DEGREE); -} - -/** - * Simple getter method to get the encoder object of the Talon motor controller -**/ -units::meter_t Drive::GetEncoder(TalonFX& master_talon) { - return (static_cast(master_talon.GetSelectedSensorPosition())) * - cfg_.meters_per_falcon_tick; -} - -} // namespace c2020 -} // namespace team114 diff --git a/c2021 v2/src/main/cpp/subsystems/drive.h b/c2021 v2/src/main/cpp/subsystems/drive.h deleted file mode 100644 index 3b888220..00000000 --- a/c2021 v2/src/main/cpp/subsystems/drive.h +++ /dev/null @@ -1,96 +0,0 @@ -#pragma once - -#include - -#include "minimal_phoenix.h" - -#include -#include -#include -#include -#include - -#include - -#include "config.h" -#include "robot_state.h" -#include "navx_ahrs.h" -#include "subsystem.h" -//#include "util/sdb_types.h" - -namespace team114 { -namespace c2020 { - -class Drive : public Subsystem { - DISALLOW_COPY_ASSIGN(Drive) - CREATE_SINGLETON(Drive) - public: - Drive(); - Drive(const conf::DriveConfig& cfg); - void Periodic() override; - void Stop() override; - void ZeroSensors() override; - void OutputTelemetry() override; - - void SetWantDriveTraj(frc::Trajectory&& traj); - bool FinishedTraj(); - - // Assumes range of -1 to 1 mps max speed - void SetWantRawOpenLoop(const frc::DifferentialDriveWheelSpeeds& openloop); - void SetWantCheesyDrive(double throttle, double wheel, bool quick_turn); - - // void SetWantOrientForShot(Limelight& limelight, double Kp, double Ki, double Kd); - // bool OrientedForShot(Limelight& limelight); - - bool BackUp(double dist); - - TalonFX left_master_, right_master_; //this is my code, and I do what I want - AHRS navx_{frc::SPI::Port::kMXP}; - - struct PeriodicOut { - ControlMode control_mode; - double left_demand; - double right_demand; - }; - PeriodicOut pout_{}; - - private: - enum class DriveState { - OPEN_LOOP, - FOLLOW_PATH, - SHOOT_ORIENT, - }; - void CheckFalconFramePeriods(); - - void UpdateRobotState(); - void UpdatePathController(); - void UpdateOrientController(); - - frc::Rotation2d GetYaw(); - units::meter_t GetEncoder(TalonFX& master_talon); - void WaitForNavxCalibration(double timeout_sec); - - // SDB_NUMERIC(unsigned int, DriveFalconResetCount) falcon_reset_count_{0}; - unsigned int falcon_reset_count_ = 0; //temporary? can't tell what line above is supposed to do - - //TalonFX left_master_, right_master_; - TalonFX left_slave_, right_slave_; - - void WriteOuts(); - - const conf::DriveConfig cfg_; - DriveState state_{DriveState::OPEN_LOOP}; - RobotState& robot_state_; - - frc::DifferentialDriveKinematics kinematics_; - frc::DifferentialDriveOdometry odometry_; - frc::RamseteController ramsete_; - std::optional curr_traj_; - frc2::Timer traj_timer{}; - - frc::ProfiledPIDController vision_rot_; - bool has_vision_target_; -}; - -} // namespace c2020 -} // namespace team114 \ No newline at end of file diff --git a/c2021 v2/src/main/cpp/subsystems/limelight.cpp b/c2021 v2/src/main/cpp/subsystems/limelight.cpp deleted file mode 100644 index a52b71e7..00000000 --- a/c2021 v2/src/main/cpp/subsystems/limelight.cpp +++ /dev/null @@ -1,90 +0,0 @@ -#include "limelight.h" - -#include "robot_state.h" -#include "number_util.h" - -namespace team114 { -namespace c2020 { - -Limelight::Limelight() : Limelight(conf::GetConfig().limelight) {} - -Limelight::Limelight(conf::LimelightConfig& cfg) : cfg_{std::move(cfg)} { - network_table_ = - nt::NetworkTableInstance::GetDefault().GetTable(cfg_.table_name); -} - -void Limelight::Periodic() { - ReadPeriodicIn(); - WritePeriodicOut(); - RobotState::GetInstance().ObserveVision(frc2::Timer::GetFPGATimestamp(), - GetTarget()); -} - -void Limelight::ReadPeriodicIn() { - per_in_.latency = - network_table_->GetNumber("tl", 0) * 1_ms + kImageCaptureLatency; - per_in_.led_mode = (int)network_table_->GetNumber("ledMode", 1.0); - per_in_.pipeline = (int)network_table_->GetNumber("pipeline", 0); - per_in_.x_off = network_table_->GetNumber("tx", 1000.0); - per_in_.y_off = network_table_->GetNumber("ty", 1000.0); - per_in_.area = network_table_->GetNumber("ta", 1000.0); - // paranoid about double equality... - sees_target_ = network_table_->GetNumber("tv", 0) > 0.5; -} - -void Limelight::WritePeriodicOut() { - if (per_out_.led_mode != per_in_.led_mode || - per_out_.pipeline != per_in_.pipeline) { - // LOG - per_out_dirty_ = true; - } - if (per_out_dirty_) { - network_table_->PutNumber("ledMode", per_out_.led_mode); - network_table_->PutNumber("camMode", per_out_.cam_mode); - network_table_->PutNumber("pipeline", per_out_.pipeline); - network_table_->PutNumber("stream", per_out_.stream); - network_table_->PutNumber("snapshot", per_out_.snapshot); - per_out_dirty_ = false; - } -} - -void Limelight::SetLedMode(LedMode mode) { - int mode_int = static_cast(mode); - if (mode_int != per_out_.led_mode) { - per_out_.led_mode = mode_int; - per_out_dirty_ = true; - } -} - -void Limelight::SetPipeline(int pipeline) { - if (pipeline != per_out_.pipeline) { - // TODO reset veision here - // RobotState::GetInstance().ResetVision(); - per_out_.pipeline = pipeline; - // LOG - per_out_dirty_ = true; - } -} - -int Limelight::GetPipeline() { return per_out_.pipeline; } - -void Limelight::ForceFlushOuts() { per_out_dirty_ = true; } - -bool Limelight::SeesTarget() { return sees_target_; } - -units::second_t Limelight::GetLatency() { return per_in_.latency; } - -std::shared_ptr Limelight::GetNetworkTable() { return network_table_; } - -std::optional Limelight::GetTarget() { - if (SeesTarget()) { - TargetInfo t; - t.horizontal = DegToRad(-per_in_.x_off); - t.vertical = DegToRad(per_in_.y_off); - return t; - } - return {}; -} - -} // namespace c2020 -} // namespace team114 diff --git a/c2021 v2/src/main/cpp/subsystems/limelight.h b/c2021 v2/src/main/cpp/subsystems/limelight.h deleted file mode 100644 index 6d046125..00000000 --- a/c2021 v2/src/main/cpp/subsystems/limelight.h +++ /dev/null @@ -1,97 +0,0 @@ -#pragma once - -#include -#include -#include - -#include "frc/geometry/Pose2d.h" -#include "frc/geometry/Rotation2d.h" -#include "frc/geometry/Translation2d.h" -#include "frc/smartdashboard/SmartDashboard.h" -#include "frc2/Timer.h" - -#include "networktables/NetworkTable.h" -#include "networktables/NetworkTableInstance.h" - -#include - -#include "config.h" -#include "subsystem.h" - -namespace team114 { -namespace c2020 { - -class Limelight : public Subsystem { - public: - // SUBSYSTEM_PRELUDE(Limelight); - const units::second_t kImageCaptureLatency = 11_ms; - - struct RawTargetInfo { - double x = 1.0; - double y; - double z; - }; - struct TargetInfo { - units::radian_t horizontal; - units::radian_t vertical; - }; - const conf::LimelightConfig cfg_; - - enum class LedMode : int { - PIPELINE = 0, - OFF = 1, - BLINK = 2, - ON = 3, - }; - - Limelight(); - Limelight(conf::LimelightConfig& cfg); - - void Periodic() final override; - - void SetLedMode(LedMode mode); - - void SetPipeline(int pipeline); - - int GetPipeline(); - - void ForceFlushOuts(); - - bool SeesTarget(); - - units::second_t GetLatency(); - - std::shared_ptr GetNetworkTable(); - - std::optional GetTarget(); - - private: - void ReadPeriodicIn(); - - void WritePeriodicOut(); - - std::shared_ptr network_table_; - - struct PeriodicIn { - units::second_t latency; - int led_mode; - int pipeline; - double x_off; - double y_off; - double area; - }; - struct PeriodicOut { - int led_mode = 1; // 0 - use pipeline mode, 1 - off, 2 - blink, 3 - on - int cam_mode = 0; // 0 - vision processing, 1 - driver camera - int pipeline = 0; // 0 - 9 - int stream = 2; // sets stream layout if another webcam is attached - int snapshot = 0; // 0 - stop snapshots, 1 - 2 Hz - }; - PeriodicIn per_in_; - PeriodicOut per_out_; - bool per_out_dirty_ = true; - bool sees_target_ = false; -}; - -} // namespace c2020 -} // namespace team114 \ No newline at end of file diff --git a/c2021 v2/src/main/include/Drivetrain.h b/c2021 v2/src/main/include/Drivetrain.h deleted file mode 100644 index 55281f50..00000000 --- a/c2021 v2/src/main/include/Drivetrain.h +++ /dev/null @@ -1,25 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -#include "ctre/Phoenix.h" - -class Drivetrain { - public: - Drivetrain(); - void Drive(); - void UpdateOdometry(); - - private: - TalonFX left_master = {0}; - TalonFX left_slave = {1}; - TalonFX right_master = {2}; - TalonFX right_slave = {3}; - -}; - - diff --git a/c2021 v2/src/main/include/Robot.h b/c2021 v2/src/main/include/Robot.h index 554437db..9b87d518 100644 --- a/c2021 v2/src/main/include/Robot.h +++ b/c2021 v2/src/main/include/Robot.h @@ -1,50 +1,51 @@ #pragma once -#include -#include -#include +#include -#include "robot_state.h" -#include "drive.h" +#include +#include +#include +#include +#include +#include -#include +#include "ctre/Phoenix.h" +#include "frc/WPILib.h" +#include -#include "controls.h" +#include +#include +#include +#include +#include +#include "Drive.h" +#include "Shoot.h" -namespace team114 { -namespace c2020 { +//Drive +frc::Joystick l_joy{0}; +frc::Joystick r_joy{1}; +frc::XboxController xbox{2}; class Robot : public frc::TimedRobot { - public: - inline static const auto kPeriod = 10_ms; - Robot(); + public: void RobotInit() override; void RobotPeriodic() override; - void AutonomousInit() override; void AutonomousPeriodic() override; - void TeleopInit() override; void TeleopPeriodic() override; - - void TestInit() override; - void TestPeriodic() override; - void DisabledInit() override; void DisabledPeriodic() override; + void TestInit() override; + void TestPeriodic() override; - private: - Controls controls_; - Drive& drive_; - RobotState& robot_state_; - frc::Joystick ljoy_; - frc::Joystick rjoy_; - frc::Joystick ojoy_; - conf::RobotConfig cfg; + private: + frc::SendableChooser m_chooser; + const std::string kAutoNameDefault = "Default"; + const std::string kAutoNameCustom = "My Auto"; + std::string m_autoSelected; - // CachingSolenoid brake_{frc::Solenoid{6}}; + Drive Drivetrain; + Shoot Shooter; }; - -} // namespace c2020 -} // namespace team114 \ No newline at end of file diff --git a/c2021 v2/src/main/include/Shooter.h b/c2021 v2/src/main/include/Shooter.h deleted file mode 100644 index 77e81455..00000000 --- a/c2021 v2/src/main/include/Shooter.h +++ /dev/null @@ -1,2 +0,0 @@ -#pragma once - diff --git a/c2021 v2/src/main/include/config.h b/c2021 v2/src/main/include/config.h deleted file mode 100644 index fd8a4c55..00000000 --- a/c2021 v2/src/main/include/config.h +++ /dev/null @@ -1,172 +0,0 @@ -#pragma once - -#include - -#include -#include "minimal_phoenix.h" //need to take care of - -namespace team114 { -namespace c2020 { -namespace conf { - -struct DriveConfig { - int left_master_id; /** < talon id of master drive motor on left side **/ - int left_slave_id; /** < talon id of slave drive motor on left side **/ - int right_master_id; /** < talon id of master drive motor on right side **/ - int right_slave_id; /** < talon id of slave drive motor on right side **/ - units::meter_t track_width; - units::meter_t meters_per_falcon_tick; /**< self explanitory, look up what a frc falcon tick is **/ - units::meters_per_second_t traj_max_vel; - units::meters_per_second_squared_t traj_max_accel; - units::meters_per_second_squared_t traj_max_centrip_accel; - double orient_kp; - double orient_ki; - double orient_kd; - double orient_vel; - double orient_acc; -}; - -struct ControlPanelConfig { - int talon_id; /** < talon id for control panel motor **/ - double current_limit; /** < current limit (amps) for said motor **/ - double ticks_per_inch; /**< self explanitory, look up what a frc falcon tick is **/ - double kP; /**< P constant for this motor's pid **/ - double kI; /**< I constant for this motor's pid **/ - double kD; /**< D constant for this motor's pid **/ - double ticks_per_color_slice; /**< # of ticks to rotate the color wheel one "slice" (which is a specific angle) **/ - double rot_control_ticks; - double scoot_cmd; - int solenoid_channel; /**< I think the wheel is lifted with a pneumatic, and this is likely its solenoid **/ - std::string sdb_key; -}; - -struct IntakeConfig { - int rot_talon_id; - int roller_talon_id; - double intake_cmd; /**< percent output for when intake is running **/ - double rot_current_limit; /**< current limit (amps) for rotator motor **/ - double roller_current_limit; /**< current limit (amps) for roller motor **/ - double zeroing_kp; - double zeroing_vel; - double abs_enc_tick_offset; - double abs_ticks_per_rot; - double rel_ticks_per_abs_tick; - double rads_per_rel_tick; - double zeroed_rad_from_vertical; - double SinekF; - double kP; - double kI; - double kD; - double profile_vel; - double profile_acc; - int stowed_pos_ticks; - int intaking_pos_ticks; - // int trench_driving_pos_ticks; -}; - -struct HoodConfig { - int talon_id; /**< talon id of motor that moves the shooter hood **/ - double ticks_per_degree; /**< self explanitory, look up what a frc falcon tick is **/ - double min_degrees; /** < smallest angle hood can move to **/ - double max_degrees; /** < largest angle hood can move to **/ - double current_limit; /** < current limit (amps) for hood motor **/ - double zeroing_current; - double zeroing_kp; - double zeroing_vel; - double profile_acc; - double profile_vel; - double kP; - double kI; - double kD; - int ctre_curve_smoothing; -}; - - -struct ShooterConifg { - int master_id; /**< The shooter flywheel takes 2 motors to spin, this is the master's id. left of shooter **/ - int slave_id; /**< The respective slave ID. Right of shooter. **/ - int turret_id; - double shooter_current_limit; /**< The current limit (amps) of the talons **/ - double kF; /**< F (feedforward) term in shooter PID **/ - double kP; /**< P term in shooter PID **/ - double kI; /**< I term in shooter PID **/ - double kD; /**< D term in shooter PID **/ - VelocityMeasPeriod meas_period; /**< Talons can return "velocity". Velocity is measured every period and averaged. */ - int meas_filter_width; /**< Helps with velocity measurement. **/ - double shootable_err_pct; /**< Perhaps ammount of error acceptable when trying to make a shot? idk josh wrote this and we now have a new system*/ - int kicker_id; /**< Motor/wheel that spins weels into shooter wheel. **/ - double kicker_current_limit; /**< Current limit (amps) for kicker talon (small green wheel below flywheel, helps feed balls into shooter) */ - double kicker_cmd; /**< Percent output for kicker talon when engaged **/ -}; - -struct BallChannelConfig { - int serializer_id; /**< Talon ID of serializing talon motor. Serializer is in front of robot, it feeds balls into channel after they are intaked**/ - int channel_id; /**< Talon id of the channel motor **/ - double current_limit; /**< The current limit (amps) of the talons **/ - double serializer_cmd; /**< percent output to serializing talon when it is turned on **/ - double channel_cmd; /**< percent output to channel talon when it is turned on **/ - int s0_port; /**< we use sensors to track the balls in the channel. this is one of them **/ - int s1_port; /**< we use sensors to track the balls in the channel. this is one of them **/ - int s2_port; /**< antiquated, we don't use this sensor **/ - int s3_port; /**< antiquated, we don't use this sensor **/ -}; - -struct ClimberConfig { - int master_id; /**< ID of master talon motor **/ - int slave_id; /**< ID of slave talon motor **/ - double current_limit; /**< The current limit (amps) of the talons **/ - // spool winding makes this weird - double avg_ticks_per_inch; /**< Probably number of ticks on motors that pass for climber to raise one inch. **/ - int release_solenoid_id; /**< ID of solenoid that controls the release pneumatic (to raise/lower climber) **/ - int brake_solenoid_id; /**< ID of brake solenoid, I think it's purpose is to hold the climber in position? **/ - int initial_step_ticks; /**< Probably the amount of ticks the motors should turn initially to raise the climber. **/ - int forward_soft_limit_ticks; /**< Probably a soft limit for how many ticks the motors should turn. **/ - double ascend_command; /**< Percent output to talons to raise climber (positive value) **/ - double descend_command; /**< Percent output to talons to lower climber (negative value) **/ -}; - -struct LimelightConfig { - std::string name; /**< Unused? Name of limelight**/ - std::string table_name; /**< Name used for limelight NetworkTable**/ - units::meter_t diff_height; /**< Not sure, unused **/ - units::radian_t angle_above_horizontal; /**< limelight isn't exaclty level; this is the offset angle **/ -}; - -struct RobotConfig { - std::string mac_address; /**< For identifying roboRIO **/ - DriveConfig drive; /**< For the drive base, go to DriveConfig for more info **/ - ControlPanelConfig ctrl_panel; /**< For control panel, go to ControlPanelConfig for more info **/ - IntakeConfig intake; /**< For the intake, go to IntakeConfig for more info **/ - HoodConfig hood; /**< For the shooter hood, go to HoodConfig for more info **/ - ShooterConifg shooter; /**< Perhaps scrapped, but for entire shooter. Go to ShooterConifg for more info **/ - BallChannelConfig ball_channel; /**< For the ball channel, go to BallChannelConfig for more info **/ - ClimberConfig climber; /**< For the climber, go to ClimberConfig for more info **/ - LimelightConfig limelight; /**< For the limelight, go to LimelightConfig for more info **/ -}; - -RobotConfig& GetConfig(); - -void DriveFalconCommonConfig(TalonFX& falcon); - -void SetDriveMasterFramePeriods(TalonFX& falcon); - -void SetDriveSlaveFramePeriods(TalonFX& falcon); - -enum class FeedbackType { - Quadrature, - PulseWidth, - Both, - None, -}; - -void SetFramePeriodsForPidTalon(TalonSRX& talon, FeedbackType feedback_type = FeedbackType::None); -void SetFramePeriodsForPidTalonFX(TalonFX& talon, FeedbackType feedback_type = FeedbackType::None); - -void SetFramePeriodsForOpenLoopTalon(TalonSRX& talon); - -void SetFramePeriodsForSlaveTalon(TalonSRX& talon); -void SetFramePeriodsForSlaveTalonFX(TalonFX& talon); - -} // namespace conf -} // namespace c2020 -} // namespace team114 \ No newline at end of file diff --git a/c2021 v2/src/main/include/controls.h b/c2021 v2/src/main/include/controls.h deleted file mode 100644 index 23eb012b..00000000 --- a/c2021 v2/src/main/include/controls.h +++ /dev/null @@ -1,91 +0,0 @@ -#pragma once - -#include - -namespace team114 { -namespace c2020 { - -class EdgeDetector { - public: - bool Pressed(bool observed) { - bool to_ret = observed && !last_; - last_ = observed; - return to_ret; - } - - private: - bool last_ = false; -}; - -class ScootStateTracker { - public: - bool PassThroughFeed(bool val) { - if (val) { - been_fed_ = true; - } - return val; - } - bool WasNotHeld() { - bool to_ret = !been_fed_; - been_fed_ = false; - return to_ret; - } - - private: - bool been_fed_ = true; -}; - -class Controls { - public: - Controls(); - - static constexpr double kTriggerThreshold = 0.3; - static constexpr double kAnalogJoyThreshold = 0.3; - - void OPrints(); - - double Throttle(); - double Wheel(); - bool QuickTurn(); - - bool ClimbUp(); - bool ClimbDown(); - - bool ShotShortPressed(); - bool ShotMedPressed(); - bool ShotLongPressed(); - - bool Intake(); - bool Unjam(); - bool Shoot(); - - bool ShortShot(); - bool LongShot(); - - bool PanelDeploy(); - bool ScootLeftPressed(); - bool ScootRightPressed(); - bool ScootReleased(); - bool RotControlPressed(); - bool PosControlRedPressed(); - bool PosControlYellowPressed(); - bool PosControlGreenPressed(); - bool PosControlBluePressed(); - - private: - frc::Joystick ljoy_; - frc::Joystick rjoy_; - frc::Joystick ojoy_; - - EdgeDetector shot_short_; - EdgeDetector shot_med_; - EdgeDetector shot_long_; - - EdgeDetector scoot_left_; - EdgeDetector scoot_right_; - ScootStateTracker scoot_tracker_; - EdgeDetector scoot_released_; -}; - -} // namespace c2020 -} // namespace team114 \ No newline at end of file diff --git a/c2021 v2/src/main/include/robot_state.h b/c2021 v2/src/main/include/robot_state.h deleted file mode 100644 index efd48faf..00000000 --- a/c2021 v2/src/main/include/robot_state.h +++ /dev/null @@ -1,51 +0,0 @@ -#pragma once - -#include - -#include - -#include "config.h" -#include "subsystem.h" -#include "limelight.h" -#include "util/construcotr_macros.h" -#include "util/interp_map.h" - -namespace team114 { -namespace c2020 { - -// Saying frame1_to_frame2 represents the transform applied to the frame1 origin -// that will bring it to the frame2 origin. -class RobotState { - DISALLOW_COPY_ASSIGN(RobotState) - CREATE_SINGLETON(RobotState) - public: - RobotState(); - RobotState(conf::RobotConfig cfg); - - std::pair GetLatestFieldToRobot(); - frc::Pose2d GetFieldToRobot(units::second_t); - void ObserveFieldToRobot(units::second_t timestamp, - const frc::Pose2d& pose); - void ResetFieldToRobot(); - - void ObserveVision(units::second_t timestamp, - std::optional target); - std::optional> - GetLatestDistanceToOuterPort(); - std::optional> - GetLatestAngleToOuterPort(); - - private: - InterpolatingMap, Pose2dInterp> - field_to_robot_; - - const conf::LimelightConfig ll_cfg_; - std::optional> - debounced_vision_; - const unsigned int kDroppableFrames = 3; - unsigned int vision_null_ct_; -}; - -} // namespace c2020 -} // namespace team114 \ No newline at end of file diff --git a/c2021 v2/src/main/include/subsystem.h b/c2021 v2/src/main/include/subsystem.h deleted file mode 100644 index 41222813..00000000 --- a/c2021 v2/src/main/include/subsystem.h +++ /dev/null @@ -1,59 +0,0 @@ -#pragma once - -//#include "util/constructor_macros.h" weird josh stuff - -namespace team114 { -namespace c2020 { - -class Subsystem { - public: - /** - * Subsystem is an abstract class (and all following functions are abstract as well). An empty constructor. - **/ - virtual ~Subsystem() = default; - - /** - * Called in Robot::TeleopPeriodic() if it makes sense (hood and intake yes, control panel(unfinished?) and climber no) - **/ - virtual void Periodic(){}; - /** - * Resets things (like setting motors back to initial position) - **/ - virtual void Stop(){}; - /** - * Presumably to reset sensors, although it currently isn't used for much in the subclasses. - **/ - virtual void ZeroSensors(){}; - /** - * Currently undefined in subclasses, but it could presumably be used to output general positional or sensory data. - **/ - virtual void OutputTelemetry(){}; -}; - -// static member is inline so as to be single across translation units -// https://stackoverflow.com/a/53705993 -#define CREATE_SINGLETON(Classname) \ - private: \ - inline static Classname* __singleton_instance_; \ - \ - public: \ - static Classname& GetInstance() { \ - if (__singleton_instance_ == nullptr) { \ - __singleton_instance_ = new Classname(); \ - } \ - return *__singleton_instance_; \ - } \ - static void DestroyInstance() { \ - delete __singleton_instance_; \ - __singleton_instance_ = nullptr; \ - } - -#define SUBSYSTEM_PRELUDE(Classname) \ - private: \ - Classname(); \ - CREATE_SINGLETON(Classname) \ - public: /* better diagnostics if public */ \ - DISALLOW_COPY_ASSIGN(Classname) - -} // namespace c2020 -} // namespace team114 \ No newline at end of file diff --git a/c2021 v2/src/main/include/util/caching_types.h b/c2021 v2/src/main/include/util/caching_types.h deleted file mode 100644 index dd91e894..00000000 --- a/c2021 v2/src/main/include/util/caching_types.h +++ /dev/null @@ -1,34 +0,0 @@ -#pragma once - -#include - -#include - -namespace team114 { -namespace c2020 { - -class CachingSolenoid { - public: - CachingSolenoid(frc::Solenoid&& sol) : cache_{false}, sol_{std::move(sol)} { - sol_.Set(false); - } - - void Set(bool actuated) { - // For some reason actually caching breaks things - // TODO fix this - sol_.Set(actuated); - // if (cache_ != actuated) { - // cache_ = actuated; - // sol_.Set(actuated); - // std::cout << "set solenoid " << sol_.GetName() << sol_.Get() - // << std::endl; - // } - } - - private: - bool cache_; - frc::Solenoid sol_; -}; - -} // namespace c2020 -} // namespace team114 \ No newline at end of file diff --git a/c2021 v2/src/main/include/util/construcotr_macros.h b/c2021 v2/src/main/include/util/construcotr_macros.h deleted file mode 100644 index 0fdb4460..00000000 --- a/c2021 v2/src/main/include/util/construcotr_macros.h +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once - -#define DISALLOW_COPY(Type) Type(const Type&) = delete; -#define DISALLOW_ASSIGN(Type) Type& operator=(const Type&) = delete; - -#define DISALLOW_COPY_ASSIGN(Type) \ - DISALLOW_COPY(Type) \ - DISALLOW_ASSIGN(Type) \ No newline at end of file diff --git a/c2021 v2/src/main/include/util/interp_map.h b/c2021 v2/src/main/include/util/interp_map.h deleted file mode 100644 index bf36ac44..00000000 --- a/c2021 v2/src/main/include/util/interp_map.h +++ /dev/null @@ -1,145 +0,0 @@ -#pragma once - -#include - -#include - -namespace team114 { -namespace c2020 { - -template double [0, - // 1) - typename TInterpolateFunctor, // (lower_val, upper_val, double [0, - // 1)) -> T - class Compare = std::less, // map::key_compare - class Alloc = - std::allocator > // map::allocator_type - > -class InterpolatingMap { - public: - typedef std::map InnerMap; - InterpolatingMap(typename InnerMap::size_type max_size) - : inverse_interp_{}, interp_{}, map_{}, max_size_{max_size} {} - InnerMap& Inner() { return map_; } - typename InnerMap::value_type InterpAt(Key key) { - // gets prev item, if key exists return that item - auto below_iter = GetLowerOrEqual(key); - // gets next item, if key exists returns next item - auto above_iter = map_.upper_bound(key); - auto end = map_.end(); - if (below_iter == end && above_iter == end) { - // map is empty, we'd rather not return an past-the-end iter as STL - // would, and this is a degenerate case, so throw - throw std::out_of_range{"interp_map is empty"}; - } - if (above_iter == end) { - // return map end - return *below_iter; - } - if (below_iter == end) { - // return map beginning - return *above_iter; - } - // both iterators are valid, interpolate: - double interp = - inverse_interp_(below_iter->first, above_iter->first, key); - const T value = interp_(below_iter->second, above_iter->second, interp); - typename InnerMap::value_type ret{key, value}; - return ret; - } - - // naming assumes time-based and that comparator is less - typename InnerMap::value_type Latest() { - auto it = map_.rbegin(); - if (it == map_.rend()) { - throw std::out_of_range{"interp_map is empty"}; - } - return *it; - } - - // DOES NOT INTERPOLATE, for insertion only - T& operator[](const Key&& key) { - if (map_.size() >= max_size_) { - map_.erase(map_.begin()); - } - return map_[key]; - } - T& operator[](const Key& key) { - if (map_.size() >= max_size_) { - map_.erase(map_.begin()); - } - return map_[key]; - } - - void CheckSize() { - if (map_.size() > max_size_) { - map_.erase(map_.begin()); - } - } - - private: - auto GetLowerOrEqual(Key key) { - auto it = map_.upper_bound(key); - if (it == map_.begin()) { - // if the map is empty, we find nothing and end == begin, in which - // case return end, thats covered - // if our key is below all other keys, our result will be the first - // element, which is begin in that case there is no lower or equal, - // so return end - return map_.end(); - } - // now we either have: - // 1. our key was past all other keys, we got one-past-end, want the - // last element - // 2. our key was not in the map, we got one-past what we want - // 3. our key was in the map, we got one-past what we want - --it; - return it; - } - - KeyInverseInterpolateFunctor inverse_interp_; - TInterpolateFunctor interp_; - InnerMap map_; - const typename InnerMap::size_type max_size_; -}; - -template -struct ArithmeticInverseInterp { - double operator()(T low, T high, T key) { - T total_diff = high - low; - T diff = key - low; - return diff / total_diff; - } -}; - -template -struct ArithmeticInterp { - T operator()(T low, T high, double pct) { - T total_diff = high - low; - return low + pct * total_diff; - } -}; - -struct Pose2dInterp { - frc::Pose2d operator()(const frc::Pose2d& low, const frc::Pose2d& high, - double pct) { - if (pct <= 0) { - return low; - } - if (pct >= 1) { - return high; - } - frc::Twist2d twist = low.Log(high); - frc::Twist2d interped_twist; - interped_twist.dx = pct * twist.dx; - interped_twist.dy = pct * twist.dy; - interped_twist.dtheta = pct * twist.dtheta; - return low.Exp(interped_twist); - } -}; - -} // namespace c2020 -} // namespace team114 \ No newline at end of file diff --git a/c2021 v2/src/main/include/util/number_util.h b/c2021 v2/src/main/include/util/number_util.h deleted file mode 100644 index 93a6a842..00000000 --- a/c2021 v2/src/main/include/util/number_util.h +++ /dev/null @@ -1,43 +0,0 @@ -#pragma once - -#include -#include - -#include - -namespace team114 { -namespace c2020 { - -template -constexpr inline bool EpsilonEq(T one, T two, T epsilon) noexcept { - return std::abs(one - two) < epsilon; -} - -template -constexpr inline T Deadband(T val, T deadband) noexcept { - constexpr T kZero = T(0.0); - if (EpsilonEq(val, kZero, deadband)) { - return kZero; - } - return val; -} - -template -constexpr inline T Clamp(T val, T min, T max) noexcept { - if (val < min) { - return min; - } else if (val > max) { - return max; - } else { - return val; - } -} - -template -constexpr inline units::radian_t DegToRad(T deg) { - constexpr T conv = M_PI / 180.0; - return units::radian_t{deg * conv}; -} - -} // namespace c2020 -} // namespace team114 \ No newline at end of file diff --git a/c2021 v2/src/main/include/util/sdb_types.h b/c2021 v2/src/main/include/util/sdb_types.h deleted file mode 100644 index a9f6ae96..00000000 --- a/c2021 v2/src/main/include/util/sdb_types.h +++ /dev/null @@ -1,83 +0,0 @@ -#pragma once - -#include -#include - -#include - -namespace team114 { -namespace c2020 { - -// no string literals in templates yet, so have to resort to an ugly macro -// defining an anonymous type. When/if those come, we can unify all types -// into one beautiful SFINAE dance, without any macros - -#define SDB_NUMERIC(type, key_ident) \ - struct __SdbKey_##key_ident { \ - static const std::string GetName() { return #key_ident; }; \ - }; \ - team114::c2020::SdbNumeric - -template -struct SdbNumeric { - SdbNumeric() : value() { Update(); }; - SdbNumeric(const NumericTy& val) : value(val) { Update(); } - const NumericTy& operator=(const NumericTy& val) { - value = val; - Update(); - return val; - } - operator NumericTy() const { return value; } - NumericTy operator++(int) { - value++; - Update(); - return value; - } - - private: - NumericTy value; - void Update() { frc::SmartDashboard::PutNumber(KeyTy::GetName(), value); } -}; - -#define SDB_BOOL(key_ident) \ - struct __SdbKey_##key_ident { \ - static const std::string GetName() { return #key_ident; }; \ - }; \ - team114::c2020::SdbBool<__SdbKey_##key_ident> - -template -struct SdbBool { - SdbBool() : value() { Update(); }; - SdbBool(const bool& val) : value(val) { Update(); } - const bool& operator=(const bool& val) { - value = val; - Update(); - return val; - } - operator bool() const { return value; } - - private: - bool value; - void Update() { frc::SmartDashboard::PutBoolean(KeyTy::GetName(), value); } -}; - -#define READING_SDB_NUMERIC(type, key_ident) \ - struct __SdbKey_##key_ident { \ - static const std::string GetName() { return #key_ident; }; \ - }; \ - team114::c2020::ReadingSdbNumeric - -template -struct ReadingSdbNumeric { - ReadingSdbNumeric() { - if (!frc::SmartDashboard::GetEntry(KeyTy::GetName()).Exists()) { - frc::SmartDashboard::PutNumber(KeyTy::GetName(), NumericTy()); - } - } - operator NumericTy() const { - return frc::SmartDashboard::GetNumber(KeyTy::GetName(), NumericTy()); - } -}; - -} // namespace c2020 -} // namespace team114 \ No newline at end of file From 203b22cf9e1f1bfbc337f45c43c253e0a2e94ef1 Mon Sep 17 00:00:00 2001 From: Alex Chang Date: Sat, 2 Oct 2021 22:22:40 -0700 Subject: [PATCH 3/4] added test code in branch Alex-branch --- 2021 test code/.gitignore | 161 +++++++++++++ 2021 test code/.vscode/launch.json | 21 ++ 2021 test code/.vscode/settings.json | 18 ++ .../.wpilib/wpilib_preferences.json | 6 + 2021 test code/WPILib-License.md | 24 ++ 2021 test code/build.gradle | 105 +++++++++ .../gradle/wrapper/gradle-wrapper.jar | Bin 0 -> 58702 bytes .../gradle/wrapper/gradle-wrapper.properties | 5 + 2021 test code/gradlew | 183 +++++++++++++++ 2021 test code/gradlew.bat | 100 ++++++++ 2021 test code/settings.gradle | 27 +++ 2021 test code/src/main/cpp/Controls.cpp | 58 +++++ 2021 test code/src/main/cpp/Drive.cpp | 28 +++ 2021 test code/src/main/cpp/Intake.cpp | 57 +++++ 2021 test code/src/main/cpp/Robot.cpp | 76 +++++++ 2021 test code/src/main/deploy/example.txt | 4 + 2021 test code/src/main/include/Controls.h | 33 +++ 2021 test code/src/main/include/Drive.h | 48 ++++ 2021 test code/src/main/include/Intake.h | 31 +++ 2021 test code/src/main/include/Robot.h | 42 ++++ 2021 test code/src/test/cpp/main.cpp | 10 + 2021 test code/vendordeps/Phoenix.json | 214 ++++++++++++++++++ .../vendordeps/WPILibNewCommands.json | 37 +++ .../vendordeps/WPILibOldCommands.json | 37 +++ 2021 test code/vendordeps/navx_frc.json | 35 +++ 25 files changed, 1360 insertions(+) create mode 100644 2021 test code/.gitignore create mode 100644 2021 test code/.vscode/launch.json create mode 100644 2021 test code/.vscode/settings.json create mode 100644 2021 test code/.wpilib/wpilib_preferences.json create mode 100644 2021 test code/WPILib-License.md create mode 100644 2021 test code/build.gradle create mode 100644 2021 test code/gradle/wrapper/gradle-wrapper.jar create mode 100644 2021 test code/gradle/wrapper/gradle-wrapper.properties create mode 100644 2021 test code/gradlew create mode 100644 2021 test code/gradlew.bat create mode 100644 2021 test code/settings.gradle create mode 100644 2021 test code/src/main/cpp/Controls.cpp create mode 100644 2021 test code/src/main/cpp/Drive.cpp create mode 100644 2021 test code/src/main/cpp/Intake.cpp create mode 100644 2021 test code/src/main/cpp/Robot.cpp create mode 100644 2021 test code/src/main/deploy/example.txt create mode 100644 2021 test code/src/main/include/Controls.h create mode 100644 2021 test code/src/main/include/Drive.h create mode 100644 2021 test code/src/main/include/Intake.h create mode 100644 2021 test code/src/main/include/Robot.h create mode 100644 2021 test code/src/test/cpp/main.cpp create mode 100644 2021 test code/vendordeps/Phoenix.json create mode 100644 2021 test code/vendordeps/WPILibNewCommands.json create mode 100644 2021 test code/vendordeps/WPILibOldCommands.json create mode 100644 2021 test code/vendordeps/navx_frc.json diff --git a/2021 test code/.gitignore b/2021 test code/.gitignore new file mode 100644 index 00000000..983678aa --- /dev/null +++ b/2021 test code/.gitignore @@ -0,0 +1,161 @@ +# Created by https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### Java ### +# Compiled class file +*.class + +# Log file +*.log + +# BlueJ files +*.ctxt + +# Mobile Tools for Java (J2ME) +.mtj.tmp/ + +# Package Files # +*.jar +*.war +*.nar +*.ear +*.zip +*.tar.gz +*.rar + +# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_pid* + +### Linux ### +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory + +# Linux trash folder which might appear on any partition or disk +.Trash-* + +# .nfs files are created when an open file is removed but is still being accessed +.nfs* + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json + +### Windows ### +# Windows thumbnail cache files +Thumbs.db +ehthumbs.db +ehthumbs_vista.db + +# Dump file +*.stackdump + +# Folder config file +[Dd]esktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msix +*.msm +*.msp + +# Windows shortcuts +*.lnk + +### Gradle ### +.gradle +/build/ + +# Ignore Gradle GUI config +gradle-app.setting + +# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) +!gradle-wrapper.jar + +# Cache of project +.gradletasknamecache + +# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 +# gradle/wrapper/gradle-wrapper.properties + +# # VS Code Specific Java Settings +.classpath +.project +.settings/ +bin/ +imgui.ini + + +# End of https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode diff --git a/2021 test code/.vscode/launch.json b/2021 test code/.vscode/launch.json new file mode 100644 index 00000000..c9c9713d --- /dev/null +++ b/2021 test code/.vscode/launch.json @@ -0,0 +1,21 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + + { + "type": "wpilib", + "name": "WPILib Desktop Debug", + "request": "launch", + "desktop": true, + }, + { + "type": "wpilib", + "name": "WPILib roboRIO Debug", + "request": "launch", + "desktop": false, + } + ] +} diff --git a/2021 test code/.vscode/settings.json b/2021 test code/.vscode/settings.json new file mode 100644 index 00000000..5e795ffd --- /dev/null +++ b/2021 test code/.vscode/settings.json @@ -0,0 +1,18 @@ +{ + "java.configuration.updateBuildConfiguration": "disabled", + "java.import.gradle.enabled": false, + "files.exclude": { + "**/.git": true, + "**/.svn": true, + "**/.hg": true, + "**/CVS": true, + "**/.DS_Store": true, + "bin/": true, + "**/.classpath": true, + "**/.project": true, + "**/.settings": true, + "**/.factorypath": true, + "**/*~": true + }, + "C_Cpp.default.configurationProvider": "vscode-wpilib" +} diff --git a/2021 test code/.wpilib/wpilib_preferences.json b/2021 test code/.wpilib/wpilib_preferences.json new file mode 100644 index 00000000..4c5e070f --- /dev/null +++ b/2021 test code/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": true, + "currentLanguage": "cpp", + "projectYear": "2021", + "teamNumber": 114 +} \ No newline at end of file diff --git a/2021 test code/WPILib-License.md b/2021 test code/WPILib-License.md new file mode 100644 index 00000000..3d5a824c --- /dev/null +++ b/2021 test code/WPILib-License.md @@ -0,0 +1,24 @@ +Copyright (c) 2009-2021 FIRST and other WPILib contributors +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of FIRST, WPILib, nor the names of other WPILib + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/2021 test code/build.gradle b/2021 test code/build.gradle new file mode 100644 index 00000000..e9bea982 --- /dev/null +++ b/2021 test code/build.gradle @@ -0,0 +1,105 @@ +plugins { + id "cpp" + id "google-test-test-suite" + id "edu.wpi.first.GradleRIO" version "2021.3.1" +} + +// Define my targets (RoboRIO) and artifacts (deployable files) +// This is added by GradleRIO's backing project EmbeddedTools. +deploy { + targets { + roboRIO("roborio") { + // Team number is loaded either from the .wpilib/wpilib_preferences.json + // or from command line. If not found an exception will be thrown. + // You can use getTeamOrDefault(team) instead of getTeamNumber if you + // want to store a team number in this file. + team = frc.getTeamNumber() + } + } + artifacts { + frcNativeArtifact('frcCpp') { + targets << "roborio" + component = 'frcUserProgram' + // Debug can be overridden by command line, for use with VSCode + debug = frc.getDebugOrDefault(false) + } + // Built in artifact to deploy arbitrary files to the roboRIO. + fileTreeArtifact('frcStaticFileDeploy') { + // The directory below is the local directory to deploy + files = fileTree(dir: 'src/main/deploy') + // Deploy to RoboRIO target, into /home/lvuser/deploy + targets << "roborio" + directory = '/home/lvuser/deploy' + } + } +} + +// Set this to true to include the src folder in the include directories passed +// to the compiler. Some eclipse project imports depend on this behavior. +// We recommend leaving this disabled if possible. Note for eclipse project +// imports this is enabled by default. For new projects, its disabled +def includeSrcInIncludeRoot = false + +// Set this to true to enable desktop support. +def includeDesktopSupport = false + +// Enable simulation gui support. Must check the box in vscode to enable support +// upon debugging +dependencies { + simulation wpi.deps.sim.gui(wpi.platforms.desktop, true) + simulation wpi.deps.sim.driverstation(wpi.platforms.desktop, true) + + // Websocket extensions require additional configuration. + // simulation wpi.deps.sim.ws_server(wpi.platforms.desktop, true) + // simulation wpi.deps.sim.ws_client(wpi.platforms.desktop, true) +} + +// Simulation configuration (e.g. environment variables). +sim { + // Sets the websocket client remote host. + // envVar "HALSIMWS_HOST", "10.0.0.2" +} + +model { + components { + frcUserProgram(NativeExecutableSpec) { + targetPlatform wpi.platforms.roborio + if (includeDesktopSupport) { + targetPlatform wpi.platforms.desktop + } + + sources.cpp { + source { + srcDir 'src/main/cpp' + include '**/*.cpp', '**/*.cc' + } + exportedHeaders { + srcDir 'src/main/include' + if (includeSrcInIncludeRoot) { + srcDir 'src/main/cpp' + } + } + } + + // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. + wpi.deps.vendor.cpp(it) + wpi.deps.wpilib(it) + } + } + testSuites { + frcUserProgramTest(GoogleTestTestSuiteSpec) { + testing $.components.frcUserProgram + + sources.cpp { + source { + srcDir 'src/test/cpp' + include '**/*.cpp' + } + } + + wpi.deps.vendor.cpp(it) + wpi.deps.wpilib(it) + wpi.deps.googleTest(it) + } + } +} diff --git a/2021 test code/gradle/wrapper/gradle-wrapper.jar b/2021 test code/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 0000000000000000000000000000000000000000..cc4fdc293d0e50b0ad9b65c16e7ddd1db2f6025b GIT binary patch literal 58702 zcma&OV~}W3vL#%;<*Hk@ZQHhO+qTVHwr$(CZQFL$+?np4n10i5zVAmKMC6WrGGd+F zD|4@NHj-D$z)bJV;MYNJ&!D%)v-fQ%q0JG$_z5GVUJTPg0MHPf1TvicY#6DXYBBQ4M`$iC~gA;06+%@0HFQPLj-JXogAJ1j+fRqw^4M` zcW^RxAfl%+w9SiS>QwBUTAfuFAjPXc2DHf6*sr+V+jLQj^m@DQgHTPmAb@F z8%GyCfcQkhWWlT31%4$PtV4tV*LI?J#C4orYI~WU(cSR{aEs^ycxY`1>j1po>yDMi zh4W$pMaecV*mCsOsPLxQ#Xc!RXhpXy*p3S2Hl8t}H7x#p5V6G5va4jV;5^S^+>+x&#zzv4!R}wB;)TyU zE_N~}nN>DTG+uZns%_eI=DL1E#<--Sccx30gvMT}^eu`2-u|{qQZ58(rA2aBYE*ZD zm|*12zg*@J$n|tbH%Mp|d|O9W%VT~xG})R=Ld5z<(z%DOO6=MF3Xh-aF%9Hf$?1N9%8Pkev{wun$jZ2 z^i*EhRt8Ve<7`Wyz~iMZDye+XVn}O%qbhV`wHL+%P+n)K&-UMuZw^RRfeQ)%K=k*m zq5l7mf`4K_WkV5B73~MxajljrjGiJqpiV#>0FkyyrB)@HY!;Ln(7JJ*W(>d5#^ubU zVAkTMs*CHzzvUa^nRu0*f-(ek+VZw+@P~}a;;(K=|!9Mhv(~y-mlW);J zb&bB=vySHG`u?j&_6dh^*se*l_B3avjlE|!!Cb0pXyEXRbLy*@WEQ4|)M<`p8Q!rfDJ2RI!u1hPzNjy&)(kcY~GaD6?)7#dCbm`NFh?Y_g$#!+Qrie7%<7P}<-+W@{sxi4JYI{iY zk0(>m$DxOI=~-&eXf2bfh^&(U@o)>(iA1_wJ%B(+nFH+ceib%HEck32QL=J(BNFh`f>St1%llF8chX7#cp*;z}& zcTeXkwsXhf+e;##!FS2yi=2cChcYfzm$wQJ z9%4kAq)wLHf5wfcj!A|xDsAiAOHRzf*)Z-|daN9y5jK-*R{Q0?xaSX-3m|WeuZ`BJ z>eTi@uQ{OGSDIJ#Iu@JPtOy!C?q)g*6SHORg)eAJGh8b-I*X_+xNqZ|OXEsQ-RWte ze`zjjeV9PpE3ac2za+Rs=PA;%QZ>T{x(TRzwWLp_X^2yC-DOEMUy5So!npzL&-@}u z#>uK#&`i&c%J$!bsntEJhY@rF(>6eY;6RoI5Qkn!&<80X5+1(x$T|wR-ad?4N1N^a0)nBj#&EkVvQ?I_+8t*%l#VK&I?uo$ERI1HMu4P2rLMeH%m3 zZ|HA^*O^dA$gb$`Cw;z9?G?m3@nH6TNYJ04Fd-M2wp8@(;vAvJ ztFoni)BLwncQ3@cO*^+6u;(&D<;N;RKb)_NQ_Qu&?@h3MWvo>6FHG%%*smTwj3;dG zQJnT7Wb?4!XmV^>N@ZkA7Jv9kAfD-gCHu2i+!A!}y98SO><8g}t;1JOOxj>#l zM!?y|j5fR3WY2(&_HSGjgMa?Zif<M@d8W z)4>Ptm@zj|xX=bbt$=j}@a_s|xdp6-tRlq6D|xb_;`9oJlkYF1AH%?Pzv$eIAogMi zf(_H*5t({Arfs5XAPj46pjiudQw?dulW-=OUqBVa)OW9E;^R+NDr&LES&m_nmP>Ga zPf)7_&Gn(3v1qu_a^qW9w4#XIEfgiHOQ(LDi=E&(-DcUSfuQE0`ULsRvS}fpS@<)3 z|CbQSi49rU{<4|XU;kiV|C7}Gld$}Yh5YXjg^W$~ovobybuZ^&YwBR^=qP3G=wxhT z?C_5Trbu~95mOoIXUmEOY646_j4ZL)ubCM{qFkl1u*%xs%#18a4!(*b<&edy<8t2w z_zUxWS5fypUp9ue+eswoJSyv*J&=*3;2;q9U?j>n^q?)}c8+}4Ns8oToBJgD;Ug=y zOa0>{VFrLJutjR{PJmm(P9lPzoPi{K!I{l)pGwDy59p-uxHB9I&7zl11lkCu(}*A< zh492AmxsgwEondBpB^{`I*L&Ut40fjM^JS8VdAWQMlwc>_RUM5|Mjes!36DGqW`xs z4tU4`CpOk|vew8!(L}fEvv5&-3#GqZ(#1EZF4ekDQ@y*$tMDEeG?nOUiS-KXG=rAZ zHUDlMo@X&yzo1TdE6b6!s#f{*45V-T3`e2)w5Ra3l>JWf46`v?Y6B&7*1$eS4M(3% z9C~G@N@RXm)8~EXL*9IObA+PwD)`%64fON_8}&pqjrg|2LmP{W^<0@W`9s^*i#F}V;E8~`-}(4@R4kz?t(RjA;y-r%s^=)15%C> zbF;NZET~nybEsmUr8sH^Hgq^xc^n$ZP=GcZ!-X-Go7J4nByj8%?aQ`c{88;p15Kf>|0h+5BLkM&@KI-(flp^npO3MC~W@Uyjv* z6Hu!4#(NtZJ0*;_{8^xcLrC4-zK$BVo7S5V=eg?R8P;BOpK3Xwms+Jt-8R6us zf_rUHFYHn~lu!)U$e$#%UBz7d8YS;mq}xx$T1PIi=4={c-_cY6OVc<=){mOVn>~J$ zW*2PB%*40eE^c+d=PP7J@bqIX_h4u6b6#W|ir<;IlR`#s`Q*_Z8Q?*s_&emuu8D;NSiPX9mK?>$CwcbjhCuv zO&u(0)@}8nZe=Fl*0uMri02oYDjs#g$OHCZ6oTXV2Y0TrZ}+o%{%i)OAJBj2xHC|F5o+`Qmq`$`2EaL=uePwq%k<;6S2n=w%_9vj$8NO|{` zTEg*tK8PU#DnQ#dQ2mMJaaL|HV;BCn?eQ%d0vY@S7Pu@7 zsf5u`T=bL7NfyYO?K^PR_|jap@K|qQ zmO8CK+&O3fzgEnp2|_=^K9ln~QhxjgMM>EQqY@k@@#np@FnZq|C{EyEP7^NurUm0q zW5rKmiy%__KE>YItATyMhE({0%ve10la=mUd<^AcB{T_$Y`2_N-x;F#3xTORXvhPZ7psmqhXy?WxxB5w!m*4&Q;?t$4Kt?m_em-htVDxora24&6~5z$MG(RT{trtp(L( zy&VDT{@p9_DGoq+I|abw$E!TyTO7j6dWQ25dqdKV*z3E?n-p|IG42ZUnNok? zY4K{y{27bUT@#|Zcni!tIgjE`j=-0rl(tVlWEn>5x7BJBkt0iw6j^4n1f2i^6ebo; zt^&Yb##}W0$3xhH&Nz*nANYpO$emARR6-FWX;C?(l7+}<97Ay#!y%BI6^st=LaJ>n zu{ORVJ9%`f*oy85MUf@Fek@T_+ML0-0b$lkEE2y8h%#P^X6+cn)IEXa@T7CQ{fV z-{^wJGN*+T!NsAH@VNM3tWG;%y{pVF2m z2*0+i?o40zSKVq_S18#=0RrJIse+;5cv#a`*`wNs+B%Ln8#e0v^I>7a_33h?lHo14 zg)CbDfGMyH2cj%7C`>|Rrg;U?$&y!z(U10>(dHKQsf9*=z)&@9u@w%y+e@*CnUS|E z*O^cQqM*!sD|e!u(yhXPi$Sl<$daf3sq@Iexafxt3F#2R&=cK z!gT-qto{oVdGUIxC0q`tg)B-Zy(pxGx}&svoA}7p=}jb3jEjQ!v6=afKI!2`&M{#tY$~3LR}#G#U2up2L{} zMGSX>Yjg6-^vWgeX0i;Nb0=gQmYa!|r0rRUshm2+z3AlehjfTqRGnRAmGhHY3`R_@ zPh4GAF@=nkRz;xMO3TPh$)9Iq?Fs5B@~)QIntSyeBy^10!ts?9Z@tK&L6xJd9 zNzaaz6zvrtr&MPQ@UD)njFUtFupwB zv+8%r`c@#asm}cKW^*x0%v_k3faHOnRLt7vzVFlqslue32rt(NNXnkS+fMSM&^u)8 zC`p{on>0pf=1id|vzdTnBLB;v%*ta`o_lzj21u+U-cTRXR%sxE%4k<(bU!orfsJ&v z3FLM2UT_*)BJm1^W;Z{0;z^_e=N&QXSO>rdB`*cp>yGnjHJt$ zcJd~52X&k1b<-`2R{bqLm*E(W{=|-)RTB*i$h4TdV12@beTkR&*iJ==ck*QlFiQ52 zBZ|o_LP06C?Sgs3VJ=oZQU0vK6#}f9gHSs)JB7TU2h~}UVe%unJA!URBgJ# zI~26)lGD4yk~ngKRg;(s4f@PccDZaL{Y=%6UKHl&k|M@Zc4vdx-DX4{belQ);URF? zyxW+|Ziv}%Y!sFdY@YO))Z|f34L(WjN*v#EfZHn6m)X@;TzQ@wIjl4B_TieZY}qY`mG}3VL{w?; z&O>sZ8)YnW+eLuW@rhClOOCZe2YP@4YWKN?P{c~zFUj*U?OayavPUo!r{uqA1<8h! zs0=rKKlwJYk~34F9$q6fQ&jnw_|@cTn{_kA8sUZ#2(Lb@R$NL*u>08yYGx{p6OeX~ zr7!lwGqMSury(v5=1_9%#*MORl2apGf(MQIQTMN35yE3l`^OS7r;SKS6&v-5q}Gw* zNWI*4OKBD&2YbCr8c{ifn~-9w-v+mV49W+k)$jjU@WA+Aok01SA#X$Sspj}*r52!- zNqOS<0%uMUZeSp+*i1TEO$KGKn7EwzW=s?(b5X^@3s5k*80ns2I2|bTHU+bWZ$x;j z`k@>)1G#JgT=F!8awgol?DqK^S4R*g?e}2rOYRVMUKKxSudO(hOLnnL zQqpxPNouLiQFYJs3?7!9f6!-#Pi83{q3-GgOA|{btKup4fYDu-JFOK~Q1c3KD@fdJ z?uABYOkHA^Fc~l0gTAy4geF<-1UqdS=b=UM6Xi30mPhy1-f^aQh9H(jwFl5w*X`Mh z=Ee5C?038GEqSVTd!67bn9*zQg-r8RIH3$$ zf8vWEBbOc`_0U{b)t)Toa~~<7c-K_=G%*iTW^?6mj9{#)@|# zku9R^IDzbzzERz~fpxFrU*it;-Iu&m!CAtM&$)6^2rMyV4 z$+e!$(e)!UY(Sc9n6hkr^n&cvqy8}NfZz+AQc8fU9lNczlP>5D3qzWoR55YvH94^* z-S%SVQ96pK3|Yo`75D&85)xij9Dl8AO8{J*{_yhs-KtsLXUYqwieO(nfrkB@%|OyI>yF+1G?m7>X&djb(HBNNw3KX;Ma*oMV)cV0xzxmIy+5>yz>l_LLH)VyRnYYce zw$?q!hJzX0TlE0+o5QJDM~sPrjVCN7#|32#rUkc>?-eN6Q0RqQTAl~`&isrQg)ass z+x5XapaYh{Dj`+V096?w)w2!Cnmh?x1WmFC$jEFY4;V)XAl3*tBS)V)3TbL)g46_g zCw9pl^!3OCTOcaEP!?==guEAw;VZ}fE6K-;@qD-Rx~td+j(N>)Wv$_mqFTH_wVZNEEuDG!0T`HXLsf+_E=X3lw4`_&d5&YMl%H733ckO){vZm znFLS`;5J#^`5~unet`V#*Y5In3yb|Ax z|A6b^F37!_z$_{6h{7l~<{u7{Fx*A*#zw{GD)6e}n6f<|)&7`S-txiz3Jm4S5hV&8 zm|Ncc{j_~`^pQ*I#w21;(jwi8GnH4efO;R|r4$tH~i;Bcmp^sP9) zjhJne@yzU&XvFNoc~i(wQ?nE`o6Hk~!;x(%xh7?zvigH2g`!v8L-vEN0DvV3?m( zSW(TZ%2AWf`rS}GGMqUj!8yCp#|fR--Vxfj=9}YD97Gocdj=S z0zkF-jsO>EcPTB1zRO$++k^bH%O`=UkHdHT^5?{$)ot<-K2XIE7js*4OjF)BsVjCJ z*KN)!FdM*sh=fB$p8*EzZmGJp?B_=a-90$FI{S$LLjBU$(lxUj;9 zIBszmA*129W+YE;Yy{J~3uyOr<2A(`*cu0IJN#tmUfz2jIWQi_h)_-V6o+5CjbX!1$lz6?QYU za&|O#F%~hmGUhil{M+J|*0<3&{a1%ONp-^!Qx*LOTYY}L!r9BbTxCjHMuUR0E(uH` z!b$*ZMdnB{b2vsb<&P6})+%O=%a8@~$fjbtfF@Z>^Q@enTOJ%VT)Rdc!wX|@iq9i}HaFZAeY6g8xGZY7h-r1sy_<#YU6}I?L zwvf0ePE5PKbK>2RiJOFO5xNhMY+kt`Qi?Oxo&@xH$<^Q;Nb(&rjPBAcv;XtmSY90z z;oIFFl%lDq$o&kYQ;aSHZHD@W({Y1hw<-I>7f_X8wc?%hNDlo~Ig;63RlHNhw~#R3 zA*f5D_Qo`4_ajY4Gr{mLs*(Fxh(U%oua_u3r%`H!TI)@R!!iqV8IOhIOzI@=7QJ=G zV$(9mEVL(7DvPn0j%_cOZN|vvNg8*PHma`6+oS;PDz%iOFyo0n0e%$<#A3r~$=I0T zDL*{AREUGx&C2}?I9cVL`UcPyawTqA4j-4%Mr-4`9#8GX1jiJkKGpHVr1~Rj#zFaZ zqmE!<|1JCi!LDG?1^Ys62xz(p;Uu!QZB7!C0#piy1_9=e?^s@-sd1gs!h$;Q`TNtf z3N4Elsgl#={#U`~&}FNvH78MLjjavl1x*4pNVr338>%sfHu>bxo2#eZN2ee9q#*Jg zDk_=OBR;8t6=pBN0aj)&Nj}pzqqUYW(tfk?bXTdKbNQFSUMCyN-!b0#3?Z;ijzx$M z^Eo6Eq*NO!Y8K;84H4MHj_xwBYc|3>+D(PFj7ejhECG@5@Pk&8dG<)HwwO2~j7KV6 z0$s}=*D;ek#8$a*sxVlC_`qFkM0%BQQ@v2H&Aq@G9XCQt^^x<8w*=MbZV)@aPrrn; z`6r*&f`x&1lp)`5>-|-4%l&W4jy~LydfN;iq?Y8Xx>Sh#2Lx@FXo|5{WKp@y-x;)7 zl;;_Y*-Nu3pcH-)p0(tP~3xO_u~>HpCdEfgyq7V-!ZZ{?`6v_b-vx< zuu|gm5mG6c@D{FYMLuzvG+A2T&6&`n>XM%s`+Qtj)5XdpyFOnz3KLSCOxaCEUl()M z3b~FYqA3FT1#SY{p36h%M^gBQpB2QzEdtM9hMBMRMu{|rf}(;S85&|A!|Aj}?fMKaju!y>_AS}#hRe_!&%8V=6+oPPtE zOOJ-Rcrf>hNq@lG{{@$H?6ikt@!A2OePLe{MBIWSPz7{u(I} z$PXzD;leHG?Xl0FnWt+Wrkrk*|e3P~YVF@N$y&L929cc=#-!*k)HZKDo8!#+t|?9p0z1KSDKclB&M6~hN5<9~^DIltXKR$+iK*h9k$|@Qoy9H}PSI;b(v>w`8(k70@sfa4nRweeiwZ-syP3zPSsyK_8Te9*(FQdm+ z84ZDah4PGehH72w=Q8bx;pK5juT67rJKb|ovD#COI^l6z0eBidn$!Y?T2;5sN+vTV z$`%Edb<%-Oq@NPZy<2Z3m;$}!9JzIuVK6;fJi>>m3q!Lr!2xXRq+l0LvZIR_PNYrP57E#sCvD^4UU2GVr*Rx`QcT}yQanF z3i~!-2Vkk4S%4Hd2baDvrM2g(&1jZaA1!vLi!I#5wX6g^&PE`0-TovM(%wuaPXAno z`a&j{ai=TsgKpc1C3|)tY#!4>SPBbMnchi}glCBwaNE(4`gi}JY0;`|m`s{HtaP@& zHxwCt#2&z9A7O+=v>za}LW~}G>_tWo$dsRX)f1L=+tZF5E&RBA#jUC|N9ZPa_&z5= zekCOsIfOh`p(&S8dnkE~9#(;BAh8qzi5JYT0nP7x&Hga3v`XFdRN|$5Ry#mq*AN$J zV)l~LSq}2d{EJ@%{TLnkRVn*sdM{_b|4!x73|Ux9{%S;FPyhfZ{xg;P2ZmMuA*cMG zipYNeI7{u98`22!_phwRk|lyX#49r%Lq1aZAabxs6MP79J3Kxh0z1E>MzLS6Ee5u+ z@od~O#6yMa;R}eI*a|ZB$ar0BT`%X4+kyxqW4s+D3rV176EAsfS**6-swZ9OIPRZ& zlmIH>ppe;l28`Kd0z(alw^r<%RlDpI6hv)6Gs?GIpffKApgx^)2-6jAzjZE0BtPBC z0z8!#C5AP${zTF$-Z^v%^ie8LI*rvR+*xc=>fa;`SRUSLAio?qL;jVFV1Bw4K>D+i zyEQ}vyG2HTx>W?Ul&MhxUXK7n;yfN)QS`foM!4>4-(PGwxW!^^UyKOz(v+1BejI*& zQSkV|m5=JF4T0k*+|h|3dx`ZKBVX7H4{5iakAxnD#J=9igW@LS;HE_8$lZy1l|$wX zn<8-$u=7&li+^MB(1y~Mz7lj7?oYf%1k{wT#?(Mep094qqnPv7*OYkQ#7$pkU5U24 zzPLEwAb<VIp_uUE~+r5)jt(>>Bg48_{)twH$QJDSBrUS!j{lX z)SK$6dfLWt)c9%Cml+sRp*OHXB?e4hbYZQo!@=6 zBPTpi&6&atD*#Cn6f@5<>79Mq7o0^E!NH)bD26g}?@qg%*AYeE6Tec@F?y9Q8i}^s zz`)l`8>;h75!kL!`&*_hsX1%2)(lWr|7!}@gn%MfwY8vN0=pMm3WesCRv5e*5m4z|u(zbYCpuxO9$bY)hkL|}mRj{3dlRgNK)#PJp#vR=ka^TZ(tKVI<>M~ekIfd2 zm3UDUNW*ZvS5L|SF334|YD>LJk(EqgPpVxtzwclUNaH70zWDVt^1+cz|F?RdF4HHn z@4~Gs`lj!0dWi2n#>7C@B$Qf7|t{1!3mtrO1H7 zi{=I#^Oa1jJiFI!j>PualW+ncHJ)TelW$bv2MqUG1xK7R z%TsQfTn)7D3}XYU+{?Hq!I&fqi4>DmryMiO?!aN!T4fnwq2vsuB^s6fPW@u*h-JwG zNniJFR(RI*?5HV=tqO)lv}CRv_eNEBR%z}Vnftv0+DUH^OCODH#&;{+aw^1vR z-c~|Mk+o?j-^Z+rR4s z-gNA5guTuab7N`{Y@eT&)!xF8#AeetvQ6d!W4BlO;0#0TxS_( zMm-A-u+h7-PjmOQHlh{Hxn+J$jh?uEtc8RG8tu->og@ z86A%eUt+P8E3oLXIrq#K(nCF@L12>=DVT3ec6Vn=B^B;>D=O%op+0BT;T)FHZ`I93 z^5|bpJC_kB92`alM40Am>Yz5o1gxkIGRYQ)x^+R|TCK)r;Qyq6+~S9Uy9nr^nkvc- zxw~#_9eBBJcZNK0yFZxUK4h>u$8;4k-KpNTblRgS(y&u~u&J;O!aqAMYJp+(BED*d z^I#F7vPOEADj}Pziprs=a{%qgz#eso$j`At7pN~bDw%&ba-+4pI}T*?w-z^_~DfD~Z3Tg+#M#u{s&uRF^dr5RFZh7<|WNEG;P z-_SzXTbHc^yD$r;WJqqJkA7^(zN`nzQ5V16nG~Zobuy)a)(T@Ik>V!qOfw;e z)?AZXjzDJg%BkIEY&bm&BczLuWY~k}3Zyx#)jxg1A9R`sz!_dCb!|13b*3PiA@(E6 z9HmG2R>-YrW93UMQO}XE4loI(*er9J*wDUd1se!pzdpoB_v6^lQl}+!6e5MS`+bU#_b*a5Pkt;o+lOV4loyn2P z$3;z-cX>$R{6M4q%b}aMBF}6N+0RCE70bB;XwHV~JLO&!EB)Cgo9ta_>>Os1HNfaY z4PNu7BGhw`6}cm>glh6i^)Ja{rpLHix?C?u;(e&GI{?!E7$9hd*5c^iL?;6Kwn z@qbBE|3UMF|F$Ok>7YY?CeMzMes@CZJQ?&|R8v5M@XvW}jjxhjl`gzl;rvy6Nn9$K z;1TKGpUgZs`vR!t-sD~2ar{58-;2k`H(MIWr_cujtSCpjue(R z(a7R{q`G+;8qD8D1e?1zWv+pPFtk=k#>f`yqZo)3KwCBgABgQbq%hu4q}h+Bdyh?* z#Rlr*$38^Ru%m9FUTQL2Xy^j|f%*4H*{zWFRsMbs6@u{JM{48fq;F;QFV%6Dn!6X0 zEAr2G{RmY8;Jlmws#%7Hl_TvQMbLnN0KGK=9)1u=Vb&#V27UwM#U+)$hn#hlXxBxO zM~<3s(W;fe-0%mVWtZ)oN|h-01@5z=u(z!V>)I9-IepH|_q6NR_DA>2hxGKt-QX;H6(^FXwcBndi1s%qn2sH-rsuON7*ARP6Qt$2XIy3d#cn8sLh&7#USTFn3 zQm-o6-Bnofon2V;oq-v1@Ye@NuH$Z~+th}Cs>F7=H#=4PKLp%-!EwR&0`a}XL=br< zF>&?HNr}9ahB-EA7a({^_6`taBwmB~hJG)p>8r^vq0J_+o`sOq<{s2~2t}W&1f5`l zj;E0nmt?YRp{ONhti9{4&rvt5uoS0CO@%+Yv>+}ROQAGP3VLu^S4fe{ZRoGviEXMF zhM=I=Eg2~^5PIwEq{~Wt?inz13!axZU3knx_)Ey9<)z<=!TnCPHvs1l^spF`@INYQ zY|J1RWri-^D9mVY5Z{u+bXg#}3rUwSXX>&@PN+017W@!L5H8CvZf0wZxQ=UrHJ{Um z$Z;~3t6ARGql*O1^YY(h4awy!h_brE6&k9B&5l;ya>jDyW5?o$q~=1iV!t7#8&QOx6P zhQIm55sij*Ef-G_?k^$AjK2j?=QQ?^=r{MDaGZ7`Yo*Kp1uoZ=&5|O)D#xAHL)n9_l6-E!b zVV@8ny;`XU#X2((4cTmv5unmYzUmJ>Hm+Kvht&a+j3nr!sljTHUZn^0w@L|WKw2TO zRO>T!>jutIzNI5U_KL}vd00oi6$aJqPeJwq)lIr(2Gt#52i@sqCFaWC)pS$pYoRCK zd*$)r6FCClYp+n>gCqVF>x)ghAbl+h${~Mc_sQGk@+sR@b(88l zcx?*Usr}v|kV!RPfS%HK>Bn{7tdEV$CB5Z@=uy4>^(o(%@R|_7dq69s1(X_8szPZ! zSS~$LCX>-}F=io=YcY~9!vqo3&dh9_Mosio`zO6i|$&p;-9%+~sdYNrVE?Q8rS+eHx z4O$l|b3FUT#2jb(WU<`oKAjGQUsoCgE1(c>3byBNPhKeJ7f4S-hBRqRyePY)im;>H z)hyFuFTDqx*ZgXo$hn+u>TGs~=Bjqr3bhPmXG)v8){EU;N*58NKU5;EIZl z9%|JomX+b6M#jS2`B%~!+`EStMD{|y^P=`xPbD$o6;|!((h!+y%7Y{DuC!NCKDIN1 zER-J?vZ$2el4y~!-0vWjNRoC|ARB`IX@M&;?ZpULcAIu`zlH9 z&JK#H);Ij~fqoT{59}OI#ViA%!lPYyd@kHg*hyI;iMdCtw2&eLHOd1*N%2Y!BG*H_ zu@E?VbtZlI{7B{C>A^b3njh=KdF!=rQ!)oIjwkP{t^I{2q&emQ-C1&U&fPC_viACTbT;(A3qRJeGINz^!0N26vQ~o|#pmjp-Zq46%+{X9n zLGKqhLh4`-(*oDHqHU~-45_+pe(BICF$*0jD&FW?ED=vn=t?p9X(%AH9+;6NcJ8JF zASkf}LfT7Z3u*#i$ml`gKIS>3jrTla--x##EDM{w{>Iu9qV!x95ECU*W_O`q>hcCa zswU!;H3R{}(A6aQ(B)lImTF$BzF;$V_?It*+8ZeiZa|b8n_DN4jUfI0jIA6Q6*c0f(uq~DxrNm!$~G=Uz=qP*)?qc(}|7MQZT&B=Um zr{Lj_R7QJAlwD=CoYpjQsUyu1)C9p5CE)%3nb)~WtP;@6(qGG`*qDT zS(zM>&R<;Z23V|80%3s!`0QpTt0Ay;*xLJeE|DP5@x?a!1)`g= z-1}G_LxiiO(*?R*{(yH#&yl|Seyx6*+ETayQtv7Htk3WPvI;U!@h-e$)gw9>pyKmB zk8#$3BF-ou%=`9_3)Q`0ttk$cymvULFS`Khmjes=2(-QY@eVjJ)rSD)z)1No&o+dz zrGItPZ$QuD;Nqt~U{J?9VlM0g{kx!4$?!?=o?um>#7tjMzrLfv<@pI&cp*5H>XPPZ zu8Xh&6y7v0pGDiQqd-~tBjK%-SO8$8kG&44|{09|FO5BoNkV6~JX>g{b#NHJW?gmM# zhbcS|M9fDc44(seG%$hK#va#4YL98mddGDi2qr;@CeiWO!!`DrF<%=_^*3JgoZiSj zdEv30G5`7ex`XP4#6cG;AQ}(|>CcCTGiom^pc*j-Mz1_oGp4iP*>N125YeWCw#L4H z*>u2Ih8jVRJ?rOj-7KbU7KXpYs2UZf)Vf}(lsM(oiB>tgqX2tILJitw_x z&7gq;`b}qrL{lEA3DaXDOi~HQ!^?xxjjVW|#Z+Ek&GKA2dYgO@zB2V*eY zx>@D06X)(FUz3xz99V3v*k7x|wxiFxv>=N$1Chfp>CErJq)gnf=P!u-QKrYnulzdQ zP56u!AH2^QVnuxTJjcQtlflq>PSm4C!$^fv4V_XsIO2d=O8|J`4bUDtjBchJ!14~3 z#mgUPYF*Z?k;Y)Igdx3yQg8L)M=c%}p3!P-0KOuXI+{*LXJ&w)$gzxeTyr`)h-Nc! z`$xa<>T2pbuU0VR?#FPEM44XDRw+cM6U1R2aLQpGHX40=4Er=lp&2aN#P1IA3|r+L z?5jaRyCgN)b(KuS+(x9rPLLjY&4^YY{0T2Ai%`f0p}sG*R!}{DSf7GdPJ=C2MT1ND zUJ@#y06`CNc9n?13R2KY1K*SYeV87wG%bjcIbn+AR8*FS<{?wWomTT5@`}~z3bFAJ zLR-wmE$iwwJ-TnVEhl{{?+??DJ?DWk~VaX-L3-RLtprT2%z-GfD{UVBR~T}zymA0 z6VZ;1Qr%5q#+Oz#3)`D(%WVWWS4BW6%ZvAtt!u25FO@e{X`)_LH>p&pFzx(wvNEO- z!2$Z}`iynmY2j&UCmRNB)9Cn3MXRls&PFVHzkzr;)B^BCMY~6lYY>0rsKT zm4}RV`Q7tbn)Aseay%@-I6ZT~PBsO?D|>kG*%(PGo=|gZ#0zsmE})xxtAvaCe&$1? z(7GyH&^jm!cguuMo@CPA&-lrdE&Aq8GIOuUK9jt{K0ldcvJJp7I`ZMx-EYj$)hl~) zFM!U~HxgO+lb$1cIK-nvz<5OPs(@d4tB6DUa3?-bJ98|dv-kIdtMS;9BuLc{a~_wW zO$u`rNymsAeMH9zh(|w=<*V z&&B{&O0Am`<$iBa)>pNZ6cO`d^3B5%=gmsH(HYZw6!U(c@}#)19F}`BT+yOfamJY$ zYOmy2m^k+ADH2klhAJMLq;6>t3)NREUgk*cjJHg{NBkVhDORNK;v5362&NN=y*Ef- z$vxYTG5Ga{SI&C93^Gsu9G-osqbC9PbsC&@xxGlF?o{!rs9|YpEE?P8ix#yS`7JUy z%ez(_Q%I^RwPrW%rFF(+mE}rp#Wtg@^>O7T(@LFA7j{LNrL=XGDyB-|3<*mqLL_UA zUZz?ulF$5O59-WWZ!d@hRxC@4d6?okW%`1$#<5w9eh>4Cyr#xe5%VPG@TBe#HA^O} z1&q{T_TMTr($f<()ah%TXapiGp}`MAC7>0I=Cx*t+bXy+gMyk*#(A~ft=&4YBdQki zQ}I=c;etc@sD4?l`eYaksPtJnx5OUaZ6u;7p64DUuI`omrWjht5$8+cqb6Hw75WNX z@D(fl7tDl2H)H%QYyX3>cL0*DZPv8+ZgaP7+t_W}wr$(CZQHhO+qUig`^@>y%s1~j z6Y)pXii(P=SQS<4iS=aOnR(rqe#b*BR~GN+bMNQSnhcMHxhVf6D7_zYs}@oo$eK9sZig1_lH0|C z&<1W;8dh6lutS+|02t0VqRfh9R+%!~9YsQ>cw-uGi!YMSo?19?Sty(u{GRqmTx8Zv zLz|nph}CNn+4a~dDzMog(j+NForDvDjLwub!b;p@dLHSBO0kjaI0CPZ)8B2(HNL&A zdr8Pw@u(POF1J*groJ~!1|E(GmnR3L6`P*3C;v?R zDw-pBC=u%}<}P_);mn-_cE}am&b1_WlqnWVzFS;*NhwoOb%+#0nI|H*Bw6_0R(=Kj z;7@eEqYkW2OvWkoz|yY1gZAJw8=>KShthS*ANzYdDT61^AK)>0H%LV4q3}hw?bkA$ zF$tz;<5T59v0Zd$)unmJ{vu_7eGDP6+pe(H&n^3E)g^rB?pn?GT9l1gztAUpR*+Kvt=FE~M zq5rZM&9v>ww1mzrK)vx*0;;?tnqA@Q;FBC@$2~=gy#jW$bAJUNIl_YpT)``*9nnkV zF!&XBK8(PeQfnScH*JaYqy{1bN4MwF=&g2)`!Kuo165*d^1Sc_d{I4>6V=>74c%g4 zXE_M`b@syq%jQx9VRp@ba!rY|MRhr!S3bN!1RT}^I(2gXE`KT57Y;maGA&dHM#`4* zy%@6YB0A6Z^?fg!$4Gq0auM47(jE$Y4osH zhydBwQ-S~vMS7)hg;AC=MRf~AHZu|Ue*bk=ff`!Ol1%=|W-a+~l)QH04q^oeMZHj~ z8$8jQn(n1#O!_7sg1hi;{v%?nd&gK7tfN3I{A0j zcg`ISk^Ir4G=(SvV$v}DE(nE+%rgFkT%cu5VR0Qa^H4-xPC*7Y*+E8#xvyepS#xYE+FyIIi0|5$J%mKAB58%MgleT%Zx42e^L`TdA~Ips z=NvgHNpYZju?*J>oNcmd^(nFUc+-bu4*+9)qIwU^g?1_4-&-`uZm&f7F^1?@3IvJc{gnlh?no$E9jFIfJ8i+33;o-!b2hD@}}{o}J4{l{44v z3Cd{3Lj%9^E43SBXmIvwsA2_8sXgRu=4=H{j9R(fYcCzOXriTZ51l+HcXr@)^?rK* zmc89=w8MW+txdobBh`X4rMvY#vuv0GIEO67sgL}mIw$pNW6s8Fd=t z@58{pFs^Oz&g}CPr8EL~QyUjk&}1qyO4;-6m0MRd4J9T2r5_j+YdeKP%Q+jnWNdV| zUJLU&d%m|g&3B83R^8K^WM{0at+=9UdVAzTnL+CqdcT#($38|-fQ|BJbHY4vk=ANj zvX?ek_oYp6t8bQz-T){|-5OGrv`IGd?>X*h(s{MvQ{j>fZbx<^-)&(j8(N+z^sftB z;V$0+Wd0oUR^&)Q+2bHfLt#V~jZT$UPUbkd#vD#zZJ&huG+-;T%sU~ONA?a`Va|T%I0yd%0*Xr3>p#slVg7Y<6o&Bx856S zg;7Q>mCFF?xq_m}VG5`(0fIX(V=yvQ;xjpwNhrLFMui8xdBw2aFOvI3t6-NG3%+d= z>1un%A{1+tFrn2nu2%`-hiqYhXDga3%{ZVkC@ROtTcA;g*E@K4i_G1&^P#Pl_9*m& zwBVKqZhrf4bhw@M)78cm zBMB!;A)H{6h6AjEv&|DGxYRmY|e_ARf_dMIvm*-i4hR#IU_#A_QYP@L|sHs zo@Ky_Bx6e2??_k;7vjibD#pM*T7`h9V&s(moOn_x^N|9{gkOtFY~gDqSo+7meUjBR zK2jiOsA%PwD|1*KC^m(-WZ5j2AWi;81kCi5t)KouHKt|R6m{m!!n|4YN3yyBo0mSZ zN^yj9>I9Y6dI&$!T7&$%3Ccxua0-&DoNJFbCV%1;h^-U&1Q+@47qrKld+QNGOrh{a z27PfD|L06XuL1+ZMc{_7rB7bd&WD%*lbypj>|K|<#2#t+qPXH zTm`5QC)ktLW5+G&4lhvX8DgOK)|mvQ_b^HuJ&=wP%Z6%;E+Bx|#|Q}vOoGR(jK}sD zk9x4A-V%Hs#G>J5XldT-W&|Kv(!mEi;J38jdK>L|Q7~<_no&|~Fdc~yhC~%VqQc2e z2|pva(YaxgaE`xa5=u=WkhtI|f`XRHhA6|>1`)hDgYzt9kByS$l*OQ2O-a#Iq%SLz zV^&-mn{^KrM6&BueyiV}>&)9rr)de2+DkV8##PSmko(<`nqPVr^n_V~UoIi`_yVdB zzcj4`b5QijKNrR%0AYi<`{NDb!y1^#Pv|K2N8<&wlO7-JDa5Yp?eM)pf>PbMq@)Wr zvki0Y1yLr2WfDb`RBPgq^VC(KH;ofR#9^i$TaMi9J6p5TP5F8<&ofnvL|`*(;urRO z?0k?7WiOd&^v);ux~R9Hznc3moOxE+O$lYV0Ku|hENFV~?Lt!QZlMNp1%d#^Rv!pC zfq`*V)n<`Io8N2XGBOjLYB}#{g#>o-?Hmb6$VyvSN@nI?3{y-pdNvcYe%&%CIeh?s zWfdM@$o~R)P|M>ElHW0BAMI=ozdH-Fle#Dvq-bpmPg-!rDY|1*o|1dvDh9{`{gt%n zFemDyrWMrywXJ+rV5r%UR~0T*75`i&rM4=%7}ulJyHu{rZw;C$r+nn@cLyLgh0d-A z(3SS5tW>ZK0in8bOH$vW>HIcipgUXYGUq49#>Ixff27cCfWz$0vR4Dmq}CBw<~4Sh zDe9adM$vVItE_)3FJT5Bgk}V=1g+Qvf5+hpxwh78gHe$<|r1^Nh?B&_~xSq+nVdY+~dc4GJ?e5EpV zXs-H~6poV`Kh5kok2qSUMD?0&WXKs7T0?Z-J8zti^WD-*_fo zhAqM(p+l2*(|b>aZC+?aK~^_VCZkP0>}TxdEC-KcmAx*YS?wTK?cW>PjS+NxM==Wg zg}e_*NcH%2(J=+WVL+;P)kz0c@48^4ZuemowCO=rriJFSD|#7D2oO{}$kCbL0#0%2 zQe&D2wwJ3%d|+L`bE=&9k_~(BOe$ZFap$YMGL$&$D0=mJ9n%He#RRlC3f=|WyrI0L zA_qS=kzzw8f_QiJYg_b?xA6UgBS0tT_Y$!9>(J-Q|m=O+8+wIPlb5i=-aU~kBf=4dD zd6Q8*EoKqRCcMNO5q%nez-osz1XT6PZ+r7r7A_{!vpDIfE$$yCUU66H>HOUO>u7aE zs*>|KS24COy<^3O^xXssCI`2iF%;A&7{j1UDk9dvv< zsUbj2HMoFr%{j!bRrmyt%jM|4UKza#}%Vf*_fEvi$*6J-h}oRdsdinr_W1-)p24zB*p9tfDdUa27+yi5W`#8+~eE_NyvNZgCP48jF8P; zgYS#IP!@sLe^SeCy4jwre}sC*A4Vk3|EzFISR4QEai+j{bL%-B#Nlt4WJN3eh+Uo) zVtaBF&A%PtbaaH`A~$h0I(5#|WARn>4Hbxy+Jn-$LdJWL+&({?oGdxCC?@gw`D44O zZ)fV$Yi@4u-zGU|!cfh6Eq?2C3Nn%TL2ZoA1+5g5O#q6$QGS|1C!;H{)PU?dDlSGU zLGKxOa;zm!C-Zghet4U7l(%LaEQnKF+>ECNt@`F07q-JO?%%X~*k}Yndc#f*iq0`hgW#iOvymYI0Ur}T;8qZ+%f1paM#v7e! zUS~+CMQqEbYZ%Ix+4iKAGa>>DLya7d_5zQo_zm&bP6F_75Qk^L7A%?p74r#_+3V6R z@m)%h$SZlQi)PpLLYyya^FulLkrPuM%+!YnWBCX|f#M*ph-`6S5IH3F;Os;ZZ&cDq z<~WF?be7SQre3OHq63A%t27ee4>e--Q*N)lFkAI_P@Yoq?Bd0s)IIqLY)xtXU`k>x zfQK0;b2n0v{oPhQju4$`uD>)Syw=X_l}YEfVF8)awhULL-sJNdq;z8~(wyAEW&sDx zxqHk8ufaTXHNnIUP~eE&k>D!g#IVt73wHY+ugJwtuy74u* z1qC32jRV4EWbz*0B5d5qGm7FB;V0Z>C63g4n6hW?!BfHU=hqZbuGx&ccdij#|lWok>4#{m^Fy>{`JdOS zjIM(Tuf4sYrJltP%2vW!U)Mt5hd5_vs^{onYW=T{?nF6taSUF>uPLMY@>8Y#vd&fU zJg$MqI>EOkIj}Gpu%?+k{%zvX7zqvMeuMm%YD6eLoHxL?e6eW>J~|~Z&lHB^r_Ag0 z{*SlMeG(r}i;4UY6e1TDhAnY@tyh=*e7>7?vlwq>&py69o*=hIE389P!iE)Fe1v;HN5fVGS&&jBzQk*Q}Rb%{FF5H zt;vL@*J)TU^_AGy%>+&9)+R@9XQHe9%Cr#w>Q$NM0~WAiktZl>9`I-Ypc0UjVU1rn z_FPNg@88w2iz;NHBJ8)vM$%1oe7QzSs;NxSieG5h->Cq6`M#YqU;tx=1hYym@h%fi zzWLOcEgsbZ>jW|mkR)qpxv-Z}J6iTzy?L3sZiv!nbZ3a;A~Hu3j6-^%FcrouBW^*9 zwOO;eD$2J8edza=ZDF&}5X#=B9O(;A4zyM&5yTvxuoqjP+FZY!ZYI`_D=;czTJF-e z1-$=(BE%9~*+c%p5UT&+n27&>tc8D77L`o(F_e)w^~KRuv4^AdNE-D~2I(p(SCPRP zc{V^gm}JdYd(~~{max0nhdPp5j3){eJ z$LuzR9V>9)451K&?27Aps3vsd_bU(1EDOA~g;@vOO2Ty`4MFO9u=`!_wEKPQp>9L& zzuUbCBGHhsuxYBy-^Uw`)=n5pSF5)!a6qfH$^u&=0GA(}B-Ixjj|ce?Bp(~$q^7BqWU|H8 zKU!?5P@+8*_63=^7)|h<=`vW)2%PZF(`Q0Lr0x5QLjWKIQZB9)OOB_ISy!Mx`E{lJ z1=1d&Ic*{{_h#6sNH^Hz)~vB7gCTbuUkVrOm(pCye57-0NUsKiFMeA#@NBB+F5<+s{(H7mQAPQx`OR z8xRz&uf&f&-?8paW&Q%EHCq$Lv~}lCIW%s>Wxj&$Majn9D~*{Yn8jBZ3b9-fuz!82Hn?&ZI2_JZYAy$kb_?7m*?J z7EcrbL2*)gJ(Wl`yg~c)vC1w>dR$LezB90-T0%EZo|KuQOirNpKJAd) zr+w2F#9m@j64vevMEx_$M}ESx!oajKsI7|Q#c-fWRsS7nAgMlxf$l`eoBx6_u1LP` z5wVEEAYNPN*iXKJza7=aP+z_r$z;5})SQGWl0SrU7qL5T>MpzjZPVq~an6pv29s{gIn1Rh z$*Vp>0p=05JN|HRiyOCbpgpZ@;9Xj|o3DNV!%Xn6t3hE>(=2$dFuEx{osGXYv`m73 z@j>86*-gsSS^3mR)HB6Bj1fy+E{@9e{bcRLU_iAqDzdQUqG)+sqNE`h1 z$3w4loJ+!{F4NdK!E7Vu6L}j5d=VnffP!j5b(b5(u}{;?o9PB`YLsrEsOeE8IUM8F zj!}~kYF^$l^i7CS$AnS+a4#EnWySE!?hNnzWe>=ETyc4WCXpNzZ9R&vLWR9n2)aFS zeT`FE>ZzLpjPr*qdk%A3<`U8cpr3K~?abpqM})l-j}Hz+9tJcw;_-BzCtzpYoNVk^ zd4xI@9~_|+Y_6S*Kx+?A$c)OqC718Wiat0Sl%qFMhix0?j{gw1XO9$zQhjjoeDj|S z8hS*$R7Ol=9=Sd-9s*OgZAC1sMC*(iexn}3CMYJdNZu8^S5)5@Bxo7ayS4fG2D@ns z(Y9t_4DB(20CAx~=eL=RM?RRc4|4V{?Qe z=>g3K7H^2nxwHm|*N+zhk9ET-=0ak5wZAxM<)DFY7|^q+@a_=>AXMj@vZG11mH%nQ zn9XfRt7)!V&u0~v+`DaED;5~WX_cQ6~@iQ$)`#bKdk&+uvYtZMGQ??&zRmpw zbc5donS&q;jPQE_7rh5{ONJKBM;cxKH>r!f)K=VDf}bfc1B4Nv3C}__D{B|kU4Q04E((6!W^q+&Xb=m`c#S!$wEEp4py_0 zDJO?v%A16hzF;#-Lt+DUyec?VXUS?%21=wBiJ<}TTQMa&n$+5wnHr4sni_Hb`tFO; z((Kg?Xh0p)JZnUc=-mE(Ls`z5)+Qr8;F0R92sj9yEJx1kK&wQ8S2S`)h+Qk?^jShBw0n z^g^Pht7xCZvs&|5W95{bypf4acXhX`O_>*QyEk183j48^Ws>JcasVrhs5G9;&2dyi z%>jCf;J1W^x5i(=Cvt|^PAWSdNG}XTJ@;UD+R!_#xn5!VD8@`C$I>Ipes@q*x>0`l z)z8=i*VF~+bxTYjaCr)lzaDau^|9V&q!IlGwQu0TKbn4oBljDL$D`d(xUR1D_M2H5 z_D)E{)YMOgPe9j&Ta=X`w!K8L8Fz1tOon!uWan9)huounS4Mh4dF)BRXPW~rZ){=b z8GKrX8h<5U_7;gkNu2?Vha=mHR?g_-tDJ7e(~;kBqw^DncZb0-heR1$Eu84i7(X`&aR*AQIwovW z>fz)N@L0uBeI%!;>fF*(y?aB?LspSl*h;#V3|hH@lSBCC>z%=##r4vBD?~% zIcaMD#Ep&MMR|QloYSVm4m`6&D~o=K)KUR!2dn`e7}AFYi4ni=M| zwlXp`cKoTc{O?pVGTu@effshzIQL;~Uran3$O8b$6lS*o0sT!BoyZd(zz&P7axA%@Nz)_qI zkD$LWxQoOtM=CJA^aux0eMxT|$TTV{XcUf%R6YWWWpb~~Wr+7tk~!$o(-O!M!{#H? z)jCw2taNz0WO)=*Gud3!7Hi9?DqB;9JQ_pLDASj_PC!c^M|om%q>Zz+S3oK5Y^V&l+!?6vHO@6@c? z%)vqVE`pRD|ItbFC1kt4ApdNC)&9im8NW=RUr>

@up^y4&I8N>~wvL%f(S2W%NN zf&x46sN${5Gh+I9cd>g-O|x3@x#@hdvU54zx*WtnC#5%quWk43w{;_G!4&;N;wy-O z?urjbDnKfp2u4gknf&*wBJS`YfdzBa#pf^Lo9ei}Z)MCk6MP}h0OYrd8`jVipqsRTq}lh>h#|o4yiA zbPQLKXatZ+L=I$?XEGfd7x*_lf|=3xKLi)yj}jQ9pD+OPrv;Mqe+~uywe$sD4D}uV z4@_J6*&E>)?K_L=^f9)ZpbIb0tyI>qF^OuZ;8LrA_T9JRowWUXNjyBVFxj7 zcFv)I!ZI!9%3&ro1=#}qZ!W@`!*%Do@xlC)>lS-KJPYY3@3mXj^ZUgyXXo8DiZ)0M z@ORv8NQ5xIiv%yy7WuvM3l7ZnaX8M-u4s`LZ2-*e2V%BIin4U@4b=3ps|#~L^v#DXv3GDk8H#;lK%qAV<%I5Z8dd3-sIMfqq2WY52;$Y7| zC@8Z_G%EJ3tOhCq_Ad3l4=IN9=Ee$7k#R%^@JPd7SnqL~*a3EWdfPj^Ft)B}bgnkr zBT1I)!g2ha@JU#wQW1op@1SkuaGVJcEJVhstebVvoHV+n`EI?;^p~M~tfk#K1CBi- zF<+3FQvDXkoVE)E6Bj9T)Vlo9rjgCj>S}EH&DnJgn49L@7ZaI=v&F?OY*>NLOQ-u43cR-0P{LGZCyKsW{^hNC8iDiqJ{~) zNqU!S?7Gb=jXSc_T>xTosLbq!#)VKVs^hKlReb|!_v(O0B(=A8tA0Fic+K)>Lc!(J zge-eb*cuWjJCE_q)D}kLQ`X73XAD=didg`EDAk|uw*rjJ1Yj*bj<;`v&pOnps=(g<^CaeJRd*q!NQ`O zTAcA*KCphxtD>M<0l)OpWo@|W=Vs)XFpM7C;96VQR+W3~AXoqC9@yN@7J9kuboR-H zHL8|U?V*D#Jg&`hR95a1#ByH}mfw|kcIP#b2%C}r_nxhIoWdo%k*DB;N)%#~P458H zR&1-?mh?}HxGi(-dh@nkK_H45IB{y)%qwup^p85vZeUpqh|G;9wr%q$_*4*|PS(bw z3$<2M;y;*(WAtHSM--PRyA1<)1Xe^(yuRRaZX9nR0oP5%Wg)P(ak|_q$^7Cd)NP#f zFt*;;hP)je2EkvO_Juc*@6Fd}(xbH@+`c?h1(9yjJzcLY^!{hs3;2?q^IfrF`+D{7 zeAjrrb~tUbxms|met4=I%jCVN6O3DEeY8_%NiNb1EvTu>AI1J!n@36jd$2##c}B>0 z4L;|^v$`6=K#^tk;MTA+ji{smQT)gaODj-((|WI%X2JbpJ46#0RZ&FMJeh+Z<&>04 z)cI;7Dm)CZ1Q9H0Ge@zDXKAsB9dZbg4?1joh3}_)K2k;c^(s6)kl-$}hLll_T0$(y z-4SgpruNv#}%R(l@3!%tj5l!d~Np>{BXo}gF5QWAP7*n?JW-N~>|I~-Sokci&_Ho87f;meu+(2@Yz45X{^W92m`3_^%9FadE5^cGO72ffn`$&G} zGOIPIF?FsLh^0eater8)<@~LjNIyP(W7F~ackhd7ase+Gfo@-RBG6$Q+CeDbE-eiO! z66k;0^Ze3P9kEj(yiZ!_vx)K5>+Jrl2af_iKMbiG*Z6y})9{?`w@LyvBpEEC99HEm z94J&4%248p>c%Nb+Y?Mm9%w8P;5(?F8nINf&_*-><^LeQ6{hj_UPeUhLmtxd+Vmgt zX+WF*G|x;d1!gF0D5?$*b6|tDV#m<_?(f{b+Jd?J92?)y8t>gZ+-KQ+Bj*PJW__xR zdf03Su)GBsi{L~F7m?zTiiu`Wk!YO=QO{H#)PP2?loJ6bfRs0oKxO3+aYm9`#}5V$ z`x646$5C08JvW-c>mV&jy+a+V^zH9IQ#Inj?BmB?I0~jhx7qLD!cSQ9{<) zCB(xvh>|7z&?P1A6fTeZ=vH4`HaRJenyQMrBMl$uNuOX#!uWTr0YsU$pvq9H4wY>t zl^X-E=|ppy073iT6Xv?zU&~*SOz)S{s$uTKR(W@_aAsUm!9UD9D`~`uK!3`Buc{%2B4{J%ioRlMx&#kB{e!Avb zJrlj#<)~p=4r6CfO9_3Cn1xhg=x7nk+LY}yn%fvBEBY;q4p`CSxj7WfX^CU5+@tJWJi(W&KcO*jj5x;xDLZ*AxFvIAYA@P8yW`o)9#pos(U zSgS*I-N9vd=^11lccI*yNQxzMgJ!_I?64MNHZL9-U_DIfm>8g{k^fj)WeFHM8I_z& zZ3l@3<|n0jQSo~R0*Qcqvf~?+vNohOl*bzy=)XeN;2a3p1~0V$$gAWoVuI=*iPkyO z;E~luur&+0{@(mshrT+g9pcf!^T48w$vch$Nigsv6ylw&q=E-ICa#nDgi$8vmBC($ z=yLuLM0U-^2^S`{_ZwTz$|kB|ZzUr`AM@J;{X1nZJEj`$4skl+fss?6#-GZt`JdU# zvVUW}%8!tF0rBe>`+r}#|FsnVkBs^MUX+ze>dHSpWnWVCqdl~T@Zci3NHq%q1q0&Z zjiRz*rIA75MSd&j>=Hq=uts|mK)cc}S884FYT9`Ym2Gbq-?zNU&7M-!u<)j1^s21K z7oJaB$L#M;cjw#E-oI~{yJTr2o((;6binRCTJm*%J0nrPf%?1jgigQI5bI~2dsFN451~NyCYYvfVfu5!YwE`!Uv%`& zB-2spw{|p}vcNP<;@k3}sV|3_r|H|Z4JC9~&KtI*)@JhM?U=mg#m3PjRVoE+M zVYM5uWSO==K5bE81EEz2?F$jdRB^ec45FWK&Dz+e}E=Op=h#{z^;qey2Dx+2Q2qzwA-MpAB% z6U&685w0+}tjouEmcVXOF$U)7w=8u*B7piVzASTr-X|xfrQR1uvc@IZr$CD4MUVF| zMre!R*v|cBT}rB>9#r~c4@(}lBCp$9)X`O$7f_9s)8|{>$Da!Go_qr=;4rtnr7TgXUpffMV9akHEvEw*Z&g!2Env6(!b;)$Zkq!j9UGy>Zopi zUQ<$5Ex<;BxM?&1+E#8>B$er2c?TqH!q^=LX)1lV=@=!xtMbm`$gt70@|} z8AM$V_n1o@=*E15EncO@{DFc)hEBSA@Nbk=GkNsF#}_mBtmF20k$-)eOP+G`q*EAP^>>5d@ea zg6^gb37{ol+=uYC3->5=jbqd}&J|19Oh}yYviQ}E@&>94`r85c>mo=XKA{q~2C*8q z1(8IqD#!fuWdW8DT^RfX)ssdyOzHq^sC=mmY``qcE8^g-o852h1`FBL)_0fHqqzW%Y(brO+X5H!1sl*7|2>*^XZQ^Um1qp- zj{+=uY~SxwTj1)2rmt7luK=kSptJDqqF#W3sech+R{=RBs5U1mcd@_EU~~8?dsmUjsf7tKBg%yZYVwFEDFu zWWQwnb~$%v)IaYXT;h~afPZz{4^@br zn($GS68Obz0BZLqKb0MyvEEp-F z%XZOu9nt29ll>hIY!o7Ulpi znv6Q&d-;x1Q#smNV37IAjmqJ`f>4;j)zs}@5Ggb8NHQ&r9}YcFk1=s0qSmfDIT zL}IzQfY+Hb7z3YWw>3^;vPtIw+@lL;+6f0j=R`K1?Rs$3&Ft1)@NM5zV1L&`Vbl&7 zswRx&Edg?U7fqYMBpWQ6jO&vI*KI5odc0(9&B?LUS$lNhs$&T-QLab-p|8suK`a9N zU;>Q)dneC-M2!FT|4RScQqNRUcScY|-Hb2FWK7ixX)w*zIKVgM!)R>CsoYSb9@Lsy zLJk9)H;@1=N~KM;fxCA80PT1w>bSwB_El6JKa7XzdPVs_qfTy_HegHLC>RgUxX-lj zs_$O^k~(_!_WADl_zRBtc0-mj? zs$_XlVRk8UA;TzI%p`NZo^_F0EiGU(u~@&bF!!jgly!a1es#9LBez7Usio}j;#J*M zYwchj{qF*wFL`?T^AP-=5n(>kT+$T_0iGHp4PM3Z+@Rs&k(ghDz;|7e>IBW%Q&>Q* z*|!8m`k0#8(2SfZzjS1JdAS)iL*a3Q>Tt-uHB0^>6;1Ac&)lXvA#A+^~TF&^<-Px{Arzw?$8;b z6(xcC)ary#!{#M(-LV!}WvwJ94Y}p+dl+)^9$xeZPD9+g#b-y4E)=6{dZvMSy(4bs zQqd@m1o^6YxMp0{hxGGmxj9Cv;|d+QcXE|*vQbI!0Pil2SOuAXlwDZl!rN-01kujv z`f06S5M~gsjn6G_ql(Z9v;Hz>hvm)t+G*Reo}Oz2DoZC~IJYFxV3=*1bcDI#V-ehb z`yS4?O;M_uUKUWRm9-0*%jA%+L}L(ouJ)NW*6>k4H0cLNq(fNgHv4Jnoecj0zTR!} zd#20Z0rVivt#5;(=aRdjZc}W37m&` zO8hf+O$5W$AK*8A8`$z*=vRHy=*QmoFlAg=(s#RhNTHVYC1}1K@hC|GVLZ=F6-*0x z{+sO$vPen^=y*Dt6A!PzJ!}(6LIqT()R5jys9m(YH-ka(Nn?~~Rtl-H*pP{zU-MQ? zlXus*&2qLymA^@KO>Y@ZjhbR)e1(|kVQ~2STn}zH$Hv*3wWt5KBjg$eN#@{G$fcMS8-`5K^IA7m_aM6 z`$)$n`bVh3x<&!)d?X1WLQ9uG9!?;qPGiS*BaH;RE}RifZm9eNEHWtim)l0DD^SyZww8iac z7r6e^#bzT+IQYWSF&Kq!LAalh*r_;Wzi*>jtu~LuXq%d^sr49_?y34lr!u2w+EXxL ztvGKYoa^y*IC%Ypz%YnJV8{reNW^fpBHc9m`O*l>0iqm+au0Ze=X^~VrnQF?&PU+5 zvDnPzI3)KOpigkw6k+Ys(1~ggta{l}hmoJQoMZf-VJ+IOf#vtk(!25;+d@FGwm{aR zAx2bT?D_&PU}I*Rt}$?_UtrnE;npz+3Wm#cQDminaPZX-ZsD&rZgNMlOP>~lPs)5- z1VY9g@uu8tU)@>Vy33Lo9Nkp)j+fdu6g^!Frwn87+^Rz~KEqIZNvGPU)wR*jLB$B}I$TO*f~!7t4654oLO6t8V2r?1+T_Q&0K0 z4682u*_{u6j(?P@{;`Y5=-T~Y%Kr<77Z}0&gZ+aQ{5EN9gm5}+3o-ZC$|VI0^CJnl zlu@4piaXoYaQOv8RMg_I3w0k1bN&6lEJ=n~1W@$^LZ*+5?6;J{!0RU%BNqm{<~-t- zYBiVcsKMtWrxI-wsbMy>B;oLhCnBi?O$~EZ4$9!UcL&30S4}6G<>y$P0t(I%#Lna} zX_$_w@IIB}3veH9GP|^0P;_>@eR7vav@g)kd8j3{^_~v_K#JRObGNy!PKV z%zyngxUd z^s@D@xs>D?9|0^XQSe9+5fMBr9-1rL2ipylxZmKI{+KWoVU3B__h9-y+tCNq0iyqW8C?N<_=wTWv36hc-;u6_5$-8<-iG^wVX{rs#%*o<0 zP`zZD%9FKz8kA)Pi`QrR2c(!`3^|x4*s*D2BB*E3p1pCB6wSJ(K~r=?GY2zKWbkSM zk97>~}>cv zb$Jz&BN$J`J1%`SPSlD!*ydwZh|}u@DspA$4$sz zuve=&^SCLUwSd_bGS|G?7q|}mlM8;PN?3s*Qn`LoL_I|_0v+g4G5lm(&>D&~sR6?l znI)Ws=bL^}57Jk}tm&JypgNPrn=57ljDoPx5vC%_rIdlHBI-9tCQd3ccs7 z8t-*ywH72aUrR7)OSDPqV2JeQ%}`Fj)8^<7+S({A|0d~}AU_#mFK*xIuPXctHbR_6 z0>4#tdv;L;zy3>@ngEyuC~{UEld$Xby%R!P6GeG0aQ`p@>*JR7p_5+YHPKN^V4fk3 zP=|o0bY4goP@xf7HieU5*Pudrp}QZK@B~{n6cMl7DMdWz@t^;~@D^eU<>!6(45Z(_ zk$+hp^uOOo|9MRR!MG0pHBKn;ANR0%BC@7!gZmJPZJXt>$m&mX8a!}cI&=T z^1$X1PVvlD`DVXD#eo%T9Hq`v^hcCB+%v=fj3To3%ZWn%=JZC_ zoex%j4J+ zbQX)n1VtYQf2U6; zl+lO7)ctA65@v(JWy3f!Jhj+syx9tcQ)P2qi3?*W-Zw#Ork|#Fs{k`fVV_!Mn!xL3 zIk}JIQwGd7Ve?#cLD_l3;B&IP`k1Ad;eT4RS=pW5A1i9B3J!lo3 z!WN4Denb)1o>9tu9*MQeIgR3$ z0rD%TiSRC-!526-Q_<1bGYn58#9j%95VT-muFHVK2w+EN#G8i;i`sA@UJgGpB~}7x zXT$xV`dKsMX!X;9Ku-Kvd`_&(SCYV;p<-2TVNbPS!mBJ-Wd&_+BDCO7!-ztt23Z4X=cs@kswD@}xU^1g^h~pu=^6pW ze8CszeDle6mmn7p6^EWdfD|dyNB$Hf%@?7eA4}|ajD2dyBKnD5ou30#)271<>qDF}GnvD)t$ z2fj&M*=&%VGF>YIAwtb!y?Ie|YWR?x(XuT5a+5#3i=W?qc_A~KjWxnJccu=Xz$PiiuHzL7#&Jt#VEx6v~-8J%V@+^q|MYi z{c+eNd4k(vCCT3b1G%D0UknFNZ?%lsqRm{_Bk#15n|;|H)9O&HOroVE-FG(hc4&ZE z(2P$V`Y^c7#KE)tx3Id<0tT%cp7~`AFs#cqf_JH!mS_Fm3^W1T!JXma96S=IrQy{} zb0%%7OB-G)J8g)5WpUWTd10Kg^gMRt${vh%)nB};`vmNAbL>TCRA6}wIE<1qWykbg zPcCUTMV-!d>owCDM3^BD{hCpJcQE*pH$gV#ErC;Wx|Pm9SnipSi4GEzX%cltZ8sf0 z4GJEGTyuxoh}YL_^g{rSCj(Mn9xB&ZpEqiyz-a5H?)=3b8E8s zNV4xhy4dT&cqJb_1$w&<_Ly*)afAyxX!#R8gU)gG)(#SXrbXZnoP4uq5;X(XFv+a6 zX>3lBn@9^3=&!a@Iy7C*kVuccxvO@qV6GM z%IEWSgV;mL3SA>lp*KOzvB5IVgDpwgX_;?gI5YK6==zNjtGgy=}3pI7Ml z*K=k&-d*&zJ{n?u+*PW8qBhLLy>UlMZiEIK|oHw$2rs9WFwD^(_d8L4@aT5=s?a8c%PT*VUVg&tO4QDy2SY zjm2bF%vg0dwTFqL)$eqaDox6HxHo5b zNFgp5r*h$E+lpT*h%KuH+&3V2#-tv2SyzkL$JGiwZeF>fbV(hQ2BwSr_!rt3?1T{# z3+p)Tl>z*Z!>MQQ>u0C#>Grq9WuFghUm2<38IZ<^qz{5X#CQaF zf*+9#(YJ9s#v$mL$-q)RasrGY`j8?J&3!QZLlA<|;QEREfPSG;1T6Zobq2^_0kt5q z09VRDG;Z8JCf6j{ENFc;@3BBW=)L0zw=Nv`9rTWlU%SG*pCtHSWjNhK_eeShOUWc1 zguBW=S8?nd=TBUyH^szUGwHcZ_085TFwz#|m8>-DLDz_i63t}Q{&1Hz4#&BBM00Rg zVBLmTo3$&AFIBXyzJFV$-LXKdTj9!w1s4u$sTtwJ%L#eIW7Q-qMV*+xeM-%y0(?Xu zYf$T);aSqS%JCFk#=-}_oMlbLI6SL(vsS@VW3P{axttW?Aj^|nTNjt{WwB<@*PDZT z83dbE=PjR;JkTlb_0}gc$vw%DL8IuHL48?t7bk-p_2$2S%@_`iYL2H6r(tbXtG6$H zi1#UpOr)gY$kAjz^D_2qA(d?Drx*fE7ciOz|S65GQ?@VtM-pB2z zI4+D&hV8ICIAo>$0u9M+c}S*w#r~(Y`X!*Ot*s<>_$|Jy`Jtq%-UyXuOq-?62R=8(;>I?z9KdCKML;#{YLY$;T>XZm?=UMn_|2rJTDP1Hb8tg|jxd^v+7b=!NmtTqBeh&ZS#8&>3NHz5w>{Y4R_ zO^gPq`R-cbRMDwPNbP_#R>)zaj_`d(XF|e#kUT~iLdsnipk{POw`}Y61ZAD0nZ%DK z`9$<-)~~Drk;!X=k_bh1nq3~u>-~rbzMYZ?_?z4aK6~P}R|Rp=V)u!VrbLFxIW+2b z>QCbRY0tN4TkELh&c0Z?EZk3qPr_Z~pM`RmqbUOkJ-FMoK2VOdHC4y-G}8eV+DZWk zX6jN-&=s0$n)ykYm32Cz^-9AHW)kRCfBXP_Rx{TG3mN7#g=+BS3*~Hwshl1}_t0Tr z@>%){i8cncHw7ld83d}Tbd$lY)kp&6w=djR4OnT|iOe!>@!}5DO!8*$5^bG9=g)2C zhntFe*FYJuTv6y}J@zbU^Oo(_A470wLp;z+iI}Hu+#FvD9GC*|JoXx#vUsEWFMWzs zrZu`29dr4^OWAsvC}BUpF4b3865d`bCI=`twM+)7OHA!s+~FKJo5g*Z3)bGBekB6l z{^OH$w2KEi*_gGoh!}k-;;t>d zONzdN&YtPqo8~CDbOb*JqmAK3!_<^zKpEMCm1_Aw;5Ap z5mLu5wB~x0{)K=s#@QHe4QB^QHDEk8EK5WS~XtNf1f;f+>NG|?7@i{z{;oEixJ8NF5> zqrFoEMY^>gJf2r0h7)7!AZa0;Q)Gm-_udiHd6-r+nLkdP8Idjb7YZHg0a|P*pi7*?SHZmWTU_)ek9rzu5jNMxZ1-PQ*8;dpg0KMZ+ zvg<$xcKwT1PCU?+SNM$wAHJ2tf2-A$Hg|CNMu7i3u;2Rm|Lb+l{H9sv<-UiSxL|KC zp<+^oL`w;+0@uOD5|ltr1!It<>CyM9qAyLPU7^`<<=sZwJj}lcAO#Jed;j1|xZP-) z_$diC9(R?o{+&~-z0B_J_6ANFjEe%X=ZqU66Q?A1(h!AWTU?EZ3$shuPcfd!pqaK8 z!fD0;=)T-Z(rPPKxoI++8v5w=@#2 zMjXbSXl5Z|#_JGO8fUn|tFn|N+D7@TQwqfCT14gR8eKfo(XD8)29;&w))lNX3C4^C z4_yvO`*Vokel4~CYWw|m?mdP`6}1AN$VtBqzG;7rd!*;vK*TA97s|PqHCZ{xFnm)~ z9s2x4@urFRS56_BvH!qM3*$k#n1pR|IB6|zmWY+93=<3xqmsN1=9s}qAI$)aN{!JH zA_;b-#~mdM`1_d@qW?<#VVuI_28>DS-W;HRhS3j+m07d#0Xp|#ZnIhhr8t)5s_EE` zT3JNF4UnQUH9EOWEO^G^5&wflY#veqIXg;kE-My3<3l<9gfNQkP1q**CvbxQNd9i4 z?}rC`rg%nf{cI18sklEK1$F*5M?}!fAVS$8bbE-G#XWNyeA8y{>>3X2v0d-+Oj2Nm zDM~hDkKQMEUONW4)V08yH^lSkurW|St2O-qg*X|7z@2eK@Q#PRzc^?S&VF!iHkZ9r zQ|_p96s8ueJgP3de8T?u*X4X7*PB1c+u43Z4}DJ|zhVoT0A8Fiv)KyX%2cjV8ZN3c ztL25YZ~Q;dWu@}E_5AmW*7O3qy%ypGR;@9T0t)F($+h1UowgLH!l=2w zK!qu7u!lkB2db9ff@F80U3Y&HLxo6uuR{t-k=~4>KaMap`91+%-=X4x zPIjb`(iwV6mt`gQh|&>5t)M7K(0ED|DJt@k5JMGy`CcbL;4X9eMpYv9y3t4yjy&B0 zXf?}(|7;DEY^&|$+8O=?lHh`ed24Gb-U*!6TTaZ0@pw}Q7YzJ;?~UHyTPQ)J#Zvh? z@zWJEmhvLkp>o(em;{^vHcBnExu;CTR9eB;(I!)lr!hG6E{)ZFyun7Nb=JW@0qs@d zEkQlh4xOnd+KSSjO@HD@I=o=|<+>iix{rdun$Lsk$f(=9m_IWJCWN&~H&6?b*q;D~ z_z1*N#2($~+O|WY^B2XDwT~$_Z>S36GLjfaX(W-3%cth0B?O@ffccd9nP^2UYXi03 z4uGbbTuq5S1&7(wk?e{h zVAQ9y(!U+Xu-73g-D=uy!XCaY0}{*g46Aw(uj3Y^`bK2@ecVX7t+Z{Sba#VZYI$;U za)t(vXQ(p)x&2Z1>e|kteyh;gzRHrGHZFI%Py~Mt0qoEdxHKWd^)3)GmjLTWKW3do zAjEvy9GP>k;}a@@mp%Hf?5FySdRRTR601M)xPFMIdDtwb#x(F{<^lxbF(}O2M7WWp zl2Z1I|46W47x`fC9WM8*U=}&;9?~EtEz$n{MNV}jhKm(Yw$~vO&R{W4Hb*>XipJ>;XH2Jpx|a+wMXI;lt6wo3Z)Ljs`DHXyJ)$LIq``b zD^gxc6cys%uUQ7+5cWzYV*7mU@Rfg|8&gPjCfdIbLD}~qVEcDktbY!{zmfonO8n{L7g&g|Bl-aN0_nVe5{2&8e+`xB zMjki8%CJ(Aq9@AD?tZ1GGLZ5Aq1*=~L5L@!tSX&ponNexPDz*N=h8YKH9L-P81rF9{!7(z-F7_b$_>=@tomyjdThM!y<6Bae zY{vdG=_1{p8)N}8ioS;C@(dr@R_)}T5C%c>V|b~c;5LhRi;iAu8)R}ulL@=&s@Zk6 z>}ySWoQ>vDwvcTPx>kHaVbZ+SX}@rki*GH~J4+^t9PC z=u|fHt=14)lle{6cYvOX)mZ&GBJ2{g$@KN8b~e?65RAYOh7N;tzih~EAExjN@1q+I z%{fZHMf2P&Y=78aW10S)9?~lu7_`s|<`1A++aoC^NWXxm+jurhppAHvH?dRhvT4g} zhq=&!vD%Yows`SWp3OsVWit8a_qg>5DDv6w@3>Lm9=CAtDXgJv-m&d;~GjW^oz$Nk(#o z1@_a2@uE@10q#}vxN(esT?KbwBA8PA?NrPEpYyT)cg5-dgKbER+m`sAk2Ta?uU_9) zg!RR|*tAsgGaqGH!bakI{!w92PLLRFM>=soXI*OIYUm4;7fv+@-Rlppk~yYy-;f~Y zcJ%Gk`t85CQyCv0$GhmhL<<5aHHdw~BEFM9lm%|p%#Hbwp&mQodTollzGque(8vY{ zR52gtrQ4dcCO!$xA&Ru#v!AX@CL$(HRaHtn!s|1duc@egD!o=UGEWK_r5cS7tNhs` zXU)qVDM>CVNreLwc-GFA*S^Fo;8zo42_DKC(|j8o_}K(;FZ+tK^h}zcEzqyTWWgS@ zh9q-VNo7ZrCv?L8M>F4XBPFc`LGn%7C|ap&BD@1pRflYD?8kcG=Bv?7FhDcF#Y3#* zBRajkVLtbCw0g{{;BLZUXNXE4Z14wHVE*azZ*o4JS@ma$C)d8`c`ZbJk2~_fGvavN z!>{FFkFc8!sb3(TVQQgHCSQ14xZrpu4#;GuWJm0@kuVUqKsRotYGY2ARIOEe##N}v zbX>=47@whw*!`#5H)A98{>QVNI>*K~_FtOT@KY!+UcqjB1B4c-kBRlkrvGYy$QybV zF8{s^o4$h=|CZeN&(Hsd7yXB2N>uui`3|dpKDi%`*(GRz2+1RcH;9hQ4`lzsvXF{^ zASDO;(yU6hckQ&eg3FKILw=zn1_~wR^}Q~zbJj$#j2DQXx|*2syq}!7`gpznAoJzm zJ{9JZ${c8jVh$6aDWuQe$D)R<=VV3+B8O&3?z7tEs@|;vc)&p7En(D+ufG#Db6+i2 zG_pH>tN{ti&V+3C6i?=zx8Hu>Rb89an+j^Ca#Z|_`WR}?UZ%#yU8jLIFGa^8Qht-2 zPIzqsHkga93Dl`Ym)3uh-Nbi}_SsrnFPardtK(KG0R0Alo=5;j>-W%a zv;YBaW_n*32D(HTYQ0$f1D}mzt}0b00pREwqaDs63=9t4-W0$vOrgWA$;f-Z?&gN` z#Y@8Jh((?U{Aty(@Y^H#kv>kR!#)il7cQQrqnK(M8+N!FX;TKysz_yWVeZyih+bxz zPFhwq*I9wiJQZaX@R@Fd zhm)M^g4J!ocM&Sr#Je(})eKrZfmJTtsBOj#%QhS~p?;xq0xat>K!`S6yqJ+fOHe7RiPEXH z=n0VtGLibuH)7tE89ep3(GVosQpm zp|j;a@eEz7Rpe-uw=-^hN9oU9&rT-Yo*rL_J%lQb4~8PawCJ#I-}SFFF?tvaaBG!b zTBym%9f;9t*5>+-4c`T6gEj75YQhMztT$#gMLkh}wXQgjGilvp^{t|I(d@IA0>GVn zVpcietfni2yDnL&wq|Q@girp$h%7qMbnk`ys)1-$xqmNOeHiRAOobh0h4dia@LIh{ zy#XGd*48bZ$YIF~Nt-&b2;LJ)iLy;M0aw48LMd|`3NK3}exvO%Kva$Hkbmypq|qc`#aotE2e&8Cg`toXsxK7lp#v2NQs4T)#v(*T` z4V-l$BJ&{B?HBmT8)3|K-ss)Yn$YH3|v82T4{qFo{drP++b-XdQ8sW`iIaxs@bhmv(W2Fxcau^uSMsEK>Rj z73{pi-93B=GkRE^q(gv}Me`lRD$4u##NtahUMW~WV<_G(mZgpxEkT>ktO&T}AiKv) zYPQQC9FaFTI5u-gy3R1+TJ&fCfwY)wTXYdcPDt(be=m1EX>Vna?{aVX*1{P79o+jr zI=)23ZJRl{?>rL)3bcdo`T_?kA{z$wVkc$8Dd{}$~`4ejC5hO@{QnXc#T z0QlFBFY^6Xn)J?tY@wU`ojVNF&?|( zbnfCK%xS|Q_1F^Kz7K?C~u(8lI(naxFtb;QU!&?z02`H&FF z!mkS)m6y@=PwvK@>EsMeD+WefGIOsvHuV@0?F+bwogS6kg5}ae=zx=nP;tE?I({Q9 zVRtg!inDjc7#8DG$VPEZA`5Im)BVEC9nv_2iK;;wK}ioH&CPgGbexUQ@(Sj9_!r)kvXCJ%encU1>SYu&bJCU4kM% zu&#jOS{6FHo~6ie5+zx|y)N0k&eb>APMu|luTQ!uedH$Hsv?C|)pDP8od%Zf@L%DB z?d11_^zWLo_?E2r{+*gqwzl}c2v(iS;|kx#LLQem@jm+B5D2$HA>`r^fywY7wJ~#Z zlu(rd>NV}eigu2Sg3_d8bT4$Y1!1Cz(0o0K*t*bc)*B~uYRT4w>&?@r zUBxz}*FN1|;CfKaECVr%Gk{uFjmY}Z+SHu@@koWD{1&W1mY!%e<_Q}MIwi={u_m2rB<#9V4J9>?*vl5oRZfXJTmY|e!7f;(GLTw$3dyXdC-ur& zs_ZQKr0CpVi2L-7ErFzqvnpB^fdXWKiYzKQQQ2%ZnB1O5i8%H>MR9pfj2#q3(f2sp zVrO!56^9YP@>1p*qBZ4b(z8B}iwWo#QPzJfZ2n5J5;l5WWJQI2))jQh@YnAnpn|kj!GlSHn`h1%4Pf10 z#$`L|cVl)t_`K}u(j}W>gTh}T{@E_S>wj}-5oWCtG&&=!2_|H?_mnV%zl1v9mRA+J zCMJ^31?>7-WTFszA&y6w3_lSx!8<+n4o@pN{Lvn?<(T0BQ29+UM7(g`QwA~LQZnP4 zU<-r)B?xOkj>kLd9>>fmqNQU{&&ZyHsS0l7`|r20kw*Fg+V}Ep%kOXy>A!Ju{=wRr z>gIY{gR!3yX{l`P-^*cF>v;4mcY)877@BGh6?uPPO0p)^#==jixyOm%O^2i+HnD$i ze?W{vh|)s_^3w|j@ozPP_FI*1=|dX1LRy)u(_anX@r5O@{4qT2{jrrkJ8^;;`Yz`p z>!R$W?6kPNC|ix|@r2;3ey4=Td0YGEQ?Ht>j(7H!;}2=V^6W0W$^`7 zI4ep!?~O!v5~B<=*F@yi7{w_Ts5@e*KyKL4voF&)g4EC{VF$Szr8e2F46~Y@w1hMV zB%|OUt0FB_LN@$5!IPUVer2bGG~Q`Jtd_L+EQLyuIkjw*8Ta0}ElPt!T7GJ#Kxo*& zonOLfp)?We+vTM-Y)^7ym3oj22{2xeP&!pdpt(j%`AtU70i5Ar?K>M$lchY5>M(Uj~|*+YrLz+Z9N3Kui`=?Fe|1= zh!)mB7k+gDHRK;^CKd1GKRWJjSI>*YMszDj=op$RO-x?XI{$YHU5cHrjt6NIvle|B z#L$juDFK31N_xp**g>|YiJyMW_!Wp>UXUE`c*Np>XD~WQ6<0EWeTxkBn;XiVq$xQnv48#Lm*K9f1Q8ZhUc3t@ zaByP4iMp@`I;U1fwS$bkGAwxxx!D;{Fr(r!oG;(WaktP|&V_b?=8BQmip6Luj5$0| zhc~53_*^ZlbQ-2(Y8FF)29@X0^xnMcQ5Se~#b*hLhQt+n2DLTSmsT`OMuM0oSz=k* zm^XohSF%XMksLI`ycclL8ia^bIX9+^&a4uqXvT>sPv0wq!P{{4E3DjB=sm@V$Y7%! zC+sm1RYq9hN$~{yN{e7VltX_cA)c|!n;*q?dYXczgf!fg(noPLrnnxesgD==To z8kL8^Xe6-n;aMKLfz8PlRF#MSv?4>??F%vaeY|2;u^2((FqEY{<}^6LdJYlC1ZqB3 z2{oA5)w({3mp4GtYs<#=m=-G}^`WExESws{F`1^KHG35pCaemZYTNP4S&coDVz1)h z8*Z79OCNUVzXp0;MeWe`E?DxliQF|%2gv+p-JXPDdv`g^VtVM@?JFJ?P6J_C73sK& z0ASccOU!}Lgai6b!cl)%Gh6~G=;U>AUOIwkc2>p3YGZLOhFEDwM3HA02;!~cRX5T<+xEU;Np547z(7REiT>>AxDj?=02(=YF7$%UbodGTeWgW)mhUq%ohVGsscH}xZ zFvAmi7P59!*J~lG8ifrnwf6T!fOnxnfy+8QVkBu4a81qdeDepEiW>$<4BTR0#DoQW#Xh48w zkOr5#77d`5aa;OS*H+0?*2SoI*}r^XC-_7qOqyh=csx#Lg>hkQ;q_?!}lL-SJD0?H4&BRTO`(T7`&1=fH z0g9@7?8b;wGwu11oSm{o@(2a)+v}dEcFaqdFJr`Tp%QNrqmIDFSa17nefwd?;NaEU z(#gt`FJTu}HP<`XFin|1%8^^}AmpUB1EQQ$c0SzBm)=_Eg<(8417DwupI)rljtaNr zZ!AN8cyEV!L^3VFlg#OVE8?Kq_gdBKK8{@L9YI6kM5O`k4C2vLnrurQ>zRO>*pd){ zz3B0|ccsUkB^<*IiL?N3Kcj2iHMHJbD41!e)8V1H5xSTc=e~^O90+yHjLh1Wa+A!h zsoiZ6;mE2e)6``%fiuL#d5-M={fwoxF9fU!#-A*n=IWKM&w6fl-e<0p zdsn$Tzxt~Hkl3`0vvVNwF?#PRg}gj1OfgXZX(wfV=*t!t0bR$4n!F}W{m&0LlNF>A&2Jm-taK&Yln0GU5z zg!R9P+|Jc4c&$~?;e0^r=y@EmV%*K6r^IyM+Jo+v?U}Zaph@_=ol40*wb0{(PeHbw z>xTsnVu8b9`43^L!`Rw3ZM>{%%-%P=J3nCihI4UopHu_=f*oEV;eU>t>SB?$kzDv;~WH^`S`elYG z*-6@0jA_omI-bj}^^@vts~0>)LPgL8s+ErVUw*UB zn`>FfTXiWa>Yw|TgrdG!mqU0}+vBytAJ2b>*|<^jXExZ(40s1!Ut^ay;5%C{%nu$2 zbZvhO{fsa>86G*RgW~X&k394u-+}H!zIo7Z&};6f5()C}?n}|IG45FpuWdi9^=+;x zLEm@I&%xhMM?DW5^0LP-2JU1xXOkf`?vdP!_h6`9Lce+3LqXD#@fSzqSMJfQsX>po z@MJYcqzFT;M4JJ6KWrV@<4Ke*#febLn_ z>w@cZkC(cLHm<6wz6*Xncuo@WbSZYya>K>a#F$Q|dc{UKB&?WBzW0e+N)Jg&82PLQ zj>?XA{Sm?dxM?5gAqP{{fM{M1+0cp!ZwQS$68d&|B}{jputRd}xdt{nA9Q$@l1OjN zwPBRPEZM+OjDqt}$}*WW&=}cSj4W?1h_)37eOx+ZRA=B&{?i+b>yYDNWV}UbYk=)Q zP>aH+hvg2lDxPoOodbaFV4spi`Gh}cc6QhgZ_BsdPLKH=`oZCekYCCWnS}93Y+G@} za!L0GzeR8iHDvG>isJs$IH~dIu+43%6sAgXN?`AKa`S4wTD&sOfq!yL+ooa`CK*a5zP0v<5_Vz--GC62C>eyW3Jv6(Yq3-K%NWL6Xy!!|CEm|)Mz%W>E z8o}p}6cv@1RSD1*Et%D)=A1BlM=CzT0YvvVP&fOXK}KZ{D8k`P?nVeeRZiT)*pEM% z=FU_qeKs+p%;7KvQdJQe#e{H?@5!Jesxq)<)e46sH(6w?SKJ)^FkwkxQ^6~{Jy>!L z?-0%cPaPB9Qg7@EGm^=Q4d9)a>IGPIM!an+Kj=s0)XsqsL{vM{mxvH33e!z(xV#6{ z`Ke{~DFS`$k{wC!l};Mz_P4M{A9wg2cg30(J!DExlI6~DOy0jNOTs*m^C+sdVS>|8 zKQbY|-cZxXWaaYAPh&a(6n8nMC$E#4Ax1dG1^7U`kbyP)eNt<$z# zeKqf8_zvmg@OpT5%}K7@-KjUNJ3r7^Rf>FD;loeDy{U_?lNQ`5X zXHyC%i3!D^8iGWLS`tcKhJXqJ60@d+&adg%I-N)y%VpG8B@euw1mA7gj8|K2kPH>G~2^m))x1XKx$48W}sSyxP{S^wVRF|HV zSk#xKrLp;$DhJ9vDqaY%EILEM2Ie>ubBPA(l^rv|ENJbGe@9V+j@`0`*N(IrXNb+t z205{qs|n4g|1uYbn6-A<23RGq1$3V8EW-~7xP9?syH(BlAPhezomNa`j4br9Fz z)=~FT)xlItaCuX3-KK2-mJdlf2&(s_-7;NWiW66eC_FeWNyhAkMMLJM8Npo?+Ozl3 zBevk_Vd?ByzGrXwCsVhv6s(Tp+}Ppw3y4LwYlS3-2BbkP8R^(QNOla#O~s?%vbkoe zBg7QnQr#UJByEJVsd2iM+}^v!s~Q^P|b?a;Rxpn}(?tsFwEWKETpFp4?3BvCi5gy4)HQYE#UD<7N|{(C=aHd(2(eQrshhDxlelF8qM>` z?!0>eag8!)0GMz9P1*xxHa$t6>2EWBNqBCD`#9Y24Ad)Tu`6xK*_p{(M;4Dbj0LQy z%O9jFpEv&AJWr7I^R~32?HCc~v6<%wf!D(hX9T6A8GT&3cqG%Ov}t_I^NJRnkCk?) z40aie{3tP3S-krhh($@gBH7JJs$BGY!0`02RLo%7Lxm;5!mS%1%yUC9v`4f>ieE4H z#l!OqX^|s43*g(cuhNd>V;JW(jq>3?_#5Zu!R`cQIIF)&sZ$kIb0@Y*8LZGeMsTds znrK>jN8=W3HoVhJ8%0!N;w!@&QL5YHfg-HJ%tTy__Huju0)K2$Wl{|%)5`w*z1p=m zqk(I6-12zJ=u`GR8QMYSslPAtZ@0EflK#cS$XoUTvUzAD5C{~PM{Op$pD8|ftE~PX z{g+?P+@KCOnx(#?cP%8e!)k;X?=ysdA>^SgL=k26OVx%=wa~L|(d(mYv!{8dcze6j z_h|LI<1^Y z5rl?QRzUbq<^7^<3Nrw4iZW@%LvB%uj&Gr+rJ~GIy%hkFrYABRAUnS$q%D0>;?e0F z*YC*NTZCx#;`B%J6dANYbnJuKuiyJ@rPo1!W(yoV9-N|E*bi?ZPSQpCp{sJ6NZ*CU zkKUycUA-@@e-CT-x2UC~bWalsYqBGg!6ArFWmEw1t)0(NT zZ%ah9P*p#+ogxb4pG<{n=s1{w6yf)5Pnc7k->i4J$D=#oy!(LeDbH6emaBR=LFm?bmTzLCYIaUSX9i+(Np3Ech~* zZHTPZ`qMW7@!C0m)ySk|8>=iz9uk3a={c)1BmX_(iy>YbGwBzbB70ITRD;4)n5Re3 zv3feudeh@Wv$Z^3LRkfij>W8`O&Xe0GmItv={wtBH*eWd&MAov7wPat zRX+eoZInHV$FwzpEE#?ASl&^}UDi!0=un=cDFEG_WE^xJtRnhKeVAkBcPLe5t$F(B zdMxkAZQBM_DexyTjp?KgPItFnTep?d7nJi;%7+2_B3wz#V@$6<-6N=m@0Eb_ma<*2 ztl1m5s--y1ew_AvXWGOBMlS{P^oSw+WJ3-`l?LTUxly?Y@u^I6d#dM}QeckO61;u5 z*oLSY({aV(R;c;E4J-16B^vd3ZXp@#!TXInjaahq0>{!8;$%ZPqW!!dTfeZcQFyZ1 z>`NnKReAcFyh{VoCo(Ecg&r#L7$AT&J50!dWuZCSI$7O;2*rs6tQS_bbKP5x$#Btj|uuR!tp8n*%I3T z#I*o#zgxZ75dLNmV{k-117H-Xi89zDKYCfrph%G{*9i8aW)#fi>{Od&bOn&EF~ftt z+7Pq>z)@g8x%{iNrNriHjL8#Tcz|$oqk6D3K2kKbzn0Hlx!8MjN0IXyEo3x@M3g3*q)7 zf=$>mM3McVz#U|myVoDXx{f+xFGNmwCa95_dZ&z|Bvtyn?%{DPH&dD&SoE3s&_z0x z;~M43AnS-z%h+87s-#;(dqrM5{(uxI-x``q{p*WxUWkEWpcdlud)Nt*NWi7ZdDIrC z_*E;|%V30~wZFY1*p<%OpJEBchiO-F5;>!XwzZz1kddp zLZ#w8zx>=scB@Ztd0c#j?z|9PpBNz*-EK)g4%Ib=AD#i#u%c_fz|}vELP1yJH;%_G zBIz&kcdB@=G(LXklqV+FuusvJHyD%Dgh&vGat^kil{edhO2WkgZP$cFd57ALEfGEm zA{ooH`(!1zw_6z}?LjLUIq8nv7yXTl)rjW5#`YLa&C~01FLasqF-bD~i?@MUFJQU& zSK^=jJ}|QE;-6WsfAZ7xKB+J(n3l$B6d_yYh*tf=XlZKuwE1eZmsuk&H(f!fH*$*- z=8VRBrHYD*9hKoEhI<&FNX$4HtbcL+-fc8Vrj^C=axFkI+|CN6am>_(t&OL%n-LR| zXL0(#i=SzkCh-Z&b)93uyM`NMyhTR&m(~3<4n_DN8BWx=fa0lu|1Wo@HZ_;#WnRA` zFqhUtg=`xdz#g5)lATxmS6KhH?*TGIn9kY;$7BRg7*A5X&9B*MBPkOrMH%aA`I`Ybng+8#5_=~W4X{{&s zp|@|-*oP4uBv0IA7toH!!d(J7dy@Ny_DjwVaC~P;D|)N5{HHp?{K9H-kn(a+Nk${B z{~CaG+Xi)9`xa=0zdbJ0|5IlAA7J1gd)GgZAo4rry6_u?XS4cB)X(^@9Ed(@ps{>e z$;(f|5Hm3q2K9j6W_=e0u=dNMOQhZ68_T_L_>>Y5@dZ<#gj*R+J$2&S-1*dXk7=Ic zjqk;++de;1`r?`E$jeg1i2Mzpa9gs94gq1K#1G6!EvdaUQY3boUDqWoRNM3Rt;Ks? z|EIDufroPId>lu~1>khSb`Z}t=!`zW%eR6~<(n0XDNNTWf@b}bdxZX%T;np@o~ z(jpSKP@+_Hy(&v?mP+^bo{8~rj4|)&GoP_^zP~ePd(Lw_=l4G;fL^t`kw|tiVN}*L z&USsIm7Jk{c%)>R9*x(!@`lVOub%65yrN#sRP#t;S$u}Rid7@pCX|9Mh#q$0D>wVy z`ks^`e)vp6hryw}6~U=;H&Wd3y($#i=Gfb3f0I37m4Co6CP43!Z(x-N`X5osp1tms ze%c3}6kDxdVi;xvDg5Kk=TLkvqlYWfL@LvboWsVW+U`h~6rz383{`x@j1I34O>A9u z(OF!w(7xw%ab7W5$HpM}K%Mf9$YGm+jk=D;r>mTjH9CcgYjXwbLtab1OI>AUy5g{C zP+qH{X$!n|DOCvC7Z1h zLb#ijLmCEVemlBALG`lx+>j-CJM z{h@xv#Js&KqkRhBOy1ko*g1^9E1Qrp(!v^?%anZ^SMoN$#p>Wa#eciXlWFTD1ES($ zH&V4-ltR*P33%k}#G;=mJh;o#As5=>+aU21_EK|k|9@jb19hYPwg}ym-xdxYfL#h6fHhzqHN zYkcGRSE)zjf>t}WM{V$3mj0`ekRsBM<`vXf`EFyewPD2G@^lO3*a69qCC@P{(GljB zE`En-IER~AWiM9AR!j4{Uk=#yOt;C+#-Op<(;EA!y|FJxLO9WFXBeaS><3EcaP&*( zzo~{Dmbt3xpYxQDABzsC^mB-j_Y4fixsHDJ@(yo#wk?L1;9ELcW8OHntM9o~DYh@8 zuPLcd@fq&(3&k|dQ~tzN!->&}k}9$L;?Dn7wRQCA2?Hg$*v-@qnn$E{Tf&&2xYXs+ z_LD(>AN;Ua#b*3^n-u!hwIU%`r>>7{oU5eb3t#wbl-7!T;3rgjJ92pfS?_rEApy7Y zS9*>cy#}|gS#39hFKYTV!#^#)X~5`sPNONB&!GZCky=_LR?Jg)3KK5)P-{=pn-RD7 z|KV4UFm2h_XU&_LWA-qv&zCnd!%S81{Fg%;N=8@A{_{GzSaQPzz=BLBF>Q^P|%BeNnwjwq79i}r|@D4J&`6WOqN zeY4?>G@M^Cmc%VrU_17)(9zUH(3Np8iJwT-!F6ng7(=exsw5C*3 z$^`UBU)w+AjcY3CzPctu1(Qyh&@|3*@)ERG>GdpMP7qb49B)w7x`l3AJg7h}x;0XH zOs6_OLo-O7?~z)8VTm_**C=p9U)bW;@Ae%!8vjrG)&fz`lo;@0df-oa--Bn=Is4xK z#g*H=;%p+BqtiVPugD@`558mx$YcUuh-p4BSDQ-0sDU59vNdxwQMcM|u4!j8JDY#` z79(TupPA21fk;WyiB1KNgrKIg*_v#(GB2B@A%#i?(d?zypHcFT)lO%(98W6yOD8?n5M)czS{wx5WqGz2>X%9Wh`BayD&NpQEt}Go42UWTnwA<_|%>>Wwvn$^e4>v zR$*TaG$)R%LWU<(G(D&=EHM@W|V)P*a|Qn z4hw+b3E`aZ&|L|Ph28KG?7aw1*qPfsFcbDhMwm-!oR~lMl;&Nk!8XJQb&MP8{HDZk z@nIuXL@4_N7sa1zs|pLiwv~uL@+mF^IG9+%O0bI^qVyq&3ni{R?O;vVhz!xpO5sA2 zlPwu61)H)UQWF_mNO7=eft6tY3qjn5ACL*xp{QoJiP>sQd;1H>C zumXmzaWkg(sYz|Yx`GcxA$*%sF8G{}N5KsPpCLiSqRSQ*W8W6=(*p?eRqY(+kLsBF zECF0j_>T|>v%g_sCZ}r@ymgC^g`4J*x!=fzKLNa*i0Hg+o}&Y=W@mJx1uo<878fG( z+vDkl-FzEfaG9BzS*t|m?iMT2se)iLW5(_odEUJ)I~zW5%Y{PefPe47&D?g75rz66 D613UA literal 0 HcmV?d00001 diff --git a/2021 test code/gradle/wrapper/gradle-wrapper.properties b/2021 test code/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 00000000..d050f177 --- /dev/null +++ b/2021 test code/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,5 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=permwrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-6.0.1-bin.zip +zipStoreBase=GRADLE_USER_HOME +zipStorePath=permwrapper/dists diff --git a/2021 test code/gradlew b/2021 test code/gradlew new file mode 100644 index 00000000..2fe81a7d --- /dev/null +++ b/2021 test code/gradlew @@ -0,0 +1,183 @@ +#!/usr/bin/env sh + +# +# Copyright 2015 the original author or authors. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# + +############################################################################## +## +## Gradle start up script for UN*X +## +############################################################################## + +# Attempt to set APP_HOME +# Resolve links: $0 may be a link +PRG="$0" +# Need this for relative symlinks. +while [ -h "$PRG" ] ; do + ls=`ls -ld "$PRG"` + link=`expr "$ls" : '.*-> \(.*\)$'` + if expr "$link" : '/.*' > /dev/null; then + PRG="$link" + else + PRG=`dirname "$PRG"`"/$link" + fi +done +SAVED="`pwd`" +cd "`dirname \"$PRG\"`/" >/dev/null +APP_HOME="`pwd -P`" +cd "$SAVED" >/dev/null + +APP_NAME="Gradle" +APP_BASE_NAME=`basename "$0"` + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Use the maximum available, or set MAX_FD != -1 to use that value. +MAX_FD="maximum" + +warn () { + echo "$*" +} + +die () { + echo + echo "$*" + echo + exit 1 +} + +# OS specific support (must be 'true' or 'false'). +cygwin=false +msys=false +darwin=false +nonstop=false +case "`uname`" in + CYGWIN* ) + cygwin=true + ;; + Darwin* ) + darwin=true + ;; + MINGW* ) + msys=true + ;; + NONSTOP* ) + nonstop=true + ;; +esac + +CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar + +# Determine the Java command to use to start the JVM. +if [ -n "$JAVA_HOME" ] ; then + if [ -x "$JAVA_HOME/jre/sh/java" ] ; then + # IBM's JDK on AIX uses strange locations for the executables + JAVACMD="$JAVA_HOME/jre/sh/java" + else + JAVACMD="$JAVA_HOME/bin/java" + fi + if [ ! -x "$JAVACMD" ] ; then + die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +else + JAVACMD="java" + which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." +fi + +# Increase the maximum file descriptors if we can. +if [ "$cygwin" = "false" -a "$darwin" = "false" -a "$nonstop" = "false" ] ; then + MAX_FD_LIMIT=`ulimit -H -n` + if [ $? -eq 0 ] ; then + if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then + MAX_FD="$MAX_FD_LIMIT" + fi + ulimit -n $MAX_FD + if [ $? -ne 0 ] ; then + warn "Could not set maximum file descriptor limit: $MAX_FD" + fi + else + warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT" + fi +fi + +# For Darwin, add options to specify how the application appears in the dock +if $darwin; then + GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\"" +fi + +# For Cygwin or MSYS, switch paths to Windows format before running java +if [ "$cygwin" = "true" -o "$msys" = "true" ] ; then + APP_HOME=`cygpath --path --mixed "$APP_HOME"` + CLASSPATH=`cygpath --path --mixed "$CLASSPATH"` + JAVACMD=`cygpath --unix "$JAVACMD"` + + # We build the pattern for arguments to be converted via cygpath + ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null` + SEP="" + for dir in $ROOTDIRSRAW ; do + ROOTDIRS="$ROOTDIRS$SEP$dir" + SEP="|" + done + OURCYGPATTERN="(^($ROOTDIRS))" + # Add a user-defined pattern to the cygpath arguments + if [ "$GRADLE_CYGPATTERN" != "" ] ; then + OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)" + fi + # Now convert the arguments - kludge to limit ourselves to /bin/sh + i=0 + for arg in "$@" ; do + CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -` + CHECK2=`echo "$arg"|egrep -c "^-"` ### Determine if an option + + if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then ### Added a condition + eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"` + else + eval `echo args$i`="\"$arg\"" + fi + i=`expr $i + 1` + done + case $i in + 0) set -- ;; + 1) set -- "$args0" ;; + 2) set -- "$args0" "$args1" ;; + 3) set -- "$args0" "$args1" "$args2" ;; + 4) set -- "$args0" "$args1" "$args2" "$args3" ;; + 5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;; + 6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;; + 7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;; + 8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;; + 9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;; + esac +fi + +# Escape application args +save () { + for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done + echo " " +} +APP_ARGS=`save "$@"` + +# Collect all arguments for the java command, following the shell quoting and substitution rules +eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS" + +exec "$JAVACMD" "$@" diff --git a/2021 test code/gradlew.bat b/2021 test code/gradlew.bat new file mode 100644 index 00000000..9618d8d9 --- /dev/null +++ b/2021 test code/gradlew.bat @@ -0,0 +1,100 @@ +@rem +@rem Copyright 2015 the original author or authors. +@rem +@rem Licensed under the Apache License, Version 2.0 (the "License"); +@rem you may not use this file except in compliance with the License. +@rem You may obtain a copy of the License at +@rem +@rem https://www.apache.org/licenses/LICENSE-2.0 +@rem +@rem Unless required by applicable law or agreed to in writing, software +@rem distributed under the License is distributed on an "AS IS" BASIS, +@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +@rem See the License for the specific language governing permissions and +@rem limitations under the License. +@rem + +@if "%DEBUG%" == "" @echo off +@rem ########################################################################## +@rem +@rem Gradle startup script for Windows +@rem +@rem ########################################################################## + +@rem Set local scope for the variables with windows NT shell +if "%OS%"=="Windows_NT" setlocal + +set DIRNAME=%~dp0 +if "%DIRNAME%" == "" set DIRNAME=. +set APP_BASE_NAME=%~n0 +set APP_HOME=%DIRNAME% + +@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" + +@rem Find java.exe +if defined JAVA_HOME goto findJavaFromJavaHome + +set JAVA_EXE=java.exe +%JAVA_EXE% -version >NUL 2>&1 +if "%ERRORLEVEL%" == "0" goto init + +echo. +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:findJavaFromJavaHome +set JAVA_HOME=%JAVA_HOME:"=% +set JAVA_EXE=%JAVA_HOME%/bin/java.exe + +if exist "%JAVA_EXE%" goto init + +echo. +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:init +@rem Get command-line arguments, handling Windows variants + +if not "%OS%" == "Windows_NT" goto win9xME_args + +:win9xME_args +@rem Slurp the command line arguments. +set CMD_LINE_ARGS= +set _SKIP=2 + +:win9xME_args_slurp +if "x%~1" == "x" goto execute + +set CMD_LINE_ARGS=%* + +:execute +@rem Setup the command line + +set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar + +@rem Execute Gradle +"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS% + +:end +@rem End local scope for the variables with windows NT shell +if "%ERRORLEVEL%"=="0" goto mainEnd + +:fail +rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of +rem the _cmd.exe /c_ return code! +if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 +exit /b 1 + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/2021 test code/settings.gradle b/2021 test code/settings.gradle new file mode 100644 index 00000000..0bc697ad --- /dev/null +++ b/2021 test code/settings.gradle @@ -0,0 +1,27 @@ +import org.gradle.internal.os.OperatingSystem + +pluginManagement { + repositories { + mavenLocal() + gradlePluginPortal() + String frcYear = '2021' + File frcHome + if (OperatingSystem.current().isWindows()) { + String publicFolder = System.getenv('PUBLIC') + if (publicFolder == null) { + publicFolder = "C:\\Users\\Public" + } + def homeRoot = new File(publicFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } else { + def userFolder = System.getProperty("user.home") + def homeRoot = new File(userFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } + def frcHomeMaven = new File(frcHome, 'maven') + maven { + name 'frcHome' + url frcHomeMaven + } + } +} diff --git a/2021 test code/src/main/cpp/Controls.cpp b/2021 test code/src/main/cpp/Controls.cpp new file mode 100644 index 00000000..768c6627 --- /dev/null +++ b/2021 test code/src/main/cpp/Controls.cpp @@ -0,0 +1,58 @@ +#include "Controls.h" + +Controls::Controls(controlMethod setMethod) : method{setMethod}, gamecube{setMethod - 1}, lJoy{setMethod * 2}, rJoy{setMethod + 1} +{ + //I know this is a horrible solution but I couldn't find a better one while still + //using an enum and I'm the only one using the gamecube so it should be fine +} + +/** + * gets the input based on control method for throttle + */ +double Controls::throttle() +{ + if(method == controlMethod::gamecubeController) + { + double value = -gamecube.GetRawAxis(1); + if (value > -0.08 || value < -0.28) + { + return value; + } else {return 0;} + }else{ return lJoy.GetY(); } +} +/** + * gets the input based on control method for turn + */ +double Controls::turn() +{ + if(method == controlMethod::gamecubeController) + { + double value = -gamecube.GetRawAxis(0); + if (value > 0.15 || value < -0.15) + { + return value; + } else {return 0;} + }else{ return -rJoy.GetX(); } + +} + +bool Controls::revShooter() { return lJoy.GetTrigger(); } //unfished for gamecube +bool Controls::shoot() { return rJoy.GetTrigger(); } //unfished for gamecube + +int Controls::intake() +{ + return lJoy.GetRawButton(3) - rJoy.GetRawButton(2); //unfished for gamecube +} +int Controls::deploy() { return lJoy.GetRawButton(1) - rJoy.GetRawButton(1); } //unfished for gamecube + +bool Controls::spinWheel() { return rJoy.GetRawButton(3); } //unfished for gamecube + +/* + +trigger 0 (check numbers) +bottom 1 +left 2 +right 3 +other stuff + +*/ \ No newline at end of file diff --git a/2021 test code/src/main/cpp/Drive.cpp b/2021 test code/src/main/cpp/Drive.cpp new file mode 100644 index 00000000..6f2d74d4 --- /dev/null +++ b/2021 test code/src/main/cpp/Drive.cpp @@ -0,0 +1,28 @@ +#include "Drive.h" + +/** + * sets direction and slave master relationships + */ +Drive::Drive() +{ + leftM.SetInverted(TalonFXInvertType::CounterClockwise); + rightM.SetInverted(TalonFXInvertType::CounterClockwise); + leftS.SetInverted(TalonFXInvertType::FollowMaster); + rightS.SetInverted(TalonFXInvertType::CounterClockwise); + + leftS.Follow(leftM); + rightS.Follow(rightM); +}; + +/** + * sets neutral mode and drives the robot + */ +void Drive::periodic(Controls& controls) +{ + leftM.SetNeutralMode(Brake); + leftS.SetNeutralMode(Brake); + rightM.SetNeutralMode(Brake); + rightS.SetNeutralMode(Brake); + driveSet.ArcadeDrive(controls.throttle(), controls.turn(), false); + +}; diff --git a/2021 test code/src/main/cpp/Intake.cpp b/2021 test code/src/main/cpp/Intake.cpp new file mode 100644 index 00000000..53f1cb0f --- /dev/null +++ b/2021 test code/src/main/cpp/Intake.cpp @@ -0,0 +1,57 @@ +#include "Intake.h" + +Intake::Intake() +{ + +} + +/** + * right no just calls intake function + */ +void Intake::periodic(Controls &controls) +{ + intake(controls); + + /*switch(state) + { + case State::Intaking: + intake(); + break; + case State::Unjamming: + unjam(); + break; + case State::Idle: + intakeMotor.Set(ControlMode::PercentOutput, 0); + break; + }*/ +} + +/** + * sets the desired state for the intake + */ +void Intake::setState(Intake::State setState) +{ + state = setState; +} + +/** + * sets the intake's motor speed based on control inputs, calls deploy function + */ +void Intake::intake(Controls &controls) +{ + deploy(controls); + intakeMotor.Set(ControlMode::PercentOutput, controls.intake() * 0.25); //test powers +} + +/** + * there is no motor or pneumatic to deploy the intake right now + */ +void Intake::deploy(Controls &controls) +{ + if(deployed || controls.deploy() == 0) + return; + //do something with pneumatics or motors + deployed = true; + + +} \ No newline at end of file diff --git a/2021 test code/src/main/cpp/Robot.cpp b/2021 test code/src/main/cpp/Robot.cpp new file mode 100644 index 00000000..fd038045 --- /dev/null +++ b/2021 test code/src/main/cpp/Robot.cpp @@ -0,0 +1,76 @@ +#include "Robot.h" +#include +#include + +/** + * nothing, mostly just use constructor instead + */ +void Robot::RobotInit() {} + +/** + * initiate robot, call subsystems' constructors + */ +Robot::Robot() : frc::TimedRobot{}, controls{Controls::controlMethod::joysticks}, drive{}, intake{} +{ + +} + +/** + * This function is called every robot packet, no matter the mode. Use + * this for items like diagnostics that you want ran during disabled, + * autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ +void Robot::RobotPeriodic() +{ + +} + +/** + * This autonomous (along with the chooser code above) shows how to select + * between different autonomous modes using the dashboard. The sendable chooser + * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard, + * remove all of the chooser code and uncomment the GetString line to get the + * auto name from the text box below the Gyro. + * + * You can add additional auto modes by adding additional comparisons to the + * if-else structure below with additional strings. If using the SendableChooser + * make sure to add them to the chooser code above as well. + */ +void Robot::AutonomousInit() {} +void Robot::AutonomousPeriodic() {} + +void Robot::TeleopInit() {} +/** + * call subsystem periodics, probably other stuff later on + */ +void Robot::TeleopPeriodic() +{ + drive.periodic(controls); + intake.periodic(controls); +} + +/** + * probably nothing, disabled shouldn't do anything + */ +void Robot::DisabledInit() {} +/** + * probably nothing, don't want anybody dying when it's disabled + */ +void Robot::DisabledPeriodic() {} + +/** + * nothing, just test with main ones + */ +void Robot::TestInit() {} +/** + * nothing, just test with main ones + */ +void Robot::TestPeriodic() {} + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif + diff --git a/2021 test code/src/main/deploy/example.txt b/2021 test code/src/main/deploy/example.txt new file mode 100644 index 00000000..68395391 --- /dev/null +++ b/2021 test code/src/main/deploy/example.txt @@ -0,0 +1,4 @@ +Files placed in this directory will be deployed to the RoboRIO into the + 'deploy' directory in the home folder. Use the 'frc::filesystem::GetDeployDirectory' + function from the 'frc/Filesystem.h' header to get a proper path relative to the deploy + directory. \ No newline at end of file diff --git a/2021 test code/src/main/include/Controls.h b/2021 test code/src/main/include/Controls.h new file mode 100644 index 00000000..52c97978 --- /dev/null +++ b/2021 test code/src/main/include/Controls.h @@ -0,0 +1,33 @@ +#pragma once +#include +#include "iostream" + +class Controls +{ + public: + enum controlMethod + { + joysticks = 0, + gamecubeController = 1 + }; + + Controls(controlMethod method); + + double throttle(); + double turn(); + + bool revShooter(); + bool shoot(); + + int intake(); + int deploy(); + + bool spinWheel(); + + private: + controlMethod method; + frc::Joystick gamecube; + frc::Joystick lJoy; + frc::Joystick rJoy; + +}; diff --git a/2021 test code/src/main/include/Drive.h b/2021 test code/src/main/include/Drive.h new file mode 100644 index 00000000..27feea9c --- /dev/null +++ b/2021 test code/src/main/include/Drive.h @@ -0,0 +1,48 @@ +#pragma once + +#include "ctre/Phoenix.h" +#include "frc/WPILib.h" +#include +#include +#include +#include +#include +#include +#include "Controls.h" +#include +#include +#include +#include +#include "iostream" + +class Drive +{ + public: + Drive(); + void periodic(Controls& controls); + + private: + WPI_TalonFX &leftM = *new WPI_TalonFX(22); + WPI_TalonFX &leftS = *new WPI_TalonFX(21); + WPI_TalonFX &rightM = *new WPI_TalonFX(23); + WPI_TalonFX &rightS = *new WPI_TalonFX(24); + /* + 25 - left flywheel + 26 - right flywheel + + 2 - kicker + 52 - hood + 20 - intake + 31 - wheel thing (not connected) + 42 - connected to nothing (farthest towards the intake/right) + 43 - serializer front (motor not connected) + 44 - second section of serializer (motor not connected) + 19 - connected to nothing + + */ + + frc::SpeedControllerGroup leftGroup{leftM, leftS}; + frc::SpeedControllerGroup rightGroup{rightM, rightS}; + frc::DifferentialDrive driveSet{leftGroup, rightGroup}; + +}; diff --git a/2021 test code/src/main/include/Intake.h b/2021 test code/src/main/include/Intake.h new file mode 100644 index 00000000..2a02c923 --- /dev/null +++ b/2021 test code/src/main/include/Intake.h @@ -0,0 +1,31 @@ +#pragma once + +#include "ctre/Phoenix.h" +#include "frc/WPILib.h" +#include + +#include "Controls.h" + +class Intake +{ + public: + enum State //Here for if it is needed in the future, no use for it right now + { + Idle, + Intaking, + Unjamming + }; + + Intake(); + void periodic(Controls &controls); + void setState(State state); + + void intake(Controls &controls); + void deploy(Controls &controls); + + private: + WPI_TalonSRX &intakeMotor = *new WPI_TalonSRX(20); + State state; + bool deployed = false; + +}; \ No newline at end of file diff --git a/2021 test code/src/main/include/Robot.h b/2021 test code/src/main/include/Robot.h new file mode 100644 index 00000000..098d33db --- /dev/null +++ b/2021 test code/src/main/include/Robot.h @@ -0,0 +1,42 @@ +#pragma once + +#include + +#include +#include +#include + +#include "Controls.h" +#include "Drive.h" +#include "Intake.h" + +class Robot : public frc::TimedRobot { + public: + + Robot(); + + void RobotInit() override; + void RobotPeriodic() override; + + void AutonomousInit() override; + void AutonomousPeriodic() override; + + void TeleopInit() override; + void TeleopPeriodic() override; + + void DisabledInit() override; + void DisabledPeriodic() override; + + void TestInit() override; + void TestPeriodic() override; + + private: + /*frc::SendableChooser m_chooser; + const std::string kAutoNameDefault = "Default"; + const std::string kAutoNameCustom = "My Auto"; + std::string m_autoSelected;*/ + + Controls controls; + Drive drive; + Intake intake; +}; diff --git a/2021 test code/src/test/cpp/main.cpp b/2021 test code/src/test/cpp/main.cpp new file mode 100644 index 00000000..b8b23d23 --- /dev/null +++ b/2021 test code/src/test/cpp/main.cpp @@ -0,0 +1,10 @@ +#include + +#include "gtest/gtest.h" + +int main(int argc, char** argv) { + HAL_Initialize(500, 0); + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + return ret; +} diff --git a/2021 test code/vendordeps/Phoenix.json b/2021 test code/vendordeps/Phoenix.json new file mode 100644 index 00000000..87f03cbf --- /dev/null +++ b/2021 test code/vendordeps/Phoenix.json @@ -0,0 +1,214 @@ +{ + "fileName": "Phoenix.json", + "name": "CTRE-Phoenix", + "version": "5.19.4", + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://devsite.ctr-electronics.com/maven/release/" + ], + "jsonUrl": "https://devsite.ctr-electronics.com/maven/release/com/ctre/phoenix/Phoenix-latest.json", + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.19.4" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.19.4" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.19.4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.19.4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simTalonSRX", + "version": "5.19.4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simVictorSPX", + "version": "5.19.4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.19.4", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.19.4", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.19.4", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.19.4", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "diagnostics", + "version": "5.19.4", + "libName": "CTRE_PhoenixDiagnostics", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "canutils", + "version": "5.19.4", + "libName": "CTRE_PhoenixCanutils", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "platform-sim", + "version": "5.19.4", + "libName": "CTRE_PhoenixPlatform", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "core", + "version": "5.19.4", + "libName": "CTRE_PhoenixCore", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simTalonSRX", + "version": "5.19.4", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simVictorSPX", + "version": "5.19.4", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + } + ] +} \ No newline at end of file diff --git a/2021 test code/vendordeps/WPILibNewCommands.json b/2021 test code/vendordeps/WPILibNewCommands.json new file mode 100644 index 00000000..83de291e --- /dev/null +++ b/2021 test code/vendordeps/WPILibNewCommands.json @@ -0,0 +1,37 @@ +{ + "fileName": "WPILibNewCommands.json", + "name": "WPILib-New-Commands", + "version": "2020.0.0", + "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-cpp", + "version": "wpilib", + "libName": "wpilibNewCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian", + "linuxaarch64bionic", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxx86-64" + ] + } + ] +} \ No newline at end of file diff --git a/2021 test code/vendordeps/WPILibOldCommands.json b/2021 test code/vendordeps/WPILibOldCommands.json new file mode 100644 index 00000000..7bdad212 --- /dev/null +++ b/2021 test code/vendordeps/WPILibOldCommands.json @@ -0,0 +1,37 @@ +{ + "fileName": "WPILibOldCommands.json", + "name": "WPILib-Old-Commands", + "version": "2020.0.0", + "uuid": "b066afc2-5c18-43c4-b758-43381fcb275e", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.wpilibOldCommands", + "artifactId": "wpilibOldCommands-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.wpilibOldCommands", + "artifactId": "wpilibOldCommands-cpp", + "version": "wpilib", + "libName": "wpilibOldCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian", + "linuxaarch64bionic", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxx86-64" + ] + } + ] +} diff --git a/2021 test code/vendordeps/navx_frc.json b/2021 test code/vendordeps/navx_frc.json new file mode 100644 index 00000000..ad23f3b9 --- /dev/null +++ b/2021 test code/vendordeps/navx_frc.json @@ -0,0 +1,35 @@ +{ + "fileName": "navx_frc.json", + "name": "KauaiLabs_navX_FRC", + "version": "4.0.428", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "mavenUrls": [ + "https://repo1.maven.org/maven2/" + ], + "jsonUrl": "https://www.kauailabs.com/dist/frc/2021/navx_frc.json", + "javaDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-java", + "version": "4.0.428" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-cpp", + "version": "4.0.428", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": false, + "libName": "navx_frc", + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian", + "windowsx86-64" + ] + } + ] +} \ No newline at end of file From ccc686edaa6f2c3d82b3e5b754060d12046d6cfc Mon Sep 17 00:00:00 2001 From: Alex Chang Date: Wed, 13 Oct 2021 18:45:02 -0700 Subject: [PATCH 4/4] added aiming and shooting basics --- 2021 test code/src/main/cpp/Channel.cpp | 17 ++++ 2021 test code/src/main/cpp/Controls.cpp | 41 +++++++--- 2021 test code/src/main/cpp/Drive.cpp | 8 +- 2021 test code/src/main/cpp/Intake.cpp | 21 ++++- 2021 test code/src/main/cpp/Limelight.cpp | 21 +++++ 2021 test code/src/main/cpp/Robot.cpp | 40 +++++++++- 2021 test code/src/main/cpp/Shoot.cpp | 87 +++++++++++++++++++++ 2021 test code/src/main/include/Channel.h | 20 +++++ 2021 test code/src/main/include/Controls.h | 2 +- 2021 test code/src/main/include/Drive.h | 3 + 2021 test code/src/main/include/Intake.h | 5 +- 2021 test code/src/main/include/Limelight.h | 19 +++++ 2021 test code/src/main/include/Robot.h | 5 ++ 2021 test code/src/main/include/Shoot.h | 44 +++++++++++ 14 files changed, 311 insertions(+), 22 deletions(-) create mode 100644 2021 test code/src/main/cpp/Channel.cpp create mode 100644 2021 test code/src/main/cpp/Limelight.cpp create mode 100644 2021 test code/src/main/cpp/Shoot.cpp create mode 100644 2021 test code/src/main/include/Channel.h create mode 100644 2021 test code/src/main/include/Limelight.h create mode 100644 2021 test code/src/main/include/Shoot.h diff --git a/2021 test code/src/main/cpp/Channel.cpp b/2021 test code/src/main/cpp/Channel.cpp new file mode 100644 index 00000000..59c2c2b0 --- /dev/null +++ b/2021 test code/src/main/cpp/Channel.cpp @@ -0,0 +1,17 @@ +#include "Channel.h" + +Channel::Channel() +{ + +} + +void Channel::periodic(Controls &controls) +{ + run(controls); +} + +void Channel::run(Controls &controls) +{ + frontMotor.Set(ControlMode::PercentOutput, controls.intake() * -0.65); //test values + sideMotor.Set(ControlMode::PercentOutput, controls.intake() * -0.65); //test values, figure out sensor thing +} \ No newline at end of file diff --git a/2021 test code/src/main/cpp/Controls.cpp b/2021 test code/src/main/cpp/Controls.cpp index 768c6627..e85d0482 100644 --- a/2021 test code/src/main/cpp/Controls.cpp +++ b/2021 test code/src/main/cpp/Controls.cpp @@ -18,7 +18,13 @@ double Controls::throttle() { return value; } else {return 0;} - }else{ return lJoy.GetY(); } + }else if (method == controlMethod::joysticks) + { + return lJoy.GetY(); + }else + { + return 0; + } } /** * gets the input based on control method for turn @@ -32,27 +38,42 @@ double Controls::turn() { return value; } else {return 0;} - }else{ return -rJoy.GetX(); } + }else if(method == controlMethod::joysticks) + { + return -rJoy.GetX(); + }else + { + return 0; + } } -bool Controls::revShooter() { return lJoy.GetTrigger(); } //unfished for gamecube +bool Controls::readyShot() { return lJoy.GetTrigger(); } //unfished for gamecube bool Controls::shoot() { return rJoy.GetTrigger(); } //unfished for gamecube int Controls::intake() { - return lJoy.GetRawButton(3) - rJoy.GetRawButton(2); //unfished for gamecube + if(method == controlMethod::gamecubeController) + { + return gamecube.GetRawButton(1) - gamecube.GetRawButton(2); + }else if(method == controlMethod::joysticks) + { + return lJoy.GetRawButton(3) - rJoy.GetRawButton(4); + }else + { + return 0; + } } -int Controls::deploy() { return lJoy.GetRawButton(1) - rJoy.GetRawButton(1); } //unfished for gamecube +int Controls::deploy() { return lJoy.GetRawButton(2) - rJoy.GetRawButton(2); } //unfished for gamecube, probably don't need -bool Controls::spinWheel() { return rJoy.GetRawButton(3); } //unfished for gamecube +bool Controls::spinWheel() { return rJoy.GetRawButton(4); } //unfished for gamecube /* -trigger 0 (check numbers) -bottom 1 -left 2 -right 3 +trigger 1 (check numbers) +bottom 2 +left 3 +right 4 other stuff */ \ No newline at end of file diff --git a/2021 test code/src/main/cpp/Drive.cpp b/2021 test code/src/main/cpp/Drive.cpp index 6f2d74d4..0959722e 100644 --- a/2021 test code/src/main/cpp/Drive.cpp +++ b/2021 test code/src/main/cpp/Drive.cpp @@ -12,7 +12,7 @@ Drive::Drive() leftS.Follow(leftM); rightS.Follow(rightM); -}; +} /** * sets neutral mode and drives the robot @@ -25,4 +25,8 @@ void Drive::periodic(Controls& controls) rightS.SetNeutralMode(Brake); driveSet.ArcadeDrive(controls.throttle(), controls.turn(), false); -}; +} + +void Drive::drive(double throttle, double turn) { driveSet.ArcadeDrive(throttle, turn, false); } +void Drive::autoDrive() { driveSet.ArcadeDrive(0.2, 0.0, false); } +void Drive::stop() { driveSet.ArcadeDrive(0, 0, false); } \ No newline at end of file diff --git a/2021 test code/src/main/cpp/Intake.cpp b/2021 test code/src/main/cpp/Intake.cpp index 53f1cb0f..f638f958 100644 --- a/2021 test code/src/main/cpp/Intake.cpp +++ b/2021 test code/src/main/cpp/Intake.cpp @@ -39,19 +39,32 @@ void Intake::setState(Intake::State setState) */ void Intake::intake(Controls &controls) { - deploy(controls); - intakeMotor.Set(ControlMode::PercentOutput, controls.intake() * 0.25); //test powers + if(controls.intake() != 0) + { + deploy(); + }else + { + retract(); + } + intakeMotor.Set(ControlMode::PercentOutput, controls.intake() * 0.65); //test powers } /** * there is no motor or pneumatic to deploy the intake right now */ -void Intake::deploy(Controls &controls) +void Intake::deploy() { - if(deployed || controls.deploy() == 0) + if(isDeployed()/* || controls.deploy() == 0*/) return; //do something with pneumatics or motors deployed = true; +} +void Intake::retract() +{ + if(!isDeployed()) + return; + //do something with pneumatics or motors + deployed = false; } \ No newline at end of file diff --git a/2021 test code/src/main/cpp/Limelight.cpp b/2021 test code/src/main/cpp/Limelight.cpp new file mode 100644 index 00000000..4bfe2833 --- /dev/null +++ b/2021 test code/src/main/cpp/Limelight.cpp @@ -0,0 +1,21 @@ +#include "Limelight.h" + +Limelight::Limelight() +{ + table = nt::NetworkTableInstance::GetDefault().GetTable("limelight"); +} + +double Limelight::getX() { return table->GetNumber("tx", 100); } +double Limelight::getY() { return table->GetNumber("ty", 100); } + +double Limelight::getDistance() +{ + //zero x? + double angle, y; + if((y = getY()) == 100) + { + return -1; + } + angle = y + VERTICAL_ANGLE_OFFSET; + return GOAL_HEIGHT / tan(angle); +} \ No newline at end of file diff --git a/2021 test code/src/main/cpp/Robot.cpp b/2021 test code/src/main/cpp/Robot.cpp index fd038045..fa218a42 100644 --- a/2021 test code/src/main/cpp/Robot.cpp +++ b/2021 test code/src/main/cpp/Robot.cpp @@ -10,7 +10,7 @@ void Robot::RobotInit() {} /** * initiate robot, call subsystems' constructors */ -Robot::Robot() : frc::TimedRobot{}, controls{Controls::controlMethod::joysticks}, drive{}, intake{} +Robot::Robot() : frc::TimedRobot{}, timer{}, controls{Controls::controlMethod::gamecubeController}, drive{}, intake{}, shoot{} { } @@ -39,17 +39,49 @@ void Robot::RobotPeriodic() * if-else structure below with additional strings. If using the SendableChooser * make sure to add them to the chooser code above as well. */ -void Robot::AutonomousInit() {} -void Robot::AutonomousPeriodic() {} +void Robot::AutonomousInit() +{ + timer.Reset(); + timer.Start(); +} +void Robot::AutonomousPeriodic() +{ + if (timer.Get() < 1) + { + drive.autoDrive(); + }else + { + drive.stop(); + } + +} void Robot::TeleopInit() {} /** * call subsystem periodics, probably other stuff later on */ -void Robot::TeleopPeriodic() +void Robot::TeleopPeriodic() //I do like using a controls class, but passing it each time feels not right, is there a way to make like a global controls class { + timer.Stop(); drive.periodic(controls); intake.periodic(controls); + channel.periodic(controls); + + Shoot::state state = shoot.periodic(controls); + switch(state) + { + case Shoot::idle: + shoot.stop(); + break; + case Shoot::aiming: //something about overriding drive? + drive.drive(0, shoot.horizontalAim()); + //something with vertical hood + break; + case Shoot::shooting: + shoot.shootShot(); + break; + } + } /** diff --git a/2021 test code/src/main/cpp/Shoot.cpp b/2021 test code/src/main/cpp/Shoot.cpp new file mode 100644 index 00000000..94a380e7 --- /dev/null +++ b/2021 test code/src/main/cpp/Shoot.cpp @@ -0,0 +1,87 @@ +#include "Shoot.h" + +Shoot::Shoot() : limelight{} +{ + leftFly.SetInverted(TalonFXInvertType::Clockwise); //TODO check for legit safety reasons + rightFly.SetInverted(TalonFXInvertType::CounterClockwise); + + //shootS.Follow(shootM); +} + +Shoot::state Shoot::periodic(Controls &controls) +{ + if(controls.readyShot()) + { + if(controls.shoot()) + { + currentState = Shoot::shooting; + }else + { + currentState = Shoot::aiming; + } + }else + { + currentState = Shoot::idle; + } + + return currentState; + +} + +void Shoot::spinUp(double speed) +{ + + flywheelGroup.Set(speed); //PID? +} + +void Shoot::stop() +{ + flywheelGroup.Set(0); + kicker.Set(0); + //something with hood +} + +void Shoot::shootShot() +{ + kicker.Set(ControlMode::PercentOutput, 0.45); //TODO test value +} + +double Shoot::horizontalAim() //TODO something with not finding a target +{ + double prevError, deltaError; + + prevError += hError; + hError = limelight.getX(); + if( abs(hError) < 0.5 ) //TODO test this value, also is this need if I have a PID? I just don't think I can perfectly fix oscilations + { + return 0; + } + deltaError = hError - prevError; + + double power = (aimTKp * hError) + (aimTKi * prevError) + (aimTKd * deltaError); + + if (power < -0.85) power = -0.85; + if (power > 0.85) power = 0.85; + + return power; +} + +double Shoot::verticalAim()//TODO very unfinished +{ + double prevError, deltaError; + + prevError += vError; + //vError = something idk + if( abs(vError) < 0.5 ) + { + return 0; + } + deltaError = vError - prevError; + + double power = (aimTKp * vError) + (aimTKi * prevError) + (aimTKd * deltaError); + + if (power < -0.85) power = -0.85; + if (power > 0.85) power = 0.85; + + return power; +} \ No newline at end of file diff --git a/2021 test code/src/main/include/Channel.h b/2021 test code/src/main/include/Channel.h new file mode 100644 index 00000000..23f7c07e --- /dev/null +++ b/2021 test code/src/main/include/Channel.h @@ -0,0 +1,20 @@ +#pragma once + +#include "ctre/Phoenix.h" +#include "frc/WPILib.h" +#include + +#include "Controls.h" + +class Channel +{ + public: + Channel(); + void periodic(Controls &controls); + void run(Controls &controls); + + private: + WPI_TalonSRX &frontMotor = *new WPI_TalonSRX(43); + WPI_TalonSRX &sideMotor = *new WPI_TalonSRX(44); + +}; \ No newline at end of file diff --git a/2021 test code/src/main/include/Controls.h b/2021 test code/src/main/include/Controls.h index 52c97978..6a88ea4e 100644 --- a/2021 test code/src/main/include/Controls.h +++ b/2021 test code/src/main/include/Controls.h @@ -16,7 +16,7 @@ class Controls double throttle(); double turn(); - bool revShooter(); + bool readyShot(); bool shoot(); int intake(); diff --git a/2021 test code/src/main/include/Drive.h b/2021 test code/src/main/include/Drive.h index 27feea9c..d6b2c849 100644 --- a/2021 test code/src/main/include/Drive.h +++ b/2021 test code/src/main/include/Drive.h @@ -20,6 +20,9 @@ class Drive public: Drive(); void periodic(Controls& controls); + void autoDrive(); + void drive(double throttle, double turn); + void stop(); private: WPI_TalonFX &leftM = *new WPI_TalonFX(22); diff --git a/2021 test code/src/main/include/Intake.h b/2021 test code/src/main/include/Intake.h index 2a02c923..c5f4fc34 100644 --- a/2021 test code/src/main/include/Intake.h +++ b/2021 test code/src/main/include/Intake.h @@ -21,7 +21,10 @@ class Intake void setState(State state); void intake(Controls &controls); - void deploy(Controls &controls); + void deploy(); + void retract(); + + bool isDeployed() {return deployed; }; private: WPI_TalonSRX &intakeMotor = *new WPI_TalonSRX(20); diff --git a/2021 test code/src/main/include/Limelight.h b/2021 test code/src/main/include/Limelight.h new file mode 100644 index 00000000..e95bb23b --- /dev/null +++ b/2021 test code/src/main/include/Limelight.h @@ -0,0 +1,19 @@ +#include "networktables/NetworkTable.h" +#include "networktables/NetworkTableInstance.h" +#include + +class Limelight{ + public: + Limelight(); + double getX(); + double getY(); + double getDistance(); + + const double VERTICAL_ANGLE_OFFSET = 0; //TODO measure values + const double VERTICAL_HEIGHT_OFFSET = 0; + const double GOAL_HEIGHT = 0; + + private: + std::shared_ptr table; //TODO figure out what shared pointer is + +}; \ No newline at end of file diff --git a/2021 test code/src/main/include/Robot.h b/2021 test code/src/main/include/Robot.h index 098d33db..007f13e4 100644 --- a/2021 test code/src/main/include/Robot.h +++ b/2021 test code/src/main/include/Robot.h @@ -9,6 +9,8 @@ #include "Controls.h" #include "Drive.h" #include "Intake.h" +#include "Channel.h" +#include "Shoot.h" class Robot : public frc::TimedRobot { public: @@ -36,7 +38,10 @@ class Robot : public frc::TimedRobot { const std::string kAutoNameCustom = "My Auto"; std::string m_autoSelected;*/ + frc::Timer timer; Controls controls; Drive drive; Intake intake; + Channel channel; + Shoot shoot; }; diff --git a/2021 test code/src/main/include/Shoot.h b/2021 test code/src/main/include/Shoot.h new file mode 100644 index 00000000..8d46dc18 --- /dev/null +++ b/2021 test code/src/main/include/Shoot.h @@ -0,0 +1,44 @@ +#pragma once + +#include +#include +#include +#include "ctre/Phoenix.h" +#include "frc/WPILib.h" +#include + +#include "Limelight.h" +#include "Controls.h" + +class Shoot +{ + public: + enum state + { + aiming, shooting, idle + }; + Shoot(); + state periodic(Controls &controls); + void spinUp(double speed); + void shootShot(); + void stop(); + double horizontalAim(); + double verticalAim(); + + const double aimTKi = 0.0; //TODO testing at some point + const double aimTKp = 0.0; + const double aimTKd = 0.0; + double hError = 0.0; + double vError = 0.0; + + private: + Limelight limelight; + + WPI_TalonSRX &kicker = *new WPI_TalonSRX(2); + WPI_TalonFX &leftFly = *new WPI_TalonFX(25); + WPI_TalonFX &rightFly = *new WPI_TalonFX(26); + + frc::SpeedControllerGroup flywheelGroup{leftFly, rightFly}; //hopefully they can go different directions + + state currentState; +}; \ No newline at end of file