-
Notifications
You must be signed in to change notification settings - Fork 1.1k
/
control.c
2389 lines (2192 loc) · 90.5 KB
/
control.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
/********************************************************************
* Description: control.c
* emcmotController() is the main loop running at the servo cycle
* rate. All state logic and trajectory calcs are called from here.
*
* Derived from a work by Fred Proctor & Will Shackleford
*
* Author:
* License: GPL Version 2
* Created on:
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
********************************************************************/
#include "posemath.h"
#include "rtapi.h"
#include "hal.h"
#include "motion.h"
#include "mot_priv.h"
#include "rtapi_math.h"
#include "tp.h"
#include "tc.h"
#include "simple_tp.h"
#include "motion_debug.h"
#include "config.h"
#include "motion_types.h"
#include "homing.h"
// Mark strings for translation, but defer translation to userspace
#define _(s) (s)
static int ext_offset_teleop_limit = 0;
static int ext_offset_coord_limit = 0;
static double ext_offset_epsilon;
/* kinematics flags */
KINEMATICS_FORWARD_FLAGS fflags = 0;
KINEMATICS_INVERSE_FLAGS iflags = 0;
/* 1/servo cycle time */
double servo_freq;
/*! \todo FIXME - debugging - uncomment the following line to log changes in
JOINT_FLAG and MOTION_FLAG */
// #define WATCH_FLAGS 1
/***********************************************************************
* LOCAL VARIABLE DECLARATIONS *
************************************************************************/
/* the (nominal) period the last time the motion handler was invoked */
static unsigned long last_period = 0;
/* servo cycle time */
static double servo_period;
extern struct emcmot_status_t *emcmotStatus;
// *pcmd_p[0] is shorthand for emcmotStatus->carte_pos_cmd.tran.x
// *pcmd_p[1] is shorthand for emcmotStatus->carte_pos_cmd.tran.y
// etc.
static double *pcmd_p[EMCMOT_MAX_AXIS];
#define EDEBUG
#undef EDEBUG
#ifdef EDEBUG
#define dprint(format, ...) rtapi_print_msg(RTAPI_MSG_INFO,format, ##__VA_ARGS__)
static int dbg_ct;
static int dbg_enable_ct;
static int dbg_disable_ct;
static void dbg_show(char*txt) {
int ano;
emcmot_axis_t *a;
char afmt[]= "%6d %4s A%d T%d C%d I%d E(cmd=%7.4f curr=%7.4f) T(cmd=%7.4f curr=%7.4f) V:%7.4f\n";
rtapi_set_msg_level(RTAPI_MSG_INFO);
dprint("\n");
for (ano=2; ano<3; ano++) {
double v;
if (ano == 0) { v=emcmotStatus->carte_pos_cmd.tran.x;
} else if (ano == 1) { v=emcmotStatus->carte_pos_cmd.tran.y;
} else if (ano == 2) { v=emcmotStatus->carte_pos_cmd.tran.z;
} else { v=999; }
a = &axes[ano];
dprint(afmt,dbg_ct,txt,ano
,GET_MOTION_TELEOP_FLAG(),GET_MOTION_COORD_FLAG(),GET_MOTION_INPOS_FLAG()
,a->ext_offset_tp.pos_cmd, a->ext_offset_tp.curr_pos
,v
);
}
}
#endif
/***********************************************************************
* LOCAL FUNCTION PROTOTYPES *
************************************************************************/
/* the following functions are called (in this order) by the main
controller function. They are an attempt to break the huge
function (originally 1600 lines) into something a little easier
to understand.
*/
/* 'process_inputs()' is responsible for reading hardware input
signals (from the HAL) and doing basic processing on them. In
the case of position feedback, that means removing backlash or
screw error comp and calculating the following error. For
switches, it means debouncing them and setting flags in the
emcmotStatus structure.
*/
static void process_inputs(void);
/* 'do forward kins()' takes the position feedback in joint coords
and applies the forward kinematics to it to generate feedback
in Cartesean coordinates. It has code to handle machines that
don't have forward kins, and other special cases, such as when
the joints have not been homed.
*/
static void do_forward_kins(void);
/* probe inputs need to be handled after forward kins are run, since
cartesian feedback position is latched when the probe fires, and it
should be based on the feedback read in on this servo cycle.
*/
static void process_probe_inputs(void);
/* 'check_for_faults()' is responsible for detecting fault conditions
such as limit switches, amp faults, following error, etc. It only
checks active axes. It is also responsible for generating an error
message. (Later, once I understand the cmd/status/error interface
better, it will probably generate error codes that can be passed
up the architecture toward the GUI - printing error messages
directly seems a little messy)
*/
static void check_for_faults(void);
/* 'set_operating_mode()' handles transitions between the operating
modes, which are free, coordinated, and teleop. This stuff needs
to be better documented. It is basically a state machine, with
a current state, a desired state, and rules determining when the
state can change. It should be rewritten as such, but for now
it consists of code copied exactly from emc1.
*/
static void set_operating_mode(void);
/* 'handle_jjogwheels()' reads jogwheels, decides if they should be
enabled, and if so, changes the free mode planner's target position
when the jogwheel(s) turn.
*/
static void handle_jjogwheels(void);
static void handle_ajogwheels(void);
/* 'do_homing_sequence()' decides what, if anything, needs to be done
related to multi-joint homing.
no prototype here, implemented in homing.c, proto in mot_priv.h
*/
/* 'do_homing()' looks at the home_state field of each joint struct
to decide what, if anything, needs to be done related to homing
the joint. Homing is implemented as a state machine, the exact
sequence of states depends on the machine configuration. It
can be as simple as immediately setting the current position to
zero, or a it can be a multi-step process (find switch, set
approximate zero, back off switch, find index, set final zero,
rapid to home position), or anywhere in between.
no prototype here, implemented in homing.c, proto in mot_priv.h
*/
/* 'get_pos_cmds()' generates the position setpoints. This includes
calling the trajectory planner and interpolating its outputs.
*/
static void get_pos_cmds(long period);
/* 'compute_screw_comp()' is responsible for calculating backlash and
lead screw error compensation. (Leadscrew error compensation is
a more sophisticated version that includes backlash comp.) It uses
the velocity in emcmotStatus->joint_vel_cmd to determine which way
each joint is moving, and the position in emcmotStatus->joint_pos_cmd
to determine where the joint is at. That information is used to
create the compensation value that is added to the joint_pos_cmd
to create motor_pos_cmd, and is subtracted from motor_pos_fb to
get joint_pos_fb. (This function does not add or subtract the
compensation value, it only computes it.) The basic compensation
value is in backlash_corr, however has makes step changes when
the direction reverses. backlash_filt is a ramped version, and
that is the one that is later added/subtracted from the position.
*/
static void compute_screw_comp(void);
/* 'output_to_hal()' writes the handles the final stages of the
control function. It applies screw comp and writes the
final motor position to the HAL (which routes it to the PID
loop). It also drives other HAL outputs, and it writes a
number of internal variables to HAL parameters so they can
be observed with halscope and halmeter.
*/
static void output_to_hal(void);
/* 'update_status()' copies assorted status information to shared
memory (the emcmotStatus structure) so that it is available to
higher level code.
*/
static void update_status(void);
static void initialize_external_offsets(void);
static void plan_external_offsets(void);
static void sync_teleop_tp_to_carte_pos(int);
static void sync_carte_pos_to_teleop_tp(int);
static void apply_ext_offsets_to_carte_pos(int);
static int update_coord_with_bound(void);
static int update_teleop_with_check(int,simple_tp_t*);
/***********************************************************************
* PUBLIC FUNCTION CODE *
************************************************************************/
/*
emcmotController() runs the trajectory and interpolation calculations
each control cycle
This function gets called at regular intervals - therefore it does NOT
have a loop within it!
Inactive axes are still calculated, but the PIDs are inhibited and
the amp enable/disable are inhibited
*/
void emcmotController(void *arg, long period)
{
static int do_once = 1;
if (do_once) {
pcmd_p[0] = &(emcmotStatus->carte_pos_cmd.tran.x);
pcmd_p[1] = &(emcmotStatus->carte_pos_cmd.tran.y);
pcmd_p[2] = &(emcmotStatus->carte_pos_cmd.tran.z);
pcmd_p[3] = &(emcmotStatus->carte_pos_cmd.a);
pcmd_p[4] = &(emcmotStatus->carte_pos_cmd.b);
pcmd_p[5] = &(emcmotStatus->carte_pos_cmd.c);
pcmd_p[6] = &(emcmotStatus->carte_pos_cmd.u);
pcmd_p[7] = &(emcmotStatus->carte_pos_cmd.v);
pcmd_p[8] = &(emcmotStatus->carte_pos_cmd.w);
do_once = 0;
}
static long long int last = 0;
long long int now = rtapi_get_clocks();
long int this_run = (long int)(now - last);
*(emcmot_hal_data->last_period) = this_run;
#ifdef HAVE_CPU_KHZ
*(emcmot_hal_data->last_period_ns) = this_run * 1e6 / cpu_khz;
#endif
// we need this for next time
last = now;
/* calculate servo period as a double - period is in integer nsec */
servo_period = period * 0.000000001;
if(period != last_period) {
emcmotSetCycleTime(period);
last_period = period;
}
/* calculate servo frequency for calcs like vel = Dpos / period */
/* it's faster to do vel = Dpos * freq */
servo_freq = 1.0 / servo_period;
/* increment head count to indicate work in progress */
emcmotStatus->head++;
/* here begins the core of the controller */
#ifdef EDEBUG
dbg_ct++;
#endif
read_homing_in_pins(emcmotConfig->numJoints);
process_inputs();
do_forward_kins();
process_probe_inputs();
check_for_faults();
set_operating_mode();
handle_jjogwheels();
handle_ajogwheels();
do_homing_sequence();
do_homing();
get_pos_cmds(period);
compute_screw_comp();
plan_external_offsets();
output_to_hal();
write_homing_out_pins(emcmotConfig->numJoints);
update_status();
/* here ends the core of the controller */
emcmotStatus->heartbeat++;
/* set tail to head, to indicate work complete */
emcmotStatus->tail = emcmotStatus->head;
/* end of controller function */
}
/***********************************************************************
* LOCAL FUNCTION CODE *
************************************************************************/
/* The protoypes and documentation for these functions are located
at the top of the file in the section called "local function
prototypes"
*/
static void process_inputs(void)
{
int joint_num, spindle_num;
double abs_ferror, scale;
joint_hal_t *joint_data;
emcmot_joint_t *joint;
unsigned char enables;
/* read spindle angle (for threading, etc) */
for (spindle_num = 0; spindle_num < emcmotConfig->numSpindles; spindle_num++){
emcmotStatus->spindle_status[spindle_num].spindleRevs =
*emcmot_hal_data->spindle[spindle_num].spindle_revs;
emcmotStatus->spindle_status[spindle_num].spindleSpeedIn =
*emcmot_hal_data->spindle[spindle_num].spindle_speed_in;
emcmotStatus->spindle_status[spindle_num].at_speed =
*emcmot_hal_data->spindle[spindle_num].spindle_is_atspeed;
}
/* compute net feed and spindle scale factors */
if ( emcmotStatus->motion_state == EMCMOT_MOTION_COORD ) {
/* use the enables that were queued with the current move */
enables = emcmotStatus->enables_queued;
} else {
/* use the enables that are in effect right now */
enables = emcmotStatus->enables_new;
}
/* feed scaling first: feed_scale, adaptive_feed, and feed_hold */
scale = 1.0;
if ( (emcmotStatus->motion_state != EMCMOT_MOTION_FREE)
&& (enables & FS_ENABLED) ) {
if (emcmotStatus->motionType == EMC_MOTION_TYPE_TRAVERSE) {
scale *= emcmotStatus->rapid_scale;
} else {
scale *= emcmotStatus->feed_scale;
}
}
if ( enables & AF_ENABLED ) {
/* read and clamp adaptive feed HAL pin */
double adaptive_feed_in = *emcmot_hal_data->adaptive_feed;
// Clip range to +/- 1.0
if ( adaptive_feed_in > 1.0 ) {
adaptive_feed_in = 1.0;
} else if (adaptive_feed_in < -1.0) {
adaptive_feed_in = -1.0;
}
// Handle case of negative adaptive feed
// Actual scale factor is always positive by default
double adaptive_feed_out = fabs(adaptive_feed_in);
// Case 1: positive to negative direction change
if ( adaptive_feed_in < 0.0 && emcmotDebug->coord_tp.reverse_run == TC_DIR_FORWARD) {
// User commands feed in reverse direction, but we're not running in reverse yet
if (tpSetRunDir(&emcmotDebug->coord_tp, TC_DIR_REVERSE) != TP_ERR_OK) {
// Need to decelerate to a stop first
adaptive_feed_out = 0.0;
}
} else if (adaptive_feed_in > 0.0 && emcmotDebug->coord_tp.reverse_run == TC_DIR_REVERSE ) {
// User commands feed in forward direction, but we're running in reverse
if (tpSetRunDir(&emcmotDebug->coord_tp, TC_DIR_FORWARD) != TP_ERR_OK) {
// Need to decelerate to a stop first
adaptive_feed_out = 0.0;
}
}
//Otherwise, if direction and sign match, we're ok
scale *= adaptive_feed_out;
}
if ( enables & FH_ENABLED ) {
/* read feed hold HAL pin */
if ( *emcmot_hal_data->feed_hold ) {
scale = 0;
}
}
/*non maskable (except during spinndle synch move) feed hold inhibit pin */
if ( enables & *emcmot_hal_data->feed_inhibit ) {
scale = 0;
}
/* save the resulting combined scale factor */
emcmotStatus->net_feed_scale = scale;
/* now do spindle scaling */
for (spindle_num=0; spindle_num < emcmotConfig->numSpindles; spindle_num++){
scale = 1.0;
if ( enables & SS_ENABLED ) {
scale *= emcmotStatus->spindle_status[spindle_num].scale;
}
/*non maskable (except during spindle synch move) spindle inhibit pin */
if ( enables & *emcmot_hal_data->spindle[spindle_num].spindle_inhibit ) {
scale = 0;
}
/* save the resulting combined scale factor */
emcmotStatus->spindle_status[spindle_num].net_scale = scale;
}
/* read and process per-joint inputs */
for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) {
/* point to joint HAL data */
joint_data = &(emcmot_hal_data->joint[joint_num]);
/* point to joint data */
joint = &joints[joint_num];
if (!GET_JOINT_ACTIVE_FLAG(joint)) {
/* if joint is not active, skip it */
continue;
}
/* copy data from HAL to joint structure */
joint->motor_pos_fb = *(joint_data->motor_pos_fb);
/* calculate pos_fb */
if (( get_homing_at_index_search_wait(joint_num) ) &&
( get_index_enable(joint_num) == 0 )) {
/* special case - we're homing the joint, and it just
hit the index. The encoder count might have made a
step change. The homing code will correct for it
later, so we ignore motor_pos_fb and set pos_fb
to match the commanded value instead. */
joint->pos_fb = joint->pos_cmd;
} else {
/* normal case: subtract backlash comp and motor offset */
joint->pos_fb = joint->motor_pos_fb -
(joint->backlash_filt + joint->motor_offset);
}
/* calculate following error */
joint->ferror = joint->pos_cmd - joint->pos_fb;
abs_ferror = fabs(joint->ferror);
/* update maximum ferror if needed */
if (abs_ferror > joint->ferror_high_mark) {
joint->ferror_high_mark = abs_ferror;
}
/* calculate following error limit */
if (joint->vel_limit > 0.0) {
joint->ferror_limit =
joint->max_ferror * fabs(joint->vel_cmd) / joint->vel_limit;
} else {
joint->ferror_limit = 0;
}
if (joint->ferror_limit < joint->min_ferror) {
joint->ferror_limit = joint->min_ferror;
}
/* update following error flag */
if (abs_ferror > joint->ferror_limit) {
SET_JOINT_FERROR_FLAG(joint, 1);
} else {
SET_JOINT_FERROR_FLAG(joint, 0);
}
/* read limit switches */
if (*(joint_data->pos_lim_sw)) {
SET_JOINT_PHL_FLAG(joint, 1);
} else {
SET_JOINT_PHL_FLAG(joint, 0);
}
if (*(joint_data->neg_lim_sw)) {
SET_JOINT_NHL_FLAG(joint, 1);
} else {
SET_JOINT_NHL_FLAG(joint, 0);
}
joint->on_pos_limit = GET_JOINT_PHL_FLAG(joint);
joint->on_neg_limit = GET_JOINT_NHL_FLAG(joint);
/* read amp fault input */
if (*(joint_data->amp_fault)) {
SET_JOINT_FAULT_FLAG(joint, 1);
} else {
SET_JOINT_FAULT_FLAG(joint, 0);
}
}
// a fault was signalled during a spindle-orient in progress
// signal error, and cancel the orient
for (spindle_num = 0; spindle_num < emcmotConfig->numSpindles; spindle_num++){
if(*(emcmot_hal_data->spindle[spindle_num].spindle_amp_fault)){
emcmotStatus->spindle_status[spindle_num].fault = 1;
}else{
emcmotStatus->spindle_status[spindle_num].fault = 0;
}
if (*(emcmot_hal_data->spindle[spindle_num].spindle_orient)) {
if (*(emcmot_hal_data->spindle[spindle_num].spindle_orient_fault)) {
emcmotStatus->spindle_status[spindle_num].orient_state = EMCMOT_ORIENT_FAULTED;
*(emcmot_hal_data->spindle[spindle_num].spindle_orient) = 0;
emcmotStatus->spindle_status[spindle_num].orient_fault =
*(emcmot_hal_data->spindle[spindle_num].spindle_orient_fault);
reportError(_("fault %d during orient in progress"),
emcmotStatus->spindle_status[spindle_num].orient_fault);
emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND;
tpAbort(&emcmotDebug->coord_tp);
SET_MOTION_ERROR_FLAG(1);
} else if (*(emcmot_hal_data->spindle[spindle_num].spindle_is_oriented)) {
*(emcmot_hal_data->spindle[spindle_num].spindle_orient) = 0;
*(emcmot_hal_data->spindle[spindle_num].spindle_locked) = 1;
emcmotStatus->spindle_status[spindle_num].locked = 1;
emcmotStatus->spindle_status[spindle_num].brake = 1;
emcmotStatus->spindle_status[spindle_num].orient_state = EMCMOT_ORIENT_COMPLETE;
rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_ORIENT complete, spindle locked");
}
}
}
}
static void do_forward_kins(void)
{
/* there are four possibilities for kinType:
IDENTITY: Both forward and inverse kins are available, and they
can used without an initial guess, even if one or more joints
are not homed. In this case, we apply the forward kins to the
joint->pos_fb to produce carte_pos_fb, and if all axes are homed
we set carte_pos_fb_ok to 1 to indicate that the feedback data
is good.
BOTH: Both forward and inverse kins are available, but the forward
kins need an initial guess, and/or the kins require all joints to
be homed before they work properly. Here we must tread carefully.
IF all the joints have been homed, we apply the forward kins to
the joint->pos_fb to produce carte_pos_fb, and set carte_pos_fb_ok
to indicate that the feedback is good. We use the previous value
of carte_pos_fb as the initial guess. If all joints have not been
homed, we don't call the kinematics, instead we set carte_pos_fb to
the cartesean coordinates of home, as stored in the global worldHome,
and we set carte_fb_ok to 0 to indicate that the feedback is invalid.
\todo FIXME - maybe setting to home isn't the right thing to do. We need
it to be set to home eventually, (right before the first attemt to
run the kins), but that doesn't mean we should say we're at home
when we're not.
INVERSE_ONLY: Only inverse kinematics are available, forward
kinematics cannot be used. So we have to fake it, the question is
simply "what way of faking it is best". In free mode, or if all
axes have not been homed, the feedback position is unknown. If
we are in teleop or coord mode, or if we are in free mode and all
axes are homed, and haven't been moved since they were homed, then
we set carte_pos_fb to carte_pos_cmd, and set carte_pos_fb_ok to 1.
If we are in free mode, and any joint is not homed, or any joint has
moved since it was homed, we leave cart_pos_fb alone, and set
carte_pos_fb_ok to 0.
FORWARD_ONLY: Only forward kinematics are available, inverse kins
cannot be used. This exists for completeness only, since EMC won't
work without inverse kinematics.
*/
/*! \todo FIXME FIXME FIXME - need to put a rate divider in here, run it
at the traj rate */
double joint_pos[EMCMOT_MAX_JOINTS] = {0,};
int joint_num, result;
emcmot_joint_t *joint;
/* copy joint position feedback to local array */
for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) {
/* point to joint struct */
joint = &joints[joint_num];
/* copy feedback */
joint_pos[joint_num] = joint->pos_fb;
}
switch (emcmotConfig->kinType) {
case KINEMATICS_IDENTITY:
kinematicsForward(joint_pos, &emcmotStatus->carte_pos_fb, &fflags,
&iflags);
if (checkAllHomed()) {
emcmotStatus->carte_pos_fb_ok = 1;
} else {
emcmotStatus->carte_pos_fb_ok = 0;
}
break;
case KINEMATICS_BOTH:
if (checkAllHomed()) {
/* is previous value suitable for use as initial guess? */
if (!emcmotStatus->carte_pos_fb_ok) {
/* no, use home position as initial guess */
emcmotStatus->carte_pos_fb = emcmotStatus->world_home;
}
/* calculate Cartesean position feedback from joint pos fb */
result =
kinematicsForward(joint_pos, &emcmotStatus->carte_pos_fb,
&fflags, &iflags);
/* check to make sure kinematics converged */
if (result < 0) {
/* error during kinematics calculations */
emcmotStatus->carte_pos_fb_ok = 0;
} else {
/* it worked! */
emcmotStatus->carte_pos_fb_ok = 1;
}
} else {
emcmotStatus->carte_pos_fb_ok = 0;
}
break;
case KINEMATICS_INVERSE_ONLY:
if ((GET_MOTION_COORD_FLAG()) || (GET_MOTION_TELEOP_FLAG())) {
/* use Cartesean position command as feedback value */
emcmotStatus->carte_pos_fb = emcmotStatus->carte_pos_cmd;
emcmotStatus->carte_pos_fb_ok = 1;
} else {
emcmotStatus->carte_pos_fb_ok = 0;
}
break;
default:
emcmotStatus->carte_pos_fb_ok = 0;
break;
}
}
static void process_probe_inputs(void)
{
static int old_probeVal = 0;
unsigned char probe_type = emcmotStatus->probe_type;
// don't error
char probe_suppress = probe_type & 1;
int axis_num;
// trigger when the probe clears, instead of the usual case of triggering when it trips
char probe_whenclears = !!(probe_type & 2);
/* read probe input */
emcmotStatus->probeVal = !!*(emcmot_hal_data->probe_input);
if (emcmotStatus->probing) {
/* check if the probe has been tripped */
if (emcmotStatus->probeVal ^ probe_whenclears) {
/* remember the current position */
emcmotStatus->probedPos = emcmotStatus->carte_pos_fb;
/* stop! */
emcmotStatus->probing = 0;
emcmotStatus->probeTripped = 1;
tpAbort(&emcmotDebug->coord_tp);
/* check if the probe hasn't tripped, but the move finished */
} else if (GET_MOTION_INPOS_FLAG() && tpQueueDepth(&emcmotDebug->coord_tp) == 0) {
/* we are already stopped, but we need to remember the current
position here, because it will still be queried */
emcmotStatus->probedPos = emcmotStatus->carte_pos_fb;
emcmotStatus->probing = 0;
if (probe_suppress) {
emcmotStatus->probeTripped = 0;
} else if(probe_whenclears) {
reportError(_("G38.4 move finished without breaking contact."));
SET_MOTION_ERROR_FLAG(1);
} else {
reportError(_("G38.2 move finished without making contact."));
SET_MOTION_ERROR_FLAG(1);
}
}
} else if (!old_probeVal && emcmotStatus->probeVal) {
// not probing, but we have a rising edge on the probe.
// this could be expensive if we don't stop.
int i;
int aborted = 0;
if(!GET_MOTION_INPOS_FLAG() && tpQueueDepth(&emcmotDebug->coord_tp) &&
tpGetExecId(&emcmotDebug->coord_tp) <= 0) {
// running an MDI command
tpAbort(&emcmotDebug->coord_tp);
reportError(_("Probe tripped during non-probe MDI command."));
SET_MOTION_ERROR_FLAG(1);
}
for(i=0; i<emcmotConfig->numJoints; i++) {
emcmot_joint_t *joint = &joints[i];
if (!GET_JOINT_ACTIVE_FLAG(joint)) {
/* if joint is not active, skip it */
continue;
}
// abort any homing
if(get_homing(i)) {
set_home_abort(i);
aborted=1;
}
// abort any joint jogs
if(joint->free_tp.enable == 1) {
joint->free_tp.enable = 0;
// since homing uses free_tp, this protection of aborted
// is needed so the user gets the correct error.
if(!aborted) aborted=2;
}
}
for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) {
emcmot_axis_t *axis;
axis = &axes[axis_num];
// abort any coordinate jogs
if (axis->teleop_tp.enable) {
axis->teleop_tp.enable = 0;
axis->teleop_tp.curr_vel = 0.0;
aborted = 3;
}
}
if(aborted == 1) {
reportError(_("Probe tripped during homing motion."));
}
if(aborted == 2) {
reportError(_("Probe tripped during a joint jog."));
}
if(aborted == 3) {
reportError(_("Probe tripped during a coordinate jog."));
}
}
old_probeVal = emcmotStatus->probeVal;
}
static void check_for_faults(void)
{
int joint_num, spindle_num;
emcmot_joint_t *joint;
int neg_limit_override, pos_limit_override;
/* check for various global fault conditions */
/* only check enable input if running */
if ( GET_MOTION_ENABLE_FLAG() != 0 ) {
if ( *(emcmot_hal_data->enable) == 0 ) {
reportError(_("motion stopped by enable input"));
emcmotDebug->enabling = 0;
}
}
/* check for spindle ampfifier errors */
for (spindle_num = 0; spindle_num < emcmotConfig->numSpindles; spindle_num++){
if(emcmotStatus->spindle_status[spindle_num].fault && GET_MOTION_ENABLE_FLAG()){
reportError(_("spindle %d amplifier fault"), spindle_num);
emcmotDebug->enabling = 0;
}
}
/* check for various joint fault conditions */
for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) {
/* point to joint data */
joint = &joints[joint_num];
/* only check active, enabled axes */
if ( GET_JOINT_ACTIVE_FLAG(joint) && GET_JOINT_ENABLE_FLAG(joint) ) {
/* are any limits for this joint overridden? */
neg_limit_override = emcmotStatus->overrideLimitMask & ( 1 << (joint_num*2));
pos_limit_override = emcmotStatus->overrideLimitMask & ( 2 << (joint_num*2));
/* check for hard limits */
if ((GET_JOINT_PHL_FLAG(joint) && ! pos_limit_override ) ||
(GET_JOINT_NHL_FLAG(joint) && ! neg_limit_override )) {
/* joint is on limit switch, should we trip? */
if (get_homing(joint_num)) {
/* no, ignore limits */
} else {
/* trip on limits */
if (!GET_JOINT_ERROR_FLAG(joint)) {
/* report the error just this once */
reportError(_("joint %d on limit switch error"),
joint_num);
}
SET_JOINT_ERROR_FLAG(joint, 1);
emcmotDebug->enabling = 0;
}
}
/* check for amp fault */
if (GET_JOINT_FAULT_FLAG(joint)) {
/* joint is faulted, trip */
if (!GET_JOINT_ERROR_FLAG(joint)) {
/* report the error just this once */
reportError(_("joint %d amplifier fault"), joint_num);
}
SET_JOINT_ERROR_FLAG(joint, 1);
emcmotDebug->enabling = 0;
}
/* check for excessive following error */
if (GET_JOINT_FERROR_FLAG(joint)) {
if (!GET_JOINT_ERROR_FLAG(joint)) {
/* report the error just this once */
reportError(_("joint %d following error"), joint_num);
}
SET_JOINT_ERROR_FLAG(joint, 1);
emcmotDebug->enabling = 0;
}
/* end of if JOINT_ACTIVE_FLAG(joint) */
}
/* end of check for joint faults loop */
}
}
static void set_operating_mode(void)
{
int joint_num, axis_num;
emcmot_joint_t *joint;
emcmot_axis_t *axis;
double positions[EMCMOT_MAX_JOINTS];
/* check for disabling */
if (!emcmotDebug->enabling && GET_MOTION_ENABLE_FLAG()) {
#ifdef EDEBUG
dbg_show("dsbl");dbg_disable_ct=dbg_ct;
#endif
/* clear out the motion emcmotDebug->coord_tp and interpolators */
tpClear(&emcmotDebug->coord_tp);
for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) {
/* point to joint data */
joint = &joints[joint_num];
/* disable free mode planner */
joint->free_tp.enable = 0;
joint->free_tp.curr_vel = 0.0;
/* drain coord mode interpolators */
cubicDrain(&(joint->cubic));
if (GET_JOINT_ACTIVE_FLAG(joint)) {
SET_JOINT_INPOS_FLAG(joint, 1);
SET_JOINT_ENABLE_FLAG(joint, 0);
set_joint_homing(joint_num,0);
set_home_idle(joint_num);
}
/* don't clear the joint error flag, since that may signify why
we just went into disabled state */
}
for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) {
/* point to axis data */
axis = &axes[axis_num];
/* disable teleop mode planner */
axis->teleop_tp.enable = 0;
axis->teleop_tp.curr_vel = 0.0;
}
SET_MOTION_ENABLE_FLAG(0);
/* don't clear the motion error flag, since that may signify why we
just went into disabled state */
}
/* check for emcmotDebug->enabling */
if (emcmotDebug->enabling && !GET_MOTION_ENABLE_FLAG()) {
#ifdef EDEBUG
dbg_show("enbl");dbg_enable_ct=dbg_ct;
#endif
if (*(emcmot_hal_data->eoffset_limited)) {
reportError("Starting beyond Soft Limits");
*(emcmot_hal_data->eoffset_limited) = 0;
}
initialize_external_offsets();
tpSetPos(&emcmotDebug->coord_tp, &emcmotStatus->carte_pos_cmd);
for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) {
/* point to joint data */
joint = &joints[joint_num];
joint->free_tp.curr_pos = joint->pos_cmd;
if (GET_JOINT_ACTIVE_FLAG(joint)) {
SET_JOINT_ENABLE_FLAG(joint, 1);
set_joint_homing(joint_num,0);
set_home_idle(joint_num);
}
/* clear any outstanding joint errors when going into enabled
state */
SET_JOINT_ERROR_FLAG(joint, 0);
}
if ( !GET_MOTION_ENABLE_FLAG() ) {
if (GET_MOTION_TELEOP_FLAG()) {
sync_teleop_tp_to_carte_pos(0);
}
}
SET_MOTION_ENABLE_FLAG(1);
/* clear any outstanding motion errors when going into enabled state */
SET_MOTION_ERROR_FLAG(0);
}
/* check for entering teleop mode */
if (emcmotDebug->teleoperating && !GET_MOTION_TELEOP_FLAG()) {
if (GET_MOTION_INPOS_FLAG()) {
/* update coordinated emcmotDebug->coord_tp position */
tpSetPos(&emcmotDebug->coord_tp, &emcmotStatus->carte_pos_cmd);
/* drain the cubics so they'll synch up */
for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) {
/* point to joint data */
joint = &joints[joint_num];
cubicDrain(&(joint->cubic));
positions[joint_num] = joint->coarse_pos;
}
/* Initialize things to do when starting teleop mode. */
SET_MOTION_TELEOP_FLAG(1);
SET_MOTION_ERROR_FLAG(0);
kinematicsForward(positions, &emcmotStatus->carte_pos_cmd, &fflags, &iflags);
// entering teleop (INPOS), remove ext offsets
sync_teleop_tp_to_carte_pos(-1);
} else {
/* not in position-- don't honor mode change */
emcmotDebug->teleoperating = 0;
}
} else {
if (GET_MOTION_INPOS_FLAG()) {
if (!emcmotDebug->teleoperating && GET_MOTION_TELEOP_FLAG()) {
SET_MOTION_TELEOP_FLAG(0);
if (!emcmotDebug->coordinating) {
for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) {
/* point to joint data */
joint = &joints[joint_num];
/* update free planner positions */
joint->free_tp.curr_pos = joint->pos_cmd;
}
}
}
}
/* check for entering coordinated mode */
if (emcmotDebug->coordinating && !GET_MOTION_COORD_FLAG()) {
if (GET_MOTION_INPOS_FLAG()) {
/* preset traj planner to current position */
apply_ext_offsets_to_carte_pos(-1); // subtract at coord mode start
tpSetPos(&emcmotDebug->coord_tp, &emcmotStatus->carte_pos_cmd);
/* drain the cubics so they'll synch up */
for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) {
/* point to joint data */
joint = &joints[joint_num];
cubicDrain(&(joint->cubic));
}
/* clear the override limits flags */
emcmotDebug->overriding = 0;
emcmotStatus->overrideLimitMask = 0;
SET_MOTION_COORD_FLAG(1);
SET_MOTION_TELEOP_FLAG(0);
SET_MOTION_ERROR_FLAG(0);
} else {
/* not in position-- don't honor mode change */
emcmotDebug->coordinating = 0;
}
}
/* check entering free space mode */
if (!emcmotDebug->coordinating && GET_MOTION_COORD_FLAG()) {
if (GET_MOTION_INPOS_FLAG()) {
for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) {
/* point to joint data */
joint = &joints[joint_num];
/* set joint planner curr_pos to current location */
joint->free_tp.curr_pos = joint->pos_cmd;
/* but it can stay disabled until a move is required */
joint->free_tp.enable = 0;
}
SET_MOTION_COORD_FLAG(0);
SET_MOTION_TELEOP_FLAG(0);
SET_MOTION_ERROR_FLAG(0);
} else {
/* not in position-- don't honor mode change */
emcmotDebug->coordinating = 1;
}
}
}
/*! \todo FIXME - this code is temporary - eventually this function will be
cleaned up and simplified, and 'motion_state' will become the master
for this info, instead of having to gather it from several flags */
if (!GET_MOTION_ENABLE_FLAG()) {
emcmotStatus->motion_state = EMCMOT_MOTION_DISABLED;
} else if (GET_MOTION_TELEOP_FLAG()) {
emcmotStatus->motion_state = EMCMOT_MOTION_TELEOP;
} else if (GET_MOTION_COORD_FLAG()) {
emcmotStatus->motion_state = EMCMOT_MOTION_COORD;
} else {
emcmotStatus->motion_state = EMCMOT_MOTION_FREE;
}
} //set_operating_mode
static void handle_jjogwheels(void)
{
int joint_num;
emcmot_joint_t *joint;
joint_hal_t *joint_data;
int new_jjog_counts, delta;
double distance, pos, stop_dist;
static int first_pass = 1; /* used to set initial conditions */
for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) {
double jaccel_limit;
/* point to joint data */
joint_data = &(emcmot_hal_data->joint[joint_num]);
joint = &joints[joint_num];
if (!GET_JOINT_ACTIVE_FLAG(joint)) {
/* if joint is not active, skip it */
continue;
}
// disallow accel bogus fractions
if ( (*(joint_data->jjog_accel_fraction) > 1)
|| (*(joint_data->jjog_accel_fraction) < 0) ) {
jaccel_limit = joint->acc_limit;
} else {
jaccel_limit = (*(joint_data->jjog_accel_fraction)) * joint->acc_limit;
}
/* get counts from jogwheel */
new_jjog_counts = *(joint_data->jjog_counts);
delta = new_jjog_counts - joint->old_jjog_counts;
/* save value for next time */
joint->old_jjog_counts = new_jjog_counts;
/* initialization complete */
if ( first_pass ) {
continue;
}
/* did the wheel move? */
if ( delta == 0 ) {
/* no, nothing to do */
continue;
}
if (GET_MOTION_TELEOP_FLAG()) {