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
