r5700 - in developers/werner/cncmap: . rect zscan

werner at docs.openmoko.org werner at docs.openmoko.org
Wed Oct 21 22:11:17 CEST 2009


Author: werner
Date: 2009-10-21 22:11:17 +0200 (Wed, 21 Oct 2009)
New Revision: 5700

Added:
   developers/werner/cncmap/zscan/
   developers/werner/cncmap/zscan/Makefile
   developers/werner/cncmap/zscan/zscan.c
Modified:
   developers/werner/cncmap/README
   developers/werner/cncmap/rect/rect.c
Log:
- README: explained what all this is about
- README, rect/rect.c: described "normalized" format used by "rect"
- zscan/: new utility - a surface scanner



Modified: developers/werner/cncmap/README
===================================================================
--- developers/werner/cncmap/README	2009-10-21 15:04:56 UTC (rev 5699)
+++ developers/werner/cncmap/README	2009-10-21 20:11:17 UTC (rev 5700)
@@ -1,3 +1,17 @@
+When milling a surface, we have the following parameters that depend on
+machine configuration and the piece:
+
+- piece location and orientation: the "lower left" edge of the piece is
+  usually not at the origin. Also, the piece may be slightly rotated.
+
+- z coordinates of the piece's surface: even a nominally level piece
+  can be bent. The machine itself may not be perfectly level. And last
+  but not least, there is a fixed but (initially) unknown offset
+  introduced by the milling bit.
+
+The "cncmap" package provides a set of tools for determining these
+parameters and transforming a 2D plot into an accurate surface
+
 The "cncmap" package consists of the following tools:
 
 millp: interactively control a Roland MDX-15/20 mill, use a "millp" contact
@@ -4,4 +18,10 @@
 sensor for precise positioning, print the current coordinates.
 
 rect: find a rectangle defined by three corner points and print its corners
-in a normalized form.
+in a normalized form, such that they are three adjacent corners of the
+rectangle, with the first corner between the two others, the second point
+"right" when looking from the first point towards the opposite corner, and
+the third point "left".
+
+zscan: scan the surface of a rectangular area with the contact sensor and
+generate a z map.

Modified: developers/werner/cncmap/rect/rect.c
===================================================================
--- developers/werner/cncmap/rect/rect.c	2009-10-21 15:04:56 UTC (rev 5699)
+++ developers/werner/cncmap/rect/rect.c	2009-10-21 20:11:17 UTC (rev 5700)
@@ -84,6 +84,16 @@
 		lr_point(p[i], p[i+1], p[i+2]);
 	lr_calc(&z0, &zx, &zy);
 
