From 326f7cd7cc879bc0e50025c551ccabda3f324917 Mon Sep 17 00:00:00 2001 From: prak132 Date: Fri, 7 Nov 2025 20:01:54 -0800 Subject: [PATCH 1/2] Tested reconfiguration of escs on reset --- src/frc846/cpp/frc846/control/MotorMonkey.cc | 61 ++++++++++++++----- .../control/hardware/SparkMXFX_interm.cc | 9 +++ .../frc846/control/hardware/TalonFX_interm.cc | 6 ++ src/frc846/cpp/frc846/robot/GenericRobot.cc | 6 ++ .../include/frc846/control/MotorMonkey.h | 9 +++ .../control/hardware/IntermediateController.h | 2 + .../control/hardware/SparkMXFX_interm.h | 21 ++++++- .../frc846/control/hardware/TalonFX_interm.h | 17 ++++++ .../control/hardware/simulation/MCSimulator.h | 4 ++ .../include/frc846/robot/GenericRobot.h | 2 + .../hardware/algal/algal_end_effector.cc | 31 +--------- .../hardware/algal/algal_end_effector.h | 2 - 12 files changed, 122 insertions(+), 48 deletions(-) diff --git a/src/frc846/cpp/frc846/control/MotorMonkey.cc b/src/frc846/cpp/frc846/control/MotorMonkey.cc index b49f6021..9293af79 100644 --- a/src/frc846/cpp/frc846/control/MotorMonkey.cc +++ b/src/frc846/cpp/frc846/control/MotorMonkey.cc @@ -71,6 +71,9 @@ units::newton_meter_t MotorMonkey::load_registry[CONTROLLER_REGISTRY_SIZE]{}; frc846::wpilib::unit_ohm MotorMonkey::circuit_resistance_registry[CONTROLLER_REGISTRY_SIZE]{}; +frc846::control::config::MotorConstructionParameters + MotorMonkey::config_registry_[CONTROLLER_REGISTRY_SIZE]{}; + units::volt_t MotorMonkey::battery_voltage{0_V}; units::volt_t MotorMonkey::last_disabled_voltage{0_V}; @@ -327,6 +330,47 @@ bool MotorMonkey::VerifyConnected() { return true; } +void MotorMonkey::ConfigureESC(size_t slot_id, + frc846::control::config::MotorConstructionParameters params, + bool first_time) { + frc846::control::hardware::IntermediateController* this_controller = nullptr; + this_controller = controller_registry[slot_id]; + + if (!first_time) { + SMART_RETRY(this_controller->SetGains(gains_registry[slot_id]), "SetGains"); + LOG_IF_ERROR("SetGains"); + } + + SMART_RETRY(this_controller->SetInverted(params.inverted), "SetInverted"); + LOG_IF_ERROR("SetInverted"); + + SMART_RETRY( + this_controller->SetNeutralMode(params.brake_mode), "SetNeutralMode"); + LOG_IF_ERROR("SetNeutralMode"); + + SMART_RETRY(this_controller->SetCurrentLimit(params.motor_current_limit), + "SetCurrentLimit"); + LOG_IF_ERROR("SetCurrentLimit"); + + SMART_RETRY( + this_controller->SetVoltageCompensation(params.voltage_compensation), + "SetSoftLimits"); + LOG_IF_ERROR("SetVoltageCompensation") + + this_controller->ClearFaults(); +} + +void MotorMonkey::CheckAndHandleResets() { + for (size_t i = 0; i < CONTROLLER_REGISTRY_SIZE; i++) { + if (controller_registry[i] != nullptr) { + if (controller_registry[i]->ResetHasOccured()) { + ConfigureESC(i, config_registry_[i], false); + loggable_.Warn("ESC has reset for slot ID {}", i); + } + } + } +} + size_t MotorMonkey::ConstructController( frc846::control::base::MotorMonkeyType type, frc846::control::config::MotorConstructionParameters params) { @@ -336,6 +380,7 @@ size_t MotorMonkey::ConstructController( slot_id_to_type_[slot_id] = type; slot_id_to_sim_[slot_id] = false; circuit_resistance_registry[slot_id] = params.circuit_resistance; + config_registry_[slot_id] = params; frc846::control::hardware::IntermediateController* this_controller = nullptr; @@ -372,21 +417,7 @@ size_t MotorMonkey::ConstructController( if (this_controller == nullptr) { return slot_id; } - SMART_RETRY(this_controller->SetInverted(params.inverted), "SetInverted"); - LOG_IF_ERROR("SetInverted"); - - SMART_RETRY( - this_controller->SetNeutralMode(params.brake_mode), "SetNeutralMode"); - LOG_IF_ERROR("SetNeutralMode"); - - SMART_RETRY(this_controller->SetCurrentLimit(params.motor_current_limit), - "SetCurrentLimit"); - LOG_IF_ERROR("SetCurrentLimit"); - - SMART_RETRY( - this_controller->SetVoltageCompensation(params.voltage_compensation), - "SetSoftLimits"); - LOG_IF_ERROR("SetVoltageCompensation") + ConfigureESC(slot_id, params, true); return slot_id; } diff --git a/src/frc846/cpp/frc846/control/hardware/SparkMXFX_interm.cc b/src/frc846/cpp/frc846/control/hardware/SparkMXFX_interm.cc index b82b3159..e5c7df60 100644 --- a/src/frc846/cpp/frc846/control/hardware/SparkMXFX_interm.cc +++ b/src/frc846/cpp/frc846/control/hardware/SparkMXFX_interm.cc @@ -29,6 +29,12 @@ bool SparkMXFX_interm::VerifyConnected() { return esc_->GetFirmwareVersion() != 0; } +bool SparkMXFX_interm::ResetHasOccured() { + return esc_->GetStickyWarnings().hasReset; +} + +void SparkMXFX_interm::ClearFaults() { set_last_error(esc_->ClearFaults()); } + SparkMXFX_interm::SparkMXFX_interm(int can_id, units::millisecond_t max_wait_time, bool is_controller_spark_flex) : can_id_{can_id} { @@ -191,6 +197,9 @@ void SparkMXFX_interm::EnableStatusFrames( configs.absoluteEncoder.SetSparkMaxDataPortConfig(); } + // reset frame + configs.signals.WarningsAlwaysOn(false); + APPLY_CONFIG_NO_RESET(); } diff --git a/src/frc846/cpp/frc846/control/hardware/TalonFX_interm.cc b/src/frc846/cpp/frc846/control/hardware/TalonFX_interm.cc index 18c2d25d..99c74fb2 100644 --- a/src/frc846/cpp/frc846/control/hardware/TalonFX_interm.cc +++ b/src/frc846/cpp/frc846/control/hardware/TalonFX_interm.cc @@ -4,6 +4,12 @@ namespace frc846::control::hardware { bool TalonFX_interm::VerifyConnected() { return talon_.IsAlive(); } +bool TalonFX_interm::ResetHasOccured() { return talon_.HasResetOccurred(); } + +void TalonFX_interm::ClearFaults() { + last_error_ = getErrorCode(talon_.ClearStickyFaults()); +} + TalonFX_interm::TalonFX_interm( int can_id, std::string_view bus, units::millisecond_t max_wait_time) : talon_(can_id, bus), max_wait_time_(max_wait_time) {} diff --git a/src/frc846/cpp/frc846/robot/GenericRobot.cc b/src/frc846/cpp/frc846/robot/GenericRobot.cc index e377d166..ef944c52 100644 --- a/src/frc846/cpp/frc846/robot/GenericRobot.cc +++ b/src/frc846/cpp/frc846/robot/GenericRobot.cc @@ -200,6 +200,12 @@ void GenericRobot::StartCompetition() { update_tick_counter_ = 0; } + // Check all ESCs + if (++check_escs_count_ >= 500) { + frc846::control::MotorMonkey::CheckAndHandleResets(); + check_escs_count_ = 0; + } + // Update graphs // Graph("time_remaining", frc::DriverStation::GetMatchTime().to()); // Graph("mode", static_cast(mode)); diff --git a/src/frc846/include/frc846/control/MotorMonkey.h b/src/frc846/include/frc846/control/MotorMonkey.h index 94cae4ff..144058f1 100644 --- a/src/frc846/include/frc846/control/MotorMonkey.h +++ b/src/frc846/include/frc846/control/MotorMonkey.h @@ -139,8 +139,14 @@ class MotorMonkey { static std::string_view parseError( frc846::control::hardware::ControllerErrorCodes err); + static void ConfigureESC(size_t slot_id, + frc846::control::config::MotorConstructionParameters params, + bool first_time); + static bool VerifyConnected(); + static void CheckAndHandleResets(); + private: static void RecalculateMaxDraw(); @@ -161,6 +167,9 @@ class MotorMonkey { static frc846::wpilib::unit_ohm circuit_resistance_registry[CONTROLLER_REGISTRY_SIZE]; + static frc846::control::config::MotorConstructionParameters + config_registry_[CONTROLLER_REGISTRY_SIZE]; + static units::volt_t battery_voltage; static units::volt_t last_disabled_voltage; diff --git a/src/frc846/include/frc846/control/hardware/IntermediateController.h b/src/frc846/include/frc846/control/hardware/IntermediateController.h index 05f36dbc..4ecf317e 100644 --- a/src/frc846/include/frc846/control/hardware/IntermediateController.h +++ b/src/frc846/include/frc846/control/hardware/IntermediateController.h @@ -85,6 +85,8 @@ class IntermediateController { virtual ControllerErrorCodes GetLastErrorCode() = 0; virtual bool VerifyConnected() = 0; + virtual bool ResetHasOccured() = 0; + virtual void ClearFaults() = 0; }; } // namespace frc846::control::hardware \ No newline at end of file diff --git a/src/frc846/include/frc846/control/hardware/SparkMXFX_interm.h b/src/frc846/include/frc846/control/hardware/SparkMXFX_interm.h index 2fd1af71..a326ffcf 100644 --- a/src/frc846/include/frc846/control/hardware/SparkMXFX_interm.h +++ b/src/frc846/include/frc846/control/hardware/SparkMXFX_interm.h @@ -169,11 +169,28 @@ class SparkMXFX_interm : public IntermediateController { /* VerifyHardware() - Checks the TalonFX hardware and verifies it is functioning correctly. + Checks the Rev hardware and verifies it is functioning correctly. @return boolean indicating whether the hardware is verified successfully. */ - bool VerifyConnected() override; // added + bool VerifyConnected() override; + + /* + ResetHasOccured() + + Checks the Rev hardware for if it has reset. + + @return boolean indicating whether the hardware has reset. + */ + bool ResetHasOccured() override; + + /* + ClearFaults() + + Clears all faults from the hardware. + + */ + void ClearFaults() override; /* GetVelocity() diff --git a/src/frc846/include/frc846/control/hardware/TalonFX_interm.h b/src/frc846/include/frc846/control/hardware/TalonFX_interm.h index fd1f916c..a0b641fa 100644 --- a/src/frc846/include/frc846/control/hardware/TalonFX_interm.h +++ b/src/frc846/include/frc846/control/hardware/TalonFX_interm.h @@ -172,6 +172,23 @@ class TalonFX_interm : public IntermediateController { */ bool VerifyConnected() override; // added + /* + ResetHasOccured() + + Checks the TalonFX for if it has reset. + + @return boolean indicating whether the hardware has reset. + */ + bool ResetHasOccured() override; + + /* + ClearFaults() + + Clears all faults from the hardware. + + */ + void ClearFaults() override; + /* GetVelocity() diff --git a/src/frc846/include/frc846/control/hardware/simulation/MCSimulator.h b/src/frc846/include/frc846/control/hardware/simulation/MCSimulator.h index 3fb30986..1539a416 100644 --- a/src/frc846/include/frc846/control/hardware/simulation/MCSimulator.h +++ b/src/frc846/include/frc846/control/hardware/simulation/MCSimulator.h @@ -36,6 +36,10 @@ class MCSimulator : public frc846::control::hardware::IntermediateController { bool VerifyConnected() override { return true; } + bool ResetHasOccured() override { return false; } + + void ClearFaults() override {} + frc846::control::hardware::ControllerErrorCodes GetLastErrorCode() override { return frc846::control::hardware::ControllerErrorCodes::kAllOK; } diff --git a/src/frc846/include/frc846/robot/GenericRobot.h b/src/frc846/include/frc846/robot/GenericRobot.h index 010565dc..96e3face 100644 --- a/src/frc846/include/frc846/robot/GenericRobot.h +++ b/src/frc846/include/frc846/robot/GenericRobot.h @@ -55,6 +55,8 @@ class GenericRobot : public frc::RobotBase, public frc846::base::Loggable { std::unordered_map autos_; int update_tick_counter_ = 0; + + int check_escs_count_{0}; }; } // namespace frc846::robot diff --git a/src/y2025/cpp/subsystems/hardware/algal/algal_end_effector.cc b/src/y2025/cpp/subsystems/hardware/algal/algal_end_effector.cc index a9a0600d..9a6c5904 100644 --- a/src/y2025/cpp/subsystems/hardware/algal/algal_end_effector.cc +++ b/src/y2025/cpp/subsystems/hardware/algal/algal_end_effector.cc @@ -22,14 +22,10 @@ AlgalEESubsystem::AlgalEESubsystem() esc_2_{frc846::control::base::SPARK_MAX_NEO550, (GetCurrentConfig(GetModifiedConfig(motor_configs_, ports::algal_ss_::end_effector_::kEE2_CANID, true)))} { - // RegisterPreference("idle_speed", 0.025); RegisterPreference("idle_speed", 2.3_fps); - // RegisterPreference("piece_thresh", 2_tps); RegisterPreference("piece_thresh", 1.0_fps); - // RegisterPreference("kick_dc", -0.2); RegisterPreference("kick_dc", -18.5_fps); - // RegisterPreference("backspin_constant", -0.24); RegisterPreference("backspin_constant", -22_fps); RegisterPreference("idle_speed_coral", -5.0_fps); RegisterPreference("coral_vel_thresh", 5_fps); @@ -101,32 +97,17 @@ AlgalEEReadings AlgalEESubsystem::ReadFromHardware() { } void AlgalEESubsystem::WriteToHardware(AlgalEETarget target) { - // Graph("target/duty_cycle", target.duty_cycle_); - Graph("readings/error", target.velocity_ - esc_helper_2_.GetVelocity()); - Graph("readings/error_coral", target.velocity_ - esc_helper_1_.GetVelocity()); Graph("readings/vel1", esc_helper_1_.GetVelocity()); Graph("readings/vel2", esc_helper_2_.GetVelocity()); - frc846::control::base::MotorGains gains; - gains.kP = GetPreferenceValue_double("gains/_kP"); - gains.kFF = GetPreferenceValue_double("gains/_kF"); - gains.kD = 0.0; - gains.kI = 0.0; - - pleasefixthis += 1; - if (pleasefixthis > 50) { gains.kFF += 0.000003; } - if (pleasefixthis == 100) { pleasefixthis = 0; } - - esc_1_.SetGains(gains); - esc_2_.SetGains(gains); + esc_1_.SetGains(GET_PIDF_GAINS()); + esc_2_.SetGains(GET_PIDF_GAINS()); target.coral_keep &= target.cmode; - // auto checkgains = frc846::control::base::MotorGains(GET_PIDF_GAINS()); - if (GetReadings().has_piece_ && target.velocity_ > 0.0_fps && !target.coral_keep) { target.velocity_ = @@ -163,14 +144,6 @@ void AlgalEESubsystem::WriteToHardware(AlgalEETarget target) { if (target.cmode && has_coral_piece_) { target.velocity_ = 0_fps; } - // if (units::math::abs(esc_helper_2_.GetVelocity()) <= - // GetPreferenceValue_unit_type( - // "piece_thresh") && - // target.velocity_ < 0.0_fps) { - // target.velocity_ = - // GetPreferenceValue_unit_type("kick_dc"); - // } - if (piece_override_) { target.velocity_ = 0.0_fps; } if (target.super_mode) { esc_helper_1_.WriteVelocityOnController( diff --git a/src/y2025/include/subsystems/hardware/algal/algal_end_effector.h b/src/y2025/include/subsystems/hardware/algal/algal_end_effector.h index 63ef63ed..a3b90f23 100644 --- a/src/y2025/include/subsystems/hardware/algal/algal_end_effector.h +++ b/src/y2025/include/subsystems/hardware/algal/algal_end_effector.h @@ -69,8 +69,6 @@ class AlgalEESubsystem double counter_ = 0; - int pleasefixthis = 0; - AlgalEEReadings ReadFromHardware() override; void WriteToHardware(AlgalEETarget target) override; From 7cfe2ef58b41b7b7c9c914802b66fe6e1bb4515f Mon Sep 17 00:00:00 2001 From: prak132 Date: Tue, 11 Nov 2025 14:58:50 -0800 Subject: [PATCH 2/2] Post season clean up * Made drive exponent a double * Reverted antitipping calculator * Cleaned up comments/iostream --- networktables.json.bck | 133 +- networktables_.json.bck | 2795 ----------------- src/frc846/cpp/frc846/control/MotorMonkey.cc | 12 +- .../cpp/frc846/control/config/soft_limits.cc | 12 +- .../hardware/simulation/MCSimulator.cc | 22 +- .../robot/calculators/AprilTagCalculator.cc | 85 - .../cpp/frc846/robot/swerve/drivetrain.cc | 6 +- src/y2025/cpp/FunkyRobot.cc | 28 - .../cpp/calculators/AntiTippingCalculator.cc | 9 +- .../commands/general/reef_auto_auto_align.cc | 2 - .../cpp/commands/teleop/algal_command.cc | 27 - .../cpp/commands/teleop/drive_command.cc | 35 +- .../commands/teleop/drive_to_reef_command.cc | 18 - .../cpp/commands/teleop/gpd_ss_command.cc | 4 - src/y2025/cpp/commands/teleop/l1_align.cc | 2 - .../commands/teleop/lock_to_reef_command.cc | 12 - .../cpp/commands/teleop/reef_auto_align.cc | 2 - src/y2025/cpp/control_triggers.cc | 2 +- .../cpp/subsystems/abstract/control_input.cc | 4 +- src/y2025/cpp/subsystems/abstract/gpd.cc | 2 - .../hardware/DrivetrainConstructor.cc | 7 +- src/y2025/cpp/subsystems/hardware/FMS.cc | 12 - .../hardware/algal/algal_end_effector.cc | 6 - .../cpp/subsystems/hardware/algal/algal_ss.cc | 8 - .../subsystems/hardware/algal/algal_wrist.cc | 12 - .../cpp/subsystems/hardware/algal/elevator.cc | 38 +- .../hardware/coral/coral_end_effector.cc | 1 - .../cpp/subsystems/hardware/coral/coral_ss.cc | 1 - .../subsystems/hardware/coral/coral_wrist.cc | 21 +- .../hardware/generic/linear_subsystem.cc | 15 - .../hardware/generic/wrist_subsystem.cc | 11 +- src/y2025/include/subsystems/hardware/FMS.h | 19 - .../include/subsystems/robot_constants.h | 3 - .../include/subsystems/robot_container.h | 31 - 34 files changed, 138 insertions(+), 3259 deletions(-) delete mode 100644 networktables_.json.bck delete mode 100644 src/y2025/cpp/subsystems/hardware/FMS.cc delete mode 100644 src/y2025/include/subsystems/hardware/FMS.h diff --git a/networktables.json.bck b/networktables.json.bck index d6c51b16..105baf3d 100644 --- a/networktables.json.bck +++ b/networktables.json.bck @@ -74,7 +74,7 @@ { "name": "/Preferences/SwerveDrivetrain/FR/cancoder_offset_ (deg)", "type": "double", - "value": -74.7, + "value": -72.158203125, "properties": { "persistent": true } @@ -82,7 +82,7 @@ { "name": "/Preferences/SwerveDrivetrain/FL/cancoder_offset_ (deg)", "type": "double", - "value": -128.67, + "value": -129.462890625, "properties": { "persistent": true } @@ -90,7 +90,7 @@ { "name": "/Preferences/SwerveDrivetrain/BL/cancoder_offset_ (deg)", "type": "double", - "value": -80.33203125, + "value": -80.595703125, "properties": { "persistent": true } @@ -98,7 +98,7 @@ { "name": "/Preferences/SwerveDrivetrain/BR/cancoder_offset_ (deg)", "type": "double", - "value": -79.013671875, + "value": -79.27734375, "properties": { "persistent": true } @@ -252,7 +252,8 @@ "type": "double", "value": 15.0, "properties": { - "persistent": true + "persistent": true, + "retained": true } }, { @@ -274,7 +275,7 @@ { "name": "/Preferences/SwerveDrivetrain/odom_fudge_factor", "type": "double", - "value": 1.077, + "value": 0.95, "properties": { "persistent": true } @@ -450,7 +451,7 @@ { "name": "/Preferences/GPD/intake_to_cam_y (in)", "type": "double", - "value": 0.0, + "value": -20.0, "properties": { "persistent": true } @@ -458,7 +459,7 @@ { "name": "/Preferences/GPD/intake_to_cam_x (in)", "type": "double", - "value": -13.0, + "value": -8.0, "properties": { "persistent": true } @@ -474,7 +475,7 @@ { "name": "/Preferences/GPD/max_gp_diff (in)", "type": "double", - "value": 10.0, + "value": 6.0, "properties": { "persistent": true } @@ -1122,7 +1123,7 @@ { "name": "/Preferences/algal_wrist/motor_configs/current_limit (A)", "type": "double", - "value": 50.0, + "value": 30.0, "properties": { "persistent": true } @@ -1162,7 +1163,7 @@ { "name": "/Preferences/algal_wrist/gains/_kD", "type": "double", - "value": -0.0006, + "value": -0.0004, "properties": { "persistent": true } @@ -1210,7 +1211,7 @@ { "name": "/Preferences/algal_wrist/limits/reduce_lower (deg)", "type": "double", - "value": 10.0, + "value": 80.0, "properties": { "persistent": true } @@ -1250,7 +1251,7 @@ { "name": "/Preferences/algal_wrist/encoder_offset (deg)", "type": "double", - "value": 3.8, + "value": -9.0, "properties": { "persistent": true } @@ -1258,7 +1259,7 @@ { "name": "/Preferences/algal_end_effector/motor_configs/current_limit (A)", "type": "double", - "value": 25.0, + "value": 17.0, "properties": { "persistent": true } @@ -1298,7 +1299,7 @@ { "name": "/Preferences/algal_ss/setpoints/stow/angle (deg)", "type": "double", - "value": 9.0, + "value": 10.0, "properties": { "persistent": true } @@ -1346,7 +1347,7 @@ { "name": "/Preferences/algal_ss/setpoints/ground_intake/angle (deg)", "type": "double", - "value": 62.0, + "value": 56.0, "properties": { "persistent": true } @@ -1354,7 +1355,7 @@ { "name": "/Preferences/algal_ss/setpoints/ground_intake/ee_dc", "type": "double", - "value": 0.9, + "value": 1.0, "properties": { "persistent": true } @@ -1378,7 +1379,7 @@ { "name": "/Preferences/algal_ss/setpoints/on_top_intake/ee_dc", "type": "double", - "value": 0.9, + "value": 1.0, "properties": { "persistent": true } @@ -1434,7 +1435,7 @@ { "name": "/Preferences/algal_ss/setpoints/l3_pick/height (in)", "type": "double", - "value": 47.5, + "value": 48.0, "properties": { "persistent": true } @@ -1578,7 +1579,7 @@ { "name": "/Preferences/climber/limits/upper_limit (deg)", "type": "double", - "value": 460.0, + "value": 270.0, "properties": { "persistent": true } @@ -1586,7 +1587,7 @@ { "name": "/Preferences/climber/limits/lower_limit (deg)", "type": "double", - "value": -255.0, + "value": -118.0, "properties": { "persistent": true } @@ -1842,7 +1843,7 @@ { "name": "/Preferences/coral_ss/setpoints/stow_net/height (in)", "type": "double", - "value": 40.0, + "value": 72.0, "properties": { "persistent": true } @@ -2522,7 +2523,7 @@ { "name": "/Preferences/algal_end_effector/idle_speed (fps)", "type": "double", - "value": 6.875, + "value": 10.0, "properties": { "persistent": true } @@ -2538,7 +2539,7 @@ { "name": "/Preferences/algal_end_effector/kick_dc (fps)", "type": "double", - "value": -30.0, + "value": -80.0, "properties": { "persistent": true } @@ -2546,7 +2547,7 @@ { "name": "/Preferences/algal_end_effector/backspin_constant (fps)", "type": "double", - "value": -55.0, + "value": -50.0, "properties": { "persistent": true } @@ -2554,7 +2555,7 @@ { "name": "/Preferences/algal_end_effector/gains/_kP", "type": "double", - "value": 0.0001, + "value": 3e-05, "properties": { "persistent": true } @@ -2586,7 +2587,7 @@ { "name": "/Preferences/algal_ss/setpoints/stow/ee_vel (fps)", "type": "double", - "value": 5.0, + "value": 7.75, "properties": { "persistent": true } @@ -2666,7 +2667,39 @@ { "name": "/Preferences/algal_ss/score_dc (fps)", "type": "double", - "value": -49.5, + "value": -60.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/latency", + "type": "int", + "value": 0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/projection_horizon", + "type": "int", + "value": 1, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/telescope/duty_cycle_output", + "type": "double", + "value": 0.03, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/coral_ss/has_peicething", + "type": "boolean", + "value": false, "properties": { "persistent": true } @@ -2674,7 +2707,7 @@ { "name": "/Preferences/algal_ss/setpoints/coral_pick/height (in)", "type": "double", - "value": 29.0, + "value": 35.0, "properties": { "persistent": true } @@ -2682,7 +2715,7 @@ { "name": "/Preferences/algal_ss/setpoints/coral_pick/angle (deg)", "type": "double", - "value": 80.0, + "value": 135.0, "properties": { "persistent": true } @@ -2690,7 +2723,7 @@ { "name": "/Preferences/algal_ss/setpoints/coral_pick/ee_vel (fps)", "type": "double", - "value": 64.15, + "value": -30.0, "properties": { "persistent": true } @@ -2698,7 +2731,7 @@ { "name": "/Preferences/algal_ss/setpoints/l1_coral_score/height (in)", "type": "double", - "value": 29.0, + "value": 31.5, "properties": { "persistent": true } @@ -2706,7 +2739,7 @@ { "name": "/Preferences/algal_ss/setpoints/l1_coral_score/angle (deg)", "type": "double", - "value": 60.0, + "value": 50.0, "properties": { "persistent": true } @@ -2714,7 +2747,7 @@ { "name": "/Preferences/algal_ss/setpoints/l1_coral_score/ee_vel (fps)", "type": "double", - "value": -20.15, + "value": -9.2, "properties": { "persistent": true } @@ -2722,7 +2755,15 @@ { "name": "/Preferences/algal_ss/score_coral_vel (fps)", "type": "double", - "value": -16.5, + "value": 40.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/algal_end_effector/testthing", + "type": "double", + "value": -0.4, "properties": { "persistent": true } @@ -2730,15 +2771,31 @@ { "name": "/Preferences/algal_end_effector/idle_speed_coral (fps)", "type": "double", - "value": -5.0, + "value": -8.0, "properties": { "persistent": true } }, { - "name": "/Preferences/algal_end_effector/testthing", + "name": "/Preferences/algal_end_effector/test1", + "type": "boolean", + "value": false, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/algal_end_effector/coral_vel_thresh (fps)", "type": "double", - "value": 0.4, + "value": 5.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/algal_end_effector/max_eject (fps)", + "type": "double", + "value": -40.0, "properties": { "persistent": true } diff --git a/networktables_.json.bck b/networktables_.json.bck deleted file mode 100644 index 35dbf7fe..00000000 --- a/networktables_.json.bck +++ /dev/null @@ -1,2795 +0,0 @@ -[ - { - "name": "/Preferences/Robot/update_tick_1", - "type": "int", - "value": 100, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Robot/update_tick_2", - "type": "int", - "value": 201, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Robot/update_reset_tick", - "type": "int", - "value": 200, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/DrivetrainConstructor/drive_motor_current_limit (A)", - "type": "double", - "value": 160.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/DrivetrainConstructor/steer_motor_current_limit (A)", - "type": "double", - "value": 120.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/DrivetrainConstructor/drive_motor_smart_current_limit (A)", - "type": "double", - "value": 120.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/DrivetrainConstructor/steer_motor_smart_current_limit (A)", - "type": "double", - "value": 80.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/DrivetrainConstructor/drive_motor_voltage_compensation (V)", - "type": "double", - "value": 16.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/DrivetrainConstructor/steer_motor_voltage_compensation (V)", - "type": "double", - "value": 10.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/FR/cancoder_offset_ (deg)", - "type": "double", - "value": -73.037109375, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/FL/cancoder_offset_ (deg)", - "type": "double", - "value": -128.408203125, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/BL/cancoder_offset_ (deg)", - "type": "double", - "value": -80.068359375, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/BR/cancoder_offset_ (deg)", - "type": "double", - "value": -78.75, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/steer_gains/_kP", - "type": "double", - "value": 2.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/steer_gains/_kI", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/steer_gains/_kD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/steer_gains/_kF", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/bearing_gains/_kP", - "type": "int", - "value": 9, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/bearing_gains/_kI", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/bearing_gains/_kD", - "type": "double", - "value": -0.6, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/bearing_gains/deadband (deg_per_s)", - "type": "double", - "value": 3.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/lock_gains/_kP", - "type": "double", - "value": -0.7, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/lock_gains/_kI", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/lock_gains/_kD", - "type": "double", - "value": 0.036, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/lock_gains/deadband (in)", - "type": "double", - "value": 0.5, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/lock_adj_rate (in)", - "type": "double", - "value": 0.05, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/lock_max_speed (fps)", - "type": "double", - "value": 9.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/auto_max_speed (fps)", - "type": "double", - "value": 15.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/drive_to_subtract (in)", - "type": "double", - "value": 2.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/april_bearing_latency (ms)", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/drive_latency (ms)", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/max_speed (fps)", - "type": "double", - "value": 15.0, - "properties": { - "persistent": true, - "retained": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/max_omega (deg_per_s)", - "type": "double", - "value": 180.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/max_omega_cut (deg_per_s)", - "type": "double", - "value": 40.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/odom_fudge_factor", - "type": "double", - "value": 0.95, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/odom_variance", - "type": "double", - "value": 0.2, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/steer_lag (s)", - "type": "double", - "value": 0.05, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/bearing_latency (s)", - "type": "double", - "value": 0.01, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/pose_estimator/pose_variance", - "type": "double", - "value": 0.1, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/pose_estimator/velocity_variance", - "type": "double", - "value": 1.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/pose_estimator/accel_variance", - "type": "double", - "value": 1.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/pose_estimator/override", - "type": "boolean", - "value": false, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/april_tags/april_variance_coeff", - "type": "double", - "value": 0.000139, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/april_tags/triangular_variance_coeff", - "type": "double", - "value": 0.0005, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/april_tags/fudge_latency1 (ms)", - "type": "double", - "value": 155.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/april_tags/fudge_latency2 (ms)", - "type": "double", - "value": 70.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/rc_control_speed (fps)", - "type": "double", - "value": 2.5, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/accel_spike_thresh (fps_sq)", - "type": "double", - "value": 45.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/max_past_accel_spike", - "type": "int", - "value": 25, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/accel_vel_stopped_thresh (fps)", - "type": "double", - "value": 0.7, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/vel_stopped_thresh (fps)", - "type": "double", - "value": 0.7, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/stopped_num_loops", - "type": "int", - "value": 25, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/lock_drive_early (in)", - "type": "double", - "value": 12.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/lock_drive_fvel (fps)", - "type": "double", - "value": 1.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/drive_correctional_gain", - "type": "double", - "value": 0.2, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/override_at_auto", - "type": "boolean", - "value": false, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/GPD/intake_to_cam_y (in)", - "type": "double", - "value": -20.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/GPD/intake_to_cam_x (in)", - "type": "double", - "value": -8.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/GPD/cam_h_angle (deg)", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/GPD/max_gp_diff (in)", - "type": "double", - "value": 6.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/GPD/use_diff_thresh (in)", - "type": "double", - "value": 40.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/telescope/motor_configs/current_limit (A)", - "type": "double", - "value": 60.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/telescope/motor_configs/smart_current_limit (A)", - "type": "double", - "value": 60.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/telescope/pidf_deadband (in)", - "type": "double", - "value": 0.35, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/telescope/gains/_kP", - "type": "double", - "value": 0.026, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/telescope/gains/_kI", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/telescope/gains/_kD", - "type": "double", - "value": -0.0015, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/telescope/gains/_kF", - "type": "double", - "value": 0.0295, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/telescope/limits/use_limits", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/telescope/limits/upper_limit (in)", - "type": "double", - "value": 91.25, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/telescope/limits/lower_limit (in)", - "type": "double", - "value": 28.5, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/telescope/limits/reduce_upper (in)", - "type": "double", - "value": 89.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/telescope/limits/reduce_lower (in)", - "type": "double", - "value": 40.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/telescope/limits/reduce_max_dc", - "type": "double", - "value": 0.45, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_wrist/motor_configs/current_limit (A)", - "type": "double", - "value": 80.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_wrist/motor_configs/smart_current_limit (A)", - "type": "double", - "value": 60.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_wrist/rezero_thresh (deg)", - "type": "double", - "value": 0.25, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_wrist/gains/_kP", - "type": "double", - "value": 0.0036, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_wrist/gains/_kI", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_wrist/gains/_kD", - "type": "double", - "value": -1e-05, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_wrist/gains/_kF", - "type": "double", - "value": 0.01, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_wrist/limits/use_limits", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_wrist/limits/upper_limit (deg)", - "type": "double", - "value": 245.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_wrist/limits/lower_limit (deg)", - "type": "double", - "value": 20.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_wrist/limits/reduce_upper (deg)", - "type": "double", - "value": 212.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_wrist/limits/reduce_lower (deg)", - "type": "double", - "value": 80.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_wrist/limits/reduce_max_dc", - "type": "double", - "value": 0.4, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_wrist/cg_offset (deg)", - "type": "double", - "value": -90.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_wrist/flip_position_load_sign", - "type": "boolean", - "value": false, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_wrist/use_sensor_threshold (deg_per_s)", - "type": "double", - "value": 250.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_wrist/encoder_offset (deg)", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_end_effector/motor_configs/current_limit (A)", - "type": "double", - "value": 40.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_end_effector/motor_configs/smart_current_limit (A)", - "type": "double", - "value": 30.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_end_effector/idle_speed", - "type": "double", - "value": -0.05, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/stow_no_piece/height (in)", - "type": "double", - "value": 28.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/stow_no_piece/angle (deg)", - "type": "double", - "value": 24.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/stow_no_piece/ee_dc", - "type": "double", - "value": -0.7, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/stow_with_piece/height (in)", - "type": "double", - "value": 40.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/stow_with_piece/angle (deg)", - "type": "double", - "value": 150.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/stow_with_piece/ee_dc", - "type": "double", - "value": -0.15, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/score_l2/height (in)", - "type": "double", - "value": 27.75, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/score_l2/angle (deg)", - "type": "double", - "value": 200.5, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/score_l2/ee_dc", - "type": "double", - "value": -0.15, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/score_l3/height (in)", - "type": "double", - "value": 49.25, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/score_l3/angle (deg)", - "type": "double", - "value": 200.5, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/score_l3/ee_dc", - "type": "double", - "value": -0.15, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/score_l4/height (in)", - "type": "double", - "value": 90.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/score_l4/angle (deg)", - "type": "double", - "value": 241.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/score_l4/ee_dc", - "type": "double", - "value": -0.15, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/dinosaur_A/height (in)", - "type": "double", - "value": 33.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/dinosaur_A/angle (deg)", - "type": "double", - "value": 140.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/dinosaur_A/ee_dc", - "type": "double", - "value": -0.3, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/dinosaur_B/height (in)", - "type": "double", - "value": 50.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/dinosaur_B/angle (deg)", - "type": "double", - "value": 200.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/dinosaur_B/ee_dc", - "type": "double", - "value": 0.3, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/score_dc", - "type": "double", - "value": 1.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/init_telescope", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/init_wrist", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/init_ee", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/disable_distance_sensor", - "type": "boolean", - "value": false, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/telescope_tolerance (in)", - "type": "double", - "value": 9.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/wrist_tolerance (deg)", - "type": "double", - "value": 15.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/telescope_adjustment (in)", - "type": "double", - "value": 0.14, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/wrist_adjustment (deg)", - "type": "double", - "value": 0.7, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/autostow", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/stow_no_piece_loop_thresh", - "type": "int", - "value": 22, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/see_reef_loop_thresh", - "type": "int", - "value": 3, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/elevator/motor_configs/current_limit (A)", - "type": "double", - "value": 60.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/elevator/motor_configs/smart_current_limit (A)", - "type": "double", - "value": 80.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/elevator/pidf_deadband (in)", - "type": "double", - "value": 0.25, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/elevator/gains/_kP", - "type": "double", - "value": 0.01, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/elevator/gains/_kI", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/elevator/gains/_kD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/elevator/gains/_kF", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/elevator/limits/use_limits", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/elevator/limits/upper_limit (in)", - "type": "double", - "value": 74.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/elevator/limits/lower_limit (in)", - "type": "double", - "value": 29.12, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/elevator/limits/reduce_upper (in)", - "type": "double", - "value": 65.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/elevator/limits/reduce_lower (in)", - "type": "double", - "value": 90.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/elevator/limits/reduce_max_dc", - "type": "double", - "value": 0.8, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_wrist/motor_configs/current_limit (A)", - "type": "double", - "value": 30.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_wrist/motor_configs/smart_current_limit (A)", - "type": "double", - "value": 40.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_wrist/rezero_thresh (deg)", - "type": "double", - "value": 2.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_wrist/gains/_kP", - "type": "double", - "value": 0.005, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_wrist/gains/_kI", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_wrist/gains/_kD", - "type": "double", - "value": -0.0004, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_wrist/gains/_kF", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_wrist/limits/use_limits", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_wrist/limits/upper_limit (deg)", - "type": "double", - "value": 150.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_wrist/limits/lower_limit (deg)", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_wrist/limits/reduce_upper (deg)", - "type": "double", - "value": 80.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_wrist/limits/reduce_lower (deg)", - "type": "double", - "value": 80.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_wrist/limits/reduce_max_dc", - "type": "double", - "value": 0.3, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_wrist/cg_offset (deg)", - "type": "double", - "value": 90.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_wrist/flip_position_load_sign", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_wrist/use_sensor_threshold (deg_per_s)", - "type": "double", - "value": 5.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_wrist/encoder_offset (deg)", - "type": "double", - "value": -9.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_end_effector/motor_configs/current_limit (A)", - "type": "double", - "value": 17.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_end_effector/motor_configs/smart_current_limit (A)", - "type": "double", - "value": 20.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_end_effector/idle_speed", - "type": "double", - "value": 0.025, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_end_effector/piece_thresh (tps)", - "type": "double", - "value": 2.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/stow/height (in)", - "type": "double", - "value": 30.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/stow/angle (deg)", - "type": "double", - "value": 10.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/stow/ee_dc", - "type": "double", - "value": 0.2, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/processor/height (in)", - "type": "double", - "value": 29.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/processor/angle (deg)", - "type": "double", - "value": 24.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/processor/ee_dc", - "type": "double", - "value": 0.2, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/ground_intake/height (in)", - "type": "double", - "value": 29.5, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/ground_intake/angle (deg)", - "type": "double", - "value": 56.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/ground_intake/ee_dc", - "type": "double", - "value": 1.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/on_top_intake/height (in)", - "type": "double", - "value": 29.5, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/on_top_intake/angle (deg)", - "type": "double", - "value": 25.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/on_top_intake/ee_dc", - "type": "double", - "value": 1.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/net/height (in)", - "type": "double", - "value": 72.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/net/angle (deg)", - "type": "double", - "value": 3.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/net/ee_dc", - "type": "double", - "value": 0.2, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/l2_pick/height (in)", - "type": "double", - "value": 39.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/l2_pick/angle (deg)", - "type": "double", - "value": 36.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/l2_pick/ee_dc", - "type": "double", - "value": 0.7, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/l3_pick/height (in)", - "type": "double", - "value": 48.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/l3_pick/angle (deg)", - "type": "double", - "value": 36.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/l3_pick/ee_dc", - "type": "double", - "value": 0.7, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/dinosaur_A/height (in)", - "type": "double", - "value": 34.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/dinosaur_A/angle (deg)", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/dinosaur_A/ee_dc", - "type": "double", - "value": -0.3, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/dinosaur_B/height (in)", - "type": "double", - "value": 45.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/dinosaur_B/angle (deg)", - "type": "double", - "value": 35.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/dinosaur_B/ee_dc", - "type": "double", - "value": 0.3, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/score_dc", - "type": "double", - "value": -0.1, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/init_elevator", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/init_wrist", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/init_ee", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/elevator_tolerance (in)", - "type": "double", - "value": 5.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/wrist_tolerance (deg)", - "type": "double", - "value": 12.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/elevator_adjustment (in)", - "type": "double", - "value": 0.04, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/wrist_adjustment (deg)", - "type": "double", - "value": 0.2, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/climber/limits/use_limits", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/climber/limits/upper_limit (deg)", - "type": "double", - "value": 270.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/climber/limits/lower_limit (deg)", - "type": "double", - "value": -118.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/climber/limits/reduce_upper (deg)", - "type": "double", - "value": 90.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/climber/limits/reduce_lower (deg)", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/climber/limits/reduce_max_dc", - "type": "double", - "value": 1.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/climber/extend_dc", - "type": "double", - "value": 1.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/climber/retract_dc", - "type": "double", - "value": -1.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/robot_container/init_drivetrain", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/robot_container/init_leds", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/robot_container/init_gpd", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/robot_container/init_coral_ss", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/robot_container/init_algal_ss", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/robot_container/init_climber", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Robot/num_coasting_loops", - "type": "int", - "value": 1000, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Robot/homing_flash_loops", - "type": "int", - "value": 50, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Robot/elevator_home_height (in)", - "type": "double", - "value": 14.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Robot/telescope_home_height (in)", - "type": "double", - "value": 33.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/MotorMonkey/voltage_min (V)", - "type": "double", - "value": 8.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/MotorMonkey/recal_voltage_thresh (V)", - "type": "double", - "value": 10.5, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/MotorMonkey/default_max_draw (A)", - "type": "double", - "value": 150.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/MotorMonkey/min_max_draw (A)", - "type": "double", - "value": 60.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/MotorMonkey/max_max_draw (A)", - "type": "double", - "value": 250.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/MotorMonkey/battery_cc (A)", - "type": "double", - "value": 700.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/MotorMonkey/brownout_perm_loops", - "type": "int", - "value": 500, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/control_input/trigger_threshold", - "type": "double", - "value": 0.3, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/control_input/translation_deadband", - "type": "double", - "value": 0.07, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/control_input/translation_exponent", - "type": "int", - "value": 2, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/control_input/rotation_deadband", - "type": "double", - "value": 0.07, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/control_input/rotation_exponent", - "type": "int", - "value": 2, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/control_input/op_deadband", - "type": "double", - "value": 0.35, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/lock_gains/_kP_f", - "type": "double", - "value": -0.65, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_wrist/deployed_encoder_tolerance (deg)", - "type": "double", - "value": 0.35, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_end_effector/kick_dc", - "type": "double", - "value": -0.2, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/stow_net/height (in)", - "type": "double", - "value": 40.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/stow_net/angle (deg)", - "type": "double", - "value": 90.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/stow_net/ee_dc", - "type": "double", - "value": -0.15, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/flick/height (in)", - "type": "double", - "value": 29.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/flick/angle (deg)", - "type": "double", - "value": 115.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/setpoints/flick/ee_dc", - "type": "double", - "value": -0.7, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/elevator/hall_effect_plus_height (in)", - "type": "double", - "value": 1.7, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/elevator/hall_effect_counter", - "type": "int", - "value": 40, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/elevator/sm_target_range (in)", - "type": "double", - "value": 34.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/elevator/sm_vel_thresh (fps)", - "type": "double", - "value": 0.02, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/elevator/sm_current_thresh (A)", - "type": "double", - "value": 30.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/elevator/sm_counter", - "type": "int", - "value": 100, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/PathLogger/sampling_rate_ms", - "type": "int", - "value": 50, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/autoflick", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/no_piece_chute_loop_thresh", - "type": "int", - "value": 30, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/no_piece_chute_wait_loop_thresh", - "type": "int", - "value": -30, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/lock_net_adj_rate (in)", - "type": "double", - "value": 1.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/telescope/gains/kF_Upper", - "type": "double", - "value": 0.295, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/telescope/telescopel4_modifer_height (in)", - "type": "double", - "value": 70.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/telescope/telescopel4_load (Nm)", - "type": "double", - "value": 2.1, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/telescope/telescope_hysteresis", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/telescope/telescopel4_modifier_height (in)", - "type": "double", - "value": 70.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/drive_to_point/kC", - "type": "double", - "value": 3.5, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/drive_to_point/kA", - "type": "double", - "value": 0.05, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/drive_to_point/kE", - "type": "double", - "value": 2.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/drive_to_point/threshold (in)", - "type": "double", - "value": 3.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/net_inter/height (in)", - "type": "double", - "value": 35.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/net_inter/angle (deg)", - "type": "double", - "value": 40.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/net_inter/ee_dc", - "type": "double", - "value": 0.1, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/source_max_speed (fps)", - "type": "double", - "value": 7.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/use_source_assist", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_end_effector/backspin_constant", - "type": "double", - "value": -0.24, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/limits/use_limits_wrist", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/limits/upper_limit_wrist (deg)", - "type": "double", - "value": 140.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/limits/lower_limit_wrist (deg)", - "type": "double", - "value": -47.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/limits/reduce_upper_wrist (deg)", - "type": "double", - "value": 120.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/limits/reduce_lower_wrist (deg)", - "type": "double", - "value": -20.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/limits/reduce_max_dc_wrist", - "type": "double", - "value": 0.2, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/limits/use_limits_pivot", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/limits/upper_limit_pivot (deg)", - "type": "double", - "value": 90.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/limits/lower_limit_pivot (deg)", - "type": "double", - "value": -17.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/limits/reduce_upper_pivot (deg)", - "type": "double", - "value": 73.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/limits/reduce_lower_pivot (deg)", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/limits/reduce_max_dc_pivot", - "type": "double", - "value": 0.15, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/gains/_kP_wrist", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/gains/_kI_wrist", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/gains/_kD_wrist", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/gains/_kF_wrist", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/gains/_kP_pivot", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/gains/_kI_pivot", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/gains/_kD_pivot", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/gains/_kF_pivot", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/wrist_cg_offset (deg)", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/j1_stow (deg)", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/j2_stow (deg)", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/j1_intake (deg)", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/j2_intake (deg)", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/j1_amp (deg)", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/j2_amp (deg)", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/j1_shoot (deg)", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/arm_/j2_shoot (deg)", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/scorer_/motor_configs/current_limit (A)", - "type": "double", - "value": 80.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/scorer_/motor_configs/smart_current_limit (A)", - "type": "double", - "value": 30.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/scorer_/feed_speed (rad_per_s)", - "type": "double", - "value": -2000.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/scorer_/intake_speed (rad_per_s)", - "type": "double", - "value": -1000.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/scorer_/eject_speed (rad_per_s)", - "type": "double", - "value": 500.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/scorer_/idle_speed", - "type": "double", - "value": 0.1, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/scorer_/shoot_speed", - "type": "double", - "value": 0.5, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/scorer_/up_to_speed_thresh (rpm)", - "type": "double", - "value": 1000.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/scorer_/gains/_kP_intake", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/scorer_/gains/_kI_intake", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/scorer_/gains/_kD_intake", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/scorer_/gains/_kF_intake", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/robot_container/init_arm", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/robot_container/init_scorer", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/source_coast_threshold (in)", - "type": "double", - "value": 10.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/source_coast_speed (fps)", - "type": "double", - "value": 2.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/net_auto_align/prepoint (in)", - "type": "double", - "value": 65.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/SwerveDrivetrain/net_auto_align/scorepoint (in)", - "type": "double", - "value": 35.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/telescope/homing_dc", - "type": "double", - "value": -0.05, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/telescope/homing_thresh (fps)", - "type": "double", - "value": 0.05, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/telescope/homing_loops", - "type": "int", - "value": 25, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/elevator/homing_dc", - "type": "double", - "value": -0.05, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/elevator/homing_thresh (fps)", - "type": "double", - "value": 0.05, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/elevator/homing_loops", - "type": "int", - "value": 25, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_end_effector/idle_speed (fps)", - "type": "double", - "value": 10.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_end_effector/piece_thresh (fps)", - "type": "double", - "value": 3.14, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_end_effector/kick_dc (fps)", - "type": "double", - "value": -80.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_end_effector/backspin_constant (fps)", - "type": "double", - "value": -30.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_end_effector/gains/_kP", - "type": "double", - "value": 3e-05, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_end_effector/gains/_kI", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_end_effector/gains/_kD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_end_effector/gains/_kF", - "type": "double", - "value": 9.6e-05, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/stow/ee_vel (fps)", - "type": "double", - "value": 7.75, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/processor/ee_vel (fps)", - "type": "double", - "value": 55.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/ground_intake/ee_vel (fps)", - "type": "double", - "value": 80.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/on_top_intake/ee_vel (fps)", - "type": "double", - "value": 247.5, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/net/ee_vel (fps)", - "type": "double", - "value": 55.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/net_inter/ee_vel (fps)", - "type": "double", - "value": 27.5, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/l2_pick/ee_vel (fps)", - "type": "double", - "value": 192.5, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/l3_pick/ee_vel (fps)", - "type": "double", - "value": 192.5, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/dinosaur_A/ee_vel", - "type": "double", - "value": -82.5, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/dinosaur_B/ee_vel", - "type": "double", - "value": 82.5, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/score_dc (fps)", - "type": "double", - "value": -50.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/telescope/latency", - "type": "int", - "value": 0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/telescope/projection_horizon", - "type": "int", - "value": 1, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/telescope/duty_cycle_output", - "type": "double", - "value": 0.03, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/coral_ss/has_peicething", - "type": "boolean", - "value": false, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/coral_pick/height (in)", - "type": "double", - "value": 35.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/coral_pick/angle (deg)", - "type": "double", - "value": 135.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/coral_pick/ee_vel (fps)", - "type": "double", - "value": -30.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/l1_coral_score/height (in)", - "type": "double", - "value": 31.5, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/l1_coral_score/angle (deg)", - "type": "double", - "value": 50.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/setpoints/l1_coral_score/ee_vel (fps)", - "type": "double", - "value": -9.2, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_ss/score_coral_vel (fps)", - "type": "double", - "value": 40.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_end_effector/testthing", - "type": "double", - "value": -0.4, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_end_effector/idle_speed_coral (fps)", - "type": "double", - "value": -8.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_end_effector/test1", - "type": "boolean", - "value": false, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/algal_end_effector/coral_vel_thresh (fps)", - "type": "double", - "value": 5.0, - "properties": { - "persistent": true - } - } -] diff --git a/src/frc846/cpp/frc846/control/MotorMonkey.cc b/src/frc846/cpp/frc846/control/MotorMonkey.cc index 9293af79..d34f184f 100644 --- a/src/frc846/cpp/frc846/control/MotorMonkey.cc +++ b/src/frc846/cpp/frc846/control/MotorMonkey.cc @@ -160,8 +160,6 @@ void MotorMonkey::Tick(bool disabled) { } } - // TODO: cleanup iostream - units::ampere_t total_pred_draw = WriteMessages(max_draw_); loggable_.Graph("total_pred_draw", total_pred_draw); @@ -182,16 +180,8 @@ void MotorMonkey::Tick(bool disabled) { dynamic_cast(controller_registry[i]); sim->SetBatteryVoltage(battery_voltage); sim->SetLoad(load_registry[i]); - if (disabled) { - sim->WriteDC(0.0); - // std::cout << "MotorMonkey: Disabled, setting DC to 0.0" << - // std::endl; - } - - // std::cout << "MotorMonkey: Tick for slot ID " << i - // << ", sim: " << (sim != nullptr) << std::endl; + if (disabled) { sim->WriteDC(0.0); } sim->Tick(); - // std::cout << "--" << std::endl; } else { controller_registry[i]->Tick(); } diff --git a/src/frc846/cpp/frc846/control/config/soft_limits.cc b/src/frc846/cpp/frc846/control/config/soft_limits.cc index 87bb96a3..b6656591 100644 --- a/src/frc846/cpp/frc846/control/config/soft_limits.cc +++ b/src/frc846/cpp/frc846/control/config/soft_limits.cc @@ -10,17 +10,7 @@ SoftLimits::SoftLimits(bool using_limits, units::radian_t forward_limit, reverse_limit_(reverse_limit), forward_reduce_(forward_reduce), reverse_reduce_(reverse_reduce), - reduce_max_dc_(reduce_max_dc) { - // assert(forward_limit >= reverse_limit && - // "Forward limit must be greater than reverse limit"); - // assert(forward_reduce_ <= forward_limit_ && - // "Forward reduce must be less than forward limit"); - // assert(reverse_reduce_ >= reverse_limit_ && - // "Reverse reduce must be greater than reverse limit"); - // assert(forward_reduce_ >= reverse_reduce_ && - // "Forward reduce must be greater than reverse reduce"); - // assert(reduce_max_dc_ > 0 && "Reduce max DC must be greater than 0"); -} + reduce_max_dc_(reduce_max_dc) {} units::radian_t SoftLimits::LimitPosition(units::radian_t position) { if (using_limits_ && position < reverse_limit_) { diff --git a/src/frc846/cpp/frc846/control/hardware/simulation/MCSimulator.cc b/src/frc846/cpp/frc846/control/hardware/simulation/MCSimulator.cc index f8cff0a9..8f4b1ec6 100644 --- a/src/frc846/cpp/frc846/control/hardware/simulation/MCSimulator.cc +++ b/src/frc846/cpp/frc846/control/hardware/simulation/MCSimulator.cc @@ -2,8 +2,6 @@ #include -#include - #include "frc846/control/calculators/CurrentTorqueCalculator.h" #include "frc846/control/calculators/VelocityPositionEstimator.h" @@ -20,8 +18,6 @@ MCSimulator::MCSimulator(frc846::control::base::MotorSpecs specs, std::chrono::system_clock::now().time_since_epoch()); } -// TODO: cleanup iostream - void MCSimulator::Tick() { double duty_cycle = 0.0; if (auto* dc = std::get_if(&control_message)) { @@ -31,21 +27,11 @@ void MCSimulator::Tick() { duty_cycle = gains.calculate((vel->to() - velocity_.to()), 0.0, 0.0, vel->to()); } else if (auto* pos = std::get_if(&control_message)) { - // std::cout << "Error: " << (pos->to() - position_.to()) - // << std::endl; - // std::cout << "Gains: " << gains.kP << ", " << gains.kI << ", " << - // gains.kD - // << ", " << gains.kFF << std::endl; duty_cycle = gains.calculate((pos->to() - position_.to()), 0.0, velocity_.to(), load_.to()); } duty_cycle = std::clamp(duty_cycle, -1.0, 1.0); - // std::cout << "Duty Cycle: " << duty_cycle << std::endl; - // std::cout << "Velocity: " << velocity_.to() - // << " rad/s, Position: " << position_.to() << " rad" - // << std::endl; - pred_current_ = frc846::control::calculators::CurrentTorqueCalculator:: predict_current_draw( duty_cycle, velocity_, battery_voltage_, circuit_resistance_, specs); @@ -53,8 +39,6 @@ void MCSimulator::Tick() { pred_current_ = units::math::min( units::math::max(pred_current_, -current_limit_), current_limit_); - // std::cout << "Predicted Current: " << pred_current_.to() << " A" - // << std::endl; if (std::fabs(duty_cycle) < 0.03 && pred_current_ < 0_A && !brake_mode_) pred_current_ = 0_A; @@ -68,15 +52,11 @@ void MCSimulator::Tick() { loop_time = units::math::min(loop_time, 30_ms); - // std::cout << "Loop Time: " << loop_time.to() << " ms" << std::endl; - units::radians_per_second_t new_velocity = frc846::control::calculators::VelocityPositionEstimator::predict_velocity( velocity_, duty_cycle, loop_time, current_limit_, load_, friction_mag, rotational_inertia_, specs, circuit_resistance_, brake_mode_); - // new_velocity = - // units::radians_per_second_t{std::clamp(new_velocity.to(), - // (-specs.free_speed).to(), specs.free_speed.to())}; + units::radians_per_second_t avg_velocity = (velocity_ + new_velocity) / 2.0; position_ = diff --git a/src/frc846/cpp/frc846/robot/calculators/AprilTagCalculator.cc b/src/frc846/cpp/frc846/robot/calculators/AprilTagCalculator.cc index 91ae6bb4..435d0ee1 100644 --- a/src/frc846/cpp/frc846/robot/calculators/AprilTagCalculator.cc +++ b/src/frc846/cpp/frc846/robot/calculators/AprilTagCalculator.cc @@ -52,91 +52,6 @@ ATCalculatorOutput AprilTagCalculator::calculate(ATCalculatorInput input) { input.angular_velocity * (tl + input.bearing_latency); // TODO: modify latency compensation // if there is angular acceleration? - - // CASE 1: Triangulate with 2 tags - // if (tags.size() >= 2 && tags.size() == distances.size() && - // tags.size() == tx.size()) { - // frc846::math::Vector2D loc_tag_1 = { - // constants_.tag_locations[tags[0]].x_pos, - // constants_.tag_locations[tags[0]].y_pos}; - // frc846::math::Vector2D loc_tag_2 = { - // constants_.tag_locations[tags[1]].x_pos, - // constants_.tag_locations[tags[1]].y_pos}; - - // auto rotated_tag1 = loc_tag_1.rotate(bearingAtCapture, false); - // auto rotated_tag2 = loc_tag_2.rotate(bearingAtCapture, false); - - // double tag1Slope = 1 / units::math::tan(tx[0]); - // double tag2Slope = 1 / units::math::tan(tx[1]); - - // units::foot_t camera_x = ((loc_tag_2[1] - tag2Slope * loc_tag_2[0]) - - // (loc_tag_1[1] - tag1Slope * loc_tag_1[0])) - // / - // (tag1Slope - tag2Slope); - // units::foot_t camera_y = - // (camera_x - loc_tag_1[0]) * tag1Slope + loc_tag_1[1]; - - // frc846::math::Vector2D camera_pos = {camera_x, camera_y}; - // frc846::math::Vector2D cam_offset = { - // constants_.camera_x_offsets[i], constants_.camera_y_offsets[i]}; - - // frc846::math::Vector2D uncompensatedPos = camera_pos - cam_offset; - // uncompensatedPos = uncompensatedPos.rotate(bearingAtCapture); - - // frc846::math::Vector2D velComp = { - // (input.pose.velocity[0] + input.old_pose.velocity[0]) / 2 * - // (tl + input.fudge_latency[i]), - // (input.pose.velocity[1] + input.old_pose.velocity[1]) / 2 * - // (tl + input.fudge_latency[i])}; - // // if (frc846::math::modulo(fieldTag1Tx, 90_deg)>15_deg - // // &&frc846::math::modulo(fieldTag2Tx, 90_deg)>15_deg){ - // // - // frc846::math::Vector2D est_pos = uncompensatedPos + velComp; - - // // output.pos += (est_pos) * 1.2; - // // variance += - // // 1 / std::max( - // // (input.triangularVarianceCoeff * - // // std::sqrt((distances.at(0)+distances.at(1)).to()/2) - // // * std::pow( - // // 1 + - // // input.pose.velocity.magnitude().to()/12, - // // 2) * - // // std::pow(1 + - // input.angular_velocity.to()/300, - // // 2)), - // // 0.0000000001); - // // totalTagWeight += - // // 180/(distances.at(0).to()+distances.at(1).to()); - - // } - // // CASE 2: Single tag estimate - // else if (tags.size() == 1 && distances.size() == 1 && tx.size() == 1) { - // if (constants_.tag_locations.contains(tags[0])) { - // frc846::math::Vector2D velComp = { - // (input.pose.velocity[0] + input.old_pose.velocity[0]) / 2 * - // (tl + input.fudge_latency[i]), - // (input.pose.velocity[1] + input.old_pose.velocity[1]) / 2 * - // (tl + input.fudge_latency[i])}; - // if (distances.at(0) < 140_in) { - // output.pos += (getPos(bearingAtCapture, tx.at(0), distances.at(0), - // tags.at(0), i) + - // velComp) * - // (48) / distances.at(0).to(); - // variance += // -0.247, 0.0699 - // 1 / - // std::max( - // (input.aprilVarianceCoeff * distances.at(0).to() * - // std::pow( - // 1 + input.pose.velocity.magnitude().to() / - // 12, 2) * - // std::pow( - // 1 + input.angular_velocity.to() / 300, 2)), - // 0.0000000001); - // totalTagWeight += (48) / distances.at(0).to(); - // } - // } - // } if (!(tags.size() == distances.size() && tags.size() == tx.size())) { continue; } diff --git a/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc b/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc index e9772bac..2a5a05ac 100644 --- a/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc +++ b/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc @@ -3,6 +3,7 @@ #include #include "frc/RobotBase.h" +#include "frc/RobotController.h" #include "frc846/math/constants.h" #include "frc846/math/fieldpoints.h" #include "frc846/robot/swerve/control/swerve_ol_calculator.h" @@ -444,15 +445,14 @@ DrivetrainSubsystem::accelClampHelper( double max_accel_corr_factor = (winding_res / - (configs_.module_common_config.avg_resistance + winding_res)) + (configs_.module_common_config.avg_resistance + winding_res) * + (frc::RobotController::GetBatteryVoltage() / 12_V)) .to(); units::feet_per_second_t accel_buffer = accel_clamp / (max_accel_corr_factor * configs_.max_accel) * (motor_specs.free_speed * configs_.module_common_config.drive_reduction); - // TODO: batt voltage compensation - auto delta = velocity.magnitude() - GetReadings().estimated_pose.velocity.magnitude(); if (units::math::abs(delta) > accel_buffer) { diff --git a/src/y2025/cpp/FunkyRobot.cc b/src/y2025/cpp/FunkyRobot.cc index 24ed7cb1..8a8c8a43 100644 --- a/src/y2025/cpp/FunkyRobot.cc +++ b/src/y2025/cpp/FunkyRobot.cc @@ -25,23 +25,11 @@ FunkyRobot::FunkyRobot() : GenericRobot{&container_} { RegisterPreference("num_coasting_loops", 1000); RegisterPreference("homing_flash_loops", 50); - - // std::thread visionThread{[&]() { - // VisionThread(&container_); - // }}; - // visionThread.detach(); } void FunkyRobot::OnInitialize() { Field::Setup(); - // for (auto x : Field::getAllAutoData()) { - // Log("Adding Auto: {}", x.name + "_red"); - // AddAuto(x.name + "_red", new GenericAuto{container_, x, false}); - // Log("Adding Auto: {}", x.name + "_blue"); - // AddAuto(x.name + "_blue", new GenericAuto{container_, x, true}); - // } - ADD_AUTO_VARIANTS(FourAndPickAuto, "5PC"); AddAuto("1CPNP/R", new L4PickNetPickAuto{container_, false, true}); AddDefaultAuto("1CPNP/B", new L4PickNetPickAuto{container_, true, true}); @@ -174,7 +162,6 @@ void FunkyRobot::OnPeriodic() { container_.coral_ss_.telescope.BrakeSubsystem(); container_.climber_.BrakeSubsystem(); container_.algal_ss_.algal_wrist.BrakeSubsystem(); - // container_.coral_ss_.coral_wrist.BrakeSubsystem(); } if (!coast_switch_.Get() && !IsEnabled()) { container_.algal_ss_.elevator.CoastSubsystem(); @@ -216,21 +203,6 @@ void FunkyRobot::OnPeriodic() { void FunkyRobot::InitTest() { container_.drivetrain_.SetDefaultCommand(DriveCommand{container_}); - // container_.climber_.SetDefaultCommand(DinosaurClimberCommand{container_}); - - // frc2::Trigger start_dinosaur_a([] { return true; }); - // start_dinosaur_a.WhileTrue(frc2::SequentialCommandGroup{ - // AlgalPositionCommand{container_, kAlgae_DINOSAUR_A, true}, - // frc2::WaitCommand{0.5_s}, - // AlgalPositionCommand{container_, kAlgae_DINOSAUR_B, true}, - // frc2::WaitCommand{0.5_s}}.Repeatedly()); - - // frc2::Trigger start_dinosaur_c([] { return true; }); - // start_dinosaur_c.WhileTrue(frc2::SequentialCommandGroup{ - // CoralPositionCommand{container_, kCoral_DINOSAUR_A, true}, - // frc2::WaitCommand{0.5_s}, - // CoralPositionCommand{container_, kCoral_DINOSAUR_B, true}, - // frc2::WaitCommand{0.5_s}}.Repeatedly()); } #ifndef RUNNING_FRC_TESTS diff --git a/src/y2025/cpp/calculators/AntiTippingCalculator.cc b/src/y2025/cpp/calculators/AntiTippingCalculator.cc index ca506bee..919b04fe 100644 --- a/src/y2025/cpp/calculators/AntiTippingCalculator.cc +++ b/src/y2025/cpp/calculators/AntiTippingCalculator.cc @@ -13,24 +13,21 @@ frc846::math::Vector3D AntiTippingCalculator::tele_cg_position_{ robot_constants::telescope::pos_x, robot_constants::telescope::pos_y, robot_constants::base::height}; -// TODO: Fix void AntiTippingCalculator::SetElevatorHeight(units::inch_t height) { - if (height > robot_constants::elevator::height_cg) { + if (height > robot_constants::elevator::min_height_off_base) { elev_cg_position_[2] = height / 2.0; } else { elev_cg_position_[2] = ((robot_constants::elevator::elevator_weight * - robot_constants::elevator::height_cg / 2.0) + + robot_constants::elevator::min_height_off_base / 2.0) + (robot_constants::elevator::end_effector_weight * height)) / (robot_constants::elevator::elevator_weight + robot_constants::elevator::end_effector_weight); } } -// TODO: Fix void AntiTippingCalculator::SetTelescopeHeight(units::inch_t height) { - tele_cg_position_[2] = - (height + robot_constants::telescope::height_cg_error) / 2.0; + tele_cg_position_[2] = height / 2.0; } frc846::math::Vector3D AntiTippingCalculator::CalculateRobotCG() { diff --git a/src/y2025/cpp/commands/general/reef_auto_auto_align.cc b/src/y2025/cpp/commands/general/reef_auto_auto_align.cc index f1970c6b..33298d19 100644 --- a/src/y2025/cpp/commands/general/reef_auto_auto_align.cc +++ b/src/y2025/cpp/commands/general/reef_auto_auto_align.cc @@ -19,8 +19,6 @@ ReefAutoAutoAlignCommand::ReefAutoAutoAlignCommand(RobotContainer& container, getModifiedPrePose( numberOnRight, blueSide, leftSide, isRetry), 13_fps, 25_fps_sq, 10_fps_sq}, - /*DriveToReefCommand{&(container.drivetrain_), is_left, false, - max_speed, max_acceleration, max_deceleration},*/ frc2::ParallelRaceGroup{ frc2::WaitUntilCommand{[&] { return !frc::RobotBase::IsSimulation() && diff --git a/src/y2025/cpp/commands/teleop/algal_command.cc b/src/y2025/cpp/commands/teleop/algal_command.cc index 0b2f28c4..616ffc0b 100644 --- a/src/y2025/cpp/commands/teleop/algal_command.cc +++ b/src/y2025/cpp/commands/teleop/algal_command.cc @@ -13,33 +13,6 @@ void AlgalCommand::OnInit() {} void AlgalCommand::Periodic() { AlgalSSTarget algal_target{}; auto ci_readings = container_.control_input_.GetReadings(); - auto cpos = container_.drivetrain_.GetReadings().estimated_pose.position; - auto cvel = container_.drivetrain_.GetReadings().estimated_pose.velocity; - - // Not added to control input yet - - // bool use_pred_pos = false; - // frc846::math::Vector2D predicted_pos; - - // if (cvel.magnitude() > 2_fps) { - // use_pred_pos = true; - // units::second_t time_step = 0.3_s; - // predicted_pos = - // cpos + frc846::math::Vector2D{cvel[0] * time_step, cvel[1] * - // time_step}; - // } - - // Logic moved to control_input - - // if (ci_readings.lock_left_reef) { - // ci_readings.algal_state = - // (ReefProvider::getClosestReefSide( - // container_.drivetrain_.GetReadings().estimated_pose.position) % - // 2 == - // 0) - // ? kAlgae_L2Pick - // : kAlgae_L3Pick; - // } if (ci_readings.position_algal) algal_target.state = ci_readings.algal_state; diff --git a/src/y2025/cpp/commands/teleop/drive_command.cc b/src/y2025/cpp/commands/teleop/drive_command.cc index 2dcdf3f3..c635ccbb 100644 --- a/src/y2025/cpp/commands/teleop/drive_command.cc +++ b/src/y2025/cpp/commands/teleop/drive_command.cc @@ -20,25 +20,28 @@ void DriveCommand::Periodic() { container_.drivetrain_.SetTarget({target}); - double translate_x = frc846::math::HorizontalDeadband( - ci_readings_.translate_x, - container_.control_input_.GetPreferenceValue_double( - "translation_deadband"), - 1, - container_.control_input_.GetPreferenceValue_int("translation_exponent"), - 1); - - double translate_y = frc846::math::HorizontalDeadband( - ci_readings_.translate_y, - container_.control_input_.GetPreferenceValue_double( - "translation_deadband"), - 1, - container_.control_input_.GetPreferenceValue_int("translation_exponent"), - 1); + double translate_x = + frc846::math::HorizontalDeadband(ci_readings_.translate_x, + container_.control_input_.GetPreferenceValue_double( + "translation_deadband"), + 1, + container_.control_input_.GetPreferenceValue_double( + "translation_exponent"), + 1); + + double translate_y = + frc846::math::HorizontalDeadband(ci_readings_.translate_y, + container_.control_input_.GetPreferenceValue_double( + "translation_deadband"), + 1, + container_.control_input_.GetPreferenceValue_double( + "translation_exponent"), + 1); double rotation = frc846::math::HorizontalDeadband(ci_readings_.rotation, container_.control_input_.GetPreferenceValue_double("rotation_deadband"), - 1, container_.control_input_.GetPreferenceValue_int("rotation_exponent"), + 1, + container_.control_input_.GetPreferenceValue_double("rotation_exponent"), 1); units::feet_per_second_t max_speed = diff --git a/src/y2025/cpp/commands/teleop/drive_to_reef_command.cc b/src/y2025/cpp/commands/teleop/drive_to_reef_command.cc index c63551b6..1ec2f9ea 100644 --- a/src/y2025/cpp/commands/teleop/drive_to_reef_command.cc +++ b/src/y2025/cpp/commands/teleop/drive_to_reef_command.cc @@ -49,23 +49,5 @@ std::pair DriveToReefCommand::GetTargetPoint() { target_pos.velocity = 0.0_fps; } } - // units::inch_t reef_drive_subtract = - // (drivetrain_->GetPreferenceValue_unit_type( - // "lock_drive_early")); - // target_pos.point = - // cpos + (target_pos.point - - // cpos).AddToMagnitude(-reef_drive_subtract); - - // if ((target_pos.point - start_point_).magnitude() <= reef_drive_subtract) - // { - // target_pos.point = start_point_; - // } - - // target_pos.velocity = - // drivetrain_->GetPreferenceValue_unit_type( - // "lock_drive_fvel"); - - // if (is_pre_point_) { target_pos.velocity = 2_fps; } - return {target_pos, true}; } \ No newline at end of file diff --git a/src/y2025/cpp/commands/teleop/gpd_ss_command.cc b/src/y2025/cpp/commands/teleop/gpd_ss_command.cc index f78bda7b..9095fd51 100644 --- a/src/y2025/cpp/commands/teleop/gpd_ss_command.cc +++ b/src/y2025/cpp/commands/teleop/gpd_ss_command.cc @@ -17,14 +17,10 @@ void GPDSSCommand::Periodic() { const auto [gpd_pos, valid] = container_.GPD_.getBestGP(gpd_readings.gamepieces); - // TODO: determine is_on_top - algal_target.state = (container_.control_input_.GetReadings().level_one) ? kAlgae_CoralPick : kAlgae_GroundIntake; - // algal_target.score = true; - container_.algal_ss_.SetTarget(algal_target); } diff --git a/src/y2025/cpp/commands/teleop/l1_align.cc b/src/y2025/cpp/commands/teleop/l1_align.cc index f8bc88a7..2ab27678 100644 --- a/src/y2025/cpp/commands/teleop/l1_align.cc +++ b/src/y2025/cpp/commands/teleop/l1_align.cc @@ -18,8 +18,6 @@ L1AutoAlignCommand::L1AutoAlignCommand(RobotContainer& container, bool is_left, frc2::SequentialCommandGroup{ DriveToReefCommand{container, is_left, true, max_speed, max_acceleration, max_deceleration}, - /*DriveToReefCommand{&(container.drivetrain_), is_left, false, - max_speed, max_acceleration, max_deceleration},*/ frc2::ParallelDeadlineGroup{ frc2::WaitCommand{0.875_s}, LockToReefCommand{container, is_left, base_adj}, diff --git a/src/y2025/cpp/commands/teleop/lock_to_reef_command.cc b/src/y2025/cpp/commands/teleop/lock_to_reef_command.cc index 66f40de9..fc240345 100644 --- a/src/y2025/cpp/commands/teleop/lock_to_reef_command.cc +++ b/src/y2025/cpp/commands/teleop/lock_to_reef_command.cc @@ -15,18 +15,6 @@ LockToReefCommand::LockToReefCommand( int reef_target_pos = ReefProvider::getClosestReefSide(pos); auto ci_readings_ = cnt.control_input_.GetReadings(); - // units::inch_t adj_rate = - // cnt.drivetrain_.GetPreferenceValue_unit_type( - // "lock_adj_rate"); - // if (ci_readings_.rc_n_x) { - // ba[0] -= adj_rate; - // } else if (ci_readings_.rc_p_x) { - // ba[0] += adj_rate; - // } else if (ci_readings_.rc_n_y) { - // ba[1] -= adj_rate; - // } else if (ci_readings_.rc_p_y) { - // ba[1] += adj_rate; - // } auto target_pos = ReefProvider::getReefScoringLocations(true, false, !(cnt.control_input_.GetReadings().coral_state == diff --git a/src/y2025/cpp/commands/teleop/reef_auto_align.cc b/src/y2025/cpp/commands/teleop/reef_auto_align.cc index bd8a5503..72860576 100644 --- a/src/y2025/cpp/commands/teleop/reef_auto_align.cc +++ b/src/y2025/cpp/commands/teleop/reef_auto_align.cc @@ -16,6 +16,4 @@ ReefAutoAlignCommand::ReefAutoAlignCommand(RobotContainer& container, frc2::SequentialCommandGroup{ DriveToReefCommand{container, is_left, true, max_speed, max_acceleration, max_deceleration}, - /*DriveToReefCommand{&(container.drivetrain_), is_left, false, - max_speed, max_acceleration, max_deceleration},*/ LockToReefCommand{container, is_left, base_adj}}} {} diff --git a/src/y2025/cpp/control_triggers.cc b/src/y2025/cpp/control_triggers.cc index edb9873a..235e2600 100644 --- a/src/y2025/cpp/control_triggers.cc +++ b/src/y2025/cpp/control_triggers.cc @@ -102,7 +102,7 @@ void ControlTriggerInitializer::InitTeleopTriggers(RobotContainer& container) { .AndThen(frc2::InstantCommand([&] { container.control_input_.SetTarget({false, false}); }).ToPtr())); - + // Assisted intake using GPD // frc2::Trigger{[&] { // return container.control_input_.GetReadings().targeting_algae && // container.GPD_.GetReadings().gamepieces.size() != 0U && diff --git a/src/y2025/cpp/subsystems/abstract/control_input.cc b/src/y2025/cpp/subsystems/abstract/control_input.cc index 06eaace3..c8178cdc 100644 --- a/src/y2025/cpp/subsystems/abstract/control_input.cc +++ b/src/y2025/cpp/subsystems/abstract/control_input.cc @@ -16,10 +16,10 @@ void ControlInputSubsystem::Setup() { RegisterPreference("trigger_threshold", 0.3); RegisterPreference("translation_deadband", 0.07); - RegisterPreference("translation_exponent", 2); + RegisterPreference("translation_exponent", 2.0); RegisterPreference("rotation_deadband", 0.07); - RegisterPreference("rotation_exponent", 2); + RegisterPreference("rotation_exponent", 2.0); RegisterPreference("op_deadband", 0.35); } diff --git a/src/y2025/cpp/subsystems/abstract/gpd.cc b/src/y2025/cpp/subsystems/abstract/gpd.cc index dae12ced..cd6977d9 100644 --- a/src/y2025/cpp/subsystems/abstract/gpd.cc +++ b/src/y2025/cpp/subsystems/abstract/gpd.cc @@ -111,8 +111,6 @@ GPDReadings GPDSubsystem::ReadFromHardware() { int num_gps = readings.gamepieces.size(); - // Graph("num_gps", num_gps); - gp_spin_ += 5_deg; #ifndef _WIN32 diff --git a/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc b/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc index 7bb53cbc..ca4dd988 100644 --- a/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc +++ b/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc @@ -123,7 +123,7 @@ DrivetrainConstructor::getDrivetrainConfigs() { .circuit_resistance = 999_Ohm, // overriden by unique config .rotational_inertia = relative_drive_inertia, .friction = 0.02, - .bus = "", + .bus = "", // overriden by module common config }; frc846::control::config::MotorConstructionParameters steer_params{ .can_id = 999, // overriden by unique config @@ -138,7 +138,7 @@ DrivetrainConstructor::getDrivetrainConfigs() { .circuit_resistance = 999_Ohm, // overriden by unique config .rotational_inertia = relative_steer_inertia, .friction = 0.11, - .bus = "", + .bus = "", // overriden by module common config }; frc846::wpilib::unit_ohm avg_resistance = @@ -149,7 +149,8 @@ DrivetrainConstructor::getDrivetrainConfigs() { configs.module_common_config = frc846::robot::swerve::SwerveModuleCommonConfig{drive_params, steer_params, mmtype, steer_reduction, drive_reduction, - avg_resistance, steer_load_factor, ""}; + avg_resistance, steer_load_factor, + ""}; // changes which bus the motors are on configs.module_unique_configs = {FR_config, FL_config, BL_config, BR_config}; configs.camera_x_offsets = {-6.25_in, -4.5_in}; diff --git a/src/y2025/cpp/subsystems/hardware/FMS.cc b/src/y2025/cpp/subsystems/hardware/FMS.cc deleted file mode 100644 index 0a2dda14..00000000 --- a/src/y2025/cpp/subsystems/hardware/FMS.cc +++ /dev/null @@ -1,12 +0,0 @@ -// #include "subsystems/hardware/FMS.h" - -// CompositionMonkeyState FMS::convertToComposition(MonkeyState state) { -// return {{state.first, state.first}, {state.second, state.second}}; -// } - -// MSMAP FMS::intermediate_state_map{ -// {TransientMonkeyState{ -// convertToComposition({kCoral_StowNoPiece, kAlgae_Stow}), -// convertToComposition({kCoral_StowWithPiece, kAlgae_Stow})}, -// CompositionMonkeyState{{kCoral_ScoreL3, kCoral_StowNoPiece}, -// {kAlgae_Stow, kAlgae_Stow}}}}; \ No newline at end of file diff --git a/src/y2025/cpp/subsystems/hardware/algal/algal_end_effector.cc b/src/y2025/cpp/subsystems/hardware/algal/algal_end_effector.cc index 9a6c5904..11c286a9 100644 --- a/src/y2025/cpp/subsystems/hardware/algal/algal_end_effector.cc +++ b/src/y2025/cpp/subsystems/hardware/algal/algal_end_effector.cc @@ -24,19 +24,13 @@ AlgalEESubsystem::AlgalEESubsystem() ports::algal_ss_::end_effector_::kEE2_CANID, true)))} { RegisterPreference("idle_speed", 2.3_fps); RegisterPreference("piece_thresh", 1.0_fps); - - RegisterPreference("kick_dc", -18.5_fps); RegisterPreference("backspin_constant", -22_fps); RegisterPreference("idle_speed_coral", -5.0_fps); RegisterPreference("coral_vel_thresh", 5_fps); - RegisterPreference("test1", false); - RegisterPreference("max_eject", -40_fps); REGISTER_PIDF_CONFIG(0.0001, 0.0, 0.0, 0.0); - RegisterPreference("testthing", 0.4); - esc_helper_1_.SetConversion(roller_reduction_); esc_helper_2_.SetConversion(roller_reduction_); esc_helper_1_.bind(&esc_1_); diff --git a/src/y2025/cpp/subsystems/hardware/algal/algal_ss.cc b/src/y2025/cpp/subsystems/hardware/algal/algal_ss.cc index b4c93c88..d5d80297 100644 --- a/src/y2025/cpp/subsystems/hardware/algal/algal_ss.cc +++ b/src/y2025/cpp/subsystems/hardware/algal/algal_ss.cc @@ -89,18 +89,14 @@ AlgalSetpoint AlgalSuperstructure::getSetpoint(AlgalStates state) { bool AlgalSuperstructure::hasReached(AlgalStates state) { bool has_reached = hasReachedWrist(state) && hasReachedElevator(state); - // Graph("has_reached", has_reached); - return has_reached; } bool AlgalSuperstructure::hasReachedWrist(AlgalStates state) { AlgalSetpoint setpoint = getSetpoint(state); - if (algal_wrist.is_initialized() && (units::math::abs(algal_wrist.GetReadings().position - setpoint.angle) > GetPreferenceValue_unit_type("wrist_tolerance"))) { - // Graph("has_reached", false); return false; } return true; @@ -108,11 +104,9 @@ bool AlgalSuperstructure::hasReachedWrist(AlgalStates state) { bool AlgalSuperstructure::hasReachedElevator(AlgalStates state) { AlgalSetpoint setpoint = getSetpoint(state); - if (elevator.is_initialized() && (units::math::abs(elevator.GetReadings().position - setpoint.height) > GetPreferenceValue_unit_type("elevator_tolerance"))) { - // Graph("has_reached", false); return false; } return true; @@ -130,8 +124,6 @@ void AlgalSuperstructure::WriteToHardware(AlgalSSTarget target) { AlgalSetpoint setpoint = getSetpoint(target.state); bool coral_mode = target.cm; - Graph("is_coral_mode", coral_mode); - if (target.super_mode) { algal_end_effector.SetTarget( {GetPreferenceValue_unit_type("score_dc"), diff --git a/src/y2025/cpp/subsystems/hardware/algal/algal_wrist.cc b/src/y2025/cpp/subsystems/hardware/algal/algal_wrist.cc index 89c908ad..4c29d4bc 100644 --- a/src/y2025/cpp/subsystems/hardware/algal/algal_wrist.cc +++ b/src/y2025/cpp/subsystems/hardware/algal/algal_wrist.cc @@ -40,17 +40,5 @@ std::pair AlgalWristSubsystem::GetSensorPos( units::degree_t sensor_pos) { units::degree_t raw_enc_pos = AlgalWristSubsystem::GetReadings().absolute_position; - // if (raw_enc_pos > 180_deg) raw_enc_pos -= 360_deg; - // return { - // ((-raw_enc_pos * (16_tr / 40_tr) + - // GetPreferenceValue_unit_type("encoder_offset")) - - // 150_deg, - // 360_deg) + - // 150_deg + 149_deg, - // true}; - // true/*units::math::abs(AlgalWristSubsystem::GetReadings().velocity) < - // GetPreferenceValue_unit_type( - // "use_sensor_threshold") && - // GetReadings().position < 30_deg*/}; return {0_deg, false}; } \ No newline at end of file diff --git a/src/y2025/cpp/subsystems/hardware/algal/elevator.cc b/src/y2025/cpp/subsystems/hardware/algal/elevator.cc index 2481fa00..be0eef2c 100644 --- a/src/y2025/cpp/subsystems/hardware/algal/elevator.cc +++ b/src/y2025/cpp/subsystems/hardware/algal/elevator.cc @@ -35,40 +35,6 @@ LinearSubsystemTarget ElevatorSubsystem::ZeroTarget() const { return LinearSubsystemTarget{0_in}; } -void ElevatorSubsystem::ExtendedSetup() { - // linear_esc_.ConfigReverseLimitSwitch( - // true, frc846::control::base::LimitSwitchDefaultState::kNormallyOff); - // homing_counter_ = 0; -} - -void ElevatorSubsystem::RHExtension() { - // homing_counter_++; - // bool sensor_triggered = linear_esc_.GetReverseLimitSwitchState(); - // Graph("elevator/homing_sensor", sensor_triggered); - // if (sensor_triggered && - // homing_counter_ > GetPreferenceValue_int("hall_effect_counter") && - // GetReadings().position < 34_in) { - // linear_esc_helper_.SetPosition( - // robot_constants::elevator::min_height_off_base + - // GetPreferenceValue_unit_type("hall_effect_plus_height")); - // homing_counter_ = 0; - // } +void ElevatorSubsystem::ExtendedSetup() {} - // if (GetTarget().position < - // GetPreferenceValue_unit_type("sm_target_range") && - // units::math::abs(linear_esc_helper_.GetVelocity()) < - // GetPreferenceValue_unit_type( - // "sm_vel_thresh") && - // GetReadings().position > 29_in && GetReadings().position < 34_in && - // linear_esc_.GetCurrent() > - // GetPreferenceValue_unit_type("sm_current_thresh")) - // { - // safety_counter_ += 1; - // if (safety_counter_ > GetPreferenceValue_int("sm_counter")) { - // linear_esc_helper_.SetPosition( - // robot_constants::elevator::min_height_off_base); - // } - // } else { - // safety_counter_ = 0; - // } -} \ No newline at end of file +void ElevatorSubsystem::RHExtension() {} \ No newline at end of file diff --git a/src/y2025/cpp/subsystems/hardware/coral/coral_end_effector.cc b/src/y2025/cpp/subsystems/hardware/coral/coral_end_effector.cc index 3462452b..fa8f13bf 100644 --- a/src/y2025/cpp/subsystems/hardware/coral/coral_end_effector.cc +++ b/src/y2025/cpp/subsystems/hardware/coral/coral_end_effector.cc @@ -76,7 +76,6 @@ CoralEEReadings CoralEESubsystem::ReadFromHardware() { } void CoralEESubsystem::WriteToHardware(CoralEETarget target) { - // Graph("target/duty_cycle", target.duty_cycle_); if (GetReadings().has_piece_ && target.duty_cycle_ < 0.0) esc_.WriteDC(GetPreferenceValue_double("idle_speed")); else diff --git a/src/y2025/cpp/subsystems/hardware/coral/coral_ss.cc b/src/y2025/cpp/subsystems/hardware/coral/coral_ss.cc index 0648b03c..ee5675f4 100644 --- a/src/y2025/cpp/subsystems/hardware/coral/coral_ss.cc +++ b/src/y2025/cpp/subsystems/hardware/coral/coral_ss.cc @@ -147,7 +147,6 @@ CoralSSReadings CoralSuperstructure::ReadFromHardware() { } else { no_piece_chute_count_ = 0; } - Graph("no_piece_chute_count", no_piece_chute_count_); bool auto_flick_valid = GetPreferenceValue_bool("autoflick") && diff --git a/src/y2025/cpp/subsystems/hardware/coral/coral_wrist.cc b/src/y2025/cpp/subsystems/hardware/coral/coral_wrist.cc index 510a3f9b..4e2b9dc6 100644 --- a/src/y2025/cpp/subsystems/hardware/coral/coral_wrist.cc +++ b/src/y2025/cpp/subsystems/hardware/coral/coral_wrist.cc @@ -46,30 +46,15 @@ void CoralWristSubsystem::ExtendedSetup() { wrist_esc_helper_.SetPosition(abs_pos); } -// TODO: fix? std::pair CoralWristSubsystem::GetSensorPos( units::degree_t sensor_pos) { - units::degree_t raw_enc_pos = - sensor_pos; // TODO: fix definitely wrong by at least one loop time + // TODO: fix, behind one loop time + units::degree_t raw_enc_pos = sensor_pos; if (raw_enc_pos > 340_deg) raw_enc_pos -= 360_deg; units::degree_t abs_pos = raw_enc_pos + GetPreferenceValue_unit_type("encoder_offset"); - bool is_valid_norm = true; /* - units::math::abs(CoralWristSubsystem::GetReadings().velocity) < - GetPreferenceValue_unit_type( - "use_sensor_threshold")*/ /*&& - GetReadings().position < 50_deg*/ - - // bool is_valid_deployed = - // units::math::abs(CoralWristSubsystem::GetReadings().velocity) < - // GetPreferenceValue_unit_type( - // "use_sensor_threshold") && - // (GetReadings().position - abs_pos) > - // GetPreferenceValue_unit_type( - // "deployed_encoder_tolerance"); - - return {abs_pos, is_valid_norm /*|| is_valid_deployed*/}; + return {abs_pos, true}; } \ No newline at end of file diff --git a/src/y2025/cpp/subsystems/hardware/generic/linear_subsystem.cc b/src/y2025/cpp/subsystems/hardware/generic/linear_subsystem.cc index 10635047..bde67b04 100644 --- a/src/y2025/cpp/subsystems/hardware/generic/linear_subsystem.cc +++ b/src/y2025/cpp/subsystems/hardware/generic/linear_subsystem.cc @@ -19,8 +19,6 @@ LinearSubsystem::LinearSubsystem(std::string name, RegisterPreference("homing_dc", -0.05); RegisterPreference("homing_thresh", 0.05_fps); RegisterPreference("homing_loops", 25); - // RegisterPreference("home_zero_height", 28.0_in); - // RegisterPreference("telescope_autohome_height", 27.0_in); } frc846::control::config::MotorConstructionParameters @@ -50,10 +48,6 @@ void LinearSubsystem::Setup() { linear_esc_helper_.SetPosition(28.5_in); linear_esc_helper_.SetSoftLimits(GET_SOFTLIMITS(units::inch_t)); - // linear_esc_helper_.SetControllerSoftLimits(GET_SOFTLIMITS(units::inch_t)); - - // linear_esc_.ConfigForwardLimitSwitch( - // false, frc846::control::base::LimitSwitchDefaultState::kNormallyOff); ExtendedSetup(); } @@ -89,15 +83,6 @@ LinearSubsystemReadings LinearSubsystem::ReadFromHardware() { RHExtension(); - // bool forward_limit = linear_esc_.GetForwardLimitSwitchState(); - - // Graph("readings/homing_sensor", forward_limit); - - // if (forward_limit && !is_homed_) { - // is_homed_ = true; - // linear_esc_helper_.SetPosition(hall_effect_loc_); - // } - return readings; } diff --git a/src/y2025/cpp/subsystems/hardware/generic/wrist_subsystem.cc b/src/y2025/cpp/subsystems/hardware/generic/wrist_subsystem.cc index fe7815ca..cd135d3e 100644 --- a/src/y2025/cpp/subsystems/hardware/generic/wrist_subsystem.cc +++ b/src/y2025/cpp/subsystems/hardware/generic/wrist_subsystem.cc @@ -43,7 +43,6 @@ void WristSubsystem::Setup() { frc846::control::config::StatusFrame::kAbsoluteFrame}); wrist_esc_helper_.SetSoftLimits(GET_SOFTLIMITS(units::degree_t)); - // wrist_esc_helper_.SetControllerSoftLimits(GET_SOFTLIMITS(units::degree_t)); const auto [sensor_pos, is_valid] = GetSensorPos(GetReadings().absolute_position); @@ -76,10 +75,8 @@ WristReadings WristSubsystem::ReadFromHardware() { const auto [sensor_pos, is_valid] = GetSensorPos(abs_pos_deg); if (!isAlgaeSubsystem()) { if (is_valid && - units::math::abs( - sensor_pos - (readings.position /* + encoder_offset_*/)) > + units::math::abs(sensor_pos - (readings.position)) > GetPreferenceValue_unit_type("rezero_thresh")) { - // encoder_offset_ = sensor_pos - readings.position; wrist_esc_helper_.SetPosition(sensor_pos); Graph("readings/sensor_pos", sensor_pos); } @@ -108,7 +105,6 @@ WristReadings WristSubsystem::ReadFromHardware() { Graph("readings/sensor_pos_valid", is_valid); Graph("readings/encoder_offset", encoder_offset_); - // readings.position += encoder_offset_; Graph("readings/position", readings.position); Graph("readings/error", GetTarget().position - readings.position); @@ -128,8 +124,8 @@ void WristSubsystem::WriteToHardware(WristTarget target) { Graph("target/position", target.position); wrist_esc_.SetGains(GET_PIDF_GAINS()); - Graph("target/raw_position", target.position /* - encoder_offset_*/); - wrist_esc_helper_.WritePosition(target.position /* - encoder_offset_*/); + Graph("target/raw_position", target.position); + wrist_esc_helper_.WritePosition(target.position); } void WristSubsystem::BrakeSubsystem() { @@ -145,7 +141,6 @@ void WristSubsystem::SetEncoderOffset() { const auto [sensor_pos, is_valid] = GetSensorPos(GetReadings().absolute_position); - // encoder_offset_ = sensor_pos - GetReadings().position; wrist_esc_helper_.SetPosition(sensor_pos); } } \ No newline at end of file diff --git a/src/y2025/include/subsystems/hardware/FMS.h b/src/y2025/include/subsystems/hardware/FMS.h deleted file mode 100644 index efd56b97..00000000 --- a/src/y2025/include/subsystems/hardware/FMS.h +++ /dev/null @@ -1,19 +0,0 @@ -// #pragma once - -// #include "subsystems/hardware/algal/algal_ss.h" -// #include "subsystems/hardware/coral/coral_ss.h" - -// using MonkeyState = std::pair; -// using CompositionMonkeyState = std::pair, -// std::pair>; -// using TransientMonkeyState = -// std::pair; - -// using MSMAP = std::map; - -// class FMS { -// public: -// static CompositionMonkeyState convertToComposition(MonkeyState state); - -// static MSMAP intermediate_state_map; -// }; \ No newline at end of file diff --git a/src/y2025/include/subsystems/robot_constants.h b/src/y2025/include/subsystems/robot_constants.h index c467e51c..600fe68c 100644 --- a/src/y2025/include/subsystems/robot_constants.h +++ b/src/y2025/include/subsystems/robot_constants.h @@ -18,8 +18,6 @@ struct robot_constants { static constexpr units::pound_t elevator_weight = 10_lb; static constexpr units::inch_t min_height_off_base = 29_in; - static constexpr units::inch_t height_cg = 28_in; - static constexpr units::pound_t end_effector_weight = 25_lb; static constexpr units::pound_t total_weight = @@ -38,7 +36,6 @@ struct robot_constants { telescope_weight + end_effector_weight; static constexpr units::inch_t min_height = 27.65_in; - static constexpr units::inch_t height_cg_error = 1.35_in; static constexpr units::inch_t pos_x = -10_in; static constexpr units::inch_t pos_y = 9_in; diff --git a/src/y2025/include/subsystems/robot_container.h b/src/y2025/include/subsystems/robot_container.h index 175000da..20672d5d 100644 --- a/src/y2025/include/subsystems/robot_container.h +++ b/src/y2025/include/subsystems/robot_container.h @@ -49,40 +49,9 @@ class RobotContainer : public frc846::robot::GenericRobotContainer { RegisterSubsystemGroupAB({{&drivetrain_, drivetrain_init}}); RegisterSubsystemGroupAB({{&GPD_, gpd_init}}); - // TODO: undo - // NVM // Superstructures not necessary - updated with extensions RegisterSubsystemGroupA({{&coral_ss_, coral_ss_init}}); RegisterSubsystemGroupB({{&algal_ss_, algal_ss_init}}); - // if (coral_ss_init) { - // coral_ss_.Init(); - // all_subsystems_.push_back(&coral_ss_); - // } - - // if (algal_ss_init) { - // algal_ss_.Init(); - // all_subsystems_.push_back(&algal_ss_); - // } - - // TODO: undo RegisterSubsystemGroupB({{&climber_, climber_init}}); } - - // void GroupAUpdateHardwareExtension() override { - // // if (control_input_.GetReadings().first_enable_exception) return; - // // coral_ss_.UpdateHardware(); - // } - - // void GroupBUpdateHardwareExtension() override { - // // if (control_input_.GetReadings().first_enable_exception) return; - // // algal_ss_.UpdateHardware(); - // } - - // void - // GroupAUpdateReadingsExtension() override { /*coral_ss_.UpdateReadings();*/ - // } - - // void - // GroupBUpdateReadingsExtension() override { /*algal_ss_.UpdateReadings();*/ - // } };