segmentqueue.h File Reference

02218 {
02219   if (1 == sq->aborting)
02220     /* we are already aborting, so let's just ignore it */
02221     return 0;
02222 
02223   if (sq==0)
02224     {
02225       diagnostics("Error in sqAbort\n");
02226       return -1;
02227     }
02228 
02229   if ( sq->paused == 1 || sq->done == 1 )
02230     {
02231       if (-1 == sqClearQueue(sq))
02232         {
02233           diagnostics("Error in sqAbort\n");
02234           return -1;
02235         }
02236     }
02237   else
02238     {
02239       sq->aborting=1;
02240       sqPause(sq);
02241     }
02242   return 0;
02243 }

01169 {
01170   double length, maxUVec;
01171   SEGMENT *newseg;
01172   EmcPose start;
01173 
01174 
01175   /* check if segment queue has been initialized */
01176   if ( sq == 0 || sq->queue == 0 )
01177     {
01178       diagnostics("Error in sqAddLine()\n");
01179       return -1;
01180     }
01181 
01182   /* check for full */
01183   if ( sq->numSegments == sq->size )
01184     {
01185       diagnostics("Panic!!!!, segmentqueue overflows!!!\n");
01186       return -1;
01187     }
01188 
01189   /* check if new motion has zero length */
01190   if ( sq->numSegments==0 )
01191     start=sq->lastPoint;
01192   else
01193     start=sq->queue[(sq->end+sq->size-1)%sq->size].end;
01194 
01195   length = sqGiveLength(start,end);
01196 
01197   if (length==0)
01198     {
01199       /* only set ID of last appended motion */
01200       sq->lastAppMotionID=ID;
01201       return 0;
01202     }
01203 
01204   /* let newseg point to the first empty place in the ring buffer... */
01205   newseg= sq->queue+sq->end;
01206   /* ...and update the ring buffer properties */
01207   sq->end = (sq->end+1) % sq->size;
01208   sq->numSegments++;
01209   if ( sq->numSegments >= sq->size - SQ_SAFETY_MARGIN )
01210     {
01211       sq->full=1;
01212     }
01213 
01214   /* fill segment parameter fields */
01215   newseg->ID             = ID;
01216   newseg->type           = SQ_LINEAR;
01217   newseg->length         = length;
01218   newseg->totLength      = length;
01219   newseg->start          = start;
01220   newseg->end            = end;
01221   newseg->maxInc         = sq->feed*sq->cycleTime;
01222   newseg->finalInc       = 0;
01223   newseg->plInitInc      = 0;
01224   newseg->plFinalInc     = 0;
01225   newseg->cruiseInc      = 0;
01226   newseg->planningDone   = 0;
01227   newseg->active         = 0;
01228   newseg->numLinkedSegs  = 0;
01229   newseg->linkedToPrevSeg= 0;
01230   newseg->totNumPoints   = 0;
01231   newseg->nextSegment    = 0;
01232 
01233   /* initialize line */
01234   pmLineInit( &newseg->line, newseg->start, newseg->end);
01235 
01236   /* set the maximum tangential acceleration for this line */
01237   maxUVec = max ( fabs(newseg->line.uVec.x) , fabs(newseg->line.uVec.y) );
01238   maxUVec = max ( fabs(newseg->line.uVec.z) , maxUVec );
01239   newseg->amaxTan = sq->maxAcc/maxUVec;
01240 
01241   diagnosticsOff("Amax tan = %d\n",(int)newseg->amaxTan);
01242 
01243   if (-1 == sqPreprocessSegment(sq, newseg))
01244     {
01245       diagnostics("Error in sqAddLine()\n");
01246       return -1;
01247     }
01248 
01249   /* set last Appended Motion ID */
01250   sq->lastAppMotionID=ID;
01251 
01252   return 0;
01253 }

01111 {
01112   if (sq==0)
01113     {
01114       diagnostics("Error in sqClearQueue(): sq == 0 \n");
01115       return -1;
01116     }
01117   sq->numSegments=0;
01118   sq->start = sq->end = 0;
01119   sq->full = 0;
01120   sq->n=0;
01121   sq->feedOverrideFactor=1.0;
01122   sq->currentID=0;
01123 
01124   sq->done=1;      /* the queue is empty, so by definition we're done */
01125   sq->paused=0;
01126   sq->stepping=0;
01127   sq->feedAdjusting=0;
01128   sq->aborting=0;
01129 
01130   sq->currentVel=0;
01131   oldDist=0;
01132   oldPos = sq->lastPoint;
01133   oldVel.tran.x=0;
01134   oldVel.tran.y=0;
01135   oldVel.tran.z=0;
01136 
01137   return 0;
01138 }

02348 {
02349   if (sq==0)
02350     {
02351       diagnostics("Error in sqGetDepth\n");
02352       return -1;
02353     }
02354   return sq->numSegments;
02355 }

02368 {
02369   if (sq==0)
02370     {
02371       diagnostics("Error in sqGetID\n");
02372       return -1;
02373     }
02374   return sq->currentID;
02375 }

02338 {
02339   if (sq==0)
02340     {
02341       diagnostics("Error in sqGetMaxAcc\n");
02342       return -1;
02343     }
02344   return sq->maxAcc;
02345 }

01365 {
01366   if ( (sq == 0) || (p == 0) )
01367     {
01368       diagnostics("Error in sqGetPosition()\n");
01369       return -1;
01370     }
01371 
01372   *p = sq->lastPoint;
01373   return 0;
01374 };

02327 {
02328   if (sq==0)
02329     {
02330       diagnostics("Error in sqGetVel\n");
02331       return -1;
02332     }
02333 
02334   return sq->currentVel;
02335 }

