From 1bd9e2628241f718cd8a1c12bdca873b178fc6c4 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Wed, 3 May 2017 13:44:38 -0700 Subject: [PATCH] Test code --- avr/test/Makefile | 14 +++--- avr/test/arc.gc | 10 ++++ avr/test/hal.c | 9 ++-- avr/test/{test.gc => line.gc} | 0 avr/test/planner-test.c | 19 +++++--- avr/test/plot.py | 90 +++++++++++++++++++++++++++++++++++ avr/test/short.gc | 59 +++++++++++++++++++++++ 7 files changed, 185 insertions(+), 16 deletions(-) create mode 100644 avr/test/arc.gc rename avr/test/{test.gc => line.gc} (100%) create mode 100755 avr/test/plot.py create mode 100644 avr/test/short.gc diff --git a/avr/test/Makefile b/avr/test/Makefile index 6327058..93aecd1 100644 --- a/avr/test/Makefile +++ b/avr/test/Makefile @@ -15,14 +15,14 @@ all: $(TESTS) planner-test: $(PLANNER_TEST_SRC) gcc -o $@ $(PLANNER_TEST_SRC) $(CFLAGS) $(LDFLAGS) -test: planner-test - ./$< < test.gc +%.csv: %.gc planner-test + ./planner-test < $< | grep -E '^-?[0-9.]+,' -csv: planner-test - @$(MAKE) -s test | grep -E '^-?[0-9.]+,' +%-test: %.gc planner-test + ./planner-test < $< -plot: planner-test - @$(MAKE) -s csv | ./plot_velocity.py +%-plot: %.gc planner-test + ./planner-test < $< | grep -E '^-?[0-9.]+,' | ./plot.py # Clean tidy: @@ -31,7 +31,7 @@ tidy: clean: tidy rm -rf $(TESTS) -.PHONY: tidy clean all test csv plot +.PHONY: tidy clean all # Dependencies -include $(shell mkdir -p .dep) $(wildcard .dep/*) diff --git a/avr/test/arc.gc b/avr/test/arc.gc new file mode 100644 index 0000000..72b612f --- /dev/null +++ b/avr/test/arc.gc @@ -0,0 +1,10 @@ +$resume + +$0vm=10000 +$1vm=10000 +$2vm=10000 +$0jm=50 +$1jm=50 +$2jm=50 + +G3 I2 J2 F10000 diff --git a/avr/test/hal.c b/avr/test/hal.c index 4f7d90d..7dc87a2 100644 --- a/avr/test/hal.c +++ b/avr/test/hal.c @@ -211,11 +211,14 @@ stat_t st_prep_line(float time, const float target[]) { DEBUG_CALL("%f, (%f, %f, %f, %f)", time, target[0], target[1], target[2], target[3]); - for (int i = 0; i < MOTORS; i++) + float p[MOTORS] = {0, 0, 0, 0}; + + for (int i = 0; i < MOTORS; i++) { motor_position[i] = target[i]; + p[i] = target[i] / motor_get_steps_per_unit(i); + } - printf("%0.10f, %0.10f, %0.10f, %0.10f\n", - time, motor_position[0], motor_position[1], motor_position[2]); + printf("%0.10f, %0.10f, %0.10f, %0.10f\n", time, p[0], p[1], p[2]); return STAT_OK; } diff --git a/avr/test/test.gc b/avr/test/line.gc similarity index 100% rename from avr/test/test.gc rename to avr/test/line.gc diff --git a/avr/test/planner-test.c b/avr/test/planner-test.c index 7e9038b..78206ee 100644 --- a/avr/test/planner-test.c +++ b/avr/test/planner-test.c @@ -28,6 +28,7 @@ #include "axis.h" #include "machine.h" #include "command.h" +#include "plan/arc.h" #include "plan/planner.h" #include "plan/exec.h" #include "plan/state.h" @@ -43,12 +44,18 @@ int main(int argc, char *argv[]) { stat_t status = STAT_OK; - while (!feof(stdin)) { - mp_state_callback(); - command_callback(); - } - while (true) { + mach_arc_callback(); // arc generation runs + + bool reading = !feof(stdin); + + if (reading && mp_queue_get_room()) { + mp_state_callback(); + command_callback(); + + if (mp_queue_get_room()) continue; + } + status = mp_exec_move(); printf("EXEC: %s\n", status_to_pgmstr(status)); @@ -66,7 +73,7 @@ int main(int argc, char *argv[]) { printf("ERROR: %s\n", status_to_pgmstr(status)); } - break; + if (!reading) break; } printf("STATE: %s\n", mp_get_state_pgmstr(mp_get_state())); diff --git a/avr/test/plot.py b/avr/test/plot.py new file mode 100755 index 0000000..28fc6c5 --- /dev/null +++ b/avr/test/plot.py @@ -0,0 +1,90 @@ +#!/usr/bin/env python3 + +import sys +import csv +import matplotlib.pyplot as plt +import numpy as np +import math + + +def compute_velocity(rows): + p = [0, 0, 0] + + for row in rows: + t = float(row[0]) + d = 0.0 + + for i in range(3): + x = float(row[i + 1]) - p[i] + d += x * x + p[i] = float(row[i + 1]) + + d = math.sqrt(d) + + yield d / t + + +time_step = 0.005 + +data = list(csv.reader(sys.stdin)) + +velocity = list(compute_velocity(data)) +acceleration = np.diff(velocity) +jerk = np.diff(acceleration) + +t = np.cumsum([float(row[0]) for row in data]) +x = [float(row[1]) for row in data] +y = [float(row[2]) for row in data] +z = [float(row[3]) for row in data] + +rows = 3 +row = 1 +if np.sum(x): rows += 1 +if np.sum(y): rows += 1 +if np.sum(z): rows += 1 + +def next_subplot(): + global row + plt.subplot(rows * 100 + 10 + row) + row += 1 + +if np.sum(x): + next_subplot() + plt.title('X') + plt.plot(t, x, 'r') + plt.ylabel(r'$mm$') + +if np.sum(y): + next_subplot() + plt.title('X') + plt.title('Y') + plt.plot(t, [row[2] for row in data], 'g') + plt.ylabel(r'$mm$') + +if np.sum(z): + next_subplot() + plt.title('Z') + plt.plot(t, [row[3] for row in data], 'b') + plt.ylabel(r'$mm$') + +next_subplot() +plt.title('Velocity') +plt.plot(t, velocity, 'g') +plt.ylabel(r'$\frac{mm}{min}$') + +next_subplot() +plt.title('Acceleration') +plt.plot(t[1:], acceleration, 'b') +plt.ylabel(r'$\frac{mm}{min^2}$') + +next_subplot() +plt.title('Jerk') +plt.plot(t[2:], jerk, 'r') +plt.ylabel(r'$\frac{mm}{min^3}$') +plt.xlabel('Seconds') + +plt.tight_layout() +plt.subplots_adjust(hspace = 0.5) +mng = plt.get_current_fig_manager() +mng.resize(*mng.window.maxsize()) +plt.show() diff --git a/avr/test/short.gc b/avr/test/short.gc new file mode 100644 index 0000000..1ba1685 --- /dev/null +++ b/avr/test/short.gc @@ -0,0 +1,59 @@ +$resume + +$0vm=10000 +$1vm=10000 +$2vm=10000 +$0jm=50 +$1jm=50 +$2jm=50 + +G0 X0.0 +G0 X0.1 +G0 X0.2 +G0 X0.3 +G0 X0.4 +G0 X0.5 +G0 X0.6 +G0 X0.7 +G0 X0.8 +G0 X0.9 +G0 X1.0 +G0 X1.1 +G0 X1.2 +G0 X1.3 +G0 X1.4 +G0 X1.5 +G0 X1.6 +G0 X1.7 +G0 X1.8 +G0 X1.9 +G0 X2.0 +G0 X2.1 +G0 X2.2 +G0 X2.3 +G0 X2.4 +G0 X2.5 +G0 X2.6 +G0 X2.7 +G0 X2.8 +G0 X2.9 +G0 X3.0 +G0 X3.1 +G0 X3.2 +G0 X3.3 +G0 X3.4 +G0 X3.5 +G0 X3.6 +G0 X3.7 +G0 X3.8 +G0 X3.9 +G0 X4.0 +G0 X4.1 +G0 X4.2 +G0 X4.3 +G0 X4.4 +G0 X4.5 +G0 X4.6 +G0 X4.7 +G0 X4.8 +G0 X4.9 -- 2.27.0