Esempio n. 1
0
  void lcd_mixer_gradient_z_end_edit() {
    ui.defer_status_screen(true);
    ui.encoder_direction_normal();
    ENCODER_RATE_MULTIPLY(true);
    if (ui.encoderPosition != 0) {
      mixer.gradient.end_z += float((int)ui.encoderPosition) * 0.1;
      ui.encoderPosition = 0;
      NOLESS(mixer.gradient.end_z, 0);
      NOMORE(mixer.gradient.end_z, Z_MAX_POS);
    }

    if (ui.should_draw()) {
      char tmp[21];
      sprintf_P(tmp, PSTR(MSG_END_Z ": %4d.%d mm"), int(mixer.gradient.end_z), int(mixer.gradient.end_z * 10) % 10);
      SETCURSOR(2, (LCD_HEIGHT - 1) / 2);
      LCDPRINT(tmp);
    }

    if (ui.lcd_clicked) {
      if (mixer.gradient.end_z < mixer.gradient.start_z)
        mixer.gradient.start_z = mixer.gradient.end_z;
      mixer.refresh_gradient();
      ui.goto_previous_screen();
    }
  }
Esempio n. 2
0
  void getHighESpeed() {
    static float oldt = 0;

    if (!autotemp_enabled) return;
    if (degTargetHotend0() + 2 < autotemp_min) return; // probably temperature set to zero.

    float high = 0.0;
    uint8_t block_index = block_buffer_tail;

    while (block_index != block_buffer_head) {
      block_t* block = &block_buffer[block_index];
      if (block->steps[X_AXIS] || block->steps[Y_AXIS] || block->steps[Z_AXIS]) {
        float se = (float)block->steps[E_AXIS] / block->step_event_count * block->nominal_speed; // mm/sec;
        NOLESS(high, se);
      }
      block_index = next_block_index(block_index);
    }

    float t = autotemp_min + high * autotemp_factor;
    t = constrain(t, autotemp_min, autotemp_max);
    if (oldt > t) {
      t *= (1 - (AUTOTEMP_OLDWEIGHT));
      t += (AUTOTEMP_OLDWEIGHT) * oldt;
    }
    oldt = t;
    setTargetHotend0(t);
  }
Esempio n. 3
0
void calculate_trapezoid_for_block(block_t *block, float entry_factor, float exit_factor) {
  unsigned long initial_rate = ceil(block->nominal_rate * entry_factor); // (step/min)
  unsigned long final_rate = ceil(block->nominal_rate * exit_factor); // (step/min)

  // Limit minimal step rate (Otherwise the timer will overflow.)
  NOLESS(initial_rate, 120);
  NOLESS(final_rate, 120);

  long acceleration = block->acceleration_st;
  int32_t accelerate_steps = ceil(estimate_acceleration_distance(initial_rate, block->nominal_rate, acceleration));
  int32_t decelerate_steps = floor(estimate_acceleration_distance(block->nominal_rate, final_rate, -acceleration));

  // Calculate the size of Plateau of Nominal Rate.
  int32_t plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps;

  // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
  // have to use intersection_distance() to calculate when to abort acceleration and start braking
  // in order to reach the final_rate exactly at the end of this block.
  if (plateau_steps < 0) {
    accelerate_steps = ceil(intersection_distance(initial_rate, final_rate, acceleration, block->step_event_count));
    accelerate_steps = max(accelerate_steps, 0); // Check limits due to numerical round-off
    accelerate_steps = min((uint32_t)accelerate_steps, block->step_event_count);//(We can cast here to unsigned, because the above line ensures that we are above zero)
    plateau_steps = 0;
  }

#if ENABLED(ADVANCE)
  volatile long initial_advance = block->advance * entry_factor * entry_factor;
  volatile long final_advance = block->advance * exit_factor * exit_factor;
#endif // ADVANCE

  // block->accelerate_until = accelerate_steps;
  // block->decelerate_after = accelerate_steps+plateau_steps;
  CRITICAL_SECTION_START;  // Fill variables used by the stepper in a critical section
  if (!block->busy) { // Don't update variables if block is busy.
    block->accelerate_until = accelerate_steps;
    block->decelerate_after = accelerate_steps+plateau_steps;
    block->initial_rate = initial_rate;
    block->final_rate = final_rate;
    #if ENABLED(ADVANCE)
      block->initial_advance = initial_advance;
      block->final_advance = final_advance;
    #endif
  }
  CRITICAL_SECTION_END;
}
Esempio n. 4
0
  void Laser::fire(float intensity/*=100.0*/){

    laser.firing = LASER_ON;
    laser.last_firing = micros(); // microseconds of last laser firing

    NOMORE(intensity, 100.0);
    NOLESS(intensity, 0.0);

    #if ENABLED(LASER_PWM_INVERT)
      intensity = 100 - intensity;
    #endif

    #if LASER_CONTROL == 1

      #if LASER_PWR_PIN == 2
        OCR3B = PWM_MAPPED_INTENSITY;
      #elif LASER_PWR_PIN == 3
        OCR3C = PWM_MAPPED_INTENSITY;
      #elif LASER_PWR_PIN == 5
        OCR3A = PWM_MAPPED_INTENSITY;
      #elif LASER_PWR_PIN == 6
        OCR4A = PWM_MAPPED_INTENSITY;
      #elif LASER_PWR_PIN == 7
        OCR4B = PWM_MAPPED_INTENSITY;
      #elif LASER_PWR_PIN == 8
        OCR4C = PWM_MAPPED_INTENSITY;
      #endif

    #elif LASER_CONTROL == 2

      #if LASER_PWM_PIN == 2
        OCR3B = PWM_MAPPED_INTENSITY;
      #elif LASER_PWM_PIN == 3
        OCR3C = PWM_MAPPED_INTENSITY;
      #elif LASER_PWM_PIN == 5
        OCR3A = PWM_MAPPED_INTENSITY;
      #elif LASER_PWM_PIN == 6
        OCR4A = PWM_MAPPED_INTENSITY;
      #elif LASER_PWM_PIN == 7
        OCR4B = PWM_MAPPED_INTENSITY;
      #elif LASER_PWM_PIN == 8
        OCR4C = PWM_MAPPED_INTENSITY;
      #endif

      WRITE(LASER_PWR_PIN, LASER_ARM);

    #endif /* LASER_CONTROL */

    if (laser.diagnostics) SERIAL_EM("Laser_byte fired");

  }