01004 {
01005   if (size <= 0 || sq == 0 || first == 0 )
01006     {
01007       diagnostics("Error in sqInitQueue()\n");
01008       return -1;
01009     }
01010 
01011   sq->queue = first;
01012 
01013   /* FIXME: third argument (size) is ignored, since the queue size for
01014      the coordinated mode queue can not be set seperately from the
01015      free mode queues */
01016   sq->size = 2000;
01017 
01018   sq->start = sq->end = 0;
01019   sq->full = 0;
01020   sq->numSegments=0;
01021 
01022   sq->initXYZ.tran.x=0;
01023   sq->initXYZ.tran.y=0;
01024   sq->initXYZ.tran.z=0;
01025   sq->lastPoint=sq->initXYZ;
01026   sq->n=0;
01027   sq->maxFeedOverrideFactor = 1.0;   /* this has to be set at start-up using
01028                                         sqSetMaxFeedOverride() */
01029   sq->feedOverrideFactor=1.0;
01030   sq->cycleTime=0;
01031   sq->maxAcc=0;
01032   sq->maxV=0;
01033   sq->currentID=0;
01034 
01035   sq->done=1;      /* the queue is empty, so by definition we're done */
01036   sq->paused=0;
01037   sq->stepping=0;
01038   sq->feedAdjusting=0;
01039   sq->aborting=0;
01040 
01041 
01042   /* initializing debug variables */
01043   oldPos = sq->lastPoint;
01044   oldVel.tran.x=0;
01045   oldVel.tran.y=0;
01046   oldVel.tran.z=0;
01047   oldDist = 0;
01048   return 0;
01049 };

02307 {
02308   if (sq==0)
02309     {
02310       diagnostics("Error in sqIsStepping\n");
02311       return -1;
02312     }
02313   return sq->done;
02314 }

02317 {
02318   if (sq==0)
02319     {
02320       diagnostics("Error in sqIsPaused\n");
02321       return -1;
02322     }
02323   return sq->paused;
02324 }

02358 {
02359   if (sq==0)
02360     {
02361       diagnostics("Error in sqIsQueueFull\n");
02362       return -1;
02363     }
02364   return sq->full;
02365 }

02297 {
02298   if (sq==0)
02299     {
02300       diagnostics("Error in sqIsStepping\n");
02301       return -1;
02302     }
02303   return sq->stepping;
02304 }

01996 {
01997   SEGMENT *as;          /* as = Active Segment */
01998   double startInc;      /* the incrediment at the time the pause command
01999                            was given */
02000   double startAcc;      /* the derivate of the inc at that time */
02001   int npow1,npow2,npow3;
02002   double amaxTan;
02003 
02004   if (sq==0 || sq->queue==0 )
02005     {
02006       diagnostics("Error in sqPause\n");
02007       return -1;
02008     }
02009 
02010   if (sq->paused == 1)
02011     /* don't do anything, system is already paused */
02012     return 0;
02013 
02014   /* set paused flag */
02015   sq->paused=1;
02016 
02017   if ( sq->numSegments==0)
02018     {
02019       /* the queue is empty */
02020       sq->done = 1; /* propably redundant */
02021       return 0;
02022     }
02023 
02024   as=sq->queue+sq->start;
02025 
02026   if ( as->active==0 && as->initInc==0 )
02027     /* then this is the very first segment and we have not started yet.
02028        So let's set the done flag and return */
02029     {
02030       sq->done=1;
02031       return 0;
02032     }
02033 
02034   sq->feedAdjusting=0;  /* if we were adjusting the speed, ignore that too */
02035 
02036   amaxTan= sqGiveMinAmaxTan(as);
02037 
02038   if ( as->active==0 || sq->n<=3)
02039     {
02040       startInc = as->plInitInc;
02041       startAcc = 0;
02042       as->active=1;
02043       as->planningDone=1; /* the planning will be done in this function */
02044 
02045       as->q= ceil ( 3 * startInc / ( 2 * amaxTan * sq->ctPow2 ) );
02046       as->m=0;
02047       as->p=0;
02048       as->totNumPoints = as->q + 3;
02049       sq->n=3;          /* start allover, but skip the first initInc steps */
02050 
02051     }
02052   else if ( sq->n < as->m + 2 )
02053     {
02054       /* recalculate the last two dist values */
02055       npow1 = sq->n - 3;                /* sq->n-2-1 */
02056       npow2 = npow1 * npow1;
02057       npow3 = npow2 * npow1;
02058       startInc = as->a1 * npow3 *sq->ctPow3 \
02059                   + as->b1 * npow2 * sq->ctPow2 \
02060                   + as->c1 * npow1 * sq->cycleTime \
02061                   + as->d1;
02062       npow1 = sq->n - 4;                /* sq->n-2-2 */
02063       npow2 = npow1 * npow1;
02064       npow3 = npow2 * npow1;
02065       startAcc = startInc - (as->a1 * npow3 *sq->ctPow3 \
02066                   + as->b1 * npow2 * sq->ctPow2 \
02067                   + as->c1 * npow1 * sq->cycleTime \
02068                   + as->d1);
02069       as->q= ceil ( 2.12* startInc / ( amaxTan * sq->ctPow2 ) );
02070       as->m=0;
02071       as->p=0;
02072       as->totNumPoints = as->q + 3;
02073       sq->n=3;          /* start allover, but skip the first initInc steps */
02074     }
02075   else if ( sq->n < as->m + as->p + 2 )
02076     {
02077       startInc = as->cruiseInc;
02078       startAcc = 0;
02079       as->q= ceil ( 3 * startInc / ( 2 * amaxTan * sq->ctPow2 ) );
02080       as->m=0;
02081       as->p=0;
02082       as->totNumPoints = as->q + 3;
02083       sq->n=3;          /* start allover, but skip the first initInc steps */
02084     }
02085   else
02086     {
02087       /* we are already decelerating, so it would be best to finish that
02088          first. After we're done with that, we can decelerate to zero */
02089 
02090       /* before messing the whole thing up, let's see if we are already
02091          decelerating to zero */
02092       if (as->plFinalInc==0)
02093         {
02094           /* Then we'll reach the end of the segment with a zero velocity.
02095              This means that we don't have to do anything but waiting at the
02096              end of the segment. This is basically the same thing that
02097              happens at the end of a step-motion. This is exactly what
02098              we will tell the system: that we're stepping, so that all of
02099              the extra things needed for a pause within a segment are skipped.
02100           */
02101           sq->paused=0;
02102           sq->stepping=1;
02103           return 0;
02104         }
02105       else
02106         {
02107 
02108           /* let's do a little trick: we copy the deceleration parameters
02109              of the current motion to the acceleration phase parameter
02110              fields in the current segment and calculate a new
02111              deceleration action from finalInc to 0. We'll decrease sq->n
02112              with a number such of steps, such that it will look like
02113              nothing has happened. sqRunCycle won't notice this, and will
02114              think it's (again) in phase 1. We know better.... */
02115 
02116           as->a1=as->a3;
02117           as->b1=as->b3;
02118           as->c1=as->c3;
02119           as->d1=as->d3;
02120 
02121           sq->n -= as->m + as->p;
02122 
02123           as->m =as->q;
02124           as->p=1 ;        /* the cruising phase now becomes the
02125                               usual final step of a segment */
02126 
02127           /* find the finalInc */
02128           startInc = as->plFinalInc;
02129           startAcc = 0;
02130           as->cruiseInc = startInc;
02131 
02132           as->q= ceil ( 3 * startInc / ( 2 * amaxTan * sq->ctPow2 ) );
02133           as->totNumPoints = as->m + as->q + as->p + 2;
02134 
02135         }
02136     }
02137   as->b3 = - ( 3 * startInc + 2 * as->q* startAcc ) \
02138     / ( as->q * as->q * sq->ctPow2 ) ;
02139   as->a3 = ( 2 * startInc + as->q* startAcc ) \
02140     / ( as->q * as->q * as->q * sq->ctPow3 ) ;
02141   as->c3 = startAcc/sq->cycleTime;
02142   as->d3 = startInc;
02143 
02144   as->plFinalInc=0;
02145 
02146   return 0;
02147 }

