Autotuning

simplefoc · Jul 21, 2024 · 26fbea0 · 26fbea0
1 parent 2de948c
commit 26fbea0
Show file tree

Hide file tree

Showing 3 changed files with 26 additions and 0 deletions.
diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp
@@ -82,7 +82,16 @@ int BLDCMotor::init() {
     // current control loop controls voltage
     PID_current_q.limit = voltage_limit;
     PID_current_d.limit = voltage_limit;
+
+    if (_isset(phase_resistance) && _isset(phase_inductance)){
+      PID_current_d.P = phase_inductance * DEF_CURR_BANDWIDTH * _2PI;
+      PID_current_d.I = PID_current_d.P * phase_resistance / phase_inductance;
+
+      PID_current_q.P = phase_inductance * DEF_CURR_BANDWIDTH * _2PI;
+      PID_current_q.I = PID_current_q.P * phase_resistance /phase_inductance;
+    }
   }
+
   if(_isset(phase_resistance) || torque_controller != TorqueControlType::voltage){
     // velocity control loop controls current
     PID_velocity.limit = current_limit;

diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp
@@ -47,6 +47,21 @@ int StepperMotor::init() {
   // constrain voltage for sensor alignment
   if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit;
 
+  // update the controller limits
+  if(current_sense){
+    // current control loop controls voltage
+    PID_current_q.limit = voltage_limit;
+    PID_current_d.limit = voltage_limit;
+
+    if (_isset(phase_resistance) && _isset(phase_inductance)){
+      PID_current_d.P = phase_inductance * DEF_CURR_BANDWIDTH * _2PI;
+      PID_current_d.I = PID_current_d.P * phase_resistance / phase_inductance;
+
+      PID_current_q.P = phase_inductance * DEF_CURR_BANDWIDTH * _2PI;
+      PID_current_q.I = PID_current_q.P * phase_resistance /phase_inductance;
+    }
+  }
+
   // update the controller limits
   if(_isset(phase_resistance)){
     // velocity control loop controls current

diff --git a/src/common/defaults.h b/src/common/defaults.h
@@ -18,6 +18,7 @@
 #define DEF_PID_CURR_RAMP 1000.0f //!< default PID controller voltage ramp value
 #define DEF_PID_CURR_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit
 #define DEF_CURR_FILTER_Tf 0.01f //!< default velocity filter time constant
+#define DEF_CURR_BANDWIDTH 300.0f //!< current bandwidth
 #else
 // for stm32, due, teensy, esp32 and similar
 #define DEF_PID_CURR_P 3 //!< default PID controller P value
@@ -26,6 +27,7 @@
 #define DEF_PID_CURR_RAMP 0  //!< default PID controller voltage ramp value
 #define DEF_PID_CURR_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit
 #define DEF_CURR_FILTER_Tf 0.005f //!< default currnet filter time constant
+#define DEF_CURR_BANDWIDTH 300.0f //!< current bandwidth
 #endif
 // default current limit values
 #define DEF_CURRENT_LIM 2.0f //!< 2Amps current limit by default