Esempio n. 5
0
  void Mixer::update_gradient_for_z(const float z) {
    if (z == prev_z) return;
    prev_z = z;

    const float slice = gradient.end_z - gradient.start_z;

    float pct = (z - gradient.start_z) / slice;
    NOLESS(pct, 0.0f); NOMORE(pct, 1.0f);

    MIXER_STEPPER_LOOP(i) {
      const mixer_perc_t sm = gradient.start_mix[i];
      mix[i] = sm + (gradient.end_mix[i] - sm) * pct;
    }

    copy_mix_to_color(gradient.color);
  }
Esempio n. 6
0
void Mixer::normalize(const uint8_t tool_index) {
  float cmax = 0;
  #ifdef MIXER_NORMALIZER_DEBUG
    float csum = 0;
  #endif
  MIXER_STEPPER_LOOP(i) {
    const float v = collector[i];
    NOLESS(cmax, v);
    #ifdef MIXER_NORMALIZER_DEBUG
      csum += v;
    #endif
  }
  #ifdef MIXER_NORMALIZER_DEBUG
    SERIAL_ECHOPGM("Mixer: Old relation : [ ");
    MIXER_STEPPER_LOOP(i) {
      SERIAL_ECHO_F(collector[i] / csum, 3);
      SERIAL_CHAR(' ');
    }
    SERIAL_ECHOLNPGM("]");
  #endif

  // Scale all values so their maximum is COLOR_A_MASK
  const float scale = float(COLOR_A_MASK) / cmax;
  MIXER_STEPPER_LOOP(i) color[tool_index][i] = collector[i] * scale;

  #ifdef MIXER_NORMALIZER_DEBUG
    csum = 0;
    SERIAL_ECHOPGM("Mixer: Normalize to : [ ");
    MIXER_STEPPER_LOOP(i) {
      SERIAL_ECHO(uint16_t(color[tool_index][i]));
      SERIAL_CHAR(' ');
      csum += color[tool_index][i];
    }
    SERIAL_ECHOLNPGM("]");
    SERIAL_ECHOPGM("Mixer: New relation : [ ");
    MIXER_STEPPER_LOOP(i) {
      SERIAL_ECHO_F(uint16_t(color[tool_index][i]) / csum, 3);
      SERIAL_CHAR(' ');
    }
    SERIAL_ECHOLNPGM("]");
  #endif

  #if ENABLED(GRADIENT_MIX)
    refresh_gradient();
  #endif
}
Esempio n. 7
0
float r8mat_amax(int m, int n, float a[])

/******************************************************************************/
/**
  Purpose:

    R8MAT_AMAX returns the maximum absolute value entry of an R8MAT.

  Discussion:

    An R8MAT is a doubly dimensioned array of R8 values, stored as a vector
    in column-major order.

  Licensing:

    This code is distributed under the GNU LGPL license.

  Modified:

    07 September 2012

  Author:

    John Burkardt

  Parameters:

    Input, int M, the number of rows in A.

    Input, int N, the number of columns in A.

    Input, float A[M*N], the M by N matrix.

    Output, float R8MAT_AMAX, the maximum absolute value entry of A.
*/
{
  float value = r8_abs(a[0 + 0 * m]);
  for (int j = 0; j < n; j++) {
    for (int i = 0; i < m; i++) {
      NOLESS(value, r8_abs(a[i + j * m]));
    }
  }
  return value;
}
Esempio n. 8
0
  void Planner::getHighESpeed() {
    static float oldt = 0;

    if (!autotemp_enabled) return;
    if (thermalManager.degTargetHotend(0) + 2 < autotemp_min) return; // probably temperature set to zero.

    float high = 0.0;
    for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
      block_t* block = &block_buffer[b];
      if (block->steps[X_AXIS] || block->steps[Y_AXIS] || block->steps[Z_AXIS]) {
        float se = (float)block->steps[E_AXIS] / block->step_event_count * block->nominal_speed; // mm/sec;
        NOLESS(high, se);
      }
    }

    float t = autotemp_min + high * autotemp_factor;
    t = constrain(t, autotemp_min, autotemp_max);
    if (oldt > t) {
      t *= (1 - (AUTOTEMP_OLDWEIGHT));
      t += (AUTOTEMP_OLDWEIGHT) * oldt;
    }
    oldt = t;
    thermalManager.setTargetHotend(t, 0);
  }
