1 files changed, 75 insertions, 0 deletions
diff --git a/stm32/app/motor.c b/stm32/app/motor.c
new file mode 100644
index 0000000..392089b
--- /dev/null
+++ b/stm32/app/motor.c
@@ -0,0 +1,75 @@
+#include "project.h"
+
+
+static unsigned motor_pos[HANDS];
+
+
+static void  coils (unsigned m, int a, int b, int c, int d)
+{
+  printf ("Motor %d: %+d %+d\r\n", m, a - b, c - d);
+}
+
+static void step (unsigned m, int d)
+{
+  unsigned s = motor_pos[m];
+
+  if (d < 0) d += MOTOR_STEPS;
+
+  s += d;
+  s %= MOTOR_STEPS;
+
+  coils (m, !! ((s + 6) & 4), !! ((s + 1) & 4), !! ((s + 4) & 4), !! ((s + 7) & 4));
+
+  if (!m)
+    BKP_DR8 = motor_pos[0] = s;
+  else
+    BKP_DR9 = motor_pos[1] = s;
+
+
+}
+
+
+
+
+
+void motor_tick (void)
+{
+  static uint32_t last;
+  unsigned i, d;
+
+  if ((ticks - last) < 10) return;
+
+  last = ticks;
+
+  if (!hands_ready) return;
+
+  //printf("HANDS: %d -> %d    %d -> %d\r\n",hands_pos[0],motor_pos[0],hands_pos[1],motor_pos[1]);
+
+
+  for (i = 0; i < HANDS; ++i) {
+    d = MOTOR_STEPS + hands_pos[i];
+    d -= motor_pos[i];
+    d %= MOTOR_STEPS;
+
+    if (d) {
+      if (d < (MOTOR_STEPS / 2))
+        step (i, 1);
+      else
+        step (i, -1);
+    }
+  }
+}
+
+
+
+
+void motor_init (void)
+{
+
+  motor_pos[0] = BKP_DR8;
+  motor_pos[1] = BKP_DR9;
+
+  step (0, 0);
+  step (1, 0);
+
+}