---

motion.c

Go to the documentation of this file.

#include "motion.h"
#include "bppio.h"
#include "bppimpl.h"


#ifdef DEBUG
const static int debuglevel =
#ifdef DEBUGLEVEL
DEBUGLEVEL;
#else
1;
#endif
#define DBUGMESS(N, PRINTARGS) if ((N) >= debuglevel) fprintf PRINTARGS
#else
#define DBUGMESS(N, PRINTARGS) /* sfa */
#define DBUGPERR(ERR, MESS) if (ERR) perror(MESS)
#endif

static const char* xlabel = "X";
static const char* ylabel = "Y";

static const int deltamag = 1;       /* nominal step size magnitude */
static const velocity_t basespeed = 50.0;
static const velocity_t vslow = 1.0 / 128;  /* speeds < vslow steps/s set to 0 */
static const velocity_t minturn = 5.0;   /* minimum speed when changing direction */
static const velocity_t mincoast = 0.5;  /* minimum speed when coasting */

static void* track (void* arg);
static int xstep(int fd, int incr);
static int ystep(int fd, int incr);
static int nextwake (const velocity_t vs, const velocity_t minspeed, struct timespec* thewake);
static double findattract (const double qn, const velocity_t pn,
                           const double qg, const velocity_t pg);
static velocity_t nextvel (const double qn, const velocity_t pn,
                           const double qg, const velocity_t pg,
                           const double qc, coord_t* delta);




void showreg (int fd)
{
   struct bpp_reg reg_dump;
   int err;

   err = ioctl(fd, BPPIOC_GETREGS, &reg_dump);
   fprintf(stderr, "Status code (register dump) %d.\n", err);
   fprintf(stderr, "dma_csr     = %x\n", reg_dump.dma_csr);
   fprintf(stderr, "dma_addr    = %x\n", reg_dump.dma_addr);
   fprintf(stderr, "dma_bcnt    = %d\n", reg_dump.dma_bcnt);
   fprintf(stderr, "dma_tst_csr = %x\n", reg_dump.dma_tst_csr);
   fprintf(stderr, "hw_config   = %x\n", reg_dump.hw_config);
   fprintf(stderr, "op_config   = %x\n", reg_dump.op_config);
   fprintf(stderr, "data        = %x\n", reg_dump.data);
   fprintf(stderr, "trans_cntl  = %x\n", reg_dump.trans_cntl);
   fprintf(stderr, "out_pins    = %x\n", reg_dump.out_pins);
   fprintf(stderr, "in_pins     = %x\n", reg_dump.in_pins);
   fprintf(stderr, "int_cntl    = %x\n", reg_dump.int_cntl);
}
   
int openstage (ptraject stage, const char* path)
{
   int err = 0;

   stage->filedescr = open(path, O_WRONLY);
   if (stage->filedescr < 0) {
      err = stage->filedescr;
      perror("Couldn't open");
   } else {
      int ax  = stage->naxes;
      ptranslation trans = stage->axis;
      while (!err && ax-- && (trans != NULL)) {
         err = start(stage->filedescr, trans, &stage->tattr);
         trans++;
      }
      if (!err && !ax) {
         err = -1;
         DBUGMESS(1, (stderr, "openstage: undefined axis\n"));
      }
   }
   return err;
}

int closestage (ptraject stage)
{
   int ax  = stage->naxes;
   ptranslation trans = stage->axis;
   int err = 0;

   while (!err && ax-- && (trans != NULL)) {
      err = stop(trans);
      trans++;
   }
   if (!err && !ax) {
      err = -1;
      DBUGMESS(1, (stderr, "closestage: undefined axis\n"));
   }
   if (!err) {
      err = close(stage->filedescr);
      if (err)
         perror("Closestage couldn't close");
   }
   return err;
}

void disposestage (ptraject stage)
{
   free(stage->axis);
   pthread_attr_destroy(&stage->tattr);
   free(stage);
}