02151 {
02152   SEGMENT *as;                  /* as = Active Segment */
02153   SEGMENT *cursor;
02154 
02155   if (sq==0 || sq->queue==0 )
02156     {
02157       diagnostics("Error in sqResume\n");
02158       return -1;
02159     }
02160 
02161   if ( sq->done!=1)
02162 
02163     /* we can't resume if the systems is not done yet with a step or pause
02164        action */
02165     {
02166       diagnosticsOff("Can't resume if not done with pause or step action\n");
02167       /* this is not a critical error, so we'll just ignore the command */
02168       return 0;
02169     }
02170 
02171   if ( sq->feedOverrideFactor == 0 )
02172     {
02173       /* we can't resume if the feed override factor is zero. To resume
02174          the user should set a new non-zero value for this factor first.
02175          This will immediately result in a resume action. */
02176       diagnostics("Can't resume if feed override is zero\n");
02177       /* not a critical error, so ignore it */
02178       return 0;
02179     }
02180 
02181   if ( sq->numSegments == 0)
02182     {
02183       /* there's not much to do */
02184       sq->paused = 0;
02185       return 0;
02186     }
02187 
02188   /* let's see if the length of the segment(chain) is large enough to
02189      accelerate from zero to finalInc */
02190   as=sq->queue+sq->start;
02191   cursor=as;
02192   /* first find the last segment in the chain */
02193   while ( cursor->nextSegment!=0 && cursor->nextSegment->linkedToPrevSeg==1)
02194     cursor=cursor->nextSegment;
02195 
02196   /* keep linking successive segments to as until the chain is long
02197      enough */
02198 
02199   if ( -1 == sqForwardLinkSegment(sq, as, sq->feedOverrideFactor ))
02200     {
02201       diagnostics("Error in sqResume \n ");
02202       return -1;
02203     }
02204 
02205   if (sq->paused == 1 && sq->stepping == 1 )
02206     sq->stepping=1;
02207   else
02208     sq->stepping=0;
02209 
02210   sq->paused=0;
02211   sq->done=0;
02212 
02213   return 0;
02214 }

