From df6a30f2fc796d5606c95fb9a3e5b40da05337ba Mon Sep 17 00:00:00 2001 From: Michael Moon Date: Fri, 24 Jan 2014 13:42:05 +1100 Subject: [PATCH] Robot,StepperMotor: per-actuator speed limits --- ConfigSamples/Smoothieboard.Kossel/config | 11 ++-- ConfigSamples/Smoothieboard.Rostock/config | 11 ++-- ConfigSamples/Smoothieboard/config | 11 ++-- src/libs/StepperMotor.cpp | 6 +++ src/libs/StepperMotor.h | 5 +- src/modules/robot/Robot.cpp | 62 ++++++++++++++++------ 6 files changed, 78 insertions(+), 28 deletions(-) diff --git a/ConfigSamples/Smoothieboard.Kossel/config b/ConfigSamples/Smoothieboard.Kossel/config index c78184ae..1a0bf27e 100644 --- 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 5c4c9477..89db26e0 100644 --- 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 938a3d4e..77cafb03 100644 --- 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 25687cf1..f031f04f 100644 --- 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 fd091c20..3f96d759 100644 --- 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 2aab7553..e5f82aea 100644 --- 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 ); -- 2.20.1