Merge pull request #422 from simplefoc/feat_new_cs_align

Feat new cs align

Author: askuric

README.md

@@ -13,7 +13,8 @@
 ![GitHub commits since tagged version](https://img.shields.io/github/commits-since/simplefoc/arduino-foc/latest/dev)
 ![GitHub commit activity (branch)](https://img.shields.io/github/commit-activity/m/simplefoc/arduino-foc/dev)
 
-[![arduino-library-badge](https://www.ardu-badge.com/badge/Simple%20FOC.svg?)](https://www.ardu-badge.com/badge/Simple%20FOC.svg)
+[![arduino-library-badge](https://ardubadge.simplefoc.com?lib=Simple%20FOC)](https://www.ardu-badge.com/badge/Simple%20FOC.svg)
+[![PlatformIO Registry](https://badges.registry.platformio.org/packages/askuric/library/Simple%20FOC.svg)](https://registry.platformio.org/libraries/askuric/Simple%20FOC)
 [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT)
 [![status](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d/status.svg)](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d)
 
@@ -28,18 +29,8 @@ Therefore this is an attempt to:
    - For official driver boards see [<span class="simple">Simple<span class="foc">FOC</span>Boards</span>](https://docs.simplefoc.com/boards)
    - Many many more boards developed by the community members, see [<span class="simple">Simple<span class="foc">FOC</span>Community</span>](https://community.simplefoc.com/)
 
-> NEW RELEASE 📢 : <span class="simple">Simple<span class="foc">FOC</span>library</span> v2.3.3
-> - Teensy4 
->   - support for low-side current sensing [#392](https://github.com/simplefoc/Arduino-FOC/pull/392)
->   - support for center aligned 6pwm and 3pwm (optional) [#392](https://github.com/simplefoc/Arduino-FOC/pull/392)
-> - stm32 
->   - support for center aligned pwm (even across multiple timers and motors/drivers) [#374](https://github.com/simplefoc/Arduino-FOC/pull/374), [#388](https://github.com/simplefoc/Arduino-FOC/pull/388)
->   - support for DMA based low-side current sensing: [#383](https://github.com/simplefoc/Arduino-FOC/pull/383),[#378](https://github.com/simplefoc/Arduino-FOC/pull/378)
->   - support for f7 architecture [#388](https://github.com/simplefoc/Arduino-FOC/pull/388),[#394](https://github.com/simplefoc/Arduino-FOC/pull/394)
-> - KV rating calculation fix [#347](https://github.com/simplefoc/Arduino-FOC/pull/347)
-> - Much more performant Space Vector PWM calculation  [#340](https://github.com/simplefoc/Arduino-FOC/pull/340)
-> - And much more:
->   - See the complete list of bugfixes and new features of v2.3.3 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/10?closed=1)
+> NEXT RELEASE 📢 : <span class="simple">Simple<span class="foc">FOC</span>library</span> v2.3.4
+> - Current sensing support for Stepper motors (lowside and inline)
 
 
 ## Arduino *SimpleFOClibrary* v2.3.3

examples/hardware_specific_examples/SimpleFOCMini/open_loop/open_loop.ino

@@ -0,0 +1,93 @@
+/**
+ *
+ * SimpleFOCMini motor control example
+ * 
+ * For Arduino UNO or the other boards with the UNO headers
+ * the most convenient way to use the board is to stack it to the pins:
+ * - 12 - ENABLE
+ * - 11 - IN1
+ * - 10 - IN2
+ * -  9 - IN3
+ *
+ */
+#include <SimpleFOC.h>
+
+
+// BLDC motor & driver instance
+BLDCMotor motor = BLDCMotor(11);
+// BLDCDriver3PWM driver = BLDCDriver3PWM(11, 10, 9, 8); // mini v1.0
+BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 12); // mini v1.1
+
+// instantiate the commander
+Commander command = Commander(Serial);
+void doMotor(char* cmd) { command.motor(&motor, cmd); }
+
+void setup() {
+  // use monitoring with serial 
+  Serial.begin(115200);
+  // enable more verbose output for debugging
+  // comment out if not needed
+  SimpleFOCDebug::enable(&Serial);
+
+  // if SimpleFOCMini is stacked in arduino headers
+  // on pins 12,11,10,9,8 
+  // pin 12 is used as ground
+  pinMode(12,OUTPUT);
+  pinMode(12,LOW);
+
+  // driver config
+  // power supply voltage [V]
+  driver.voltage_power_supply = 12;
+  driver.init();
+  // link the motor and the driver
+  motor.linkDriver(&driver);
+
+  // aligning voltage [V]
+  motor.voltage_sensor_align = 3;
+
+  // set motion control loop to be used
+  motor.controller = MotionControlType::velocity_openloop;
+
+  // default voltage_power_supply
+  motor.voltage_limit = 2; // Volts
+
+  // comment out if not needed
+  motor.useMonitoring(Serial);
+
+  // initialize motor
+  motor.init();
+  // align encoder and start FOC
+  motor.initFOC();
+
+  // add target command M
+  command.add('M', doMotor, "motor");
+
+  Serial.println(F("Motor ready."));
+  Serial.println(F("Set the target velocity using serial terminal:"));
+  
+  motor.target = 1; //initial target velocity 1 rad/s
+  Serial.println("Target velocity: 1 rad/s");
+  Serial.println("Voltage limit 2V");
+  _delay(1000);
+}
+
+void loop() {
+  // main FOC algorithm function
+  // the faster you run this function the better
+  // Arduino UNO loop  ~1kHz
+  // Bluepill loop ~10kHz
+  motor.loopFOC();
+
+  // Motion control function
+  // velocity, position or voltage (defined in motor.controller)
+  // this function can be run at much lower frequency than loopFOC() function
+  // You can also use motor.move() and set the motor.target in the code
+  motor.move();
+
+  // function intended to be used with serial plotter to monitor motor variables
+  // significantly slowing the execution down!!!!
+  // motor.monitor();
+
+  // user communication
+  command.run();
+}

src/BLDCMotor.cpp

@@ -202,7 +202,7 @@ int BLDCMotor::alignCurrentSense() {
   SIMPLEFOC_DEBUG("MOT: Align current sense.");
 
   // align current sense and the driver
-  exit_flag = current_sense->driverAlign(voltage_sensor_align);
+  exit_flag = current_sense->driverAlign(voltage_sensor_align, modulation_centered);
   if(!exit_flag){
     // error in current sense - phase either not measured or bad connection
     SIMPLEFOC_DEBUG("MOT: Align error!");

src/BLDCMotor.h

@@ -4,6 +4,7 @@
 #include "Arduino.h"
 #include "common/base_classes/FOCMotor.h"
 #include "common/base_classes/Sensor.h"
+#include "common/base_classes/FOCDriver.h"
 #include "common/base_classes/BLDCDriver.h"
 #include "common/foc_utils.h"
 #include "common/time_utils.h"

src/StepperMotor.cpp

@@ -108,12 +108,28 @@ int  StepperMotor::initFOC() {
   // alignment necessary for encoders!
   // sensor and motor alignment - can be skipped
   // by setting motor.sensor_direction and motor.zero_electric_angle
-  _delay(500);
   if(sensor){
     exit_flag *= alignSensor();
     // added the shaft_angle update
     sensor->update();
-    shaft_angle = sensor->getAngle();
+    shaft_angle = shaftAngle();
+
+    // aligning the current sensor - can be skipped
+    // checks if driver phases are the same as current sense phases
+    // and checks the direction of measuremnt.
+    if(exit_flag){
+      if(current_sense){ 
+        if (!current_sense->initialized) {
+          motor_status = FOCMotorStatus::motor_calib_failed;
+          SIMPLEFOC_DEBUG("MOT: Init FOC error, current sense not initialized");
+          exit_flag = 0;
+        }else{
+          exit_flag *= alignCurrentSense();
+        }
+      }
+      else { SIMPLEFOC_DEBUG("MOT: No current sense."); }
+    }
+
   } else {
     SIMPLEFOC_DEBUG("MOT: No sensor.");
     if ((controller == MotionControlType::angle_openloop || controller == MotionControlType::velocity_openloop)){
@@ -136,6 +152,26 @@ int  StepperMotor::initFOC() {
   return exit_flag;
 }
 
+// Calibrate the motor and current sense phases
+int StepperMotor::alignCurrentSense() {
+  int exit_flag = 1; // success
+
+  SIMPLEFOC_DEBUG("MOT: Align current sense.");
+
+  // align current sense and the driver
+  exit_flag = current_sense->driverAlign(voltage_sensor_align, modulation_centered);
+  if(!exit_flag){
+    // error in current sense - phase either not measured or bad connection
+    SIMPLEFOC_DEBUG("MOT: Align error!");
+    exit_flag = 0;
+  }else{
+    // output the alignment status flag
+    SIMPLEFOC_DEBUG("MOT: Success: ", exit_flag);
+  }
+
+  return exit_flag > 0;
+}
+
 // Encoder alignment to electrical 0 angle
 int StepperMotor::alignSensor() {
   int exit_flag = 1; //success
@@ -261,8 +297,6 @@ void StepperMotor::loopFOC() {
 
   // if open-loop do nothing
   if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return;
-  // shaft angle
-  shaft_angle = shaftAngle();
 
   // if disabled do nothing
   if(!enabled) return;
@@ -271,7 +305,40 @@ void StepperMotor::loopFOC() {
   // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() 
   // which is in range 0-2PI
   electrical_angle = electricalAngle();
-
+  switch (torque_controller) {
+    case TorqueControlType::voltage:
+      // no need to do anything really
+      break;
+    case TorqueControlType::dc_current:
+      if(!current_sense) return;
+      // read overall current magnitude
+      current.q = current_sense->getDCCurrent(electrical_angle);
+      // filter the value values
+      current.q = LPF_current_q(current.q);
+      // calculate the phase voltage
+      voltage.q = PID_current_q(current_sp - current.q);
+      // d voltage  - lag compensation
+      if(_isset(phase_inductance)) voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+      else voltage.d = 0;
+      break;
+    case TorqueControlType::foc_current:
+      if(!current_sense) return;
+      // read dq currents
+      current = current_sense->getFOCCurrents(electrical_angle);
+      // filter values
+      current.q = LPF_current_q(current.q);
+      current.d = LPF_current_d(current.d);
+      // calculate the phase voltages
+      voltage.q = PID_current_q(current_sp - current.q);
+      voltage.d = PID_current_d(-current.d);
+      // d voltage - lag compensation - TODO verify
+      // if(_isset(phase_inductance)) voltage.d = _constrain( voltage.d - current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+      break;
+    default:
+      // no torque control selected
+      SIMPLEFOC_DEBUG("MOT: no torque control selected!");
+      break;
+  }
   // set the phase voltage - FOC heart function :)
   setPhaseVoltage(voltage.q, voltage.d, electrical_angle);
 }
@@ -310,56 +377,70 @@ void StepperMotor::move(float new_target) {
   // estimate the motor current if phase reistance available and current_sense not available
   if(!current_sense && _isset(phase_resistance)) current.q = (voltage.q - voltage_bemf)/phase_resistance;
 
-  // choose control loop
+   // upgrade the current based voltage limit
   switch (controller) {
     case MotionControlType::torque:
-      if(!_isset(phase_resistance))  voltage.q = target; // if voltage torque control
-      else  voltage.q =  target*phase_resistance + voltage_bemf;
-      voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit);
-      // set d-component (lag compensation if known inductance)
-      if(!_isset(phase_inductance)) voltage.d = 0;
-      else voltage.d = _constrain( -target*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+      if(torque_controller == TorqueControlType::voltage){ // if voltage torque control
+        if(!_isset(phase_resistance))  voltage.q = target;
+        else  voltage.q =  target*phase_resistance + voltage_bemf;
+        voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit);
+        // set d-component (lag compensation if known inductance)
+        if(!_isset(phase_inductance)) voltage.d = 0;
+        else voltage.d = _constrain( -target*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+      }else{
+        current_sp = target; // if current/foc_current torque control
+      }
       break;
     case MotionControlType::angle:
+      // TODO sensor precision: this calculation is not numerically precise. The target value cannot express precise positions when
+      //                        the angles are large. This results in not being able to command small changes at high position values.
+      //                        to solve this, the delta-angle has to be calculated in a numerically precise way.
       // angle set point
       shaft_angle_sp = target;
       // calculate velocity set point
       shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle );
-      shaft_velocity_sp = _constrain(shaft_velocity_sp, -velocity_limit, velocity_limit);
-      // calculate the torque command
+      shaft_velocity_sp = _constrain(shaft_velocity_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
-      // use voltage if phase-resistance not provided
-      if(!_isset(phase_resistance))  voltage.q = current_sp;
-      else  voltage.q =  _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
-      // set d-component (lag compensation if known inductance)
-      if(!_isset(phase_inductance)) voltage.d = 0;
-      else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+      if(torque_controller == TorqueControlType::voltage){
+        // use voltage if phase-resistance not provided
+        if(!_isset(phase_resistance))  voltage.q = current_sp;
+        else  voltage.q =  _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
+        // set d-component (lag compensation if known inductance)
+        if(!_isset(phase_inductance)) voltage.d = 0;
+        else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+      }
       break;
     case MotionControlType::velocity:
-      // velocity set point
+      // velocity set point - sensor precision: this calculation is numerically precise.
       shaft_velocity_sp = target;
       // calculate the torque command
       current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control
       // if torque controlled through voltage control
-      // use voltage if phase-resistance not provided
-      if(!_isset(phase_resistance))  voltage.q = current_sp;
-      else  voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
-      // set d-component (lag compensation if known inductance)
-      if(!_isset(phase_inductance)) voltage.d = 0;
-      else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+      if(torque_controller == TorqueControlType::voltage){
+        // use voltage if phase-resistance not provided
+        if(!_isset(phase_resistance))  voltage.q = current_sp;
+        else  voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
+        // set d-component (lag compensation if known inductance)
+        if(!_isset(phase_inductance)) voltage.d = 0;
+        else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+      }
       break;
     case MotionControlType::velocity_openloop:
-      // velocity control in open loop
+      // velocity control in open loop - sensor precision: this calculation is numerically precise.
       shaft_velocity_sp = target;
       voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor
-      voltage.d = 0; // TODO d-component lag-compensation 
+      voltage.d = 0;
       break;
     case MotionControlType::angle_openloop:
-      // angle control in open loop
+      // angle control in open loop - 
+      // TODO sensor precision: this calculation NOT numerically precise, and subject
+      //                        to the same problems in small set-point changes at high angles 
+      //                        as the closed loop version.
       shaft_angle_sp = target;
       voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor
-      voltage.d = 0; // TODO d-component lag-compensation 
+      voltage.d = 0;
       break;
   }
 }

src/StepperMotor.h

@@ -89,6 +89,8 @@ class StepperMotor: public FOCMotor
     int alignSensor();
     /** Motor and sensor alignment to the sensors absolute 0 angle  */
     int absoluteZeroSearch();
+    /** Current sense and motor phase alignment */
+    int alignCurrentSense();
 
     // Open loop motion control    
     /**