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 #include <string.h>
00118 #include <math.h>
00119 #include <float.h>
00120
00121 #include "rcs.hh"
00122 #include "nml_mod.hh"
00123 #include "emc.hh"
00124 #include "usrmotintf.h"
00125 #include "emcmot.h"
00126 #include "emcmotcfg.h"
00127 #include "canon.hh"
00128 #include "posemath.h"
00129 #include "initraj.hh"
00130 #include "iniaxis.hh"
00131 #include "inispin.hh"
00132 #include "emcglb.h"
00133 #include "emcpos.h"
00134
00135
00136 #ifndef __GNUC__
00137 #ifndef __attribute__
00138 #define __attribute__(x)
00139 #endif
00140 #endif
00141
00142 static char __attribute__((unused)) ident[] = "$Id: minimilltaskintf.cc,v 1.20 2001/08/17 22:05:41 proctor Exp $";
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158 #ifdef linux
00159 #include <stdio.h>
00160 #define ISNAN_TRAP
00161 #endif
00162
00163 static EMCMOT_COMMAND emcmotCommand;
00164
00165 static int emcmotTrajInited = 0;
00166 static int emcmotAxisInited = 0;
00167 static int emcmotIoInited = 0;
00168 static int emcmotion_initialized=0;
00169
00170
00171
00172
00173 static double lastVel = -1.0;
00174
00175
00176
00177
00178 static unsigned long localMotionHeartbeat = 0;
00179 static int localMotionCommandType = 0;
00180 static int localMotionEchoSerialNumber = 0;
00181 static unsigned char localEmcAxisAxisType[EMCMOT_MAX_AXIS];
00182 static double localEmcAxisUnits[EMCMOT_MAX_AXIS];
00183 static double localEmcMaxAcceleration = DBL_MAX;
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193 int emcAxisSetAxis(int axis, unsigned char axisType)
00194 {
00195 if (axis < 0 || axis >= EMCMOT_MAX_AXIS)
00196 {
00197 return 0;
00198 }
00199
00200 localEmcAxisAxisType[axis] = axisType;
00201
00202 return 0;
00203 }
00204
00205 int emcAxisSetUnits(int axis, double units)
00206 {
00207 if (axis < 0 || axis >= EMCMOT_MAX_AXIS)
00208 {
00209 return 0;
00210 }
00211
00212 localEmcAxisUnits[axis] = units;
00213
00214 return 0;
00215 }
00216
00217 int emcAxisSetGains(int axis, double p, double i, double d,
00218 double ff0, double ff1, double ff2,
00219 double backlash, double bias, double maxError,
00220 double deadband)
00221 {
00222 if (axis < 0 || axis >= EMCMOT_MAX_AXIS)
00223 {
00224 return 0;
00225 }
00226
00227 emcmotCommand.command = EMCMOT_SET_PID;
00228 emcmotCommand.axis = axis;
00229
00230 emcmotCommand.pid.p = p;
00231 emcmotCommand.pid.i = i;
00232 emcmotCommand.pid.d = d;
00233 emcmotCommand.pid.ff0 = ff0;
00234 emcmotCommand.pid.ff1 = ff1;
00235 emcmotCommand.pid.ff2 = ff2;
00236 emcmotCommand.pid.backlash = backlash;
00237 emcmotCommand.pid.bias = bias;
00238 emcmotCommand.pid.maxError = maxError;
00239 emcmotCommand.pid.deadband = deadband;
00240
00241 #ifdef ISNAN_TRAP
00242 if (isnan(emcmotCommand.pid.p) ||
00243 isnan(emcmotCommand.pid.i) ||
00244 isnan(emcmotCommand.pid.d) ||
00245 isnan(emcmotCommand.pid.ff0) ||
00246 isnan(emcmotCommand.pid.ff1) ||
00247 isnan(emcmotCommand.pid.ff2) ||
00248 isnan(emcmotCommand.pid.backlash) ||
00249 isnan(emcmotCommand.pid.bias) ||
00250 isnan(emcmotCommand.pid.maxError) ||
00251 isnan(emcmotCommand.pid.deadband))
00252 {
00253 printf("isnan error in emcAxisSetGains\n");
00254 return -1;
00255 }
00256 #endif
00257
00258 return usrmotWriteEmcmotCommand(&emcmotCommand);
00259 }
00260
00261 int emcAxisSetCycleTime(int axis, double cycleTime)
00262 {
00263 if (axis < 0 || axis >= EMCMOT_MAX_AXIS)
00264 {
00265 return 0;
00266 }
00267
00268 if (cycleTime <= 0.0)
00269 {
00270 return -1;
00271 }
00272
00273 emcmotCommand.command = EMCMOT_SET_SERVO_CYCLE_TIME;
00274 emcmotCommand.cycleTime = cycleTime;
00275
00276 return usrmotWriteEmcmotCommand(&emcmotCommand);
00277 }
00278
00279 int emcAxisSetInputScale(int axis, double scale, double offset)
00280 {
00281 if (axis < 0 || axis >= EMCMOT_MAX_AXIS)
00282 {
00283 return 0;
00284 }
00285
00286 emcmotCommand.command = EMCMOT_SET_INPUT_SCALE;
00287 emcmotCommand.axis = axis;
00288 emcmotCommand.scale = scale;
00289 emcmotCommand.offset = offset;
00290
00291 #ifdef ISNAN_TRAP
00292 if (isnan(emcmotCommand.scale) ||
00293 isnan(emcmotCommand.offset))
00294 {
00295 printf("isnan eror in emcAxisSetInputScale\n");
00296 return -1;
00297 }
00298 #endif
00299
00300 return usrmotWriteEmcmotCommand(&emcmotCommand);
00301 }
00302
00303 int emcAxisSetOutputScale(int axis, double scale, double offset)
00304 {
00305 if (axis < 0 || axis >= EMCMOT_MAX_AXIS)
00306 {
00307 return 0;
00308 }
00309
00310 emcmotCommand.command = EMCMOT_SET_OUTPUT_SCALE;
00311 emcmotCommand.axis = axis;
00312 emcmotCommand.scale = scale;
00313 emcmotCommand.offset = offset;
00314
00315 #ifdef ISNAN_TRAP
00316 if (isnan(emcmotCommand.scale) ||
00317 isnan(emcmotCommand.offset))
00318 {
00319 printf("isnan eror in emcAxisSetOutputScale\n");
00320 return -1;
00321 }
00322 #endif
00323
00324 return usrmotWriteEmcmotCommand(&emcmotCommand);
00325 }
00326
00327
00328
00329 static double saveMinLimit[EMCMOT_MAX_AXIS];
00330 static double saveMaxLimit[EMCMOT_MAX_AXIS];
00331
00332 int emcAxisSetMinPositionLimit(int axis, double limit)
00333 {
00334 if (axis < 0 || axis >= EMCMOT_MAX_AXIS)
00335 {
00336 return 0;
00337 }
00338
00339 emcmotCommand.command = EMCMOT_SET_POSITION_LIMITS;
00340 emcmotCommand.axis = axis;
00341 emcmotCommand.maxLimit = saveMaxLimit[axis];
00342 emcmotCommand.minLimit = limit;
00343 saveMinLimit[axis] = limit;
00344
00345 #ifdef ISNAN_TRAP
00346 if (isnan(emcmotCommand.maxLimit) ||
00347 isnan(emcmotCommand.minLimit))
00348 {
00349 printf("isnan error in emcAxisSetMinPosition\n");
00350 return -1;
00351 }
00352 #endif
00353
00354 return usrmotWriteEmcmotCommand(&emcmotCommand);
00355 }
00356
00357 int emcAxisSetMaxPositionLimit(int axis, double limit)
00358 {
00359 if (axis < 0 || axis >= EMCMOT_MAX_AXIS)
00360 {
00361 return 0;
00362 }
00363
00364 emcmotCommand.command = EMCMOT_SET_POSITION_LIMITS;
00365 emcmotCommand.axis = axis;
00366 emcmotCommand.minLimit = saveMinLimit[axis];
00367 emcmotCommand.maxLimit = limit;
00368 saveMaxLimit[axis] = limit;
00369
00370 #ifdef ISNAN_TRAP
00371 if (isnan(emcmotCommand.maxLimit) ||
00372 isnan(emcmotCommand.minLimit))
00373 {
00374 printf("isnan error in emcAxisSetMaxPosition\n");
00375 return -1;
00376 }
00377 #endif
00378
00379 return usrmotWriteEmcmotCommand(&emcmotCommand);
00380 }
00381
00382
00383
00384 static double saveMinOutput[EMCMOT_MAX_AXIS];
00385 static double saveMaxOutput[EMCMOT_MAX_AXIS];
00386
00387 int emcAxisSetMinOutputLimit(int axis, double limit)
00388 {
00389 if (axis < 0 || axis >= EMCMOT_MAX_AXIS)
00390 {
00391 return 0;
00392 }
00393
00394 emcmotCommand.command = EMCMOT_SET_OUTPUT_LIMITS;
00395 emcmotCommand.axis = axis;
00396 emcmotCommand.maxLimit = saveMaxOutput[axis];
00397 emcmotCommand.minLimit = limit;
00398 saveMinOutput[axis] = limit;
00399
00400 #ifdef ISNAN_TRAP
00401 if (isnan(emcmotCommand.maxLimit) ||
00402 isnan(emcmotCommand.minLimit))
00403 {
00404 printf("isnan error in emcAxisSetMinOutputLimit\n");
00405 return -1;
00406 }
00407 #endif
00408
00409 return usrmotWriteEmcmotCommand(&emcmotCommand);
00410 }
00411
00412 int emcAxisSetMaxOutputLimit(int axis, double limit)
00413 {
00414 if (axis < 0 || axis >= EMCMOT_MAX_AXIS)
00415 {
00416 return 0;
00417 }
00418
00419 emcmotCommand.command = EMCMOT_SET_OUTPUT_LIMITS;
00420 emcmotCommand.axis = axis;
00421 emcmotCommand.minLimit = saveMinOutput[axis];
00422 emcmotCommand.maxLimit = limit;
00423 saveMaxOutput[axis] = limit;
00424
00425 #ifdef ISNAN_TRAP
00426 if (isnan(emcmotCommand.maxLimit) ||
00427 isnan(emcmotCommand.minLimit))
00428 {
00429 printf("isnan error in emcAxisSetMaxOutputLimit\n");
00430 return -1;
00431 }
00432 #endif
00433
00434 return usrmotWriteEmcmotCommand(&emcmotCommand);
00435 }
00436
00437 int emcAxisSetFerror(int axis, double ferror)
00438 {
00439 if (axis < 0 || axis >= EMCMOT_MAX_AXIS)
00440 {
00441 return 0;
00442 }
00443
00444 emcmotCommand.command = EMCMOT_SET_MAX_FERROR;
00445 emcmotCommand.axis = axis;
00446 emcmotCommand.maxFerror = ferror;
00447
00448 #ifdef ISNAN_TRAP
00449 if (isnan(emcmotCommand.maxFerror))
00450 {
00451 printf("isnan error in emcAxisSetFerror\n");
00452 return -1;
00453 }
00454 #endif
00455
00456 return usrmotWriteEmcmotCommand(&emcmotCommand);
00457 }
00458
00459 int emcAxisSetMinFerror(int axis, double ferror)
00460 {
00461 if (axis < 0 || axis >= EMCMOT_MAX_AXIS)
00462 {
00463 return 0;
00464 }
00465
00466 emcmotCommand.command = EMCMOT_SET_MIN_FERROR;
00467 emcmotCommand.axis = axis;
00468 emcmotCommand.minFerror = ferror;
00469
00470 #ifdef ISNAN_TRAP
00471 if (isnan(emcmotCommand.minFerror))
00472 {
00473 printf("isnan error in emcAxisSetMinFerror\n");
00474 return -1;
00475 }
00476 #endif
00477
00478 return usrmotWriteEmcmotCommand(&emcmotCommand);
00479 }
00480
00481 int emcAxisSetHomingVel(int axis, double homingVel)
00482 {
00483 if (axis < 0 || axis >= EMCMOT_MAX_AXIS)
00484 {
00485 return 0;
00486 }
00487
00488 emcmotCommand.command = EMCMOT_SET_HOMING_VEL;
00489 emcmotCommand.axis = axis;
00490 emcmotCommand.vel = homingVel;
00491
00492 #ifdef ISNAN_TRAP
00493 if (isnan(emcmotCommand.vel))
00494 {
00495 printf("isnan error in emcAxisSetHomingVel\n");
00496 return -1;
00497 }
00498 #endif
00499
00500 return usrmotWriteEmcmotCommand(&emcmotCommand);
00501 }
00502
00503 int emcAxisSetMaxVelocity(int axis, double vel)
00504 {
00505 if (axis < 0 || axis >= EMC_AXIS_MAX)
00506 {
00507 return 0;
00508 }
00509
00510 if (vel < 0.0)
00511 {
00512 vel = 0.0;
00513 }
00514
00515 AXIS_MAX_VELOCITY[axis] = vel;
00516
00517 emcmotCommand.command = EMCMOT_SET_AXIS_VEL_LIMIT;
00518 emcmotCommand.axis = axis;
00519 emcmotCommand.vel = vel;
00520
00521 return usrmotWriteEmcmotCommand(&emcmotCommand);
00522 }
00523
00524 int emcAxisSetHomeOffset(int axis, double offset)
00525 {
00526 if (axis < 0 || axis >= EMCMOT_MAX_AXIS)
00527 {
00528 return 0;
00529 }
00530
00531 emcmotCommand.command = EMCMOT_SET_HOME_OFFSET;
00532 emcmotCommand.axis = axis;
00533 emcmotCommand.offset = offset;
00534
00535 #ifdef ISNAN_TRAP
00536 if (isnan(emcmotCommand.offset))
00537 {
00538 printf("isnan error in emcAxisSetHomeOffset\n");
00539 return -1;
00540 }
00541 #endif
00542
00543 return usrmotWriteEmcmotCommand(&emcmotCommand);
00544 }
00545
00546 int emcAxisSetEnablePolarity(int axis, int level)
00547 {
00548 if (axis < 0 || axis >= EMCMOT_MAX_AXIS)
00549 {
00550 return 0;
00551 }
00552
00553 emcmotCommand.command = EMCMOT_SET_POLARITY;
00554 emcmotCommand.axis = axis;
00555 emcmotCommand.level = level;
00556 emcmotCommand.axisFlag = EMCMOT_AXIS_ENABLE_BIT;
00557
00558 return usrmotWriteEmcmotCommand(&emcmotCommand);
00559 }
00560
00561 int emcAxisSetMinLimitSwitchPolarity(int axis, int level)
00562 {
00563 if (axis < 0 || axis >= EMCMOT_MAX_AXIS)
00564 {
00565 return 0;
00566 }
00567
00568 emcmotCommand.command = EMCMOT_SET_POLARITY;
00569 emcmotCommand.axis = axis;
00570 emcmotCommand.level = level;
00571 emcmotCommand.axisFlag = EMCMOT_AXIS_MIN_HARD_LIMIT_BIT;
00572
00573 return usrmotWriteEmcmotCommand(&emcmotCommand);
00574 }
00575
00576 int emcAxisSetMaxLimitSwitchPolarity(int axis, int level)
00577 {
00578 if (axis < 0)
00579 {
00580 axis = 0;
00581 }
00582 else if (axis >= EMCMOT_MAX_AXIS)
00583 {
00584 axis = EMCMOT_MAX_AXIS - 1;
00585 }
00586
00587 emcmotCommand.command = EMCMOT_SET_POLARITY;
00588 emcmotCommand.axis = axis;
00589 emcmotCommand.level = level;
00590 emcmotCommand.axisFlag = EMCMOT_AXIS_MAX_HARD_LIMIT_BIT;
00591
00592 return usrmotWriteEmcmotCommand(&emcmotCommand);
00593 }
00594
00595 int emcAxisSetHomeSwitchPolarity(int axis, int level)
00596 {
00597 if (axis < 0 || axis >= EMCMOT_MAX_AXIS)
00598 {
00599 return 0;
00600 }
00601
00602 emcmotCommand.command = EMCMOT_SET_POLARITY;
00603 emcmotCommand.axis = axis;
00604 emcmotCommand.level = level;
00605 emcmotCommand.axisFlag = EMCMOT_AXIS_HOME_SWITCH_BIT;
00606
00607 return usrmotWriteEmcmotCommand(&emcmotCommand);
00608 }
00609
00610 int emcAxisSetHomingPolarity(int axis, int level)
00611 {
00612 if (axis < 0 || axis >= EMCMOT_MAX_AXIS)
00613 {
00614 return 0;
00615 }
00616
00617 emcmotCommand.command = EMCMOT_SET_POLARITY;
00618 emcmotCommand.axis = axis;
00619 emcmotCommand.level = level;
00620 emcmotCommand.axisFlag = EMCMOT_AXIS_HOMING_BIT;
00621
00622 return usrmotWriteEmcmotCommand(&emcmotCommand);
00623 }
00624
00625 int emcAxisSetFaultPolarity(int axis, int level)
00626 {
00627 if (axis < 0 || axis >= EMCMOT_MAX_AXIS)
00628 {
00629 return 0;
00630 }
00631
00632 emcmotCommand.command = EMCMOT_SET_POLARITY;
00633 emcmotCommand.axis = axis;
00634 emcmotCommand.level = level;
00635 emcmotCommand.axisFlag = EMCMOT_AXIS_FAULT_BIT;
00636
00637 return usrmotWriteEmcmotCommand(&emcmotCommand);
00638 }
00639
00640 int emcAxisInit(int axis)
00641 {
00642 int retval = 0;
00643
00644 if (axis < 0 || axis >= EMCMOT_MAX_AXIS)
00645 {
00646 return 0;
00647 }
00648
00649
00650 if (! emcmotAxisInited &&
00651 ! emcmotTrajInited &&
00652 ! emcmotIoInited)
00653 {
00654 usrmotIniLoad(EMC_INIFILE);
00655
00656 if (0 != usrmotInit())
00657 {
00658 return -1;
00659 }
00660 }
00661 emcmotAxisInited = 1;
00662
00663 if (0 != iniAxis(axis, EMC_INIFILE))
00664 {
00665 retval = -1;
00666 }
00667
00668 return retval;
00669 }
00670
00671 int emcAxisHalt(int axis)
00672 {
00673 if (axis < 0 || axis >= EMCMOT_MAX_AXIS)
00674 {
00675 return 0;
00676 }
00677
00678
00679 if(NULL != emcStatus && emcmotion_initialized && emcmotAxisInited)
00680 {
00681 dumpAxis(axis, EMC_INIFILE, &emcStatus->motion.axis[axis]);
00682 }
00683
00684 if (! emcmotTrajInited &&
00685 ! emcmotIoInited &&
00686 emcmotAxisInited)
00687 {
00688 usrmotExit();
00689 }
00690
00691
00692 emcmotAxisInited = 0;
00693
00694 return 0;
00695 }
00696
00697 int emcAxisAbort(int axis)
00698 {
00699 if (axis < 0 || axis >= EMCMOT_MAX_AXIS)
00700 {
00701 return 0;
00702 }
00703
00704 emcmotCommand.command = EMCMOT_ABORT;
00705 emcmotCommand.axis = axis;
00706
00707 return usrmotWriteEmcmotCommand(&emcmotCommand);
00708 }
00709
00710 int emcAxisActivate(int axis)
00711 {
00712 if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
00713 return 0;
00714 }
00715
00716 emcmotCommand.command = EMCMOT_ACTIVATE_AXIS;
00717 emcmotCommand.axis = axis;
00718
00719 return usrmotWriteEmcmotCommand(&emcmotCommand);
00720 }
00721
00722 int emcAxisDeactivate(int axis)
00723 {
00724 if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
00725 return 0;
00726 }
00727
00728 emcmotCommand.command = EMCMOT_DEACTIVATE_AXIS;
00729 emcmotCommand.axis = axis;
00730
00731 return usrmotWriteEmcmotCommand(&emcmotCommand);
00732 }
00733
00734 int emcAxisOverrideLimits(int axis)
00735 {
00736
00737 if (axis >= EMCMOT_MAX_AXIS) {
00738 return 0;
00739 }
00740
00741 emcmotCommand.command = EMCMOT_OVERRIDE_LIMITS;
00742 emcmotCommand.axis = axis;
00743
00744 return usrmotWriteEmcmotCommand(&emcmotCommand);
00745 }
00746
00747 int emcAxisSetOutput(int axis, double output)
00748 {
00749 if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
00750 return 0;
00751 }
00752
00753 emcmotCommand.command = EMCMOT_DAC_OUT;
00754 emcmotCommand.axis = axis;
00755 emcmotCommand.dacOut = output;
00756
00757 return usrmotWriteEmcmotCommand(&emcmotCommand);
00758 }
00759
00760 int emcAxisEnable(int axis)
00761 {
00762 if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
00763 return 0;
00764 }
00765
00766 emcmotCommand.command = EMCMOT_ENABLE_AMPLIFIER;
00767 emcmotCommand.axis = axis;
00768
00769 return usrmotWriteEmcmotCommand(&emcmotCommand);
00770 }
00771
00772 int emcAxisDisable(int axis)
00773 {
00774 if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
00775 return 0;
00776 }
00777
00778 emcmotCommand.command = EMCMOT_DISABLE_AMPLIFIER;
00779 emcmotCommand.axis = axis;
00780
00781 return usrmotWriteEmcmotCommand(&emcmotCommand);
00782 }
00783
00784 int emcAxisHome(int axis)
00785 {
00786 if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
00787 return 0;
00788 }
00789
00790 emcmotCommand.command = EMCMOT_HOME;
00791 emcmotCommand.axis = axis;
00792
00793 return usrmotWriteEmcmotCommand(&emcmotCommand);
00794 }
00795
00796 int emcAxisSetHome(int axis, double home)
00797 {
00798 if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
00799 return 0;
00800 }
00801
00802 emcmotCommand.command = EMCMOT_SET_JOINT_HOME;
00803 emcmotCommand.axis = axis;
00804 emcmotCommand.offset = home;
00805
00806 return usrmotWriteEmcmotCommand(&emcmotCommand);
00807 }
00808
00809 int emcAxisJog(int axis, double vel)
00810 {
00811 if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
00812 return 0;
00813 }
00814
00815 if (vel > AXIS_MAX_VELOCITY[axis]) {
00816 vel = AXIS_MAX_VELOCITY[axis];
00817 }
00818 else if (vel < -AXIS_MAX_VELOCITY[axis]) {
00819 vel = -AXIS_MAX_VELOCITY[axis];
00820 }
00821
00822 emcmotCommand.command = EMCMOT_JOG_CONT;
00823 emcmotCommand.axis = axis;
00824 emcmotCommand.vel = vel;
00825
00826 return usrmotWriteEmcmotCommand(&emcmotCommand);
00827 }
00828
00829 int emcAxisIncrJog(int axis, double incr, double vel)
00830 {
00831 if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
00832 return 0;
00833 }
00834
00835 if (vel > AXIS_MAX_VELOCITY[axis]) {
00836 vel = AXIS_MAX_VELOCITY[axis];
00837 }
00838 else if (vel < -AXIS_MAX_VELOCITY[axis]) {
00839 vel = -AXIS_MAX_VELOCITY[axis];
00840 }
00841
00842 emcmotCommand.command = EMCMOT_JOG_INCR;
00843 emcmotCommand.axis = axis;
00844 emcmotCommand.vel = vel;
00845 emcmotCommand.offset = incr;
00846
00847 return usrmotWriteEmcmotCommand(&emcmotCommand);
00848 }
00849
00850 int emcAxisAbsJog(int axis, double pos, double vel)
00851 {
00852 if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
00853 return 0;
00854 }
00855
00856 if (vel > AXIS_MAX_VELOCITY[axis]) {
00857 vel = AXIS_MAX_VELOCITY[axis];
00858 }
00859 else if (vel < -AXIS_MAX_VELOCITY[axis]) {
00860 vel = -AXIS_MAX_VELOCITY[axis];
00861 }
00862
00863 emcmotCommand.command = EMCMOT_JOG_ABS;
00864 emcmotCommand.axis = axis;
00865 emcmotCommand.vel = vel;
00866 emcmotCommand.offset = pos;
00867
00868 return usrmotWriteEmcmotCommand(&emcmotCommand);
00869 }
00870
00871 int emcAxisLoadComp(int axis, const char * file)
00872 {
00873 return usrmotLoadComp(axis, file);
00874 }
00875
00876 int emcAxisAlter(int axis, double alter)
00877 {
00878 return usrmotAlter(axis, alter);
00879 }
00880
00881 static EMCMOT_CONFIG emcmotConfig;
00882 int get_emcmot_debug_info = 0;
00883
00884
00885
00886
00887
00888 static EMCMOT_STATUS emcmotStatus;
00889 static EMCMOT_DEBUG emcmotDebug;
00890 static char errorString[EMCMOT_ERROR_LEN];
00891 static int new_config = 0;
00892
00893 int emcAxisUpdate(EMC_AXIS_STAT stat[], int numAxes)
00894 {
00895 int axis;
00896
00897
00898 if (numAxes <= 0 || numAxes > EMCMOT_MAX_AXIS) {
00899 return -1;
00900 }
00901
00902 for (axis = 0; axis < numAxes; axis++) {
00903 stat[axis].axisType = localEmcAxisAxisType[axis];
00904 stat[axis].units = localEmcAxisUnits[axis];
00905
00906 stat[axis].inputScale = emcmotStatus.inputScale[axis];
00907 stat[axis].inputOffset = emcmotStatus.inputOffset[axis];
00908 stat[axis].outputScale = emcmotStatus.outputScale[axis];
00909 stat[axis].outputOffset = emcmotStatus.outputOffset[axis];
00910
00911 if (new_config) {
00912 stat[axis].p = emcmotConfig.pid[axis].p;
00913 stat[axis].i = emcmotConfig.pid[axis].i;
00914 stat[axis].d = emcmotConfig.pid[axis].d;
00915 stat[axis].ff0 = emcmotConfig.pid[axis].ff0;
00916 stat[axis].ff1 = emcmotConfig.pid[axis].ff1;
00917 stat[axis].ff2 = emcmotConfig.pid[axis].ff2;
00918 stat[axis].backlash = emcmotConfig.pid[axis].backlash;
00919 stat[axis].bias = emcmotConfig.pid[axis].bias;
00920 stat[axis].maxError = emcmotConfig.pid[axis].maxError;
00921 stat[axis].deadband = emcmotConfig.pid[axis].deadband;
00922 stat[axis].cycleTime = emcmotConfig.servoCycleTime;
00923 stat[axis].minPositionLimit = emcmotConfig.minLimit[axis];
00924 stat[axis].maxPositionLimit = emcmotConfig.maxLimit[axis];
00925 stat[axis].minOutputLimit = emcmotConfig.minOutput[axis];
00926 stat[axis].maxOutputLimit = emcmotConfig.maxOutput[axis];
00927 stat[axis].minFerror = emcmotConfig.minFerror[axis];
00928 stat[axis].maxFerror = emcmotConfig.maxFerror[axis];
00929 stat[axis].homeOffset = emcmotConfig.homeOffset[axis];
00930 stat[axis].enablePolarity = (emcmotConfig.axisPolarity[axis] &
00931 EMCMOT_AXIS_ENABLE_BIT) ? 1 : 0;
00932 stat[axis].minLimitSwitchPolarity = (emcmotConfig.axisPolarity[axis] &
00933 EMCMOT_AXIS_MIN_HARD_LIMIT_BIT) ? 1 : 0;
00934 stat[axis].maxLimitSwitchPolarity = (emcmotConfig.axisPolarity[axis] &
00935 EMCMOT_AXIS_MAX_HARD_LIMIT_BIT) ? 1 : 0;
00936 stat[axis].homeSwitchPolarity = (emcmotConfig.axisPolarity[axis] &
00937 EMCMOT_AXIS_HOME_SWITCH_BIT) ? 1 : 0;
00938 stat[axis].homingPolarity = (emcmotConfig.axisPolarity[axis] &
00939 EMCMOT_AXIS_HOMING_BIT) ? 1 : 0;
00940 stat[axis].faultPolarity = (emcmotConfig.axisPolarity[axis] &
00941 EMCMOT_AXIS_FAULT_BIT) ? 1 : 0;
00942 }
00943
00944 stat[axis].setpoint = emcmotStatus.axisPos[axis];
00945
00946 if (get_emcmot_debug_info) {
00947 stat[axis].ferrorCurrent = emcmotDebug.ferrorCurrent[axis];
00948 stat[axis].ferrorHighMark = emcmotDebug.ferrorHighMark[axis];
00949 }
00950
00951 stat[axis].output = emcmotStatus.output[axis];
00952 stat[axis].input = emcmotStatus.input[axis];
00953 stat[axis].homing = (emcmotStatus.axisFlag[axis] & EMCMOT_AXIS_HOMING_BIT ? 1 : 0);
00954 stat[axis].homed = (emcmotStatus.axisFlag[axis] & EMCMOT_AXIS_HOMED_BIT ? 1 : 0);
00955 stat[axis].fault = (emcmotStatus.axisFlag[axis] & EMCMOT_AXIS_FAULT_BIT ? 1 : 0);
00956 stat[axis].enabled = (emcmotStatus.axisFlag[axis] & EMCMOT_AXIS_ENABLE_BIT ? 1 : 0);
00957
00958 stat[axis].minSoftLimit = (emcmotStatus.axisFlag[axis] & EMCMOT_AXIS_MIN_SOFT_LIMIT_BIT ? 1 : 0);
00959 stat[axis].maxSoftLimit = (emcmotStatus.axisFlag[axis] & EMCMOT_AXIS_MAX_SOFT_LIMIT_BIT ? 1 : 0);
00960 stat[axis].minHardLimit = (emcmotStatus.axisFlag[axis] & EMCMOT_AXIS_MIN_HARD_LIMIT_BIT ? 1 : 0);
00961 stat[axis].maxHardLimit = (emcmotStatus.axisFlag[axis] & EMCMOT_AXIS_MAX_HARD_LIMIT_BIT ? 1 : 0);
00962 stat[axis].overrideLimits = (emcmotStatus.overrideLimits);
00963
00964 stat[axis].scale = emcmotStatus.axVscale[axis];
00965 usrmotQueryAlter(axis, &stat[axis].alter);
00966
00967 if (emcmotStatus.axisFlag[axis] & EMCMOT_AXIS_ERROR_BIT) {
00968 if (stat[axis].status != RCS_ERROR) {
00969 rcs_print_error("Error on axis %d.\n",axis);
00970 stat[axis].status = RCS_ERROR;
00971 }
00972 }
00973 else if (emcmotStatus.axisFlag[axis] & EMCMOT_AXIS_INPOS_BIT) {
00974 stat[axis].status = RCS_DONE;
00975 }
00976 else {
00977 stat[axis].status = RCS_EXEC;
00978 }
00979 }
00980
00981 return 0;
00982 }
00983
00984
00985
00986
00987 static int localEmcTrajAxes = 0;
00988 static double localEmcTrajLinearUnits = 1.0;
00989 static double localEmcTrajAngularUnits = 1.0;
00990 static int localEmcTrajMotionId = 0;
00991
00992 int emcTrajSetAxes(int axes)
00993 {
00994 if (axes <= 0 || axes > EMCMOT_MAX_AXIS)
00995 {
00996 return -1;
00997 }
00998
00999 localEmcTrajAxes = axes;
01000
01001 return 0;
01002 }
01003
01004 int emcTrajSetUnits(double linearUnits, double angularUnits)
01005 {
01006 if (linearUnits <= 0.0 ||
01007 angularUnits <= 0.0)
01008 {
01009 return -1;
01010 }
01011
01012 localEmcTrajLinearUnits = linearUnits;
01013 localEmcTrajAngularUnits = angularUnits;
01014
01015 return 0;
01016 }
01017
01018 int emcTrajSetCycleTime(double cycleTime)
01019 {
01020 if (cycleTime <= 0.0)
01021 {
01022 return -1;
01023 }
01024
01025 emcmotCommand.command = EMCMOT_SET_TRAJ_CYCLE_TIME;
01026 emcmotCommand.cycleTime = cycleTime;
01027
01028 return usrmotWriteEmcmotCommand(&emcmotCommand);
01029 }
01030
01031 int emcTrajSetMode(int mode)
01032 {
01033 switch (mode)
01034 {
01035 case EMC_TRAJ_MODE_FREE:
01036 emcmotCommand.command = EMCMOT_FREE;
01037 return usrmotWriteEmcmotCommand(&emcmotCommand);
01038
01039 case EMC_TRAJ_MODE_COORD:
01040 emcmotCommand.command = EMCMOT_COORD;
01041 return usrmotWriteEmcmotCommand(&emcmotCommand);
01042
01043 case EMC_TRAJ_MODE_TELEOP:
01044 emcmotCommand.command = EMCMOT_TELEOP;
01045 return usrmotWriteEmcmotCommand(&emcmotCommand);
01046
01047 default:
01048 return -1;
01049 }
01050 }
01051
01052
01053 int emcTrajSetTeleopVector(EmcPose vel)
01054 {
01055 emcmotCommand.command = EMCMOT_SET_TELEOP_VECTOR;
01056 emcmotCommand.pos = vel;
01057 return usrmotWriteEmcmotCommand(&emcmotCommand);
01058 }
01059
01060
01061 int emcTrajSetVelocity(double vel)
01062 {
01063 int retval;
01064
01065 if (vel < 0.0)
01066 {
01067 vel = 0.0;
01068 }
01069 else if (vel > TRAJ_MAX_VELOCITY)
01070 {
01071 vel = TRAJ_MAX_VELOCITY;
01072 }
01073
01074 #if 0
01075
01076 if (vel == lastVel)
01077 {
01078
01079 return 0;
01080 }
01081 #endif
01082
01083 emcmotCommand.command = EMCMOT_SET_VEL;
01084 emcmotCommand.vel = vel;
01085
01086 retval = usrmotWriteEmcmotCommand(&emcmotCommand);
01087
01088 if (0 == retval)
01089 {
01090 lastVel = vel;
01091 }
01092
01093 return retval;
01094 }
01095
01096 int emcTrajSetAcceleration(double acc)
01097 {
01098 if (acc < 0.0)
01099 {
01100 acc = 0.0;
01101 }
01102 else if (acc > localEmcMaxAcceleration)
01103 {
01104 acc = localEmcMaxAcceleration;
01105 }
01106
01107 emcmotCommand.command = EMCMOT_SET_ACC;
01108 emcmotCommand.acc = acc;
01109
01110 return usrmotWriteEmcmotCommand(&emcmotCommand);
01111 }
01112
01113
01114
01115
01116
01117 int emcTrajSetMaxVelocity(double vel)
01118 {
01119 if (vel < 0.0)
01120 {
01121 vel = 0.0;
01122 }
01123
01124 TRAJ_MAX_VELOCITY = vel;
01125
01126 emcmotCommand.command = EMCMOT_SET_VEL_LIMIT;
01127 emcmotCommand.vel = vel;
01128
01129 return usrmotWriteEmcmotCommand(&emcmotCommand);
01130 }
01131
01132 int emcTrajSetMaxAcceleration(double acc)
01133 {
01134 if (acc < 0.0)
01135 {
01136 acc = 0.0;
01137 }
01138
01139 localEmcMaxAcceleration = acc;
01140
01141 return 0;
01142 }
01143
01144 int emcTrajSetHome(EmcPose home)
01145 {
01146 emcmotCommand.command = EMCMOT_SET_WORLD_HOME;
01147
01148 emcmotCommand.pos = home;
01149
01150
01151 #ifdef ISNAN_TRAP
01152 if (isnan(emcmotCommand.pos.tran.x) ||
01153 isnan(emcmotCommand.pos.tran.y) ||
01154 isnan(emcmotCommand.pos.tran.z))
01155 {
01156 printf("isnan error in emcTrajSetHome\n");
01157 return 0;
01158 }
01159 #endif
01160
01161 return usrmotWriteEmcmotCommand(&emcmotCommand);
01162 }
01163
01164 int emcTrajSetScale(double scale)
01165 {
01166 if (scale < 0.0)
01167 {
01168 scale = 0.0;
01169 }
01170
01171 emcmotCommand.command = EMCMOT_SCALE;
01172 emcmotCommand.scale = scale;
01173
01174 return usrmotWriteEmcmotCommand(&emcmotCommand);
01175 }
01176
01177 int emcTrajSetMotionId(int id)
01178 {
01179
01180 if(EMC_DEBUG_MOTION_TIME & EMC_DEBUG)
01181 {
01182 if(id != localEmcTrajMotionId)
01183 {
01184 rcs_print("Outgoing motion id is %d.\n",id);
01185 }
01186 }
01187
01188 localEmcTrajMotionId = id;
01189
01190 return 0;
01191 }
01192
01193 int emcTrajInit()
01194 {
01195 int retval = 0;
01196
01197
01198 if (! emcmotAxisInited &&
01199 ! emcmotTrajInited &&
01200 ! emcmotIoInited)
01201 {
01202 usrmotIniLoad(EMC_INIFILE);
01203
01204 if (0 != usrmotInit())
01205 {
01206 return -1;
01207 }
01208 }
01209 emcmotTrajInited = 1;
01210
01211
01212 if (0 != iniTraj(EMC_INIFILE))
01213 {
01214 retval = -1;
01215 }
01216
01217 return retval;
01218 }
01219
01220 int emcTrajHalt()
01221 {
01222 if (! emcmotAxisInited &&
01223 ! emcmotIoInited &&
01224 emcmotTrajInited)
01225 {
01226 usrmotExit();
01227 }
01228
01229
01230 emcmotTrajInited = 0;
01231
01232 return 0;
01233 }
01234
01235 int emcTrajEnable()
01236 {
01237 emcmotCommand.command = EMCMOT_ENABLE;
01238
01239 return usrmotWriteEmcmotCommand(&emcmotCommand);
01240 }
01241
01242 int emcTrajDisable()
01243 {
01244 emcmotCommand.command = EMCMOT_DISABLE;
01245
01246 return usrmotWriteEmcmotCommand(&emcmotCommand);
01247 }
01248
01249 int emcTrajAbort()
01250 {
01251 emcmotCommand.command = EMCMOT_ABORT;
01252
01253 return usrmotWriteEmcmotCommand(&emcmotCommand);
01254 }
01255
01256 int emcTrajPause()
01257 {
01258 emcmotCommand.command = EMCMOT_PAUSE;
01259
01260 return usrmotWriteEmcmotCommand(&emcmotCommand);
01261 }
01262
01263 int emcTrajStep()
01264 {
01265 emcmotCommand.command = EMCMOT_STEP;
01266
01267 return usrmotWriteEmcmotCommand(&emcmotCommand);
01268 }
01269
01270 int emcTrajResume()
01271 {
01272 emcmotCommand.command = EMCMOT_RESUME;
01273
01274 return usrmotWriteEmcmotCommand(&emcmotCommand);
01275 }
01276
01277 int emcTrajDelay(double delay)
01278 {
01279
01280
01281 return 0;
01282 }
01283
01284 int emcTrajSetTermCond(int cond)
01285 {
01286 emcmotCommand.command = EMCMOT_SET_TERM_COND;
01287 emcmotCommand.termCond = (cond == EMC_TRAJ_TERM_COND_STOP ? EMCMOT_TERM_COND_STOP : EMCMOT_TERM_COND_BLEND);
01288
01289 return usrmotWriteEmcmotCommand(&emcmotCommand);
01290 }
01291
01292 int emcTrajLinearMove(EmcPose end)
01293 {
01294 emcmotCommand.command = EMCMOT_SET_LINE;
01295
01296 emcmotCommand.pos = end;
01297
01298 emcmotCommand.id = localEmcTrajMotionId;
01299
01300 #ifdef ISNAN_TRAP
01301 if (isnan(emcmotCommand.pos.tran.x) ||
01302 isnan(emcmotCommand.pos.tran.y) ||
01303 isnan(emcmotCommand.pos.tran.z))
01304 {
01305 printf("isnan error in emcTrajLinearMove\n");
01306 return 0;
01307 }
01308 #endif
01309
01310 return usrmotWriteEmcmotCommand(&emcmotCommand);
01311 }
01312
01313 int emcTrajCircularMove(EmcPose end, PM_CARTESIAN center,
01314 PM_CARTESIAN normal, int turn)
01315 {
01316 emcmotCommand.command = EMCMOT_SET_CIRCLE;
01317
01318 emcmotCommand.pos = end;
01319
01320 emcmotCommand.center.x = center.x;
01321 emcmotCommand.center.y = center.y;
01322 emcmotCommand.center.z = center.z;
01323
01324 emcmotCommand.normal.x = normal.x;
01325 emcmotCommand.normal.y = normal.y;
01326 emcmotCommand.normal.z = normal.z;
01327
01328 emcmotCommand.turn = turn;
01329 emcmotCommand.id = localEmcTrajMotionId;
01330
01331 #ifdef ISNAN_TRAP
01332 if (isnan(emcmotCommand.pos.tran.x) ||
01333 isnan(emcmotCommand.pos.tran.y) ||
01334 isnan(emcmotCommand.pos.tran.z) ||
01335 isnan(emcmotCommand.pos.a) ||
01336 isnan(emcmotCommand.pos.b) ||
01337 isnan(emcmotCommand.pos.c) ||
01338 isnan(emcmotCommand.center.x) ||
01339 isnan(emcmotCommand.center.y) ||
01340 isnan(emcmotCommand.center.z) ||
01341 isnan(emcmotCommand.normal.x) ||
01342 isnan(emcmotCommand.normal.y) ||
01343 isnan(emcmotCommand.normal.z))
01344 {
01345 printf("isnan error in emcTrajCircularMove\n");
01346 return 0;
01347 }
01348 #endif
01349
01350 return usrmotWriteEmcmotCommand(&emcmotCommand);
01351 }
01352
01353 int emcTrajSetProbeIndex(int index)
01354 {
01355 emcmotCommand.command = EMCMOT_SET_PROBE_INDEX;
01356 emcmotCommand.probeIndex = index;
01357
01358 return usrmotWriteEmcmotCommand(&emcmotCommand);
01359 }
01360
01361 int emcTrajSetProbePolarity(int polarity)
01362 {
01363 emcmotCommand.command = EMCMOT_SET_PROBE_POLARITY;
01364 emcmotCommand.level = polarity;
01365
01366 return usrmotWriteEmcmotCommand(&emcmotCommand);
01367 }
01368
01369 int emcTrajClearProbeTrippedFlag()
01370 {
01371 emcmotCommand.command = EMCMOT_CLEAR_PROBE_FLAGS;
01372
01373 return usrmotWriteEmcmotCommand(&emcmotCommand);
01374 }
01375
01376 int emcTrajProbe(EmcPose pos)
01377 {
01378 emcmotCommand.command = EMCMOT_PROBE;
01379
01380 emcmotCommand.pos.tran.x = pos.tran.x;
01381 emcmotCommand.pos.tran.y = pos.tran.y;
01382 emcmotCommand.pos.tran.z = pos.tran.z;
01383 emcmotCommand.id = localEmcTrajMotionId;
01384
01385 return usrmotWriteEmcmotCommand(&emcmotCommand);
01386 }
01387
01388 static int last_id=0;
01389 static int last_id_printed=0;
01390 static int last_status=0;
01391 static double last_id_time;
01392
01393 int emcTrajUpdate(EMC_TRAJ_STAT *stat)
01394 {
01395 int axis;
01396
01397 stat->axes = localEmcTrajAxes;
01398 stat->linearUnits = localEmcTrajLinearUnits;
01399 stat->angularUnits = localEmcTrajAngularUnits;
01400
01401 stat->mode =
01402 emcmotStatus.motionFlag & EMCMOT_MOTION_TELEOP_BIT ? EMC_TRAJ_MODE_TELEOP :
01403 ( emcmotStatus.motionFlag & EMCMOT_MOTION_COORD_BIT ? EMC_TRAJ_MODE_COORD :
01404 EMC_TRAJ_MODE_FREE ) ;
01405
01406
01407 stat->enabled = 0;
01408 if (emcmotStatus.motionFlag & EMCMOT_MOTION_ENABLE_BIT) {
01409 for (axis = 0; axis < localEmcTrajAxes; axis++) {
01410 if (! emcmotStatus.axisFlag[axis] & EMCMOT_AXIS_ENABLE_BIT) {
01411 break;
01412 }
01413
01414
01415 stat->enabled = 1;
01416 }
01417 }
01418
01419 stat->inpos = emcmotStatus.motionFlag & EMCMOT_MOTION_INPOS_BIT ? 1 : 0;
01420 stat->queue = emcmotStatus.depth;
01421 stat->activeQueue = emcmotStatus.activeDepth;
01422 stat->queueFull = emcmotStatus.queueFull;
01423 stat->id = emcmotStatus.id;
01424 if (EMC_DEBUG_MOTION_TIME & EMC_DEBUG) {
01425 if (stat->id != last_id) {
01426 if (last_id != last_id_printed) {
01427 rcs_print("Motion id %d took %f seconds.\n",last_id, etime()-last_id_time);
01428 last_id_printed = last_id;
01429 }
01430 last_id = stat->id;
01431 last_id_time = etime();
01432 }
01433 }
01434
01435 stat->paused = emcmotStatus.paused;
01436 stat->scale = emcmotStatus.qVscale;
01437
01438 stat->position = emcmotStatus.pos;
01439
01440 stat->actualPosition = emcmotStatus.actualPos;
01441
01442 stat->velocity = emcmotStatus.vel;
01443 stat->acceleration = emcmotStatus.acc;
01444 stat->maxAcceleration = localEmcMaxAcceleration;
01445
01446 if (emcmotStatus.motionFlag & EMCMOT_MOTION_ERROR_BIT) {
01447 stat->status = RCS_ERROR;
01448 }
01449 else if (stat->inpos &&
01450 (stat->queue == 0)) {
01451 stat->status = RCS_DONE;
01452 }
01453 else {
01454 stat->status = RCS_EXEC;
01455 }
01456
01457 if (EMC_DEBUG_MOTION_TIME & EMC_DEBUG) {
01458 if(stat->status == RCS_DONE && last_status != RCS_DONE && stat->id != last_id_printed) {
01459 rcs_print("Motion id %d took %f seconds.\n",last_id, etime()-last_id_time);
01460 last_id_printed = last_id = stat->id;
01461 last_id_time = etime();
01462 }
01463 }
01464
01465
01466 if (emcmotStatus.logOpen) {
01467
01468 emcStatus->logType = emcmotStatus.logType;
01469 emcStatus->logSize = emcmotStatus.logSize;
01470 emcStatus->logSkip = emcmotStatus.logSkip;
01471 emcStatus->logOpen = emcmotStatus.logOpen;
01472 emcStatus->logStarted = emcmotStatus.logStarted;
01473 emcStatus->logPoints = emcmotStatus.logPoints;
01474 }
01475
01476
01477
01478 stat->probedPosition.tran.x = emcmotStatus.probedPos.tran.x;
01479 stat->probedPosition.tran.y = emcmotStatus.probedPos.tran.y;
01480 stat->probedPosition.tran.z = emcmotStatus.probedPos.tran.z;
01481 stat->probeval = emcmotStatus.probeval;
01482 stat->probe_tripped = emcmotStatus.probeTripped;
01483
01484 if (new_config) {
01485 stat->cycleTime = emcmotConfig.trajCycleTime;
01486 stat->probe_index = emcmotConfig.probeIndex;
01487 stat->probe_polarity = emcmotConfig.probePolarity;
01488 stat->kinematics_type = emcmotConfig.kinematics_type;
01489 stat->maxVelocity = emcmotConfig.limitVel;
01490 }
01491
01492 return 0;
01493 }
01494
01495
01496 int emcMotionInit()
01497 {
01498 int r1;
01499 int r2;
01500 int axis;
01501
01502 r1 = -1;
01503 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++)
01504 {
01505 if (0 == emcAxisInit(axis))
01506 {
01507 r1 = 0;
01508 }
01509 }
01510
01511 r2 = emcTrajInit();
01512
01513 if(r1 == 0 && r2 == 0)
01514 {
01515 emcmotion_initialized=1;
01516 }
01517
01518 return (r1 == 0 &&
01519 r2 == 0) ? 0 : -1;
01520 }
01521
01522 int emcMotionHalt()
01523 {
01524 int r1;
01525 int r2;
01526 int t;
01527
01528 r1 = -1;
01529 for (t = 0; t < EMCMOT_MAX_AXIS; t++)
01530 {
01531 if (0 == emcAxisHalt(t))
01532 {
01533 r1 = 0;
01534 }
01535 }
01536
01537 r2 = emcTrajHalt();
01538
01539 emcmotion_initialized=0;
01540
01541 return (r1 == 0 &&
01542 r2 == 0) ? 0 : -1;
01543 }
01544
01545 int emcMotionAbort()
01546 {
01547 int r1;
01548 int r2;
01549 int t;
01550
01551 r1 = -1;
01552 for (t = 0; t < EMCMOT_MAX_AXIS; t++)
01553 {
01554 if (0 == emcAxisAbort(t))
01555 {
01556 r1 = 0;
01557 }
01558 }
01559
01560 r2 = emcTrajAbort();
01561
01562
01563 lastVel = -1.0;
01564
01565 return (r1 == 0 &&
01566 r2 == 0) ? 0 : -1;
01567 }
01568
01569 int emcMotionSetDebug(int debug)
01570 {
01571 emcmotCommand.command = EMCMOT_ABORT;
01572
01573 return usrmotWriteEmcmotCommand(&emcmotCommand);
01574 }
01575
01576 int emcMotionSetAout(unsigned char index, double start, double end)
01577 {
01578 emcmotCommand.command = EMCMOT_SET_AOUT;
01579
01580
01581 emcmotCommand.out = index;
01582 emcmotCommand.minLimit = start;
01583 emcmotCommand.maxLimit = end;
01584
01585 return usrmotWriteEmcmotCommand(&emcmotCommand);
01586 }
01587
01588 int emcMotionSetDout(unsigned char index, unsigned char start, unsigned char end)
01589 {
01590 emcmotCommand.command = EMCMOT_SET_DOUT;
01591 emcmotCommand.out = index;
01592 emcmotCommand.start = start;
01593 emcmotCommand.end = end;
01594
01595 return usrmotWriteEmcmotCommand(&emcmotCommand);
01596 }
01597
01598 int emcMotionUpdate(EMC_MOTION_STAT *stat)
01599 {
01600 int r1;
01601 int r2;
01602 int axis;
01603 int error;
01604 int exec;
01605
01606
01607 if (0 != usrmotReadEmcmotStatus(&emcmotStatus)) {
01608 return -1;
01609 }
01610
01611 new_config = 0;
01612 if (emcmotStatus.config_num != emcmotConfig.config_num) {
01613 if (0 != usrmotReadEmcmotConfig(&emcmotConfig)) {
01614 return -1;
01615 }
01616 new_config = 1;
01617 }
01618
01619 if (get_emcmot_debug_info) {
01620 if (0 != usrmotReadEmcmotDebug(&emcmotDebug)) {
01621 return -1;
01622 }
01623 }
01624
01625
01626 if (0 != usrmotReadEmcmotError(errorString)) {
01627
01628 }
01629 else {
01630
01631 emcOperatorError(0, errorString);
01632 }
01633
01634
01635
01636 localMotionHeartbeat = emcmotStatus.heartbeat;
01637 localMotionCommandType = emcmotStatus.commandEcho;
01638 localMotionEchoSerialNumber = emcmotStatus.commandNumEcho;
01639
01640 r1 = emcAxisUpdate(&stat->axis[0], EMCMOT_MAX_AXIS);
01641 r2 = emcTrajUpdate(&stat->traj);
01642 stat->heartbeat = localMotionHeartbeat;
01643 stat->command_type = localMotionCommandType;
01644 stat->echo_serial_number = localMotionEchoSerialNumber;
01645 stat->debug = emcmotConfig.debug;
01646
01647
01648 error = 0;
01649 exec = 0;
01650
01651 for (axis = 0; axis < stat->traj.axes; axis++) {
01652 if (stat->axis[axis].status == RCS_ERROR) {
01653 error = 1;
01654 break;
01655 }
01656 if (stat->axis[axis].status == RCS_EXEC) {
01657 exec = 1;
01658 break;
01659 }
01660 }
01661
01662 if (stat->traj.status == RCS_ERROR) {
01663 error = 1;
01664 }
01665 else if (stat->traj.status == RCS_EXEC) {
01666 exec = 1;
01667 }
01668
01669 if (error) {
01670 stat->status = RCS_ERROR;
01671 }
01672 else if (exec) {
01673 stat->status = RCS_EXEC;
01674 }
01675 else {
01676 stat->status = RCS_DONE;
01677 }
01678
01679 return (r1 == 0 &&
01680 r2 == 0) ? 0 : -1;
01681 }
01682
01683
01684
01685
01686 static RCS_CMD_CHANNEL *emcIoCommandBuffer = 0;
01687 static RCS_STAT_CHANNEL *emcIoStatusBuffer = 0;
01688
01689
01690 EMC_IO_STAT *emcIoStatus = 0;
01691
01692
01693 static int emcIoCommandSerialNumber = 0;
01694 static double EMCIO_BUFFER_GET_TIMEOUT=5.0;
01695
01696 static int emcioNmlGet()
01697 {
01698 int retval = 0;
01699 double start_time;
01700 RCS_PRINT_DESTINATION_TYPE orig_dest;
01701 if (emcIoCommandBuffer == 0)
01702 {
01703 orig_dest = get_rcs_print_destination();
01704 set_rcs_print_destination(RCS_PRINT_TO_NULL);
01705 start_time = etime();
01706 while(start_time-etime() < EMCIO_BUFFER_GET_TIMEOUT)
01707 {
01708 emcIoCommandBuffer = new RCS_CMD_CHANNEL(emcFormat, "toolCmd", "emc", EMC_NMLFILE);
01709 if (! emcIoCommandBuffer->valid())
01710 {
01711 delete emcIoCommandBuffer;
01712 emcIoCommandBuffer = 0;
01713 }
01714 else
01715 {
01716 break;
01717 }
01718 esleep(0.1);
01719 }
01720 set_rcs_print_destination(orig_dest);
01721 }
01722
01723 if (emcIoCommandBuffer == 0)
01724 {
01725 emcIoCommandBuffer = new RCS_CMD_CHANNEL(emcFormat, "toolCmd", "emc", EMC_NMLFILE);
01726 if (! emcIoCommandBuffer->valid())
01727 {
01728 delete emcIoCommandBuffer;
01729 emcIoCommandBuffer = 0;
01730 retval=-1;
01731 }
01732 }
01733
01734 if (emcIoStatusBuffer == 0)
01735 {
01736 orig_dest = get_rcs_print_destination();
01737 set_rcs_print_destination(RCS_PRINT_TO_NULL);
01738 start_time = etime();
01739 while(start_time-etime() < EMCIO_BUFFER_GET_TIMEOUT)
01740 {
01741 emcIoStatusBuffer = new RCS_STAT_CHANNEL(emcFormat, "toolSts", "emc", EMC_NMLFILE);
01742 if (! emcIoStatusBuffer->valid())
01743 {
01744 delete emcIoStatusBuffer;
01745 emcIoStatusBuffer = 0;
01746 }
01747 else
01748 {
01749 emcIoStatus = (EMC_IO_STAT *) emcIoStatusBuffer->get_address();
01750
01751 emcIoCommandSerialNumber = emcIoStatus->echo_serial_number;
01752 break;
01753 }
01754 esleep(0.1);
01755 }
01756 set_rcs_print_destination(orig_dest);
01757 }
01758
01759 if (emcIoStatusBuffer == 0)
01760 {
01761 emcIoStatusBuffer = new RCS_STAT_CHANNEL(emcFormat, "toolSts", "emc", EMC_NMLFILE);
01762 if (! emcIoStatusBuffer->valid() ||
01763 EMC_IO_STAT_TYPE != emcIoStatusBuffer->peek())
01764 {
01765 delete emcIoStatusBuffer;
01766 emcIoStatusBuffer = 0;
01767 emcIoStatus = 0;
01768 retval = -1;
01769 }
01770 else
01771 {
01772 emcIoStatus = (EMC_IO_STAT *) emcIoStatusBuffer->get_address();
01773
01774 emcIoCommandSerialNumber = emcIoStatus->echo_serial_number;
01775 }
01776 }
01777
01778 return retval;
01779 }
01780
01781
01782
01783
01784
01785
01786
01787 static int sendCommand(RCS_CMD_MSG *msg)
01788 {
01789
01790 if (0 == emcIoCommandBuffer) {
01791 return -1;
01792 }
01793
01794
01795 if (0 == emcIoStatusBuffer ||
01796 ! emcIoStatusBuffer->valid()) {
01797 return -1;
01798 }
01799
01800 double send_command_timeout = etime() + 30.0;
01801
01802
01803 while(etime() < send_command_timeout) {
01804 emcIoStatusBuffer->peek();
01805 if (emcIoStatus->echo_serial_number != emcIoCommandSerialNumber ||
01806 emcIoStatus->status == RCS_EXEC) {
01807 esleep(0.001);
01808 continue;
01809 }
01810 else {
01811 break;
01812 }
01813 }
01814
01815 if (emcIoStatus->echo_serial_number != emcIoCommandSerialNumber ||
01816 emcIoStatus->status == RCS_EXEC) {
01817
01818 return -1;
01819 }
01820
01821
01822 msg->serial_number = ++emcIoCommandSerialNumber;
01823 if (0 != emcIoCommandBuffer->write(msg)) {
01824 return -1;
01825 }
01826
01827 return 0;
01828 }
01829
01830
01831
01832
01833
01834 static int forceCommand(RCS_CMD_MSG *msg)
01835 {
01836
01837 if (0 == emcIoCommandBuffer) {
01838 return -1;
01839 }
01840
01841
01842 if (0 == emcIoStatusBuffer ||
01843 ! emcIoStatusBuffer->valid()) {
01844 return -1;
01845 }
01846
01847
01848 msg->serial_number = ++emcIoCommandSerialNumber;
01849 if (0 != emcIoCommandBuffer->write(msg)) {
01850 return -1;
01851 }
01852
01853 return 0;
01854 }
01855
01856
01857
01858 int emcIoInit()
01859 {
01860 EMC_TOOL_INIT ioInitMsg;
01861 int retval;
01862
01863
01864 if (0 != (retval = emcioNmlGet())) {
01865 rcs_print_error("emcioNmlGet() failed.\n");
01866 return -1;
01867 }
01868
01869
01870
01871
01872
01873 if (0 != iniSpindle(EMC_INIFILE))
01874 {
01875 rcs_print_error("iniSpindle failed.\n");
01876 return -1;
01877 }
01878
01879
01880
01881 if (! emcmotAxisInited &&
01882 ! emcmotTrajInited &&
01883 ! emcmotIoInited)
01884 {
01885 usrmotIniLoad(EMC_INIFILE);
01886
01887 if (0 != usrmotInit())
01888 {
01889 rcs_print_error("usrmotInit failed.\n");
01890 return -1;
01891 }
01892 }
01893 emcmotIoInited = 1;
01894
01895 emcmotCommand.command = EMCMOT_DEACTIVATE_AXIS;
01896 emcmotCommand.axis = SPINDLE_ENABLE_INDEX;
01897
01898 if(usrmotWriteEmcmotCommand(&emcmotCommand))
01899 {
01900 rcs_print_error("Can't send EMCMOT_DEACTIVATE_AXIS for the spindle axis.\n");
01901 return -1;
01902 }
01903
01904
01905 if(forceCommand(&ioInitMsg))
01906 {
01907 rcs_print_error("Can't forceCommand(ioInitMsg)\n");
01908 return -1;
01909 }
01910
01911 return 0;
01912 }
01913
01914 int emcIoHalt()
01915 {
01916 EMC_TOOL_HALT ioHaltMsg;
01917
01918
01919 if (emcIoCommandBuffer != 0)
01920 {
01921 forceCommand(&ioHaltMsg);
01922 }
01923
01924 if (! emcmotAxisInited &&
01925 ! emcmotTrajInited &&
01926 emcmotIoInited)
01927 {
01928 usrmotExit();
01929 }
01930
01931
01932 emcmotIoInited = 0;
01933
01934
01935
01936 if (emcIoStatusBuffer != 0)
01937 {
01938 delete emcIoStatusBuffer;
01939 emcIoStatusBuffer = 0;
01940 emcIoStatus = 0;
01941 }
01942
01943 if (emcIoCommandBuffer != 0)
01944 {
01945 delete emcIoCommandBuffer;
01946 emcIoCommandBuffer = 0;
01947 }
01948
01949 return 0;
01950 }
01951
01952 int emcIoAbort()
01953 {
01954 EMC_TOOL_ABORT ioAbortMsg;
01955
01956
01957 sendCommand(&ioAbortMsg);
01958
01959 return 0;
01960 }
01961
01962 int emcIoSetDebug(int debug)
01963 {
01964 EMC_SET_DEBUG ioDebugMsg;
01965
01966 ioDebugMsg.debug = debug;
01967
01968 return sendCommand(&ioDebugMsg);
01969 }
01970
01971 int emcAuxEstopOn()
01972 {
01973 EMC_AUX_ESTOP_ON estopOnMsg;
01974
01975 return sendCommand(&estopOnMsg);
01976 }
01977
01978 int emcAuxEstopOff()
01979 {
01980 EMC_AUX_ESTOP_OFF estopOffMsg;
01981
01982 return sendCommand(&estopOffMsg);
01983 }
01984
01985 static double simSpindleSpeed = 0.0;
01986 static int simSpindleDirection = 0;
01987 static int simSpindleBrake = 1;
01988 static int simSpindleIncreasing = 0;
01989 static int simSpindleEnabled = 0;
01990
01991 int emcSpindleAbort()
01992 {
01993 return emcSpindleOff();
01994 }
01995
01996
01997 #define VOLTS_PER_RPM 0.001
01998 int emcSpindleOn(double speed)
01999 {
02000 int r1, r2;
02001
02002 emcmotCommand.command = EMCMOT_DAC_OUT;
02003 emcmotCommand.axis = SPINDLE_ON_INDEX;
02004 emcmotCommand.dacOut = speed * VOLTS_PER_RPM;
02005
02006 r1 = emcSpindleEnable();
02007
02008 r2 = usrmotWriteEmcmotCommand(&emcmotCommand);
02009
02010 if (0 == r1 && 0 == r2)
02011 {
02012 simSpindleSpeed = speed;
02013 simSpindleDirection = (speed > 0.0 ? 1 : speed < 0.0 ? -1 : 0);
02014 return 0;
02015 }
02016
02017 return -1;
02018 }
02019
02020
02021 int emcSpindleEnable()
02022 {
02023 int r1;
02024
02025 emcmotCommand.command = EMCMOT_ENABLE_AMPLIFIER;
02026 emcmotCommand.axis = SPINDLE_ENABLE_INDEX;
02027 emcmotCommand.dacOut = 0.0;
02028
02029 r1 = usrmotWriteEmcmotCommand(&emcmotCommand);
02030
02031 if (0 == r1)
02032 {
02033 simSpindleEnabled = 1;
02034 return 0;
02035 }
02036
02037 return -1;
02038 }
02039
02040
02041 int emcSpindleDisable()
02042 {
02043 int r1;
02044
02045 emcmotCommand.command = EMCMOT_DISABLE_AMPLIFIER;
02046 emcmotCommand.axis = SPINDLE_ENABLE_INDEX;
02047
02048 r1 = usrmotWriteEmcmotCommand(&emcmotCommand);
02049
02050 if (0 == r1)
02051 {
02052 simSpindleEnabled = 0;
02053 return 0;
02054 }
02055
02056 return -1;
02057 }
02058
02059 int emcSpindleOff()
02060 {
02061 int r1, r2;
02062
02063 r1 = emcSpindleOn(0.0);
02064
02065 r2 = emcSpindleDisable();
02066
02067 if (0 == r1 && 0 == r2)
02068 {
02069 return 0;
02070 }
02071
02072 return -1;
02073 }
02074
02075 int emcSpindleBrakeRelease()
02076 {
02077
02078 simSpindleBrake = 0;
02079
02080 return 0;
02081 }
02082
02083 int emcSpindleBrakeEngage()
02084 {
02085
02086 simSpindleBrake = 1;
02087
02088 return 0;
02089 }
02090
02091 int emcSpindleIncrease()
02092 {
02093
02094 simSpindleIncreasing = 1;
02095
02096 return 0;
02097 }
02098
02099 int emcSpindleDecrease()
02100 {
02101
02102 simSpindleIncreasing = -1;
02103
02104 return 0;
02105 }
02106
02107 int emcSpindleConstant()
02108 {
02109
02110 simSpindleIncreasing = 0;
02111
02112 return 0;
02113 }
02114
02115 static int simCoolantMist = 0;
02116 static int simCoolantFlood = 0;
02117
02118 int emcCoolantMistOn()
02119 {
02120
02121 simCoolantMist = 1;
02122
02123 return 0;
02124 }
02125
02126 int emcCoolantMistOff()
02127 {
02128
02129 simCoolantMist = 0;
02130
02131 return 0;
02132 }
02133
02134 int emcCoolantFloodOn()
02135 {
02136
02137 simCoolantFlood = 1;
02138
02139 return 0;
02140 }
02141
02142 int emcCoolantFloodOff()
02143 {
02144
02145 simCoolantFlood = 0;
02146
02147 return 0;
02148 }
02149
02150 static int simLubeOn = 0;
02151 static int simLubeLevel = 1;
02152
02153 int emcLubeInit()
02154 {
02155 return 0;
02156 }
02157
02158 int emcLubeHalt()
02159 {
02160 return 0;
02161 }
02162
02163 int emcLubeAbort()
02164 {
02165 simLubeOn = 0;
02166
02167 return 0;
02168 }
02169
02170 int emcLubeOn()
02171 {
02172 simLubeOn = 1;
02173
02174 return 0;
02175 }
02176
02177 int emcLubeOff()
02178 {
02179 simLubeOn = 0;
02180
02181 return 0;
02182 }
02183
02184 int emcToolPrepare(int tool)
02185 {
02186 EMC_TOOL_PREPARE toolPrepareMsg;
02187
02188 toolPrepareMsg.tool = tool;
02189 sendCommand(&toolPrepareMsg);
02190
02191 return 0;
02192 }
02193
02194 int emcToolLoad()
02195 {
02196 EMC_TOOL_LOAD toolLoadMsg;
02197
02198 sendCommand(&toolLoadMsg);
02199
02200 return 0;
02201 }
02202
02203 int emcToolUnload()
02204 {
02205 EMC_TOOL_UNLOAD toolUnloadMsg;
02206
02207 sendCommand(&toolUnloadMsg);
02208
02209 return 0;
02210 }
02211
02212 int emcToolLoadToolTable(const char *file)
02213 {
02214 EMC_TOOL_LOAD_TOOL_TABLE toolLoadToolTableMsg;
02215
02216 strcpy(toolLoadToolTableMsg.file, file);
02217
02218 sendCommand(&toolLoadToolTableMsg);
02219
02220 return 0;
02221 }
02222
02223 int emcToolSetOffset(int tool, double length, double diameter)
02224 {
02225 EMC_TOOL_SET_OFFSET toolSetOffsetMsg;
02226
02227 toolSetOffsetMsg.tool = tool;
02228 toolSetOffsetMsg.length = length;
02229 toolSetOffsetMsg.diameter = diameter;
02230
02231 sendCommand(&toolSetOffsetMsg);
02232
02233 return 0;
02234 }
02235
02236 int emcLogOpen(char *file, int type, int size, int skip, int which, int triggerType, int triggerVar, double triggerThreshold )
02237 {
02238 int r1;
02239
02240 emcmotCommand.command = EMCMOT_OPEN_LOG;
02241 emcmotCommand.logSize = size;
02242 emcmotCommand.logSkip = skip;
02243 emcmotCommand.axis = which;
02244 emcmotCommand.logTriggerType = triggerType;
02245 emcmotCommand.logTriggerVariable = triggerVar;
02246 emcmotCommand.logTriggerThreshold = triggerThreshold;
02247
02248
02249
02250 switch (type)
02251 {
02252 case EMC_LOG_TYPE_AXIS_POS:
02253 emcmotCommand.logType = EMCMOT_LOG_TYPE_AXIS_POS;
02254 break;
02255
02256 case EMC_LOG_TYPE_AXES_INPOS:
02257 emcmotCommand.logType = EMCMOT_LOG_TYPE_ALL_INPOS;
02258 break;
02259
02260 case EMC_LOG_TYPE_AXES_OUTPOS:
02261 emcmotCommand.logType = EMCMOT_LOG_TYPE_ALL_OUTPOS;
02262 break;
02263
02264 case EMC_LOG_TYPE_AXIS_VEL:
02265 emcmotCommand.logType = EMCMOT_LOG_TYPE_AXIS_VEL;
02266 break;
02267
02268 case EMC_LOG_TYPE_AXES_FERROR:
02269 emcmotCommand.logType = EMCMOT_LOG_TYPE_ALL_FERROR;
02270 break;
02271
02272 case EMC_LOG_TYPE_TRAJ_POS:
02273 emcmotCommand.logType = EMCMOT_LOG_TYPE_TRAJ_POS;
02274 break;
02275
02276 case EMC_LOG_TYPE_TRAJ_VEL:
02277 emcmotCommand.logType = EMCMOT_LOG_TYPE_TRAJ_VEL;
02278 break;
02279
02280 case EMC_LOG_TYPE_TRAJ_ACC:
02281 emcmotCommand.logType = EMCMOT_LOG_TYPE_TRAJ_ACC;
02282 break;
02283
02284 case EMC_LOG_TYPE_POS_VOLTAGE:
02285 emcmotCommand.logType = EMCMOT_LOG_TYPE_POS_VOLTAGE;
02286 break;
02287
02288 default:
02289 return -1;
02290 }
02291
02292 r1 = usrmotWriteEmcmotCommand(&emcmotCommand);
02293
02294 if (r1 == 0)
02295 {
02296 strncpy(emcStatus->logFile, file, EMC_LOG_FILENAME_LEN - 1);
02297 emcStatus->logFile[EMC_LOG_FILENAME_LEN - 1] = 0;
02298 }
02299
02300
02301
02302 return r1;
02303 }
02304
02305 int emcLogStart()
02306 {
02307 int r1;
02308
02309 emcmotCommand.command = EMCMOT_START_LOG;
02310
02311 r1 = usrmotWriteEmcmotCommand(&emcmotCommand);
02312 return(0);
02313 }
02314
02315 int emcLogStop()
02316 {
02317 int r1;
02318
02319 emcmotCommand.command = EMCMOT_STOP_LOG;
02320
02321 r1 = usrmotWriteEmcmotCommand(&emcmotCommand);
02322
02323 return r1;
02324 }
02325
02326 int emcLogClose()
02327 {
02328 int r1;
02329 int r2;
02330
02331
02332 if (emcStatus->logFile[0] == 0 ||
02333 emcStatus->logSize == 0)
02334 {
02335 return 0;
02336 }
02337
02338 emcmotCommand.command = EMCMOT_CLOSE_LOG;
02339
02340 r1 = usrmotWriteEmcmotCommand(&emcmotCommand);
02341 r2 = usrmotDumpLog(emcStatus->logFile, EMCLOG_INCLUDE_HEADER);
02342
02343 emcStatus->logFile[0] = 0;
02344 emcStatus->logType = 0;
02345 emcStatus->logSize = 0;
02346 emcStatus->logSkip = 0;
02347 emcStatus->logOpen = 0;
02348 emcStatus->logStarted = 0;
02349
02350 return (r1 || r2 ? -1 : 0);
02351 }
02352
02353
02354
02355 int emcIoUpdate(EMC_IO_STAT *stat)
02356 {
02357 if (0 == emcIoStatusBuffer ||
02358 ! emcIoStatusBuffer->valid())
02359 {
02360 return -1;
02361 }
02362
02363 switch (emcIoStatusBuffer->peek())
02364 {
02365 case -1:
02366
02367 return -1;
02368 break;
02369
02370 case 0:
02371 case EMC_IO_STAT_TYPE:
02372
02373 break;
02374
02375 default:
02376
02377 return -1;
02378 break;
02379 }
02380
02381
02382 *stat = *emcIoStatus;
02383
02384
02385
02386
02387
02388
02389
02390 if (stat->echo_serial_number != emcIoCommandSerialNumber)
02391 {
02392 stat->status = RCS_EXEC;
02393 }
02394
02395
02396
02397 stat->spindle.speed = simSpindleSpeed;
02398 stat->spindle.direction = simSpindleDirection;
02399 stat->spindle.brake = simSpindleBrake;
02400 stat->spindle.increasing = simSpindleIncreasing;
02401 stat->spindle.enabled = simSpindleEnabled;
02402 stat->coolant.mist = simCoolantMist;
02403 stat->coolant.flood = simCoolantFlood;
02404 stat->lube.on = simLubeOn;
02405 stat->lube.level = simLubeLevel;
02406
02407 return 0;
02408 }