forked from roboard/Print3D
/
g_code.cpp
1588 lines (1412 loc) · 50 KB
/
g_code.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
#include "g_code.h"
#include "planner.h"
#include "command.h"
#include "protocol.h"
#include "communication.h"
#include "temperature.h"
#include "vector_3.h"
#include "qr_solve.h"
#include "global_setting.h"
#include <Arduino.h>
/************g_code private variables************/
/*****NFA*****/
#define SYS_STATE_NORMAL (0x00)
#define SYS_STATE_HOMING (0x01)
#define SYS_STATE_Z_PROBE (0x02)
#define SYS_STATE_SINGLE_Z_PROBE (0x03)
#define SYS_STATE_ALARM (0x04)
#define SYS_STATE_SET_TEMP (0x05)
#define SYS_STATE_SET_FAN (0x06)
#define SYS_STATE_PID_AUTOTUNE (0x07)
#define SYS_STATE_DISABLE_MOTOR (0x08)
#define SYS_STATE_ARC (0x09)
#define SYS_STATE_CREATE_TEMP_TABLE (0x0A)
#define SYS_STATE_SELECT_TEMP_TABLE (0x0B)
static int sys_state = SYS_STATE_NORMAL; //紀錄目前NFA的狀態
/*****end NFA*****/
/*****CMD*****/
static char *cmdbuffer; //存放一條G code的buffer
static char *strchr_pointer; //讀取數字所需記錄的起始位置
/*****end CMD*****/
/*****水平校正*****/
// "A" matrix of the linear system of equations
static double *eqnAMatrix; //水平校正使用的變數
// "B" vector of Z points
static double *eqnBVector; //水平校正使用的變數
/*****end 水平校正*****/
static bool relative_mode = false; //Determines Absolute or Relative Coordinates
const char *axis_codes = "XYZE";
static double *destination;
static double offset[3] = {0.0, 0.0, 0.0};
double *current_position;
double add_homeing[3]={0,0,0};
bool *axis_relative_modes;
static double feedrate = 1500.0, next_feedrate, saved_feedrate;
unsigned char active_extruder = 0;
int saved_feedmultiply;
int feedmultiply = 100; //100->1 200->2
static bool home_all_axis = true;
double *homing_feedrate; // set the homing speeds (mm/min)
bool axis_known_position[3] = {false, false, false};
double min_pos[3];// = { machine->x_min_pos, machine->y_min_pos, machine->z_min_pos };
double max_pos[3];// = { machine->x_max_pos, machine->y_max_pos, machine->z_max_pos };
double zprobe_zoffset;
int get_command() {
int serial_count = 0;
char serial_char;
while((serial_char = popCommandChr()) != '\0') {
if(serial_char == -1 || serial_count >= sys->max_cmd_size) {
// error mag
return -1;
}
cmdbuffer[serial_count++] = serial_char;
}
cmdbuffer[serial_count++] = '\0';
return 0;
}
double code_value() {
return (strtod(&cmdbuffer[strchr_pointer - cmdbuffer + 1], NULL));
}
long code_value_long() {
return (strtol(&cmdbuffer[strchr_pointer - cmdbuffer + 1], NULL, 10));
}
bool code_seen(char code) {
strchr_pointer = strchr(cmdbuffer, code);
return (strchr_pointer != NULL); //Return True if a character was found
}
void get_coordinates() {
bool seen[4]={false,false,false,false};
for(int i=0; i < machine->num_axis; i++) {
if(code_seen(axis_codes[i])) {
destination[i] = (double)code_value() + (axis_relative_modes[i] || relative_mode)*current_position[i];
seen[i]=true;
} else {
destination[i] = current_position[i]; //Are these else lines really needed?
}
}
if(code_seen('F')) {
next_feedrate = code_value();
if(next_feedrate > 0.0) feedrate = next_feedrate;
}
}
void get_arc_coordinates() {
get_coordinates();
if(code_seen('I')) {
offset[0] = code_value();
} else {
offset[0] = 0.0;
}
if(code_seen('J')) {
offset[1] = code_value();
} else {
offset[1] = 0.0;
}
}
void clamp_to_software_endstops(double target[]) {
if (machine->min_software_limit == 1) {
if (target[X_AXIS] < min_pos[X_AXIS]) target[X_AXIS] = min_pos[X_AXIS];
if (target[Y_AXIS] < min_pos[Y_AXIS]) target[Y_AXIS] = min_pos[Y_AXIS];
if (target[Z_AXIS] < min_pos[Z_AXIS]) target[Z_AXIS] = min_pos[Z_AXIS];
}
if (machine->max_software_limit == 1) {
if (target[X_AXIS] > max_pos[X_AXIS]) target[X_AXIS] = max_pos[X_AXIS];
if (target[Y_AXIS] > max_pos[Y_AXIS]) target[Y_AXIS] = max_pos[Y_AXIS];
if (target[Z_AXIS] > max_pos[Z_AXIS]) target[Z_AXIS] = max_pos[Z_AXIS];
}
}
void prepare_move() {
clamp_to_software_endstops(destination);
// Do not use feedmultiply for E or Z only moves
if( (current_position[X_AXIS] == destination[X_AXIS]) && (current_position[Y_AXIS] == destination[Y_AXIS])) {
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
} else {
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
}
for(int i=0; i < machine->num_axis; i++) {
current_position[i] = destination[i];
}
}
#define ARC_CW (0)
#define ARC_CCW (1)
static char clockflag;
void prepare_arc_move() {
static int arc_state = 0;
double r;
static double feed_rate;
static double center_axis0;
static double center_axis1;
double linear_travel;
double extruder_travel;
static double r_axis0;
static double r_axis1;
double rt_axis0;
double rt_axis1;
double angular_travel;
double millimeters_of_travel;
static unsigned int segments;
static double theta_per_segment;
static double linear_per_segment;
static double extruder_per_segment;
static double cos_T;
static double sin_T;
static double arc_target[4];
static double sin_Ti = 0.0;
static double cos_Ti = 0.0;
static double r_axisi = 0.0;
static unsigned int i = 1;
static char count = 0;
unsigned int bound;
switch (arc_state)
{
case 0:
r = hypot(offset[X_AXIS], offset[Y_AXIS]); // Compute arc radius for mc_arc
feed_rate = feedrate*feedmultiply/60/100.0;
// Trace the arc
// int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled();
// plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc
center_axis0 = current_position[X_AXIS] + offset[X_AXIS];
center_axis1 = current_position[Y_AXIS] + offset[Y_AXIS];
linear_travel = destination[Z_AXIS] - current_position[Z_AXIS];
extruder_travel = destination[E_AXIS] - current_position[E_AXIS];
r_axis0 = -offset[X_AXIS]; // Radius vector from center to current location
r_axis1 = -offset[Y_AXIS];
rt_axis0 = destination[X_AXIS] - center_axis0;
rt_axis1 = destination[Y_AXIS] - center_axis1;
// CCW angle between position and target from circle center. Only one atan2() trig computation required.
angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
//if (angular_travel < 0) { angular_travel += 2*M_PI; }
//if (clockflag == ARC_CCW) { angular_travel -= 2*M_PI; }
if (clockflag == ARC_CW) { // Correct atan2 output per direction
if (angular_travel >= 0) { angular_travel -= 2*M_PI; }
} else {
if (angular_travel <= 0) { angular_travel += 2*M_PI; }
}
millimeters_of_travel = hypot(angular_travel*r, fabs(linear_travel));
if (millimeters_of_travel < 0.001) {
arc_state = 2;
return;
}
segments = floor(millimeters_of_travel/machine->mm_per_arc_segment);
if(segments == 0) segments = 1;
/*
// Multiply inverse feed_rate to compensate for the fact that this movement is approximated
// by a number of discrete segments. The inverse feed_rate should be correct for the sum of
// all segments.
if (invert_feed_rate) { feed_rate *= segments; }
*/
theta_per_segment = angular_travel/segments;
linear_per_segment = linear_travel/segments;
extruder_per_segment = extruder_travel/segments;
/* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
r_T = [cos(phi) -sin(phi);
sin(phi) cos(phi] * r ;
For arc generation, the center of the circle is the axis of rotation and the radius vector is
defined from the circle center to the initial position. Each line segment is formed by successive
vector rotations. This requires only two cos() and sin() computations to form the rotation
matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
all double numbers are single precision on the Arduino. (True double precision will not have
round off issues for CNC applications.) Single precision error can accumulate to be greater than
tool precision in some cases. Therefore, arc path correction is implemented.
Small angle approximation may be used to reduce computation overhead further. This approximation
holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
issue for CNC machines with the single precision Arduino calculations.
This approximation also allows mc_arc to immediately insert a line segment into the planner
without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead.
This is important when there are successive arc motions.
*/
// Vector rotation matrix values
cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation
sin_T = theta_per_segment;
// Initialize the linear axis
arc_target[Z_AXIS] = current_position[Z_AXIS];
// Initialize the extruder axis
arc_target[E_AXIS] = current_position[E_AXIS];
arc_state = 1;
i = 1;
break;
case 1:
bound = i + 10;
for (; i < bound && i < segments; i++) { // Increment (segments-1)
if (count < machine->n_arc_correction) {
// Apply vector rotation matrix
r_axisi = r_axis0*sin_T + r_axis1*cos_T;
r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
r_axis1 = r_axisi;
count++;
} else {
// Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
// Compute exact location by applying transformation matrix from initial radius vector(=-offset).
cos_Ti = cos(i*theta_per_segment);
sin_Ti = sin(i*theta_per_segment);
r_axis0 = -offset[X_AXIS]*cos_Ti + offset[Y_AXIS]*sin_Ti;
r_axis1 = -offset[X_AXIS]*sin_Ti - offset[Y_AXIS]*cos_Ti;
count = 0;
}
// Update arc_target location
arc_target[X_AXIS] = center_axis0 + r_axis0;
arc_target[Y_AXIS] = center_axis1 + r_axis1;
arc_target[Z_AXIS] += linear_per_segment;
arc_target[E_AXIS] += extruder_per_segment;
clamp_to_software_endstops(arc_target);
plan_buffer_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], arc_target[E_AXIS], feed_rate, active_extruder);
}
if(i >= segments) arc_state = 2;
break;
case 2:
// Ensure last segment arrives at target location.
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feed_rate, active_extruder);
// plan_set_acceleration_manager_enabled(acceleration_manager_was_enabled);
// As far as the parser is concerned, the position is now == target. In reality the
// motion control system might still be processing the action and the real tool position
// in any intermediate location.
for(int i=0; i < machine->num_axis; i++) {
current_position[i] = destination[i];
}
arc_state = 0;
sys_state = SYS_STATE_NORMAL;
break;
}
}
double base_min_pos(int axis) {
if(axis == X_AXIS) return machine->x_min_pos_mm;
if(axis == Y_AXIS) return machine->y_min_pos_mm;
if(axis == Z_AXIS) return machine->z_min_pos_mm;
}
double base_max_pos(int axis) {
if(axis == X_AXIS) return machine->x_max_pos_mm;
if(axis == Y_AXIS) return machine->y_max_pos_mm;
if(axis == Z_AXIS) return machine->z_max_pos_mm;
}
double base_home_pos(int axis) {
if(axis == X_AXIS) return machine->x_home_pos_mm;
if(axis == Y_AXIS) return machine->y_home_pos_mm;
if(axis == Z_AXIS) return machine->z_home_pos_mm;
}
double max_length(int axis) {
if(axis == X_AXIS) return machine->x_max_pos_mm - machine->x_min_pos_mm;
if(axis == Y_AXIS) return machine->y_max_pos_mm - machine->y_min_pos_mm;
if(axis == Z_AXIS) return machine->z_max_pos_mm - machine->z_min_pos_mm;
}
double home_retract_mm(int axis) {
if(axis == X_AXIS) return machine->x_home_retract_mm;
if(axis == Y_AXIS) return machine->y_home_retract_mm;
if(axis == Z_AXIS) return machine->z_home_retract_mm;
}
int home_dir(int axis) {
if(axis == X_AXIS) return machine->x_home_dir;
if(axis == Y_AXIS) return machine->y_home_dir;
if(axis == Z_AXIS) return machine->z_home_dir;
}
static void axis_is_at_home(int axis) {
current_position[axis] = base_home_pos(axis) + add_homeing[axis];
min_pos[axis] = base_min_pos(axis) + add_homeing[axis];
max_pos[axis] = base_max_pos(axis) + add_homeing[axis];
}
static bool homeaxis(int axis) {
static int homeaxis_state = 0;
int axis_home_dir = home_dir(axis);
if (!plan_buffer_null() || !st_buffer_null()) return false;
switch (homeaxis_state)
{
case 0:
current_position[axis] = 0;
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
destination[axis] = 1.5 * max_length(axis) * axis_home_dir;
feedrate = homing_feedrate[axis];
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
homeaxis_state = 1;
break;
case 1:
current_position[axis] = 0;
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
destination[axis] = -home_retract_mm(axis) * axis_home_dir;
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
homeaxis_state = 2;
break;
case 2:
destination[axis] = 2*home_retract_mm(axis) * axis_home_dir;
feedrate = homing_feedrate[axis]/2 ;
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
homeaxis_state = 3;
break;
case 3:
current_position[axis] = 0;
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
destination[axis] = machine->home_offset[axis] * (-axis_home_dir);
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
homeaxis_state = 4;
break;
case 4:
axis_is_at_home(axis);
destination[axis] = current_position[axis];
feedrate = 0.0;
//endstops_hit_on_purpose(); //stepper中check的設定
axis_known_position[axis] = true;
homeaxis_state = 0;
return true;
}
return false;
}
//#define HOMEAXIS(LETTER) homeaxis(LETTER##_AXIS)
static void homing()
{
static int home_state = 0;
switch (home_state)
{
case 0:
if (level->enable_auto_bed_leveling != 0)
plan_bed_level_matrix.set_to_identity(); //Reset the plane ("erase" all leveling data)
saved_feedrate = feedrate;
saved_feedmultiply = feedmultiply;
feedmultiply = 100;
for(int i=0; i < machine->num_axis; i++) {
destination[i] = current_position[i];
}
feedrate = 0.0;
home_all_axis = !((code_seen(axis_codes[X_AXIS])) || (code_seen(axis_codes[Y_AXIS])) || (code_seen(axis_codes[Z_AXIS])));
home_state = 1;
break;
case 1:
if (!(home_all_axis) && !(code_seen(axis_codes[Y_AXIS])))
home_state = 2;
else if (homeaxis(Y_AXIS) == true)
home_state = 2;
else
;
break;
case 2:
if (!(home_all_axis) && !(code_seen(axis_codes[X_AXIS])))
home_state = 3;
else if (homeaxis(X_AXIS) == true)
home_state = 3;
else
;
break;
case 3:
if (!(home_all_axis) && !(code_seen(axis_codes[Z_AXIS])))
home_state = 4;
else if (homeaxis(Z_AXIS) == true)
home_state = 4;
else
;
break;
case 4:
if(code_seen(axis_codes[X_AXIS])) {
if(code_value_long() != 0) {
current_position[X_AXIS]=code_value()+add_homeing[0];
}
}
if(code_seen(axis_codes[Y_AXIS])) {
if(code_value_long() != 0) {
current_position[Y_AXIS]=code_value()+add_homeing[1];
}
}
if(code_seen(axis_codes[Z_AXIS])) {
if(code_value_long() != 0) {
current_position[Z_AXIS]=code_value()+add_homeing[2];
}
}
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
feedrate = saved_feedrate;
feedmultiply = saved_feedmultiply;
COMM_WRITE("Bed");
COMM_WRITE(" X: ");
COMM_WRITE_FLOAT(current_position[X_AXIS]);
COMM_WRITE(" Y: ");
COMM_WRITE_FLOAT(current_position[Y_AXIS]);
COMM_WRITE(" Z: ");
COMM_WRITE_FLOAT(current_position[Z_AXIS]);
COMM_WRITE("\n");
home_state = 0;
sys_state = SYS_STATE_NORMAL;
break;
}
}
static void set_bed_level_equation_lsq(double *plane_equation_coefficients)
{
vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
planeNormal.debug("planeNormal");
plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
//bedLevel.debug("bedLevel");
//plan_bed_level_matrix.debug("bed level before");
//vector_3 uncorrected_position = plan_get_position_mm();
//uncorrected_position.debug("position before");
vector_3 corrected_position = plan_get_position();
// corrected_position.debug("position after");
current_position[X_AXIS] = corrected_position.x;
current_position[Y_AXIS] = corrected_position.y;
current_position[Z_AXIS] = corrected_position.z;
// but the bed at 0 so we don't go below it.
current_position[Z_AXIS] = zprobe_zoffset; // in the lsq we reach here after raising the extruder due to the loop structure
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
}
static void set_bed_level_equation_3pts(double z_at_pt_1, double z_at_pt_2, double z_at_pt_3) {
plan_bed_level_matrix.set_to_identity();
vector_3 pt1 = vector_3(level->abl_probe_pt_1_x, level->abl_probe_pt_1_y, z_at_pt_1);
vector_3 pt2 = vector_3(level->abl_probe_pt_2_x, level->abl_probe_pt_2_y, z_at_pt_2);
vector_3 pt3 = vector_3(level->abl_probe_pt_3_x, level->abl_probe_pt_3_y, z_at_pt_3);
vector_3 from_2_to_1 = (pt1 - pt2).get_normal();
vector_3 from_2_to_3 = (pt3 - pt2).get_normal();
vector_3 planeNormal = vector_3::cross(from_2_to_1, from_2_to_3).get_normal();
planeNormal = vector_3(planeNormal.x, planeNormal.y, fabs(planeNormal.z));
plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
plan_bed_level_matrix = matrix_3x3::transpose(plan_bed_level_matrix);
vector_3 corrected_position = plan_get_position();
current_position[X_AXIS] = corrected_position.x;
current_position[Y_AXIS] = corrected_position.y;
current_position[Z_AXIS] = corrected_position.z;
// put the bed at 0 so we don't go below it.
current_position[Z_AXIS] = zprobe_zoffset;
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
}
static bool run_z_probe() {
static int run_z_probe_state = 0;
static double zPosition;
if (!plan_buffer_null() || !st_buffer_null()) return false;
switch (run_z_probe_state)
{
case 0:
plan_bed_level_matrix.set_to_identity();
feedrate = homing_feedrate[Z_AXIS];
// move down until you find the bed
zPosition = -10;
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
run_z_probe_state = 1;
break;
case 1:
// we have to let the planner know where we are right now as it is not where we said to go.
zPosition = st_get_position_mm(Z_AXIS);
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS]);
// move up the retract distance
zPosition += home_retract_mm(Z_AXIS);
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
run_z_probe_state = 2;
break;
case 2:
// move back down slowly to find bed
feedrate = homing_feedrate[Z_AXIS]/4;
zPosition -= home_retract_mm(Z_AXIS) * 2;
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], zPosition, current_position[E_AXIS], feedrate/60, active_extruder);
run_z_probe_state = 3;
break;
case 3:
current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
// make sure the planner knows where we are as it may be a bit different than we last said to move to
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
run_z_probe_state = 0;
return true;
}
return false;
}
static bool do_blocking_move_to(double x, double y, double z) {
static int do_blocking_move_to_state = 0;
static double oldFeedRate;
switch (do_blocking_move_to_state)
{
case 0:
oldFeedRate = feedrate;
feedrate = level->xy_travel_speed;
current_position[X_AXIS] = x;
current_position[Y_AXIS] = y;
current_position[Z_AXIS] = z;
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], feedrate/60, active_extruder);
do_blocking_move_to_state = 1;
break;
case 1:
if (!plan_buffer_null() || !st_buffer_null()) break;
feedrate = oldFeedRate;
do_blocking_move_to_state = 0;
return true;
}
return false;
}
static bool do_blocking_move_relative(double offset_x, double offset_y, double offset_z) {
return do_blocking_move_to(current_position[X_AXIS] + offset_x, current_position[Y_AXIS] + offset_y, current_position[Z_AXIS] + offset_z);
}
static void setup_for_endstop_move() {
saved_feedrate = feedrate;
saved_feedmultiply = feedmultiply;
feedmultiply = 100;
// previous_millis_cmd = millis();
// enable_endstops(true);
}
static void clean_up_after_endstop_move() {
// #ifdef ENDSTOPS_ONLY_FOR_HOMING
// enable_endstops(false);
// #endif
feedrate = saved_feedrate;
feedmultiply = saved_feedmultiply;
// previous_millis_cmd = millis();
}
static void engage_z_probe() {
}
static void retract_z_probe() {
}
/// Probe bed height at position (x,y), returns the measured z value
static bool probe_pt(double x, double y, double z_before, double *pz_probe) {
static int probe_pt_state = 0;
switch (probe_pt_state)
{
case 0:
// move to right place
if (do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_before) == true)
probe_pt_state = 1;
break;
case 1:
if (do_blocking_move_to(x - level->x_probe_offset_from_extruder, y - level->y_probe_offset_from_extruder, current_position[Z_AXIS]) == true)
probe_pt_state = 2;
break;
case 2:
engage_z_probe(); // Engage Z Servo endstop if available
probe_pt_state = 3;
break;
case 3:
if (run_z_probe() == true) {
*pz_probe = current_position[Z_AXIS];
retract_z_probe();
/*SERIAL_PROTOCOLPGM(MSG_BED);
SERIAL_PROTOCOLPGM(" x: ");
SERIAL_PROTOCOL(x);
SERIAL_PROTOCOLPGM(" y: ");
SERIAL_PROTOCOL(y);
SERIAL_PROTOCOLPGM(" z: ");
SERIAL_PROTOCOL(measured_z);
SERIAL_PROTOCOLPGM("\n");*/
COMM_WRITE("Bed");
COMM_WRITE(" x: ");
COMM_WRITE_FLOAT(x);
COMM_WRITE(" y: ");
COMM_WRITE_FLOAT(y);
COMM_WRITE(" z: ");
COMM_WRITE_FLOAT(*pz_probe);
COMM_WRITE("\n");
probe_pt_state = 0;
return true;
}
break;
}
return false;
}
static bool probe_grid()
{
static int probe_grid_state = 0;
static int xGridSpacing, yGridSpacing;
static int probePointCounter, yProbe, xProbe, xInc, xCount;
static bool zig;
static double z_before;
double measured_z;
switch (probe_grid_state)
{
case 0:
// probe at the points of a lattice grid
xGridSpacing = (level->right_probe_bed_position - level->left_probe_bed_position) / (level->auto_bed_leveling_grid_points-1);
yGridSpacing = (level->back_probe_bed_position - level->front_probe_bed_position) / (level->auto_bed_leveling_grid_points-1);
// solve the plane equation ax + by + d = z
// A is the matrix with rows [x y 1] for all the probed points
// B is the vector of the Z positions
// the normal vector to the plane is formed by the coefficients of the plane equation in the standard form, which is Vx*x+Vy*y+Vz*z+d = 0
// so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
probePointCounter = 0;
zig = true;
yProbe = level->front_probe_bed_position;
probe_grid_state = 1;
break;
case 1:
if (yProbe <= level->back_probe_bed_position) {
if (zig)
{
xProbe = level->left_probe_bed_position;
//xEnd = level->right_probe_bed_position;
xInc = xGridSpacing;
zig = false;
} else // zag
{
xProbe = level->right_probe_bed_position;
//xEnd = level->left_probe_bed_position;
xInc = -xGridSpacing;
zig = true;
}
xCount = 0;
probe_grid_state = 2;
} else {
probe_grid_state = 4;
}
break;
case 2:
if (xCount < level->auto_bed_leveling_grid_points) {
if (probePointCounter == 0)
{
// raise before probing
z_before = level->z_raise_before_probing;
} else
{
// raise extruder
z_before = current_position[Z_AXIS] + level->z_raise_between_probings;
}
probe_grid_state = 3;
} else {
yProbe += yGridSpacing;
probe_grid_state = 1;
}
break;
case 3:
if (probe_pt(xProbe, yProbe, z_before, &measured_z) == false) break;
eqnBVector[probePointCounter] = measured_z;
eqnAMatrix[probePointCounter + 0*level->auto_bed_leveling_grid_points*level->auto_bed_leveling_grid_points] = xProbe;
eqnAMatrix[probePointCounter + 1*level->auto_bed_leveling_grid_points*level->auto_bed_leveling_grid_points] = yProbe;
eqnAMatrix[probePointCounter + 2*level->auto_bed_leveling_grid_points*level->auto_bed_leveling_grid_points] = 1;
probePointCounter++;
xProbe += xInc;
xCount++;
probe_grid_state = 2;
break;
case 4:
clean_up_after_endstop_move();
// solve lsq problem
double *plane_equation_coefficients = qr_solve(level->auto_bed_leveling_grid_points*level->auto_bed_leveling_grid_points, 3, eqnAMatrix, eqnBVector);
/*SERIAL_PROTOCOLPGM("Eqn coefficients: a: ");
SERIAL_PROTOCOL(plane_equation_coefficients[0]);
SERIAL_PROTOCOLPGM(" b: ");
SERIAL_PROTOCOL(plane_equation_coefficients[1]);
SERIAL_PROTOCOLPGM(" d: ");
SERIAL_PROTOCOLLN(plane_equation_coefficients[2]);*/
COMM_WRITE("Eqn coefficients: a: ");
COMM_WRITE_FLOAT(plane_equation_coefficients[0]);
COMM_WRITE(" b: ");
COMM_WRITE_FLOAT(plane_equation_coefficients[1]);
COMM_WRITE(" d: ");
COMM_WRITE_FLOAT(plane_equation_coefficients[2]);
set_bed_level_equation_lsq(plane_equation_coefficients);
free(plane_equation_coefficients);
probe_grid_state = 0;
return true;
}
return false;
}
static bool probe_at_3_points()
{
static int probe_at_3_points_state = 0;
static double z_at_pt_1, z_at_pt_2, z_at_pt_3;
switch (probe_at_3_points_state)
{
case 0:
// Probe at 3 arbitrary points
// probe 1
if (probe_pt(level->abl_probe_pt_1_x, level->abl_probe_pt_1_y, level->z_raise_before_probing, &z_at_pt_1) == true)
probe_at_3_points_state = 1;
break;
case 1:
// probe 2
if (probe_pt(level->abl_probe_pt_2_x, level->abl_probe_pt_2_y, current_position[Z_AXIS] + level->z_raise_between_probings, &z_at_pt_2) == true)
probe_at_3_points_state = 2;
break;
case 2:
// probe 3
if (probe_pt(level->abl_probe_pt_3_x, level->abl_probe_pt_3_y, current_position[Z_AXIS] + level->z_raise_between_probings, &z_at_pt_3) == true)
probe_at_3_points_state = 3;
break;
case 3:
clean_up_after_endstop_move();
set_bed_level_equation_3pts(z_at_pt_1, z_at_pt_2, z_at_pt_3);
probe_at_3_points_state = 0;
return true;
}
return false;
}
static vector_3 uncorrected_position;
static void Auto_Z_Probe()
{
static int auto_z_probe_state = 0;
double x_tmp, y_tmp, z_tmp, real_z;
//#if Z_MIN_PIN == -1
//#error "You must have a Z_MIN endstop in order to enable Auto Bed Leveling feature!!! Z_MIN_PIN must point to a valid hardware pin."
//#endif
switch (auto_z_probe_state)
{
case 0:
// Prevent user from running a G29 without first homing in X and Y
if (! (axis_known_position[X_AXIS] && axis_known_position[Y_AXIS]) )
{
// LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
// SERIAL_ECHO_START;
// SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
COMM_WRITE("echo:");
COMM_WRITE("Home X/Y before Z\n");
sys_state = SYS_STATE_NORMAL;
break; // abort G29, since we don't know where we are
}
auto_z_probe_state = 1;
break;
case 1:
if (!plan_buffer_null() || !st_buffer_null()) break;
// make sure the bed_level_rotation_matrix is identity or the planner will get it incorectly
//vector_3 corrected_position = plan_get_position_mm();
//corrected_position.debug("position before G29");
plan_bed_level_matrix.set_to_identity();
uncorrected_position = plan_get_position();
//uncorrected_position.debug("position durring G29");
current_position[X_AXIS] = uncorrected_position.x;
current_position[Y_AXIS] = uncorrected_position.y;
current_position[Z_AXIS] = uncorrected_position.z;
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
setup_for_endstop_move();
feedrate = homing_feedrate[Z_AXIS];
auto_z_probe_state = 2;
break;
case 2:
if ((level->auto_bed_leveling_grid != 0 && probe_grid() == true)
|| (level->auto_bed_leveling_grid == 0 && probe_at_3_points() == true))
auto_z_probe_state = 3;
break;
case 3:
if (!plan_buffer_null() || !st_buffer_null()) break;
// The following code correct the Z height difference from z-probe position and hotend tip position.
// The Z height on homing is measured by Z-Probe, but the probe is quite far from the hotend.
// When the bed is uneven, this height must be corrected.
real_z = double(st_get_position(Z_AXIS))/axis_steps_per_unit[Z_AXIS]; //get the real Z (since the auto bed leveling is already correcting the plane)
x_tmp = current_position[X_AXIS] + level->x_probe_offset_from_extruder;
y_tmp = current_position[Y_AXIS] + level->y_probe_offset_from_extruder;
z_tmp = current_position[Z_AXIS];
apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp); //Apply the correction sending the probe offset
current_position[Z_AXIS] = z_tmp - real_z + current_position[Z_AXIS]; //The difference is added to current position and sent to planner.
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
COMM_WRITE("Bed");
COMM_WRITE(" X: ");
COMM_WRITE_FLOAT(current_position[X_AXIS]);
COMM_WRITE(" Y: ");
COMM_WRITE_FLOAT(current_position[Y_AXIS]);
COMM_WRITE(" Z: ");
COMM_WRITE_FLOAT(current_position[Z_AXIS]);
COMM_WRITE("\n");
auto_z_probe_state = 0;
sys_state = SYS_STATE_NORMAL;
break;
}
}
static void Single_Z_Probe()
{
static int single_z_probe_state = 0;
switch (single_z_probe_state)
{
case 0:
engage_z_probe(); // Engage Z Servo endstop if available
single_z_probe_state = 1;
break;
case 1:
setup_for_endstop_move();
feedrate = homing_feedrate[Z_AXIS];
single_z_probe_state = 2;
break;
case 2:
if (run_z_probe() == true)
single_z_probe_state = 3;
break;
case 3:
COMM_WRITE("Bed");
COMM_WRITE(" X: ");
COMM_WRITE_FLOAT(current_position[X_AXIS]);
COMM_WRITE(" Y: ");
COMM_WRITE_FLOAT(current_position[Y_AXIS]);
COMM_WRITE(" Z: ");
COMM_WRITE_FLOAT(current_position[Z_AXIS]);
COMM_WRITE("\n");
clean_up_after_endstop_move();
retract_z_probe(); // Retract Z Servo endstop if available
single_z_probe_state = 0;
sys_state = SYS_STATE_NORMAL;
break;
}
}
static double reg_temp;
static bool ChangeTempWait = false;
static bool CooldownNoWait = false;
static bool ChangeTempFirst = false;
static bool target_direction;
static void set_temperature()
{
if (!plan_buffer_null() || !st_buffer_null()) return;
if (ChangeTempWait) {
if (ChangeTempFirst) {
setTargetHotend(reg_temp, EXTRUDER0);
target_direction = isHeatingHotend(EXTRUDER0);
ChangeTempFirst = false;
}
if (target_direction ? (!isHeatingHotend(EXTRUDER0)) : (!isCoolingHotend(EXTRUDER0)||(CooldownNoWait==true)))
{
ChangeTempWait = false;
CooldownNoWait = false;
sys_state = SYS_STATE_NORMAL;
}
} else {
setTargetHotend(reg_temp, EXTRUDER0);
sys_state = SYS_STATE_NORMAL;
}
}
static double fanSpeed = 0.0;
static void set_fan()
{
if (!plan_buffer_null() || !st_buffer_null()) return;
setFanPeriod(COLDEDFAN0, fanSpeed);
sys_state = SYS_STATE_NORMAL;
}
static double pid_autotune_temp = 150.0;