예제 #1
0
파일: planner.cpp 프로젝트: T3P3/Marlin-1
// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in 
// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
// calculation the caller must also provide the physical length of the line in millimeters.
void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder)
{
  // Calculate the buffer head after we push this byte
  int next_buffer_head = next_block_index(block_buffer_head);

  // If the buffer is full: good! That means we are well ahead of the robot. 
  // Rest here until there is room in the buffer.
  while(block_buffer_tail == next_buffer_head)
  {
    manage_heater(); 
    manage_inactivity(); 
    lcd_update();
  }

  // The target position of the tool in absolute steps
  // Calculate target position in absolute steps
  //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
  long target[4];
  target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
  target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
  target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);     
  target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);

  #ifdef PREVENT_DANGEROUS_EXTRUDE
  if(target[E_AXIS]!=position[E_AXIS])
  {
    if(degHotend(active_extruder)<extrude_min_temp)
    {
      position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
      SERIAL_ECHO_START;
      SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
    }
        #ifdef PREVENT_LENGTHY_EXTRUDE
    if(labs(target[E_AXIS]-position[E_AXIS])>axis_steps_per_unit[E_AXIS]*EXTRUDE_MAXLENGTH)
    {
#ifdef EASY_LOAD
	  if (!allow_lengthy_extrude_once) {
#endif
        position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
        SERIAL_ECHO_START;
        SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
#ifdef EASY_LOAD
	  }
	  allow_lengthy_extrude_once = false;
#endif
    }
    #endif  // PREVENT_LENGTHY_EXTRUDE
  }
  #endif

  // Prepare to set up new block
  block_t *block = &block_buffer[block_buffer_head];

  // Mark block as not busy (Not executed by the stepper interrupt)
  block->busy = false;

  // Number of steps for each axis
#ifndef COREXY
// default non-h-bot planning
block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
#else
// corexy planning
// these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
block->steps_x = labs((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]));
block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]));
#endif
  block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
  block->steps_e = labs(target[E_AXIS]-position[E_AXIS]);
  block->steps_e *= extrudemultiply;
  block->steps_e /= 100;
  block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e)));

  // Bail if this is a zero-length block
  if (block->step_event_count <= dropsegments)
  { 
    return; 
  }

  block->fan_speed = fanSpeed;
  #ifdef BARICUDA
  block->valve_pressure = ValvePressure;
  block->e_to_p_pressure = EtoPPressure;
  #endif

  // Compute direction bits for this block 
  block->direction_bits = 0;
#ifndef COREXY
  if (target[X_AXIS] < position[X_AXIS])
  {
    block->direction_bits |= (1<<X_AXIS); 
  }
  if (target[Y_AXIS] < position[Y_AXIS])
  {
    block->direction_bits |= (1<<Y_AXIS); 
  }
#else
  if ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]) < 0)
  {
    block->direction_bits |= (1<<X_AXIS); 
  }
  if ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]) < 0)
  {
    block->direction_bits |= (1<<Y_AXIS); 
  }
#endif
  if (target[Z_AXIS] < position[Z_AXIS])
  {
    block->direction_bits |= (1<<Z_AXIS); 
  }
  if (target[E_AXIS] < position[E_AXIS])
  {
    block->direction_bits |= (1<<E_AXIS); 
  }

  block->active_extruder = extruder;

  //enable active axes
  #ifdef COREXY
  if((block->steps_x != 0) || (block->steps_y != 0))
  {
    enable_x();
    enable_y();
  }
  #else
  if(block->steps_x != 0) enable_x();
  if(block->steps_y != 0) enable_y();
  #endif
#ifndef Z_LATE_ENABLE
  if(block->steps_z != 0) enable_z();
#endif

  // Enable all
  if(block->steps_e != 0)
  {
    enable_e0();
    enable_e1();
    enable_e2(); 
  }

  if (block->steps_e == 0)
  {
    if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
  }
  else
  {
    if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
  } 

  float delta_mm[4];
  #ifndef COREXY
    delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
    delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
  #else
    delta_mm[X_AXIS] = ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[X_AXIS];
    delta_mm[Y_AXIS] = ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[Y_AXIS];
  #endif
  delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];
  delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*extrudemultiply/100.0;
  if ( block->steps_x <=dropsegments && block->steps_y <=dropsegments && block->steps_z <=dropsegments )
  {
    block->millimeters = fabs(delta_mm[E_AXIS]);
  } 
  else
  {
    block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
  }
  float inverse_millimeters = 1.0/block->millimeters;  // Inverse millimeters to remove multiple divides 

    // Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
  float inverse_second = feed_rate * inverse_millimeters;

  int moves_queued=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);

  // slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill
#ifdef OLD_SLOWDOWN
  if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1)
    feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5); 
#endif

#ifdef SLOWDOWN
  //  segment time im micro seconds
  unsigned long segment_time = lround(1000000.0/inverse_second);
  if ((moves_queued > 1) && (moves_queued < (BLOCK_BUFFER_SIZE * 0.5)))
  {
    if (segment_time < minsegmenttime)
    { // buffer is draining, add extra time.  The amount of time added increases if the buffer is still emptied more.
      inverse_second=1000000.0/(segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued));
      #ifdef XY_FREQUENCY_LIMIT
         segment_time = lround(1000000.0/inverse_second);
      #endif
    }
  }
#endif
  //  END OF SLOW DOWN SECTION    


  block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
  block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0

  // Calculate and limit speed in mm/sec for each axis
  float current_speed[4];
  float speed_factor = 1.0; //factor <=1 do decrease speed
  for(int i=0; i < 4; i++)
  {
    current_speed[i] = delta_mm[i] * inverse_second;
    if(fabs(current_speed[i]) > max_feedrate[i])
      speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i]));
  }

  // Max segement time in us.
#ifdef XY_FREQUENCY_LIMIT
#define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT)
  // Check and limit the xy direction change frequency
  unsigned char direction_change = block->direction_bits ^ old_direction_bits;
  old_direction_bits = block->direction_bits;
  segment_time = lround((float)segment_time / speed_factor);
  
  if((direction_change & (1<<X_AXIS)) == 0)
  {
    x_segment_time[0] += segment_time;
  }
  else
  {
    x_segment_time[2] = x_segment_time[1];
    x_segment_time[1] = x_segment_time[0];
    x_segment_time[0] = segment_time;
  }
  if((direction_change & (1<<Y_AXIS)) == 0)
  {
    y_segment_time[0] += segment_time;
  }
  else
  {
    y_segment_time[2] = y_segment_time[1];
    y_segment_time[1] = y_segment_time[0];
    y_segment_time[0] = segment_time;
  }
  long max_x_segment_time = max(x_segment_time[0], max(x_segment_time[1], x_segment_time[2]));
  long max_y_segment_time = max(y_segment_time[0], max(y_segment_time[1], y_segment_time[2]));
  long min_xy_segment_time =min(max_x_segment_time, max_y_segment_time);
  if(min_xy_segment_time < MAX_FREQ_TIME)
    speed_factor = min(speed_factor, speed_factor * (float)min_xy_segment_time / (float)MAX_FREQ_TIME);
#endif

  // Correct the speed  
  if( speed_factor < 1.0)
  {
    for(unsigned char i=0; i < 4; i++)
    {
      current_speed[i] *= speed_factor;
    }
    block->nominal_speed *= speed_factor;
    block->nominal_rate *= speed_factor;
  }

  // Compute and limit the acceleration rate for the trapezoid generator.  
  float steps_per_mm = block->step_event_count/block->millimeters;
  if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)
  {
    block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
  }
  else
  {
    block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
    // Limit acceleration per axis
    if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS])
      block->acceleration_st = axis_steps_per_sqr_second[X_AXIS];
    if(((float)block->acceleration_st * (float)block->steps_y / (float)block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS])
      block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS];
    if(((float)block->acceleration_st * (float)block->steps_e / (float)block->step_event_count) > axis_steps_per_sqr_second[E_AXIS])
      block->acceleration_st = axis_steps_per_sqr_second[E_AXIS];
    if(((float)block->acceleration_st * (float)block->steps_z / (float)block->step_event_count ) > axis_steps_per_sqr_second[Z_AXIS])
      block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS];
  }
  block->acceleration = block->acceleration_st / steps_per_mm;
  block->acceleration_rate = (long)((float)block->acceleration_st * (16777216.0 / (F_CPU / 8.0)));

#if 0  // Use old jerk for now
  // 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 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

  // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
  if ((block_buffer_head != block_buffer_tail) && (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 = - 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] ;

    // Skip and use default max junction speed for 0 degree acute junction.
    if (cos_theta < 0.95) {
      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.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(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
      }
    }
  }
