1 files changed, 78 insertions, 0 deletions

diff --git a/linz/glasslathe/scurve/calc.cpp b/linz/glasslathe/scurve/calc.cpp
new file mode 100644
index 0000000..1371577
--- /dev/null
+++ b/linz/glasslathe/scurve/calc.cpp
@@ -0,0 +1,78 @@
+#include "stdio.h"
+#include <cmath>
+#include <algorithm>
+
+struct {
+  int direction = 0;
+  int frequency = 0;
+  int potValue  = 1023;
+} current, target;
+
+const int frequency_max = 6000;
+int newPotValue;
+bool timer_enabled = false;
+
+int x = 0;
+bool loop()
+{
+  const int step_delay = 300;
+  if (current.frequency != target.frequency || current.direction != target.direction)
+  {
+    if (!timer_enabled && target.frequency != 0)
+    {
+      timer_enabled = true;
+      fprintf(stderr, "enable motor\n");
+      x += step_delay;
+      printf("[%d, %d],\n", x, 0);
+    }
+
+    /*int step = (current.potValue < 20) ? 1
+      : (current.potValue < 100) ? 2
+      : (current.potValue < 300) ? 3
+      : (current.potValue < 400) ? 5
+      : (current.potValue < 500) ? 10
+      : 20;*/
+    int step = (current.frequency > 5000) ? 1
+      : (current.frequency > 4500) ? 2
+      : (current.frequency > 4000) ? 5
+      : (current.frequency > 3000) ? 10
+      : (current.frequency > 1000) ? 25
+      : 50;
+    int frequency = (current.direction == target.direction) ? target.frequency : 0;
+    step = std::min(abs(frequency - current.frequency), step);
+    if (frequency < current.frequency)
+      step *= -1;
+    current.frequency += step;
+
+    x += step_delay;
+    printf("[%d, %d],\n", x, current.frequency);
+
+    if (current.frequency == 0 && current.direction != target.direction)
+    {
+      timer_enabled = false;
+      current.direction = target.direction;
+      fprintf(stderr, "disable motor. changed direction to %d\n", current.direction);
+      x += step_delay;
+      printf("[%d, %d],\n", x, 0);
+    }
+  }
+  else if (timer_enabled && target.potValue == 0)
+  {
+    timer_enabled = false;
+    fprintf(stderr, "disable motor\n");
+    x += step_delay;
+    printf("[%d, %d],\n", x, 0);
+  }
+  else
+    return false;
+  return true;
+}
+
+
+int main(int argc, char *argv[])
+{
+  target.frequency = frequency_max;
+  target.potValue = 1023;
+  while(loop());
+  return 0;
+}