From 2622f4d8ebbec0e33839d66d8b108c24a2bef718 Mon Sep 17 00:00:00 2001
From: PAJohnson <pajohns4@ncsu.edu>
Date: Thu, 29 Apr 2021 23:01:44 -0400
Subject: [PATCH] Fixed spinout error reset on odrive.clear_errors()

---
 Firmware/MotorControl/controller.cpp | 4 ++--
 Firmware/MotorControl/foc.cpp        | 1 +
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp
index 73b2ac39..fc50692f 100644
--- a/Firmware/MotorControl/controller.cpp
+++ b/Firmware/MotorControl/controller.cpp
@@ -13,6 +13,8 @@ void Controller::reset() {
     vel_setpoint_ = 0.0f;
     vel_integrator_torque_ = 0.0f;
     torque_setpoint_ = 0.0f;
+    mechanical_power_ = 0.0f;
+    electrical_power_ = 0.0f;
 }
 
 void Controller::set_error(Error error) {
@@ -352,8 +354,6 @@ bool Controller::update() {
     if (mechanical_power_ < 0 && electrical_power_ > config_.spinout_power_margin) {
         axis_->encoder_.error_ |= Encoder::ERROR_INCORRECT_OFFSET;
         set_error(ERROR_NONE);
-        electrical_power_ = 0.0f;
-        mechanical_power_ = 0.0f;
         return false;
     }
 
diff --git a/Firmware/MotorControl/foc.cpp b/Firmware/MotorControl/foc.cpp
index 0f58cd20..efe07996 100644
--- a/Firmware/MotorControl/foc.cpp
+++ b/Firmware/MotorControl/foc.cpp
@@ -49,6 +49,7 @@ void FieldOrientedController::reset() {
     v_current_control_integral_q_ = 0.0f;
     vbus_voltage_measured_ = std::nullopt;
     Ialpha_beta_measured_ = std::nullopt;
+    power_ = 0.0f;
 }
 
 Motor::Error FieldOrientedController::on_measurement(