#endif
  // Start with a safe speed
  float vmax_junction = max_xy_jerk/2; 
  float vmax_junction_factor = 1.0; 
  if(fabs(current_speed[Z_AXIS]) > max_z_jerk/2) 
    vmax_junction = min(vmax_junction, max_z_jerk/2);
  if(fabs(current_speed[E_AXIS]) > max_e_jerk/2) 
    vmax_junction = min(vmax_junction, max_e_jerk/2);
  vmax_junction = min(vmax_junction, block->nominal_speed);
  float safe_speed = vmax_junction;

  if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
    float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2));
    //    if((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
    vmax_junction = block->nominal_speed;
    //    }
    if (jerk > max_xy_jerk) {
      vmax_junction_factor = (max_xy_jerk/jerk);
    } 
    if(fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk) {
      vmax_junction_factor= min(vmax_junction_factor, (max_z_jerk/fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS])));
    } 
    if(fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]) > max_e_jerk) {
      vmax_junction_factor = min(vmax_junction_factor, (max_e_jerk/fabs(current_speed[E_AXIS] - previous_speed[E_AXIS])));
    } 
    vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
  }
  block->max_entry_speed = vmax_junction;

  // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
  double v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
  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(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[]
  previous_nominal_speed = block->nominal_speed;


#ifdef ADVANCE
  // Calculate advance rate
  if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) {
    block->advance_rate = 0;
    block->advance = 0;
  }
  else {
    long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
    float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) * 
      (current_speed[E_AXIS] * current_speed[E_AXIS] * EXTRUTION_AREA * EXTRUTION_AREA)*256;
    block->advance = advance;
    if(acc_dist == 0) {
      block->advance_rate = 0;
    } 
    else {
      block->advance_rate = advance / (float)acc_dist;
    }
  }
  /*
    SERIAL_ECHO_START;
   SERIAL_ECHOPGM("advance :");
   SERIAL_ECHO(block->advance/256.0);
   SERIAL_ECHOPGM("advance rate :");
   SERIAL_ECHOLN(block->advance_rate/256.0);
   */
#endif // ADVANCE

  calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed,
  safe_speed/block->nominal_speed);

  // Move buffer head
  block_buffer_head = next_buffer_head;

  // Update position
  memcpy(position, target, sizeof(target)); // position[] = target[]

  planner_recalculate();

  st_wake_up();
}
예제 #2
0
  void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t extruder)
