FEAT preparations for current control

Author: askuric

examples/motion_control/torque_voltage_control/magnetic_sensor/voltage_control/voltage_control.ino

@@ -11,7 +11,7 @@
 // magnetic sensor instance - SPI
 MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 10);
 // magnetic sensor instance - I2C
-//MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
+// MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
 // magnetic sensor instance - analog output
 // MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);
 

keywords.txt

@@ -62,6 +62,7 @@ angleOpenloop	KEYWORD2
 velocityOpenloop	KEYWORD2
 
 voltage_q	KEYWORD2
+voltage_d	KEYWORD2
 shaft_angle_sp	KEYWORD2
 shaft_velocity_sp	KEYWORD2
 shaft_angle	KEYWORD2

src/BLDCMotor.cpp

@@ -33,7 +33,7 @@ void BLDCMotor::linkDriver(BLDCDriver* _driver) {
 void BLDCMotor::init() {
   if(monitor_port) monitor_port->println("MOT: Initialise variables.");
   // sanity check for the voltage limit configuration
-  if(voltage_limit > driver->voltage_power_supply) voltage_limit =  driver->voltage_power_supply;
+  if(voltage_limit > driver->voltage_limit) voltage_limit =  driver->voltage_limit;
   // constrain voltage for sensor alignment
   if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit;
   // update the controller limits
@@ -98,13 +98,13 @@ int BLDCMotor::alignSensor() {
   float start_angle = shaftAngle();
   for (int i = 0; i <=5; i++ ) {
     float angle = _3PI_2 + _2PI * i / 6.0;
-    setPhaseVoltage(voltage_sensor_align,  angle);
+    setPhaseVoltage(voltage_sensor_align, 0,  angle);
     _delay(200);
   }
   float mid_angle = shaftAngle();
   for (int i = 5; i >=0; i-- ) {
     float angle = _3PI_2 + _2PI * i / 6.0;
-    setPhaseVoltage(voltage_sensor_align,  angle);
+    setPhaseVoltage(voltage_sensor_align, 0,  angle);
     _delay(200);
   }
   if (mid_angle < start_angle) {
@@ -119,7 +119,7 @@ int BLDCMotor::alignSensor() {
   // set sensor to zero
   sensor->initRelativeZero();
   _delay(500);
-  setPhaseVoltage(0,0);
+  setPhaseVoltage(0, 0, 0);
   _delay(200);
 
   // find the index if available
@@ -151,7 +151,7 @@ int BLDCMotor::absoluteZeroAlign() {
   }
   voltage_q = 0;
   // disable motor
-  setPhaseVoltage(0,0);
+  setPhaseVoltage(0, 0, 0);
 
   // align absolute zero if it has been found
   if(!sensor->needsAbsoluteZeroSearch()){
@@ -170,7 +170,7 @@ void BLDCMotor::loopFOC() {
   // shaft angle 
   shaft_angle = shaftAngle();
   // set the phase voltage - FOC heart function :) 
-  setPhaseVoltage(voltage_q, _electricalAngle(shaft_angle,pole_pairs));
+  setPhaseVoltage(voltage_q, voltage_d, _electricalAngle(shaft_angle,pole_pairs));
 }
 
 // Iterative function running outer loop of the FOC algorithm
@@ -217,16 +217,17 @@ void BLDCMotor::move(float new_target) {
 }
 
 
-// Method using FOC to set Uq to the motor at the optimal angle
+// Method using FOC to set Uq and Ud to the motor at the optimal angle
 // Function implementing Space Vector PWM and Sine PWM algorithms
 // 
 // Function using sine approximation
 // regular sin + cos ~300us    (no memory usaage)
 // approx  _sin + _cos ~110us  (400Byte ~ 20% of memory)
-void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) {
+void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
 
   const bool centered = true;
   int sector;
+  float _ca,_sa;
 
   switch (foc_modulation)
   {
@@ -243,9 +244,9 @@ void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) {
       Uc = Uq + trap_120_map[sector][2] * Uq;
 
       if (centered) {
-        Ua += (voltage_power_supply)/2 -Uq;
-        Ub += (voltage_power_supply)/2 -Uq;
-        Uc += (voltage_power_supply)/2 -Uq;
+        Ua += (driver->voltage_limit)/2 -Uq;
+        Ub += (driver->voltage_limit)/2 -Uq;
+        Uc += (driver->voltage_limit)/2 -Uq;
       }
     break;
 
@@ -263,9 +264,9 @@ void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) {
 
       //center
       if (centered) {
-        Ua += (voltage_power_supply)/2 -Uq;
-        Ub += (voltage_power_supply)/2 -Uq;
-        Uc += (voltage_power_supply)/2 -Uq;
+        Ua += (driver->voltage_limit)/2 -Uq;
+        Ub += (driver->voltage_limit)/2 -Uq;
+        Uc += (driver->voltage_limit)/2 -Uq;
       }
 
     break;
@@ -277,20 +278,22 @@ void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) {
       // angle normalization in between 0 and 2pi
       // only necessary if using _sin and _cos - approximation functions
       angle_el = _normalizeAngle(angle_el + zero_electric_angle);
+      _ca = _cos(angle_el);
+      _sa = _sin(angle_el);
       // Inverse park transform
-      Ualpha =  -_sin(angle_el) * Uq;  // -sin(angle) * Uq;
-      Ubeta =  _cos(angle_el) * Uq;    //  cos(angle) * Uq;
+      Ualpha =  _ca * Ud - _sa * Uq;  // -sin(angle) * Uq;
+      Ubeta =  _sa * Ud + _ca * Uq;    //  cos(angle) * Uq;
 
       // Clarke transform
-      Ua = Ualpha + driver->voltage_power_supply/2;
-      Ub = -0.5 * Ualpha  + _SQRT3_2 * Ubeta + driver->voltage_power_supply/2;
-      Uc = -0.5 * Ualpha - _SQRT3_2 * Ubeta + driver->voltage_power_supply/2;
+      Ua = Ualpha + driver->voltage_limit/2;
+      Ub = -0.5 * Ualpha  + _SQRT3_2 * Ubeta + driver->voltage_limit/2;
+      Uc = -0.5 * Ualpha - _SQRT3_2 * Ubeta + driver->voltage_limit/2;
 
       if (!centered) {
         float Umin = min(Ua, min(Ub, Uc));
-        Ua -=Umin;
-        Ub -=Umin;
-        Uc -=Umin;
+        Ua -= Umin;
+        Ub -= Umin;
+        Uc -= Umin;
       }
 
       break;
@@ -311,12 +314,12 @@ void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) {
       // find the sector we are in currently
       sector = floor(angle_el / _PI_3) + 1;
       // calculate the duty cycles
-      float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uq/driver->voltage_power_supply;
-      float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uq/driver->voltage_power_supply;
+      float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uq/driver->voltage_limit;
+      float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uq/driver->voltage_limit;
       // two versions possible 
       float T0 = 0; // pulled to 0 - better for low power supply voltage
       if (centered) { 
-        T0 = 1 - T1 - T2; //centered around driver->voltage_power_supply/2
+        T0 = 1 - T1 - T2; //centered around driver->voltage_limit/2
       } 
 
       // calculate the duty cycles(times)
@@ -360,9 +363,9 @@ void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) {
       }
 
       // calculate the phase voltages and center
-      Ua = Ta*driver->voltage_power_supply;
-      Ub = Tb*driver->voltage_power_supply;
-      Uc = Tc*driver->voltage_power_supply;
+      Ua = Ta*driver->voltage_limit;
+      Ub = Tb*driver->voltage_limit;
+      Uc = Tc*driver->voltage_limit;
       break;
 
   }
@@ -386,7 +389,7 @@ void BLDCMotor::velocityOpenloop(float target_velocity){
   shaft_angle += target_velocity*Ts; 
 
   // set the maximal allowed voltage (voltage_limit) with the necessary angle
-  setPhaseVoltage(voltage_limit, _electricalAngle(shaft_angle, pole_pairs));
+  setPhaseVoltage(voltage_limit,  0, _electricalAngle(shaft_angle, pole_pairs));
 
   // save timestamp for next call
   open_loop_timestamp = now_us;
@@ -409,7 +412,7 @@ void BLDCMotor::angleOpenloop(float target_angle){
     shaft_angle = target_angle;
 
   // set the maximal allowed voltage (voltage_limit) with the necessary angle
-  setPhaseVoltage(voltage_limit, _electricalAngle(shaft_angle, pole_pairs));
+  setPhaseVoltage(voltage_limit,  0, _electricalAngle(shaft_angle, pole_pairs));
 
   // save timestamp for next call
   open_loop_timestamp = now_us;

src/BLDCMotor.h

@@ -76,9 +76,10 @@ class BLDCMotor: public FOCMotor
     * Heart of the FOC algorithm
     * 
     * @param Uq Current voltage in q axis to set to the motor
+    * @param Ud Current voltage in d axis to set to the motor
     * @param angle_el current electrical angle of the motor
     */
-    void setPhaseVoltage(float Uq, float angle_el);
+    void setPhaseVoltage(float Uq, float Ud, float angle_el);
     /** Sensor alignment to electrical 0 angle of the motor */
     int alignSensor();
     /** Motor and sensor alignment to the sensors absolute 0 angle  */

src/StepperMotor.cpp

@@ -24,7 +24,7 @@ void StepperMotor::init() {
   if(monitor_port) monitor_port->println("MOT: Init variables.");
 
   // sanity check for the voltage limit configuration
-  if(voltage_limit > voltage_power_supply) voltage_limit =  voltage_power_supply;
+  if(voltage_limit > driver->voltage_limit) voltage_limit =  driver->voltage_limit;
   // constrain voltage for sensor alignment
   if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit;
 
@@ -91,13 +91,13 @@ int StepperMotor::alignSensor() {
   float start_angle = shaftAngle();
   for (int i = 0; i <=5; i++ ) {
     float angle = _3PI_2 + _2PI * i / 6.0;
-    setPhaseVoltage(voltage_sensor_align,  angle);
+    setPhaseVoltage(voltage_sensor_align, 0, angle);
     _delay(200);
   }
   float mid_angle = shaftAngle();
   for (int i = 5; i >=0; i-- ) {
     float angle = _3PI_2 + _2PI * i / 6.0;
-    setPhaseVoltage(voltage_sensor_align,  angle);
+    setPhaseVoltage(voltage_sensor_align, 0,  angle);
     _delay(200);
   }
   if (mid_angle < start_angle) {
@@ -112,7 +112,7 @@ int StepperMotor::alignSensor() {
   // set sensor to zero
   sensor->initRelativeZero();
   _delay(500);
-  setPhaseVoltage(0,0);
+  setPhaseVoltage(0, 0, 0);
   _delay(200);
 
   // find the index if available
@@ -143,8 +143,9 @@ int StepperMotor::absoluteZeroAlign() {
     voltage_q = PID_velocity(velocity_index_search - shaftVelocity());
   }
   voltage_q = 0;
+  voltage_d = 0;
   // disable motor
-  setPhaseVoltage(0,0);
+  setPhaseVoltage(0, 0, 0);
 
   // align absolute zero if it has been found
   if(!sensor->needsAbsoluteZeroSearch()){
@@ -163,7 +164,7 @@ void StepperMotor::loopFOC() {
   // shaft angle 
   shaft_angle = shaftAngle();
   // set the phase voltage - FOC heart function :) 
-  setPhaseVoltage(voltage_q, _electricalAngle(shaft_angle, pole_pairs));
+  setPhaseVoltage(voltage_q, voltage_d, _electricalAngle(shaft_angle, pole_pairs));
 }
 
 // Iterative function running outer loop of the FOC algorithm
@@ -210,36 +211,25 @@ void StepperMotor::move(float new_target) {
 }
 
 
-// Method using FOC to set Uq to the motor at the optimal angle
-// Function implementingSine PWM algorithms
+// Method using FOC to set Uq and Ud to the motor at the optimal angle
+// Function implementing Sine PWM algorithms
 // - space vector not implemented yet
 // 
 // Function using sine approximation
 // regular sin + cos ~300us    (no memory usaage)
 // approx  _sin + _cos ~110us  (400Byte ~ 20% of memory)
-void StepperMotor::setPhaseVoltage(float Uq, float angle_el) {
+void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
   // Sinusoidal PWM modulation 
   // Inverse Park transformation
 
   // angle normalization in between 0 and 2pi
   // only necessary if using _sin and _cos - approximation functions
-  angle_el = _normalizeAngle(angle_el + zero_electric_angle);
+  angle_el = _normalizeAngle(angle_el + zero_electric_angle); 
+  float _ca = _cos(angle_el);
+  float _sa = _sin(angle_el);
   // Inverse park transform
-  Ualpha =  -(_sin(angle_el)) * Uq;  // -sin(angle) * Uq;
-  Ubeta =  (_cos(angle_el)) * Uq;    //  cos(angle) * Uq;
-  // if (angle_el < _PI_2) {
-  //   Ualpha = 0;
-  //   Ubeta =  Uq;
-  // }else if (angle_el < _PI) {
-  //   Ualpha =- Uq;
-  //   Ubeta =  0;
-  // }else if (angle_el < _3PI_2) {
-  //   Ualpha = 0;
-  //   Ubeta =  -Uq;
-  // }else{
-  //   Ualpha = Uq;
-  //   Ubeta =  0;
-  // }
+  Ualpha =  _ca * Ud - _sa * Uq;  // -sin(angle) * Uq;
+  Ubeta =  _sa * Ud + _ca * Uq;    //  cos(angle) * Uq;
 
   // set the voltages in hardware
   driver->setPwm(Ualpha, Ubeta);
@@ -258,7 +248,7 @@ void StepperMotor::velocityOpenloop(float target_velocity){
   shaft_angle += target_velocity*Ts; 
 
   // set the maximal allowed voltage (voltage_limit) with the necessary angle
-  setPhaseVoltage(voltage_limit, _electricalAngle(shaft_angle,pole_pairs));
+  setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle,pole_pairs));
 
   // save timestamp for next call
   open_loop_timestamp = now_us;
@@ -281,7 +271,7 @@ void StepperMotor::angleOpenloop(float target_angle){
     shaft_angle = target_angle;
 
   // set the maximal allowed voltage (voltage_limit) with the necessary angle
-  setPhaseVoltage(voltage_limit, _electricalAngle(shaft_angle,pole_pairs));
+  setPhaseVoltage(voltage_limit,  0, _electricalAngle(shaft_angle,pole_pairs));
 
   // save timestamp for next call
   open_loop_timestamp = now_us;

src/StepperMotor.h

@@ -84,9 +84,10 @@ class StepperMotor: public FOCMotor
     * Heart of the FOC algorithm
     * 
     * @param Uq Current voltage in q axis to set to the motor
+    * @param Ud Current voltage in d axis to set to the motor
     * @param angle_el current electrical angle of the motor
     */
-    void setPhaseVoltage(float Uq, float angle_el);
+    void setPhaseVoltage(float Uq, float Ud , float angle_el);
 
     /** Sensor alignment to electrical 0 angle of the motor */
     int alignSensor();

src/common/base_classes/FOCMotor.cpp

@@ -5,13 +5,10 @@
  */
 FOCMotor::FOCMotor()
 {
-  // Power supply voltage
-  voltage_power_supply = DEF_POWER_SUPPLY;
-
   // maximum angular velocity to be used for positioning 
   velocity_limit = DEF_VEL_LIM;
   // maximum voltage to be set to the motor
-  voltage_limit = voltage_power_supply;
+  voltage_limit = DEF_POWER_SUPPLY;
 
   // index search velocity
   velocity_index_search = DEF_INDEX_SEARCH_TARGET_VELOCITY;
@@ -26,6 +23,8 @@ FOCMotor::FOCMotor()
 
   // default target value
   target = 0;
+  voltage_d = 0;
+  voltage_q = 0;
 
   //monitor_port 
   monitor_port = nullptr;

src/common/base_classes/FOCMotor.h

@@ -103,9 +103,9 @@ class FOCMotor
     float shaft_velocity_sp;//!< current target velocity
     float shaft_angle_sp;//!< current target angle
     float voltage_q;//!< current voltage u_q set
+    float voltage_d;//!< current voltage u_d set
 
     // motor configuration parameters
-    float voltage_power_supply;//!< Power supply voltage
     float voltage_sensor_align;//!< sensor and motor align voltage parameter
     float velocity_index_search;//!< target velocity for index search 
     int pole_pairs;//!< Motor pole pairs number

src/drivers/hardware_specific/esp32_mcu.cpp

@@ -283,12 +283,12 @@ void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, float dead_zone, i
     if(esp32_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found
       // se the PWM on the slot timers
       // transform duty cycle from [0,1] to [0,100.0]
-      mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100);
-      mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100);
-      mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100);
-      mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100);
-      mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100);
-      mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100);
+      mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0);
+      mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0);
+      mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0);
+      mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0);
+      mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0);
+      mcpwm_set_duty(esp32_bldc_6pwm_motor_slots[i].mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0);
       break;
     }
   }