+	/*
+	 * Reorder the points such that "o" becomes the corner, "a" and "b"
+	 * define the sides of the rectangle, and the angle from "a" to "b" is
+	 * as close to 90 degrees as possible, in a counter-clockwise
+	 * direction.
+	 *
+	 * If no angle can be found that's reasonably close to 90 degrees, we
+	 * give up.
+	 */
+
 	best_angle = 0;
 	for (o = 0; o != 9; o += 3) {
 		for (a = 0; a != 9; a += 3) {

Added: developers/werner/cncmap/zscan/Makefile
===================================================================
--- developers/werner/cncmap/zscan/Makefile	                        (rev 0)
+++ developers/werner/cncmap/zscan/Makefile	2009-10-21 20:11:17 UTC (rev 5700)
@@ -0,0 +1,32 @@
+#
+# zscan/Makefile - Build the surface scanner
+#
+# Written 2008, 2009 by Werner Almesberger
+# Copyright 2008, 2009 Werner Almesberger
+#
+# This program is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
+#
+
+
+MAIN = zscan
+OBJS = zscan.o serial.o usb.o
+
+CFLAGS = -Wall -Wshadow -iquote ../millp
+LDFLAGS = -lm -lusb
+
+all:		$(MAIN)
+
+$(MAIN):	$(OBJS)
+		$(CC) $(LDFLAGS) -o $(MAIN) $(OBJS)
+
+serial.o:	../millp/serial.c
+		$(CC) $(CFLAGS) -c -o $@ $<
+
+usb.o:		../millp/usb.c
+		$(CC) $(CFLAGS) -c -o $@ $<
+
+clean:
+		rm -f $(OBJS)

Added: developers/werner/cncmap/zscan/zscan.c
===================================================================
--- developers/werner/cncmap/zscan/zscan.c	                        (rev 0)
+++ developers/werner/cncmap/zscan/zscan.c	2009-10-21 20:11:17 UTC (rev 5700)
@@ -0,0 +1,255 @@
+/*
+ * zscan.c - Scan a surface with the contact sensor and generate a z map
+ *
+ * Written 2009 by Werner Almesberger
+ * Copyright 2009 Werner Almesberger
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <math.h>
+#include <sys/time.h>
+
+#include "serial.h"
+#include "usb.h"
+
+
+#define units(mm)	((mm)*40.0)
+
+#define	XY_STEP_DEFAULT		5.0
+#define	XZ_RATIO_DEFAULT	0.1
+#define	Z_STEP			0.025
+
+
+#define MIN_X	  0	/*  0 cm */
+#define MIN_Y	  0	/*  0 cm */
+#define MIN_Z	-60	/*  6 cm */
+
+#define MAX_X	150	/* 15 cm */
+#define MAX_Y	100	/* 10 cm */
+#define MAX_Z	  0	/*  0 cm */
+
+
+static double p[9];
+static double xy_step = XY_STEP_DEFAULT;
+static double xz_ratio = XZ_RATIO_DEFAULT;
+static double x, y, z;
+
+
+#define	MILL_NORMAL	0
+#define	MILL_CONTACT	-1
+#define	MILL_FAULT	-2
+
+
+static int mill_move(void)
+{
+	if (x < MIN_X || x > MAX_X || y < MIN_Y || y > MAX_Y ||
+	    z < MIN_Z || z > MAX_X)
+		return 1;
+	serial_printf("!ZZ%.1f,%.1f,%.1f\n", units(x), units(y), units(z));
+	return 0;
+}
+
+
+static void mill_reset(void)
+{
+	serial_printf("\nIN;!MC0\n");
+	x = y = z = 0;
+	mill_move();
+	sleep(12);
+}
+
+
+static int mill_wait_ms(unsigned ms)
+{
+	static struct timeval last;
+	static int leds;
+	struct timeval end, now;
+	char ch;
+	int probe;
+
+	if (!(last.tv_sec || last.tv_usec))
+		gettimeofday(&last, NULL);
+	end = last;
+	end.tv_usec += ms*1000;
+	while (end.tv_usec > 999999) {
+		end.tv_usec -= 1000000;
+		end.tv_sec++;
+	}
+
+	while (1) {
+	        ch = cycle(leds, &probe);
+
+		gettimeofday(&now, NULL);
+		if (now.tv_sec < end.tv_sec)
+			continue;
+		if (now.tv_sec == end.tv_sec && now.tv_usec < end.tv_usec)
+			continue;
+
+		if (probe & PROBE_NORMAL)
+			leds &= ~LED_CONTACT;
+		else {
+			leds |= LED_CONTACT;
+			return MILL_CONTACT;
+		}
+
+		if (probe & PROBE_TEST_H)
+			leds |= LED_FAULT_H;
+		else
+			leds &= ~LED_FAULT_H;
+		if (probe & PROBE_TEST_L)
+			leds |= LED_FAULT_L;
+		else
+			leds &= ~LED_FAULT_L;
+		if (probe & (PROBE_TEST_H | PROBE_TEST_L))
+			return MILL_FAULT;
+
+		if (ch)
+			return ch;
+
+		break;
+	}
+	last = now;
+	return MILL_NORMAL;
+}
+
+
+void scan(void)
+{
+	double a_len, b_len;
+	double a_n, b_n;
+	double *x_v, *y_v, *z_v;
+	int a, b;
+
+	serial_open("/dev/ttyS0");
+	open_usb();
+	mill_reset();
+
+	x = p[0];
+	y = p[1];
+	z = p[2]+10;
+	mill_move();
+	sleep(10);
+
+	a_len = hypot(p[3]-p[0], p[4]-p[1]);
+	b_len = hypot(p[6]-p[0], p[7]-p[1]);
+	a_n = a_len/xy_step;
+	b_n = b_len/xy_step;
+
+	x_v = malloc(sizeof(double)*(b_n+1));
+	y_v = malloc(sizeof(double)*(b_n+1));
+	z_v = malloc(sizeof(double)*(b_n+1));
+
+	for (a = 0; a <= a_n; a++) {
+		for (b = !(a & 1) ? 0 : b_n;
+		    !(a & 1) ? b <= b_n  : b >= 0;
+		    !(a & 1) ? b++ : b--) {
+			x = p[0]+(p[3]-p[0])*((double) a/a_n)+
+			    (p[6]-p[0])*((double) b/b_n);
+			y = p[1]+(p[4]-p[1])*((double) a/a_n)+
+			    (p[7]-p[1])*((double) b/b_n);
+
+			while (1) {
+				z += xy_step*xz_ratio;
+				if (mill_move()) {
+					fprintf(stderr, "end reached\n");
+					exit(1);
+				}
+				switch (mill_wait_ms(500)) {
+				case MILL_CONTACT:
+					continue;
+				case MILL_FAULT:
+					z -= Z_STEP;
+					continue;
+				case MILL_NORMAL:
+					goto seek;
+				default:
+					fprintf(stderr, "button\n");
+					exit(1);
+				}
+			}
+seek:
+			while (1) {
+				z -= Z_STEP;
+				if (mill_move()) {
+					fprintf(stderr, "end reached\n");
+					exit(1);
+				}
+				switch (mill_wait_ms(50)) {
+				case MILL_CONTACT:
+					goto found;
+				case MILL_FAULT:
+					z += Z_STEP;
+					continue;
+				case MILL_NORMAL:
+					continue;
+				default:
+					fprintf(stderr, "button\n");
+					exit(1);
+				}
+			}
+found:
+			x_v[b] = x;
+			y_v[b] = y;
+			z_v[b] = z;
+		}
+
+		for (b = 0; b <= b_n; b++)
+			printf("%g %g %g\n", x_v[b], y_v[b], z_v[b]);
+		printf("\n");
+		fflush(stdout);
+	}
+}
+
+
+static void usage(const char *name)
+{
+	fprintf(stderr,
+"usage: %s x0 y0 z0 ax ay az bx by bz [step [ratio]]\n\n"
+"    x0, y0, z0  corner point, as returned by \"rect\"\n"
+"    ax, ay, az  end of \"right\" side, as returned by \"rect\"\n"
+"    bx, by, bz  end of \"left\" side, as returned by \"rect\"\n"
+"    step        scanning step in mm (default: %f mm)\n"
+"    ratio       maximum z change per xy distance (default: %f)\n"
+    , name, XY_STEP_DEFAULT, XZ_RATIO_DEFAULT);
+	exit(1);
+}
+
+
+int main(int argc, char **argv)
+{
+	char *end;
+	int i;
+
+	switch (argc) {
+	case 12:
+		xz_ratio = strtod(argv[11], &end);
+		if (*end)
+			usage(*argv);
+		/* fall through */
+	case 11:
+		xy_step = strtod(argv[10], &end);
+		if (*end)
+			usage(*argv);
+		/* fall through */
+	case 10:
+		for (i = 0; i != 9; i++) {
+			p[i] = strtod(argv[i+1], &end);
+			if (*end)
+				usage(*argv);
+		}
+		break;
+	default:
+		usage(*argv);
+	}
+
+	scan();
+
+	return 0;
+}




More information about the commitlog mailing list