#endif  // AUTO_BED_LEVELING_FEATURE
{
  // Calculate the buffer head after we push this byte
  int next_buffer_head = next_block_index(block_buffer_head);

  // If the buffer is full: good! That means we are well ahead of the robot.
  // Rest here until there is room in the buffer.
  while (block_buffer_tail == next_buffer_head) idle();

  #if ENABLED(MESH_BED_LEVELING)
    if (mbl.active) z += mbl.get_z(x, y);
  #elif ENABLED(AUTO_BED_LEVELING_FEATURE)
    apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
  #endif

  // The target position of the tool in absolute steps
  // Calculate target position in absolute steps
  //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
  long target[NUM_AXIS];
  target[X_AXIS] = lround(x * axis_steps_per_unit[X_AXIS]);
  target[Y_AXIS] = lround(y * axis_steps_per_unit[Y_AXIS]);
  target[Z_AXIS] = lround(z * axis_steps_per_unit[Z_AXIS]);
  target[E_AXIS] = lround(e * axis_steps_per_unit[E_AXIS]);

  float dx = target[X_AXIS] - position[X_AXIS],
        dy = target[Y_AXIS] - position[Y_AXIS],
        dz = target[Z_AXIS] - position[Z_AXIS];

  // DRYRUN ignores all temperature constraints and assures that the extruder is instantly satisfied
  if (marlin_debug_flags & DEBUG_DRYRUN)
    position[E_AXIS] = target[E_AXIS];

  float de = target[E_AXIS] - position[E_AXIS];

  #if ENABLED(PREVENT_DANGEROUS_EXTRUDE)
    if (de) {
      if (degHotend(extruder) < extrude_min_temp) {
        position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
        de = 0; // no difference
        SERIAL_ECHO_START;
        SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
      }
      #if ENABLED(PREVENT_LENGTHY_EXTRUDE)
        if (labs(de) > axis_steps_per_unit[E_AXIS] * EXTRUDE_MAXLENGTH) {
          position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
          de = 0; // no difference
          SERIAL_ECHO_START;
          SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
        }
      #endif
    }
  #endif

  // Prepare to set up new block
  block_t *block = &block_buffer[block_buffer_head];

  // Mark block as not busy (Not executed by the stepper interrupt)
  block->busy = false;

  // Number of steps for each axis
  #if ENABLED(COREXY)
    // corexy planning
    // these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
    block->steps[A_AXIS] = labs(dx + dy);
    block->steps[B_AXIS] = labs(dx - dy);
    block->steps[Z_AXIS] = labs(dz);
  #elif ENABLED(COREXZ)
    // corexz planning
    block->steps[A_AXIS] = labs(dx + dz);
    block->steps[Y_AXIS] = labs(dy);
    block->steps[C_AXIS] = labs(dx - dz);
  #else
    // default non-h-bot planning
    block->steps[X_AXIS] = labs(dx);
    block->steps[Y_AXIS] = labs(dy);
    block->steps[Z_AXIS] = labs(dz);
  #endif

  block->steps[E_AXIS] = labs(de);
  block->steps[E_AXIS] *= volumetric_multiplier[extruder];
  block->steps[E_AXIS] *= extruder_multiplier[extruder];
  block->steps[E_AXIS] /= 100;
  block->step_event_count = max(block->steps[X_AXIS], max(block->steps[Y_AXIS], max(block->steps[Z_AXIS], block->steps[E_AXIS])));

  // Bail if this is a zero-length block
  if (block->step_event_count <= dropsegments) return;

  block->fan_speed = fanSpeed;
  #if ENABLED(BARICUDA)
    block->valve_pressure = ValvePressure;
    block->e_to_p_pressure = EtoPPressure;
  #endif

  // Compute direction bits for this block
  uint8_t db = 0;
  #if ENABLED(COREXY)
    if (dx < 0) db |= BIT(X_HEAD); // Save the real Extruder (head) direction in X Axis
    if (dy < 0) db |= BIT(Y_HEAD); // ...and Y
    if (dz < 0) db |= BIT(Z_AXIS);
    if (dx + dy < 0) db |= BIT(A_AXIS); // Motor A direction
    if (dx - dy < 0) db |= BIT(B_AXIS); // Motor B direction
  #elif ENABLED(COREXZ)
    if (dx < 0) db |= BIT(X_HEAD); // Save the real Extruder (head) direction in X Axis
    if (dy < 0) db |= BIT(Y_AXIS);
    if (dz < 0) db |= BIT(Z_HEAD); // ...and Z
    if (dx + dz < 0) db |= BIT(A_AXIS); // Motor A direction
    if (dx - dz < 0) db |= BIT(C_AXIS); // Motor B direction
  #else
    if (dx < 0) db |= BIT(X_AXIS);
    if (dy < 0) db |= BIT(Y_AXIS);
    if (dz < 0) db |= BIT(Z_AXIS);
  #endif
  if (de < 0) db |= BIT(E_AXIS);
  block->direction_bits = db;

  block->active_extruder = extruder;

  //enable active axes
  #if ENABLED(COREXY)
    if (block->steps[A_AXIS] || block->steps[B_AXIS]) {
      enable_x();
      enable_y();
    }
    #if DISABLED(Z_LATE_ENABLE)
      if (block->steps[Z_AXIS]) enable_z();
    #endif
  #elif ENABLED(COREXZ)
    if (block->steps[A_AXIS] || block->steps[C_AXIS]) {
      enable_x();
      enable_z();
    }
    if (block->steps[Y_AXIS]) enable_y();
  #else
    if (block->steps[X_AXIS]) enable_x();
    if (block->steps[Y_AXIS]) enable_y();
    #if DISABLED(Z_LATE_ENABLE)
      if (block->steps[Z_AXIS]) enable_z();
    #endif
  #endif

  // Enable extruder(s)
  if (block->steps[E_AXIS]) {
    if (DISABLE_INACTIVE_EXTRUDER) { //enable only selected extruder

      for (int i=0; i<EXTRUDERS; i++)
        if (g_uc_extruder_last_move[i] > 0) g_uc_extruder_last_move[i]--;

      switch(extruder) {
        case 0:
          enable_e0();
          g_uc_extruder_last_move[0] = BLOCK_BUFFER_SIZE * 2;
          #if EXTRUDERS > 1
            if (g_uc_extruder_last_move[1] == 0) disable_e1();
            #if EXTRUDERS > 2
              if (g_uc_extruder_last_move[2] == 0) disable_e2();
              #if EXTRUDERS > 3
                if (g_uc_extruder_last_move[3] == 0) disable_e3();
              #endif
            #endif
          #endif
        break;
        #if EXTRUDERS > 1
          case 1:
            enable_e1();
            g_uc_extruder_last_move[1] = BLOCK_BUFFER_SIZE * 2;
            if (g_uc_extruder_last_move[0] == 0) disable_e0();
            #if EXTRUDERS > 2
              if (g_uc_extruder_last_move[2] == 0) disable_e2();
              #if EXTRUDERS > 3
                if (g_uc_extruder_last_move[3] == 0) disable_e3();
              #endif
            #endif
          break;
          #if EXTRUDERS > 2
            case 2:
              enable_e2();
              g_uc_extruder_last_move[2] = BLOCK_BUFFER_SIZE * 2;
              if (g_uc_extruder_last_move[0] == 0) disable_e0();
              if (g_uc_extruder_last_move[1] == 0) disable_e1();
              #if EXTRUDERS > 3
                if (g_uc_extruder_last_move[3] == 0) disable_e3();
              #endif
            break;
            #if EXTRUDERS > 3
              case 3:
                enable_e3();
                g_uc_extruder_last_move[3] = BLOCK_BUFFER_SIZE * 2;
                if (g_uc_extruder_last_move[0] == 0) disable_e0();
                if (g_uc_extruder_last_move[1] == 0) disable_e1();
                if (g_uc_extruder_last_move[2] == 0) disable_e2();
              break;
            #endif // EXTRUDERS > 3
          #endif // EXTRUDERS > 2
        #endif // EXTRUDERS > 1
      }
    }
    else { // enable all
      enable_e0();
      enable_e1();
      enable_e2();
      enable_e3();
    }
  }

  if (block->steps[E_AXIS])
    NOLESS(feed_rate, minimumfeedrate);
  else
    NOLESS(feed_rate, mintravelfeedrate);

  /**
   * This part of the code calculates the total length of the movement.
   * For cartesian bots, the X_AXIS is the real X movement and same for Y_AXIS.
   * But for corexy bots, that is not true. The "X_AXIS" and "Y_AXIS" motors (that should be named to A_AXIS
   * and B_AXIS) cannot be used for X and Y length, because A=X+Y and B=X-Y.
   * So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head.
   * Having the real displacement of the head, we can calculate the total movement length and apply the desired speed.
   */
  #if ENABLED(COREXY)
    float delta_mm[6];
    delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS];
    delta_mm[Y_HEAD] = dy / axis_steps_per_unit[B_AXIS];
    delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS];
    delta_mm[A_AXIS] = (dx + dy) / axis_steps_per_unit[A_AXIS];
    delta_mm[B_AXIS] = (dx - dy) / axis_steps_per_unit[B_AXIS];
  #elif ENABLED(COREXZ)
    float delta_mm[6];
    delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS];
    delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS];
    delta_mm[Z_HEAD] = dz / axis_steps_per_unit[C_AXIS];
    delta_mm[A_AXIS] = (dx + dz) / axis_steps_per_unit[A_AXIS];
    delta_mm[C_AXIS] = (dx - dz) / axis_steps_per_unit[C_AXIS];
  #else
    float delta_mm[4];
    delta_mm[X_AXIS] = dx / axis_steps_per_unit[X_AXIS];
    delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS];
    delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS];
  #endif
  delta_mm[E_AXIS] = (de / axis_steps_per_unit[E_AXIS]) * volumetric_multiplier[extruder] * extruder_multiplier[extruder] / 100.0;

  if (block->steps[X_AXIS] <= dropsegments && block->steps[Y_AXIS] <= dropsegments && block->steps[Z_AXIS] <= dropsegments) {
    block->millimeters = fabs(delta_mm[E_AXIS]);
  }
  else {
    block->millimeters = sqrt(
      #if ENABLED(COREXY)
        square(delta_mm[X_HEAD]) + square(delta_mm[Y_HEAD]) + square(delta_mm[Z_AXIS])
      #elif ENABLED(COREXZ)
        square(delta_mm[X_HEAD]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_HEAD])
      #else
        square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS])
      #endif
    );
  }
  float inverse_millimeters = 1.0 / block->millimeters;  // Inverse millimeters to remove multiple divides

  // Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
  float inverse_second = feed_rate * inverse_millimeters;

  int moves_queued = movesplanned();

  // Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
  #if ENABLED(OLD_SLOWDOWN) || ENABLED(SLOWDOWN)
    bool mq = moves_queued > 1 && moves_queued < BLOCK_BUFFER_SIZE / 2;
    #if ENABLED(OLD_SLOWDOWN)
      if (mq) feed_rate *= 2.0 * moves_queued / BLOCK_BUFFER_SIZE;
    #endif
    #if ENABLED(SLOWDOWN)
      //  segment time im micro seconds
      unsigned long segment_time = lround(1000000.0/inverse_second);
      if (mq) {
        if (segment_time < minsegmenttime) {
          // buffer is draining, add extra time.  The amount of time added increases if the buffer is still emptied more.
          inverse_second = 1000000.0 / (segment_time + lround(2 * (minsegmenttime - segment_time) / moves_queued));
          #ifdef XY_FREQUENCY_LIMIT
            segment_time = lround(1000000.0 / inverse_second);
          #endif
        }
      }
    #endif
  #endif

  block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
  block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0

  #if ENABLED(FILAMENT_SENSOR)
    //FMM update ring buffer used for delay with filament measurements

    if (extruder == FILAMENT_SENSOR_EXTRUDER_NUM && delay_index2 > -1) {  //only for extruder with filament sensor and if ring buffer is initialized

      const int MMD = MAX_MEASUREMENT_DELAY + 1, MMD10 = MMD * 10;

      delay_dist += delta_mm[E_AXIS];  // increment counter with next move in e axis
      while (delay_dist >= MMD10) delay_dist -= MMD10; // loop around the buffer
      while (delay_dist < 0) delay_dist += MMD10;

      delay_index1 = delay_dist / 10.0;  // calculate index
      delay_index1 = constrain(delay_index1, 0, MAX_MEASUREMENT_DELAY); // (already constrained above)

      if (delay_index1 != delay_index2) { // moved index
        meas_sample = widthFil_to_size_ratio() - 100;  // Subtract 100 to reduce magnitude - to store in a signed char
        while (delay_index1 != delay_index2) {
          // Increment and loop around buffer
          if (++delay_index2 >= MMD) delay_index2 -= MMD;
          delay_index2 = constrain(delay_index2, 0, MAX_MEASUREMENT_DELAY);
          measurement_delay[delay_index2] = meas_sample;
        }
      }
    }
  #endif

  // Calculate and limit speed in mm/sec for each axis
  float current_speed[NUM_AXIS];
  float speed_factor = 1.0; //factor <=1 do decrease speed
  for (int i = 0; i < NUM_AXIS; i++) {
    current_speed[i] = delta_mm[i] * inverse_second;
    float cs = fabs(current_speed[i]), mf = max_feedrate[i];
    if (cs > mf) speed_factor = min(speed_factor, mf / cs);
  }

  // Max segement time in us.
  #ifdef XY_FREQUENCY_LIMIT
    #define MAX_FREQ_TIME (1000000.0 / XY_FREQUENCY_LIMIT)

    // Check and limit the xy direction change frequency
    unsigned char direction_change = block->direction_bits ^ old_direction_bits;
    old_direction_bits = block->direction_bits;
    segment_time = lround((float)segment_time / speed_factor);

    long xs0 = axis_segment_time[X_AXIS][0],
         xs1 = axis_segment_time[X_AXIS][1],
         xs2 = axis_segment_time[X_AXIS][2],
         ys0 = axis_segment_time[Y_AXIS][0],
         ys1 = axis_segment_time[Y_AXIS][1],
         ys2 = axis_segment_time[Y_AXIS][2];

    if ((direction_change & BIT(X_AXIS)) != 0) {
      xs2 = axis_segment_time[X_AXIS][2] = xs1;
      xs1 = axis_segment_time[X_AXIS][1] = xs0;
      xs0 = 0;
    }
    xs0 = axis_segment_time[X_AXIS][0] = xs0 + segment_time;

    if ((direction_change & BIT(Y_AXIS)) != 0) {
      ys2 = axis_segment_time[Y_AXIS][2] = axis_segment_time[Y_AXIS][1];
      ys1 = axis_segment_time[Y_AXIS][1] = axis_segment_time[Y_AXIS][0];
      ys0 = 0;
    }
    ys0 = axis_segment_time[Y_AXIS][0] = ys0 + segment_time;

    long max_x_segment_time = max(xs0, max(xs1, xs2)),
         max_y_segment_time = max(ys0, max(ys1, ys2)),
         min_xy_segment_time = min(max_x_segment_time, max_y_segment_time);
    if (min_xy_segment_time < MAX_FREQ_TIME) {
      float low_sf = speed_factor * min_xy_segment_time / MAX_FREQ_TIME;
      speed_factor = min(speed_factor, low_sf);
    }
  #endif // XY_FREQUENCY_LIMIT

  // Correct the speed
  if (speed_factor < 1.0) {
    for (unsigned char i = 0; i < NUM_AXIS; i++) current_speed[i] *= speed_factor;
    block->nominal_speed *= speed_factor;
    block->nominal_rate *= speed_factor;
  }

  // Compute and limit the acceleration rate for the trapezoid generator.
  float steps_per_mm = block->step_event_count / block->millimeters;
  long bsx = block->steps[X_AXIS], bsy = block->steps[Y_AXIS], bsz = block->steps[Z_AXIS], bse = block->steps[E_AXIS];
  if (bsx == 0 && bsy == 0 && bsz == 0) {
    block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
  }
  else if (bse == 0) {
    block->acceleration_st = ceil(travel_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
  }
  else {
    block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
  }
  // Limit acceleration per axis
  unsigned long acc_st = block->acceleration_st,
                xsteps = axis_steps_per_sqr_second[X_AXIS],
                ysteps = axis_steps_per_sqr_second[Y_AXIS],
                zsteps = axis_steps_per_sqr_second[Z_AXIS],
                esteps = axis_steps_per_sqr_second[E_AXIS];
  if ((float)acc_st * bsx / block->step_event_count > xsteps) acc_st = xsteps;
  if ((float)acc_st * bsy / block->step_event_count > ysteps) acc_st = ysteps;
  if ((float)acc_st * bsz / block->step_event_count > zsteps) acc_st = zsteps;
  if ((float)acc_st * bse / block->step_event_count > esteps) acc_st = esteps;

  block->acceleration_st = acc_st;
  block->acceleration = acc_st / steps_per_mm;
  block->acceleration_rate = (long)(acc_st * 16777216.0 / (F_CPU / 8.0));

  #if 0  // Use old jerk for now
    // 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 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

    // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
    if ((block_buffer_head != block_buffer_tail) && (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 = - 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] ;

      // Skip and use default max junction speed for 0 degree acute junction.
      if (cos_theta < 0.95) {
        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.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(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
        }
      }
    }
  #endif

  // Start with a safe speed
  float vmax_junction = max_xy_jerk / 2;
  float vmax_junction_factor = 1.0;
  float mz2 = max_z_jerk / 2, me2 = max_e_jerk / 2;
  float csz = current_speed[Z_AXIS], cse = current_speed[E_AXIS];
  if (fabs(csz) > mz2) vmax_junction = min(vmax_junction, mz2);
  if (fabs(cse) > me2) vmax_junction = min(vmax_junction, me2);
  vmax_junction = min(vmax_junction, block->nominal_speed);
  float safe_speed = vmax_junction;

  if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
    float dx = current_speed[X_AXIS] - previous_speed[X_AXIS],
          dy = current_speed[Y_AXIS] - previous_speed[Y_AXIS],
          dz = fabs(csz - previous_speed[Z_AXIS]),
          de = fabs(cse - previous_speed[E_AXIS]),
          jerk = sqrt(dx * dx + dy * dy);

    //    if ((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
    vmax_junction = block->nominal_speed;
    //    }
    if (jerk > max_xy_jerk) vmax_junction_factor = max_xy_jerk / jerk;
    if (dz > max_z_jerk) vmax_junction_factor = min(vmax_junction_factor, max_z_jerk / dz);
    if (de > max_e_jerk) vmax_junction_factor = min(vmax_junction_factor, max_e_jerk / de);

    vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
  }
  block->max_entry_speed = vmax_junction;

  // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
  double v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters);
  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.
  block->nominal_length_flag = (block->nominal_speed <= v_allowable);
  block->recalculate_flag = true; // Always calculate trapezoid for new block

  // Update previous path unit_vector and nominal speed
  for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = current_speed[i];
  previous_nominal_speed = block->nominal_speed;

  #if ENABLED(ADVANCE)
    // Calculate advance rate
    if (!bse || (!bsx && !bsy && !bsz)) {
      block->advance_rate = 0;
      block->advance = 0;
    }
    else {
      long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
      float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) * (cse * cse * EXTRUSION_AREA * EXTRUSION_AREA) * 256;
      block->advance = advance;
      block->advance_rate = acc_dist ? advance / (float)acc_dist : 0;
    }
    /*
      SERIAL_ECHO_START;
     SERIAL_ECHOPGM("advance :");
     SERIAL_ECHO(block->advance/256.0);
     SERIAL_ECHOPGM("advance rate :");
     SERIAL_ECHOLN(block->advance_rate/256.0);
     */
  #endif // ADVANCE

  calculate_trapezoid_for_block(block, block->entry_speed / block->nominal_speed, safe_speed / block->nominal_speed);

  // Move buffer head
  block_buffer_head = next_buffer_head;

  // Update position
  for (int i = 0; i < NUM_AXIS; i++) position[i] = target[i];

  planner_recalculate();

  st_wake_up();

} // plan_buffer_line()
예제 #3
0
파일: Planner.cpp 프로젝트: kein0r/Dory
/**
 * Adds movement to position to buffer if there is still space available in buffer. Position is
 * given in axis coordinate system.
 * @param [in] target Position in steps to move to in axis coordinate system
 * @param [in] feedRate Feedrate in steps/sec for this move
 * @param [in] activeExtruder Extruder used during this move
 * @return true if position was added to the buffer, false if not because buffer is full right now
 * @note Part of former plan_buffer_line from planner.cpp
 */
