Anti-tipping code finished, tested

.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",

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
+    }
   }
 ]

simgui-ds.json

@@ -1,4 +1,9 @@
 {
+  "Keyboard 0 Settings": {
+    "window": {
+      "visible": true
+    }
+  },
   "keyboardJoysticks": [
     {
       "axisConfig": [

src/frc846/cpp/frc846/control/MotorMonkey.cc

@@ -80,13 +80,16 @@ units::ampere_t MotorMonkey::max_draw_{0.0_A};
 
 std::queue<MotorMonkey::MotorMessage> 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);
   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);
+  loggable_.RegisterPreference("brownout_perm_loops", 500);
 
   max_draw_ = loggable_.GetPreferenceValue_unit_type<units::ampere_t>(
       "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<units::ampere_t>("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<simulation::MCSimulator*>(controller_registry[i]);
         sim->SetBatteryVoltage(battery_voltage);
         sim->SetLoad(load_registry[i]);
+        // sim->Tick();
+        // TODO: fix sim
       }
     }
   }

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;

src/frc846/cpp/frc846/control/hardware/simulation/MCSimulator.cc

@@ -2,6 +2,8 @@
 
 #include <units/math.h>
 
+#include <iostream>
+
 #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<double>(), 0.0, velocity_.to<double>(), load_.to<double>());
   }
+  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::microseconds>(
           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<double>(control_message) &&
          std::get<double>(control_message) == duty_cycle;
 }
 bool MCSimulator::IsDuplicateControlMessage(
     units::radians_per_second_t velocity) {
+  if (inverted) velocity = -velocity;
   return std::holds_alternative<units::radians_per_second_t>(control_message) &&
          std::get<units::radians_per_second_t>(control_message) == velocity;
 }
 bool MCSimulator::IsDuplicateControlMessage(units::radian_t position) {
+  if (inverted) position = -position;
   return std::holds_alternative<units::radian_t>(control_message) &&
          std::get<units::radian_t>(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;
 }

src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc

@@ -57,17 +57,12 @@ std::pair<double, double> 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);

src/frc846/cpp/frc846/math/RampRateLimiter.cc

@@ -1,5 +1,7 @@
 #include "frc846/math/RampRateLimiter.h"
 
+#include <iostream>
+
 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::milliseconds>(
-      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

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,
   };

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<units::degree_t>("cancoder_offset_");
+
     if (position.IsAllGood()) {
-      units::degree_t position_zero =
-          -position.GetValue() +
-          GetPreferenceValue_unit_type<units::degree_t>("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() {

src/frc846/include/frc846/control/HMCHelper.h

@@ -41,7 +41,7 @@ template <typename T> 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 <typename T> 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 <typename T> 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 <typename T> class HMCHelper {
   HigherMotorController* hmc_;
 
   conv_unit conv_;
+
+  pos_unit mark_offset{0};
 };
 
 }  // namespace frc846::control

src/frc846/include/frc846/control/MotorMonkey.h

@@ -155,6 +155,8 @@ class MotorMonkey {
   };
 
   static std::queue<MotorMessage> control_messages;
+
+  static int num_loops_last_brown;
 };
 
 }  // namespace frc846::control

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;

src/frc846/include/frc846/math/vectors.h

@@ -206,7 +206,10 @@ template <typename UT, size_t N> 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

src/y2025/cpp/calculators/AntiTippingCalculator.cc

@@ -3,6 +3,8 @@
 #include <units/force.h>
 #include <units/torque.h>
 
+#include <iostream>
+
 #include "frc846/math/constants.h"
 #include "subsystems/robot_constants.h"
 
@@ -46,12 +48,13 @@ frc846::math::Vector3D AntiTippingCalculator::CalculateRobotCG() {
 
 frc846::math::VectorND<units::feet_per_second_squared, 2>
 AntiTippingCalculator::LimitAcceleration(
-    frc846::math::VectorND<units::feet_per_second_squared, 2> accel,
+    frc846::math::VectorND<units::feet_per_second, 2> vel_vec,
     units::degree_t bearing) {
-  frc846::math::VectorND<units::feet_per_second_squared, 2> inertial =
-      accel.rotate(180_deg, true);
-  frc846::math::VectorND<units::feet_per_second_squared, 2> 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<units::feet_per_second, 2> 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<units::inch>(closest_wheel);
+      v_neg_dir.projectOntoThis<units::inch>(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<units::newtons, 3> 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<units::newtons, 3>{
-              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};
 }

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<double>())};
-  units::feet_per_second_squared_t accel_y{
-      rampRateLimiter_y_.findRate(target.velocity[1].to<double>())};
-
   // TODO: plug real heights into AntiTippingCalculator
 
   AntiTippingCalculator::SetTelescopeHeight(10_in);
-  AntiTippingCalculator::SetElevatorHeight(10_in);
+  AntiTippingCalculator::SetElevatorHeight(30_in);
+
+  auto delta_dir = (frc846::math::VectorND<units::feet_per_second, 2>{
+                        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<double>(),

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<units::feet_per_second_squared, 2>
-  LimitAcceleration(
-      frc846::math::VectorND<units::feet_per_second_squared, 2> accel,
+  LimitAcceleration(frc846::math::VectorND<units::feet_per_second, 2> vel_vec,
       units::degree_t bearing);
 
 private:

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;