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

Richard Unger · Richard Unger · commit e0cceab53860 · 2022-02-03T19:58:59.000+01:00
diff --git a/README.md b/README.md
@@ -18,17 +18,27 @@ Therefore this is an attempt to:
    - See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller)
 
 
+
 <blockquote class="info">
-   <p class="heading">FUTURE RELEASE 📢: <span class="simple">Simple<span class="foc">FOC</span>library</span> v2.2.1 </p>
+   <p class="heading">NEW RELEASE 📢: <span class="simple">Simple<span class="foc">FOC</span>library</span> v2.2.1 <a href="https://github.com/simplefoc/Arduino-FOC/releases/tag/v2.2.1">see release</a></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>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>
-      <li>Initial support for esp32s2 and esp32s3 - separation of the esp32 mcpwm and led implementation</li>
-      <li><b>esp32 arduino package transfer to v2.0.1+</b> - helpful PR#149 by <a href="https://github.com/samguns">samguns</a></li>
-   </ul>
+      <li>Sensor class init bugfix <a href="https://github.com/simplefoc/Arduino-FOC/issues/121">#121</a></li>
+      <li>Voltage/current limit handling bugs <a href="https://github.com/simplefoc/Arduino-FOC/issues/118">#118</a></li>
+      <li>Added the new motion control interface to the commander <a href="https://docs.simplefoc.com/commander_target">see docs</a>
+      <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>
+      <li>New MCU support <a href="https://docs.simplefoc.com/microcontrollers">see docs</a>
+      <ul>
+        <li>NRF52 series mcus support by <a href="https://github.com/Polyphe">@Polyphe</a></li>
+        <li><b>esp32 arduino package transfer to v2.0.1+</b> - helpful <a href="https://github.com/simplefoc/Arduino-FOC/pull/92/149">PR#149</a> by <a href="https://github.com/samguns">samguns</a></li>
+        <li>Initial support for esp32s2 and esp32s3 - separation of the esp32 mcpwm and led implementation</li>
+      </ul>
+      </li>
+      <li>Generic sensor class - to implement a new sensor only implement one function <a href="https://docs.simplefoc.com/generic_sensor">see docs</a></li>
+      </ul>
 </blockquote>
 
 ## Arduino *SimpleFOClibrary* v2.2
diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino b/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)
diff --git a/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino b/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
diff --git a/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino b/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
diff --git a/keywords.txt b/keywords.txt
@@ -122,6 +122,7 @@ pullup	KEYWORD2
 quadrature	KEYWORD2
 foc_modulation	KEYWORD2
 target	KEYWORD2
+motion	KEYWORD2
 pwm_frequency	KEYWORD2
 dead_zone	KEYWORD2
 gain_a	KEYWORD2
diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp
@@ -101,9 +101,7 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
 
   // if target setting
   if(isDigit(user_command[0]) || user_command[0] == '-' || user_command[0] == '+'){
-    printVerbose(F("Target: "));
-    motor->target = atof(user_command);
-    println(motor->target);
+    target(motor, user_command);
     return;
   }
 
@@ -177,65 +175,9 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
       }
       break;
     case CMD_MOTION_TYPE:
-      printVerbose(F("Motion:"));
-      switch(sub_cmd){
-        case SCMD_DOWNSAMPLE:
-            printVerbose(F(" downsample: "));
-            if(!GET) motor->motion_downsample = value;
-            println((int)motor->motion_downsample);
-          break;
-        default:
-          // change control type
-          if(!GET && value >= 0 && (int)value < 5) // if set command
-            motor->controller = (MotionControlType)value;
-          switch(motor->controller){
-            case MotionControlType::torque:
-              println(F("torque"));
-              break;
-            case MotionControlType::velocity:
-              println(F("vel"));
-              break;
-            case MotionControlType::angle:
-              println(F("angle"));
-              break;
-            case MotionControlType::velocity_openloop:
-              println(F("vel open"));
-              break;
-            case MotionControlType::angle_openloop:
-              println(F("angle open"));
-              break;
-          }
-            break;
-        }
-      break;
     case CMD_TORQUE_TYPE:
-      // change control type
-      printVerbose(F("Torque: "));
-      if(!GET && (int8_t)value >= 0 && (int8_t)value < 3)// if set command
-        motor->torque_controller = (TorqueControlType)value;
-      switch(motor->torque_controller){
-        case TorqueControlType::voltage:
-          println(F("volt"));
-          // change the velocity control limits if necessary
-          if( !_isset(motor->phase_resistance) ) motor->PID_velocity.limit = motor->voltage_limit;
-          break;
-        case TorqueControlType::dc_current:
-          println(F("dc curr"));
-          // change the velocity control limits if necessary
-          motor->PID_velocity.limit = motor->current_limit;
-          break;
-        case TorqueControlType::foc_current:
-          println(F("foc curr"));
-          // change the velocity control limits if necessary
-          motor->PID_velocity.limit = motor->current_limit;
-          break;
-      }
-      break;
     case CMD_STATUS:
