Trammell Hudson avatar Trammell Hudson committed 65059d7 Merge

merged python and c code

Comments (0)

Files changed (9)

+e2de0d3520315700adc6efba5581709b5aaf05ed before-usb-split
+CFLAGS += \
+	-std=c99 \
+	-g \
+	-O3 \
+	-W \
+	-Wall \
+
+LDFLAGS += \
+	-lm \
+	-lpthread \
+
+all: servo
+
+servo: servo.o ik.o
+	$(CC) -o $@ $^ $(LDFLAGS)
+
+fk: fk.o
+	$(CC) -o $@ $^ $(LDFLAGS)
+
+ik: ik-main.o ik.o
+	$(CC) -o $@ $^ $(LDFLAGS)
+
+servo.o: servo.c
+
+clean:
+	$(RM) *.o *.a core a.out
+
+Mounting box:
+155x145 base
+35mm separation for the controllers (30mm + 3mm)
+60mm for the power supply (50mm + 3mm)
+130mm total height
+
+
+Axis 1: (positive rotation ccw viewed from above)
+-11200 == +90 deg
++12000 == -90  deg
+= 128.8 ticks/deg
+
+Axis 2:
+-18000 == -90 deg
++16500 == 90 deg
+= 192 ticks/deg
+21 cm
+
+
+axis 3:
+^CLERD 1 0
++10500 == +90
+-11200 == -90
+= 120.5 ticks/deg
+26 cm
+
+--------
+
+1 2 3 4 5
+ 6 7 8 9
+
+wire color/stripe color
+
+axis 1:
+1 blue/brown (1a)
+4 red/blue (1b)
+6 orange/red (index?)
+8 grey (+5v)
+9 red/orange (gnd)
+
+axis 2:
+1 white/blue
+4 blue/white
+6 white/red
+nc orange/white
+
+axis 3:
+1 red/green
+4 green/brown
+6 red/blue
+nc brown/red
+
+axis 4:
+1 white/brown (4a)
+4 brown/white (4b)
+6 white/gray
+nc gray/white
+
+axis 5:
+1 red/gray (5a)
+4 gray/brown (5b)
+6 blue/black
+nc black/blue
+
+axis 6:
+1 orange/black
+4 black/brown
+6 green/white
+nc white/green
+
+
+
+-------
+
+Cross coupling
+command: 4 -40000
+to fix: 5 9000
+p5 command = p5 desired + p4 * (-40000/9000)
+
+command: 4 10000
+to fix: 6 -1200
+p6 command = p6 desired + p5 * (-10000 / 1200)
+
+--------
+
+Six axis controller features:
+
+Each axis:
+- 30 V PWM drivers
+- +5V Quadrature power and counter input
+- Index pulse input
+- Amperage measurement (or something proportional to it)
+- Tunable PID closed loop PVA control
+
+Overall:
+- +5V regulated output for CPU board
+- Brake engage signal
+- Input voltage measurement
+- Kill switch detection (to save current points)
+- USB pendant input
+- Aux port for user pins (on PUMA arms)
+- 8 additional analog inputs
+
+Software features:
+- "Teach mode" for limit detection
+- Compound motion: move to a new set of angles and reach them at the same time.
+- Chaining: once the new position is reached, move to a new position
+- No wind up: when the motor power is re-engaged, sample the current pos
+/** \file
+ * Forward Kinesmatics for the PUMA arms.
+ */
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <math.h>
+
+#include "matrix.h"
+
+/**
+ * Rotations are right-hand rule!  (Counter clockwise)
+ *
+ * 1. Translate by d along the new z axis.
+ * 2. Rotate by the new theta about the new z axis.
+ * 3. Translate by a along the old x axis.
+ * 4. Rotate by a about the old x axis.
+ *
+ * This does not handle cross coupling!
+ *
+ * Note that for the PUMA the straight-up home position is
+ * [0, -90, 90, 0, 0, 0], not all zeros.  This is since the X
+ * axis is aligned with the horizontal plane.
+ */
+typedef struct {
+	double alpha; // rotation around old x axis
+	double a; // translation along old x axis
+	double d; // translation along new z axis
+} dh_param_t;
+
+static const dh_param_t dh_params[] = {
+	{   0,   0,   0 }, // origin is at center of shoulder
+	{ -90,   0,  70 }, // 
+	{   0, 205,  60 },
+	{  90,   0, 225 }, // to the center of the wrist joint
+	{ -90,   0,   0 }, // no translation
+	{  90,   0,  60 }, // from the center of the wrist to the tool platform
+};
+
+
+static m44_t
+fk_dh_build(
+	const dh_param_t * dh,
+	const double theta
+)
+{
+	const double ct = cos(theta);
+	const double st = sin(theta);
+	const double ca = cos(dh->alpha * M_PI/180);
+	const double sa = sin(dh->alpha * M_PI/180);
+
+	return (m44_t) { .v = {
+		{  ct,    -st,      0,     dh->a },
+		{  st*ca,  ct*ca, -sa, -sa*dh->d },
+		{  st*sa,  ct*sa,  ca,  ca*dh->d },
+		{  0,      0,       0,         1 },
+	}};
+}
+
+
+static void
+m44_print(
+	const m44_t * const m
+)
+{
+	int i, j;
+	for (i=0 ; i < 4 ; i++)
+	{
+		for (j=0 ; j < 4 ; j++)
+			printf(" %.3f", m->v[i][j]);
+		printf("\n");
+	}
+}
+
+
+int
+fk(
+	double * pos_out,
+	const double * xyz, // position in final joint space
+	const double * theta, // axes angles
+	const int num_axes
+)
+{
+	double xyz4[4] = { xyz[0], xyz[1], xyz[2], 1 };
+
+	for (int i = num_axes-1 ; i >= 0 ; i--)
+	{
+		m44_t t = fk_dh_build(&dh_params[i], theta[i]);
+
+		//printf("%d: %f\n", i, theta[i]);
+		//m44_print(&t);
+		//printf("\n");
+
+		m44_mult4v(pos_out, &t, xyz4);
+		xyz4[0] = pos_out[0];
+		xyz4[1] = pos_out[1];
+		xyz4[2] = pos_out[2];
+		xyz4[3] = pos_out[3];
+	}
+
+	return 0;
+}
+
+
+int main(int argc, char **argv)
+{
+	double theta[6];
+
+	if (argc != 7)
+	{
+		fprintf(stderr, "need 6 axis angles\n");
+		return -1;
+	}
+
+	for (int i = 1 ; i < 7 ; i++)
+		theta[i-1] = atof(argv[i]) * M_PI / 180;
+
+	double xyz[3] = { 0, 0, 0 };
+	double pos[4];
+	fk(pos, xyz, theta, 6);
+
+	printf("%f %f %f %f\n", pos[0], pos[1], pos[2], pos[3]);
+	return 0;
+}
+/*
+ * Hole pattern is
+ * 28.2x28.2, radius 40
+ *
+ *     L
+ *  X     X
+ * 
+ *
+ *  X     X
+ */
+
+render() difference() {
+	cylinder(r=25, h=3);
+
+	for (i=[0:3])
+	{
+		rotate([0,0,90*i+45]) translate([0,20.1,-1])
+		cylinder(r=5/2, h=7, $fs=1);
+	}
+
+	rotate([0,0,0]) translate([0,21,-1])
+	cylinder(r=6/2, h=7, $fs=1);
+}
+
+
+translate([0,0,8]) rotate([0,90,0]) {
+	render() difference() {
+		union() {
+			translate([4,0,0]) cube([8,6*2,40], center=true);
+			cylinder(r=6, h=40, center=true);
+		}
+		cylinder(r=8/2, h=42, center=true, $fs=1);
+	}
+}
+/** \file
+ * Direct inverse kinesmatics for the PUMA.
+ *
+ * Based on "A geometric approach in solving the inverse kinematics of
+ * PUMA robots" by Lee and Ziegler, 1983.
+ *
+ * http://deepblue.lib.umich.edu/bitstream/handle/2027.42/6192/bac6709.0001.001.pdf?sequence=5
+ */
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include "ik.h"
+
+
+
+int
+ik_first(
+	const joint_cfg_t * const joints,
+	double * const theta, // output commands
+	const double * const xyz, // desired position
+	const int right, // right arm = +1, left arm = -1
+	const int above // elbow above arm = +1, below arm = -1
+)
+{
+	const double px = xyz[0];
+	const double py = xyz[1];
+	const double pz = xyz[2];
+
+	const double a2 = joints[2].a;
+	const double d2 = joints[2].d;
+	const double a3 = joints[3].a;
+	const double d4 = joints[4].d;
+
+	if (px*px + py*py < d2*d2)
+		return 0;
+
+	const double r = sqrt(px*px + py*py - d2*d2);
+	const double R = sqrt(px*px + py*py + pz*pz - d2*d2);
+
+	//printf("r=%f R=%f\n", r, R);
+
+	// theta[0] defined in equation 26
+	theta[0] = atan2(
+		-right * py * r - px * d2,
+		-right * px * r + py * d2
+	);
+
+	// theta[1] is equations 28 - 35.
+	{
+		const double sin_alpha = -pz / R;
+		const double cos_alpha = -right * r / R;
+		const double cos_beta = (a2*a2 + R*R - (d4*d4 + a3*a3)) / (2*a2*R);
+		if (cos_beta > 1 || cos_beta < -1)
+			return 0;
+
+		const double sin_beta = sqrt(1 - cos_beta*cos_beta);
+		const double sin_t2 = sin_alpha * cos_beta + right * above * cos_alpha * sin_beta;
+		const double cos_t2 = cos_alpha * cos_beta - right * above * sin_alpha * sin_beta;
+		theta[1] = atan2(sin_t2, cos_t2);
+	}
+
+	// theta[2] 
+	{
+		const double t2 = d4*d4 + a3*a3;
+		const double t = sqrt(t2);
+		const double cos_phi = (a2*a2 + t2 - R*R) / (2 * a2 * t);
+		if (cos_phi > 1 || cos_phi < -1)
+			return 0;
+		const double sin_phi = right * above * sqrt(1 - cos_phi*cos_phi);
+		const double sin_beta = d4 / t;
+		const double cos_beta = fabs(a3) / t;
+		const double sin_t3 = sin_phi*cos_beta - cos_phi*sin_beta;
+		const double cos_t3 = cos_phi*cos_beta + sin_phi*sin_beta;
+		theta[2] = atan2(sin_t3, cos_t3);
+	}
+
+	return 1;
+}
+
+
+int
+ik_wrist(
+	const joint_cfg_t * const joints,
+	double * const theta, // input/output commands
+	const double * const xyz, // desired position (in mm)
+	const double * const a, // desired approach vector
+	const double * const s, // desired sliding vector (for hand opening)
+	const double * const n, // desired normal vector
+	const int wrist // wrist up = +1, wrist down = -1
+)
+{
+	(void) joints;
+	(void) xyz;
+	(void) wrist;
+
+	const double M = 1;
+	const double ax = a[0];
+	const double ay = a[1];
+	const double az = a[2];
+
+	const double sx = s[0];
+	const double sy = s[1];
+	const double sz = s[2];
+
+	const double nx = n[0];
+	const double ny = n[1];
+	const double nz = n[2];
+
+	const double C1 = cos(theta[0]);
+	const double S1 = sin(theta[0]);
+	const double C23 = cos(theta[1] + theta[2]);
+	const double S23 = sin(theta[1] + theta[2]);
+
+	const double S4 = M * (C1*ay - S1*ax);
+	const double C4 = M * (C1*C23*ax + S1*C23*ay- S23*az);
+
+	theta[3] = atan2(S4, C4);
+
+	const double S5 = (C1*C23*C4 - S1*S4)*ax + (S1*C23*C4 + C1*S4)*ay - C4* S23*az;
+	const double C5 = C1*S23*ax + S1*S23*ay + C23*az;
+	theta[4] = atan2(S5,C5);
+
+	const double S6 = (-S1*C4 - C1*C23*S4)*nx + (C1*C4-S1*C23*S4)*ny + S4*S23*nz;
+	const double C6 = (-S1*C4-C1*C23*S4)*sx + (C1*C4-S1*C23*S4)*sy + S4*S23*sz;
+
+	theta[5] = atan2(S6,C6);
+
+	return 1;
+}
+
+
+void
+fk(
+	const joint_cfg_t * const joints,
+	const double counters[],
+	double xyz_out[3]
+)
+{
+}
+
+
+#if 0
+int main(int argc, char **argv)
+{
+	if (argc != 4)
+	{
+		fprintf(stderr, "need xyz args\n");
+		return -1;
+	}
+
+	const double xyz[3] = {
+		atof(argv[1]),
+		atof(argv[2]),
+		atof(argv[3])
+	};
+	double theta[6];
+
+	double a[] = { 0, 1, 0 };
+	double s[] = { 1, 0, 0 };
+	double n[] = { 0, 0, 1 };
+
+	// right, above
+	if (!ik_first(theta, xyz, 1, 1))
+	{
+		fprintf(stderr, "[%f,%f,%f] unreachable\n", xyz[0], xyz[1], xyz[2]);
+		return -1;
+	}
+
+	// We now have our first three joint angles
+	// compute the hand hangles
+	if (!ik_wrist(theta, xyz, a, s, n, 1))
+	{
+		printf("unreachable wrist\n");
+		return 0;
+	}
+
+
+	for(int i = 0 ; i < 6 ; i++)
+		printf("%f\n", theta[i] * 180 / M_PI);
+
+	return 0;
+}
+#endif
+#pragma once
+
+/** Combine the DH parameters for FK/IK and limits/scaling for drive
+ *
+ * Rotations are right-hand rule!  (Counter clockwise)
+ *
+ * 1. Translate by d along the new z axis.
+ * 2. Rotate by the new theta about the new z axis.
+ * 3. Translate by a along the old x axis.
+ * 4. Rotate by a about the old x axis.
+ *
+ * Note that for the PUMA the straight-up home position is
+ * [0, -90, 90, 0, 0, 0], not all zeros.  This is since the X
+ * axis is aligned with the horizontal plane.
+ */
+typedef struct {
+	// DH parameters
+	double d;
+	double a;
+	double alpha;
+	double angle; // offset from "0" in the DH map
+
+	// Mechanical details about the joint
+	double coupled; // amount this axis is cross coupled to the previous
+	double scale; // rads to counter ticks
+	int min;
+	int max;
+} joint_cfg_t;
+
+extern int
+ik_first(
+	const joint_cfg_t * const joints,
+	double * const theta, // output commands
+	const double * const xyz, // desired position (in mm)
+	const int right, // right arm = +1, left arm = -1
+	const int above // elbow above arm = +1, below arm = -1
+);
+
+
+extern int
+ik_wrist(
+	const joint_cfg_t * const joints,
+	double * const theta, // input(0-2)/output(3-5) commands
+	const double * const xyz, // desired position (in mm)
+	const double * const a, // desired approach vector
+	const double * const s, // desired sliding vector (for hand opening)
+	const double * const n, // desired normal vector
+	const int wrist // wrist up = +1, wrist down = -1
+);
+/** \file
+ * 4x4 matrix math
+ */
+
+#pragma once
+
+typedef struct
+{
+	double v[4][4];
+} m44_t;
+
+
+static inline void
+m44_mult(
+	m44_t * c,
+	const m44_t * a,
+	const m44_t * b
+)
+{
+	int i, j, k;
+
+	for (i = 0 ; i < 4 ; i++)
+	{
+		for (j = 0 ; j < 4 ; j++)
+		{
+			double s = 0;
+			for (k = 0 ; k < 4 ; k++)
+				s += a->v[k][j] * b->v[i][k];
+			c->v[i][j] = s;
+		}
+	}
+}
+
+
+static inline void
+m44_mult4v(
+	double * const c,
+	const m44_t * const a,
+	const double * const b
+)
+{
+	int i, j;
+
+	for (i = 0 ; i < 4 ; i++)
+	{
+		double v = 0;
+
+		for (j = 0 ; j < 4 ; j++)
+			v += a->v[i][j] * b[j];
+
+		c[i] = v;
+	}
+}
+
+
+static inline m44_t
+m44_eye(void)
+{
+	return (m44_t) { .v = {
+		{ 1, 0, 0, 0},
+		{ 0, 1, 0, 0},
+		{ 0, 0, 1, 0},
+		{ 0, 0, 0, 1},
+	}};
+}
+
+
+static inline m44_t
+m44_zero(void)
+{
+	return (m44_t) { .v = {
+		{ 0, 0, 0, 0},
+		{ 0, 0, 0, 0},
+		{ 0, 0, 0, 0},
+		{ 0, 0, 0, 0},
+	}};
+}
+/** \file
+ * Display current positions.
+ */
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdarg.h>
+#include <fcntl.h>
+#include <string.h>
+#include <sys/select.h>
+#include <sys/types.h>
+#include <sys/time.h>
+#include <unistd.h>
+#include <termios.h>
+#include <errno.h>
+#include <ctype.h>
+#include <pthread.h>
+#include <math.h>
+#include "ik.h"
+
+typedef struct _servo_t servo_t;
+typedef struct _controller_t controller_t;
+typedef struct _robot_t robot_t;
+
+#ifndef M_PI
+# define M_PI           3.14159265358979323846
+#endif
+
+
+static joint_cfg_t joints[] = {
+	[1] = {
+		.alpha = 0,
+		.a = 0,
+		.d = 0,
+		.scale = (-11200 - +12000),
+		.min = -16000,
+		.max = +16000,
+	},
+
+	[2] = {
+		.scale = (+18000 - -18000),
+		.min = -24000,
+		.max = +24000,
+		.angle = -90,
+		.d = 130, // mm from center of rotation to center of arm
+		.a = 205, // mm along first arm
+	},
+
+	[3] = {
+		.scale = (-10500 - +11200),
+		.min = -18500,
+		.max = +18500,
+		.angle = +90,
+		.a = 0, // mm between center of rotation of elbow?
+	},
+
+	[4] = {
+		.scale = (+8000 - -8000),
+		.min = -12000,
+		.max = +20000,
+		.d = 225, // mm along second arm to center of wrist
+	},
+
+	[5] = { 
+		.scale = (-8000 - +8000),
+		.min = -10000,
+		.max = +10000,
+		.coupled = 9000.0 / -40000.0,
+	},
+
+	[6] = {
+		.scale = (-6500 - +6500),
+		.min = -14000,
+		.max = +14000,
+		.coupled = 1200.0 / -10000.0,
+	},
+};
+
+
+
+/** Return the current time in usec */
+unsigned long
+now_usec(void)
+{
+	struct timeval tp;
+	gettimeofday(&tp, NULL);
+	return tp.tv_sec * 1000000 + tp.tv_usec;
+}
+
+
+int verbose = 0;
+FILE * logfile;
+
+static double last_position[3];
+
+controller_t *
+controller_create(
+	const char * device
+);
+
+
+robot_t *
+robot_create(void);
+
+
+int
+robot_add_controller(
+	robot_t * const robot,
+	controller_t * const controller
+);
+
+
+
+struct _servo_t
+{
+	int command;
+	int position;
+	int speed;
+	int amps;
+	int transiting; // reset by the done flag
+};
+
+
+struct _controller_t
+{
+	robot_t * robot;
+	char * device;
+
+	int fd;
+	char buf[64];
+	size_t offset;
+
+	servo_t servo[2];
+};
+
+
+typedef struct {
+	controller_t * controller;
+	int channel;
+	double scale; // counts/radians
+	double length; // mm of bone
+	double shift; // mm of offset from previous joint
+} robot_axis_t;
+
+
+struct _robot_t
+{
+	int controller_count;
+	controller_t * controllers[16];
+	robot_axis_t servos[64];
+};
+
+
+void
+robot_add_mapping(
+	robot_t * const robot,
+	const int axis_num,
+	controller_t * const controller,
+	const int channel
+)
+{
+	// warn if this collides
+	robot_axis_t * const axis = &robot->servos[axis_num];
+	if (axis->controller)
+	{
+		if (axis->controller != controller
+		||  axis->channel != channel)
+		{
+			fprintf(stderr, "overriding axis %d\n", axis_num);
+		}
+	}
+
+	printf("%s: axis %d channel %d\n", controller->device, axis_num, channel);
+	axis->controller = controller;
+	axis->channel = channel;
+}
+
+
+
+int
+controller_parse(
+	controller_t * const controller,
+	const char * buf
+)
+{
+	// looking for lines of the form FOO=x:y
+	char type[32];
+	int x, y;
+
+	if (sscanf(buf, "%16[^=]=%d:%d", type, &x, &y) != 3)
+	{
+		// parse error; ignore it
+		return 0;
+	}
+
+	if (verbose > 2)
+		printf("%s: type='%s' x=%d y=%d\n", controller->device, type, x, y);
+
+	if (strcmp(type, "ACTR") == 0)
+	{
+		robot_add_mapping(controller->robot, x, controller, 1);
+		robot_add_mapping(controller->robot, y, controller, 2);
+		return 0;
+	}
+
+	if (strcmp(type, "C") == 0)
+	{
+		controller->servo[0].position = x;
+		controller->servo[1].position = y;
+		return 0;
+	}
+
+	if (strcmp(type, "A") == 0)
+	{
+		controller->servo[0].amps = x;
+		controller->servo[1].amps = y;
+		return 0;
+	}
+
+	if (strcmp(type, "S") == 0)
+	{
+		controller->servo[0].speed = x;
+		controller->servo[1].speed = y;
+		return 0;
+	}
+
+	if (strcmp(type, "DR") == 0)
+	{
+		// reset the transiting flag when we have reached
+		// the destination point.  it is only enabled
+		// by sending a command to move the axis.
+		// this seems broken.
+		if (x && controller->servo[0].transiting)
+		{
+			printf("x transit done\n");
+			controller->servo[0].transiting = 0;
+		}
+		if (y && controller->servo[1].transiting)
+		{
+			printf("y transit done\n");
+			controller->servo[1].transiting = 0;
+		}
+		return 0;
+	}
+
+	// unknown, but who cares
+	return 0;
+}
+
+
+int
+controller_send(
+	controller_t * const controller,
+	const char * const fmt,
+	...
+)
+{
+	if (controller->fd < 0)
+		return 0;
+
+	char buf[128];
+	va_list ap;
+	va_start(ap, fmt);
+	size_t len = vsnprintf(buf, sizeof(buf), fmt, ap);
+	va_end(ap);
+
+	if (len > sizeof(buf))
+		len = sizeof(buf);
+
+	//fprintf(stdout, "sending: '%s'\n", buf);
+
+	size_t offset = 0;
+	while (offset < len)
+	{
+		ssize_t rc = write(controller->fd, buf + offset, len - offset);
+		if (rc < 0)
+		{
+			if (errno == EINTR || errno == EWOULDBLOCK)
+				continue;
+
+			perror(controller->device);
+			if (errno == ENXIO)
+				continue;
+
+			close(controller->fd);
+			controller->fd = -1;
+			break;
+		}
+
+		offset += rc;
+	}
+
+	return 0;
+}
+
+
+static void
+print_escape(
+	const void * const ptr,
+	const size_t len
+)
+{
+	const char * const buf = ptr;
+
+	for (size_t i = 0 ; i < len ; i++)
+	{
+		char c = buf[i];
+		if (isprint(c))
+			putchar(c);
+		else
+		if (c == '\n')
+			printf("\\n");
+		else
+		if (c == '\r')
+			printf("\\r");
+		else
+			printf("\\%03o", (int) c);
+	}
+}
+
+
+int
+controller_poll(
+	controller_t * const controller
+)
+{
+	int rem = sizeof(controller->buf) - controller->offset - 1;
+	if (rem == 0)
+	{
+		// no new line?  we're toast.  throw away the line and retry
+		fprintf(stderr, "%s: overflow\n", controller->device);
+		controller->offset = 0;
+		rem = sizeof(controller->buf);
+	}
+
+	ssize_t rc = read(
+		controller->fd,
+		controller->buf + controller->offset,
+		rem
+	);
+
+	if (rc < 0)
+	{
+		if (errno == EINTR || errno == EWOULDBLOCK)
+			return 0;
+
+		perror(controller->device);
+		close(controller->fd);
+		controller->fd  = -1;
+		return -1;
+	}
+
+	// file closed.  we're done.
+	if (rc == 0)
+	{
+		fprintf(stderr, "%s: device closed\n", controller->device);
+		close(controller->fd);
+		controller->fd = -1;
+		return 0;
+	}
+
+	controller->offset += rc;
+	controller->buf[controller->offset] = '\0';
+
+	if (0)
+	{
+		printf("%s: '", controller->device);
+		print_escape(controller->buf, controller->offset);
+		printf("'\n");
+	}
+
+	// Look for a newline in the portion that we've read
+	while (1)
+	{
+		char * nl = index(controller->buf, '\r');
+		if (!nl)
+			nl = index(controller->buf, '\n');
+		if (!nl)
+			break;
+
+		*nl++ = '\0';
+		size_t nl_offset = nl - controller->buf;
+
+		//printf("%s: '%s' %zu/%zu\n", controller->device, controller->buf, nl_offset, controller->offset);
+
+		controller_parse(controller, controller->buf);
+
+		// shuffle things downwards, including the nul at the end
+		memmove(controller->buf, nl, controller->offset - nl_offset + 1);
+		controller->offset -= nl_offset;
+	}
+
+	return 0;
+}
+
+
+int
+controller_move(
+	controller_t * const controller,
+	const int channel,
+	const int pos,
+	const int absolute
+)
+{
+	servo_t * const servo = &controller->servo[channel];
+	servo->transiting = 1;
+
+	//printf("%s: channel %d pos %d\n", controller->device, channel, pos);
+	controller_send(controller,
+		"!P%s %d %d\r\n",
+		absolute ? "" : "R",
+		channel,
+		pos
+	);
+	return 0;
+}
+
+
+int
+controller_init(
+	controller_t * const controller
+)
+{
+	controller_send(controller, "~ACTR\r\n");
+	usleep(100000);
+	controller_send(controller, "^CLERD 1 0\r\n");
+	usleep(100000);
+	controller_send(controller, "^CLERD 2 0\r\n");
+	usleep(100000);
+	controller_send(controller, "!s 1 4000\r\n");
+	usleep(50000);
+	controller_send(controller, "!s 2 4000\r\n");
+	usleep(50000);
+	controller_send(controller, "!ac 1 40000\r\n");
+	usleep(50000);
+	controller_send(controller, "!dc 1 40000\r\n");
+	usleep(50000);
+	controller_send(controller, "!ac 2 40000\r\n");
+	usleep(50000);
+	controller_send(controller, "!dc 2 40000\r\n");
+	usleep(50000);
+	controller_send(controller, "# C\r\n");
+	usleep(100000);
+	controller_send(controller, "?C\r\n");
+	usleep(100000);
+	controller_send(controller, "?M\r\n");
+	usleep(100000);
+	controller_send(controller, "?A\r\n");
+	usleep(100000);
+	controller_send(controller, "?S\r\n");
+	usleep(100000);
+	//controller_send(controller, "?DR\r\n");
+	usleep(100000);
+	controller_send(controller, "# 50\r\n");
+	usleep(100000);
+
+	return 0;
+}
+
+
+int
+serial_open(
+	const char * dev
+)
+{
+	const int fd = open(dev, O_RDWR | O_NONBLOCK | O_NOCTTY, 0666);
+	if (fd < 0)
+		return -1;
+
+	// Disable modem control signals
+	struct termios attr;
+	tcgetattr(fd, &attr);
+	attr.c_cflag |= CLOCAL | CREAD;
+	tcsetattr(fd, TCSANOW, &attr);
+
+	return fd;
+}
+
+
+int
+controller_open(
+	controller_t * const controller
+)
+{
+	if (controller->fd >= 0)
+		return 1;
+
+	const int fd = serial_open(controller->device);
+	if (fd < 0)
+		return 0;
+
+	controller->fd = fd;
+
+	if (getenv("ROBOT_NO_INIT") == NULL)
+		controller_init(controller);
+	return 1;
+}
+
+
+
+controller_t *
+controller_create(
+	const char * device
+)
+{
+	controller_t * const controller = calloc(1, sizeof(*controller));
+	if (!controller)
+		return NULL;
+
+	controller->device = strdup(device);
+	controller->fd = -1;
+
+	controller_open(controller);
+
+	return controller;
+}
+
+servo_t *
+robot_servo(
+	robot_t * const robot,
+	int i
+)
+{
+	controller_t * const controller = robot->servos[i].controller;
+	if (!controller)
+		return NULL;
+
+	return &controller->servo[robot->servos[i].channel-1];
+}
+
+
+void
+robot_add(
+	robot_t * const robot,
+	controller_t * const controller
+)
+{
+	controller->robot = robot;
+	robot->controllers[robot->controller_count++] = controller;
+}
+
+
+robot_t *
+robot_create(void)
+{
+	robot_t * const robot = calloc(1, sizeof(*robot));
+	if (!robot)
+		return NULL;
+
+	robot->controller_count = 0;
+	return robot;
+}
+
+
+int
+robot_move(
+	robot_t * const robot,
+	const int axis_num,
+	const int pos,
+	int absolute
+)
+{
+	robot_axis_t * const axis = &robot->servos[axis_num];
+	if (!axis->controller)
+		return -1;
+
+	return controller_move(axis->controller, axis->channel, pos, absolute);
+}
+
+
+int
+robot_position(
+	robot_t * const robot,
+	const int axis_num
+)
+{
+	servo_t * const servo = robot_servo(robot, axis_num);
+	if (!servo)
+		return 0;
+
+	return servo->position;
+}
+
+
+int
+robot_ik_test(
+	robot_t * const robot,
+	double x,
+	double y,
+	double z
+)
+{
+	const double xyz[] = { x, y, z };
+	double theta[6];
+
+	// Adjust xyz by the desired wrist orientation
+	// to determine the position of the spherical joint
+	double a[] = { -1, 0, 0 };
+	double s[] = { 0, 0, -1 };
+	double n[] = { 0, 1, 0 };
+
+
+	if (!ik_first(joints, theta, xyz, 1, 1))
+	{
+		printf("unreachable\n");
+		return 0;
+	}
+
+	// We now have our first three joint angles
+	// compute the hand hangles
+	if (!ik_wrist(joints, theta, xyz, a, s, n, 1))
+	{
+		printf("unreachable wrist\n");
+		return 0;
+	}
+
+	double commands[6];
+
+	for (int i = 0 ; i < 6 ; i++)
+	{
+		const joint_cfg_t * const lim = &joints[i+1];
+		double command = theta[i] - lim->angle * M_PI/180;
+		command = command * lim->scale / M_PI;
+
+		// handle cross coupling
+		if (i > 0)
+			command += commands[i-1] * lim->coupled;
+
+		if(0)
+		printf("%d: %f => %.0f\n", i, theta[i] * 180 /M_PI, command);
+
+		if (command < lim->min || command > lim->max)
+		{
+			fprintf(stderr, "%d: %f => %d out of limits! (%f,%f,%f)\n",
+				i,
+				theta[i],
+				(int) command,
+				x,
+				y,
+				z
+			);
+			return 0;
+		}
+
+		commands[i] = command;
+	}
+
+	for (int i = 0 ; i < 6 ; i++)
+	{
+		if (0)
+		printf("command: %d %f => %d\n",
+			i,
+			theta[i] * 180/M_PI,
+			(int) commands[i]
+		);
+
+		robot_move(robot, i+1, commands[i], 1);
+	}
+
+	// Copy the xyz into the position vector for later iteration
+	for (int i = 0 ; i < 3 ; i++)
+		last_position[i] = xyz[i];
+
+	return 0;
+}
+
+
+int
+robot_test(
+	robot_t * const robot,
+	double x,
+	double y,
+	double z
+)
+{
+	robot_ik_test(robot, x, y, z);
+	return 0;
+}
+
+
+int
+robot_line(
+	robot_t * const robot,
+	const double x1,
+	const double y1,
+	const double x2,
+	const double y2,
+	const int steps,
+	const int delay
+)
+{
+	for (int i = 0 ; i < steps ; i++)
+	{
+		const double x = x1 + i * (x2 - x1) / steps;
+		const double y = y1 + i * (y2 - y1) / steps;
+
+		robot_ik_test(robot, x, y, 0);
+		usleep(delay);
+	}
+
+	return 0;
+}
+
+
+int
+robot_test_box(
+	robot_t * const robot
+)
+{
+	const double x1 = -200;
+	const double x2 =  200;
+
+	const double y1 =  200;
+	const double y2 =  300;
+
+	robot_line(robot, x1, y1, x1, y2, 250/5, 50000);
+	robot_line(robot, x1, y2, x2, y2, 500/5, 50000);
+	robot_line(robot, x2, y2, x2, y1, 250/5, 50000);
+	robot_line(robot, x2, y1, x1, y1, 500/5, 50000);
+
+	printf("box done!\n");
+
+	return 0;
+}
+
+
+int
+robot_linear(
+	robot_t * const robot,
+	const double x2,
+	const double y2,
+	const double z2,
+	const int steps,
+	const int delay
+)
+{
+	const double x1 = last_position[0];
+	const double y1 = last_position[1];
+	const double z1 = last_position[2];
+
+	if (x1 == 0 && y1 == 0 && y2 == 0)
+		return robot_test(robot, x2, y2, z2);
+
+	for (int i = 0 ; i < steps ; i++)
+	{
+		const double x = x1 + i * (x2 - x1) / steps;
+		const double y = y1 + i * (y2 - y1) / steps;
+		const double z = z1 + i * (z2 - z1) / steps;
+
+		robot_ik_test(robot, x, y, z);
+		usleep(delay);
+	}
+
+	return 0;
+}
+
+
+static void
+robot_print_counts(
+	const robot_t * const robot,
+	FILE * const file
+)
+{
+	for (int i = 0 ; i < 6; i++)
+	{
+		const robot_axis_t * const axis = &robot->servos[i+1];
+		if (!axis->controller)
+		{
+			fprintf(file, " ?????");
+			continue;
+		}
+
+		const servo_t * const servo = &axis->controller->servo[axis->channel - 1];
+		fprintf(file, " %+5d",
+			servo->position
+		);
+	}
+
+	fprintf(file, "\n");
+}
+
+
+static void
+command_parse(
+	robot_t * const robot,
+	const char * const line
+)
+{
+	if (line[0] == '?')
+	{
+		robot_print_counts(robot, stdout);
+		return;
+	}
+
+	if (line[0] == 's')
+	{
+		// sample the current positions and command them
+		printf("new cmd:");
+		for (int i = 0 ; i < 6; i++)
+		{
+			robot_axis_t * const axis = &robot->servos[i+1];
+			if (!axis->controller)
+			{
+				printf(" ?????");
+				continue;
+			}
+
+			servo_t * const servo = &axis->controller->servo[axis->channel - 1];
+			printf(" %+5d",
+				servo->position
+			);
+
+			robot_move(robot, i+1, servo->position, 1);
+		}
+
+		printf("\n");
+		return;
+	}
+
+	if (line[0] == 'h')
+	{
+		// go home
+		printf("HOME\n");
+		for (int i = 0 ; i < 6; i++)
+			robot_move(robot, i+1, 0, 1);
+
+		return;
+	}
+
+	int x = 0;
+	int y = 0;
+	int z = 0;
+	char cmd[32];
+
+	int rc = sscanf(line, "%16[^ ] %d %d %d", cmd, &x, &y, &z);
+	if (rc != 3 && rc != 4)
+	{
+		printf("?\n");
+		return;
+	}
+
+	if (strcmp(cmd, "v") == 0)
+	{
+		verbose = x;
+		return;
+	}
+
+	if (strcmp(cmd, "p") == 0)
+	{
+		robot_move(robot, x, y, 1);
+		return;
+	}
+
+	if (strcmp(cmd, "r") == 0)
+	{
+		robot_test(robot, x, y, 0);
+		return;
+	}
+
+	if (strcmp(cmd, "x") == 0)
+	{
+		robot_test(robot, x, y, z);
+		return;
+	}
+
+	if (strcmp(cmd, "l") == 0)
+	{
+		robot_linear(robot, x, y, z, 20, 50000);
+		return;
+	}
+
+	if (strcmp(cmd, "box") == 0)
+	{
+		robot_test_box(robot);
+		return;
+	}
+
+
+	printf("?\n");
+}
+
+
+static int
+command_handler(
+	robot_t * const robot,
+	int fd
+)
+{
+	static char line[128];
+	static size_t offset;
+
+	ssize_t rc = read(fd, line + offset, sizeof(line) - offset - 1);
+
+	// error or end of file, stop the processing
+	if (rc <= 0)
+		return -1;
+
+	// mark the end of the buffer thus far
+	offset += rc;
+	line[offset] = '\0';
+
+	int count = 0;
+
+	while (1)
+	{
+		char * eol = strchr(line, '\n');
+		if (!eol)
+			return count;
+		*eol++ = '\0';
+
+		// We have a full line.  Process it
+		command_parse(robot, line);
+		size_t new_offset = offset - (eol - line);
+		memmove(line, eol, new_offset);
+		offset = new_offset;
+		count++;
+	}
+}
+
+
+int
+robot_poll(
+	robot_t * const robot
+)
+{
+	int max_fd = 0;
+	fd_set fds;
+	FD_ZERO(&fds);
+
+	struct timeval timeout = { 1, 0 };
+
+	for (int i = 0 ; i < robot->controller_count ; i++)
+	{
+		controller_t * const controller = robot->controllers[i];
+		if (controller->fd < 0)
+		{
+			// should poll to see if it came back
+			continue;
+		}
+
+		FD_SET(controller->fd, &fds);
+		if (max_fd < controller->fd)
+			max_fd = controller->fd;
+	}
+
+	int rc = select(max_fd+1, &fds, NULL, NULL, &timeout);
+	if (rc < 0)
+		return -1;
+
+	if (rc == 0)
+		return 0;
+
+	for (int i = 0 ; i < robot->controller_count ; i++)
+	{
+		controller_t * const controller = robot->controllers[i];
+		if (controller->fd < 0)
+			continue;
+
+		if (!FD_ISSET(controller->fd, &fds))
+			continue;
+
+		if (controller_poll(controller) < 0)
+			return -1;
+	}
+
+/*
+	const servo_t * s1 = robot_servo(robot, 1);
+	const servo_t * s2 = robot_servo(robot, 2);
+	const servo_t * s3 = robot_servo(robot, 3);
+
+	if (!s1 || !s2 || !s3)
+		return 0;
+
+	fprintf(logfile, "%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
+		s1->position,
+		s1->amps,
+		s1->speed,
+		s2->position,
+		s2->amps,
+		s2->speed,
+		s3->position,
+		s3->amps,
+		s3->speed
+	);
+	fflush(logfile);
+*/
+
+	return 1;
+}
+
+
+
+typedef struct
+{
+	robot_t * robot;
+	int fd;
+	unsigned axis;
+	unsigned long last_send;
+} handheld_t;
+
+static const char handheld_axis_set_map[] = " XYZABR";
+static const char handheld_axis_unset_map[] = " xyzabr";
+
+
+void
+handheld_axis(
+	handheld_t * const hh,
+	const unsigned axis
+)
+{
+	if (axis == 0)
+	{
+		write(
+			hh->fd,
+			handheld_axis_unset_map + 1,
+			sizeof(handheld_axis_unset_map) - 1
+		);
+		hh->axis = 0;
+		return;
+	}
+
+	if (axis > 6 || axis == hh->axis)
+		return;
+
+	write(hh->fd, &handheld_axis_set_map[axis], 1);
+	write(hh->fd, &handheld_axis_unset_map[hh->axis], 1);
+	hh->axis = axis;
+}
+
+
+handheld_t *
+handheld_create(
+	const char * const dev,
+	robot_t * const robot
+)
+{
+	int fd = serial_open(dev);
+	if (fd < 0)
+		return NULL;
+
+	handheld_t * const handheld = calloc(1, sizeof(*handheld));
+	if (!handheld)
+	{
+		close(fd);
+		return NULL;
+	}
+
+	*handheld = (handheld_t) {
+		.robot		= robot,
+		.fd		= fd,
+		.axis		= 0,
+	};
+
+	handheld_axis(handheld, 0);
+
+	return handheld;
+}
+
+
+int
+handheld_parse(
+	handheld_t * const hh,
+	const char * const line
+)
+{
+	unsigned bitmask;
+	char jog_dir;
+	int jog;
+	int rate;
+
+	int rc = sscanf(line, "%04x,%c%02x,%03x",
+		&bitmask,
+		&jog_dir,
+		&jog,
+		&rate
+	);
+	if (rc != 4)
+		return -1;
+	if (jog_dir == '-')
+		jog = -jog;
+	else
+	if (jog_dir == '+')
+		jog = jog;
+	else
+		return -1;
+
+	if (0)
+	printf("%04x %d %d\n",
+		bitmask,
+		jog,
+		rate
+	);
+
+	unsigned new_axis = 0;
+	for (unsigned axis = 0 ; axis < 6 ; axis++)
+	{
+		unsigned mask = 1 << axis;
+		if (axis == 5)
+			mask <<= 1;
+
+		if (bitmask & mask)
+			new_axis = axis + 1;
+	}
+
+	if ((bitmask & 0x100) && (bitmask & 0x080))
+	{
+		robot_move(hh->robot, 1, 0, 1);
+		robot_move(hh->robot, 2, 0, 1);
+		robot_move(hh->robot, 3, 0, 1);
+		robot_move(hh->robot, 4, 0, 1);
+		robot_move(hh->robot, 5, 0, 1);
+		robot_move(hh->robot, 6, 0, 1);
+	}
+
+	if (new_axis)
+		handheld_axis(hh, new_axis);
+
+	if (jog && hh->axis)
+	{
+		// limit to 20 updates per second
+		unsigned long now = now_usec();
+		if (now - hh->last_send > 75000)
+		{
+			int delta = (jog * rate)/32;
+			printf("%d: %d * %+d = %d\n",
+				hh->axis,
+				rate,
+				jog,
+				delta
+			);
+
+			robot_move(hh->robot, hh->axis, delta, 0);
+			hh->last_send = now;
+		}
+	}
+
+	return 0;
+}
+
+
+void *
+handheld_poll_thread(
+	void * hh_ptr
+)
+{
+	handheld_t * const hh = hh_ptr;
+	char line[64];
+	size_t offset = 0;
+
+	while (1)
+	{
+		ssize_t rc = read(hh->fd, line + offset, sizeof(line) - offset - 1);
+		if (rc < 0)
+		{
+			if (errno == EINTR || errno == EAGAIN)
+				continue;
+			perror("read from handheld");
+			break;
+		}
+		if (rc == 0)
+			break;
+
+		// nul terminate what we have read so far
+		offset += rc;
+		line[offset + 1] = '\0';
+
+		char * eol = index(line, '\n');
+		if (!eol)
+		{
+			// too much input data?  skip this line
+			if (offset == sizeof(line) - 1)
+			{
+				fprintf(stderr, "%s: long line %zu: '%s'\n",
+					__func__, offset, line);
+				offset = 0;
+			}
+
+			continue;
+		}
+
+		*eol = '\0';
+
+		// trim any \r if there are some
+		char * cr = index(line, '\r');
+		if (cr)
+			*cr = '\0';
+
+		if (handheld_parse(hh, line) < 0)
+		{
+			fprintf(stderr, "Parse error: '%s'\n", line);
+		}
+
+		size_t eol_offset = eol - line + 1;
+		memmove(line, eol, offset - eol_offset);
+		offset -= eol_offset;
+	}
+
+	fprintf(stderr, "!!!!! handheld closed!\n");
+	return NULL;
+}
+
+
+static void *
+robot_poll_thread(
+	void * robot_ptr
+)
+{
+	robot_t * const robot = robot_ptr;
+
+	while (1)
+		robot_poll(robot);
+
+	return NULL;
+}
+
+
+static int
+command_loop(
+	robot_t * const robot,
+	int cmd_fd
+)
+{
+	while (1)
+	{
+		if (verbose)
+			robot_print_counts(robot, stdout);
+
+		int max_fd = cmd_fd;
+		fd_set fds;
+		FD_ZERO(&fds);
+		FD_SET(cmd_fd, &fds);
+
+		struct timeval timeout = { 1, 0 };
+		int rc = select(max_fd+1, &fds, NULL, NULL, &timeout);
+		if (rc < 0)
+			return -1;
+
+		if (rc == 0)
+			continue;
+
+		command_handler(robot, cmd_fd);
+	}
+}
+
+
+int
+main(
+	int argc,
+	char ** argv
+)
+{
+	robot_t * const robot = robot_create();
+
+	robot->servos[1].scale = (12000 - -11200) / M_PI;
+	robot->servos[1].length = 300;
+
+	robot->servos[2].scale = (16500 - -18000) / M_PI;
+	robot->servos[2].length = 210;
+
+	robot->servos[3].scale = -(10500 - -11200) / M_PI;
+	robot->servos[3].length = 260;
+
+	logfile = fopen("/tmp/robot.log", "w");
+	fprintf(logfile, "p1,a1,s1,p2,a2,s2,p3,a3,s3\n");
+
+	for (int i = 1 ; i < argc ; i++)
+	{
+		const char * const device = argv[i];
+		controller_t * const controller = controller_create(device);
+		if (!controller)
+		{
+			fprintf(stderr, "%s: failed\n", device);
+			return -1;
+		}
+
+		robot_add(robot, controller);
+	}
+
+	pthread_t poll_id;
+	if (pthread_create(&poll_id, NULL, robot_poll_thread, robot) < 0)
+	{
+		perror("pthread");
+		return -1;
+	}
+
+#if 0
+	handheld_t * handheld = handheld_create(argv[1], robot);
+	if (handheld)
+	{
+		pthread_t handheld_id;
+		if (pthread_create(&handheld_id, NULL, handheld_poll_thread, handheld) < 0)
+		{
+			perror("pthread");
+			return -1;
+		}
+	} else {
+		fprintf(stderr, "handheld not found!\n");
+	}
+#endif
+
+#if 0
+	while (1)
+		robot_poll(robot);
+#endif
+
+	command_loop(robot, STDIN_FILENO);
+	return 0;
+}
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.