int movestage (ptraject stage, pmotion motn)
{
   int err = 0;
   int ax  = stage->naxes;

   ptranslation trans = stage->axis;
   while (!err && ax-- && (trans != NULL) && (motn != NULL)) {
      DBUGMESS(1, (stderr, "Movestage: starting %s axis\n", trans->label));
      err = moveto(trans, motn);
      trans++;
      motn++;
   }
   if (err) DBUGMESS(1, (stderr, "Movestage: error on axis %d, code %d\n", ax, err));
   if (!err && !ax) {
      err = -1;
      if (trans == NULL)
         DBUGMESS(1, (stderr, "movestage: undefined axis\n"));
      if (motn == NULL)
         DBUGMESS(1, (stderr, "movestage: undefined motion\n"));
   }
   return err;
}

int findstage (ptraject stage, pmotion motn)
{
   int err = 0;
   int ax  = stage->naxes;

   ptranslation trans = stage->axis;
   while (!err && ax-- && (trans != NULL) && (motn != NULL)) {
      DBUGMESS(1, (stderr, "Findstage: looking at %s axis\n", trans->label));
      err = where(trans, motn);
      trans++;
      motn++;
   }
   if (err) DBUGMESS(1, (stderr, "Findstage: error on axis %d, code %d\n", ax, err));
   if (!err && !ax) {
      err = -1;
      if (trans == NULL)
         DBUGMESS(1, (stderr, "Findstage: undefined axis\n"));
      if (motn == NULL)
         DBUGMESS(1, (stderr, "Findstage: undefined motion\n"));
   }
   return err;
}

ptraject newxystage (void)
{
   ptraject stage;

   stage = (ptraject)malloc(sizeof(struct Traject));
   if (stage != NULL) {
     int err = pthread_attr_init(&stage->tattr);
     if (!err) {
        if (err = pthread_attr_setdetachstate(&stage->tattr, PTHREAD_CREATE_DETACHED))
           pthread_attr_destroy(&stage->tattr);
     }
     if (err) {
        free(stage);
        stage == NULL;
     }
   }
   if (stage != NULL) {
      stage->naxes = 2;
      stage->axis = (ptranslation)calloc(2, sizeof(struct Translation));
      if (stage->axis == NULL) {
         free(stage);
         stage = NULL;
      } else {
         stage->axis[0].label   = xlabel;
         stage->axis[0].step    = xstep;
         stage->axis[0].pos     = 0;
         stage->axis[0].goalpos = 0;
         stage->axis[0].backlash= 0;
         stage->axis[0].maxlash = 13;
         stage->axis[0].vel     = 0;
         stage->axis[0].goalvel = 0;
         stage->axis[1].label   = ylabel;
         stage->axis[1].step    = ystep;
         stage->axis[1].pos     = 0;
         stage->axis[1].goalpos = 0;
         stage->axis[1].backlash= 0;
         stage->axis[1].maxlash = 2;
         stage->axis[1].vel     = 0;
         stage->axis[1].goalvel = 0;
      }
   }
   return (stage);
}

static double findattract (const double qn, const velocity_t pn,
                           const double qg, const velocity_t pg)
{
   double qc;

   if (fabs(qn - qg) < deltamag/2.0) {
      qc = (fabs(pn - pg) <= fabs(pg)) ? qn : qn + pg;
   } else if ((fabs((pg + pn)/2) < deltamag/100.0) || (fabs(qn - qg) < deltamag/2.0)) {   /* colinear cases */
      qc = (qn + qg)/2;
   } else {
      qc = (pow(pn,2) - pow(pg,2) + pow(qn,2) - pow(qg,2)) / (2*(qn - qg));
   }
   return (qc);
}