Esempio n. 9
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()
Esempio n. 10
0
// Get the Z adjustment for non-linear bed leveling
float bilinear_z_offset(const float raw[XYZ]) {

  static float z1, d2, z3, d4, L, D, ratio_x, ratio_y,
               last_x = -999.999, last_y = -999.999;

  // Whole units for the grid line indices. Constrained within bounds.
  static int8_t gridx, gridy, nextx, nexty,
                last_gridx = -99, last_gridy = -99;

  // XY relative to the probed area
  const float rx = raw[X_AXIS] - bilinear_start[X_AXIS],
              ry = raw[Y_AXIS] - bilinear_start[Y_AXIS];

  #if ENABLED(EXTRAPOLATE_BEYOND_GRID)
    // Keep using the last grid box
    #define FAR_EDGE_OR_BOX 2
  #else
    // Just use the grid far edge
    #define FAR_EDGE_OR_BOX 1
  #endif

  if (last_x != rx) {
    last_x = rx;
    ratio_x = rx * ABL_BG_FACTOR(X_AXIS);
    const float gx = constrain(FLOOR(ratio_x), 0, ABL_BG_POINTS_X - (FAR_EDGE_OR_BOX));
    ratio_x -= gx;      // Subtract whole to get the ratio within the grid box

    #if DISABLED(EXTRAPOLATE_BEYOND_GRID)
      // Beyond the grid maintain height at grid edges
      NOLESS(ratio_x, 0); // Never < 0.0. (> 1.0 is ok when nextx==gridx.)
    #endif

    gridx = gx;
    nextx = MIN(gridx + 1, ABL_BG_POINTS_X - 1);
  }

  if (last_y != ry || last_gridx != gridx) {

    if (last_y != ry) {
      last_y = ry;
      ratio_y = ry * ABL_BG_FACTOR(Y_AXIS);
      const float gy = constrain(FLOOR(ratio_y), 0, ABL_BG_POINTS_Y - (FAR_EDGE_OR_BOX));
      ratio_y -= gy;

      #if DISABLED(EXTRAPOLATE_BEYOND_GRID)
        // Beyond the grid maintain height at grid edges
        NOLESS(ratio_y, 0); // Never < 0.0. (> 1.0 is ok when nexty==gridy.)
      #endif

      gridy = gy;
      nexty = MIN(gridy + 1, ABL_BG_POINTS_Y - 1);
    }

    if (last_gridx != gridx || last_gridy != gridy) {
      last_gridx = gridx;
      last_gridy = gridy;
      // Z at the box corners
      z1 = ABL_BG_GRID(gridx, gridy);       // left-front
      d2 = ABL_BG_GRID(gridx, nexty) - z1;  // left-back (delta)
      z3 = ABL_BG_GRID(nextx, gridy);       // right-front
      d4 = ABL_BG_GRID(nextx, nexty) - z3;  // right-back (delta)
    }

    // Bilinear interpolate. Needed since ry or gridx has changed.
                L = z1 + d2 * ratio_y;   // Linear interp. LF -> LB
    const float R = z3 + d4 * ratio_y;   // Linear interp. RF -> RB

    D = R - L;
  }

  const float offset = L + ratio_x * D;   // the offset almost always changes

  /*
  static float last_offset = 0;
  if (ABS(last_offset - offset) > 0.2) {
    SERIAL_ECHOPGM("Sudden Shift at ");
    SERIAL_ECHOPAIR("x=", rx);
    SERIAL_ECHOPAIR(" / ", bilinear_grid_spacing[X_AXIS]);
    SERIAL_ECHOLNPAIR(" -> gridx=", gridx);
    SERIAL_ECHOPAIR(" y=", ry);
    SERIAL_ECHOPAIR(" / ", bilinear_grid_spacing[Y_AXIS]);
    SERIAL_ECHOLNPAIR(" -> gridy=", gridy);
    SERIAL_ECHOPAIR(" ratio_x=", ratio_x);
    SERIAL_ECHOLNPAIR(" ratio_y=", ratio_y);
    SERIAL_ECHOPAIR(" z1=", z1);
    SERIAL_ECHOPAIR(" z2=", z2);
    SERIAL_ECHOPAIR(" z3=", z3);
    SERIAL_ECHOLNPAIR(" z4=", z4);
    SERIAL_ECHOPAIR(" L=", L);
    SERIAL_ECHOPAIR(" R=", R);
    SERIAL_ECHOLNPAIR(" offset=", offset);
  }
  last_offset = offset;
  //*/

  return offset;
}
Esempio n. 11
0
/**
 * M48: Z probe repeatability measurement function.
 *
 * Usage:
 *   M48 <P#> <X#> <Y#> <V#> <E> <L#> <S>
 *     P = Number of sampled points (4-50, default 10)
 *     X = Sample X position
 *     Y = Sample Y position
 *     V = Verbose level (0-4, default=1)
 *     E = Engage Z probe for each reading
 *     L = Number of legs of movement before probe
 *     S = Schizoid (Or Star if you prefer)
 *
 * This function requires the machine to be homed before invocation.
 */
