commander refactorred and separated motion control into a new callback

Author: askuric

README.md

@@ -22,7 +22,11 @@ Therefore this is an attempt to:
    <p class="heading">FUTURE RELEASE 📢: <span class="simple">Simple<span class="foc">FOC</span>library</span> v2.2.1 </p>
    <ul>
       <li>Sensor class init bugfix #121</li>
-      <li>Added the new motion control interface to the commander- possible to set the position, velocity and torque target at once</li>
+      <li>Added the new motion control interface to the commander
+      <ul>
+         <li>New target setting - possible to set the position, velocity and torque target at once</li>
+         <li>Separated the motion control interface from full motor callback - only motion control and torque control type, enable disable and target setting</li>
+      </ul>
       <li>NRF52 series mcus support by <a href="https://github.com/Polyphe">@Polyphe</a></li>
       <li>Voltage/current limit handling bugs #118</li>
       <li>Generic position and current sense classes - to implement a new sensor only implement one function</li>

examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino

@@ -17,7 +17,7 @@ InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2);
 
 // commander communication instance
 Commander command = Commander(Serial);
-void doTarget(char* cmd){ command.scalar(&motor.target, cmd); }
+void doMotion(char* cmd){ command.motion(&motor, cmd); }
 // void doMotor(char* cmd){ command.motor(&motor, cmd); }
 
 void setup() {
@@ -74,7 +74,7 @@ void setup() {
   motor.target = 2;
 
   // subscribe motor to the commander
-  command.add('T', doTarget, "target");
+  command.add('T', doMotion, "motion control");
   // command.add('M', doMotor, "motor");
 
   // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com)

examples/utils/calibration/find_current_sense_gains/encoder/find_current_sense_gains/find_current_sense_gains.ino

@@ -0,0 +1,176 @@
+/**
+ * Utility arduino sketch which finds pole pair number of the motor
+ *
+ * To run it just set the correct pin numbers for the BLDC driver and encoder A and B channel as well as the encoder PPR value.
+ *
+ * The program will rotate your motor a specific amount and check how much it moved, and by doing a simple calculation calculate your pole pair number.
+ * The pole pair number will be outputted to the serial terminal.
+ *
+ * If the pole pair number is well estimated your motor will start to spin in voltage mode with 2V target.
+ *
+ * If the code calculates negative pole pair number please invert your encoder A and B channel pins or motor connector.
+ *
+ * Try running this code several times to avoid statistical errors.
+ * > But in general if your motor spins, you have a good pole pairs number.
+ */
+#include <SimpleFOC.h>
+
+// BLDC motor instance
+BLDCMotor motor = BLDCMotor(10);
+BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
+
+//  Encoder(int encA, int encB , int cpr, int index)
+Encoder encoder = Encoder(2, 3, 2048);
+// interrupt routine intialisation
+void doA(){encoder.handleA();}
+void doB(){encoder.handleB();}
+
+// current sensor
+// shunt resistor value
+// gain value
+// pins phase A,B, (C optional)
+InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, A0, A2);
+
+void setup() {
+
+  // initialise encoder hardware
+  encoder.init();
+  // hardware interrupt enable
+  encoder.enableInterrupts(doA, doB);
+  // link the motor to the sensor
+  motor.linkSensor(&encoder);
+
+  // power supply voltage
+  // default 12V
+  driver.voltage_power_supply = 12;
+  driver.init();
+  motor.linkDriver(&driver);
+
+  // initialize motor
+  motor.init();
+  // monitoring port
+  Serial.begin(115200);
+
+  // initialise the current sensing
+  current_sense.init();
+
+  // pole pairs calculation routine
+  Serial.println("Pole pairs (PP) estimator");
+  Serial.println("-\n");
+
+  float pp_search_voltage = 4; // maximum power_supply_voltage/2
+  float pp_search_angle = 8*M_PI; // search electrical angle to turn
+  float pp_vel_limit = 1; 
+
+  // move motor to the electrical angle 0
+  motor.controller = MotionControlType::angle_openloop;
+  motor.voltage_limit = pp_search_voltage;
+  motor.velocity_limit = pp_vel_limit;
+  motor.move(0);
+  _delay(1000);
+
+  // move the motor slowly to the electrical angle pp_search_angle
+  float a_max=0,b_max=0,c_max=0;
+  while(motor_angle <= pp_search_angle){
+    motor.move(pp_search_angle);
+    PhaseCurrent_s c = current_sense.getPhaseCurrents();
+    a_max = fabs(c.a) > a_max ? fabs(c.a) : a_max;
+    b_max = fabs(c.b) > b_max ? fabs(c.b) : b_max;
+    c_max = fabs(c.c) > c_max ? fabs(c.c) : c_max;
+  }
+  _delay(1000);
+  
+  Serial.print(a_max);
+  Serial.print("\t");
+  Serial.print(b_max);
+  Serial.print("\t");
+  Serial.println(b_max);
+
+  return;
+  // calculate the pole pair number
+  int pp = round((pp_search_angle)/(angle_end-angle_begin));
+
+  Serial.print(F("Estimated PP : "));
+  Serial.println(pp);
+  Serial.println(F("PP = Electrical angle / Encoder angle "));
+  Serial.print(pp_search_angle*180/M_PI);
+  Serial.print("/");
+  Serial.print((angle_end-angle_begin)*180/M_PI);
+  Serial.print(" = ");
+  Serial.println((pp_search_angle)/(angle_end-angle_begin));
+  Serial.println();
+
+
+  // a bit of monitoring the result
+  if(pp <= 0 ){
+    Serial.println(F("PP number cannot be negative"));
+    Serial.println(F(" - Try changing the search_voltage value or motor/encoder configuration."));
+    return;
+  }else if(pp > 30){
+    Serial.println(F("PP number very high, possible error."));
+  }else{
+    Serial.println(F("If PP is estimated well your motor should turn now!"));
+    Serial.println(F(" - If it is not moving try to relaunch the program!"));
+    Serial.println(F(" - You can also try to adjust the target voltage using serial terminal!"));
+  }
+
+
+  // set FOC loop to be used
+  motor.controller = MotionControlType::torque;
+  // set the pole pair number to the motor
+  motor.pole_pairs = pp;
+  //align encoder and start FOC
+  motor.initFOC();
+  _delay(1000);
+
+  Serial.println(F("\n Motor ready."));
+  Serial.println(F("Set the target voltage using serial terminal:"));
+}
+
+// uq voltage
+float target_voltage = 2;
+
+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(target_voltage);
+
+  // communicate with the user
+  serialReceiveUserCommand();
+}
+
+
+// utility function enabling serial communication with the user to set the target values
+// this function can be implemented in serialEvent function as well
+void serialReceiveUserCommand() {
+
+  // a string to hold incoming data
+  static String received_chars;
+
+  while (Serial.available()) {
+    // get the new byte:
+    char inChar = (char)Serial.read();
+    // add it to the string buffer:
+    received_chars += inChar;
+    // end of user input
+    if (inChar == '\n') {
+
+      // change the motor target
+      target_voltage = received_chars.toFloat();
+      Serial.print("Target voltage: ");
+      Serial.println(target_voltage);
+
+      // reset the command buffer
+      received_chars = "";
+    }
+  }
+}