01377 {
01378   SEGMENT *as;             /* to use instead of sq->queue[sq->start],
01379                               stands for Active Segment */
01380   int i;
01381   SEGMENT *cursor;
01382   double finalInc;
01383   int done;
01384   int minNumSteps;
01385   double angleToGo;
01386   double amaxTan;
01387   int turn;
01388   PmCartesian normal, center;
01389 
01390   int npow1, npow2, npow3; /* to speed up cubic calculations */
01391 
01392   if (sq==0 )
01393     {
01394       diagnostics("Error in sqRunCycle(): Segmentqueue doesn't exist!\n");
01395       return -1;
01396     }
01397 
01398   if ( sq->full == 1 && sq->numSegments < sq->size - SQ_SAFETY_MARGIN )
01399     diagnostics("sq->full == 1 although queue's not full!!!!\n");
01400 
01401   if ( sq->done==1 )
01402     {
01403       /* do nothing */
01404       return 0;
01405     }
01406   /* if buffer is empty give last point (i.e. do nothing) */
01407   if ( sq->numSegments==0 )
01408     {
01409       /* set the 'done' flag ..... */
01410       sq->done=1;
01411 
01412       /* check if currentID is the same as the ID of the last appended motion.
01413          The only case in which this will not be true is when the last motion
01414          has a zero length */
01415       if (sq->currentID != sq->lastAppMotionID )
01416         sq->currentID=  sq->lastAppMotionID;
01417       return 0;
01418     }
01419   as=sq->queue+sq->start;   /* much shorter ..... */
01420   if (as == 0 )
01421     {
01422       diagnostics("Panic: as = NULL in sqRunCycle\n");
01423       return -1;
01424     }
01425 
01426   if (as->active==0)
01427     /* we're about to start with a new segment */
01428     {
01429       if (-1 == sqPlanSegment(sq, as))
01430         {
01431           diagnostics("Error in sqRunCycle\n");
01432           return -1;
01433         }
01434       /* mark the whole chain as active */
01435       cursor=as;
01436       for (i=0;i<=as->numLinkedSegs;i++)
01437         {
01438           if (cursor == 0 )
01439             {
01440               diagnostics("Panic: cursor = NULL in sqRunCycle\n");
01441               return -1;
01442             }
01443           cursor->active=1;
01444           cursor=cursor->nextSegment;
01445         }
01446       oldDist=0;
01447       sq->dist=0;
01448 
01449       /* reset base, cursor, offset and cumlength and n */
01450       sq->base = as->start;
01451       sq->cursor=as;
01452       sq->offset=0;
01453       sq->cumLength=as->length;
01454       sq->n=1;
01455       sq->currentID=as->ID;
01456 
01457     }
01458   /* else: planning has been done before and the parameters are correctly
01459      initialized, unless someone deliberately changed planningDone to 1.
01460      Let's trust our user and not check for that here. */
01461 
01462   /* depending of in what phase of the motion we are, determine the new
01463      distance */
01464   if ( sq->n==1 || sq->n==2 )
01465     {
01466       sq->dist += as->plInitInc;
01467     }
01468   else if ( sq->n <= as->m +2 )
01469     /* acceleration phase */
01470     {
01471       npow1 = sq->n - 2;
01472       npow2 = npow1 * npow1;
01473       npow3 = npow2 * npow1;
01474       sq->dist += as->a1 * npow3 *sq->ctPow3 \
01475                   + as->b1 * npow2 * sq->ctPow2 \
01476                   + as->c1 * npow1 * sq->cycleTime \
01477                   + as->d1;
01478     }
01479   else if ( sq->n <= as->m + as->p + 2 )
01480     /* cruising phase */
01481     {
01482       sq->dist += as->cruiseInc;
01483     }
01484   else if ( sq->n <= as->m + as->p + as->q +2 )
01485     /* deceleration phase */
01486     {
01487       npow1 = sq->n - as->m - as->p - 2;
01488       npow2 = npow1 * npow1;
01489       npow3 = npow2 * npow1;
01490       sq->dist += as->a3 * npow3 *sq->ctPow3 \
01491                   + as->b3 * npow2 * sq->ctPow2 \
01492                   + as->c3 * npow1 * sq->cycleTime \
01493                   + as->d3;
01494 
01495     }
01496   else if ( sq->n == as->m + as->p + as->q + 3 )
01497     /* last step */
01498     {
01499       sq->dist+= as->plFinalInc;
01500     }
01501   else
01502     /* we have a problem, because this should never happen */
01503     {
01504       diagnostics("Error in sqRunCycle\n");
01505       return -1;
01506     }
01507 
01508   /* transform the dist scalar into a XYZ triplet */
01509   if (as->nextSegment !=0 && ( as->nextSegment->linkedToPrevSeg ==1 ||
01510                                sq->paused == 1))
01511     /* the active segment is the first segment of a chain */
01512     /* the sq->paused == 1 test is added to make sure that if a pause
01513        command is given just before the end of the segment, that the
01514        following segment is used to finish decelerating to zero */
01515     {
01516       while ( (sq->dist > sq->cumLength)
01517               && ( sq->cursor->nextSegment!=0 )
01518               && ( (sq->cursor->nextSegment->linkedToPrevSeg==1 ) ||
01519                    sq->paused==1 ) )
01520         {
01521           sq->offset  = sq->cumLength;
01522           sq->base    = sq->cursor->end;
01523           sq->cursor  = sq->cursor->nextSegment;
01524           sq->cumLength += sq->cursor->length;
01525 
01526         }
01527       /* set currentID */
01528       sq->currentID=sq->cursor->ID;
01529 
01530       if ( sq->cursor->type == SQ_LINEAR )
01531         pmLinePoint( &sq->cursor->line, sq->dist - sq->offset, &sq->lastPoint);
01532       else
01533         pmCirclePoint( &sq->cursor->circle, \
01534                        (sq->dist - sq->offset)/sq->cursor->helixRadius,\
01535                        &sq->lastPoint );
01536     }
01537 
01538   else
01539     /* the active segment has no other segments linked to it, which makes
01540        things much easier... */
01541     {
01542       if ( sq->cursor->type == SQ_LINEAR )
01543         pmLinePoint(&as->line, sq->dist, &sq->lastPoint);
01544       else
01545         pmCirclePoint(&as->circle, sq->dist/as->helixRadius, &sq->lastPoint);
01546     }
01547 
01548   sq->n++;
01549 
01550   if (sq->n > as->totNumPoints )
01551     {
01552       if (sq->aborting == 1)
01553         {
01554           if (-1 == sqClearQueue(sq))
01555             {
01556               diagnostics("Error in sqRunCycle\n");
01557               return -1;
01558             }
01559         }
01560 
01561       else if (sq->paused == 1 || sq->feedAdjusting == 1)
01562         {
01563 
01564           if (sq->paused==1)
01565             {
01566               sq->done=1;
01567               finalInc=0;
01568             }
01569           else
01570             {
01571               sq->feedAdjusting=0;
01572               finalInc=as->plFinalInc;
01573             }
01574 
01575           /* remove all segments preceding the current segment */
01576           cursor=as;
01577           while ( cursor!=sq->cursor )
01578             {
01579               cursor=cursor->nextSegment;
01580               if (cursor == 0 )
01581                 {
01582                   diagnostics("Panic: cursor = NULL in sqRunCycle\n");
01583                   return -1;
01584                 }
01585               sq->numSegments--;
01586             }
01587           if ( sq->numSegments < sq->size - SQ_SAFETY_MARGIN )
01588             sq->full = 0;
01589           if ( sq->numSegments < 0 )
01590             {
01591               diagnostics("Panic: sq->numSegments <0  in sqRunCycle\n");
01592               return -1;
01593             }
01594 
01595           cursor->prevSegment=0;
01596           cursor->linkedToPrevSeg=0;
01597           sq->start= sq->cursor - sq->queue;
01598           as=cursor;
01599 
01600           as->planningDone=0;
01601           as->plInitInc=finalInc;
01602           as->start=sq->lastPoint;
01603 
01604           if ( as->type == SQ_LINEAR)
01605             {
01606               as->length=sqGiveLength(as->start,as->end);
01607               as->totLength=as->length;
01608               pmLineInit(&as->line,as->start,as->end);
01609             }
01610           else
01611             {
01612               angleToGo= as->circle.angle-\
01613                          (sq->dist-sq->offset)/as->helixRadius;
01614               turn = floor(angleToGo / ( 2 * PM_PI ));
01615               normal = as->circle.normal;
01616               center = as->circle.center;
01617               pmCircleInit( &as->circle, as->start, as->end, \
01618                             center, normal, turn);
01619               as->length=as->circle.angle* as->helixRadius;
01620               as->totLength= as->length;
01621             }
01622 
01623 
01624           as->active=0;         /* mark the first segment of the chain
01625                                    as not active */
01626 
01627           /* determine how many segments are linked to as */
01628           as->numLinkedSegs=0;
01629           while (cursor->nextSegment!=0 &&
01630                  cursor->nextSegment->linkedToPrevSeg==1 )
01631             {
01632               cursor=cursor->nextSegment;
01633               as->numLinkedSegs++;
01634               as->totLength+=cursor->length;
01635             }
01636           /* find the minimum of the amax's of the segments in the chain */
01637           amaxTan=sqGiveMinAmaxTan(as);
01638 
01639           done=0;
01640           /* keep linking segments until the chain is long enough */
01641           while (!done)
01642             {
01643               if ( cursor->nextSegment!=0 && cursor->nextSegment->maxInc < \
01644                    cursor->maxInc)
01645                 finalInc=min(cursor->finalInc,\
01646                         cursor->nextSegment->maxInc*sq->feedOverrideFactor);
01647 
01648               else
01649                 finalInc=min(cursor->finalInc,\
01650                         cursor->maxInc*sq->feedOverrideFactor);
01651 
01652               amaxTan= min(amaxTan, cursor->amaxTan);
01653               minNumSteps = ceil ( 3 * fabs(finalInc-as->plInitInc) / \
01654                                    ( 2 * amaxTan * sq->ctPow2 ));
01655               if ((minNumSteps+1)* (as->plInitInc+finalInc)/2-as->plInitInc >\
01656                   as->totLength - 5 * max(as->plInitInc,finalInc ))
01657 
01658                 {
01659                   if (-1 ==sqLinkSegments(cursor,cursor->nextSegment,SQ_HIGH_LINKING_PRIORITY))
01660                     {
01661                       diagnostics("Error in sqRunCycle\n");
01662                       return -1;
01663                     }
01664                   cursor=cursor->nextSegment;
01665                   if (cursor == 0 )
01666                     {
01667                       diagnostics("Panic: cursor = NULL in sqRunCycle\n");
01668                       return -1;
01669                     }
01670 
01671                 }
01672               else
01673                 done=1;
01674             }
01675 
01676         }
01677       else if ( sq->stepping == 1)
01678         /* we are at the end of the segment, but shouldn't go on
01679            with the next one */
01680         {
01681           sq->numSegments--;
01682           sq->start = (sq->start + 1 ) % sq->size;
01683           if ( sq->numSegments < sq->size - SQ_SAFETY_MARGIN )
01684             sq->full = 0;
01685           if ( sq->numSegments < 0 )
01686             {
01687               diagnostics("Panic: sq->numSegments < 0  in sqRunCycle\n");
01688               return -1;
01689             }
01690 
01691           as= sq->queue+sq->start;
01692           as->plInitInc=0;
01693           as->active=0;
01694           sq->done=1;
01695         }
01696 
01697       else
01698         {
01699           /* end of segment reached */
01700           sq->numSegments -= 1 + as->numLinkedSegs;
01701           sq->start = ( sq->start + as->numLinkedSegs + 1 ) % sq->size;
01702           if ( sq->numSegments < sq->size - SQ_SAFETY_MARGIN )
01703             sq->full = 0;   /* we just removed some segments ... */
01704           if ( sq->numSegments < 0 )
01705             {
01706               diagnostics("Panic: sq->numSegments <0  in sqRunCycle\n");
01707               return -1;
01708             }
01709 
01710         }
01711     }
01712 
01713   /* for debugging */
01714 
01715   oldVel=newVel;
01716   newVel.tran.x= sq->lastPoint.tran.x - oldPos.tran.x;
01717   newVel.tran.y= sq->lastPoint.tran.y - oldPos.tran.y;
01718   newVel.tran.z= sq->lastPoint.tran.z - oldPos.tran.z;
01719 
01720   oldPos=sq->lastPoint;
01721 
01722   newAcc.tran.x= newVel.tran.x - oldVel.tran.x;
01723   newAcc.tran.y= newVel.tran.y - oldVel.tran.y;
01724   newAcc.tran.z= newVel.tran.z - oldVel.tran.z;
01725 
01726   if (fabs(newAcc.tran.x) > sq->maxAcc*sq->ctPow2 || \
01727       fabs(newAcc.tran.y) > sq->maxAcc*sq->ctPow2 || \
01728       fabs(newAcc.tran.z) > sq->maxAcc*sq->ctPow2 )
01729     {
01730       diagnosticsOff("MaxAcc limited violated on motion %d\n",sq->currentID);
01731       diagnosticsOff("ddx=%d ddy=%d ddz=%d\n",(int)(newAcc.tran.x*1000000.0),(int)(newAcc.tran.y*1000000.0),(int)(newAcc.tran.z*1000000.0 ));
01732     }
01733 
01734   sq->currentVel = sq->dist - oldDist;
01735   oldDist = sq->dist;
01736 
01737   return 0;
01738 }

