#define GAMMA_STEPPER 2
#define clear_vector(a) memset(a, 0, sizeof(a))
+#define clear_vector_double(a) memset(a, 0.0, sizeof(a))
#define max(a,b) (((a) > (b)) ? (a) : (b))
-#define dd(...) LPC_GPIO2->FIOCLR = 0xffff; LPC_GPIO2->FIOSET = __VA_ARGS__
+#define dd(...) LPC_GPIO2->FIODIR = 0xffff; LPC_GPIO2->FIOCLR = 0xffff; LPC_GPIO2->FIOSET = __VA_ARGS__
#endif
Block::Block(){
clear_vector(this->steps);
- clear_vector(this->speeds);
+ //clear_vector(this->speeds);
}
void Block::debug(Kernel* kernel){
- kernel->serial->printf(" steps:%4d|%4d|%4d(max:%4d) speeds:%8.2f|%8.2f|%8.2f nominal:r%10d/s%6.1f mm:%9.6f rdelta:%8d acc:%5d dec:%5d factor:%6.4f rates:%10d>%10d \r\n", this->steps[0], this->steps[1], this->steps[2], this->steps_event_count, this->speeds[0], this->speeds[1], this->speeds[2], this->nominal_rate, this->nominal_speed, this->millimeters, this->rate_delta, this->accelerate_until, this->decelerate_after, this->entry_factor, this->initial_rate, this->final_rate );
+ kernel->serial->printf(" steps:%4d|%4d|%4d(max:%4d) nominal:r%10d/s%6.1f mm:%9.6f rdelta:%8d acc:%5d dec:%5d rates:%10d>%10d \r\n", this->steps[0], this->steps[1], this->steps[2], this->steps_event_count, this->nominal_rate, this->nominal_speed, this->millimeters, this->rate_delta, this->accelerate_until, this->decelerate_after, this->initial_rate, this->final_rate );
}
// +-------------+
// time -->
void Block::calculate_trapezoid( double entryfactor, double exitfactor ){
- this->initial_rate = ceil(this->nominal_rate * entryfactor);
- this->final_rate = ceil(this->nominal_rate * exitfactor);
+ this->initial_rate = ceil(this->nominal_rate * entryfactor); // (step/min)
+ this->final_rate = ceil(this->nominal_rate * exitfactor); // (step/min)
double acceleration_per_minute = this->rate_delta * this->planner->kernel->stepper->acceleration_ticks_per_second * 60.0;
int accelerate_steps = ceil( this->estimate_acceleration_distance( this->initial_rate, this->nominal_rate, acceleration_per_minute ) );
int decelerate_steps = ceil( this->estimate_acceleration_distance( this->nominal_rate, this->final_rate, -acceleration_per_minute ) );
// in order to reach the final_rate exactly at the end of this block.
if (plateau_steps < 0) {
accelerate_steps = ceil(this->intersection_distance(this->initial_rate, this->final_rate, acceleration_per_minute, this->steps_event_count));
+ accelerate_steps = max( accelerate_steps, 0 ); // Check limits due to numerical round-off
+ accelerate_steps = min( accelerate_steps, int(this->steps_event_count) );
plateau_steps = 0;
}
return((2*acceleration*distance-initialrate*initialrate+finalrate*finalrate)/(4*acceleration));
}
+/*
// "Junction jerk" in this context is the immediate change in speed at the junction of two blocks.
// This method will calculate the junction jerk as the euclidean distance between the nominal
// velocities of the respective blocks.
pow(before->speeds[Z_AXIS]-after->speeds[Z_AXIS], 2))
);
}
-
+*/
// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
// acceleration within the allotted distance.
// Called by Planner::recalculate() when scanning the plan from last to first entry.
void Block::reverse_pass(Block* next, Block* previous){
- double entryfactor = 1.0;
- double exitfactor;
- if (next) { exitfactor = next->entry_factor; } else { exitfactor = this->compute_factor_for_safe_speed(); }
-
- // Calculate the entry_factor for the this block.
- if (previous) {
- // Reduce speed so that junction_jerk is within the maximum allowed
- double jerk = junction_jerk(previous, this);
- if (jerk > this->planner->max_jerk ) { //TODO: Get from config settings.max_jerk
- entryfactor = (this->planner->max_jerk/jerk);
- }
- // If the required deceleration across the block is too rapid, reduce the entry_factor accordingly.
- if (entryfactor > exitfactor) {
- double max_entry_speed = max_allowable_speed(-this->planner->acceleration,this->nominal_speed*exitfactor, this->millimeters);
- double max_entry_factor = max_entry_speed/this->nominal_speed;
- if (max_entry_factor < entryfactor) {
- entryfactor = max_entry_factor;
+ if (next) {
+ // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
+ // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
+ // check for maximum allowable speed reductions to ensure maximum possible planned speed.
+ if (this->entry_speed != this->max_entry_speed) {
+
+ // If nominal length true, max junction speed is guaranteed to be reached. Only compute
+ // for max allowable speed if block is decelerating and nominal length is false.
+ if ((!this->nominal_length_flag) && (this->max_entry_speed > next->entry_speed)) {
+ this->entry_speed = min( this->max_entry_speed, max_allowable_speed(-this->planner->acceleration,next->entry_speed,this->millimeters));
+ } else {
+ this->entry_speed = this->max_entry_speed;
}
- }
- } else {
- entryfactor = this->compute_factor_for_safe_speed();
- }
- this->entry_factor = entryfactor;
-}
+ this->recalculate_flag = true;
+ }
+ } // Skip last block. Already initialized and set for recalculation.
+}
// Called by Planner::recalculate() when scanning the plan from first to last entry.
void Block::forward_pass(Block* previous, Block* next){
- if(previous) {
- // If the previous block is an acceleration block, but it is not long enough to
- // complete the full speed change within the block, we need to adjust out entry
- // speed accordingly. Remember this->entry_factor equals the exit factor of
- // the previous block.
- if(previous->entry_factor < this->entry_factor) {
- double max_entry_speed = max_allowable_speed(-this->planner->acceleration, this->nominal_speed*previous->entry_factor, previous->millimeters);
- double max_entry_factor = max_entry_speed/this->nominal_speed;
- if (max_entry_factor < this->entry_factor) {
- this->entry_factor = max_entry_factor;
+
+ if(!previous) { return; } // Begin planning after buffer_tail
+
+ // If the previous block is an acceleration block, but it is not long enough to complete the
+ // full speed change within the block, we need to adjust the entry speed accordingly. Entry
+ // speeds have already been reset, maximized, and reverse planned by reverse planner.
+ // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
+ if (!previous->nominal_length_flag) {
+ if (previous->entry_speed < this->entry_speed) {
+ double entry_speed = min( this->entry_speed,
+ max_allowable_speed(-this->planner->acceleration,previous->entry_speed,previous->millimeters) );
+
+ // Check for junction speed change
+ if (this->entry_speed != entry_speed) {
+ this->entry_speed = entry_speed;
+ this->recalculate_flag = true;
}
- }
- }
+ }
+ }
+
}
#include "Planner.h"
class Planner;
+
+double max_allowable_speed( double acceleration, double target_velocity, double distance);
+
class Block {
public:
Block();
vector<std::string> commands;
unsigned short steps[3]; // Number of steps for each axis for this block
- float speeds[3]; // Speeds for each axis, used in the planning process
+ //float speeds[3]; // Speeds for each axis, used in the planning process
unsigned short steps_event_count; // Steps for the longest axis
unsigned int nominal_rate; // Nominal rate in steps per minute
float nominal_speed; // Nominal speed in mm per minute
float millimeters; // Distance for this move
- float entry_factor; // Starting speed for the block
+ double entry_speed;
unsigned int rate_delta; // Nomber of steps to add to the speed for each acceleration tick
unsigned int initial_rate; // Initial speed in steps per minute
unsigned int final_rate; // Final speed in steps per minute
unsigned short decelerate_after; // Start decelerating after this number of steps
unsigned int direction_bits; // Direction for each axis in bit form, relative to the direction port's mask
+
+ uint8_t recalculate_flag; // Planner flag to recalculate trapezoids on entry junction
+ uint8_t nominal_length_flag; // Planner flag for nominal speed always reached
+
+ double max_entry_speed;
Planner* planner;
};
#include "Planner.h"
Planner::Planner(){
- clear_vector(this->position);
+ clear_vector(this->position);
+ clear_vector_double(this->previous_unit_vec);
+ this->previous_nominal_speed = 0.0;
this->has_deleted_block = false;
}
// Append a block to the queue, compute it's speed factors
-void Planner::append_block( int target[], double feed_rate, double distance, double speeds[] ){
-
+void Planner::append_block( int target[], double feed_rate, double distance, double deltas[] ){
+
// TODO : Check if this is necessary
- this->computing = true;
// Do not append block with no movement
if( target[ALPHA_STEPPER] == this->position[ALPHA_STEPPER] && target[BETA_STEPPER] == this->position[BETA_STEPPER] && target[GAMMA_STEPPER] == this->position[GAMMA_STEPPER] ){ this->computing = false; return; }
-
- // Stall here if the queue is full
+
+ // Stall here if the queue is ful
while( this->queue.size() >= this->queue.capacity() ){ wait_us(100); }
// Add/get a new block from the queue
this->queue.push_back(Block());
Block* block = this->queue.get_ref( this->queue.size()-1 );
block->planner = this;
+
+ this->computing = true;
+
+ // Direction bits
+ block->direction_bits = 0;
+ char direction_bits[3] = {this->kernel->stepper->alpha_dir_pin, this->kernel->stepper->beta_dir_pin, this->kernel->stepper->gamma_dir_pin};
+ for( int stepper=ALPHA_STEPPER; stepper<=GAMMA_STEPPER; stepper++){
+ if( target[stepper] < position[stepper] ){ block->direction_bits |= (1<<direction_bits[stepper]); }
+ }
// Number of steps for each stepper
for( int stepper=ALPHA_STEPPER; stepper<=GAMMA_STEPPER; stepper++){ block->steps[stepper] = labs(target[stepper] - this->position[stepper]); }
// Max number of steps, for all axes
block->steps_event_count = max( block->steps[ALPHA_STEPPER], max( block->steps[BETA_STEPPER], block->steps[GAMMA_STEPPER] ) );
+ if( block->steps_event_count == 0 ){ this->computing = false; return; }
- // Speeds for each axis, needed for junction_jerk
- for( int stepper=ALPHA_STEPPER; stepper<=GAMMA_STEPPER; stepper++){ block->speeds[stepper] = speeds[stepper]; }
-
- // Set rates and speeds
- double microseconds = ( distance / feed_rate ) * 1000000;
- double multiplier = 60*1000000/microseconds;
- block->nominal_rate = block->steps_event_count*multiplier;
- block->nominal_speed = distance*multiplier;
block->millimeters = distance;
- block->entry_factor = 0;
-
+ double inverse_millimeters = 1.0/distance;
+
+ // Calculate speed in mm/minute for each axis. No divide by zero due to previous checks.
+ // NOTE: Minimum stepper speed is limited by MINIMUM_STEPS_PER_MINUTE in stepper.c
+ double inverse_minute = feed_rate * inverse_millimeters;
+ block->nominal_speed = block->millimeters * inverse_minute; // (mm/min) Always > 0
+ block->nominal_rate = ceil(block->steps_event_count * inverse_minute); // (step/min) Always > 0
+
// Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line
// average travel per step event changes. For a line along one axis the travel per step event
// is equal to the travel/step in the particular axis. For a 45 degree line the steppers of both
// axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2).
// To generate trapezoids with contant acceleration between blocks the rate_delta must be computed
// specifically for each line to compensate for this phenomenon:
- double travel_per_step = (double) ((double) block->millimeters / (double) block->steps_event_count);
- block->rate_delta = ceil(( (this->acceleration*60) / this->kernel->stepper->acceleration_ticks_per_second ) / travel_per_step);
+ // Convert universal acceleration for direction-dependent stepper rate change parameter
+ block->rate_delta = ceil( block->steps_event_count*inverse_millimeters * this->acceleration*60.0 / this->kernel->stepper->acceleration_ticks_per_second ); // (step/min/acceleration_tick)
- // Comupte a preliminary conservative acceleration trapezoid
- double safe_speed_factor = block->compute_factor_for_safe_speed();
- block->calculate_trapezoid( safe_speed_factor, safe_speed_factor );
-
- // Direction bits
- block->direction_bits = 0;
- char direction_bits[3] = {this->kernel->stepper->alpha_dir_pin, this->kernel->stepper->beta_dir_pin, this->kernel->stepper->gamma_dir_pin};
- for( int stepper=ALPHA_STEPPER; stepper<=GAMMA_STEPPER; stepper++){
- if( target[stepper] < position[stepper] ){ block->direction_bits |= (1<<direction_bits[stepper]); }
+
+ // Compute path unit vector
+ double unit_vec[3];
+ unit_vec[X_AXIS] = deltas[X_AXIS]*inverse_millimeters;
+ unit_vec[Y_AXIS] = deltas[Y_AXIS]*inverse_millimeters;
+ unit_vec[Z_AXIS] = deltas[Z_AXIS]*inverse_millimeters;
+
+ // Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
+ // Let a circle be tangent to both previous and current path line segments, where the junction
+ // deviation is defined as the distance from the junction to the closest edge of the circle,
+ // colinear with the circle center. The circular segment joining the two paths represents the
+ // path of centripetal acceleration. Solve for max velocity based on max acceleration about the
+ // radius of the circle, defined indirectly by junction deviation. This may be also viewed as
+ // path width or max_jerk in the previous grbl version. This approach does not actually deviate
+ // from path, but used as a robust way to compute cornering speeds, as it takes into account the
+ // nonlinearities of both the junction angle and junction velocity.
+ double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
+
+ if (this->queue.size() > 1 && (this->previous_nominal_speed > 0.0)) {
+ // 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.
+ double 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.95) {
+ vmax_junction = min(this->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.95) {
+ // Compute maximum junction velocity based on maximum acceleration and junction deviation
+ double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
+ vmax_junction = min(vmax_junction,
+ sqrt(this->acceleration*60*60 * 0.05 * sin_theta_d2/(1.0-sin_theta_d2)) ); // TODO: Get from config
+ }
+ }
}
-
+ block->max_entry_speed = vmax_junction;
+
+ // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
+ double v_allowable = this->max_allowable_speed(-this->acceleration,0.0,block->millimeters); //TODO: Get from config
+ block->entry_speed = min(vmax_junction, v_allowable);
+
+ // Initialize planner efficiency flags
+ // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
+ // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
+ // the current block and next block junction speeds are guaranteed to always be at their maximum
+ // junction speeds in deceleration and acceleration, respectively. This is due to how the current
+ // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
+ // the reverse and forward planners, the corresponding block junction speed will always be at the
+ // the maximum junction speed and may always be ignored for any speed reduction checks.
+ if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; }
+ else { block->nominal_length_flag = false; }
+ block->recalculate_flag = true; // Always calculate trapezoid for new block
+
+
+ // Update previous path unit_vector and nominal speed
+ memcpy(this->previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[]
+ this->previous_nominal_speed = block->nominal_speed;
+
memcpy(this->position, target, sizeof(int)*3);
this->recalculate();
this->computing = false;
+
this->kernel->call_event(ON_STEPPER_WAKE_UP, this);
}
// implements the reverse pass.
void Planner::reverse_pass(){
// For each block
- for( int index = this->queue.size()-1; index >= 0; index-- ){
- //this->queue.get_ref(index)->reverse_pass((index==this->queue.size()-1?NULL:this->queue.get_ref(index+1)), (index==0?NULL:this->queue.get_ref(index-1)));
+ for( int index = this->queue.size()-1; index > 0; index-- ){ // Skip buffer tail/first block to prevent over-writing the initial entry speed.
this->queue.get_ref(index)->reverse_pass((index==this->queue.size()-1?NULL:this->queue.get_ref(index+1)), (index==0? (this->has_deleted_block?&(this->last_deleted_block):NULL) :this->queue.get_ref(index-1)));
}
}
// Planner::recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
// implements the forward pass.
void Planner::forward_pass() {
- // For each block
+ // For each block
for( int index = 0; index <= this->queue.size()-1; index++ ){
this->queue.get_ref(index)->forward_pass((index==0?NULL:this->queue.get_ref(index-1)),(index==this->queue.size()-1?NULL:this->queue.get_ref(index+1)));
}
}
-// Recalculates the trapezoid speed profiles for all blocks in the plan according to the
-// entry_factor for each junction. Must be called by Planner::recalculate() after
-// updating the blocks.
+// Recalculates the trapezoid speed profiles for flagged blocks in the plan according to the
+// entry_speed for each junction and the entry_speed of the next junction. Must be called by
+// planner_recalculate() after updating the blocks. Any recalulate flagged junction will
+// compute the two adjacent trapezoids to the junction, since the junction speed corresponds
+// to exit speed and entry speed of one another.
void Planner::recalculate_trapezoids() {
// For each block
- for( int index = 0; index <= this->queue.size()-1; index++ ){
+ for( int index = 0; index <= this->queue.size()-1; index++ ){ // We skip the first one because we need a previous
if( this->queue.size()-1 == index ){ //last block
- this->queue.get_ref(index)->calculate_trapezoid( this->queue.get_ref(index)->entry_factor, this->queue.get_ref(index)->compute_factor_for_safe_speed() );
+ Block* last = this->queue.get_ref(index);
+ last->calculate_trapezoid( last->entry_speed / last->nominal_speed, MINIMUM_PLANNER_SPEED / last->nominal_speed );
}else{
- this->queue.get_ref(index)->calculate_trapezoid( this->queue.get_ref(index)->entry_factor, this->queue.get_ref(index+1)->entry_factor );
- }
+ Block* current = this->queue.get_ref(index);
+ Block* next = this->queue.get_ref(index+1);
+ if( current->recalculate_flag || next->recalculate_flag ){
+ current->calculate_trapezoid( current->entry_speed/current->nominal_speed, next->entry_speed/current->nominal_speed );
+ current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
+ }
+ }
}
}
this->queue.get_ref(index)->debug(this->kernel);
}
}
+
+
+
+// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
+// acceleration within the allotted distance.
+double Planner::max_allowable_speed(double acceleration, double target_velocity, double distance) {
+ return(
+ sqrt(target_velocity*target_velocity-2L*acceleration*60*60*distance) //Was acceleration*60*60*distance, in case this breaks, but here we prefer to use seconds instead of minutes
+ );
+}
+
+
#define acceleration_checksum 25326
#define max_jerk_checksum 61012
+// TODO: Get from config
+#define MINIMUM_PLANNER_SPEED 0.0
using namespace std;
+
+
+
+
class Planner : public Module {
public:
Planner();
- void append_block( int target[], double feed_rate, double distance, double speeds[] );
+ void append_block( int target[], double feed_rate, double distance, double deltas[] );
+ double max_allowable_speed( double acceleration, double target_velocity, double distance);
void attach_gcode_to_queue(Gcode* gcode);
void recalculate();
void reverse_pass();
void on_config_reload(void* argument);
int position[3]; // Current position, in steps
- RingBuffer<Block,128> queue; // Queue of Blocks
+ double previous_unit_vec[3];
+ RingBuffer<Block,64> queue; // Queue of Blocks
bool computing; // Whether or not we are currently computing the queue, TODO: Checks if this is necessary
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
+ float previous_nominal_speed;
double acceleration; // Setting
double max_jerk; // Setting
/*
- This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl).
+ This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl) with additions from Sungeun K. Jeon (https://github.com/chamnit/grbl)
Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
this->arm_solution->millimeters_to_steps( target, steps );
- double millimeters_of_travel = sqrt( pow( target[X_AXIS]-this->last_milestone[X_AXIS], 2 ) + pow( target[Y_AXIS]-this->last_milestone[Y_AXIS], 2 ) + pow( target[Z_AXIS]-this->last_milestone[Z_AXIS], 2 ) );
+ double deltas[3];
+ for(int axis=X_AXIS;axis<=Z_AXIS;axis++){deltas[axis]=target[axis]-this->last_milestone[axis];}
+
+ double millimeters_of_travel = sqrt( pow( deltas[X_AXIS], 2 ) + pow( deltas[Y_AXIS], 2 ) + pow( deltas[Z_AXIS], 2 ) );
if( millimeters_of_travel < 0.001 ){ return; }
double duration = millimeters_of_travel / rate;
- double speeds[3];
- for(int axis=X_AXIS;axis<=Z_AXIS;axis++){speeds[axis]=(target[axis]-this->last_milestone[axis])/duration;}
-
- this->kernel->planner->append_block( steps, rate, millimeters_of_travel, speeds );
+ this->kernel->planner->append_block( steps, rate*60, millimeters_of_travel, deltas );
memcpy(this->last_milestone, target, sizeof(double)*3); // this->position[] = target[];
this->append_milestone(target, rate);
}
-void Robot::append_arc(double theta_start, double angular_travel, double radius, double depth, double rate){
- // The arc is approximated by generating a huge number of tiny, linear segments. The length of each
+void Robot::append_arc( double target[], double offset[], double radius, bool is_clockwise ){
+
+ double center_axis0 = this->current_position[this->plane_axis_0] + offset[this->plane_axis_0];
+ double center_axis1 = this->current_position[this->plane_axis_1] + offset[this->plane_axis_1];
+ double linear_travel = target[this->plane_axis_2] - this->current_position[this->plane_axis_2];
+ double r_axis0 = -offset[this->plane_axis_0]; // Radius vector from center to current location
+ double r_axis1 = -offset[this->plane_axis_1];
+ double rt_axis0 = target[this->plane_axis_0] - center_axis0;
+ double rt_axis1 = target[this->plane_axis_1] - center_axis1;
+
+ // CCW angle between position and target from circle center. Only one atan2() trig computation required.
+ double angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
+ if (angular_travel < 0) { angular_travel += 2*M_PI; }
+ if (is_clockwise) { angular_travel -= 2*M_PI; }
+
+ double millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));
+ if (millimeters_of_travel == 0.0) { return; }
+ uint16_t segments = floor(millimeters_of_travel/this->mm_per_arc_segment);
+
+ double theta_per_segment = angular_travel/segments;
+ double linear_per_segment = linear_travel/segments;
+
+ /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
+ and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
+ r_T = [cos(phi) -sin(phi);
+ sin(phi) cos(phi] * r ;
+ For arc generation, the center of the circle is the axis of rotation and the radius vector is
+ defined from the circle center to the initial position. Each line segment is formed by successive
+ vector rotations. This requires only two cos() and sin() computations to form the rotation
+ matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
+ all double numbers are single precision on the Arduino. (True double precision will not have
+ round off issues for CNC applications.) Single precision error can accumulate to be greater than
+ tool precision in some cases. Therefore, arc path correction is implemented.
+
+ Small angle approximation may be used to reduce computation overhead further. This approximation
+ holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
+ theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
+ to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
+ numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
+ issue for CNC machines with the single precision Arduino calculations.
+ This approximation also allows mc_arc to immediately insert a line segment into the planner
+ without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
+ a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
+ This is important when there are successive arc motions.
+ */
+ // Vector rotation matrix values
+ double cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation
+ double sin_T = theta_per_segment;
+
+ double arc_target[3];
+ double sin_Ti;
+ double cos_Ti;
+ double r_axisi;
+ uint16_t i;
+ int8_t count = 0;
+
+ // Initialize the linear axis
+ arc_target[this->plane_axis_2] = this->current_position[this->plane_axis_2];
+
+ for (i = 1; i<segments; i++) { // Increment (segments-1)
+
+ if (count < 25 ) { // TODO: Get from config
+ // Apply vector rotation matrix
+ r_axisi = r_axis0*sin_T + r_axis1*cos_T;
+ r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
+ r_axis1 = r_axisi;
+ count++;
+ } else {
+ // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
+ // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
+ cos_Ti = cos(i*theta_per_segment);
+ sin_Ti = sin(i*theta_per_segment);
+ r_axis0 = -offset[this->plane_axis_0]*cos_Ti + offset[this->plane_axis_1]*sin_Ti;
+ r_axis1 = -offset[this->plane_axis_0]*sin_Ti - offset[this->plane_axis_1]*cos_Ti;
+ count = 0;
+ }
+
+ // Update arc_target location
+ arc_target[this->plane_axis_0] = center_axis0 + r_axis0;
+ arc_target[this->plane_axis_1] = center_axis1 + r_axis1;
+ arc_target[this->plane_axis_2] += linear_per_segment;
+ this->append_milestone(arc_target, this->feed_rate);
+
+ }
+ // Ensure last segment arrives at target location.
+ this->append_milestone(target, this->feed_rate);
+}
+
+/*
+void robot::append_arc_old(double theta_start, double angular_travel, double radius, double depth, double rate){
+
+ // the arc is approximated by generating a huge number of tiny, linear segments. the length of each
// segment is configured in settings.mm_per_arc_segment.
double millimeters_of_travel = hypot(angular_travel*radius, labs(depth));
if (millimeters_of_travel == 0.0) { return; }
}
}
-
+*/
void Robot::compute_arc(double offset[], double target[]){
- /*
+
+ // Find the radius
+ double radius = hypot(offset[this->plane_axis_0], offset[this->plane_axis_1]);
+
+ // Set clockwise/counter-clockwise sign for mc_arc computations
+ bool is_clockwise = false;
+ if( this->motion_mode == MOTION_MODE_CW_ARC ){ is_clockwise = true; }
+
+ // Append arc
+ this->append_arc( target, offset, radius, is_clockwise );
+
+}
+
+
+/*
+void Robot::compute_arc_old(double offset[], double target[]){
This segment sets up an clockwise or counterclockwise arc from the current position to the target position around
the center designated by the offset vector. All theta-values measured in radians of deviance from the positive
y-axis.
* /
C <- theta_start (e.g. -145 degrees: theta_start == -PI*(3/4))
- */
// calculate the theta (angle) of the current point
double theta_start = theta(-offset[this->plane_axis_0], -offset[this->plane_axis_1]);
// Finish off with a line to make sure we arrive exactly where we think we are
this->append_milestone( target, this->feed_rate );
}
-
+*/
// Convert from inches to millimeters ( our internal storage unit ) if needed
inline double Robot::to_millimeters( double value ){
void execute_gcode(Gcode* gcode);
void append_milestone( double target[], double feed_rate);
void append_line( double target[], double feed_rate);
- void append_arc(double theta_start, double angular_travel, double radius, double depth, double rate);
+ //void append_arc(double theta_start, double angular_travel, double radius, double depth, double rate);
+ void append_arc( double target[], double offset[], double radius, bool is_clockwise );
+
+
void compute_arc(double offset[], double target[]);
double to_millimeters(double value);
double theta(double x, double y);
Stepper::Stepper(){
this->current_block = NULL;
+ this->step_events_completed = 0;
this->divider = 0;
}
extern "C" void TIMER0_IRQHandler (void){
if((LPC_TIM0->IR >> 1) & 1){
LPC_TIM0->IR |= 1 << 1;
+ dd(1);
stepper->step_gpio_port->FIOCLR = stepper->step_mask;
}
if((LPC_TIM0->IR >> 0) & 1){
LPC_TIM0->IR |= 1 << 0;
+ dd(2);
stepper->main_interrupt();
}
}
for( int stpr=ALPHA_STEPPER; stpr<=GAMMA_STEPPER; stpr++){ this->counters[stpr] = 0; this->stepped[stpr] = 0; }
this->step_events_completed = 0;
this->kernel->call_event(ON_BLOCK_BEGIN, this->current_block);
+ //this->kernel->planner->dump_queue();
}else{
// Go to sleep
LPC_TIM0->MR0 = 10000;
LPC_TIM0->TCR = 0;
}
}
-
-
+
if( this->current_block != NULL ){
// Set bits for direction and steps
this->out_bits = this->current_block->direction_bits;
}else{
this->out_bits = 0;
}
+
}
void Stepper::update_offsets(){
// interrupt. It can be assumed that the trapezoid-generator-parameters and the
// current_block stays untouched by outside handlers for the duration of this function call.
void Stepper::trapezoid_generator_tick() {
+ //if( this->trapezoid_generator_busy || this->kernel->planner->computing ){ return; }
+ //this->trapezoid_generator_busy = true;
- if( this->trapezoid_generator_busy || this->kernel->planner->computing ){ return; }
- this->trapezoid_generator_busy = true;
if(this->current_block) {
- if (this->step_events_completed < this->current_block->accelerate_until) {
+ //this->kernel->serial->printf("#%d %u %u\r\n", this->step_events_completed, this->current_block->accelerate_until, this->current_block->accelerate_until<<16);
+ if(this->step_events_completed < this->current_block->accelerate_until<<16) {
this->trapezoid_adjusted_rate += this->current_block->rate_delta;
if (this->trapezoid_adjusted_rate > this->current_block->nominal_rate ) {
this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
}
this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
- } else if (this->step_events_completed > this->current_block->decelerate_after) {
+ } else if (this->step_events_completed > this->current_block->decelerate_after<<16) {
// NOTE: We will only reduce speed if the result will be > 0. This catches small
// rounding errors that might leave steps hanging after the last trapezoid tick.
if (this->trapezoid_adjusted_rate > this->current_block->rate_delta) {
}
}
}
- this->trapezoid_generator_busy = false;
-
+ //this->trapezoid_generator_busy = false;
+
}
}
void Stepper::set_step_events_per_minute( double steps_per_minute ){
- if( ! this->current_block || this->kernel->planner->computing ){ return; }
-
+ //if( ! this->current_block || this->kernel->planner->computing ){ return; }
+
// We do not step slower than this
if( steps_per_minute < this->minimum_steps_per_minute ){ steps_per_minute = this->minimum_steps_per_minute; } //TODO: Get from config
while(1<<i+1 < speed_factor){ i++; } // Probably not optimal ...
this->divider = i;
+ //this->kernel->serial->printf(">> %d %f %f\r\n", this->divider, speed_factor, steps_per_minute);
+
// Set the Timer interval
LPC_TIM0->MR0 = floor( ( SystemCoreClock/4 ) / ( (steps_per_minute/60L) * (1<<this->divider) ) );
HCoop / clinton / Smoothieware.git / commitdiff