#include <stdio.h>
#include "rcs_prnt.hh"
#include <stdlib.h>
#include "posemath.h"
#include "tc.h"
#include "tp.h"
Include dependency graph for tp.c:
Go to the source code of this file.
Functions | |
char | __attribute__ ((unused)) ident[]="$Id |
int | tpDelete (TP_STRUCT *tp) |
int | tpClear (TP_STRUCT *tp) |
int | tpInit (TP_STRUCT *tp) |
int | tpSetCycleTime (TP_STRUCT *tp, double secs) |
int | tpSetVmax (TP_STRUCT *tp, double vMax) |
int | tpSetWmax (TP_STRUCT *tp, double wMax) |
int | tpSetVlimit (TP_STRUCT *tp, double vLimit) |
int | tpSetVscale (TP_STRUCT *tp, double scale) |
int | tpSetAmax (TP_STRUCT *tp, double aMax) |
int | tpSetWDotmax (TP_STRUCT *tp, double wdotmax) |
int | tpSetId (TP_STRUCT *tp, int id) |
int | tpGetNextId (TP_STRUCT *tp) |
int | tpGetExecId (TP_STRUCT *tp) |
int | tpSetTermCond (TP_STRUCT *tp, int cond) |
int | tpGetTermCond (TP_STRUCT *tp) |
int | tpSetPos (TP_STRUCT *tp, EmcPose pos) |
int | tpAddLine (TP_STRUCT *tp, EmcPose end) |
int | tpAddCircle (TP_STRUCT *tp, EmcPose end, PmCartesian center, PmCartesian normal, int turn) |
int | tpRunCycle (TP_STRUCT *tp) |
int | tpPause (TP_STRUCT *tp) |
int | tpResume (TP_STRUCT *tp) |
int | tpAbort (TP_STRUCT *tp) |
EmcPose | tpGetPos (TP_STRUCT *tp) |
int | tpIsDone (TP_STRUCT *tp) |
int | tpIsPaused (TP_STRUCT *tp) |
int | tpQueueDepth (TP_STRUCT *tp) |
int | tpActiveDepth (TP_STRUCT *tp) |
void | tpPrint (TP_STRUCT *tp) |
int | tpSetAout (TP_STRUCT *tp, unsigned char index, double start, double end) |
int | tpSetDout (TP_STRUCT *tp, unsigned char index, unsigned char start, unsigned char end) |
|
Definition at line 58 of file tp.c. 00058 : tp.c,v 1.9 2001/08/17 22:05:41 proctor Exp $"; 00059 00060 int tpCreate(TP_STRUCT *tp, int _queueSize, TC_STRUCT *tcSpace) 00061 { 00062 if (0 == tp) 00063 { 00064 return -1; 00065 } 00066 00067 if (_queueSize <= 0) 00068 { 00069 tp->queueSize = TP_DEFAULT_QUEUE_SIZE; 00070 } 00071 else 00072 { 00073 tp->queueSize = _queueSize; 00074 } 00075 00076 /* create the queue */ 00077 if (-1 == tcqCreate(&tp->queue, tp->queueSize, tcSpace)) 00078 { 00079 return -1; 00080 } 00081 00082 /* init the rest of our data */ 00083 return tpInit(tp); 00084 } |
|
Definition at line 756 of file tp.c. 00757 { 00758 if (0 == tp) 00759 { 00760 return -1; 00761 } 00762 00763 if (! tp->aborting) 00764 { 00765 /* to abort, signal a pause and set our abort flag */ 00766 tpPause(tp); 00767 tp->aborting = 1; 00768 } 00769 return 0; 00770 } |
|
Definition at line 834 of file tp.c. 00835 { 00836 if (0 == tp) 00837 { 00838 return 0; 00839 } 00840 00841 return tp->activeDepth; 00842 } |
|
Definition at line 433 of file tp.c. 00435 { 00436 TC_STRUCT tc; 00437 PmCircle circle; 00438 PmLine line_abc; 00439 PmPose endPose, circleGoalPose; 00440 PmPose abc_pose,goal_abc_pose; 00441 endPose.tran = end.tran; 00442 endPose.rot.s = 1.0; 00443 endPose.rot.x = endPose.rot.y = endPose.rot.z = 0.0; 00444 circleGoalPose.tran = tp->goalPos.tran; 00445 circleGoalPose.rot.s = 1.0; 00446 circleGoalPose.rot.x = circleGoalPose.rot.y = circleGoalPose.rot.z = 0.0; 00447 00448 if (0 == tp) { 00449 return -1; 00450 } 00451 00452 if (tp->aborting) { 00453 return -1; 00454 } 00455 00456 tcInit(&tc); 00457 pmCircleInit(&circle, circleGoalPose, endPose, center, normal, turn); 00458 tcSetCycleTime(&tc, tp->cycleTime); 00459 tcSetTVmax(&tc, tp->vMax); 00460 tcSetTAmax(&tc, tp->aMax); 00461 tcSetRVmax(&tc, tp->wMax); 00462 tcSetRAmax(&tc, tp->wDotMax); 00463 tcSetVscale(&tc, tp->vScale); 00464 tcSetVlimit(&tc, tp->vLimit); 00465 00466 abc_pose.tran.x = end.a; 00467 abc_pose.tran.y = end.b; 00468 abc_pose.tran.z = end.c; 00469 abc_pose.rot.s = 1.0; 00470 abc_pose.rot.x = abc_pose.rot.y = abc_pose.rot.z = 0.0; 00471 goal_abc_pose.tran.x = tp->goalPos.a; 00472 goal_abc_pose.tran.y = tp->goalPos.b; 00473 goal_abc_pose.tran.z = tp->goalPos.c; 00474 goal_abc_pose.rot.s = 1.0; 00475 goal_abc_pose.rot.x = goal_abc_pose.rot.y = goal_abc_pose.rot.z = 0.0; 00476 pmLineInit(&line_abc, goal_abc_pose, abc_pose); 00477 00478 tcSetCircle(&tc, circle, line_abc); 00479 tcSetId(&tc, tp->nextId); 00480 tcSetTermCond(&tc, tp->termCond); 00481 00482 if (tp->douts) { 00483 tcSetDout(&tc, tp->douts, tp->doutstart, tp->doutend); 00484 tp->douts = 0; 00485 tp->doutstart = 0; 00486 tp->doutend = 0; 00487 } 00488 00489 if (-1 == tcqPut(&tp->queue, tc)) { 00490 return -1; 00491 } 00492 00493 tp->goalPos = end; 00494 tp->done = 0; 00495 tp->depth = tcqLen(&tp->queue); 00496 tp->nextId++; 00497 00498 return 0; 00499 } |
|
Definition at line 367 of file tp.c. 00368 { 00369 TC_STRUCT tc; 00370 PmLine line, line_abc; 00371 PmPose tran_pose, goal_tran_pose; 00372 PmPose abc_pose, goal_abc_pose; 00373 00374 if (0 == tp) { 00375 return -1; 00376 } 00377 00378 if (tp->aborting) { 00379 return -1; 00380 } 00381 00382 tran_pose.tran = end.tran; 00383 tran_pose.rot.s = 1.0; 00384 tran_pose.rot.x = tran_pose.rot.y = tran_pose.rot.z = 0.0; 00385 goal_tran_pose.tran = tp->goalPos.tran; 00386 goal_tran_pose.rot.s = 1.0; 00387 goal_tran_pose.rot.x = goal_tran_pose.rot.y = goal_tran_pose.rot.z = 0.0; 00388 00389 abc_pose.tran.x = end.a; 00390 abc_pose.tran.y = end.b; 00391 abc_pose.tran.z = end.c; 00392 abc_pose.rot.s = 1.0; 00393 abc_pose.rot.x = abc_pose.rot.y = abc_pose.rot.z = 0.0; 00394 goal_abc_pose.tran.x = tp->goalPos.a; 00395 goal_abc_pose.tran.y = tp->goalPos.b; 00396 goal_abc_pose.tran.z = tp->goalPos.c; 00397 goal_abc_pose.rot.s = 1.0; 00398 goal_abc_pose.rot.x = goal_abc_pose.rot.y = goal_abc_pose.rot.z = 0.0; 00399 00400 tcInit(&tc); 00401 00402 pmLineInit(&line, goal_tran_pose, tran_pose); 00403 pmLineInit(&line_abc, goal_abc_pose, abc_pose); 00404 tcSetCycleTime(&tc, tp->cycleTime); 00405 tcSetTVmax(&tc, tp->vMax); 00406 tcSetTAmax(&tc, tp->aMax); 00407 tcSetRVmax(&tc, tp->wMax); 00408 tcSetRAmax(&tc, tp->wDotMax); 00409 tcSetVscale(&tc, tp->vScale); 00410 tcSetVlimit(&tc, tp->vLimit); 00411 tcSetLine(&tc, line, line_abc); 00412 tcSetId(&tc, tp->nextId); 00413 tcSetTermCond(&tc, tp->termCond); 00414 if (tp->douts) { 00415 tcSetDout(&tc, tp->douts, tp->doutstart, tp->doutend); 00416 tp->douts = 0; 00417 tp->doutstart = 0; 00418 tp->doutend = 0; 00419 } 00420 00421 if (-1 == tcqPut(&tp->queue, tc)) { 00422 return -1; 00423 } 00424 00425 tp->goalPos = end; 00426 tp->done = 0; 00427 tp->depth = tcqLen(&tp->queue); 00428 tp->nextId++; 00429 00430 return 0; 00431 } |
|
Definition at line 107 of file tp.c. 00108 { 00109 tcqInit(&tp->queue); 00110 tp->goalPos.tran.x = tp->currentPos.tran.x; 00111 tp->goalPos.tran.y = tp->currentPos.tran.y; 00112 tp->goalPos.tran.z = tp->currentPos.tran.z; 00113 tp->goalPos.a = tp->currentPos.a; 00114 tp->goalPos.b = tp->currentPos.b; 00115 tp->goalPos.c = tp->currentPos.c; 00116 tp->nextId = 0; 00117 tp->execId = 0; 00118 tp->termCond = TC_TERM_COND_BLEND; 00119 tp->done = 1; 00120 tp->depth = 0; 00121 tp->activeDepth = 0; 00122 tp->aborting = 0; 00123 tp->pausing = 0; 00124 tp->vScale = tp->vRestore; 00125 00126 return 0; 00127 } |
|
Definition at line 86 of file tp.c. 00087 { 00088 if (0 == tp) 00089 { 00090 return 0; 00091 } 00092 00093 return tcqDelete(&tp->queue); 00094 } |
|
Definition at line 310 of file tp.c. 00311 { 00312 if (0 == tp) 00313 { 00314 return -1; 00315 } 00316 00317 return tp->execId; 00318 } |
|
Definition at line 296 of file tp.c. 00297 { 00298 if (0 == tp) 00299 { 00300 return -1; 00301 } 00302 00303 return tp->nextId; 00304 } |
|
|
|
Definition at line 344 of file tp.c. 00345 { 00346 if (0 == tp) 00347 { 00348 return -1; 00349 } 00350 00351 return tp->termCond; 00352 } |
|
Definition at line 129 of file tp.c. 00130 { 00131 tp->cycleTime = 0.0; 00132 tp->vLimit = 0.0; 00133 tp->vScale = tp->vRestore = 1.0; 00134 tp->aMax = 0.0; 00135 tp->vMax = 0.0; 00136 tp->wMax = 0.0; 00137 tp->wDotMax = 0.0; 00138 00139 tp->currentPos.tran.x = 0.0; 00140 tp->currentPos.tran.y = 0.0; 00141 tp->currentPos.tran.z = 0.0; 00142 tp->currentPos.a = 0.0; 00143 tp->currentPos.b = 0.0; 00144 tp->currentPos.c = 0.0; 00145 00146 return tpClear(tp); 00147 } |
|
Definition at line 786 of file tp.c. 00787 { 00788 if (0 == tp) 00789 { 00790 return 0; 00791 } 00792 00793 return tp->done; 00794 } |
|
Definition at line 801 of file tp.c. 00802 { 00803 int t; 00804 00805 if (0 == tp) 00806 { 00807 return 0; 00808 } 00809 00810 if (0 == tp->depth) 00811 { 00812 return tp->pausing; 00813 } 00814 00815 for (t = 0; t < tp->activeDepth; t++) 00816 { 00817 if (! tcIsPaused(tcqItem(&tp->queue, t, 0))) 00818 return 0; 00819 } 00820 00821 return 1; 00822 } |
|
Definition at line 707 of file tp.c. 00708 { 00709 if (0 == tp) 00710 { 00711 return -1; 00712 } 00713 00714 if (! tp->pausing) 00715 { 00716 /* save the restore value */ 00717 tp->vRestore = tp->vScale; 00718 00719 /* apply 0 scale to queued motions */ 00720 tpSetVscale(tp, 0); 00721 00722 /* mark us pausing-- do this after the call to toSetVscale 00723 since this looks at the pausing flag to decide whether to 00724 set vRestore (if pausing) or vScale (if not). We want 00725 vScale to be set in the call for this one */ 00726 tp->pausing = 1; 00727 } 00728 00729 return 0; 00730 } |
|
Definition at line 844 of file tp.c. 00845 { 00846 #ifndef NO_STDIO_SUPPORT 00847 int t; 00848 00849 if (0 == tp) 00850 { 00851 rcs_print("(null)\n"); 00852 return; 00853 } 00854 00855 rcs_print("queueSize:\t%d\n", tp->queueSize); 00856 rcs_print("cycleTime:\t%f\n", tp->cycleTime); 00857 rcs_print("vMax:\t\t%f\n", tp->vMax); 00858 rcs_print("aMax:\t\t%f\n", tp->aMax); 00859 rcs_print("currentPos:\t%f\t%f\t%f\n", 00860 tp->currentPos.tran.x, tp->currentPos.tran.y, tp->currentPos.tran.z); 00861 rcs_print("goalPos:\t%f\t%f\t%f\n", 00862 tp->goalPos.tran.x, tp->goalPos.tran.y, tp->goalPos.tran.z); 00863 rcs_print("done: \t%d\n", tpIsDone(tp)); 00864 rcs_print("paused: \t%d\n", tpIsPaused(tp)); 00865 rcs_print("queueDepth:\t%d\n", tpQueueDepth(tp)); 00866 rcs_print("activeDepth:\t%d\n", tpActiveDepth(tp)); 00867 00868 for (t = 0; t < tp->depth; t++) 00869 { 00870 rcs_print("\t---\tTC %d\t---\n", t+1); 00871 tcPrint(tcqItem(&tp->queue, t, 0)); 00872 } 00873 #endif /* STDIO_SUPPORT */ 00874 } |
|
Definition at line 824 of file tp.c. 00825 { 00826 if (0 == tp) 00827 { 00828 return 0; 00829 } 00830 00831 return tp->depth; 00832 } |
|
Definition at line 732 of file tp.c. 00733 { 00734 if (0 == tp) 00735 { 00736 return -1; 00737 } 00738 00739 if (tp->pausing) 00740 { 00741 /* mark us not pausing-- this must be done before the call to 00742 tpSetVscale since that function will only apply the restored 00743 scale value if we're not pausing */ 00744 tp->pausing = 0; 00745 00746 /* restore scale value */ 00747 tp->vScale = tp->vRestore; 00748 00749 /* apply the restored scale value to queued motions */ 00750 tpSetVscale(tp, tp->vScale); 00751 } 00752 00753 return 0; 00754 } |
|
Definition at line 501 of file tp.c. 00502 { 00503 EmcPose sumPos; 00504 PmCartesian unitCart; 00505 double thisAccell =0.0, thisVel=0.0; 00506 PmCartesian thisAccellCart, thisVelCart; 00507 PmCartesian accellCart, velCart; 00508 double currentAccellMag =0.0, currentVelMag =0.0; 00509 double dot =0.0; 00510 00511 int toRemove = 0; 00512 TC_STRUCT *thisTc=0; 00513 int lastTcWasPureRotation = 0; 00514 int thisTcIsPureRotation = 0; 00515 int t; 00516 EmcPose before, after; 00517 double preVMax = 0.0; 00518 double preAMax = 0.0; 00519 00520 if (0 == tp) 00521 { 00522 return -1; 00523 } 00524 00525 sumPos.tran.x = sumPos.tran.y = sumPos.tran.z = 0.0; 00526 unitCart.x = unitCart.y = unitCart.z = 0.0; 00527 accellCart.x = accellCart.y = accellCart.z = 0.0; 00528 velCart.x = velCart.y = velCart.z = 0.0; 00529 sumPos.a = sumPos.b = sumPos.c = 0.0; 00530 00531 /* correct accumulation of errors between currentPos and before */ 00532 after.tran.x = before.tran.x = tp->currentPos.tran.x; 00533 after.tran.y = before.tran.y = tp->currentPos.tran.y; 00534 after.tran.z = before.tran.z = tp->currentPos.tran.z; 00535 after.a = before.a = tp->currentPos.a; 00536 after.b = before.b = tp->currentPos.b; 00537 after.c = before.c = tp->currentPos.c; 00538 00539 /* run all TCs at and before this one */ 00540 tp->activeDepth = 0; 00541 for (t = 0; t < tcqLen(&tp->queue); t++) 00542 { 00543 lastTcWasPureRotation = thisTcIsPureRotation; 00544 thisTc = tcqItem(&tp->queue, t, 0); 00545 thisTcIsPureRotation = thisTc->tmag < 1e-6; 00546 if (0 == thisTc || 00547 tcIsDone(thisTc)) 00548 { 00549 if (t <= toRemove) 00550 { 00551 toRemove++; 00552 } 00553 continue; 00554 } 00555 00556 if (thisTc->currentPos <= 0.0 && (tp->pausing || tp->aborting)) 00557 { 00558 continue; 00559 } 00560 // If either this move or the last move was a pure rotation 00561 // reset the velocity and accelleration and block any blending. 00562 if(lastTcWasPureRotation || thisTcIsPureRotation) 00563 { 00564 velCart.x = velCart.y = velCart.z = 0.0; 00565 accellCart.x = accellCart.y = accellCart.z = 0.0; 00566 currentVelMag = 0.0; 00567 currentAccellMag = 0.0; 00568 if(thisVel > 1e-6) 00569 { 00570 preVMax = thisTc->vMax; 00571 } 00572 if(thisAccell > 1e-6 || thisAccell < -1e-6) 00573 { 00574 preAMax = thisTc->aMax; 00575 } 00576 } 00577 else 00578 { 00579 unitCart = tcGetUnitCart(thisTc); 00580 00581 00582 /* The combined velocity of this move and the next one 00583 will be square root(currentVelocity^2 + nextVel^2 + 2*the dot product). 00584 to prevent the combined move from exceeding vMax preVMax may need adjustment. 00585 tcRunCycle will subtract preVMax from vMax and clamp the velocity to 00586 this value. 00587 */ 00588 pmCartMag(velCart,¤tVelMag); 00589 if(currentVelMag >= 1e-6) 00590 { 00591 pmCartCartDot(velCart,unitCart,&dot); 00592 preVMax = thisTc->vMax + dot - pmSqrt(pmSq(dot) -pmSq(currentVelMag) + pmSq(thisTc->vMax)); 00593 } 00594 else 00595 { 00596 preVMax = 0.0; 00597 } 00598 00599 /* The combined accelleration of this move and the next one 00600 will be square root(currentAccelleration^2 + nextAccell^2 + 2*the dot product). 00601 to prevent the combined move from exceeding vMax preVMax may need adjustment. 00602 tcRunCycle will subtract preVMax from vMax and clamp the accelleration to 00603 this value. 00604 */ 00605 pmCartMag(accellCart,¤tAccellMag); 00606 if(currentAccellMag >= 1e-6) 00607 { 00608 pmCartCartDot(accellCart,unitCart,&dot); 00609 preAMax = thisTc->aMax + dot - pmSqrt(pmSq(dot) -pmSq(currentAccellMag) + pmSq(thisTc->aMax)); 00610 } 00611 else 00612 { 00613 preAMax = 0.0; 00614 } 00615 } 00616 00617 before = tcGetPos(thisTc); 00618 tcSetPremax(thisTc, preVMax, preAMax); 00619 tcRunCycle(thisTc); 00620 after = tcGetPos(thisTc); 00621 if (tp->activeDepth <= toRemove) 00622 { 00623 tp->execId = tcGetId(thisTc); 00624 } 00625 00626 pmCartCartSub(after.tran, before.tran, &after.tran); 00627 pmCartCartAdd(sumPos.tran, after.tran, &sumPos.tran); 00628 sumPos.a += after.a - before.a; 00629 sumPos.b += after.b - before.b; 00630 sumPos.c += after.c - before.c; 00631 00632 if (tcIsDone(thisTc)) 00633 { 00634 /* this one is done-- blend in the next one */ 00635 if (t <= toRemove) 00636 { 00637 toRemove++; 00638 } 00639 continue; 00640 } 00641 00642 /* this one is still active-- increment active count */ 00643 tp->activeDepth++; 00644 00645 if (tcIsDecel(thisTc) && 00646 tcGetTermCond(thisTc) == TC_TERM_COND_BLEND) 00647 { 00648 /* this one is decelerating-- blend in the next one with 00649 credit for this decel */ 00650 thisAccell = tcGetAccel(thisTc); 00651 thisVel = tcGetVel(thisTc); 00652 unitCart = tcGetUnitCart(thisTc); 00653 pmCartScalMult(unitCart,thisAccell,&thisAccellCart); 00654 pmCartCartAdd(thisAccellCart,accellCart,&accellCart); 00655 pmCartScalMult(unitCart,thisVel,&thisVelCart); 00656 pmCartCartAdd(thisVelCart,velCart,&velCart); 00657 continue; 00658 } 00659 else 00660 { 00661 /* this one is either accelerating or constant-- no 00662 room for any more blending */ 00663 preVMax = 0.0; 00664 preAMax = 0.0; 00665 break; 00666 } 00667 } 00668 00669 if (toRemove > 0) 00670 { 00671 tcqRemove(&tp->queue, toRemove); 00672 tp->depth = tcqLen(&tp->queue); 00673 if (tp->depth == 0) 00674 { 00675 tp->done = 1; 00676 tp->activeDepth = 0; 00677 tp->execId = 0; 00678 } 00679 } 00680 00681 /* increment current position with sum of increments */ 00682 pmCartCartAdd(tp->currentPos.tran, sumPos.tran, &tp->currentPos.tran); 00683 tp->currentPos.a += sumPos.a; 00684 tp->currentPos.b += sumPos.b; 00685 tp->currentPos.c += sumPos.c; 00686 00687 00688 /* check for abort done */ 00689 if (tp->aborting && 00690 (tpIsPaused(tp) || 00691 tpIsDone(tp))) 00692 { 00693 /* all paused and we're aborting-- clear out the TP queue */ 00694 tcqInit(&tp->queue); 00695 tp->goalPos = tp->currentPos; 00696 tp->done = 1; 00697 tp->depth = 0; 00698 tp->activeDepth = 0; 00699 tp->aborting = 0; 00700 tp->execId = 0; 00701 tpResume(tp); 00702 } 00703 00704 return 0; 00705 } |
|
Definition at line 248 of file tp.c. 00249 { 00250 if (0 == tp || 00251 aMax <= 0.0) 00252 { 00253 return -1; 00254 } 00255 00256 tp->aMax = aMax; 00257 00258 return 0; 00259 } |
|
Definition at line 876 of file tp.c. 00877 { 00878 /* FIXME-- unimplemented due to large size required for doubles */ 00879 return 0; 00880 } |
|
Definition at line 149 of file tp.c. 00150 { 00151 if (0 == tp || 00152 secs <= 0.0) 00153 { 00154 return -1; 00155 } 00156 00157 tp->cycleTime = secs; 00158 00159 return 0; 00160 } |
|
Definition at line 882 of file tp.c. 00883 { 00884 if (0 == tp) { 00885 return 0; 00886 } 00887 00888 if (index > 7) { 00889 return 0; 00890 } 00891 00892 tp->douts |= (1 << index); 00893 if (start == 0) { 00894 tp->doutstart &= (0xFF ^ (1 << index)); 00895 } 00896 else { 00897 tp->doutstart |= (1 << index); 00898 } 00899 if (end == 0) { 00900 tp->doutend &= (0xFF ^ (1 << index)); 00901 } 00902 else { 00903 tp->doutend |= (1 << index); 00904 } 00905 00906 return 0; 00907 } |
|
Definition at line 281 of file tp.c. 00282 { 00283 if (0 == tp) 00284 { 00285 return -1; 00286 } 00287 00288 tp->nextId = id; 00289 00290 return 0; 00291 } |
|
Definition at line 354 of file tp.c. 00355 { 00356 if (0 == tp) 00357 { 00358 return -1; 00359 } 00360 00361 tp->currentPos = pos; 00362 tp->goalPos = pos; 00363 00364 return 0; 00365 } |
|
Definition at line 326 of file tp.c. 00327 { 00328 if (0 == tp) 00329 { 00330 return -1; 00331 } 00332 00333 if (cond != TC_TERM_COND_STOP && 00334 cond != TC_TERM_COND_BLEND) 00335 { 00336 return -1; 00337 } 00338 00339 tp->termCond = cond; 00340 00341 return 0; 00342 } |
|
Definition at line 190 of file tp.c. 00191 { 00192 if (0 == tp || 00193 vLimit <= 0.0) 00194 { 00195 return -1; 00196 } 00197 00198 tp->vLimit = vLimit; 00199 00200 return 0; 00201 } |
|
Definition at line 162 of file tp.c. 00163 { 00164 if (0 == tp || 00165 vMax <= 0.0) 00166 { 00167 return -1; 00168 } 00169 00170 tp->vMax = vMax; 00171 00172 return 0; 00173 } |
|
Definition at line 203 of file tp.c. 00204 { 00205 TC_STRUCT *thisTc; 00206 int t; 00207 int depth; 00208 00209 if (0 == tp) 00210 { 00211 return -1; 00212 } 00213 00214 if (scale < 0.0) 00215 { 00216 /* clamp it */ 00217 scale = 0.0; 00218 } 00219 00220 /* record the scale factor */ 00221 if (tp->pausing) 00222 { 00223 /* if we're pausing, our scale is 0 and needs to stay there 00224 so that it's applied to any added motions. We'll put the 00225 requested scale in the restore value so that when we 00226 resume the new scale is applied. Also, don't send this 00227 down to the queued motions-- they're already paused */ 00228 tp->vRestore = scale; 00229 } 00230 else 00231 { 00232 /* we're not pausing, so it goes right in and is applied to 00233 the global value for subsequent moves, and also all 00234 queued moves */ 00235 tp->vScale = scale; 00236 00237 depth = tcqLen(&tp->queue); 00238 for (t = 0; t < depth; t++) 00239 { 00240 thisTc = tcqItem(&tp->queue, t, 0); 00241 tcSetVscale(thisTc, scale); 00242 } 00243 } 00244 00245 return 0; 00246 } |
|
Definition at line 261 of file tp.c. 00262 { 00263 if (0 == tp || 00264 wdotmax <= 0.0) 00265 { 00266 return -1; 00267 } 00268 00269 tp->wDotMax = wdotmax; 00270 00271 return 0; 00272 } |
|
Definition at line 175 of file tp.c. 00176 { 00177 if (0 == tp || 00178 wMax <= 0.0) 00179 { 00180 return -1; 00181 } 00182 00183 tp->wMax = wMax; 00184 00185 return 0; 00186 } |