From 2f904fb2db2a521ee01418a480c0b591e1414dda Mon Sep 17 00:00:00 2001 From: VyaasBaskar Date: Sat, 11 Jan 2025 22:20:03 -0800 Subject: [PATCH 1/7] 2025 updates testing and odometry testing --- src/frc846/cpp/frc846/robot/swerve/drivetrain.cc | 8 ++++---- .../robot/swerve/odometry/swerve_odometry_calculator.cc | 7 +++---- src/frc846/include/frc846/robot/swerve/drivetrain.h | 7 +------ .../cpp/subsystems/hardware/DrivetrainConstructor.cc | 3 +-- 4 files changed, 9 insertions(+), 16 deletions(-) diff --git a/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc b/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc index 62247de..16a904f 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}; @@ -118,9 +118,9 @@ DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() { frc846::robot::swerve::odometry::SwervePose new_pose{ .position = odometry_ - .calculate({bearing, steer_positions, drive_positions, - GetPreferenceValue_double("odom_fudge_factor")}) - .position, + .calculate({bearing, steer_positions, drive_positions, + GetPreferenceValue_double("odom_fudge_factor")}) + .position, .bearing = bearing, .velocity = velocity, }; 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/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 685fad2..8cedd0b 100644 --- a/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc +++ b/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc @@ -24,8 +24,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; From 2aff36e0851b09b43418e2389b2351bfc29d64f7 Mon Sep 17 00:00:00 2001 From: prak132 Date: Sun, 12 Jan 2025 14:56:21 -0800 Subject: [PATCH 2/7] Added current and duty cycle limiting functions --- src/frc846/cpp/frc846/control/MotorMonkey.cc | 32 ++++++++++++++----- .../calculators/CurrentTorqueCalculator.cc | 27 ++++++++++++++++ .../hardware/simulation/MCSimulator.cc | 6 +++- .../cpp/frc846/robot/swerve/drivetrain.cc | 6 ++-- .../calculators/CurrentTorqueCalculator.h | 7 ++++ 5 files changed, 66 insertions(+), 12 deletions(-) diff --git a/src/frc846/cpp/frc846/control/MotorMonkey.cc b/src/frc846/cpp/frc846/control/MotorMonkey.cc index 53af625..1d78783 100644 --- a/src/frc846/cpp/frc846/control/MotorMonkey.cc +++ b/src/frc846/cpp/frc846/control/MotorMonkey.cc @@ -147,9 +147,10 @@ void MotorMonkey::WriteMessages(units::ampere_t max_draw) { 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); + total_current += + units::math::abs(frc846::control::calculators::CurrentTorqueCalculator:: + predict_current_draw(duty_cycle, velocity, battery_voltage, + circuit_resistance_registry[msg.slot_id], motor_type)); temp_messages.pop(); } @@ -168,20 +169,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 +203,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"); } diff --git a/src/frc846/cpp/frc846/control/calculators/CurrentTorqueCalculator.cc b/src/frc846/cpp/frc846/control/calculators/CurrentTorqueCalculator.cc index 35124e5..84a78c5 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,31 @@ 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; + unit_ohm winding_resistance = + 12_V / (specs.stall_current - specs.free_current); + 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) { + units::ampere_t current_draw = units::math::abs(predict_current_draw( + duty_cycle, rpm, v_supply, circuit_resistance, specs)); + if (current_draw > current_limit) { + return current_control( + current_limit, 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/robot/swerve/drivetrain.cc b/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc index 16a904f..9a3125a 100644 --- a/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc +++ b/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc @@ -118,9 +118,9 @@ DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() { frc846::robot::swerve::odometry::SwervePose new_pose{ .position = odometry_ - .calculate({bearing, steer_positions, drive_positions, - GetPreferenceValue_double("odom_fudge_factor")}) - .position, + .calculate({bearing, steer_positions, drive_positions, + GetPreferenceValue_double("odom_fudge_factor")}) + .position, .bearing = bearing, .velocity = 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() From b194641842d8152e3c321ed9596df297309fc4db Mon Sep 17 00:00:00 2001 From: VyaasBaskar Date: Sun, 12 Jan 2025 15:42:10 -0800 Subject: [PATCH 3/7] Fixed current limiting, decreased loop time --- .../cpp/frc846/control/HigherMotorController.cc | 5 +++++ .../control/calculators/CurrentTorqueCalculator.cc | 13 ++++++------- src/frc846/include/frc846/robot/GenericRobot.h | 2 +- 3 files changed, 12 insertions(+), 8 deletions(-) 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/calculators/CurrentTorqueCalculator.cc b/src/frc846/cpp/frc846/control/calculators/CurrentTorqueCalculator.cc index 84a78c5..c5afa01 100644 --- a/src/frc846/cpp/frc846/control/calculators/CurrentTorqueCalculator.cc +++ b/src/frc846/cpp/frc846/control/calculators/CurrentTorqueCalculator.cc @@ -37,8 +37,6 @@ double CurrentTorqueCalculator::scale_current_draw(double scale_factor, units::volt_t v_supply, MotorMonkeyType mmtype) { MotorSpecs specs = MotorSpecificationPresets::get(mmtype); double pct_speed = rpm / specs.free_speed; - unit_ohm winding_resistance = - 12_V / (specs.stall_current - specs.free_current); 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; @@ -48,11 +46,12 @@ double CurrentTorqueCalculator::scale_current_draw(double scale_factor, 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) { - units::ampere_t current_draw = units::math::abs(predict_current_draw( - duty_cycle, rpm, v_supply, circuit_resistance, specs)); - if (current_draw > current_limit) { - return current_control( - current_limit, rpm, v_supply, circuit_resistance, 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; } 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); From 9765d28e70848709103ad609b30e41205def8f8a Mon Sep 17 00:00:00 2001 From: VyaasBaskar Date: Tue, 14 Jan 2025 09:01:01 -0800 Subject: [PATCH 4/7] Added dynamic maximum current prediction; Added DoubleSyncBuffer --- README.md | 1 + build.gradle | 2 +- networktables.json | 72 ++++++++++++++++++ networktables.json.bck | 48 ++++++++++++ src/frc846/cpp/frc846/control/MotorMonkey.cc | 61 +++++++++++++-- .../cpp/frc846/math/DoubleSyncBuffer.cc | 75 +++++++++++++++++++ src/frc846/cpp/frc846/robot/GenericRobot.cc | 9 +-- .../include/frc846/control/MotorMonkey.h | 20 ++++- .../include/frc846/math/DoubleSyncBuffer.h | 53 +++++++++++++ 9 files changed, 326 insertions(+), 15 deletions(-) create mode 100644 src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc create mode 100644 src/frc846/include/frc846/math/DoubleSyncBuffer.h 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..787063f 100644 --- a/build.gradle +++ b/build.gradle @@ -37,7 +37,7 @@ deploy { def deployArtifact = deploy.targets.roborio.artifacts.frcCpp // Set this to true to enable desktop support. -def includeDesktopSupport = false +def includeDesktopSupport = true // Set to true to run simulation in debug mode wpi.cpp.debugSimulation = true diff --git a/networktables.json b/networktables.json index c15bf00..c07b524 100644 --- a/networktables.json +++ b/networktables.json @@ -182,5 +182,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/networktables.json.bck b/networktables.json.bck index 903bd45..c15bf00 100644 --- a/networktables.json.bck +++ b/networktables.json.bck @@ -134,5 +134,53 @@ "properties": { "persistent": true } + }, + { + "name": "/Preferences/SwerveDrivetrain/SwerveDrivetrain/max_speed (fps)", + "type": "double", + "value": 15.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/SwerveDrivetrain/SwerveDrivetrain/max_omega (deg_per_s)", + "type": "double", + "value": 180.0, + "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 + } } ] diff --git a/src/frc846/cpp/frc846/control/MotorMonkey.cc b/src/frc846/cpp/frc846/control/MotorMonkey.cc index 1d78783..b082d04 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,63 @@ 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", 9.5_V); + loggable_.RegisterPreference("default_max_draw", 150.0_A); + loggable_.RegisterPreference("battery_cc", 600_A); + + max_draw_ = loggable_.GetPreferenceValue_unit_type( + "default_max_draw"); +} + +void MotorMonkey::RecalculateMaxDraw() { + auto sy_trough = sync_buffer.GetTrough(); + units::ampere_t cc_current{sy_trough.first}; + 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; + + max_draw_ = current_draw * (last_disabled_voltage - voltage_min) / + (last_disabled_voltage - voltage); +} + +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_); + units::ampere_t cc_current = + loggable_.GetPreferenceValue_unit_type("battery_cc") - + total_pred_draw; + sync_buffer.Add(cc_current.to(), battery_voltage.to()); - WriteMessages(max_draw); + RecalculateMaxDraw(); - // TODO: Improve battery voltage estimation for simulation + loggable_.Graph("max_draw", max_draw_); for (size_t i = 0; i < CONTROLLER_REGISTRY_SIZE; i++) { if (controller_registry[i] != nullptr) { @@ -97,7 +143,7 @@ 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; std::queue temp_messages{control_messages}; @@ -215,6 +261,7 @@ void MotorMonkey::WriteMessages(units::ampere_t max_draw) { control_messages.pop(); } + return total_current; } bool MotorMonkey::VerifyConnected() { diff --git a/src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc b/src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc new file mode 100644 index 0000000..ac530d9 --- /dev/null +++ b/src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc @@ -0,0 +1,75 @@ +#include "frc846/math/DoubleSyncBuffer.h" + +#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()); + } + + if (IsValid()) Sync(); +} + +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; + for (size_t i = 0; i < m_buffer_1.size(); i++) { + correlation += m_buffer_1[i] * m_buffer_2[i + max_sync_diff_]; + } + 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/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/math/DoubleSyncBuffer.h b/src/frc846/include/frc846/math/DoubleSyncBuffer.h new file mode 100644 index 0000000..a90a0c1 --- /dev/null +++ b/src/frc846/include/frc846/math/DoubleSyncBuffer.h @@ -0,0 +1,53 @@ +#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); + + bool IsValid(); + + // Returns the computed sync difference between each container buffer + int GetSyncDiff() { return sync_diff_; } + + std::pair GetTrough(); + +private: + // Computes the sync difference between the two buffers + void Sync(); + + 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 From 8be36bbf3e97f18645f0cfdecc903a953d4974dd Mon Sep 17 00:00:00 2001 From: Vyaas Baskar <68795520+VyaasBaskar@users.noreply.github.com> Date: Tue, 14 Jan 2025 09:06:46 -0800 Subject: [PATCH 5/7] Update build.gradle: removed includeDesktopSupport --- build.gradle | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/build.gradle b/build.gradle index 787063f..8014394 100644 --- a/build.gradle +++ b/build.gradle @@ -37,7 +37,7 @@ deploy { def deployArtifact = deploy.targets.roborio.artifacts.frcCpp // Set this to true to enable desktop support. -def includeDesktopSupport = true +def includeDesktopSupport = false // Set to true to run simulation in debug mode wpi.cpp.debugSimulation = true @@ -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 +} From e1ec2830797bcda583c55022017ba886e16ffa85 Mon Sep 17 00:00:00 2001 From: VyaasBaskar Date: Tue, 14 Jan 2025 20:47:02 -0800 Subject: [PATCH 6/7] Dynamic prediction and voltage drop limiting tested --- src/frc846/cpp/frc846/control/MotorMonkey.cc | 69 +++++++++++-------- .../cpp/frc846/math/DoubleSyncBuffer.cc | 5 +- .../include/frc846/math/DoubleSyncBuffer.h | 7 +- 3 files changed, 46 insertions(+), 35 deletions(-) diff --git a/src/frc846/cpp/frc846/control/MotorMonkey.cc b/src/frc846/cpp/frc846/control/MotorMonkey.cc index b082d04..f01d070 100644 --- a/src/frc846/cpp/frc846/control/MotorMonkey.cc +++ b/src/frc846/cpp/frc846/control/MotorMonkey.cc @@ -82,17 +82,27 @@ std::queue MotorMonkey::control_messages{}; void MotorMonkey::Setup() { loggable_.RegisterPreference("voltage_min", 7.5_V); - loggable_.RegisterPreference("recal_voltage_thresh", 9.5_V); + loggable_.RegisterPreference("recal_voltage_thresh", 10.5_V); loggable_.RegisterPreference("default_max_draw", 150.0_A); - loggable_.RegisterPreference("battery_cc", 600_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; @@ -106,8 +116,20 @@ void MotorMonkey::RecalculateMaxDraw() { if (voltage > recal_voltage_thresh) return; - max_draw_ = current_draw * (last_disabled_voltage - voltage_min) / - (last_disabled_voltage - voltage); + 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) { @@ -121,6 +143,9 @@ void MotorMonkey::Tick(bool disabled) { } units::ampere_t total_pred_draw = WriteMessages(max_draw_); + + loggable_.Graph("total_pred_draw", total_pred_draw); + units::ampere_t cc_current = loggable_.GetPreferenceValue_unit_type("battery_cc") - total_pred_draw; @@ -145,6 +170,7 @@ void MotorMonkey::Tick(bool disabled) { 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; @@ -158,8 +184,6 @@ units::ampere_t 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) { @@ -167,36 +191,25 @@ units::ampere_t 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 += - units::math::abs(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(); } @@ -261,7 +274,7 @@ units::ampere_t MotorMonkey::WriteMessages(units::ampere_t max_draw) { control_messages.pop(); } - return total_current; + return second_total_current; } bool MotorMonkey::VerifyConnected() { diff --git a/src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc b/src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc index ac530d9..169f808 100644 --- a/src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc +++ b/src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc @@ -1,5 +1,6 @@ #include "frc846/math/DoubleSyncBuffer.h" +#include #include namespace frc846::math { @@ -18,8 +19,6 @@ void DoubleSyncBuffer::Add(double val1, double val2) { m_buffer_1.erase(m_buffer_1.begin()); m_buffer_2.erase(m_buffer_2.begin()); } - - if (IsValid()) Sync(); } bool DoubleSyncBuffer::IsValid() { @@ -42,7 +41,7 @@ void DoubleSyncBuffer::Sync() { for (int shift = 0; shift < max_sync_diff_; shift++) { double correlation = 0.0; for (size_t i = 0; i < m_buffer_1.size(); i++) { - correlation += m_buffer_1[i] * m_buffer_2[i + max_sync_diff_]; + correlation += m_buffer_1[i] * m_buffer_2[i + shift]; } if (correlation > max_correlation) { max_correlation = correlation; diff --git a/src/frc846/include/frc846/math/DoubleSyncBuffer.h b/src/frc846/include/frc846/math/DoubleSyncBuffer.h index a90a0c1..c70a07f 100644 --- a/src/frc846/include/frc846/math/DoubleSyncBuffer.h +++ b/src/frc846/include/frc846/math/DoubleSyncBuffer.h @@ -25,11 +25,13 @@ class DoubleSyncBuffer { 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 @@ -38,9 +40,6 @@ class DoubleSyncBuffer { std::pair GetTrough(); private: - // Computes the sync difference between the two buffers - void Sync(); - std::vector m_buffer_1; std::vector m_buffer_2; From df66b9c94ddf5e60932bfb8e7c045ed487a9e1a1 Mon Sep 17 00:00:00 2001 From: VyaasBaskar Date: Wed, 15 Jan 2025 14:58:01 -0800 Subject: [PATCH 7/7] Modify DoubleSyncBuffer sync --- src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc b/src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc index 169f808..8ae83d8 100644 --- a/src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc +++ b/src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc @@ -40,9 +40,11 @@ void DoubleSyncBuffer::Sync() { for (int shift = 0; shift < max_sync_diff_; shift++) { double correlation = 0.0; - for (size_t i = 0; i < m_buffer_1.size(); i++) { + 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;