fist implementation of the custom motion control type

Author: askuric

src/common/base_classes/FOCMotor.cpp

@@ -584,7 +584,8 @@ void FOCMotor::loopFOC() {
   if (sensor) sensor->update();
 
   // if disabled do nothing
-  if(!enabled) return;
+  // or if the motor is not ready (e.g. failed to calibrate or not calibrated yet) do nothing
+  if(!enabled || motor_status != FOCMotorStatus::motor_ready) return;
 
   // if open-loop do nothing
   if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) 
@@ -691,7 +692,8 @@ void FOCMotor::move(float new_target) {
   }
 
   // if disabled do nothing
-  if(!enabled) return;
+  // and if 
+  if(!enabled || motor_status != FOCMotorStatus::motor_ready) return;
 
 
   // upgrade the current based voltage limit
@@ -745,6 +747,12 @@ void FOCMotor::move(float new_target) {
       // returned values correspond to the voltage_limit and current_limit
       current_sp = angleOpenloop(shaft_angle_sp); 
       break;
+    case MotionControlType::custom:
+      // custom control - user provides the function that calculates the current_sp 
+      // based on the target value and the motor state
+      // user makes sure to use it with appropriate torque control mode 
+      if(customMotionControlCallback) 
+        current_sp = customMotionControlCallback(this, target);
   }
 }
 
@@ -768,8 +776,8 @@ int  FOCMotor::initFOC() {
     SIMPLEFOC_MOTOR_DEBUG("No sensor.");
     if ((controller == MotionControlType::angle_openloop || controller == MotionControlType::velocity_openloop)){
       exit_flag = 1;    
-      SIMPLEFOC_MOTOR_ERROR("Openloop only!");
     }else{
+      SIMPLEFOC_MOTOR_ERROR("Only openloop allowed!");
       exit_flag = 0; // no FOC without sensor
     }
   }
@@ -781,13 +789,13 @@ int  FOCMotor::initFOC() {
     if(current_sense){ 
       if (!current_sense->initialized) {
         motor_status = FOCMotorStatus::motor_calib_failed;
-        SIMPLEFOC_MOTOR_ERROR("Init FOC error, current sense not init");
+        SIMPLEFOC_MOTOR_ERROR("Current sense not init!");
         exit_flag = 0;
       }else{
         exit_flag *= alignCurrentSense();
       }
     }
-    else { SIMPLEFOC_MOTOR_ERROR("No current sense"); }
+    else { SIMPLEFOC_MOTOR_ERROR("No current sense."); }
   }
 
   if(exit_flag){

src/common/base_classes/FOCMotor.h

@@ -48,7 +48,8 @@ enum MotionControlType : uint8_t {
   angle             = 0x02,     //!< Position/angle motion control
   velocity_openloop = 0x03,
   angle_openloop    = 0x04,
-  angle_nocascade   = 0x05      //!< Position/angle motion control without velocity cascade
+  angle_nocascade   = 0x05,     //!< Position/angle motion control without velocity cascade
+  custom            = 0x06      //!< Custom control method - control method added by user
 };
 
 /**
@@ -367,6 +368,16 @@ class FOCMotor
      */
     float angleOpenloop(float target_angle);
 
+
+    /**
+     * Function setting a custom motion control method defined by the user
+     * @note the custom control method has to be defined by the user and should follow the signature: float controlMethod(FOCMotor* motor, float target)
+     * @param controlMethod - pointer to the custom control method function defined by the user
+     */
+    void setCustomMotionControlMethod(float (*controlMethod)(FOCMotor* motor, float target)){
+      customMotionControlCallback = controlMethod;
+    }
+
   protected:
 
     /**
@@ -410,6 +421,8 @@ class FOCMotor
     // open loop variables
     uint32_t open_loop_timestamp;
 
+    // function pointer for custom control method
+    float (*customMotionControlCallback)(FOCMotor* motor, float target) = nullptr;
 
 };
 

src/communication/Commander.cpp

@@ -439,7 +439,7 @@ void Commander::motion(FOCMotor* motor, char* user_cmd, char* separator){
           break;
         default:
           // change control type
-          if(!GET && value >= 0 && (int)value < 6) // if set command
+          if(!GET && value >= 0 && (int)value < 7) // if set command
             motor->updateMotionControlType((MotionControlType)value); // update motion control type
           switch(motor->controller){
             case MotionControlType::torque:
@@ -460,14 +460,17 @@ void Commander::motion(FOCMotor* motor, char* user_cmd, char* separator){
             case MotionControlType::angle_nocascade:
               println(F("angle nocascade"));
               break;
+            case MotionControlType::custom:
+              println(F("custom"));
+              break;
           }
             break;
         }
       break;
     case CMD_TORQUE_TYPE:
       // change control type
       printVerbose(F("Torque: "));
-      if(!GET && (int8_t)value >= 0 && (int8_t)value < 4)// if set command
+      if(!GET && (int8_t)value >= 0 && (int8_t)value < 4) // if set command
         motor->updateTorqueControlType((TorqueControlType)value); // update torque control type
       switch(motor->torque_controller){
         case TorqueControlType::voltage: