movfeat wip

diff --git a/hostside/movpos.c b/hostside/movpos.c
index 3332ce17af323b01017fe34553b8985df20358d7..b4935d6112d5f97cf4de1b3b48d28a371e4ea575 100644 (file)
--- a/hostside/movpos.c
+++ b/hostside/movpos.c
@@ -102,6 +102,7 @@ typedef struct {                 /* Allocated  Reserved   Confirmed       */
                      /* in queue?    absent     reserved   confirmed      */
   Change h;
   PtSlot deadline;   /*              ~0         relative   absolute    <- */
+  MovPosComb actual; /*              undef      undef      see below      */
   int n_motions;     /*              alloc'd    alloc'd    undone         */
   Motion motions[];  /*   [0].i:     0          0          non-0       <- */
                      /*  [..].i:     undef      undef      non-0          */
@@ -116,6 +117,21 @@ typedef struct {                 /* Allocated  Reserved   Confirmed       */
    *      A?   to mean  statedet says Allocated, but may be queued
    *  etc.  They are only allowed while we are in a pt_... method function.
    */
+  /* PointReq.actual is subtly differnet to MovPosChange.actual,
+   * as follows:
+   *                              in MovPosChange     in PointReq
+   *  Position unknown              -1                  0
+   *  Position partly known         -1                  unknown feats are 0
+   *  Position completely known     exact               exact
+   *
+   * The partial knowledge positions can only occur in requests that
+   * are confirmed with as many motions as features, so we know that
+   * if we complete a request we know that we can copy actual out
+   * to MovPosChange.
+   *
+   * If we abandon a half-done change to a multi-feat segment
+   * we lose the partial knowledge.
+   */
 
 #define CDU_RECHARGE   250 /*ms*/
 #define POINT_MOVEMENT  50 /*ms*/
@@ -240,6 +256,7 @@ static ErrorCode pt_enqueue(PointQueue *q, PointReq *r) { /* XA -> X */
     return EC_BufferFull;
   }
 
+fprintf(stderr,"  pt_enqueue\n");
   for (insat= q->n;
        insat>0 && (PtSlotSigned)(r->deadline - q->l[insat-1]->deadline) < 0;
        insat--)
@@ -256,6 +273,7 @@ static ErrorCode pt_enqueue(PointQueue *q, PointReq *r) { /* XA -> X */
 static Change *point_allocate(int alloc_motions) {
   PointReq *r;
 
+fprintf(stderr,"  point allocate %d\n",alloc_motions);
   assert(pt_cdu_charged>=0);
   if (!alloc_motions)
     /* we need at least one motion in the table so we can tell
@@ -285,6 +303,8 @@ static ErrorCode point_confirm(Change *chg, Segment *move,
   int allow_failure;
   ErrorCode ec;
 
+fprintf(stderr,"  point confirm\n");
+
   /* If the segment is moving, these motions are already based on the
    * actual physical position which is stored in the existing request.
    * So we try removing the existing request from the queue and put
@@ -308,6 +328,12 @@ static ErrorCode point_confirm(Change *chg, Segment *move,
   r->n_motions= n_motions;
   r->deadline= newdeadline + pt_cslot;
 
+  if (n_motions == move->i->n_movfeats)
+    r->actual= 0;
+  else
+    r->actual= chg->actual;
+  assert(r->actual >= 0);
+
   /* state CA */
   ec= pt_enqueue(&pt_confirmed, r);
   assert(allow_failure || !ec);
@@ -356,9 +382,11 @@ static void pt_check_action(void) {
     pt_cdu_charged= 0;
 
     MovPosComb above_weight= m->i->weight * m->i->posns;
-    MovPosComb above= r->h.actual / above_weight;
-    MovPosComb below= r->h.actual % m->i->weight;
-    r->h.actual= above*above_weight + m->posn*m->i->weight + below;
+    MovPosComb above= r->actual / above_weight;
+    MovPosComb below= r->actual % m->i->weight;
+    r->actual= above*above_weight + m->posn*m->i->weight + below;
+    if (r->h.actual >= 0 || !r->n_motions)
+      r->h.actual= r->actual;
   }
 
   if (!r->n_motions) {
@@ -409,15 +437,20 @@ void on_pic_charged(const PicInsnInfo *pii, const PicInsn *pi, int objnum) {
 /*========== dummy `nomove' kind ==========*/
 
 static Change *nomove_allocate(int alloc_motions) {
+fprintf(stderr,"  nomove allocate %d\n",alloc_motions);
   return mmalloc(sizeof(Change));
 }
-static void nomove_destroy(Change *chg) { free(chg); }
+static void nomove_destroy(Change *chg) {
+  free(chg);
+}
 
 static ErrorCode nomove_reserve(Change *chg, Segment *move, int ms) {
+fprintf(stderr,"  nomove reserve\n");
   return 0;
 }
 static ErrorCode nomove_confirm(Change *chg, Segment *move, int n_motions,
                       const Motion *motions, int ms) {
+fprintf(stderr,"  nomove confirm\n");
   nomove_destroy(chg);
   return 0;
 }
@@ -458,45 +491,62 @@ movpos_change(Segment *back, Segment *move, Segment *fwd,
     actual= move->movposcomb;
   }
 
+fprintf(stderr,"moving %s\n",move->i->pname);
   for (tcomb=0, pci=movei->poscombs;
        tcomb<movei->n_poscombs;
        tcomb++, pci++) {
+fprintf(stderr," tcomb %lu\n",tcomb);
     Segment *tback= &segments[pci->backwards.next];
     Segment *tfwd=  &segments[pci->forwards .next];
+fprintf(stderr,"  tback %s tfwd %s\n",
+       tback?tback->i->pname:"-",
+       tfwd?tfwd->i->pname:"-"
+       );
     if (back && !(back==tback || back==tfwd)) continue;
     if (fwd  && !(fwd ==tback || fwd ==tfwd)) continue;
 
     if (movei->n_movfeats>1) {
+fprintf(stderr,"  several feats\n");
       /* we have to search for the one which is least effort, then */
       for (feat=0, feati=movei->movfeats, tchanges=0;
           feat<movei->n_movfeats;
           feat++)
-       if ((tcomb - actual) / feati->weight % feati->posns)
+       if (actual<0 || (tcomb - actual) / feati->weight % feati->posns)
          tchanges++;
       if (tchanges > bestchanges)
        continue;
+    } else {
+      tchanges= 1;
     }
-    tcomb= bestcomb;
-    tchanges= bestchanges;
+fprintf(stderr,"  best %lu changes %d\n",tcomb,tchanges);
+    bestcomb= tcomb;
+    bestchanges= tchanges;
   }
 
+fprintf(stderr," best %lu changes %d\n",bestcomb,bestchanges);
   if (bestchanges==INT_MAX) { ec= EC_Invalid; goto x; }
 
   {
     int n_motions=0;
     Motion motions[movei->n_movfeats];
 
+fprintf(stderr," motions...  best=%lu actual=%ld\n",bestcomb,actual);
     for (feat=0, feati=movei->movfeats;
         feat<movei->n_movfeats;
         feat++, feati++) {
-      if ((bestcomb - actual) / feati->weight % feati->posns)
+fprintf(stderr,"  checking %s w=%lu posns=%d\n",
+       feati->pname,feati->weight,(int)feati->posns);
+      if (actual>=0 && !((bestcomb - actual) / feati->weight % feati->posns))
        continue;
+      MovPosComb posn= bestcomb / feati->weight % feati->posns;
+fprintf(stderr,"    motion %s %lu kind=%d\n",feati->pname,posn,kind);
       if (kind) {
        if (feati->kind != kind) { ec= EC_MovFeatKindsCombination; goto x; }
+      } else {
        kind= feati->kind;
       }
       motions[n_motions].i= feati;
-      motions[n_motions].posn= bestcomb / feati->weight % feati->posns;
+      motions[n_motions].posn= posn;
       n_motions++;
     }
 
@@ -507,9 +557,11 @@ movpos_change(Segment *back, Segment *move, Segment *fwd,
     } else {
       chg= mp_allocate(ki,move,n_motions);
     }
-    chg->actual=    actual;
+    chg->actual= actual;
 
+fprintf(stderr," confirming %d motions...\n",n_motions);
     ec= ki->confirm(chg, move, n_motions, motions, maxdelay_ms);
+fprintf(stderr," confirming gave %s\n",errorcodelist[ec]);
     if (ec) goto x;
   }
   return 0;

diff --git a/hostside/safety.h b/hostside/safety.h
index 8e6d0e5c6b31f5766aaedd2c6b98d804427e236a..82caacf389ed165f93e748c2e86c743e40516b56 100644 (file)
--- a/hostside/safety.h
+++ b/hostside/safety.h
@@ -81,7 +81,7 @@ struct Segment {
     res_detect:1; /* detection noticed here during resolution */
   TimeInterval until_here, /* ) nonnegative; */  /* ) always valid but      */
     until_detect;          /* ) 0 if already */  /* )  meaningful iff owner */
-  MovPosComb movposcomb;
+  MovPosComb movposcomb; /* -1 means not known */
   MovPosChange *moving; /* non-0 iff feature(s) have been told to change */
   const SegmentInfo *i;
 };