inital autotuning implementation

Author: askuric

src/BLDCMotor.cpp

@@ -75,15 +75,6 @@ int BLDCMotor::init() {
   // constrain voltage for sensor alignment
   if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit;
 
-  // update the controller limits
-  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 limits in the motor controllers
   updateCurrentLimit(current_limit);
   updateVoltageLimit(voltage_limit);

src/common/base_classes/FOCMotor.cpp

@@ -309,6 +309,9 @@ int FOCMotor::characteriseMotor(float voltage, float correction_factor=1.0f){
       SIMPLEFOC_DEBUG("WARN: MOT: Measured Q inductance is more than twice the D inductance. This is probably wrong. From experience, the lower value is probably close to reality.");
     }    
 
+    // store the measured values
+    phase_resistance = 2.0f * resistance;
+    phase_inductance = (Ld + Lq) / 2.0f;
     return 0;
 
 }
@@ -520,4 +523,42 @@ void FOCMotor::updateMotionControlType(MotionControlType new_motion_controller)
 
   // finally set the new controller
   controller = new_motion_controller; 
+}
+
+
+int FOCMotor::tuneCurrentController(float bandwidth) {
+  if (bandwidth <= 0.0f) {
+    // check bandwidth is positive
+    SIMPLEFOC_DEBUG("ERR: MOT: Cannot tune current controller: bandwidth must be positive");
+    return 1;
+  }
+  if (loop_time_us && bandwidth > 0.5f * (1e6f / loop_time_us)) {
+    // check bandwidth is not too high for the control loop frequency
+    SIMPLEFOC_DEBUG("ERR: MOT: Bandwidth too high, current loop freq:" , (1e6f / loop_time_us));
+    return 2;
+  }
+  if (!_isset(phase_resistance) || !_isset(phase_inductance)) {
+    // need motor parameters to tune the controller
+    SIMPLEFOC_DEBUG("MOT: Measuring motor parameters!");
+    if(characteriseMotor( voltage_sensor_align )) { 
+      return 3;
+    }
+  }
+
+  // Simple tuning method for a first order system
+  float Kp = phase_inductance * (_2PI * bandwidth);
+  float Ki = phase_resistance * (_2PI * bandwidth);
+
+  PID_current_q.P = Kp;
+  PID_current_q.I = Ki;
+  PID_current_d.P = Kp;
+  PID_current_d.I = Ki;
+  LPF_current_d.Tf = 1.0f / (_2PI * bandwidth * 5.0f); // filter cutoff at 5x bandwidth
+  LPF_current_q.Tf = 1.0f / (_2PI * bandwidth * 5.0f); // filter cutoff at 5x bandwidth
+
+  SIMPLEFOC_DEBUG("MOT: Current controller tuned:\nMOT: Bandwidth (Hz): ", bandwidth);
+  SIMPLEFOC_DEBUG("MOT:   Kp: ", Kp);
+  SIMPLEFOC_DEBUG("MOT:   Ki: ", Ki);
+
+  return 0;
 }

src/common/base_classes/FOCMotor.h

@@ -159,6 +159,16 @@ class FOCMotor
      */
     int characteriseMotor(float voltage, float correction_factor);
 
+    /**
+     * Auto-tune the current controller PID parameters based on desired bandwidth.
+     * Uses a simple method that assumes a first order system and requires knowledge of
+     * the motor phase resistance and inductance (if not set, the characteriseMotor function can be used).
+     * 
+     * @param bandwidth Desired closed-loop bandwidth in Hz.
+     * @returns returns 0 for success, >0 for failure
+     */
+    int tuneCurrentController(float bandwidth);
+
     // state variables
     float target; //!< current target value - depends of the controller
     float feed_forward_velocity = 0.0f; //!< current feed forward velocity

src/common/defaults.h

@@ -18,16 +18,16 @@
 #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
+#define DEF_CURR_BANDWIDTH 100.0f //!< current bandwidth
 #else
 // for stm32, due, teensy, esp32 and similar
 #define DEF_PID_CURR_P 3 //!< default PID controller P value
 #define DEF_PID_CURR_I 300.0f //!<  default PID controller I value
 #define DEF_PID_CURR_D 0.0f //!<  default PID controller D value
 #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
+#define DEF_CURR_FILTER_Tf 1/(_2PI*DEF_CURR_BANDWIDTH) //!< default currnet filter time constant
 #endif
 // default current limit values
 #define DEF_CURRENT_LIM 2.0f //!< 2Amps current limit by default

src/communication/Commander.cpp

@@ -269,6 +269,18 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
           motor->initFOC();
           println(F("done"));
           break;
+        case SCMD_TUNE_CURR:
+          printVerbose(F("PI tune curr.| "));
+          {
+            int res = motor->tuneCurrentController(value);
+            if(res == 0){
+              println(F("done"));
+            } else {
+              printVerbose(F("failed, err code: "));
+              println(res);
+            }
+          }
+          break;
         default:
           printError();
           break;

src/communication/commands.h

@@ -49,7 +49,8 @@
  #define SCMD_PWMMOD_TYPE   'T'  //!<< Pwm modulation type
  #define SCMD_PWMMOD_CENTER 'C'  //!<< Pwm modulation center flag
 
- #define SCMD_LOOPFOC_TIME   'L' //!< loopFOC execution time
+ #define SCMD_LOOPFOC_TIME  'L' //!< loopFOC execution time
  #define SCMD_REINIT_FOC    'R' //!< reinitialize FOC
+ #define SCMD_TUNE_CURR     'C' //!< tune current controller
 
 #endif