init v2.0.2

Author: askuric

library_source/BLDCMotor.cpp

@@ -1,41 +1,39 @@
 #include "BLDCMotor.h"
 
-// BLDCMotor( int phA, int phB, int phC, int pp, int cpr, int en)
+// // BLDCMotor( int phA, int phB, int phC, int pp, int cpr, int en)
+// // - phA, phB, phC - motor A,B,C phase pwm pins
+// // - pp            - pole pair number
+// // - cpr           - counts per rotation number (cpm=ppm*4)
+// // - enable pin    - (optional input)
+// BLDCMotor::BLDCMotor( int phA, int phB, int phC, int pp, int cpr, int en){
+//   noop;
+// }
+
+// BLDCMotor( int pp)
 // - phA, phB, phC - motor A,B,C phase pwm pins
 // - pp            - pole pair number
 // - cpr           - counts per rotation number (cpm=ppm*4)
 // - enable pin    - (optional input)
-BLDCMotor::BLDCMotor(int phA, int phB, int phC, int pp, int en)
+BLDCMotor::BLDCMotor(int pp)
 : FOCMotor()
 {
-  // Pin initialization
-  pwmA = phA;
-  pwmB = phB;
-  pwmC = phC;
+  // save pole pairs number
   pole_pairs = pp;
+}
 
-  // enable_pin pin
-  enable_pin = en;
 
+/**
+	Link the driver which controls the motor
+*/
+void BLDCMotor::linkDriver(BLDCDriver* _driver) {
+  driver = _driver;
 }
 
-
 // init hardware pins   
