From d9b13efac28a6486008ac8237dc9a994b2b9fcb6 Mon Sep 17 00:00:00 2001
From: Paul Guenette <pfguenette@gmail.com>
Date: Wed, 22 Nov 2017 20:49:09 -0500
Subject: [PATCH] Check for max current based on HW version

---
 Firmware/MotorControl/low_level.c | 10 +++++++++-
 Firmware/MotorControl/low_level.h |  1 +
 2 files changed, 10 insertions(+), 1 deletion(-)

diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c
index 04fa245f..3062fd60 100644
--- a/Firmware/MotorControl/low_level.c
+++ b/Firmware/MotorControl/low_level.c
@@ -144,6 +144,7 @@ Motor_t motors[] = {
             .calib_pos_threshold = 1.0f,
             .calib_vel_threshold = 1.0f,
         },
+        .max_allowed_current = 0.0f,
     },
     {   // M1
         .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t
@@ -233,7 +234,8 @@ Motor_t motors[] = {
             .calib_anticogging = false,
             .calib_pos_threshold = 1.0f,
             .calib_vel_threshold = 1.0f,
-        }
+        },
+        .max_allowed_current = 0.0f,
     }
 };
 const int num_motors = sizeof(motors)/sizeof(motors[0]);
@@ -402,6 +404,7 @@ static void DRV8301_setup(Motor_t* motor) {
         local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV;
 
         switch (local_regs->Ctrl_Reg_2.GAIN) {
+
             case DRV8301_ShuntAmpGain_10VpV:
                 motor->phase_current_rev_gain = 1.0f/10.0f;
                 break;
@@ -416,6 +419,11 @@ static void DRV8301_setup(Motor_t* motor) {
                 break;
         }
 
+        float margin = 0.95f;
+        float max_input = margin * 0.3f * motor->shunt_conductance;
+        float max_swing = margin * 1.6f * motor->shunt_conductance / motor->phase_current_rev_gain;
+        motor->max_allowed_current = MACRO_MIN(max_input, max_swing);
+
         local_regs->SndCmd = true;
         DRV8301_writeData(gate_driver, local_regs);
         local_regs->RcvCmd = true;
diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h
index 12eac1ba..4293f935 100644
--- a/Firmware/MotorControl/low_level.h
+++ b/Firmware/MotorControl/low_level.h
@@ -146,6 +146,7 @@ typedef struct {
     int timing_log_index;
     uint16_t timing_log[TIMING_LOG_SIZE];
     Anticogging_t anticogging;
+    float max_allowed_current;
 } Motor_t;
 
 typedef struct{