From f88ea874e947ac8c8ed2b40d4606e9eec5fac629 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 14 May 2018 00:03:50 -0700 Subject: [PATCH] implement snap of requested current range --- Firmware/MotorControl/motor.cpp | 69 +++++++++++++++++++-------------- Firmware/MotorControl/motor.hpp | 8 ++-- 2 files changed, 44 insertions(+), 33 deletions(-) diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 482bbf87..0c8707eb 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -65,42 +65,51 @@ void Motor::update_current_controller_gains() { // @brief Set up the gate drivers void Motor::DRV8301_setup() { - DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs_; - - DRV8301_enable(&gate_driver_); - DRV8301_setupSpi(&gate_driver_, local_regs); - - // TODO we can use reporting only if we actually wire up the nOCTW pin - local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown; - // Overcurrent set to approximately 150A at 100degC. This may need tweaking. - local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V; + // for reference: // 20V/V on 500uOhm gives a range of +/- 150A // 40V/V on 500uOhm gives a range of +/- 75A // 20V/V on 666uOhm gives a range of +/- 110A // 40V/V on 666uOhm gives a range of +/- 55A - local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_80VpV; - // local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV; - // local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_20VpV; - switch (local_regs->Ctrl_Reg_2.GAIN) { - case DRV8301_ShuntAmpGain_10VpV: - phase_current_rev_gain_ = 1.0f / 10.0f; - break; - case DRV8301_ShuntAmpGain_20VpV: - phase_current_rev_gain_ = 1.0f / 20.0f; - break; - case DRV8301_ShuntAmpGain_40VpV: - phase_current_rev_gain_ = 1.0f / 40.0f; - break; - case DRV8301_ShuntAmpGain_80VpV: - phase_current_rev_gain_ = 1.0f / 80.0f; - break; - } + // Solve for exact gain, then snap down to have equal or larger range as requested + // or largest possible range otherwise + static const float kMargin = 0.90f; + static const float max_output_swing = 1.6f; // [V] out of amplifier + float max_unity_gain_current = kMargin * max_output_swing * hw_config_.shunt_conductance; // [A] + float requested_gain = max_unity_gain_current / config_.requested_current_range; // [V/V] - float margin = 0.90f; - float max_input = margin * 0.3f * hw_config_.shunt_conductance; - float max_swing = margin * 1.6f * hw_config_.shunt_conductance * phase_current_rev_gain_; - current_control_.max_allowed_current = std::min(max_input, max_swing); + // Decoding array for snapping gain + std::array, 4> gain_choices = { + std::make_pair(10.0f, DRV8301_ShuntAmpGain_10VpV), + std::make_pair(20.0f, DRV8301_ShuntAmpGain_20VpV), + std::make_pair(40.0f, DRV8301_ShuntAmpGain_40VpV), + std::make_pair(80.0f, DRV8301_ShuntAmpGain_80VpV) + }; + + // We use lower_bound in reverse because it snaps up by default, we want to snap down. + auto gain_snap_down = std::lower_bound(gain_choices.crbegin(), gain_choices.crend(), requested_gain, + [](std::pair pair, float val){ + return pair.first > val; + }); + + // If we snap to outside the array, clip to smallest val + if(gain_snap_down == gain_choices.crend()) + --gain_snap_down; + + // Values for current controller + phase_current_rev_gain_ = 1.0f / gain_snap_down->first; + // Clip all current control to actual usable range + current_control_.max_allowed_current = max_unity_gain_current * phase_current_rev_gain_; + + // We now have the gain settings we want to use, lets set up DRV chip + DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs_; + DRV8301_enable(&gate_driver_); + DRV8301_setupSpi(&gate_driver_, local_regs); + + local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown; + // Overcurrent set to approximately 150A at 100degC. This may need tweaking. + local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V; + local_regs->Ctrl_Reg_2.GAIN = gain_snap_down->second; local_regs->SndCmd = true; DRV8301_writeData(&gate_driver_, local_regs); diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 961c05ce..852a64f4 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -46,9 +46,10 @@ typedef struct { Motor_type_t motor_type = MOTOR_TYPE_HIGH_CURRENT; // Read out max_allowed_current to see max supported value for current_lim. - // You can change DRV8301_ShuntAmpGain to get a different range. - // float current_lim = 75.0f; //[A] + // float current_lim = 70.0f; //[A] float current_lim = 10.0f; //[A] + // Value used to compute shunt amplifier gains + float requested_current_range = 70.0f; // [A] } MotorConfig_t; class Motor { @@ -207,7 +208,8 @@ public: make_protocol_property("phase_resistance", &config_.phase_resistance), make_protocol_property("direction", &config_.direction), make_protocol_property("motor_type", &config_.motor_type), - make_protocol_property("current_lim", &config_.current_lim) + make_protocol_property("current_lim", &config_.current_lim), + make_protocol_property("requested_current_range", &config_.requested_current_range) ) ); }