Fixed dynamic power handling

Team846 · Jan 17, 2025 · d5b7f8e · d5b7f8e
1 parent af3b164
commit d5b7f8e
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 3 deletions.
diff --git 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
@@ -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];