|
@@ -435,7 +435,8 @@ void getHighESpeed()
|
435
|
435
|
}
|
436
|
436
|
#endif
|
437
|
437
|
|
438
|
|
-void check_axes_activity() {
|
|
438
|
+void check_axes_activity()
|
|
439
|
+{
|
439
|
440
|
unsigned char x_active = 0;
|
440
|
441
|
unsigned char y_active = 0;
|
441
|
442
|
unsigned char z_active = 0;
|
|
@@ -444,10 +445,12 @@ void check_axes_activity() {
|
444
|
445
|
unsigned char tail_fan_speed = 0;
|
445
|
446
|
block_t *block;
|
446
|
447
|
|
447
|
|
- if(block_buffer_tail != block_buffer_head) {
|
|
448
|
+ if(block_buffer_tail != block_buffer_head)
|
|
449
|
+ {
|
448
|
450
|
uint8_t block_index = block_buffer_tail;
|
449
|
451
|
tail_fan_speed = block_buffer[block_index].fan_speed;
|
450
|
|
- while(block_index != block_buffer_head) {
|
|
452
|
+ while(block_index != block_buffer_head)
|
|
453
|
+ {
|
451
|
454
|
block = &block_buffer[block_index];
|
452
|
455
|
if(block->steps_x != 0) x_active++;
|
453
|
456
|
if(block->steps_y != 0) y_active++;
|
|
@@ -457,27 +460,31 @@ void check_axes_activity() {
|
457
|
460
|
block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1);
|
458
|
461
|
}
|
459
|
462
|
}
|
460
|
|
- else {
|
461
|
|
-#if FAN_PIN > -1
|
|
463
|
+ else
|
|
464
|
+ {
|
|
465
|
+ #if FAN_PIN > -1
|
462
|
466
|
if (FanSpeed != 0){
|
463
|
467
|
analogWrite(FAN_PIN,FanSpeed); // If buffer is empty use current fan speed
|
464
|
468
|
}
|
465
|
|
-#endif
|
|
469
|
+ #endif
|
466
|
470
|
}
|
467
|
471
|
if((DISABLE_X) && (x_active == 0)) disable_x();
|
468
|
472
|
if((DISABLE_Y) && (y_active == 0)) disable_y();
|
469
|
473
|
if((DISABLE_Z) && (z_active == 0)) disable_z();
|
470
|
|
- if((DISABLE_E) && (e_active == 0)) {
|
|
474
|
+ if((DISABLE_E) && (e_active == 0))
|
|
475
|
+ {
|
471
|
476
|
disable_e0();
|
472
|
477
|
disable_e1();
|
473
|
478
|
disable_e2();
|
474
|
479
|
}
|
475
|
480
|
#if FAN_PIN > -1
|
476
|
|
- if((FanSpeed == 0) && (fan_speed ==0)) {
|
|
481
|
+ if((FanSpeed == 0) && (fan_speed ==0))
|
|
482
|
+ {
|
477
|
483
|
analogWrite(FAN_PIN, 0);
|
478
|
484
|
}
|
479
|
485
|
|
480
|
|
- if (FanSpeed != 0 && tail_fan_speed !=0) {
|
|
486
|
+ if (FanSpeed != 0 && tail_fan_speed !=0)
|
|
487
|
+ {
|
481
|
488
|
analogWrite(FAN_PIN,tail_fan_speed);
|
482
|
489
|
}
|
483
|
490
|
#endif
|
|
@@ -498,7 +505,8 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
|
498
|
505
|
|
499
|
506
|
// If the buffer is full: good! That means we are well ahead of the robot.
|
500
|
507
|
// Rest here until there is room in the buffer.
|
501
|
|
- while(block_buffer_tail == next_buffer_head) {
|
|
508
|
+ while(block_buffer_tail == next_buffer_head)
|
|
509
|
+ {
|
502
|
510
|
manage_heater();
|
503
|
511
|
manage_inactivity();
|
504
|
512
|
LCD_STATUS;
|
|
@@ -513,23 +521,26 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
|
513
|
521
|
target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
|
514
|
522
|
target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
|
515
|
523
|
|
516
|
|
-#ifdef PREVENT_DANGEROUS_EXTRUDE
|
517
|
|
- if(target[E_AXIS]!=position[E_AXIS])
|
|
524
|
+ #ifdef PREVENT_DANGEROUS_EXTRUDE
|
|
525
|
+ if(target[E_AXIS]!=position[E_AXIS])
|
|
526
|
+ {
|
518
|
527
|
if(degHotend(active_extruder)<EXTRUDE_MINTEMP && !allow_cold_extrude)
|
519
|
528
|
{
|
520
|
529
|
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
|
521
|
530
|
SERIAL_ECHO_START;
|
522
|
531
|
SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
|
523
|
532
|
}
|
524
|
|
-#ifdef PREVENT_LENGTHY_EXTRUDE
|
525
|
|
- if(labs(target[E_AXIS]-position[E_AXIS])>axis_steps_per_unit[E_AXIS]*EXTRUDE_MAXLENGTH)
|
526
|
|
- {
|
527
|
|
- position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
|
528
|
|
- SERIAL_ECHO_START;
|
529
|
|
- SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
|
|
533
|
+
|
|
534
|
+ #ifdef PREVENT_LENGTHY_EXTRUDE
|
|
535
|
+ if(labs(target[E_AXIS]-position[E_AXIS])>axis_steps_per_unit[E_AXIS]*EXTRUDE_MAXLENGTH)
|
|
536
|
+ {
|
|
537
|
+ position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
|
|
538
|
+ SERIAL_ECHO_START;
|
|
539
|
+ SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
|
|
540
|
+ }
|
|
541
|
+ #endif
|
530
|
542
|
}
|
531
|
|
-#endif
|
532
|
|
-#endif
|
|
543
|
+ #endif
|
533
|
544
|
|
534
|
545
|
// Prepare to set up new block
|
535
|
546
|
block_t *block = &block_buffer[block_buffer_head];
|
|
@@ -547,24 +558,29 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
|
547
|
558
|
block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e)));
|
548
|
559
|
|
549
|
560
|
// Bail if this is a zero-length block
|
550
|
|
- if (block->step_event_count <= dropsegments) {
|
|
561
|
+ if (block->step_event_count <= dropsegments)
|
|
562
|
+ {
|
551
|
563
|
return;
|
552
|
|
- };
|
|
564
|
+ }
|
553
|
565
|
|
554
|
566
|
block->fan_speed = FanSpeed;
|
555
|
567
|
|
556
|
568
|
// Compute direction bits for this block
|
557
|
569
|
block->direction_bits = 0;
|
558
|
|
- if (target[X_AXIS] < position[X_AXIS]) {
|
|
570
|
+ if (target[X_AXIS] < position[X_AXIS])
|
|
571
|
+ {
|
559
|
572
|
block->direction_bits |= (1<<X_AXIS);
|
560
|
573
|
}
|
561
|
|
- if (target[Y_AXIS] < position[Y_AXIS]) {
|
|
574
|
+ if (target[Y_AXIS] < position[Y_AXIS])
|
|
575
|
+ {
|
562
|
576
|
block->direction_bits |= (1<<Y_AXIS);
|
563
|
577
|
}
|
564
|
|
- if (target[Z_AXIS] < position[Z_AXIS]) {
|
|
578
|
+ if (target[Z_AXIS] < position[Z_AXIS])
|
|
579
|
+ {
|
565
|
580
|
block->direction_bits |= (1<<Z_AXIS);
|
566
|
581
|
}
|
567
|
|
- if (target[E_AXIS] < position[E_AXIS]) {
|
|
582
|
+ if (target[E_AXIS] < position[E_AXIS])
|
|
583
|
+ {
|
568
|
584
|
block->direction_bits |= (1<<E_AXIS);
|
569
|
585
|
}
|
570
|
586
|
|
|
@@ -578,16 +594,19 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
|
578
|
594
|
#endif
|
579
|
595
|
|
580
|
596
|
// Enable all
|
581
|
|
- if(block->steps_e != 0) {
|
|
597
|
+ if(block->steps_e != 0)
|
|
598
|
+ {
|
582
|
599
|
enable_e0();
|
583
|
600
|
enable_e1();
|
584
|
601
|
enable_e2();
|
585
|
602
|
}
|
586
|
603
|
|
587
|
|
- if (block->steps_e == 0) {
|
|
604
|
+ if (block->steps_e == 0)
|
|
605
|
+ {
|
588
|
606
|
if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
|
589
|
607
|
}
|
590
|
|
- else {
|
|
608
|
+ else
|
|
609
|
+ {
|
591
|
610
|
if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
|
592
|
611
|
}
|
593
|
612
|
|
|
@@ -596,10 +615,12 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
|
596
|
615
|
delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
|
597
|
616
|
delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];
|
598
|
617
|
delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*extrudemultiply/100.0;
|
599
|
|
- if ( block->steps_x <=dropsegments && block->steps_y <=dropsegments && block->steps_z <=dropsegments ) {
|
|
618
|
+ if ( block->steps_x <=dropsegments && block->steps_y <=dropsegments && block->steps_z <=dropsegments )
|
|
619
|
+ {
|
600
|
620
|
block->millimeters = fabs(delta_mm[E_AXIS]);
|
601
|
621
|
}
|
602
|
|
- else {
|
|
622
|
+ else
|
|
623
|
+ {
|
603
|
624
|
block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
|
604
|
625
|
}
|
605
|
626
|
float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides
|
|
@@ -611,14 +632,17 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
|
611
|
632
|
|
612
|
633
|
// slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill
|
613
|
634
|
#ifdef OLD_SLOWDOWN
|
614
|
|
- if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1) feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5);
|
|
635
|
+ if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1)
|
|
636
|
+ feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5);
|
615
|
637
|
#endif
|
616
|
638
|
|
617
|
639
|
#ifdef SLOWDOWN
|
618
|
640
|
// segment time im micro seconds
|
619
|
641
|
unsigned long segment_time = lround(1000000.0/inverse_second);
|
620
|
|
- if ((moves_queued > 1) && (moves_queued < (BLOCK_BUFFER_SIZE * 0.5))) {
|
621
|
|
- if (segment_time < minsegmenttime) { // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
|
|
642
|
+ if ((moves_queued > 1) && (moves_queued < (BLOCK_BUFFER_SIZE * 0.5)))
|
|
643
|
+ {
|
|
644
|
+ if (segment_time < minsegmenttime)
|
|
645
|
+ { // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
|
622
|
646
|
inverse_second=1000000.0/(segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued));
|
623
|
647
|
}
|
624
|
648
|
}
|
|
@@ -632,7 +656,8 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
|
632
|
656
|
// Calculate and limit speed in mm/sec for each axis
|
633
|
657
|
float current_speed[4];
|
634
|
658
|
float speed_factor = 1.0; //factor <=1 do decrease speed
|
635
|
|
- for(int i=0; i < 4; i++) {
|
|
659
|
+ for(int i=0; i < 4; i++)
|
|
660
|
+ {
|
636
|
661
|
current_speed[i] = delta_mm[i] * inverse_second;
|
637
|
662
|
if(fabs(current_speed[i]) > max_feedrate[i])
|
638
|
663
|
speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i]));
|
|
@@ -646,18 +671,22 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
|
646
|
671
|
unsigned char direction_change = block->direction_bits ^ old_direction_bits;
|
647
|
672
|
old_direction_bits = block->direction_bits;
|
648
|
673
|
|
649
|
|
- if((direction_change & (1<<X_AXIS)) == 0) {
|
|
674
|
+ if((direction_change & (1<<X_AXIS)) == 0)
|
|
675
|
+ {
|
650
|
676
|
x_segment_time[0] += segment_time;
|
651
|
677
|
}
|
652
|
|
- else {
|
|
678
|
+ else
|
|
679
|
+ {
|
653
|
680
|
x_segment_time[2] = x_segment_time[1];
|
654
|
681
|
x_segment_time[1] = x_segment_time[0];
|
655
|
682
|
x_segment_time[0] = segment_time;
|
656
|
683
|
}
|
657
|
|
- if((direction_change & (1<<Y_AXIS)) == 0) {
|
|
684
|
+ if((direction_change & (1<<Y_AXIS)) == 0)
|
|
685
|
+ {
|
658
|
686
|
y_segment_time[0] += segment_time;
|
659
|
687
|
}
|
660
|
|
- else {
|
|
688
|
+ else
|
|
689
|
+ {
|
661
|
690
|
y_segment_time[2] = y_segment_time[1];
|
662
|
691
|
y_segment_time[1] = y_segment_time[0];
|
663
|
692
|
y_segment_time[0] = segment_time;
|
|
@@ -665,12 +694,15 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
|
665
|
694
|
long max_x_segment_time = max(x_segment_time[0], max(x_segment_time[1], x_segment_time[2]));
|
666
|
695
|
long max_y_segment_time = max(y_segment_time[0], max(y_segment_time[1], y_segment_time[2]));
|
667
|
696
|
long min_xy_segment_time =min(max_x_segment_time, max_y_segment_time);
|
668
|
|
- if(min_xy_segment_time < MAX_FREQ_TIME) speed_factor = min(speed_factor, speed_factor * (float)min_xy_segment_time / (float)MAX_FREQ_TIME);
|
|
697
|
+ if(min_xy_segment_time < MAX_FREQ_TIME)
|
|
698
|
+ speed_factor = min(speed_factor, speed_factor * (float)min_xy_segment_time / (float)MAX_FREQ_TIME);
|
669
|
699
|
#endif
|
670
|
700
|
|
671
|
701
|
// Correct the speed
|
672
|
|
- if( speed_factor < 1.0) {
|
673
|
|
- for(unsigned char i=0; i < 4; i++) {
|
|
702
|
+ if( speed_factor < 1.0)
|
|
703
|
+ {
|
|
704
|
+ for(unsigned char i=0; i < 4; i++)
|
|
705
|
+ {
|
674
|
706
|
current_speed[i] *= speed_factor;
|
675
|
707
|
}
|
676
|
708
|
block->nominal_speed *= speed_factor;
|
|
@@ -679,10 +711,12 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
|
679
|
711
|
|
680
|
712
|
// Compute and limit the acceleration rate for the trapezoid generator.
|
681
|
713
|
float steps_per_mm = block->step_event_count/block->millimeters;
|
682
|
|
- if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0) {
|
|
714
|
+ if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)
|
|
715
|
+ {
|
683
|
716
|
block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
|
684
|
717
|
}
|
685
|
|
- else {
|
|
718
|
+ else
|
|
719
|
+ {
|
686
|
720
|
block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
|
687
|
721
|
// Limit acceleration per axis
|
688
|
722
|
if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS])
|