Merge pull request #384 from runger1101001/nocascade

Add a MotionControlMode::angle_nocascade which uses a single PID for angle control

Author: askuric

src/BLDCMotor.cpp

@@ -442,6 +442,15 @@ void BLDCMotor::move(float new_target) {
     case MotionControlType::torque:
         current_sp =  target;
         break;
+     case MotionControlType::angle_nocascade:
+      // 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 the torque command - no velocity loop
+      current_sp = P_angle( shaft_angle_sp - shaft_angle );
+      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.

src/StepperMotor.cpp

@@ -46,7 +46,6 @@ int StepperMotor::init() {
   if(voltage_limit > driver->voltage_limit) voltage_limit =  driver->voltage_limit;
   // constrain voltage for sensor alignment
   if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit;
-
 
   // update limits in the motor controllers
   updateCurrentLimit(current_limit);
@@ -414,6 +413,15 @@ void StepperMotor::move(float new_target) {
     case MotionControlType::torque:
         current_sp =  target;
         break;
+    case MotionControlType::angle_nocascade:
+      // 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 the torque command - no velocity loop
+      current_sp = P_angle( shaft_angle_sp - shaft_angle );
+      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.

src/common/base_classes/FOCMotor.h

@@ -28,8 +28,9 @@ enum MotionControlType : uint8_t {
   torque            = 0x00,     //!< Torque control
   velocity          = 0x01,     //!< Velocity motion control
   angle             = 0x02,     //!< Position/angle motion control
-  velocity_openloop = 0x03,     //!< Open loop velocity control
-  angle_openloop    = 0x04      //!< Open loop position/angle control
+  velocity_openloop = 0x03,
+  angle_openloop    = 0x04,
+  angle_nocascade   = 0x05      //!< Position/angle motion control without velocity cascade
 };
 
 /**

src/communication/Commander.cpp

@@ -404,6 +404,9 @@ void Commander::motion(FOCMotor* motor, char* user_cmd, char* separator){
             case MotionControlType::angle_openloop:
               println(F("angle open"));
               break;
+            case MotionControlType::angle_nocascade:
+              println(F("angle nocascade"));
+              break;
           }
             break;
         }
@@ -513,6 +516,7 @@ void Commander::target(FOCMotor* motor,  char* user_cmd, char* separator){
   float pos, vel, torque;
   char* next_value;
   switch(motor->controller){
+    case MotionControlType::angle_nocascade:
     case MotionControlType::torque: // setting torque target
       torque = atof(strtok (user_cmd, separator));
       motor->target = torque;