00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293
00294
00295
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324 #if defined(rtlinux) || defined(rtai)
00325 #ifdef rtai
00326 #define __KERNEL__
00327 #undef _RTAI_SHM_H_
00328 #endif
00329 #ifndef MODULE
00330 #define MODULE
00331 #endif
00332 #endif
00333
00334
00335 #define NO_ROUNDING
00336
00337
00338 #define COMPING
00339
00340 #if defined(rtlinux) || defined(rtai)
00341
00342
00343
00344
00345
00346 #include <linux/version.h>
00347 #include <linux/module.h>
00348 #include <linux/kernel.h>
00349 #include <linux/errno.h>
00350
00351 #if defined(rtlinux) && !defined(rtlinux2)
00352 #include <linux/mm.h>
00353 #endif
00354
00355 #ifdef rtlinux
00356 #include <rtl_sched.h>
00357 #endif
00358
00359 #ifdef rtai
00360 #include <linux/types.h>
00361 #include <linux/fcntl.h>
00362 #include <linux/mman.h>
00363 #include <linux/malloc.h>
00364 #include <math.h>
00365 #include "rtai.h"
00366 #include "rtai_sched.h"
00367 #include "rtai_shm.h"
00368 #include <linux/posix_types.h>
00369 #endif
00370
00371 #if defined(__KERNEL__) && defined(linux)
00372 #include <linux/kernel.h>
00373 #endif
00374
00375 #if defined(linux) || defined(rtlinux)
00376 #include <linux/fs.h>
00377 #include <sys/io.h>
00378 #endif
00379
00380
00381 #ifndef KERNEL_VERSION
00382 #define KERNEL_VERSION(a,b,c) (((a) << 16) + ((b) << 8) + (c))
00383 #endif
00384
00385
00386
00387 #if LINUX_VERSION_CODE < KERNEL_VERSION(2,2,0)
00388
00389 #endif
00390
00391 #ifndef rtai
00392 #include <math.h>
00393 #endif
00394 #include <stdarg.h>
00395
00396 #ifdef COMPING
00397 #include <float.h>
00398 #endif
00399
00400 #else
00401
00402 #include "_timer.h"
00403 #include "_shm.h"
00404 #if !defined(UNDER_CE) && !defined(NO_STDIO_SUPPORT)
00405 #include <stdio.h>
00406 #endif
00407 #include <stdlib.h>
00408 #include <math.h>
00409 #include <stdarg.h>
00410 #include <string.h>
00411 #ifdef COMPING
00412 #include <float.h>
00413 #endif
00414
00415 #endif
00416
00417 #include "posemath.h"
00418 #include "emcpos.h"
00419 #include "emcmotcfg.h"
00420 #include "emcmotglb.h"
00421 #include "emcmot.h"
00422 #include "pid.h"
00423 #include "cubic.h"
00424 #include "tc.h"
00425 #include "tp.h"
00426 #include "extintf.h"
00427 #include "mmxavg.h"
00428 #include "emcmotlog.h"
00429
00430
00431 #ifndef __GNUC__
00432 #ifndef __attribute__
00433 #define __attribute__(x)
00434 #endif
00435 #endif
00436
00437 static char __attribute__((unused)) ident[] = "$Id: emcmot.c,v 1.56 2001/11/08 23:55:47 paul_c Exp $";
00438
00439 #ifdef rtai
00440 static RTIME rtaiTickPeriod,now;
00441 static long rtaiTickPeriod_long;
00442 #endif
00443
00444 #ifdef STEPPER_MOTORS
00445
00446
00447 #include "parport.h"
00448
00449
00450
00451 #define OUTPUT_DELTA 1.0e-6
00452
00453 #endif
00454
00455 #if ((defined(rtlinux2) || defined(RTLINUX_V2)) && !defined(CONFIG_RTL_USE_V1_API)) || defined(RTLINUX_V3)
00456 #define USE_RTL2
00457 #endif
00458
00459 #ifdef USE_RTL2
00460 #include <rtl.h>
00461 #include <time.h>
00462 #include "mbuff.h"
00463 #endif
00464
00465 #ifdef SIMULATED_MOTORS
00466 #include "sim.h"
00467 #endif
00468
00469
00470
00471 static int PERIOD = 48000;
00472
00473 #ifndef STEPPER_MOTORS
00474 extern unsigned short STG_BASE_ADDRESS;
00475 extern int FIND_STG_BASE_ADDRESS;
00476 #else
00477 extern unsigned long int PARPORT_IO_ADDRESS;
00478
00479 #define DEFAULT_FREQ_TASK_PRIORITY 1
00480 #define DEFAULT_FREQ_TASK_STACK_SIZE 4096
00481
00482 static int FREQ_TASK_PRIORITY = DEFAULT_FREQ_TASK_PRIORITY;
00483 static int FREQ_TASK_STACK_SIZE = DEFAULT_FREQ_TASK_STACK_SIZE;
00484 #endif
00485
00486 #ifdef rtlinux
00487 #ifdef USE_RTL2
00488 static hrtime_t rtperiod;
00489 #else
00490
00491 static RTIME rtstart;
00492 static RTIME rtperiod;
00493
00494
00495
00496 #define SECS_TO_RTIME(secs) ((RTIME) (RT_TICKS_PER_SEC * secs))
00497
00498 #endif
00499 #endif
00500
00501 #if defined(rtlinux) || defined(rtai)
00502 static int iperiod;
00503 #endif
00504
00505 #ifdef rtai
00506 static RTIME iperiod_rtime;
00507 #endif
00508
00509
00510 #if defined(rtlinux)|| defined(rtai)
00511 #ifdef rtai
00512 static RT_TASK emcmotTask;
00513 #else
00514 #ifdef USE_RTL2
00515 static pthread_t emcmotTask;
00516 #else
00517 static RT_TASK emcmotTask;
00518 #endif
00519 #endif
00520
00521 #ifdef STEPPER_MOTORS
00522 #ifdef rtai
00523 static RT_TASK freqTask;
00524 #else
00525 #ifdef USE_RTL2
00526 static pthread_t freqTask;
00527 #else
00528 static RT_TASK freqTask;
00529 #endif
00530 #endif
00531 #endif
00532 #endif
00533
00534 static int emcmotTask_created = 0;
00535 static int freqTask_created = 0;
00536
00537 #define DEFAULT_EMCMOT_TASK_PRIORITY 2
00538
00539 #define DEFAULT_EMCMOT_TASK_STACK_SIZE 8192
00540 static int EMCMOT_TASK_PRIORITY=DEFAULT_EMCMOT_TASK_PRIORITY;
00541 static int EMCMOT_TASK_STACK_SIZE=DEFAULT_EMCMOT_TASK_STACK_SIZE;
00542
00543
00544 #define SOUND_PORT 0x61
00545 #define SOUND_MASK 0x02
00546 #if defined(rtlinux) || defined(rtai)
00547 static unsigned char soundByte;
00548 #endif
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566 EMCMOT_STRUCT *emcmotStruct;
00567
00568 #ifdef rtlinux
00569 #ifndef realtimeonly
00570 #define realtimeonly
00571 #endif
00572 #endif
00573
00574 #ifdef rtai
00575 #ifndef realtimeonly
00576 #define realtimeonly
00577 #endif
00578 #endif
00579
00580 #ifndef realtimeonly
00581
00582 static shm_t *shmem = NULL;
00583 #endif
00584
00585
00586
00587 static EMCMOT_COMMAND *emcmotCommand;
00588 static EMCMOT_STATUS *emcmotStatus;
00589 static EMCMOT_STATUS *emcmotStatus;
00590 static EMCMOT_CONFIG *emcmotConfig;
00591 static EMCMOT_DEBUG *emcmotDebug;
00592 static EMCMOT_ERROR *emcmotError;
00593 static EMCMOT_LOG *emcmotLog;
00594 static EMCMOT_COMP *emcmotComp[EMCMOT_MAX_AXIS];
00595
00596 static EMCMOT_LOG_STRUCT ls;
00597 static int logSkip = 0;
00598 static int loggingAxis = 0;
00599 static int logIt = 0;
00600 static int logStartTime;
00601
00602
00603
00604 static EmcPose worldHome = {{0.0, 0.0, 0.0},
00605 0.0, 0.0, 0.0};
00606
00607
00608 static KINEMATICS_FORWARD_FLAGS fflags = 0;
00609 static KINEMATICS_INVERSE_FLAGS iflags = 0;
00610
00611
00612 static int kinType = 0;
00613
00614
00615
00616
00617
00618
00619
00620 static int rehomeAll = 0;
00621
00622
00623 #define LIMIT_SWITCH_DEBOUNCE 10
00624 #define AMP_FAULT_DEBOUNCE 100
00625 #define POSITION_INPUT_DEBOUNCE 10
00626 static double positionInputDebounce[EMCMOT_MAX_AXIS] = {0.0};
00627
00628
00629 #if defined(rtlinux) || defined(rtai)
00630 #ifdef rtai
00631 #define diagnostics rt_printk
00632 #else
00633 #define diagnostics rtl_printf
00634 #endif
00635 #else
00636 #ifdef UNDER_CE
00637 #include "rcs_prnt.hh"
00638 #define diagnostics rcs_print
00639 #else
00640 #define diagnostics printf
00641 #endif
00642 #endif
00643
00644 static inline void MARK_EMCMOT_CONFIG_CHANGE(void)
00645 {
00646 if(emcmotConfig->head == emcmotConfig->tail) {
00647 emcmotConfig->config_num++;
00648 emcmotStatus->config_num = emcmotConfig->config_num;
00649 emcmotConfig->head++;
00650 }
00651 }
00652
00653
00654
00655
00656
00657 #define GET_MOTION_ERROR_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_ERROR_BIT ? 1 : 0)
00658
00659 #define SET_MOTION_ERROR_FLAG(fl) if (fl) emcmotStatus->motionFlag |= EMCMOT_MOTION_ERROR_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_ERROR_BIT;
00660
00661 #define GET_MOTION_COORD_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_COORD_BIT ? 1 : 0)
00662
00663 #define SET_MOTION_COORD_FLAG(fl) if (fl) emcmotStatus->motionFlag |= EMCMOT_MOTION_COORD_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_COORD_BIT;
00664
00665 #define GET_MOTION_TELEOP_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_TELEOP_BIT ? 1 : 0)
00666
00667 #define SET_MOTION_TELEOP_FLAG(fl) if (fl) emcmotStatus->motionFlag |= EMCMOT_MOTION_TELEOP_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_TELEOP_BIT;
00668
00669 #define GET_MOTION_INPOS_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_INPOS_BIT ? 1 : 0)
00670
00671 #define SET_MOTION_INPOS_FLAG(fl) if (fl) emcmotStatus->motionFlag |= EMCMOT_MOTION_INPOS_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_INPOS_BIT;
00672
00673 #define GET_MOTION_ENABLE_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_ENABLE_BIT ? 1 : 0)
00674
00675 #define SET_MOTION_ENABLE_FLAG(fl) if (fl) emcmotStatus->motionFlag |= EMCMOT_MOTION_ENABLE_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_ENABLE_BIT;
00676
00677
00678
00679 #define GET_AXIS_ENABLE_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_ENABLE_BIT ? 1 : 0)
00680
00681 #define SET_AXIS_ENABLE_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_ENABLE_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_ENABLE_BIT;
00682
00683 #define GET_AXIS_ACTIVE_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_ACTIVE_BIT ? 1 : 0)
00684
00685 #define SET_AXIS_ACTIVE_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_ACTIVE_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_ACTIVE_BIT;
00686
00687 #define GET_AXIS_INPOS_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_INPOS_BIT ? 1 : 0)
00688
00689 #define SET_AXIS_INPOS_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_INPOS_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_INPOS_BIT;
00690
00691 #define GET_AXIS_ERROR_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_ERROR_BIT ? 1 : 0)
00692
00693 #define SET_AXIS_ERROR_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_ERROR_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_ERROR_BIT;
00694
00695 #define GET_AXIS_PSL_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_MAX_SOFT_LIMIT_BIT ? 1 : 0)
00696
00697 #define SET_AXIS_PSL_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_MAX_SOFT_LIMIT_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_MAX_SOFT_LIMIT_BIT;
00698
00699 #define GET_AXIS_NSL_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_MIN_SOFT_LIMIT_BIT ? 1 : 0)
00700
00701 #define SET_AXIS_NSL_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_MIN_SOFT_LIMIT_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_MIN_SOFT_LIMIT_BIT;
00702
00703 #define GET_AXIS_PHL_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_MAX_HARD_LIMIT_BIT ? 1 : 0)
00704
00705 #define SET_AXIS_PHL_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_MAX_HARD_LIMIT_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_MAX_HARD_LIMIT_BIT;
00706
00707 #define GET_AXIS_NHL_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_MIN_HARD_LIMIT_BIT ? 1 : 0)
00708
00709 #define SET_AXIS_NHL_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_MIN_HARD_LIMIT_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_MIN_HARD_LIMIT_BIT;
00710
00711 #define GET_AXIS_HOME_SWITCH_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_HOME_SWITCH_BIT ? 1 : 0)
00712
00713 #define SET_AXIS_HOME_SWITCH_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_HOME_SWITCH_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_HOME_SWITCH_BIT;
00714
00715 #define GET_AXIS_HOMING_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_HOMING_BIT ? 1 : 0)
00716
00717 #define SET_AXIS_HOMING_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_HOMING_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_HOMING_BIT;
00718
00719 #define GET_AXIS_HOMED_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_HOMED_BIT ? 1 : 0)
00720
00721 #define SET_AXIS_HOMED_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_HOMED_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_HOMED_BIT;
00722
00723 #define GET_AXIS_FERROR_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_FERROR_BIT ? 1 : 0)
00724
00725 #define SET_AXIS_FERROR_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_FERROR_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_FERROR_BIT;
00726
00727 #define GET_AXIS_FAULT_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_FAULT_BIT ? 1 : 0)
00728
00729 #define SET_AXIS_FAULT_FLAG(ax,fl) if (fl) emcmotStatus->axisFlag[ax] |= EMCMOT_AXIS_FAULT_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_FAULT_BIT;
00730
00731
00732
00733 #define GET_AXIS_ENABLE_POLARITY(ax) (emcmotConfig->axisPolarity[ax] & EMCMOT_AXIS_ENABLE_BIT ? 1 : 0)
00734
00735 #define SET_AXIS_ENABLE_POLARITY(ax,fl) MARK_EMCMOT_CONFIG_CHANGE(); if (fl) emcmotConfig->axisPolarity[ax] |= EMCMOT_AXIS_ENABLE_BIT; else emcmotConfig->axisPolarity[ax] &= ~EMCMOT_AXIS_ENABLE_BIT;
00736
00737 #define GET_AXIS_PHL_POLARITY(ax) (emcmotConfig->axisPolarity[ax] & EMCMOT_AXIS_MAX_HARD_LIMIT_BIT ? 1 : 0)
00738
00739 #define SET_AXIS_PHL_POLARITY(ax,fl) if (fl) emcmotConfig->axisPolarity[ax] |= EMCMOT_AXIS_MAX_HARD_LIMIT_BIT; else emcmotConfig->axisPolarity[ax] &= ~EMCMOT_AXIS_MAX_HARD_LIMIT_BIT;
00740
00741 #define GET_AXIS_NHL_POLARITY(ax) (emcmotConfig->axisPolarity[ax] & EMCMOT_AXIS_MIN_HARD_LIMIT_BIT ? 1 : 0)
00742
00743 #define SET_AXIS_NHL_POLARITY(ax,fl) if (fl) emcmotConfig->axisPolarity[ax] |= EMCMOT_AXIS_MIN_HARD_LIMIT_BIT; else emcmotConfig->axisPolarity[ax] &= ~EMCMOT_AXIS_MIN_HARD_LIMIT_BIT;
00744
00745 #define GET_AXIS_HOME_SWITCH_POLARITY(ax) (emcmotConfig->axisPolarity[ax] & EMCMOT_AXIS_HOME_SWITCH_BIT ? 1 : 0)
00746
00747 #define SET_AXIS_HOME_SWITCH_POLARITY(ax,fl) if (fl) emcmotConfig->axisPolarity[ax] |= EMCMOT_AXIS_HOME_SWITCH_BIT; else emcmotConfig->axisPolarity[ax] &= ~EMCMOT_AXIS_HOME_SWITCH_BIT;
00748
00749 #define GET_AXIS_HOMING_POLARITY(ax) (emcmotConfig->axisPolarity[ax] & EMCMOT_AXIS_HOMING_BIT ? 1 : 0)
00750
00751 #define SET_AXIS_HOMING_POLARITY(ax,fl) if (fl) emcmotConfig->axisPolarity[ax] |= EMCMOT_AXIS_HOMING_BIT; else emcmotConfig->axisPolarity[ax] &= ~EMCMOT_AXIS_HOMING_BIT;
00752
00753 #define GET_AXIS_FAULT_POLARITY(ax) (emcmotConfig->axisPolarity[ax] & EMCMOT_AXIS_FAULT_BIT ? 1 : 0)
00754
00755 #define SET_AXIS_FAULT_POLARITY(ax,fl) if (fl) emcmotConfig->axisPolarity[ax] |= EMCMOT_AXIS_FAULT_BIT; else emcmotConfig->axisPolarity[ax] &= ~EMCMOT_AXIS_FAULT_BIT;
00756
00757
00758 int EMCMOT_NO_FORWARD_KINEMATICS = 0;
00759
00760 #if defined(rtlinux) || defined(rtai)
00761
00762 #if LINUX_VERSION_CODE > KERNEL_VERSION(2,4,3)
00763 extern unsigned long cpu_khz;
00764
00765
00766 #define rdtsc2(low,high) \
00767 __asm__ __volatile__("rdtsc" : "=a" (low), "=d" (high))
00768
00769 #else
00770
00771 #ifdef rtai
00772 static RTIME starting_nanos=0;
00773 static int starting_nanos_initialized=0;
00774 #endif
00775
00776 #endif
00777
00778 static double etime(void)
00779 {
00780 #if LINUX_VERSION_CODE > KERNEL_VERSION(2,4,3)
00781 double cycles_d, khz_d;
00782 unsigned long eax, edx;
00783
00784 rdtsc2(eax,edx);
00785 cycles_d = (edx*4294967296.0)+eax;
00786 #if 0
00787 rt_printk("eax=%d\n",eax);
00788 rt_printk("edx=%d\n",edx);
00789 rt_printk("cycles_d=%d\n", ((int) (cycles_d/1000.0)));
00790 #endif
00791
00792 khz_d = (double) cpu_khz;
00793 return cycles_d/(khz_d*1.0e3);
00794
00795 #else
00796
00797 #ifdef rtai
00798 RTIME current_nanos = 0;
00799 double current_nanos_d = 0.0;
00800 if(!starting_nanos_initialized)
00801 {
00802 starting_nanos= rt_get_time_ns();
00803 current_nanos = 0;
00804 starting_nanos_initialized=1;
00805 }
00806 else
00807 {
00808 current_nanos =rt_get_time_ns() - starting_nanos;
00809 }
00810 current_nanos_d = (double) current_nanos;
00811 return (current_nanos_d*1.0e-9);
00812 #else
00813 #ifdef USE_RTL2
00814 hrtime_t now;
00815
00816 now = gethrtime();
00817
00818 return ((double) now) / ((double) HRTICKS_PER_SEC);
00819 #else
00820
00821 RTIME now;
00822
00823 now = rt_get_time();
00824
00825 return ((double) now) / ((double) RT_TICKS_PER_SEC);
00826 #endif
00827 #endif
00828 #endif
00829 }
00830
00831 #endif
00832
00833 #ifdef UNDER_CE
00834 #include "rcs_ce.h"
00835 #endif
00836
00837 static void reportError(const char *fmt, ...)
00838 {
00839 va_list args;
00840 char error[EMCMOT_ERROR_LEN];
00841
00842 va_start(args, fmt);
00843 #ifndef UNDER_CE
00844 vsprintf(error, fmt, args);
00845 #else
00846 RCS_CE_VSPRINTF(error,fmt,args);
00847 #endif
00848 va_end(args);
00849
00850 emcmotErrorPut(emcmotError, error);
00851 }
00852
00853
00854 static int isHoming(void)
00855 {
00856 int axis;
00857
00858 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
00859 if (GET_AXIS_HOMING_FLAG(axis)) {
00860 return 1;
00861 }
00862 }
00863
00864
00865 return 0;
00866 }
00867
00868
00869
00870
00871
00872
00873
00874
00875 static void clearHomes(int axis)
00876 {
00877 int t;
00878
00879 if (kinType == KINEMATICS_INVERSE_ONLY) {
00880 if (rehomeAll) {
00881 for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
00882 SET_AXIS_HOMED_FLAG(t, 0);
00883 }
00884 }
00885 else {
00886 SET_AXIS_HOMED_FLAG(axis, 0);
00887 }
00888 }
00889 if (NULL != emcmotDebug) {
00890 emcmotDebug->allHomed = 0;
00891 }
00892 }
00893
00894
00895 #define AXRANGE(axis) ((emcmotConfig->maxLimit[axis] - emcmotConfig->minLimit[axis]))
00896
00897
00898
00899 static int inRange(EmcPose pos)
00900 {
00901 double joint[EMCMOT_MAX_AXIS];
00902 int axis;
00903
00904
00905 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
00906 joint[axis] = 0.0;
00907 }
00908
00909
00910 kinematicsInverse(&pos, joint, &iflags, &fflags);
00911
00912 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
00913 if (! GET_AXIS_ACTIVE_FLAG(axis)) {
00914
00915 continue;
00916 }
00917
00918 if (joint[axis] > emcmotConfig->maxLimit[axis] ||
00919 joint[axis] < emcmotConfig->minLimit[axis]) {
00920 return 0;
00921 }
00922 }
00923
00924
00925 return 1;
00926 }
00927
00928
00929
00930 static int checkLimits(void)
00931 {
00932 int axis;
00933
00934 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
00935 if (! GET_AXIS_ACTIVE_FLAG(axis)) {
00936
00937 continue;
00938 }
00939
00940 if (GET_AXIS_PSL_FLAG(axis) ||
00941 GET_AXIS_NSL_FLAG(axis) ||
00942 GET_AXIS_PHL_FLAG(axis) ||
00943 GET_AXIS_NHL_FLAG(axis)) {
00944 return 0;
00945 }
00946 }
00947
00948 return 1;
00949 }
00950
00951
00952
00953
00954
00955 static int checkJog(int axis, double vel)
00956 {
00957 if (emcmotStatus->overrideLimits) {
00958 return 1;
00959 }
00960
00961 if (axis < 0 ||
00962 axis >= EMCMOT_MAX_AXIS) {
00963 reportError("Can't jog out of range axis %d.",axis);
00964 return 0;
00965 }
00966
00967 if (vel > 0.0 &&
00968 GET_AXIS_PSL_FLAG(axis) )
00969 {
00970 reportError("Can't jog axis %d further past max soft limit.",axis);
00971 return 0;
00972 }
00973
00974 if (vel > 0.0 &&
00975 GET_AXIS_PHL_FLAG(axis) )
00976 {
00977 reportError("Can't jog axis %d further past max hard limit.",axis);
00978 return 0;
00979 }
00980
00981 if (vel < 0.0 &&
00982 GET_AXIS_NSL_FLAG(axis) )
00983 {
00984 reportError("Can't jog axis %d further past min soft limit.",axis);
00985 return 0;
00986 }
00987
00988 if (vel < 0.0 &&
00989 GET_AXIS_NHL_FLAG(axis) )
00990 {
00991 reportError("Can't jog axis %d further past min hard limit.",axis);
00992 return 0;
00993 }
00994
00995
00996 return 1;
00997 }
00998
00999
01000
01001
01002
01003
01004
01005
01006 static int interpolationCounter = 0;
01007
01008
01009 static void setTrajCycleTime(double secs)
01010 {
01011 static int t;
01012
01013
01014 if (secs <= 0.0) {
01015 return;
01016 }
01017
01018 MARK_EMCMOT_CONFIG_CHANGE();
01019
01020
01021 emcmotConfig->interpolationRate =
01022 (int) (secs / emcmotConfig->servoCycleTime + 0.5);
01023
01024
01025 tpSetCycleTime(&emcmotDebug->queue, secs);
01026
01027
01028 for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
01029 tpSetCycleTime(&emcmotDebug->freeAxis[t], secs);
01030 cubicSetInterpolationRate(&emcmotDebug->cubic[t], emcmotConfig->interpolationRate);
01031 }
01032
01033
01034 emcmotConfig->trajCycleTime = secs;
01035 }
01036
01037
01038 static void setServoCycleTime(double secs)
01039 {
01040 static int t;
01041
01042
01043 if (secs <= 0.0) {
01044 return;
01045 }
01046
01047 MARK_EMCMOT_CONFIG_CHANGE();
01048
01049
01050 emcmotConfig->interpolationRate =
01051 (int) (emcmotConfig->trajCycleTime / secs + 0.5);
01052
01053
01054 for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
01055 cubicSetInterpolationRate(&emcmotDebug->cubic[t], emcmotConfig->interpolationRate);
01056 cubicSetSegmentTime(&emcmotDebug->cubic[t], secs);
01057 pidSetCycleTime(&emcmotConfig->pid[t], secs);
01058 }
01059
01060
01061 emcmotConfig->servoCycleTime = secs;
01062 }
01063
01064
01065 #define MOT_DOUT_PORT 0x278
01066 #define MOT_DOUT_ABORT_VAL 0
01067 void extMotDout(unsigned char byte)
01068 {
01069 #if defined(rtlinux) || defined(rtai)
01070 outb(byte, MOT_DOUT_PORT);
01071 #endif
01072 }
01073
01074
01075
01076
01077
01078 static int emcmotCommandHandler(void)
01079 {
01080 int axis;
01081 int valid;
01082
01083
01084 if (emcmotCommand->head != emcmotCommand->tail) {
01085 emcmotDebug->split++;
01086 return 0;
01087 }
01088
01089 if (emcmotCommand->commandNum != emcmotStatus->commandNumEcho) {
01090
01091 emcmotStatus->head++;
01092 emcmotDebug->head++;
01093
01094
01095 emcmotStatus->commandEcho = emcmotCommand->command;
01096 emcmotStatus->commandNumEcho = emcmotCommand->commandNum;
01097
01098
01099 emcmotStatus->commandStatus = EMCMOT_COMMAND_OK;
01100
01101
01102 if (emcmotStatus->logStarted &&
01103 emcmotStatus->logType == EMCMOT_LOG_TYPE_CMD) {
01104 ls.item.cmd.time = etime();
01105
01106 ls.item.cmd.command = emcmotCommand->command;
01107 ls.item.cmd.commandNum = emcmotCommand->commandNum;
01108 emcmotLogAdd(emcmotLog, ls);
01109 emcmotStatus->logPoints = emcmotLog->howmany;
01110 }
01111
01112
01113 switch (emcmotCommand->command) {
01114 case EMCMOT_FREE:
01115
01116
01117
01118
01119 emcmotDebug->coordinating = 0;
01120 emcmotDebug->teleoperating = 0;
01121 break;
01122
01123 case EMCMOT_COORD:
01124
01125
01126
01127
01128 emcmotDebug->coordinating = 1;
01129 emcmotDebug->teleoperating = 0;
01130 if (kinType != KINEMATICS_IDENTITY) {
01131 if (!emcmotDebug->allHomed) {
01132 reportError("all axes must be homed before going into coordinated mode");
01133 emcmotDebug->coordinating = 0;
01134 break;
01135 }
01136 }
01137 break;
01138
01139 case EMCMOT_TELEOP:
01140
01141
01142
01143
01144 emcmotDebug->teleoperating = 1;
01145 if (kinType != KINEMATICS_IDENTITY) {
01146 if (! emcmotDebug->allHomed) {
01147 reportError("all axes must be homed before going into teleop mode");
01148 emcmotDebug->teleoperating = 0;
01149 break;
01150 }
01151
01152 }
01153 break;
01154
01155 case EMCMOT_SET_NUM_AXES:
01156
01157
01158 axis = emcmotCommand->axis;
01159
01160
01161
01162
01163
01164 if (axis <= 0 ||
01165 axis > EMCMOT_MAX_AXIS) {
01166 break;
01167 }
01168 NUM_AXES = axis;
01169 break;
01170
01171 case EMCMOT_SET_WORLD_HOME:
01172 worldHome = emcmotCommand->pos;
01173 break;
01174
01175 case EMCMOT_SET_JOINT_HOME:
01176 axis = emcmotCommand->axis;
01177 if (axis < 0 ||
01178 axis >= EMCMOT_MAX_AXIS) {
01179 break;
01180 }
01181 emcmotDebug->jointHome[axis] = emcmotCommand->offset;
01182 break;
01183
01184 case EMCMOT_SET_HOME_OFFSET:
01185 MARK_EMCMOT_CONFIG_CHANGE();
01186 axis = emcmotCommand->axis;
01187 if (axis < 0 ||
01188 axis >= EMCMOT_MAX_AXIS) {
01189 break;
01190 }
01191 emcmotConfig->homeOffset[axis] = emcmotCommand->offset;
01192 break;
01193
01194 case EMCMOT_OVERRIDE_LIMITS:
01195 if (emcmotCommand->axis < 0) {
01196
01197 emcmotStatus->overrideLimits = 0;
01198 }
01199 else {
01200 emcmotStatus->overrideLimits = 1;
01201 }
01202 emcmotDebug->overriding = 0;
01203 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
01204 SET_AXIS_ERROR_FLAG(axis, 0);
01205 }
01206 break;
01207
01208 case EMCMOT_SET_TRAJ_CYCLE_TIME:
01209
01210
01211
01212
01213
01214 setTrajCycleTime(emcmotCommand->cycleTime);
01215 break;
01216
01217 case EMCMOT_SET_SERVO_CYCLE_TIME:
01218
01219
01220
01221
01222
01223
01224 #ifdef rtai
01225 if(rtaiTickPeriod > 0)
01226 {
01227 rtaiTickPeriod_long= (long) rtaiTickPeriod;
01228 iperiod = nano2count((int) (1e9* emcmotCommand->cycleTime));
01229
01230 if ( iperiod < rtaiTickPeriod_long)
01231 {
01232 iperiod = rtaiTickPeriod_long;
01233 }
01234 iperiod /= rtaiTickPeriod_long;
01235 iperiod *= rtaiTickPeriod_long;
01236 iperiod_rtime = (RTIME) iperiod;
01237 now = rt_get_time();
01238 rt_task_make_periodic(&emcmotTask,now+rtaiTickPeriod,iperiod_rtime);
01239 setServoCycleTime(count2nano(iperiod)*1e-9);
01240 }
01241 #else
01242
01243 #ifdef rtlinux
01244 #ifdef USE_RTL2
01245 iperiod = (HRTICKS_PER_SEC * emcmotCommand->cycleTime);
01246 #else
01247 iperiod = (RT_TICKS_PER_SEC * emcmotCommand->cycleTime);
01248 #endif
01249
01250
01251 iperiod = iperiod / PERIOD;
01252 iperiod = iperiod * PERIOD;
01253 if (iperiod < PERIOD) {
01254 iperiod = PERIOD;
01255 }
01256 #ifdef USE_RTL2
01257 rtperiod = (hrtime_t) iperiod;
01258 #else
01259 rtperiod = (RTIME) iperiod;
01260 #endif
01261
01262 #ifdef USE_RTL2
01263 pthread_make_periodic_np(emcmotTask,
01264 gethrtime() + rtperiod,
01265 rtperiod);
01266 #else
01267 rt_task_make_periodic(&emcmotTask,
01268 rt_get_time() + rtperiod,
01269 rtperiod);
01270 #endif
01271
01272 #ifdef USE_RTL2
01273 setServoCycleTime(((double) iperiod) / ((double) HRTICKS_PER_SEC));
01274 #else
01275 setServoCycleTime(((double) iperiod) / ((double) RT_TICKS_PER_SEC));
01276 #endif
01277 #else
01278 setServoCycleTime(emcmotCommand->cycleTime);
01279 #endif
01280 #endif
01281 break;
01282
01283 case EMCMOT_SET_POSITION_LIMITS:
01284 MARK_EMCMOT_CONFIG_CHANGE();
01285
01286
01287 axis = emcmotCommand->axis;
01288 if (axis < 0 ||
01289 axis >= EMCMOT_MAX_AXIS) {
01290 break;
01291 }
01292 emcmotConfig->minLimit[axis] = emcmotCommand->minLimit;
01293 emcmotConfig->maxLimit[axis] = emcmotCommand->maxLimit;
01294 break;
01295
01296 case EMCMOT_SET_OUTPUT_LIMITS:
01297 MARK_EMCMOT_CONFIG_CHANGE();
01298
01299
01300 axis = emcmotCommand->axis;
01301 if (axis < 0 ||
01302 axis >= EMCMOT_MAX_AXIS) {
01303 break;
01304 }
01305
01306 emcmotConfig->minOutput[axis] = emcmotCommand->minLimit;
01307 emcmotConfig->maxOutput[axis] = emcmotCommand->maxLimit;
01308 break;
01309
01310 case EMCMOT_SET_OUTPUT_SCALE:
01311 axis = emcmotCommand->axis;
01312 if (axis < 0 ||
01313 axis >= EMCMOT_MAX_AXIS ||
01314 emcmotCommand->scale == 0) {
01315 break;
01316 }
01317 emcmotStatus->outputScale[axis] = emcmotCommand->scale;
01318 emcmotStatus->outputOffset[axis] = emcmotCommand->offset;
01319 emcmotDebug->inverseOutputScale[axis] = 1.0 / emcmotStatus->outputScale[axis];
01320 break;
01321
01322 case EMCMOT_SET_INPUT_SCALE:
01323
01324
01325
01326
01327
01328
01329
01330 axis = emcmotCommand->axis;
01331 if (axis < 0 ||
01332 axis >= EMCMOT_MAX_AXIS ||
01333 emcmotCommand->scale == 0.0) {
01334 break;
01335 }
01336 #if 0
01337
01338
01339 emcmotDebug->oldInput[axis] = emcmotDebug->oldInput[axis] * emcmotStatus->inputScale[axis] +
01340 emcmotStatus->inputOffset[axis];
01341 emcmotDebug->oldInput[axis] = (emcmotDebug->oldInput[axis] - emcmotCommand->offset) /
01342 emcmotCommand->scale;
01343 #endif
01344 emcmotDebug->oldInputValid[axis] = 0;
01345
01346
01347 emcmotStatus->inputScale[axis] = emcmotCommand->scale;
01348 emcmotStatus->inputOffset[axis] = emcmotCommand->offset;
01349 emcmotDebug->inverseInputScale[axis] = 1.0 / emcmotStatus->inputScale[axis];
01350
01351 break;
01352
01353
01354
01355
01356
01357
01358
01359
01360
01361
01362 case EMCMOT_SET_MAX_FERROR:
01363 MARK_EMCMOT_CONFIG_CHANGE();
01364 axis = emcmotCommand->axis;
01365 if (axis < 0 ||
01366 axis >= EMCMOT_MAX_AXIS ||
01367 emcmotCommand->maxFerror < 0.0) {
01368 break;
01369 }
01370 emcmotConfig->maxFerror[axis] = emcmotCommand->maxFerror;
01371 break;
01372
01373 case EMCMOT_SET_MIN_FERROR:
01374 MARK_EMCMOT_CONFIG_CHANGE();
01375 axis = emcmotCommand->axis;
01376 if (axis < 0 ||
01377 axis >= EMCMOT_MAX_AXIS ||
01378 emcmotCommand->minFerror < 0.0) {
01379 break;
01380 }
01381 emcmotConfig->minFerror[axis] = emcmotCommand->minFerror;
01382 break;
01383
01384 case EMCMOT_JOG_CONT:
01385
01386
01387
01388
01389
01390 axis = emcmotCommand->axis;
01391 if (axis < 0 ||
01392 axis >= EMCMOT_MAX_AXIS) {
01393 break;
01394 }
01395
01396
01397 if (GET_MOTION_COORD_FLAG() )
01398 {
01399 reportError("Can't jog axis in coordinated mode.");
01400 SET_AXIS_ERROR_FLAG(axis, 1);
01401 break;
01402 }
01403
01404 if( ! GET_MOTION_INPOS_FLAG() )
01405 {
01406 reportError("Can't jog axis when not in position.");
01407 SET_AXIS_ERROR_FLAG(axis, 1);
01408 break;
01409 }
01410
01411 if( ! GET_MOTION_ENABLE_FLAG()) {
01412 reportError("Can't jog axis when not enabled.");
01413 SET_AXIS_ERROR_FLAG(axis, 1);
01414 break;
01415 }
01416
01417
01418 if (! checkJog(axis, emcmotCommand->vel)) {
01419 SET_AXIS_ERROR_FLAG(axis, 1);
01420 break;
01421 }
01422
01423 if (emcmotCommand->vel > 0.0) {
01424 if (GET_AXIS_HOMED_FLAG(axis)) {
01425 emcmotDebug->freePose.tran.x = emcmotConfig->maxLimit[axis];
01426 }
01427 else {
01428 emcmotDebug->freePose.tran.x = AXRANGE(axis);
01429 }
01430 }
01431 else {
01432 if (GET_AXIS_HOMED_FLAG(axis)) {
01433 emcmotDebug->freePose.tran.x = emcmotConfig->minLimit[axis];
01434 }
01435 else {
01436 emcmotDebug->freePose.tran.x = - AXRANGE(axis);
01437 }
01438 }
01439
01440 tpSetVmax(&emcmotDebug->freeAxis[axis], fabs(emcmotCommand->vel));
01441 tpAddLine(&emcmotDebug->freeAxis[axis], emcmotDebug->freePose);
01442 SET_AXIS_ERROR_FLAG(axis, 0);
01443
01444
01445
01446
01447 clearHomes(axis);
01448 break;
01449
01450 case EMCMOT_JOG_INCR:
01451
01452
01453
01454 axis = emcmotCommand->axis;
01455 if (axis < 0 ||
01456 axis >= EMCMOT_MAX_AXIS) {
01457 break;
01458 }
01459
01460
01461 if (GET_MOTION_COORD_FLAG() ||
01462 ! GET_MOTION_INPOS_FLAG() ||
01463 ! GET_MOTION_ENABLE_FLAG()) {
01464 SET_AXIS_ERROR_FLAG(axis, 1);
01465 break;
01466 }
01467
01468
01469 if (! checkJog(axis, emcmotCommand->vel)) {
01470 SET_AXIS_ERROR_FLAG(axis, 1);
01471 break;
01472 }
01473
01474 if (emcmotCommand->vel > 0.0) {
01475 emcmotDebug->freePose.tran.x = emcmotDebug->jointPos[axis] + emcmotCommand->offset;
01476 if (GET_AXIS_HOMED_FLAG(axis)) {
01477 if (emcmotDebug->freePose.tran.x > emcmotConfig->maxLimit[axis]) {
01478 emcmotDebug->freePose.tran.x = emcmotConfig->maxLimit[axis];
01479 }
01480 }
01481 }
01482 else {
01483 emcmotDebug->freePose.tran.x = emcmotDebug->jointPos[axis] - emcmotCommand->offset;
01484 if (GET_AXIS_HOMED_FLAG(axis)) {
01485 if (emcmotDebug->freePose.tran.x < emcmotConfig->minLimit[axis]) {
01486 emcmotDebug->freePose.tran.x = emcmotConfig->minLimit[axis];
01487 }
01488 }
01489 }
01490
01491 tpSetVmax(&emcmotDebug->freeAxis[axis], fabs(emcmotCommand->vel));
01492 tpAddLine(&emcmotDebug->freeAxis[axis], emcmotDebug->freePose);
01493 SET_AXIS_ERROR_FLAG(axis, 0);
01494
01495
01496
01497
01498 clearHomes(axis);
01499
01500 break;
01501
01502 case EMCMOT_JOG_ABS:
01503
01504
01505
01506 axis = emcmotCommand->axis;
01507 if (axis < 0 ||
01508 axis >= EMCMOT_MAX_AXIS) {
01509 break;
01510 }
01511
01512
01513 if (GET_MOTION_COORD_FLAG() ||
01514 ! GET_MOTION_INPOS_FLAG() ||
01515 ! GET_MOTION_ENABLE_FLAG()) {
01516 SET_AXIS_ERROR_FLAG(axis, 1);
01517 break;
01518 }
01519
01520
01521 if (! checkJog(axis, emcmotCommand->vel)) {
01522 SET_AXIS_ERROR_FLAG(axis, 1);
01523 break;
01524 }
01525
01526 emcmotDebug->freePose.tran.x = emcmotCommand->offset;
01527 if (GET_AXIS_HOMED_FLAG(axis)) {
01528 if (emcmotDebug->freePose.tran.x > emcmotConfig->maxLimit[axis]) {
01529 emcmotDebug->freePose.tran.x = emcmotConfig->maxLimit[axis];
01530 }
01531 else if (emcmotDebug->freePose.tran.x < emcmotConfig->minLimit[axis]) {
01532 emcmotDebug->freePose.tran.x = emcmotConfig->minLimit[axis];
01533 }
01534 }
01535
01536 tpSetVmax(&emcmotDebug->freeAxis[axis], fabs(emcmotCommand->vel));
01537 tpAddLine(&emcmotDebug->freeAxis[axis], emcmotDebug->freePose);
01538 SET_AXIS_ERROR_FLAG(axis, 0);
01539
01540
01541
01542
01543 clearHomes(axis);
01544
01545 break;
01546
01547 case EMCMOT_SET_TERM_COND:
01548
01549 tpSetTermCond(&emcmotDebug->queue, emcmotCommand->termCond);
01550 break;
01551
01552 case EMCMOT_SET_LINE:
01553
01554
01555 if (! GET_MOTION_COORD_FLAG() ||
01556 ! GET_MOTION_ENABLE_FLAG()) {
01557 reportError("need to be enabled, in coord mode for linear move");
01558 emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND;
01559 SET_MOTION_ERROR_FLAG(1);
01560 break;
01561 }
01562 else if (! inRange(emcmotCommand->pos)) {
01563 reportError("linear move %d out of range",
01564 emcmotCommand->id);
01565 emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
01566 tpAbort(&emcmotDebug->queue);
01567 SET_MOTION_ERROR_FLAG(1);
01568 break;
01569 }
01570 else if (! checkLimits()) {
01571 reportError("can't do linear move with limits exceeded");
01572 emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
01573 tpAbort(&emcmotDebug->queue);
01574 SET_MOTION_ERROR_FLAG(1);
01575 break;
01576 }
01577
01578
01579 tpSetId(&emcmotDebug->queue, emcmotCommand->id);
01580 if (-1 == tpAddLine(&emcmotDebug->queue, emcmotCommand->pos)) {
01581 reportError("can't add linear move");
01582 emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC;
01583 tpAbort(&emcmotDebug->queue);
01584 SET_MOTION_ERROR_FLAG(1);
01585 break;
01586 }
01587 else {
01588 SET_MOTION_ERROR_FLAG(0);
01589
01590
01591 rehomeAll = 1;
01592 }
01593 break;
01594
01595 case EMCMOT_SET_CIRCLE:
01596
01597
01598 if (! GET_MOTION_COORD_FLAG() ||
01599 ! GET_MOTION_ENABLE_FLAG()) {
01600 reportError("need to be enabled, in coord mode for circular move");
01601 emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND;
01602 SET_MOTION_ERROR_FLAG(1);
01603 break;
01604 }
01605 else if (! inRange(emcmotCommand->pos)) {
01606 reportError("circular move %d out of range",
01607 emcmotCommand->id);
01608 emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
01609 tpAbort(&emcmotDebug->queue);
01610 SET_MOTION_ERROR_FLAG(1);
01611 break;
01612 }
01613 else if (! checkLimits()) {
01614 reportError("can't do circular move with limits exceeded");
01615 emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
01616 tpAbort(&emcmotDebug->queue);
01617 SET_MOTION_ERROR_FLAG(1);
01618 break;
01619 }
01620
01621
01622 tpSetId(&emcmotDebug->queue, emcmotCommand->id);
01623 if (-1 ==
01624 tpAddCircle(&emcmotDebug->queue, emcmotCommand->pos,
01625 emcmotCommand->center, emcmotCommand->normal,
01626 emcmotCommand->turn)) {
01627 reportError("can't add circular move");
01628 emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC;
01629 tpAbort(&emcmotDebug->queue);
01630 SET_MOTION_ERROR_FLAG(1);
01631 break;
01632 }
01633 else {
01634 SET_MOTION_ERROR_FLAG(0);
01635
01636
01637 rehomeAll = 1;
01638 }
01639 break;
01640
01641 case EMCMOT_SET_VEL:
01642
01643
01644 emcmotStatus->vel = emcmotCommand->vel;
01645 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
01646 tpSetVmax(&emcmotDebug->freeAxis[axis], emcmotStatus->vel);
01647 }
01648 tpSetVmax(&emcmotDebug->queue, emcmotStatus->vel);
01649 break;
01650
01651 case EMCMOT_SET_VEL_LIMIT:
01652 MARK_EMCMOT_CONFIG_CHANGE();
01653
01654
01655 emcmotConfig->limitVel = emcmotCommand->vel;
01656 tpSetVlimit(&emcmotDebug->queue, emcmotConfig->limitVel);
01657 break;
01658
01659 case EMCMOT_SET_AXIS_VEL_LIMIT:
01660 MARK_EMCMOT_CONFIG_CHANGE();
01661
01662 axis = emcmotCommand->axis;
01663 if (axis < 0 ||
01664 axis >= EMCMOT_MAX_AXIS) {
01665 break;
01666 }
01667 tpSetVlimit(&emcmotDebug->freeAxis[axis], emcmotCommand->vel);
01668 emcmotConfig->axisLimitVel[axis] = emcmotCommand->vel;
01669 emcmotDebug->bigVel[axis] = 10 * emcmotCommand->vel;
01670 break;
01671
01672 case EMCMOT_SET_HOMING_VEL:
01673 MARK_EMCMOT_CONFIG_CHANGE();
01674
01675
01676
01677
01678
01679 axis = emcmotCommand->axis;
01680 if (axis < 0 ||
01681 axis >= EMCMOT_MAX_AXIS) {
01682 break;
01683 }
01684
01685 if (emcmotCommand->vel < 0.0) {
01686 emcmotConfig->homingVel[axis] = - emcmotCommand->vel;
01687 SET_AXIS_HOMING_POLARITY(axis, 0);
01688 }
01689 else {
01690 emcmotConfig->homingVel[axis] = emcmotCommand->vel;
01691 SET_AXIS_HOMING_POLARITY(axis, 1);
01692 }
01693 break;
01694
01695 case EMCMOT_SET_ACC:
01696
01697
01698 emcmotStatus->acc = emcmotCommand->acc;
01699 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
01700 tpSetAmax(&emcmotDebug->freeAxis[axis], emcmotStatus->acc);
01701 }
01702 tpSetAmax(&emcmotDebug->queue, emcmotStatus->acc);
01703 break;
01704
01705 case EMCMOT_PAUSE:
01706
01707
01708 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
01709 tpPause(&emcmotDebug->freeAxis[axis]);
01710 }
01711 tpPause(&emcmotDebug->queue);
01712 emcmotStatus->paused = 1;
01713 break;
01714
01715 case EMCMOT_RESUME:
01716
01717
01718 emcmotDebug->stepping = 0;
01719 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
01720 tpResume(&emcmotDebug->freeAxis[axis]);
01721 }
01722 tpResume(&emcmotDebug->queue);
01723 emcmotStatus->paused = 0;
01724 break;
01725
01726 case EMCMOT_STEP:
01727
01728
01729 emcmotDebug->idForStep = emcmotStatus->id;
01730 emcmotDebug->stepping = 1;
01731 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
01732 tpResume(&emcmotDebug->freeAxis[axis]);
01733 }
01734 tpResume(&emcmotDebug->queue);
01735 emcmotStatus->paused = 0;
01736 break;
01737
01738 case EMCMOT_SCALE:
01739
01740
01741 if (emcmotCommand->scale < 0.0) {
01742 emcmotCommand->scale = 0.0;
01743 }
01744 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
01745 tpSetVscale(&emcmotDebug->freeAxis[axis], emcmotCommand->scale);
01746 emcmotStatus->axVscale[axis] = emcmotCommand->scale;
01747 }
01748 tpSetVscale(&emcmotDebug->queue, emcmotCommand->scale);
01749 emcmotStatus->qVscale = emcmotCommand->scale;
01750 break;
01751
01752 case EMCMOT_ABORT:
01753
01754
01755
01756 if (GET_MOTION_TELEOP_FLAG()) {
01757 emcmotDebug->teleop_data.desiredVel.tran.x = 0.0;
01758 emcmotDebug->teleop_data.desiredVel.tran.y = 0.0;
01759 emcmotDebug->teleop_data.desiredVel.tran.z = 0.0;
01760 emcmotDebug->teleop_data.desiredVel.a = 0.0;
01761 emcmotDebug->teleop_data.desiredVel.b = 0.0;
01762 emcmotDebug->teleop_data.desiredVel.c = 0.0;
01763 }
01764 else if (GET_MOTION_COORD_FLAG()) {
01765 tpAbort(&emcmotDebug->queue);
01766 SET_MOTION_ERROR_FLAG(0);
01767 }
01768 else {
01769
01770 axis = emcmotCommand->axis;
01771 if (axis < 0 ||
01772 axis >= EMCMOT_MAX_AXIS) {
01773 break;
01774 }
01775 tpAbort(&emcmotDebug->freeAxis[axis]);
01776 SET_AXIS_HOMING_FLAG(axis, 0);
01777 SET_AXIS_ERROR_FLAG(axis, 0);
01778 }
01779
01780 extMotDout(MOT_DOUT_ABORT_VAL);
01781 break;
01782
01783 case EMCMOT_DISABLE:
01784
01785
01786
01787
01788 emcmotDebug->enabling = 0;
01789 if (kinType == KINEMATICS_INVERSE_ONLY) {
01790 emcmotDebug->teleoperating = 0;
01791 emcmotDebug->coordinating = 0;
01792 }
01793 break;
01794
01795 case EMCMOT_ENABLE:
01796
01797
01798
01799
01800 emcmotDebug->enabling = 1;
01801 if (kinType == KINEMATICS_INVERSE_ONLY) {
01802 emcmotDebug->teleoperating = 0;
01803 emcmotDebug->coordinating = 0;
01804 }
01805 break;
01806
01807 case EMCMOT_SET_PID:
01808 MARK_EMCMOT_CONFIG_CHANGE();
01809
01810 axis = emcmotCommand->axis;
01811 if (axis < 0 ||
01812 axis >= EMCMOT_MAX_AXIS) {
01813 break;
01814 }
01815 pidSetGains(&emcmotConfig->pid[axis],
01816 emcmotCommand->pid);
01817 break;
01818
01819 case EMCMOT_ACTIVATE_AXIS:
01820
01821
01822
01823 axis = emcmotCommand->axis;
01824 if (axis < 0 ||
01825 axis >= EMCMOT_MAX_AXIS) {
01826 break;
01827 }
01828 SET_AXIS_ACTIVE_FLAG(axis, 1);
01829 break;
01830
01831 case EMCMOT_DEACTIVATE_AXIS:
01832
01833
01834
01835 axis = emcmotCommand->axis;
01836 if (axis < 0 ||
01837 axis >= EMCMOT_MAX_AXIS) {
01838 break;
01839 }
01840 SET_AXIS_ACTIVE_FLAG(axis, 0);
01841 break;
01842
01843 case EMCMOT_ENABLE_AMPLIFIER:
01844
01845
01846 axis = emcmotCommand->axis;
01847 if (axis < 0 ||
01848 axis >= EMCMOT_MAX_AXIS) {
01849 break;
01850 }
01851 extAmpEnable(axis, GET_AXIS_ENABLE_POLARITY(axis));
01852 break;
01853
01854 case EMCMOT_DISABLE_AMPLIFIER:
01855
01856
01857
01858 axis = emcmotCommand->axis;
01859 if (axis < 0 ||
01860 axis >= EMCMOT_MAX_AXIS) {
01861 break;
01862 }
01863 extAmpEnable(axis, ! GET_AXIS_ENABLE_POLARITY(axis));
01864 break;
01865
01866 case EMCMOT_OPEN_LOG:
01867
01868 axis = emcmotCommand->axis;
01869 valid = 0;
01870 if (emcmotCommand->logSize > 0 &&
01871 emcmotCommand->logSize <= EMCMOT_LOG_MAX) {
01872
01873 switch (emcmotCommand->logType) {
01874 case EMCMOT_LOG_TYPE_AXIS_POS:
01875 case EMCMOT_LOG_TYPE_AXIS_VEL:
01876 case EMCMOT_LOG_TYPE_POS_VOLTAGE:
01877 if (axis >= 0 &&
01878 axis < EMCMOT_MAX_AXIS) {
01879 valid = 1;
01880 }
01881 break;
01882
01883 default:
01884 valid = 1;
01885 break;
01886 }
01887 }
01888
01889 if (valid) {
01890
01891 loggingAxis = axis;
01892 emcmotLogInit(emcmotLog,
01893 emcmotCommand->logType,
01894 emcmotCommand->logSize);
01895 emcmotStatus->logOpen = 1;
01896 emcmotStatus->logStarted = 0;
01897 emcmotStatus->logSize = emcmotCommand->logSize;
01898 emcmotStatus->logSkip = emcmotCommand->logSkip;
01899 emcmotStatus->logType = emcmotCommand->logType;
01900 emcmotStatus->logTriggerType = emcmotCommand->logTriggerType;
01901 emcmotStatus->logTriggerVariable = emcmotCommand->logTriggerVariable;
01902 emcmotStatus->logTriggerThreshold = emcmotCommand->logTriggerThreshold;
01903 if (axis >= 0 &&
01904 axis < EMCMOT_MAX_AXIS &&
01905 emcmotStatus->logTriggerType == EMCLOG_DELTA_TRIGGER)
01906 {
01907 switch(emcmotStatus->logTriggerVariable)
01908 {
01909 case EMCLOG_TRIGGER_ON_FERROR:
01910 emcmotStatus->logStartVal = emcmotDebug->ferrorCurrent[loggingAxis];
01911 break;
01912
01913 case EMCLOG_TRIGGER_ON_VOLT:
01914 emcmotStatus->logStartVal = emcmotDebug->rawOutput[loggingAxis];
01915 break;
01916 case EMCLOG_TRIGGER_ON_POS:
01917 emcmotStatus->logStartVal = emcmotDebug->jointPos[loggingAxis];
01918 break;
01919 case EMCLOG_TRIGGER_ON_VEL:
01920 emcmotStatus->logStartVal = emcmotDebug->jointPos[loggingAxis] - emcmotDebug->oldJointPos[loggingAxis];
01921 break;
01922
01923 default:
01924 break;
01925 }
01926 }
01927 }
01928 break;
01929
01930 case EMCMOT_START_LOG:
01931
01932
01933 if (emcmotStatus->logType == EMCMOT_LOG_TYPE_POS_VOLTAGE) {
01934 break;
01935 }
01936
01937
01938 if (emcmotStatus->logOpen &&
01939 emcmotStatus->logTriggerType == EMCLOG_MANUAL_TRIGGER) {
01940 logStartTime = etime();
01941 emcmotStatus->logStarted = 1;
01942 logSkip = 0;
01943 }
01944 break;
01945
01946 case EMCMOT_STOP_LOG:
01947
01948 emcmotStatus->logStarted = 0;
01949 break;
01950
01951 case EMCMOT_CLOSE_LOG:
01952 emcmotStatus->logOpen = 0;
01953 emcmotStatus->logStarted = 0;
01954 emcmotStatus->logSize = 0;
01955 emcmotStatus->logSkip = 0;
01956 emcmotStatus->logType = 0;
01957 break;
01958
01959 case EMCMOT_DAC_OUT:
01960
01961
01962 axis = emcmotCommand->axis;
01963 if (axis < 0 ||
01964 axis >= EMCMOT_MAX_AXIS) {
01965 break;
01966 }
01967
01968 if (emcmotStatus->logType == EMCMOT_LOG_TYPE_POS_VOLTAGE &&
01969 loggingAxis == axis &&
01970 emcmotStatus->logOpen != 0) {
01971 emcmotStatus->logStarted = 1;
01972 logSkip = 0;
01973 }
01974 emcmotStatus->output[axis] = emcmotCommand->dacOut;
01975 break;
01976
01977 case EMCMOT_HOME:
01978
01979
01980
01981 axis = emcmotCommand->axis;
01982 if (axis < 0 ||
01983 axis >= EMCMOT_MAX_AXIS) {
01984 break;
01985 }
01986 if (GET_MOTION_COORD_FLAG() ||
01987 ! GET_MOTION_ENABLE_FLAG()) {
01988 break;
01989 }
01990
01991 if (GET_AXIS_HOMING_POLARITY(axis)) {
01992 emcmotDebug->freePose.tran.x = + 2.0 * AXRANGE(axis);
01993 }
01994 else {
01995 emcmotDebug->freePose.tran.x = - 2.0 * AXRANGE(axis);
01996 }
01997
01998 tpSetVmax(&emcmotDebug->freeAxis[axis], emcmotConfig->homingVel[axis]);
01999 tpAddLine(&emcmotDebug->freeAxis[axis], emcmotDebug->freePose);
02000 emcmotDebug->homingPhase[axis]=1;
02001 SET_AXIS_HOMING_FLAG(axis, 1);
02002 SET_AXIS_HOMED_FLAG(axis, 0);
02003 break;
02004
02005 case EMCMOT_ENABLE_WATCHDOG:
02006 emcmotDebug->wdEnabling = 1;
02007 emcmotDebug->wdWait = emcmotCommand->wdWait;
02008 if (emcmotDebug->wdWait < 0) {
02009 emcmotDebug->wdWait = 0;
02010 }
02011 break;
02012
02013 case EMCMOT_DISABLE_WATCHDOG:
02014 emcmotDebug->wdEnabling = 0;
02015 break;
02016
02017 case EMCMOT_SET_POLARITY:
02018 MARK_EMCMOT_CONFIG_CHANGE();
02019 axis = emcmotCommand->axis;
02020 if (axis < 0 ||
02021 axis >= EMCMOT_MAX_AXIS) {
02022 break;
02023 }
02024 if (emcmotCommand->level) {
02025
02026 emcmotConfig->axisPolarity[axis] |= emcmotCommand->axisFlag;
02027 }
02028 else {
02029
02030 emcmotConfig->axisPolarity[axis] &= ~emcmotCommand->axisFlag;
02031 }
02032 break;
02033
02034 #ifdef ENABLE_PROBING
02035 case EMCMOT_SET_PROBE_INDEX:
02036 MARK_EMCMOT_CONFIG_CHANGE();
02037 emcmotConfig->probeIndex = emcmotCommand->probeIndex;
02038 break;
02039
02040 case EMCMOT_SET_PROBE_POLARITY:
02041 MARK_EMCMOT_CONFIG_CHANGE();
02042 emcmotConfig->probePolarity = emcmotCommand->level;
02043 break;
02044
02045 case EMCMOT_CLEAR_PROBE_FLAGS:
02046 emcmotStatus->probeTripped = 0;
02047 emcmotStatus->probing = 1;
02048 break;
02049
02050 case EMCMOT_PROBE:
02051
02052
02053
02054 if (! GET_MOTION_COORD_FLAG() ||
02055 ! GET_MOTION_ENABLE_FLAG()) {
02056 reportError("need to be enabled, in coord mode for probe move");
02057 emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND;
02058 SET_MOTION_ERROR_FLAG(1);
02059 break;
02060 }
02061 else if (! inRange(emcmotCommand->pos)) {
02062 reportError("probe move %d out of range",
02063 emcmotCommand->id);
02064 emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
02065 tpAbort(&emcmotDebug->queue);
02066 SET_MOTION_ERROR_FLAG(1);
02067 break;
02068 }
02069 else if (! checkLimits()) {
02070 reportError("can't do probe move with limits exceeded");
02071 emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
02072 tpAbort(&emcmotDebug->queue);
02073 SET_MOTION_ERROR_FLAG(1);
02074 break;
02075 }
02076
02077
02078 tpSetId(&emcmotDebug->queue, emcmotCommand->id);
02079 if (-1 == tpAddLine(&emcmotDebug->queue, emcmotCommand->pos)) {
02080 reportError("can't add probe move");
02081 emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC;
02082 tpAbort(&emcmotDebug->queue);
02083 SET_MOTION_ERROR_FLAG(1);
02084 break;
02085 }
02086 else {
02087 emcmotStatus->probeTripped = 0;
02088 emcmotStatus->probing = 1;
02089 SET_MOTION_ERROR_FLAG(0);
02090
02091
02092 rehomeAll = 1;
02093 }
02094 break;
02095 #endif
02096
02097 case EMCMOT_SET_TELEOP_VECTOR:
02098 if (! GET_MOTION_TELEOP_FLAG() ||
02099 ! GET_MOTION_ENABLE_FLAG()) {
02100 reportError("need to be enabled, in teleop mode for teleop move");
02101 }
02102 else {
02103 double velmag;
02104 emcmotDebug->teleop_data.desiredVel = emcmotCommand->pos;
02105 pmCartMag(emcmotDebug->teleop_data.desiredVel.tran, &velmag);
02106 if (emcmotDebug->teleop_data.desiredVel.a > velmag) {
02107 velmag = emcmotDebug->teleop_data.desiredVel.a;
02108 }
02109 if (emcmotDebug->teleop_data.desiredVel.b > velmag) {
02110 velmag = emcmotDebug->teleop_data.desiredVel.b;
02111 }
02112 if (emcmotDebug->teleop_data.desiredVel.c > velmag) {
02113 velmag = emcmotDebug->teleop_data.desiredVel.c;
02114 }
02115 if (velmag > emcmotConfig->limitVel) {
02116 pmCartScalMult(emcmotDebug->teleop_data.desiredVel.tran,
02117 emcmotConfig->limitVel/velmag,
02118 &emcmotDebug->teleop_data.desiredVel.tran);
02119 emcmotDebug->teleop_data.desiredVel.a *=
02120 emcmotConfig->limitVel/velmag;
02121 emcmotDebug->teleop_data.desiredVel.b *=
02122 emcmotConfig->limitVel/velmag;
02123 emcmotDebug->teleop_data.desiredVel.c *=
02124 emcmotConfig->limitVel/velmag;
02125 }
02126
02127
02128 rehomeAll = 1;
02129 }
02130 break;
02131
02132 case EMCMOT_SET_DEBUG:
02133 emcmotConfig->debug = emcmotCommand->debug;
02134 MARK_EMCMOT_CONFIG_CHANGE();
02135 break;
02136
02137 case EMCMOT_SET_AOUT:
02138 tpSetAout(&emcmotDebug->queue, emcmotCommand->out, emcmotCommand->minLimit, emcmotCommand->maxLimit);
02139 break;
02140
02141 case EMCMOT_SET_DOUT:
02142 tpSetDout(&emcmotDebug->queue, emcmotCommand->out, emcmotCommand->start, emcmotCommand->end);
02143 break;
02144
02145 default:
02146 reportError("unrecognized command %d", emcmotCommand->command);
02147 emcmotStatus->commandStatus = EMCMOT_COMMAND_UNKNOWN_COMMAND;
02148 break;
02149 }
02150
02151
02152 emcmotStatus->tail = emcmotStatus->head;
02153 emcmotConfig->tail = emcmotConfig->head;
02154 emcmotDebug->tail = emcmotDebug->head;
02155
02156 }
02157
02158 return 0;
02159 }
02160
02161
02162
02163
02164
02165
02166
02167
02168
02169
02170 static double axisComp(int axis, int dir, double nominput)
02171 {
02172 int index;
02173 double avgint;
02174 double compin;
02175 int lower, upper;
02176 int total;
02177 double *nominal;
02178 double *ptr;
02179 double denom;
02180
02181
02182 nominput += emcmotComp[axis]->alter;
02183
02184 total = emcmotComp[axis]->total;
02185 if (total < 2) {
02186 return nominput;
02187 }
02188 avgint = emcmotComp[axis]->avgint;
02189 index = (int) (nominput / avgint);
02190 nominal = emcmotComp[axis]->nominal;
02191 if (dir < 0) {
02192 ptr = emcmotComp[axis]->reverse;
02193 }
02194 else {
02195 ptr = emcmotComp[axis]->forward;
02196 }
02197
02198
02199 compin = nominput;
02200 lower = upper = 0;
02201 while (index >= 0 && index < total) {
02202 if (nominput > nominal[index]) {
02203 if (index == total - 1) {
02204
02205 break;
02206 }
02207 else if (nominput <= nominal[index + 1]) {
02208
02209 lower = index;
02210 upper = index + 1;
02211 denom = nominal[upper] - nominal[lower];
02212 if (denom < DBL_MIN) {
02213 compin = nominput;
02214
02215 }
02216 else {
02217 compin = ptr[lower] + (nominput - nominal[lower]) * (ptr[upper] - ptr[lower]) / denom;
02218 }
02219 break;
02220 }
02221 else {
02222
02223 index++;
02224 continue;
02225 }
02226 }
02227 else if (nominput < nominal[index]) {
02228 if (index == 0) {
02229
02230 break;
02231 }
02232 else if (nominput >= nominal[index - 1]) {
02233
02234 lower = index - 1;
02235 upper = index;
02236 denom = nominal[upper] - nominal[lower];
02237 if (denom < DBL_MIN) {
02238 compin = nominput;
02239
02240 }
02241 else {
02242 compin = ptr[lower] + (nominput - nominal[lower]) * (ptr[upper] - ptr[lower]) / denom;
02243 }
02244 break;
02245 }
02246 else {
02247
02248 index--;
02249 continue;
02250 }
02251 }
02252 else {
02253
02254 compin = ptr[index];
02255 lower = index;
02256 upper = index;
02257 break;
02258 }
02259 }
02260
02261 return compin;
02262 }
02263
02264
02265 static int debouncecount[EMCMOT_MAX_AXIS] = {0};
02266
02267
02268
02269
02270
02271
02272
02273
02274 #ifdef USE_RTL2
02275 static void *emcmotController(void *arg)
02276 #else
02277 static void emcmotController(int arg)
02278 #endif
02279 {
02280 int first = 1;
02281 double start, end, delta;
02282 int homeFlag;
02283 int axis;
02284 int t;
02285 int isLimit;
02286 int whichCycle;
02287 int fault;
02288 #ifndef NO_ROUNDING
02289 double numres;
02290 #endif
02291 double thisFerror[EMCMOT_MAX_AXIS] = {0.0};
02292 double limitFerror;
02293 double magFerror;
02294 double oldbcomp;
02295 int retval;
02296
02297 #ifdef STEPPER_MOTORS
02298 double pdmultfloat;
02299 #endif
02300 #ifdef COMPING
02301 int dir[EMCMOT_MAX_AXIS] = {1};
02302 #endif
02303
02304 #if defined(rtai) || defined(rtlinux)
02305 for (;;) {
02306 #endif
02307
02308 #ifdef SIMULATED_MOTORS
02309 #if defined(rtlinux) || defined(rtai)
02310 simMotInit(NULL);
02311 #endif
02312 #endif
02313
02314 #if defined(rtlinux) || defined(rtai)
02315
02316
02317 if (emcmotDebug->wdEnabling && ! emcmotDebug->wdEnabled) {
02318
02319 emcmotDebug->wdCount = emcmotDebug->wdWait;
02320 emcmotDebug->wdToggle = 0;
02321 emcmotDebug->wdEnabled = 1;
02322 }
02323 if (! emcmotDebug->wdEnabling && emcmotDebug->wdEnabled) {
02324
02325
02326 soundByte = inb(SOUND_PORT);
02327 soundByte &= ~SOUND_MASK;
02328 outb(soundByte, SOUND_PORT);
02329 emcmotDebug->wdEnabled = 0;
02330 }
02331
02332 if (emcmotDebug->wdEnabled && emcmotDebug->wdCount-- <= 0) {
02333 soundByte = inb(SOUND_PORT);
02334 emcmotDebug->wdToggle = ! emcmotDebug->wdToggle;
02335 if (emcmotDebug->wdToggle) {
02336 soundByte |= SOUND_MASK;
02337 }
02338 else {
02339 soundByte &= ~SOUND_MASK;
02340 }
02341 outb(soundByte, SOUND_PORT);
02342 emcmotDebug->wdCount = emcmotDebug->wdWait;
02343 }
02344 #endif
02345
02346
02347 start = etime();
02348
02349
02350 emcmotCommandHandler();
02351
02352
02353 emcmotStatus->head++;
02354
02355
02356
02357
02358
02359
02360 extEncoderReadAll(EMCMOT_MAX_AXIS, emcmotDebug->rawInput);
02361
02362
02363 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02364
02365 if (! first && emcmotDebug->oldInputValid[axis] ) {
02366 emcmotDebug->oldInput[axis] = emcmotStatus->input[axis];
02367 }
02368
02369 emcmotStatus->input[axis] =
02370 (emcmotDebug->rawInput[axis] - emcmotStatus->inputOffset[axis]) *
02371 emcmotDebug->inverseInputScale[axis] - emcmotDebug->bcompincr[axis];
02372
02373 #ifdef COMPING
02374
02375 if (GET_AXIS_HOMED_FLAG(axis)) {
02376 emcmotStatus->input[axis] = axisComp(axis, dir[axis], emcmotStatus->input[axis]);
02377 }
02378 #endif
02379 if (first || !emcmotDebug->oldInputValid[axis] ) {
02380 emcmotDebug->oldInput[axis] = emcmotStatus->input[axis];
02381 emcmotDebug->oldInputValid[axis] = 1;
02382 first = 0;
02383 }
02384
02385
02386 #ifndef SIMULATED_MOTORS
02387 if (fabs(emcmotStatus->input[axis] - emcmotDebug->oldInput[axis]) /
02388 emcmotConfig->servoCycleTime > emcmotDebug->bigVel[axis]) {
02389
02390
02391 if (++positionInputDebounce[axis] > POSITION_INPUT_DEBOUNCE) {
02392
02393
02394 emcmotStatus->input[axis] = emcmotDebug->oldInput[axis] +
02395 emcmotDebug->jointVel[axis] * emcmotConfig->servoCycleTime;
02396 }
02397 else {
02398
02399
02400
02401
02402 emcmotStatus->input[axis] = emcmotDebug->oldInput[axis];
02403 }
02404
02405 debouncecount[axis]++;
02406 }
02407 else {
02408
02409 positionInputDebounce[axis] = 0;
02410 }
02411 #endif
02412
02413
02414
02415
02416
02417
02418 if (GET_AXIS_ACTIVE_FLAG(axis)) {
02419 extMaxLimitSwitchRead(axis, &isLimit);
02420 if (isLimit == GET_AXIS_PHL_POLARITY(axis)) {
02421 if (++emcmotDebug->maxLimitSwitchCount[axis] > LIMIT_SWITCH_DEBOUNCE) {
02422 SET_AXIS_PHL_FLAG(axis, 1);
02423 emcmotDebug->maxLimitSwitchCount[axis] = LIMIT_SWITCH_DEBOUNCE;
02424 if (emcmotStatus->overrideLimits ||
02425 isHoming()) {
02426 }
02427 else {
02428 SET_AXIS_ERROR_FLAG(axis, 1);
02429 emcmotDebug->enabling = 0;
02430 }
02431 }
02432 }
02433 else {
02434 SET_AXIS_PHL_FLAG(axis, 0);
02435 emcmotDebug->maxLimitSwitchCount[axis] = 0;
02436 }
02437 }
02438 if (GET_AXIS_ACTIVE_FLAG(axis)) {
02439 extMinLimitSwitchRead(axis, &isLimit);
02440 if (isLimit == GET_AXIS_NHL_POLARITY(axis)) {
02441 if (++emcmotDebug->minLimitSwitchCount[axis] > LIMIT_SWITCH_DEBOUNCE) {
02442 SET_AXIS_NHL_FLAG(axis, 1);
02443 emcmotDebug->minLimitSwitchCount[axis] = LIMIT_SWITCH_DEBOUNCE;
02444 if (emcmotStatus->overrideLimits ||
02445 isHoming()) {
02446 }
02447 else {
02448 SET_AXIS_ERROR_FLAG(axis, 1);
02449 emcmotDebug->enabling = 0;
02450 }
02451 }
02452 }
02453 else {
02454 SET_AXIS_NHL_FLAG(axis, 0);
02455 emcmotDebug->minLimitSwitchCount[axis] = 0;
02456 }
02457 }
02458
02459 if (GET_AXIS_ACTIVE_FLAG(axis) &&
02460 GET_AXIS_ENABLE_FLAG(axis)) {
02461 extAmpFault(axis, &fault);
02462 if (fault == GET_AXIS_FAULT_POLARITY(axis)) {
02463 if (++emcmotDebug->ampFaultCount[axis] > AMP_FAULT_DEBOUNCE) {
02464 SET_AXIS_ERROR_FLAG(axis, 1);
02465 SET_AXIS_FAULT_FLAG(axis, 1);
02466 emcmotDebug->ampFaultCount[axis] = AMP_FAULT_DEBOUNCE;
02467 emcmotDebug->enabling = 0;
02468 }
02469 }
02470 else {
02471 SET_AXIS_FAULT_FLAG(axis, 0);
02472 emcmotDebug->ampFaultCount[axis] = 0;
02473 }
02474 }
02475
02476
02477
02478 if (GET_AXIS_ACTIVE_FLAG(axis)) {
02479 extHomeSwitchRead(axis, &homeFlag);
02480 if (homeFlag == GET_AXIS_HOME_SWITCH_POLARITY(axis)) {
02481 SET_AXIS_HOME_SWITCH_FLAG(axis, 1);
02482 }
02483 else {
02484 SET_AXIS_HOME_SWITCH_FLAG(axis, 0);
02485 }
02486 }
02487
02488 }
02489
02490
02491
02492 if (emcmotStatus->logOpen &&
02493 ! emcmotStatus->logStarted &&
02494 loggingAxis >= 0 &&
02495 loggingAxis < EMCMOT_MAX_AXIS) {
02496 double val = 0.0;
02497
02498 switch (emcmotStatus->logTriggerVariable) {
02499 case EMCLOG_TRIGGER_ON_FERROR:
02500 val = emcmotDebug->ferrorCurrent[loggingAxis];
02501 break;
02502
02503 case EMCLOG_TRIGGER_ON_VOLT:
02504 val = emcmotDebug->rawOutput[loggingAxis];
02505 break;
02506
02507 case EMCLOG_TRIGGER_ON_POS:
02508 val = emcmotDebug->jointPos[loggingAxis];
02509 break;
02510
02511 case EMCLOG_TRIGGER_ON_VEL:
02512 val = emcmotDebug->jointPos[loggingAxis] - emcmotDebug->oldJointPos[loggingAxis];
02513 break;
02514
02515 default:
02516 break;
02517 }
02518
02519 switch (emcmotStatus->logTriggerType) {
02520 case EMCLOG_MANUAL_TRIGGER:
02521 break;
02522
02523 case EMCLOG_DELTA_TRIGGER:
02524 if (emcmotStatus->logStartVal - val < -emcmotStatus->logTriggerThreshold ||
02525 emcmotStatus->logStartVal - val > emcmotStatus->logTriggerThreshold) {
02526 emcmotStatus->logStarted = 1;
02527 }
02528 break;
02529
02530 case EMCLOG_OVER_TRIGGER:
02531 if (val > emcmotStatus->logTriggerThreshold) {
02532 emcmotStatus->logStarted = 1;
02533 }
02534 break;
02535
02536 case EMCLOG_UNDER_TRIGGER:
02537 if (val < emcmotStatus->logTriggerThreshold) {
02538 emcmotStatus->logStarted = 1;
02539 }
02540 }
02541 }
02542
02543
02544
02545
02546
02547
02548
02549
02550
02551 if (! emcmotDebug->enabling && GET_MOTION_ENABLE_FLAG()) {
02552
02553 tpClear(&emcmotDebug->queue);
02554 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02555 tpClear(&emcmotDebug->freeAxis[axis]);
02556 cubicDrain(&emcmotDebug->cubic[axis]);
02557 if (GET_AXIS_ACTIVE_FLAG(axis)) {
02558 extAmpEnable(axis, ! GET_AXIS_ENABLE_POLARITY(axis));
02559 SET_AXIS_ENABLE_FLAG(axis, 0);
02560 SET_AXIS_HOMING_FLAG(axis, 0);
02561 emcmotStatus->output[axis] = 0.0;
02562 }
02563
02564
02565 }
02566
02567
02568
02569
02570 interpolationCounter = 0;
02571 SET_MOTION_ENABLE_FLAG(0);
02572
02573
02574 }
02575
02576
02577 if (emcmotDebug->enabling && ! GET_MOTION_ENABLE_FLAG()) {
02578 tpSetPos(&emcmotDebug->queue, emcmotStatus->pos);
02579 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02580 emcmotDebug->freePose.tran.x = emcmotDebug->jointPos[axis];
02581 tpSetPos(&emcmotDebug->freeAxis[axis], emcmotDebug->freePose);
02582 pidReset(&emcmotConfig->pid[axis]);
02583 if (GET_AXIS_ACTIVE_FLAG(axis)) {
02584 extAmpEnable(axis, GET_AXIS_ENABLE_POLARITY(axis));
02585 SET_AXIS_ENABLE_FLAG(axis, 1);
02586 SET_AXIS_HOMING_FLAG(axis, 0);
02587 }
02588
02589 SET_AXIS_ERROR_FLAG(axis, 0);
02590 }
02591 SET_MOTION_ENABLE_FLAG(1);
02592
02593 SET_MOTION_ERROR_FLAG(0);
02594 }
02595
02596
02597 if (emcmotDebug->teleoperating && ! GET_MOTION_TELEOP_FLAG()) {
02598 if (GET_MOTION_INPOS_FLAG()) {
02599
02600
02601 tpSetPos(&emcmotDebug->queue, emcmotStatus->pos);
02602
02603 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02604 cubicDrain(&emcmotDebug->cubic[axis]);
02605 }
02606
02607 emcmotDebug->teleop_data.currentVel.tran.x = 0.0;
02608 emcmotDebug->teleop_data.currentVel.tran.y = 0.0;
02609 emcmotDebug->teleop_data.currentVel.tran.z = 0.0;
02610 emcmotDebug->teleop_data.currentVel.a = 0.0;
02611 emcmotDebug->teleop_data.currentVel.b = 0.0;
02612 emcmotDebug->teleop_data.currentVel.c = 0.0;
02613 emcmotDebug->teleop_data.desiredVel.tran.x = 0.0;
02614 emcmotDebug->teleop_data.desiredVel.tran.y = 0.0;
02615 emcmotDebug->teleop_data.desiredVel.tran.z = 0.0;
02616 emcmotDebug->teleop_data.desiredVel.a = 0.0;
02617 emcmotDebug->teleop_data.desiredVel.b = 0.0;
02618 emcmotDebug->teleop_data.desiredVel.c = 0.0;
02619 emcmotDebug->teleop_data.currentAccell.tran.x = 0.0;
02620 emcmotDebug->teleop_data.currentAccell.tran.y = 0.0;
02621 emcmotDebug->teleop_data.currentAccell.tran.z = 0.0;
02622 emcmotDebug->teleop_data.currentAccell.a = 0.0;
02623 emcmotDebug->teleop_data.currentAccell.b = 0.0;
02624 emcmotDebug->teleop_data.currentAccell.c = 0.0;
02625 emcmotDebug->teleop_data.desiredAccell.tran.x = 0.0;
02626 emcmotDebug->teleop_data.desiredAccell.tran.y = 0.0;
02627 emcmotDebug->teleop_data.desiredAccell.tran.z = 0.0;
02628 emcmotDebug->teleop_data.desiredAccell.a = 0.0;
02629 emcmotDebug->teleop_data.desiredAccell.b = 0.0;
02630 emcmotDebug->teleop_data.desiredAccell.c = 0.0;
02631 SET_MOTION_TELEOP_FLAG(1);
02632 SET_MOTION_ERROR_FLAG(0);
02633 }
02634 else {
02635
02636 emcmotDebug->teleoperating = 0;
02637 }
02638 }
02639 else {
02640 if (GET_MOTION_INPOS_FLAG()) {
02641 if (!emcmotDebug->teleoperating && GET_MOTION_TELEOP_FLAG()) {
02642 SET_MOTION_TELEOP_FLAG(0);
02643 if (! emcmotDebug->coordinating) {
02644 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02645
02646 emcmotDebug->freePose.tran.x = emcmotDebug->jointPos[axis];
02647 tpSetPos(&emcmotDebug->freeAxis[axis], emcmotDebug->freePose);
02648
02649 cubicDrain(&emcmotDebug->cubic[axis]);
02650 }
02651 }
02652 }
02653 }
02654
02655
02656 if (emcmotDebug->coordinating && ! GET_MOTION_COORD_FLAG()) {
02657 if (GET_MOTION_INPOS_FLAG()) {
02658
02659 tpSetPos(&emcmotDebug->queue, emcmotStatus->pos);
02660
02661 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02662 cubicDrain(&emcmotDebug->cubic[axis]);
02663 }
02664
02665 emcmotDebug->overriding = 0;
02666 emcmotStatus->overrideLimits = 0;
02667 SET_MOTION_COORD_FLAG(1);
02668 SET_MOTION_TELEOP_FLAG(0);
02669 SET_MOTION_ERROR_FLAG(0);
02670 }
02671 else {
02672
02673 emcmotDebug->coordinating = 0;
02674 }
02675 }
02676
02677
02678 if (! emcmotDebug->coordinating && GET_MOTION_COORD_FLAG()) {
02679 if (GET_MOTION_INPOS_FLAG()) {
02680 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02681
02682 emcmotDebug->freePose.tran.x = emcmotDebug->jointPos[axis];
02683 tpSetPos(&emcmotDebug->freeAxis[axis], emcmotDebug->freePose);
02684
02685 cubicDrain(&emcmotDebug->cubic[axis]);
02686 }
02687 SET_MOTION_COORD_FLAG(0);
02688 SET_MOTION_TELEOP_FLAG(0);
02689 SET_MOTION_ERROR_FLAG(0);
02690 }
02691 else {
02692
02693 emcmotDebug->coordinating = 1;
02694 }
02695 }
02696
02697 }
02698
02699
02700 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02701 if (GET_AXIS_HOMING_FLAG(axis)) {
02702 if (tpIsDone(&emcmotDebug->freeAxis[axis])) {
02703
02704 if (emcmotDebug->homingPhase[axis] == 5) {
02705
02706 emcmotDebug->homingPhase[axis] = 0;
02707
02708
02709
02710
02711
02712
02713 kinematicsHome(&worldHome, emcmotDebug->jointHome, &fflags, &iflags);
02714
02715
02716
02717 rehomeAll = 0;
02718
02719
02720
02721
02722
02723
02724
02725 emcmotStatus->inputOffset[axis] = emcmotDebug->rawInput[axis] -
02726 emcmotDebug->jointHome[axis] * emcmotStatus->inputScale[axis];
02727
02728
02729
02730 emcmotStatus->input[axis] = emcmotDebug->jointHome[axis];
02731 emcmotDebug->oldInput[axis] = emcmotStatus->input[axis];
02732
02733
02734 cubicOffset(&emcmotDebug->cubic[axis], emcmotDebug->jointHome[axis] - emcmotDebug->coarseJointPos[axis]);
02735
02736
02737 emcmotDebug->jointPos[axis] = emcmotDebug->jointHome[axis];
02738 emcmotDebug->freePose.tran.x = emcmotDebug->jointHome[axis];
02739 tpSetPos(&emcmotDebug->freeAxis[axis], emcmotDebug->freePose);
02740
02741 SET_AXIS_HOMING_FLAG(axis, 0);
02742 SET_AXIS_HOMED_FLAG(axis, 1);
02743
02744
02745
02746 emcmotDebug->allHomed = 1;
02747 for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
02748 if (GET_AXIS_ACTIVE_FLAG(t) &&
02749 ! GET_AXIS_HOMED_FLAG(t)) {
02750
02751 emcmotDebug->allHomed = 0;
02752 break;
02753 }
02754 }
02755
02756 if (emcmotDebug->allHomed) {
02757 emcmotStatus->pos = worldHome;
02758 emcmotStatus->actualPos = worldHome;
02759 }
02760 }
02761
02762 if (emcmotDebug->homingPhase[axis] == 4) {
02763
02764
02765 emcmotDebug->freePose.tran.x =
02766 (emcmotDebug->saveLatch[axis] -
02767 emcmotStatus->inputOffset[axis]) *
02768 emcmotDebug->inverseInputScale[axis] +
02769 emcmotConfig->homeOffset[axis];
02770
02771
02772
02773
02774
02775
02776 tpSetVmax(&emcmotDebug->freeAxis[axis],
02777 2 * (emcmotConfig->homingVel[axis]));
02778 if ((retval = tpAddLine(&emcmotDebug->freeAxis[axis], emcmotDebug->freePose)) == 0) {
02779
02780 emcmotDebug->homingPhase[axis] = 5;
02781 }
02782 }
02783 }
02784 }
02785 }
02786
02787
02788
02789
02790
02791
02792
02793 whichCycle = 0;
02794 if (GET_MOTION_ENABLE_FLAG()) {
02795
02796 whichCycle = 1;
02797
02798
02799 while (cubicNeedNextPoint(&emcmotDebug->cubic[0])) {
02800
02801
02802
02803
02804
02805 if (GET_MOTION_TELEOP_FLAG()) {
02806 double accell_mag;
02807
02808 emcmotDebug->teleop_data.desiredAccell.tran.x = (emcmotDebug->teleop_data.desiredVel.tran.x - emcmotDebug->teleop_data.currentVel.tran.x)/emcmotConfig->trajCycleTime;
02809 emcmotDebug->teleop_data.desiredAccell.tran.y = (emcmotDebug->teleop_data.desiredVel.tran.y - emcmotDebug->teleop_data.currentVel.tran.y)/emcmotConfig->trajCycleTime;
02810 emcmotDebug->teleop_data.desiredAccell.tran.z = (emcmotDebug->teleop_data.desiredVel.tran.z - emcmotDebug->teleop_data.currentVel.tran.z)/emcmotConfig->trajCycleTime;
02811 pmCartMag(emcmotDebug->teleop_data.desiredAccell.tran, &accell_mag);
02812 emcmotDebug->teleop_data.desiredAccell.a = (emcmotDebug->teleop_data.desiredVel.a - emcmotDebug->teleop_data.currentVel.a)/emcmotConfig->trajCycleTime;
02813 emcmotDebug->teleop_data.desiredAccell.b = (emcmotDebug->teleop_data.desiredVel.b - emcmotDebug->teleop_data.currentVel.b)/emcmotConfig->trajCycleTime;
02814 emcmotDebug->teleop_data.desiredAccell.c = (emcmotDebug->teleop_data.desiredVel.c - emcmotDebug->teleop_data.currentVel.c)/emcmotConfig->trajCycleTime;
02815 if (emcmotDebug->teleop_data.desiredAccell.a > accell_mag) {
02816 accell_mag = emcmotDebug->teleop_data.desiredAccell.a;
02817 }
02818 if (emcmotDebug->teleop_data.desiredAccell.b > accell_mag) {
02819 accell_mag = emcmotDebug->teleop_data.desiredAccell.b;
02820 }
02821 if (emcmotDebug->teleop_data.desiredAccell.c > accell_mag) {
02822 accell_mag = emcmotDebug->teleop_data.desiredAccell.c;
02823 }
02824 if (accell_mag > emcmotStatus->acc) {
02825 pmCartScalMult(emcmotDebug->teleop_data.desiredAccell.tran, emcmotStatus->acc/accell_mag,
02826 &emcmotDebug->teleop_data.currentAccell.tran);
02827 emcmotDebug->teleop_data.currentAccell.a =
02828 emcmotDebug->teleop_data.desiredAccell.a * emcmotStatus->acc/accell_mag;
02829 emcmotDebug->teleop_data.currentAccell.b =
02830 emcmotDebug->teleop_data.desiredAccell.b * emcmotStatus->acc/accell_mag;
02831 emcmotDebug->teleop_data.currentAccell.c =
02832 emcmotDebug->teleop_data.desiredAccell.c * emcmotStatus->acc/accell_mag;
02833 emcmotDebug->teleop_data.currentVel.tran.x += emcmotDebug->teleop_data.currentAccell.tran.x*emcmotConfig->trajCycleTime;
02834 emcmotDebug->teleop_data.currentVel.tran.y += emcmotDebug->teleop_data.currentAccell.tran.y*emcmotConfig->trajCycleTime;
02835 emcmotDebug->teleop_data.currentVel.tran.z += emcmotDebug->teleop_data.currentAccell.tran.z*emcmotConfig->trajCycleTime;
02836 emcmotDebug->teleop_data.currentVel.a += emcmotDebug->teleop_data.currentAccell.a*emcmotConfig->trajCycleTime;
02837 emcmotDebug->teleop_data.currentVel.b += emcmotDebug->teleop_data.currentAccell.b*emcmotConfig->trajCycleTime;
02838 emcmotDebug->teleop_data.currentVel.c += emcmotDebug->teleop_data.currentAccell.c*emcmotConfig->trajCycleTime;
02839 }
02840 else {
02841 emcmotDebug->teleop_data.currentAccell = emcmotDebug->teleop_data.desiredAccell;
02842 emcmotDebug->teleop_data.currentVel = emcmotDebug->teleop_data.desiredVel;
02843 }
02844
02845 emcmotStatus->pos.tran.x += emcmotDebug->teleop_data.currentVel.tran.x*emcmotConfig->trajCycleTime;
02846 emcmotStatus->pos.tran.y += emcmotDebug->teleop_data.currentVel.tran.y*emcmotConfig->trajCycleTime;
02847 emcmotStatus->pos.tran.z += emcmotDebug->teleop_data.currentVel.tran.z*emcmotConfig->trajCycleTime;
02848 emcmotStatus->pos.a += emcmotDebug->teleop_data.currentVel.a*emcmotConfig->trajCycleTime;
02849 emcmotStatus->pos.b += emcmotDebug->teleop_data.currentVel.b*emcmotConfig->trajCycleTime;
02850 emcmotStatus->pos.c += emcmotDebug->teleop_data.currentVel.c*emcmotConfig->trajCycleTime;
02851
02852
02853 kinematicsInverse(&emcmotStatus->pos, emcmotDebug->coarseJointPos, &iflags, &fflags);
02854
02855
02856
02857
02858 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02859 cubicAddPoint(&emcmotDebug->cubic[axis], emcmotDebug->coarseJointPos[axis]);
02860 }
02861
02862 if (kinType == KINEMATICS_IDENTITY) {
02863
02864
02865 kinematicsForward(emcmotStatus->input, &emcmotStatus->actualPos, &fflags, &iflags);
02866 }
02867 else {
02868
02869 emcmotStatus->actualPos = emcmotStatus->pos;
02870 }
02871 }
02872 else {
02873 if (GET_MOTION_COORD_FLAG()) {
02874
02875
02876
02877
02878
02879
02880
02881 whichCycle = 2;
02882
02883
02884 tpRunCycle(&emcmotDebug->queue);
02885
02886
02887 emcmotStatus->pos = tpGetPos(&emcmotDebug->queue);
02888
02889
02890 kinematicsInverse(&emcmotStatus->pos, emcmotDebug->coarseJointPos, &iflags, &fflags);
02891
02892
02893
02894
02895 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02896 cubicAddPoint(&emcmotDebug->cubic[axis], emcmotDebug->coarseJointPos[axis]);
02897 }
02898
02899 if (kinType == KINEMATICS_IDENTITY) {
02900
02901
02902 kinematicsForward(emcmotStatus->input, &emcmotStatus->actualPos, &fflags, &iflags);
02903 }
02904 else {
02905
02906 emcmotStatus->actualPos = emcmotStatus->pos;
02907 }
02908
02909
02910
02911
02912 }
02913 else {
02914
02915 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02916
02917
02918
02919
02920 whichCycle = 2;
02921
02922
02923 tpRunCycle(&emcmotDebug->freeAxis[axis]);
02924
02925
02926
02927
02928 emcmotDebug->coarseJointPos[axis] = tpGetPos(&emcmotDebug->freeAxis[axis]).tran.x;
02929
02930
02931
02932
02933 cubicAddPoint(&emcmotDebug->cubic[axis], emcmotDebug->coarseJointPos[axis]);
02934 }
02935
02936 if (kinType == KINEMATICS_IDENTITY) {
02937
02938 kinematicsForward(emcmotStatus->input, &emcmotStatus->actualPos, &fflags, &iflags);
02939
02940 kinematicsForward(emcmotDebug->coarseJointPos, &emcmotStatus->pos, &fflags, &iflags);
02941 }
02942 else if (kinType != KINEMATICS_INVERSE_ONLY) {
02943
02944
02945
02946
02947
02948
02949
02950 EmcPose temp = emcmotStatus->pos;
02951 if (0 == kinematicsForward(emcmotStatus->input, &temp, &fflags, &iflags)) {
02952 emcmotStatus->pos = temp;
02953 emcmotStatus->actualPos = temp;
02954 }
02955
02956 }
02957 else {
02958
02959
02960 }
02961
02962
02963
02964
02965 }
02966 }
02967 }
02968
02969
02970
02971
02972
02973
02974
02975
02976
02977
02978
02979
02980
02981
02982
02983
02984
02985
02986
02987
02988
02989
02990
02991
02992
02993
02994
02995 emcmotDebug->onLimit = 0;
02996 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02997 SET_AXIS_PSL_FLAG(axis, 0);
02998 SET_AXIS_NSL_FLAG(axis, 0);
02999 if (GET_AXIS_HOMED_FLAG(axis)) {
03000 if (emcmotDebug->coarseJointPos[axis] > emcmotConfig->maxLimit[axis]) {
03001 SET_AXIS_ERROR_FLAG(axis, 1);
03002 SET_AXIS_PSL_FLAG(axis, 1);
03003 emcmotDebug->onLimit = 1;
03004 }
03005 else if (emcmotDebug->coarseJointPos[axis] < emcmotConfig->minLimit[axis]) {
03006 SET_AXIS_ERROR_FLAG(axis, 1);
03007 SET_AXIS_NSL_FLAG(axis, 1);
03008 }
03009 }
03010 }
03011
03012
03013
03014
03015
03016
03017 if (emcmotDebug->onLimit) {
03018 if (! emcmotDebug->wasOnLimit) {
03019
03020 tpAbort(&emcmotDebug->queue);
03021 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
03022 tpAbort(&emcmotDebug->freeAxis[axis]);
03023 emcmotDebug->wasOnLimit = 1;
03024 }
03025 }
03026
03027 }
03028 else {
03029
03030
03031 emcmotDebug->wasOnLimit = 0;
03032 }
03033
03034 if (whichCycle == 2) {
03035
03036
03037 logIt = 0;
03038
03039 if (emcmotStatus->logStarted &&
03040 emcmotStatus->logSkip >= 0) {
03041
03042
03043 emcmotDebug->newVel.tran.x = (emcmotStatus->pos.tran.x - emcmotDebug->oldPos.tran.x) /
03044 emcmotConfig->trajCycleTime;
03045 emcmotDebug->newVel.tran.y = (emcmotStatus->pos.tran.y - emcmotDebug->oldPos.tran.y) /
03046 emcmotConfig->trajCycleTime;
03047 emcmotDebug->newVel.tran.z = (emcmotStatus->pos.tran.z - emcmotDebug->oldPos.tran.z) /
03048 emcmotConfig->trajCycleTime;
03049 emcmotDebug->oldPos = emcmotStatus->pos;
03050 emcmotDebug->newAcc.tran.x = (emcmotDebug->newVel.tran.x - emcmotDebug->oldVel.tran.x) /
03051 emcmotConfig->trajCycleTime;
03052 emcmotDebug->newAcc.tran.y = (emcmotDebug->newVel.tran.y - emcmotDebug->oldVel.tran.y) /
03053 emcmotConfig->trajCycleTime;
03054 emcmotDebug->newAcc.tran.z = (emcmotDebug->newVel.tran.z - emcmotDebug->oldVel.tran.z) /
03055 emcmotConfig->trajCycleTime;
03056 emcmotDebug->oldVel = emcmotDebug->newVel;
03057
03058
03059 ls.type = emcmotStatus->logType;
03060
03061
03062 switch (emcmotStatus->logType) {
03063
03064 case EMCMOT_LOG_TYPE_TRAJ_POS:
03065 if (logSkip-- <= 0) {
03066 ls.item.trajPos.time = start - logStartTime;
03067 ls.item.trajPos.pos = emcmotStatus->pos.tran;
03068 logSkip = emcmotStatus->logSkip;
03069 logIt = 1;
03070 }
03071 break;
03072
03073 case EMCMOT_LOG_TYPE_TRAJ_VEL:
03074 if (logSkip-- <= 0) {
03075 ls.item.trajVel.time = start - logStartTime;
03076 ls.item.trajVel.vel = emcmotDebug->newVel.tran;
03077 pmCartMag(emcmotDebug->newVel.tran, &ls.item.trajVel.mag);
03078 logSkip = emcmotStatus->logSkip;
03079 logIt = 1;
03080 }
03081 break;
03082
03083 case EMCMOT_LOG_TYPE_TRAJ_ACC:
03084 if (logSkip-- <= 0) {
03085 ls.item.trajAcc.time = start - logStartTime;
03086 ls.item.trajAcc.acc = emcmotDebug->newAcc.tran;
03087 pmCartMag(emcmotDebug->newAcc.tran, &ls.item.trajAcc.mag);
03088 logSkip = emcmotStatus->logSkip;
03089 logIt = 1;
03090 }
03091 break;
03092
03093 default:
03094 break;
03095 }
03096
03097
03098 if (logIt) {
03099 emcmotLogAdd(emcmotLog, ls);
03100 emcmotStatus->logPoints = emcmotLog->howmany;
03101 logIt = 0;
03102 }
03103 }
03104
03105 }
03106
03107
03108 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
03109
03110 emcmotDebug->oldJointPos[axis] = emcmotDebug->jointPos[axis];
03111 emcmotDebug->jointPos[axis] = cubicInterpolate(&emcmotDebug->cubic[axis], 0, 0, 0, 0);
03112 emcmotDebug->jointVel[axis] = (emcmotDebug->jointPos[axis] - emcmotDebug->oldJointPos[axis]) / emcmotConfig->servoCycleTime;
03113 #ifdef COMPING
03114
03115
03116 if (emcmotDebug->jointVel[axis] > 0.0 && dir[axis] < 0) {
03117 dir[axis] = 1;
03118 }
03119 else if (emcmotDebug->jointVel[axis] < 0.0 && dir[axis] > 0) {
03120 dir[axis] = -1;
03121 }
03122
03123 #endif
03124
03125
03126 if (GET_AXIS_ACTIVE_FLAG(axis)) {
03127
03128
03129
03130 if (emcmotDebug->jointPos[axis] - emcmotDebug->oldJointPos[axis] > 0.0) {
03131 oldbcomp = emcmotDebug->bcomp[axis];
03132 emcmotDebug->bcomp[axis] = + emcmotConfig->pid[axis].backlash / 2.0;
03133 if (emcmotDebug->bcompdir[axis] != + 1) {
03134 emcmotDebug->bac_done[axis] = 0;
03135 emcmotDebug->bac_di[axis] = emcmotDebug->bcompincr[axis];
03136 emcmotDebug->bac_d[axis] = 0;
03137 emcmotDebug->bac_D[axis] = emcmotDebug->bcomp[axis] - oldbcomp;
03138 emcmotDebug->bac_halfD[axis] = 0.5 * emcmotDebug->bac_D[axis];
03139 emcmotDebug->bac_incrincr[axis] = emcmotStatus->acc *
03140 emcmotConfig->servoCycleTime * emcmotConfig->servoCycleTime;
03141 emcmotDebug->bac_incr[axis] = - 0.5 * emcmotDebug->bac_incrincr[axis];
03142 emcmotDebug->bcompdir[axis] = + 1;
03143 }
03144 }
03145 else if (emcmotDebug->jointPos[axis] - emcmotDebug->oldJointPos[axis] < 0.0) {
03146 oldbcomp = emcmotDebug->bcomp[axis];
03147 emcmotDebug->bcomp[axis] = - emcmotConfig->pid[axis].backlash / 2.0;
03148 if (emcmotDebug->bcompdir[axis] != - 1) {
03149 emcmotDebug->bac_done[axis] = 0;
03150 emcmotDebug->bac_di[axis] = emcmotDebug->bcompincr[axis];
03151 emcmotDebug->bac_d[axis] = 0;
03152 emcmotDebug->bac_D[axis] = emcmotDebug->bcomp[axis] - oldbcomp;
03153 emcmotDebug->bac_halfD[axis] = 0.5 * emcmotDebug->bac_D[axis];
03154 emcmotDebug->bac_incrincr[axis] = emcmotStatus->acc *
03155 emcmotConfig->servoCycleTime * emcmotConfig->servoCycleTime;
03156 emcmotDebug->bac_incr[axis] = - 0.5 * emcmotDebug->bac_incrincr[axis];
03157 emcmotDebug->bcompdir[axis] = - 1;
03158 }
03159 }
03160
03161
03162
03163 if (! emcmotDebug->bac_done[axis]) {
03164 if (emcmotDebug->bac_D[axis] > 0.0) {
03165 emcmotDebug->bcompincr[axis] = emcmotDebug->bac_di[axis] + emcmotDebug->bac_d[axis];
03166 if (emcmotDebug->bac_d[axis] < emcmotDebug->bac_halfD[axis]) {
03167 emcmotDebug->bac_incr[axis] += emcmotDebug->bac_incrincr[axis];
03168 emcmotDebug->bac_d[axis] += emcmotDebug->bac_incr[axis];
03169 }
03170 else if (emcmotDebug->bac_d[axis] < emcmotDebug->bac_D[axis]) {
03171 emcmotDebug->bac_incr[axis] -= emcmotDebug->bac_incrincr[axis];
03172 emcmotDebug->bac_d[axis] += emcmotDebug->bac_incr[axis];
03173 }
03174 if (emcmotDebug->bac_d[axis] >= emcmotDebug->bac_D[axis] || emcmotDebug->bac_incr[axis] <= 0.0) {
03175 emcmotDebug->bac_done[axis] = 1;
03176 }
03177 }
03178 else {
03179 emcmotDebug->bcompincr[axis] = emcmotDebug->bac_di[axis] + emcmotDebug->bac_d[axis];
03180 if (emcmotDebug->bac_d[axis] > emcmotDebug->bac_halfD[axis]) {
03181 emcmotDebug->bac_incr[axis] += emcmotDebug->bac_incrincr[axis];
03182 emcmotDebug->bac_d[axis] -= emcmotDebug->bac_incr[axis];
03183 }
03184 else if (emcmotDebug->bac_d[axis] > emcmotDebug->bac_D[axis]) {
03185 emcmotDebug->bac_incr[axis] -= emcmotDebug->bac_incrincr[axis];
03186 emcmotDebug->bac_d[axis] -= emcmotDebug->bac_incr[axis];
03187 }
03188 if (emcmotDebug->bac_d[axis] <= emcmotDebug->bac_D[axis] || emcmotDebug->bac_incr[axis] <= 0.0) {
03189 emcmotDebug->bac_done[axis] = 1;
03190 }
03191 }
03192 }
03193
03194
03195
03196
03197
03198
03199
03200 emcmotDebug->outJointPos[axis] = emcmotDebug->jointPos[axis] + emcmotDebug->bcompincr[axis];
03201 #ifndef NO_ROUNDING
03202 numres = outJointPos[axis] * emcmotStatus->inputScale[axis];
03203 if (numres < 0.0) {
03204 outJointPos[axis] = emcmotDebug->inverseInputScale[axis] * ((int) (numres - 0.5));
03205 }
03206 else {
03207 outJointPos[axis] = emcmotDebug->inverseInputScale[axis] * ((int) (numres + 0.5));
03208 }
03209 #endif
03210
03211
03212
03213
03214
03215
03216
03217
03218
03219
03220
03221
03222
03223
03224
03225
03226
03227
03228
03229 emcmotStatus->output[axis] =
03230 pidRunCycle(&emcmotConfig->pid[axis],
03231 emcmotStatus->input[axis] + emcmotDebug->bcompincr[axis],
03232 emcmotDebug->outJointPos[axis]);
03233
03234
03235
03236
03237 thisFerror[axis] = emcmotDebug->jointPos[axis] - emcmotStatus->input[axis];
03238 magFerror = fabs(thisFerror[axis]);
03239 emcmotDebug->ferrorCurrent[axis] = thisFerror[axis];
03240
03241
03242 if (emcmotDebug->ferrorHighMark[axis] < magFerror) {
03243 emcmotDebug->ferrorHighMark[axis] = magFerror;
03244 }
03245
03246
03247 limitFerror = emcmotConfig->maxFerror[axis] /
03248 emcmotConfig->limitVel *
03249 emcmotDebug->jointVel[axis];
03250 if (limitFerror < emcmotConfig->minFerror[axis]) {
03251 limitFerror = emcmotConfig->minFerror[axis];
03252 }
03253
03254
03255 if (magFerror > limitFerror) {
03256
03257 SET_AXIS_ERROR_FLAG(axis, 1);
03258 SET_AXIS_FERROR_FLAG(axis, 1);
03259 if (emcmotDebug->enabling) {
03260
03261 reportError("axis %d following error", axis);
03262 }
03263 emcmotDebug->enabling = 0;
03264 }
03265 else {
03266 SET_AXIS_FERROR_FLAG(axis, 0);
03267 }
03268 }
03269 else {
03270
03271
03272 }
03273
03274
03275
03276
03277
03278
03279
03280
03281 if (emcmotStatus->output[axis] < emcmotConfig->minOutput[axis]) {
03282 emcmotStatus->output[axis] = emcmotConfig->minOutput[axis];
03283 }
03284 else if (emcmotStatus->output[axis] > emcmotConfig->maxOutput[axis]) {
03285 emcmotStatus->output[axis] = emcmotConfig->maxOutput[axis];
03286 }
03287
03288
03289
03290
03291
03292
03293
03294
03295
03296
03297
03298
03299
03300
03301
03302 if (emcmotDebug->homingPhase[axis] == 3) {
03303
03304 extEncoderReadLatch(axis, &emcmotDebug->latchFlag[axis]);
03305 if (emcmotDebug->latchFlag[axis]) {
03306
03307
03308
03309 if (tpAbort(&emcmotDebug->freeAxis[axis]) == 0) {
03310
03311 emcmotDebug->homingPhase[axis] = 4;
03312
03313 emcmotDebug->saveLatch[axis] = emcmotDebug->rawInput[axis];
03314 }
03315 }
03316 }
03317
03318
03319 if (emcmotDebug->homingPhase[axis] == 2) {
03320 if (GET_AXIS_HOMING_POLARITY(axis)) {
03321 emcmotDebug->freePose.tran.x = - 2.0 * AXRANGE(axis);
03322 } else {
03323 emcmotDebug->freePose.tran.x = + 2.0 * AXRANGE(axis);
03324 }
03325 if ((retval = tpAddLine(&emcmotDebug->freeAxis[axis], emcmotDebug->freePose)) == 0) {
03326
03327
03328 extEncoderResetIndex(axis);
03329 emcmotDebug->homingPhase[axis] = 3;
03330 }
03331 }
03332
03333
03334
03335 if (emcmotDebug->homingPhase[axis] == 1) {
03336 if (GET_AXIS_HOME_SWITCH_FLAG(axis) ||
03337 GET_AXIS_PHL_FLAG(axis) ||
03338 GET_AXIS_NHL_FLAG(axis)) {
03339 if (tpAbort(&emcmotDebug->freeAxis[axis]) == 0) {
03340
03341 emcmotDebug->homingPhase[axis] = 2;
03342 }
03343 }
03344 }
03345 }
03346 }
03347 else {
03348
03349
03350
03351
03352
03353
03354
03355
03356
03357
03358
03359
03360 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
03361 emcmotDebug->coarseJointPos[axis] = emcmotStatus->input[axis];
03362 emcmotDebug->oldJointPos[axis] = emcmotDebug->jointPos[axis];
03363 emcmotDebug->jointPos[axis] = emcmotDebug->coarseJointPos[axis];
03364 emcmotDebug->jointVel[axis] = (emcmotDebug->jointPos[axis] - emcmotDebug->oldJointPos[axis]) / emcmotConfig->servoCycleTime;
03365 }
03366
03367
03368
03369
03370 if (--interpolationCounter <= 0) {
03371 if (kinType != KINEMATICS_INVERSE_ONLY) {
03372
03373 EmcPose temp = emcmotStatus->pos;
03374 if (0 == kinematicsForward(emcmotStatus->input, &temp, &fflags, &iflags)) {
03375 emcmotStatus->pos = temp;
03376 emcmotStatus->actualPos = temp;
03377 }
03378 }
03379
03380
03381
03382
03383 interpolationCounter = emcmotConfig->interpolationRate;
03384 }
03385 }
03386
03387 #ifdef ENABLE_PROBING
03388 extDioRead(emcmotConfig->probeIndex, &emcmotStatus->probeval);
03389 if (emcmotStatus->probing && emcmotStatus->probeTripped) {
03390 tpClear(&emcmotDebug->queue);
03391 emcmotStatus->probing = 0;
03392 }
03393 else if (emcmotStatus->probing && GET_MOTION_INPOS_FLAG() &&
03394 tpQueueDepth(&emcmotDebug->queue) == 0) {
03395 emcmotStatus->probing = 0;
03396 }
03397 else if (emcmotStatus->probing) {
03398 if (emcmotStatus->probeval == emcmotConfig->probePolarity) {
03399 emcmotStatus->probeTripped = 1;
03400 emcmotStatus->probedPos = emcmotStatus->actualPos;
03401 if (GET_MOTION_COORD_FLAG()) {
03402 tpAbort(&emcmotDebug->queue);
03403 SET_MOTION_ERROR_FLAG(0);
03404 }
03405 else {
03406
03407 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
03408 tpAbort(&emcmotDebug->freeAxis[axis]);
03409 SET_AXIS_HOMING_FLAG(axis, 0);
03410 SET_AXIS_ERROR_FLAG(axis, 0);
03411 }
03412 }
03413 }
03414 }
03415 #endif
03416
03417
03418
03419 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
03420 #ifndef STEPPER_MOTORS
03421 emcmotDebug->rawOutput[axis] =
03422 (emcmotStatus->output[axis] - emcmotStatus->outputOffset[axis]) *
03423 emcmotDebug->inverseOutputScale[axis];
03424 #else
03425
03426
03427
03428
03429
03430 emcmotDebug->rawOutput[axis] = emcmotStatus->output[axis];
03431 if (emcmotDebug->rawOutput[axis] < +OUTPUT_DELTA &&
03432 emcmotDebug->rawOutput[axis] > -OUTPUT_DELTA) {
03433 emcmotDebug->enable[axis] = 0;
03434 }
03435 else {
03436 #ifdef rtai
03437 pdmultfloat = 1e9 /
03438 (emcmotDebug->rawOutput[axis] *
03439 ((double) emcmotStatus->outputScale[axis]) *
03440 ((double) PERIOD));
03441 #else
03442 #ifdef USE_RTL2
03443 pdmultfloat = ((double) HRTICKS_PER_SEC) /
03444 (emcmotDebug->rawOutput[axis] *
03445 ((double) emcmotStatus->outputScale[axis]) *
03446 ((double) PERIOD));
03447 #else
03448 pdmultfloat = ((double) RT_TICKS_PER_SEC) /
03449 (emcmotDebug->rawOutput[axis] *
03450 ((double) emcmotStatus->outputScale[axis]) *
03451 ((double) PERIOD));
03452 #endif
03453 #endif
03454
03455 if (pdmultfloat < 0.0) {
03456 emcmotDebug->pdmult[axis] = (int) (pdmultfloat - 0.5);
03457 emcmotDebug->enable[axis] = 1;
03458
03459
03460
03461
03462
03463
03464
03465
03466 if (emcmotDebug->pdmult[axis] == 0) {
03467 emcmotDebug->pdmult[axis] = -1;
03468 }
03469 }
03470 else {
03471 emcmotDebug->pdmult[axis] = (int) (pdmultfloat + 0.5);
03472 emcmotDebug->enable[axis] = 1;
03473
03474 if (emcmotDebug->pdmult[axis] == 0) {
03475 emcmotDebug->pdmult[axis] = 1;
03476 }
03477 }
03478 }
03479 #endif
03480 }
03481
03482
03483
03484
03485
03486
03487
03488 extDacWriteAll(EMCMOT_MAX_AXIS, emcmotDebug->rawOutput);
03489
03490
03491
03492
03493
03494 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
03495 emcmotStatus->axisPos[axis] = emcmotDebug->jointPos[axis];
03496 }
03497
03498
03499 emcmotStatus->heartbeat++;
03500
03501
03502 emcmotStatus->depth = tpQueueDepth(&emcmotDebug->queue);
03503 emcmotStatus->activeDepth = tpActiveDepth(&emcmotDebug->queue);
03504 emcmotStatus->id = tpGetExecId(&emcmotDebug->queue);
03505 emcmotStatus->queueFull = tcqFull(&emcmotDebug->queue.queue);
03506 SET_MOTION_INPOS_FLAG(0);
03507 if (tpIsDone(&emcmotDebug->queue)) {
03508 SET_MOTION_INPOS_FLAG(1);
03509 }
03510
03511
03512 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
03513 SET_AXIS_INPOS_FLAG(axis, 0);
03514 if (tpIsDone(&emcmotDebug->freeAxis[axis])) {
03515 SET_AXIS_INPOS_FLAG(axis, 1);
03516 }
03517 else {
03518
03519 if (emcmotStatus->overrideLimits) {
03520 emcmotDebug->overriding = 1;
03521 }
03522 }
03523 }
03524
03525
03526
03527 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
03528 if (! GET_AXIS_INPOS_FLAG(axis)) {
03529 break;
03530 }
03531 }
03532 if (axis == EMCMOT_MAX_AXIS) {
03533
03534 if (emcmotDebug->overriding) {
03535 emcmotDebug->overriding = 0;
03536 emcmotStatus->overrideLimits = 0;
03537 }
03538 }
03539
03540
03541
03542 if (emcmotDebug->stepping && emcmotDebug->idForStep != emcmotStatus->id) {
03543 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
03544 tpPause(&emcmotDebug->freeAxis[axis]);
03545 }
03546 tpPause(&emcmotDebug->queue);
03547 emcmotDebug->stepping = 0;
03548 emcmotStatus->paused = 1;
03549 }
03550
03551
03552 end = etime();
03553 delta = end - start;
03554
03555 emcmotDebug->last_time = emcmotDebug->cur_time;
03556 emcmotDebug->cur_time = end;
03557
03558 if (emcmotDebug->last_time != 0.0) {
03559 mmxavgAdd(&emcmotDebug->yMmxavg, (emcmotDebug->cur_time - emcmotDebug->last_time));
03560 emcmotDebug->yMin = mmxavgMin(&emcmotDebug->yMmxavg);
03561 emcmotDebug->yMax = mmxavgMax(&emcmotDebug->yMmxavg);
03562 emcmotDebug->yAvg = mmxavgAvg(&emcmotDebug->yMmxavg);
03563 }
03564
03565 emcmotStatus->computeTime = delta;
03566 if (2 == whichCycle) {
03567
03568 mmxavgAdd(&emcmotDebug->tMmxavg, delta);
03569 emcmotDebug->tMin = mmxavgMin(&emcmotDebug->tMmxavg);
03570 emcmotDebug->tMax = mmxavgMax(&emcmotDebug->tMmxavg);
03571 emcmotDebug->tAvg = mmxavgAvg(&emcmotDebug->tMmxavg);
03572 }
03573 else if (1 == whichCycle) {
03574
03575 mmxavgAdd(&emcmotDebug->sMmxavg, delta);
03576 emcmotDebug->sMin = mmxavgMin(&emcmotDebug->sMmxavg);
03577 emcmotDebug->sMax = mmxavgMax(&emcmotDebug->sMmxavg);
03578 emcmotDebug->sAvg = mmxavgAvg(&emcmotDebug->sMmxavg);
03579 }
03580 else {
03581
03582 mmxavgAdd(&emcmotDebug->nMmxavg, delta);
03583 emcmotDebug->nMin = mmxavgMin(&emcmotDebug->nMmxavg);
03584 emcmotDebug->nMax = mmxavgMax(&emcmotDebug->nMmxavg);
03585 emcmotDebug->nAvg = mmxavgAvg(&emcmotDebug->nMmxavg);
03586 }
03587
03588
03589 logIt = 0;
03590 if (emcmotStatus->logStarted &&
03591 emcmotStatus->logSkip >= 0) {
03592
03593
03594 ls.type = emcmotStatus->logType;
03595
03596
03597 switch (ls.type) {
03598
03599 case EMCMOT_LOG_TYPE_AXIS_POS:
03600 if (logSkip-- <= 0) {
03601 ls.item.axisPos.time = end - logStartTime;
03602 ls.item.axisPos.input = emcmotStatus->input[loggingAxis];
03603 ls.item.axisPos.output = emcmotDebug->jointPos[loggingAxis];
03604 logSkip = emcmotStatus->logSkip;
03605 logIt = 1;
03606 }
03607 break;
03608
03609 case EMCMOT_LOG_TYPE_ALL_INPOS:
03610 if (logSkip-- <= 0) {
03611 ls.item.allInpos.time = end - logStartTime;
03612 for (axis = 0;
03613 axis < EMCMOT_LOG_NUM_AXES &&
03614 axis < EMCMOT_MAX_AXIS;
03615 axis++) {
03616 ls.item.allInpos.input[axis] = emcmotStatus->input[axis];
03617 }
03618 logSkip = emcmotStatus->logSkip;
03619 logIt = 1;
03620 }
03621 break;
03622
03623 case EMCMOT_LOG_TYPE_ALL_OUTPOS:
03624 if (logSkip-- <= 0) {
03625 ls.item.allOutpos.time = end - logStartTime;
03626 for (axis = 0;
03627 axis < EMCMOT_LOG_NUM_AXES &&
03628 axis < EMCMOT_MAX_AXIS;
03629 axis++) {
03630 ls.item.allOutpos.output[axis] = emcmotDebug->jointPos[axis];
03631 }
03632 logSkip = emcmotStatus->logSkip;
03633 logIt = 1;
03634 }
03635 break;
03636
03637 case EMCMOT_LOG_TYPE_AXIS_VEL:
03638 if (logSkip-- <= 0) {
03639 ls.item.axisVel.time = end - logStartTime;
03640 ls.item.axisVel.cmdVel = emcmotDebug->jointPos[loggingAxis] -
03641 emcmotDebug->oldJointPos[loggingAxis];
03642 ls.item.axisVel.actVel = emcmotStatus->input[loggingAxis] -
03643 emcmotDebug->oldInput[loggingAxis];
03644 logSkip = emcmotStatus->logSkip;
03645 logIt = 1;
03646 }
03647 break;
03648
03649 case EMCMOT_LOG_TYPE_ALL_FERROR:
03650 if (logSkip-- <= 0) {
03651 ls.item.allFerror.time = end - logStartTime;
03652 for (axis = 0;
03653 axis < EMCMOT_LOG_NUM_AXES &&
03654 axis < EMCMOT_MAX_AXIS;
03655 axis++) {
03656 ls.item.allFerror.ferror[axis] = emcmotDebug->ferrorCurrent[axis];
03657 }
03658 logSkip = emcmotStatus->logSkip;
03659 logIt = 1;
03660 }
03661 break;
03662
03663 case EMCMOT_LOG_TYPE_POS_VOLTAGE:
03664
03665 if (emcmotLog->howmany >= emcmotStatus->logSize) {
03666 emcmotStatus->output[loggingAxis] = 0.0;
03667 emcmotStatus->logStarted = 0;
03668 logIt = 0;
03669 break;
03670 }
03671 if (logSkip-- <= 0) {
03672 ls.item.posVoltage.time = end - logStartTime;
03673 for (axis = 0;
03674 axis < EMCMOT_LOG_NUM_AXES &&
03675 axis < EMCMOT_MAX_AXIS;
03676 axis++) {
03677 ls.item.posVoltage.pos = emcmotStatus->input[loggingAxis];
03678 ls.item.posVoltage.voltage = emcmotDebug->rawOutput[loggingAxis];
03679 }
03680 logSkip = emcmotStatus->logSkip;
03681 logIt = 1;
03682 }
03683 break;
03684
03685 default:
03686 break;
03687 }
03688
03689
03690 if (logIt) {
03691 emcmotLogAdd(emcmotLog, ls);
03692 emcmotStatus->logPoints = emcmotLog->howmany;
03693 logIt = 0;
03694 }
03695 }
03696
03697 emcmotDebug->running_time = etime() - emcmotDebug->start_time;
03698
03699
03700 emcmotStatus->tail = emcmotStatus->head;
03701 emcmotDebug->tail = emcmotDebug->head;
03702 emcmotConfig->tail = emcmotConfig->head;
03703
03704 #ifdef rtai
03705 rt_task_wait_period();
03706 while(0 == emcmotStruct)
03707 {
03708 rt_task_wait_period();
03709 }
03710 #endif
03711 #ifdef rtlinux
03712 #ifdef USE_RTL2
03713 pthread_wait_np();
03714 #else
03715 rt_task_wait();
03716 #endif
03717 #endif
03718 #if defined(rtai) || defined(rtlinux)
03719 }
03720 #endif
03721
03722 #ifdef USE_RTL2
03723 return NULL;
03724 #endif
03725
03726 }
03727
03728 #ifdef STEPPER_MOTORS
03729
03730
03731
03732
03733
03734
03735
03736
03737
03738
03739 #ifdef USE_RTL2
03740 static void *freqfunc(void *arg)
03741 #else
03742 static void freqfunc(int i)
03743 #endif
03744 {
03745 double start, end, cur_time, last_time;
03746
03747 #define FREQ_AXES 6
03748 int axis = 0;
03749 int pdmag = 0;
03750 #if defined(rtlinux) || defined(rtai)
03751
03752 int oldpdmult[FREQ_AXES] = {0};
03753 char direction[FREQ_AXES] = {0};
03754 char reversal[FREQ_AXES] = {0};
03755 int upmag[FREQ_AXES] = {0};
03756 int downmag[FREQ_AXES] = {0};
03757 int upcount[FREQ_AXES] = {0};
03758 int downcount[FREQ_AXES] = {0};
03759 char loByte = 0;
03760 char hiByte = 0;
03761 #else
03762
03763 static int oldpdmult[FREQ_AXES] = {0};
03764 static char direction[FREQ_AXES] = {0};
03765 static char reversal[FREQ_AXES] = {0};
03766 static int upmag[FREQ_AXES] = {0};
03767 static int downmag[FREQ_AXES] = {0};
03768 static int upcount[FREQ_AXES] = {0};
03769 static int downcount[FREQ_AXES] = {0};
03770 static char loByte = 0;
03771 static char hiByte = 0;
03772 #endif
03773
03774 start = end = cur_time = last_time = 0;
03775
03776 #if defined(rtlinux) || defined(rtai)
03777 while (1) {
03778 #endif
03779 start = etime();
03780
03781 for (axis = 0; axis < FREQ_AXES; axis++) {
03782 if (emcmotDebug->enable[axis]) {
03783
03784 reversal[axis] = 0;
03785
03786 if (emcmotDebug->pdmult[axis] != oldpdmult[axis]) {
03787 if (emcmotDebug->pdmult[axis] == 0) {
03788 pdmag = 0;
03789
03790 }
03791 else if (emcmotDebug->pdmult[axis] < 0) {
03792 pdmag = -emcmotDebug->pdmult[axis];
03793 if (direction[axis] == 0) {
03794 reversal[axis] = 1;
03795 }
03796 direction[axis] = 1;
03797 }
03798 else {
03799 pdmag = emcmotDebug->pdmult[axis];
03800 if (direction[axis] != 0) {
03801 reversal[axis] = 1;
03802 }
03803 direction[axis] = 0;
03804 }
03805
03806
03807 if (pdmag > 1) {
03808 upmag[axis] = (pdmag - 2) / 2;
03809 downmag[axis] = (pdmag - 1) / 2;
03810 }
03811 else {
03812 upmag[axis] = downmag[axis] = 0;
03813 }
03814
03815
03816 if (upcount[axis] > upmag[axis]) {
03817 upcount[axis] = upmag[axis];
03818 }
03819 if (downcount[axis] > downmag[axis]) {
03820 downcount[axis] = downmag[axis];
03821 }
03822
03823
03824 if (direction[axis]) {
03825 if (axis < 4) {
03826 loByte |= (0x01 << (axis << 1));
03827 }
03828 else {
03829 hiByte |= (0x01 << ((axis - 4) << 1));
03830 }
03831 }
03832 else {
03833 if (axis < 4) {
03834 loByte &= ~(0x01 << (axis << 1));
03835 }
03836 else {
03837 hiByte &= ~(0x01 << ((axis - 4) << 1));
03838 }
03839 }
03840
03841 oldpdmult[axis] = emcmotDebug->pdmult[axis];
03842 }
03843
03844
03845
03846 if (! reversal[axis]) {
03847 if (upcount[axis] > 0) {
03848
03849 --upcount[axis];
03850 }
03851 else if (upcount[axis] == 0) {
03852
03853 if (axis < 4) {
03854 loByte &= ~(0x02 << (axis << 1));
03855 }
03856 else {
03857 hiByte &= ~(0x02 << ((axis - 4) << 1));
03858 }
03859
03860 --upcount[axis];
03861 }
03862 else if (downcount[axis] > 0) {
03863
03864 --downcount[axis];
03865 } else if (downcount[axis] == 0) {
03866
03867 if (axis < 4) {
03868 loByte |= (0x02 << (axis << 1));
03869 }
03870 else {
03871 hiByte |= (0x02 << ((axis - 4) << 1));
03872 }
03873
03874 if (direction[axis]) {
03875 --emcmotDebug->stepperCount[axis];
03876 }
03877 else {
03878 ++emcmotDebug->stepperCount[axis];
03879 }
03880
03881 upcount[axis] = upmag[axis];
03882 downcount[axis] = downmag[axis];
03883 }
03884 }
03885 }
03886 }
03887
03888
03889 pptDioByteWrite(0, loByte);
03890 pptDioByteWrite(1, hiByte);
03891
03892 end=etime();
03893 mmxavgAdd(&emcmotDebug->fMmxavg, end - start);
03894 emcmotDebug->fMin = mmxavgMin(&emcmotDebug->fMmxavg);
03895 emcmotDebug->fMax = mmxavgMax(&emcmotDebug->fMmxavg);
03896 emcmotDebug->fAvg = mmxavgAvg(&emcmotDebug->fMmxavg);
03897 last_time = cur_time;
03898 cur_time = end;
03899 if(last_time != 0)
03900 {
03901 mmxavgAdd(&emcmotDebug->fyMmxavg, cur_time - last_time);
03902 emcmotDebug->fyMin = mmxavgMin(&emcmotDebug->fyMmxavg);
03903 emcmotDebug->fyMax = mmxavgMax(&emcmotDebug->fyMmxavg);
03904 emcmotDebug->fyAvg = mmxavgAvg(&emcmotDebug->fyMmxavg);
03905 }
03906
03907 #ifdef rtai
03908 rt_task_wait_period();
03909 while(0 == emcmotStruct)
03910 {
03911 rt_task_wait_period();
03912 }
03913 #endif
03914 #ifdef rtlinux
03915 #ifdef USE_RTL2
03916 pthread_wait_np();
03917 #else
03918 rt_task_wait();
03919 #endif
03920 #endif
03921 #if defined(rtlinux) || defined(rtai)
03922 }
03923 #endif
03924 #undef FREQ_AXES
03925 }
03926
03927
03928
03929
03930
03931
03932
03933
03934
03935
03936
03937
03938
03939
03940
03941
03942
03943
03944
03945
03946
03947
03948
03949
03950
03951
03952
03953
03954
03955
03956
03957
03958
03959
03960
03961
03962 #ifdef USE_RTL2
03963 static void *phasefunc(void *arg)
03964 #else
03965 static void phasefunc(int i)
03966 #endif
03967 {
03968 #define FREQ_AXES 6
03969 int axis = 0;
03970 int pdmag = 0;
03971 char ptemp;
03972 #if defined(rtlinux) || defined(rtai)
03973
03974 int oldpdmult[FREQ_AXES] = {0};
03975 char direction[FREQ_AXES] = {0};
03976 int upmag[FREQ_AXES] = {0};
03977 int downmag[FREQ_AXES] = {0};
03978 int upcount[FREQ_AXES] = {0};
03979 int downcount[FREQ_AXES] = {0};
03980 char p13[FREQ_AXES] = {0};
03981 char p19[FREQ_AXES] = {0};
03982 char loByte = 0;
03983 char hiByte = 0;
03984 #else
03985
03986 static int oldpdmult[FREQ_AXES] = {0};
03987 static char direction[FREQ_AXES] = {0};
03988 static int upmag[FREQ_AXES] = {0};
03989 static int downmag[FREQ_AXES] = {0};
03990 static int upcount[FREQ_AXES] = {0};
03991 static int downcount[FREQ_AXES] = {0};
03992 static char p13[FREQ_AXES] = {0};
03993 static char p19[FREQ_AXES] = {0};
03994 static char loByte = 0;
03995 static char hiByte = 0;
03996 #endif
03997 double cur_time, last_time, start,end;
03998
03999 cur_time=last_time = start = end = 0.0;
04000
04001 #if defined(rtlinux) || defined(rtai)
04002 while (1) {
04003 #endif
04004 start=etime();
04005
04006
04007 for (axis = 0; axis < FREQ_AXES; axis++) {
04008 if (emcmotDebug->enable[axis]) {
04009
04010 if (emcmotDebug->pdmult[axis] != oldpdmult[axis]) {
04011 if (emcmotDebug->pdmult[axis] == 0) {
04012 pdmag = 0;
04013
04014 }
04015 else if (emcmotDebug->pdmult[axis] < 0) {
04016 pdmag = -emcmotDebug->pdmult[axis];
04017 direction[axis] = 1;
04018 }
04019 else {
04020 pdmag = emcmotDebug->pdmult[axis];
04021 direction[axis] = 0;
04022 }
04023
04024
04025 if (pdmag > 1) {
04026 upmag[axis] = (pdmag - 2) / 2;
04027 downmag[axis] = (pdmag - 1) / 2;
04028 }
04029 else {
04030 upmag[axis] = downmag[axis] = 0;
04031 }
04032
04033
04034 if (upcount[axis] > upmag[axis]) {
04035 upcount[axis] = upmag[axis];
04036 }
04037 if (downcount[axis] > downmag[axis]) {
04038 downcount[axis] = downmag[axis];
04039 }
04040
04041 oldpdmult[axis] = emcmotDebug->pdmult[axis];
04042 }
04043
04044
04045 if (upcount[axis] > 0) {
04046
04047 --upcount[axis];
04048 }
04049 else if (upcount[axis] == 0) {
04050
04051 --upcount[axis];
04052 }
04053 else if (downcount[axis] > 0) {
04054
04055 --downcount[axis];
04056 } else if (downcount[axis] == 0) {
04057
04058 ptemp = (p19[axis] != direction[axis]);
04059 p19[axis] = (p13[axis] == direction[axis]);
04060 p13[axis] = ptemp;
04061
04062
04063 if (axis < 4) {
04064 loByte &= ~(3 << (axis << 1));
04065 loByte |= ((p13[axis] << 1 | p19[axis]) << (axis << 1));
04066 }
04067 else {
04068 hiByte &= ~(3 << ((axis-4) << 1));
04069 hiByte |= ((p13[axis] << 1 | p19[axis]) << ((axis - 4) << 1));
04070 }
04071
04072 if (direction[axis]) {
04073 --emcmotDebug->stepperCount[axis];
04074 }
04075 else {
04076 ++emcmotDebug->stepperCount[axis];
04077 }
04078
04079 upcount[axis] = upmag[axis];
04080 downcount[axis] = downmag[axis];
04081 }
04082 }
04083 }
04084
04085
04086 pptDioByteWrite(0, loByte);
04087 pptDioByteWrite(1, hiByte);
04088
04089 end=etime();
04090 mmxavgAdd(&emcmotDebug->fMmxavg, end - start);
04091 emcmotDebug->fMin = mmxavgMin(&emcmotDebug->fMmxavg);
04092 emcmotDebug->fMax = mmxavgMax(&emcmotDebug->fMmxavg);
04093 emcmotDebug->fAvg = mmxavgAvg(&emcmotDebug->fMmxavg);
04094 last_time = cur_time;
04095 cur_time = end;
04096 if(last_time != 0)
04097 {
04098 mmxavgAdd(&emcmotDebug->fyMmxavg, cur_time - last_time);
04099 emcmotDebug->fyMin = mmxavgMin(&emcmotDebug->fyMmxavg);
04100 emcmotDebug->fyMax = mmxavgMax(&emcmotDebug->fyMmxavg);
04101 emcmotDebug->fyAvg = mmxavgAvg(&emcmotDebug->fyMmxavg);
04102 }
04103
04104 #ifdef rtai
04105 rt_task_wait_period();
04106 while(0 == emcmotStruct)
04107 {
04108 rt_task_wait_period();
04109 }
04110 #endif
04111
04112 #ifdef rtlinux
04113 #ifdef USE_RTL2
04114 pthread_wait_np();
04115 #else
04116 rt_task_wait();
04117 #endif
04118 #endif
04119
04120 #if defined(rtlinux) || defined(rtai)
04121 }
04122 #endif
04123 #undef FREQ_AXES
04124 }
04125
04126
04127
04128
04129
04130
04131
04132
04133
04134
04135
04136
04137
04138
04139
04140
04141
04142
04143
04144
04145
04146 #ifdef USE_RTL2
04147 static void *lpg_phase_drive(void *arg)
04148 #else
04149 static void lpg_phase_drive(int i)
04150 #endif
04151 {
04152 int axis;
04153 double start,end, cur_time, last_time;
04154
04155
04156
04157
04158 #define PHASE_LENGTH 4
04159 static int phase[PHASE_LENGTH]= {0,1,3,2};
04160 static int active_phase[EMCMOT_MAX_AXIS] = {0};
04161 static int downcount[EMCMOT_MAX_AXIS] = {0};
04162 static unsigned int phasedrive = 0;
04163
04164 static unsigned int lastdrive = 0;
04165
04166 int req_dly = 0;
04167
04168 start = end = cur_time = last_time = 0;
04169
04170 #if defined(rtlinux) || defined(rtai)
04171 while (1)
04172 {
04173 #endif
04174 start=etime();
04175
04176
04177
04178 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
04179 {
04180 if (emcmotDebug->enable[axis])
04181 {
04182 req_dly = emcmotDebug->pdmult[axis];
04183 if ( req_dly < 0 )
04184 req_dly = -req_dly;
04185
04186
04187
04188
04189
04190
04191
04192
04193
04194
04195 if (req_dly < 4 )
04196 req_dly = 4;
04197
04198 if ( downcount[axis] > 0 )
04199 {
04200
04201 --downcount[axis];
04202 if ( req_dly < downcount[axis] )
04203 downcount[axis] = req_dly;
04204 }
04205 else
04206 {
04207
04208 downcount[axis] = req_dly;
04209
04210
04211
04212
04213 phasedrive &= ~( 3 << (axis << 1));
04214
04215 if (emcmotDebug->pdmult[axis] >= 0)
04216 {
04217
04218 ++active_phase[axis];
04219 if (active_phase[axis] >= PHASE_LENGTH)
04220 active_phase[axis] = 0;
04221
04222
04223 phasedrive |= phase[active_phase[axis]] << (axis << 1);
04224
04225
04226 ++emcmotDebug->stepperCount[axis];
04227 }
04228 else
04229 {
04230
04231 --active_phase[axis];
04232 if (active_phase[axis] < 0)
04233 active_phase[axis] = PHASE_LENGTH -1;
04234
04235
04236 phasedrive |= phase[active_phase[axis]] << (axis << 1);
04237
04238
04239 --emcmotDebug->stepperCount[axis];
04240 }
04241 }
04242 }
04243 }
04244
04245
04246
04247 if(phasedrive != lastdrive)
04248 {
04249 pptDioByteWrite(0, phasedrive);
04250 pptDioByteWrite(1, phasedrive >> 8);
04251 lastdrive = phasedrive;
04252 }
04253
04254 end=etime();
04255 mmxavgAdd(&emcmotDebug->fMmxavg, end - start);
04256 emcmotDebug->fMin = mmxavgMin(&emcmotDebug->fMmxavg);
04257 emcmotDebug->fMax = mmxavgMax(&emcmotDebug->fMmxavg);
04258 emcmotDebug->fAvg = mmxavgAvg(&emcmotDebug->fMmxavg);
04259 last_time = cur_time;
04260 cur_time = end;
04261 if(last_time != 0)
04262 {
04263 mmxavgAdd(&emcmotDebug->fyMmxavg, cur_time - last_time);
04264 emcmotDebug->fyMin = mmxavgMin(&emcmotDebug->fyMmxavg);
04265 emcmotDebug->fyMax = mmxavgMax(&emcmotDebug->fyMmxavg);
04266 emcmotDebug->fyAvg = mmxavgAvg(&emcmotDebug->fyMmxavg);
04267 }
04268
04269 #ifdef rtai
04270 rt_task_wait_period();
04271 while(0 == emcmotStruct)
04272 {
04273 rt_task_wait_period();
04274 }
04275 #endif
04276
04277 #ifdef rtlinux
04278
04279 #ifdef USE_RTL2
04280 pthread_wait_np();
04281 #else
04282 rt_task_wait();
04283 #endif
04284 #endif
04285
04286 #if defined(rtlinux) || defined(rtai)
04287 }
04288 #endif
04289 }
04290
04291 #endif
04292
04293 int STEPPING_TYPE;
04294
04295 #if defined(rtlinux) || defined(rtai)
04296 #if LINUX_VERSION_CODE > KERNEL_VERSION(2,2,0)
04297
04298
04299 MODULE_PARM(PERIOD, "i");
04300 MODULE_PARM(SHMEM_BASE_ADDRESS, "l");
04301 MODULE_PARM(SHMEM_KEY, "i");
04302 #ifndef STEPPER_MOTORS
04303 MODULE_PARM(STG_BASE_ADDRESS, "h");
04304 MODULE_PARM(FIND_STG_BASE_ADDRESS, "i");
04305 #else
04306 MODULE_PARM(PARPORT_IO_ADDRESS, "l");
04307 MODULE_PARM(STEPPING_TYPE, "i");
04308 MODULE_PARM(FREQ_TASK_PRIORITY, "i");
04309 MODULE_PARM(FREQ_TASK_STACK_SIZE, "i");
04310 #endif
04311 MODULE_PARM(EMCMOT_TASK_PRIORITY, "i");
04312 MODULE_PARM(EMCMOT_TASK_STACK_SIZE, "i");
04313 MODULE_PARM(EMCMOT_NO_FORWARD_KINEMATICS, "i");
04314 #endif
04315 #endif
04316
04317 #ifdef USE_RTL2
04318 static clockid_t local_clock;
04319 #endif
04320
04321 #ifdef rtai
04322 void freqsig(void)
04323 {
04324 }
04325 void emcmotsig(void)
04326 {
04327 }
04328 #endif
04329
04330
04331 int init_module(void)
04332 {
04333 int axis;
04334 int t;
04335 #ifdef USE_RTL2
04336 hrtime_t now;
04337 pthread_attr_t attr;
04338 struct sched_param sched_param;
04339 struct timespec resolution;
04340 int resperiod;
04341 hrtime_t hr_timer_period;
04342 #endif
04343 PID_STRUCT pid;
04344 #ifdef rtai
04345 int error_code;
04346 #endif
04347
04348
04349 #ifdef STEPPER_MOTORS
04350 char *pstype;
04351 switch(STEPPING_TYPE){
04352 case 0: pstype = "Step/Dir"; break;
04353 case 1: pstype = "Phase"; break;
04354 case 2: pstype = "Quadrature/other"; break;
04355 default: pstype = "Invalid";
04356 }
04357 diagnostics("emcmot: STEPPING_TYPE = %s\n",pstype);
04358 diagnostics("emcmot: PARPORT_IO_ADDRESS = 0x%x\n",PARPORT_IO_ADDRESS);
04359 #endif
04360 diagnostics("emcmot: SHMEM_BASE_ADDRESS = %ld\n",SHMEM_BASE_ADDRESS);
04361 diagnostics("emcmot: SHMEM_KEY = %d\n",SHMEM_KEY);
04362 diagnostics("emcmot: PERIOD = %dns\n",PERIOD);
04363 diagnostics("emcmot: EMCMOT_TASK_PRIORITY = %d\n",EMCMOT_TASK_PRIORITY);
04364 diagnostics("emcmot: EMCMOT_TASK_STACK_SIZE = %d\n",EMCMOT_TASK_STACK_SIZE);
04365 #ifdef STEPPER_MOTORS
04366 diagnostics("emcmot: FREQ_TASK_PRIORITY = %d\n",FREQ_TASK_PRIORITY);
04367 diagnostics("emcmot: FREQ_TASK_STACK_SIZE = %d\n",FREQ_TASK_STACK_SIZE);
04368 #endif
04369 #ifdef rtai
04370 diagnostics("emcmot: sizeof(RT_TASK) = %d\n", sizeof(RT_TASK));
04371 #endif
04372
04373
04374 #if 0
04375 diagnostics("emcmot: STG_BASE_ADDRESS = 0x%hx\n",STG_BASE_ADDRESS);
04376 diagnostics("emcmot: FIND_STG_BASE_ADDRESS = %d\n",FIND_STG_BASE_ADDRESS);
04377 #endif
04378
04379
04380 emcmotTask_created=0;
04381 freqTask_created=0;
04382 emcmotStruct=0;
04383 emcmotDebug=0;
04384 emcmotStatus=0;
04385 emcmotCommand=0;
04386 emcmotConfig=0;
04387
04388
04389 kinType = kinematicsType();
04390 if (EMCMOT_NO_FORWARD_KINEMATICS) {
04391 kinType = KINEMATICS_INVERSE_ONLY;
04392 }
04393
04394 #ifdef rtai
04395 rt_mount_rtai();
04396 rt_linux_use_fpu(1);
04397 emcmotStruct = rtai_kmalloc(SHMEM_KEY, sizeof(EMCMOT_STRUCT));
04398 if(0 == emcmotStruct)
04399 {
04400 diagnostics("emcmot: can't allocate shared memory with rtai_kmalloc(%d,%d)\n.", SHMEM_KEY,sizeof(EMCMOT_STRUCT));
04401 return -1;
04402 }
04403 diagnostics("emcmot: emcmotStruct allocated at 0x%X from rtai_kmalloc(%d,%d)\n", ((unsigned int) emcmotStruct), SHMEM_KEY, sizeof(EMCMOT_STRUCT));
04404
04405 rt_set_periodic_mode();
04406
04407
04408 rtaiTickPeriod = start_rt_timer(nano2count(PERIOD));
04409 if (rtaiTickPeriod < 1 )
04410 {
04411 diagnostics("emcmot: problem with start_rt_timer(%d)\n",
04412 nano2count(PERIOD));
04413 return(-1);
04414 }
04415 rtaiTickPeriod_long = (long) rtaiTickPeriod;
04416 diagnostics("emcmot: rtaiTickPeriod=%d\n",rtaiTickPeriod_long);
04417 #else
04418 #ifdef rtlinux
04419
04420
04421 #ifdef USE_RTL2
04422 local_clock = rtl_getschedclock();
04423
04424
04425
04426
04427
04428
04429
04430
04431 clock_getres (local_clock, &resolution);
04432 resperiod = timespec_to_ns(&resolution);
04433
04434
04435 PERIOD /= resperiod;
04436 PERIOD *= resperiod;
04437 if (PERIOD < resperiod) {
04438 PERIOD = resperiod;
04439 }
04440 hr_timer_period = (hrtime_t) PERIOD;
04441
04442 if (rtl_setclockmode(local_clock, RTL_CLOCK_MODE_PERIODIC,
04443 hr_timer_period)) {
04444 diagnostics("can't set periodic mode\n");
04445 if (rtl_setclockmode(local_clock, RTL_CLOCK_MODE_ONESHOT,0)) {
04446 diagnostics("can't set oneshot mode\n"); ;
04447 }
04448 }
04449
04450 #else
04451
04452
04453
04454
04455
04456 PERIOD = (int) (((double) PERIOD) / 1.0e9 * ((double) RT_TICKS_PER_SEC));
04457 rtstart = rtl_set_periodic_mode(PERIOD);
04458
04459 if (rtstart == RT_TIME_END) {
04460
04461 rtl_set_oneshot_mode();
04462 diagnostics("reverting to oneshot mode\n");
04463 }
04464 else {
04465 diagnostics("running in periodic mode\n");
04466 }
04467 #endif
04468
04469 #ifdef USE_RTL2
04470
04471
04472 emcmotStruct = (EMCMOT_STRUCT *) mbuff_alloc("emcmotStruct", sizeof(EMCMOT_STRUCT));
04473 #else
04474 #if defined(rtlinux2)
04475
04476 emcmotStruct = (EMCMOT_STRUCT *) ioremap(SHMEM_BASE_ADDRESS, sizeof(EMCMOT_STRUCT));
04477 #else
04478
04479 emcmotStruct = (EMCMOT_STRUCT *) vremap(SHMEM_BASE_ADDRESS, sizeof(EMCMOT_STRUCT));
04480 #endif
04481
04482 #endif
04483
04484 #else
04485
04486 shmem = rcs_shm_open(SHMEM_KEY, sizeof(EMCMOT_STRUCT), 1, 0666);
04487 if (NULL == shmem ||
04488 NULL == shmem->addr) {
04489 diagnostics("can't get shared memory\n");
04490 return -1;
04491 }
04492
04493 memset(shmem->addr, 0, sizeof(EMCMOT_STRUCT));
04494
04495 emcmotStruct = (EMCMOT_STRUCT *) shmem->addr;
04496
04497 #endif
04498
04499 #endif
04500
04501
04502 emcmotCommand = (EMCMOT_COMMAND *) &emcmotStruct->command;
04503 emcmotStatus = (EMCMOT_STATUS *) &emcmotStruct->status;
04504 emcmotConfig = (EMCMOT_CONFIG *) &emcmotStruct->config;
04505 emcmotDebug = (EMCMOT_DEBUG *) &emcmotStruct->debug;
04506
04507 emcmotError = (EMCMOT_ERROR *) &emcmotStruct->error;
04508 emcmotLog = (EMCMOT_LOG *) &emcmotStruct->log;
04509 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
04510 emcmotComp[axis] = (EMCMOT_COMP *) &emcmotStruct->comp[axis];
04511
04512 emcmotDebug->bcomp[axis]=0;
04513 emcmotDebug->bcompdir[axis]=0;
04514 emcmotDebug->bcompincr[axis]=0;
04515 emcmotDebug->bac_done[axis]=0;
04516 emcmotDebug->bac_d[axis]=0;
04517 emcmotDebug->bac_di[axis]=0;
04518 emcmotDebug->bac_D[axis]=0;
04519 emcmotDebug->bac_halfD[axis]=0;
04520 emcmotDebug->bac_incrincr[axis]=0;
04521 emcmotDebug->bac_incr[axis]=0;
04522 }
04523
04524
04525 for (t = 0; t < sizeof(EMCMOT_STRUCT); t++) {
04526 ((char *) emcmotStruct)[t] = 0;
04527 }
04528
04529
04530 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
04531 emcmotDebug->maxLimitSwitchCount[axis] = 0;
04532 emcmotDebug->minLimitSwitchCount[axis] = 0;
04533 emcmotDebug->ampFaultCount[axis] = 0;
04534 }
04535
04536
04537 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
04538 emcmotComp[axis]->total = 0;
04539 emcmotComp[axis]->alter = 0.0;
04540
04541
04542 }
04543
04544
04545 emcmotErrorInit(emcmotError);
04546
04547
04548 emcmotCommand->head = 0;
04549 emcmotCommand->command = 0;
04550 emcmotCommand->commandNum = 0;
04551 emcmotCommand->tail = 0;
04552
04553
04554
04555 emcmotStatus->head = 0;
04556 emcmotDebug->head = 0;
04557 emcmotConfig->head = 0;
04558
04559 emcmotStatus->motionFlag = 0;
04560 SET_MOTION_ERROR_FLAG(0);
04561 SET_MOTION_COORD_FLAG(0);
04562 SET_MOTION_TELEOP_FLAG(0);
04563 emcmotDebug->split = 0;
04564 emcmotStatus->commandEcho = 0;
04565 emcmotStatus->commandNumEcho = 0;
04566 emcmotStatus->commandStatus = 0;
04567 emcmotStatus->heartbeat = 0;
04568 emcmotStatus->computeTime = 0.0;
04569 emcmotConfig->numAxes = EMCMOT_MAX_AXIS;
04570 emcmotConfig->trajCycleTime = TRAJ_CYCLE_TIME;
04571 emcmotConfig->servoCycleTime = SERVO_CYCLE_TIME;
04572 emcmotStatus->pos.tran.x = 0.0;
04573 emcmotStatus->pos.tran.y = 0.0;
04574 emcmotStatus->pos.tran.z = 0.0;
04575 emcmotStatus->actualPos.tran.x = 0.0;
04576 emcmotStatus->actualPos.tran.y = 0.0;
04577 emcmotStatus->actualPos.tran.z = 0.0;
04578 emcmotStatus->vel = VELOCITY;
04579 emcmotConfig->limitVel = VELOCITY;
04580 emcmotStatus->acc = ACCELERATION;
04581 emcmotStatus->qVscale = 1.0;
04582 emcmotStatus->id = 0;
04583 emcmotStatus->depth = 0;
04584 emcmotStatus->activeDepth = 0;
04585 emcmotStatus->paused = 0;
04586 emcmotStatus->overrideLimits = 0;
04587 SET_MOTION_INPOS_FLAG(1);
04588 emcmotStatus->logOpen = 0;
04589 emcmotStatus->logStarted = 0;
04590 emcmotStatus->logSize = 0;
04591 emcmotStatus->logSkip = 0;
04592 emcmotStatus->logPoints = 0;
04593 SET_MOTION_ENABLE_FLAG(0);
04594 emcmotConfig->kinematics_type = kinType;
04595
04596 emcmotDebug->oldPos = emcmotStatus->pos;
04597 emcmotDebug->oldVel.tran.x = 0.0;
04598 emcmotDebug->oldVel.tran.y = 0.0;
04599 emcmotDebug->oldVel.tran.z = 0.0;
04600
04601 MARK_EMCMOT_CONFIG_CHANGE();
04602
04603 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
04604 emcmotConfig->homingVel[axis] = VELOCITY;
04605 emcmotConfig->homeOffset[axis] = 0.0;
04606 emcmotStatus->axisFlag[axis] = 0;
04607 emcmotConfig->maxLimit[axis] = MAX_LIMIT;
04608 emcmotConfig->minLimit[axis] = MIN_LIMIT;
04609 emcmotConfig->maxOutput[axis] = MAX_OUTPUT;
04610 emcmotConfig->minOutput[axis] = MIN_OUTPUT;
04611 emcmotConfig->minFerror[axis] = 0.0;
04612 emcmotConfig->maxFerror[axis] = MAX_FERROR;
04613 emcmotDebug->ferrorCurrent[axis] = 0.0;
04614 emcmotDebug->ferrorHighMark[axis] = 0.0;
04615 emcmotStatus->outputScale[axis] = OUTPUT_SCALE;
04616 emcmotStatus->outputOffset[axis] = OUTPUT_OFFSET;
04617 emcmotStatus->inputScale[axis] = INPUT_SCALE;
04618 emcmotDebug->inverseInputScale[axis] = 1.0 / INPUT_SCALE;
04619 emcmotStatus->inputOffset[axis] = INPUT_OFFSET;
04620 emcmotDebug->inverseOutputScale[axis] = 1.0 / OUTPUT_SCALE;
04621 emcmotStatus->axVscale[axis] = 1.0;
04622 emcmotConfig->axisLimitVel[axis] = 1.0;
04623 emcmotDebug->bigVel[axis] = 1.0;
04624 SET_AXIS_ENABLE_FLAG(axis, 0);
04625 SET_AXIS_ACTIVE_FLAG(axis, 0);
04626
04627 SET_AXIS_NSL_FLAG(axis, 0);
04628 SET_AXIS_PSL_FLAG(axis, 0);
04629 SET_AXIS_NHL_FLAG(axis, 0);
04630 SET_AXIS_PHL_FLAG(axis, 0);
04631 SET_AXIS_INPOS_FLAG(axis, 1);
04632 SET_AXIS_HOMING_FLAG(axis, 0);
04633 SET_AXIS_HOMED_FLAG(axis, 0);
04634 SET_AXIS_FERROR_FLAG(axis, 0);
04635 SET_AXIS_FAULT_FLAG(axis, 0);
04636 SET_AXIS_ERROR_FLAG(axis, 0);
04637 emcmotConfig->axisPolarity[axis] = (EMCMOT_AXIS_FLAG) 0xFFFFFFFF;
04638
04639 }
04640
04641
04642
04643
04644 mmxavgInit(&emcmotDebug->tMmxavg, emcmotDebug->tMmxavgSpace, MMXAVG_SIZE);
04645 mmxavgInit(&emcmotDebug->sMmxavg, emcmotDebug->sMmxavgSpace, MMXAVG_SIZE);
04646 mmxavgInit(&emcmotDebug->nMmxavg, emcmotDebug->nMmxavgSpace, MMXAVG_SIZE);
04647 mmxavgInit(&emcmotDebug->yMmxavg, emcmotDebug->yMmxavgSpace, MMXAVG_SIZE);
04648 mmxavgInit(&emcmotDebug->fMmxavg, emcmotDebug->fMmxavgSpace, MMXAVG_SIZE);
04649 mmxavgInit(&emcmotDebug->fyMmxavg, emcmotDebug->fyMmxavgSpace, MMXAVG_SIZE);
04650 emcmotDebug->tMin = 0.0;
04651 emcmotDebug->tMax = 0.0;
04652 emcmotDebug->tAvg = 0.0;
04653 emcmotDebug->sMin = 0.0;
04654 emcmotDebug->sMax = 0.0;
04655 emcmotDebug->sAvg = 0.0;
04656 emcmotDebug->nMin = 0.0;
04657 emcmotDebug->nMax = 0.0;
04658 emcmotDebug->nAvg = 0.0;
04659 emcmotDebug->yMin = 0.0;
04660 emcmotDebug->yMax = 0.0;
04661 emcmotDebug->yAvg = 0.0;
04662 emcmotDebug->fyMin = 0.0;
04663 emcmotDebug->fyMax = 0.0;
04664 emcmotDebug->fyAvg = 0.0;
04665 emcmotDebug->fMin = 0.0;
04666 emcmotDebug->fMax = 0.0;
04667 emcmotDebug->fAvg = 0.0;
04668
04669 emcmotDebug->cur_time = emcmotDebug->last_time = 0.0;
04670 emcmotDebug->start_time = etime();
04671 emcmotDebug->running_time = 0.0;
04672
04673
04674 if (-1 == tpCreate(&emcmotDebug->queue, DEFAULT_TC_QUEUE_SIZE, emcmotDebug->queueTcSpace)) {
04675 diagnostics("can't create motion emcmotDebug->queue\n");
04676 return -1;
04677 }
04678 tpInit(&emcmotDebug->queue);
04679 tpSetCycleTime(&emcmotDebug->queue, emcmotConfig->trajCycleTime);
04680 tpSetPos(&emcmotDebug->queue, emcmotStatus->pos);
04681 tpSetVmax(&emcmotDebug->queue, emcmotStatus->vel);
04682 tpSetAmax(&emcmotDebug->queue, emcmotStatus->acc);
04683
04684
04685 pid.p = P_GAIN;
04686 pid.i = I_GAIN;
04687 pid.d = D_GAIN;
04688 pid.ff0 = FF0_GAIN;
04689 pid.ff1 = FF1_GAIN;
04690 pid.ff2 = FF2_GAIN;
04691 pid.backlash = BACKLASH;
04692 pid.bias = BIAS;
04693 pid.maxError = MAX_ERROR;
04694
04695 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
04696 if (-1 == tpCreate(&emcmotDebug->freeAxis[axis], FREE_AXIS_QUEUE_SIZE, emcmotDebug->freeAxisTcSpace[axis])) {
04697 diagnostics("can't create axis emcmotDebug->queue %d\n", axis);
04698 return -1;
04699 }
04700 tpInit(&emcmotDebug->freeAxis[axis]);
04701 tpSetCycleTime(&emcmotDebug->freeAxis[axis], emcmotConfig->trajCycleTime);
04702
04703 tpSetPos(&emcmotDebug->freeAxis[axis], emcmotDebug->freePose);
04704 tpSetVmax(&emcmotDebug->freeAxis[axis], emcmotStatus->vel);
04705 tpSetAmax(&emcmotDebug->freeAxis[axis], emcmotStatus->acc);
04706 pidInit(&emcmotConfig->pid[axis]);
04707 pidSetGains(&emcmotConfig->pid[axis], pid);
04708 cubicInit(&emcmotDebug->cubic[axis]);
04709 }
04710
04711
04712
04713 setTrajCycleTime(TRAJ_CYCLE_TIME);
04714 setServoCycleTime(SERVO_CYCLE_TIME);
04715
04716
04717
04718 if (-1 == extMotInit((const char *) EMCMOT_INIFILE)) {
04719 diagnostics("can't initialize motion hardware\n");
04720 return -1;
04721 }
04722
04723 if (-1 == extDioInit((const char *) EMCMOT_INIFILE)) {
04724 diagnostics("can't initialize DIO hardware\n");
04725 return -1;
04726 }
04727
04728 if (-1 == extAioInit((const char *) EMCMOT_INIFILE)) {
04729 diagnostics("can't initialize AIO hardware\n");
04730 return -1;
04731 }
04732
04733 extEncoderSetIndexModel(EXT_ENCODER_INDEX_MODEL_MANUAL);
04734 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
04735 emcmotDebug->rawInput[axis] = 0.0;
04736 emcmotDebug->rawOutput[axis] = 0.0;
04737 emcmotDebug->coarseJointPos[axis] = 0.0;
04738 emcmotDebug->jointPos[axis] = 0.0;
04739 emcmotDebug->jointVel[axis] = 0.0;
04740 emcmotStatus->axisPos[axis] = 0.0;
04741 emcmotDebug->oldJointPos[axis] = 0.0;
04742 emcmotDebug->outJointPos[axis] = 0.0;
04743
04744 emcmotDebug->homingPhase[axis] = 0;
04745 emcmotDebug->latchFlag[axis] = 0;
04746 emcmotDebug->saveLatch[axis] = 0.0;
04747
04748 emcmotStatus->input[axis] = 0.0;
04749 emcmotDebug->oldInput[axis] = 0.0;
04750 emcmotDebug->oldInputValid[axis]=0;
04751 emcmotStatus->output[axis] = 0.0;
04752
04753 emcmotDebug->jointHome[axis] = 0.0;
04754
04755 extAmpEnable(axis,
04756 ! GET_AXIS_ENABLE_POLARITY(axis));
04757 }
04758
04759 emcmotStatus->tail = 0;
04760
04761 #ifdef rtai
04762 diagnostics("emcmot: initializing emcmotTask\n");
04763 if( 0 != ( error_code =
04764 rt_task_init(&emcmotTask,
04765 emcmotController,
04766 1,
04767 EMCMOT_TASK_STACK_SIZE,
04768 EMCMOT_TASK_PRIORITY,
04769 1,
04770 emcmotsig))
04771 )
04772 {
04773 diagnostics("rt_task_init failed for emcmotTask error_code=%d\n",
04774 error_code);
04775 return -1;
04776 }
04777 now = rt_get_time();
04778 diagnostics("emcmot: rtaiTickPeriod=%d\n",rtaiTickPeriod);
04779 if(rtaiTickPeriod > 0)
04780 {
04781 rtaiTickPeriod_long = (long) rtaiTickPeriod;
04782 diagnostics("servoCycleTime=%dns\n",
04783 (int) (1e9* emcmotConfig->servoCycleTime));
04784 iperiod = nano2count((int) (1e9* emcmotConfig->servoCycleTime));
04785 diagnostics("servoCycleTime=%d ticks\n",
04786 iperiod);
04787
04788 if ( iperiod < rtaiTickPeriod_long)
04789 {
04790 iperiod = rtaiTickPeriod_long;
04791 }
04792 iperiod = (iperiod + rtaiTickPeriod_long/2)/ rtaiTickPeriod_long;
04793 iperiod *= rtaiTickPeriod_long;
04794 iperiod_rtime = (RTIME) iperiod;
04795 now = rt_get_time();
04796 setServoCycleTime(count2nano(iperiod)*1e-9);
04797 diagnostics("emcmot: making emcmotTask periodic at a rate of %d ticks or %d nanoseconds \n", iperiod , count2nano(iperiod));
04798 if ( 0 != (error_code =
04799 rt_task_make_periodic(&emcmotTask,
04800 now+5*rtaiTickPeriod,
04801 iperiod_rtime)
04802 ))
04803 {
04804 diagnostics("rt_task_make_periodic failed for emcmotTask error_code=%d\n",
04805 error_code);
04806 return -1;
04807 }
04808 emcmotTask_created=1;
04809 }
04810 diagnostics("emcmot: rtaiTickPeriod=%d\n",rtaiTickPeriod);
04811
04812 #ifdef STEPPER_MOTORS
04813 diagnostics("emcmot: initializing freqTask\n");
04814 switch (STEPPING_TYPE)
04815 {
04816 case 1:
04817 error_code = rt_task_init(&freqTask,
04818 phasefunc,
04819 1,
04820 FREQ_TASK_STACK_SIZE,
04821 FREQ_TASK_PRIORITY,
04822 1,
04823 freqsig);
04824 break;
04825 case 2:
04826 error_code = rt_task_init(&freqTask,
04827 lpg_phase_drive,
04828 1,
04829 FREQ_TASK_STACK_SIZE,
04830 FREQ_TASK_PRIORITY,
04831 1,
04832 freqsig);
04833 default:
04834 error_code = rt_task_init(&freqTask,
04835 freqfunc,
04836 1,
04837 FREQ_TASK_STACK_SIZE,
04838 FREQ_TASK_PRIORITY,
04839 1,
04840 freqsig);
04841 break;
04842 }
04843 diagnostics("emcmot: rtaiTickPeriod=%d\n",rtaiTickPeriod);
04844 if(error_code != 0)
04845 {
04846 diagnostics("rt_task_init failed for freqTask error_code = %d\n",
04847 error_code);
04848 return -1;
04849 }
04850 if( rtaiTickPeriod > 0 )
04851 {
04852 now = rt_get_time();
04853 diagnostics("emcmot: making freqTask periodic at a rate of %d ticks or %d nanoseconds \n", ((int) rtaiTickPeriod_long) , ((int) count2nano(rtaiTickPeriod)));
04854 error_code = rt_task_make_periodic(&freqTask,
04855 now+5*rtaiTickPeriod,
04856 rtaiTickPeriod);
04857 if(error_code != 0)
04858 {
04859 diagnostics("rt_task_make_periodic failed for freqTask error_code = %d\n",
04860 error_code);
04861 return -1;
04862 }
04863 freqTask_created=1;
04864 }
04865
04866 #endif
04867
04868 #else
04869
04870 #ifdef rtlinux
04871
04872
04873
04874 #ifdef USE_RTL2
04875 pthread_attr_init (&attr);
04876 sched_param.sched_priority = EMCMOT_TASK_PRIORITY;
04877 pthread_attr_setschedparam (&attr, &sched_param);
04878 attr.use_fp = 1;
04879 attr.stack_size = EMCMOT_TASK_STACK_SIZE;
04880 pthread_create( &emcmotTask, &attr, emcmotController, (void *) 1);
04881
04882
04883 now =gethrtime();
04884
04885 pthread_setfp_np(emcmotTask,1);
04886 #else
04887 rt_task_init(&emcmotTask,
04888 emcmotController,
04889 0,
04890 EMCMOT_TASK_STACK_SIZE,
04891 EMCMOT_TASK_PRIORITY);
04892
04893
04894 #if defined(rtlinux_09J) || defined(rtlinux_1_0) || defined(rtlinux2)
04895 rt_task_use_fp(&emcmotTask, 1);
04896 #else
04897 rt_use_fp(1);
04898 #endif
04899 #endif
04900
04901
04902 #ifdef USE_RTL2
04903 iperiod = (int) (HRTICKS_PER_SEC * emcmotConfig->servoCycleTime);
04904 #else
04905 iperiod = (int) (RT_TICKS_PER_SEC * emcmotConfig->servoCycleTime);
04906 #endif
04907
04908 iperiod = iperiod / PERIOD;
04909 iperiod = iperiod * PERIOD;
04910
04911 #ifdef USE_RTL2
04912 rtperiod = (hrtime_t) iperiod;
04913 #else
04914 rtperiod = (RTIME) iperiod;
04915 #endif
04916 #ifdef USE_RTL2
04917 pthread_make_periodic_np(emcmotTask,
04918 gethrtime() + rtperiod,
04919 rtperiod);
04920 #else
04921 rt_task_make_periodic(&emcmotTask,
04922 rtstart + rtperiod,
04923 rtperiod);
04924 #endif
04925
04926 #endif
04927
04928 #ifdef STEPPER_MOTORS
04929
04930
04931 #ifdef USE_RTL2
04932 pthread_attr_init (&attr);
04933 sched_param.sched_priority = FREQ_TASK_PRIORITY;
04934 pthread_attr_setschedparam (&attr, &sched_param);
04935 attr.use_fp = 1;
04936 attr.stack_size = FREQ_TASK_STACK_SIZE;
04937 switch (STEPPING_TYPE)
04938 {
04939 case 1:
04940 pthread_create(&freqTask, &attr, phasefunc, (void *) 1); break;
04941 case 2:
04942 pthread_create(&freqTask, &attr, lpg_phase_drive, (void *) 1); break;
04943 default:
04944 pthread_create(&freqTask, &attr, freqfunc, (void *) 1);
04945 }
04946
04947
04948 pthread_setfp_np(freqTask, 1);
04949 pthread_make_periodic_np(freqTask,
04950 gethrtime() + PERIOD,
04951 PERIOD);
04952
04953 #else
04954 switch (STEPPING_TYPE)
04955 {
04956 case 1:
04957 rt_task_init(&freqTask,
04958 phasefunc,
04959 0,
04960 FREQ_TASK_STACK_SIZE,
04961 FREQ_TASK_PRIORITY);
04962 break;
04963
04964 case 2:
04965 rt_task_init(&freqTask,
04966 lpg_phase_drive,
04967 0,
04968 FREQ_TASK_STACK_SIZE,
04969 FREQ_TASK_PRIORITY);
04970 break;
04971
04972 default:
04973 rt_task_init(&freqTask,
04974 freqfunc,
04975 0,
04976 FREQ_TASK_STACK_SIZE,
04977 FREQ_TASK_PRIORITY);
04978 }
04979
04980
04981 rt_task_use_fp(&freqTask, 1);
04982
04983 rt_task_make_periodic(&freqTask,
04984 rt_get_time() + PERIOD,
04985 PERIOD);
04986
04987 #endif
04988 #endif
04989 #endif
04990
04991 diagnostics("emcmot: init_module finished\n");
04992
04993 return 0;
04994 }
04995
04996 void cleanup_module(void)
04997 {
04998 int axis;
04999
05000 diagnostics("emcmot: cleanup started.\n");
05001
05002 #if 0
05003
05004
05005
05006
05007
05008
05009 if( 0 != emcmotStruct)
05010 {
05011 if(0 != emcmotDebug )
05012 {
05013 tpDelete(&emcmotDebug->queue);
05014 }
05015
05016
05017 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
05018 extAmpEnable(axis,
05019 ! GET_AXIS_ENABLE_POLARITY(axis));
05020 }
05021
05022
05023 extAioQuit();
05024 extDioQuit();
05025 extMotQuit();
05026 }
05027 #endif
05028
05029 #ifdef rtai
05030 stop_rt_timer();
05031 if(emcmotTask_created)
05032 {
05033 diagnostics("emcmot: deleting emcmotTask.\n");
05034 rt_task_delete(&emcmotTask);
05035 emcmotTask_created=0;
05036 }
05037 #ifdef STEPPER_MOTORS
05038 if(freqTask_created)
05039 {
05040 diagnostics("emcmot: deleting freqTask.\n");
05041 rt_task_delete(&freqTask);
05042 freqTask_created=0;
05043 }
05044 #endif
05045 #endif
05046
05047
05048 #ifdef rtlinux
05049 #ifdef USE_RTL2
05050 pthread_delete_np(emcmotTask);
05051 #else
05052 rt_task_delete(&emcmotTask);
05053 #endif
05054
05055 #ifdef STEPPER_MOTORS
05056
05057 #ifdef USE_RTL2
05058 pthread_delete_np(freqTask);
05059 #else
05060 rt_task_delete(&freqTask);
05061 #endif
05062 #endif
05063 #endif
05064
05065
05066
05067
05068 if( emcmotStruct != 0 && emcmotDebug != 0 && emcmotConfig != 0)
05069 {
05070 diagnostics("emcmot: disabling amps\n");
05071 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
05072 extAmpEnable(axis,
05073 ! GET_AXIS_ENABLE_POLARITY(axis));
05074 }
05075
05076
05077 diagnostics("emcmot: quitting analog, digital and motor interfaces\n");
05078 extAioQuit();
05079 extDioQuit();
05080 extMotQuit();
05081 }
05082
05083
05084 #ifdef rtai
05085 if(emcmotStruct != 0)
05086 {
05087 diagnostics("emcmot: releasing rtai shared memory.\n");
05088 rtai_kfree(SHMEM_KEY);
05089 emcmotStruct = 0;
05090 }
05091 #endif
05092
05093 #ifdef rtlinux
05094 #ifdef USE_RTL2
05095 mbuff_free("emcmotStruct", emcmotStruct);
05096 #else
05097 #if defined(rtlinux2)
05098 iounmap(emcmotStruct);
05099 #else
05100 vfree(emcmotStruct);
05101 #endif
05102 #endif
05103
05104
05105 #ifdef USE_RTL2
05106 rtl_setclockmode(local_clock, RTL_CLOCK_MODE_ONESHOT,0);
05107 #else
05108 rtl_set_oneshot_mode();
05109 #endif
05110 #endif
05111
05112 #if !defined(rtlinux) && !defined(rtai)
05113
05114 if (NULL != shmem) {
05115 rcs_shm_close(shmem);
05116 shmem = NULL;
05117 }
05118
05119 #endif
05120
05121 #ifdef rtai
05122 rt_umount_rtai();
05123 #endif
05124
05125
05126 diagnostics("emcmot: debounce counts: ");
05127 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
05128 diagnostics("%d ", debouncecount[axis]);
05129 }
05130 diagnostics("\n");
05131
05132 diagnostics("emcmot: cleanup finished.\n");
05133 }
05134
05135
05136
05137 #if !defined(rtlinux) && !defined(rtai)
05138 static int emcmot_done = 0;
05139
05140 static void emcmot_quit(int sig)
05141 {
05142 emcmot_done = 1;
05143 }
05144
05145 #ifndef UNDER_CE
05146 #include <signal.h>
05147 #endif
05148
05149 #ifndef UNDER_CE
05150 static int getArgs(int argc, char *argv[])
05151 {
05152 int t;
05153 int len;
05154
05155 for (t = 1; t < argc; t++) {
05156
05157
05158
05159
05160 if (!strcmp(argv[t], "-ini")) {
05161 if (t == argc - 1) {
05162 diagnostics("missing -ini parameter\n");
05163 return -1;
05164 }
05165 else {
05166 strcpy(EMCMOT_INIFILE, argv[t+1]);
05167 t++;
05168 continue;
05169 }
05170 }
05171
05172
05173 len = strlen("SHMEM_BASE_ADDRESS");
05174 if (! strncmp(argv[t], "SHMEM_BASE_ADDRESS", len)) {
05175 if (argv[t][len] != '=' ||
05176 1 != sscanf(&argv[t][len + 1], "%li", &SHMEM_BASE_ADDRESS)) {
05177 diagnostics("syntax: %s SHMEM_BASE_ADDRESS=integer\n", argv[0]);
05178 return -1;
05179 }
05180 else {
05181 continue;
05182 }
05183 }
05184
05185
05186 len = strlen("SHMEM_KEY");
05187 if (! strncmp(argv[t], "SHMEM_KEY", len)) {
05188 if (argv[t][len] != '=' ||
05189 1 != sscanf(&argv[t][len + 1], "%i", &SHMEM_KEY)) {
05190 diagnostics("syntax: %s SHMEM_KEY=integer\n", argv[0]);
05191 return -1;
05192 }
05193 else {
05194 continue;
05195 }
05196 }
05197
05198
05199
05200 if (!strcmp(argv[t], "-noforward")) {
05201 EMCMOT_NO_FORWARD_KINEMATICS = 1;
05202 kinType = KINEMATICS_INVERSE_ONLY;
05203 continue;
05204 }
05205
05206
05207 diagnostics("bad argument to %s: %s\n", argv[0], argv[t]);
05208 return -1;
05209 }
05210
05211 return 0;
05212 }
05213 #endif
05214
05215
05216
05217
05218
05219
05220 #ifndef UNDER_CE
05221 int main(int argc, char *argv[])
05222 #else
05223 int emcmotsim()
05224 #endif
05225 {
05226 double delta;
05227 int cycles=0;
05228 double start_time,cur_time;
05229 #ifndef UNDER_CE
05230
05231 signal(SIGINT, emcmot_quit);
05232
05233
05234 if (0 != getArgs(argc, argv)) {
05235 diagnostics("can't init module-- bad arguments\n");
05236 exit(1);
05237 }
05238 #endif
05239
05240
05241 if (-1 == init_module()) {
05242 diagnostics("can't init module-- execution permission problems?\n");
05243 #ifndef UNDER_CE
05244 exit(1);
05245 #else
05246 return 1;
05247 #endif
05248 }
05249
05250 start_time = etime();
05251 while (! emcmot_done) {
05252 cycles++;
05253 delta = etime();
05254 emcmotController(0);
05255 cur_time = etime();
05256 delta = cur_time - delta;
05257 delta = emcmotConfig->servoCycleTime - delta;
05258
05259 if (delta > 0.0) {
05260 esleep(delta);
05261 }
05262 }
05263
05264 cleanup_module();
05265 #ifndef UNDER_CE
05266 exit(0);
05267 #else
05268 return 0;
05269 #endif
05270 }
05271
05272 #endif