// It goes over the list in both direction, every time a block is added, re-doing the math to make sure everything is optimal
Planner::Planner(){
- clear_vector_float(this->previous_unit_vec);
this->has_deleted_block = false;
}
// Append a block to the queue, compute it's speed factors
-void Planner::append_block( float actuator_pos[], float rate_mm_s, float distance, float unit_vec[] )
+void Planner::append_block( float actuator_pos[], float rate_mm_s, float distance, float cos_theta )
{
// Create ( recycle ) a new block
Block* block = THEKERNEL->conveyor->queue.head_ref();
if (!THEKERNEL->conveyor->queue.is_empty())
{
float previous_nominal_speed = THEKERNEL->conveyor->queue.item_ref(THEKERNEL->conveyor->queue.prev(THEKERNEL->conveyor->queue.head_i))->nominal_speed;
-
- if (previous_nominal_speed > 0.0F) {
- // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
- // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
- float cos_theta = - this->previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- - this->previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- - this->previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
-
- // Skip and use default max junction speed for 0 degree acute junction.
- if (cos_theta < 0.95F) {
- vmax_junction = min(previous_nominal_speed, block->nominal_speed);
- // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
- if (cos_theta > -0.95F) {
- // Compute maximum junction velocity based on maximum acceleration and junction deviation
- float sin_theta_d2 = sqrtf(0.5F * (1.0F - cos_theta)); // Trig half angle identity. Always positive.
- vmax_junction = min(vmax_junction, sqrtf(this->acceleration * this->junction_deviation * sin_theta_d2 / (1.0F - sin_theta_d2)));
- }
+ // Skip and use default max junction speed for 0 degree acute junction.
+ if (cos_theta < 0.95F) {
+ vmax_junction = min(previous_nominal_speed, block->nominal_speed);
+ // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
+ if (cos_theta > -0.95F) {
+ // Compute maximum junction velocity based on maximum acceleration and junction deviation
+ float sin_theta_d2 = sqrtf(0.5F * (1.0F - cos_theta)); // Trig half angle identity. Always positive.
+ vmax_junction = min(vmax_junction, sqrtf(this->acceleration * this->junction_deviation * sin_theta_d2 / (1.0F - sin_theta_d2)));
}
}
}
// Always calculate trapezoid for new block
block->recalculate_flag = true;
- // Update previous path unit_vector and nominal speed
- memcpy(this->previous_unit_vec, unit_vec, sizeof(previous_unit_vec)); // previous_unit_vec[] = unit_vec[]
-
// Math-heavy re-computing of the whole queue to take the new
this->recalculate();
class Planner : public Module {
public:
Planner();
- void append_block( float target[], float rate_mm_s, float distance, float unit_vec[] );
+ void append_block( float target[], float rate_mm_s, float distance, float cos_theta );
float max_allowable_speed( float acceleration, float target_velocity, float distance);
void recalculate();
Block* get_current_block();
void on_module_loaded();
void on_config_reload(void* argument);
- float previous_unit_vec[3];
Block last_deleted_block; // Item -1 in the queue, TODO: Grbl does not need this, but Smoothie won't work without it, we are probably doing something wrong
bool has_deleted_block; // Flag for above value
this->select_plane(X_AXIS, Y_AXIS, Z_AXIS);
this->arm_solution = NULL;
seconds_per_minute = 60.0F;
+ clear_vector_float(prev_unit_vec);
}
//Called when the module has just been loaded
for (int i = 0; i < 3; i++)
unit_vec[i] = deltas[i] / millimeters_of_travel;
+ float cos_theta = 1.0F;
+
+ if (THEKERNEL->conveyor->queue.is_empty())
+ {
+ clear_vector_float(prev_unit_vec);
+ }
+ else
+ {
+ cos_theta = - prev_unit_vec[X_AXIS] * unit_vec[X_AXIS]
+ - prev_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
+ - prev_unit_vec[Z_AXIS] * unit_vec[Z_AXIS];
+
+ memcpy(prev_unit_vec, unit_vec, sizeof(prev_unit_vec));
+ }
+
// Do not move faster than the configured cartesian limits
for (int axis = X_AXIS; axis <= Z_AXIS; axis++)
{
// find actuator position given cartesian position
arm_solution->cartesian_to_actuator( target, actuator_pos );
- append_actuator_milestone(actuator_pos, rate_mm_s, millimeters_of_travel, unit_vec);
+ append_actuator_milestone(actuator_pos, rate_mm_s, millimeters_of_travel, cos_theta);
// Update the last_milestone to the current target for the next time we use last_milestone
for (int i = 0; i < 3; i++)
axes[i].change_last_milestone(target[i]);
}
-void Robot::append_actuator_milestone( float* actuator_pos, float rate_mm_s, float cartesian_distance, float* unit_vec)
+void Robot::append_actuator_milestone( float* actuator_pos, float rate_mm_s, float cartesian_distance, float cos_theta)
{
// check per-actuator speed limits
for (int actuator = 0; actuator <= 2; actuator++)
}
// Append the block to the planner
- THEKERNEL->planner->append_block( actuator_pos, rate_mm_s, cartesian_distance, unit_vec );
+ THEKERNEL->planner->append_block( actuator_pos, rate_mm_s, cartesian_distance, cos_theta );
// actuator milestones are updated inside Planner::append_block in a way that does not experience drift due to use of floats
}
private:
void distance_in_gcode_is_known(Gcode* gcode);
void append_milestone( float target[], float rate_mm_s);
- void append_actuator_milestone( float target[], float rate_mm_s, float cartesian_distance, float* unit_vec);
+ void append_actuator_milestone( float target[], float rate_mm_s, float cartesian_distance, float cos_theta);
void append_line( Gcode* gcode, float target[], float rate_mm_s);
//void append_arc(float theta_start, float angular_travel, float radius, float depth, float rate);
void append_arc( Gcode* gcode, float target[], float offset[], float radius, bool is_clockwise );
float mm_per_line_segment; // Setting : Used to split lines into segments
float mm_per_arc_segment; // Setting : Used to split arcs into segmentrs
float delta_segments_per_second; // Setting : Used to split lines into segments for delta based on speed
+ float prev_unit_vec[3];
// Number of arc generation iterations by small angle approximation before exact arc trajectory
// correction. This parameter maybe decreased if there are issues with the accuracy of the arc
HCoop / clinton / Smoothieware.git / commitdiff