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 c07b524..22440e2 100644 --- a/networktables.json +++ b/networktables.json @@ -183,6 +183,14 @@ "persistent": true } }, + { + "name": "/Preferences/Robot/Robot/permissible_current_draw (A)", + "type": "double", + "value": 300.0, + "properties": { + "persistent": true + } + }, { "name": "/Preferences/SwerveDrivetrain/steer_gains/_kP", "type": "double", @@ -254,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/networktables.json.bck b/networktables.json.bck index c15bf00..b89113a 100644 --- a/networktables.json.bck +++ b/networktables.json.bck @@ -182,5 +182,85 @@ "properties": { "persistent": true } + }, + { + "name": "/Preferences/Robot/Robot/permissible_current_draw (A)", + "type": "double", + "value": 300.0, + "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/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/simgui.json b/simgui.json index 2708274..2e61c3e 100644 --- a/simgui.json +++ b/simgui.json @@ -9,6 +9,25 @@ "/SmartDashboard/zero_bearing": "Command" } }, + "NetworkTables": { + "transitory": { + "Shuffleboard": { + ".metadata": { + "open": true + }, + "open": true + }, + "SmartDashboard": { + "open": true, + "verify_hardware": { + "open": true + }, + "zero_bearing": { + "open": true + } + } + } + }, "NetworkTables Info": { "Connections": { "open": true 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 new file mode 100644 index 0000000..bf2a83f --- /dev/null +++ b/src/frc846/cpp/frc846/math/RampRateLimiter.cc @@ -0,0 +1,31 @@ +#include "frc846/math/RampRateLimiter.h" + +#include + +namespace frc846::math { + +RampRateLimiter::RampRateLimiter() { + m_lastTime = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()); +} + +double RampRateLimiter::limit(double value, double rateLimit) { + auto currentTime = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()); + auto elapsedTime = currentTime - m_lastTime; + m_lastTime = currentTime; + + 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) { + value = m_lastValue - maxChange; + } + + m_lastValue = value; + return value; +} + +} // 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..0e31534 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 { 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/cpp/frc846/robot/xbox.cc b/src/frc846/cpp/frc846/robot/xbox.cc index 4d44f74..eed07e9 100644 --- a/src/frc846/cpp/frc846/robot/xbox.cc +++ b/src/frc846/cpp/frc846/robot/xbox.cc @@ -9,8 +9,8 @@ XboxReadings::XboxReadings(frc::XboxController& xbox, double trigger_threshold) right_stick_y(-xbox.GetRightY()), // negated so + is up left_trigger(xbox.GetLeftTriggerAxis() >= trigger_threshold), right_trigger(xbox.GetRightTriggerAxis() >= trigger_threshold), - left_bumper(xbox.GetLeftBumper()), - right_bumper(xbox.GetRightBumper()), + left_bumper(xbox.GetLeftBumperButton()), + right_bumper(xbox.GetRightBumperButton()), back_button(xbox.GetBackButton()), start_button(xbox.GetStartButton()), rsb(xbox.GetRightStickButton()), 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/control/hardware/IntermediateController.h b/src/frc846/include/frc846/control/hardware/IntermediateController.h index 00a6799..bdb79f8 100644 --- a/src/frc846/include/frc846/control/hardware/IntermediateController.h +++ b/src/frc846/include/frc846/control/hardware/IntermediateController.h @@ -65,7 +65,7 @@ class IntermediateController { virtual ControllerErrorCodes GetLastErrorCode() = 0; - virtual bool VerifyConnected() = 0; // changed + virtual bool VerifyConnected() = 0; }; } // namespace frc846::control::hardware \ No newline at end of file diff --git a/src/frc846/include/frc846/math/RampRateLimiter.h b/src/frc846/include/frc846/math/RampRateLimiter.h new file mode 100644 index 0000000..503453b --- /dev/null +++ b/src/frc846/include/frc846/math/RampRateLimiter.h @@ -0,0 +1,23 @@ +#pragma once + +#include + +namespace frc846::math { + +/* +RampRateLimiter + +A class that limits the rate of change of a value. +*/ +class RampRateLimiter { +public: + RampRateLimiter(); + + double limit(double value, double rateLimit); + +private: + double m_lastValue = 0.0; + std::chrono::milliseconds m_lastTime; +}; + +} \ No newline at end of file diff --git a/src/frc846/include/frc846/math/vectors.h b/src/frc846/include/frc846/math/vectors.h index 416c455..f89b15d 100644 --- a/src/frc846/include/frc846/math/vectors.h +++ b/src/frc846/include/frc846/math/vectors.h @@ -147,18 +147,22 @@ template class VectorND { data[0] * units::math::sin(angle) + data[1] * units::math::cos(angle)}; } - // Returns the dot product of this vector and another - T dot(const VectorND& other) const { - T result = T{}; + // Returns the dot product of this vector and another with the units of the + // other vector + template + units::unit_t dot(const VectorND& other) const { + units::unit_t result{}; for (size_t i = 0; i < N; ++i) { - result += data[i] * other[i].template to(); + result += data[i].template to() * other[i]; } return result; } // Returns the cross product of this vector and another // Cross product is only defined for 3D vectors - VectorND cross(const VectorND& other) const { + template + VectorND, N> cross( + const VectorND& other) const { static_assert(N == 3, "Cross product is only defined for 3D vectors."); return {data[1] * other[2] - data[2] * other[1], data[2] * other[0] - data[0] * other[2], @@ -180,13 +184,18 @@ template class VectorND { } // Projects this vector onto another and returns - VectorND projectOntoAnother(const VectorND& other) const { + template + VectorND projectOntoAnother(const VectorND& other) const { return other.projectOntoThis(*this); } // Projects another vector onto this and returns - VectorND projectOntoThis(const VectorND& other) const { - return unit() * dot(other).template to(); + template + VectorND projectOntoThis(const VectorND& other) const { + assert(N == 2 && "Projection is only defined for 2D vectors."); + double unit_x = unit()[0].template to(); + double unit_y = unit()[1].template to(); + return {unit_x * dot(other), unit_y * dot(other)}; } // Returns the angle of this vector @@ -197,12 +206,16 @@ 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 + template units::degree_t angleTo( - const VectorND& other, bool angleIsBearing = false) const { + const VectorND& other, bool angleIsBearing = false) const { return other.angle(angleIsBearing) - angle(angleIsBearing); } diff --git a/src/frc846/include/frc846/robot/GenericCommand.h b/src/frc846/include/frc846/robot/GenericCommand.h index 09cce49..c3b0ab8 100644 --- a/src/frc846/include/frc846/robot/GenericCommand.h +++ b/src/frc846/include/frc846/robot/GenericCommand.h @@ -106,7 +106,8 @@ class GenericCommandGroup private: frc2::InstantCommand end_command_addition{[&] { Log("Command group ending. Took {} ms to complete.", - (frc846::wpilib::CurrentFPGATime() - command_start_time_).to()); + (frc846::wpilib::CurrentFPGATime() - command_start_time_) + .template to()); }}; frc2::InstantCommand start_command_addition{[&] { diff --git a/src/y2025/cpp/calculators/AntiTippingCalculator.cc b/src/y2025/cpp/calculators/AntiTippingCalculator.cc new file mode 100644 index 0000000..bab0ea9 --- /dev/null +++ b/src/y2025/cpp/calculators/AntiTippingCalculator.cc @@ -0,0 +1,104 @@ +#include "calculators/AntiTippingCalculator.h" + +#include +#include + +#include + +#include "frc846/math/constants.h" +#include "subsystems/robot_constants.h" + +frc846::math::Vector3D AntiTippingCalculator::elev_cg_position_{ + robot_constants::elevator::pos_x, robot_constants::elevator::pos_y, + robot_constants::base::height}; +frc846::math::Vector3D AntiTippingCalculator::tele_cg_position_{ + robot_constants::telescope::pos_x, robot_constants::telescope::pos_y, + robot_constants::base::height}; + +void AntiTippingCalculator::SetElevatorHeight(units::inch_t height) { + 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::min_height_off_base / 2.0) + + (robot_constants::elevator::end_effector_weight * height)) / + (robot_constants::elevator::elevator_weight + + robot_constants::elevator::end_effector_weight); + } +} + +void AntiTippingCalculator::SetTelescopeHeight(units::inch_t height) { + tele_cg_position_[2] = height / 2.0; +} + +frc846::math::Vector3D AntiTippingCalculator::CalculateRobotCG() { + double total_weight_scalar = robot_constants::total_weight.to(); + + frc846::math::Vector3D weighted_sum = + (elev_cg_position_ * + robot_constants::elevator::total_weight.to() + + tele_cg_position_ * + robot_constants::telescope::total_weight.to() + + frc846::math::Vector3D{0_in, 0_in, robot_constants::base::height} * + robot_constants::base::weight.to()); + + return weighted_sum / total_weight_scalar; +} + +frc846::math::VectorND +AntiTippingCalculator::LimitAcceleration( + frc846::math::VectorND vel_vec, + units::degree_t bearing) { + 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(); + + std::pair kModuleLocationSigns[4] = { + {+0.5, +0.5}, // FR + {-0.5, +0.5}, // FL + {-0.5, -0.5}, // BL + {+0.5, -0.5}, // BR + }; + + frc846::math::Vector2D wheel_vecs[4]; + + for (size_t i = 0; i < 4; i++) { + wheel_vecs[i] = + frc846::math::Vector2D{ + kModuleLocationSigns[i].first * robot_constants::base::wheelbase_x, + kModuleLocationSigns[i].second * robot_constants::base::wheelbase_y} + .rotate(bearing, true); + } + + size_t closest_wheel_vec = 0U; + units::degree_t closest_angle = 360_deg; + + for (size_t i = 0; i < 4; 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; + } + } + + frc846::math::Vector2D closest_wheel = wheel_vecs[closest_wheel_vec]; + + frc846::math::Vector2D effective_wheel_vec = + 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; + + 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 {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 980ff7f..7ee9a13 100644 --- a/src/y2025/cpp/commands/teleop/drive_command.cc +++ b/src/y2025/cpp/commands/teleop/drive_command.cc @@ -1,5 +1,7 @@ #include "commands/teleop/drive_command.h" +#include "calculators/AntiTippingCalculator.h" + DriveCommand::DriveCommand(RobotContainer &container) : frc846::robot::GenericCommand{ container, "drive_command"} { @@ -46,6 +48,32 @@ void DriveCommand::Periodic() { "max_omega"); target.velocity = {translate_x * max_speed, translate_y * max_speed}; + + // TODO: plug real heights into AntiTippingCalculator + + AntiTippingCalculator::SetTelescopeHeight(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( + 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(), + accel_limited[0].to()); + target.velocity[1] = + 1_fps * rampRateLimiter_y_.limit(target.velocity[1].to(), + accel_limited[1].to()); + target.angular_velocity = rotation * max_omega; container_.drivetrain_.SetTarget({target}); diff --git a/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc b/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc index 8cedd0b..bffd013 100644 --- a/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc +++ b/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc @@ -3,6 +3,7 @@ #include "frc846/control/calculators/CircuitResistanceCalculator.h" #include "frc846/math/constants.h" #include "ports.h" +#include "subsystems/robot_constants.h" DrivetrainConstructor::DrivetrainConstructor() : Loggable{"DrivetrainConstructor"} { @@ -42,8 +43,8 @@ DrivetrainConstructor::getDrivetrainConfigs() { (frc846::math::constants::geometry::pi * wheel_diameter) / 6.12_tr; frc846::robot::swerve::steer_conv_unit steer_reduction = 7_tr / 150_tr; - configs.wheelbase_forward_dim = 26_in; - configs.wheelbase_horizontal_dim = 26_in; + configs.wheelbase_forward_dim = robot_constants::base::wheelbase_y; + configs.wheelbase_horizontal_dim = robot_constants::base::wheelbase_x; units::pound_t wheel_approx_weight = 2_lb; units::inch_t wheel_weight_radius = 1_in; diff --git a/src/y2025/include/calculators/AntiTippingCalculator.h b/src/y2025/include/calculators/AntiTippingCalculator.h new file mode 100644 index 0000000..47b88ce --- /dev/null +++ b/src/y2025/include/calculators/AntiTippingCalculator.h @@ -0,0 +1,56 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "frc846/math/calculator.h" +#include "frc846/math/constants.h" +#include "frc846/math/vectors.h" + +/* +AntiTippingCalculator + +Class that provides static methods to aid in preventing the robot from tipping. +*/ +class AntiTippingCalculator { +public: + /* + SetElevatorHeight() + + Sets the position of the end effector from the ground. Location of the CG + of the elevator is heuristically determined. + */ + static void SetElevatorHeight(units::inch_t height); + + /* + SetElevatorHeight() + + Sets the position of the end effector from the ground. Location of the CG + of the elevator is heuristically determined. + */ + static void SetTelescopeHeight(units::inch_t height); + + /* + CalculateRobotCG() + + Finds the location of the center of gravity of the robot. + */ + static frc846::math::Vector3D CalculateRobotCG(); + + /* + LimitAcceleration() + + Limits the acceleration of the robot to prevent tipping. + */ + static frc846::math::VectorND + LimitAcceleration(frc846::math::VectorND vel_vec, + units::degree_t bearing); + +private: + static frc846::math::Vector3D elev_cg_position_; + static frc846::math::Vector3D tele_cg_position_; +}; \ No newline at end of file diff --git a/src/y2025/include/commands/teleop/drive_command.h b/src/y2025/include/commands/teleop/drive_command.h index 432354f..a543663 100644 --- a/src/y2025/include/commands/teleop/drive_command.h +++ b/src/y2025/include/commands/teleop/drive_command.h @@ -1,5 +1,6 @@ #pragma once +#include "frc846/math/RampRateLimiter.h" #include "frc846/robot/GenericCommand.h" #include "subsystems/robot_container.h" @@ -15,4 +16,8 @@ class DriveCommand void OnEnd(bool interrupted) override; bool IsFinished() override; + +private: + frc846::math::RampRateLimiter rampRateLimiter_x_; + frc846::math::RampRateLimiter rampRateLimiter_y_; }; \ No newline at end of file diff --git a/src/y2025/include/subsystems/robot_constants.h b/src/y2025/include/subsystems/robot_constants.h new file mode 100644 index 0000000..10fccec --- /dev/null +++ b/src/y2025/include/subsystems/robot_constants.h @@ -0,0 +1,39 @@ +#pragma once + +#include + +#include "frc846/math/vectors.h" + +struct robot_constants { + struct base { + 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; + }; + + struct elevator { + static constexpr units::pound_t elevator_weight = 20_lb; + static constexpr units::inch_t min_height_off_base = 38_in; + + static constexpr units::pound_t end_effector_weight = 10_lb; + + static constexpr units::pound_t total_weight = + elevator_weight + end_effector_weight; + + static constexpr units::inch_t pos_x = -5_in; + static constexpr units::inch_t pos_y = -7_in; + }; + + struct telescope { + static constexpr units::pound_t total_weight = 20_lb; + + static constexpr units::inch_t pos_x = 5_in; + static constexpr units::inch_t pos_y = -7_in; + }; + + static constexpr units::pound_t total_weight = + base::weight + elevator::elevator_weight + elevator::end_effector_weight + + telescope::total_weight; +}; \ No newline at end of file