Esempio n. 1
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();
          #if ENABLED(DUAL_X_CARRIAGE)
            if (extruder_duplication_enabled) {
              enable_e1();
              g_uc_extruder_last_move[1] = BLOCK_BUFFER_SIZE * 2;
            }
          #endif
          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

    // 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. 2
0
		m_CpuUsage.UpdateStats();

		cpuUsage.Write(L" | EE: %3d%%", m_CpuUsage.GetEEcorePct());
		cpuUsage.Write(L" | GS: %3d%%", m_CpuUsage.GetGsPct());

		if (THREAD_VU1)
			cpuUsage.Write(L" | VU: %3d%%", m_CpuUsage.GetVUPct());

		pxNonReleaseCode(cpuUsage.Write(L" | UI: %3d%%", m_CpuUsage.GetEEcorePct());)
	}

	const u64& smode2 = *(u64*)PS2GS_BASE(GS_SMODE2);

	SetTitle( pxsFmt( L"Slot %d | Speed%ls: %3d%% (%.02f)%ls | %ls-%ls | %s",
		States_GetCurrentSlot(),
		limiterStr, lround(per), fps,
		cpuUsage.c_str(),
		(smode2 & 2) ? L"frame" : L"field",
		(smode2 & 1) ? L"i" : L"p",
		WX_STR(fromUTF8(gsDest)))
	);
}

void GSFrame::OnActivate( wxActivateEvent& evt )
{
	if( IsBeingDeleted() ) return;

	evt.Skip();
	if( wxWindow* gsPanel = GetViewport() ) gsPanel->SetFocus();
}
Esempio n. 3
0
int main()
{
	int choice,  res,  no1, no2, ch;
	double n, m, s1, s2, res1, ans, val, num;
	float x, y;
	double result = 0, n1 = 0, fradecimal;
	long y1;
	char c;
	char frabinary[100], hexa[MAX];
	long int binaryval;
	div_t1 temp;
	poly * poly1, * poly2, * poly3;
	FILE *fp;
	printf("\n\n....NEW USER FIRST READ THE MANUAL TEXT FILE TO UNDERSTANS THE FUNCTIONS USAGE....\n\n");
	while(1) {
		printmenu1();
		printf("Enter your choice\n");
		scanf("%d", &ch);
		switch(ch) {
			case 1:
			
				fp = fopen("Manual.txt", "r");
				if(!fp)
					printf("Cannot open file\n");
				while((c = fgetc(fp) )!= EOF)
					printf("%c", c);
					fclose(fp);
				break;
			case 2:
				while(1) {
					printmenu();
					printf("Enter your choice\n");
					scanf("%d" ,&choice);
					switch(choice) {
						case 1:
							printf(".....sqrt() function finds the square root of a number.....\n");	
							printf("Enter your number:\n");
							scanf("%lf", &n);
							m = sqrt(n); //square root
							if( m == 0) {
								printf(" You entered negative number\n");
								break;				
							}
							printf("square root is %.3lf\n", m);
							waita();
							break;
						case 2:
							printf(".....mod() finds remainder when one number is divided 									by 2nd 	number.....\n"); 
						
							printf("enter the number to be divided :\t\n");
							scanf("%lf",&s1);
							printf("enter the divisor:\t\n");
							scanf("%lf",&s2);
							ans = fmod(s1, s2); //remainder
							printf("remainder is : %lf\n", ans);
							waita();
							break;
						case 3:
							printf(".....fabs()  finds the absolute value of floating 									point number.....\n");
						
							printf("Enter the number to find the absolute value\n");
							scanf("%lf", &val);
							res1 = fabs(val);  //absolute value
							printf("The absolute value of %lf is %lf\n", val, res1);
							waita();
							break;
						case 4:
							printf(".....ceilx() function finds ceil value of a 									number.....\n");	
							printf("Enter the value\n");
							scanf("%lf", &val);
							ans = ceilx(val); //ceil function
							printf("Value=%lf\n", ans);
							waita();
							break;   
 	
						case 5:
							printf(".....floorx() function finds floor value of a 										number.....\n");	
							printf("Enter the value\n");
							scanf("%lf", &val);
   							ans = floorx(val); //floor function
							printf("Value=%lf\n", ans);
							waita();
							break;
						case 6:
							printf(".....expoential() function finds expoential value of a 									number 	i.e. e^x.....\n");
							printf("Enter the value of x\n");
    							scanf("%lf", &n);
   						        printf("e^x = %lf\n", exponential(n)); //exponential function
							waita();
							break;
						case 7:
							printf(".....hypot()finds the value of hypotenuse when two 									sides are given.....\n");
							
							printf("Enter sides: \n");
							scanf ("%lf %lf",&s1,&s2);
							printf("Hypotenus of %f %f is: %f\n", s1, s2,hypot(s1,s2)); 								//hypotenuse function
							waita();
							break;
						case 8: 
							printf(".....cbrt() finds the cube root of proper number i.e. 									cbrt(8)= 2.....\n"); 
						 
							printf("enter the number to find the cube root\n");
 							scanf("%lf",&n1);
 							if(n1 < 0) {
 	
 								 printf("enter only +ve integer value\n");
						  		 waita();
 								 break;
 							}
 							result = cbrtx(n1); //cube root function
 							if(result)
 								 printf("cube root of %lf is %lf\n",n1,result);
 								else {
								printf("not a proper value for finding the cube 										root\n");
								waita();
 								break;
							}	
							
							waita();
							break;
						case 9:
							printf(".....pow() function finds the x^y power.....\n");
							printf("Enter the number and the power\n");
							scanf("%lf%lf", &s1, &s2);
							res1 = Pow(s1, s2); //power function
							printf("The %lf^%lf is %lf\n", s1, s2, res1);
							
							waita();
							break;
						case 10:
							printf(".....trunc() function truncates the floating 									points.....\n");	
							printf("Enter the value for truncating\n");
							scanf("%lf", &s1);
							res1 = trunc(s1); // truncating a number
							printf("The value after truncating is %lf\n", res1);
							
							waita();
							break;
						case 11:
							printf(".....round() function rounds up a floating point 									number..... \n");
							printf("Enter the value for rounding\n");
							scanf("%lf", &s1);
							res1 = round(s1); //rounding a number
							printf("The value after rounding is %lf\n", res1);
							
							waita();
							break;
						case 12:
							printf(".....abs() function finds absolute value of integer 									number.....\n");
							printf("Enter the integer number to find the absolute value\n");
							scanf("%lf", &n);
							if(n - (int)n == 0) {
								res = abs(n);  //absolute value
								printf("The absolute value of %lf is %d\n", n, res);
								waita();
								break;
							}
							else {
								printf("Enter integer number\n");
								waita();
								break;
							}
							waita();
							break;
						case 13:
							printf(".....fmax() function finds maximum between two 									numbers.....\n");	
							printf("Enter two numbers to find maximun number\n");	
							scanf("%lf%lf", &s1, &s2);
							res1 = fmax(s1,s2);
							printf("The max number is %f\n",res1);
							waita();
							break;
						case 14:
							printf(".....fmin() function finds minimum between two 									numbers.....\n");	
							printf("Enter two numbers to find minimum number\n");	
							scanf("%lf%lf", &s1, &s2);
							res1 = fmin(s1,s2);
							printf("The min number is %f\n",res1);
							waita();
							break;
						case 15:
							printf(".....fdim()finds +ve difference bet^n 2 no's & if fails 									it returns 0.....\n"); 
						
							printf("Enter two numbers to find difference of two numbers\n");
							scanf("%lf%lf", &s1, &s2);
							res1 = fdim(s1,s2);
							if(res1 == 0) {
								printf("This functions returns 0 when  first number is 										less than or equal to second number\n");
								waita();
								break;
							}		
							printf("The positive Difference  is %f\n",res1);
							waita();
							break;
						case 16:
							printf(".....roundf() function rounds up a floating 										value.....		\n");	
							printf("Enter the value for rounding\n");
							scanf("%f", &x);
							y = roundf(x);
							printf("The value after rounding is %f\n", y);
							waita();
							break;
						case 17:
							printf(".....lround() function rounds up a long value.....\n");
							printf("Enter the value for rounding\n");
							scanf("%lf", &n);
							y1 = lround(n);
							printf("The value after rounding is %li\n", y1);
							waita();
							break;
						case 18:
							printf(".....add_poly() function adds two polynomials.....\n");
							printf("\nCreate 1st expression\n");
							create(&poly1);
							printf("\nStored the 1st expression");
							show(poly1);
		
							printf("\nCreate 2nd expression\n");
							create(&poly2);
							printf("\nStored the 2nd expression");
							show(poly2);
						
							add_poly(&poly3, poly1, poly2);
							show(poly3);
							waita();
							break;
						case 19:
							printf(".....sub_poly() function subtract two 											polynomials.....		\n");	
							printf("\nCreate 1st expression\n");
							create(&poly1);
							printf("\nStored the 1st expression");
							show(poly1);	
							printf("\nCreate 2nd expression\n");
							create(&poly2);
							printf("\nStored the 2nd expression");
							show(poly2);
	
							sub_poly(&poly3, poly1, poly2);
							show(poly3);
							waita();
							break;
						case 20:
							printf(".....division() function givesquotient and remainder 									after division.....\n");
							printf("Enter divident\n");
							scanf("%d", &no1);
							printf("Enter divisor\n");
							scanf("%d", &no2);
							temp = division(no1, no2);
							printf("quotient=%d\tremainder=%d\n", temp.quot, temp.rem);
							waita();
							break;			
						case 21:
							printf(".....ldexp() function returns value * 									2^expoential.....		\n");
							printf("Enter the value\n");
							scanf("%lf", &n);
							printf("Enter the integer exponent\n");
							scanf("%d", &no1);
							m = ldexp(n, no1);
							printf("value=%lf\n", m);	
							waita();
							break;;
						case 22:
							printf(".....exp2() function returns 2^expoential.....\n");
							printf("Enter the expoential\n");
							scanf("%lf", &n);
							m = exp2(n);
							printf("exp2(%lf)=%lf\n", n, m);
							waita();
							break;
						case 23:
							printf(".....tgamma() fuction calculates gamma value.....\n");
							printf("Enter the number to find gamma value\n");
							scanf("%lf", &num);
							if( num <= 0) {
								printf("Gamma of negative number and 0 is not 										defined		\n");	
								waita();
								break;
							}
							ans = tgamma(num);
							printf("gamma(%lf)=%lf\n", num, ans);
							waita();
							break;			
						case 24:
							printf(".....fact() function returns factorial of a 										number.....		\n");	
							printf("Enter number to find factorial\n");
							scanf("%lf", &n);
							ans = fact(n);
							printf("The factorial of %lf is %lf\n", n, ans);
							waita();
							break;
						case 25:
							printf(".....square() function calculates square of a 									number.....\n");	
							printf("Enter number to find square of entered number\n");
							scanf("%lf", &n);
							ans = square(n);
							printf("The square of %lf is %lf\n",n, ans);
							waita();
							break;
						case 26:
							printf(".....cube() function calculates cube of a 									number.....		\n");	
							printf("Enter number to find cube of a entered number\n");
							scanf("%lf", &n);
							ans = cube(n);
							printf("The cube of %lf is %lf\n",n, ans);
							
							waita();
							break;
						case 27:
							printf("dec_bin() function converts decimal number into 								binary			\n");
							printf("Enter any fractional Decimal number\n");
							scanf("%lf", &fradecimal);
							printf("eduivalent binary value is %lf\n",dec_bin(fradecimal));
							waita();
							break;
						case 28:
							printf("bin_dec() function converts binary number into 								decimal 		\n");
							printf("Enter any fractional Binary number\n");
							scanf("%s", frabinary);
							printf("eduivalent decimal value is %lf\n",bin_dec(frabinary));
							waita();
							break;
						case 29:
							printf("This fun. converts decimal number to octal number\n");
							printf("Enter a decimal number:\n ");
       							scanf("%d", &no1);
       							printf("%d in decimal = %d in octal\n", no1, 									decimal_octal(no1));	
							waita();
							break;
						case 30:
							printf("This fun. converts octal number to decimal number\n");
							printf("Enter a octal number:\n ");
       							scanf("%d", &no1);
       							printf("%d in octal = %d in decimal\n", no1, 									decimal_octal(no1));	
							waita();
							break;
						case 31:
							printf("This fun. converts binary number to octal number\n");
							printf("Enter a binary number:\n ");
       							scanf("%d",&no1);
        						printf("%d in binary = %d in octal\n", no1, binary_octal(no1));
							waita();
							break;
						case 32:
							printf("This fun. converts octal number to binary number\n");
							printf("Enter a octal number:\n ");
        						scanf("%d",&no1);
        						printf("%d in octal = %d in binary\n",no1, octal_binary(no1));
							waita();
   
							break;
						case 33:
							printf("This function converts binary into hex number\n");
							printf("Enter the binary number:\n ");
    							scanf("%ld", &binaryval);
    
    							printf("Equivalent hexadecimal value: %lX\n", 									binary_hex(binaryval));
							waita();
							break;
						case 34:
							printf("This function converts hex into binary number\n");
							printf("Enter the value for hexadecimal\n ");
    							scanf("%s", hexa);
   							hex_binary(hexa);
							waita();
							break;
						case 35:
							return 0;
						default:
							printf(".....wrong choice.....\n");
					}	
				}	
			case 3:	
				return 0;
			default:
				printf("Invalid Choice\n");
			}	
			
		} 
		return 0;		
	}		
