Main Page   Class Hierarchy   Alphabetical List   Data Structures   File List   Data Fields   Globals  

cubic.h File Reference

This graph shows which files directly or indirectly include this file:

Included by dependency graph

Go to the source code of this file.

Data Structures

struct  CUBIC_STRUCT

Defines

#define __attribute__(x)

Functions

char __attribute__ ((unused)) cubic_h[]="$Id
int cubicInit (CUBIC_STRUCT *ci)
int cubicSetSegmentTime (CUBIC_STRUCT *ci, double time)
double cubicGetSegmentTime (CUBIC_STRUCT *ci)
int cubicSetInterpolationRate (CUBIC_STRUCT *ci, int rate)
int cubicGetInterpolationRate (CUBIC_STRUCT *ci)
int cubicAddPoint (CUBIC_STRUCT *ci, double point)
int cubicOffset (CUBIC_STRUCT *ci, double offset)
double cubicGetInterpolationIncrement (CUBIC_STRUCT *ci)
CUBIC_COEFF cubicGetCubicCoeff (CUBIC_STRUCT *ci)
int cubicFilled (CUBIC_STRUCT *ci)
double cubicInterpolate (CUBIC_STRUCT *ci, double *x, double *v, double *a, double *j)
int cubicNeedNextPoint (CUBIC_STRUCT *ci)
int cubicDrain (CUBIC_STRUCT *ci)

Variables

 CUBIC_COEFF

Define Documentation

#define __attribute__ (  x    ) 
 

Definition at line 28 of file cubic.h.

Function Documentation

char __attribute__ (  (unused)    )  [static]
 

Definition at line 32 of file cubic.h.

00032                                                     : cubic.h,v 1.2 2000/10/27 20:34:42 terrylr Exp $";
00033 
00034 typedef struct
00035 {
00036   double a;
00037   double b;
00038   double c;
00039   double d;
00040 } CUBIC_COEFF;

int cubicAddPoint (  CUBIC_STRUCT *    ci,
double    point
) 
 

Definition at line 251 of file cubic.c.

Referenced by cubicInterpolate(), and emcmotController(). 

00252 {
00253   if (0 == ci ||
00254       ! (ci->configured == ALL_SET))
00255     {
00256       return -1;
00257     }
00258 
00259   if (! ci->needNextPoint)
00260     {
00261       return -1;
00262     }
00263 
00264   if (! ci->filled)
00265     {
00266       ci->x0 = point;
00267       ci->x1 = point;
00268       ci->x2 = point;
00269       ci->x3 = point;
00270       ci->filled = 1;
00271     }
00272   else
00273     {
00274       ci->x0 = ci->x1;
00275       ci->x1 = ci->x2;
00276       ci->x2 = ci->x3;
00277       ci->x3 = point;
00278     }
00279 
00280   /* calculate way points and coeff */
00281   ci->wp0 = wayPoint(ci->x0, ci->x1, ci->x2);
00282   ci->wp1 = wayPoint(ci->x1, ci->x2, ci->x3);
00283   ci->velp0 = velPoint(ci->x0, ci->x2, ci->segmentTime);
00284   ci->velp1 = velPoint(ci->x1, ci->x3, ci->segmentTime);
00285   ci->coeff = cubicCoeff(ci->wp0, ci->velp0, ci->wp1,
00286                          ci->velp1, ci->segmentTime);
00287   ci->interpolationTime = 0.0;
00288   ci->needNextPoint = 0;
00289 
00290   return 0;
00291 }

int cubicDrain (  CUBIC_STRUCT *    ci ) 
 

Definition at line 394 of file cubic.c.

Referenced by cubicInit(), and emcmotController(). 

00395 {
00396   ci->x0 = ci->x1 = ci->x2 = ci->x3 = 0.0;
00397   ci->wp0 = ci->wp1 = 0.0;
00398   ci->velp0 = ci->velp1 = 0.0;
00399   ci->filled = 0;
00400   ci->needNextPoint = 1;
00401   ci->coeff.a = 0.0;
00402   ci->coeff.b = 0.0;
00403   ci->coeff.c = 0.0;
00404   ci->coeff.d = 0.0;
00405 
00406   return 0;
00407 }

int cubicFilled (  CUBIC_STRUCT *    ci ) 
 

Definition at line 326 of file cubic.c.

00327 {
00328   if (0 == ci)
00329     {
00330       return 0;
00331     }
00332 
00333   return ci->filled;
00334 }

CUBIC_COEFF cubicGetCubicCoeff (  CUBIC_STRUCT *    ci ) 
 

Definition at line 219 of file cubic.c.

00220 {
00221   CUBIC_COEFF errorReturn;
00222 
00223   if (0 == ci ||
00224       ! ci->filled)
00225     {
00226       errorReturn.a = 0.0;
00227       errorReturn.b = 0.0;
00228       errorReturn.c = 0.0;
00229       errorReturn.d = 0.0;
00230 
00231       return errorReturn;
00232     }
00233 
00234   return ci->coeff;
00235 }

double cubicGetInterpolationIncrement (  CUBIC_STRUCT *    ci ) 
 

Definition at line 208 of file cubic.c.

00209 {
00210   if (0 == ci ||
00211       ci->configured != ALL_SET)
00212     {
00213       return 0;
00214     }
00215 
00216   return ci->interpolationIncrement;
00217 }