bool Planner::addMovement(Kinematics_AxisCoordinates_t target, uint32_t feedRate, uint8_t activeExtruder)
{
  static uint32_t previousNominalSpeed;
  // Calculate the buffer head after we push this byte
  Planner_blockIndex_t nextBufferHead = getNextBlockIndex(blockBufferHead);
  if (blockBufferTail != nextBufferHead)
    {
      // Prepare to set up new block
      Planner_Block_t *block = &(blockBuffer[blockBufferHead]);

      // Mark block as not busy (Not executed by the stepper interrupt)
      block->busy = false;

      block->steps.x = labs(target.x-lastGivenPosition.x);
      block->steps.y = labs(target.y-lastGivenPosition.y);
      block->steps.z = labs(target.z-lastGivenPosition.z);
      block->steps.e = labs(target.e-lastGivenPosition.e);
      block->step_event_count = max(block->steps.x, max(block->steps.y, max(block->steps.z, block->steps.e)));

      // Only continue if this is a non zero-length block, if not move will be merged with next move
      if (block->step_event_count >= dropsegments)
        {
          // Compute direction bits for this block
          block->direction.value = 0;
          if (target.x < lastGivenPosition.x)
          {
              block->direction.bits.x = 1;
          }
          if (target.y < lastGivenPosition.y)
          {
              block->direction.bits.y = 1;
          }
          if (target.z < lastGivenPosition.z)
          {
              block->direction.bits.z = 1;
          }
          if (target.e < lastGivenPosition.e)
          {
              block->direction.bits.e = 1;
          }

          block->active_extruder = activeExtruder;

          //enable active axes
          if(block->steps.x != 0) enable_x();
          if(block->steps.y != 0) enable_y();
          if(block->steps.z != 0) enable_z();
          if(block->steps.e != 0) enable_e();
          /* TODO: Add DISABLE_INACTIVE_EXTRUDER stuff here */

          if ( block->steps.x <= dropsegments && block->steps.y <= dropsegments && block->steps.z <= dropsegments )
            {
              block->totalTravelSteps = fabs(block->steps.e);
            }
          else
            {
              block->totalTravelSteps = sqrt(sq(block->steps.x) + sq(block->steps.y) + sq(block->steps.z));
            }
          float inverseSteps = 1.0/block->totalTravelSteps;  // Inverse total steps to remove multiple divides
          // Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
          float inverse_second = feedRate * inverseSteps;

          int moves_queued = (blockBufferHead - blockBufferTail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);

#ifdef SLOWDOWN
          //  segment time in micro seconds
          unsigned long segment_time = lround(1000000.0/inverse_second);
          if ((moves_queued > 1) && (moves_queued < (BLOCK_BUFFER_SIZE * 0.5)))
            {
              if (segment_time < minsegmenttime)
                { // buffer is draining, add extra time.  The amount of time added increases if the buffer is still emptied more.
                  inverse_second=1000000.0/(segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued));
#ifdef XY_FREQUENCY_LIMIT
                  segment_time = lround(1000000.0/inverse_second);
#endif
                }
            }
#endif //  #ifdef SLOWDOWN

          block->nominal_speed = feedRate; // Set nominal speed to given, possibly corrected, feedRate
          block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0

          // Calculate and limit speed in steps/sec for each axis
          Kinematics_AxisCoordinates_t current_speed;
          float speedFactor = 1.0; //factor <=1 do decrease speed
          current_speed.x = block->steps.x * inverse_second;
          if (current_speed.x > configuration.maxFeedrate.x)
            speedFactor = min(speedFactor, configuration.maxFeedrate.x / current_speed.x);
          current_speed.y = block->steps.y * inverse_second;
          if (current_speed.y > configuration.maxFeedrate.y)
            speedFactor = min(speedFactor, configuration.maxFeedrate.y / current_speed.y);
          current_speed.z = block->steps.z * inverse_second;
          if (current_speed.z > configuration.maxFeedrate.z)
            speedFactor = min(speedFactor, configuration.maxFeedrate.z / current_speed.z);
          current_speed.e = block->steps.e * inverse_second;
          if (current_speed.e > configuration.maxFeedrate.e)
            speedFactor = min(speedFactor, configuration.maxFeedrate.e / current_speed.e);

          /* TODO: Add #ifdef XY_FREQUENCY_LIMIT from planner.cpp here */

          /* Correct all speeds by speedFactor */
          if( speedFactor < 1.0)
          {
              current_speed.x *= speedFactor;
              current_speed.y *= speedFactor;
              current_speed.z *= speedFactor;
              current_speed.e *= speedFactor;
              block->nominal_speed *= speedFactor;
              block->nominal_rate *= speedFactor;
          }

          /* TODO: Add acceleration limiter from planner.cpp here */
          block->acceleration_st = configuration.maxAcceleration;
          //block->acceleration = block->acceleration_st  / steps_per_mm;
          block->acceleration_rate = (long)((float)block->acceleration_st * (16777216.0 / (F_CPU / 8.0)));

          /* Jerk control will control the jerk between two moves. Value of maxEntrySpeed for this block
           * is set depending on the nominalSpeed (aka feedRate) or the last block
           */
          /* TODO: Add jerk control from planner.cpp here. For now we set maxEntrySpeed to half max allowed jerk */
          block->max_entry_speed = configuration.max_xy_jerk/2;

          /* Compute maximum allowed entry speed based on deceleration over complete travel distance down
           * to user-defined MINIMUM_PLANNER_SPEED */
          uint32_t maxAllowableSpeed = maxStartSpeed(-block->acceleration, MINIMUM_PLANNER_SPEED,block->totalTravelSteps);
          block->entry_speed = min(block->max_entry_speed, maxAllowableSpeed);

          // 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 <= maxAllowableSpeed) {
            block->nominal_length_flag = true;
          }
          else {
            block->nominal_length_flag = false;
          }

          block->recalculate_flag = true; // Always calculate trapezoid for new block

          calculateTrapezoidForBlock(block);

          // Move buffer head
          blockBufferHead = nextBufferHead;

          /* update lastGivenPosition and previousNominalSpeed variable */
          memcpy(&lastGivenPosition, &target, sizeof(Kinematics_AxisCoordinates_t)); // lastGivenPosition[] = target[]
          previousNominalSpeed = block->nominal_speed;

          recalculate();

          st_wake_up();
        }
      return true;
    }
  else return false;
}
예제 #4
0
파일: planner.cpp 프로젝트: regis93/Marlin
void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder)
#endif  //LEVEL_SENSOR
{
  // Calculate the buffer head after we push this byte
  next_buffer_head = next_block_index(block_buffer_head);

  // If the buffer is full: good! That means we are well ahead of the robot. 
  // Rest here until there is room in the buffer.
#ifndef DOGLCD
  buffer_recursivity++;
#endif // DOGLCD

  while(block_buffer_tail == next_buffer_head)
  {
    next_buffer_head = next_block_index(block_buffer_head);

    temp::TemperatureManager::single::instance().manageTemperatureControl(); 
#ifndef DOGLCD
        manage_inactivity(); 
#endif //DOGLCD
    lcd_update();

#ifdef DOGLCD
    if (stop_planner_buffer == true)
    {
      stop_planner_buffer = false;
      planner_buffer_stopped = true;
      return;
    }
#endif // DOGLCD
  }

#ifndef DOGLCD
  buffer_recursivity--;
#endif // DOGLCD

#ifdef LEVEL_SENSOR
  apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
#endif // LEVEL_SENSOR

  // The target position of the tool in absolute steps
  // Calculate target position in absolute steps
  //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
  long target[4];
  target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
  target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
  target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);     
  target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);

  #ifdef PREVENT_DANGEROUS_EXTRUDE
  if(target[E_AXIS]!=position[E_AXIS])
  {
    if(degHotend(active_extruder)<extrude_min_temp)
    {
      position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
      SERIAL_ECHO_START;
      SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
    }
    
    #ifdef PREVENT_LENGTHY_EXTRUDE
    if(labs(target[E_AXIS]-position[E_AXIS])>axis_steps_per_unit[E_AXIS]*EXTRUDE_MAXLENGTH)
    {
      position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
      SERIAL_ECHO_START;
      SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
    }
    #endif
  }
  #endif

  // Prepare to set up new block
  block_t *block = &block_buffer[block_buffer_head];

  // Mark block as not busy (Not executed by the stepper interrupt)
  block->busy = false;

  // Number of steps for each axis