void GcodeSuite::M48() {

  if (axis_unhomed_error()) return;

  const int8_t verbose_level = parser.byteval('V', 1);
  if (!WITHIN(verbose_level, 0, 4)) {
    SERIAL_ECHOLNPGM("?(V)erbose level is implausible (0-4).");
    return;
  }

  if (verbose_level > 0)
    SERIAL_ECHOLNPGM("M48 Z-Probe Repeatability Test");

  const int8_t n_samples = parser.byteval('P', 10);
  if (!WITHIN(n_samples, 4, 50)) {
    SERIAL_ECHOLNPGM("?Sample size not plausible (4-50).");
    return;
  }

  const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE;

  float X_current = current_position[X_AXIS],
        Y_current = current_position[Y_AXIS];

  const float X_probe_location = parser.linearval('X', X_current + X_PROBE_OFFSET_FROM_EXTRUDER),
              Y_probe_location = parser.linearval('Y', Y_current + Y_PROBE_OFFSET_FROM_EXTRUDER);

  if (!position_is_reachable_by_probe(X_probe_location, Y_probe_location)) {
    SERIAL_ECHOLNPGM("? (X,Y) out of bounds.");
    return;
  }

  bool seen_L = parser.seen('L');
  uint8_t n_legs = seen_L ? parser.value_byte() : 0;
  if (n_legs > 15) {
    SERIAL_ECHOLNPGM("?Number of legs in movement not plausible (0-15).");
    return;
  }
  if (n_legs == 1) n_legs = 2;

  const bool schizoid_flag = parser.boolval('S');
  if (schizoid_flag && !seen_L) n_legs = 7;

  /**
   * Now get everything to the specified probe point So we can safely do a
   * probe to get us close to the bed.  If the Z-Axis is far from the bed,
   * we don't want to use that as a starting point for each probe.
   */
  if (verbose_level > 2)
    SERIAL_ECHOLNPGM("Positioning the probe...");

  // Disable bed level correction in M48 because we want the raw data when we probe

  #if HAS_LEVELING
    const bool was_enabled = planner.leveling_active;
    set_bed_leveling_enabled(false);
  #endif

  setup_for_endstop_or_probe_move();

  float mean = 0.0, sigma = 0.0, min = 99999.9, max = -99999.9, sample_set[n_samples];

  // Move to the first point, deploy, and probe
  const float t = probe_pt(X_probe_location, Y_probe_location, raise_after, verbose_level);
  bool probing_good = !isnan(t);

  if (probing_good) {
    randomSeed(millis());

    for (uint8_t n = 0; n < n_samples; n++) {
      if (n_legs) {
        const int dir = (random(0, 10) > 5.0) ? -1 : 1;  // clockwise or counter clockwise
        float angle = random(0, 360);
        const float radius = random(
          #if ENABLED(DELTA)
            (int) (0.1250000000 * (DELTA_PRINTABLE_RADIUS)),
            (int) (0.3333333333 * (DELTA_PRINTABLE_RADIUS))
          #else
            (int) 5.0, (int) (0.125 * MIN(X_BED_SIZE, Y_BED_SIZE))
          #endif
        );

        if (verbose_level > 3) {
          SERIAL_ECHOPAIR("Starting radius: ", radius);
          SERIAL_ECHOPAIR("   angle: ", angle);
          SERIAL_ECHOPGM(" Direction: ");
          if (dir > 0) SERIAL_ECHOPGM("Counter-");
          SERIAL_ECHOLNPGM("Clockwise");
        }

        for (uint8_t l = 0; l < n_legs - 1; l++) {
          float delta_angle;

          if (schizoid_flag)
            // The points of a 5 point star are 72 degrees apart.  We need to
            // skip a point and go to the next one on the star.
            delta_angle = dir * 2.0 * 72.0;

          else
            // If we do this line, we are just trying to move further
            // around the circle.
            delta_angle = dir * (float) random(25, 45);

          angle += delta_angle;

          while (angle > 360.0)   // We probably do not need to keep the angle between 0 and 2*PI, but the
            angle -= 360.0;       // Arduino documentation says the trig functions should not be given values
          while (angle < 0.0)     // outside of this range.   It looks like they behave correctly with
            angle += 360.0;       // numbers outside of the range, but just to be safe we clamp them.

          X_current = X_probe_location - (X_PROBE_OFFSET_FROM_EXTRUDER) + cos(RADIANS(angle)) * radius;
          Y_current = Y_probe_location - (Y_PROBE_OFFSET_FROM_EXTRUDER) + sin(RADIANS(angle)) * radius;

          #if DISABLED(DELTA)
            X_current = constrain(X_current, X_MIN_POS, X_MAX_POS);
            Y_current = constrain(Y_current, Y_MIN_POS, Y_MAX_POS);
          #else
            // If we have gone out too far, we can do a simple fix and scale the numbers
            // back in closer to the origin.
            while (!position_is_reachable_by_probe(X_current, Y_current)) {
              X_current *= 0.8;
              Y_current *= 0.8;
              if (verbose_level > 3) {
                SERIAL_ECHOPAIR("Pulling point towards center:", X_current);
                SERIAL_ECHOLNPAIR(", ", Y_current);
              }
            }
          #endif
          if (verbose_level > 3) {
            SERIAL_ECHOPGM("Going to:");
            SERIAL_ECHOPAIR(" X", X_current);
            SERIAL_ECHOPAIR(" Y", Y_current);
            SERIAL_ECHOLNPAIR(" Z", current_position[Z_AXIS]);
          }
          do_blocking_move_to_xy(X_current, Y_current);
        } // n_legs loop
      } // n_legs

      // Probe a single point
      sample_set[n] = probe_pt(X_probe_location, Y_probe_location, raise_after, 0);

      // Break the loop if the probe fails
      probing_good = !isnan(sample_set[n]);
      if (!probing_good) break;

      /**
       * Get the current mean for the data points we have so far
       */
      float sum = 0.0;
      for (uint8_t j = 0; j <= n; j++) sum += sample_set[j];
      mean = sum / (n + 1);

      NOMORE(min, sample_set[n]);
      NOLESS(max, sample_set[n]);

      /**
       * Now, use that mean to calculate the standard deviation for the
       * data points we have so far
       */
      sum = 0.0;
      for (uint8_t j = 0; j <= n; j++)
        sum += sq(sample_set[j] - mean);

      sigma = SQRT(sum / (n + 1));
      if (verbose_level > 0) {
        if (verbose_level > 1) {
          SERIAL_ECHO(n + 1);
          SERIAL_ECHOPAIR(" of ", (int)n_samples);
          SERIAL_ECHOPAIR_F(": z: ", sample_set[n], 3);
          if (verbose_level > 2) {
            SERIAL_ECHOPAIR_F(" mean: ", mean, 4);
            SERIAL_ECHOPAIR_F(" sigma: ", sigma, 6);
            SERIAL_ECHOPAIR_F(" min: ", min, 3);
            SERIAL_ECHOPAIR_F(" max: ", max, 3);
            SERIAL_ECHOPAIR_F(" range: ", max-min, 3);
          }
          SERIAL_EOL();
        }
      }

    } // n_samples loop
  }

  STOW_PROBE();

  if (probing_good) {
    SERIAL_ECHOLNPGM("Finished!");

    if (verbose_level > 0) {
      SERIAL_ECHOPAIR_F("Mean: ", mean, 6);
      SERIAL_ECHOPAIR_F(" Min: ", min, 3);
      SERIAL_ECHOPAIR_F(" Max: ", max, 3);
      SERIAL_ECHOLNPAIR_F(" Range: ", max-min, 3);
    }

    SERIAL_ECHOLNPAIR_F("Standard Deviation: ", sigma, 6);
    SERIAL_EOL();
  }

  clean_up_after_endstop_or_probe_move();

  // Re-enable bed level correction if it had been on
  #if HAS_LEVELING
    set_bed_leveling_enabled(was_enabled);
  #endif

  report_current_position();
}
Esempio n. 12
0
    // Get the Z adjustment for non-linear bed leveling
    float Bed_level::bilinear_z_offset(const float logical[XYZ]) {

      static float  z1, d2, z3, d4, L, D, ratio_x, ratio_y,
                    last_x = -999.999, last_y = -999.999;

      // Whole units for the grid line indices. Constrained within bounds.
      static int8_t gridx, gridy, nextx, nexty,
                    last_gridx = -99, last_gridy = -99;

      // XY relative to the probed area
      const float x = RAW_X_POSITION(logical[X_AXIS]) - bilinear_start[X_AXIS],
                  y = RAW_Y_POSITION(logical[Y_AXIS]) - bilinear_start[Y_AXIS];

      if (last_x != x) {
        last_x = x;
        ratio_x = x * ABL_BG_FACTOR(X_AXIS);
        const float gx = constrain(floor(ratio_x), 0, ABL_BG_POINTS_X - 1);
        ratio_x -= gx;      // Subtract whole to get the ratio within the grid box
        NOLESS(ratio_x, 0); // Never < 0.0. (> 1.0 is ok when nextx==gridx.)
        gridx = gx;
        nextx = min(gridx + 1, ABL_BG_POINTS_X - 1);
      }

      if (last_y != y || last_gridx != gridx) {

        if (last_y != y) {
          last_y = y;
          ratio_y = y * ABL_BG_FACTOR(Y_AXIS);
          const float gy = constrain(floor(ratio_y), 0, ABL_BG_POINTS_Y - 1);
          ratio_y -= gy;
          NOLESS(ratio_y, 0);
          gridy = gy;
          nexty = min(gridy + 1, ABL_BG_POINTS_Y - 1);
        }

        if (last_gridx != gridx || last_gridy != gridy) {
          last_gridx = gridx;
          last_gridy = gridy;
          // Z at the box corners
          z1 = ABL_BG_GRID(gridx, gridy);       // left-front
          d2 = ABL_BG_GRID(gridx, nexty) - z1;  // left-back (delta)
          z3 = ABL_BG_GRID(nextx, gridy);       // right-front
          d4 = ABL_BG_GRID(nextx, nexty) - z3;  // right-back (delta)
        }

        // Bilinear interpolate. Needed since y or gridx has changed.
                    L = z1 + d2 * ratio_y;   // Linear interp. LF -> LB
        const float R = z3 + d4 * ratio_y;   // Linear interp. RF -> RB

        D = R - L;
      }

      const float offset = L + ratio_x * D;   // the offset almost always changes

      /*
      static float last_offset = 0;
      if (FABS(last_offset - offset) > 0.2) {
        SERIAL_MSG("Sudden Shift at ");
        SERIAL_MV("x=", x);
        SERIAL_MV(" / ", ABL_BG_SPACING(X_AXIS));
        SERIAL_EMV(" -> gridx=", gridx);
        SERIAL_MV(" y=", y);
        SERIAL_MV(" / ", ABL_BG_SPACING(Y_AXIS));
        SERIAL_EMV(" -> gridy=", gridy);
        SERIAL_MV(" ratio_x=", ratio_x);
        SERIAL_EMV(" ratio_y=", ratio_y);
        SERIAL_MV(" z1=", z1);
        SERIAL_MV(" d2=", d2);
        SERIAL_MV(" z3=", z3);
        SERIAL_EMV(" d4=", d4);
        SERIAL_MV(" L=", L);
        SERIAL_MV(" R=", R);
        SERIAL_EMV(" offset=", offset);
      }
      last_offset = offset;
      */

      return offset;
    }
