From 6557d30eac7446cc676aaf44165ec69f9ebf5d97 Mon Sep 17 00:00:00 2001 From: VyaasBaskar Date: Sun, 19 Jan 2025 16:09:11 -0800 Subject: [PATCH] Anti-tipping code finished, tested --- .vscode/settings.json | 4 +- networktables.json | 16 +++++++ simgui-ds.json | 5 +++ src/frc846/cpp/frc846/control/MotorMonkey.cc | 13 ++++++ .../frc846/control/hardware/TalonFX_interm.cc | 4 +- .../hardware/simulation/MCSimulator.cc | 30 ++++++++++--- .../cpp/frc846/math/DoubleSyncBuffer.cc | 9 +--- src/frc846/cpp/frc846/math/RampRateLimiter.cc | 14 ++----- .../cpp/frc846/robot/swerve/drivetrain.cc | 8 ++-- .../cpp/frc846/robot/swerve/swerve_module.cc | 14 +++++-- src/frc846/include/frc846/control/HMCHelper.h | 22 ++++++++-- .../include/frc846/control/MotorMonkey.h | 2 + .../include/frc846/math/RampRateLimiter.h | 2 - src/frc846/include/frc846/math/vectors.h | 5 ++- .../cpp/calculators/AntiTippingCalculator.cc | 42 +++++++------------ .../cpp/commands/teleop/drive_command.cc | 19 +++++---- .../calculators/AntiTippingCalculator.h | 3 +- .../include/subsystems/robot_constants.h | 4 +- 18 files changed, 137 insertions(+), 79 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 8fa1620..b5e5878 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -114,7 +114,9 @@ "xtr1common": "cpp", "xtree": "cpp", "xutility": "cpp", - "queue": "cpp" + "queue": "cpp", + "expected": "cpp", + "source_location": "cpp" }, "wpilib.useWindbgX": true, "C_Cpp.errorSquiggles": "enabled", diff --git a/networktables.json b/networktables.json index b89113a..22440e2 100644 --- a/networktables.json +++ b/networktables.json @@ -262,5 +262,21 @@ "properties": { "persistent": true } + }, + { + "name": "/Preferences/MotorMonkey/MotorMonkey/min_max_draw (A)", + "type": "double", + "value": 40.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/MotorMonkey/MotorMonkey/max_max_draw (A)", + "type": "double", + "value": 300.0, + "properties": { + "persistent": true + } } ] diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..07f1415 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "Keyboard 0 Settings": { + "window": { + "visible": true + } + }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/frc846/cpp/frc846/control/MotorMonkey.cc b/src/frc846/cpp/frc846/control/MotorMonkey.cc index b81b27d..7d0db96 100644 --- a/src/frc846/cpp/frc846/control/MotorMonkey.cc +++ b/src/frc846/cpp/frc846/control/MotorMonkey.cc @@ -80,6 +80,8 @@ units::ampere_t MotorMonkey::max_draw_{0.0_A}; std::queue MotorMonkey::control_messages{}; +int MotorMonkey::num_loops_last_brown = 4000; + void MotorMonkey::Setup() { loggable_.RegisterPreference("voltage_min", 7.5_V); loggable_.RegisterPreference("recal_voltage_thresh", 10.5_V); @@ -87,6 +89,7 @@ void MotorMonkey::Setup() { loggable_.RegisterPreference("min_max_draw", 40_A); loggable_.RegisterPreference("max_max_draw", 300_A); loggable_.RegisterPreference("battery_cc", 400_A); + loggable_.RegisterPreference("brownout_perm_loops", 500); max_draw_ = loggable_.GetPreferenceValue_unit_type( "default_max_draw"); @@ -95,6 +98,14 @@ void MotorMonkey::Setup() { void MotorMonkey::RecalculateMaxDraw() { if (!sync_buffer.IsValid()) return; + if (frc::RobotController::IsBrownedOut()) num_loops_last_brown = 0; + if (num_loops_last_brown < + loggable_.GetPreferenceValue_int("brownout_perm_loops")) { + max_draw_ = + loggable_.GetPreferenceValue_unit_type("min_max_draw"); + return; + } + sync_buffer.Sync(); loggable_.Graph("sync_diff_", sync_buffer.GetSyncDiff()); @@ -163,6 +174,8 @@ void MotorMonkey::Tick(bool disabled) { dynamic_cast(controller_registry[i]); sim->SetBatteryVoltage(battery_voltage); sim->SetLoad(load_registry[i]); + // sim->Tick(); + // TODO: fix sim } } } diff --git a/src/frc846/cpp/frc846/control/hardware/TalonFX_interm.cc b/src/frc846/cpp/frc846/control/hardware/TalonFX_interm.cc index 3916d55..6b8348e 100644 --- a/src/frc846/cpp/frc846/control/hardware/TalonFX_interm.cc +++ b/src/frc846/cpp/frc846/control/hardware/TalonFX_interm.cc @@ -94,9 +94,9 @@ void TalonFX_interm::EnableStatusFrames( last_status_code = talon_.GetSupplyCurrent().SetUpdateFrequency(10_Hz); } else if (frame == frc846::control::config::StatusFrame::kPositionFrame) { last_status_code = - talon_.GetPosition().SetUpdateFrequency(50_Hz, max_wait_time_); + talon_.GetPosition().SetUpdateFrequency(200_Hz, max_wait_time_); } else if (frame == frc846::control::config::StatusFrame::kVelocityFrame) { - last_status_code = talon_.GetVelocity().SetUpdateFrequency(50_Hz); + last_status_code = talon_.GetVelocity().SetUpdateFrequency(200_Hz); } last_error_ = getErrorCode(last_status_code); if (last_error_ != ControllerErrorCodes::kAllOK) return; diff --git a/src/frc846/cpp/frc846/control/hardware/simulation/MCSimulator.cc b/src/frc846/cpp/frc846/control/hardware/simulation/MCSimulator.cc index cdc0f10..6385752 100644 --- a/src/frc846/cpp/frc846/control/hardware/simulation/MCSimulator.cc +++ b/src/frc846/cpp/frc846/control/hardware/simulation/MCSimulator.cc @@ -2,6 +2,8 @@ #include +#include + #include "frc846/control/calculators/CurrentTorqueCalculator.h" #include "frc846/control/calculators/VelocityPositionEstimator.h" @@ -29,6 +31,8 @@ void MCSimulator::Tick() { duty_cycle = gains.calculate( pos->to(), 0.0, velocity_.to(), load_.to()); } + duty_cycle = std::clamp(duty_cycle, -1.0, 1.0); + pred_current_ = frc846::control::calculators::CurrentTorqueCalculator:: predict_current_draw( duty_cycle, velocity_, battery_voltage_, circuit_resistance_, specs); @@ -37,8 +41,6 @@ void MCSimulator::Tick() { pred_current_, specs); torque_output -= load_; - if (inverted) torque_output = -torque_output; - std::chrono::microseconds current_time = std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch()); @@ -62,21 +64,31 @@ void MCSimulator::SetInverted(bool inverted) { position_ = -position_; } -void MCSimulator::WriteDC(double duty_cycle) { control_message = duty_cycle; } +void MCSimulator::WriteDC(double duty_cycle) { + if (inverted) duty_cycle = -duty_cycle; + control_message = duty_cycle; +} void MCSimulator::WriteVelocity(units::radians_per_second_t velocity) { + if (inverted) velocity = -velocity; control_message = velocity; } void MCSimulator::WritePosition(units::radian_t position) { + if (inverted) position_ = -position_; control_message = position; } 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_; } +units::radian_t MCSimulator::GetPosition() { + return inverted ? -position_ : position_; +} +units::radians_per_second_t MCSimulator::GetVelocity() { + return inverted ? -velocity_ : velocity_; +} void MCSimulator::ZeroEncoder(units::radian_t position) { + if (inverted) position = -position; this->position_ = position; } @@ -103,15 +115,18 @@ void MCSimulator::EnableStatusFrames( } bool MCSimulator::IsDuplicateControlMessage(double duty_cycle) { + if (inverted) duty_cycle = -duty_cycle; return std::holds_alternative(control_message) && std::get(control_message) == duty_cycle; } bool MCSimulator::IsDuplicateControlMessage( units::radians_per_second_t velocity) { + if (inverted) velocity = -velocity; return std::holds_alternative(control_message) && std::get(control_message) == velocity; } bool MCSimulator::IsDuplicateControlMessage(units::radian_t position) { + if (inverted) position = -position; return std::holds_alternative(control_message) && std::get(control_message) == position; } @@ -120,7 +135,10 @@ void MCSimulator::SetGains(frc846::control::base::MotorGains gains) { this->gains = gains; } -void MCSimulator::SetLoad(units::newton_meter_t load) { this->load_ = load; } +void MCSimulator::SetLoad(units::newton_meter_t load) { + if (inverted) load = -load; + this->load_ = load; +} void MCSimulator::SetBatteryVoltage(units::volt_t voltage) { this->battery_voltage_ = voltage; } diff --git a/src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc b/src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc index a36bd3a..1ad7116 100644 --- a/src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc +++ b/src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc @@ -57,17 +57,12 @@ std::pair DoubleSyncBuffer::GetTrough() { 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_2[i + sync_diff_]; - if (added < min_combination) { - min_combination = added; - min1 = m_buffer_1[i]; - min2 = m_buffer_2[i + sync_diff_]; - } + if (m_buffer_2[i + sync_diff_] < min2) min2 = m_buffer_2[i + sync_diff_]; + if (m_buffer_1[i] < min1) min1 = m_buffer_1[i]; } return std::make_pair(min1, min2); diff --git a/src/frc846/cpp/frc846/math/RampRateLimiter.cc b/src/frc846/cpp/frc846/math/RampRateLimiter.cc index d1c00df..bf2a83f 100644 --- a/src/frc846/cpp/frc846/math/RampRateLimiter.cc +++ b/src/frc846/cpp/frc846/math/RampRateLimiter.cc @@ -1,5 +1,7 @@ #include "frc846/math/RampRateLimiter.h" +#include + namespace frc846::math { RampRateLimiter::RampRateLimiter() { @@ -13,8 +15,9 @@ double RampRateLimiter::limit(double value, double rateLimit) { auto elapsedTime = currentTime - m_lastTime; m_lastTime = currentTime; - double maxChange = rateLimit * elapsedTime.count() / 1000.0; + double maxChange = std::abs(rateLimit * elapsedTime.count() / 1000.0); double change = value - m_lastValue; + if (change > maxChange) { value = m_lastValue + maxChange; } else if (change < -maxChange) { @@ -25,13 +28,4 @@ double RampRateLimiter::limit(double value, double rateLimit) { return value; } -double RampRateLimiter::findRate(double value) { - auto currentTime = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()); - auto elapsedTime = currentTime - m_lastTime; - m_lastTime = currentTime; - - return (value - m_lastValue) / (elapsedTime.count() / 1000.0); -} - } // namespace frc846::math \ No newline at end of file diff --git a/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc b/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc index 9a3125a..9be1c2e 100644 --- a/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc +++ b/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc @@ -42,8 +42,8 @@ void DrivetrainSubsystem::Setup() { module->InitByParent(); module->Setup(); module->SetSteerGains(steer_gains); - module->ZeroWithCANcoder(); } + ZeroBearing(); } DrivetrainTarget DrivetrainSubsystem::ZeroTarget() const { @@ -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/swerve_module.cc b/src/frc846/cpp/frc846/robot/swerve/swerve_module.cc index c026d9e..cece5b1 100644 --- a/src/frc846/cpp/frc846/robot/swerve/swerve_module.cc +++ b/src/frc846/cpp/frc846/robot/swerve/swerve_module.cc @@ -94,20 +94,26 @@ void SwerveModuleSubsystem::ZeroWithCANcoder() { Log("CANCoder zero attempt {}/{}", attempts, kMaxAttempts); auto position = cancoder_.GetAbsolutePosition(); + units::degree_t position_zero = + -position.GetValue() + + GetPreferenceValue_unit_type("cancoder_offset_"); + if (position.IsAllGood()) { - units::degree_t position_zero = - -position.GetValue() + - GetPreferenceValue_unit_type("cancoder_offset_"); steer_helper_.SetPosition(position_zero); Log("Zeroed to {}!", position_zero); return; + } else if (attempts == kMaxAttempts) { + Error("Unable to zero normally after {} attempts - attempting anyways", + kMaxAttempts); + steer_helper_.SetPosition(position_zero); + Warn("Unreliably zeroed to {}!", position_zero); + return; } Warn("Attempt to zero failed, sleeping {} ms...", kSleepTimeMs); std::this_thread::sleep_for(std::chrono::milliseconds(kSleepTimeMs)); } - Error("Unable to zero after {} attempts", kMaxAttempts); } SwerveModuleReadings SwerveModuleSubsystem::ReadFromHardware() { diff --git a/src/frc846/include/frc846/control/HMCHelper.h b/src/frc846/include/frc846/control/HMCHelper.h index 76f022f..7a45391 100644 --- a/src/frc846/include/frc846/control/HMCHelper.h +++ b/src/frc846/include/frc846/control/HMCHelper.h @@ -41,7 +41,7 @@ template class HMCHelper { void WritePosition(pos_unit position) { CHECK_HMC(); - hmc_->WritePosition(position / conv_); + hmc_->WritePosition(position / conv_ - mark_offset); } void WriteVelocityOnController(vel_unit velocity) { @@ -61,7 +61,7 @@ template class HMCHelper { pos_unit GetPosition() { CHECK_HMC(); - return hmc_->GetPosition() * conv_; + return hmc_->GetPosition() * conv_ + mark_offset; } units::current::ampere_t GetCurrent() { @@ -74,11 +74,23 @@ template class HMCHelper { hmc_->SetPosition(position / conv_); } + /* + OffsetPositionTo() + + Does NOT zero motor encoder. Simply calculates then stores an offset, then + adds it when GetPosition() is called. Subtracts offset when calling + WritePosition(). + */ + void OffsetPositionTo(pos_unit position) { + CHECK_HMC(); + mark_offset = position - GetPosition(); + } + /* SetControllerSoftLimits() - Set software limits for forward and reverse motion. Will be managed onboard - the motor controller. + Set software limits for forward and reverse motion. Will be managed + onboard the motor controller. */ void SetControllerSoftLimits(pos_unit forward_limit, pos_unit reverse_limit) { CHECK_HMC(); @@ -104,6 +116,8 @@ template class HMCHelper { HigherMotorController* hmc_; conv_unit conv_; + + pos_unit mark_offset{0}; }; } // namespace frc846::control \ No newline at end of file diff --git a/src/frc846/include/frc846/control/MotorMonkey.h b/src/frc846/include/frc846/control/MotorMonkey.h index c7194d7..2db61c8 100644 --- a/src/frc846/include/frc846/control/MotorMonkey.h +++ b/src/frc846/include/frc846/control/MotorMonkey.h @@ -155,6 +155,8 @@ class MotorMonkey { }; static std::queue control_messages; + + static int num_loops_last_brown; }; } // namespace frc846::control \ No newline at end of file diff --git a/src/frc846/include/frc846/math/RampRateLimiter.h b/src/frc846/include/frc846/math/RampRateLimiter.h index 591b947..503453b 100644 --- a/src/frc846/include/frc846/math/RampRateLimiter.h +++ b/src/frc846/include/frc846/math/RampRateLimiter.h @@ -15,8 +15,6 @@ class RampRateLimiter { double limit(double value, double rateLimit); - double findRate(double value); - private: double m_lastValue = 0.0; std::chrono::milliseconds m_lastTime; diff --git a/src/frc846/include/frc846/math/vectors.h b/src/frc846/include/frc846/math/vectors.h index 7daac12..f89b15d 100644 --- a/src/frc846/include/frc846/math/vectors.h +++ b/src/frc846/include/frc846/math/vectors.h @@ -206,7 +206,10 @@ template class VectorND { if (angleIsBearing) { return units::math::atan2(data[0], data[1]); } try { return units::math::atan2(data[1], data[0]); - } catch (std::exception& exc) { return 0_deg; } + } catch (std::exception& exc) { + (void)exc; + return 0_deg; + } } // Returns the angle between this vector and another diff --git a/src/y2025/cpp/calculators/AntiTippingCalculator.cc b/src/y2025/cpp/calculators/AntiTippingCalculator.cc index 940b498..bab0ea9 100644 --- a/src/y2025/cpp/calculators/AntiTippingCalculator.cc +++ b/src/y2025/cpp/calculators/AntiTippingCalculator.cc @@ -3,6 +3,8 @@ #include #include +#include + #include "frc846/math/constants.h" #include "subsystems/robot_constants.h" @@ -46,12 +48,13 @@ frc846::math::Vector3D AntiTippingCalculator::CalculateRobotCG() { frc846::math::VectorND AntiTippingCalculator::LimitAcceleration( - frc846::math::VectorND accel, + frc846::math::VectorND vel_vec, units::degree_t bearing) { - frc846::math::VectorND inertial = - accel.rotate(180_deg, true); - frc846::math::VectorND inertial_dir = - inertial.unit(); + if (units::math::abs(vel_vec[0]) < 0.05_fps) vel_vec[0] = 0.05_fps; + if (units::math::abs(vel_vec[1]) < 0.05_fps) vel_vec[1] = 0.05_fps; + + frc846::math::VectorND v_neg_dir = + vel_vec.rotate(180_deg, true).unit(); frc846::math::Vector3D robot_cg = CalculateRobotCG(); @@ -76,8 +79,7 @@ AntiTippingCalculator::LimitAcceleration( units::degree_t closest_angle = 360_deg; for (size_t i = 0; i < 4; i++) { - units::degree_t angle = - units::math::abs(inertial_dir.angleTo(wheel_vecs[i])); + units::degree_t angle = units::math::abs(v_neg_dir.angleTo(wheel_vecs[i])); if (angle < closest_angle) { closest_angle = angle; closest_wheel_vec = i; @@ -87,30 +89,16 @@ AntiTippingCalculator::LimitAcceleration( frc846::math::Vector2D closest_wheel = wheel_vecs[closest_wheel_vec]; frc846::math::Vector2D effective_wheel_vec = - inertial_dir.projectOntoThis(closest_wheel); + v_neg_dir.projectOntoThis(closest_wheel); frc846::math::Vector3D effective_wheel_vec_3d{ effective_wheel_vec[0], effective_wheel_vec[1], 0_in}; frc846::math::Vector3D r_vec = robot_cg - effective_wheel_vec_3d; - frc846::math::VectorND grav_vec{0_N, 0_N, - -frc846::math::constants::physics::g * robot_constants::total_weight}; - - units::newton_meter_t torque_grav = r_vec.cross(grav_vec).magnitude(); - - // TODO: Check all signs - - units::newton_meter_t torque_inertial = - r_vec - .cross(frc846::math::VectorND{ - inertial[0] * robot_constants::total_weight, - inertial[1] * robot_constants::total_weight, 0_N}) - .magnitude(); - - if (torque_inertial < torque_grav || torque_inertial == 0.0_Nm) { - return accel; - } - double rescale_factor = torque_grav / torque_inertial; + units::feet_per_second_squared_t perm_accel_x = + frc846::math::constants::physics::g * r_vec[0] / r_vec[2]; + units::feet_per_second_squared_t perm_accel_y = + frc846::math::constants::physics::g * r_vec[1] / r_vec[2]; - return accel * rescale_factor; + return {perm_accel_x, perm_accel_y}; } \ No newline at end of file diff --git a/src/y2025/cpp/commands/teleop/drive_command.cc b/src/y2025/cpp/commands/teleop/drive_command.cc index 33c853c..7ee9a13 100644 --- a/src/y2025/cpp/commands/teleop/drive_command.cc +++ b/src/y2025/cpp/commands/teleop/drive_command.cc @@ -49,18 +49,23 @@ void DriveCommand::Periodic() { target.velocity = {translate_x * max_speed, translate_y * max_speed}; - units::feet_per_second_squared_t accel_x{ - rampRateLimiter_x_.findRate(target.velocity[0].to())}; - units::feet_per_second_squared_t accel_y{ - rampRateLimiter_y_.findRate(target.velocity[1].to())}; - // TODO: plug real heights into AntiTippingCalculator AntiTippingCalculator::SetTelescopeHeight(10_in); - AntiTippingCalculator::SetElevatorHeight(10_in); + AntiTippingCalculator::SetElevatorHeight(30_in); + + auto delta_dir = (frc846::math::VectorND{ + target.velocity[0], target.velocity[1]} - + container_.drivetrain_.GetReadings().pose.velocity); + + Graph("delta_dir_x", delta_dir[0]); + Graph("delta_dir_y", delta_dir[1]); auto accel_limited = AntiTippingCalculator::LimitAcceleration( - {accel_x, accel_y}, container_.drivetrain_.GetReadings().pose.bearing); + delta_dir, container_.drivetrain_.GetReadings().pose.bearing); + + Graph("limited_accel_x", accel_limited[0]); + Graph("limited_accel_y", accel_limited[1]); target.velocity[0] = 1_fps * rampRateLimiter_x_.limit(target.velocity[0].to(), diff --git a/src/y2025/include/calculators/AntiTippingCalculator.h b/src/y2025/include/calculators/AntiTippingCalculator.h index 9678c7d..47b88ce 100644 --- a/src/y2025/include/calculators/AntiTippingCalculator.h +++ b/src/y2025/include/calculators/AntiTippingCalculator.h @@ -47,8 +47,7 @@ class AntiTippingCalculator { Limits the acceleration of the robot to prevent tipping. */ static frc846::math::VectorND - LimitAcceleration( - frc846::math::VectorND accel, + LimitAcceleration(frc846::math::VectorND vel_vec, units::degree_t bearing); private: diff --git a/src/y2025/include/subsystems/robot_constants.h b/src/y2025/include/subsystems/robot_constants.h index 615b032..10fccec 100644 --- a/src/y2025/include/subsystems/robot_constants.h +++ b/src/y2025/include/subsystems/robot_constants.h @@ -6,8 +6,8 @@ struct robot_constants { struct base { - static constexpr units::inch_t wheelbase_x = 26_in; - static constexpr units::inch_t wheelbase_y = 26_in; + static constexpr units::inch_t wheelbase_x = 28_in; + static constexpr units::inch_t wheelbase_y = 28_in; static constexpr units::pound_t weight = 75_lb; static constexpr units::inch_t height = 6_in;