01074 {
01075   if (sq==0 || secs==0 )
01076     {
01077       diagnostics("Cycletime is zero!!!\n");
01078       return -1;
01079     }
01080   sq->cycleTime=secs;
01081   sq->ctPow2=secs *secs;
01082   sq->ctPow3=sq->ctPow2 * secs;
01083   return 0;
01084 };

01154 {
01155   if (sq==0 || feed<=0)
01156     {
01157       diagnostics("Error in sqSetFeed()\n");
01158       return -1;
01159     }
01160 
01161   if (feed > sq->maxV)
01162     sq->feed = sq->maxV;
01163   else
01164     sq->feed=feed;
01165   return 0;
01166 }

01742 {
01743 
01744   SEGMENT *as, *cursor;   /* as = Active Segment */
01745   double startInc,finalInc,startAcc=0;
01746   double prevInc;
01747   int npow1,npow2,npow3;
01748   double angleToGo;
01749   int turn;
01750   PmCartesian normal, center;
01751 
01752   if ( sq==0 || sq->queue==0 )
01753     {
01754       diagnostics("Error in sqSetFeedOverride\n");
01755       return -1;
01756     }
01757 
01758   /* if fo is out of the valid range: clamp it */
01759   if ( fo < 0 )
01760     fo=0;
01761   else if ( fo > sq->maxFeedOverrideFactor )
01762     fo=sq->maxFeedOverrideFactor;
01763 
01764   if ( sq->feedOverrideFactor==fo )
01765     /* don't do anything, just return */
01766     return 0;
01767 
01768   if ( fo == 0 )
01769     {
01770       /* then this is actually a pause action */
01771       if ( -1 == sqPause(sq) )
01772         {
01773           diagnostics("Error in sqSetFeedOverride\n");
01774           return -1;
01775         }
01776       else
01777         {
01778           sq->feedOverrideFactor=0;
01779           return 0;
01780         }
01781     }
01782 
01783   if ( sq->numSegments==0 )
01784     {
01785       /* the queue is empty */
01786       sq->feedOverrideFactor=fo;
01787       return 0;
01788     }
01789 
01790   as=sq->queue+sq->start;
01791 
01792   if ( sq->paused==1 )
01793     {
01794       if ( sq->feedOverrideFactor == 0 )
01795         {
01796           /* If the previous feed override factor equals zero, then the
01797              pause action was actually done by setting the feed
01798              override to zero */
01799           sq->feedOverrideFactor=fo;
01800           if ( -1 == sqResume(sq) )
01801             {
01802               diagnostics("Error in sqSetFeedOverride\n");
01803               return -1;
01804             }
01805           else
01806             return 0;
01807         }
01808       else
01809         {
01810           /* else: don't do anything else*/
01811           sq->feedOverrideFactor=fo;
01812           return 0;
01813         }
01814     }
01815 
01816   sq->feedOverrideFactor=fo;
01817   if ( as->active==0 )
01818     {
01819       /* the previous segment has just been finished. as still needs to
01820          be planned. So: don't do anything */
01821     }
01822 
01823   else if ( sq->n < as->m + 2 || sq->feedAdjusting==1 )
01824     {
01825       /* we are accelerating to (macInc * 'previous fo'). We need to adjust
01826          this phase in order get to the desired cruising feed */
01827 
01828       if ( sq->feedAdjusting!=1 && as->p==0 && as->maxInc*fo > as->cruiseInc )
01829         /* the the segment is too short to reach the current maxInc and it
01830            will be too short too to reach the new feed */
01831         return 0;
01832 
01833       if (sq->feedAdjusting==1)
01834         {
01835           /* copy the phase 3 params to the phase 1 params, since these will
01836              be used to find estimates for the current velocity and
01837              acceleration */
01838           as->a1=as->a3;
01839           as->b1=as->b3;
01840           as->c1=as->c3;
01841           as->d1=as->d3;
01842         }
01843 
01844       finalInc=as->maxInc*fo;
01845 
01846       /* recalculate the last two dist values to find startInc and to use
01847          for making an estimation of startAcc */
01848       if (sq->n==1 || sq->n==2 || sq->n==3)
01849         {
01850           startInc=as->plInitInc;
01851           startAcc=0;
01852         }
01853       else if ( sq->n==3)
01854         {
01855           npow1 = 1;
01856           npow2 = 1;
01857           npow3 = 1;
01858           startInc = as->a1 * npow3 *sq->ctPow3 \
01859             + as->b1 * npow2 * sq->ctPow2 \
01860             + as->c1 * npow1 * sq->cycleTime \
01861             + as->d1;
01862           startAcc = (startInc - as->plInitInc );
01863         }
01864       else
01865         {
01866           npow1 = sq->n - 3;            /* sq->n-2-1 */
01867           npow2 = npow1 * npow1;
01868           npow3 = npow2 * npow1;
01869           startInc = as->a1 * npow3 *sq->ctPow3 \
01870             + as->b1 * npow2 * sq->ctPow2 \
01871             + as->c1 * npow1 * sq->cycleTime \
01872             + as->d1;
01873           npow1 = sq->n - 4;            /* sq->n-2-2 */
01874           npow2 = npow1 * npow1;
01875           npow3 = npow2 * npow1;
01876           prevInc = (as->a1 * npow3 *sq->ctPow3 \
01877                                  + as->b1 * npow2 * sq->ctPow2 \
01878                                  + as->c1 * npow1 * sq->cycleTime \
01879                                  + as->d1);
01880           startAcc = startInc - prevInc;
01881         }
01882       as->q= ceil ( 1.7393877* fabs(startInc-finalInc) / \
01883                     ( sq->maxAcc * sq->ctPow2 ) +
01884                     0.5967755904 * fabs(startInc-finalInc) / \
01885                     ( sq->maxAcc * sq->maxAcc * sq->ctPow2 * sq->ctPow2) *\
01886                     startAcc-(sq->maxAcc * sq->ctPow2)/2);
01887       as->m=0;
01888       as->p=0;
01889       as->totNumPoints = as->q + 1;
01890       sq->n=3;          /* start all over, but skip the first initInc steps */
01891 
01892       as->b3 = - ( 3 * (startInc-finalInc) + 2 * as->q* startAcc ) \
01893         / ( as->q * as->q * sq->ctPow2 ) ;
01894       as->a3 = ( 2 * (startInc-finalInc) + as->q* startAcc ) \
01895         / ( as->q * as->q * as->q * sq->ctPow3 ) ;
01896       as->c3 = startAcc/sq->cycleTime;
01897       as->d3 = startInc;
01898 
01899       as->plFinalInc=finalInc;
01900       /* when the desired feed is reached, some things off this segment
01901          will have to be recalculated, which is done by runCycle. By setting
01902          this flag, runCycle will know that it needs to do this
01903       */
01904       sq->feedAdjusting=1;
01905 
01906     }
01907   else if ( sq->n < as->m + as->p + 2 )
01908     {
01909 
01910     /* change the active segment such that it looks like we are starting
01911        with a new segment */
01912 
01913       startInc=as->cruiseInc;
01914 
01915       /* delete all previous segments */
01916       cursor=as;
01917       while ( cursor!=sq->cursor )
01918         {
01919           cursor=cursor->nextSegment;
01920           if (cursor == 0 )
01921             {
01922               diagnostics("Panic 1: cursor = NULL in sqSetFeedOverride\n");
01923               return -1;
01924             }
01925           sq->numSegments--;
01926         }
01927       if ( sq->numSegments < sq->size - SQ_SAFETY_MARGIN )
01928         sq->full = 0;
01929       if ( sq->numSegments < 0 )
01930         {
01931           diagnostics("Panic: sq->numSegments <0  in sqSetFeedOverride\n");
01932           return -1;
01933         }
01934 
01935 
01936       cursor->prevSegment=0;
01937       cursor->linkedToPrevSeg=0;
01938       sq->start= sq->cursor - sq->queue;
01939       as=cursor;
01940 
01941       as->planningDone=0;
01942       as->plInitInc=startInc;
01943       as->start=sq->lastPoint;
01944       if ( as->type == SQ_LINEAR)
01945         {
01946           as->length=sqGiveLength(as->start,as->end);
01947           as->totLength=as->length;
01948           pmLineInit(&as->line,as->start,as->end);
01949         }
01950       else
01951         {
01952           angleToGo= as->circle.angle-(sq->dist-sq->offset)/as->helixRadius;
01953           turn = floor(angleToGo / ( 2 * PM_PI ));
01954           normal = as->circle.normal;
01955           center = as->circle.center;
01956           pmCircleInit( &as->circle, as->start, as->end, center, normal, turn);
01957           as->length=as->circle.angle* as->helixRadius;
01958           as->totLength= as->length;
01959         }
01960 
01961 
01962       as->active=0;     /* mark the first segment of the chain
01963                            as not active */
01964 
01965       /* determine how many segments are linked to as */
01966       as->numLinkedSegs=0;
01967       while (cursor->nextSegment!=0 &&
01968              cursor->nextSegment->linkedToPrevSeg==1 )
01969         {
01970           cursor=cursor->nextSegment;
01971           as->numLinkedSegs++;
01972           as->totLength+=cursor->length;
01973         }
01974 
01975       /* keep linking successive segments to as until the chain is
01976          long enough */
01977       if ( -1 == sqForwardLinkSegment(sq,as,sq->feedOverrideFactor))
01978         {
01979           diagnostics("Error in sqSetFeedOverride\n");
01980           return -1;
01981         }
01982     }
01983   else
01984     {
01985       /* we are already decelerating, which is in most cases
01986          necessary.  Therefore, let's not interrupt this and finish
01987          the current segment first. The new segment will be planned
01988          with the new feedOverrideFactor.*/
01989     }
01990 
01991   return 0;
01992 }