Esempio n. 13
0
/**
 * M420: Enable/Disable Bed Leveling and/or set the Z fade height.
 *
 *   S[bool]   Turns leveling on or off
 *   Z[height] Sets the Z fade height (0 or none to disable)
 *   V[bool]   Verbose - Print the leveling grid
 *
 * With AUTO_BED_LEVELING_UBL only:
 *
 *   L[index]  Load UBL mesh from index (0 is default)
 *   T[map]    0:Human-readable 1:CSV 2:"LCD" 4:Compact
 *
 * With mesh-based leveling only:
 *
 *   C         Center mesh on the mean of the lowest and highest
 *
 * With MARLIN_DEV_MODE:
 *   S2        Create a simple random mesh and enable
 */
void GcodeSuite::M420() {
  const bool seen_S = parser.seen('S'),
             to_enable = seen_S ? parser.value_bool() : planner.leveling_active;

  #if ENABLED(MARLIN_DEV_MODE)
    if (parser.intval('S') == 2) {
      #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
        bilinear_start[X_AXIS] = MIN_PROBE_X;
        bilinear_start[Y_AXIS] = MIN_PROBE_Y;
        bilinear_grid_spacing[X_AXIS] = (MAX_PROBE_X - (MIN_PROBE_X)) / (GRID_MAX_POINTS_X - 1);
        bilinear_grid_spacing[Y_AXIS] = (MAX_PROBE_Y - (MIN_PROBE_Y)) / (GRID_MAX_POINTS_Y - 1);
      #endif
      for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
        for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
          Z_VALUES(x, y) = 0.001 * random(-200, 200);
      SERIAL_ECHOPGM("Simulated " STRINGIFY(GRID_MAX_POINTS_X) "x" STRINGIFY(GRID_MAX_POINTS_X) " mesh ");
      SERIAL_ECHOPAIR(" (", MIN_PROBE_X);
      SERIAL_CHAR(','); SERIAL_ECHO(MIN_PROBE_Y);
      SERIAL_ECHOPAIR(")-(", MAX_PROBE_X);
      SERIAL_CHAR(','); SERIAL_ECHO(MAX_PROBE_Y);
      SERIAL_ECHOLNPGM(")");
    }
  #endif

  // If disabling leveling do it right away
  // (Don't disable for just M420 or M420 V)
  if (seen_S && !to_enable) set_bed_leveling_enabled(false);

  const float oldpos[] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] };

  #if ENABLED(AUTO_BED_LEVELING_UBL)

    // L to load a mesh from the EEPROM
    if (parser.seen('L')) {

      set_bed_leveling_enabled(false);

      #if ENABLED(EEPROM_SETTINGS)
        const int8_t storage_slot = parser.has_value() ? parser.value_int() : ubl.storage_slot;
        const int16_t a = settings.calc_num_meshes();

        if (!a) {
          SERIAL_ECHOLNPGM("?EEPROM storage not available.");
          return;
        }

        if (!WITHIN(storage_slot, 0, a - 1)) {
          SERIAL_ECHOLNPGM("?Invalid storage slot.");
          SERIAL_ECHOLNPAIR("?Use 0 to ", a - 1);
          return;
        }

        settings.load_mesh(storage_slot);
        ubl.storage_slot = storage_slot;

      #else

        SERIAL_ECHOLNPGM("?EEPROM storage not available.");
        return;

      #endif
    }

    // L or V display the map info
    if (parser.seen('L') || parser.seen('V')) {
      ubl.display_map(parser.byteval('T'));
      SERIAL_ECHOPGM("Mesh is ");
      if (!ubl.mesh_is_valid()) SERIAL_ECHOPGM("in");
      SERIAL_ECHOLNPAIR("valid\nStorage slot: ", ubl.storage_slot);
    }

  #endif // AUTO_BED_LEVELING_UBL

  const bool seenV = parser.seen('V');

  #if HAS_MESH

    if (leveling_is_valid()) {

      // Subtract the given value or the mean from all mesh values
      if (parser.seen('C')) {
        const float cval = parser.value_float();
        #if ENABLED(AUTO_BED_LEVELING_UBL)

          set_bed_leveling_enabled(false);
          ubl.adjust_mesh_to_mean(true, cval);

        #else

          #if ENABLED(M420_C_USE_MEAN)

            // Get the sum and average of all mesh values
            float mesh_sum = 0;
            for (uint8_t x = GRID_MAX_POINTS_X; x--;)
              for (uint8_t y = GRID_MAX_POINTS_Y; y--;)
                mesh_sum += Z_VALUES(x, y);
            const float zmean = mesh_sum / float(GRID_MAX_POINTS);

          #else

            // Find the low and high mesh values
            float lo_val = 100, hi_val = -100;
            for (uint8_t x = GRID_MAX_POINTS_X; x--;)
              for (uint8_t y = GRID_MAX_POINTS_Y; y--;) {
                const float z = Z_VALUES(x, y);
                NOMORE(lo_val, z);
                NOLESS(hi_val, z);
              }
            // Take the mean of the lowest and highest
            const float zmean = (lo_val + hi_val) / 2.0 + cval;

          #endif

          // If not very close to 0, adjust the mesh
          if (!NEAR_ZERO(zmean)) {
            set_bed_leveling_enabled(false);
            // Subtract the mean from all values
            for (uint8_t x = GRID_MAX_POINTS_X; x--;)
              for (uint8_t y = GRID_MAX_POINTS_Y; y--;)
                Z_VALUES(x, y) -= zmean;
            #if ENABLED(ABL_BILINEAR_SUBDIVISION)
              bed_level_virt_interpolate();
            #endif
          }

        #endif
      }

    }
    else if (to_enable || seenV) {
      SERIAL_ECHO_MSG("Invalid mesh.");
      goto EXIT_M420;
    }

  #endif // HAS_MESH

  // V to print the matrix or mesh
  if (seenV) {
    #if ABL_PLANAR
      planner.bed_level_matrix.debug(PSTR("Bed Level Correction Matrix:"));
    #else
      if (leveling_is_valid()) {
        #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
          print_bilinear_leveling_grid();
          #if ENABLED(ABL_BILINEAR_SUBDIVISION)
            print_bilinear_leveling_grid_virt();
          #endif
        #elif ENABLED(MESH_BED_LEVELING)
          SERIAL_ECHOLNPGM("Mesh Bed Level data:");
          mbl.report_mesh();
        #endif
      }
    #endif
  }

  #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
    if (parser.seen('Z')) set_z_fade_height(parser.value_linear_units(), false);
  #endif

  // Enable leveling if specified, or if previously active
  set_bed_leveling_enabled(to_enable);

  #if HAS_MESH
    EXIT_M420:
  #endif

  // Error if leveling failed to enable or reenable
  if (to_enable && !planner.leveling_active)
    SERIAL_ERROR_MSG(MSG_ERR_M420_FAILED);

  SERIAL_ECHO_START();
  SERIAL_ECHOPGM("Bed Leveling ");
  serialprintln_onoff(planner.leveling_active);

  #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
    SERIAL_ECHO_START();
    SERIAL_ECHOPGM("Fade Height ");
    if (planner.z_fade_height > 0.0)
      SERIAL_ECHOLN(planner.z_fade_height);
    else
      SERIAL_ECHOLNPGM(MSG_OFF);
  #endif

  // Report change in position
  if (memcmp(oldpos, current_position, sizeof(oldpos)))
    report_current_position();
}
Esempio n. 14
0
    bool _O2 unified_bed_leveling::prepare_segmented_line_to(const float (&rtarget)[XYZE], const float &feedrate) {

      if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS]))  // fail if moving outside reachable boundary
        return true; // did not move, so current_position still accurate

      const float total[XYZE] = {
        rtarget[X_AXIS] - current_position[X_AXIS],
        rtarget[Y_AXIS] - current_position[Y_AXIS],
        rtarget[Z_AXIS] - current_position[Z_AXIS],
        rtarget[E_AXIS] - current_position[E_AXIS]
      };

      const float cartesian_xy_mm = HYPOT(total[X_AXIS], total[Y_AXIS]);  // total horizontal xy distance

      #if IS_KINEMATIC
        const float seconds = cartesian_xy_mm / feedrate;                                  // seconds to move xy distance at requested rate
        uint16_t segments = lroundf(delta_segments_per_second * seconds),                  // preferred number of segments for distance @ feedrate
                 seglimit = lroundf(cartesian_xy_mm * (1.0f / (DELTA_SEGMENT_MIN_LENGTH))); // number of segments at minimum segment length
        NOMORE(segments, seglimit);                                                        // limit to minimum segment length (fewer segments)
      #else
        uint16_t segments = lroundf(cartesian_xy_mm * (1.0f / (DELTA_SEGMENT_MIN_LENGTH))); // cartesian fixed segment length
      #endif

      NOLESS(segments, 1U);                        // must have at least one segment
      const float inv_segments = 1.0f / segments;  // divide once, multiply thereafter

      #if IS_SCARA // scale the feed rate from mm/s to degrees/s
        scara_feed_factor = cartesian_xy_mm * inv_segments * feedrate;
        scara_oldA = planner.get_axis_position_degrees(A_AXIS);
        scara_oldB = planner.get_axis_position_degrees(B_AXIS);
      #endif

      const float diff[XYZE] = {
        total[X_AXIS] * inv_segments,
        total[Y_AXIS] * inv_segments,
        total[Z_AXIS] * inv_segments,
        total[E_AXIS] * inv_segments
      };

      // Note that E segment distance could vary slightly as z mesh height
      // changes for each segment, but small enough to ignore.

      float raw[XYZE] = {
        current_position[X_AXIS],
        current_position[Y_AXIS],
        current_position[Z_AXIS],
        current_position[E_AXIS]
      };

      // Only compute leveling per segment if ubl active and target below z_fade_height.
      if (!planner.leveling_active || !planner.leveling_active_at_z(rtarget[Z_AXIS])) {   // no mesh leveling
        while (--segments) {
          LOOP_XYZE(i) raw[i] += diff[i];
          ubl_buffer_segment_raw(raw, feedrate);
        }
        ubl_buffer_segment_raw(rtarget, feedrate);
        return false; // moved but did not set_current_from_destination();
      }

      // Otherwise perform per-segment leveling

      #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
        const float fade_scaling_factor = planner.fade_scaling_factor_for_z(rtarget[Z_AXIS]);
      #endif

      // increment to first segment destination
      LOOP_XYZE(i) raw[i] += diff[i];

      for (;;) {  // for each mesh cell encountered during the move

        // Compute mesh cell invariants that remain constant for all segments within cell.
        // Note for cell index, if point is outside the mesh grid (in MESH_INSET perimeter)
        // the bilinear interpolation from the adjacent cell within the mesh will still work.
        // Inner loop will exit each time (because out of cell bounds) but will come back
        // in top of loop and again re-find same adjacent cell and use it, just less efficient
        // for mesh inset area.

        int8_t cell_xi = (raw[X_AXIS] - (MESH_MIN_X)) * (1.0f / (MESH_X_DIST)),
               cell_yi = (raw[Y_AXIS] - (MESH_MIN_Y)) * (1.0f / (MESH_Y_DIST));

        cell_xi = constrain(cell_xi, 0, (GRID_MAX_POINTS_X) - 1);
        cell_yi = constrain(cell_yi, 0, (GRID_MAX_POINTS_Y) - 1);

        const float x0 = mesh_index_to_xpos(cell_xi),   // 64 byte table lookup avoids mul+add
                    y0 = mesh_index_to_ypos(cell_yi);

        float z_x0y0 = z_values[cell_xi  ][cell_yi  ],  // z at lower left corner
              z_x1y0 = z_values[cell_xi+1][cell_yi  ],  // z at upper left corner
              z_x0y1 = z_values[cell_xi  ][cell_yi+1],  // z at lower right corner
              z_x1y1 = z_values[cell_xi+1][cell_yi+1];  // z at upper right corner

        if (isnan(z_x0y0)) z_x0y0 = 0;              // ideally activating planner.leveling_active (G29 A)
        if (isnan(z_x1y0)) z_x1y0 = 0;              //   should refuse if any invalid mesh points
        if (isnan(z_x0y1)) z_x0y1 = 0;              //   in order to avoid isnan tests per cell,
        if (isnan(z_x1y1)) z_x1y1 = 0;              //   thus guessing zero for undefined points

        float cx = raw[X_AXIS] - x0,   // cell-relative x and y
              cy = raw[Y_AXIS] - y0;

        const float z_xmy0 = (z_x1y0 - z_x0y0) * (1.0f / (MESH_X_DIST)),   // z slope per x along y0 (lower left to lower right)
                    z_xmy1 = (z_x1y1 - z_x0y1) * (1.0f / (MESH_X_DIST));   // z slope per x along y1 (upper left to upper right)

              float z_cxy0 = z_x0y0 + z_xmy0 * cx;            // z height along y0 at cx (changes for each cx in cell)

        const float z_cxy1 = z_x0y1 + z_xmy1 * cx,            // z height along y1 at cx
                    z_cxyd = z_cxy1 - z_cxy0;                 // z height difference along cx from y0 to y1

              float z_cxym = z_cxyd * (1.0f / (MESH_Y_DIST));  // z slope per y along cx from y0 to y1 (changes for each cx in cell)

        //    float z_cxcy = z_cxy0 + z_cxym * cy;            // interpolated mesh z height along cx at cy (do inside the segment loop)

        // As subsequent segments step through this cell, the z_cxy0 intercept will change
        // and the z_cxym slope will change, both as a function of cx within the cell, and
        // each change by a constant for fixed segment lengths.

        const float z_sxy0 = z_xmy0 * diff[X_AXIS],                                     // per-segment adjustment to z_cxy0
                    z_sxym = (z_xmy1 - z_xmy0) * (1.0f / (MESH_Y_DIST)) * diff[X_AXIS];  // per-segment adjustment to z_cxym

        for (;;) {  // for all segments within this mesh cell

          if (--segments == 0)                      // if this is last segment, use rtarget for exact
            COPY(raw, rtarget);

          const float z_cxcy = (z_cxy0 + z_cxym * cy) // interpolated mesh z height along cx at cy
            #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
              * fade_scaling_factor                   // apply fade factor to interpolated mesh height
            #endif
          ;

          const float z = raw[Z_AXIS];
          raw[Z_AXIS] += z_cxcy;
          ubl_buffer_segment_raw(raw, feedrate);
          raw[Z_AXIS] = z;

          if (segments == 0)                        // done with last segment
            return false;                           // did not set_current_from_destination()

          LOOP_XYZE(i) raw[i] += diff[i];

          cx += diff[X_AXIS];
          cy += diff[Y_AXIS];

          if (!WITHIN(cx, 0, MESH_X_DIST) || !WITHIN(cy, 0, MESH_Y_DIST))    // done within this cell, break to next
            break;

          // Next segment still within same mesh cell, adjust the per-segment
          // slope and intercept to compute next z height.

          z_cxy0 += z_sxy0;   // adjust z_cxy0 by per-segment z_sxy0
          z_cxym += z_sxym;   // adjust z_cxym by per-segment z_sxym

        } // segment loop
      } // cell loop

      return false; // caller will update current_position
    }