#ifndef COREXY
// default non-h-bot planning
block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
#else
// corexy planning
// these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
block->steps_x = labs((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]));
block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]));
#endif
  block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
  block->steps_e = labs(target[E_AXIS]-position[E_AXIS]);
  block->steps_e *= volumetric_multiplier[active_extruder];
  block->steps_e *= extrudemultiply;
  block->steps_e /= 100;
  block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e)));

  // Bail if this is a zero-length block
  if (block->step_event_count <= dropsegments)
  { 
    return; 
  }

  block->fan_speed = fanSpeed;
  #ifdef BARICUDA
  block->valve_pressure = ValvePressure;
  block->e_to_p_pressure = EtoPPressure;
  #endif

  // Compute direction bits for this block 
  block->direction_bits = 0;
#ifndef COREXY
  if (target[X_AXIS] < position[X_AXIS])
  {
    block->direction_bits |= (1<<X_AXIS); 
  }
  if (target[Y_AXIS] < position[Y_AXIS])
  {
    block->direction_bits |= (1<<Y_AXIS); 
  }
#else
  if (target[X_AXIS] < position[X_AXIS])
  {
    block->direction_bits |= (1<<X_HEAD); //AlexBorro: Save the real Extruder (head) direction in X Axis
  }
  if (target[Y_AXIS] < position[Y_AXIS])
  {
    block->direction_bits |= (1<<Y_HEAD); //AlexBorro: Save the real Extruder (head) direction in Y Axis
  }
  if ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]) < 0)
  {
    block->direction_bits |= (1<<X_AXIS); //AlexBorro: Motor A direction (Incorrectly implemented as X_AXIS)
  }
  if ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]) < 0)
  {
    block->direction_bits |= (1<<Y_AXIS); //AlexBorro: Motor B direction (Incorrectly implemented as Y_AXIS)
  }
#endif
  if (target[Z_AXIS] < position[Z_AXIS])
  {
    block->direction_bits |= (1<<Z_AXIS); 
  }
  if (target[E_AXIS] < position[E_AXIS])
  {
    block->direction_bits |= (1<<E_AXIS); 
  }

  block->active_extruder = extruder;

  //enable active axes
  #ifdef COREXY
  if((block->steps_x != 0) || (block->steps_y != 0))
  {
    enable_x();
    enable_y();
  }
  #else
  if(block->steps_x != 0) enable_x();
  if(block->steps_y != 0) enable_y();
  #endif
#ifndef Z_LATE_ENABLE
  if(block->steps_z != 0) enable_z();
