Robot,StepperMotor: per-actuator speed limits

diff --git a/ConfigSamples/Smoothieboard.Kossel/config b/ConfigSamples/Smoothieboard.Kossel/config
index c78184a..1a0bf27 100644 (file)
--- a/ConfigSamples/Smoothieboard.Kossel/config
+++ b/ConfigSamples/Smoothieboard.Kossel/config
@@ -27,24 +27,29 @@ microseconds_per_step_pulse                  1                # Duration of step
 minimum_steps_per_minute                     1200             # Never step slower than this
 base_stepping_frequency                      100000           # Base frequency for stepping, higher gives smoother movement
 
+# Cartesian axis speed limits - prevent extreme forces on carriage
+x_axis_max_speed                             30000            # mm/min
+y_axis_max_speed                             30000            # mm/min
+z_axis_max_speed                             30000            # mm/min
+
 # Stepper module pins ( ports, and pin numbers, appending "!" to the number will invert a pin )
 alpha_step_pin                               2.0              # Pin for alpha stepper step signal
 alpha_dir_pin                                0.5              # Pin for alpha stepper direction
 alpha_en_pin                                 0.4              # Pin for alpha enable pin
 alpha_current                                1.5              # X stepper motor current
-x_axis_max_speed                             30000            # mm/min
+alpha_max_rate                               30000.0          # mm/min
 
 beta_step_pin                                2.1              # Pin for beta stepper step signal
 beta_dir_pin                                 0.11             # Pin for beta stepper direction
 beta_en_pin                                  0.10             # Pin for beta enable
 beta_current                                 1.5              # Y stepper motor current
-y_axis_max_speed                             30000            # mm/min
+beta_max_rate                                30000.0          # mm/min
 
 gamma_step_pin                               2.2              # Pin for gamma stepper step signal
 gamma_dir_pin                                0.20             # Pin for gamma stepper direction
 gamma_en_pin                                 0.19             # Pin for gamma enable
 gamma_current                                1.5              # Z stepper motor current
-z_axis_max_speed                             30000            # mm/min
+gamma_max_rate                               30000.0          # mm/min
 
 # Serial communications configuration ( baud rate default to 9600 if undefined )
 uart0.baud_rate                              115200           # Baud rate for the default serial port

diff --git a/ConfigSamples/Smoothieboard.Rostock/config b/ConfigSamples/Smoothieboard.Rostock/config
index 5c4c947..89db26e 100644 (file)
--- a/ConfigSamples/Smoothieboard.Rostock/config
+++ b/ConfigSamples/Smoothieboard.Rostock/config
@@ -31,24 +31,29 @@ microseconds_per_step_pulse                  1                # Duration of step
 minimum_steps_per_minute                     1200             # Never step slower than this
 base_stepping_frequency                      100000           # Base frequency for stepping, higher gives smoother movement
 
+# Cartesian axis speed limits - prevent extreme forces on carriage
+x_axis_max_speed                             30000            # mm/min
+y_axis_max_speed                             30000            # mm/min
+z_axis_max_speed                             30000            # mm/min
+
 # Stepper module pins ( ports, and pin numbers, appending "!" to the number will invert a pin )
 alpha_step_pin                               2.0              # Pin for alpha stepper step signal
 alpha_dir_pin                                0.5              # Pin for alpha stepper direction
 alpha_en_pin                                 0.4              # Pin for alpha enable pin
 alpha_current                                1.5              # X stepper motor current
-x_axis_max_speed                             30000            # mm/min
+alpha_max_rate                               30000.0          # mm/min
 
 beta_step_pin                                2.1              # Pin for beta stepper step signal
 beta_dir_pin                                 0.11             # Pin for beta stepper direction
 beta_en_pin                                  0.10             # Pin for beta enable
 beta_current                                 1.5              # Y stepper motor current
-y_axis_max_speed                             30000            # mm/min
+beta_max_rate                                30000.0          # mm/min
 
 gamma_step_pin                               2.2              # Pin for gamma stepper step signal
 gamma_dir_pin                                0.20             # Pin for gamma stepper direction
 gamma_en_pin                                 0.19             # Pin for gamma enable
 gamma_current                                1.5              # Z stepper motor current
-z_axis_max_speed                             30000            # mm/min
+gamma_max_rate                               30000.0          # mm/min
 
 # Serial communications configuration ( baud rate default to 9600 if undefined )
 uart0.baud_rate                              115200           # Baud rate for the default serial port

