#include "posemath.h"
#include "emcpos.h"
Include dependency graph for tc.h:
This graph shows which files directly or indirectly include this file:
Go to the source code of this file.
Data Structures | |
struct | TC_QUEUE_STRUCT |
Defines | |
#define | __attribute__(x) |
#define | TC_IS_UNSET -1 |
#define | TC_IS_DONE 1 |
#define | TC_IS_ACCEL 2 |
#define | TC_IS_CONST 3 |
#define | TC_IS_DECEL 4 |
#define | TC_IS_PAUSED 5 |
#define | TC_TERM_COND_STOP 1 |
#define | TC_TERM_COND_BLEND 2 |
#define | TC_LINEAR 1 |
#define | TC_CIRCULAR 2 |
Functions | |
char | __attribute__ ((unused)) tc_h[]="$Id |
int | tcInit (TC_STRUCT *tc) |
int | tcSetCycleTime (TC_STRUCT *tc, double secs) |
int | tcSetLine (TC_STRUCT *tc, PmLine line, PmLine line_abc) |
int | tcSetCircle (TC_STRUCT *tc, PmCircle circle, PmLine line_abc) |
int | tcSetTVmax (TC_STRUCT *tc, double vmax) |
int | tcSetRVmax (TC_STRUCT *tc, double vmax) |
int | tcSetVscale (TC_STRUCT *tc, double vscale) |
int | tcSetTAmax (TC_STRUCT *tc, double amax) |
int | tcSetRAmax (TC_STRUCT *tc, double wmax) |
int | tcSetPremax (TC_STRUCT *tc, double vmax, double amax) |
int | tcSetVlimit (TC_STRUCT *tc, double vlimit) |
int | tcSetId (TC_STRUCT *tc, int id) |
int | tcGetId (TC_STRUCT *tc) |
int | tcSetTermCond (TC_STRUCT *tc, int cond) |
int | tcGetTermCond (TC_STRUCT *tc) |
int | tcRunCycle (TC_STRUCT *tc) |
EmcPose | tcGetPos (TC_STRUCT *tc) |
EmcPose | tcGetGoalPos (TC_STRUCT *tc) |
double | tcGetVel (TC_STRUCT *tc) |
double | tcGetAccel (TC_STRUCT *tc) |
PmCartesian | tcGetUnitCart (TC_STRUCT *tc) |
int | tcGetTcFlag (TC_STRUCT *tc) |
int | tcIsDone (TC_STRUCT *tc) |
int | tcIsAccel (TC_STRUCT *tc) |
int | tcIsConst (TC_STRUCT *tc) |
int | tcIsDecel (TC_STRUCT *tc) |
int | tcIsPaused (TC_STRUCT *tc) |
void | tcPrint (TC_STRUCT *tc) |
double | tcRunPreCycle (const TC_STRUCT *tc) |
int | tcForceCycle (TC_STRUCT *tc, double ratio) |
int | tcSetDout (TC_STRUCT *tc, unsigned char douts, unsigned char starts, unsigned char ends) |
int | tcqCreate (TC_QUEUE_STRUCT *tcq, int _size, TC_STRUCT *tcSpace) |
int | tcqDelete (TC_QUEUE_STRUCT *tcq) |
int | tcqInit (TC_QUEUE_STRUCT *tcq) |
int | tcqPut (TC_QUEUE_STRUCT *tcq, TC_STRUCT tc) |
TC_STRUCT | tcqGet (TC_QUEUE_STRUCT *tcq, int *status) |
int | tcqRemove (TC_QUEUE_STRUCT *tcq, int n) |
int | tcqLen (TC_QUEUE_STRUCT *tcq) |
TC_STRUCT * | tcqItem (TC_QUEUE_STRUCT *tcq, int n, int *status) |
TC_STRUCT * | tcqLast (TC_QUEUE_STRUCT *tcq, int *status) |
int | tcqFull (TC_QUEUE_STRUCT *tcq) |
Variables | |
TC_STRUCT |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Definition at line 44 of file tc.h. 00044 : tc.h,v 1.6 2001/08/17 22:05:41 proctor Exp $"; 00045 00046 /* values for tcFlag */ 00047 #define TC_IS_UNSET -1 00048 #define TC_IS_DONE 1 00049 #define TC_IS_ACCEL 2 00050 #define TC_IS_CONST 3 00051 #define TC_IS_DECEL 4 00052 #define TC_IS_PAUSED 5 00053 00054 /* values for endFlag */ 00055 #define TC_TERM_COND_STOP 1 00056 #define TC_TERM_COND_BLEND 2 00057 00058 #define TC_LINEAR 1 00059 #define TC_CIRCULAR 2 00060 00061 /* structure for individual trajectory elements */ 00062 00063 typedef struct 00064 { 00065 double cycleTime; 00066 double targetPos; /* positive motion progession */ 00067 double vMax; /* max velocity */ 00068 double vScale; /* scale factor for vMax */ 00069 double aMax; /* max accel */ 00070 double preVMax; /* vel from previous blend */ 00071 double preAMax; /* decel (negative) from previous blend */ 00072 double vLimit; /* abs vel limit, including scale */ 00073 double toGo; 00074 double currentPos; 00075 double currentVel; 00076 double currentAccel; 00077 int tcFlag; /* TC_IS_DONE,ACCEL,CONST,DECEL*/ 00078 int type; /* TC_LINEAR, TC_CIRCULAR */ 00079 int id; /* id for motion segment */ 00080 int termCond; /* TC_END_STOP,BLEND */ 00081 PmLine line; 00082 PmLine line_abc; 00083 PmCircle circle; 00084 double tmag; /* magnitude of translation */ 00085 double abc_mag; /* magnitude of rotation */ 00086 double tvMax; /* maximum translational velocity */ 00087 double taMax; /* maximum translational accelleration */ 00088 double abc_vMax; /* maximum rotational velocity */ 00089 double abc_aMax; /* maximum rotational accelleration */ 00090 PmCartesian unitCart; 00091 unsigned char douts; /* mask for douts to set */ 00092 unsigned char doutstarts; /* mask for dout start vals */ 00093 unsigned char doutends; /* mask for dout end vals */ 00094 } TC_STRUCT; |
|
Definition at line 1022 of file tc.c. 01023 { 01024 double newPos; 01025 double newToGo; 01026 double newVel; 01027 double newAccel; 01028 int newTcFlag; 01029 01030 if (0 == tc) 01031 { 01032 return -1; 01033 } 01034 01035 newPos = ratio * tc->targetPos; 01036 newToGo = newPos - tc->currentPos; 01037 01038 if (ratio >= 1.0) 01039 { 01040 newVel = 0.0; 01041 newAccel = 0.0; 01042 newTcFlag = TC_IS_DONE; 01043 } 01044 else 01045 { 01046 newVel = (newPos - tc->currentPos) / tc->cycleTime; 01047 newAccel = (newVel - tc->currentVel) / tc->cycleTime; 01048 if (newAccel > 0.0) 01049 { 01050 newTcFlag = TC_IS_ACCEL; 01051 } 01052 else if (newAccel < 0.0) 01053 { 01054 newTcFlag = TC_IS_DECEL; 01055 } 01056 else 01057 { 01058 newTcFlag = TC_IS_CONST; 01059 } 01060 } 01061 01062 tc->currentPos = newPos; 01063 tc->toGo = newToGo; 01064 tc->currentVel = newVel; 01065 tc->currentAccel = newAccel; 01066 tc->tcFlag = newTcFlag; 01067 01068 return 0; 01069 } |
|
Definition at line 641 of file tc.c. Referenced by tpRunCycle().
00642 { 00643 if (0 == tc) 00644 { 00645 return 0.0; 00646 } 00647 00648 return tc->currentAccel; 00649 } |
|
Definition at line 591 of file tc.c. 00592 { 00593 PmPose v; 00594 EmcPose ev; 00595 00596 if (0 == tc) 00597 { 00598 ev.tran.x = ev.tran.y = ev.tran.z = 0.0; 00599 ev.a = ev.b = ev.c = 0.0; 00600 return ev; 00601 } 00602 00603 if (tc->type == TC_LINEAR) 00604 { 00605 v = tc->line.end; 00606 } 00607 else if (tc->type == TC_CIRCULAR) 00608 { 00609 /* we don't save start or end vector in TC_STRUCT to save space. 00610 To get end, call pmCirclePoint with final angle. tcGetGoalPos() 00611 is called infrequently so this space-time tradeoff is done. If 00612 this function is called often, we should save end point in the 00613 PM_CIRCLE struct. This will increase all TC_STRUCTS but make 00614 tcGetGoalPos() run faster. */ 00615 pmCirclePoint(&tc->circle, tc->circle.angle, &v); 00616 } 00617 else 00618 { 00619 v.tran.x = v.tran.y = v.tran.z = 0.0; 00620 v.rot.s = 1.0; 00621 v.rot.x = v.rot.y = v.rot.z = 0.0; 00622 } 00623 00624 ev.tran = v.tran; 00625 ev.a = tc->line_abc.end.tran.x; 00626 ev.b = tc->line_abc.end.tran.y; 00627 ev.c = tc->line_abc.end.tran.z; 00628 return ev; 00629 } |
|
Definition at line 345 of file tc.c. Referenced by tpRunCycle().
00346 { 00347 if (0 == tc) 00348 { 00349 return -1; 00350 } 00351 00352 return tc->id; 00353 } |
|
Definition at line 536 of file tc.c. Referenced by tpRunCycle().
00537 { 00538 EmcPose v; 00539 PmPose v1; 00540 PmPose v2; 00541 00542 if (0 == tc) 00543 { 00544 v1.tran.x = v1.tran.y = v1.tran.z = 0.0; 00545 v1.rot.s = 1.0; 00546 v1.rot.x = v1.rot.y = v1.rot.z = 0.0; 00547 return v; 00548 } 00549 00550 /* note: was if tc->targetPos <= 0.0 return basePos */ 00551 if (tc->type == TC_LINEAR) 00552 { 00553 pmLinePoint(&tc->line, tc->currentPos, &v1); 00554 } 00555 else if (tc->type == TC_CIRCULAR) 00556 { 00557 pmCirclePoint(&tc->circle, tc->currentPos / tc->circle.radius, &v1); 00558 } 00559 else 00560 { 00561 v1.tran.x = v1.tran.y = v1.tran.z = 0.0; 00562 v1.rot.s = 1.0; 00563 v1.rot.x = v1.rot.y = v1.rot.z = 0.0; 00564 } 00565 v.tran = v1.tran; 00566 if(tc->abc_mag > 1e-6) 00567 { 00568 if(tc->tmag > 1e-6 ) 00569 { 00570 pmLinePoint(&tc->line_abc,(tc->currentPos *tc->abc_mag /tc->tmag),&v2); 00571 v.a = v2.tran.x; 00572 v.b = v2.tran.y; 00573 v.c = v2.tran.z; 00574 } 00575 else 00576 { 00577 pmLinePoint(&tc->line_abc,tc->currentPos,&v2); 00578 v.a = v2.tran.x; 00579 v.b = v2.tran.y; 00580 v.c = v2.tran.z; 00581 } 00582 } 00583 else 00584 { 00585 v.a = v.b = v.c = 0.0; 00586 } 00587 00588 return v; 00589 } |
|
Definition at line 651 of file tc.c. 00652 { 00653 if (0 == tc) 00654 { 00655 return TC_IS_UNSET; 00656 } 00657 00658 return tc->tcFlag; 00659 } |
|
Definition at line 373 of file tc.c. Referenced by tpRunCycle().
00374 { 00375 if (0 == tc) 00376 { 00377 return -1; 00378 } 00379 00380 return tc->termCond; 00381 } |
|
Definition at line 1072 of file tc.c. Referenced by tpRunCycle().
01073 { 01074 PmPose currentPose; 01075 PmCartesian radialCart; 01076 static const PmCartesian fake= {1.0,0.0,0.0}; 01077 01078 if(tc->type == TC_LINEAR) 01079 { 01080 pmCartCartSub(tc->line.end.tran,tc->line.start.tran,&tc->unitCart); 01081 #ifdef USE_PM_CART_NORM 01082 pmCartNorm(tc->unitCart,&tc->unitCart); 01083 #else 01084 pmCartUnit(tc->unitCart,&tc->unitCart); 01085 #endif 01086 return(tc->unitCart); 01087 } 01088 else if(tc->type == TC_CIRCULAR) 01089 { 01090 pmCirclePoint(&tc->circle,tc->currentPos,¤tPose); 01091 pmCartCartSub(currentPose.tran, tc->circle.center, &radialCart); 01092 pmCartCartCross(tc->circle.normal, radialCart,&tc->unitCart); 01093 #ifdef USE_PM_CART_NORM 01094 pmCartNorm(tc->unitCart,&tc->unitCart); 01095 #else 01096 pmCartUnit(tc->unitCart,&tc->unitCart); 01097 #endif 01098 return(tc->unitCart); 01099 } 01100 // It should never really get here. 01101 return fake; 01102 } |
|
Definition at line 631 of file tc.c. Referenced by tpRunCycle().
00632 { 00633 if (0 == tc) 00634 { 00635 return 0.0; 00636 } 00637 00638 return tc->currentVel; 00639 } |
|
Referenced by tpAddCircle(), and tpAddLine().
|
|
Definition at line 671 of file tc.c. 00672 { 00673 if (0 == tc) 00674 { 00675 return 0; 00676 } 00677 00678 return (tc->tcFlag == TC_IS_ACCEL); 00679 } |
|
Definition at line 681 of file tc.c. 00682 { 00683 if (0 == tc) 00684 { 00685 return 0; 00686 } 00687 00688 return (tc->tcFlag == TC_IS_CONST); 00689 } |
|
Definition at line 691 of file tc.c. Referenced by tpRunCycle().
00692 { 00693 if (0 == tc) 00694 { 00695 return 0; 00696 } 00697 00698 return (tc->tcFlag == TC_IS_DECEL); 00699 } |
|
Definition at line 661 of file tc.c. Referenced by tpRunCycle().
00662 { 00663 if (0 == tc) 00664 { 00665 return 0; 00666 } 00667 00668 return (tc->tcFlag == TC_IS_DONE); 00669 } |
|
Definition at line 701 of file tc.c. Referenced by tpIsPaused().
00702 { 00703 if (0 == tc) 00704 { 00705 return 0; 00706 } 00707 00708 return (tc->tcFlag == TC_IS_PAUSED); 00709 } |
|
Definition at line 711 of file tc.c. Referenced by tpPrint().
00712 { 00713 #ifndef NO_STDIO_SUPPORT 00714 if (0 == tc) 00715 { 00716 rcs_print("(null)\n"); 00717 return; 00718 } 00719 00720 rcs_print("cycleTime: %f\n", tc->cycleTime); 00721 rcs_print("targetPos: %f\n", tc->targetPos); 00722 rcs_print("vMax: %f\n", tc->vMax); 00723 rcs_print("vLimit: %f\n", tc->vLimit); 00724 rcs_print("vScale %f\n", tc->vScale); 00725 rcs_print("aMax: %f\n", tc->aMax); 00726 rcs_print("toGo: %f\n", tc->toGo); 00727 rcs_print("currentPos: %f\n", tc->currentPos); 00728 rcs_print("currentVel: %f\n", tc->currentVel); 00729 rcs_print("currentAccel: %f\n", tc->currentAccel); 00730 rcs_print("tcFlag: %s\n", tc->tcFlag == TC_IS_UNSET ? "UNSET" : 00731 tc->tcFlag == TC_IS_DONE ? "DONE" : 00732 tc->tcFlag == TC_IS_ACCEL ? "ACCEL" : 00733 tc->tcFlag == TC_IS_CONST ? "CONST" : 00734 tc->tcFlag == TC_IS_DECEL ? "DECEL" : 00735 tc->tcFlag == TC_IS_PAUSED ? "PAUSED" : "?"); 00736 rcs_print("type: %s\n", tc->type == TC_LINEAR ? "LINEAR" : 00737 tc->type == TC_CIRCULAR ? "CIRCULAR" : "?"); 00738 rcs_print("id: %d\n", tc->id); 00739 #endif /* STDIO_SUPPORT */ 00740 } |
|
Definition at line 383 of file tc.c. Referenced by tcRunPreCycle(), and tpRunCycle().
00384 { 00385 double newPos; 00386 double newVel; 00387 double newAccel; 00388 double discr; 00389 int isScaleDecel; 00390 int oldTcFlag; 00391 00392 if (0 == tc) { 00393 return -1; 00394 } 00395 00396 /* save the old flag-- we'll use it when deciding to flag deceleration 00397 if we're scaling back velocity */ 00398 oldTcFlag = tc->tcFlag; 00399 00400 if (tc->tcFlag == TC_IS_DONE) { 00401 tc->currentVel = 0.0; 00402 tc->currentAccel = 0.0; 00403 return 0; 00404 } 00405 00406 tc->toGo = tc->targetPos - tc->currentPos; 00407 00408 if (tc->aMax <= 0.0 || tc->vMax <= 0.0 || tc->cycleTime <= 0.0) { 00409 return -1; 00410 } 00411 00412 /* compute newvel = 0 limit first */ 00413 discr = 0.5 * tc->cycleTime * tc->currentVel - tc->toGo; 00414 if (discr > 0.0) { 00415 newVel = 0.0; 00416 } 00417 else { 00418 discr = 0.25 * tc->cycleTime * tc->cycleTime - 2.0 / tc->aMax * discr; 00419 newVel = - 0.5 * tc->aMax * tc->cycleTime + tc->aMax * sqrt(discr); 00420 } 00421 00422 if (tc->tcFlag == TC_IS_UNSET) { 00423 /* it's the start of this segment, so set any start output bits */ 00424 if (tc->douts) { 00425 doutbyte |= (tc->douts & tc->doutstarts); 00426 doutbyte &= (~tc->douts | tc->doutstarts); 00427 extMotDout(doutbyte); 00428 } 00429 } 00430 00431 if (newVel <= 0.0) { 00432 newVel = 0.0; 00433 newAccel = 0; 00434 newPos = tc->targetPos; 00435 tc->tcFlag = TC_IS_DONE; 00436 /* set any end output bits */ 00437 if (tc->douts) { 00438 doutbyte |= (tc->douts & tc->doutends); 00439 doutbyte &= (~tc->douts | tc->doutends); 00440 extMotDout(doutbyte); 00441 } 00442 } 00443 else { 00444 /* clamp velocity to scaled max, and note if it's scaled 00445 back. This will cause a deceleration which we will NOT 00446 flag as a TC_IS_DECEL unless we were previously in 00447 a decel mode, since that would cause the next 00448 motion in the queue to begin to be planned. Make it 00449 TC_IS_CONST instead. */ 00450 isScaleDecel = 0; 00451 if (newVel > (tc->vMax - tc->preVMax) * tc->vScale) { 00452 newVel = (tc->vMax - tc->preVMax) * tc->vScale; 00453 isScaleDecel = 1; 00454 } 00455 00456 /* clamp scaled velocity against absolute limit */ 00457 if (newVel > tc->vLimit) { 00458 newVel = tc->vLimit; 00459 } 00460 00461 if (tc->type == TC_CIRCULAR) { 00462 if (newVel > pmSqrt(tc->aMax*tc->circle.radius)) { 00463 newVel = pmSqrt(tc->aMax*tc->circle.radius); 00464 } 00465 } 00466 00467 /* calc resulting accel */ 00468 newAccel = (newVel - tc->currentVel) / tc->cycleTime; 00469 00470 /* clamp accel if necessary, and recalc velocity */ 00471 /* also give credit for previous segment's decel, in preAMax, 00472 which is a negative value since it's a decel */ 00473 if (newAccel > 0.0) { 00474 if (newAccel > tc->aMax - tc->preAMax) { 00475 newAccel = tc->aMax - tc->preAMax; 00476 /* if tc->preMax was calculated correctly this check is 00477 redundant. 00478 (Just because I'm paranoid doesn't mean they are not out to get me!) */ 00479 if (newAccel < 0.0) { 00480 newAccel = 0.0; 00481 } 00482 newVel = tc->currentVel + newAccel * tc->cycleTime; 00483 } 00484 } 00485 else { 00486 if (newAccel < -tc->aMax) { 00487 newAccel = -tc->aMax; 00488 newVel = tc->currentVel + newAccel * tc->cycleTime; 00489 } 00490 } 00491 00492 #ifdef A_CHANGE_MAX 00493 if (newAccel > A_CHANGE_MAX*tc->cycleTime+tc->currentAccel) { 00494 newAccel = A_CHANGE_MAX*tc->cycleTime + tc->currentAccel; 00495 newVel = tc->currentVel + newAccel * tc->cycleTime; 00496 } 00497 else if(newAccel < -A_CHANGE_MAX*tc->cycleTime+tc->currentAccel) { 00498 newAccel = -A_CHANGE_MAX*tc->cycleTime + tc->currentAccel; 00499 newVel = tc->currentVel + newAccel * tc->cycleTime; 00500 } 00501 #endif 00502 00503 tc->toGo = (newVel + tc->currentVel) * 0.5 * tc->cycleTime; 00504 newPos = tc->currentPos + tc->toGo; 00505 00506 if (newAccel < 0.0) { 00507 if (isScaleDecel && oldTcFlag != TC_IS_DECEL) { 00508 /* we're decelerating, but blending next move is not 00509 being done yet, so don't flag a decel. This will 00510 prevent premature blending */ 00511 tc->tcFlag = TC_IS_ACCEL; 00512 } 00513 else { 00514 tc->tcFlag = TC_IS_DECEL; 00515 } 00516 } 00517 else if (newAccel > 0.0) { 00518 tc->tcFlag = TC_IS_ACCEL; 00519 } 00520 else if (newVel < TC_VEL_EPSILON && 00521 tc->vScale < TC_SCALE_EPSILON) { 00522 tc->tcFlag = TC_IS_PAUSED; 00523 } 00524 else { 00525 tc->tcFlag = TC_IS_CONST; 00526 } 00527 } 00528 00529 tc->currentPos = newPos; 00530 tc->currentVel = newVel; 00531 tc->currentAccel = newAccel; 00532 00533 return 0; 00534 } |
|
Definition at line 979 of file tc.c. 00980 { 00981 TC_STRUCT preTc; 00982 double ratio; /* ratio of (new pos) / (target pos) */ 00983 00984 if (0 == tc) 00985 { 00986 return -1; 00987 } 00988 00989 if (tc->targetPos <= 0.0) 00990 { 00991 /* already there */ 00992 ratio = 1.0; 00993 } 00994 else 00995 { 00996 preTc = *tc; 00997 tcRunCycle(&preTc); 00998 ratio = preTc.currentPos / preTc.targetPos; 00999 } 01000 01001 if (preTc.tcFlag == TC_IS_DONE) 01002 { 01003 /* if it'll be done, may as well return 100% */ 01004 return 1.0; 01005 } 01006 01007 return ratio; 01008 } |
|
Definition at line 219 of file tc.c. Referenced by tpAddCircle().
00220 { 00221 if (0 == tc) 00222 { 00223 return -1; 00224 } 00225 00226 tc->circle = circle; 00227 tc->line_abc = line_abc; 00228 00229 /* for circular motion, path param is arc length */ 00230 tc->tmag = tc->targetPos = circle.angle * circle.radius; 00231 00232 tc->currentPos = 0.0; 00233 tc->type = TC_CIRCULAR; 00234 00235 tc->aMax = tc->taMax; 00236 tc->vMax = tc->tvMax; 00237 00238 pmCartCartDisp(line_abc.end.tran, line_abc.start.tran, &tc->abc_mag); 00239 00240 return 0; 00241 } |
|
Definition at line 133 of file tc.c. Referenced by tpAddCircle(), and tpAddLine().
00134 { 00135 if (secs <= 0.0 || 00136 0 == tc) 00137 { 00138 return -1; 00139 } 00140 00141 tc->cycleTime = secs; 00142 00143 return 0; 00144 } |
|
Definition at line 1104 of file tc.c. Referenced by tpAddCircle(), and tpAddLine().
01105 { 01106 if (0 == tc) { 01107 return -1; 01108 } 01109 01110 tc->douts = douts; 01111 tc->doutstarts = starts; 01112 tc->doutends = ends; 01113 01114 return 0; 01115 } |
|
Definition at line 333 of file tc.c. Referenced by tpAddCircle(), and tpAddLine().
00334 { 00335 if (0 == tc) 00336 { 00337 return -1; 00338 } 00339 00340 tc->id = _id; 00341 00342 return 0; 00343 } |
|
Definition at line 146 of file tc.c. Referenced by tpAddLine().
00147 { 00148 double tmag,abc_mag; 00149 00150 if (0 == tc) 00151 { 00152 return -1; 00153 } 00154 00155 tc->line = line; 00156 tc->line_abc = line_abc; 00157 00158 00159 /* set targetPos to be scalar difference */ 00160 pmCartCartDisp(line.end.tran, line.start.tran, &tmag); 00161 pmCartCartDisp(line_abc.end.tran, line_abc.start.tran, &abc_mag); 00162 00163 tc->abc_mag = abc_mag; 00164 tc->tmag = tmag; 00165 00166 if(tc->abc_aMax <= 0.0 && tc->taMax > 0.0) 00167 { 00168 tc->abc_aMax = tc->taMax; 00169 } 00170 00171 if(tc->abc_vMax <= 0.0 && tc->tvMax > 0.0) 00172 { 00173 tc->abc_vMax = tc->tvMax; 00174 } 00175 00176 if(tc->tmag < 1e-6) 00177 { 00178 tc->aMax = tc->abc_aMax; 00179 tc->vMax = tc->abc_vMax; 00180 tc->targetPos = abc_mag; 00181 } 00182 else 00183 { 00184 if(0) 00185 #if 0 00186 if(tc->abc_mag > 1e-6) 00187 #endif 00188 { 00189 if(tc->abc_aMax * tmag/abc_mag < tc->taMax) 00190 { 00191 tc->aMax = tc->abc_aMax *tmag/abc_mag; 00192 } 00193 else 00194 { 00195 tc->aMax = tc->taMax; 00196 } 00197 if(tc->abc_vMax * tmag/abc_mag < tc->tvMax) 00198 { 00199 tc->vMax = tc->abc_vMax * tmag /abc_mag; 00200 } 00201 else 00202 { 00203 tc->vMax = tc->tvMax; 00204 } 00205 } 00206 else 00207 { 00208 tc->aMax = tc->taMax; 00209 tc->vMax = tc->tvMax; 00210 } 00211 tc->targetPos = tmag; 00212 } 00213 00214 tc->currentPos = 0.0; 00215 tc->type = TC_LINEAR; 00216 return 0; 00217 } |
|
Definition at line 308 of file tc.c. Referenced by tpRunCycle().
00309 { 00310 if (0 == tc) 00311 { 00312 return -1; 00313 } 00314 00315 tc->preVMax = vMax; 00316 tc->preAMax = aMax; 00317 00318 return 0; 00319 } |
|
Definition at line 269 of file tc.c. Referenced by tpAddCircle(), and tpAddLine().
00270 { 00271 if (_WDotMax < 0.0 || 00272 0 == tc) 00273 { 00274 return -1; 00275 } 00276 00277 tc->abc_aMax = _WDotMax; 00278 00279 return 0; 00280 } |
|
Definition at line 256 of file tc.c. Referenced by tpAddCircle(), and tpAddLine().
00257 { 00258 if (_Rvmax < 0.0 || 00259 0 == tc) 00260 { 00261 return -1; 00262 } 00263 00264 tc->abc_vMax = _Rvmax; 00265 00266 return 0; 00267 } |
|
Definition at line 295 of file tc.c. Referenced by tpAddCircle(), and tpAddLine().
00296 { 00297 if (_aMax <= 0.0 || 00298 0 == tc) 00299 { 00300 return -1; 00301 } 00302 00303 tc->taMax = _aMax; 00304 00305 return 0; 00306 } |
|
Definition at line 243 of file tc.c. Referenced by tpAddCircle(), and tpAddLine().
00244 { 00245 if (_vMax < 0.0 || 00246 0 == tc) 00247 { 00248 return -1; 00249 } 00250 00251 tc->tvMax = _vMax; 00252 00253 return 0; 00254 } |
|
Definition at line 355 of file tc.c. Referenced by tpAddCircle(), and tpAddLine().
00356 { 00357 if (0 == tc) 00358 { 00359 return -1; 00360 } 00361 00362 if (cond != TC_TERM_COND_STOP && 00363 cond != TC_TERM_COND_BLEND) 00364 { 00365 return -1; 00366 } 00367 00368 tc->termCond = cond; 00369 00370 return 0; 00371 } |
|
Definition at line 321 of file tc.c. Referenced by tpAddCircle(), and tpAddLine().
00322 { 00323 if (0 == tc) 00324 { 00325 return -1; 00326 } 00327 00328 tc->vLimit = vLimit; 00329 00330 return 0; 00331 } |
|
Definition at line 282 of file tc.c. Referenced by tpAddCircle(), tpAddLine(), and tpSetVscale().
00283 { 00284 if (_vScale < 0.0 || 00285 0 == tc) 00286 { 00287 return -1; 00288 } 00289 00290 tc->vScale = _vScale; 00291 00292 return 0; 00293 } |
|
Definition at line 744 of file tc.c. Referenced by __attribute__().
00745 { 00746 if (_size <= 0 || 00747 0 == tcq) 00748 { 00749 return -1; 00750 } 00751 else 00752 { 00753 tcq->queue = tcSpace; 00754 tcq->size = _size; 00755 tcq->_len = 0; 00756 tcq->start = tcq->end = 0; 00757 tcq->allFull = 0; 00758 00759 if (0 == tcq->queue) 00760 { 00761 return -1; 00762 } 00763 return 0; 00764 } 00765 } |
|
Definition at line 767 of file tc.c. Referenced by tpDelete().
|
|
Definition at line 942 of file tc.c. Referenced by emcmotController().
00943 { 00944 if (0 == tcq) 00945 { 00946 return 1; /* null queue is full, for safety */ 00947 } 00948 00949 /* call the queue full if the length is into the margin, so reduce 00950 the effect of a race condition where the appending process may 00951 not see the full status immediately and send another motion */ 00952 00953 if (tcq->size <= TC_QUEUE_MARGIN) 00954 { 00955 /* no margin available, so full means really all full */ 00956 return tcq->allFull; 00957 } 00958 00959 if (tcq->_len >= tcq->size - TC_QUEUE_MARGIN) 00960 { 00961 /* we're into the margin, so call it full */ 00962 return 1; 00963 } 00964 00965 /* we're not into the margin */ 00966 return 0; 00967 } |
|
Definition at line 826 of file tc.c. 00827 { 00828 TC_STRUCT tc; 00829 00830 if ((0 == tcq) || 00831 (0 == tcq->queue) || /* not initialized */ 00832 ((tcq->start == tcq->end) && ! tcq->allFull)) /* empty queue */ 00833 { 00834 if (0 != status) 00835 { 00836 *status = -1; /* set status flag, if passed */ 00837 } 00838 tc.cycleTime=0; 00839 tc.targetPos=0; /* positive motion progession */ 00840 tc.vMax=0; /* max velocity */ 00841 tc.vScale=0; /* scale factor for vMax */ 00842 tc.aMax=0; /* max accel */ 00843 tc.preVMax=0; /* vel from previous blend */ 00844 tc.preAMax=0; /* decel (negative) from previous blend */ 00845 tc.vLimit=0; /* abs vel limit, including scale */ 00846 tc.toGo=0; 00847 tc.currentPos=0; 00848 tc.currentVel=0; 00849 tc.currentAccel=0; 00850 tc.tcFlag=0; /* TC_IS_DONE,ACCEL,CONST,DECEL*/ 00851 tc.type=0; /* TC_LINEAR, TC_CIRCULAR */ 00852 tc.id=0; /* id for motion segment */ 00853 tc.termCond=0; /* TC_END_STOP,BLEND */ 00854 return tc; 00855 } 00856 00857 /* get current int and set status for returning */ 00858 if (0 != status) 00859 { 00860 *status = 0; 00861 } 00862 tc = tcq->queue[tcq->start]; 00863 00864 /* update start ptr and reset allFull flag and len */ 00865 tcq->start = (tcq->start + 1) % tcq->size; 00866 tcq->allFull = 0; 00867 tcq->_len--; 00868 00869 return tc; 00870 } |
|
Definition at line 779 of file tc.c. Referenced by tpClear(), and tpRunCycle().
|
|
Definition at line 908 of file tc.c. Referenced by tcqLast(), tpIsPaused(), tpPrint(), tpRunCycle(), and tpSetVscale().
00909 { 00910 if ((0 == tcq) || 00911 (0 == tcq->queue) || /* not initialized */ 00912 (n < 0) || 00913 (n >= tcq->_len)) /* n too large */ 00914 { 00915 if (0 != status) 00916 { 00917 *status = -1; 00918 } 00919 return (TC_STRUCT *) 0; 00920 } 00921 00922 if (0 != status) 00923 { 00924 *status = 0; 00925 } 00926 return &(tcq->queue[(tcq->start + n) % tcq->size]); 00927 } |
|
|
|
Definition at line 897 of file tc.c. Referenced by tpAddCircle(), tpAddLine(), tpRunCycle(), and tpSetVscale().
00898 { 00899 if (0 == tcq) 00900 { 00901 return -1; 00902 } 00903 00904 return tcq->_len; 00905 } |
|
Definition at line 794 of file tc.c. Referenced by tpAddCircle(), and tpAddLine().
00795 { 00796 /* check for initialized */ 00797 if (0 == tcq || 00798 0 == tcq->queue) 00799 { 00800 return -1; 00801 } 00802 00803 /* check for allFull, so we don't overflow the queue */ 00804 if (tcq->allFull) 00805 { 00806 return -1; 00807 } 00808 00809 /* add it */ 00810 tcq->queue[tcq->end] = tc; 00811 tcq->_len++; 00812 00813 /* update end ptr, modulo size of queue */ 00814 tcq->end = (tcq->end + 1) % tcq->size; 00815 00816 /* set allFull flag if we're really full */ 00817 if (tcq->end == tcq->start) 00818 { 00819 tcq->allFull = 1; 00820 } 00821 00822 return 0; 00823 } |
|
Definition at line 873 of file tc.c. Referenced by tpRunCycle().
00874 { 00875 if (n <= 0) 00876 { 00877 return 0; /* okay to remove 0 or fewer */ 00878 } 00879 00880 if ((0 == tcq) || 00881 (0 == tcq->queue) || /* not initialized */ 00882 ((tcq->start == tcq->end) && ! tcq->allFull) || /* empty queue */ 00883 (n > tcq->_len)) /* too many requested */ 00884 { 00885 return -1; 00886 } 00887 00888 /* update start ptr and reset allFull flag and len */ 00889 tcq->start = (tcq->start + n) % tcq->size; 00890 tcq->allFull = 0; 00891 tcq->_len -= n; 00892 00893 return 0; 00894 } |
|
|