int cubicGetInterpolationRate (  CUBIC_STRUCT *    ci ) 
 

Definition at line 197 of file cubic.c.

00198 {
00199   if (0 == ci ||
00200       ! (ci->configured & INTERPOLATION_RATE_SET))
00201     {
00202       return 0;
00203     }
00204 
00205   return ci->interpolationRate;
00206 }

double cubicGetSegmentTime (  CUBIC_STRUCT *    ci ) 
 

Definition at line 168 of file cubic.c.

00169 {
00170   if (0 == ci ||
00171       ! (ci->configured & SEGMENT_TIME_SET))
00172     {
00173       return 0.0;
00174     }
00175 
00176   return ci->segmentTime;
00177 }

int cubicInit (  CUBIC_STRUCT *    ci ) 
 

Definition at line 134 of file cubic.c.

Referenced by init_module(). 

00135 {
00136   if (0 == ci)
00137     {
00138       return -1;
00139     }
00140 
00141   ci->configured = 0;
00142   ci->segmentTime = 0.0;
00143   ci->interpolationRate = 0;
00144   ci->interpolationIncrement = 0.0;
00145   cubicDrain(ci);
00146 
00147   return 0;
00148 }

double cubicInterpolate (  CUBIC_STRUCT *    ci,
double *    x,
double *    v,
double *    a,
double *    j
) 
 

Definition at line 336 of file cubic.c.

Referenced by emcmotController(). 

00341 {
00342   double retval;
00343 
00344   if (0 == ci ||
00345       ! (ci->configured == ALL_SET))
00346     {
00347       return 0.0;
00348     }
00349 
00350   if (ci->needNextPoint)
00351     {
00352       /* queue ran out-- fill right with last point */
00353       cubicAddPoint(ci, ci->x3);
00354     }
00355 
00356   retval = interpolateCubic(ci->coeff, ci->interpolationTime);
00357 
00358   /* do optional ones */
00359   if (x != 0)
00360     {
00361       *x = retval;
00362     }
00363   if (v != 0)
00364     {
00365       *v = interpolateVel(ci->coeff, ci->interpolationTime);
00366     }
00367   if (a != 0)
00368     {
00369       *a = interpolateAccel(ci->coeff, ci->interpolationTime);
00370     }
00371   if (j != 0)
00372     {
00373       *j = interpolateJerk(ci->coeff, ci->interpolationTime);
00374     }
00375 
00376   ci->interpolationTime += ci->interpolationIncrement;
00377 
00378   /* check to see if the next point is at (close to) the segment end */
00379   if (fabs(ci->segmentTime - ci->interpolationTime)
00380       < 0.5 * ci->interpolationIncrement)
00381     {
00382       /* just computed last point-- flag that we need a new one */
00383       ci->needNextPoint = 1;
00384     }
00385 
00386   return retval;
00387 }

int cubicNeedNextPoint (  CUBIC_STRUCT *    ci ) 
 

Definition at line 389 of file cubic.c.

Referenced by emcmotController(). 

00390 {
00391   return ci->needNextPoint;
00392 }

int cubicOffset (  CUBIC_STRUCT *    ci,
double    offset
) 
 

Definition at line 302 of file cubic.c.

Referenced by emcmotController(). 

00303 {
00304   if (0 == ci ||
00305       ! (ci->configured == ALL_SET))
00306     {
00307       return -1;
00308     }
00309 
00310   ci->x0 += offset;
00311   ci->x1 += offset;
00312   ci->x2 += offset;
00313   ci->x3 += offset;
00314   ci->wp0 += offset;
00315   ci->wp1 += offset;
00316 
00317   /* leave velp0, velp1 alone, since these are velocity points and
00318      are unaffected by position offsets */
00319 
00320   /* only the D coeff is affected, so we can change this directly */
00321   ci->coeff.d += offset;
00322 
00323   return 0;
00324 }

int cubicSetInterpolationRate (  CUBIC_STRUCT *    ci,
int    rate
) 
 

Definition at line 179 of file cubic.c.

Referenced by setServoCycleTime(), and setTrajCycleTime(). 

00180 {
00181   if (0 == ci ||
00182       rate <= 0)
00183     {
00184       return -1;
00185     }
00186 
00187   ci->interpolationRate = rate;
00188   ci->configured |= INTERPOLATION_RATE_SET;
00189   if (ci->configured == ALL_SET)
00190     {
00191       ci->interpolationIncrement = ci->segmentTime / ci->interpolationRate;
00192     }
00193 
00194   return 0;
00195 }

int cubicSetSegmentTime (  CUBIC_STRUCT *    ci,
double    time
) 
 

Definition at line 150 of file cubic.c.

Referenced by setServoCycleTime(). 

00151 {
00152   if (0 == ci ||
00153       time <= 0.0)
00154     {
00155       return -1;
00156     }
00157 
00158   ci->segmentTime = time;
00159   ci->configured |= SEGMENT_TIME_SET;
00160   if (ci->configured == ALL_SET)
00161     {
00162       ci->interpolationIncrement = ci->segmentTime / ci->interpolationRate;
00163     }
00164 
00165   return 0;
00166 }

Variable Documentation

CUBIC_COEFF
 

Definition at line 40 of file cubic.h.

Generated on Sun Dec 2 15:27:50 2001 for EMC by doxygen1.2.11.1 written by Dimitri van Heesch, © 1997-2001