diff --git a/ConfigSamples/Smoothieboard/config b/ConfigSamples/Smoothieboard/config
index 938a3d4..77cafb0 100644 (file)
--- a/ConfigSamples/Smoothieboard/config
+++ b/ConfigSamples/Smoothieboard/config
@@ -22,24 +22,29 @@ microseconds_per_step_pulse                  1                # Duration of step
 minimum_steps_per_minute                     1200             # Never step slower than this
 base_stepping_frequency                      100000           # Base frequency for stepping, higher gives smoother movement
 
+# Cartesian axis speed limits
+x_axis_max_speed                             30000            # mm/min
+y_axis_max_speed                             30000            # mm/min
+z_axis_max_speed                             300              # mm/min
+
 # Stepper module pins ( ports, and pin numbers, appending "!" to the number will invert a pin )
 alpha_step_pin                               2.0              # Pin for alpha stepper step signal
 alpha_dir_pin                                0.5              # Pin for alpha stepper direction
 alpha_en_pin                                 0.4              # Pin for alpha enable pin
 alpha_current                                1.5              # X stepper motor current
-x_axis_max_speed                             30000            # mm/min
+alpha_max_rate                               30000.0          # mm/min
 
 beta_step_pin                                2.1              # Pin for beta stepper step signal
 beta_dir_pin                                 0.11             # Pin for beta stepper direction
 beta_en_pin                                  0.10             # Pin for beta enable
 beta_current                                 1.5              # Y stepper motor current
-y_axis_max_speed                             30000            # mm/min
+beta_max_rate                                30000.0          # mm/min
 
 gamma_step_pin                               2.2              # Pin for gamma stepper step signal
 gamma_dir_pin                                0.20             # Pin for gamma stepper direction
 gamma_en_pin                                 0.19             # Pin for gamma enable
 gamma_current                                1.5              # Z stepper motor current
-z_axis_max_speed                             300              # mm/min
+gamma_max_rate                               300.0            # mm/min
 
 # Serial communications configuration ( baud rate default to 9600 if undefined )
 uart0.baud_rate                              115200           # Baud rate for the default hardware serial port

diff --git a/src/libs/StepperMotor.cpp b/src/libs/StepperMotor.cpp
index 25687cf..f031f04 100644 (file)
--- a/src/libs/StepperMotor.cpp
+++ b/src/libs/StepperMotor.cpp
@@ -24,6 +24,9 @@ StepperMotor::StepperMotor(){
     this->signal_step = false;
     this->step_signal_hook = new Hook();
 
+    steps_per_mm         = 1.0F;
+    max_rate             = 50.0F;
+
     last_milestone_steps = 0;
     last_milestone_mm    = 0.0F;
 }
@@ -43,6 +46,9 @@ StepperMotor::StepperMotor(Pin& step, Pin& dir, Pin& en) : step_pin(step), dir_p
     enable(false);
     set_high_on_debug(en.port_number, en.pin);
 
+    steps_per_mm         = 1.0F;
+    max_rate             = 50.0F;
+
     last_milestone_steps = 0;
     last_milestone_mm    = 0.0F;
 }

