From b9ac842e8978cca0548d5663f2929c1c2b481b3f Mon Sep 17 00:00:00 2001
From: ian <ian>
Date: Sun, 11 May 2008 11:15:35 +0000
Subject: [PATCH] wagglers as a movfeatkind

---
 hostside/movpos.c   | 92 +++++++++++++++++++++++++++++++++++++++++++--
 hostside/realtime.h |  2 +
 hostside/startup.c  |  4 +-
 3 files changed, 92 insertions(+), 6 deletions(-)

diff --git a/hostside/movpos.c b/hostside/movpos.c
index efac841..4b77d7c 100644
--- a/hostside/movpos.c
+++ b/hostside/movpos.c
@@ -85,6 +85,8 @@ static void motion_done(Segment *move, MovPosComb actual) {
 	  move->i->pname, posnpname(move, move->movposcomb));
 }
 
+static void ignore_all_abandon(Method *m) { }
+
 /*========== points and other fixed timeslot movfeats ==========*/
 
 /*
@@ -565,6 +567,91 @@ static FsqMethod points_method= {
   { .lag_ms= POINT_MOVEMENT, .slot_ms= CDU_RECHARGE, .move= point_move }
 };
 
+/*========== relays ==========*/
+
+/*
+ * Waggler states:
+ *
+ *    ____________		           	conf'd
+ *   /  wagglers_ \		    ready  	  .n
+ *   |   all_      |
+ *   |   abandon   |
+ *   |             V
+ *   |from       UNKNOWN		-1	0
+ *   |any       <=Sta_Settling
+ *  ^^^^^^^^     (start)
+ *                 |
+ *     ___________ |turning
+ *    /           \| _on
+ *   |             V
+ *   |           CHARGING		0	any
+ *   |          >=Sta_Resolving
+ *   |             |
+ *   |             |on_pic
+ *   |             |_charged
+ *   |             V
+ *   ^           READY			1	any
+ *   |             |
+ *   |             |fsq_check_action
+ *   |             | calls point_move which fires a point
+ *    \___________/
+ *
+ */
+
+static FsqMethod waggle;
+
+static Change *waggle_allocate(Method *mm, int alloc_motions) {
+  FsqMethod *m= (void*)mm;
+  assert(m->f.ready>=0);
+  return fsq_allocate(mm, alloc_motions);
+}
+
+static void waggle_do(FsqMethod *m, const MovFeatInfo *mfi, int posn) {
+  /* actually firing points, yay! */
+  PicInsn piob;
+  enco_pic_waggle(&piob, mfi->boob[0], posn);
+  serial_transmit(&piob);
+}
+
+static void waggle_settle_check(void) {
+  SEG_IV;
+  int feat;
+  
+  FOR_SEG {
+    if (seg->movposcomb<0) continue;
+    for (feat=0; feat < seg->i->n_movfeats; feat++) {
+      const MovFeatInfo *feati= &seg->i->movfeats[feat];
+      if (feati->kind != mfk_relay) continue;
+      assert(feati->posns==2);
+      waggle.f.ready= 0;
+      waggle_do(&waggle, feati, (seg->movposcomb / feati->weight) & 1);
+      return;
+    }
+  }
+}
+
+void waggle_settle(void) {
+  waggle.f.ready= 1;
+  waggle_settle_check();
+}
+  
+void on_pic_waggled(const PicInsnInfo *pii, const PicInsn *pi, int objnum) {
+  if (sta_state < Sta_Settling) return;
+
+  waggle.f.ready= 1;
+  if (sta_state >= Sta_Resolving) { fsq_check_action(&waggle); return; }
+
+  assert(sta_state == Sta_Settling);
+  waggle_settle_check();
+}
+
+static FsqMethod relays_method= {
+  { "relay", mfk_relay,
+    waggle_allocate, fsq_reserve, fsq_confirm,
+    fsq_destroy, fsq_all_abandon },
+  { .lag_ms= 5, .slot_ms= 50, .move= waggle_do }
+};
+
 /*========== dummy `nomove' kind ==========*/
 
 static Change *nomove_allocate(Method *m, int alloc_motions) {
@@ -582,11 +669,10 @@ static ErrorCode nomove_confirm(Method *m, Change *c, Segment *move, int n,
   nomove_destroy(m,c);
   return 0;
 }
-static void nomove_all_abandon(Method *m) { }
 
 static Method nomove_method= {
   "nomove", mfk_none, nomove_allocate, nomove_reserve, nomove_confirm,
-  nomove_destroy, nomove_all_abandon
+  nomove_destroy, ignore_all_abandon
 };
 
 /*========== method-independent machinery ==========*/
@@ -594,7 +680,7 @@ static Method nomove_method= {
 static Method *methods[]= {
   &nomove_method,
   (Method*)&points_method,
-  0, /* relay */
+  (Method*)&relays_method,
   0
 };
 
diff --git a/hostside/realtime.h b/hostside/realtime.h
index 3a573d2..065f1c0 100644
--- a/hostside/realtime.h
+++ b/hostside/realtime.h
@@ -133,6 +133,8 @@ void resolve_begin(void); /* from resolve.c */
 int resolve_complete(void);
 void resolve_motioncheck(void);
 
+void waggle_settle(void); /* from movpos.c */
+
 /*---------- from/for record.c and persist.c ----------*/
 
 void records_parse(const char **argv);
diff --git a/hostside/startup.c b/hostside/startup.c
index 0fdcfcb..de23ac0 100644
--- a/hostside/startup.c
+++ b/hostside/startup.c
@@ -85,7 +85,7 @@ static void sta_goto(StartupState new_state) {
   case Sta_Off:   if (sta_state > Sta_Ping) enco_pic_off(&piob);   break;
   case Sta_Ping:                                                   break;
   case Sta_Fault:                                                  break;
-  case Sta_Settling:                        enco_pic_off(&piob);   break;
+  case Sta_Settling:   waggle_settle();     enco_pic_off(&piob);   break;
   case Sta_Resolving:
     resolve_begin();
     points_turning_on();
@@ -213,8 +213,6 @@ void on_pic_wtimeout(const PicInsnInfo *pii, const PicInsn *pi, int objnum) {
 
 void on_pic_hello(const PicInsnInfo *pii, const PicInsn *pi, int objnum)
   { abort(); }
-void on_pic_waggled(const PicInsnInfo *pii, const PicInsn *pi, int objnum)
-  { abort(); }
 void on_pic_aaargh(const PicInsnInfo *pii, const PicInsn *pi, int objnum)
   { abort(); }
 void on_pic_spurious(const PicInsnInfo *pii, const PicInsn *pi, int objnum) {
-- 
2.30.2