r5781 - developers/werner/cncmap/gp2rml

[werner at docs.openmoko.org]

Author: werner
Date: 2010-01-07 20:57:57 +0100 (Thu, 07 Jan 2010)
New Revision: 5781

Modified:
   developers/werner/cncmap/gp2rml/gp2rml.c
Log:
- gp2rml/gp2rml (output_paths): added distance and run time estimate



Modified: developers/werner/cncmap/gp2rml/gp2rml.c
===================================================================
--- developers/werner/cncmap/gp2rml/gp2rml.c	2010-01-06 20:23:43 UTC (rev 5780)
+++ developers/werner/cncmap/gp2rml/gp2rml.c	2010-01-07 19:57:57 UTC (rev 5781)
@@ -26,11 +26,21 @@
  * We choose Z0 = maximum pen down.
  */
 
+/*
+ * @@@ speed selection anomaly:
+ *
+ * Seems that !ZZ movements of the MDX-15 only use !VZ and VS is completely
+ * ignored.
+ */
+
 #include <stdlib.h>
 #include <stdio.h>
 #include <math.h>
 
 
+#define	V_MAX	15.0
+
+
 struct segment {
 	double x, y, z;
 	struct segment *next;
@@ -118,6 +128,8 @@
 {
 	const struct path *path;
 	const struct segment *seg;
+	double x = 0, y = 0, z = 0;
+	double d, s = 0, t = 0;
 
 	printf("IN;!MC1;PA;VS%f;!VZ%f\n", xy_speed, z_speed);
 	printf("!VO%d;!PZ0,%d;PU\n", units(z_max), units(z_clear));
@@ -129,16 +141,36 @@
 		printf("!PZ%d,%d;PA%d,%d;PD\n",
 		    units(seg->z-z_max), units(z_clear),
 		    units(seg->x), units(seg->y));
+		d = hypot(x-seg->x, y-seg->y)+(z-z_max);
+		s += d;
+		t += d/V_MAX;
+		x = seg->x;
+		y = seg->y;
+		z = seg->z;
 		seg = seg->next;
 		while (seg) {
 			printf("!ZZ%d,%d,%d\n", units(seg->x), units(seg->y),
 			    units(seg->z-z_max));
+			d = hypot(hypot(x-seg->x, y-seg->y), z-seg->z);
+			s += d;
+			t += d/z_speed;
+			x = seg->x;
+			y = seg->y;
+			z = seg->z;
 			seg = seg->next;
 		}
 		printf("PU\n");
+		d = z_max+z_clear-z;
+		s += d;
+		t += d/V_MAX;
+		z = z_max+z_clear;
 	}
 
-	printf("IN;!MC0\n");
+	printf("!MC0;!VO0;!PZ0,0;PU0,0;IN\n");
+	d = -z+hypot(x, y);
+	s += d;
+	t += d/V_MAX;
+	fprintf(stderr, "Distance %.1f mm, time %.1f s\n", s, t);
 }