#endif

  // Enable extruder(s)
  if(block->steps_e != 0)
  {
    if (DISABLE_INACTIVE_EXTRUDER) //enable only selected extruder
    {

      if(g_uc_extruder_last_move[0] > 0) g_uc_extruder_last_move[0]--;
      if(g_uc_extruder_last_move[1] > 0) g_uc_extruder_last_move[1]--;
      if(g_uc_extruder_last_move[2] > 0) g_uc_extruder_last_move[2]--;
      if(g_uc_extruder_last_move[3] > 0) g_uc_extruder_last_move[3]--;
      
      switch(extruder)
      {
        case 0: 
          enable_e0(); 
          g_uc_extruder_last_move[0] = BLOCK_BUFFER_SIZE*2;
          
          if(g_uc_extruder_last_move[1] == 0) disable_e1(); 
          if(g_uc_extruder_last_move[2] == 0) disable_e2(); 
          if(g_uc_extruder_last_move[3] == 0) disable_e3(); 
        break;
        case 1:
          enable_e1(); 
          g_uc_extruder_last_move[1] = BLOCK_BUFFER_SIZE*2;
          
          if(g_uc_extruder_last_move[0] == 0) disable_e0(); 
          if(g_uc_extruder_last_move[2] == 0) disable_e2(); 
          if(g_uc_extruder_last_move[3] == 0) disable_e3(); 
        break;
        case 2:
          enable_e2(); 
          g_uc_extruder_last_move[2] = BLOCK_BUFFER_SIZE*2;
          
          if(g_uc_extruder_last_move[0] == 0) disable_e0(); 
          if(g_uc_extruder_last_move[1] == 0) disable_e1(); 
          if(g_uc_extruder_last_move[3] == 0) disable_e3(); 
        break;        
        case 3:
          enable_e3(); 
          g_uc_extruder_last_move[3] = BLOCK_BUFFER_SIZE*2;
          
          if(g_uc_extruder_last_move[0] == 0) disable_e0(); 
          if(g_uc_extruder_last_move[1] == 0) disable_e1(); 
          if(g_uc_extruder_last_move[2] == 0) disable_e2(); 
        break;        
      }
    }
    else //enable all
    {
      enable_e0();
      enable_e1();
      enable_e2();
      enable_e3();
    }
  }

  if (block->steps_e == 0)
  {
    if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
  }
  else
  {
    if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
  } 

/* This part of the code calculates the total length of the movement. 
For cartesian bots, the X_AXIS is the real X movement and same for Y_AXIS.
But for corexy bots, that is not true. The "X_AXIS" and "Y_AXIS" motors (that should be named to A_AXIS
and B_AXIS) cannot be used for X and Y length, because A=X+Y and B=X-Y.
So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head. 
Having the real displacement of the head, we can calculate the total movement length and apply the desired speed.
*/ 
  #ifndef COREXY
    float delta_mm[4];
    delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
    delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
  #else
    float delta_mm[6];
    delta_mm[X_HEAD] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
    delta_mm[Y_HEAD] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
    delta_mm[X_AXIS] = ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[X_AXIS];
    delta_mm[Y_AXIS] = ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[Y_AXIS];
  #endif
  delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];
  delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*volumetric_multiplier[active_extruder]*extrudemultiply/100.0;
  if ( block->steps_x <=dropsegments && block->steps_y <=dropsegments && block->steps_z <=dropsegments )
  {
    block->millimeters = fabs(delta_mm[E_AXIS]);
  } 
  else
  {
    #ifndef COREXY
      block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
	#else
	  block->millimeters = sqrt(square(delta_mm[X_HEAD]) + square(delta_mm[Y_HEAD]) + square(delta_mm[Z_AXIS]));
    #endif	
  }
  float inverse_millimeters = 1.0/block->millimeters;  // Inverse millimeters to remove multiple divides 

    // Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
  float inverse_second = feed_rate * inverse_millimeters;

  int moves_queued=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);

  // slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill
#ifdef OLD_SLOWDOWN
  if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1)
    feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5); 
#endif

#ifdef SLOWDOWN
  //  segment time im micro seconds
  unsigned long segment_time = lround(1000000.0/inverse_second);
  if ((moves_queued > 1) && (moves_queued < (BLOCK_BUFFER_SIZE * 0.5)))
  {
    if (segment_time < minsegmenttime)
    { // buffer is draining, add extra time.  The amount of time added increases if the buffer is still emptied more.
      inverse_second=1000000.0/(segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued));
      #ifdef XY_FREQUENCY_LIMIT
         segment_time = lround(1000000.0/inverse_second);
      #endif
    }
  }
#endif
  //  END OF SLOW DOWN SECTION    


  block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
  block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0

#ifdef FILAMENT_SENSOR
  //FMM update ring buffer used for delay with filament measurements
  
  
    if((extruder==FILAMENT_SENSOR_EXTRUDER_NUM) && (delay_index2 > -1))  //only for extruder with filament sensor and if ring buffer is initialized
  	  {
    delay_dist = delay_dist + delta_mm[E_AXIS];  //increment counter with next move in e axis
  
    while (delay_dist >= (10*(MAX_MEASUREMENT_DELAY+1)))  //check if counter is over max buffer size in mm
      	  delay_dist = delay_dist - 10*(MAX_MEASUREMENT_DELAY+1);  //loop around the buffer
    while (delay_dist<0)
    	  delay_dist = delay_dist + 10*(MAX_MEASUREMENT_DELAY+1); //loop around the buffer
      
    delay_index1=delay_dist/10.0;  //calculate index
    
    //ensure the number is within range of the array after converting from floating point
    if(delay_index1<0)
    	delay_index1=0;
    else if (delay_index1>MAX_MEASUREMENT_DELAY)
    	delay_index1=MAX_MEASUREMENT_DELAY;
    	
    if(delay_index1 != delay_index2)  //moved index
  	  {
    	meas_sample=widthFil_to_size_ratio()-100;  //subtract off 100 to reduce magnitude - to store in a signed char
  	  }
    while( delay_index1 != delay_index2)
  	  {
  	  delay_index2 = delay_index2 + 1;
  	if(delay_index2>MAX_MEASUREMENT_DELAY)
  			  delay_index2=delay_index2-(MAX_MEASUREMENT_DELAY+1);  //loop around buffer when incrementing
  	  if(delay_index2<0)
  		delay_index2=0;
  	  else if (delay_index2>MAX_MEASUREMENT_DELAY)
  		delay_index2=MAX_MEASUREMENT_DELAY;  
  	  
  	  measurement_delay[delay_index2]=meas_sample;
  	  }
    	
    
  	  }
#endif


  // Calculate and limit speed in mm/sec for each axis
  float current_speed[4];
  float speed_factor = 1.0; //factor <=1 do decrease speed
  for(int i=0; i < 4; i++)
  {
    current_speed[i] = delta_mm[i] * inverse_second;
    if(fabs(current_speed[i]) > max_feedrate[i])
      speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i]));
  }

  // Max segement time in us.
#ifdef XY_FREQUENCY_LIMIT
#define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT)
  // Check and limit the xy direction change frequency
  unsigned char direction_change = block->direction_bits ^ old_direction_bits;
  old_direction_bits = block->direction_bits;
  segment_time = lround((float)segment_time / speed_factor);
  
  if((direction_change & (1<<X_AXIS)) == 0)
  {
    x_segment_time[0] += segment_time;
  }
  else
  {
    x_segment_time[2] = x_segment_time[1];
    x_segment_time[1] = x_segment_time[0];
    x_segment_time[0] = segment_time;
  }
  if((direction_change & (1<<Y_AXIS)) == 0)
  {
    y_segment_time[0] += segment_time;
  }
  else
  {
    y_segment_time[2] = y_segment_time[1];
    y_segment_time[1] = y_segment_time[0];
    y_segment_time[0] = segment_time;
  }
  long max_x_segment_time = max(x_segment_time[0], max(x_segment_time[1], x_segment_time[2]));
  long max_y_segment_time = max(y_segment_time[0], max(y_segment_time[1], y_segment_time[2]));
  long min_xy_segment_time =min(max_x_segment_time, max_y_segment_time);
  if(min_xy_segment_time < MAX_FREQ_TIME)
    speed_factor = min(speed_factor, speed_factor * (float)min_xy_segment_time / (float)MAX_FREQ_TIME);
