From d5b7f8ecee9e348218d7d964caf46626bec5da0a Mon Sep 17 00:00:00 2001
From: VyaasBaskar <vyaasbaskar@gmail.com>
Date: Thu, 16 Jan 2025 18:02:37 -0800
Subject: [PATCH] Fixed dynamic power handling

---
 src/frc846/cpp/frc846/control/MotorMonkey.cc   | 11 +++++++++--
 src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc |  2 +-
 2 files changed, 10 insertions(+), 3 deletions(-)

diff --git a/src/frc846/cpp/frc846/control/MotorMonkey.cc b/src/frc846/cpp/frc846/control/MotorMonkey.cc
index f01d070..b81b27d 100644
--- a/src/frc846/cpp/frc846/control/MotorMonkey.cc
+++ b/src/frc846/cpp/frc846/control/MotorMonkey.cc
@@ -74,7 +74,7 @@ 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};
+frc846::math::DoubleSyncBuffer MotorMonkey::sync_buffer{100U, 35};
 
 units::ampere_t MotorMonkey::max_draw_{0.0_A};
 
@@ -208,7 +208,14 @@ units::ampere_t MotorMonkey::WriteMessages(units::ampere_t max_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);
+
+    if (velocity > 0_rad_per_s && pred_draw < 0_A) {
+      (void)pred_draw;  // Regen braking mode
+    } else if (velocity < 0_rad_per_s && pred_draw > 0_A) {
+      (void)pred_draw;  // Regen braking mode
+    } else {
+      second_total_current += units::math::abs(pred_draw);
+    }
     total_current += units::math::abs(pred_draw);
     temp_messages.pop();
   }
diff --git a/src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc b/src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc
index 8ae83d8..a36bd3a 100644
--- a/src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc
+++ b/src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc
@@ -62,7 +62,7 @@ std::pair<double, double> DoubleSyncBuffer::GetTrough() {
   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_];
+    double added = m_buffer_2[i + sync_diff_];
     if (added < min_combination) {
       min_combination = added;
       min1 = m_buffer_1[i];