FEAT space vector fixed, initFOC update, natural dir impl, restructured examples

Author: askuric

keywords.txt

@@ -29,6 +29,7 @@ ControlType	KEYWORD1
 FOCModulationType	KEYWORD2
 Quadrature	KEYWORD1
 Pullup	KEYWORD1
+Direction	KEYWORD1
 
 linkSensor	KEYWORD2
 handleA	KEYWORD2
@@ -64,6 +65,13 @@ velocity	KEYWORD2
 velocity_ultra_slow	KEYWORD2
 angle	KEYWORD2
 
+
+ON	KEYWORD2
+OFF	KEYWORD2
+CW	KEYWORD2
+CCW	KEYWORD2
+UNKNOWN	KEYWORD2
+
 P	KEYWORD2
 I	KEYWORD2
 Tf	KEYWORD2

library.properties

@@ -1,9 +1,9 @@
 name=Simple FOC
 version=1.4.2
-author=Antun Skuric <antun.skuric@outlook.com>
-maintainer=Antun Skuric <antun.skuric@outlook.com>
+author=Antun Skuric <info@simplefoc.com>
+maintainer=Antun Skuric <info@simplefoc.com>
 sentence=A library demistifying FOC for BLDC motors
-paragraph=Simple library intended for hobby comunity to run the BLDC motor using FOC algorithm. It is interded both for plug&play and simple enough to be easy to extend. Library supports Arudino devices such as Arduino UNO, MEGA and similar, but also stm32 boards Nucleo and Bluepill.
+paragraph=Simple library intended for hobby comunity to run the BLDC motor using FOC algorithm. It is intended to support as many BLDC motor+sensor+driver combinations as possible and in the same time maintain simplicity of usage. Library supports Arudino devices such as Arduino UNO, MEGA, NANO and similar, stm32 boards Nucleo and Bluepill and ESP32 boards.
 category=Device Control
 url=https://docs.simplefoc.com
 architectures=*

src/BLDCMotor.cpp

@@ -131,14 +131,13 @@ int BLDCMotor::alignSensor() {
   } else if (mid_angle == start_angle) {
     if(monitor_port) monitor_port->println("MOT: Sensor failed to notice movement");
   }
-  _delay(500);
-  
-  // let the motor stabilize for 1 sec
-  _delay(500);
+
+  // let the motor stabilize for 2 sec
+  _delay(2000);
   // set sensor to zero
   sensor->initRelativeZero();
   _delay(500);
-  setPhaseVoltage(0,angle);
+  setPhaseVoltage(0,0);
   _delay(200);
 
   // find the index if available
@@ -177,7 +176,7 @@ int BLDCMotor::absoluteZeroAlign() {
     // align the sensor with the absolute zero
     float zero_offset = sensor->initAbsoluteZero();
     // remember zero electric angle
-    zero_electric_angle = electricAngle(zero_offset);
+    zero_electric_angle = normalizeAngle(electricAngle(zero_offset));
   }
   // return bool if zero found
   return !sensor->needsAbsoluteZeroSearch() ? 1 : -1;
@@ -213,12 +212,21 @@ float BLDCMotor::electricAngle(float shaftAngle) {
   FOC functions
 */
 // FOC initialization function
-int  BLDCMotor::initFOC() {
-  // sensor and motor alignment
-  _delay(500);
-  int exit_flag = alignSensor();
-  _delay(500);
-  
+int  BLDCMotor::initFOC( float zero_electric_offset, Direction sensor_direction) {
+  int exit_flag = 1;
+  // align motor if necessary
+  // alignment necessary for encoders!
+  if(zero_electric_offset != NOT_SET){
+    // abosolute zero offset provided - no need to align
+    zero_electric_angle = zero_electric_offset;
+    // set the sensor direction - default CW
+    sensor->natural_direction = sensor_direction;
+  }else{
+    // sensor and motor alignment
+    _delay(500);
+    exit_flag = alignSensor();
+    _delay(500);
+    }
   if(monitor_port) monitor_port->println("MOT: Motor ready.");
 
   return exit_flag;
@@ -307,13 +315,13 @@ void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) {
       // find the sector we are in currently
       int sector = floor(angle_el / _PI_3) + 1;
       // calculate the duty cycles
-      float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el);
-      float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3);
+      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;
       // two versions possible 
       // centered around voltage_power_supply/2
       float T0 = 1 - T1 - T2;
       // pulled to 0 - better for low power supply voltage
-      //T0 = 0;
+      //float T0 = 0;
 
       // calculate the duty cycles(times)
       float Ta,Tb,Tc; 
@@ -356,9 +364,9 @@ void BLDCMotor::setPhaseVoltage(float Uq, float angle_el) {
       }
 
       // calculate the phase voltages and center
-      Ua = Ta*Uq  + (voltage_power_supply - Uq) / 2;
-      Ub = Tb*Uq  + (voltage_power_supply - Uq) / 2;
-      Uc = Tc*Uq  + (voltage_power_supply - Uq) / 2;
+      Ua = Ta*voltage_power_supply;//  + (voltage_power_supply - Uq) / 2;
+      Ub = Tb*voltage_power_supply;//  + (voltage_power_supply - Uq) / 2;
+      Uc = Tc*voltage_power_supply;//  + (voltage_power_supply - Uq) / 2;
       break;
   }
 