diff --git a/src/libs/StepperMotor.h b/src/libs/StepperMotor.h
index fd091c2..3f96d75 100644 (file)
--- a/src/libs/StepperMotor.h
+++ b/src/libs/StepperMotor.h
@@ -66,17 +66,16 @@ class StepperMotor {
         bool paused;
 
         float steps_per_mm;
+        float max_rate;
 
         int32_t last_milestone_steps;
         float   last_milestone_mm;
 
-        //bool direction_bit;
-        //bool step_bit;
-
         uint32_t steps_to_move;
         uint32_t stepped;
         uint32_t fx_counter;
         uint32_t fx_ticks_per_step;
+
         bool     direction;
 
         //bool exit_tick;

diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp
index 2aab755..e5f82ae 100644 (file)
--- a/src/modules/robot/Robot.cpp
+++ b/src/modules/robot/Robot.cpp
@@ -60,6 +60,11 @@ using std::string;
 #define  beta_steps_per_mm_checksum          CHECKSUM("beta_steps_per_mm")
 #define  gamma_steps_per_mm_checksum         CHECKSUM("gamma_steps_per_mm")
 
+#define  alpha_max_rate_checksum             CHECKSUM("alpha_max_rate")
+#define  beta_max_rate_checksum              CHECKSUM("beta_max_rate")
+#define  gamma_max_rate_checksum             CHECKSUM("gamma_max_rate")
+
+
 // new-style actuator stuff
 #define  actuator_checksum                   CHEKCSUM("actuator")
 
@@ -68,6 +73,7 @@ using std::string;
 #define  en_pin_checksum                     CHECKSUM("en_pin")
 
 #define  steps_per_mm_checksum               CHECKSUM("steps_per_mm")
+#define  max_rate_checksum                   CHECKSUM("max_rate")
 
 #define  alpha_checksum                      CHECKSUM("alpha")
 #define  beta_checksum                       CHECKSUM("beta")
@@ -180,6 +186,10 @@ void Robot::on_config_reload(void* argument){
     beta_stepper_motor->change_steps_per_mm(steps_per_mm[1]);
     gamma_stepper_motor->change_steps_per_mm(steps_per_mm[2]);
 
+    alpha_stepper_motor->max_rate = THEKERNEL->config->value(alpha_max_rate_checksum)->by_default(30000.0F)->as_number() / 60.0F;
+    beta_stepper_motor->max_rate  = THEKERNEL->config->value(beta_max_rate_checksum )->by_default(30000.0F)->as_number() / 60.0F;
+    gamma_stepper_motor->max_rate = THEKERNEL->config->value(gamma_max_rate_checksum)->by_default(30000.0F)->as_number() / 60.0F;
+
     actuators.clear();
     actuators.push_back(alpha_stepper_motor);
     actuators.push_back(beta_stepper_motor);
@@ -436,28 +446,48 @@ void Robot::reset_axis_position(float position, int axis) {
 
 
 // Convert target from millimeters to steps, and append this to the planner
-void Robot::append_milestone( float target[], float rate ){
-    float actuator_pos[3]; //Holds the result of the conversion
-
-    // We use an arm solution object so exotic arm solutions can be used and neatly abstracted
-    this->arm_solution->cartesian_to_actuator( target, actuator_pos );
-
+void Robot::append_milestone( float target[], float rate )
+{
     float deltas[3];
-    for(int axis=X_AXIS;axis<=Z_AXIS;axis++){deltas[axis]=target[axis]-this->last_milestone[axis];}
+    float unit_vec[3];
+    float actuator_pos[3];
+    float millimeters_of_travel;
+
+    // find distance moved by each axis
+    for (int axis = X_AXIS; axis <= Z_AXIS; axis++)
+        deltas[axis] = target[axis] - last_milestone[axis];
 
     // Compute how long this move moves, so we can attach it to the block for later use
-    float millimeters_of_travel = sqrtf( pow( deltas[X_AXIS], 2 ) +  pow( deltas[Y_AXIS], 2 ) +  pow( deltas[Z_AXIS], 2 ) );
-
-    // Do not move faster than the configured limits
-    for(int axis=X_AXIS;axis<=Z_AXIS;axis++){
-        if( this->max_speeds[axis] > 0 ){
-            float axis_speed = ( fabs(deltas[axis]) / ( millimeters_of_travel / rate )) * seconds_per_minute;
-            if( axis_speed > this->max_speeds[axis] ){
-                rate = rate * ( this->max_speeds[axis] / axis_speed );
-            }
+    millimeters_of_travel = sqrtf( pow( deltas[X_AXIS], 2 ) +  pow( deltas[Y_AXIS], 2 ) +  pow( deltas[Z_AXIS], 2 ) );
+
+    // find distance unit vector
+    for (int i = 0; i < 3; i++)
+        unit_vec[i] = deltas[i] / millimeters_of_travel;
+
+    // Do not move faster than the configured cartesian limits
+    for (int axis = X_AXIS; axis <= Z_AXIS; axis++)
+    {
+        if ( max_speeds[axis] > 0 )
+        {
+            float axis_speed = fabs(unit_vec[axis] * rate) * seconds_per_minute;
+
+            if (axis_speed > max_speeds[axis])
+                rate = rate * ( max_speeds[axis] / axis_speed );
         }
     }
 
+    // find actuator position given cartesian position
+    arm_solution->cartesian_to_actuator( target, actuator_pos );
+
+    // check per-actuator speed limits
+    for (int actuator = 0; actuator <= 2; actuator++)
+    {
+        float actuator_rate  = fabs(actuator_pos[actuator] - actuators[actuator]->last_milestone_mm) * rate / millimeters_of_travel;
+
+        if (actuator_rate > actuators[actuator]->max_rate)
+            rate *= (actuators[actuator]->max_rate / actuator_rate);
+    }
+
     // Append the block to the planner
     THEKERNEL->planner->append_block( actuator_pos, rate * seconds_per_minute, millimeters_of_travel, unit_vec );