#endif // XY_FREQUENCY_LIMIT

  // Correct the speed  
  if (speed_factor < 1.0) {
    for (unsigned char i = 0; i < NUM_AXIS; i++) current_speed[i] *= speed_factor;
    block->nominal_speed *= speed_factor;
    block->nominal_rate *= speed_factor;
  }

  // Compute and limit the acceleration rate for the trapezoid generator.  
  float steps_per_mm = block->step_event_count / block->millimeters;
  long bsx = block->steps_x, bsy = block->steps_y, bsz = block->steps_z, bse = block->steps_e;
  if (bsx == 0 && bsy == 0 && bsz == 0) {
    block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
  }
  else if (bse == 0) {
    block->acceleration_st = ceil(travel_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
  }
  else {
    block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
  }
  // Limit acceleration per axis
  unsigned long acc_st = block->acceleration_st,
                xsteps = axis_steps_per_sqr_second[X_AXIS],
                ysteps = axis_steps_per_sqr_second[Y_AXIS],
                zsteps = axis_steps_per_sqr_second[Z_AXIS],
                esteps = axis_steps_per_sqr_second[E_AXIS];
  if ((float)acc_st * bsx / block->step_event_count > xsteps) acc_st = xsteps;
  if ((float)acc_st * bsy / block->step_event_count > ysteps) acc_st = ysteps;
  if ((float)acc_st * bsz / block->step_event_count > zsteps) acc_st = zsteps;
  if ((float)acc_st * bse / block->step_event_count > esteps) acc_st = esteps;
 
  block->acceleration_st = acc_st;
  block->acceleration = acc_st / steps_per_mm;
  block->acceleration_rate = (long)(acc_st * 16777216.0 / (F_CPU / 8.0));

#if 0  // Use old jerk for now
  // 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 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

  // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
  if ((block_buffer_head != block_buffer_tail) && (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 = - 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] ;

    // Skip and use default max junction speed for 0 degree acute junction.
    if (cos_theta < 0.95) {
      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.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(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
      }
    }
  }
#endif
  // Start with a safe speed
  float vmax_junction = max_xy_jerk / 2;
  float vmax_junction_factor = 1.0; 
  float mz2 = max_z_jerk / 2, me2 = max_e_jerk / 2;
  float csz = current_speed[Z_AXIS], cse = current_speed[E_AXIS];
  if (fabs(csz) > mz2) vmax_junction = min(vmax_junction, mz2);
  if (fabs(cse) > me2) vmax_junction = min(vmax_junction, me2);
  vmax_junction = min(vmax_junction, block->nominal_speed);
  float safe_speed = vmax_junction;

  if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
    float dx = current_speed[X_AXIS] - previous_speed[X_AXIS],
          dy = current_speed[Y_AXIS] - previous_speed[Y_AXIS],
          dz = fabs(csz - previous_speed[Z_AXIS]),
          de = fabs(cse - previous_speed[E_AXIS]),
          jerk = sqrt(dx * dx + dy * dy);

    //    if ((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
    vmax_junction = block->nominal_speed;
    //    }
    if (jerk > max_xy_jerk) vmax_junction_factor = max_xy_jerk / jerk;
    if (dz > max_z_jerk) vmax_junction_factor = min(vmax_junction_factor, max_z_jerk / dz);
    if (de > max_e_jerk) vmax_junction_factor = min(vmax_junction_factor, max_e_jerk / de);

    vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
  }
  block->max_entry_speed = vmax_junction;

  // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
  double v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters);
  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.
  block->nominal_length_flag = (block->nominal_speed <= v_allowable); 
  block->recalculate_flag = true; // Always calculate trapezoid for new block

  // Update previous path unit_vector and nominal speed
  for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = current_speed[i];
  previous_nominal_speed = block->nominal_speed;


#ifdef ADVANCE
  // Calculate advance rate
  if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) {
    block->advance_rate = 0;
    block->advance = 0;
  }
  else {
    long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
    float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) * 
      (current_speed[E_AXIS] * current_speed[E_AXIS] * EXTRUSION_AREA * EXTRUSION_AREA)*256;
    block->advance = advance;
    if(acc_dist == 0) {
      block->advance_rate = 0;
    } 
    else {
      block->advance_rate = advance / (float)acc_dist;
    }
  }
  /*
    SERIAL_ECHO_START;
   SERIAL_ECHOPGM("advance :");
   SERIAL_ECHO(block->advance/256.0);
   SERIAL_ECHOPGM("advance rate :");
   SERIAL_ECHOLN(block->advance_rate/256.0);
   */
#endif // ADVANCE

  calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed, safe_speed/block->nominal_speed);

  // Move buffer head
  block_buffer_head = next_buffer_head;

  // Update position
  for (int i = 0; i < NUM_AXIS; i++) position[i] = target[i];

  planner_recalculate();

  st_wake_up();
}
void linear_move() { // make linear move with preset speeds and destinations, see G0 and G1
    //Determine direction of movement
    if (destination_x > current_x) {
        p_X_dir = !INVERT_X_DIR;
    } else {
        p_X_dir = INVERT_X_DIR;
    }

    if (destination_y > current_y) {
        p_Y_dir = !INVERT_Y_DIR;
    } else {
        p_Y_dir = INVERT_Y_DIR;
    }

    if (destination_z > current_z) {
        p_Z_dir = !INVERT_Z_DIR;
    } else {
        p_Z_dir = INVERT_Z_DIR;
    }

    if (destination_e > current_e) {
        p_E_dir = !INVERT_E_DIR;
    } else {
        p_E_dir = INVERT_E_DIR;
    }

    //Only enable axis that are moving. If the axis doesn't need to move then it can stay disabled depending on configuration.
    if (x_steps_remaining) enable_x();
    if (y_steps_remaining) enable_y();
    if (z_steps_remaining) enable_z();
    if (e_steps_remaining) enable_e();

    check_x_min_endstop();
    check_y_min_endstop();
    check_z_min_endstop();

    previous_millis_heater = millis();

    while (x_steps_remaining + y_steps_remaining + z_steps_remaining + e_steps_remaining > 0) { // move until no more steps remain
        if (x_steps_remaining>0) {
            if ((micros()-previous_micros_x) >= x_interval) {
                do_x_step();
                x_steps_remaining--;
            }
            check_x_min_endstop();
            led1 = 1;
        } else {
            led1 = 0;
            wait_us(2);
        }

        if (y_steps_remaining>0) {
            if ((micros()-previous_micros_y) >= y_interval) {
                do_y_step();
                y_steps_remaining--;
            }
            check_y_min_endstop();
            led2=1;
        } else {
            led2=0;
            wait_us(2);
        }

        if (z_steps_remaining>0) {
            if ((micros()-previous_micros_z) >= z_interval) {
                do_z_step();
                z_steps_remaining--;
            }
            check_z_min_endstop();
            led3=1;
        } else {
            led3=0;
            wait_us(2);
        }

        if (e_steps_remaining>0) {
            if ((micros()-previous_micros_e) >= e_interval) {
                do_e_step();
                e_steps_remaining--;
                led4=1;
            }
        } else {
            led4=0;
            wait_us(2);
        }

        if ( (millis() - previous_millis_heater) >= 500 ) {
            manage_heater();
            previous_millis_heater = millis();

            manage_inactivity(2);
        }

        wait_us(2);
    }

    led1=0;
    led2=0;
    led3=0;
    led4=0;

    if (DISABLE_X) disable_x();
    if (DISABLE_Y) disable_y();
    if (DISABLE_Z) disable_z();
    if (DISABLE_E) disable_e();

    // Update current position partly based on direction, we probably can combine this with the direction code above...
    if (destination_x > current_x) current_x = current_x + x_steps_to_take/x_steps_per_unit;
    else current_x = current_x - x_steps_to_take/x_steps_per_unit;
    if (destination_y > current_y) current_y = current_y + y_steps_to_take/y_steps_per_unit;
    else current_y = current_y - y_steps_to_take/y_steps_per_unit;
    if (destination_z > current_z) current_z = current_z + z_steps_to_take/z_steps_per_unit;
    else current_z = current_z - z_steps_to_take/z_steps_per_unit;
    if (destination_e > current_e) current_e = current_e + e_steps_to_take/e_steps_per_unit;
    else current_e = current_e - e_steps_to_take/e_steps_per_unit;
}
예제 #6
0
// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in 
// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
// calculation the caller must also provide the physical length of the line in millimeters.
void plan_buffer_line(float x, float y, float z, float e, float feed_rate) {
    int current_temp;
  
    // Calculate the buffer head after we push this byte
    int next_buffer_head = next_block_index(block_buffer_head);

    // If the buffer is full: good! That means we are well ahead of the robot. 
    // Rest here until there is room in the buffer.
    while(block_buffer_tail == next_buffer_head) { 
        manage_inactivity(1); 
#if (MINIMUM_FAN_START_SPEED > 0)
        manage_fan_start_speed();
#endif 
    }

    // The target position of the tool in absolute steps
    // Calculate target position in absolute steps
    //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
    long target[4];
    target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
    target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
    target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);     
    target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
  
    current_temp = analog2temp( current_raw );
  
#if PREVENT_DANGEROUS_EXTRUDE > 0
    if(target[E_AXIS]!=position[E_AXIS]) {
        if(current_temp < EXTRUDE_MINTEMP && prevent_cold_extrude) {
            position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
            serial_send(TXT_COLD_EXTRUSION_PREVENTED_CRLF);
        }
    
#if PREVENT_LENGTHY_EXTRUDE > 0
        if(labs(target[E_AXIS]-position[E_AXIS]) > axis_steps_per_unit[E_AXIS] * EXTRUDE_MAXLENGTH) {
            position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
            serial_send(TXT_LONG_EXTRUSION_PREVENTED_CRLF);
        }
#endif
    }
