Cg calcs

.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

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

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

simgui-ds.json

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

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

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

@@ -0,0 +1,31 @@
+#include "frc846/math/RampRateLimiter.h"
+
+#include <iostream>
+
+namespace frc846::math {
+
+RampRateLimiter::RampRateLimiter() {
+  m_lastTime = std::chrono::duration_cast<std::chrono::milliseconds>(
+      std::chrono::system_clock::now().time_since_epoch());
+}
+
+double RampRateLimiter::limit(double value, double rateLimit) {
+  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;
+
+  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

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 {