src/BLDCMotor.h

@@ -97,8 +97,14 @@ class BLDCMotor
     /**
      * Function initializing FOC algorithm
      * and aligning sensor's and motors' zero position 
+     * 
+     * - If zero_electric_offset parameter is set the alignment procedure is skipped
+     * 
+     * @param zero_electric_offset value of the sensors absolute position electrical offset in respect to motor's electrical 0 position.
+     * @param sensor_direction  sensor natural direction - default is CW
+     *
      */  
-    int initFOC(); 
+    int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW); 
     /**
      * Function running FOC algorithm in real-time
      * it calculates the gets motor angle and sets the appropriate voltages 

src/Encoder.cpp

@@ -107,7 +107,7 @@ void Encoder::handleIndex() {
 	Shaft angle calculation
 */
 float Encoder::getAngle(){
-  return  _2PI * (pulse_counter) / ((float)cpr);
+  return  natural_direction * _2PI * (pulse_counter) / ((float)cpr);
 }
 /*
   Shaft velocity calculation
@@ -145,7 +145,7 @@ float Encoder::getVelocity(){
   // save velocity calculation variables
   prev_Th = Th;
   prev_pulse_counter = pulse_counter;
-  return (velocity);
+  return natural_direction * (velocity);
 }
 
 // getter for index pin

src/MagneticSensorI2C.cpp

@@ -11,7 +11,7 @@ MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution,
   // angle read register of the magnetic sensor
   angle_register_msb = _angle_register_msb;
   // register maximum value (counts per revolution)
-  cpr = pow(2,_bit_resolution);
+  cpr = pow(2, _bit_resolution);
 
   // depending on the sensor architecture there are different combinations of
   // LSB and MSB register used bits
@@ -60,7 +60,7 @@ float MagneticSensorI2C::getAngle(){
   angle_data -= (int)zero_offset;
   // return the full angle 
   // (number of full rotations)*2PI + current sensor angle
-  return full_rotation_offset + ( angle_data / (float)cpr) * _2PI;
+  return natural_direction * (full_rotation_offset + ( angle_data / (float)cpr) * _2PI);
 }
 
 // Shaft velocity calculation
@@ -85,7 +85,7 @@ float MagneticSensorI2C::getVelocity(){
 // return the angle [rad] difference
 float MagneticSensorI2C::initRelativeZero(){
   float angle_offset = -getAngle();
-  zero_offset = getRawCount();
+  zero_offset = natural_direction * getRawCount();
 
   // angle tracking variables
   full_rotation_offset = 0;

src/MagneticSensorSPI.cpp

@@ -25,9 +25,11 @@ void MagneticSensorSPI::init(){
 
 	//SPI has an internal SPI-device counter, it is possible to call "begin()" from different devices
 	SPI.begin();
+#ifndef ESP_H // if not ESP32 board
 	SPI.setBitOrder(MSBFIRST); // Set the SPI_1 bit order
 	SPI.setDataMode(SPI_MODE1) ;
 	SPI.setClockDivider(SPI_CLOCK_DIV8);
+#endif
 
 	digitalWrite(chip_select_pin, HIGH);
 	// velocity calculation init
@@ -60,7 +62,7 @@ float MagneticSensorSPI::getAngle(){
   angle_data -= (int)zero_offset;
   // return the full angle 
   // (number of full rotations)*2PI + current sensor angle
-  return full_rotation_offset + ( angle_data / (float)cpr) * _2PI;
+  return natural_direction * (full_rotation_offset + ( angle_data / (float)cpr) * _2PI);
 }
 
 // Shaft velocity calculation
@@ -85,7 +87,7 @@ float MagneticSensorSPI::getVelocity(){
 // return the angle [rad] difference
 float MagneticSensorSPI::initRelativeZero(){
   float angle_offset = -getAngle();
-  zero_offset = getRawCount();
+  zero_offset = natural_direction * getRawCount();
 
   // angle tracking variables
   full_rotation_offset = 0;
@@ -164,13 +166,21 @@ word MagneticSensorSPI::read(word angle_register){
 
   //Send the command
   digitalWrite(chip_select_pin, LOW);
+#ifndef ESP_H // if not ESP32 board
   digitalWrite(chip_select_pin, LOW);
+#endif
   SPI.transfer(left_byte);
   SPI.transfer(right_byte);
   digitalWrite(chip_select_pin,HIGH);
+#ifndef ESP_H // if not ESP32 board
   digitalWrite(chip_select_pin,HIGH);
+#endif
 
+#if defined( ESP_H ) // if ESP32 board
+  delayMicroseconds(50);
+#else
   delayMicroseconds(10);
+#endif
 
   //Now read the response
   digitalWrite(chip_select_pin, LOW);