Block::Block(){
clear_vector(this->steps);
- //clear_vector(this->speeds);
}
void Block::debug(Kernel* kernel){
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.
-inline double junction_jerk(Block* before, Block* after) {
- //printf(" compute jerk: before_speed_x: %f, after_speed_x: %f, difference: %f, pow: %f \r\n", before->speeds[X_AXIS], after->speeds[X_AXIS], before->speeds[X_AXIS]-after->speeds[X_AXIS], pow(before->speeds[X_AXIS]-after->speeds[X_AXIS], 2) );
- return(sqrt(
- pow(before->speeds[X_AXIS]-after->speeds[X_AXIS], 2)+
- pow(before->speeds[Y_AXIS]-after->speeds[Y_AXIS], 2)+
- 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.
inline double max_allowable_speed(double acceleration, double target_velocity, double distance) {
// Append a block to the queue, compute it's speed factors
void Planner::append_block( int target[], double feed_rate, double distance, double deltas[] ){
- // TODO : Check if this is necessary
-
// 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; }
Block* block = this->queue.get_ref( this->queue.size()-1 );
block->planner = this;
- this->computing = true;
+ this->computing = true; //TODO: Check if this is necessary
// Direction bits
block->direction_bits = 0;
int position[3]; // Current position, in steps
double previous_unit_vec[3];
- RingBuffer<Block,64> queue; // Queue of Blocks
+ RingBuffer<Block,128> 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
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; }
- uint16_t segments = ceil(millimeters_of_travel/ this->mm_per_arc_segment );
- // The angular motion for each segment
- double theta_per_segment = angular_travel/segments;
- // The linear motion for each segment
- double linear_per_segment = depth/segments;
- // Compute the center of this circle
- double center_x = this->current_position[this->plane_axis_0]-sin(theta_start)*radius;
- double center_y = this->current_position[this->plane_axis_1]-cos(theta_start)*radius;
- // a vector to track the end point of each segment
- double target[3];
-
- // Initialize the linear axis
- target[this->plane_axis_2] = this->current_position[this->plane_axis_2];
- for (int i=0; i<segments; i++) { //NOTE: Major grbl diff
- target[this->plane_axis_2] += linear_per_segment;
- theta_start += theta_per_segment;
- target[this->plane_axis_0] = center_x+sin(theta_start)*radius;
- target[this->plane_axis_1] = center_y+cos(theta_start)*radius;
- this->append_milestone(target, rate);
- }
-
-}
-*/
void Robot::compute_arc(double offset[], double target[]){
}
-/*
-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.
-
- | <- theta == 0
- * * *
- * *
- * *
- * O ----T <- theta_end (e.g. 90 degrees: theta_end == PI/2)
- * /
- 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]);
- // calculate the theta (angle) of the target point
- double theta_end = theta(target[this->plane_axis_0] - offset[this->plane_axis_0] - this->current_position[this->plane_axis_0], target[this->plane_axis_1] - offset[this->plane_axis_1] - this->current_position[this->plane_axis_1]);
- //printf(" theta_start: %3.3f, theta_end: %3.3f \r\n", theta_start, theta_end);
-
- // ensure that the difference is positive so that we have clockwise travel
- if (theta_end <= theta_start) { theta_end += 2*M_PI; }
- double angular_travel = theta_end-theta_start;
- // Invert angular motion if the g-code wanted a counterclockwise arc
- if (this->motion_mode == MOTION_MODE_CCW_ARC) { angular_travel = angular_travel-2*M_PI; }
- // Find the radius
- double radius = hypot(offset[this->plane_axis_0], offset[this->plane_axis_1]);
- // Calculate the motion along the depth axis of the helix
- double depth = target[this->plane_axis_2]-this->current_position[this->plane_axis_2];
- // Trace the arc
- this->append_arc(theta_start, angular_travel, radius, depth, this->feed_rate);
- // 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 ){
return this->inch_mode ? value/25.4 : value;
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();
}
}
// 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->current_block) {
- //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_generator_busy = false;
-
}
}
void Stepper::set_step_events_per_minute( double steps_per_minute ){
- //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