|
@@ -33,7 +33,7 @@
|
33
|
33
|
#include "Marlin.h"
|
34
|
34
|
#include "speed_lookuptable.h"
|
35
|
35
|
|
36
|
|
-char version_string[] = "0.9.3";
|
|
36
|
+char version_string[] = "0.9.8";
|
37
|
37
|
|
38
|
38
|
#ifdef SDSUPPORT
|
39
|
39
|
#include "SdFat.h"
|
|
@@ -1167,10 +1167,9 @@ void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit
|
1167
|
1167
|
if(final_rate < 120) final_rate=120;
|
1168
|
1168
|
|
1169
|
1169
|
// Calculate the acceleration steps
|
1170
|
|
- long acceleration = block->acceleration;
|
|
1170
|
+ long acceleration = block->acceleration_st;
|
1171
|
1171
|
long accelerate_steps = estimate_acceleration_distance(initial_rate, block->nominal_rate, acceleration);
|
1172
|
1172
|
long decelerate_steps = estimate_acceleration_distance(final_rate, block->nominal_rate, acceleration);
|
1173
|
|
-
|
1174
|
1173
|
// Calculate the size of Plateau of Nominal Rate.
|
1175
|
1174
|
long plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps;
|
1176
|
1175
|
|
|
@@ -1214,15 +1213,15 @@ inline float max_allowable_speed(float acceleration, float target_velocity, floa
|
1214
|
1213
|
inline float junction_jerk(block_t *before, block_t *after) {
|
1215
|
1214
|
return(sqrt(
|
1216
|
1215
|
pow((before->speed_x-after->speed_x), 2)+
|
1217
|
|
- pow((before->speed_y-after->speed_y), 2)+
|
1218
|
|
- pow((before->speed_z-after->speed_z)*axis_steps_per_unit[Z_AXIS]/axis_steps_per_unit[X_AXIS], 2)));
|
|
1216
|
+ pow((before->speed_y-after->speed_y), 2)));
|
1219
|
1217
|
}
|
1220
|
1218
|
|
1221
|
1219
|
// Return the safe speed which is max_jerk/2, e.g. the
|
1222
|
1220
|
// speed under which you cannot exceed max_jerk no matter what you do.
|
1223
|
1221
|
float safe_speed(block_t *block) {
|
1224
|
1222
|
float safe_speed;
|
1225
|
|
- safe_speed = max_jerk/2;
|
|
1223
|
+ safe_speed = max_xy_jerk/2;
|
|
1224
|
+ if(abs(block->speed_z) > max_z_jerk/2) safe_speed = max_z_jerk/2;
|
1226
|
1225
|
if (safe_speed > block->nominal_speed) safe_speed = block->nominal_speed;
|
1227
|
1226
|
return safe_speed;
|
1228
|
1227
|
}
|
|
@@ -1250,12 +1249,15 @@ void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *n
|
1250
|
1249
|
if((previous->steps_x == 0) && (previous->steps_y == 0)) {
|
1251
|
1250
|
entry_speed = safe_speed(current);
|
1252
|
1251
|
}
|
1253
|
|
- else if (jerk > max_jerk) {
|
1254
|
|
- entry_speed = (max_jerk/jerk) * entry_speed;
|
|
1252
|
+ else if (jerk > max_xy_jerk) {
|
|
1253
|
+ entry_speed = (max_xy_jerk/jerk) * entry_speed;
|
1255
|
1254
|
}
|
|
1255
|
+ if(abs(previous->speed_z - current->speed_z) > max_z_jerk) {
|
|
1256
|
+ entry_speed = (max_z_jerk/abs(previous->speed_z - current->speed_z)) * entry_speed;
|
|
1257
|
+ }
|
1256
|
1258
|
// If the required deceleration across the block is too rapid, reduce the entry_factor accordingly.
|
1257
|
1259
|
if (entry_speed > exit_speed) {
|
1258
|
|
- float max_entry_speed = max_allowable_speed(-acceleration,exit_speed, current->millimeters);
|
|
1260
|
+ float max_entry_speed = max_allowable_speed(-current->acceleration,exit_speed, current->millimeters);
|
1259
|
1261
|
if (max_entry_speed < entry_speed) {
|
1260
|
1262
|
entry_speed = max_entry_speed;
|
1261
|
1263
|
}
|
|
@@ -1275,16 +1277,16 @@ void planner_reverse_pass() {
|
1275
|
1277
|
block_t *block[3] = {
|
1276
|
1278
|
NULL, NULL, NULL };
|
1277
|
1279
|
while(block_index != block_buffer_tail) {
|
1278
|
|
- block_index--;
|
1279
|
|
- if(block_index < 0) {
|
1280
|
|
- block_index = BLOCK_BUFFER_SIZE-1;
|
1281
|
|
- }
|
1282
|
1280
|
block[2]= block[1];
|
1283
|
1281
|
block[1]= block[0];
|
1284
|
1282
|
block[0] = &block_buffer[block_index];
|
1285
|
1283
|
planner_reverse_pass_kernel(block[0], block[1], block[2]);
|
|
1284
|
+ block_index--;
|
|
1285
|
+ if(block_index < 0) {
|
|
1286
|
+ block_index = BLOCK_BUFFER_SIZE-1;
|
|
1287
|
+ }
|
1286
|
1288
|
}
|
1287
|
|
- planner_reverse_pass_kernel(NULL, block[0], block[1]);
|
|
1289
|
+// planner_reverse_pass_kernel(NULL, block[0], block[1]);
|
1288
|
1290
|
}
|
1289
|
1291
|
|
1290
|
1292
|
// The kernel called by planner_recalculate() when scanning the plan from first to last entry.
|
|
@@ -1298,7 +1300,7 @@ void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *n
|
1298
|
1300
|
// speed accordingly. Remember current->entry_factor equals the exit factor of
|
1299
|
1301
|
// the previous block.
|
1300
|
1302
|
if(previous->entry_speed < current->entry_speed) {
|
1301
|
|
- float max_entry_speed = max_allowable_speed(-acceleration, previous->entry_speed, previous->millimeters);
|
|
1303
|
+ float max_entry_speed = max_allowable_speed(-previous->acceleration, previous->entry_speed, previous->millimeters);
|
1302
|
1304
|
if (max_entry_speed < current->entry_speed) {
|
1303
|
1305
|
current->entry_speed = max_entry_speed;
|
1304
|
1306
|
}
|
|
@@ -1422,7 +1424,7 @@ void plan_buffer_line(float x, float y, float z, float e, float feed_rate) {
|
1422
|
1424
|
target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
|
1423
|
1425
|
target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
|
1424
|
1426
|
target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
|
1425
|
|
-
|
|
1427
|
+
|
1426
|
1428
|
// Calculate the buffer head after we push this byte
|
1427
|
1429
|
int next_buffer_head = (block_buffer_head + 1) & BLOCK_BUFFER_MASK;
|
1428
|
1430
|
|
|
@@ -1450,6 +1452,12 @@ void plan_buffer_line(float x, float y, float z, float e, float feed_rate) {
|
1450
|
1452
|
if (block->step_event_count == 0) {
|
1451
|
1453
|
return;
|
1452
|
1454
|
};
|
|
1455
|
+
|
|
1456
|
+ //enable active axes
|
|
1457
|
+ if(block->steps_x != 0) enable_x();
|
|
1458
|
+ if(block->steps_y != 0) enable_y();
|
|
1459
|
+ if(block->steps_z != 0) enable_z();
|
|
1460
|
+ if(block->steps_e != 0) enable_e();
|
1453
|
1461
|
|
1454
|
1462
|
float delta_x_mm = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
|
1455
|
1463
|
float delta_y_mm = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
|
|
@@ -1492,7 +1500,7 @@ void plan_buffer_line(float x, float y, float z, float e, float feed_rate) {
|
1492
|
1500
|
block->speed_e = delta_e_mm * multiplier;
|
1493
|
1501
|
block->nominal_speed = block->millimeters * multiplier;
|
1494
|
1502
|
block->nominal_rate = ceil(block->step_event_count * multiplier / 60);
|
1495
|
|
-
|
|
1503
|
+
|
1496
|
1504
|
if(block->nominal_rate < 120) block->nominal_rate = 120;
|
1497
|
1505
|
block->entry_speed = safe_speed(block);
|
1498
|
1506
|
|
|
@@ -1502,18 +1510,19 @@ void plan_buffer_line(float x, float y, float z, float e, float feed_rate) {
|
1502
|
1510
|
block->acceleration = ceil( (retract_acceleration)/travel_per_step); // convert to: acceleration steps/sec^2
|
1503
|
1511
|
}
|
1504
|
1512
|
else {
|
1505
|
|
- block->acceleration = ceil( (acceleration)/travel_per_step); // convert to: acceleration steps/sec^2
|
|
1513
|
+ block->acceleration_st = ceil( (acceleration)/travel_per_step); // convert to: acceleration steps/sec^2
|
1506
|
1514
|
// Limit acceleration per axis
|
1507
|
|
- if((block->acceleration * block->steps_x / block->step_event_count) > axis_steps_per_sqr_second[X_AXIS])
|
1508
|
|
- block->acceleration = axis_steps_per_sqr_second[X_AXIS];
|
1509
|
|
- if((block->acceleration * block->steps_y / block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS])
|
1510
|
|
- block->acceleration = axis_steps_per_sqr_second[Y_AXIS];
|
1511
|
|
- if((block->acceleration * block->steps_e / block->step_event_count) > axis_steps_per_sqr_second[E_AXIS])
|
1512
|
|
- block->acceleration = axis_steps_per_sqr_second[E_AXIS];
|
1513
|
|
- if((block->acceleration * block->steps_z / block->step_event_count) > axis_steps_per_sqr_second[Z_AXIS])
|
1514
|
|
- block->acceleration = axis_steps_per_sqr_second[Z_AXIS];
|
|
1515
|
+ if((block->acceleration_st * block->steps_x / block->step_event_count) > axis_steps_per_sqr_second[X_AXIS])
|
|
1516
|
+ block->acceleration_st = axis_steps_per_sqr_second[X_AXIS];
|
|
1517
|
+ if((block->acceleration_st * block->steps_y / block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS])
|
|
1518
|
+ block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS];
|
|
1519
|
+ if((block->acceleration_st * block->steps_e / block->step_event_count) > axis_steps_per_sqr_second[E_AXIS])
|
|
1520
|
+ block->acceleration_st = axis_steps_per_sqr_second[E_AXIS];
|
|
1521
|
+ if(((block->acceleration_st / block->step_event_count) * block->steps_z ) > axis_steps_per_sqr_second[Z_AXIS])
|
|
1522
|
+ block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS];
|
1515
|
1523
|
}
|
1516
|
|
-
|
|
1524
|
+ block->acceleration = block->acceleration_st * travel_per_step;
|
|
1525
|
+
|
1517
|
1526
|
#ifdef ADVANCE
|
1518
|
1527
|
// Calculate advance rate
|
1519
|
1528
|
if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) {
|
|
@@ -1521,7 +1530,7 @@ void plan_buffer_line(float x, float y, float z, float e, float feed_rate) {
|
1521
|
1530
|
block->advance = 0;
|
1522
|
1531
|
}
|
1523
|
1532
|
else {
|
1524
|
|
- long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration);
|
|
1533
|
+ long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
|
1525
|
1534
|
float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) *
|
1526
|
1535
|
(block->speed_e * block->speed_e * EXTRUTION_AREA * EXTRUTION_AREA / 3600.0)*65536;
|
1527
|
1536
|
block->advance = advance;
|
|
@@ -1554,12 +1563,6 @@ void plan_buffer_line(float x, float y, float z, float e, float feed_rate) {
|
1554
|
1563
|
block->direction_bits |= (1<<E_AXIS);
|
1555
|
1564
|
}
|
1556
|
1565
|
|
1557
|
|
- //enable active axes
|
1558
|
|
- if(block->steps_x != 0) enable_x();
|
1559
|
|
- if(block->steps_y != 0) enable_y();
|
1560
|
|
- if(block->steps_z != 0) enable_z();
|
1561
|
|
- if(block->steps_e != 0) enable_e();
|
1562
|
|
-
|
1563
|
1566
|
// Move buffer head
|
1564
|
1567
|
block_buffer_head = next_buffer_head;
|
1565
|
1568
|
|
|
@@ -1729,6 +1732,7 @@ inline void trapezoid_generator_reset() {
|
1729
|
1732
|
final_advance = current_block->final_advance;
|
1730
|
1733
|
deceleration_time = 0;
|
1731
|
1734
|
advance_rate = current_block->advance_rate;
|
|
1735
|
+
|
1732
|
1736
|
// step_rate to timer interval
|
1733
|
1737
|
acc_step_rate = initial_rate;
|
1734
|
1738
|
acceleration_time = calc_timer(acc_step_rate);
|