static velocity_t nextvel (const double qn, const velocity_t pn,
                       const double qg, const velocity_t pg,
                       const double qc, coord_t* delta)
{
   double r, r2;
   velocity_t v;

   if ((fabs(qn - qg) < deltamag/2.0) &&
       ((fabs(pn - pg) < vslow) || (fabs(pn - pg) < (fabs(pn) + fabs(pg))))){
      DBUGMESS(1, (stderr, "*"));
      *delta = 0;
      v = pg;
      DBUGMESS(1, (stderr, "--->\n"));
      return (v);
   }
   r2 = pow(qc - qg, 2) + pow(pg, 2);
   r  = sqrt(r2);
   if ((pn > 0) && (qn > (qc + r - (1 - 1E-4L)*deltamag))) {
      *delta = 0;
      v = -fabs(pn);
   } else if ((pn < 0) && (qn < (qc - r + (1 - 1E-4L)*deltamag))) {
      *delta = 0;
      v = fabs(pn);
   } else {
      *delta = ((pn < 0) || (pn == 0 && (qn > qc))) ? -deltamag : deltamag;
      v = sqrt(fabs(r2 - pow((qc - qn - *delta), 2)));
      if (*delta < 0)
         v = -v;
   }
   DBUGMESS(1, (stderr, "r=%10.4f qc=%10.4f qn=%10.4f qg=%10.4f pn=%10.4f pg=%10.4f v=%10.4f\n",
                   r, qc, qn, qg, pn, pg, v));
   return v;
}

static int nextwake (const velocity_t vs, const velocity_t minspeed, struct timespec* thewake)
{
   long  tick;
   time_t sec;
   int err = 0;
   velocity_t v = fabs(vs);

   if (v < fabs(minspeed))
      v = fabs(minspeed);
   sec = (time_t)floor(1.0 / v);
   tick = (long)((double)NANOSEC * (1.0/v - sec));
   err = clock_gettime(CLOCK_REALTIME, thewake);
   if (err) {
      perror("Couldn't get time");
   } else {
      thewake->tv_sec += sec;
      if ((thewake->tv_nsec += tick) >= NANOSEC) {
         thewake->tv_nsec -= NANOSEC;
         thewake->tv_sec++;
      }
   }
   return (err);
}

static int xstep(int fd, int incr)
{  
   struct bpp_delta move;
   
   move.x = 0;
   move.y = incr;
   return ioctl(fd, BPPIOC_STEP, &move);
}

static int ystep(int fd, int incr)
{
   struct bpp_delta move;
   
   move.x = incr;
   move.y = 0;
   return ioctl(fd, BPPIOC_STEP, &move);
}

int moveto (ptranslation state, pmotion goal)
{
   int err = 0;

   err = pthread_mutex_lock(&state->modlock);
   if (err) {
      DBUGMESS(1, (stderr, "moveto (%s axis): couldn't acquire lock, error %d\n",
                      state->label, err));
      return (err);
   }
   DBUGMESS(1, (stderr, "moveto (%s axis): going to %d, target speed %10.4f waiting....\n",
                   state->label, goal->pos, goal->vel));
   while (!err && ((state->pos != state->goalpos) ||
                   (fabs(state->vel - state->goalvel) >= vslow))) {
      err = pthread_cond_wait(&state->ontrack, &state->modlock);
   }
   if (err) {
      DBUGMESS(1, (stderr, "moveto (waiting on cv): invalid value\n"));
   } else {
      if (goal->relative) {
         state->goalpos = state->pos + goal->pos;
         state->goalvel = state->vel + goal->vel;
      } else {
         state->goalpos = goal->pos;
         state->goalvel = goal->vel;
      }
      state->scale = sqrt(120*(1 - (1/(1 + pow(state->vel - state->goalvel, 2) +
                                           pow(state->pos - state->goalpos, 2)))));
      if (state->scale < 0.25) state->scale = 0.25;
      state->attract = findattract(state->pos,     state->vel / state->scale, 
                                   state->goalpos, state->goalvel / state->scale);
      pthread_cond_signal(&state->offtrack);
      if (err)
         DBUGMESS(1, (stderr, "moveto (cv signal): invalid value\n"));
   }
   DBUGMESS(1, (stderr, "moveto (%s axis): going to %d, initial speed %10.4f\n",
                   state->label, state->goalpos, state->vel));
   pthread_mutex_unlock(&state->modlock);
   return (err);
}

