-
Notifications
You must be signed in to change notification settings - Fork 292
/
canonical_machine.c
executable file
·2053 lines (1828 loc) · 75.1 KB
/
canonical_machine.c
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
/*
* canonical_machine.c - rs274/ngc canonical machine.
* This file is part of the TinyG project
*
* Copyright (c) 2010 - 2015 Alden S Hart, Jr.
* Copyright (c) 2014 - 2015 Robert Giseburt
*
* This file ("the software") is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License, version 2 as published by the
* Free Software Foundation. You should have received a copy of the GNU General Public
* License, version 2 along with the software. If not, see <http://www.gnu.org/licenses/>.
*
* As a special exception, you may use this file as part of a software library without
* restriction. Specifically, if other files instantiate templates or use macros or
* inline functions from this file, or you compile this file and link it with other
* files to produce an executable, this file does not by itself cause the resulting
* executable to be covered by the GNU General Public License. This exception does not
* however invalidate any other reasons why the executable file might be covered by the
* GNU General Public License.
*
* THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY
* WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT
* SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF
* OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
/*
* This code is a loose implementation of Kramer, Proctor and Messina's canonical
* machining functions as described in the NIST RS274/NGC v3
*
* The canonical machine is the layer between the Gcode parser and the motion control
* code for a specific robot. It keeps state and executes commands - passing the
* stateless commands to the motion planning layer.
*/
/* --- System state contexts - Gcode models ---
*
* Useful reference for doing C callbacks http://www.newty.de/fpt/fpt.html
*
* There are 3 temporal contexts for system state:
* - The gcode model in the canonical machine (the MODEL context, held in gm)
* - The gcode model used by the planner (PLANNER context, held in bf's and mm)
* - The gcode model used during motion for reporting (RUNTIME context, held in mr)
*
* It's a bit more complicated than this. The 'gm' struct contains the core Gcode model
* context. This originates in the canonical machine and is copied to each planner buffer
* (bf buffer) during motion planning. Finally, the gm context is passed to the runtime
* (mr) for the RUNTIME context. So at last count the Gcode model exists in as many as
* 30 copies in the system. (1+28+1)
*
* Depending on the need, any one of these contexts may be called for reporting or by
* a function. Most typically, all new commends from the gcode parser work form the MODEL
* context, and status reports pull from the RUNTIME while in motion, and from MODEL when
* at rest. A convenience is provided in the ACTIVE_MODEL pointer to point to the right
* context.
*/
/* --- Synchronizing command execution ---
*
* Some gcode commands only set the MODEL state for interpretation of the current Gcode
* block. For example, cm_set_feed_rate(). This sets the MODEL so the move time is
* properly calculated for the current (and subsequent) blocks, so it's effected
* immediately.
*
* "Synchronous commands" are commands that affect the runtime need to be synchronized
* with movement. Examples include G4 dwells, program stops and ends, and most M commands.
* These are queued into the planner queue and execute from the queue. Synchronous commands
* work like this:
*
* - Call the cm_xxx_xxx() function which will do any input validation and return an
* error if it detects one.
*
* - The cm_ function calls mp_queue_command(). Arguments are a callback to the _exec_...()
* function, which is the runtime execution routine, and any arguments that are needed
* by the runtime. See typedef for *exec in planner.h for details
*
* - mp_queue_command() stores the callback and the args in a planner buffer.
*
* - When planner execution reaches the buffer it executes the callback w/ the args.
* Take careful note that the callback executes under an interrupt, so beware of
* variables that may need to be volatile.
*
* Note:
* - The synchronous command execution mechanism uses 2 vectors in the bf buffer to store
* and return values for the callback. It's obvious, but impractical to pass the entire
* bf buffer to the callback as some of these commands are actually executed locally
* and have no buffer.
*/
#include "tinyg.h" // #1
#include "config.h" // #2
#include "text_parser.h"
#include "canonical_machine.h"
#include "controller.h"
#include "plan_arc.h"
#include "planner.h"
#include "stepper.h"
#include "encoder.h"
#include "spindle.h"
#include "report.h"
#include "gpio.h"
#include "switch.h"
#include "hardware.h"
#include "util.h"
#include "xio.h" // for serial queue flush
/*
#ifdef __cplusplus
extern "C"{
#endif
*/
/***********************************************************************************
**** STRUCTURE ALLOCATIONS ********************************************************
***********************************************************************************/
cmSingleton_t cm; // canonical machine controller singleton
/***********************************************************************************
**** GENERIC STATIC FUNCTIONS AND VARIABLES ***************************************
***********************************************************************************/
// command execution callbacks from planner queue
static void _exec_offset(float *value, float *flag);
static void _exec_change_tool(float *value, float *flag);
static void _exec_select_tool(float *value, float *flag);
static void _exec_mist_coolant_control(float *value, float *flag);
static void _exec_flood_coolant_control(float *value, float *flag);
static void _exec_absolute_origin(float *value, float *flag);
static void _exec_program_finalize(float *value, float *flag);
static int8_t _get_axis(const index_t index);
static int8_t _get_axis_type(const index_t index);
/***********************************************************************************
**** CODE *************************************************************************
***********************************************************************************/
/********************************
* Internal getters and setters *
********************************/
/*
* Canonical Machine State functions
*
* cm_get_combined_state() - combines raw states into something a user might want to see
* cm_get_machine_state()
* cm_get_motion_state()
* cm_get_cycle_state()
* cm_get_hold_state()
* cm_get_homing_state()
* cm_set_motion_state() - adjusts active model pointer as well
*/
uint8_t cm_get_combined_state()
{
if (cm.cycle_state == CYCLE_OFF) { cm.combined_state = cm.machine_state;}
else if (cm.cycle_state == CYCLE_PROBE) { cm.combined_state = COMBINED_PROBE;}
else if (cm.cycle_state == CYCLE_HOMING) { cm.combined_state = COMBINED_HOMING;}
else if (cm.cycle_state == CYCLE_JOG) { cm.combined_state = COMBINED_JOG;}
else {
if (cm.motion_state == MOTION_RUN) cm.combined_state = COMBINED_RUN;
if (cm.motion_state == MOTION_HOLD) cm.combined_state = COMBINED_HOLD;
}
if (cm.machine_state == MACHINE_SHUTDOWN) { cm.combined_state = COMBINED_SHUTDOWN;}
return cm.combined_state;
}
uint8_t cm_get_machine_state() { return cm.machine_state;}
uint8_t cm_get_cycle_state() { return cm.cycle_state;}
uint8_t cm_get_motion_state() { return cm.motion_state;}
uint8_t cm_get_hold_state() { return cm.hold_state;}
uint8_t cm_get_homing_state() { return cm.homing_state;}
uint8_t cm_get_buffer_drain_state() { return cm.buffer_drain_state;}
void cm_set_motion_state(uint8_t motion_state)
{
cm.motion_state = motion_state;
switch (motion_state) {
case (MOTION_STOP): { ACTIVE_MODEL = MODEL; break; }
case (MOTION_RUN): { ACTIVE_MODEL = RUNTIME; break; }
case (MOTION_HOLD): { ACTIVE_MODEL = RUNTIME; break; }
}
}
void cm_set_buffer_drain_state(uint8_t buffer_drain_state)
{
cm.buffer_drain_state = buffer_drain_state;
}
/***********************************
* Model State Getters and Setters *
***********************************/
/* These getters and setters will work on any gm model with inputs:
* MODEL (GCodeState_t *)&cm.gm // absolute pointer from canonical machine gm model
* PLANNER (GCodeState_t *)&bf->gm // relative to buffer *bf is currently pointing to
* RUNTIME (GCodeState_t *)&mr.gm // absolute pointer from runtime mm struct
* ACTIVE_MODEL cm.am // active model pointer is maintained by state management
*/
uint32_t cm_get_linenum(GCodeState_t *gcode_state) { return gcode_state->linenum;}
uint8_t cm_get_motion_mode(GCodeState_t *gcode_state) { return gcode_state->motion_mode;}
uint8_t cm_get_coord_system(GCodeState_t *gcode_state) { return gcode_state->coord_system;}
uint8_t cm_get_units_mode(GCodeState_t *gcode_state) { return gcode_state->units_mode;}
uint8_t cm_get_select_plane(GCodeState_t *gcode_state) { return gcode_state->select_plane;}
uint8_t cm_get_path_control(GCodeState_t *gcode_state) { return gcode_state->path_control;}
uint8_t cm_get_distance_mode(GCodeState_t *gcode_state) { return gcode_state->distance_mode;}
uint8_t cm_get_feed_rate_mode(GCodeState_t *gcode_state) { return gcode_state->feed_rate_mode;}
uint8_t cm_get_tool(GCodeState_t *gcode_state) { return gcode_state->tool;}
uint8_t cm_get_spindle_mode(GCodeState_t *gcode_state) { return gcode_state->spindle_mode;}
uint8_t cm_get_block_delete_switch() { return cm.gmx.block_delete_switch;}
uint8_t cm_get_runtime_busy() { return (mp_get_runtime_busy());}
float cm_get_feed_rate(GCodeState_t *gcode_state) { return gcode_state->feed_rate;}
void cm_set_motion_mode(GCodeState_t *gcode_state, uint8_t motion_mode) { gcode_state->motion_mode = motion_mode;}
void cm_set_spindle_mode(GCodeState_t *gcode_state, uint8_t spindle_mode) { gcode_state->spindle_mode = spindle_mode;}
void cm_set_spindle_speed_parameter(GCodeState_t *gcode_state, float speed) { gcode_state->spindle_speed = speed;}
void cm_set_tool_number(GCodeState_t *gcode_state, uint8_t tool) { gcode_state->tool = tool;}
void cm_set_absolute_override(GCodeState_t *gcode_state, uint8_t absolute_override)
{
gcode_state->absolute_override = absolute_override;
cm_set_work_offsets(MODEL); // must reset offsets if you change absolute override
}
void cm_set_model_linenum(uint32_t linenum)
{
cm.gm.linenum = linenum; // you must first set the model line number,
nv_add_object((const char_t *)"n"); // then add the line number to the nv list
}
/***********************************************************************************
* COORDINATE SYSTEMS AND OFFSETS
* Functions to get, set and report coordinate systems and work offsets
* These functions are not part of the NIST defined functions
***********************************************************************************/
/*
* Notes on Coordinate System and Offset functions
*
* All positional information in the canonical machine is kept as absolute coords and in
* canonical units (mm). The offsets are only used to translate in and out of canonical form
* during interpretation and response.
*
* Managing the coordinate systems & offsets is somewhat complicated. The following affect offsets:
* - coordinate system selected. 1-9 correspond to G54-G59
* - absolute override: forces current move to be interpreted in machine coordinates: G53 (system 0)
* - G92 offsets are added "on top of" the coord system offsets -- if origin_offset_enable == true
* - G28 and G30 moves; these are run in absolute coordinates
*
* The offsets themselves are considered static, are kept in cm, and are supposed to be persistent.
*
* To reduce complexity and data load the following is done:
* - Full data for coordinates/offsets is only accessible by the canonical machine, not the downstream
* - A fully resolved set of coord and G92 offsets, with per-move exceptions can be captured as "work_offsets"
* - The core gcode context (gm) only knows about the active coord system and the work offsets
*/
/*
* cm_get_active_coord_offset() - return the currently active coordinate offset for an axis
*
* Takes G5x, G92 and absolute override into account to return the active offset for this move
*
* This function is typically used to evaluate and set offsets, as opposed to cm_get_work_offset()
* which merely returns what's in the work_offset[] array.
*/
float cm_get_active_coord_offset(uint8_t axis)
{
if (cm.gm.absolute_override == true) return (0); // no offset if in absolute override mode
float offset = cm.offset[cm.gm.coord_system][axis];
if (cm.gmx.origin_offset_enable == true)
offset += cm.gmx.origin_offset[axis]; // includes G5x and G92 components
return (offset);
}
/*
* cm_get_work_offset() - return a coord offset from the gcode_state
*
* This function accepts as input:
* MODEL (GCodeState_t *)&cm.gm // absolute pointer from canonical machine gm model
* PLANNER (GCodeState_t *)&bf->gm // relative to buffer *bf is currently pointing to
* RUNTIME (GCodeState_t *)&mr.gm // absolute pointer from runtime mm struct
* ACTIVE_MODEL cm.am // active model pointer is maintained by state management
*/
float cm_get_work_offset(GCodeState_t *gcode_state, uint8_t axis)
{
return (gcode_state->work_offset[axis]);
}
/*
* cm_set_work_offsets() - capture coord offsets from the model into absolute values in the gcode_state
*
* This function accepts as input:
* MODEL (GCodeState_t *)&cm.gm // absolute pointer from canonical machine gm model
* PLANNER (GCodeState_t *)&bf->gm // relative to buffer *bf is currently pointing to
* RUNTIME (GCodeState_t *)&mr.gm // absolute pointer from runtime mm struct
* ACTIVE_MODEL cm.am // active model pointer is maintained by state management
*/
void cm_set_work_offsets(GCodeState_t *gcode_state)
{
for (uint8_t axis = AXIS_X; axis < AXES; axis++) {
gcode_state->work_offset[axis] = cm_get_active_coord_offset(axis);
}
}
/*
* cm_get_absolute_position() - get position of axis in absolute coordinates
*
* This function accepts as input:
* MODEL (GCodeState_t *)&cm.gm // absolute pointer from canonical machine gm model
* RUNTIME (GCodeState_t *)&mr.gm // absolute pointer from runtime mm struct
*
* NOTE: Only MODEL and RUNTIME are supported (no PLANNER or bf's)
* NOTE: Machine position is always returned in mm mode. No units conversion is performed
*/
float cm_get_absolute_position(GCodeState_t *gcode_state, uint8_t axis)
{
if (gcode_state == MODEL) return (cm.gmx.position[axis]);
return (mp_get_runtime_absolute_position(axis));
}
/*
* cm_get_work_position() - return work position in external form
*
* ... that means in prevailing units (mm/inch) and with all offsets applied
*
* NOTE: This function only works after the gcode_state struct as had the work_offsets setup by
* calling cm_get_model_coord_offset_vector() first.
*
* This function accepts as input:
* MODEL (GCodeState_t *)&cm.gm // absolute pointer from canonical machine gm model
* RUNTIME (GCodeState_t *)&mr.gm // absolute pointer from runtime mm struct
*
* NOTE: Only MODEL and RUNTIME are supported (no PLANNER or bf's)
*/
float cm_get_work_position(GCodeState_t *gcode_state, uint8_t axis)
{
float position;
if (gcode_state == MODEL) {
position = cm.gmx.position[axis] - cm_get_active_coord_offset(axis);
} else {
position = mp_get_runtime_work_position(axis);
}
if (gcode_state->units_mode == INCHES) { position /= MM_PER_INCH; }
return (position);
}
/***********************************************************************************
* CRITICAL HELPERS
* Core functions supporting the canonical machining functions
* These functions are not part of the NIST defined functions
***********************************************************************************/
/*
* cm_finalize_move() - perform final operations for a traverse or feed
* cm_update_model_position_from_runtime() - set endpoint position from final runtime position
*
* These routines set the point position in the gcode model.
*
* Note: As far as the canonical machine is concerned the final position of a Gcode block (move)
* is achieved as soon as the move is planned and the move target becomes the new model position.
* In reality the planner will (in all likelihood) have only just queued the move for later
* execution, and the real tool position is still close to the starting point.
*/
void cm_finalize_move() {
copy_vector(cm.gmx.position, cm.gm.target); // update model position
// if in ivnerse time mode reset feed rate so next block requires an explicit feed rate setting
if ((cm.gm.feed_rate_mode == INVERSE_TIME_MODE) && (cm.gm.motion_mode == MOTION_MODE_STRAIGHT_FEED)) {
cm.gm.feed_rate = 0;
}
}
void cm_update_model_position_from_runtime() { copy_vector(cm.gmx.position, mr.gm.target); }
/*
* cm_deferred_write_callback() - write any changed G10 values back to persistence
*
* Only runs if there is G10 data to write, there is no movement, and the serial queues are quiescent
* This could be made tighter by issuing an XOFF or ~CTS beforehand and releasing it afterwards.
*/
stat_t cm_deferred_write_callback()
{
if ((cm.cycle_state == CYCLE_OFF) && (cm.deferred_write_flag == true)) {
#ifdef __AVR
if (xio_isbusy()) return (STAT_OK); // don't write back if serial RX is not empty
#endif
cm.deferred_write_flag = false;
nvObj_t nv;
for (uint8_t i=1; i<=COORDS; i++) {
for (uint8_t j=0; j<AXES; j++) {
sprintf((char *)nv.token, "g%2d%c", 53+i, ("xyzabc")[j]);
nv.index = nv_get_index((const char_t *)"", nv.token);
nv.value = cm.offset[i][j];
nv_persist(&nv); // Note: only writes values that have changed
}
}
}
return (STAT_OK);
}
/*
* cm_set_model_target() - set target vector in GM model
*
* This is a core routine. It handles:
* - conversion of linear units to internal canonical form (mm)
* - conversion of relative mode to absolute (internal canonical form)
* - translation of work coordinates to machine coordinates (internal canonical form)
* - computation and application of axis modes as so:
*
* DISABLED - Incoming value is ignored. Target value is not changed
* ENABLED - Convert axis values to canonical format and store as target
* INHIBITED - Same processing as ENABLED, but axis will not actually be run
* RADIUS - ABC axis value is provided in Gcode block in linear units
* - Target is set to degrees based on axis' Radius value
* - Radius mode is only processed for ABC axes. Application to XYZ is ignored.
*
* Target coordinates are provided in target[]
* Axes that need processing are signaled in flag[]
*/
// ESTEE: _calc_ABC is a fix to workaround a gcc compiler bug wherein it runs out of spill
// registers we moved this block into its own function so that we get a fresh stack push
// ALDEN: This shows up in avr-gcc 4.7.0 and avr-libc 1.8.0
static float _calc_ABC(uint8_t axis, float target[], float flag[])
{
if ((cm.a[axis].axis_mode == AXIS_STANDARD) || (cm.a[axis].axis_mode == AXIS_INHIBITED)) {
return(target[axis]); // no mm conversion - it's in degrees
}
return(_to_millimeters(target[axis]) * 360 / (2 * M_PI * cm.a[axis].radius));
}
void cm_set_model_target(float target[], float flag[])
{
uint8_t axis;
float tmp = 0;
// process XYZABC for lower modes
for (axis=AXIS_X; axis<=AXIS_Z; axis++) {
if ((fp_FALSE(flag[axis])) || (cm.a[axis].axis_mode == AXIS_DISABLED)) {
continue; // skip axis if not flagged for update or its disabled
} else if ((cm.a[axis].axis_mode == AXIS_STANDARD) || (cm.a[axis].axis_mode == AXIS_INHIBITED)) {
if (cm.gm.distance_mode == ABSOLUTE_MODE) {
cm.gm.target[axis] = cm_get_active_coord_offset(axis) + _to_millimeters(target[axis]);
} else {
cm.gm.target[axis] += _to_millimeters(target[axis]);
}
}
}
// FYI: The ABC loop below relies on the XYZ loop having been run first
for (axis=AXIS_A; axis<=AXIS_C; axis++) {
if ((fp_FALSE(flag[axis])) || (cm.a[axis].axis_mode == AXIS_DISABLED)) {
continue; // skip axis if not flagged for update or its disabled
} else {
tmp = _calc_ABC(axis, target, flag);
}
if (cm.gm.distance_mode == ABSOLUTE_MODE) {
cm.gm.target[axis] = tmp + cm_get_active_coord_offset(axis); // sacidu93's fix to Issue #22
} else {
cm.gm.target[axis] += tmp;
}
}
}
/*
* cm_test_soft_limits() - return error code if soft limit is exceeded
*
* Must be called with target properly set in GM struct. Best done after cm_set_model_target().
*
* Tests for soft limit for any homed axis if min and max are different values. You can set min
* and max to 0,0 to disable soft limits for an axis. Also will not test a min or a max if the
* value is < -1000000 (negative one million). This allows a single end to be tested w/the other
* disabled, should that requirement ever arise.
*/
stat_t cm_test_soft_limits(float target[])
{
if (cm.soft_limit_enable == true) {
for (uint8_t axis = AXIS_X; axis < AXES; axis++) {
if (cm.homed[axis] != true) continue; // don't test axes that are not homed
if (fp_EQ(cm.a[axis].travel_min, cm.a[axis].travel_max)) continue;
if ((cm.a[axis].travel_min > DISABLE_SOFT_LIMIT) && (target[axis] < cm.a[axis].travel_min)) {
return (STAT_SOFT_LIMIT_EXCEEDED);
}
if ((cm.a[axis].travel_max > DISABLE_SOFT_LIMIT) && (target[axis] > cm.a[axis].travel_max)) {
return (STAT_SOFT_LIMIT_EXCEEDED);
}
}
}
return (STAT_OK);
}
/*************************************************************************
* CANONICAL MACHINING FUNCTIONS
* Values are passed in pre-unit_converted state (from gn structure)
* All operations occur on gm (current model state)
*
* These are organized by section number (x.x.x) in the order they are
* found in NIST RS274 NGCv3
************************************************************************/
/******************************************
* Initialization and Termination (4.3.2) *
******************************************/
/*
* canonical_machine_init() - Config init cfg_init() must have been run beforehand
*/
void canonical_machine_init()
{
// If you can assume all memory has been zeroed by a hard reset you don't need this code:
// memset(&cm, 0, sizeof(cm)); // do not reset canonicalMachineSingleton once it's been initialized
memset(&cm.gm, 0, sizeof(GCodeState_t)); // clear all values, pointers and status
memset(&cm.gn, 0, sizeof(GCodeInput_t));
memset(&cm.gf, 0, sizeof(GCodeInput_t));
canonical_machine_init_assertions(); // establish assertions
ACTIVE_MODEL = MODEL; // setup initial Gcode model pointer
// set gcode defaults
cm_set_units_mode(cm.units_mode);
cm_set_coord_system(cm.coord_system);
cm_select_plane(cm.select_plane);
cm_set_path_control(cm.path_control);
cm_set_distance_mode(cm.distance_mode);
cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE);// always the default
cm.gmx.block_delete_switch = true;
// never start a machine in a motion mode
cm.gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE;
// reset request flags
cm.feedhold_requested = false;
cm.queue_flush_requested = false;
cm.cycle_start_requested = false;
// signal that the machine is ready for action
cm.machine_state = MACHINE_READY;
cm.combined_state = COMBINED_READY;
// sub-system inits
cm_spindle_init();
cm_arc_init();
}
/*
* canonical_machine_init_assertions()
* canonical_machine_test_assertions() - test assertions, return error code if violation exists
*/
void canonical_machine_init_assertions(void)
{
cm.magic_start = MAGICNUM;
cm.magic_end = MAGICNUM;
cm.gmx.magic_start = MAGICNUM;
cm.gmx.magic_end = MAGICNUM;
arc.magic_start = MAGICNUM;
arc.magic_end = MAGICNUM;
}
stat_t canonical_machine_test_assertions(void)
{
if ((cm.magic_start != MAGICNUM) || (cm.magic_end != MAGICNUM)) return (STAT_CANONICAL_MACHINE_ASSERTION_FAILURE);
if ((cm.gmx.magic_start != MAGICNUM) || (cm.gmx.magic_end != MAGICNUM)) return (STAT_CANONICAL_MACHINE_ASSERTION_FAILURE);
if ((arc.magic_start != MAGICNUM) || (arc.magic_end != MAGICNUM)) return (STAT_CANONICAL_MACHINE_ASSERTION_FAILURE);
return (STAT_OK);
}
/*
* cm_soft_alarm() - alarm state; send an exception report and stop processing input
* cm_clear() - clear soft alarm
* cm_hard_alarm() - alarm state; send an exception report and shut down machine
*/
stat_t cm_soft_alarm(stat_t status)
{
rpt_exception(status); // send alarm message
cm.machine_state = MACHINE_ALARM;
return (status); // NB: More efficient than inlining rpt_exception() call.
}
stat_t cm_clear(nvObj_t *nv) // clear soft alarm
{
if (cm.cycle_state == CYCLE_OFF) {
cm.machine_state = MACHINE_PROGRAM_STOP;
} else {
cm.machine_state = MACHINE_CYCLE;
}
return (STAT_OK);
}
stat_t cm_hard_alarm(stat_t status)
{
// stop the motors and the spindle
stepper_init(); // hard stop
cm_spindle_control(SPINDLE_OFF);
// disable all MCode functions
// gpio_set_bit_off(SPINDLE_BIT); //++++ this current stuff is temporary
// gpio_set_bit_off(SPINDLE_DIR);
// gpio_set_bit_off(SPINDLE_PWM);
// gpio_set_bit_off(MIST_COOLANT_BIT); //++++ replace with exec function
// gpio_set_bit_off(FLOOD_COOLANT_BIT); //++++ replace with exec function
rpt_exception(status); // send shutdown message
cm.machine_state = MACHINE_SHUTDOWN;
return (status);
}
/**************************
* Representation (4.3.3) *
**************************/
/**************************************************************************
* Representation functions that affect the Gcode model only (asynchronous)
*
* cm_select_plane() - G17,G18,G19 select axis plane
* cm_set_units_mode() - G20, G21
* cm_set_distance_mode() - G90, G91
* cm_set_coord_offsets() - G10 (delayed persistence)
*
* These functions assume input validation occurred upstream.
*/
stat_t cm_select_plane(uint8_t plane)
{
cm.gm.select_plane = plane;
return (STAT_OK);
}
stat_t cm_set_units_mode(uint8_t mode)
{
cm.gm.units_mode = mode; // 0 = inches, 1 = mm.
return(STAT_OK);
}
stat_t cm_set_distance_mode(uint8_t mode)
{
cm.gm.distance_mode = mode; // 0 = absolute mode, 1 = incremental
return (STAT_OK);
}
/*
* cm_set_coord_offsets() - G10 L2 Pn (affects MODEL only)
*
* This function applies the offset to the GM model but does not persist the offsets
* during the Gcode cycle. The persist flag is used to persist offsets once the cycle
* has ended. You can also use $g54x - $g59c config functions to change offsets.
*
* It also does not reset the work_offsets which may be accomplished by calling
* cm_set_work_offsets() immediately afterwards.
*/
stat_t cm_set_coord_offsets(uint8_t coord_system, float offset[], float flag[])
{
if ((coord_system < G54) || (coord_system > COORD_SYSTEM_MAX)) { // you can't set G53
return (STAT_INPUT_VALUE_RANGE_ERROR);
}
for (uint8_t axis = AXIS_X; axis < AXES; axis++) {
if (fp_TRUE(flag[axis])) {
cm.offset[coord_system][axis] = _to_millimeters(offset[axis]);
cm.deferred_write_flag = true; // persist offsets once machining cycle is over
}
}
return (STAT_OK);
}
/******************************************************************************************
* Representation functions that affect gcode model and are queued to planner (synchronous)
*/
/*
* cm_set_coord_system() - G54-G59
* _exec_offset() - callback from planner
*/
stat_t cm_set_coord_system(uint8_t coord_system)
{
cm.gm.coord_system = coord_system;
float value[AXES] = { (float)coord_system,0,0,0,0,0 }; // pass coordinate system in value[0] element
mp_queue_command(_exec_offset, value, value); // second vector (flags) is not used, so fake it
return (STAT_OK);
}
static void _exec_offset(float *value, float *flag)
{
uint8_t coord_system = ((uint8_t)value[0]); // coordinate system is passed in value[0] element
float offsets[AXES];
for (uint8_t axis = AXIS_X; axis < AXES; axis++) {
offsets[axis] = cm.offset[coord_system][axis] + (cm.gmx.origin_offset[axis] * cm.gmx.origin_offset_enable);
}
mp_set_runtime_work_offset(offsets);
cm_set_work_offsets(MODEL); // set work offsets in the Gcode model
}
/*
* cm_set_position() - set the position of a single axis in the model, planner and runtime
*
* This command sets an axis/axes to a position provided as an argument.
* This is useful for setting origins for homing, probing, and other operations.
*
* !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
* !!!!! DO NOT CALL THIS FUNCTION WHILE IN A MACHINING CYCLE !!!!!
* !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
*
* More specifically, do not call this function if there are any moves in the planner or
* if the runtime is moving. The system must be quiescent or you will introduce positional
* errors. This is true because the planned / running moves have a different reference frame
* than the one you are now going to set. These functions should only be called during
* initialization sequences and during cycles (such as homing cycles) when you know there
* are no more moves in the planner and that all motion has stopped.
* Use cm_get_runtime_busy() to be sure the system is quiescent.
*/
void cm_set_position(uint8_t axis, float position)
{
// TODO: Interlock involving runtime_busy test
cm.gmx.position[axis] = position;
cm.gm.target[axis] = position;
mp_set_planner_position(axis, position);
mp_set_runtime_position(axis, position);
mp_set_steps_to_runtime_position();
}
/*** G28.3 functions and support ***
*
* cm_set_absolute_origin() - G28.3 - model, planner and queue to runtime
* _exec_absolute_origin() - callback from planner
*
* cm_set_absolute_origin() takes a vector of origins (presumably 0's, but not necessarily)
* and applies them to all axes where the corresponding position in the flag vector is true (1).
*
* This is a 2 step process. The model and planner contexts are set immediately, the runtime
* command is queued and synchronized with the planner queue. This includes the runtime position
* and the step recording done by the encoders. At that point any axis that is set is also marked
* as homed.
*/
stat_t cm_set_absolute_origin(float origin[], float flag[])
{
float value[AXES];
for (uint8_t axis = AXIS_X; axis < AXES; axis++) {
if (fp_TRUE(flag[axis])) {
value[axis] = _to_millimeters(origin[axis]);
cm.gmx.position[axis] = value[axis]; // set model position
cm.gm.target[axis] = value[axis]; // reset model target
mp_set_planner_position(axis, value[axis]); // set mm position
}
}
mp_queue_command(_exec_absolute_origin, value, flag);
return (STAT_OK);
}
static void _exec_absolute_origin(float *value, float *flag)
{
for (uint8_t axis = AXIS_X; axis < AXES; axis++) {
if (fp_TRUE(flag[axis])) {
mp_set_runtime_position(axis, value[axis]);
cm.homed[axis] = true; // G28.3 is not considered homed until you get here
}
}
mp_set_steps_to_runtime_position();
}
/*
* cm_set_origin_offsets() - G92
* cm_reset_origin_offsets() - G92.1
* cm_suspend_origin_offsets() - G92.2
* cm_resume_origin_offsets() - G92.3
*
* G92's behave according to NIST 3.5.18 & LinuxCNC G92
* http://linuxcnc.org/docs/html/gcode/gcode.html#sec:G92-G92.1-G92.2-G92.3
*/
stat_t cm_set_origin_offsets(float offset[], float flag[])
{
// set offsets in the Gcode model extended context
cm.gmx.origin_offset_enable = 1;
for (uint8_t axis = AXIS_X; axis < AXES; axis++) {
if (fp_TRUE(flag[axis])) {
cm.gmx.origin_offset[axis] = cm.gmx.position[axis] -
cm.offset[cm.gm.coord_system][axis] - _to_millimeters(offset[axis]);
}
}
// now pass the offset to the callback - setting the coordinate system also applies the offsets
float value[AXES] = { (float)cm.gm.coord_system,0,0,0,0,0 }; // pass coordinate system in value[0] element
mp_queue_command(_exec_offset, value, value); // second vector is not used
return (STAT_OK);
}
stat_t cm_reset_origin_offsets()
{
cm.gmx.origin_offset_enable = 0;
for (uint8_t axis = AXIS_X; axis < AXES; axis++) {
cm.gmx.origin_offset[axis] = 0;
}
float value[AXES] = { (float)cm.gm.coord_system,0,0,0,0,0 };
mp_queue_command(_exec_offset, value, value);
return (STAT_OK);
}
stat_t cm_suspend_origin_offsets()
{
cm.gmx.origin_offset_enable = 0;
float value[AXES] = { (float)cm.gm.coord_system,0,0,0,0,0 };
mp_queue_command(_exec_offset, value, value);
return (STAT_OK);
}
stat_t cm_resume_origin_offsets()
{
cm.gmx.origin_offset_enable = 1;
float value[AXES] = { (float)cm.gm.coord_system,0,0,0,0,0 };
mp_queue_command(_exec_offset, value, value);
return (STAT_OK);
}
stat_t cm_get_position()
{
// M114: Get current position, see https://www.reprap.org/wiki/G-code#M114:_Get_Current_Position
size_t len = 0;
for (uint8_t axis = AXIS_X; axis < AXES; axis++) {
float position = cm.gm.target[axis] - cm_get_active_coord_offset(axis);
len += sprintf(global_string_buf + len, "%c:%0.4f ", "XYZABC"[axis], position);
}
// Replace last trailing space with newline.
global_string_buf[len-1] = '\n';
fprintf(stderr, global_string_buf);
return (STAT_OK);
}
stat_t cm_get_firmware()
{
// M115: Get Firmware Version and Capabilities, see https://www.reprap.org/wiki/G-code#M115:_Get_Firmware_Version_and_Capabilities.
static const char m115_response[] PROGMEM = "FIRMWARE_NAME:TinyG, FIRMWARE_URL:https%%3A//github.com/synthetos/TinyG, FIRMWARE_VERSION:%0.2f, FIRMWARE_BUILD:%0.2f, HARDWARE_PLATFORM:%0.2f, HARDWARE_VERSION:%0.2f\n";
fprintf_P(stderr, m115_response, cs.fw_version, cs.fw_build, cs.hw_platform, cs.hw_version);
return (STAT_OK);
}
stat_t cm_set_jerk(float offset[], float flag[])
{
// M201.3: Set axes jerk limits, transiently.
for (uint8_t axis = AXIS_X; axis < AXES; axis++) {
if (fp_TRUE(flag[axis])) {
// convert from unit/s^3 to unit/min^3 and divide by the multiplier.
cm_set_axis_jerk(axis, max(1, _to_millimeters(offset[axis])*(60.f*60.f*60.f)/JERK_MULTIPLIER));
}
}
return (STAT_OK);
}
stat_t cm_wait_for_completion()
{
// M400: Wait for current moves to finish, see https://www.reprap.org/wiki/G-code#M400:_Wait_for_current_moves_to_finish
// Wait for all buffers to be drained, before accepting the next command.
cm_set_buffer_drain_state(DRAIN_REQUESTED);
// Suppress the status message, if in text mode. A prompt will be issued when drained.
if (cfg.comm_mode == TEXT_MODE)
return (STAT_EAGAIN);
return (STAT_OK);
}
/*****************************
* Free Space Motion (4.3.4) *
*****************************/
/*
* cm_straight_traverse() - G0 linear rapid
*/
stat_t cm_straight_traverse(float target[], float flags[])
{
cm.gm.motion_mode = MOTION_MODE_STRAIGHT_TRAVERSE;
cm_set_model_target(target, flags);
// test soft limits
stat_t status = cm_test_soft_limits(cm.gm.target);
if (status != STAT_OK) return (cm_soft_alarm(status));
// prep and plan the move
cm_set_work_offsets(&cm.gm); // capture the fully resolved offsets to the state
cm_cycle_start(); // required for homing & other cycles
mp_aline(&cm.gm); // send the move to the planner
cm_finalize_move();
return (STAT_OK);
}
/*
* cm_set_g28_position() - G28.1
* cm_goto_g28_position() - G28
* cm_set_g30_position() - G30.1
* cm_goto_g30_position() - G30
*/
stat_t cm_set_g28_position(void)
{
copy_vector(cm.gmx.g28_position, cm.gmx.position);
return (STAT_OK);
}
stat_t cm_goto_g28_position(float target[], float flags[])
{
cm_set_absolute_override(MODEL, true);
cm_straight_traverse(target, flags); // move through intermediate point, or skip
while (mp_get_planner_buffers_available() == 0); // make sure you have an available buffer
float f[] = {1,1,1,1,1,1};
return(cm_straight_traverse(cm.gmx.g28_position, f));// execute actual stored move
}
stat_t cm_set_g30_position(void)
{
copy_vector(cm.gmx.g30_position, cm.gmx.position);
return (STAT_OK);
}
stat_t cm_goto_g30_position(float target[], float flags[])
{
cm_set_absolute_override(MODEL, true);
cm_straight_traverse(target, flags); // move through intermediate point, or skip
while (mp_get_planner_buffers_available() == 0); // make sure you have an available buffer
float f[] = {1,1,1,1,1,1};
return(cm_straight_traverse(cm.gmx.g30_position, f));// execute actual stored move
}
/********************************
* Machining Attributes (4.3.5) *
********************************/
/*
* cm_set_feed_rate() - F parameter (affects MODEL only)
*
* Normalize feed rate to mm/min or to minutes if in inverse time mode
*/
stat_t cm_set_feed_rate(float feed_rate)
{
if (cm.gm.feed_rate_mode == INVERSE_TIME_MODE) {
cm.gm.feed_rate = 1 / feed_rate; // normalize to minutes (NB: active for this gcode block only)
} else {
cm.gm.feed_rate = _to_millimeters(feed_rate);
}
return (STAT_OK);
}
/*
* cm_set_feed_rate_mode() - G93, G94 (affects MODEL only)
*
* INVERSE_TIME_MODE = 0, // G93
* UNITS_PER_MINUTE_MODE, // G94
* UNITS_PER_REVOLUTION_MODE // G95 (unimplemented)
*/
stat_t cm_set_feed_rate_mode(uint8_t mode)
{
cm.gm.feed_rate_mode = mode;
return (STAT_OK);
}
/*
* cm_set_path_control() - G61, G61.1, G64 (affects MODEL only)
*/
stat_t cm_set_path_control(uint8_t mode)
{
cm.gm.path_control = mode;
return (STAT_OK);
}
/*******************************
* Machining Functions (4.3.6) *
*******************************/
/*
* cm_arc_feed() - SEE plan_arc.c(pp)
*/
/*
* cm_dwell() - G4, P parameter (seconds)
*/
stat_t cm_dwell(float seconds)
{
cm.gm.parameter = seconds;
mp_dwell(seconds);
return (STAT_OK);
}
/*
* cm_straight_feed() - G1
*/
stat_t cm_straight_feed(float target[], float flags[])
{
// trap zero feed rate condition
if ((cm.gm.feed_rate_mode != INVERSE_TIME_MODE) && (fp_ZERO(cm.gm.feed_rate))) {
return (STAT_GCODE_FEEDRATE_NOT_SPECIFIED);
}
cm.gm.motion_mode = MOTION_MODE_STRAIGHT_FEED;