-
Notifications
You must be signed in to change notification settings - Fork 1.1k
/
code-notes.adoc
1409 lines (1048 loc) · 62 KB
/
code-notes.adoc
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
:lang: en
:toc:
[[cha:code-notes]]
= Code Notes
// Custom lang highlight
// must come after the doc title, to work around a bug in asciidoc 8.6.6
:ini: {basebackend@docbook:'':ini}
:hal: {basebackend@docbook:'':hal}
:ngc: {basebackend@docbook:'':ngc}
== Intended audience
This document is a collection of notes about the internals of LinuxCNC. It
is primarily of interest to developers, however much of the information
here may also be of interest to system integrators and others who are
simply curious about how LinuxCNC works. Much of this information is now
outdated and has never been reviewed for accuracy.
== Organization
There will be a chapter for each of the major components of LinuxCNC, as
well as chapter(s) covering how they work together. This document is
very much a work in progress, and its layout may change in the future.
== Terms and definitions
* 'AXIS' - An axis is one of the nine degrees of freedom that define a tool
position in three-dimensional Cartesian space. Those nine axes are
referred to as X, Y, Z, A, B, C, U, V, and W. The linear orthogonal
coordinates X, Y, and Z determine where the tip of the tool is
positioned. The angular coordinates A, B, and C determine the tool
orientation. A second set of linear orthogonal coordinates U, V, and W
allows tool motion (typically for cutting actions) relative to the
previously offset and rotated axes.
Unfortunately "axis" is also
sometimes used to mean a degree of freedom of the machine itself, such
as the saddle, table, or quill of a Bridgeport type milling machine. On
a Bridgeport this causes no confusion, since movement of the table
directly corresponds to movement along the X axis. However, the
shoulder and elbow joints of a robot arm and the linear actuators of a
hexapod do not correspond to movement along any Cartesian axis, and in
general it is important to make the distinction between the Cartesian
axes and the machine degrees of freedom. In this document, the latter
will be called 'joints', not axes. The GUIs and some other parts of
the code may not always follow this distinction, but the internals of
the motion controller do.
* 'JOINT' - A joint is one of the movable parts of the machine. Joints are
distinct from axes, although the two terms are sometimes (mis)used to
mean the same thing. In LinuxCNC, a joint is a physical thing that can be
moved, not a coordinate in space. For example, the quill, knee, saddle,
and table of a Bridgeport mill are all joints. The shoulder, elbow, and
wrist of a robot arm are joints, as are the linear actuators of a
hexapod. Every joint has a motor or actuator of some type associated
with it. Joints do not necessarily correspond to the X, Y, and Z axes,
although for machines with trivial kinematics that may be the case.
Even on those machines, joint position and axis position are
fundamentally different things. In this document, the terms 'joint' and
'axis' are used carefully to respect their distinct meanings.
Unfortunately that isn't necessarily true everywhere else. In
particular, GUIs for machines with trivial kinematics may gloss over or
completely hide the distinction between joints and axes. In addition,
the INI file uses the term 'axis' for data that would more accurately
be described as joint data, such as input and output scaling, etc.
[NOTE]
This distinction was made in version 2.8 of LinuxCNC.
The INI file got a new section [JOINT_<num>]. Many of the parameters that were previously
proper to the [AXIS_<letter>] section are now in the new section. Other sections,
such as [KINS], also take on new parameters to match this.
An update script has been provided to transform old INI files to the new axes/joints configuration.
* 'POSE' - A pose is a fully specified position in 3D Cartesian space. In
the LinuxCNC motion controller, when we refer to a pose we mean an
EmcPose structure, containing six linear coordinates (X, Y, Z, U,
V, and W) and three angular ones (A, B, and C).
* 'coord', or coordinated mode, means that all articulations are synchronized and
they move together as directed by the higher-level code. It is the normal mode when machining.
In coordinated mode, commands are assumed to be given in the Cartesian reference frame,
and if the machine is not Cartesian, the commands are translated by the kinematics to drive
each joint into the joint space as needed.
* 'free' means that commands are interpreted in joint space.
It is used to manually move (jog) individual joints, although it does not prevent them from moving
multiple joints at once (I think).
Homing is also done in free mode; in fact, machines with non-trivial kinematics
must be homed before they can go into coord or teleop mode.
* 'teleop' is the mode you probably need if you are jogging with a hexapod.
The jog commands implemented by the motion controller are joint jogs, which
work in free mode. But if you want to move a hexapod or similar machine along a
cartesian axis in particular, you must operate more than one joint.
That's what 'teleop' is for.
== Architecture overview
There are four components contained in the LinuxCNC Architecture: a motion
controller (EMCMOT), a discrete IO controller (EMCIO), a task executor
which coordinates them (EMCTASK) and several text-mode and graphical
User Interfaces. Each of them will be described in the current
document, both from the design point of view and from the developers
point of view (where to find needed data, how to easily extend/modify
things, etc.).
image::LinuxCNC-block-diagram-small.png[align="center"]
=== LinuxCNC software architecture
At the coarsest level, LinuxCNC is a
hierarchy of three controllers: the task level command handler and program
interpreter, the motion controller, and the discrete I/O controller. The
discrete I/O controller is implemented as a hierarchy of controllers,
in this case for spindle, coolant, and auxiliary (e.g., estop)
subsystems. The task controller coordinates the actions of the motion and
discrete I/O controllers. Their actions are programmed in conventional
numerical control "G and M code" programs, which are interpreted by
the task controller into NML messages and sent to the motion.
== Motion Controller Introduction
The motion controller is a realtime component. It receives motion control commands from the non-realtime parts of LinuxCNC (i.e. the G-code interpreter/Task, GUIs, etc) and executes those commands within its realtime context. The communication from non-realtime context to realtime context happens via a message-passing IPC mechanism using shared memory, and via the Hardware Abstraction Layer (HAL).
The status of the motion controller is made available to the rest of LinuxCNC through the same message-passing shared memory IPC, and through HAL.
The motion controller interacts with the motor controllers and other realtime and non-realtime hardware using HAL.
This document assumes that the reader has a basic understanding
of the HAL, and uses terms like HAL pins, HAL signals, etc, without
explaining them. For more information about the HAL, see the
HAL Manual. Another chapter of this document will
eventually go into the internals of the HAL itself, but in this
chapter, we only use the HAL API as defined in src/hal/hal.h.
=== Motion Controller Modules
The realtime functions of the motion controller are implemented with realtime modules -- userspace shared objects for Preempt-RT systems or kernel modules for some kernel-mode realtime implementations such as RTAI:
* 'tpmod' - trajectory planning
* 'homemod' - homing functions
* 'motmod' - processes NML commands and controls hardware via HAL
* 'kinematics module' - performs forward (joints-->coordinates) and inverse (coordinates->joints) kinematics calculations
LinuxCNC is started by a *linuxcnc* script which reads a
configuration INI file and starts all needed processes. For
realtime motion control, the script first loads the default tpmod
and homemod modules and then loads the kinematics and motion
modules according to settings in halfiles specified by the INI
file.
Custom (user-built) homing or trajectory-planning modules can
be used in place of the default modules via INI file settings
or command line options. Custom modules must implement all
functions used by the default modules. The halcompile utility
can be used to create a custom module.
image::LinuxCNC-motion-controller-small.png[align="center"]
== Block diagrams and Data Flow
The following figure is a block diagram
of a joint controller. There is one joint controller per joint. The
joint controllers work at a lower level than the kinematics, a level
where all joints are completely independent. All the data for a joint
is in a single joint structure. Some members of that structure are
visible in the block diagram, such as coarse_pos, pos_cmd, and
motor_pos_fb.
.Joint Controller Block Diagram
image::emc2-motion-joint-controller-block-diag.png[align="center"]
The above figure shows five of the
seven sets of position information that form the main data flow through
the motion controller. The seven forms of position data are as follows:
* 'emcmotStatus\->carte_pos_cmd' - This is the desired position, in
Cartesian coordinates. It is updated at the traj rate, not the servo
rate. In coord mode, it is determined by the traj planner. In teleop
mode, it is determined by the traj planner? In free mode, it is either
copied from actualPos, or generated by applying forward kins to (2) or
(3).
* 'emcmotStatus\->joints[n].coarse_pos' - This is the desired position, in
joint coordinates, but before interpolation. It is updated at the traj
rate, not the servo rate. In coord mode, it is generated by applying
inverse kins to (1) In teleop mode, it is generated by applying inverse
kins to (1) In free mode, it is copied from (3), I think.
* 'emcmotStatus\->joints[n].pos_cmd - This is the desired position, in
joint coords, after interpolation. A new set of these coords is
generated every servo period. In coord mode, it is generated from (2)
by the interpolator. In teleop mode, it is generated from (2) by the
interpolator. In free mode, it is generated by the free mode traj
planner.
* 'emcmotStatus\->joints[n].motor_pos_cmd' - This is the desired position,
in motor coords. Motor coords are generated by adding backlash
compensation, lead screw error compensation, and offset (for homing) to
(3). It is generated the same way regardless of the mode, and is the
output to the PID loop or other position loop.
* 'emcmotStatus\->joints[n].motor_pos_fb' - This is the actual position, in
motor coords. It is the input from encoders or other feedback device
(or from virtual encoders on open loop machines). It is "generated" by
reading the feedback device.
* 'emcmotStatus\->joints[n].pos_fb' - This is the actual position, in joint
coordinates. It is generated by subtracting offset, lead screw error
compensation, and backlash compensation from (5). It is generated the
same way regardless of the operating mode.
* 'emcmotStatus\->carte_pos_fb' - This is the actual position, in Cartesian
coordinates. It is updated at the traj rate, not the servo rate.
Ideally, actualPos would always be calculated by applying forward
kinematics to (6). However, forward kinematics may not be available, or
they may be unusable because one or more axes aren't homed. In that
case, the options are: A) fake it by copying (1), or B) admit that we
don't really know the Cartesian coordinates, and simply don't update
actualPos. Whatever approach is used, I can see no reason not to do it
the same way regardless of the operating mode. I would propose the
following: If there are forward kins, use them, unless they don't work
because of unhomed axes or other problems, in which case do (B). If no
forward kins, do (A), since otherwise actualPos would _never_ get
updated.
== Homing
=== Homing state diagram
image::homing.svg[align="center"]
=== Another homing diagram
image::hss.svg[align="center"]
== Commands
This section simply lists all of the commands that can be sent to the
motion module, along with detailed explanations of what they do. The
command names are defined in a large typedef enum in
{linuxcnc}/src/emc/motion/motion.h, called cmd_code_t. (Note that in the
code, each command name starts with 'EMCMOT_', which is omitted here.)
The commands are implemented by a large switch statement in the
function emcmotCommandHandler(), which is called at the servo rate.
More on that function later.
There are approximately 44 commands - this list is still under
construction.
[NOTE]
The cmd_code_t enumeration, in motion.h, contains 73 commands, but the switch
statement in command.c contemplates only 70 commands (as of 6/5/2020).
ENABLE_WATCHDOG / DISABLE_WATCHDOG commands are in motion-logger.c. Maybe they are obsolete.
The SET_TELEOP_VECTOR command only appears in motion-logger.c, with no effect other than its own log.
=== ABORT
The ABORT command simply stops all motion. It can be issued at any
time, and will always be accepted. It does not disable the motion
controller or change any state information, it simply cancels any
motion that is currently in progress.footnote:[It seems that the
higher level code (TASK and above) also use ABORT to clear faults.
Whenever there is a persistent fault (such as being outside the
hardware limit switches), the higher level code sends a constant
stream of ABORTs to the motion controller trying to make the
fault go away. Thousands of them.... That means that the motion
controller should avoid persistent faults. This needs to be looked
into.]
==== Requirements
None. The command is always accepted and acted on immediately.
==== Results
In free mode, the free mode trajectory planners are disabled. That
results in each joint stopping as fast as its accel (decel) limit
allows. The stop is not coordinated. In teleop mode, the commanded
Cartesian velocity is set to zero. I don't know exactly what kind of
stop results (coordinated, uncoordinated, etc), but will figure it out
eventually. In coord mode, the coord mode trajectory planner is told to
abort the current move. Again, I don't know the exact result of this,
but will document it when I figure it out.
=== FREE
The FREE command puts the motion controller in free mode. Free mode
means that each joint is independent of all the other joints. Cartesian
coordinates, poses, and kinematics are ignored when in free mode. In
essence, each joint has its own simple trajectory planner, and each
joint completely ignores the other joints. Some commands (like Joint
JOG and HOME) only work in free mode. Other commands, including anything
that deals with Cartesian coordinates, do not work at all in free mode.
==== Requirements
The command handler applies no requirements to the FREE command, it
will always be accepted. However, if any joint is in motion
(GET_MOTION_INPOS_FLAG() == FALSE), then the command will be ignored.
This behavior is controlled by code that is now located in the function
'set_operating_mode()' in control.c, that code needs to be cleaned up.
I believe the command should not be silently ignored, instead the
command handler should determine whether it can be executed and return
an error if it cannot.
==== Results
If the machine is already in free mode, nothing. Otherwise, the
machine is placed in free mode. Each joint's free mode trajectory
planner is initialized to the current location of the joint, but the
planners are not enabled and the joints are stationary.
=== TELEOP
The TELEOP command places the machine in teleoperating mode. In teleop
mode, movement of the machine is based on Cartesian coordinates using
kinematics, rather than on individual joints as in free mode. However
the trajectory planner per se is not used, instead movement is
controlled by a velocity vector. Movement in teleop mode is much like
jogging, except that it is done in Cartesian space instead of joint
space. On a machine with trivial kinematics, there is little difference
between teleop mode and free mode, and GUIs for those machines might
never even issue this command. However for non-trivial machines like
robots and hexapods, teleop mode is used for most user commanded jog
type movements.
==== Requirements
The command handler will reject the TELEOP command with an error
message if the kinematics cannot be activated because the one or more
joints have not been homed. In addition, if any joint is in motion
(GET_MOTION_INPOS_FLAG() == FALSE), then the command will be ignored
(with no error message). This behavior is controlled by code that is
now located in the function 'set_operating_mode()' in control.c. I
believe the command should not be silently ignored, instead the command
handler should determine whether it can be executed and return an error
if it cannot.
==== Results
If the machine is already in teleop mode, nothing. Otherwise the
machine is placed in teleop mode. The kinematics code is activated,
interpolators are drained and flushed, and the Cartesian velocity
commands are set to zero.
=== COORD
The COORD command places the machine in coordinated mode. In coord
mode, movement of the machine is based on Cartesian coordinates using
kinematics, rather than on individual joints as in free mode. In
addition, the main trajectory planner is used to generate motion, based
on queued LINE, CIRCLE, and/or PROBE commands. Coord mode is the mode
that is used when executing a G-code program.
==== Requirements
The command handler will reject the COORD command with an error
message if the kinematics cannot be activated because the one or more
joints have not been homed. In addition, if any joint is in motion
(GET_MOTION_INPOS_FLAG() == FALSE), then the command will be ignored
(with no error message). This behavior is controlled by code that is
now located in the function 'set_operating_mode()' in control.c. I
believe the command should not be silently ignored, instead the command
handler should determine whether it can be executed and return an error
if it cannot.
==== Results
If the machine is already in coord mode, nothing. Otherwise, the
machine is placed in coord mode. The kinematics code is activated,
interpolators are drained and flushed, and the trajectory planner
queues are empty. The trajectory planner is active and awaiting a LINE,
CIRCLE, or PROBE command.
=== ENABLE
The ENABLE command enables the motion controller.
==== Requirements
None. The command can be issued at any time, and will always be
accepted.
==== Results
If the controller is already enabled, nothing. If not, the controller
is enabled. Queues and interpolators are flushed. Any movement or
homing operations are terminated. The amp-enable outputs associated
with active joints are turned on. If forward kinematics are not
available, the machine is switched to free mode.
=== DISABLE
The DISABLE command disables the motion controller.
==== Requirements
None. The command can be issued at any time, and will always be
accepted.
==== Results
If the controller is already disabled, nothing. If not, the controller
is disabled. Queues and interpolators are flushed. Any movement or
homing operations are terminated. The amp-enable outputs associated
with active joints are turned off. If forward kinematics are not
available, the machine is switched to free mode.
=== ENABLE_AMPLIFIER
The ENABLE_AMPLIFIER command turns on the amp enable output for a
single output amplifier, without changing anything else. Can be used to
enable a spindle speed controller.
==== Requirements
None. The command can be issued at any time, and will always be
accepted.
==== Results
Currently, nothing. (A call to the old extAmpEnable function is
currently commented out.) Eventually it will set the amp enable HAL pin
true.
=== DISABLE_AMPLIFIER
The DISABLE_AMPLIFIER command turns off the amp enable output for a
single amplifier, without changing anything else. Again, useful for
spindle speed controllers.
==== Requirements
None. The command can be issued at any time, and will always be
accepted.
==== Results
Currently, nothing. (A call to the old extAmpEnable function is
currently commented out.) Eventually it will set the amp enable HAL pin
false.
=== ACTIVATE_JOINT
The ACTIVATE_JOINT command turns on all the calculations associated
with a single joint, but does not change the joint's amp enable output
pin.
==== Requirements
None. The command can be issued at any time, and will always be
accepted.
==== Results
Calculations for the specified joint are enabled. The amp enable pin
is not changed, however, any subsequent ENABLE or DISABLE commands will
modify the joint's amp enable pin.
=== DEACTIVATE_JOINT
The DEACTIVATE_JOINT command turns off all the calculations associated
with a single joint, but does not change the joint's amp enable output
pin.
==== Requirements
None. The command can be issued at any time, and will always be
accepted.
==== Results
Calculations for the specified joint are enabled. The amp enable pin
is not changed, and subsequent ENABLE or DISABLE commands will not
modify the joint's amp enable pin.
=== ENABLE_WATCHDOG
The ENABLE_WATCHDOG command enables a hardware based watchdog (if
present).
==== Requirements
None. The command can be issued at any time, and will always be
accepted.
==== Results
Currently nothing. The old watchdog was a strange thing that used a
specific sound card. A new watchdog interface may be designed in the
future.
=== DISABLE_WATCHDOG
The DISABLE_WATCHDOG command disables a hardware based watchdog (if
present).
==== Requirements
None. The command can be issued at any time, and will always be
accepted.
==== Results
Currently nothing. The old watchdog was a strange thing that used a
specific sound card. A new watchdog interface may be designed in the
future.
=== PAUSE
The PAUSE command stops the trajectory planner. It has no effect in
free or teleop mode. At this point I don't know if it pauses all motion
immediately, or if it completes the current move and then pauses before
pulling another move from the queue.
==== Requirements
None. The command can be issued at any time, and will always be
accepted.
==== Results
The trajectory planner pauses.
=== RESUME
The RESUME command restarts the trajectory planner if it is paused. It
has no effect in free or teleop mode, or if the planner is not paused.
==== Requirements
None. The command can be issued at any time, and will always be
accepted.
==== Results
The trajectory planner resumes.
=== STEP
The STEP command restarts the trajectory planner if it is paused, and
tells the planner to stop again when it reaches a specific point. It
has no effect in free or teleop mode. At this point I don't know
exactly how this works. I'll add more documentation here when I dig
deeper into the trajectory planner.
==== Requirements
None. The command can be issued at any time, and will always be
accepted.
==== Results
The trajectory planner resumes, and later pauses when it reaches a
specific point.
=== SCALE
The SCALE command scales all velocity limits and commands by a
specified amount. It is used to implement feed rate override and other
similar functions. The scaling works in free, teleop, and coord modes,
and affects everything, including homing velocities, etc. However,
individual joint velocity limits are unaffected.
==== Requirements
None. The command can be issued at any time, and will always be
accepted.
==== Results
All velocity commands are scaled by the specified constant.
=== OVERRIDE_LIMITS
The OVERRIDE_LIMITS command prevents limits from tripping until the
end of the next JOG command. It is normally used to allow a machine to
be jogged off of a limit switch after tripping. (The command can
actually be used to override limits, or to cancel a previous override.)
==== Requirements
None. The command can be issued at any time, and will always be
accepted. (I think it should only work in free mode.)
==== Results
Limits on all joints are over-ridden until the end of the next JOG
command. (This is currently broken... once an OVERRIDE_LIMITS command
is received, limits are ignored until another OVERRIDE_LIMITS command
re-enables them.)
=== HOME
The HOME command initiates a homing sequence on a specified joint. The
actual homing sequence is determined by a number of configuration
parameters, and can range from simply setting the current position to
zero, to a multi-stage search for a home switch and index pulse,
followed by a move to an arbitrary home location. For more information
about the homing sequence, see the homing section of the Integrator Manual.
==== Requirements
The command will be ignored silently unless the machine is in free mode.
==== Results
Any jog or other joint motion is aborted, and the homing sequence
starts.
=== JOG_CONT
The JOG_CONT command initiates a continuous jog on a single joint. A
continuous jog is generated by setting the free mode trajectory
planner's target position to a point beyond the end of the joint's
range of travel. This ensures that the planner will move constantly
until it is stopped by either the joint limits or an ABORT command.
Normally, a GUI sends a JOG_CONT command when the user presses a jog
button, and ABORT when the button is released.
==== Requirements
The command handler will reject the JOG_CONT command with an error
message if machine is not in free mode, or if any joint is in motion
(GET_MOTION_INPOS_FLAG() == FALSE), or if motion is not enabled. It
will also silently ignore the command if the joint is already at or
beyond its limit and the commanded jog would make it worse.
==== Results
The free mode trajectory planner for the joint identified by
emcmotCommand\->axis is activated, with a target position beyond the end
of joint travel, and a velocity limit of emcmotCommand\->vel. This
starts the joint moving, and the move will continue until stopped by an
ABORT command or by hitting a limit. The free mode planner accelerates
at the joint accel limit at the beginning of the move, and will
decelerate at the joint accel limit when it stops.
=== JOG_INCR
The JOG_INCR command initiates an incremental jog on a single joint.
Incremental jogs are cumulative, in other words, issuing two JOG_INCR
commands that each ask for 0.100 inches of movement will result in
0.200 inches of travel, even if the second command is issued before the
first one finishes. Normally incremental jogs stop when they have
traveled the desired distance, however they also stop when they hit a
limit, or on an ABORT command.
==== Requirements
The command handler will silently reject the JOG_INCR command if
machine is not in free mode, or if any joint is in motion
(GET_MOTION_INPOS_FLAG() == FALSE), or if motion is not enabled. It
will also silently ignore the command if the joint is already at or
beyond its limit and the commanded jog would make it worse.
==== Results
The free mode trajectory planner for the joint identified by
emcmotCommand\->axis is activated, the target position is
incremented/decremented by emcmotCommand\->offset, and the velocity
limit is set to emcmotCommand\->vel. The free mode trajectory planner
will generate a smooth trapezoidal move from the present position to
the target position. The planner can correctly handle changes in the
target position that happen while the move is in progress, so multiple
JOG_INCR commands can be issued in quick succession. The free mode
planner accelerates at the joint accel limit at the beginning of the
move, and will decelerate at the joint accel limit to stop at the
target position.
=== JOG_ABS
The JOG_ABS command initiates an absolute jog on a single joint. An
absolute jog is a simple move to a specific location, in joint
coordinates. Normally absolute jogs stop when they reach the desired
location, however they also stop when they hit a limit, or on an ABORT
command.
==== Requirements
The command handler will silently reject the JOG_ABS command if
machine is not in free mode, or if any joint is in motion
(GET_MOTION_INPOS_FLAG() == FALSE), or if motion is not enabled. It
will also silently ignore the command if the joint is already at or
beyond its limit and the commanded jog would make it worse.
==== Results
The free mode trajectory planner for the joint identified by
emcmotCommand\->axis is activated, the target position is set to
emcmotCommand\->offset, and the velocity limit is set to
emcmotCommand\->vel. The free mode trajectory planner will generate a
smooth trapezoidal move from the present position to the target
position. The planner can correctly handle changes in the target
position that happen while the move is in progress. If multiple JOG_ABS
commands are issued in quick succession, each new command changes the
target position and the machine goes to the final commanded position.
The free mode planner accelerates at the joint accel limit at the
beginning of the move, and will decelerate at the joint accel limit to
stop at the target position.
=== SET_LINE
The SET_LINE command adds a straight line to the trajectory planner
queue.
(More later)
=== SET_CIRCLE
The SET_CIRCLE command adds a circular move to the trajectory planner
queue.
(More later)
=== SET_TELEOP_VECTOR
The SET_TELEOP_VECTOR command instructs the motion controller to move
along a specific vector in Cartesian space.
(More later)
=== PROBE
The PROBE command instructs the motion controller to move toward a
specific point in Cartesian space, stopping and recording its
position if the probe input is triggered.
(More later)
=== CLEAR_PROBE_FLAG
The CLEAR_PROBE_FLAG command is used to reset the probe input in
preparation for a PROBE command. (Question: why shouldn't the PROBE
command automatically reset the input?)
(More later)
=== SET_xix
There are approximately 15 SET_xxx commands, where xxx is the name of
some configuration parameter. It is anticipated that there will be
several more SET commands as more parameters are added. I would like to
find a cleaner way of setting and reading configuration parameters. The
existing methods require many lines of code to be added to multiple
files each time a parameter is added. Much of that code is identical or
nearly identical for every parameter.
== Backlash and Screw Error Compensation
+ FIXME Backlash and Screw Error Compensation
== Task controller (EMCTASK)
=== State
Task has three possible internal states: *E-stop*, *E-stop Reset*,
and *Machine On*.
image::task-state-transitions.svg[align="center"]
== IO controller (EMCIO)
The I/O Controller is part of TASK.
It interacts with external I/O using HAL pins.
Currently ESTOP/Enable, coolant, and tool changing are handled by
iocontrol. These are relatively low speed events, high speed coordinated I/O is handled in motion.
emctaskmain.cc sends I/O commands via taskclass.cc.
iocontrol main loop process:
- checks to see it HAL inputs have changed
- checks if read_tool_inputs() indicates the tool change is finished and set emcioStatus.status
== User Interfaces
FIXME User Interfaces
== libnml Introduction
libnml is derived from the NIST rcslib without all the multi-platform
support. Many of the wrappers around platform specific code has been
removed along with much of the code that is not required by LinuxCNC. It is
hoped that sufficient compatibility remains with rcslib so that
applications can be implemented on non-Linux platforms and still be
able to communicate with LinuxCNC.
This chapter is not intended to be a definitive guide to using libnml
(or rcslib), instead, it will eventually provide an overview of each
C++ class and their member functions. Initially, most of these notes
will be random comments added as the code scrutinized and modified.
== LinkedList
Base class to maintain a linked list. This is one of the core building
blocks used in passing NML messages and assorted internal data
structures.
== LinkedListNode
Base class for producing a linked list - Purpose, to hold pointers to
the previous and next nodes, pointer to the data, and the size of the
data.
No memory for data storage is allocated.
== SharedMemory
Provides a block of shared memory along with a semaphore (inherited
from the Semaphore class). Creation and destruction of the semaphore is
handled by the SharedMemory constructor and destructor.
== ShmBuffer
Class for passing NML messages between local processes using a shared
memory buffer. Much of internal workings are inherited from the CMS
class.
== Timer
The Timer class provides a periodic timer limited only by the
resolution of the system clock. If, for example, a process needs to be
run every 5 seconds regardless of the time taken to run the process,
the following code snippet demonstrates how :
[source,c]
----
main()
{
timer = new Timer(5.0); /* Initialize a timer with a 5 second loop */
while(0) {
/* Do some process */
timer.wait(); /* Wait till the next 5 second interval */
}
delete timer;
}
----
== Semaphore
The Semaphore class provides a method of mutual exclusions for
accessing a shared resource. The function to get a semaphore can either
block until access is available, return after a timeout, or return
immediately with or without gaining the semaphore. The constructor will
create a semaphore or attach to an existing one if the ID is already in
use.
The Semaphore::destroy() must be called by the last process only.
== CMS
At the heart of libnml is the CMS class, it contains most of the
functions used by libnml and ultimately NML. Many of the internal
functions are overloaded to allow for specific hardware dependent
methods of data passing. Ultimately, everything revolves around a
central block of memory (referred to as the 'message buffer' or just
'buffer'). This buffer may exist as a shared memory block accessed by
other CMS/NML processes, or a local and private buffer for data being
transferred by network or serial interfaces.
The buffer is dynamically allocated at run time to allow for greater
flexibility of the CMS/NML sub-system. The buffer size must be large
enough to accommodate the largest message, a small amount for internal
use and allow for the message to be encoded if this option is chosen
(encoded data will be covered later). The following figure is an
internal view of the buffer space.
image::CMS_buffer.png[align="center"]
.CMS buffer
The CMS base class is primarily responsible for creating the
communications pathways and interfacing to the operating system.
////////////////////////////////////////////////////////////////////////
== NML Notes /* FIX ME */
A collection of random notes and thought whilst studying the libnml
and rcslib code.
Much of this needs to be edited and re-written in a coherent manner
before publication.
////////////////////////////////////////////////////////////////////////
== Configuration file format
NML configuration consists of two types of line formats. One for
Buffers, and a second for Processes that connect to the buffers.
=== Buffer line
The original NIST format of the buffer line is:
* 'B name type host size neut RPC# buffer# max_procs key [type specific configs]'
* 'B' - identifies this line as a Buffer configuration.
* 'name' - is the identifier of the buffer.
* 'type' - describes the buffer type - SHMEM, LOCMEM, FILEMEM, PHANTOM, or GLOBMEM.
* 'host' - is either an IP address or host name for the NML server
* 'size' - is the size of the buffer
* 'neut' - a boolean to indicate if the data in the buffer is encoded in a
machine independent format, or raw.
* 'RPC#' - Obsolete - Place holder retained for backward compatibility only.
* 'buffer#' - A unique ID number used if a server controls multiple buffers.
* 'max_procs' - is the maximum processes allowed to connect to this buffer.
* 'key' - is a numerical identifier for a shared memory buffer
=== Type specific configs
The buffer type implies additional configuration options whilst the
host operating system precludes certain combinations. In an attempt to
distill published documentation in to a coherent format, only the *SHMEM*
buffer type will be covered.
* 'mutex=os_sem' - default mode for providing semaphore locking of the buffer memory.
* 'mutex=none' - Not used
* 'mutex=no_interrupts' - not applicable on a Linux system
* 'mutex=no_switching' - not applicable on a Linux system
* 'mutex=mao split' - Splits the buffer in to half (or more) and allows
one process to access part of the buffer whilst a second process is
writing to another part.
* 'TCP=(port number)' - Specifies which network port to use.
* 'UDP=(port number)' - ditto
* 'STCP=(port number)' - ditto
* 'serialPortDevName=(serial port)' - Undocumented.
* 'passwd=file_name.pwd' - Adds a layer of security to the buffer by
requiring each process to provide a password.
* 'bsem' - NIST documentation implies a key for a blocking semaphore,
and if bsem=-1, blocking reads are prevented.
* 'queue' - Enables queued message passing.
* 'ascii' - Encode messages in a plain text format
* 'disp' - Encode messages in a format suitable for display (???)
* 'xdr' - Encode messages in External Data Representation. (see rpc/xdr.h for details).
* 'diag' - Enables diagnostics stored in the buffer (timings and byte counts ?)
=== Process line
The original NIST format of the process line is:
*P name buffer type host ops server timeout master c_num [type specific configs]*
* 'P' - identifies this line as a Process configuration.
* 'name' - is the identifier of the process.
* 'buffer' - is one of the buffers defined elsewhere in the config file.
* 'type' - defines whether this process is local or remote relative to the buffer.
* 'host' - specifies where on the network this process is running.
* 'ops' - gives the process read only, write only, or read/write access to the buffer.
* 'server' - specifies if this process will running a server for this buffer.
* 'timeout' - sets the timeout characteristics for accesses to the buffer.
* 'master' - indicates if this process is responsible for creating and destroying the buffer.
* 'c_num' - an integer between zero and (max_procs -1)
=== Configuration Comments
Some of the configuration combinations are invalid, whilst others
imply certain constraints. On a Linux system, GLOBMEM is obsolete,
whilst PHANTOM is only really useful in the testing stage of an
application, likewise for FILEMEM. LOCMEM is of little use for a
multi-process application, and only offers limited performance
advantages over SHMEM. This leaves SHMEM as the only buffer type to use
with LinuxCNC.
The neut option is only of use in a multi-processor system where
different (and incompatible) architectures are sharing a block of
memory. The likelihood of seeing a system of this type outside of a
museum or research establishment is remote and is only relevant to
GLOBMEM buffers.
The RPC number is documented as being obsolete and is retained only
for compatibility reasons.
With a unique buffer name, having a numerical identity seems to be
pointless. Need to review the code to identify the logic. Likewise, the
key field at first appears to be redundant, and it could be derived
from the buffer name.
The purpose of limiting the number of processes allowed to connect to
any one buffer is unclear from existing documentation and from the
original source code. Allowing unspecified multiple processes to
connect to a buffer is no more difficult to implement.
The mutex types boil down to one of two, the default "os_sem" or "mao
split". Most of the NML messages are relatively short and can be copied
to or from the buffer with minimal delays, so split reads are not
essential.
Data encoding is only relevant when transmitted to a remote process -
Using TCP or UDP implies XDR encoding. Whilst ASCII encoding may have
some use in diagnostics or for passing data to an embedded system that
does not implement NML.
UDP protocols have fewer checks on data and allows a percentage of
packets to be dropped. TCP is more reliable, but is marginally slower.
If LinuxCNC is to be connected to a network, one would hope that it is
local and behind a firewall. About the only reason to allow access to
LinuxCNC via the Internet would be for remote diagnostics - This can be
achieved far more securely using other means, perhaps by a web
interface.
The exact behavior when timeout is set to zero or a negative value is
unclear from the NIST documents. Only INF and positive values are
mentioned. However, buried in the source code of rcslib, it is apparent