Esempio n. 15
0
  FORCE_INLINE void MarlinSerial<Cfg>::store_rxd_char() {

    static EmergencyParser::State emergency_state; // = EP_RESET

    // Get the tail - Nothing can alter its value while this ISR is executing, but there's
    // a chance that this ISR interrupted the main process while it was updating the index.
    // The backup mechanism ensures the correct value is always returned.
    const ring_buffer_pos_t t = atomic_read_rx_tail();

    // Get the head pointer - This ISR is the only one that modifies its value, so it's safe to read here
    ring_buffer_pos_t h = rx_buffer.head;

    // Get the next element
    ring_buffer_pos_t i = (ring_buffer_pos_t)(h + 1) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);

    // This must read the R_UCSRA register before reading the received byte to detect error causes
    if (Cfg::DROPPED_RX && B_DOR && !++rx_dropped_bytes) --rx_dropped_bytes;
    if (Cfg::RX_OVERRUNS && B_DOR && !++rx_buffer_overruns) --rx_buffer_overruns;
    if (Cfg::RX_FRAMING_ERRORS && B_FE && !++rx_framing_errors) --rx_framing_errors;

    // Read the character from the USART
    uint8_t c = R_UDR;

    if (Cfg::EMERGENCYPARSER) emergency_parser.update(emergency_state, c);

    // If the character is to be stored at the index just before the tail
    // (such that the head would advance to the current tail), the RX FIFO is
    // full, so don't write the character or advance the head.
    if (i != t) {
      rx_buffer.buffer[h] = c;
      h = i;
    }
    else if (Cfg::DROPPED_RX && !++rx_dropped_bytes)
      --rx_dropped_bytes;

    if (Cfg::MAX_RX_QUEUED) {
      // Calculate count of bytes stored into the RX buffer
      const ring_buffer_pos_t rx_count = (ring_buffer_pos_t)(h - t) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);

      // Keep track of the maximum count of enqueued bytes
      NOLESS(rx_max_enqueued, rx_count);
    }

    if (Cfg::XONOFF) {
      // If the last char that was sent was an XON
      if ((xon_xoff_state & XON_XOFF_CHAR_MASK) == XON_CHAR) {

        // Bytes stored into the RX buffer
        const ring_buffer_pos_t rx_count = (ring_buffer_pos_t)(h - t) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);

        // If over 12.5% of RX buffer capacity, send XOFF before running out of
        // RX buffer space .. 325 bytes @ 250kbits/s needed to let the host react
        // and stop sending bytes. This translates to 13mS propagation time.
        if (rx_count >= (Cfg::RX_SIZE) / 8) {

          // At this point, definitely no TX interrupt was executing, since the TX ISR can't be preempted.
          // Don't enable the TX interrupt here as a means to trigger the XOFF char, because if it happens
          // to be in the middle of trying to disable the RX interrupt in the main program, eventually the
          // enabling of the TX interrupt could be undone. The ONLY reliable thing this can do to ensure
          // the sending of the XOFF char is to send it HERE AND NOW.

          // About to send the XOFF char
          xon_xoff_state = XOFF_CHAR | XON_XOFF_CHAR_SENT;

          // Wait until the TX register becomes empty and send it - Here there could be a problem
          // - While waiting for the TX register to empty, the RX register could receive a new
          //   character. This must also handle that situation!
          while (!B_UDRE) {

            if (B_RXC) {
              // A char arrived while waiting for the TX buffer to be empty - Receive and process it!

              i = (ring_buffer_pos_t)(h + 1) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);

              // Read the character from the USART
              c = R_UDR;

              if (Cfg::EMERGENCYPARSER) emergency_parser.update(emergency_state, c);

              // If the character is to be stored at the index just before the tail
              // (such that the head would advance to the current tail), the FIFO is
              // full, so don't write the character or advance the head.
              if (i != t) {
                rx_buffer.buffer[h] = c;
                h = i;
              }
              else if (Cfg::DROPPED_RX && !++rx_dropped_bytes)
                --rx_dropped_bytes;
            }
            sw_barrier();
          }

          R_UDR = XOFF_CHAR;

          // Clear the TXC bit -- "can be cleared by writing a one to its bit
          // location". This makes sure flush() won't return until the bytes
          // actually got written
          B_TXC = 1;

          // At this point there could be a race condition between the write() function
          // and this sending of the XOFF char. This interrupt could happen between the
          // wait to be empty TX buffer loop and the actual write of the character. Since
          // the TX buffer is full because it's sending the XOFF char, the only way to be
          // sure the write() function will succeed is to wait for the XOFF char to be
          // completely sent. Since an extra character could be received during the wait
          // it must also be handled!
          while (!B_UDRE) {

            if (B_RXC) {
              // A char arrived while waiting for the TX buffer to be empty - Receive and process it!

              i = (ring_buffer_pos_t)(h + 1) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);

              // Read the character from the USART
              c = R_UDR;

              if (Cfg::EMERGENCYPARSER)
                emergency_parser.update(emergency_state, c);

              // If the character is to be stored at the index just before the tail
              // (such that the head would advance to the current tail), the FIFO is
              // full, so don't write the character or advance the head.
              if (i != t) {
                rx_buffer.buffer[h] = c;
                h = i;
              }
              else if (Cfg::DROPPED_RX && !++rx_dropped_bytes)
                --rx_dropped_bytes;
            }
            sw_barrier();
          }

          // At this point everything is ready. The write() function won't
          // have any issues writing to the UART TX register if it needs to!
        }
      }
    }

    // Store the new head value - The main loop will retry until the value is stable
    rx_buffer.head = h;
  }