void planner_init() { block_buffer_head = 0; block_buffer_tail = 0; planner_set_position( CONFIG_X_ORIGIN_OFFSET, CONFIG_Y_ORIGIN_OFFSET, CONFIG_Z_ORIGIN_OFFSET ); }
void planner_init() { block_buffer_head = 0; block_buffer_tail = 0; block_buffer_tail_write = 0; raster_buffer_next = 0; raster_buffer_count = 0; clear_vector(position); planner_set_position( CONFIG_X_ORIGIN_OFFSET, CONFIG_Y_ORIGIN_OFFSET, CONFIG_Z_ORIGIN_OFFSET ); position_update_requested = false; clear_vector_double(previous_unit_vec); previous_nominal_speed = 0.0; }
// Add a new linear movement to the buffer. x, y and z is // the signed, absolute target position in millimeters. Feed rate specifies the speed of the motion. static void planner_movement(double x, double y, double z, double feed_rate, double acceleration, uint8_t nominal_laser_intensity, uint16_t ppi, raster_t *raster) { // calculate target position in absolute steps int32_t target[3]; // Make sure we stay within our limits #if defined(CONFIG_X_MIN) x=max(x, CONFIG_X_MIN); #endif #if defined(CONFIG_X_MAX) x=min(x, CONFIG_X_MAX); #endif #if defined(CONFIG_Y_MIN) y=max(y, CONFIG_Y_MIN); #endif #if defined(CONFIG_Y_MAX) y=min(y, CONFIG_Y_MAX); #endif #if defined(CONFIG_Z_MIN) z=max(z, CONFIG_Z_MIN); #endif #if defined(CONFIG_Z_MAX) z=min(z, CONFIG_Z_MAX); #endif target[X_AXIS] = lround(x*CONFIG_X_STEPS_PER_MM); target[Y_AXIS] = lround(y*CONFIG_Y_STEPS_PER_MM); target[Z_AXIS] = lround(z*CONFIG_Z_STEPS_PER_MM); // calculate the buffer head and check for space int next_buffer_head = next_block_index( block_buffer_head ); while(block_buffer_tail == next_buffer_head) { // buffer full condition // good! We are well ahead of the robot. Rest here until buffer has room. // sleep_mode(); } block_buffers_used++; // handle position update after a stop if (position_update_requested) { planner_set_position(stepper_get_position_x(), stepper_get_position_y(), stepper_get_position_z()); position_update_requested = false; //printString("planner pos update\n"); // debug } // prepare to set up new block block_t *block = &block_buffer[block_buffer_head]; // Setup the block type if (raster == NULL) { block->block_type = BLOCK_TYPE_LINE; } else { block->block_type = BLOCK_TYPE_RASTER_LINE; memcpy(&block->raster, raster, sizeof(raster_t)); } // set nominal laser intensity block->laser_pwm = nominal_laser_intensity; // compute direction bits for this block block->direction_bits = 0; if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= (1<<STEP_X_DIR); } if (target[Y_AXIS] < position[Y_AXIS]) { block->direction_bits |= (1<<STEP_Y_DIR); } #ifndef MOTOR_Z if (target[Z_AXIS] < position[Z_AXIS]) { block->direction_bits |= (1<<STEP_Z_DIR); } #else if (target[Z_AXIS] < position[Z_AXIS]) { block->direction_bits |= STEP_Z_MASK; } #endif // number of steps for each axis block->steps_x = labs(target[X_AXIS]-position[X_AXIS]); block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]); block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]); block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z)); if (block->step_event_count == 0) { return; }; // bail if this is a zero-length block // compute path vector in terms of absolute step target and current positions double delta_mm[3]; delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/CONFIG_X_STEPS_PER_MM; delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/CONFIG_Y_STEPS_PER_MM; delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/CONFIG_Z_STEPS_PER_MM; block->millimeters = sqrt( (delta_mm[X_AXIS]*delta_mm[X_AXIS]) + (delta_mm[Y_AXIS]*delta_mm[Y_AXIS]) + (delta_mm[Z_AXIS]*delta_mm[Z_AXIS]) ); double inverse_millimeters = 1.0/block->millimeters; // store for efficency // calculate nominal_speed (mm/min) and nominal_rate (step/min) // 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; // always > 0 block->nominal_rate = ceil(block->step_event_count * inverse_minute); // always > 0 block->acceleration = acceleration; // compute the acceleration rate for this block. (step/min/acceleration_tick) block->rate_delta = ceil( block->step_event_count * inverse_millimeters * block->acceleration / (60 * ACCELERATION_TICKS_PER_SECOND) ); // Calculate the ppi steps block->laser_ppi_steps = 0; if (ppi > 0) { block->laser_ppi = ppi; // Only used by LCD output. block->laser_ppi_steps = CONFIG_X_STEPS_PER_MM * MM_PER_INCH / ppi; } //// acceleeration manager calculations // Compute path unit vector double unit_vec[3]; unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters; unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters; unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters; // Compute max junction speed 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 = ZERO_SPEED; // prime for junctions close to 0 degree if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) { // Compute cosine of angle between previous and current path. // vmax_junction is computed without sin() or acos() by trig half angle identity. double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; if (cos_theta < 0.95) { // any junction *not* close to 0 degree vmax_junction = min(previous_nominal_speed, block->nominal_speed); // prime for close to 180 if (cos_theta > -0.95) { // any junction not close to neither 0 and 180 degree -> compute vmax double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. vmax_junction = min( vmax_junction, sqrt( block->acceleration * CONFIG_JUNCTION_DEVIATION * sin_theta_d2/(1.0-sin_theta_d2) ) ); } } } block->vmax_junction = vmax_junction; // Initialize entry_speed. Compute based on deceleration to zero. // This will be updated in the forward and reverse planner passes. double v_allowable = max_allowable_speed(-block->acceleration, ZERO_SPEED, block->millimeters); block->entry_speed = min(vmax_junction, v_allowable); // Set nominal_length_flag for more efficiency. // If a block can de/ac-celerate from nominal speed to zero within the length of // the block, then the 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 unit_vector and nominal speed memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[] previous_nominal_speed = block->nominal_speed; //// end of acceleeration manager calculations // move buffer head and update position block_buffer_head = next_buffer_head; memcpy(position, target, sizeof(target)); // position[] = target[] planner_recalculate(); // make sure the stepper interrupt is processing stepper_wake_up(); }
// Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase // characters and signed floating point values (no whitespace). Comments and block delete // characters have been removed. uint8_t gcode_execute_line(char *line) { uint8_t char_counter = 0; char letter; double value; int int_value; double unit_converted_value; uint8_t next_action = NEXT_ACTION_NONE; double target[3]; double p = 0.0; int cs = 0; int l = 0; bool got_actual_line_command = false; // as opposed to just e.g. G1 F1200 gc.status_code = STATUS_OK; //// Pass 1: Commands while(next_statement(&letter, &value, line, &char_counter)) { int_value = trunc(value); switch(letter) { case 'G': switch(int_value) { case 0: gc.motion_mode = next_action = NEXT_ACTION_SEEK; break; case 1: gc.motion_mode = next_action = NEXT_ACTION_FEED; break; case 4: next_action = NEXT_ACTION_DWELL; break; case 10: next_action = NEXT_ACTION_SET_COORDINATE_OFFSET; break; case 20: gc.inches_mode = true; break; case 21: gc.inches_mode = false; break; case 30: next_action = NEXT_ACTION_HOMING_CYCLE; break; case 54: gc.offselect = OFFSET_G54; break; case 55: gc.offselect = OFFSET_G55; break; case 90: gc.absolute_mode = true; break; case 91: gc.absolute_mode = false; break; default: FAIL(STATUS_UNSUPPORTED_STATEMENT); } break; case 'M': switch(int_value) { case 7: next_action = NEXT_ACTION_AIR_ENABLE;break; case 8: next_action = NEXT_ACTION_GAS_ENABLE;break; case 9: next_action = NEXT_ACTION_AIRGAS_DISABLE;break; default: FAIL(STATUS_UNSUPPORTED_STATEMENT); } break; } if (gc.status_code) { break; } } // bail when errors if (gc.status_code) { return gc.status_code; } char_counter = 0; memcpy(target, gc.position, sizeof(target)); // i.e. target = gc.position //// Pass 2: Parameters while(next_statement(&letter, &value, line, &char_counter)) { if (gc.inches_mode) { unit_converted_value = value * MM_PER_INCH; } else { unit_converted_value = value; } switch(letter) { case 'F': if (unit_converted_value <= 0) { FAIL(STATUS_BAD_NUMBER_FORMAT); } if (gc.motion_mode == NEXT_ACTION_SEEK) { gc.seek_rate = unit_converted_value; } else { gc.feed_rate = unit_converted_value; } break; case 'X': case 'Y': case 'Z': if (gc.absolute_mode) { target[letter - 'X'] = unit_converted_value; } else { target[letter - 'X'] += unit_converted_value; } got_actual_line_command = true; break; case 'P': // dwelling seconds or CS selector if (next_action == NEXT_ACTION_SET_COORDINATE_OFFSET) { cs = trunc(value); } else { p = value; } break; case 'S': gc.nominal_laser_intensity = value; break; case 'L': // G10 qualifier l = trunc(value); break; } } // bail when error if (gc.status_code) { return(gc.status_code); } //// Perform any physical actions switch (next_action) { case NEXT_ACTION_SEEK: if (got_actual_line_command) { planner_line( target[X_AXIS] + gc.offsets[3*gc.offselect+X_AXIS], target[Y_AXIS] + gc.offsets[3*gc.offselect+Y_AXIS], target[Z_AXIS] + gc.offsets[3*gc.offselect+Z_AXIS], gc.seek_rate, 0 ); } break; case NEXT_ACTION_FEED: if (got_actual_line_command) { planner_line( target[X_AXIS] + gc.offsets[3*gc.offselect+X_AXIS], target[Y_AXIS] + gc.offsets[3*gc.offselect+Y_AXIS], target[Z_AXIS] + gc.offsets[3*gc.offselect+Z_AXIS], gc.feed_rate, gc.nominal_laser_intensity ); } break; case NEXT_ACTION_DWELL: planner_dwell(p, gc.nominal_laser_intensity); break; // case NEXT_ACTION_STOP: // planner_stop(); // stop and cancel the remaining program // gc.position[X_AXIS] = stepper_get_position_x(); // gc.position[Y_AXIS] = stepper_get_position_y(); // gc.position[Z_AXIS] = stepper_get_position_z(); // planner_set_position(gc.position[X_AXIS], gc.position[Y_AXIS], gc.position[Z_AXIS]); // // move to table origin // target[X_AXIS] = 0; // target[Y_AXIS] = 0; // target[Z_AXIS] = 0; // planner_line( target[X_AXIS] + gc.offsets[3*gc.offselect+X_AXIS], // target[Y_AXIS] + gc.offsets[3*gc.offselect+Y_AXIS], // target[Z_AXIS] + gc.offsets[3*gc.offselect+Z_AXIS], // gc.seek_rate, 0 ); // break; case NEXT_ACTION_HOMING_CYCLE: stepper_homing_cycle(); // now that we are at the physical home // zero all the position vectors clear_vector(gc.position); clear_vector(target); planner_set_position(0.0, 0.0, 0.0); // move head to g54 offset gc.offselect = OFFSET_G54; target[X_AXIS] = 0; target[Y_AXIS] = 0; target[Z_AXIS] = 0; planner_line( target[X_AXIS] + gc.offsets[3*gc.offselect+X_AXIS], target[Y_AXIS] + gc.offsets[3*gc.offselect+Y_AXIS], target[Z_AXIS] + gc.offsets[3*gc.offselect+Z_AXIS], gc.seek_rate, 0 ); break; case NEXT_ACTION_SET_COORDINATE_OFFSET: if (cs == OFFSET_G54 || cs == OFFSET_G55) { if (l == 2) { //set offset to target, eg: G10 L2 P1 X15 Y15 Z0 gc.offsets[3*cs+X_AXIS] = target[X_AXIS]; gc.offsets[3*cs+Y_AXIS] = target[Y_AXIS]; gc.offsets[3*cs+Z_AXIS] = target[Z_AXIS]; // Set target in ref to new coord system so subsequent moves are calculated correctly. target[X_AXIS] = (gc.position[X_AXIS] + gc.offsets[3*gc.offselect+X_AXIS]) - gc.offsets[3*cs+X_AXIS]; target[Y_AXIS] = (gc.position[Y_AXIS] + gc.offsets[3*gc.offselect+Y_AXIS]) - gc.offsets[3*cs+Y_AXIS]; target[Z_AXIS] = (gc.position[Z_AXIS] + gc.offsets[3*gc.offselect+Z_AXIS]) - gc.offsets[3*cs+Z_AXIS]; } else if (l == 20) { // set offset to current pos, eg: G10 L20 P2 gc.offsets[3*cs+X_AXIS] = gc.position[X_AXIS] + gc.offsets[3*gc.offselect+X_AXIS]; gc.offsets[3*cs+Y_AXIS] = gc.position[Y_AXIS] + gc.offsets[3*gc.offselect+Y_AXIS]; gc.offsets[3*cs+Z_AXIS] = gc.position[Z_AXIS] + gc.offsets[3*gc.offselect+Z_AXIS]; target[X_AXIS] = 0; target[Y_AXIS] = 0; target[Z_AXIS] = 0; } } break; case NEXT_ACTION_AIRGAS_DISABLE: planner_control_airgas_disable(); break; case NEXT_ACTION_AIR_ENABLE: planner_control_air_enable(); break; case NEXT_ACTION_GAS_ENABLE: planner_control_gas_enable(); break; } // As far as the parser is concerned, the position is now == target. In reality the // motion control system might still be processing the action and the real tool position // in any intermediate location. memcpy(gc.position, target, sizeof(double)*3); // gc.position[] = target[]; return gc.status_code; }