|
@@ -103,12 +103,11 @@ volatile unsigned char block_buffer_tail; // Index of the block to pro
|
103
|
103
|
bool allow_cold_extrude=false;
|
104
|
104
|
#endif
|
105
|
105
|
#ifdef XY_FREQUENCY_LIMIT
|
|
106
|
+#define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT)
|
106
|
107
|
// Used for the frequency limit
|
107
|
108
|
static unsigned char old_direction_bits = 0; // Old direction bits. Used for speed calculations
|
108
|
|
-static long x_segment_time[3]={
|
109
|
|
- 0,0,0}; // Segment times (in us). Used for speed calculations
|
110
|
|
-static long y_segment_time[3]={
|
111
|
|
- 0,0,0};
|
|
109
|
+static long x_segment_time[3]={MAX_FREQ_TIME + 1,0,0}; // Segment times (in us). Used for speed calculations
|
|
110
|
+static long y_segment_time[3]={MAX_FREQ_TIME + 1,0,0};
|
112
|
111
|
#endif
|
113
|
112
|
|
114
|
113
|
// Returns the index of the next block in the ring buffer
|
|
@@ -435,7 +434,7 @@ void getHighESpeed()
|
435
|
434
|
}
|
436
|
435
|
#endif
|
437
|
436
|
|
438
|
|
-void check_axes_activity()
|
|
437
|
+void check_axes_activity()
|
439
|
438
|
{
|
440
|
439
|
unsigned char x_active = 0;
|
441
|
440
|
unsigned char y_active = 0;
|
|
@@ -445,11 +444,11 @@ void check_axes_activity()
|
445
|
444
|
unsigned char tail_fan_speed = 0;
|
446
|
445
|
block_t *block;
|
447
|
446
|
|
448
|
|
- if(block_buffer_tail != block_buffer_head)
|
|
447
|
+ if(block_buffer_tail != block_buffer_head)
|
449
|
448
|
{
|
450
|
449
|
uint8_t block_index = block_buffer_tail;
|
451
|
450
|
tail_fan_speed = block_buffer[block_index].fan_speed;
|
452
|
|
- while(block_index != block_buffer_head)
|
|
451
|
+ while(block_index != block_buffer_head)
|
453
|
452
|
{
|
454
|
453
|
block = &block_buffer[block_index];
|
455
|
454
|
if(block->steps_x != 0) x_active++;
|
|
@@ -460,7 +459,7 @@ void check_axes_activity()
|
460
|
459
|
block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1);
|
461
|
460
|
}
|
462
|
461
|
}
|
463
|
|
- else
|
|
462
|
+ else
|
464
|
463
|
{
|
465
|
464
|
#if FAN_PIN > -1
|
466
|
465
|
if (FanSpeed != 0){
|
|
@@ -471,19 +470,19 @@ void check_axes_activity()
|
471
|
470
|
if((DISABLE_X) && (x_active == 0)) disable_x();
|
472
|
471
|
if((DISABLE_Y) && (y_active == 0)) disable_y();
|
473
|
472
|
if((DISABLE_Z) && (z_active == 0)) disable_z();
|
474
|
|
- if((DISABLE_E) && (e_active == 0))
|
|
473
|
+ if((DISABLE_E) && (e_active == 0))
|
475
|
474
|
{
|
476
|
475
|
disable_e0();
|
477
|
476
|
disable_e1();
|
478
|
477
|
disable_e2();
|
479
|
478
|
}
|
480
|
479
|
#if FAN_PIN > -1
|
481
|
|
- if((FanSpeed == 0) && (fan_speed ==0))
|
|
480
|
+ if((FanSpeed == 0) && (fan_speed ==0))
|
482
|
481
|
{
|
483
|
482
|
analogWrite(FAN_PIN, 0);
|
484
|
483
|
}
|
485
|
484
|
|
486
|
|
- if (FanSpeed != 0 && tail_fan_speed !=0)
|
|
485
|
+ if (FanSpeed != 0 && tail_fan_speed !=0)
|
487
|
486
|
{
|
488
|
487
|
analogWrite(FAN_PIN,tail_fan_speed);
|
489
|
488
|
}
|
|
@@ -505,7 +504,7 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
|
505
|
504
|
|
506
|
505
|
// If the buffer is full: good! That means we are well ahead of the robot.
|
507
|
506
|
// Rest here until there is room in the buffer.
|
508
|
|
- while(block_buffer_tail == next_buffer_head)
|
|
507
|
+ while(block_buffer_tail == next_buffer_head)
|
509
|
508
|
{
|
510
|
509
|
manage_heater();
|
511
|
510
|
manage_inactivity();
|
|
@@ -522,7 +521,7 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
|
522
|
521
|
target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
|
523
|
522
|
|
524
|
523
|
#ifdef PREVENT_DANGEROUS_EXTRUDE
|
525
|
|
- if(target[E_AXIS]!=position[E_AXIS])
|
|
524
|
+ if(target[E_AXIS]!=position[E_AXIS])
|
526
|
525
|
{
|
527
|
526
|
if(degHotend(active_extruder)<EXTRUDE_MINTEMP && !allow_cold_extrude)
|
528
|
527
|
{
|
|
@@ -530,7 +529,7 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
|
530
|
529
|
SERIAL_ECHO_START;
|
531
|
530
|
SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
|
532
|
531
|
}
|
533
|
|
-
|
|
532
|
+
|
534
|
533
|
#ifdef PREVENT_LENGTHY_EXTRUDE
|
535
|
534
|
if(labs(target[E_AXIS]-position[E_AXIS])>axis_steps_per_unit[E_AXIS]*EXTRUDE_MAXLENGTH)
|
536
|
535
|
{
|
|
@@ -538,7 +537,7 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
|
538
|
537
|
SERIAL_ECHO_START;
|
539
|
538
|
SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
|
540
|
539
|
}
|
541
|
|
- #endif
|
|
540
|
+ #endif
|
542
|
541
|
}
|
543
|
542
|
#endif
|
544
|
543
|
|
|
@@ -558,7 +557,7 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
|
558
|
557
|
block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e)));
|
559
|
558
|
|
560
|
559
|
// Bail if this is a zero-length block
|
561
|
|
- if (block->step_event_count <= dropsegments)
|
|
560
|
+ if (block->step_event_count <= dropsegments)
|
562
|
561
|
{
|
563
|
562
|
return;
|
564
|
563
|
}
|
|
@@ -567,19 +566,19 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
|
567
|
566
|
|
568
|
567
|
// Compute direction bits for this block
|
569
|
568
|
block->direction_bits = 0;
|
570
|
|
- if (target[X_AXIS] < position[X_AXIS])
|
|
569
|
+ if (target[X_AXIS] < position[X_AXIS])
|
571
|
570
|
{
|
572
|
571
|
block->direction_bits |= (1<<X_AXIS);
|
573
|
572
|
}
|
574
|
|
- if (target[Y_AXIS] < position[Y_AXIS])
|
|
573
|
+ if (target[Y_AXIS] < position[Y_AXIS])
|
575
|
574
|
{
|
576
|
575
|
block->direction_bits |= (1<<Y_AXIS);
|
577
|
576
|
}
|
578
|
|
- if (target[Z_AXIS] < position[Z_AXIS])
|
|
577
|
+ if (target[Z_AXIS] < position[Z_AXIS])
|
579
|
578
|
{
|
580
|
579
|
block->direction_bits |= (1<<Z_AXIS);
|
581
|
580
|
}
|
582
|
|
- if (target[E_AXIS] < position[E_AXIS])
|
|
581
|
+ if (target[E_AXIS] < position[E_AXIS])
|
583
|
582
|
{
|
584
|
583
|
block->direction_bits |= (1<<E_AXIS);
|
585
|
584
|
}
|
|
@@ -594,18 +593,18 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
|
594
|
593
|
#endif
|
595
|
594
|
|
596
|
595
|
// Enable all
|
597
|
|
- if(block->steps_e != 0)
|
|
596
|
+ if(block->steps_e != 0)
|
598
|
597
|
{
|
599
|
598
|
enable_e0();
|
600
|
599
|
enable_e1();
|
601
|
600
|
enable_e2();
|
602
|
601
|
}
|
603
|
602
|
|
604
|
|
- if (block->steps_e == 0)
|
|
603
|
+ if (block->steps_e == 0)
|
605
|
604
|
{
|
606
|
605
|
if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
|
607
|
606
|
}
|
608
|
|
- else
|
|
607
|
+ else
|
609
|
608
|
{
|
610
|
609
|
if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
|
611
|
610
|
}
|
|
@@ -615,11 +614,11 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
|
615
|
614
|
delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
|
616
|
615
|
delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];
|
617
|
616
|
delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*extrudemultiply/100.0;
|
618
|
|
- if ( block->steps_x <=dropsegments && block->steps_y <=dropsegments && block->steps_z <=dropsegments )
|
|
617
|
+ if ( block->steps_x <=dropsegments && block->steps_y <=dropsegments && block->steps_z <=dropsegments )
|
619
|
618
|
{
|
620
|
619
|
block->millimeters = fabs(delta_mm[E_AXIS]);
|
621
|
620
|
}
|
622
|
|
- else
|
|
621
|
+ else
|
623
|
622
|
{
|
624
|
623
|
block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
|
625
|
624
|
}
|
|
@@ -632,18 +631,21 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
|
632
|
631
|
|
633
|
632
|
// slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill
|
634
|
633
|
#ifdef OLD_SLOWDOWN
|
635
|
|
- if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1)
|
|
634
|
+ if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1)
|
636
|
635
|
feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5);
|
637
|
636
|
#endif
|
638
|
637
|
|
639
|
638
|
#ifdef SLOWDOWN
|
640
|
639
|
// segment time im micro seconds
|
641
|
640
|
unsigned long segment_time = lround(1000000.0/inverse_second);
|
642
|
|
- if ((moves_queued > 1) && (moves_queued < (BLOCK_BUFFER_SIZE * 0.5)))
|
|
641
|
+ if ((moves_queued > 1) && (moves_queued < (BLOCK_BUFFER_SIZE * 0.5)))
|
643
|
642
|
{
|
644
|
|
- if (segment_time < minsegmenttime)
|
|
643
|
+ if (segment_time < minsegmenttime)
|
645
|
644
|
{ // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
|
646
|
645
|
inverse_second=1000000.0/(segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued));
|
|
646
|
+ #ifdef XY_FREQUENCY_LIMIT
|
|
647
|
+ segment_time = lround(1000000.0/inverse_second);
|
|
648
|
+ #endif
|
647
|
649
|
}
|
648
|
650
|
}
|
649
|
651
|
#endif
|
|
@@ -656,7 +658,7 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
|
656
|
658
|
// Calculate and limit speed in mm/sec for each axis
|
657
|
659
|
float current_speed[4];
|
658
|
660
|
float speed_factor = 1.0; //factor <=1 do decrease speed
|
659
|
|
- for(int i=0; i < 4; i++)
|
|
661
|
+ for(int i=0; i < 4; i++)
|
660
|
662
|
{
|
661
|
663
|
current_speed[i] = delta_mm[i] * inverse_second;
|
662
|
664
|
if(fabs(current_speed[i]) > max_feedrate[i])
|
|
@@ -666,26 +668,26 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
|
666
|
668
|
// Max segement time in us.
|
667
|
669
|
#ifdef XY_FREQUENCY_LIMIT
|
668
|
670
|
#define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT)
|
669
|
|
-
|
670
|
671
|
// Check and limit the xy direction change frequency
|
671
|
672
|
unsigned char direction_change = block->direction_bits ^ old_direction_bits;
|
672
|
673
|
old_direction_bits = block->direction_bits;
|
673
|
|
-
|
674
|
|
- if((direction_change & (1<<X_AXIS)) == 0)
|
|
674
|
+ segment_time = lround((float)segment_time / speed_factor);
|
|
675
|
+
|
|
676
|
+ if((direction_change & (1<<X_AXIS)) == 0)
|
675
|
677
|
{
|
676
|
678
|
x_segment_time[0] += segment_time;
|
677
|
679
|
}
|
678
|
|
- else
|
|
680
|
+ else
|
679
|
681
|
{
|
680
|
682
|
x_segment_time[2] = x_segment_time[1];
|
681
|
683
|
x_segment_time[1] = x_segment_time[0];
|
682
|
684
|
x_segment_time[0] = segment_time;
|
683
|
685
|
}
|
684
|
|
- if((direction_change & (1<<Y_AXIS)) == 0)
|
|
686
|
+ if((direction_change & (1<<Y_AXIS)) == 0)
|
685
|
687
|
{
|
686
|
688
|
y_segment_time[0] += segment_time;
|
687
|
689
|
}
|
688
|
|
- else
|
|
690
|
+ else
|
689
|
691
|
{
|
690
|
692
|
y_segment_time[2] = y_segment_time[1];
|
691
|
693
|
y_segment_time[1] = y_segment_time[0];
|
|
@@ -694,14 +696,14 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
|
694
|
696
|
long max_x_segment_time = max(x_segment_time[0], max(x_segment_time[1], x_segment_time[2]));
|
695
|
697
|
long max_y_segment_time = max(y_segment_time[0], max(y_segment_time[1], y_segment_time[2]));
|
696
|
698
|
long min_xy_segment_time =min(max_x_segment_time, max_y_segment_time);
|
697
|
|
- if(min_xy_segment_time < MAX_FREQ_TIME)
|
|
699
|
+ if(min_xy_segment_time < MAX_FREQ_TIME)
|
698
|
700
|
speed_factor = min(speed_factor, speed_factor * (float)min_xy_segment_time / (float)MAX_FREQ_TIME);
|
699
|
701
|
#endif
|
700
|
702
|
|
701
|
703
|
// Correct the speed
|
702
|
|
- if( speed_factor < 1.0)
|
|
704
|
+ if( speed_factor < 1.0)
|
703
|
705
|
{
|
704
|
|
- for(unsigned char i=0; i < 4; i++)
|
|
706
|
+ for(unsigned char i=0; i < 4; i++)
|
705
|
707
|
{
|
706
|
708
|
current_speed[i] *= speed_factor;
|
707
|
709
|
}
|
|
@@ -711,11 +713,11 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
|
711
|
713
|
|
712
|
714
|
// Compute and limit the acceleration rate for the trapezoid generator.
|
713
|
715
|
float steps_per_mm = block->step_event_count/block->millimeters;
|
714
|
|
- if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)
|
|
716
|
+ if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)
|
715
|
717
|
{
|
716
|
718
|
block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
|
717
|
719
|
}
|
718
|
|
- else
|
|
720
|
+ else
|
719
|
721
|
{
|
720
|
722
|
block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
|
721
|
723
|
// Limit acceleration per axis
|