examples/utils/calibration/find_current_sense_gains/magnetic_sensor/find_current_sense_gains/find_current_sense_gains.ino

@@ -0,0 +1,173 @@
+/**
+ * Utility arduino sketch which finds pole pair number of the motor
+ *
+ * To run it just set the correct pin numbers for the BLDC driver and sensor CPR value and chip select pin.
+ *
+ * The program will rotate your motor a specific amount and check how much it moved, and by doing a simple calculation calculate your pole pair number.
+ * The pole pair number will be outputted to the serial terminal.
+ *
+ * If the pole pair number is well estimated your motor will start to spin in voltage mode with 2V target.
+ *
+ * If the code calculates negative pole pair number please invert your motor connector.
+ *
+ * Try running this code several times to avoid statistical errors.
+ * > But in general if your motor spins, you have a good pole pairs number.
+ */
+#include <SimpleFOC.h>
+
+// BLDC motor instance
+// its important to put pole pairs number as 1!!!
+BLDCMotor motor = BLDCMotor(1);
+BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
+// Stepper motor instance
+// its important to put pole pairs number as 1!!!
+//StepperMotor motor = StepperMotor(1);
+//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6,  8);
+
+// magnetic sensor instance - SPI
+MagneticSensorSPI sensor = MagneticSensorSPI(10, 14, 0x3FFF);
+// magnetic sensor instance - I2C
+//MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0X0C, 4);
+// magnetic sensor instance - analog output
+// MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);
+
+void setup() {
+
+  // initialise magnetic sensor hardware
+  sensor.init();
+  // link the motor to the sensor
+  motor.linkSensor(&sensor);
+
+  // power supply voltage
+  // default 12V
+  driver.voltage_power_supply = 12;
+  driver.init();
+  motor.linkDriver(&driver);
+
+  // initialize motor hardware
+  motor.init();
+
+  // monitoring port
+  Serial.begin(115200);
+
+  // pole pairs calculation routine
+  Serial.println("Pole pairs (PP) estimator");
+  Serial.println("-\n");
+
+  float pp_search_voltage = 4; // maximum power_supply_voltage/2
+  float pp_search_angle = 6*M_PI; // search electrical angle to turn
+
+  // move motor to the electrical angle 0
+  motor.controller = MotionControlType::angle_openloop;
+  motor.voltage_limit=pp_search_voltage;
+  motor.move(0);
+  _delay(1000);
+  // read the sensor angle
+  sensor.update();
+  float angle_begin = sensor.getAngle();
+  _delay(50);
+
+  // move the motor slowly to the electrical angle pp_search_angle
+  float motor_angle = 0;
+  while(motor_angle <= pp_search_angle){
+    motor_angle += 0.01f;
+    sensor.update(); // keep track of the overflow
+    motor.move(motor_angle);
+    _delay(1);
+  }
+  _delay(1000);
+  // read the sensor value for 180
+  sensor.update(); 
+  float angle_end = sensor.getAngle();
+  _delay(50);
+  // turn off the motor
+  motor.move(0);
+  _delay(1000);
+
+  // calculate the pole pair number
+  int pp = round((pp_search_angle)/(angle_end-angle_begin));
+
+  Serial.print(F("Estimated PP : "));
+  Serial.println(pp);
+  Serial.println(F("PP = Electrical angle / Encoder angle "));
+  Serial.print(pp_search_angle*180/M_PI);
+  Serial.print(F("/"));
+  Serial.print((angle_end-angle_begin)*180/M_PI);
+  Serial.print(F(" = "));
+  Serial.println((pp_search_angle)/(angle_end-angle_begin));
+  Serial.println();
+
+
+  // a bit of monitoring the result
+  if(pp <= 0 ){
+    Serial.println(F("PP number cannot be negative"));
+    Serial.println(F(" - Try changing the search_voltage value or motor/sensor configuration."));
+    return;
+  }else if(pp > 30){
+    Serial.println(F("PP number very high, possible error."));
+  }else{
+    Serial.println(F("If PP is estimated well your motor should turn now!"));
+    Serial.println(F(" - If it is not moving try to relaunch the program!"));
+    Serial.println(F(" - You can also try to adjust the target voltage using serial terminal!"));
+  }
+
+
+  // set motion control loop to be used
+  motor.controller = MotionControlType::torque;
+  // set the pole pair number to the motor
+  motor.pole_pairs = pp;
+  //align sensor and start FOC
+  motor.initFOC();
+  _delay(1000);
+
+  Serial.println(F("\n Motor ready."));
+  Serial.println(F("Set the target voltage using serial terminal:"));
+}
+
+// uq voltage
+float target_voltage = 2;
+
+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(target_voltage);
+
+  // communicate with the user
+  serialReceiveUserCommand();
+}
+
+
+// utility function enabling serial communication with the user to set the target values
+// this function can be implemented in serialEvent function as well
+void serialReceiveUserCommand() {
+
+  // a string to hold incoming data
+  static String received_chars;
+
+  while (Serial.available()) {
+    // get the new byte:
+    char inChar = (char)Serial.read();
+    // add it to the string buffer:
+    received_chars += inChar;
+    // end of user input
+    if (inChar == '\n') {
+
+      // change the motor target
+      target_voltage = received_chars.toFloat();
+      Serial.print("Target voltage: ");
+      Serial.println(target_voltage);
+
+      // reset the command buffer
+      received_chars = "";
+    }
+  }
+}

examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino

@@ -73,6 +73,7 @@ void setup() {
   while(motor_angle <= pp_search_angle){
     motor_angle += 0.01f;
     motor.move(motor_angle);
+    _delay(1);
   }
   _delay(1000);
   // read the encoder value for 180

examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino

@@ -73,6 +73,7 @@ void setup() {
     motor_angle += 0.01f;
     sensor.update(); // keep track of the overflow
     motor.move(motor_angle);
+    _delay(1);
   }
   _delay(1000);
   // read the sensor value for 180