diff --git a/README.md b/README.md index cbbd3ec..9f51550 100644 --- a/README.md +++ b/README.md @@ -173,6 +173,7 @@ To undo the going back: ## CppCheck Warnings ``` +src\frc846\cpp\frc846\control\calculators\CurrentTorqueCalculator.cc:49:30: warning: Variable 'duty_cycle_original' is assigned a value that is never used. [unreadVariable] src\frc846\cpp\frc846\robot\swerve\drivetrain.cc:160:64: warning: Variable 'accel_target' is assigned a value that is never used. [unreadVariable] src\frc846\cpp\frc846\math\collection.cc:25:0: warning: The function 'VerticalDeadband' is never used. [unusedFunction] src\frc846\cpp\frc846\math\collection.cc:39:0: warning: The function 'CoterminalDifference' is never used. [unusedFunction] diff --git a/build.gradle b/build.gradle index 2f2d0c8..8014394 100644 --- a/build.gradle +++ b/build.gradle @@ -204,4 +204,4 @@ runCppcheck.onlyIf { !project.hasProperty('fromCI') } if (!project.hasProperty('fromCI')) { check.dependsOn runCppcheck runCppcheck.dependsOn spotlessApply -} \ No newline at end of file +} diff --git a/networktables.json b/networktables.json index 4f33ee8..b89113a 100644 --- a/networktables.json +++ b/networktables.json @@ -230,5 +230,37 @@ "properties": { "persistent": true } + }, + { + "name": "/Preferences/MotorMonkey/MotorMonkey/voltage_min (V)", + "type": "double", + "value": 7.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/MotorMonkey/MotorMonkey/recal_voltage_thresh (V)", + "type": "double", + "value": 9.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/MotorMonkey/MotorMonkey/default_max_draw (A)", + "type": "double", + "value": 150.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/MotorMonkey/MotorMonkey/battery_cc (A)", + "type": "double", + "value": 600.0, + "properties": { + "persistent": true + } } ] diff --git a/networktables.json.bck b/networktables.json.bck index f7bf6ec..b89113a 100644 --- a/networktables.json.bck +++ b/networktables.json.bck @@ -190,5 +190,77 @@ "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/odom_fudge_factor", + "type": "double", + "value": 0.875, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/MotorMonkey/MotorMonkey/voltage_min (V)", + "type": "double", + "value": 7.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/MotorMonkey/MotorMonkey/recal_voltage_thresh (V)", + "type": "double", + "value": 9.5, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/MotorMonkey/MotorMonkey/default_max_draw (A)", + "type": "double", + "value": 150.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/MotorMonkey/MotorMonkey/battery_cc (A)", + "type": "double", + "value": 600.0, + "properties": { + "persistent": true + } } ] diff --git a/src/frc846/cpp/frc846/control/HigherMotorController.cc b/src/frc846/cpp/frc846/control/HigherMotorController.cc index 9cc972c..faa5bcb 100644 --- a/src/frc846/cpp/frc846/control/HigherMotorController.cc +++ b/src/frc846/cpp/frc846/control/HigherMotorController.cc @@ -33,6 +33,11 @@ void HigherMotorController::WriteDC(double duty_cycle) { if (soft_limits_.has_value()) { duty_cycle = soft_limits_.value().LimitDC(duty_cycle, GetPosition()); } + duty_cycle = + frc846::control::calculators::CurrentTorqueCalculator::limit_current_draw( + duty_cycle, constr_params_.smart_current_limit, GetVelocity(), 12.0_V, + constr_params_.circuit_resistance, + frc846::control::base::MotorSpecificationPresets::get(mmtype_)); MotorMonkey::WriteDC(slot_id_, duty_cycle); } diff --git a/src/frc846/cpp/frc846/control/MotorMonkey.cc b/src/frc846/cpp/frc846/control/MotorMonkey.cc index 53af625..f01d070 100644 --- a/src/frc846/cpp/frc846/control/MotorMonkey.cc +++ b/src/frc846/cpp/frc846/control/MotorMonkey.cc @@ -13,8 +13,6 @@ #include "frc846/control/hardware/simulation/SIMLEVEL.h" #include "frc846/math/collection.h" -// TODO: Add dynamic can/power management - namespace frc846::control { #define CHECK_SLOT_ID() \ @@ -74,15 +72,88 @@ frc846::wpilib::unit_ohm units::volt_t MotorMonkey::battery_voltage{0_V}; +units::volt_t MotorMonkey::last_disabled_voltage{0_V}; + +frc846::math::DoubleSyncBuffer MotorMonkey::sync_buffer{50U, 15}; + +units::ampere_t MotorMonkey::max_draw_{0.0_A}; + std::queue MotorMonkey::control_messages{}; -void MotorMonkey::Tick(units::ampere_t max_draw) { +void MotorMonkey::Setup() { + loggable_.RegisterPreference("voltage_min", 7.5_V); + loggable_.RegisterPreference("recal_voltage_thresh", 10.5_V); + loggable_.RegisterPreference("default_max_draw", 150.0_A); + loggable_.RegisterPreference("min_max_draw", 40_A); + loggable_.RegisterPreference("max_max_draw", 300_A); + loggable_.RegisterPreference("battery_cc", 400_A); + + max_draw_ = loggable_.GetPreferenceValue_unit_type( + "default_max_draw"); +} + +void MotorMonkey::RecalculateMaxDraw() { + if (!sync_buffer.IsValid()) return; + + sync_buffer.Sync(); + loggable_.Graph("sync_diff_", sync_buffer.GetSyncDiff()); + + auto sy_trough = sync_buffer.GetTrough(); + units::ampere_t cc_current{sy_trough.first}; + + loggable_.Graph("cc_current", cc_current); + + units::ampere_t current_draw = + loggable_.GetPreferenceValue_unit_type("battery_cc") - + cc_current; + units::volt_t voltage{sy_trough.second}; + + units::volt_t voltage_min = + loggable_.GetPreferenceValue_unit_type("voltage_min"); + units::volt_t recal_voltage_thresh = + loggable_.GetPreferenceValue_unit_type( + "recal_voltage_thresh"); + + if (voltage > recal_voltage_thresh) return; + + units::ampere_t temp_max_draw_ = current_draw * + (last_disabled_voltage - voltage_min) / + (last_disabled_voltage - voltage); + + if (temp_max_draw_ < + loggable_.GetPreferenceValue_unit_type("min_max_draw")) { + return; + } else if (temp_max_draw_ > + loggable_.GetPreferenceValue_unit_type( + "max_max_draw")) { + return; + } else { + max_draw_ = temp_max_draw_; + } +} + +void MotorMonkey::Tick(bool disabled) { battery_voltage = frc::RobotController::GetBatteryVoltage(); - loggable_.Graph("battery_voltage", battery_voltage.to()); + loggable_.Graph("battery_voltage", battery_voltage); + loggable_.Graph("last_disabled_voltage", last_disabled_voltage); + + if (disabled) { + last_disabled_voltage = battery_voltage; + return; + } + + units::ampere_t total_pred_draw = WriteMessages(max_draw_); + + loggable_.Graph("total_pred_draw", total_pred_draw); - WriteMessages(max_draw); + units::ampere_t cc_current = + loggable_.GetPreferenceValue_unit_type("battery_cc") - + total_pred_draw; + sync_buffer.Add(cc_current.to(), battery_voltage.to()); - // TODO: Improve battery voltage estimation for simulation + RecalculateMaxDraw(); + + loggable_.Graph("max_draw", max_draw_); for (size_t i = 0; i < CONTROLLER_REGISTRY_SIZE; i++) { if (controller_registry[i] != nullptr) { @@ -97,8 +168,9 @@ void MotorMonkey::Tick(units::ampere_t max_draw) { } } -void MotorMonkey::WriteMessages(units::ampere_t max_draw) { +units::ampere_t MotorMonkey::WriteMessages(units::ampere_t max_draw) { units::ampere_t total_current = 0.0_A; + units::ampere_t second_total_current = 0.0_A; std::queue temp_messages{control_messages}; double scale_factor = 1.0; @@ -112,8 +184,6 @@ void MotorMonkey::WriteMessages(units::ampere_t max_draw) { units::radians_per_second_t velocity = controller_registry[msg.slot_id]->GetVelocity(); - frc846::control::base::MotorGains gains = gains_registry[msg.slot_id]; - double duty_cycle = 0.0; switch (msg.type) { @@ -121,35 +191,25 @@ void MotorMonkey::WriteMessages(units::ampere_t max_draw) { duty_cycle = std::get(msg.value); break; case MotorMessage::Type::Position: { - duty_cycle = - gains.calculate((std::get(msg.value) - - controller_registry[msg.slot_id]->GetPosition()) - .to(), - 0.0, velocity.to(), 0.0); - duty_cycle = std::clamp(duty_cycle, -1.0, 1.0); + // Onboard control should only be used with low-current draw systems + duty_cycle = 0.0; break; } case MotorMessage::Type::Velocity: { - duty_cycle = gains.calculate( - (std::get(msg.value) - velocity) - .to(), - 0.0, 0.0, 0.0); - duty_cycle += - gains.kFF * - (std::get(msg.value) / - frc846::control::base::MotorSpecificationPresets::get(motor_type) - .free_speed) - .to(); - duty_cycle = std::clamp(duty_cycle, -1.0, 1.0); + // Onboard control should only be used with low-current draw systems + duty_cycle = 0.0; break; } default: throw std::runtime_error( "Unsupported MotorMessage type in MotorMonkey WriteMessages"); } - total_current += frc846::control::calculators::CurrentTorqueCalculator:: - predict_current_draw(duty_cycle, velocity, battery_voltage, - circuit_resistance_registry[msg.slot_id], motor_type); + units::ampere_t pred_draw = + frc846::control::calculators::CurrentTorqueCalculator:: + predict_current_draw(duty_cycle, velocity, battery_voltage, + circuit_resistance_registry[msg.slot_id], motor_type); + second_total_current += units::math::max(0_A, pred_draw); + total_current += units::math::abs(pred_draw); temp_messages.pop(); } @@ -168,20 +228,33 @@ void MotorMonkey::WriteMessages(units::ampere_t max_draw) { const auto& msg = control_messages.front(); auto* controller = controller_registry[msg.slot_id]; + units::radians_per_second_t velocity = + controller_registry[msg.slot_id]->GetVelocity(); + + frc846::control::base::MotorMonkeyType motor_type = + slot_id_to_type_[msg.slot_id]; + size_t slot_id = msg.slot_id; switch (msg.type) { case MotorMessage::Type::DC: { - double duty_cycle_ = std::get(msg.value) * scale_factor; - if (!controller->IsDuplicateControlMessage(duty_cycle_)) { - controller->WriteDC(duty_cycle_); + double scaled_duty_cycle = + frc846::control::calculators::CurrentTorqueCalculator:: + scale_current_draw(scale_factor, std::get(msg.value), + velocity, battery_voltage, motor_type); + if (!controller->IsDuplicateControlMessage(scaled_duty_cycle) || + controller->GetLastErrorCode() != + frc846::control::hardware::ControllerErrorCodes::kAllOK) { + controller->WriteDC(scaled_duty_cycle); LOG_IF_ERROR("WriteDC"); } break; } case MotorMessage::Type::Position: { auto pos = std::get(msg.value); - if (!controller->IsDuplicateControlMessage(pos)) { + if (!controller->IsDuplicateControlMessage(pos) || + controller->GetLastErrorCode() != + frc846::control::hardware::ControllerErrorCodes::kAllOK) { controller->WritePosition(pos); LOG_IF_ERROR("WritePosition"); } @@ -189,7 +262,9 @@ void MotorMonkey::WriteMessages(units::ampere_t max_draw) { } case MotorMessage::Type::Velocity: { auto vel = std::get(msg.value); - if (!controller->IsDuplicateControlMessage(vel)) { + if (!controller->IsDuplicateControlMessage(vel) || + controller->GetLastErrorCode() != + frc846::control::hardware::ControllerErrorCodes::kAllOK) { controller->WriteVelocity(vel); LOG_IF_ERROR("WriteVelocity"); } @@ -199,6 +274,7 @@ void MotorMonkey::WriteMessages(units::ampere_t max_draw) { control_messages.pop(); } + return second_total_current; } bool MotorMonkey::VerifyConnected() { diff --git a/src/frc846/cpp/frc846/control/calculators/CurrentTorqueCalculator.cc b/src/frc846/cpp/frc846/control/calculators/CurrentTorqueCalculator.cc index 35124e5..c5afa01 100644 --- a/src/frc846/cpp/frc846/control/calculators/CurrentTorqueCalculator.cc +++ b/src/frc846/cpp/frc846/control/calculators/CurrentTorqueCalculator.cc @@ -1,5 +1,7 @@ #include "frc846/control/calculators/CurrentTorqueCalculator.h" +#include + #include namespace frc846::control::calculators { @@ -30,6 +32,30 @@ units::newton_meter_t CurrentTorqueCalculator::predict_torque(double duty_cycle, return current_to_torque(current_draw, specs); } +double CurrentTorqueCalculator::scale_current_draw(double scale_factor, + double duty_cycle, units::revolutions_per_minute_t rpm, + units::volt_t v_supply, MotorMonkeyType mmtype) { + MotorSpecs specs = MotorSpecificationPresets::get(mmtype); + double pct_speed = rpm / specs.free_speed; + units::volt_t back_emf = pct_speed * v_supply; + units::volt_t voltage_difference = duty_cycle * v_supply - back_emf; + units::volt_t output = voltage_difference * scale_factor + back_emf; + return output / v_supply; +} + +double CurrentTorqueCalculator::limit_current_draw(double duty_cycle, + units::ampere_t current_limit, units::revolutions_per_minute_t rpm, + units::volt_t v_supply, unit_ohm circuit_resistance, MotorSpecs specs) { + double duty_cycle_original = duty_cycle; + units::ampere_t current_draw = predict_current_draw( + duty_cycle, rpm, v_supply, circuit_resistance, specs); + if (units::math::abs(current_draw) > current_limit) { + return current_control(units::math::copysign(current_limit, current_draw), + rpm, v_supply, circuit_resistance, specs); + } + return duty_cycle; +} + units::ampere_t CurrentTorqueCalculator::torque_to_current( units::newton_meter_t torque, MotorSpecs specs) { return (torque / specs.stall_torque) * specs.stall_current; diff --git a/src/frc846/cpp/frc846/control/hardware/simulation/MCSimulator.cc b/src/frc846/cpp/frc846/control/hardware/simulation/MCSimulator.cc index ebf342c..cdc0f10 100644 --- a/src/frc846/cpp/frc846/control/hardware/simulation/MCSimulator.cc +++ b/src/frc846/cpp/frc846/control/hardware/simulation/MCSimulator.cc @@ -1,5 +1,7 @@ #include "frc846/control/hardware/simulation/MCSimulator.h" +#include + #include "frc846/control/calculators/CurrentTorqueCalculator.h" #include "frc846/control/calculators/VelocityPositionEstimator.h" @@ -68,7 +70,9 @@ void MCSimulator::WritePosition(units::radian_t position) { control_message = position; } -units::ampere_t MCSimulator::GetCurrent() { return pred_current_; } +units::ampere_t MCSimulator::GetCurrent() { + return units::math::abs(pred_current_); +} units::radian_t MCSimulator::GetPosition() { return position_; } units::radians_per_second_t MCSimulator::GetVelocity() { return velocity_; } diff --git a/src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc b/src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc new file mode 100644 index 0000000..8ae83d8 --- /dev/null +++ b/src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc @@ -0,0 +1,76 @@ +#include "frc846/math/DoubleSyncBuffer.h" + +#include +#include + +namespace frc846::math { + +DoubleSyncBuffer::DoubleSyncBuffer(size_t sz, int max_sync_diff) + : def_size{sz}, max_sync_diff_{max_sync_diff} { + m_buffer_1.reserve(def_size); + m_buffer_2.reserve(def_size); +} + +void DoubleSyncBuffer::Add(double val1, double val2) { + m_buffer_1.push_back(val1); + m_buffer_2.push_back(val2); + + if (m_buffer_1.size() > def_size) { + m_buffer_1.erase(m_buffer_1.begin()); + m_buffer_2.erase(m_buffer_2.begin()); + } +} + +bool DoubleSyncBuffer::IsValid() { + if (m_buffer_1.size() != m_buffer_2.size()) + throw std::runtime_error( + "DoubleSyncBuffer contained buffer sizes are inequal"); + + return m_buffer_1.size() == def_size; +} + +void DoubleSyncBuffer::Sync() { + if (!IsValid()) + throw std::runtime_error( + "DoubleSyncBuffer::Sync() called on invalid buffer"); + + sync_diff_ = 0; + + double max_correlation = -1.0; + + for (int shift = 0; shift < max_sync_diff_; shift++) { + double correlation = 0.0; + size_t sz = m_buffer_2.size() - shift; + for (size_t i = 0; i < sz; i++) { + correlation += m_buffer_1[i] * m_buffer_2[i + shift]; + } + correlation /= sz; + if (correlation > max_correlation) { + max_correlation = correlation; + sync_diff_ = shift; + } + } +} + +std::pair DoubleSyncBuffer::GetTrough() { + if (!IsValid()) + throw std::runtime_error( + "DoubleSyncBuffer::GetTrough() called on invalid buffer"); + + double min_combination = 100000000.0; + double min1 = 100000000.0; + double min2 = 100000000.0; + + for (size_t i = 1; i < m_buffer_2.size() - sync_diff_; i++) { + double added = m_buffer_1[i] * m_buffer_2[i + sync_diff_]; + if (added < min_combination) { + min_combination = added; + min1 = m_buffer_1[i]; + min2 = m_buffer_2[i + sync_diff_]; + } + } + + return std::make_pair(min1, min2); +} + +} // namespace frc846::math \ No newline at end of file diff --git a/src/frc846/cpp/frc846/robot/GenericRobot.cc b/src/frc846/cpp/frc846/robot/GenericRobot.cc index 8ced2ca..95e96fe 100644 --- a/src/frc846/cpp/frc846/robot/GenericRobot.cc +++ b/src/frc846/cpp/frc846/robot/GenericRobot.cc @@ -31,8 +31,6 @@ GenericRobot::GenericRobot(GenericRobotContainer* container) FRC_CheckErrorStatus(status, "{}", "InitializeNotifier"); HAL_SetNotifierName(notifier_, "Robot", &status); - - RegisterPreference("permissible_current_draw", 300_A); } GenericRobot::~GenericRobot() { @@ -56,6 +54,9 @@ void GenericRobot::StartCompetition() { frc846::base::FunkyLogSystem::Start(20000); + // Setup MotorMonkey + frc846::control::MotorMonkey::Setup(); + // Add dashboard buttons frc::SmartDashboard::PutData("verify_hardware", @@ -177,9 +178,7 @@ void GenericRobot::StartCompetition() { generic_robot_container_->UpdateHardware(); // Tick MotorMonkey - frc846::control::MotorMonkey::Tick( - GetPreferenceValue_unit_type( - "permissible_current_draw")); + frc846::control::MotorMonkey::Tick(mode == Mode::kDisabled); // Update dashboards frc::SmartDashboard::UpdateValues(); diff --git a/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc b/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc index 62247de..9a3125a 100644 --- a/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc +++ b/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc @@ -9,7 +9,7 @@ DrivetrainSubsystem::DrivetrainSubsystem(DrivetrainConfigs configs) : GenericSubsystem{"SwerveDrivetrain"}, configs_{configs}, modules_{}, - navX_{studica::AHRS::NavXComType::kMXP_SPI} { + navX_{configs.navX_connection_mode} { for (int i = 0; i < 4; i++) { modules_[i] = new SwerveModuleSubsystem{*this, configs_.module_unique_configs[i], configs_.module_common_config}; diff --git a/src/frc846/cpp/frc846/robot/swerve/odometry/swerve_odometry_calculator.cc b/src/frc846/cpp/frc846/robot/swerve/odometry/swerve_odometry_calculator.cc index 59a19f1..1641e16 100644 --- a/src/frc846/cpp/frc846/robot/swerve/odometry/swerve_odometry_calculator.cc +++ b/src/frc846/cpp/frc846/robot/swerve/odometry/swerve_odometry_calculator.cc @@ -12,12 +12,11 @@ SwerveOdometryOutput SwerveOdometryCalculator::calculate( frc846::math::Vector2D displacement{0_ft, 0_ft}; for (int i = 0; i < 4; i++) { - displacement += - frc846::math::Vector2D{module_diffs[i], inputs.steer_pos[i], true} / - 4.0; + displacement += frc846::math::Vector2D{module_diffs[i], + inputs.steer_pos[i] + inputs.bearing, true} / + 4.0; } displacement *= inputs.odom_ff_; - displacement.rotate(inputs.bearing, true); last_position_ += displacement; diff --git a/src/frc846/include/frc846/control/MotorMonkey.h b/src/frc846/include/frc846/control/MotorMonkey.h index e7e3ec3..c7194d7 100644 --- a/src/frc846/include/frc846/control/MotorMonkey.h +++ b/src/frc846/include/frc846/control/MotorMonkey.h @@ -14,6 +14,7 @@ #include "frc846/control/config/construction_params.h" #include "frc846/control/config/status_frames.h" #include "frc846/control/hardware/TalonFX_interm.h" +#include "frc846/math/DoubleSyncBuffer.h" #define CONTROLLER_REGISTRY_SIZE 64 @@ -27,20 +28,28 @@ CAN utilization as well as power. */ class MotorMonkey { public: + /* + Setup() + + Sets up the preferences used by MotorMonkey. + */ + static void Setup(); + /* Tick() Updates all motor controllers. Should be called each loop. */ - static void Tick(units::ampere_t max_draw); + static void Tick(bool disabled); /* WriteMessages() Writes all messages in the message queue to the motor controllers. Also, dynamically manages current draw and drops redundant messages. + Returns predicted total current draw. */ - static void WriteMessages(units::ampere_t max_draw); + static units::ampere_t WriteMessages(units::ampere_t max_draw); /* ConstructController() @@ -112,6 +121,8 @@ class MotorMonkey { static bool VerifyConnected(); private: + static void RecalculateMaxDraw(); + static frc846::base::Loggable loggable_; static size_t slot_counter_; @@ -130,6 +141,11 @@ class MotorMonkey { circuit_resistance_registry[CONTROLLER_REGISTRY_SIZE]; static units::volt_t battery_voltage; + static units::volt_t last_disabled_voltage; + + static frc846::math::DoubleSyncBuffer sync_buffer; + + static units::ampere_t max_draw_; struct MotorMessage { enum class Type { DC, Position, Velocity }; diff --git a/src/frc846/include/frc846/control/calculators/CurrentTorqueCalculator.h b/src/frc846/include/frc846/control/calculators/CurrentTorqueCalculator.h index 3ce74e2..f1d014e 100644 --- a/src/frc846/include/frc846/control/calculators/CurrentTorqueCalculator.h +++ b/src/frc846/include/frc846/control/calculators/CurrentTorqueCalculator.h @@ -44,6 +44,13 @@ class CurrentTorqueCalculator { MotorSpecificationPresets::get(mmtype)); } + static double scale_current_draw(double scale_factor, double duty_cycle, + units::revolutions_per_minute_t rpm, units::volt_t v_supply, + MotorMonkeyType mmtype); + + static double limit_current_draw(double duty_cycle, + units::ampere_t current_limit, units::revolutions_per_minute_t rpm, + units::volt_t v_supply, unit_ohm circuit_resistance, MotorSpecs specs); /* predict_torque() diff --git a/src/frc846/include/frc846/math/DoubleSyncBuffer.h b/src/frc846/include/frc846/math/DoubleSyncBuffer.h new file mode 100644 index 0000000..c70a07f --- /dev/null +++ b/src/frc846/include/frc846/math/DoubleSyncBuffer.h @@ -0,0 +1,52 @@ +#pragma once + +#include + +#include + +namespace frc846::math { + +/* +DoubleSyncBuffer + +A class that contains two rolling buffers of doubles and syncs them. +*/ +class DoubleSyncBuffer { +public: + /* + DoubleSyncBuffer() + + Constructs a DoubleSyncBuffer with the given size. + */ + DoubleSyncBuffer(size_t sz = 50U, int max_sync_diff = 15); + + /* + Add() + + Adds a value to each contained buffer. The values may be unsynced. Sync() will + be called automatically. + The second signal may be time delayed. + */ + void Add(double val1, double val2); + + // Computes the sync difference between the two buffers + void Sync(); + + bool IsValid(); + + // Returns the computed sync difference between each container buffer + int GetSyncDiff() { return sync_diff_; } + + std::pair GetTrough(); + +private: + std::vector m_buffer_1; + std::vector m_buffer_2; + + size_t def_size; + int max_sync_diff_; + + int sync_diff_; +}; + +} // namespace frc846::math \ No newline at end of file diff --git a/src/frc846/include/frc846/robot/GenericRobot.h b/src/frc846/include/frc846/robot/GenericRobot.h index 95747a9..bd93a01 100644 --- a/src/frc846/include/frc846/robot/GenericRobot.h +++ b/src/frc846/include/frc846/robot/GenericRobot.h @@ -15,7 +15,7 @@ enum Mode { kNone, kDisabled, kAutonomous, kTeleop, kTest }; class GenericRobot : public frc::RobotBase, public frc846::base::Loggable { public: - static constexpr auto kPeriod = 20_ms; // 50hz + static constexpr auto kPeriod = 10_ms; // 100Hz GenericRobot(GenericRobotContainer* container); diff --git a/src/frc846/include/frc846/robot/swerve/drivetrain.h b/src/frc846/include/frc846/robot/swerve/drivetrain.h index a30bbcb..1449ac0 100644 --- a/src/frc846/include/frc846/robot/swerve/drivetrain.h +++ b/src/frc846/include/frc846/robot/swerve/drivetrain.h @@ -12,18 +12,13 @@ namespace frc846::robot::swerve { -enum NavX_connection_type { - kSPI, - kSerial, -}; - /* DrivetrainConfigs Contains all configs related to the specific drivetrain in use. */ struct DrivetrainConfigs { - NavX_connection_type navX_connection_mode; + studica::AHRS::NavXComType navX_connection_mode; SwerveModuleCommonConfig module_common_config; std::array module_unique_configs; diff --git a/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc b/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc index 265c7cf..bffd013 100644 --- a/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc +++ b/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc @@ -25,8 +25,7 @@ DrivetrainConstructor::getDrivetrainConfigs() { TODO: Set these values during season */ - configs.navX_connection_mode = - frc846::robot::swerve::NavX_connection_type::kSerial; + configs.navX_connection_mode = studica::AHRS::NavXComType::kMXP_SPI; units::inch_t wheel_diameter = 4_in;