initial support for common position+velocity+torque setting with commander #81

Author: askuric

src/communication/Commander.cpp

@@ -1,5 +1,5 @@
 #include "Commander.h"
-
+#include <String.h>
 
 Commander::Commander(Stream& serial, char eol, bool echo){
   com_port = &serial;
@@ -217,12 +217,18 @@ void Commander::motor(FOCMotor* motor, char* user_command) {
       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;
@@ -439,6 +445,61 @@ void Commander::scalar(float* value,  char* user_cmd){
   println(*value);
 }
 
+
+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;
+        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;
+        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;
+        // 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;
+        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);
+}
+
+
 bool Commander::isSentinel(char ch)
 {
   if(ch == eol)

src/communication/Commander.h

@@ -173,7 +173,17 @@ class Commander
      *  - It can be set by sending 'value' - (ex. 0.01f for settin *value=0.01)
      */
     void scalar(float* value, char* user_cmd);
-
+    /**
+     *  Target setting interface, enables setting the target and limiting variables at once. 
+     *  The valeus are sent separated by a space. ex. P2.34 70 2
+     *  `P` is the user defined command, `2.34` is the target angle `70` is the target 
+     *  velocity and `2` is the desired max current.
+     *  It depends of the motion control mode:
+     *  - torque   : torque (ex. P2.5) 
+     *  - velocity : velocity torque (ex.P10 2.5) 
+     *  - angle    : angle velocity torque (ex.P3.5 10 2.5)
+     */
+    void target(FOCMotor* motor, char* user_cmd);
   private:
     // Subscribed command callback variables
     CommandCallback call_list[20];//!< array of command callback pointers - 20 is an arbitrary number