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()

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(); }

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; }

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]); }

// 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]); }

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; }

// 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; }

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(¬eSym, "%s%d", notes[mNote%12], oct); } else { oct = ((mNote+1) / 12) - 2; asprintf(¬eSym, "%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(¢Sym, "%+.2lf", (double)(lround(cents * 1000))/1000.); // post("%s %f", centSym, cents); atom_setsym(outList+1, gensym(centSym)); } else { asprintf(¢Sym, "%+.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(¬eSym, "%s%d", notes[mNote%12], oct); } else { oct = ((mNote+1) / 12) - 2; asprintf(¬eSym, "%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; } }

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 [email protected]_ = "Cylinder" dfe->addEnumText(gettext(TEXT("[email protected]_"))); // LKTOKEN [email protected]_ = "Line" dfe->addEnumText(gettext(TEXT("[email protected]_"))); // LKTOKEN [email protected]_ = "FAI Sector" dfe->addEnumText(gettext(TEXT("[email protected]_"))); } 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 [email protected]_ = "Cylinder" dfe->addEnumText(gettext(TEXT("[email protected]_"))); // LKTOKEN [email protected]_ = "Line" dfe->addEnumText(gettext(TEXT("[email protected]_"))); // LKTOKEN [email protected]_ = "FAI Sector" dfe->addEnumText(gettext(TEXT("[email protected]_"))); } 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 [email protected]_ = "Cylinder" dfe->addEnumText(gettext(TEXT("[email protected]_"))); // LKTOKEN [email protected]_ = "FAI Sector" dfe->addEnumText(gettext(TEXT("[email protected]_"))); 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 [email protected]_ = "Manual" dfe->addEnumText(gettext(TEXT("[email protected]_"))); // LKTOKEN [email protected]_ "Auto" dfe->addEnumText(gettext(TEXT("[email protected]_"))); // LKTOKEN [email protected]_ = "Arm" dfe->addEnumText(gettext(TEXT("[email protected]_"))); // LKTOKEN [email protected]_ = "Arm start" dfe->addEnumText(gettext(TEXT("[email protected]_"))); } 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); } }

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)); }

size_t hundredNanoSecondsToFrames( ::REFERENCE_TIME timeHundredNs, size_t sampleRate ) { return lround( (double)sampleRate * (double)timeHundredNs * 0.0000001 ); }

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"."); } }

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); }

// 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(); }

/*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 }

/* * 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); }

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; }

/* * 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(); }

/* * 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); }

inline long lround(__gmp_expr<T,U> const& x, const Policy& pol) { return lround(static_cast<mpfr_class>(x), pol); }

// 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(); }