first

extras/tests/pc_based/RampChecker.h

@@ -0,0 +1,142 @@
+#include <stdlib.h>
+#include <string.h>
+
+class RampChecker {
+ public:
+  uint64_t total_ticks;
+  uint32_t last_dt;
+  uint32_t min_dt;
+  bool increase_ok;
+  bool flat_ok;
+  bool decrease_ok;
+  bool first;
+  bool dir_high;
+  bool reversing_allowed;
+  uint32_t accelerate_till;
+  uint32_t coast_till;
+  uint32_t time_coasting;
+  uint32_t pos;
+  uint32_t ticks_since_last_step;
+  float avg_accel;
+  FILE *gp_file;
+  char filename[100];
+
+  void next_ramp() {
+    increase_ok = true;
+    decrease_ok = false;
+    last_dt = ~0;
+    min_dt = ~0;
+    first = true;
+    dir_high = true;
+    coast_till = 0;
+    time_coasting = 0;
+    accelerate_till = 0;
+    reversing_allowed = false;
+  }
+  RampChecker() {
+    ticks_since_last_step = 0;
+    avg_accel = 0;
+    gp_file = NULL;
+    total_ticks = 0;
+    pos = 0;
+    next_ramp();
+  }
+  void start_plot(char *fname) {
+    int n = strlen(fname) - 8; // remove .gnuplot
+    strncpy(filename, fname, n);
+    filename[n] = 0;
+    gp_file = fopen(fname, "w");
+    fprintf(gp_file, "$data <<EOF\n");
+  }
+  void finish_plot() {
+    if (gp_file != NULL) {
+      fprintf(gp_file, "EOF\n");
+      fprintf(gp_file, "set term pngcairo size 1600, 800\n");
+      fprintf(gp_file, "set output \"%s.png\"\n", filename);
+      fprintf(gp_file, "set multiplot layout 2,2\n");
+      fprintf(gp_file, "set title \"speed [steps/s] over time [s]\"\n");
+      fprintf(gp_file, "plot $data using 1:2 with lines notitle\n");
+      fprintf(gp_file, "set title \"speed [steps/s] over position\"\n");
+      fprintf(gp_file, "plot $data using 4:2 with lines notitle\n");
+      fprintf(gp_file, "set title \"position over time [s]\"\n");
+      fprintf(gp_file, "plot $data using 1:4 with lines notitle\n");
+      fprintf(gp_file,
+              "set title \"averaged (!) acceleration [steps/s*s] over time "
+              "[s]\"\n");
+      fprintf(gp_file, "plot $data using 1:5 with lines notitle\n");
+      //fprintf(gp_file, "pause -1\n");
+      fclose(gp_file);
+      gp_file = NULL;
+    }
+  }
+  void check_section(struct queue_entry *e) {
+    uint8_t steps = e->steps;
+    if (steps == 0) {
+      // Just a pause
+      if (ticks_since_last_step <= 0xffff0000) {
+        ticks_since_last_step += e->ticks;
+      }
+      total_ticks += e->ticks;
+      printf("process pause %d => %u\n", e->ticks, ticks_since_last_step);
+      return;
+    }
+    if (e->toggle_dir) {
+      assert(reversing_allowed);
+      dir_high = !dir_high;
+      increase_ok = true;
+      last_dt = ~0;
+      decrease_ok = false;
+    }
+    if (dir_high) {
+      pos += steps;
+    } else {
+      pos -= steps;
+    }
+    uint32_t curr_dt = ticks_since_last_step;
+    total_ticks += steps * e->ticks;
+    if (!first) {
+      min_dt = min(min_dt, curr_dt);
+    }
+    float accel = 0;
+    if (last_dt != ~0) {
+      accel = (16000000.0 / float(curr_dt) - 16000000.0 / float(last_dt)) /
+              (1.0 / 16000000.0 * float(steps * curr_dt));
+      avg_accel += (accel - avg_accel) / (steps * 20);
+    }
+    printf(
+        "process command in ramp checker @%.6fs: steps = %d last = %u current "
+        "= %u "
+        " min_dt "
+        "= %u   accel=%.6f inc=%s dec=%s\n",
+        total_ticks / 16000000.0, steps, last_dt, curr_dt, min_dt, accel,
+        increase_ok ? "ALLOW" : "NO", decrease_ok ? "ALLOW" : "NO");
+
+    if (gp_file != NULL) {
+      fprintf(gp_file, "%.6f %.2f %d %d %f\n", total_ticks / 16000000.0,
+              16000000.0 / last_dt, last_dt, pos, avg_accel);
+    }
+
+    assert(first || (steps * curr_dt > 0));
+
+    if (last_dt > curr_dt) {
+      assert(increase_ok);
+      accelerate_till = total_ticks;
+      decrease_ok = true;
+    } else if (last_dt < curr_dt) {
+      if (increase_ok) {
+        coast_till = total_ticks - curr_dt;
+      }
+      assert(decrease_ok);
+      increase_ok = false;
+    } else {
+      time_coasting += steps * curr_dt;
+    }
+
+    if (!first) {
+      last_dt = curr_dt;
+    }
+
+    ticks_since_last_step = e->ticks;
+    first = false;
+  }
+};