home *** CD-ROM | disk | FTP | other *** search
/ Programmer 7500 / MAX_PROGRAMMERS.iso / INFO / C / ROBOT01.ZIP / RCCL042 < prev    next >
Encoding:
Text File  |  1987-03-02  |  2.6 KB  |  122 lines
  1. /*
  2.  * RCCL Version 1.0           Author :  Vincent Hayward
  3.  *                                      School of Electrical Engineering
  4.  *                                      Purdue University
  5.  *      Dir     : src
  6.  *      File    : drive.c
  7.  *      Remarks : Straight line trajectory drive function.
  8.  *                Exact implementation of Paul's book.
  9.  *      Usage   : part of the lib
  10.  */
  11.  
  12. #include "../h/rccl.h"
  13. #include "../h/manip.h"
  14. #include "../h/umac.h"
  15.  
  16. /*
  17.  * set drive parameters of a motion defined by the p1 p2 transforms
  18.  */
  19.  
  20. setpar_n(dpt, p1, p2) /*::*/
  21. register DRVP_PTR dpt;
  22. register TRSF_PTR p1, p2;
  23. {
  24.     real dot();
  25.     VECT diff;             /* local vector for storing p2p - p1p */
  26.     real n1, n2, n3;   /* local variables for common subexpr */
  27.     real x, y;
  28.     real lthe, lpsi;
  29.     real spsi, cpsi, vthe, cthe, sthe;
  30.  
  31.     /* x y z */
  32.  
  33.     diff.x = p2->p.x - p1->p.x;
  34.     diff.y = p2->p.y - p1->p.y;
  35.     diff.z = p2->p.z - p1->p.z;
  36.  
  37.     dpt->dx = dot(&p1->n, &diff);
  38.     dpt->dy = dot(&p1->o, &diff);
  39.     dpt->dz = dot(&p1->a, &diff);
  40.  
  41.     /* psi */
  42.  
  43.     n1 = dot(&p1->o, &p2->a);
  44.     n2 = dot(&p1->n, &p2->a);
  45.  
  46.     if (FABS(n1) < SMALL && FABS(n2) < SMALL) {
  47.         lpsi = dpt->dpsi = 0.;
  48.     }
  49.     else {
  50.         lpsi = dpt->dpsi = atan2(n1, n2);
  51.     }
  52.  
  53.     /* theta */
  54.  
  55.     y = sqrt(n2 * n2 + n1 * n1);
  56.     x = dot(&p1->a, &p2->a);
  57.  
  58.     if (y < SMALL && FABS(x) < SMALL) {
  59.         lthe = dpt->dthe = 0.;
  60.     }
  61.     else {
  62.         lthe = dpt->dthe = atan2(y, x);
  63.     }
  64.  
  65.     /* phi */
  66.  
  67.     SINCOS(spsi, cpsi, lpsi);
  68.     SINCOS(sthe, cthe, lthe);
  69.     vthe = 1. - cthe;
  70.  
  71.     n1 = - spsi * cpsi * vthe;
  72.     n2 =   cpsi * cpsi * vthe + cthe;
  73.     n3 = - spsi * sthe;
  74.  
  75.     y = n1 * dot(&p1->n, &p2->n) +
  76.         n2 * dot(&p1->o, &p2->n) +
  77.         n3 * dot(&p1->a, &p2->n);
  78.  
  79.     x = n1 * dot(&p1->n, &p2->o) +
  80.         n2 * dot(&p1->o, &p2->o) +
  81.         n3 * dot(&p1->a, &p2->o);
  82.  
  83.     if (FABS(y) < SMALL && FABS(x) < SMALL) {
  84.         dpt->dphi = 0.;
  85.     }
  86.     else {
  87.         dpt->dphi = atan2(y, x);
  88.     }
  89. }
  90.  
  91.  
  92. /*
  93.  * drive function : computes the drive transform given the drive parameters
  94.  */
  95.  
  96. drivefn_n(t, d)  /*::*/
  97. register TRSF_PTR t;
  98. register DRVP_PTR d;
  99. {
  100.     real sphi, cphi, spsi, cpsi, sthe, cthe, vthe, loc;
  101.  
  102.     t->p.x = d->dx;
  103.     t->p.y = d->dy;
  104.     t->p.z = d->dz;
  105.  
  106.     SINCOS(sphi, cphi, d->dphi);
  107.     SINCOS(spsi, cpsi, d->dpsi);
  108.     SINCOS(sthe, cthe, d->dthe);
  109.     vthe = 1. - cthe;
  110.  
  111.     t->a.x = cpsi * sthe;
  112.     t->a.y = spsi * sthe;
  113.     t->a.z = cthe;
  114.  
  115.     t->o.x = - sphi * (spsi * spsi * vthe + cthe)
  116.          + cphi * (loc = - spsi * cpsi * vthe);
  117.     t->o.y = - sphi * (loc) + cphi * (cpsi * cpsi * vthe + cthe);
  118.     t->o.z = sphi * cpsi * sthe - cphi * spsi * sthe;
  119.  
  120.     Cross(&t->n, &t->o, &t->a);
  121. }
  122.