01052 {
01053   if (sq==0 || amax <=0 )
01054     {
01055       diagnostics("Error in SetMaxAcc!!!\n");
01056       return -1;
01057     }
01058   sq->maxAcc=amax/2;
01059   return 0;
01060 };

01087 {
01088   if (sq==0 || mfo<=0 )
01089     {
01090       diagnostics("Error in sqSetMaxFeedOverride()\n");
01091       return -1;
01092     }
01093   sq->maxFeedOverrideFactor = mfo;
01094   return 0;
01095 }

01099 {
01100   if (sq==0)
01101     {
01102       diagnostics("Error in sqSetPos()\n");
01103       return -1;
01104     }
01105   sq->initXYZ=pos;
01106   sq->lastPoint=sq->initXYZ;
01107   return 0;
01108 }

01063 {
01064   if (sq==0 || vmax <= 0 )
01065     {
01066       diagnostics("Error in SetVmax!!!\n");
01067       return -1;
01068     }
01069   sq->maxV=vmax;
01070   return 0;
01071 };

02247 {
02248   SEGMENT *as;                  /* as = Active Segment */
02249   if (sq==0)
02250     {
02251       diagnostics("Error in sqStep\n");
02252       return -1;
02253     }
02254 
02255   if (sq->done!=1 || sq->numSegments==0 )
02256     /* step should only be used when system is paused and waiting... */
02257     {
02258       diagnostics("Stepping can only be done when system is paused and waiting\n");
02259       /* not a critical error, let's ignore it */
02260       return 0;;
02261     }
02262 
02263   if ( sq->feedOverrideFactor == 0 )
02264     {
02265       /* we can't step if the feed override factor is zero. To step
02266          the user should set a new non-zero value for this factor first.
02267          This will immediately result in a resume action. */
02268       diagnostics("Can't resume if feed override is zero\n");
02269       /* not a critical error, so ignore it */
02270       return 0;
02271     }
02272 
02273 
02274   /* make finalInc of the current segment 0, set sq->stepping and resume.
02275      This means that after this is segment is done, it will
02276      wait for the next step or resume command */
02277 
02278   as=sq->queue+sq->start;
02279   as->finalInc=0;
02280 
02281   /* if the next segment is linked to the current one, unlink it */
02282   if (as->nextSegment!=0 && as->nextSegment->linkedToPrevSeg==1)
02283     {
02284       as->nextSegment->linkedToPrevSeg=0;
02285       as->nextSegment->numLinkedSegs=as->numLinkedSegs-1;
02286       as->nextSegment->totLength=as->totLength - as->length;
02287       as->numLinkedSegs=0;
02288       as->totLength=as->length;
02289     }
02290   sq->done=0;
02291   sq->stepping=1;
02292   sq->paused=0;
02293   return 0;
02294 }

