machctl: r24 - sys

Author: vedge
Date: 2009-03-25 05:34:44 -0300 (Wed, 25 Mar 2009)
New Revision: 24

Added:
   sys/cnc_fixunssfdi.c
   sys/cnc_quad.h
Modified:
   sys/cnc.c
   sys/cnc.h
   sys/cnc_device.c
   sys/cnc_devicevar.h
   sys/cnc_estop.c
   sys/cnc_estopvar.h
   sys/cnc_math.c
   sys/cnc_math.h
   sys/cnc_servo.c
   sys/cnc_servovar.h
   sys/cnc_spindle.c
   sys/cnc_spindlevar.h
   sys/files.cnc
Log:
- rewrite MOVE op for proper n-axis support.
- implement JOG op (using registered mpg(4) devices).
- report execution status to cnclcd(4) and cncstatled(4) devices.
- new ioctls CNC_{GET,SET,CAL}TIMINGS.


Modified: sys/cnc.c
===================================================================
--- sys/cnc.c	2009-03-25 08:24:59 UTC (rev 23)
+++ sys/cnc.c	2009-03-25 08:34:44 UTC (rev 24)
 <at>  <at>  -1,7 +1,5  <at>  <at> 
-/*	$OpenBSD$	*/
-
 /*
- * Copyright (c) 2007 Hypertriton, Inc. <http://www.hypertriton.com/>
+ * Copyright (c) 2007-2009 Hypertriton, Inc. <http://www.hypertriton.com/>
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
 <at>  <at>  -46,21 +44,47  <at>  <at> 
 
 #include <dev/gpio/gpiovar.h>
 
+#include <machine/bus.h>
+
 #include "cnc_devicevar.h"
 #include "cnc_servovar.h"
+#include "cnc_spindlevar.h"
 #include "cnc_estopvar.h"
-#include "cnc_spindlevar.h"
+#include "cnc_encodervar.h"
+#include "cnc_mpgvar.h"
+#include "cnc_statledvar.h"
+#include "cnc_lcdvar.h"
+
 #include "cncvar.h"
 
+#include "servo.h"
+#include "spindle.h"
+#include "estop.h"
+#include "encoder.h"
+#include "mpg.h"
+#include "cnclcd.h"
+#include "cncstatled.h"
+
 #define STEPMAX	(INT64_MAX-1)
 #define STEPLEN 20336			/* 20336 units = 1 step */
 #define MAXSTEPS (STEPMAX/STEPLEN)
 
-cnc_real_t cnc_Vmax = 2000.0;		/* maximum velocity (steps/sec) */
-u_long cnc_timebase = 0;		/* delay loop calibration */
+struct cnc_kinlimits cnc_kinlimits = {
+	20000,				/* Max steps/sec */
+	200000,				/* Max steps/ms^2 */
+	8000000				/* Max steps/ms^3 */
+};
 
+const char *cnc_axis_names[] = {
+	"X", "Y", "Z",			/* Primary axes */
+	"U", "V", "W",			/* Secondary axes */
+	"I", "J", "K",			/* Arc center vectors */
+	"A", "B", "C"			/* Rotary axes */
+};
+
 const char *cnc_insn_names[] = {
 	"MOVE",
+	"JOG",
 	"SET_INTERP",
 	"SPINDLE_DIR",
 	"SPINDLE_SPEED",
 <at>  <at>  -85,13 +109,22  <at>  <at> 
 int cnc_opened;
 TAILQ_HEAD(,cnc_insn) cnc_prog;
 struct cnc_vector cnc_pos;
-struct cnc_device *cnc_servos[CNC_NAXES];
-struct cnc_device *cnc_spindles[CNC_MAX_SPINDLES];
-struct cnc_device *cnc_estops[CNC_MAX_ESTOPS];
-struct cnc_device *cnc_estop;
+struct cnc_timings cnc_timings;
+
+struct servo_softc      *cnc_servos[CNC_MAX_SERVOS];
+struct spindle_softc    *cnc_spindles[CNC_MAX_SPINDLES];
+struct estop_softc      *cnc_estops[CNC_MAX_ESTOPS];
+struct encoder_softc    *cnc_encoders[CNC_MAX_ENCODERS];
+struct mpg_softc        *cnc_mpgs[CNC_MAX_MPGS];
+struct cnclcd_softc     *cnc_lcds[CNC_MAX_LCDS];
+struct cncstatled_softc *cnc_status_led = NULL;
+
 int cnc_nservos = 0;
 int cnc_nspindles = 0;
 int cnc_nestops = 0;
+int cnc_nencoders = 0;
+int cnc_nmpgs = 0;
+int cnc_nlcds = 0;
 
 void
 cncattach(int num)
 <at>  <at>  -101,8 +134,6  <at>  <at> 
 	if (num > 1)
 		return;
 
-	printf("cnc: STEPLEN=%u, Vmax = %d steps/s\n",
-	    (u_int)STEPLEN, (int)cnc_Vmax);
 	cnc_opened = 0;
 	pool_init(&cnc_insnpl, sizeof(struct cnc_insn), 0, 0, 0, "cncinsnpl",
 	    NULL);
 <at>  <at>  -111,11 +142,19  <at>  <at> 
 
 	for (i = 0; i < CNC_NAXES; i++)
 		cnc_pos.v[i] = 0;
+	
+	cnc_timings.hz = 0;
+	cnc_timings.move_jog = 0;
 }
 
 int
 cncdetach(struct device *self, int flags)
 {
+#if NCNCLCD > 0
+	int i;
+	for (i = 0; i < cnc_nlcds; i++)
+		cnclcd_close(cnc_lcds[i]);
+#endif
 	cnc_prog_reset();
 	return (0);
 }
 <at>  <at>  -144,22 +183,12  <at>  <at> 
 	return (0);
 }
 