int where (ptranslation state, pmotion here)
{
   int err = 0;

   err = pthread_mutex_lock(&state->modlock);
   if (err) {
      DBUGMESS(1, (stderr, "where (%s axis): couldn't acquire lock, error %d\n",
                      state->label, err));
      return (err);
   }
   while (!err && (state->pos != state->goalpos)
               && (fabs(state->vel - state->goalvel) < vslow)) {
      err = pthread_cond_wait(&state->ontrack, &state->modlock);
   }
   if (err) {
      DBUGMESS(1, (stderr, "where: invalid value\n"));
   } else {
      here->pos = state->pos;
      here->vel = state->vel;
   }
   pthread_mutex_unlock(&state->modlock);
   return (err);
}

static void* track (void* arg)
{
   ptranslation state = (ptranslation)arg;
   struct timespec wakeup;
   int delta;
   int err = 0;

   err = pthread_mutex_lock(&state->modlock);
   if (err)
      DBUGMESS(1, (stderr, "track (%s axis): couldn't acquire lock, error %d\n", state->label, err));
   while (!err) {
      /* Simple case: at rest . . . . */
      while (!err && (state->pos == state->goalpos)
                  && (fabs(state->vel) < vslow) && (fabs(state->goalvel) < vslow)) {
         DBUGMESS(1, (stderr, "%s resting\n", state->label));
         err = pthread_cond_wait(&state->offtrack, &state->modlock);
      }
      /* In motion . . . . */
      delta = ((state->vel > 0) ||
               ((state->vel == 0) && (state->pos < state->goalpos))) ? deltamag : -deltamag;
      err = nextwake(state->vel, mincoast, &wakeup);
      /* Coasting . . . . */
      while (!err && (state->pos == state->goalpos)
                  && (fabs(state->vel - state->goalvel) < vslow)) {
         DBUGMESS(1, (stderr, "%s coasting pos = %d vel = %10.4f\n",
                         state->label, state->pos, state->vel));
         err = pthread_cond_timedwait(&state->offtrack, &state->modlock, &wakeup);
         if (err == ETIMEDOUT) {
            err = (*(state->step))(state->device, delta);
            if (!err) {
               err = nextwake(state->vel, mincoast, &wakeup);
               if (abs(state->backlash + delta) < state->maxlash) {
                  state->backlash += delta;
               } else {
                  state->pos += delta;
                  state->goalpos += delta;
               }
            }
         }
      }
      /* Moving to goal . . . . */
      delta = ((state->vel > 0) ||
               ((state->vel == 0) && (state->pos < state->goalpos))) ? deltamag : -deltamag;
      err = nextwake(state->vel, minturn, &wakeup);
      while (!err && ((state->pos != state->goalpos) ||
                      (fabs(state->vel - state->goalvel) >= vslow))) {
         DBUGMESS(1, (stderr, ">>> %s moving pos = %d vel = %10.4f\n",
                         state->label, state->pos, state->vel));
         err = pthread_cond_timedwait(&state->ontrack, &state->modlock, &wakeup);
         if (err == ETIMEDOUT) {
            if (delta) {
               err = (*(state->step))(state->device, delta);
               if (err)
                  perror("driving motor");
               if (abs(state->backlash + delta) < state->maxlash) {
                  state->backlash += delta;
               } else {
                  state->pos += delta;
               }
            } else {
               err = 0;
            }
            if (!err) {
               state->vel = nextvel(state->pos,     state->vel / state->scale, 
                                    state->goalpos, state->goalvel / state->scale,
                                    state->attract, &delta) * state->scale;
               err = nextwake(state->vel, minturn, &wakeup);
            }
         }
      }
      if (!err && (state->pos == state->goalpos)) {
         DBUGMESS(1, (stderr, "*** %s reached goal pos = %d vel = %10.4f\n",
                         state->label, state->pos, state->vel));
         
         pthread_cond_signal(&state->ontrack);
      }
   }
   DBUGMESS(1, (stderr, "Exiting on code %d\n", err));
   pthread_mutex_unlock(&state->modlock);
   return (NULL);
} 