Esempio n. 4
0
void plan_set_e_position(const float &e) {
  position[E_AXIS] = lround(e * axis_steps_per_unit[E_AXIS + active_extruder]);
  last_extruder = active_extruder;
  st_set_e_position(position[E_AXIS]);
}
Esempio n. 5
0
// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in 
// millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
// rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes.
// All position data passed to the planner must be in terms of machine position to keep the planner 
// independent of any coordinate system changes and offsets, which are handled by the g-code parser.
// NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control.
// Also the feed rate input value is used in three ways: as a normal feed rate if invert_feed_rate
// is false, as inverse time if invert_feed_rate is true, or as seek/rapids rate if the feed_rate
// value is negative (and invert_feed_rate always false).
void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rate) 
{
  // Prepare to set up new block
  block_t *block = &block_buffer[block_buffer_head];

  // Calculate target position in absolute steps
  int32_t target[N_AXIS];
  target[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]);
  target[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]);
  target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]);     
  
  // Number of steps for each axis
  block->steps_x = labs(target[X_AXIS]-pl.position[X_AXIS]);
  block->steps_y = labs(target[Y_AXIS]-pl.position[Y_AXIS]);
  block->steps_z = labs(target[Z_AXIS]-pl.position[Z_AXIS]);
  block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z));

  // Bail if this is a zero-length block
  if (block->step_event_count == 0) { return; };
  
  // Compute path vector in terms of absolute step target and current positions
  float delta_mm[N_AXIS];
  delta_mm[X_AXIS] = x-pl.last_x;
  delta_mm[Y_AXIS] = y-pl.last_y; 
  delta_mm[Z_AXIS] = z-pl.last_z;
  block->millimeters = sqrt(delta_mm[X_AXIS]*delta_mm[X_AXIS] + delta_mm[Y_AXIS]*delta_mm[Y_AXIS] + 
                            delta_mm[Z_AXIS]*delta_mm[Z_AXIS]);

  // Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids)
  // TODO: Need to distinguish a rapids vs feed move for overrides. Some flag of some sort.
  if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later
  else if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; }

  // Calculate the unit vector of the line move and the block maximum feed rate and acceleration limited
  // by the maximum possible values. Block rapids rates are computed or feed rates are scaled down so
  // they don't exceed the maximum axes velocities. The block acceleration is maximized based on direction
  // and axes properties as well.
  // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes,
  // if they are also orthogonal/independent. Operates on the absolute value of the unit vector.
  uint8_t i;
  float unit_vec[N_AXIS], inverse_unit_vec_value;
  float inverse_millimeters = 1.0/block->millimeters;  // Inverse millimeters to remove multiple float divides	
  block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration in loop
  for (i=0; i<N_AXIS; i++) { 
    if (delta_mm[i] == 0) { 
      unit_vec[i] = 0;  // Store zero value. And avoid divide by zero.
    } else {
      // Compute unit vector and its absolute inverse value
      unit_vec[i] = delta_mm[i]*inverse_millimeters;
      inverse_unit_vec_value = abs(1.0/unit_vec[i]);
      // Check and limit feed rate against max axis velocities and scale accelerations to maximums
      feed_rate = min(feed_rate,settings.max_velocity[i]*inverse_unit_vec_value);
      block->acceleration = min(block->acceleration,settings.acceleration[i]*inverse_unit_vec_value);
    }
  }

  // Compute nominal speed and rates
  block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min)^2. Always > 0
  block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)

  // Compute the acceleration and distance traveled per step event for the stepper algorithm.
  block->rate_delta = ceil(block->acceleration*
    ((RANADE_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
  block->d_next = ceil((block->millimeters*RANADE_MULTIPLIER)/block->step_event_count); // (mult*mm/step)
  
  // Compute direction bits. Bit enabled always means direction is negative.
  block->direction_bits = 0;
  if (unit_vec[X_AXIS] < 0) { block->direction_bits |= (1<<X_DIRECTION_BIT); }
  if (unit_vec[Y_AXIS] < 0) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
  if (unit_vec[Z_AXIS] < 0) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }

  // 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.
  // NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path 
  // mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact
  // stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here
  // is exactly the same. Instead of motioning all the way to junction point, the machine will
  // just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform
  // a continuous mode path, but ARM-based microcontrollers most certainly do.

  // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
  block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED;
  if ((block_buffer_head != block_buffer_tail) && (pl.previous_nominal_speed_sqr > 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.
    float cos_theta = - pl.previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] 
                      - pl.previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] 
                      - pl.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) {
      block->max_entry_speed_sqr = min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr);
      // 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
        float sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
        block->max_entry_speed_sqr = min(block->max_entry_speed_sqr,
           block->acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2));
      }
    }
  }  

  // Initialize block entry speed. Compute block entry velocity backwards from user-defined MINIMUM_PLANNER_SPEED.
  // TODO: This could be moved to the planner recalculate function.
  block->entry_speed_sqr = min( block->max_entry_speed_sqr,
      MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*block->acceleration*block->millimeters);

  // Set new block to be recalculated for conversion to stepper data.
  block->recalculate_flag = true;

  // Update previous path unit_vector and nominal speed (squared)
  memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[]
  pl.previous_nominal_speed_sqr = block->nominal_speed_sqr;
    
  // Update planner position
  memcpy(pl.position, target, sizeof(target)); // pl.position[] = target[]
  pl.last_x = x;
  pl.last_y = y;
  pl.last_z = z;

  // Update buffer head and next buffer head indices
  block_buffer_head = next_buffer_head;  
  next_buffer_head = next_block_index(block_buffer_head);

  planner_recalculate(); 
}
void plan_set_e_position(const float &e)
{
  position[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);  
  st_set_e_position(position[E_AXIS]);
}
Esempio n. 7
0
static void Sampled_getSum2AndDefinitionRange
	(Sampled me, double xmin, double xmax, long ilevel, int unit, double mean, bool interpolate, double *return_sum2, double *return_definitionRange)
{
	/*
		This function computes the area under the linearly interpolated squared difference curve between xmin and xmax.
		Outside [x1-dx/2, xN+dx/2], the curve is undefined and neither times nor values are counted.
		In [x1-dx/2,x1] and [xN,xN+dx/2], the curve is linearly extrapolated.
	*/
	long imin, imax;
	double sum2 = 0.0, definitionRange = 0.0;
	Function_unidirectionalAutowindow (me, & xmin, & xmax);
	if (Function_intersectRangeWithDomain (me, & xmin, & xmax)) {
		if (interpolate) {
			if (Sampled_getWindowSamples (me, xmin, xmax, & imin, & imax)) {
				double leftEdge = my x1 - 0.5 * my dx, rightEdge = leftEdge + my nx * my dx;
				for (long isamp = imin; isamp <= imax; isamp ++) {
					double value = my v_getValueAtSample (isamp, ilevel, unit);   // a fast way to integrate a linearly interpolated curve; works everywhere except at the edges
					if (NUMdefined (value)) {
						value -= mean;
						value *= value;
						definitionRange += 1.0;
						sum2 += value;
					}
				}
				/*
				 * Corrections within the first and last sampling intervals.
				 */
				if (xmin > leftEdge) {   // otherwise, constant extrapolation over 0.5 sample is OK
					double phase = (my x1 + (imin - 1) * my dx - xmin) / my dx;   // this fraction of sampling interval is still to be determined
					double rightValue = Sampled_getValueAtSample (me, imin, ilevel, unit);
					double leftValue = Sampled_getValueAtSample (me, imin - 1, ilevel, unit);
					if (NUMdefined (rightValue)) {
						rightValue -= mean;
						rightValue *= rightValue;
						definitionRange -= 0.5;   // delete constant extrapolation over 0.5 sample
						sum2 -= 0.5 * rightValue;
						if (NUMdefined (leftValue)) {
							leftValue -= mean;
							leftValue *= leftValue;
							definitionRange += phase;   // add current fraction
							sum2 += phase * (rightValue + 0.5 * phase * (leftValue - rightValue));   // interpolate to outside sample
						} else {
							if (phase > 0.5) phase = 0.5;
							definitionRange += phase;   // add current fraction, but never more than 0.5
							sum2 += phase * rightValue;
						}
					} else if (NUMdefined (leftValue) && phase > 0.5) {
						leftValue -= mean;
						leftValue *= leftValue;
						definitionRange += phase - 0.5;
						sum2 += (phase - 0.5) * leftValue;
					}
				}
				if (xmax < rightEdge) {   // otherwise, constant extrapolation is OK
					double phase = (xmax - (my x1 + (imax - 1) * my dx)) / my dx;   // this fraction of sampling interval is still to be determined
					double leftValue = Sampled_getValueAtSample (me, imax, ilevel, unit);
					double rightValue = Sampled_getValueAtSample (me, imax + 1, ilevel, unit);
					if (NUMdefined (leftValue)) {
						leftValue -= mean;
						leftValue *= leftValue;
						definitionRange -= 0.5;   // delete constant extrapolation over 0.5 sample
						sum2 -= 0.5 * leftValue;
						if (NUMdefined (rightValue)) {
							rightValue -= mean;
							rightValue *= rightValue;
							definitionRange += phase;   // add current fraction
							sum2 += phase * (leftValue + 0.5 * phase * (rightValue - leftValue));   // interpolate to outside sample
						} else {
							if (phase > 0.5) phase = 0.5;
							definitionRange += phase;   // add current fraction, but never more than 0.5
							sum2 += phase * leftValue;
						}
					} else if (NUMdefined (rightValue) && phase > 0.5) {
						rightValue -= mean;
						rightValue *= rightValue;
						definitionRange += phase - 0.5;
						sum2 += (phase - 0.5) * rightValue;
					}
				}
			} else {   // no sample centres between xmin and xmax
				/*
				 * Try to return the mean of the interpolated values at these two points.
				 * Thus, a small (xmin, xmax) range gives the same value as the (xmin+xmax)/2 point.
				 */
				double leftValue = Sampled_getValueAtSample (me, imax, ilevel, unit);
				double rightValue = Sampled_getValueAtSample (me, imin, ilevel, unit);
				double phase1 = (xmin - (my x1 + (imax - 1) * my dx)) / my dx;
				double phase2 = (xmax - (my x1 + (imax - 1) * my dx)) / my dx;
				if (imin == imax + 1) {   // not too far from sample definition region
					if (NUMdefined (leftValue)) {
						leftValue -= mean;
						leftValue *= leftValue;
						if (NUMdefined (rightValue)) {
							rightValue -= mean;
							rightValue *= rightValue;
							definitionRange += phase2 - phase1;
							sum2 += (phase2 - phase1) * (leftValue + 0.5 * (phase1 + phase2) * (rightValue - leftValue));
						} else if (phase1 < 0.5) {
							if (phase2 > 0.5) phase2 = 0.5;
							definitionRange += phase2 - phase1;
							sum2 += (phase2 - phase1) * leftValue;
						}
					} else if (NUMdefined (rightValue) && phase2 > 0.5) {
						rightValue -= mean;
						rightValue *= rightValue;
						if (phase1 < 0.5) phase1 = 0.5;
						definitionRange += phase2 - phase1;
						sum2 += (phase2 - phase1) * rightValue;
					}
				}
			}
		} else {   // no interpolation
			double rimin = Sampled_xToIndex (me, xmin), rimax = Sampled_xToIndex (me, xmax);
			if (rimax >= 0.5 && rimin < my nx + 0.5) {
				imin = rimin < 0.5 ? 0 : lround (rimin);
				imax = rimax >= my nx + 0.5 ? my nx + 1 : lround (rimax);
				for (long isamp = imin + 1; isamp < imax; isamp ++) {
					double value = my v_getValueAtSample (isamp, ilevel, unit);
					if (NUMdefined (value)) {
						value -= mean;
						value *= value;
						definitionRange += 1.0;
						sum2 += value;
					}
				}
				if (imin == imax) {
					double value = my v_getValueAtSample (imin, ilevel, unit);
					if (NUMdefined (value)) {
						double phase = rimax - rimin;
						value -= mean;
						value *= value;
						definitionRange += phase;
						sum2 += phase * value;
					}
				} else {
					if (imin >= 1) {
						double value = my v_getValueAtSample (imin, ilevel, unit);
						if (NUMdefined (value)) {
							double phase = imin - rimin + 0.5;
							value -= mean;
							value *= value;
							definitionRange += phase;
							sum2 += phase * value;
						}
					}
					if (imax <= my nx) {
						double value = my v_getValueAtSample (imax, ilevel, unit);
						if (NUMdefined (value)) {
							double phase = rimax - imax + 0.5;
							value -= mean;
							value *= value;
							definitionRange += phase;
							sum2 += phase * value;
						}
					}
				}
			}
		}
	}
	if (return_sum2) *return_sum2 = sum2;
	if (return_definitionRange) *return_definitionRange = definitionRange;
}
Esempio n. 8
0
// Homes the specified cycle axes, sets the machine position, and performs a pull-off motion after
// completing. Homing is a special motion case, which involves rapid uncontrolled stops to locate
// the trigger point of the limit switches. The rapid stops are handled by a system level axis lock
// mask, which prevents the stepper algorithm from executing step pulses. Homing motions typically
// circumvent the processes for executing motions in normal operation.
// NOTE: Only the abort runtime command can interrupt this process.
void limits_go_home(uint8_t cycle_mask)
{
  if (sys.abort) { return; } // Block if system reset has been issued.

  // Initialize homing in search mode to quickly engage the specified cycle_mask limit switches.
  bool approach = true;
  float homing_rate = settings.homing_seek_rate;
  uint8_t invert_pin, idx;
  uint8_t n_cycle = (2*N_HOMING_LOCATE_CYCLE+1);
  float target[N_AXIS];

  // Determine travel distance to the furthest homing switch based on user max travel settings.
  // NOTE: settings.max_travel[] is stored as a negative value.
  float max_travel = settings.max_travel[X_AXIS];
  if (max_travel > settings.max_travel[Y_AXIS]) { max_travel = settings.max_travel[Y_AXIS]; }
  if (max_travel > settings.max_travel[Z_AXIS]) { max_travel = settings.max_travel[Z_AXIS]; }
  max_travel *= -HOMING_AXIS_SEARCH_SCALAR; // Ensure homing switches engaged by over-estimating max travel.

  plan_reset(); // Reset planner buffer to zero planner current position and to clear previous motions.

  do {
    // Initialize invert_pin boolean based on approach and invert pin user setting.
    if (bit_isfalse(settings.flags,BITFLAG_INVERT_LIMIT_PINS)) { invert_pin = approach; }
    else { invert_pin = !approach; }

    // Set target location and rate for active axes.
    uint8_t n_active_axis = 0;
    for (idx=0; idx<N_AXIS; idx++) {
      if (bit_istrue(cycle_mask,bit(idx))) {
        n_active_axis++;
        if (!approach) { target[idx] = -max_travel; }
        else { target[idx] = max_travel; }
      } else {
        target[idx] = 0.0;
      }
    }
    if (bit_istrue(settings.homing_dir_mask,(1<<X_DIRECTION_BIT))) { target[X_AXIS] = -target[X_AXIS]; }
    if (bit_istrue(settings.homing_dir_mask,(1<<Y_DIRECTION_BIT))) { target[Y_AXIS] = -target[Y_AXIS]; }
    if (bit_istrue(settings.homing_dir_mask,(1<<Z_DIRECTION_BIT))) { target[Z_AXIS] = -target[Z_AXIS]; }

    homing_rate *= sqrt(n_active_axis); // [sqrt(N_AXIS)] Adjust so individual axes all move at homing rate.

      // Reset homing axis locks based on cycle mask.
    uint8_t axislock = 0;
    if (bit_istrue(cycle_mask,bit(X_AXIS))) { axislock |= (1<<X_STEP_BIT); }
    if (bit_istrue(cycle_mask,bit(Y_AXIS))) { axislock |= (1<<Y_STEP_BIT); }
    if (bit_istrue(cycle_mask,bit(Z_AXIS))) { axislock |= (1<<Z_STEP_BIT); }
    sys.homing_axis_lock = axislock;

    // Perform homing cycle. Planner buffer should be empty, as required to initiate the homing cycle.
    uint8_t limit_state;
    #ifdef USE_LINE_NUMBERS
    plan_buffer_line(target, homing_rate, false, HOMING_CYCLE_LINE_NUMBER); // Bypass mc_line(). Directly plan homing motion.
    #else
    plan_buffer_line(target, homing_rate, false); // Bypass mc_line(). Directly plan homing motion.
    #endif
    st_prep_buffer(); // Prep and fill segment buffer from newly planned block.
    st_wake_up(); // Initiate motion
    do {
      // Check limit state. Lock out cycle axes when they change.
      limit_state = LIMIT_PIN;
      if (invert_pin) { limit_state ^= LIMIT_MASK; }
      if (axislock & (1<<X_STEP_BIT)) {
        if (limit_state & (1<<X_LIMIT_BIT)) { axislock &= ~(1<<X_STEP_BIT); }
      }
      if (axislock & (1<<Y_STEP_BIT)) {
        if (limit_state & (1<<Y_LIMIT_BIT)) { axislock &= ~(1<<Y_STEP_BIT); }
      }
      if (axislock & (1<<Z_STEP_BIT)) {
        if (limit_state & (1<<Z_LIMIT_BIT)) { axislock &= ~(1<<Z_STEP_BIT); }
      }
      sys.homing_axis_lock = axislock;
      st_prep_buffer(); // Check and prep segment buffer. NOTE: Should take no longer than 200us.
      // Check only for user reset. No time to run protocol_execute_runtime() in this loop.
      if (sys.execute & EXEC_RESET) { protocol_execute_runtime(); return; }
    } while (STEP_MASK & axislock);

    st_reset(); // Immediately force kill steppers and reset step segment buffer.
    plan_reset(); // Reset planner buffer. Zero planner positions. Ensure homing motion is cleared.

    delay_ms(settings.homing_debounce_delay); // Delay to allow transient dynamics to dissipate.

    // Reverse direction and reset homing rate for locate cycle(s).
    homing_rate = settings.homing_feed_rate;
    approach = !approach;

  } while (n_cycle-- > 0);

  // The active cycle axes should now be homed and machine limits have been located. By
  // default, grbl defines machine space as all negative, as do most CNCs. Since limit switches
  // can be on either side of an axes, check and set axes machine zero appropriately. Also,
  // set up pull-off maneuver from axes limit switches that have been homed. This provides
  // some initial clearance off the switches and should also help prevent them from falsely
  // triggering when hard limits are enabled or when more than one axes shares a limit pin.
  for (idx=0; idx<N_AXIS; idx++) {
    // Set up pull off targets and machine positions for limit switches homed in the negative
    // direction, rather than the traditional positive. Leave non-homed positions as zero and
    // do not move them.
    // NOTE: settings.max_travel[] is stored as a negative value.
    if (cycle_mask & bit(idx)) {
      if ( settings.homing_dir_mask & get_direction_mask(idx) ) {
        target[idx] = settings.homing_pulloff+settings.max_travel[idx];
        sys.position[idx] = lround(settings.max_travel[idx]*settings.steps_per_mm[idx]);
      } else {
        target[idx] = -settings.homing_pulloff;
        sys.position[idx] = 0;
      }
    } else { // Non-active cycle axis. Set target to not move during pull-off.
      target[idx] = (float)sys.position[idx]/settings.steps_per_mm[idx];
    }
  }
  plan_sync_position(); // Sync planner position to current machine position for pull-off move.
  #ifdef USE_LINE_NUMBERS
  plan_buffer_line(target, settings.homing_seek_rate, false, HOMING_CYCLE_LINE_NUMBER); // Bypass mc_line(). Directly plan motion.
  #else
  plan_buffer_line(target, settings.homing_seek_rate, false); // Bypass mc_line(). Directly plan motion.
  #endif

  // Initiate pull-off using main motion control routines.
  // TODO : Clean up state routines so that this motion still shows homing state.
  sys.state = STATE_QUEUED;
  sys.execute |= EXEC_CYCLE_START;
  protocol_execute_runtime();
  protocol_buffer_synchronize(); // Complete pull-off motion.

  // Set system state to homing before returning.
  sys.state = STATE_HOMING;
}
Esempio n. 9
0
int fton_fton(t_fton *x, double f)
{
	char *notes[12];
	if(x->accdntls == 1){
		char *sharps[] = {"C", "C#", "D", "D#", "E", "F", "F#", "G", "G#", "A", "A#", "B"};
		memcpy(notes, sharps, 12 * sizeof(char *));
	} else if (x->accdntls == 0) {
		char *flats[] = {"C", "Db", "D", "Eb", "E", "F", "Gb", "G", "Ab", "A", "Bb", "B"};
		memcpy(notes, flats, 12 * sizeof(char *));
	} else {
		object_error((t_object *)x, "accidental mode can be 0: flats or 1: sharps only");
		return 0;
	}
	
	double mCents, cents;
	int mNote, oct;
	char *roundCents, *noteSym;
	t_atom outList[2];
	
	if(f){
		mCents = 69 + (12 * log2(fabs(f)/x->a4));
	
		switch (x->centMode) {
			case 1:
				if(mCents > 0){
					asprintf(&roundCents, "%0.5f", mCents);
					mNote = (int)trunc(atof(roundCents) + 0.5);
				} else {
					asprintf(&roundCents, "%0.5f", mCents);
					mNote = (int)trunc(atof(roundCents) - 0.5);
				}
				//post("%d", mNote);
				if(mNote >= 0){
					oct = (mNote / 12) - 1;
					asprintf(&noteSym, "%s%d", notes[mNote%12], oct);
				} else {
					oct = ((mNote+1) / 12) - 2;
					asprintf(&noteSym, "%s%d", notes[(288+mNote)%12], oct);	
				}
//				post("%s", noteSym);
				atom_setsym(outList, gensym(noteSym));

				cents = 100 * (mCents - mNote);
				if (fabs(cents) < 0.001)//round off error tweak
					cents = 0;
				
				char *centSym;
				if(cents >= 0){
					asprintf(&centSym, "%+.2lf", (double)(lround(cents * 1000))/1000.);
				//	post("%s %f", centSym, cents);
					atom_setsym(outList+1, gensym(centSym));
				} else {
					asprintf(&centSym, "%+.2lf", (double)(lround(cents * 1000))/1000.);
				//	post("%s %f", centSym, cents);
					atom_setfloat(outList+1, (float)atof(centSym));
				}
				
				free(centSym);
				break;
			case 0:
				asprintf(&roundCents, "%0.5f", mCents);
				mNote = (int)trunc(atof(roundCents));

				if(mNote >= 0){
					oct = (mNote / 12) - 1;
					asprintf(&noteSym, "%s%d", notes[mNote%12], oct);
				} else {
					oct = ((mNote+1) / 12) - 2;
					asprintf(&noteSym, "%s%d", notes[(288+mNote)%12], oct);	
				}
				atom_setsym(outList, gensym(noteSym));
				
				cents = 100 * (mCents - mNote);
				if (fabs(cents) < 0.001)//round off error tweak
					cents = 0;
				atom_setfloat(outList+1, cents);
				break;
			default:
				break;
		}
		critical_enter(x->lock);
		memcpy(x->n, outList, 2 * sizeof(t_atom));
		critical_exit(x->lock);
		free(noteSym);
		free(roundCents);
		return 1;
	} else {
		outlet_anything(x->outlet, gensym("zero"), 0, NULL);
		free(noteSym);
		free(roundCents);
		return 0;
	}
}
Esempio n. 10
0
static void SetValues(bool first=false) {
  WndProperty* wp;

  wp = (WndProperty*)wf->FindByName(TEXT("prpTaskFinishLine"));
  if (wp) {
    DataFieldEnum* dfe;
    dfe = (DataFieldEnum*)wp->GetDataField();
    if (first) {
	// LKTOKEN  _@M210_ = "Cylinder" 
      dfe->addEnumText(gettext(TEXT("_@M210_")));
	// LKTOKEN  _@M393_ = "Line" 
      dfe->addEnumText(gettext(TEXT("_@M393_")));
	// LKTOKEN  _@M274_ = "FAI Sector" 
      dfe->addEnumText(gettext(TEXT("_@M274_")));
    }
    dfe->Set(FinishLine);
    wp->RefreshDisplay();
  }

  wp = (WndProperty*)wf->FindByName(TEXT("prpTaskFinishRadius"));
  if (wp) {
    wp->GetDataField()->SetAsFloat(lround(FinishRadius*DISTANCEMODIFY*DISTANCE_ROUNDING)/DISTANCE_ROUNDING);
    wp->GetDataField()->SetUnits(Units::GetDistanceName());
    wp->RefreshDisplay();
  }

  wp = (WndProperty*)wf->FindByName(TEXT("prpTaskStartLine"));
  if (wp) {
    DataFieldEnum* dfe;
    dfe = (DataFieldEnum*)wp->GetDataField();
    if (first) {
	// LKTOKEN  _@M210_ = "Cylinder" 
      dfe->addEnumText(gettext(TEXT("_@M210_")));
	// LKTOKEN  _@M393_ = "Line" 
      dfe->addEnumText(gettext(TEXT("_@M393_")));
	// LKTOKEN  _@M274_ = "FAI Sector" 
      dfe->addEnumText(gettext(TEXT("_@M274_")));
    }
    dfe->SetDetachGUI(true); // disable call to OnAATEnabled
    dfe->Set(StartLine);
    dfe->SetDetachGUI(false);
    wp->RefreshDisplay();
  }

  wp = (WndProperty*)wf->FindByName(TEXT("prpTaskStartRadius"));
  if (wp) {
    wp->GetDataField()->SetAsFloat(lround(StartRadius*DISTANCEMODIFY*DISTANCE_ROUNDING)/DISTANCE_ROUNDING);
    wp->GetDataField()->SetUnits(Units::GetDistanceName());
    wp->RefreshDisplay();
  }

  wp = (WndProperty*)wf->FindByName(TEXT("prpTaskFAISector"));
  if (wp) {
    // 110223 CAN ANYONE PLEASE CHECK WHAT THE HACK IS A BOOL FOR BILL GATES? BECAUSE IF FALSE IS -1 THEN
    // WE HAVE MANY PROBLEMS! I THINK IT IS TIME TO GO BACK TO bool AND GET RID OF MS BOOLS!!
    wp->SetVisible(AATEnabled==0);
    DataFieldEnum* dfe;
    dfe = (DataFieldEnum*)wp->GetDataField();
    if (first) {
	// LKTOKEN  _@M210_ = "Cylinder" 
      dfe->addEnumText(gettext(TEXT("_@M210_")));
	// LKTOKEN  _@M274_ = "FAI Sector" 
      dfe->addEnumText(gettext(TEXT("_@M274_")));
      dfe->addEnumText(gettext(TEXT("DAe 0.5/10")));
    }
    dfe->SetDetachGUI(true); // disable call to OnAATEnabled
    dfe->Set(SectorType);
    dfe->SetDetachGUI(false);
    wp->RefreshDisplay();
  }

  wp = (WndProperty*)wf->FindByName(TEXT("prpTaskSectorRadius"));
  if (wp) {
    wp->SetVisible(AATEnabled==0);
    wp->GetDataField()->SetAsFloat(lround(SectorRadius*DISTANCEMODIFY*DISTANCE_ROUNDING)/DISTANCE_ROUNDING);
    wp->GetDataField()->SetUnits(Units::GetDistanceName());
    wp->RefreshDisplay();
  }

  wp = (WndProperty*)wf->FindByName(TEXT("prpAutoAdvance"));
  if (wp) {
    DataFieldEnum* dfe;
    dfe = (DataFieldEnum*)wp->GetDataField();
    if (first) {
	// LKTOKEN  _@M418_ = "Manual" 
      dfe->addEnumText(gettext(TEXT("_@M418_")));
	// LKTOKEN _@M897_ "Auto"
      dfe->addEnumText(gettext(TEXT("_@M897_")));
	// LKTOKEN  _@M97_ = "Arm" 
      dfe->addEnumText(gettext(TEXT("_@M97_")));
	// LKTOKEN  _@M96_ = "Arm start" 
      dfe->addEnumText(gettext(TEXT("_@M96_")));
    }
    dfe->Set(AutoAdvance);
    wp->RefreshDisplay();
  }

  wp = (WndProperty*)wf->FindByName(TEXT("prpMinTime"));
  if (wp) {
    wp->SetVisible(AATEnabled>0 && !ISPARAGLIDER);
    wp->GetDataField()->SetAsFloat(AATTaskLength);
    wp->RefreshDisplay();
  }

  wp = (WndProperty*)wf->FindByName(TEXT("prpEnableMultipleStartPoints"));
  if (wp) {
    wp->SetVisible(!ISPARAGLIDER);
    wp->GetDataField()->Set(EnableMultipleStartPoints);
    wp->RefreshDisplay();
  }

  wp = (WndProperty*)wf->FindByName(TEXT("prpAATEnabled"));
  if (wp) {
	if (ISPARAGLIDER) {
		wp->SetVisible(false);
		AATEnabled=TRUE;
		wp->RefreshDisplay(); 
	} else {
		bool aw = (AATEnabled != 0);
		wp->GetDataField()->Set(aw);
		wp->RefreshDisplay(); 
	}
  }

  WndButton* wb;
  wb = (WndButton *)wf->FindByName(TEXT("EditStartPoints"));
  if (wb) {
    wb->SetVisible(EnableMultipleStartPoints!=0 && !ISPARAGLIDER);
  }

}
Esempio n. 11
0
bool SonarMap::convertToCell(const double m_x, const double m_y, int &c_x, int &c_y) const
{
  c_x = lround(m_x / resolution_);
  c_y = lround(m_y / resolution_);
  return (map_.isInX(c_x) && map_.isInY(c_y));
}
Esempio n. 12
0
size_t hundredNanoSecondsToFrames( ::REFERENCE_TIME timeHundredNs, size_t sampleRate )
{
	return lround( (double)sampleRate * (double)timeHundredNs * 0.0000001 );
}
Esempio n. 13
0
void LongSound_concatenate (SoundAndLongSoundList me, MelderFile file, int audioFileType, int numberOfBitsPerSamplePoint) {
	try {
		long sampleRate, n;   /* Integer sampling frequencies only, because of possible rounding errors. */
		int numberOfChannels;
		if (my size < 1) Melder_throw (U"No Sound or LongSound objects to concatenate.");
		/*
		 * The sampling frequencies and numbers of channels must be equal for all (long)sounds.
		 */
		Sampled data = my at [1];
		if (data -> classInfo == classSound) {
			Sound sound = (Sound) data;
			sampleRate = lround (1.0 / sound -> dx);
			numberOfChannels = sound -> ny;
			n = sound -> nx;
		} else {
			LongSound longSound = (LongSound) data;
			sampleRate = longSound -> sampleRate;
			numberOfChannels = longSound -> numberOfChannels;
			n = longSound -> nx;
		}
		/*
		 * Check whether all the sampling frequencies and channels match.
		 */
		for (long i = 2; i <= my size; i ++) {
			int sampleRatesMatch, numbersOfChannelsMatch;
			data = my at [i];
			if (data -> classInfo == classSound) {
				Sound sound = (Sound) data;
				sampleRatesMatch = round (1.0 / sound -> dx) == sampleRate;
				numbersOfChannelsMatch = sound -> ny == numberOfChannels;
				n += sound -> nx;
			} else {
				LongSound longSound = (LongSound) data;
				sampleRatesMatch = longSound -> sampleRate == sampleRate;
				numbersOfChannelsMatch = longSound -> numberOfChannels == numberOfChannels;
				n += longSound -> nx;
			}
			if (! sampleRatesMatch)
				Melder_throw (U"Sampling frequencies are not equal.");
			if (! numbersOfChannelsMatch)
				Melder_throw (U"Cannot mix stereo and mono.");
		}
		/*
		 * Create output file and write header.
		 */
		autoMelderFile mfile = MelderFile_create (file);
		if (file -> filePointer) {
			MelderFile_writeAudioFileHeader (file, audioFileType, sampleRate, n, numberOfChannels, numberOfBitsPerSamplePoint);
		}
		for (long i = 1; i <= my size; i ++) {
			data = my at [i];
			if (data -> classInfo == classSound) {
				Sound sound = (Sound) data;
				if (file -> filePointer) {
					MelderFile_writeFloatToAudio (file, sound -> ny, Melder_defaultAudioFileEncoding (audioFileType, numberOfBitsPerSamplePoint),
						sound -> z, sound -> nx, true);
				}
			} else {
				LongSound longSound = (LongSound) data;
				writePartToOpenFile (longSound, audioFileType, 1, longSound -> nx, file, 0, numberOfBitsPerSamplePoint);
			}
		}
		MelderFile_writeAudioFileTrailer (file, audioFileType, sampleRate, n, numberOfChannels, numberOfBitsPerSamplePoint);
		mfile.close ();
	} catch (MelderError) {
		Melder_throw (U"Sounds not concatenated and not saved to ", file, U".");
	}
}
Esempio n. 14
0
void PiqslImageView::imageUpdated(int x, int y, int w, int h)
{
    int zoom = lround(m_zoom);
    update(lround(m_tlPos.x()) + zoom*x, lround(m_tlPos.y()) + zoom*y,
            zoom*w, zoom*h);
}
Esempio n. 15
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();
}
Esempio n. 16
0
/*Mainloop*/
void loop() {
	/*Auswertung des anstehenden Aufgaben im Scheduler*/
	int i; unsigned char new_value = 0; unsigned char new_heading = 0; 
	for (i = 0; i < MAX_TASKS; i++) {
		if (scheduler[i].task == NOTHING || get_current_millis() < scheduler[i].tv_msec) {continue;}
		switch (scheduler[i].task) {
			case NOTHING: break;
			case MEASURE:
				/*Fordert den als Parameter uebergebenen Sensor auf eine Messung zu starten und bestimmt den Lese-Zeitpunkt*/			
				srf[scheduler[i].param - SE0_ADDRESS].measure();
				schedule_add((long)srf_speed[scheduler[i].param - SE0_ADDRESS], READ_MEASURE, scheduler[i].param);
				scheduler[i].task = NOTHING;
				break;
			case READ_MEASURE: {
				unsigned short index = scheduler[i].param - SE0_ADDRESS;
				/*Liest die Messdaten des als Parameter uebergebenen Sensors aus und veranlasst erneute Messung*/
				srf[index].read_it(get_current_millis());
				schedule_add((long)srf[index].get_delay(), MEASURE, scheduler[i].param);
				if(state == HOLD_STILL || state == MEASURE_ENVIRONMENT || state == GET_ANCHOR) {
					nvalue[index] = 1;
				}
				/*if (state == HOLD_STILL || state == HEAD_TO_MIDDLE || state = HTM_DELAY) {
					nvalue2[scheduler[i].param - SE0_ADDRESS] = 1;
				}*/
				new_value = scheduler[i].param;
				
				/*Korrektur der Messwerte bei Schieflage des Copters bei glaubhaften Winkeln*/
				if ((index == 0 || index == 1) && (abs(current_roll_rad) < MAX_ROLL_ANGLE))		srf[index].set_mean((unsigned short)(srf[index].get_mean()*cos(current_roll_rad)));
				else if ((index == 2 || index == 3) && (abs(current_pitch_rad) < MAX_PITCH_ANGLE))	srf[index].set_mean((unsigned short)(srf[index].get_mean()*cos(current_roll_rad)));
				
				/*Korrektur der Messwerte, wenn sich ein Sensor im Nahbereich (< SE_MIN_DISTANCE) befindet*/
				static unsigned char 	rm_state[SE_COUNT];
				static short		rm_min[SE_COUNT];
				if (state != IDLE && state != INIT && state != DELAY) {
					if (rm_state[index] == 0 && srf[index].get_mean() < SE_MIN_DISTANCE) {
						rm_state[index] = 1;
						if (index == 0 || index == 2)	rm_min[index] = srf[index+1].get_mean() - SE_MIN_DIFF;
						else				rm_min[index] = srf[index-1].get_mean() - SE_MIN_DIFF;
						srf[index].set_mean(SE_MIN);
					} else if (rm_state[index] == 1) {
						if ((index == 0 || index == 2) && srf[index+1].get_mean() < rm_min[index]) {
							rm_state[index] = 0;
						} else if ((index == 1 || index == 3) && srf[index-1].get_mean() < rm_min[index]) {
							rm_state[index] = 0;
						} else {
							srf[index].set_mean(SE_MIN);
						}
					}
				}
				if (rm_state[0] == 1 && rm_state[1] == 1) {rm_state[0] = 0; rm_state[1] = 0;}
				if (rm_state[2] == 1 && rm_state[3] == 1) {rm_state[2] = 0; rm_state[3] = 0;}
				
				if (state == HOLD_STILL || state ==  GET_ANCHOR || state == HTM_DELAY) {
					if (nvalue2[0] == 1 && nvalue2[1] == 1 && nvalue2[2] == 1 && nvalue2[3] == 1) {
						slam_insert_v((srf[0].get_cm_per_s() + srf[1].get_cm_per_s())/2,(srf[2].get_cm_per_s() + srf[3].get_cm_per_s())/2,current_heading,get_current_millis());
						nvalue2[0] = 0; nvalue2[1] = 0; nvalue2[2] = 0; nvalue2[3] = 0;
					}
				}
				
				/*Speichert den gelesenen Messwert in der entsprechenden Log-Datei*/
				#if LOG > 0
				FILE *fd;
				if (scheduler[i].param == SE0_ADDRESS) 	fd = fd_112;
				else if (scheduler[i].param == SE1_ADDRESS) 	fd = fd_113;
				else if (scheduler[i].param == SE2_ADDRESS) 	fd = fd_114;
				else						fd = fd_115;
				fprintf(fd,"%lu\t%u\t%u\n", srf[scheduler[i].param - SE0_ADDRESS].get_msec(), srf[scheduler[i].param - SE0_ADDRESS].get_data(), srf[scheduler[i].param - SE0_ADDRESS].get_mean());
				#endif
				scheduler[i].task = NOTHING;
				break;
			}
			case SAVE_LOG:
				/*Sichert die bisher geloggten Daten*/
				#if LOG > 0
				fclose(fd_112);
				fclose(fd_113);
				fclose(fd_114);
				fclose(fd_115);
				fclose(fd_data);
				char tmp_dir[32]; strcpy(tmp_dir,log_dir);
				fd_112 = fopen(strcat(tmp_dir,"/112"), "a"); strcpy(tmp_dir,log_dir);
				fd_113 = fopen(strcat(tmp_dir,"/113"), "a"); strcpy(tmp_dir,log_dir);
				fd_114 = fopen(strcat(tmp_dir,"/114"), "a"); strcpy(tmp_dir,log_dir);
				fd_115 = fopen(strcat(tmp_dir,"/115"), "a"); strcpy(tmp_dir,log_dir);
				fd_data= fopen(strcat(tmp_dir,"/data"),"a"); strcpy(tmp_dir,log_dir);
				schedule_add(LOG_SPEED,SAVE_LOG,0);
				#endif
				scheduler[i].task = NOTHING;
				break;   
			case CHECK_STILL:
				/*Prueft auf Stillstand des Copters*/
				if (still && state == HOLD_STILL) {
					if (scheduler[i].param < CHECK_STILL_COUNT) {
						schedule_add(CHECK_STILL_SPEED,CHECK_STILL,scheduler[i].param+1);
					} else {
						state = MEASURE_ENVIRONMENT;
						still = 0;
						schedule_add(CHECK_STILL_SPEED,CHECK_STILL, 0);
					}
				} else {
					schedule_add(CHECK_STILL_SPEED,CHECK_STILL,0);
				}
				scheduler[i].task = NOTHING;
				break;
			case CHANGE_STATE:
				/*Ändert den aktuellen Status zum als Parameter übergebenen Status*/
				state = (states)scheduler[i].param;
				if ((states)scheduler[i].param == HOLD_STILL) hs_state = 0;
				scheduler[i].task = NOTHING;
				break;
			case SHOW_ME: {
				/*Schreibt Messwerte und Neigungswinkel auf das Terminal*/
				char st[16];
				switch (state) {
					case INIT: 			sprintf(st, "INIT"); 		break;
					case MEASURE_ENVIRONMENT: 	sprintf(st, "ME"); 		break;
					case IDLE: 			sprintf(st, "IDLE"); 		break;
					case HOLD_STILL: 		sprintf(st, "HOLD_STILL"); 	break;
					case GET_ALIGNMENT: 		sprintf(st, "GET_ALIGNMENT"); 	break;
					case GET_ANCHOR: 		sprintf(st, "GET_ANCHOR"); 	break;
					case DELAY:			sprintf(st, "DELAY");		break;
					default: 			sprintf(st, "???"); 		break;
				}
					
				if (roll > 9 || roll < 0) 	printf("roll: %d\tpitch: %d\tyaw: %d\theading: %d\tdhead: %d\tSE0: %d\tSE1: %d\tSE2: %d\tSE3:%d\tstate: %s\n", roll, pitch, yaw, current_heading, desired_heading, srf[0].get_mean(), srf[1].get_mean(), srf[2].get_mean(), srf[3].get_mean(),st);
				else 				printf("roll: %d\t\tpitch: %d\tyaw: %d\theading: %d\tdhead. %d\tSE0: %d\tSE1: %d\tSE2: %d\tSE3:%d\tstate: %s\n", roll, pitch, yaw, current_heading, desired_heading, srf[0].get_mean(), srf[1].get_mean(), srf[2].get_mean(), srf[3].get_mean(),st);
				schedule_add(100,SHOW_ME,0);
				scheduler[i].task = NOTHING;
				
				/*Mavlinkkommunikation mit QGroundcontrol (basierend auf Based on http://qgroundcontrol.org/dev/mavlink_linux_integration_tutorial)*/
				mavlink_message_t msg;
				uint16_t len;
				uint8_t buf[MAVLINK_MAX_PACKET_LEN];
				
				static int i; i++;
				if (i == 10) {
					i = 0;
					mavlink_msg_heartbeat_pack(0, 0, &msg, 0, 0, 0, 0, 0);
					len = mavlink_msg_to_send_buffer(buf, &msg);
					sendto(s, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
				}
				mavlink_msg_debug_vect_pack(0,0,&msg,"DEBUG",0,roll,srf[0].get_mean(),srf[0].get_data());
				len = mavlink_msg_to_send_buffer(buf, &msg);
				sendto(s, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
				break;
			}
			default: if(WARNINGS){perror("LOOP: Unbekannte Aufgabe im Scheduler.");} break; 
		}
	}

	/*Auswertung der Daten, die an der seriellen Schnittstelle anliegen.*/
	struct timeval tv; tv.tv_sec = 0; tv.tv_usec = 0;
	/*Prueft, ob Daten an der seriellen Schnittstelle anliegen*/
	FD_ZERO(&tty_fdset);
	FD_SET(tty_fd, &tty_fdset);
	if (select(tty_fd+1, &tty_fdset, NULL, NULL, &tv) == -1) {perror("LOOP: select() fehlgeschlagen"); exit(1);}

	/*Liest ein einzelnes Byte von der seriellen Schnittstelle und prueft, ob ein Mavlinkpaket vervollstaendigt wurde*/
	if (FD_ISSET(tty_fd, &tty_fdset)) {
		static mavlink_message_t msg;
		static mavlink_status_t status;
		char c[1];
		if (read(tty_fd,c,1) == -1) {if(WARNINGS){perror("LOOP: Fehler beim Lesen aus " TTY_DEVICE);}}
		#if DEBUG_LEVEL > 2
		printf("%#x\n",c[0]);
		#endif
		if (mavlink_parse_char(MAVLINK_COMM_0,(uint8_t) *c, &msg, &status)) {
			#if DEBUG_LEVEL > 1
			printf("%u Mavlinkpaket empfangen: %d\n", get_current_millis(), msg.msgid);
			#endif
			//printf("%lu Mavlinkpaket empfangen: %d\n", get_current_millis(), msg.msgid);
			
			switch (msg.msgid) {
				case MAVLINK_MSG_ID_HEARTBEAT:
					/*Beim Empfang des ersten Heartbeats wird zunaechst ein eventuell vorhandener Datenstream gekuendigt und ein neuer Datenstream abonnemiert*/
					mavlink_heartbeat_t hb;
					mavlink_msg_heartbeat_decode(&msg,&hb);
					if (!first_heartbeat) {
						first_heartbeat = 1;
						request_data_stream(MAV_DATA_STREAM_ALL,0,0);
					}
					//if (state == IDLE) {
					if (state == IDLE && (hb.custom_mode == 13 /*EXT_CTRL*/ || desktop_build)) {
						std::cout << "State changed from IDLE to INIT.\n";
						state = INIT;
						schedule_add(2000,CHANGE_STATE,(int)HOLD_STILL);
						request_data_stream(MAV_DATA_STREAM_EXTRA2/*VFR_HUD*/,DATA_STREAM_SPEED,1);
						init_state = 1;
					}
					break;
				case MAVLINK_MSG_ID_VFR_HUD:
					/*Speichert bei Empfang von HUD-Daten die aktuelle Ausrichtung des Copters*/
					mavlink_vfr_hud_t hud_data;
					mavlink_msg_vfr_hud_decode(&msg, &hud_data);
					old_heading = current_heading;
					current_heading = hud_data.heading;
					new_heading = 1;
					break;
				case MAVLINK_MSG_ID_ATTITUDE:
					mavlink_attitude_t adata;
					mavlink_msg_attitude_decode(&msg,&adata);
					old_heading = current_heading;
					current_heading = ((short)lround(DEG(adata.yaw))+360)%360;
					current_pitch_rad = adata.pitch;
					current_roll_rad = adata.roll;
					new_heading = 1;
					break;
				case MAVLINK_MSG_ID_RAW_IMU:
					/*Speichert die Beschleunigungen auf der x- und y-Achse*/
					mavlink_raw_imu_t raw_imu;
					mavlink_msg_raw_imu_decode(&msg,&raw_imu);
					xacc = raw_imu.xacc;
					yacc = raw_imu.yacc;
					//slam_insert_acc(xacc,yacc,current_heading,get_current_millis());
					std::cout << "xacc: " << xacc << " yacc: " << yacc << "\n";
					break;
				case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
					/*Prüft, ob eine Umstellung auf externe Kontrolle erfolgt ist*/
					/*if (state != IDLE) {break;}
					mavlink_rc_channels_raw_t rc;
					mavlink_msg_rc_channels_raw_decode(&msg,&rc);
					if (desktop_build) init_state = 1; rc.chan5_raw = MODE_SWITCH_RANGE_UP-1;
					if (init_state && rc.chan5_raw < MODE_SWITCH_RANGE_UP && rc.chan5_raw > MODE_SWITCH_RANGE_DOWN) {
						std::cout << "State changed from IDLE to INIT.\n";
						schedule_add(0,CHANGE_STATE,(int)INIT);
						schedule_add(2000,CHANGE_STATE,(int)HOLD_STILL);
						request_data_stream(MAV_DATA_STREAM_RC_CHANNELS,0,0);
						request_data_stream(MAV_DATA_STREAM_EXTRA2,DATA_STREAM_SPEED,1);
						request_data_stream(MAV_DATA_STREAM_RAW_SENSORS,DATA_STREAM_SPEED,1);
					}
					if(rc.chan5_raw > MODE_SWITCH_RANGE_UP || rc.chan5_raw < MODE_SWITCH_RANGE_DOWN) {
						init_state = 1;
					}*/
					break;
				default: break;
			}
		}
	}		

	/*Berechnung der vorgeschlagenen Werte fuer roll, pitch und yaw an Hand der neuen Messwerte*/
	if (state != INIT && !new_value && !new_heading) {/*Wenn keine neuen Daten vorhanden sind -> Abarbeitung überspringen*/return;}
	switch (state) {	
		case IDLE: /*Wartet auf Aktivierung der externen Kontrolle*/	break;
		case INIT:
			std::cout << "INIT\n";
			/*Initialisiert Abarbeitung, wenn externe Kontrolle zum ersten Mal aktiviert wurde*/
			schedule_add(5000,CHECK_STILL,0);
			#if LOG > 0
			schedule_add(LOG_SPEED,SAVE_LOG,0);
			//schedule_add(SLAM_SPEED,SAVE_SLAM,0);
			#endif
			for (int i = 0; i < SE_COUNT; i++) {
				if ((int)pow(2,i) & ENABLED_SENSORS) {
					schedule_add(0, MEASURE, SE0_ADDRESS + i);
				}
			}
			slam_init(current_heading,get_current_millis());
			state = DELAY;
			break;
		case HOLD_STILL:
			if(1) {
				static short set_point[SE_COUNT];
				if (hs_state == 0) {
					hs_state = 1;
					set_point[0] = (srf[0].get_mean() + srf[1].get_mean())/2;
					set_point[1] = set_point[0];
					set_point[2] = (srf[2].get_mean() + srf[3].get_mean())/2;
					set_point[3] = set_point[2];
					//for (int i = 0; i < SE_COUNT; i++) {
					//	set_point[i] = srf[i].get_mean();
					//	std::cout << "Set_Point " << i << ":" << set_point[i] << "\n";
					//}
					/*for (int i = 0; i < SE_COUNT; i++) {
						if (set_point[i] < 100 || set_point[i] > 0) {
							if (i == 0 || i == 2) {
								set_point[i+1] -= (100 - set_point[i]);
							} else {
								set_point[i-1] -= (100 - set_point[i]);
							}
							set_point[i] = 100;
						}
					}*/
					for (int i = 0; i < SE_COUNT; i++) {
						std::cout << "Set_Point " << i << ":" << set_point[i] << "\n";
					}
				}
				/*Veranlasst den Copter auf Grundlage der Änderungen der Messwerte still zu stehen*/
				if (!new_value) break;
				if (nvalue[0] == 1 && nvalue[1] == 1) {
					if (srf[0].get_mean() == SE_MIN)	roll = between<short>(pid_roll.get(0 - srf[1].get_mean() + set_point[1],srf[1].get_msec_diff()),-max_roll,max_roll);
					else if (srf[1].get_mean() == SE_MIN) 	roll = between<short>(pid_roll.get(srf[0].get_mean() - set_point[0],srf[0].get_msec_diff()),-max_roll,max_roll);
					else 					roll = between<short>(pid_roll.get(((srf[0].get_mean() - set_point[0]) - (srf[1].get_mean() - set_point[1]))/2,(srf[0].get_msec_diff() + srf[1].get_msec_diff())/2),-max_roll,max_roll);
					nvalue[0] = 0; nvalue[1] = 0;
				}
				if (nvalue[2] == 1 && nvalue[3] == 1) {
					if (srf[2].get_mean() == SE_MIN)	pitch = between<short>(pid_pitch.get(0 - srf[3].get_mean() + set_point[3],srf[3].get_msec_diff()),-max_pitch,max_pitch);
					else if (srf[3].get_mean() == SE_MIN) 	pitch = between<short>(pid_pitch.get(srf[2].get_mean() - set_point[2],srf[2].get_msec_diff()),-max_pitch,max_pitch);
					else 					pitch = between<short>(pid_pitch.get(((srf[2].get_mean() - set_point[2]) - (srf[3].get_mean() - set_point[3]))/2,(srf[2].get_msec_diff() + srf[3].get_msec_diff())/2),-max_pitch,max_pitch);
					nvalue[2] = 0; nvalue[3] = 0;
				}
				yaw = 0;
				send_ext_ctrl();
				/*Prüfung auf Stillstand*/
				if (abs(roll) <= STILL_FAK_ROLL*max_roll/100 && abs(pitch) <= STILL_FAK_PITCH*max_pitch/100) {
					//still = 1; 
					still = 0; /*FIXME*/
				} else {
					still = 0;
				}
			}
		break;
		case MEASURE_ENVIRONMENT:
			/*Vermisst die Umgebung durch eine Drehung um 360/SE_COUNT Grad*/
			if (breakpoint == 1) {state = HOLD_STILL; hs_state= 0; break;}
			if (abs(xacc) > HOLD_STILL_MAX_XACC || abs(yacc) > HOLD_STILL_MAX_YACC) {/*Wurde der Copter zu stark bewegt: Abbruch*/state = HOLD_STILL; hs_state = 0; std::cout << "State changed to HOLD_STILL\n"; break;}
			if (first_heading == -1) {
				/*Speichert die anfaengliche Ausrichtung und veranlasst den Copter sich zu drehen*/
				std::cout << "State changed to MEASURE_ENVIRONMENT\n";
				first_heading = current_heading;
				roll = 0; pitch = 0; rotation = 0; yaw = rotation_angle_sign*CONST_YAW;
				send_ext_ctrl();
			}
			//if (new_value) slam_insert_measurement(srf[new_value-SE0_ADDRESS].get_mean(),(current_heading + (360/SE_COUNT)*(new_value-SE0_ADDRESS))%360);
			/*Berechnung der aktuellen Drehung*/
			if (new_heading) {
				if (abs(current_heading - old_heading) > 180/SE_COUNT)	rotation += current_heading - old_heading - sign(current_heading - old_heading)*360;
				else 							rotation += current_heading - old_heading;
				for(int i = 0; i < SE_COUNT; i++) {
					if(nvalue[i] == 1) {
						env[(current_heading + srf[i].get_alignment()*(360/SE_COUNT))%360] = srf[i].get_mean();
						nvalue[i] = 0;
					}
				}
				//if (abs(rotation) >= 360/SE_COUNT) {
				if (abs(rotation) >= rotation_angle) {
					/*Copter hat sich ausreichend gedreht, Abbruch der Vermessung*/
					//state = HEAD_TO_MIDDLE;
					state = GET_ALIGNMENT;
					roll = 0; pitch = 0; yaw = 0;
					send_ext_ctrl();
					rotation_angle_sign = sign(rotation);
				}
			}
			break;
		case GET_ALIGNMENT: {
			/*Bestimme für jeden Sensor die Minimalentfernung der letzten Drehung*/
			/*short min_v[SE_COUNT];
			short min_h[SE_COUNT];
			for (int i = 0; i < SE_COUNT; i++) {
				min_v[i] = 0;
				min_h[i] = 0;
			}*/
			short min_v = 0;
			for (int i = 0; i < rotation_angle; i++) {
				if (min_v < env[(first_heading + rotation_angle_sign*i + 360 + srf[align_se].get_alignment()*(360/SE_COUNT))%360]) {
					min_v = env[(first_heading + rotation_angle_sign*i + 360 + srf[align_se].get_alignment()*(360/SE_COUNT))%360];
					desired_heading = (first_heading + rotation_angle_sign*i + 360 + srf[align_se].get_alignment()*(360/SE_COUNT))%360;
				}
				/*for (int j = 0; j < SE_COUNT; j++) {
					if (min_v[j] < env[(first_heading + rotation_angle_sign*i + j*360/SE_COUNT + 360)%360]) {
						min_v[j] = env[(first_heading + rotation_angle_sign*i + j*360/SE_COUNT + 360)%360];
						min_h[j] = (first_heading + rotation_angle_sign*i + 360)%360;
					}
				}*/
			}
			//desired_heading = round((float)(min_h[0]+min_h[1]+min_h[2]+min_h[3])/4);
			/*Bestimmung der künftigen Drehrichtung*/
			for (int k = 0; k < rotation_angle/2; k++) {
				/*Wenn Minimum in der ersten Hälfte der Drehung gefunden wurde, Wechsel der Drehrichtung bzw. Abbruch*/
				if (desired_heading == (first_heading+rotation_angle_sign*k)%360) {
					rotation_angle_sign *= -1;
					if (init_state == 2) init_state = 3;
					break;
				}
			}
			/*Cleanup*/
			first_heading = -1; yaw = 0; pid_roll.reset(); pid_pitch.reset();
			for(int i = 0; i < 360; i++) env[i] = 0;
			
			if (init_state == 1) 	init_state = 2;
			if (init_state == 3) {
				state = GET_ANCHOR;
				pid_yaw.reset();
				pid_yaw.set(HTM_YAW_KP,HTM_YAW_TN,HTM_YAW_TV);
				pid_yaw.set_target(0);
				roll = 0; pitch = 0; yaw = 0;
				std::cout << "State changed to GET_ANCHOR\n";
				tv_old_heading = get_current_millis();
			} else {
				state = HOLD_STILL;
				hs_state = 0;
			}
			break;
		}
		case GET_ANCHOR:
			/*Copter versucht sich stabil auf ANCHOR_DISTANCE und desired_heading zu stellen*/
			if (new_heading) {
				short heading_diff;
				/*Berechnung der Differenz zwischen aktueller Ausrichtung und gewünschter Ausrichtung*/
				if (abs(desired_heading - current_heading) > 180) {
					heading_diff = desired_heading - current_heading - sign(desired_heading - current_heading)*360;
				} else {
					heading_diff = desired_heading - current_heading;
				}
				yaw = pid_yaw.get(heading_diff,get_current_millis() - tv_old_heading);
				tv_old_heading = get_current_millis();
			}
			if (new_value) {
				roll = 0; pitch = 0;
				if (nvalue[align_se] == 1) {
					if (align_se == 0)	roll   = between<short>(pid_roll_anc.get(srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_roll,  max_roll);
					else if (align_se == 1)	roll   = between<short>(pid_roll_anc.get((-1)*srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_roll,  max_roll);
					else if (align_se == 2)	pitch  = between<short>(pid_pitch_anc.get(srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_pitch,  max_pitch);
					else if (align_se == 3)	pitch  = between<short>(pid_pitch_anc.get((-1)*srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_pitch,  max_pitch);
					nvalue[align_se] = 0;
				}
				if 	(srf[0].get_mean() < 30) 	roll = 1000;
				else if (srf[1].get_mean() < 30)	roll = -1000;
				else if (srf[2].get_mean() < 30)	pitch = 1000;
				else if (srf[3].get_mean() < 30)	pitch = -1000;
				if (abs(srf[0].get_mean() - ANCHOR_DISTANCE) < 5) {
					for (int i = 0; i < SE_COUNT; i++) nvalue[i] = 0;
					/*state = MOVE_BACKWARD*/
					/*TODO Neuausrichtung des Copters?*/
				}
			}
			if (breakpoint == 3) 		{roll = 0; pitch = 0; send_ext_ctrl();}
			else if (breakpoint != 2) 	{send_ext_ctrl();}
			break;
		case DELAY: 	break;
		case HTM_DELAY:	break;
		default:	break;
	}
	#if LOG > 0
	fprintf(fd_data,"%lu\t%d\t%d\t%d\t%d\t%d\t%f\t%f\n", (unsigned long)get_current_millis(), roll, pitch, yaw, current_heading, state, current_roll_rad, current_pitch_rad);
	#endif	
}
Esempio n. 17
0
/*
 * Break yuyv image in little square pieces
 * args:
 *    frame  - pointer to frame buffer (yuyv format)
 *    width  - frame width
 *    height - frame height
 *    piece_size - multiple of 2 (we need at least 2 pixels to get the entire pixel information)
 *
 * asserts:
 *    frame is not null
 */
static void fx_yuyv_pieces(uint8_t* frame, int width, int height, int piece_size )
{
	int numx = width / piece_size; //number of pieces in x axis
	int numy = height / piece_size; //number of pieces in y axis
	uint8_t *piece = calloc (piece_size * piece_size * 2, sizeof(uint8_t));
	if(piece == NULL)
	{
		fprintf(stderr,"RENDER: FATAL memory allocation failure (fx_pieces): %s\n", strerror(errno));
		exit(-1);
	}
	int i = 0, j = 0, line = 0, column = 0, linep = 0, px = 0, py = 0;

	/*random generator setup*/
	gsl_rng_env_setup();
	const gsl_rng_type *T = gsl_rng_default;
	gsl_rng *r = gsl_rng_alloc (T);

	int rot = 0;

	for(j = 0; j < numy; j++)
	{
		int row = j * piece_size;
		for(i = 0; i < numx; i++)
		{
			column = i * piece_size * 2;
			//get piece
			for(py = 0; py < piece_size; py++)
			{
				linep = py * piece_size * 2;
				line = (py + row) * width * 2;
				for(px=0 ; px < piece_size * 2; px++)
				{
					piece[px + linep] = frame[(px + column) + line];
				}
			}
			/*rotate piece and copy it to frame*/
			//rotation is random
			rot = (int) lround(8 * gsl_rng_uniform (r)); /*0 to 8*/

			switch(rot)
			{
				case 0: // do nothing
					break;
				case 5:
				case 1: //mirror
					fx_yuyv_mirror(piece, piece_size, piece_size);
					break;
				case 6:
				case 2: //upturn
					fx_yuyv_upturn(piece, piece_size, piece_size);
					break;
				case 4:
				case 3://mirror upturn
					fx_yuyv_upturn(piece, piece_size, piece_size);
					fx_yuyv_mirror(piece, piece_size, piece_size);
					break;
				default: //do nothing
					break;
			}
			//write piece
			for(py = 0; py < piece_size; py++)
			{
				linep = py * piece_size * 2;
				line = (py + row) * width * 2;
				for(px=0 ; px < piece_size * 2; px++)
				{
					frame[(px + column) + line] = piece[px + linep];
				}
			}
		}
	}

	/*free the random seed generator*/
	gsl_rng_free (r);
	/*free the piece buffer*/
	free(piece);
}
Esempio n. 18
0
stfio::filetype stfio_file_type(HDRTYPE* hdr) {
#ifdef __LIBBIOSIG2_H__
        switch (biosig_get_filetype(hdr)) {
#else
        switch (hdr->TYPE) {
#endif

#if (BIOSIG_VERSION > 10500)
        case ABF2:	return stfio::abf;
#endif
        case ABF:	return stfio::abf;
        case ATF:	return stfio::atf;
        case CFS:	return stfio::cfs;
        case HEKA:	return stfio::heka;
        case HDF:	return stfio::hdf5;
#if (BIOSIG_VERSION > 10403)
        case AXG:	return stfio::axg;
        case IBW:	return stfio::igor;
        case SMR:	return stfio::son;
#endif
        default:	return stfio::none;
        }
}

#if (defined(WITH_BIOSIG) || defined(WITH_BIOSIG2))
bool stfio::check_biosig_version(int a, int b, int c) {
	return (BIOSIG_VERSION >= 10000*a + 100*b + c);
}
#endif

stfio::filetype stfio::importBiosigFile(const std::string &fName, Recording &ReturnData, ProgressInfo& progDlg) {

    std::string errorMsg("Exception while calling std::importBSFile():\n");
    std::string yunits;
    stfio::filetype type;

    // =====================================================================================================================
    //
    // importBiosig opens file with libbiosig
    //	- performs an automated identification of the file format
    //  - and decides whether the data is imported through importBiosig (currently CFS, HEKA, ABF1, GDF, and others)
    //  - or handed back to other import*File functions (currently ABF2, AXG, HDF5)
    //
    // There are two implementations, level-1 and level-2 interface of libbiosig.
    //   level 1 is used when -DWITH_BIOSIG, -lbiosig
    //   level 2 is used when -DWITH_BIOSIG2, -lbiosig2
    //
    //   level 1 is better tested, but it does not provide ABI compatibility between MinGW and VisualStudio
    //   level 2 interface has been developed to provide ABI compatibility, but it is less tested
    //      and the API might still undergo major changes.
    // =====================================================================================================================


#ifdef __LIBBIOSIG2_H__

    HDRTYPE* hdr =  sopen( fName.c_str(), "r", NULL );
    if (hdr==NULL) {
        ReturnData.resize(0);
        return stfio::none;
    }

    type = stfio_file_type(hdr);
    if (biosig_check_error(hdr)) {
        ReturnData.resize(0);
        destructHDR(hdr);
        return type;
    }
    enum FileFormat biosig_filetype=biosig_get_filetype(hdr);
    if (biosig_filetype==ATF || biosig_filetype==ABF2 || biosig_filetype==HDF ) {
        // ATF, ABF2 and HDF5 support should be handled by importATF, and importABF, and importHDF5 not importBiosig
        ReturnData.resize(0);
        destructHDR(hdr);
        return type;
    }

    // earlier versions of biosig support only the file type identification, but did not properly read the files
    if ( (BIOSIG_VERSION < 10603)
      && (biosig_filetype==AXG)
       ) {
        ReturnData.resize(0);
        destructHDR(hdr);
        return type;
    }

    // ensure the event table is in chronological order	
    sort_eventtable(hdr);

    // allocate local memory for intermediate results;
    const int strSize=100;
    char str[strSize];

    /*
	count sections and generate list of indices indicating start and end of sweeps
     */	

    double fs = biosig_get_eventtable_samplerate(hdr);
    size_t numberOfEvents = biosig_get_number_of_events(hdr);
    size_t nsections = biosig_get_number_of_segments(hdr);
    size_t *SegIndexList = (size_t*)malloc((nsections+1)*sizeof(size_t));
    SegIndexList[0] = 0;
    SegIndexList[nsections] = biosig_get_number_of_samples(hdr);
    std::string annotationTableDesc = std::string();
    for (size_t k=0, n=0; k < numberOfEvents; k++) {
        uint32_t pos;
        uint16_t typ;
#if BIOSIG_VERSION < 10605
        char *desc;
#else
        const char *desc;
#endif
        /*
        uint32_t dur;
        uint16_t chn;
        gdftype  timestamp;
        */

        biosig_get_nth_event(hdr, k, &typ, &pos, NULL, NULL, NULL, &desc);

        if (typ == 0x7ffe) {
            SegIndexList[++n] = pos;
        }
        else if (typ < 256) {
            sprintf(str,"%f s:\t%s\n", pos/fs, desc);
            annotationTableDesc += std::string( str );
        }
    }
    int numberOfChannels = biosig_get_number_of_channels(hdr);

    /*************************************************************************
        rescale data to mV and pA
     *************************************************************************/
    for (int ch=0; ch < numberOfChannels; ++ch) {
        CHANNEL_TYPE *hc = biosig_get_channel(hdr, ch);
        switch (biosig_channel_get_physdimcode(hc) & 0xffe0) {
        case 4256:  // Volt
		//biosig_channel_scale_to_unit(hc, "mV");
		biosig_channel_change_scale_to_physdimcode(hc, 4274);
		break;
        case 4160:  // Ampere
		//biosig_channel_scale_to_unit(hc, "pA");
		biosig_channel_change_scale_to_physdimcode(hc, 4181);
		break;
	    }
    }

    /*************************************************************************
        read bulk data
     *************************************************************************/
    biosig_data_type *data = biosig_get_data(hdr, 0);
    size_t SPR = biosig_get_number_of_samples(hdr);

#ifdef _STFDEBUG
    std::cout << "Number of events: " << numberOfEvents << std::endl;
    /*int res = */ hdr2ascii(hdr, stdout, 4);
#endif

    for (int NS=0; NS < numberOfChannels; ) {
        CHANNEL_TYPE *hc = biosig_get_channel(hdr, NS);
        Channel TempChannel(nsections);
        TempChannel.SetChannelName(biosig_channel_get_label(hc));
        TempChannel.SetYUnits(biosig_channel_get_physdim(hc));

        for (size_t ns=1; ns<=nsections; ns++) {
	        size_t SPS = SegIndexList[ns]-SegIndexList[ns-1];	// length of segment, samples per segment

		int progbar = int(100.0*(1.0*ns/nsections + NS)/numberOfChannels);
		std::ostringstream progStr;
		progStr << "Reading channel #" << NS + 1 << " of " << numberOfChannels
			<< ", Section #" << ns << " of " << nsections;
		progDlg.Update(progbar, progStr.str());

		/* unused //
		char sweepname[20];
		sprintf(sweepname,"sweep %i",(int)ns);
		*/
		Section TempSection(
                                SPS, // TODO: hdr->nsamplingpoints[nc][ns]
                                "" // TODO: hdr->sectionname[nc][ns]
        );

		std::copy(&(data[NS*SPR + SegIndexList[ns-1]]),
			  &(data[NS*SPR + SegIndexList[ns]]),
			  TempSection.get_w().begin() );

        try {
            TempChannel.InsertSection(TempSection, ns-1);
        }
        catch (...) {
			ReturnData.resize(0);
			destructHDR(hdr);
			return type;
		}
	}
    try {
        if ((int)ReturnData.size() < numberOfChannels) {
            ReturnData.resize(numberOfChannels);
		}
		ReturnData.InsertChannel(TempChannel, NS++);
    }
    catch (...) {
		ReturnData.resize(0);
		destructHDR(hdr);
		return type;
        }
    }

    free(SegIndexList);

    ReturnData.SetComment ( biosig_get_recording_id(hdr) );

    sprintf(str,"v%i.%i.%i (compiled on %s %s)",BIOSIG_VERSION_MAJOR,BIOSIG_VERSION_MINOR,BIOSIG_PATCHLEVEL,__DATE__,__TIME__);
    std::string Desc = std::string("importBiosig with libbiosig ")+std::string(str) + " ";

    const char* tmpstr;
    if ((tmpstr=biosig_get_technician(hdr)))
            Desc += std::string ("\nTechnician:\t") + std::string (tmpstr) + " ";
    Desc += std::string( "\nCreated with: ");
    if ((tmpstr=biosig_get_manufacturer_name(hdr)))
        Desc += std::string( tmpstr ) + " ";
    if ((tmpstr=biosig_get_manufacturer_model(hdr)))
        Desc += std::string( tmpstr ) + " ";
    if ((tmpstr=biosig_get_manufacturer_version(hdr)))
        Desc += std::string( tmpstr ) + " ";
    if ((tmpstr=biosig_get_manufacturer_serial_number(hdr)))
        Desc += std::string( tmpstr ) + " ";

    Desc += std::string ("\nUser specified Annotations:\n")+annotationTableDesc;

    ReturnData.SetFileDescription(Desc);

#if (BIOSIG_VERSION > 10509)
    tmpstr = biosig_get_application_specific_information(hdr);
    if (tmpstr != NULL) /* MSVC2008 can not properly handle std::string( (char*)NULL ) */
        ReturnData.SetGlobalSectionDescription(tmpstr);
#endif

    ReturnData.SetXScale(1000.0/biosig_get_samplerate(hdr));
    ReturnData.SetXUnits("ms");
    ReturnData.SetScaling("biosig scaling factor");

    /*************************************************************************
        Date and time conversion
     *************************************************************************/
    struct tm T;
    biosig_get_startdatetime(hdr, &T);
    ReturnData.SetDateTime(T);

    destructHDR(hdr);


#else  // #ifndef __LIBBIOSIG2_H__


    HDRTYPE* hdr =  sopen( fName.c_str(), "r", NULL );
    if (hdr==NULL) {
        ReturnData.resize(0);
        return stfio::none;
    }
    type = stfio_file_type(hdr);

#if !defined(BIOSIG_VERSION) || (BIOSIG_VERSION < 10501)
    if (hdr->TYPE==ABF) {
        /*
           biosig v1.5.0 and earlier does not always return
           with a proper error message for ABF files.
           This causes problems with the ABF fallback mechanism
        */
#else
    if ( hdr->TYPE==ABF2 ) {
        // ABF2 support should be handled by importABF not importBiosig
        ReturnData.resize(0);
        destructHDR(hdr);
        return type;
    }
    if (hdr->TYPE==ABF && hdr->AS.B4C_ERRNUM) {
        /* this triggers the fall back mechanims w/o reporting an error message */
#endif
        ReturnData.resize(0);
        destructHDR(hdr);	// free allocated memory
        return type;
    }

#if defined(BIOSIG_VERSION) && (BIOSIG_VERSION > 10400)
    if (hdr->AS.B4C_ERRNUM) {
#else
    if (B4C_ERRNUM) {
#endif
        ReturnData.resize(0);
        destructHDR(hdr);	// free allocated memory
        return type;
    }
    if ( hdr->TYPE==ATF || hdr->TYPE==HDF) {
        // ATF, HDF5 support should be handled by importATF and importHDF5 not importBiosig
        ReturnData.resize(0);
        destructHDR(hdr);
        return type;
    }

    // earlier versions of biosig support only the file type identification, but did not read AXG files
#if defined(BIOSIG_VERSION) && (BIOSIG_VERSION > 10403)
    if ( (BIOSIG_VERSION < 10600)
         && (hdr->TYPE==AXG)
       ) {
        // biosig's AXG import crashes on Windows at this time
        ReturnData.resize(0);
        destructHDR(hdr);
        return type;
    }
#endif
    
    // ensure the event table is in chronological order
    sort_eventtable(hdr);

    // allocate local memory for intermediate results;
    const int strSize=100;
    char str[strSize];

    /*
	count sections and generate list of indices indicating start and end of sweeps
     */
    size_t numberOfEvents = hdr->EVENT.N;
    size_t LenIndexList = 256;
    if (LenIndexList > numberOfEvents) LenIndexList = numberOfEvents + 2;
    size_t *SegIndexList = (size_t*)malloc(LenIndexList*sizeof(size_t));
    uint32_t nsections = 0;
    SegIndexList[nsections] = 0;
    size_t MaxSectionLength = 0;
    for (size_t k=0; k <= numberOfEvents; k++) {
        if (LenIndexList <= nsections+2) {
            // allocate more memory as needed
		    LenIndexList *=2;
		    SegIndexList = (size_t*)realloc(SegIndexList, LenIndexList*sizeof(size_t));
	    }
        /*
            count number of sections and stores it in nsections;
            EVENT.TYP==0x7ffe indicate number of breaks between sweeps
	        SegIndexList includes index to first sample and index to last sample,
	        thus, the effective length of SegIndexList is the number of 0x7ffe plus two.
	    */
        if (0)                              ;
        else if (k >= hdr->EVENT.N)         SegIndexList[++nsections] = hdr->NRec*hdr->SPR;
        else if (hdr->EVENT.TYP[k]==0x7ffe) SegIndexList[++nsections] = hdr->EVENT.POS[k];
        else                                continue;

        size_t SPS = SegIndexList[nsections]-SegIndexList[nsections-1];	// length of segment, samples per segment
	    if (MaxSectionLength < SPS) MaxSectionLength = SPS;
    }

    int numberOfChannels = 0;
    for (int k=0; k < hdr->NS; k++)
        if (hdr->CHANNEL[k].OnOff==1)
            numberOfChannels++;

    /*************************************************************************
        rescale data to mV and pA
     *************************************************************************/    
    for (int ch=0; ch < hdr->NS; ++ch) {
        CHANNEL_TYPE *hc = hdr->CHANNEL+ch;
        if (hc->OnOff != 1) continue;
        double scale = PhysDimScale(hc->PhysDimCode); 
        switch (hc->PhysDimCode & 0xffe0) {
        case 4256:  // Volt
                hc->PhysDimCode = 4274; // = PhysDimCode("mV");
                scale *=1e3;   // V->mV
                hc->PhysMax *= scale;         
                hc->PhysMin *= scale;         
                hc->Cal *= scale;         
                hc->Off *= scale;         
                break; 
        case 4160:  // Ampere
                hc->PhysDimCode = 4181; // = PhysDimCode("pA");
                scale *=1e12;   // A->pA
                hc->PhysMax *= scale;         
                hc->PhysMin *= scale;         
                hc->Cal *= scale;         
                hc->Off *= scale;         
                break; 
        }     
    }

    /*************************************************************************
        read bulk data 
     *************************************************************************/    
    hdr->FLAG.ROW_BASED_CHANNELS = 0;
    /* size_t blks = */ sread(NULL, 0, hdr->NRec, hdr);
    biosig_data_type *data = hdr->data.block;
    size_t SPR = hdr->NRec*hdr->SPR;

#ifdef _STFDEBUG
    std::cout << "Number of events: " << numberOfEvents << std::endl;
    /*int res = */ hdr2ascii(hdr, stdout, 4);
#endif

    int NS = 0;   // number of non-empty channels
    for (size_t nc=0; nc < hdr->NS; ++nc) {

        if (hdr->CHANNEL[nc].OnOff == 0) continue;

        Channel TempChannel(nsections);
        TempChannel.SetChannelName(hdr->CHANNEL[nc].Label);
#if defined(BIOSIG_VERSION) && (BIOSIG_VERSION > 10301)
        TempChannel.SetYUnits(PhysDim3(hdr->CHANNEL[nc].PhysDimCode));
#else
        PhysDim(hdr->CHANNEL[nc].PhysDimCode,str);
        TempChannel.SetYUnits(str);
#endif

        for (size_t ns=1; ns<=nsections; ns++) {
	        size_t SPS = SegIndexList[ns]-SegIndexList[ns-1];	// length of segment, samples per segment

		int progbar = 100.0*(1.0*ns/nsections + NS)/numberOfChannels;
		std::ostringstream progStr;
		progStr << "Reading channel #" << NS + 1 << " of " << numberOfChannels
			<< ", Section #" << ns << " of " << nsections;
		progDlg.Update(progbar, progStr.str());

		/* unused //
		char sweepname[20];
		sprintf(sweepname,"sweep %i",(int)ns);		
		*/
		Section TempSection(
                                SPS, // TODO: hdr->nsamplingpoints[nc][ns]
                                "" // TODO: hdr->sectionname[nc][ns]
            	);

		std::copy(&(data[NS*SPR + SegIndexList[ns-1]]),
			  &(data[NS*SPR + SegIndexList[ns]]),
			  TempSection.get_w().begin() );

        try {
            TempChannel.InsertSection(TempSection, ns-1);
        }
        catch (...) {
			ReturnData.resize(0);
			destructHDR(hdr);
			return type;
		}
	}        
    try {
        if ((int)ReturnData.size() < numberOfChannels) {
            ReturnData.resize(numberOfChannels);
        }
        ReturnData.InsertChannel(TempChannel, NS++);
    }
    catch (...) {
		ReturnData.resize(0);
		destructHDR(hdr);
		return type;
        }
    }

    free(SegIndexList); 	

    ReturnData.SetComment ( hdr->ID.Recording );

    sprintf(str,"v%i.%i.%i (compiled on %s %s)",BIOSIG_VERSION_MAJOR,BIOSIG_VERSION_MINOR,BIOSIG_PATCHLEVEL,__DATE__,__TIME__);
    std::string Desc = std::string("importBiosig with libbiosig ")+std::string(str);

    if (hdr->ID.Technician)
            Desc += std::string ("\nTechnician:\t") + std::string (hdr->ID.Technician);
    Desc += std::string( "\nCreated with: ");
    if (hdr->ID.Manufacturer.Name)
        Desc += std::string( hdr->ID.Manufacturer.Name );
    if (hdr->ID.Manufacturer.Model)
        Desc += std::string( hdr->ID.Manufacturer.Model );
    if (hdr->ID.Manufacturer.Version)
        Desc += std::string( hdr->ID.Manufacturer.Version );
    if (hdr->ID.Manufacturer.SerialNumber)
        Desc += std::string( hdr->ID.Manufacturer.SerialNumber );

    Desc += std::string ("\nUser specified Annotations:\n");
    for (size_t k=0; k < numberOfEvents; k++) {
        if (hdr->EVENT.TYP[k] < 256) {
            sprintf(str,"%f s:\t",hdr->EVENT.POS[k]/hdr->EVENT.SampleRate);
            Desc += std::string( str );
            Desc += std::string( hdr->EVENT.CodeDesc[hdr->EVENT.TYP[k]] ) + "\n";
        }
    }
    ReturnData.SetFileDescription(Desc);
    // hdr->AS.bci2000 is an alias to hdr->AS.fpulse, which available only in libbiosig v1.6.0 or later

    if (hdr->AS.bci2000) ReturnData.SetGlobalSectionDescription(std::string(hdr->AS.bci2000));

    ReturnData.SetXScale(1000.0/hdr->SampleRate);
    ReturnData.SetXUnits("ms");
    ReturnData.SetScaling("biosig scaling factor");

    /*************************************************************************
        Date and time conversion
     *************************************************************************/
    struct tm T;
#if (BIOSIG_VERSION_MAJOR > 0)
    gdf_time2tm_time_r(hdr->T0, &T);
#else
    struct tm* Tp;
    Tp = gdf_time2tm_time(hdr->T0);
    T = *Tp;
#endif

    ReturnData.SetDateTime(T);

    destructHDR(hdr);

#endif
    return stfio::biosig;
}


    // =====================================================================================================================
    //
    // Save file with libbiosig into GDF format
    //
    // There basically two implementations, one with libbiosig before v1.6.0 and
    // and one for libbiosig v1.6.0 and later
    //
    // =====================================================================================================================

bool stfio::exportBiosigFile(const std::string& fName, const Recording& Data, stfio::ProgressInfo& progDlg) {
/*
    converts the internal data structure to libbiosig's internal structure
    and saves the file as gdf file.

    The data in converted into the raw data format, and not into the common
    data matrix.
*/

#ifdef __LIBBIOSIG2_H__

    size_t numberOfChannels = Data.size();
    HDRTYPE* hdr = constructHDR(numberOfChannels, 0);

	/* Initialize all header parameters */
    biosig_set_filetype(hdr, GDF);

    biosig_set_startdatetime(hdr, Data.GetDateTime());

    const char *xunits = Data.GetXUnits().c_str();
    uint16_t pdc = PhysDimCode(xunits);

    if ((pdc & 0xffe0) != PhysDimCode("s")) {
        fprintf(stderr,"Stimfit exportBiosigFile: xunits [%s] has not proper units, assume [ms]\n",Data.GetXUnits().c_str());
        pdc = PhysDimCode("ms");
    }

    double fs = 1.0/(PhysDimScale(pdc) * Data.GetXScale());
    biosig_set_samplerate(hdr, fs);

#if (BIOSIG_VERSION < 10700)
    biosig_set_flags(hdr, 0, 0, 0);
#else
    biosig_reset_flag(hdr, BIOSIG_FLAG_COMPRESSION | BIOSIG_FLAG_UCAL | BIOSIG_FLAG_OVERFLOWDETECTION | BIOSIG_FLAG_ROW_BASED_CHANNELS );
#endif

    size_t k, m, numberOfEvents=0;
    size_t NRec=0;	// corresponds to hdr->NRec
    size_t SPR=1;	// corresponds to hdr->SPR
    size_t chSPR=0;	// corresponds to hc->SPR

    /* Initialize all channel parameters */
#ifndef DONOTUSE_DYNAMIC_ALLOCATION_FOR_CHANSPR
	size_t *chanSPR = (size_t*)malloc(numberOfChannels*sizeof(size_t));
#endif
    for (k = 0; k < numberOfChannels; ++k) {
        CHANNEL_TYPE *hc = biosig_get_channel(hdr, k);

		biosig_channel_set_datatype_to_double(hc);
		biosig_channel_set_scaling(hc, 1e9, -1e9, 1e9, -1e9);
		biosig_channel_set_label(hc, Data[k].GetChannelName().c_str());
		biosig_channel_set_physdim(hc, Data[k].GetYUnits().c_str());

		biosig_channel_set_filter(hc, NAN, NAN, NAN);
		biosig_channel_set_timing_offset(hc, 0.0);
		biosig_channel_set_impedance(hc, NAN);

        chSPR    = SPR;

        // each segment gets one marker, roughly
        numberOfEvents += Data[k].size();

        size_t m,len = 0;
        for (len=0, m = 0; m < Data[k].size(); ++m) {
            unsigned div = lround(Data[k][m].GetXScale()/Data.GetXScale());
            chSPR = lcm(chSPR,div);  // sampling interval of m-th segment in k-th channel
            len += div*Data[k][m].size();
        }
        SPR = lcm(SPR, chSPR);

        /*
            hc->SPR (i.e. chSPR) is 'abused' to store final hdr->SPR/hc->SPR, this is corrected in the loop below
            its a hack to avoid the need for another malloc().
        */
#ifdef DONOTUSE_DYNAMIC_ALLOCATION_FOR_CHANSPR
        biosig_channel_set_samples_per_record(hc, chSPR);
#else
        chanSPR[k]=chSPR;
#endif

        if (k==0) {
            NRec = len;
        }
        else if ((size_t)NRec != len) {
            destructHDR(hdr);
            throw std::runtime_error("File can't be exported:\n"
                "No data or traces have different sizes" );

            return false;
        }
    }

    biosig_set_number_of_samples(hdr, NRec, SPR);
    size_t bpb = 0;
    for (k = 0; k < numberOfChannels; ++k) {
        CHANNEL_TYPE *hc = biosig_get_channel(hdr, k);
        // the 'abuse' of hc->SPR described above is corrected
#ifdef DONOTUSE_DYNAMIC_ALLOCATION_FOR_CHANSPR
        size_t spr = biosig_channel_get_samples_per_record(hc);
        spr = SPR / spr;
        biosig_channel_set_samples_per_record(hc, spr);
#else
        size_t spr = SPR/chanSPR[k];
        chanSPR[k] = spr;
#endif
        bpb += spr * 8; /* its always double */
    }

	/***
	    build Event table for storing segment information
	    pre-allocate memory for even table
         ***/

        numberOfEvents *= 2;    // about two events per segment
        biosig_set_number_of_events(hdr, numberOfEvents);

    /* check whether all segments have same size */
    {
        char flag = (numberOfChannels > 0);
        size_t m, POS, pos;
        for (k=0; k < numberOfChannels; ++k) {
            pos = Data[k].size();
            if (k==0)
                POS = pos;
            else
                flag &= (POS == pos);
        }
        for (m=0; flag && (m < Data[(size_t)0].size()); ++m) {
            for (k=0; k < biosig_get_number_of_channels(hdr); ++k) {
                pos = Data[k][m].size() * lround(Data[k][m].GetXScale()/Data.GetXScale());
                if (k==0)
                    POS = pos;
                else
                    flag &= (POS == pos);
            }
        }
        if (!flag) {
            destructHDR(hdr);
            throw std::runtime_error(
                    "File can't be exported:\n"
                    "Traces have different sizes or no channels found"
            );
            return false;
        }
    }

        size_t N=0;
        k=0;
        size_t pos = 0;
        for (m=0; m < (Data[k].size()); ++m) {
            if (pos > 0) {
                uint16_t typ=0x7ffe;
                uint32_t pos32=pos;
                uint16_t chn=0;
                uint32_t dur=0;
                // set break marker
                biosig_set_nth_event(hdr, N++, &typ, &pos32, &chn, &dur, NULL, NULL);
                /*
                // set annotation
                const char *Desc = Data[k][m].GetSectionDescription().c_str();
                 if (Desc != NULL && strlen(Desc)>0)
                    biosig_set_nth_event(hdr, N++, NULL, &pos32, &chn, &dur, NULL, Desc);   // TODO
                */
            }
            pos += Data[k][m].size() * lround(Data[k][m].GetXScale()/Data.GetXScale());
        }

        biosig_set_number_of_events(hdr, N);
        biosig_set_eventtable_samplerate(hdr, fs);
        sort_eventtable(hdr);

        /* convert data into GDF rawdata from  */
        uint8_t *rawdata = (uint8_t*)malloc(bpb * NRec);

        size_t bi=0;
	for (k=0; k < numberOfChannels; ++k) {
        CHANNEL_TYPE *hc = biosig_get_channel(hdr, k);
#ifdef DONOTUSE_DYNAMIC_ALLOCATION_FOR_CHANSPR
        size_t chSPR = biosig_channel_get_samples_per_record(hc);
#else
        size_t chSPR = chanSPR[k];
#endif
        size_t m,n,len=0;
        for (m=0; m < Data[k].size(); ++m) {

            size_t div = lround(Data[k][m].GetXScale()/Data.GetXScale());
            size_t div2 = SPR/div;		  // TODO: avoid using hdr->SPR

            // fprintf(stdout,"k,m,div,div2: %i,%i,%i,%i\n",(int)k,(int)m,(int)div,(int)div2);  //
            for (n=0; n < Data[k][m].size(); ++n) {
                uint64_t val;
                double d = Data[k][m][n];
#if !defined(__MINGW32__) && !defined(_MSC_VER) && !defined(__APPLE__)
                val = htole64(*(uint64_t*)&d);
#else
                val = *(uint64_t*)&d;
#endif
                size_t p, spr = (len + n*div) / SPR;

                for (p=0; p < div2; p++)
                   *(uint64_t*)(rawdata + bi + bpb * spr + p*8) = val;
            }
            len += div*Data[k][m].size();
        }
		bi += chSPR*8;
    }

#ifndef DONOTUSE_DYNAMIC_ALLOCATION_FOR_CHANSPR
	if (chanSPR) free(chanSPR);
#endif

    /******************************
        write to file
    *******************************/
    std::string errorMsg("Exception while calling std::exportBiosigFile():\n");

    hdr = sopen( fName.c_str(), "w", hdr );
    if (serror2(hdr)) {
        errorMsg += biosig_get_errormsg(hdr);
        destructHDR(hdr);
        throw std::runtime_error(errorMsg.c_str());
        return false;
    }

    ifwrite(rawdata, bpb, NRec, hdr);

    sclose(hdr);
    destructHDR(hdr);
    free(rawdata);


#else   // #ifndef __LIBBIOSIG2_H__


    HDRTYPE* hdr = constructHDR(Data.size(), 0);
    assert(hdr->NS == Data.size());

	/* Initialize all header parameters */
    hdr->TYPE = GDF;
#if (BIOSIG_VERSION >= 10508)
    /* transition in biosig to rename HDR->VERSION to HDR->Version
       to avoid name space conflict with macro VERSION
     */
    hdr->Version = 3.0;   // select latest supported version of GDF format
#else
    hdr->VERSION = 3.0;   // select latest supported version of GDF format
#endif

    struct tm t = Data.GetDateTime();

    hdr->T0 = tm_time2gdf_time(&t);

    const char *xunits = Data.GetXUnits().c_str();
#if (BIOSIG_VERSION_MAJOR > 0)
    uint16_t pdc = PhysDimCode(xunits);
#else
    uint16_t pdc = PhysDimCode((char*)xunits);
#endif
    if ((pdc & 0xffe0) == PhysDimCode("s")) {
        fprintf(stderr,"Stimfit exportBiosigFile: xunits [%s] has not proper units, assume [ms]\n",Data.GetXUnits().c_str());
        pdc = PhysDimCode("ms");
    }
    hdr->SampleRate = 1.0/(PhysDimScale(pdc) * Data.GetXScale());
    hdr->SPR  = 1;

    hdr->FLAG.UCAL = 0;
    hdr->FLAG.OVERFLOWDETECTION = 0;

    hdr->FILE.COMPRESSION = 0;

	/* Initialize all channel parameters */
    size_t k, m;
    for (k = 0; k < hdr->NS; ++k) {
        CHANNEL_TYPE *hc = hdr->CHANNEL+k;

        hc->PhysMin = -1e9;
        hc->PhysMax =  1e9;
        hc->DigMin  = -1e9;
        hc->DigMax  =  1e9;
        hc->Cal     =  1.0;
        hc->Off     =  0.0;

        /* Channel descriptions. */
        strncpy(hc->Label, Data[k].GetChannelName().c_str(), MAX_LENGTH_LABEL);
#if (BIOSIG_VERSION_MAJOR > 0)
        hc->PhysDimCode = PhysDimCode(Data[k].GetYUnits().c_str());
#else
        hc->PhysDimCode = PhysDimCode((char*)Data[k].GetYUnits().c_str());
#endif
        hc->OnOff      = 1;
        hc->LeadIdCode = 0;

        hc->TOffset  = 0.0;
        hc->Notch    = NAN;
        hc->LowPass  = NAN;
        hc->HighPass = NAN;
        hc->Impedance= NAN;

        hc->SPR    = hdr->SPR;
        hc->GDFTYP = 17; 	// double

        // each segment gets one marker, roughly
        hdr->EVENT.N += Data[k].size();

        size_t m,len = 0;
        for (len=0, m = 0; m < Data[k].size(); ++m) {
            unsigned div = lround(Data[k][m].GetXScale()/Data.GetXScale());
            hc->SPR = lcm(hc->SPR,div);  // sampling interval of m-th segment in k-th channel
            len += div*Data[k][m].size();
        }
        hdr->SPR = lcm(hdr->SPR, hc->SPR);

        if (k==0) {
            hdr->NRec = len;
        }
        else if ((size_t)hdr->NRec != len) {
            destructHDR(hdr);
            throw std::runtime_error("File can't be exported:\n"
                "No data or traces have different sizes" );

            return false;
        }
    }

    hdr->AS.bpb = 0;
    for (k = 0; k < hdr->NS; ++k) {
        CHANNEL_TYPE *hc = hdr->CHANNEL+k;
        hc->SPR = hdr->SPR / hc->SPR;
        hc->bi  = hdr->AS.bpb;
        hdr->AS.bpb += hc->SPR * 8; /* its always double */
    }

	/***
	    build Event table for storing segment information
	 ***/
	size_t N = hdr->EVENT.N * 2;    // about two events per segment
	hdr->EVENT.POS = (uint32_t*)realloc(hdr->EVENT.POS, N * sizeof(*hdr->EVENT.POS));
	hdr->EVENT.DUR = (uint32_t*)realloc(hdr->EVENT.DUR, N * sizeof(*hdr->EVENT.DUR));
	hdr->EVENT.TYP = (uint16_t*)realloc(hdr->EVENT.TYP, N * sizeof(*hdr->EVENT.TYP));
	hdr->EVENT.CHN = (uint16_t*)realloc(hdr->EVENT.CHN, N * sizeof(*hdr->EVENT.CHN));
#if (BIOSIG_VERSION >= 10500)
	hdr->EVENT.TimeStamp = (gdf_time*)realloc(hdr->EVENT.TimeStamp, N * sizeof(gdf_time));
#endif

    /* check whether all segments have same size */
    {
        char flag = (hdr->NS>0);
        size_t m, POS, pos;
        for (k=0; k < hdr->NS; ++k) {
            pos = Data[k].size();
            if (k==0)
                POS = pos;
            else
                flag &= (POS == pos);
        }
        for (m=0; flag && (m < Data[(size_t)0].size()); ++m) {
            for (k=0; k < hdr->NS; ++k) {
                pos = Data[k][m].size() * lround(Data[k][m].GetXScale()/Data.GetXScale());
                if (k==0)
                    POS = pos;
                else
                    flag &= (POS == pos);
            }
        }
        if (!flag) {
            destructHDR(hdr);
            throw std::runtime_error(
                    "File can't be exported:\n"
                    "Traces have different sizes or no channels found"
            );
            return false;
        }
    }

        N=0;
        k=0;
        size_t pos = 0;
        for (m=0; m < (Data[k].size()); ++m) {
            if (pos > 0) {
                // start of new segment after break
                hdr->EVENT.POS[N] = pos;
                hdr->EVENT.TYP[N] = 0x7ffe;
                hdr->EVENT.CHN[N] = 0;
                hdr->EVENT.DUR[N] = 0;
                N++;
            }
#if 0
            // event description
            hdr->EVENT.POS[N] = pos;
            FreeTextEvent(hdr, N, "myevent");
            //FreeTextEvent(hdr, N, Data[k][m].GetSectionDescription().c_str()); // TODO
            hdr->EVENT.CHN[N] = k;
            hdr->EVENT.DUR[N] = 0;
            N++;
#endif
            pos += Data[k][m].size() * lround(Data[k][m].GetXScale()/Data.GetXScale());
        }

        hdr->EVENT.N = N;
        hdr->EVENT.SampleRate = hdr->SampleRate;

        sort_eventtable(hdr);

	/* convert data into GDF rawdata from  */
	hdr->AS.rawdata = (uint8_t*)realloc(hdr->AS.rawdata, hdr->AS.bpb*hdr->NRec);
	for (k=0; k < hdr->NS; ++k) {
        CHANNEL_TYPE *hc = hdr->CHANNEL+k;

        size_t m,n,len=0;
        for (m=0; m < Data[k].size(); ++m) {
            size_t div = lround(Data[k][m].GetXScale()/Data.GetXScale());
            size_t div2 = hdr->SPR/div;

            // fprintf(stdout,"k,m,div,div2: %i,%i,%i,%i\n",(int)k,(int)m,(int)div,(int)div2);  //
            for (n=0; n < Data[k][m].size(); ++n) {
                uint64_t val;
                double d = Data[k][m][n];
#if !defined(__MINGW32__) && !defined(_MSC_VER) && !defined(__APPLE__)
                val = htole64(*(uint64_t*)&d);
#else
                val = *(uint64_t*)&d;
#endif
                size_t p, spr = (len + n*div) / hdr->SPR;
                for (p=0; p < div2; p++)
                   *(uint64_t*)(hdr->AS.rawdata + hc->bi + hdr->AS.bpb * spr + p*8) = val;
            }
            len += div*Data[k][m].size();
        }
    }

    /******************************
        write to file
    *******************************/
    std::string errorMsg("Exception while calling std::exportBiosigFile():\n");

    hdr = sopen( fName.c_str(), "w", hdr );
#if (BIOSIG_VERSION > 10400)
    if (serror2(hdr)) {
        errorMsg += hdr->AS.B4C_ERRMSG;
#else
    if (serror()) {
	    errorMsg += B4C_ERRMSG;
#endif
        destructHDR(hdr);
        throw std::runtime_error(errorMsg.c_str());
        return false;
    }

    ifwrite(hdr->AS.rawdata, hdr->AS.bpb, hdr->NRec, hdr);

    sclose(hdr);
    destructHDR(hdr);
#endif

    return true;
}
Esempio n. 19
0
/*
 * Break yu12 image in little square pieces
 * args:
 *    frame  - pointer to frame buffer (yu12 format)
 *    width  - frame width
 *    height - frame height
 *    piece_size - multiple of 2 (we need at least 2 pixels to get the entire pixel information)
 *
 * asserts:
 *    frame is not null
 */
static void fx_yu12_pieces(uint8_t* frame, int width, int height, int piece_size )
{
	int numx = width / piece_size; //number of pieces in x axis
	int numy = height / piece_size; //number of pieces in y axis
	
	uint8_t piece[(piece_size * piece_size * 3) / 2];
	uint8_t *ppiece = piece;
	
	int i = 0, j = 0, w = 0, h = 0;

	/*random generator setup*/
	gsl_rng_env_setup();
	const gsl_rng_type *T = gsl_rng_default;
	gsl_rng *r = gsl_rng_alloc (T);

	int rot = 0;
	
	uint8_t *py = NULL;
	uint8_t *pu = NULL;
	uint8_t *pv = NULL;
	
	for(h = 0; h < height; h += piece_size)
	{	
		for(w = 0; w < width; w += piece_size)
		{	
			uint8_t *ppy = piece;
			uint8_t *ppu = piece + (piece_size * piece_size);
			uint8_t *ppv = ppu + ((piece_size * piece_size) / 4);
	
			for(i = 0; i < piece_size; ++i)
			{		
				py = frame + ((h + i) * width) + w;
				for (j=0; j < piece_size; ++j)
				{
					*ppy++ = *py++; 
				}	
			}
			
			for(i = 0; i < piece_size; i += 2)
			{	
				uint8_t *pu = frame + (width * height) + (((h + i) * width) / 4) + (w / 2);
				uint8_t *pv = pu + ((width * height) / 4);
				
				for(j = 0; j < piece_size; j += 2)
				{
					*ppu++ = *pu++;
					*ppv++ = *pv++;
				}
			}
			
			ppy = piece;
			ppu = piece + (piece_size * piece_size);
			ppv = ppu + ((piece_size * piece_size) / 4);
			
			/*rotate piece and copy it to frame*/
			//rotation is random
			rot = (int) lround(8 * gsl_rng_uniform (r)); /*0 to 8*/

			switch(rot)
			{
				case 0: // do nothing
					break;
				case 5:
				case 1: //mirror
					fx_yu12_mirror(piece, piece_size, piece_size);
					break;
				case 6:
				case 2: //upturn
					fx_yu12_upturn(piece, piece_size, piece_size);
					break;
				case 4:
				case 3://mirror upturn
					fx_yu12_upturn(piece, piece_size, piece_size);
					fx_yu12_mirror(piece, piece_size, piece_size);
					break;
				default: //do nothing
					break;
			}
			
			ppy = piece;
			ppu = piece + (piece_size * piece_size);
			ppv = ppu + ((piece_size * piece_size) / 4);
		
			for(i = 0; i < piece_size; ++i)
			{
				py = frame + ((h + i) * width) + w;
				for (j=0; j < piece_size; ++j)
				{
					*py++ = *ppy++; 
				}	
			}
			
			for(i = 0; i < piece_size; i += 2)
			{
				uint8_t *pu = frame + (width * height) + (((h + i) * width) / 4) + (w / 2);
				uint8_t *pv = pu + ((width * height) / 4);
				
				for(j = 0; j < piece_size; j += 2)
				{
					*pu++ = *ppu++;
					*pv++ = *ppv++;
				}
			}
		}
	}

	/*free the random seed generator*/
	gsl_rng_free (r);
}
// 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)
{
	//Dryrain changes-----------------------
	#ifdef HYSTERESIS_H
	//Hysteresis correction if needed
	hysteresis.InsertCorrection(x,y,z,e);
	#endif
	//------------------------------
	
  // 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)
    {
      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 *= 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();
}
Esempio n. 21
0
/*
 * Trail of particles obtained from the image frame
 * args:
 *    frame  - pointer to frame buffer (yuyv format)
 *    width  - frame width
 *    height - frame height
 *    trail_size  - trail size (in frames)
 *    particle_size - maximum size in pixels - should be even (square - size x size)
 *
 * asserts:
 *    frame is not null
 *
 * returns: void
 */
static void fx_particles(uint8_t* frame, int width, int height, int trail_size, int particle_size)
{
	/*asserts*/
	assert(frame != NULL);

	int i,j,w,h = 0;
	int part_w = width>>7;
	int part_h = height>>6;

	/*random generator setup*/
	gsl_rng_env_setup();
	const gsl_rng_type *T = gsl_rng_default;
	gsl_rng *r = gsl_rng_alloc (T);

	/*allocation*/
	if (particles == NULL)
	{
		particles = calloc(trail_size * part_w * part_h, sizeof(particle_t));
		if(particles == NULL)
		{
			fprintf(stderr,"RENDER: FATAL memory allocation failure (fx_particles): %s\n", strerror(errno));
			exit(-1);
		}
	}

	particle_t *part = particles;
	particle_t *part1 = part;

	/*move particles in trail*/
	for (i = trail_size; i > 1; --i)
	{
		part  += (i - 1) * part_w * part_h;
		part1 += (i - 2) * part_w * part_h;

		for (j= 0; j < part_w * part_h; ++j)
		{
			if(part1->decay > 0)
			{
				part->PX = part1->PX + (int) lround(3 * gsl_rng_uniform (r)); /*0  to 3*/
				part->PY = part1->PY -4 + (int) lround(5 * gsl_rng_uniform (r));/*-4 to 1*/

				if(ODD(part->PX)) part->PX++; /*make sure PX is allways even*/

				if((part->PX > (width-particle_size)) || (part->PY > (height-particle_size)) || (part->PX < 0) || (part->PY < 0))
				{
					part->PX = 0;
					part->PY = 0;
					part->decay = 0;
				}
				else
				{
					part->decay = part1->decay - 1;
				}

				part->Y = part1->Y;
				part->U = part1->U;
				part->V = part1->V;
				part->size = part1->size;
			}
			else
			{
				part->decay = 0;
			}
			part++;
			part1++;
		}
		part = particles; /*reset*/
		part1 = part;
	}

	part = particles; /*reset*/
	
	/*get particles from frame (one pixel per particle - make PX allways even)*/
	for(i =0; i < part_w * part_h; i++)
	{
		/* (2 * particle_size) to (width - 4 * particle_size)*/
		part->PX = 2 * particle_size + (int) lround( (width - 6 * particle_size) * gsl_rng_uniform (r));
		/* (2 * particle_size) to (height - 4 * particle_size)*/
		part->PY = 2 * particle_size + (int) lround( (height - 6 * particle_size) * gsl_rng_uniform (r));

		if(ODD(part->PX)) part->PX++;

		int y_pos = part->PX + (part->PY * width);
		int u_pos = (part->PX + (part->PY * width / 2)) / 2;
		int v_pos = u_pos + ((width * height) / 4);

		part->Y = frame[y_pos];
		part->U = frame[u_pos];
		part->V = frame[v_pos];

		part->size = 1 + (int) lround((particle_size -1) * gsl_rng_uniform (r));
		if(ODD(part->size)) part->size++;

		part->decay = (float) trail_size;

		part++; /*next particle*/
	}

	part = particles; /*reset*/
	int line = 0;
	float blend =0;
	float blend1 =0;
	/*render particles to frame (expand pixel to particle size)*/
	for (i = 0; i < trail_size * part_w * part_h; i++)
	{
		if(part->decay > 0)
		{
			int y_pos = part->PX + (part->PY * width);
			int u_pos = (part->PX + (part->PY * width / 2)) / 2;
			int v_pos = u_pos + ((width * height) / 4);

			blend = part->decay/trail_size;
			blend1= 1 - blend;

			//y
			for(h = 0; h <(part->size); h++)
			{
				line = h * width;
				for (w = 0; w <(part->size); w++)
				{
					frame[y_pos + line + w] = CLIP((part->Y * blend) + (frame[y_pos + line + w] * blend1));
				}
			}

			//u v
			for(h = 0; h <(part->size); h+=2)
			{
				line = (h * width) / 4;
				for (w = 0; w <(part->size); w+=2)
				{
					frame[u_pos + line + (w / 2)] = CLIP((part->U * blend) + (frame[u_pos + line + (w / 2)] * blend1));
					frame[v_pos + line + (w / 2)] = CLIP((part->V * blend) + (frame[v_pos + line + (w / 2)] * blend1));
				}
			}
		}
		part++;
	}

	/*free the random seed generator*/
	gsl_rng_free (r);
}
Esempio n. 22
0
inline long lround(__gmp_expr<T,U> const& x, const Policy& pol)
{
   return lround(static_cast<mpfr_class>(x), pol);
}
Esempio n. 23
0
// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in 
// millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
// rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes.
// All position data passed to the planner must be in terms of machine position to keep the planner 
// independent of any coordinate system changes and offsets, which are handled by the g-code parser.
// NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control.
void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rate) 
{
  // Prepare to set up new block
  block_t *block = &block_buffer[block_buffer_head];

  // Calculate target position in absolute steps
  int32_t target[3];
  target[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]);
  target[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]);
  target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]);     

  // Compute direction bits for this block
  block->direction_bits = 0;
  if (target[X_AXIS] < pl.position[X_AXIS]) { block->direction_bits |= (1<<X_DIRECTION_BIT); }
  if (target[Y_AXIS] < pl.position[Y_AXIS]) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
  if (target[Z_AXIS] < pl.position[Z_AXIS]) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
  
  // Number of steps for each axis
  block->steps_x = labs(target[X_AXIS]-pl.position[X_AXIS]);
  block->steps_y = labs(target[Y_AXIS]-pl.position[Y_AXIS]);
  block->steps_z = labs(target[Z_AXIS]-pl.position[Z_AXIS]);
  block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z));

  // Bail if this is a zero-length block
  if (block->step_event_count == 0) { return; };
  
  // Compute path vector in terms of absolute step target and current positions
  float delta_mm[3];
  delta_mm[X_AXIS] = (target[X_AXIS]-pl.position[X_AXIS])/settings.steps_per_mm[X_AXIS];
  delta_mm[Y_AXIS] = (target[Y_AXIS]-pl.position[Y_AXIS])/settings.steps_per_mm[Y_AXIS];
  delta_mm[Z_AXIS] = (target[Z_AXIS]-pl.position[Z_AXIS])/settings.steps_per_mm[Z_AXIS];
  block->millimeters = sqrt(delta_mm[X_AXIS]*delta_mm[X_AXIS] + delta_mm[Y_AXIS]*delta_mm[Y_AXIS] + 
                            delta_mm[Z_AXIS]*delta_mm[Z_AXIS]);
  float inverse_millimeters = 1.0/block->millimeters;  // Inverse millimeters to remove multiple divides	
  
  // Calculate speed in mm/minute for each axis. No divide by zero due to previous checks.
  // NOTE: Minimum stepper speed is limited by MINIMUM_STEPS_PER_MINUTE in stepper.c
  float inverse_minute;
  if (!invert_feed_rate) {
    inverse_minute = feed_rate * inverse_millimeters;
  } else {
    inverse_minute = 1.0 / feed_rate;
  }
  block->nominal_speed = block->millimeters * inverse_minute; // (mm/min) Always > 0
  block->nominal_rate = ceil(block->step_event_count * inverse_minute); // (step/min) Always > 0
  
  // Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line
  // average travel per step event changes. For a line along one axis the travel per step event
  // is equal to the travel/step in the particular axis. For a 45 degree line the steppers of both
  // axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2).
  // To generate trapezoids with contant acceleration between blocks the rate_delta must be computed 
  // specifically for each line to compensate for this phenomenon:
  // Convert universal acceleration for direction-dependent stepper rate change parameter
  block->rate_delta = ceil( block->step_event_count*inverse_millimeters *  
        settings.acceleration / (60 * ACCELERATION_TICKS_PER_SECOND )); // (step/min/acceleration_tick)

  // Compute path unit vector                            
  float 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.
  // NOTE: This is basically an exact path mode (G61), but it doesn't come to a complete stop unless
  // the junction deviation value is high. In the future, if continuous mode (G64) is desired, the
  // math here is exactly the same. Instead of motioning all the way to junction point, the machine
  // will just need to follow the arc circle defined above and check if the arc radii are no longer
  // than half of either line segment to ensure no overlapping. Right now, the Arduino likely doesn't
  // have the horsepower to do these calculations at high feed rates.
  float 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) && (pl.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.
    float cos_theta = - pl.previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] 
                       - pl.previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] 
                       - pl.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(pl.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
        float sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
        vmax_junction = min(vmax_junction,
          sqrt(settings.acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
      }
    }
  }
  block->max_entry_speed = vmax_junction;
  
  // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
  float v_allowable = max_allowable_speed(-settings.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(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[]
  pl.previous_nominal_speed = block->nominal_speed;
  
  // Update buffer head and next buffer head indices
  block_buffer_head = next_buffer_head;  
  next_buffer_head = next_block_index(block_buffer_head);
  
  // Update planner position
  memcpy(pl.position, target, sizeof(target)); // pl.position[] = target[]

  planner_recalculate(); 
}