-      // enable/disable
-      printVerbose(F("Status: "));
-      if(!GET) (bool)value ? motor->enable() : motor->disable();
-       println(motor->enabled);
+      motion(motor, &user_command[0]);
       break;
     case CMD_PWMMOD:
        // PWM modulation change
@@ -384,6 +326,80 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
   }
 }
 
+void Commander::motion(FOCMotor* motor, char* user_cmd, char* separator){
+  char cmd = user_cmd[0];
+  char sub_cmd = user_cmd[1];
+  bool GET  = isSentinel(user_cmd[1]);
+  float value = atof(&user_cmd[(sub_cmd >= 'A'  && sub_cmd <= 'Z') ?  2 :  1]);
+
+  switch(cmd){
+    case CMD_MOTION_TYPE:
+      printVerbose(F("Motion:"));
+      switch(sub_cmd){
+        case SCMD_DOWNSAMPLE:
+            printVerbose(F(" downsample: "));
+            if(!GET) motor->motion_downsample = value;
+            println((int)motor->motion_downsample);
+          break;
+        default:
+          // change control type
+          if(!GET && value >= 0 && (int)value < 5) // if set command
+            motor->controller = (MotionControlType)value;
+          switch(motor->controller){
+            case MotionControlType::torque:
+              println(F("torque"));
+              break;
+            case MotionControlType::velocity:
+              println(F("vel"));
+              break;
+            case MotionControlType::angle:
+              println(F("angle"));
+              break;
+            case MotionControlType::velocity_openloop:
+              println(F("vel open"));
+              break;
+            case MotionControlType::angle_openloop:
+              println(F("angle open"));
+              break;
+          }
+            break;
+        }
+      break;
+    case CMD_TORQUE_TYPE:
+      // change control type
+      printVerbose(F("Torque: "));
+      if(!GET && (int8_t)value >= 0 && (int8_t)value < 3)// if set command
+        motor->torque_controller = (TorqueControlType)value;
+      switch(motor->torque_controller){
+        case TorqueControlType::voltage:
+          println(F("volt"));
+          // change the velocity control limits if necessary
+          if( !_isset(motor->phase_resistance) ) motor->PID_velocity.limit = motor->voltage_limit;
+          break;
+        case TorqueControlType::dc_current:
+          println(F("dc curr"));
+          // change the velocity control limits if necessary
+          motor->PID_velocity.limit = motor->current_limit;
+          break;
+        case TorqueControlType::foc_current:
+          println(F("foc curr"));
+          // change the velocity control limits if necessary
+          motor->PID_velocity.limit = motor->current_limit;
+          break;
+      }
+      break;
+    case CMD_STATUS:
+      // enable/disable
+      printVerbose(F("Status: "));
+      if(!GET) (bool)value ? motor->enable() : motor->disable();
+       println(motor->enabled);
+      break;
+    default:
+      target(motor,  user_cmd, separator);
+      break;
+  }
+}
+
 void Commander::pid(PIDController* pid, char* user_cmd){
   char cmd = user_cmd[0];
   bool GET  = isSentinel(user_cmd[1]);
@@ -445,57 +461,91 @@ void Commander::scalar(float* value,  char* user_cmd){
 }
 
 
-void Commander::target(FOCMotor* motor,  char* user_cmd){
-  bool GET  = isSentinel(user_cmd[0]);
-  if(!GET){
-    float pos, vel, torque;
-    switch(motor->controller){
-      case MotionControlType::torque: // setting torque target
-        torque= atof(strtok (user_cmd," "));
-        motor->target = torque;
-        break;
-      case MotionControlType::velocity: // setting velocity target + torque limit
-        vel= atof(strtok (user_cmd," "));
-        torque= atof(strtok (NULL," "));
-        motor->target = vel;
+void Commander::target(FOCMotor* motor,  char* user_cmd, char* separator){
+  // if no values sent
+  if(isSentinel(user_cmd[0])) return;
+  
+  float pos, vel, torque;
+  char* next_value;
+  switch(motor->controller){
+    case MotionControlType::torque: // setting torque target
+      torque = atof(strtok (user_cmd, separator));
+      motor->target = torque;
+      break;
+    case MotionControlType::velocity: // setting velocity target + torque limit
+      // set the target
+      vel= atof(strtok (user_cmd, separator));
+      motor->target = vel;
+
+      // allow for setting only the target velocity without chaning the torque limit
+      next_value = strtok (NULL, separator);
+      if (next_value){
+        torque = atof(next_value);
         motor->PID_velocity.limit = torque;
         // torque command can be voltage or current
         if(!_isset(motor->phase_resistance) && motor->torque_controller == TorqueControlType::voltage) motor->voltage_limit = torque;
         else  motor->current_limit = torque;
-        break;
-      case MotionControlType::angle: // setting angle target + torque, velocity limit
-        pos= atof(strtok (user_cmd," "));
-        vel= atof(strtok (NULL," "));
-        torque= atof(strtok (NULL," "));
-        motor->target = pos;
+      }
+      break;
+    case MotionControlType::angle: // setting angle target + torque, velocity limit
+      // setting the target position
+      pos= atof(strtok (user_cmd, separator));
+      motor->target = pos;
+
+      // allow for setting only the target position without chaning the velocity/torque limits 
+      next_value = strtok (NULL, separator);
+      if( next_value ){
+        vel = atof(next_value);
         motor->velocity_limit = vel;
         motor->P_angle.limit = vel;
-        motor->PID_velocity.limit = torque;
-        // torque command can be voltage or current
-        if(!_isset(motor->phase_resistance) && motor->torque_controller == TorqueControlType::voltage) motor->voltage_limit = torque;
-        else  motor->current_limit = torque;
-        break;
-      case MotionControlType::velocity_openloop: // setting velocity target + torque limit
-        vel= atof(strtok (user_cmd," "));
-        torque= atof(strtok (NULL," "));
-        motor->target = vel;
+  
+        // allow for setting only the target position and velocity limit without the torque limit 
+        next_value = strtok (NULL, separator);
+        if( next_value ){
+          torque= atof(next_value);
+          motor->PID_velocity.limit = torque;
+          // torque command can be voltage or current
+          if(!_isset(motor->phase_resistance) && motor->torque_controller == TorqueControlType::voltage) motor->voltage_limit = torque;
+          else  motor->current_limit = torque;
+        }
+      }
+      break;
+    case MotionControlType::velocity_openloop: // setting velocity target + torque limit
+      // set the target
+      vel= atof(strtok (user_cmd, separator));
+      motor->target = vel;
+      // allow for setting only the target velocity without chaning the torque limit
+      next_value = strtok (NULL, separator);
+      if (next_value ){
+        torque = atof(next_value);
         // torque command can be voltage or current
         if(!_isset(motor->phase_resistance)) motor->voltage_limit = torque;
         else  motor->current_limit = torque;
-        break;
-      case MotionControlType::angle_openloop: // setting angle target + torque, velocity limit
-        pos= atof(strtok (user_cmd," "));
-        vel= atof(strtok (NULL," "));
-        torque= atof(strtok (NULL," "));
-        motor->target = pos;
+      }
+      break;
+    case MotionControlType::angle_openloop: // setting angle target + torque, velocity limit
+      // set the target
+      pos= atof(strtok (user_cmd, separator));
+      motor->target = pos; 
+      
+      // allow for setting only the target position without chaning the velocity/torque limits 
+      next_value = strtok (NULL, separator);
+      if( next_value ){
+        vel = atof(next_value);
         motor->velocity_limit = vel;
-        // torque command can be voltage or current
-        if(!_isset(motor->phase_resistance)) motor->voltage_limit = torque;
-        else  motor->current_limit = torque;
-        break;
-    }
-  } 
-  //println(*value);
+        // allow for setting only the target velocity without chaning the torque limit
+        next_value = strtok (NULL, separator);
+        if (next_value ){
+          torque = atof(next_value);
+          // torque command can be voltage or current
+          if(!_isset(motor->phase_resistance)) motor->voltage_limit = torque;
+          else  motor->current_limit = torque;
+        }
+      }
+      break;
+  }
+  printVerbose(F("Target: "));
+  println(motor->target);
 }
 
 
diff --git a/src/communication/Commander.h b/src/communication/Commander.h

Original file line number	Diff line number	Diff line change
`@@ -73,6 +73,7 @@ void setup() {`
`73`	`73`	`while(motor_angle <= pp_search_angle){`
`74`	`74`	`motor_angle += 0.01f;`
`75`	`75`	`motor.move(motor_angle);`
	`76`	`+ _delay(1);`
`76`	`77`	`}`
`77`	`78`	`_delay(1000);`
`78`	`79`	`// read the encoder value for 180`