int start (int fd, ptranslation state, pthread_attr_t* ptattr)
{
   int err = 0;

   state->device = fd;
   if (err = pthread_mutex_init(&state->modlock, NULL)) {
      DBUGMESS(1, (stderr, "start (%s mutex init): ", state->label));
   } else if (err = pthread_cond_init(&state->offtrack, NULL)) {
      DBUGMESS(1, (stderr, "start (%s offtrack init): ", state->label));
   } else if (err = pthread_cond_init(&state->ontrack, NULL)) {
      DBUGMESS(1, (stderr, "start (%s ontrack init): ", state->label));
   } else if (err = pthread_create(&state->tid, ptattr, track, (void*)state)) {
      DBUGMESS(1, (stderr, "start (%s thread creation): ", state->label));
   }
   switch (err) {
   case ENOMEM:
      DBUGMESS(1, (stderr, "not enough memory.\n"));
      break;
   case EAGAIN:
      DBUGMESS(1, (stderr, "system limit reached.\n"));
      break;
   case EINVAL:
      DBUGMESS(1, (stderr, "invalid attribute.\n"));
      break;
   case ESRCH:
      DBUGMESS(1, (stderr, "invalid thread ID.\n"));
      break;
   default:
      if (err)
         DBUGMESS(1, (stderr, "error %d.\n", err));
   }
   return (err);
}

int stop (ptranslation state)
{
   int err = 0;

   DBUGMESS(1, (stderr, "stop (%s axis): waiting . . . .\n", state->label));
   pthread_mutex_lock(&state->modlock);
   if (err) {
      DBUGMESS(1, (stderr, "stop (%s axis): couldn't acquire lock, error %d\n",
                      state->label, err));
      return (err);
   }
   while (!err && ((state->pos != state->goalpos)  ||
                   (fabs(state->vel - state->goalvel) >= vslow))) {
      err = pthread_cond_wait(&state->ontrack, &state->modlock);
   }
   DBUGMESS(1, (stderr, "stop (%s axis): starting cleanup\n", state->label));
   if (err) {
      DBUGMESS(1, (stderr, "stop (%s waiting for move to complete): ", state->label));
   } else if (err = pthread_cancel(state->tid)) {
      DBUGMESS(1, (stderr, "stop (%s cancelling thread): ", state->label));
   }
   pthread_mutex_unlock(&state->modlock);
   if (!err) {
      if (err = pthread_cond_destroy(&state->offtrack)) {
         DBUGMESS(1, (stderr, "stop (%s offtrack destroy): ", state->label));
      } else if (err = pthread_cond_destroy(&state->ontrack)) {
         DBUGMESS(1, (stderr, "stop (%s ontrack destroy): ", state->label));
      } else if (err = pthread_mutex_destroy(&state->modlock)) {
         DBUGMESS(1, (stderr, "stop (%s mutex destroy): ", state->label));
      }
   }
   switch (err) {
   case ESRCH:
      DBUGMESS(1, (stderr, "invalid thread ID.\n"));
      break;
   case EINVAL:
      DBUGMESS(1, (stderr, "invalid attribute.\n"));
      break;
   default:
      if (err)
         DBUGMESS(1, (stderr, "error %d.\n", err));
   }
   return (err);
}

---