Merge branch 'dev' of github.com:simplefoc/Arduino-FOC

Author: Padok

examples/utils/calibration/find_sensor_offset_and_direction/find_sensor_offset_and_direction.ino

@@ -2,8 +2,9 @@
  * Simple example intended to help users find the zero offset and natural direction of the sensor. 
  * 
  * These values can further be used to avoid motor and sensor alignment procedure. 
- * 
- * motor.initFOC(zero_offset, sensor_direction);
+ * To use these values add them to the code:");
+ *    motor.sensor_direction=Direction::CW; // or Direction::CCW
+ *    motor.zero_electric_angle=1.2345;     // use the real value!
  * 
  * This will only work for abosolute value sensors - magnetic sensors. 
  * Bypassing the alignment procedure is not possible for the encoders and for the current implementation of the Hall sensors. 
@@ -44,6 +45,9 @@ void setup() {
   // set motion control loop to be used
   motor.controller = MotionControlType::torque;
 
+  // force direction search - because default is CW
+  motor.sensor_direction = Direction::UNKNOWN;
+
   // initialize motor
   motor.init();
   // align sensor and start FOC
@@ -54,9 +58,16 @@ void setup() {
   Serial.println("Sensor zero offset is:");
   Serial.println(motor.zero_electric_angle, 4);
   Serial.println("Sensor natural direction is: ");
-  Serial.println(motor.sensor_direction == 1 ? "Direction::CW" : "Direction::CCW");
+  Serial.println(motor.sensor_direction == Direction::CW ? "Direction::CW" : "Direction::CCW");
+
+  Serial.println("To use these values add them to the code:");
+  Serial.print("   motor.sensor_direction=");
+  Serial.print(motor.sensor_direction == Direction::CW ? "Direction::CW" : "Direction::CCW");
+  Serial.println(";");
+  Serial.print("   motor.zero_electric_angle=");
+  Serial.print(motor.zero_electric_angle, 4);
+  Serial.println(";");
 
-  Serial.println("To use these values provide them to the: motor.initFOC(offset, direction)");
   _delay(1000);
   Serial.println("If motor is not moving the alignment procedure was not successfull!!");
 }

src/BLDCMotor.cpp

@@ -44,7 +44,9 @@ BLDCMotor::BLDCMotor(int pp, float _R, float _KV, float _inductance)
   phase_resistance = _R;
   // save back emf constant KV = 1/KV
   // 1/sqrt(2) - rms value
-  KV_rating = _KV*_SQRT2;
+  KV_rating = NOT_SET;
+  if (_isset(_KV))
+    KV_rating = _KV*_SQRT2;
   // save phase inductance
   phase_inductance = _inductance;
 
@@ -124,20 +126,13 @@ void BLDCMotor::enable()
   FOC functions
 */
 // FOC initialization function
-int  BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction) {
+int  BLDCMotor::initFOC() {
   int exit_flag = 1;
 
   motor_status = FOCMotorStatus::motor_calibrating;
 
   // align motor if necessary
   // alignment necessary for encoders!
-  if(_isset(zero_electric_offset)){
-    // abosolute zero offset provided - no need to align
-    zero_electric_angle = zero_electric_offset;
-    // set the sensor direction - default CW
-    sensor_direction = _sensor_direction;
-  }
-
   // sensor and motor alignment - can be skipped
   // by setting motor.sensor_direction and motor.zero_electric_angle
   _delay(500);
@@ -211,7 +206,7 @@ int BLDCMotor::alignSensor() {
   if(!exit_flag) return exit_flag;
 
   // if unknown natural direction
-  if(!_isset(sensor_direction)){
+  if(sensor_direction==Direction::UNKNOWN){
 
     // find natural direction
     // move one electrical revolution forward
@@ -418,7 +413,8 @@ void BLDCMotor::move(float new_target) {
       // angle set point
       shaft_angle_sp = target;
       // calculate velocity set point
-      shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle );
+      shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle );
+      shaft_angle_sp = _constrain(shaft_angle_sp,-velocity_limit, velocity_limit);
       // calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation
       current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control
       // if torque controlled through voltage
@@ -543,12 +539,8 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
     case FOCModulationType::SinePWM :
       // Sinusoidal PWM modulation
       // Inverse Park + Clarke transformation
+      _sincos(angle_el, &_sa, &_ca);
 
-      // angle normalization in between 0 and 2pi
-      // only necessary if using _sin and _cos - approximation functions
-      angle_el = _normalizeAngle(angle_el);
-      _ca = _cos(angle_el);
-      _sa = _sin(angle_el);
       // Inverse park transform
       Ualpha =  _ca * Ud - _sa * Uq;  // -sin(angle) * Uq;
       Ubeta =  _sa * Ud + _ca * Uq;    //  cos(angle) * Uq;

src/BLDCMotor.h

@@ -49,7 +49,7 @@ class BLDCMotor: public FOCMotor
      * Function initializing FOC algorithm
      * and aligning sensor's and motors' zero position 
      */  
-    int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW) override;
+    int initFOC() override;
     /**
      * Function running FOC algorithm in real-time
      * it calculates the gets motor angle and sets the appropriate voltages 

src/StepperMotor.cpp

@@ -92,20 +92,13 @@ void StepperMotor::enable()
   FOC functions
 */
 // FOC initialization function
-int  StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direction ) {
+int  StepperMotor::initFOC() {
   int exit_flag = 1;
 
   motor_status = FOCMotorStatus::motor_calibrating;
 
   // align motor if necessary
   // alignment necessary for encoders!
-  if(_isset(zero_electric_offset)){
-    // abosolute zero offset provided - no need to align
-    zero_electric_angle = zero_electric_offset;
-    // set the sensor direction - default CW
-    sensor_direction = _sensor_direction;
-  }
-
   // sensor and motor alignment - can be skipped
   // by setting motor.sensor_direction and motor.zero_electric_angle
   _delay(500);
@@ -307,7 +300,8 @@ void StepperMotor::move(float new_target) {
       // angle set point
       shaft_angle_sp = target;
       // calculate velocity set point
-      shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle );
+      shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle );
+      shaft_angle_sp = _constrain(shaft_angle_sp, -velocity_limit, velocity_limit);
       // calculate the torque command
       current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control
       // if torque controlled through voltage
@@ -357,12 +351,9 @@ void StepperMotor::move(float new_target) {
 void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
   // Sinusoidal PWM modulation
   // Inverse Park transformation
+  float _sa, _ca;
+  _sincos(angle_el, &_sa, &_ca);
 
-  // angle normalization in between 0 and 2pi
-  // only necessary if using _sin and _cos - approximation functions
-  angle_el = _normalizeAngle(angle_el);
-  float _ca = _cos(angle_el);
-  float _sa = _sin(angle_el);
   // Inverse park transform
   Ualpha =  _ca * Ud - _sa * Uq;  // -sin(angle) * Uq;
   Ubeta =  _sa * Ud + _ca * Uq;    //  cos(angle) * Uq;

src/StepperMotor.h

@@ -54,12 +54,8 @@ class StepperMotor: public FOCMotor
      * 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( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW) override;
+    int initFOC() override;
     /**
      * Function running FOC algorithm in real-time
      * it calculates the gets motor angle and sets the appropriate voltages 

src/common/base_classes/CurrentSense.cpp

@@ -38,8 +38,12 @@ float CurrentSense::getDCCurrent(float motor_electrical_angle){
     // if motor angle provided function returns signed value of the current
     // determine the sign of the current
     // sign(atan2(current.q, current.d)) is the same as c.q > 0 ? 1 : -1  
-    if(motor_electrical_angle) 
-        sign = (i_beta * _cos(motor_electrical_angle) - i_alpha*_sin(motor_electrical_angle)) > 0 ? 1 : -1;  
+    if(motor_electrical_angle) {
+        float ct;
+        float st;
+        _sincos(motor_electrical_angle, &st, &ct);
+        sign = (i_beta*ct - i_alpha*st) > 0 ? 1 : -1;  
+    }
     // return current magnitude
     return sign*_sqrt(i_alpha*i_alpha + i_beta*i_beta);
 }
@@ -78,8 +82,9 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){
     }
 
     // calculate park transform
-    float ct = _cos(angle_el);
-    float st = _sin(angle_el);
+    float ct;
+    float st;
+    _sincos(angle_el, &st, &ct);
     DQCurrent_s return_current;
     return_current.d = i_alpha * ct + i_beta * st;
     return_current.q = i_beta * ct - i_alpha * st;

src/common/base_classes/FOCMotor.h

@@ -104,12 +104,8 @@ class FOCMotor
      * 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
-     *
      */  
-    virtual int initFOC( float zero_electric_offset = NOT_SET , Direction sensor_direction = Direction::CW)=0; 
+    virtual int initFOC()=0;
     /**
      * Function running FOC algorithm in real-time
      * it calculates the gets motor angle and sets the appropriate voltages 
@@ -155,6 +151,7 @@ class FOCMotor
 
     // state variables
     float target; //!< current target value - depends of the controller
+    float feed_forward_velocity = 0.0f; //!< current feed forward velocity
   	float shaft_angle;//!< current motor angle
   	float electrical_angle;//!< current electrical angle
   	float shaft_velocity;//!< current motor velocity 
@@ -208,7 +205,7 @@ class FOCMotor
     // sensor related variabels
     float sensor_offset; //!< user defined sensor zero offset
     float zero_electric_angle = NOT_SET;//!< absolute zero electric angle - if available
-    int sensor_direction = NOT_SET; //!< if sensor_direction == Direction::CCW then direction will be flipped to CW
+    Direction sensor_direction = Direction::CW; //!< default is CW. if sensor_direction == Direction::CCW then direction will be flipped compared to CW. Set to UNKNOWN to set by calibration
 
     /**
      * Function providing BLDCMotor class with the 

src/common/base_classes/Sensor.cpp

@@ -18,7 +18,12 @@ void Sensor::update() {
 float Sensor::getVelocity() {
     // calculate sample time
     float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6;
-    // TODO handle overflow - we do need to reset vel_angle_prev_ts
+    if (Ts < 0.0f) {    // handle micros() overflow - we need to reset vel_angle_prev_ts
+        vel_angle_prev = angle_prev;
+        vel_full_rotations = full_rotations;
+        vel_angle_prev_ts = angle_prev_ts;
+        return velocity;
+    }
     if (Ts < min_elapsed_time) return velocity; // don't update velocity if deltaT is too small
 
     velocity = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts;

src/common/base_classes/Sensor.h

@@ -42,6 +42,7 @@ enum Pullup : uint8_t {
  * 
  */
 class Sensor{
+	friend class SmoothingSensor;
     public:
         /**
          * Get mechanical shaft angle in the range 0 to 2PI. This value will be as precise as possible with

src/common/foc_utils.cpp

@@ -1,50 +1,52 @@
 #include "foc_utils.h"
 
-// int array instead of float array
-// 4x200 points per 360 deg
-// 2x storage save (int 2Byte float 4 Byte )
-// sin*10000
-const int sine_array[200] = {0,79,158,237,316,395,473,552,631,710,789,867,946,1024,1103,1181,1260,1338,1416,1494,1572,1650,1728,1806,1883,1961,2038,2115,2192,2269,2346,2423,2499,2575,2652,2728,2804,2879,2955,3030,3105,3180,3255,3329,3404,3478,3552,3625,3699,3772,3845,3918,3990,4063,4135,4206,4278,4349,4420,4491,4561,4631,4701,4770,4840,4909,4977,5046,5113,5181,5249,5316,5382,5449,5515,5580,5646,5711,5775,5839,5903,5967,6030,6093,6155,6217,6279,6340,6401,6461,6521,6581,6640,6699,6758,6815,6873,6930,6987,7043,7099,7154,7209,7264,7318,7371,7424,7477,7529,7581,7632,7683,7733,7783,7832,7881,7930,7977,8025,8072,8118,8164,8209,8254,8298,8342,8385,8428,8470,8512,8553,8594,8634,8673,8712,8751,8789,8826,8863,8899,8935,8970,9005,9039,9072,9105,9138,9169,9201,9231,9261,9291,9320,9348,9376,9403,9429,9455,9481,9506,9530,9554,9577,9599,9621,9642,9663,9683,9702,9721,9739,9757,9774,9790,9806,9821,9836,9850,9863,9876,9888,9899,9910,9920,9930,9939,9947,9955,9962,9969,9975,9980,9985,9989,9992,9995,9997,9999,10000,10000};
 
 // function approximating the sine calculation by using fixed size array
-// ~40us (float array)
-// ~50us (int array)
-// precision +-0.005
-// it has to receive an angle in between 0 and 2PI
-float _sin(float a){
-  if(a < _PI_2){
-    //return sine_array[(int)(199.0f*( a / (_PI/2.0)))];
-    //return sine_array[(int)(126.6873f* a)];           // float array optimized
-    return 0.0001f*sine_array[_round(126.6873f* a)];      // int array optimized
-  }else if(a < _PI){
-    // return sine_array[(int)(199.0f*(1.0f - (a-_PI/2.0) / (_PI/2.0)))];
-    //return sine_array[398 - (int)(126.6873f*a)];          // float array optimized
-    return 0.0001f*sine_array[398 - _round(126.6873f*a)];     // int array optimized
-  }else if(a < _3PI_2){
-    // return -sine_array[(int)(199.0f*((a - _PI) / (_PI/2.0)))];
-    //return -sine_array[-398 + (int)(126.6873f*a)];           // float array optimized
-    return -0.0001f*sine_array[-398 + _round(126.6873f*a)];      // int array optimized
-  } else {
-    // return -sine_array[(int)(199.0f*(1.0f - (a - 3*_PI/2) / (_PI/2.0)))];
-    //return -sine_array[796 - (int)(126.6873f*a)];           // float array optimized
-    return -0.0001f*sine_array[796 - _round(126.6873f*a)];      // int array optimized
+// uses a 65 element lookup table and interpolation
+// thanks to @dekutree for his work on optimizing this
+__attribute__((weak)) float _sin(float a){
+  // 16bit integer array for sine lookup. interpolation is used for better precision
+  // 16 bit precision on sine value, 8 bit fractional value for interpolation, 6bit LUT size
+  // resulting precision compared to stdlib sine is 0.00006480 (RMS difference in range -PI,PI for 3217 steps)
+  static uint16_t sine_array[65] = {0,804,1608,2411,3212,4011,4808,5602,6393,7180,7962,8740,9512,10279,11039,11793,12540,13279,14010,14733,15447,16151,16846,17531,18205,18868,19520,20160,20788,21403,22006,22595,23170,23732,24279,24812,25330,25833,26320,26791,27246,27684,28106,28511,28899,29269,29622,29957,30274,30572,30853,31114,31357,31581,31786,31972,32138,32286,32413,32522,32610,32679,32729,32758,32768};
+  unsigned int i = (unsigned int)(a * (64*4*256 /_2PI));
+  int t1, t2, frac = i & 0xff;
+  i = (i >> 8) & 0xff;
+  if (i < 64) {
+    t1 = sine_array[i]; t2 = sine_array[i+1];
+  }
+  else if(i < 128) {
+    t1 = sine_array[128 - i]; t2 = sine_array[127 - i];
+  }
+  else if(i < 192) {
+    t1 = -sine_array[-128 + i]; t2 = -sine_array[-127 + i];
   }
+  else {
+    t1 = -sine_array[256 - i]; t2 = -sine_array[255 - i];
+  }
+  return (1.0f/32768.0f) * (t1 + (((t2 - t1) * frac) >> 8));
 }
 
 // function approximating cosine calculation by using fixed size array
 // ~55us (float array)
 // ~56us (int array)
 // precision +-0.005
 // it has to receive an angle in between 0 and 2PI
-float _cos(float a){
+__attribute__((weak)) float _cos(float a){
   float a_sin = a + _PI_2;
   a_sin = a_sin > _2PI ? a_sin - _2PI : a_sin;
   return _sin(a_sin);
 }
 
 
+__attribute__((weak)) void _sincos(float a, float* s, float* c){
+  *s = _sin(a);
+  *c = _cos(a);
+}
+
+
 // normalizing radian angle to [0,2PI]
-float _normalizeAngle(float angle){
+__attribute__((weak)) float _normalizeAngle(float angle){
   float a = fmod(angle, _2PI);
   return a >= 0 ? a : (a + _2PI);
 }
@@ -57,15 +59,13 @@ float _electricalAngle(float shaft_angle, int pole_pairs) {
 // square root approximation function using
 // https://reprap.org/forum/read.php?147,219210
 // https://en.wikipedia.org/wiki/Fast_inverse_square_root
-float _sqrtApprox(float number) {//low in fat
-  long i;
-  float y;
+__attribute__((weak)) float _sqrtApprox(float number) {//low in fat
   // float x;
   // const float f = 1.5F; // better precision
 
   // x = number * 0.5F;
-  y = number;
-  i = * ( long * ) &y;
+  float y = number;
+  long i = * ( long * ) &y;
   i = 0x5f375a86 - ( i >> 1 );
   y = * ( float * ) &i;
   // y = y * ( f - ( x * y * y ) ); // better precision