#endif
    
    // Prepare to set up new block
    block_t *block = &block_buffer[block_buffer_head];
  
    // Mark block as not busy (Not executed by the stepper interrupt)
    block->busy = 0;

    // 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->steps_e = labs(target[E_AXIS]-position[E_AXIS]);
    block->steps_e = (long)(block->steps_e * extrudemultiply / 100.0);
    block->step_event_count = MAX(block->steps_x, MAX(block->steps_y, MAX(block->steps_z, block->steps_e)));
  
    // Bail if this is a zero-length block
    if (block->step_event_count <= DROP_SEGMENTS) return;

    // Compute direction bits for this block 
    block->direction_bits = 0;
    if (target[X_AXIS] < position[X_AXIS]) block->direction_bits |= (1<<X_AXIS);
    if (target[Y_AXIS] < position[Y_AXIS]) block->direction_bits |= (1<<Y_AXIS);
    if (target[Z_AXIS] < position[Z_AXIS]) block->direction_bits |= (1<<Z_AXIS);
    if (target[E_AXIS] < position[E_AXIS]) { 
            block->direction_bits |= (1<<E_AXIS); 
            //High Feedrate for retract
            max_E_feedrate_calc = MAX_RETRACT_FEEDRATE;
            retract_feedrate_aktiv = 1;
    }
    else {
        if(retract_feedrate_aktiv) {
                if(block->steps_e > 0) retract_feedrate_aktiv = 0;
        }
        else max_E_feedrate_calc = max_feedrate[E_AXIS]; 
    }

#ifdef DELAY_ENABLE
    if(block->steps_x != 0) {
        enable_x();
        delayMicroseconds(DELAY_ENABLE);
    }
    if(block->steps_y != 0) {
        enable_y();
        delayMicroseconds(DELAY_ENABLE);
    }
    if(block->steps_z != 0) {
        enable_z();
        delayMicroseconds(DELAY_ENABLE);
    }
    if(block->steps_e != 0) {
        enable_e();
        delayMicroseconds(DELAY_ENABLE);
    }
#else
    //enable active axes
    if(block->steps_x != 0) enable_x();
    if(block->steps_y != 0) enable_y();
    if(block->steps_z != 0) enable_z();
    if(block->steps_e != 0) enable_e();
#endif 
 
    if (block->steps_e == 0) {
        if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
    }
    else {
    	if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
    } 

    // slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
    int moves_queued=(block_buffer_head-block_buffer_tail + block_buffer_size) & block_buffer_mask;
#ifdef SLOWDOWN  
    if(moves_queued < (block_buffer_size * 0.5) && moves_queued > MIN_MOVES_QUEUED_FOR_SLOWDOWN)
        feed_rate = feed_rate*moves_queued / (float)(block_buffer_size * 0.5); 
#endif

    float delta_mm[4];
    delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/(float)(axis_steps_per_unit[X_AXIS]);
    delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/(float)(axis_steps_per_unit[Y_AXIS]);
    delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/(float)(axis_steps_per_unit[Z_AXIS]);
    //delta_mm[E_AXIS] = (target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS];
    delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/(float)(axis_steps_per_unit[E_AXIS]))*extrudemultiply/100.0;
  
    if ( block->steps_x <= DROP_SEGMENTS && block->steps_y <= DROP_SEGMENTS && block->steps_z <= DROP_SEGMENTS )
        block->millimeters = fabs(delta_mm[E_AXIS]);
    else
        block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
  
    float inverse_millimeters = 1.0/(float)(block->millimeters);  // Inverse millimeters to remove multiple divides 
  
    // Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
    float inverse_second = feed_rate * inverse_millimeters;
  
    block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
    block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0
  
    /*
      #ifdef SLOWDOWN
      //  segment time im micro seconds
      long segment_time = lround(1000000.0/inverse_second);
      if ((moves_queued>0) && (moves_queued < (BLOCK_BUFFER_SIZE - 4))) {
      if (segment_time<minsegmenttime)  { // buffer is draining, add extra time.  The amount of time added increases if the buffer is still emptied more.
      segment_time=segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued);
      }
      }
      else {
      if (segment_time<minsegmenttime) segment_time=minsegmenttime;
      }
      #endif
      //  END OF SLOW DOWN SECTION
      */

    // Calculate and limit speed in mm/sec for each axis
    float current_speed[4];
    float speed_factor = 1.0; //factor <=1 do decrease speed
    for(int i=0; i < 3; i++) {
        current_speed[i] = delta_mm[i] * inverse_second;
        if(fabs(current_speed[i]) > max_feedrate[i])
            speed_factor = fmin(speed_factor, max_feedrate[i] / fabs(current_speed[i]));
    }
  
    current_speed[E_AXIS] = delta_mm[E_AXIS] * inverse_second;
    if(fabs(current_speed[E_AXIS]) > max_E_feedrate_calc)
        speed_factor = fmin(speed_factor, max_E_feedrate_calc / fabs(current_speed[E_AXIS]));


    // Correct the speed  
    if( speed_factor < 1.0)  {
            for(unsigned char i=0; i < 4; i++) current_speed[i] *= speed_factor;
            block->nominal_speed *= speed_factor;
            block->nominal_rate *= speed_factor;
    }
    
    // Compute and limit the acceleration rate for the trapezoid generator.  
    float steps_per_mm = block->step_event_count/(float)(block->millimeters);
    if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0) 
        block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
    else {
        block->acceleration_st = ceil(move_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
        // Limit acceleration per axis
        if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS])
            block->acceleration_st = axis_steps_per_sqr_second[X_AXIS];
        if(((float)block->acceleration_st * (float)block->steps_y / (float)block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS])
	
            block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS];
        if(((float)block->acceleration_st * (float)block->steps_e / (float)block->step_event_count) > axis_steps_per_sqr_second[E_AXIS])
            block->acceleration_st = axis_steps_per_sqr_second[E_AXIS];
        if(((float)block->acceleration_st * (float)block->steps_z / (float)block->step_event_count ) > axis_steps_per_sqr_second[Z_AXIS])
            block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS];
    }
  
    block->acceleration = block->acceleration_st / (float)(steps_per_mm);
    block->acceleration_rate = (long)((float)block->acceleration_st * 8.388608);
  
#if 0  // Use old jerk for now
    // 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 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

    // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
    if ((block_buffer_head != block_buffer_tail) && (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 = - 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] ;
                           
        // Skip and use default max junction speed for 0 degree acute junction.
        if (cos_theta < 0.95) {
            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.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(block->acceleration * junction_deviation * sin_theta_d2/(float)(1.0-sin_theta_d2)) );
            }
        }
    }
#endif
    // Start with a safe speed
    float vmax_junction = max_xy_jerk/2.0; 
    float vmax_junction_factor = 1.0; 

    if(fabs(current_speed[Z_AXIS]) > max_z_jerk/2.0) vmax_junction = fmin(vmax_junction, max_z_jerk/2.0);
    if(fabs(current_speed[E_AXIS]) > max_e_jerk/2.0) vmax_junction = fmin(vmax_junction, max_e_jerk/2.0);
    if(G92_reset_previous_speed == 1) {
            vmax_junction = 0.1;
            G92_reset_previous_speed = 0;  
    }

    vmax_junction = fmin(vmax_junction, block->nominal_speed);
    float safe_speed = vmax_junction;

    if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
        float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2));
        //    if((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
        vmax_junction = block->nominal_speed;
        //    }
        if (jerk > max_xy_jerk) vmax_junction_factor = (max_xy_jerk/(float)(jerk));
        if(fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk)
            vmax_junction_factor= fmin(vmax_junction_factor, (max_z_jerk/fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS])));
        if(fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]) > max_e_jerk)
            vmax_junction_factor = fmin(vmax_junction_factor, (max_e_jerk/fabs(current_speed[E_AXIS] - previous_speed[E_AXIS])));
        vmax_junction = fmin(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
    }
  
  
    block->max_entry_speed = vmax_junction;

    // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
    double v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
    block->entry_speed = fmin(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 = 1;
    else block->nominal_length_flag = 0;
    block->recalculate_flag = 1; // Always calculate trapezoid for new block
  
    // Update previous path unit_vector and nominal speed
    memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[]
    previous_nominal_speed = block->nominal_speed;
  
    calculate_trapezoid_for_block(block, block->entry_speed/(float)(block->nominal_speed),
                                  safe_speed/(float)(block->nominal_speed));
    
    // Move buffer head
    CRITICAL_SECTION_START;
    block_buffer_head = next_buffer_head;
    CRITICAL_SECTION_END;
  
    // Update position
    memcpy(position, target, sizeof(target)); // position[] = target[]

    planner_recalculate();
#ifdef AUTOTEMP
    getHighESpeed();
#endif
    st_wake_up();
}