-struct cnc_device *
-cnc_get_spindle(int id)
-{
-	if (id < 0 || id >= cnc_nspindles) {
-		return (NULL);
-	}
-	return (cnc_spindles[id]);
-}
-
 int
 cncwrite(dev_t dev, struct uio *uio, int ioflag)
 {
 	const char *cause;
 	struct cnc_insn insn, *iNew;
 	u_int ip = 0;
-	void *p;
 
 	while (uio->uio_resid > 0) {
 		int rv;
 <at>  <at>  -178,6 +207,7  <at>  <at> 
 		 * before it is executed.
 		 */
 		switch (insn.i_type) {
+#if NSERVO > 0
 		case CNC_MOVE:
 			if (cnc_vec_distance(&cnc_pos, &insn.i_pos) == 0) {
 				cause = "L=0";
 <at>  <at>  -186,6 +216,14  <at>  <at> 
 			if (insn.i_Amax == 0) { cause = "Amax=0"; goto fail; }
 			if (insn.i_Jmax == 0) { cause = "Jmax=0"; goto fail; }
 			break;
+		case CNC_JOG:
+			break;
+#else
+		case CNC_MOVE:
+		case CNC_JOG:
+			cause = "no servo devices available";
+			goto fail;
+#endif /* NSERVO */
 		case CNC_SET_INTERP:
 			if (insn.i_interp_mode < 0 ||
 			    insn.i_interp_mode >= CNC_INTERP_LAST) {
 <at>  <at>  -193,8 +231,9  <at>  <at> 
 				goto fail;
 			}
 			break;
+#if NSPINDLE > 0
 		case CNC_SPINDLE_DIR:
-			if (cnc_get_spindle(insn.i_spindle_id) == NULL) {
+			if (spindle_find(insn.i_spindle_id) == NULL) {
 				cause = "no such spindle";
 				goto fail;
 			}
 <at>  <at>  -205,25 +244,37  <at>  <at> 
 			}
 			break;
 		case CNC_SPINDLE_SPEED:
-			if ((p = cnc_get_spindle(insn.i_spindle_id)) == NULL) {
-				cause = "no such spindle";
-				goto fail;
-			} else {
-				struct spindle_softc *sc = p;
+			{
+				struct spindle_soft *sp;
 
-				if (insn.i_spindle_speed > sc->sc_speed_max) {
-					cause = "speed exceeds spindle limit";
+				if ((sp = spindle_find(insn.i_spindle_id)) != NULL) {
+					if (insn.i_spindle_speed > sp->sc_speed_max) {
+						cause = "requested speed exceeds "
+						        "spindle capabilities";
+						goto fail;
+					}
+				} else {
+					cause = "no such spindle";
 					goto fail;
 				}
 			}
 			break;
 		case CNC_SPINDLE_START:
 		case CNC_SPINDLE_STOP:
-			if ((p = cnc_get_spindle(insn.i_spindle_id)) == NULL) {
+			if (spindle_find(insn.i_spindle_id) == NULL) {
 				cause = "no such spindle";
 				goto fail;
 			}
 			break;
+#else /* NSPINDLE */
+		case CNC_SPINDLE_DIR:
+		case CNC_SPINDLE_SPEED:
+		case CNC_SPINDLE_START:
+		case CNC_SPINDLE_STOP:
+			cause = "no spindle devices available";
+			goto fail;
+#endif /* NSPINDLE */
+
 		default:
 			break;
 		}
 <at>  <at>  -235,7 +286,7  <at>  <at> 
 		TAILQ_INSERT_TAIL(&cnc_prog, iNew, prog);
 		ip++;
 	}
-	printf("cnc: added %u insns to queue\n", ip);
+	printf("cnc: added %u insns to program queue\n", ip);
 	return (0);
 fail:
 	printf("%s[%d]: %s\n", cnc_insn_names[insn.i_type], ip, cause);
 <at>  <at>  -254,8 +305,26  <at>  <at> 
 		ninsns++;
 	}
 /*	printf("cnc: deleted %u insns\n", ninsns); */
+#if NCNCSTATLED > 0
+	cncstatled_set(0);
+#endif
 }
 
+void
+cnc_message(const char *msg)
+{
+#if NCNCLCD > 0
+	int i;
+	for (i = 0; i < cnc_nlcds; i++) {
+		struct cnclcd_softc *lcd = cnc_lcds[i];
+		if (lcd->sc_open) {
+			cnclcd_puts(lcd, msg);
+			cnclcd_putc(lcd, '\n');
+		}
+	}
+#endif
+}
+
 int
 cnc_prog_exec(void)
 {
 <at>  <at>  -263,102 +332,207  <at>  <at> 
 	u_int ninsns = 0;
 	int s;
 
+	s = splhigh();
+
+#if NCNCSTATLED > 0
+	/* Initialize error LEDs and establish communication with LCDs. */
+	cncstatled_set(0);
+#endif
+
+#if NCNCLCD > 0
+	{
+	int i;
+
+	for (i = 0; i < cnc_nlcds; i++) {
+		struct cnclcd_softc *lcd = cnc_lcds[i];
+
+		if (!lcd->sc_open &&
+		    cnclcd_open(cnc_lcds[i], 9600, 7, CNCLCD_PEVEN, 1) == -1) {
+			printf("%s: cannot open\n", ((struct device *)lcd)->dv_xname);
+			continue;
+		}
+		cnclcd_puts(lcd, ((struct device *)lcd)->dv_xname);
+		cnclcd_putc(lcd, '\n');
+	}
+	}
+#endif
+
+	if (cnc_timings.hz == 0 || cnc_timings.move_jog == 0) {
+		printf("cnc: timings are not calibrated!\n");
+		goto fail;
+	}
 	if (cnc_nservos != CNC_NAXES) {
+		/* XXX */
 		printf("cnc: nservos=%d, but CNC_NAXES=%d\n", cnc_nservos,
 		    CNC_NAXES);
-		return (EIO);
+		goto fail;
 	}
-
-	printf("cnc: executing program\n");
-
-	s = splclock();
+	cnc_message("Executing program\n");
 	TAILQ_FOREACH(insn, &cnc_prog, prog) {
+		if (insn->i_type >= CNC_LAST_INSN) {
+			continue;
+		}
+		cnc_message(cnc_insn_names[insn->i_type]);
+		cnc_message("\n");
 		switch (insn->i_type) {
+#if NSERVO > 0
 		case CNC_MOVE:
 			if (cnc_move(insn) == -1) {
 				goto fail;
 			}
 			break;
+# if NMPG > 0
+		case CNC_JOG:
+			if (cnc_nmpgs < 1) {
+				printf("cnc: JOG: no MPGs are available\n");
+				goto fail;
+			}
+			if (insn->i_mult == 1) {
+				if (cnc_move_jog_singlestep(insn) == -1)
+					goto fail;
+			} else {
+				if (cnc_move_jog(insn, 0) == -1)
+					goto fail;
+			}
+			break;
+# endif /* NMPG */
+#endif /* NSERVO */
+
 #if NSPINDLE > 0
 		case CNC_SPINDLE_START:
-			spindle_start(cnc_spindles[insn->i_spindle_id], insn);
+			if (spindle_start(cnc_spindles[insn->i_spindle_id],
+			    insn) == -1) {
+				goto fail;
+			}
 			break;
 		case CNC_SPINDLE_STOP:
-			spindle_stop(cnc_spindles[insn->i_spindle_id], insn);
+			if (spindle_stop(cnc_spindles[insn->i_spindle_id],
+			    insn) == -1) {
+				goto fail;
+			}
 			break;
 		case CNC_SPINDLE_SPEED:
-			spindle_speed(cnc_spindles[insn->i_spindle_id], insn);
+			if (spindle_speed(cnc_spindles[insn->i_spindle_id],
+			    insn) == -1) {
+				goto fail;
+			}
 			break;
-#endif
+#endif /* NSPINDLE */
+
 		default:
+			printf("cnc: illegal instruction 0x%x\n", insn->i_type);
 			break;
 		}
-		printf("insn %u: type=%d\n", ninsns, insn->i_type);
 		ninsns++;
 	}
+#if 0
+	{
+		int i;
+
+		for (i = 0; i < cnc_nlcds; i++) {
+			struct cnclcd_softc *lcd = cnc_lcds[i];
+			if (lcd->sc_open)
+				cnclcd_close(lcd);
+		}
+	}
+#endif
 	splx(s);
 	return (0);
 fail:
+#if NCNCSTATLED > 0
+	cncstatled_set(1);
+#endif
 	splx(s);
 	return (EIO);
 }
 
-/*
- * Try to calibrate cnc_timebase to a value that will give us as
- * close as possible to 1 second. There is no portable way to do
- * this so we repeatedly benchmark results of a real delay loop
- * until the error is reduced to a minimum.
- *
- * This can take a while, but only needs to be run once on any
- * given machine.
- *
- * XXX the algorithm should be optimized based on the rate of change
- * in the results.
- */
-u_long
-cnc_calibrate_timebase(void)
+/* Calibrate the delay loop for 1Hz reference. */
+cnc_utime_t
+cnc_calibrate_hz(void)
 {
+	const int maxIters = 100;
+	const cnc_utime_t tTgt = 1e6; /* 1s */
 	struct timeval tv1, tv2;
-	long ds, du, d;
-	u_long timebase = 10000000, i;
+	cnc_utime_t t, r, i;
+	cnc_time_t d;
 	int s, j;
 
+	/* Start from an arbitrary value. XXX TODO use cpu info */
+	r = 380000000U;
+
 	s = splhigh();
-	printf("cnc: calibrating timebase...");
-	for (j = 0; j < 500; j++) {
+	printf("cnc: calibrating delay loop for 1Hz (%d iters max)...",
+	    maxIters);
+	for (j = 0; j < maxIters; j++) {
 		microtime(&tv1);
-		for (i = 0; i < timebase; i++)
+		for (i = 0; i < r; i++)
 			;;
 		microtime(&tv2);
-		ds = tv2.tv_sec - tv1.tv_sec;
-		du = tv2.tv_usec - tv1.tv_usec;
 
-		printf(" %ld", 1000000-du);
-		if (ds < 0) {
-			timebase += 1000000;
-		} else if (ds > 0) {
-			timebase -= 1000000;
-		} else if (ds == 0 && du < 1000000) {
-			d = 1000000 - du;
-			timebase += d*4;
+		t = (tv2.tv_sec - tv1.tv_sec)*1e6 +
+		    (tv2.tv_usec - tv1.tv_usec);
+
+		printf(" %ld", tTgt-t);
+		if (t < tTgt) {
+			d = tTgt - t;
 			if (d < 10) { break; }
-		} else if (ds == 0 && du > 1000000) {
-			d = du - 1000000;
-			timebase -= d*4;
+			r += d*4;
+		} else if (t > tTgt) {
+			d = t - tTgt;
 			if (d < 10) { break; }
+			r -= d*4;
 		} else {
 			break;
 		}
 	}
 	splx(s);
-	printf("\n");
-	return (timebase);
+	printf("...using %llu\n", r);
+	return (r);
 }
 
+/* Benchmark cnc_move_jog(). */
+cnc_utime_t
+cnc_calibrate_move_jog(void)
+{
+#if NMPG > 0
+	struct timespec tv1, tv2;
+	struct cnc_insn insn;
+	cnc_utime_t r;
+	int i, s;
+
+	if (cnc_nmpgs < 1)
+		return (1);
+	
+	printf("cnc: simulating cnc_move_jog()...");
+
+	insn.i_type = CNC_JOG;
+	insn.i_v0 = 100;
+	insn.i_F = 10000;
+	insn.i_Amax = 1234;
+	insn.i_Jmax = 1234;
+	insn.i_mult = 100;
+	for (i = 0; i < CNC_NAXES; i++)
+		cnc_pos.v[i] = 0.0;
+
+	/* Benchmark the routine using the system clock. */
+	s = splhigh();
+	nanotime(&tv1);
+	cnc_move_jog(&insn, 1);
+	nanotime(&tv2);
+	splx(s);
+
+	r = (cnc_utime_t)(tv2.tv_sec - tv1.tv_sec)*1e12 +
+	                 (tv2.tv_nsec - tv1.tv_nsec);
+	printf("...%lluns\n", r);
+	return (r);
+#else
+	return (1);
+#endif
+}
+
 int
 cncioctl(dev_t dev, u_long cmd, caddr_t data, int flag, struct proc *p)
 {
-	struct cnc_kinematics *kin;
 	cnc_vec_t *pos;
 	int i;
 
 <at>  <at>  -376,8 +550,8  <at>  <at> 
 	case CNC_SETPOS:
 		pos = (cnc_vec_t *)data;
 		for (i = 0; i < CNC_NAXES; i++) {
-			printf("cnc: axis %d: %lu -> %lu", i, cnc_pos.v[i],
-			    pos->v[i]);
+			printf("cnc: SETPOS axis#%d: %lu -> %lu\n", i,
+			    cnc_pos.v[i], pos->v[i]);
 			cnc_pos.v[i] = pos->v[i];
 		}
 		return (0);
 <at>  <at>  -390,27 +564,32  <at>  <at> 
 	case CNC_GETNESTOPS:
 		*(int *)data = cnc_nestops;
 		return (0);
+	case CNC_GETNENCODERS:
+		*(int *)data = cnc_nencoders;
+		return (0);
+	case CNC_GETNMPGS:
+		*(int *)data = cnc_nmpgs;
+		return (0);
 	case CNC_GETKINLIMITS:
-		kin = (struct cnc_kinematics *)data;
-//		kin->k_Ts = (int)cnc_Ts;
-		kin->k_Vmax = (int)cnc_Vmax;
+		bcopy(&cnc_kinlimits, (void *)data, sizeof(struct cnc_kinlimits));
 		return (0);
 	case CNC_SETKINLIMITS:
-		kin = (struct cnc_kinematics *)data;
-		if (kin->k_Ts < 0 || kin->k_Vmax < 0) {
-			return (EINVAL);
-		}
-//		cnc_Ts = (cnc_real_t)kin->k_Ts;
-		cnc_Vmax = (cnc_real_t)kin->k_Vmax;
+		bcopy((const void *)data, &cnc_kinlimits, sizeof(struct cnc_kinlimits));
 		return (0);
-	case CNC_GETTIMEBASE:
-		*(u_long *)data = cnc_timebase;
+	case CNC_GETTIMINGS:
+		bcopy(&cnc_timings, (void *)data, sizeof(struct cnc_timings));
 		return (0);
-	case CNC_SETTIMEBASE:
-		cnc_timebase = *(u_long *)data;
+	case CNC_SETTIMINGS:
+		bcopy((const void *)data, &cnc_timings, sizeof(struct cnc_timings));
 		return (0);
-	case CNC_CALTIMEBASE:
-		*(u_long *)data = cnc_calibrate_timebase();
+	case CNC_CALTIMINGS:
+		if (cnc_timings.hz == 0) {
+			cnc_timings.hz = cnc_calibrate_hz();
+		}
+		if (cnc_timings.move_jog == 0) {
+			cnc_timings.move_jog = cnc_calibrate_move_jog();
+		}
+		bcopy(&cnc_timings, (void *)data, sizeof(struct cnc_timings));
 		return (0);
 	default:
 		break;
 <at>  <at>  -418,180 +597,351  <at>  <at> 
 	return (ENODEV);
 }
 
+#if NSERVO > 0
 void
 cnc_move_axis(enum cnc_axis axis, int dir)
 {
 	servo_step(cnc_servos[axis], dir);
 	cnc_pos.v[axis] += dir;
 }
+#endif
 
-/* Check whether an e-stop device is active. */
+/*
+ * General-purpose "update" callback. This is invoked periodically during
+ * program execution. If it returns -1, the program should abort.
+ */
 int
-cnc_estop_raised(void)
+cnc_update(void)
 {
-	int i;
-
-	for (i = 0; i < cnc_nestops; i++) {
-		if (estop_get_state(cnc_estops[i])) {
-			printf("%s: emergency stop\n",
-			    cnc_estops[i]->cd_dev.dv_xname);
-			return (1);
-		}
+#if NESTOP > 0
+	if (estop_raised()) {
+		printf("cnc: e-stop!\n");
+		return (-1);
 	}
+#endif
+#if NENCODER > 0
+	{
+		int i;
+		for (i = 0; i < cnc_nencoders; i++)
+			encoder_update(cnc_encoders[i]);
+	}
+#endif
 	return (0);
 }
 
+#if NSERVO > 0
+
 /*
- * Coordinated movement of the n axes to a target position in n-dimensional
- * space at the specified feed rate.
+ * Move a group of servo/stepper driven axes in a coordinated fashion at
+ * the specified velocity, subject to our kinematic limits (Amax/Jmax/F)..
  *
- * TODO Specialized applications could use another algorithm to provide a
- * more constant velocity through simultaneous deceleration and acceleration
- * of the axes, at the cost of contour errors proportional to Ts.
+ * TODO Implement simultaneous acceleration/deceleration of axes at the
+ * cost of contour errors, for specialized applications.
  */
 int
 cnc_move(struct cnc_insn *insn)
 {
-	struct cnc_quintic_profile qprof;
+	struct cnc_quintic_profile Q;
 	cnc_real_t t;
-#if CNC_NAXES == 3
-	cnc_pos_t x1 = cnc_pos.v[CNC_X];
-	cnc_pos_t y1 = cnc_pos.v[CNC_Y];
-	cnc_pos_t z1 = cnc_pos.v[CNC_Z];
-	cnc_pos_t x2 = insn->i_pos.v[CNC_X];
-	cnc_pos_t y2 = insn->i_pos.v[CNC_Y];
-	cnc_pos_t z2 = insn->i_pos.v[CNC_Z];
-	cnc_pos_t dx, dy, dz;
-	cnc_pos_t x, y, z;
-	int xdir, ydir, zdir;
-#endif
+	cnc_vec_t v1 = cnc_pos;
+	cnc_vec_t v2 = insn->i_pos;
+	cnc_vec_t d;
+	cnc_pos_t inc, incMin;
+	int dir[CNC_NAXES];
+	int i, nonzero = 0;
+	int axisMajor = 0;
+	cnc_pos_t vMajor = 0;
 
-#if CNC_NAXES == 3
-	dx = abs(x2 - x1);
-	dy = abs(y2 - y1);
-	dz = abs(z2 - z1);
+	/*
+	 * Compute the difference from the current to the new position,
+	 * set axisMajor to the axis with the greatest displacement.
+	 */
+	for (i = 0; i < CNC_NAXES; i++) {
+		d.v[i] = abs(v2.v[i] - v1.v[i]);
+		if (d.v[i] > vMajor) {
+			vMajor = d.v[i];
+			axisMajor = i;
+		}
+		dir[i] = (v2.v[i] > v1.v[i]) ? 1 : -1;
+		if (d.v[i] != 0) { nonzero++; }
+	}
+	if (!nonzero)
+		return (0);		/* Already at target position */
 
-	if (dx == 0 && dy == 0 && dz == 0)
-		return (0);
+	/*
+	 * Rescale the d[] values of the other axes to STEPLEN over the
+	 * displacement of axisMajor.
+	 */
+	for (i = 0; i < CNC_NAXES; i++) {
+		if (i != axisMajor)
+			d.v[i] = d.v[i]*STEPLEN/d.v[axisMajor];
+	}
 
-	xdir = (x2 > x1) ? 1 : -1;
-	ydir = (y2 > y1) ? 1 : -1;
-	zdir = (z2 > z1) ? 1 : -1;
+	/* Compute the time constants for our quintic velocity profile. */
+	if (cnc_quintic_init(&Q,
+	    (cnc_real_t)d.v[axisMajor],
+	    (cnc_real_t)insn->i_v0,
+	    (cnc_real_t)insn->i_F,
+	    (cnc_real_t)insn->i_Amax,
+	    (cnc_real_t)insn->i_Jmax)
+	    == -1) {
+		goto fail;
+	}
 
-	printf("MOVE: Delta=%ld,%ld,%ld\n", dx, dy, dz);
-	printf("MOVE: Dir=%d,%d,%d\n", xdir, ydir, zdir);
-	printf("MOVE: F = %d st/s, Amax = %d st/s^2, Jmax = %d st/s^3\n",
-	    insn->i_F, insn->i_Amax, insn->i_Jmax);
+	/*
+	 * Enter the signal generation loop. We iterate over the displacement
+	 * of the major axis.
+	 */
+	for (inc=0, t=0.0;
+	     inc < d.v[axisMajor] && t < MAXSTEPS;
+	     inc++, t+=1.0) {
+		cnc_real_t v;
+		cnc_utime_t delay, j;
 
-	if (dx >= dy && dx >= dz) {
-		dy = dy*STEPLEN/dx;
-		dz = dz*STEPLEN/dx;
-		if (cnc_quintic_init(&qprof, insn, (cnc_real_t)dx) == -1) {
+		if (cnc_update() == -1)
 			goto fail;
-		}
-		for (x = 0, t = 0.0;
-		     x < dx && t < MAXSTEPS;
-		     x++, t+=1.0) {
-			y = (dy*ydir*t)/STEPLEN;
-			z = (dz*zdir*t)/STEPLEN;
-			if (cnc_estop_raised()) {
-				goto fail;
+
+		/* Increment the major axis once per iteration. */
+		cnc_move_axis(axisMajor, dir[axisMajor]);
+		
+		/* Increment the other axes by their scaled intervals. */
+		for (i = 0; i < CNC_NAXES; i++) {
+			if (i != axisMajor) {
+				incMin = (dir[i]*d.v[i]*t)/STEPLEN;
+				if (v1.v[i]+incMin != cnc_pos.v[i])
+					cnc_move_axis(i, dir[i]);
 			}
-			cnc_move_axis(CNC_X, xdir);
-			if (y1+y != cnc_pos.v[CNC_Y]) {
-				cnc_move_axis(CNC_Y, y);
-				cnc_pos.v[CNC_Y] = y1+y;
-			}
-			if (z1+z != cnc_pos.v[CNC_Z]) {
-				cnc_move_axis(CNC_Z, z);
-				cnc_pos.v[CNC_Z] = z1+z;
-			}
-			cnc_move_delay(&qprof, t, (cnc_real_t)dx);
 		}
-	} else if (dy >= dx && dy >= dz) {
-		dx = dx*STEPLEN/dy;
-		dz = dz*STEPLEN/dy;
-		if (cnc_quintic_init(&qprof, insn, (cnc_real_t)dy) == -1) {
-			goto fail;
+
+		/*
+		 * Evaluate optimal velocity at t in steps/second, and
+		 * enter the corresponding delay loop.
+		 */
+		v = cnc_quintic_step(&Q,
+		    t*(Q.Ta + Q.To)/((cnc_real_t)d.v[axisMajor]))*Q.F /
+		    (Q.v0 + Q.v1 + Q.v2) +
+		    Q.v0;
+		if (v > 0) {
+			delay = (cnc_utime_t)(cnc_timings.hz/v);
+			for (j = 0; j < delay; j++)
+				;;
 		}
-		for (y = 0, t = 0.0;
-		     y < dy && t < MAXSTEPS;
-		     y++, t+=1.0) {
-			x = (dx*xdir*t)/STEPLEN;
-			z = (dz*zdir*t)/STEPLEN;
-			if (cnc_estop_raised()) {
-				goto fail;
-			}
-			cnc_move_axis(CNC_Y, ydir);
-			if (x1+x != cnc_pos.v[CNC_X]) {
-				cnc_move_axis(CNC_X, x);
-				cnc_pos.v[CNC_X] = x1+x;
-			}
-			if (z1+z != cnc_pos.v[CNC_Z]) {
-				cnc_move_axis(CNC_Z, z);
-				cnc_pos.v[CNC_Z] = z1+z;
-			}
-			cnc_move_delay(&qprof, t, (cnc_real_t)dy);
-		}
-	} else if (dz >= dx && dz >= dy) {
-		dx = dx*STEPLEN/dz;
-		dy = dy*STEPLEN/dz;
-		if (cnc_quintic_init(&qprof, insn, (cnc_real_t)dz) == -1) {
-			goto fail;
-		}
-		for (z = 0, t = 0.0;
-		     z < dz && t < MAXSTEPS;
-		     z++, t+=1.0) {
-			x = (dx*xdir*t)/STEPLEN;
-			y = (dy*ydir*t)/STEPLEN;
-			if (cnc_estop_raised()) {
-				goto fail;
-			}
-			cnc_move_axis(CNC_Z, zdir);
-			if (x1+x != cnc_pos.v[CNC_X]) {
-				cnc_move_axis(CNC_X, x);
-				cnc_pos.v[CNC_X] = x1+x;
-			}
-			if (y1+y != cnc_pos.v[CNC_Y]) {
-				cnc_move_axis(CNC_Y, y);
-				cnc_pos.v[CNC_Y] = y1+y;
-			}
-			cnc_move_delay(&qprof, t, (cnc_real_t)dz);
-		}
-	} else {
-		printf("MOVE: Bad case\n");
-		goto fail;
 	}
-#endif
 	return (0);
 fail:
-	printf("MOVE: Aborted instruction\n");
+	printf("cnc: MOVE: Aborted instruction\n");
 	return (-1);
 }
 
+#if NMPG > 0
 /*
- * Enter a precisely-calculated loop that will give us the proper delay
- * between two steps.
+ * Manually control the servos with a MPG until estop is raised.
+ * The MPG pulses are translated directly to servo pulses.
  */
-void
-cnc_move_delay(const struct cnc_quintic_profile *q, cnc_real_t t, cnc_real_t L)
+int
+cnc_move_jog_singlestep(struct cnc_insn *insn)
 {
-	cnc_real_t v;
-	u_long delay;
-	u_long j;
+	int i;
 
-	v = cnc_quintic_step(q, t*(q->Ta + q->To)/L)*q->F/(q->v0+q->v1+q->v2) +
-	    q->v0;
-	delay = (u_long)(cnc_timebase/v);
-	if (delay > cnc_timebase) {
-		delay = cnc_timebase;
+	if (cnc_nmpgs < 1) {
+		printf("cnc: JOG: No MPGs\n");
+		return (-1);
 	}
-#if 1
-	for (j = 0; j < delay; j++)
-		;;
-#else
-	printf("[%lu] %s steps/sec (delay=%lu)\n", (u_long)t, cnc_fmt_real(v),
-	    delay);
+
+	/* Fetch the initial quadrature signal state. */
+	for (i = 0; i < cnc_nmpgs; i++)
+		mpg_jog_init(cnc_mpgs[i]);
+
+	/*
+	 * Loop translating the quadrature signal to directly to step
+	 * by step motion.
+	 */
+	while (!estop_raised()) {
+		for (i = 0; i < cnc_nmpgs; i++) {
+			struct mpg_softc *mpg = cnc_mpgs[i];
+			struct mpg_axis *axis;
+			int axisIdx;
+
+			axisIdx = mpg_get_axis(mpg);
+			axis = &mpg->sc_axes[axisIdx];
+			axis->A = gpio_pin_read(mpg->sc_gpio, &mpg->sc_map, MPG_PIN_A);
+			axis->B = gpio_pin_read(mpg->sc_gpio, &mpg->sc_map, MPG_PIN_B);
+
+			if (axis->A == axis->Aprev &&
+			    axis->B == axis->Bprev)
+				continue;
+
+			if (MPG_TRANSITION_CW(axis)) { cnc_move_axis(axisIdx, 1); }
+			if (MPG_TRANSITION_CCW(axis)) { cnc_move_axis(axisIdx, -1); }
+			
+			axis->Aprev = axis->A;
+			axis->Bprev = axis->B;
+		}
+	}
+	return (0);
+}
+
+/*
+ * Variant of cnc_move() for JOG mode. We use the MPGs to manipulate a
+ * desired target position. The routine spins moving the axes to the
+ * target position following a quintic velocity profile.
+ */
+int
+cnc_move_jog(struct cnc_insn *insn, int simulate)
+{
+	struct mpg_softc *mpg;
+	struct cnc_quintic_profile Q, Qprev;
+	cnc_vec_t vTgt, vTgtLast;
+	cnc_real_t t = 0.0, dt = 0.0, L;
+	int dir, axis;
+	cnc_utime_t Telapsed = 0;
+	int moving = 0;
+/*	int Tprint1 = 0; */
+
+	/* Initialize the MPG for jog. */
+	if (cnc_nmpgs < 1) {
+		printf("cnc: JOG: No MPGs\n");
+		return (-1);
+	}
+	mpg = cnc_mpgs[0];		/* TODO allow multiple mpgs */
+	mpg_jog_init(mpg);
+
+	/* Initialize the target position. */
+	vTgt = cnc_pos;
+	vTgtLast = vTgt;
+	Q.Ta = 0.0;
+	Q.To = 0.0;
+
+	/* Enter the jog loop. */
+	while (!estop_raised()) {
+		/*
+		 * Use the MPG to control the target position, fetch
+		 * the currently selected axis.
+		 */
+		mpg_jog(mpg, &vTgt, insn->i_mult);
+		axis = mpg->sc_sel_axis;
+
+		/* Compute direction and distance to target. */
+		L = (cnc_real_t)(vTgt.v[axis] - cnc_pos.v[axis]);
+		if (L < 0.0) { L = -L; }
+		dir = (vTgt.v[axis] > cnc_pos.v[axis]) ? +1 : -1;
+
+#if 0
+		if (++Tprint1 > 10000) {
+			Tprint1 = 0;
+			printf("[t=%s/", cnc_fmt_real(t));
+			printf("%s] d=%lld (cnc_pos=%lld)\n",
+			    cnc_fmt_real(Q.Ta+Q.To),
+			    vTgt.v[axis] - cnc_pos.v[axis],
+			    cnc_pos.v[axis]);
+		}
 #endif
+		/*
+		 * If the target position has changed, recompute the time
+		 * constants and time increment.
+		 */
+		if (!cnc_vec_same(&vTgtLast, &vTgt)) {
+			cnc_real_t t7prev;
+
+			if (!moving) {
+				moving = 1;
+				if (!simulate) {
+					printf("%s: JOG: Moving to %s=%lld\n",
+					    ((struct device *)mpg)->dv_xname,
+					    cnc_axis_names[axis], vTgt.v[axis]);
+					cnc_message("Moving ");
+					cnc_message(cnc_axis_names[axis]);
+					cnc_message("\n");
+				}
+			}
+			vTgtLast = vTgt;
+			Qprev = Q;
+			if (cnc_quintic_init(&Q, L,
+			    insn->i_v0,
+			    insn->i_F,
+			    insn->i_Amax,
+			    insn->i_Jmax) == -1) {
+				goto fail;
+			}
+			dt = (Q.Ta+Q.To)/L;
+
+			printf("cnc: Quintic: L=%s ", cnc_fmt_real(L));
+			printf("dt=%s ", cnc_fmt_real(dt));
+			printf("F=%s ", cnc_fmt_real(Q.F));
+			printf("v0=%s ", cnc_fmt_real(Q.v0));
+			printf("v1=%s ", cnc_fmt_real(Q.v1));
+			printf("v2=%s ", cnc_fmt_real(Q.v2));
+			printf("Aref=%s ", cnc_fmt_real(Q.Aref));
+			printf("[Ts=%s", cnc_fmt_real(Q.Ts));
+			printf(" Ta=%s", cnc_fmt_real(Q.Ta));
+			printf(" To=%s]\n", cnc_fmt_real(Q.To));
+
+			t = 0.0;
+			Telapsed = 0;
+			if (t7prev >= 1e-6) {
+				printf("cnc: Rescaling t for (Ta+To)=%s: ",
+				    cnc_fmt_real(Q.Ta+Q.To));
+				t = t/t7prev*(Q.Ta+Q.To); /* Rescale time */
+				printf("t=%ss\n", cnc_fmt_real(t));
+				Telapsed = 0;
+			}
+		}
+
+		/*
+		 * Move one step to the target position if the velocity profile
+		 * has determined that this iteration must move one step.
+		 */
+		if (moving) {
+			if (L > CNC_POS_ERROR) {
+				cnc_utime_t v;		/* Steps/second */
+				int clipped = 0;
+
+				v = (cnc_utime_t)cnc_quintic_step(&Q, t);
+				if (v > (cnc_real_t)cnc_kinlimits.Fmax) {
+					v = (cnc_real_t)cnc_kinlimits.Fmax;
+					clipped = 1;
+				}
+				if (v < Q.v0) {
+					v = Q.v0;
+					clipped = =1;
+				}
+#if NCNCSTATLED > 0
+				cncstatled_set(clipped);
+#endif
+				/*
+				 * Compare the estimated time elapsed (ns)
+				 * to the current velocity (steps/sec).
+				 */
+				if ((Telapsed += cnc_timings.move_jog) >
+				    1000000000UL/v) {
+					Telapsed = 0;
+					if (!simulate) {
+						cnc_move_axis(axis, dir);
+					} else {
+						cnc_pos.v[axis] += dir;
+					}
+					t += dt;
+				}
+			} else {
+				if (!simulate) {
+					printf("%s: JOG: Reached target %s=%lld ",
+					    ((struct device *)mpg)->dv_xname,
+					    cnc_axis_names[axis], cnc_pos.v[axis]);
+					printf("in %s seconds\n", cnc_fmt_real(t));
+				}
+				t = 0.0;
+				moving = 0;
+			}
+		}
+
+		if (simulate)
+			break;
+	}
+	return (0);
+fail:
+	printf("cnc: JOG: Aborted instruction\n");
+	return (-1);
 }
+#endif /* NMPG > 0 */
 
+#endif /* NSERVO > 0 */

Modified: sys/cnc.h
===================================================================
--- sys/cnc.h	2009-03-25 08:24:59 UTC (rev 23)
+++ sys/cnc.h	2009-03-25 08:34:44 UTC (rev 24)
 <at>  <at>  -1,4 +1,3  <at>  <at> 
-/*	$OpenBSD$	*/
 /*	Public domain	*/
 
 #ifndef _SYS_CNC_H_
 <at>  <at>  -7,23 +6,44  <at>  <at> 
 #include <sys/stdint.h>
 
 #define CNC_BUF_SIZE		1024		/* size of instruction buffer */
-#define CNC_MAX_AXES		3		/* max axes (min. 1) */
+#define CNC_MAX_AXES		3
 #define CNC_NAXES		CNC_MAX_AXES
-#define CNC_MAX_SPINDLES	2		/* max spindles */
-#define CNC_MAX_ESTOPS		13		/* max e-stops/limit switches */
 
+#define CNC_MAX_SERVOS		3
+#define CNC_MAX_SPINDLES	2
+#define CNC_MAX_ESTOPS		13
+#define CNC_MAX_ENCODERS	4
+#define CNC_MAX_MPGS		2
+#define CNC_MAX_LCDS		4
+
+/* Allowed position error (steps) */
+#define CNC_POS_ERROR		1e-6
+
 enum cnc_axis {
-	CNC_X, CNC_Y, CNC_Z,
-	CNC_A, CNC_B, CNC_C
+	CNC_X = 0,
+	CNC_Y = 1,
+	CNC_Z = 2,
+	CNC_U = 3,
+	CNC_V = 4,
+	CNC_W = 5,
+	CNC_I = 6,
+	CNC_J = 7,
+	CNC_K = 8,
+	CNC_A = 9,
+	CNC_B = 10,
+	CNC_C = 11,
+	CNC_AXIS_LAST
 };
 
 typedef uint64_t cnc_step_t;
 typedef int64_t cnc_pos_t;
 typedef int64_t cnc_time_t;
+typedef int64_t cnc_utime_t;
 
 enum cnc_insn_type {
 	/* servo(4) operations */
 	CNC_MOVE,			/* coordinated move to given position */
+	CNC_JOG,			/* enter jog mode (until estop) */
 	CNC_SET_INTERP,			/* set interpolation mode */
 	/* spindle(4) operations */
 	CNC_SPINDLE_DIR,		/* change current direction */
 <at>  <at>  -71,23 +91,37  <at>  <at> 
  * Kinematic limits. These values must be adjusted based on the physical
  * limits of the motors and of the load they are driving.
  */
-struct cnc_kinematics {
-	int k_Ts;		/* third-order jerk limit (ms) */
-	int k_Vmax;		/* maximum velocity (steps/sec) */
+struct cnc_kinlimits {
+	cnc_utime_t Fmax;		/* Maximum velocity (steps/sec) */
+	cnc_utime_t Amax;		/* Maximum acceleration (steps/ms^2) */
+	cnc_utime_t Jmax;		/* Maximum jerk (steps/ms^3) */
 };
 
+/* Calibrated delay loop timings. */
+struct cnc_timings {
+	cnc_utime_t hz;		/* 1Hz reference */
+	cnc_utime_t move_jog;	/* Execution time of cnc_move_jog() */
+};
+
 /* Program instruction */
 struct cnc_insn {
 	enum cnc_insn_type i_type;
 	union {
-	struct {
-			cnc_vec_t i_pos; /* relative target coords */
-			u_long i_v0;	 /* minimum velocity (steps/s) */
-			u_long i_F;	 /* maximum velocity (steps/s) */
-			u_long i_Amax;	 /* max acceleration (steps/ms^2) */
-			u_long i_Jmax;	 /* max jerk (steps/ms^3) */
+		struct {
+			u_long i_v0;	 /* min steps/s */
+			u_long i_F;	 /* max steps/s */
+			u_long i_Amax;	 /* max steps/ms^2 */
+			u_long i_Jmax;	 /* max steps/ms^3 */
+			cnc_vec_t i_pos; /* target position */
 		} i_move;
 		struct {
+			u_long i_v0;	/* min steps/s */
+			u_long i_F;	/* max steps/s */
+			u_long i_Amax;	/* max steps/ms^2 */
+			u_long i_Jmax;	/* max steps/ms^3 */
+			int i_mult;	/* MPG pulse multiplier */
+		} i_jog;
+		struct {
 			int i_id;	/* name of spindle */
 			int i_dir;	/* 1=CW, -1=CCW */
 			u_int i_speed;	/* speed in RPM */
 <at>  <at>  -107,6 +141,7  <at>  <at> 
 #define i_v0			i_arg.i_move.i_v0
 #define i_Amax			i_arg.i_move.i_Amax
 #define i_Jmax			i_arg.i_move.i_Jmax
+#define i_mult			i_arg.i_jog.i_mult
 #define i_interp_mode		i_arg.i_interp_mode
 #define i_spindle_id		i_arg.i_spindle.i_id
 #define i_spindle_dir		i_arg.i_spindle.i_dir
 <at>  <at>  -125,11 +160,13  <at>  <at> 
 #define CNC_SETPOS		_IOWR('C', 3, struct cnc_vector)
 #define CNC_GETNSERVOS		_IOR('C', 4, int)
 #define CNC_GETNSPINDLES	_IOR('C', 5, int)
-#define CNC_GETKINLIMITS	_IOR('C', 6, struct cnc_kinematics)
-#define CNC_SETKINLIMITS	_IOWR('C', 7, struct cnc_kinematics)
-#define CNC_GETTIMEBASE		_IOR('C', 8, u_long)
-#define CNC_SETTIMEBASE		_IOWR('C', 9, u_long)
-#define CNC_CALTIMEBASE		_IOWR('C', 10, u_long)
+#define CNC_GETKINLIMITS	_IOR('C', 6, struct cnc_kinlimits)
+#define CNC_SETKINLIMITS	_IOWR('C', 7, struct cnc_kinlimits)
+#define CNC_GETTIMINGS		_IOR('C', 8, struct cnc_timings)
+#define CNC_SETTIMINGS		_IOWR('C', 9, struct cnc_timings)
+#define CNC_CALTIMINGS		_IOWR('C', 10, struct cnc_timings)
 #define CNC_GETNESTOPS		_IOR('C', 11, int)
+#define CNC_GETNENCODERS	_IOR('C', 12, int)
+#define CNC_GETNMPGS		_IOR('C', 13, int)
 
 #endif	/* !_SYS_CNC_H_ */

Modified: sys/cnc_device.c
===================================================================
--- sys/cnc_device.c	2009-03-25 08:24:59 UTC (rev 23)
+++ sys/cnc_device.c	2009-03-25 08:34:44 UTC (rev 24)
 <at>  <at>  -1,7 +1,5  <at>  <at> 
-/*	$OpenBSD$	*/
-
 /*
- * Copyright (c) 2007 Hypertriton, Inc. <http://www.hypertriton.com/>
+ * Copyright (c) 2007-2009 Hypertriton, Inc. <http://www.hypertriton.com/>
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
 <at>  <at>  -66,28 +64,52  <at>  <at> 
 	switch (type) {
 	case CNC_DEVICE_SERVO:
 		if ((cnc_nservos+1) <= CNC_NAXES) {
-			printf(": axis %d\n", cnc_nservos);
-			cnc_servos[cnc_nservos++] = cd;
+			printf(": axis #%d\n", cnc_nservos);
+			cnc_servos[cnc_nservos++] = (struct servo_softc *)cd;
 		} else {
 			printf(": ignored (bump CNC_NAXES)\n");
 		}
 		break;
 	case CNC_DEVICE_SPINDLE:
 		if ((cnc_nspindles+1) <= CNC_MAX_SPINDLES) {
-			printf(": spindle %d\n", cnc_nspindles);
-			cnc_spindles[cnc_nspindles++] = cd;
+			printf(": spindle #%d\n", cnc_nspindles);
+			cnc_spindles[cnc_nspindles++] = (struct spindle_softc *)cd;
 		} else {
 			printf(": ignored (bump CNC_MAX_SPINDLES)\n");
 		}
 		break;
 	case CNC_DEVICE_ESTOP:
 		if ((cnc_nestops+1) <= CNC_MAX_ESTOPS) {
-			printf(": estop %d\n", cnc_nestops);
-			cnc_estops[cnc_nestops++] = cd;
+			printf(": estop #%d\n", cnc_nestops);
+			cnc_estops[cnc_nestops++] = (struct estop_softc *)cd;
 		} else {
 			printf(": ignored (bump CNC_MAX_ESTOPS)\n");
 		}
 		break;
+	case CNC_DEVICE_ENCODER:
+		if ((cnc_nencoders+1) <= CNC_MAX_ENCODERS) {
+			printf(": encoder #%d\n", cnc_nencoders);
+			cnc_encoders[cnc_nencoders++] = (struct encoder_softc *)cd;
+		} else {
+			printf(": ignored (bump CNC_MAX_ENCODERS)\n");
+		}
+		break;
+	case CNC_DEVICE_MPG:
+		if ((cnc_nmpgs+1) <= CNC_MAX_MPGS) {
+			printf(": mpg #%d\n", cnc_nmpgs);
+			cnc_mpgs[cnc_nmpgs++] = (struct mpg_softc *)cd;
+		} else {
+			printf(": ignored (bump CNC_MAX_MPGS)\n");
+		}
+		break;
+	case CNC_DEVICE_LCD:
+		if ((cnc_nlcds+1) <= CNC_MAX_LCDS) {
+			printf(": lcd #%d\n", cnc_nlcds);
+			cnc_lcds[cnc_nlcds++] = (struct cnclcd_softc *)cd;
+		} else {
+			printf(": ignored (bump CNC_MAX_LCDS)\n");
+		}
+		break;
 	default:
 		printf(": unimplemented\n");
 		break;

Modified: sys/cnc_devicevar.h
===================================================================
--- sys/cnc_devicevar.h	2009-03-25 08:24:59 UTC (rev 23)
+++ sys/cnc_devicevar.h	2009-03-25 08:34:44 UTC (rev 24)
 <at>  <at>  -1,4 +1,3  <at>  <at> 
-/*	$OpenBSD$	*/
 /*	Public domain	*/
 
 enum cnc_device_type {
 <at>  <at>  -7,7 +6,10  <at>  <at> 
 	CNC_DEVICE_SPINDLE,		/* Machine spindle controller */
 	CNC_DEVICE_ATC,			/* Automatic tool changer */
 	CNC_DEVICE_LASER,		/* Laser power supply controller */
-	CNC_DEVICE_PICKPLACE		/* Pick and place controller */
+	CNC_DEVICE_PICKPLACE,		/* Pick and place controller */
+	CNC_DEVICE_ENCODER,		/* Position-sensing encoder (general-purpose) */
+	CNC_DEVICE_MPG,			/* Manual pulse generator */
+	CNC_DEVICE_LCD			/* Serial interface to text-based LCD */
 };
 
 struct cnc_device {
 <at>  <at>  -24,3 +26,35  <at>  <at> 
 int cnc_device_detach(void *);
 int cnc_device_activate(void *);
 
+/* Attach routine: Map a standard input pin */
+#define CNC_MAP_INPUT(sc, n, name) \
+	do { \
+		int caps; \
+		caps = gpio_pin_caps((sc)->sc_gpio, &(sc)->sc_map, (n)); \
+		if (!(caps & GPIO_PIN_INPUT)) { \
+			printf(": pin#%d (%s) cannot be used for input\n", \
+			    (n), (name)); \
+			goto fail; \
+		} \
+		printf(" %s[%d]", name, (sc)->sc_map.pm_map[n]); \
+		gpio_pin_ctl((sc)->sc_gpio, &(sc)->sc_map, (n), GPIO_PIN_INPUT); \
+	} while (/*CONSTCOND*/0)
+
+/* Attach routine: Map a standard output pin */
+#define CNC_MAP_OUTPUT(sc, n, name) \
+	do { \
+		int caps, ctl; \
+		caps = gpio_pin_caps((sc)->sc_gpio, &(sc)->sc_map, (n)); \
+		if (!(caps & GPIO_PIN_OUTPUT)) { \
+			printf(": pin#%d (%s) cannot drive output\n", \
+			    (n), (name)); \
+			goto fail; \
+		} \
+		printf(" >%s[%d]", name, (sc)->sc_map.pm_map[n]); \
+		ctl = GPIO_PIN_OUTPUT; \
+		if (caps & GPIO_PIN_PUSHPULL) { \
+			ctl |= GPIO_PIN_PUSHPULL; \
+		} \
+		gpio_pin_ctl((sc)->sc_gpio, &(sc)->sc_map, (n), ctl); \
+	} while (/*CONSTCOND*/0)
+

Modified: sys/cnc_estop.c
===================================================================
--- sys/cnc_estop.c	2009-03-25 08:24:59 UTC (rev 23)
+++ sys/cnc_estop.c	2009-03-25 08:34:44 UTC (rev 24)
 <at>  <at>  -1,7 +1,5  <at>  <at> 
-/*	$OpenBSD$	*/
-
 /*
- * Copyright (c) 2007 Hypertriton, Inc. <http://www.hypertriton.com/>
+ * Copyright (c) 2007-2009 Hypertriton, Inc. <http://www.hypertriton.com/>
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
 <at>  <at>  -81,8 +79,11  <at>  <at> 
 {
 	struct estop_softc *sc = (struct estop_softc *)self;
 	struct gpio_attach_args *ga = aux;
-	int caps, ctl;
 	
+	/* Generic CNC device initialization. */
+	if (cnc_device_attach(sc, CNC_DEVICE_ESTOP) == -1)
+		return;
+	
 	sc->sc_flags = 0;
 
 	if (gpio_npins(ga->ga_mask) != ESTOP_NPINS) {
 <at>  <at>  -96,18 +97,14  <at>  <at> 
 		printf(": can't map pins\n");
 		return;
 	}
-	
-	caps = gpio_pin_caps(sc->sc_gpio, &sc->sc_map, ESTOP_PIN);
-	if (!(caps & GPIO_PIN_INPUT)) {
-		printf(": ESTOP pin is unable to read input\n");
-		gpio_pin_unmap(sc->sc_gpio, &sc->sc_map);
-		return;
-	}
-	printf(": ESTOP[%d]\n", sc->sc_map.pm_map[ESTOP_PIN]);
-	ctl = GPIO_PIN_INPUT;
-	if (caps & GPIO_PIN_PUSHPULL) { ctl |= GPIO_PIN_PUSHPULL; }
-	gpio_pin_ctl(sc->sc_gpio, &sc->sc_map, ESTOP_PIN, ctl);
-	gpio_pin_write(sc->sc_gpio, &sc->sc_map, ESTOP_PIN, GPIO_PIN_LOW);
+
+	/* Map the input signal. */
+	CNC_MAP_INPUT(sc, ESTOP_PIN, "ESTOP");
+
+	printf("\n");
+	return;
+fail:
+	gpio_pin_unmap(sc->sc_gpio, &sc->sc_map);
 }
 
 int
 <at>  <at>  -122,9 +119,22  <at>  <at> 
 	return (0);
 }
 
+/* Return e-stop status. */
 int
-estop_get_state(void *p)
+estop_get_state(struct estop_softc *sc)
 {
-	struct estop_softc *sc = p;
 	return (gpio_pin_read(sc->sc_gpio, &sc->sc_map, ESTOP_PIN));
 }
+
+/* Return 1 if one of the registered e-stops are raised. */
+int
+estop_raised(void)
+{
+	int i;
+
+	for (i = 0; i < cnc_nestops; i++) {
+		if (estop_get_state(cnc_estops[i]))
+			return (1);
+	}
+	return (0);
+}

Modified: sys/cnc_estopvar.h
===================================================================
--- sys/cnc_estopvar.h	2009-03-25 08:24:59 UTC (rev 23)
+++ sys/cnc_estopvar.h	2009-03-25 08:34:44 UTC (rev 24)
 <at>  <at>  -1,4 +1,3  <at>  <at> 
-/*	$OpenBSD$	*/
 /*	Public domain	*/
 
 #define ESTOP_NPINS	1
 <at>  <at>  -13,4 +12,5  <at>  <at> 
 	int __map[ESTOP_NPINS];
 };
 
-int estop_get_state(void *);
+int estop_raised(void);
+int estop_get_state(struct estop_softc *);

Added: sys/cnc_fixunssfdi.c
===================================================================
--- sys/cnc_fixunssfdi.c	                        (rev 0)
+++ sys/cnc_fixunssfdi.c	2009-03-25 08:34:44 UTC (rev 24)
 <at>  <at>  -0,0 +1,93  <at>  <at> 
+/*	$OpenBSD: fixunssfdi.c,v 1.5 2005/08/08 08:05:35 espie Exp $ */
+/*-
+ * Copyright (c) 1992, 1993
+ *	The Regents of the University of California.  All rights reserved.
+ *
+ * This software was developed by the Computer Systems Engineering group
+ * at Lawrence Berkeley Laboratory under DARPA contract BG 91-66 and
+ * contributed to Berkeley.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of the University nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include "cnc_quad.h"
+
+#define	ONE_FOURTH	((int)1 << (INT_BITS - 2))
+#define	ONE_HALF	(ONE_FOURTH * 2.0)
+#define	ONE		(ONE_FOURTH * 4.0)
+
+/*
+ * Convert float to (unsigned) quad.  We do most of our work in double,
+ * out of sheer paranoia.
+ *
+ * Not sure what to do with negative numbers---for now, anything out
+ * of range becomes UQUAD_MAX.
+ *
+ * N.B.: must use new ANSI syntax (sorry).
+ */
+u_quad_t
+__fixunssfdi(float f)
+{
+	double x, toppart;
+	union uu t;
+
+	if (f < 0)
+		return (UQUAD_MAX);	/* ??? should be 0?  ERANGE??? */
+#ifdef notdef				/* this falls afoul of a GCC bug */
+	if (f >= UQUAD_MAX)
+		return (UQUAD_MAX);
+#else					/* so we wire in 2^64-1 instead */
+	if (f >= 18446744073709551615.0)	/* XXX */
+		return (UQUAD_MAX);
+#endif
+	x = f;
+	/*
+	 * Get the upper part of the result.  Note that the divide
+	 * may round up; we want to avoid this if possible, so we
+	 * subtract `1/2' first.
+	 */
+	toppart = (x - ONE_HALF) / ONE;
+	/*
+	 * Now build a u_quad_t out of the top part.  The difference
+	 * between x and this is the bottom part (this may introduce
+	 * a few fuzzy bits, but what the heck).  With any luck this
+	 * difference will be nonnegative: x should wind up in the
+	 * range [0..UINT_MAX].  For paranoia, we assume [INT_MIN..
+	 * 2*UINT_MAX] instead.
+	 */
+	t.ul[H] = (unsigned int)toppart;
+	t.ul[L] = 0;
+	x -= (double)t.uq;
+	if (x < 0) {
+		t.ul[H]--;
+		x += UINT_MAX;
+	}
+	if (x > UINT_MAX) {
+		t.ul[H]++;
+		x -= UINT_MAX;
+	}
+	t.ul[L] = (u_int)x;
+	return (t.uq);
+}

Modified: sys/cnc_math.c
===================================================================
--- sys/cnc_math.c	2009-03-25 08:24:59 UTC (rev 23)
+++ sys/cnc_math.c	2009-03-25 08:34:44 UTC (rev 24)
 <at>  <at>  -1,4 +1,3  <at>  <at> 
-/*	$OpenBSD$	*/
 /*	Public domain	*/
 
 #include <sys/param.h>
 <at>  <at>  -83,6 +82,19  <at>  <at> 
 	return (cnc_vec_length(&vd));
 }
 
+/* Return 1 if the two arguments are the same vector. */
+int
+cnc_vec_same(const cnc_vec_t *v1, const cnc_vec_t *v2)
+{
+	int i;
+
+	for (i = 0; i < CNC_NAXES; i++) {
+		if (v1->v[i] != v2->v[i])
+			return (0);
+	}
+	return (1);
+}
+
 char *
 cnc_fmt_real(cnc_real_t v)
 {

Modified: sys/cnc_math.h
===================================================================
--- sys/cnc_math.h	2009-03-25 08:24:59 UTC (rev 23)
+++ sys/cnc_math.h	2009-03-25 08:34:44 UTC (rev 24)
 <at>  <at>  -1,9 +1,10  <at>  <at> 
-/*	$OpenBSD$	*/
+/*	Public domain	*/
 
 typedef float cnc_real_t;
 
 #define	CNC_PI		3.14159265358979323846	/* pi */
 #define	CNC_PI_2	1.57079632679489661923	/* pi/2 */
+#define	CNC_PI_P2	(CNC_PI*CNC_PI)		/* pi^2 */
 #define CNC_MSECS(s)	((u_long)(s*1e3))
 
 double cnc_sin(double);
 <at>  <at>  -11,11 +12,13  <at>  <at> 
 double cnc_cbrt(double);
 double cnc_log10(double);
 double cnc_modf(double, double *);
-char *cnc_fmt_real(cnc_real_t);
 
-void cnc_vec_init(cnc_vec_t);
-void cnc_vec_sub(cnc_vec_t *, const cnc_vec_t *, const cnc_vec_t *);
-void cnc_vec_add(cnc_vec_t *, const cnc_vec_t *, const cnc_vec_t *);
+char  *cnc_fmt_real(cnc_real_t);
+
+void       cnc_vec_init(cnc_vec_t);
+void       cnc_vec_sub(cnc_vec_t *, const cnc_vec_t *, const cnc_vec_t *);
+void       cnc_vec_add(cnc_vec_t *, const cnc_vec_t *, const cnc_vec_t *);
 cnc_real_t cnc_vec_dotprod(cnc_vec_t *, const cnc_vec_t *, const cnc_vec_t *);
 cnc_real_t cnc_vec_length(const cnc_vec_t *);
 cnc_real_t cnc_vec_distance(const cnc_vec_t *, const cnc_vec_t *);
+int        cnc_vec_same(const cnc_vec_t *, const cnc_vec_t *);

Added: sys/cnc_quad.h
===================================================================
--- sys/cnc_quad.h	                        (rev 0)
+++ sys/cnc_quad.h	2009-03-25 08:34:44 UTC (rev 24)
 <at>  <at>  -0,0 +1,135  <at>  <at> 
+/*-
+ * Copyright (c) 1992, 1993
+ *	The Regents of the University of California.  All rights reserved.
+ *
+ * This software was developed by the Computer Systems Engineering group
+ * at Lawrence Berkeley Laboratory under DARPA contract BG 91-66 and
+ * contributed to Berkeley.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of the University nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ *	$OpenBSD: quad.h,v 1.6 2004/11/28 07:09:13 mickey Exp $
+ */
+
+/*
+ * Quad arithmetic.
+ *
+ * This library makes the following assumptions:
+ *
+ *  - The type long long (aka quad_t) exists.
+ *
+ *  - A quad variable is exactly twice as long as `int'.
+ *
+ *  - The machine's arithmetic is two's complement.
+ *
+ * This library can provide 128-bit arithmetic on a machine with 128-bit
+ * quads and 64-bit ints, for instance, or 96-bit arithmetic on machines
+ * with 48-bit ints.
+ */
+
+#include <sys/types.h>
+#if !defined(_KERNEL) && !defined(_STANDALONE)
+#include <limits.h>
+#else
+#include <sys/limits.h>
+#endif
+
+/*
+ * Depending on the desired operation, we view a `long long' (aka quad_t) in
+ * one or more of the following formats.
+ */
+union uu {
+	quad_t	q;		/* as a (signed) quad */
+	u_quad_t uq;		/* as an unsigned quad */
+	int	sl[2];		/* as two signed ints */
+	u_int	ul[2];		/* as two unsigned ints */
+};
+
+/*
+ * Define high and low parts of a quad_t.
+ */
+#define	H		_QUAD_HIGHWORD
+#define	L		_QUAD_LOWWORD
+
+/*
+ * Total number of bits in a quad_t and in the pieces that make it up.
+ * These are used for shifting, and also below for halfword extraction
+ * and assembly.
+ */
+#define	QUAD_BITS	(sizeof(quad_t) * CHAR_BIT)
+#define	INT_BITS	(sizeof(int) * CHAR_BIT)
+#define	HALF_BITS	(sizeof(int) * CHAR_BIT / 2)
+
+/*
+ * Extract high and low shortwords from longword, and move low shortword of
+ * longword to upper half of long, i.e., produce the upper longword of
+ * ((quad_t)(x) << (number_of_bits_in_int/2)).  (`x' must actually be u_int.)
+ *
+ * These are used in the multiply code, to split a longword into upper
+ * and lower halves, and to reassemble a product as a quad_t, shifted left
+ * (sizeof(int)*CHAR_BIT/2).
+ */
+#define	HHALF(x)	((u_int)(x) >> HALF_BITS)
+#define	LHALF(x)	((u_int)(x) & (((int)1 << HALF_BITS) - 1))
+#define	LHUP(x)		((u_int)(x) << HALF_BITS)
+
+/*
+ * XXX
+ * Compensate for gcc 1 vs gcc 2.  Gcc 1 defines ?sh?di3's second argument
+ * as u_quad_t, while gcc 2 correctly uses int.  Unfortunately, we still use
+ * both compilers.
+ */
+#if __GNUC_PREREQ__(2, 0) || defined(lint)
+typedef unsigned int	qshift_t;
+#else
+typedef u_quad_t	qshift_t;
+#endif
+
+quad_t __adddi3(quad_t, quad_t);
+quad_t __anddi3(quad_t, quad_t);
+quad_t __ashldi3(quad_t, qshift_t);
+quad_t __ashrdi3(quad_t, qshift_t);
+int __cmpdi2(quad_t, quad_t);
+quad_t __divdi3(quad_t, quad_t);
+quad_t __fixdfdi(double);
+quad_t __fixsfdi(float);
+u_quad_t __fixunsdfdi(double);
+u_quad_t __fixunssfdi(float);
+double __floatdidf(quad_t);
+float __floatdisf(quad_t);
+double __floatunsdidf(u_quad_t);
+quad_t __iordi3(quad_t, quad_t);
+quad_t __lshldi3(quad_t, qshift_t);
+quad_t __lshrdi3(quad_t, qshift_t);
+quad_t __moddi3(quad_t, quad_t);
+quad_t __muldi3(quad_t, quad_t);
+quad_t __negdi2(quad_t);
+quad_t __one_cmpldi2(quad_t);
+u_quad_t __qdivrem(u_quad_t, u_quad_t, u_quad_t *);
+quad_t __subdi3(quad_t, quad_t);
+int __ucmpdi2(u_quad_t, u_quad_t);
+u_quad_t __udivdi3(u_quad_t, u_quad_t );
+u_quad_t __umoddi3(u_quad_t, u_quad_t );
+quad_t __xordi3(quad_t, quad_t);

Modified: sys/cnc_servo.c
===================================================================
--- sys/cnc_servo.c	2009-03-25 08:24:59 UTC (rev 23)
+++ sys/cnc_servo.c	2009-03-25 08:34:44 UTC (rev 24)
 <at>  <at>  -1,7 +1,5  <at>  <at> 
-/*	$OpenBSD$	*/
-
 /*
- * Copyright (c) 2007 Hypertriton, Inc. <http://www.hypertriton.com/>
+ * Copyright (c) 2007-2009 Hypertriton, Inc. <http://www.hypertriton.com/>
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
 <at>  <at>  -79,11 +77,10  <at>  <at> 
 {
 	struct servo_softc *sc = (struct servo_softc *)self;
 	struct gpio_attach_args *ga = aux;
-	int caps, ctl;
 
 	/* Generic CNC device initialization. */
 	if (cnc_device_attach(sc, CNC_DEVICE_SERVO) == -1)
-		goto fail;
+		return;
 
 	/* Check that we have enough pins */
 	if (gpio_npins(ga->ga_mask) != SERVO_NPINS) {
 <at>  <at>  -100,32 +97,23  <at>  <at> 
 		return;
 	}
 	
-	/* Configure STEP pin (output). Initialize to LOW. */
-	caps = gpio_pin_caps(sc->sc_gpio, &sc->sc_map, SERVO_PIN_STEP);
-	if (!(caps & GPIO_PIN_OUTPUT)) {
-		printf(": STEP pin is unable to drive output\n");
-		goto fail;
-	}
-	printf(": STEP[%d]", sc->sc_map.pm_map[SERVO_PIN_STEP]);
-	ctl = GPIO_PIN_OUTPUT;
-	if (caps & GPIO_PIN_PUSHPULL) { ctl |= GPIO_PIN_PUSHPULL; }
-	gpio_pin_ctl(sc->sc_gpio, &sc->sc_map, SERVO_PIN_STEP, ctl);
+	/* Configure step and direction outputs. */
+	CNC_MAP_OUTPUT(sc, SERVO_PIN_STEP, "STEP");
+	CNC_MAP_OUTPUT(sc, SERVO_PIN_DIR, "DIR");
+
+	/* Some motor controllers need an initial pulse. */
+	gpio_pin_write(sc->sc_gpio, &sc->sc_map, SERVO_PIN_DIR, GPIO_PIN_LOW);
 	gpio_pin_write(sc->sc_gpio, &sc->sc_map, SERVO_PIN_STEP, GPIO_PIN_LOW);
-	sc->sc_step = 0;
-	
-	/* Configure DIR pin (output). Initialize to LOW. */
-	caps = gpio_pin_caps(sc->sc_gpio, &sc->sc_map, SERVO_PIN_DIR);
-	if (!(caps & GPIO_PIN_OUTPUT)) {
-		printf(": DIR pin is unable to drive output\n");
-		goto fail;
-	}
-	printf(" DIR[%d]", sc->sc_map.pm_map[SERVO_PIN_DIR]);
-	ctl = GPIO_PIN_OUTPUT;
-	if (caps & GPIO_PIN_PUSHPULL) { ctl |= GPIO_PIN_PUSHPULL; }
-	gpio_pin_ctl(sc->sc_gpio, &sc->sc_map, SERVO_PIN_DIR, ctl);
+	delay(100000);
+	gpio_pin_write(sc->sc_gpio, &sc->sc_map, SERVO_PIN_DIR, GPIO_PIN_HIGH);
+	gpio_pin_write(sc->sc_gpio, &sc->sc_map, SERVO_PIN_STEP, GPIO_PIN_HIGH);
+	delay(100000);
 	gpio_pin_write(sc->sc_gpio, &sc->sc_map, SERVO_PIN_DIR, GPIO_PIN_LOW);
-	sc->sc_dir = 0;
+	gpio_pin_write(sc->sc_gpio, &sc->sc_map, SERVO_PIN_STEP, GPIO_PIN_LOW);
 
+	sc->sc_step = 0;
+	sc->sc_dir = 0;
+	
 	printf("\n");
 	return;
 fail:
 <at>  <at>  -145,10 +133,8  <at>  <at> 
 }
 
 void
-servo_set_dir(void *arg, int dir)
+servo_set_dir(struct servo_softc *sc, int dir)
 {
-	struct servo_softc *sc = arg;
-
 	if (sc->sc_dir != dir) {
 		sc->sc_dir = dir;
 		gpio_pin_write(sc->sc_gpio, &sc->sc_map, SERVO_PIN_DIR,
 <at>  <at>  -157,71 +143,16  <at>  <at> 
 }
 
 void
-servo_step(void *arg, int dir)
+servo_step(struct servo_softc *sc, int dir)
 {
-	struct servo_softc *sc = arg;
-
 	if (sc->sc_dir != dir) {
 		sc->sc_dir = dir;
+		delay(1);
 		gpio_pin_write(sc->sc_gpio, &sc->sc_map, SERVO_PIN_DIR,
 		    (dir == 1) ? GPIO_PIN_HIGH : GPIO_PIN_LOW);
+		delay(1);
 	}
 	sc->sc_step = !sc->sc_step;
 	gpio_pin_write(sc->sc_gpio, &sc->sc_map, SERVO_PIN_STEP,
 	    sc->sc_step ? GPIO_PIN_HIGH : GPIO_PIN_LOW);
 }
-
-#if 0
-void
-servo_accel_test(void *p)
-{
-	struct servo_softc *sc = p;
-	int j, k, delay;
-	int slow = 8000;
-	int fast = 800;
-	int steady = 10000;
-	int accel = 100;
-	int decel = 100;
-
-	printf("%s: velocity test\n", sc->sc_cdev.cd_dev.dv_xname);
-
-	delay = slow;
-	k = 0;
-	while (1) {
-		sc->sc_step = !sc->sc_step;
-		gpio_pin_write(sc->sc_gpio, &sc->sc_map, SERVO_PIN_STEP,
-		    sc->sc_step ? GPIO_PIN_HIGH : GPIO_PIN_LOW);
-		for (j = 0; j < delay; j++)
-			;;
-		if (++k == accel) {
-			k = 0;
-			if (delay > fast) {
-				delay -= 10;
-			} else {
-				if (--steady == 0)
-					goto next;
-			}
-		}
-	}
-next:
-	delay = fast;
-	k = 0;
-	while (1) {
-		gpio_pin_write(sc->sc_gpio, &sc->sc_map, SERVO_PIN_STEP,
-		    sc->sc_step ? GPIO_PIN_HIGH : GPIO_PIN_LOW);
-		for (j = 0; j < delay; j++)
-			;;
-		if (++k == decel) {
-			k = 0;
-			if (delay < slow) {
-				delay += 10;
-			} else {
-				goto out;
-			}
-		}
-	}
-out:
-	printf("%s: velocity test done\n", sc->sc_cdev.cd_dev.dv_xname);
-	return;
-}
-#endif

Modified: sys/cnc_servovar.h
===================================================================
--- sys/cnc_servovar.h	2009-03-25 08:24:59 UTC (rev 23)
+++ sys/cnc_servovar.h	2009-03-25 08:34:44 UTC (rev 24)
 <at>  <at>  -14,5 +14,5  <at>  <at> 
 	int sc_dir;				/* last direction */
 };
 
-void servo_set_dir(void *, int);
-void servo_step(void *, int);
+void servo_set_dir(struct servo_softc *, int);
+void servo_step(struct servo_softc *, int);

Modified: sys/cnc_spindle.c
===================================================================
--- sys/cnc_spindle.c	2009-03-25 08:24:59 UTC (rev 23)
+++ sys/cnc_spindle.c	2009-03-25 08:34:44 UTC (rev 24)
 <at>  <at>  -1,7 +1,5  <at>  <at> 
-/*	$OpenBSD$	*/
-
 /*
- * Copyright (c) 2007 Hypertriton, Inc. <http://www.hypertriton.com/>
+ * Copyright (c) 2007-2009 Hypertriton, Inc. <http://www.hypertriton.com/>
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
 <at>  <at>  -80,7 +78,6  <at>  <at> 
 {
 	struct spindle_softc *sc = (struct spindle_softc *)self;
 	struct gpio_attach_args *ga = aux;
-	int caps, ctl;
 
 	/* Generic CNC device initialization. */
 	if (cnc_device_attach(sc, CNC_DEVICE_SPINDLE) == -1)
 <at>  <at>  -100,59 +97,18  <at>  <at> 
 		printf(": can't map pins\n");
 		return;
 	}
-	
-	/* Initialize DIR pin to LOW (CW) */
-	caps = gpio_pin_caps(sc->sc_gpio, &sc->sc_map, SPINDLE_PIN_DIR);
-	if (!(caps & GPIO_PIN_OUTPUT)) {
-		printf(": DIR pin is unable to drive output\n");
-		goto fail;
-	}
-	printf(": DIR[%d]", sc->sc_map.pm_map[SPINDLE_PIN_DIR]);
-	ctl = GPIO_PIN_OUTPUT;
-	if (caps & GPIO_PIN_PUSHPULL) { ctl |= GPIO_PIN_PUSHPULL; }
-	gpio_pin_ctl(sc->sc_gpio, &sc->sc_map, SPINDLE_PIN_DIR, ctl);
-	gpio_pin_write(sc->sc_gpio, &sc->sc_map, SPINDLE_PIN_DIR, GPIO_PIN_LOW);
-	sc->sc_dir = 1;
 
-	/* Initialize INCREMENT pin to LOW. */
-	caps = gpio_pin_caps(sc->sc_gpio, &sc->sc_map, SPINDLE_PIN_INCREMENT);
-	if (!(caps & GPIO_PIN_OUTPUT)) {
-		printf(": INCREMENT pin is unable to drive output\n");
-		goto fail;
-	}
-	printf(" INCREMENT[%d]", sc->sc_map.pm_map[SPINDLE_PIN_INCREMENT]);
-	ctl = GPIO_PIN_OUTPUT;
-	if (caps & GPIO_PIN_PUSHPULL) { ctl |= GPIO_PIN_PUSHPULL; }
-	gpio_pin_ctl(sc->sc_gpio, &sc->sc_map, SPINDLE_PIN_INCREMENT, ctl);
-	gpio_pin_write(sc->sc_gpio, &sc->sc_map, SPINDLE_PIN_INCREMENT,
-	    GPIO_PIN_LOW);
+	/* Map the pins */
+	CNC_MAP_OUTPUT(sc, SPINDLE_PIN_DIR, "DIR");
+	CNC_MAP_OUTPUT(sc, SPINDLE_PIN_INCREMENT, "INCREMENT");
+	CNC_MAP_OUTPUT(sc, SPINDLE_PIN_DECREMENT, "DECREMENT");
+	CNC_MAP_INPUT(sc, SPINDLE_PIN_TACHO, "TACHO");
 
-	/* Initialize DECREMENT pin to LOW. */
-	caps = gpio_pin_caps(sc->sc_gpio, &sc->sc_map, SPINDLE_PIN_DECREMENT);
-	if (!(caps & GPIO_PIN_OUTPUT)) {
-		printf(": DECREMENT pin is unable to drive output\n");
-		goto fail;
-	}
-	printf(" DECREMENT[%d]", sc->sc_map.pm_map[SPINDLE_PIN_DECREMENT]);
-	ctl = GPIO_PIN_OUTPUT;
-	if (caps & GPIO_PIN_PUSHPULL) { ctl |= GPIO_PIN_PUSHPULL; }
-	gpio_pin_ctl(sc->sc_gpio, &sc->sc_map, SPINDLE_PIN_DECREMENT, ctl);
-	gpio_pin_write(sc->sc_gpio, &sc->sc_map, SPINDLE_PIN_DECREMENT,
-	    GPIO_PIN_LOW);
+	/* Initialize outputs */
+	gpio_pin_write(sc->sc_gpio, &sc->sc_map, SPINDLE_PIN_DIR, GPIO_PIN_LOW);
+	gpio_pin_write(sc->sc_gpio, &sc->sc_map, SPINDLE_PIN_INCREMENT, GPIO_PIN_LOW);
+	gpio_pin_write(sc->sc_gpio, &sc->sc_map, SPINDLE_PIN_DECREMENT, GPIO_PIN_LOW);
 
-	/* Configure TACHO pin for input. */
-	caps = gpio_pin_caps(sc->sc_gpio, &sc->sc_map, SPINDLE_PIN_TACHO);
-	if (!(caps & GPIO_PIN_INPUT)) {
-		printf(": TACHO pin is unable to read input\n");
-		goto fail;
-	}
-	printf(" TACHO[%d]", sc->sc_map.pm_map[SPINDLE_PIN_TACHO]);
-	ctl = GPIO_PIN_INPUT;
-	gpio_pin_ctl(sc->sc_gpio, &sc->sc_map, SPINDLE_PIN_TACHO, ctl);
-	gpio_pin_write(sc->sc_gpio, &sc->sc_map, SPINDLE_PIN_TACHO,
-	    GPIO_PIN_LOW);
-
-	/* Initialize the structure. */
 	sc->sc_speed = 0;
 	sc->sc_speed_tacho = 0;
 	sc->sc_speed_max = 3000;			/* XXX configure */
 <at>  <at>  -208,10 +164,8  <at>  <at> 
  * speed, within error.
  */
 int
-spindle_speed(void *p, struct cnc_insn *insn)
+spindle_speed(struct spindle_softc *sc, struct cnc_insn *insn)
 {
-	struct spindle_softc *sc = p;
-
 	if (insn.i_spindle_speed > sc->sc_speed_max) {
 		printf("%s: %u rpm exceeds spindle limit (%u)\n",
 		    sc->sc_dev.dv_xname, insn.i_spindle_speed,
 <at>  <at>  -226,10 +180,8  <at>  <at> 
 /* Process CNC_SPINDLE_START instruction. */
 /* XXX block until target speed is reached? */
 int
-spindle_start(void *p, struct cnc_insn *insn)
+spindle_start(struct spindle_softc *sc, struct cnc_insn *insn)
 {
-	struct spindle_softc *sc = p;
-
 	/* TODO */
 	printf("%s: startup\n", sc->sc_dev.dv_xname);
 	return (0);
 <at>  <at>  -238,66 +190,19  <at>  <at> 
 /* Process CNC_SPINDLE_STOP instruction. */
 /* XXX block until target speed is reached? */
 int
-spindle_stop(void *p, struct cnc_insn *insn)
+spindle_stop(struct spindle_softc *sc, struct cnc_insn *insn)
 {
-	struct spindle_softc *sc = p;
-
 	/* TODO */
 	printf("%s: spindle stopped\n", sc->sc_dev.dv_xname);
 	return (0);
 }
 
-#if 0
-void
-spindle_accel_test(void *p)
+/* Return a spindle device by name. */
+struct spindle_softc *
+spindle_find(int id)
 {
-	struct spindle_softc *sc = p;
-	int j, k, delay;
-	int slow = 8000;
-	int fast = 800;
-	int steady = 10000;
-	int accel = 100;
-	int decel = 100;
-
-	printf("%s: velocity test\n", sc->sc_cdev.cd_dev.dv_xname);
-
-	delay = slow;
-	k = 0;
-	while (1) {
-		sc->sc_step = !sc->sc_step;
-		gpio_pin_write(sc->sc_gpio, &sc->sc_map, SPINDLE_PIN_STEP,
-		    sc->sc_step ? GPIO_PIN_HIGH : GPIO_PIN_LOW);
-		for (j = 0; j < delay; j++)
-			;;
-		if (++k == accel) {
-			k = 0;
-			if (delay > fast) {
-				delay -= 10;
-			} else {
-				if (--steady == 0)
-					goto next;
-			}
-		}
+	if (id < 0 || id >= cnc_nspindles) {
+		return (NULL);
 	}
-next:
-	delay = fast;
-	k = 0;
-	while (1) {
-		gpio_pin_write(sc->sc_gpio, &sc->sc_map, SPINDLE_PIN_STEP,
-		    sc->sc_step ? GPIO_PIN_HIGH : GPIO_PIN_LOW);
-		for (j = 0; j < delay; j++)
-			;;
-		if (++k == decel) {
-			k = 0;
-			if (delay < slow) {
-				delay += 10;
-			} else {
-				goto out;
-			}
-		}
-	}
-out:
-	printf("%s: velocity test done\n", sc->sc_cdev.cd_dev.dv_xname);
-	return;
+	return (cnc_spindles[id]);
 }
-#endif

Modified: sys/cnc_spindlevar.h
===================================================================
--- sys/cnc_spindlevar.h	2009-03-25 08:24:59 UTC (rev 23)
+++ sys/cnc_spindlevar.h	2009-03-25 08:34:44 UTC (rev 24)
 <at>  <at>  -1,4 +1,3  <at>  <at> 
-/*	$OpenBSD$	*/
 /*	Public domain	*/
 
 #define SPINDLE_NPINS		4
 <at>  <at>  -18,6 +17,8  <at>  <at> 
 	u_int sc_speed_max;		/* maximum allowed speed (rpm) */
 };
 
-int spindle_speed(void *, struct cnc_insn *);
-int spindle_start(void *, struct cnc_insn *);
-int spindle_stop(void *, struct cnc_insn *);
+int spindle_speed(struct spindle_softc *, struct cnc_insn *);
+int spindle_start(struct spindle_softc *, struct cnc_insn *);
+int spindle_stop(struct spindle_softc *, struct cnc_insn *);
+struct spindle_softc *spindle_find(int);
+

Modified: sys/files.cnc
===================================================================
--- sys/files.cnc	2009-03-25 08:24:59 UTC (rev 23)
+++ sys/files.cnc	2009-03-25 08:34:44 UTC (rev 24)
 <at>  <at>  -10,6 +10,7  <at>  <at> 
 file	dev/cnc/cnc_cbrt.c		cnc needs-flag
 file	dev/cnc/cnc_log10.S		cnc needs-flag
 file	dev/cnc/cnc_modf.c		cnc needs-flag
+file	dev/cnc/cnc_fixunssfdi.c	cnc needs-flag
 file	dev/cnc/cnc_quintic.c		cnc needs-flag
 
 # Servo or stepping motor with clock/direction interface.
 <at>  <at>  -31,3 +32,24  <at>  <at> 
 device	estop
 attach	estop at gpio
 file	dev/cnc/cnc_estop.c		estop		needs-flag
+
+# General-purpose position-sensing encoder.
+device	encoder
+attach	encoder at gpio
+file	dev/cnc/cnc_encoder.c		encoder		needs-flag
+
+# Manual pulse generator with optional axis selection.
+device	mpg
+attach	mpg at gpio
+file	dev/cnc/cnc_mpg.c		mpg		needs-flag
+
+# General-purpose status LED.
+device	cncstatled
+attach	cncstatled at gpio
+file	dev/cnc/cnc_statled.c		cncstatled	needs-flag
+
+# Serial LCD interface (std. 8250/16[45]50 in polled mode).
+device	cnclcd
+file	dev/cnc/cnc_lcd.c		cnclcd		needs-flag
+attach	cnclcd at isa with cnclcd_isa
+file	dev/cnc/cnc_lcd_isa.c		cnclcd_isa