01141 {
01142   if ( sq == 0 || sq->queue == 0 )
01143     {
01144       diagnostics("Error in sqTrashQueue()\n");
01145       return -1;
01146     }
01147   sqClearQueue(sq);
01148   sq->queue=0;
01149   return 0;
01150 };

Data Structures
struct	_seg
struct	SEGMENTQUEUE
Defines
#define	max(A, B) ( (A) > (B) ? (A) : (B))
#define	min(A, B) ( (A) < (B) ? (A) : (B))
#define	PI 3.141592654
#define	SQ_LINEAR 1
#define	SQ_CIRCULAR 2
Typedefs
typedef _seg	SEGMENT
Functions
int	sqInitQueue (SEGMENTQUEUE sq, SEGMENT first, int size)
int	sqSetMaxAcc (SEGMENTQUEUE *sq, double amax)
int	sqSetVmax (SEGMENTQUEUE *sq, double vmax)
int	sqSetCycleTime (SEGMENTQUEUE *sq, double secs)
int	sqSetMaxFeedOverride (SEGMENTQUEUE *sq, double mfo)
int	sqSetPos (SEGMENTQUEUE *sq, EmcPose pos)
int	sqTrashQueue (SEGMENTQUEUE *sq)
int	sqClearQueue (SEGMENTQUEUE *sq)
int	sqAddLine (SEGMENTQUEUE *sq, EmcPose end, int ID)
int	sqAddCircle (SEGMENTQUEUE *sq, EmcPose end, PmCartesian center, PmCartesian normal, int turn, int ID)
int	sqSetFeed (SEGMENTQUEUE *sq, double feed)
int	sqSetFeedOverride (SEGMENTQUEUE *sq, double factor)
int	sqRunCycle (SEGMENTQUEUE *sq)
int	sqGetPosition (SEGMENTQUEUE sq, EmcPose p)
int	sqPause (SEGMENTQUEUE *sq)
int	sqResume (SEGMENTQUEUE *sq)
int	sqAbort (SEGMENTQUEUE *sq)
int	sqStep (SEGMENTQUEUE *sq)
int	sqIsDone (SEGMENTQUEUE *sq)
int	sqIsPaused (SEGMENTQUEUE *sq)
int	sqIsStepping (SEGMENTQUEUE *sq)
double	sqGetVel (SEGMENTQUEUE *sq)
double	sqGetMaxAcc (SEGMENTQUEUE *sq)
int	sqGetDepth (SEGMENTQUEUE *sq)
int	sqIsQueueFull (SEGMENTQUEUE *sq)
int	sqGetID (SEGMENTQUEUE *sq)

segmentqueue.h File Reference

Data Structures

Defines

Typedefs

Functions

Define Documentation

Typedef Documentation

Function Documentation