-void BLDCMotor::init(long pwm_frequency) {
-  if(monitor_port) monitor_port->println("MOT: Init pins.");
-  // PWM pins
-  pinMode(pwmA, OUTPUT);
-  pinMode(pwmB, OUTPUT);
-  pinMode(pwmC, OUTPUT);
-  if(enable_pin != NOT_SET) pinMode(enable_pin, OUTPUT);
-
-  if(monitor_port) monitor_port->println("MOT: PWM config.");
-  // Increase PWM frequency to 32 kHz
-  // make silent
-  _setPwmFrequency(pwm_frequency, pwmA, pwmB, pwmC);
-
+void BLDCMotor::init() {
+  if(monitor_port) monitor_port->println("MOT: Initialise 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;
   // update the controller limits
@@ -44,7 +42,7 @@ void BLDCMotor::init(long pwm_frequency) {
 
   _delay(500);
   // enable motor
-  if(monitor_port) monitor_port->println("MOT: Enable.");
+  if(monitor_port) monitor_port->println("MOT: Enable driver.");
   enable();
   _delay(500);
 }
@@ -53,18 +51,18 @@ void BLDCMotor::init(long pwm_frequency) {
 // disable motor driver
 void BLDCMotor::disable()
 {
-  // disable the driver - if enable_pin pin available
-  if (enable_pin != NOT_SET) digitalWrite(enable_pin, LOW);
   // set zero to PWM
-  setPwm(0, 0, 0);
+  driver->setPwm(0, 0, 0);
+  // disable the driver 
+  driver->disable();
 }
 // enable motor driver
 void BLDCMotor::enable()
 {
+  // enable the driver 
+  driver->enable();
   // set zero to PWM
-  setPwm(0, 0, 0);
-  // enable_pin the driver - if enable_pin pin available
-  if (enable_pin != NOT_SET) digitalWrite(enable_pin, HIGH);
+  driver->setPwm(0, 0, 0);
 
 }
 
@@ -100,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) {
@@ -121,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
@@ -153,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()){
@@ -172,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
@@ -219,30 +217,85 @@ 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)
   {
+    case FOCModulationType::Trapezoid_120 :
+      // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5
+      static int trap_120_map[6][3] = {
+        {0,1,-1},{-1,1,0},{-1,0,1},{0,-1,1},{1,-1,0},{1,0,-1} // each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z
+      };
+      // static int trap_120_state = 0;
+      sector = 6 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes
+
+      Ua = Uq + trap_120_map[sector][0] * Uq;
+      Ub = Uq + trap_120_map[sector][1] * Uq;
+      Uc = Uq + trap_120_map[sector][2] * Uq;
+
+      if (centered) {
+        Ua += (driver->voltage_limit)/2 -Uq;
+        Ub += (driver->voltage_limit)/2 -Uq;
+        Uc += (driver->voltage_limit)/2 -Uq;
+      }
+    break;
+
+    case FOCModulationType::Trapezoid_150 :
+      // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8
+      static int trap_150_map[12][3] = {
+        {0,1,-1},{-1,1,-1},{-1,1,0},{-1,1,1},{-1,0,1},{-1,-1,1},{0,-1,1},{1,-1,1},{1,-1,0},{1,-1,-1},{1,0,-1},{1,1,-1} // each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z
+      };
+      // static int trap_150_state = 0;
+      sector = 12 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes
+      
+      Ua = Uq + trap_150_map[sector][0] * Uq;
+      Ub = Uq + trap_150_map[sector][1] * Uq;
+      Uc = Uq + trap_150_map[sector][2] * Uq;
+
+      //center
+      if (centered) {
+        Ua += (driver->voltage_limit)/2 -Uq;
+        Ub += (driver->voltage_limit)/2 -Uq;
+        Uc += (driver->voltage_limit)/2 -Uq;
+      }
+
+    break;
+
     case FOCModulationType::SinePWM :
       // Sinusoidal PWM modulation 
       // Inverse Park + Clarke 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);
+      _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 + voltage_power_supply/2;
-      Ub = -0.5 * Ualpha  + _SQRT3_2 * Ubeta + voltage_power_supply/2;
-      Uc = -0.5 * Ualpha - _SQRT3_2 * Ubeta + 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;
+      }
+
       break;
 
     case FOCModulationType::SpaceVectorPWM :
@@ -259,15 +312,15 @@ void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) {
       angle_el = _normalizeAngle(angle_el + zero_electric_angle + _PI_2);
 
       // find the sector we are in currently
-      int sector = floor(angle_el / _PI_3) + 1;
+      sector = floor(angle_el / _PI_3) + 1;
       // calculate the duty cycles
-      float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uq/voltage_power_supply;
-      float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uq/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 
-      // centered around voltage_power_supply/2
-      float T0 = 1 - T1 - T2;
-      // pulled to 0 - better for low power supply voltage
-      //float T0 = 0;
+      float T0 = 0; // pulled to 0 - better for low power supply voltage
+      if (centered) { 
+        T0 = 1 - T1 - T2; //centered around driver->voltage_limit/2
+      } 
 
       // calculate the duty cycles(times)
       float Ta,Tb,Tc; 
@@ -310,27 +363,15 @@ void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) {
       }
 
       // calculate the phase voltages and center
-      Ua = Ta*voltage_power_supply;
-      Ub = Tb*voltage_power_supply;
-      Uc = Tc*voltage_power_supply;
+      Ua = Ta*driver->voltage_limit;
+      Ub = Tb*driver->voltage_limit;
+      Uc = Tc*driver->voltage_limit;
       break;
+
   }
 
-  // set the voltages in hardware
-  setPwm(Ua, Ub, Uc);
-}
-
-
-
-// Set voltage to the pwm pin
-void BLDCMotor::setPwm(float Ua, float Ub, float Uc) {      
-  // calculate duty cycle
-  // limited in [0,1]
-  float dc_a = constrain(Ua / voltage_power_supply, 0 , 1 );
-  float dc_b = constrain(Ub / voltage_power_supply, 0 , 1 );
-  float dc_c = constrain(Uc / voltage_power_supply, 0 , 1 );
-  // hardware specific writing
-  _writeDutyCycle(dc_a, dc_b, dc_c, pwmA, pwmB, pwmC );
+  // set the voltages in driver
+  driver->setPwm(Ua, Ub, Uc);
 }
 
 
@@ -348,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;
@@ -371,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;