Commits

Trammell Hudson committed 11251c6

configure and read axes

Comments (0)

Files changed (2)

+CFLAGS = \
+	-std=c99 \
+	-g \
+	-O3 \
+	-W \
+	-Wall \
+
+all: monitor
+
+servo: servo.o
+	$(CC) -o $@ $^ $(LDFLAGS)
+
+clean:
+	$(RM) *.o *.a core a.out
+
+/** \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 <unistd.h>
+#include <termios.h>
+#include <errno.h>
+
+typedef struct _servo_t servo_t;
+typedef struct _controller_t controller_t;
+typedef struct _robot_t robot_t;
+
+
+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;
+};
+
+
+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;
+} 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);
+		}
+	}
+
+	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;
+	}
+
+	//printf("type='%s' x=%d y=%d\n", type, x, y);
+
+	if (strcmp(type, "ACTR") == 0)
+	{
+		robot_add_mapping(controller->robot, x, controller, 0);
+		robot_add_mapping(controller->robot, y, controller, 1);
+		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;
+	}
+
+	// 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);
+
+	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);
+			close(controller->fd);
+			controller->fd = -1;
+			break;
+		}
+
+		offset += rc;
+	}
+
+	return 0;
+}
+
+
+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';
+
+	// Look for a newline in the portion that we've read
+	while (1)
+	{
+		char * nl = index(controller->buf, '\r');
+		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
+		memmove(controller->buf, nl, controller->offset - nl_offset);
+		controller->offset -= nl_offset;
+	}
+
+	return 0;
+}
+
+
+int
+controller_init(
+	controller_t * const controller
+)
+{
+	controller_send(controller, "# C\r\n");
+	controller_send(controller, "~ACTR\r\n");
+	controller_send(controller, "?P\r\n");
+	controller_send(controller, "?M\r\n");
+	controller_send(controller, "?A\r\n");
+	controller_send(controller, "?S\r\n");
+	controller_send(controller, "# 50\r\n");
+
+	return 0;
+}
+	
+
+int
+controller_open(
+	controller_t * const controller
+)
+{
+	if (controller->fd >= 0)
+		return 1;
+
+	int fd = open(controller->device, O_RDWR | O_NONBLOCK | O_NOCTTY, 0666);
+	if (fd < 0)
+		return 0;
+
+	// Disable modem control signals
+	struct termios attr;
+	tcgetattr(fd, &attr);
+	attr.c_cflag |= CLOCAL | CREAD;
+	tcsetattr(fd, TCSANOW, &attr);
+
+	controller->fd = fd;
+
+	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;
+}
+
+
+int
+robot_poll(
+	robot_t * const robot
+)
+{
+	int max_fd = 0;
+	fd_set fds;
+	FD_ZERO(&fds);
+	struct timeval timeout = { 0, 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;
+	}
+
+	return 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
+main(
+	int argc,
+	char ** argv
+)
+{
+	robot_t * const robot = robot_create();
+
+	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);
+	}
+
+	while (1)
+	{
+		int rc = robot_poll(robot);
+		if (rc < 0)
+			break;
+		if (rc == 0)
+			continue;
+
+		for (int i = 0 ; i < 6; i++)
+		{
+			robot_axis_t * const axis = &robot->servos[i+1];
+			if (!axis->controller)
+			{
+				printf(" ?????");
+			} else {
+				printf(" %+5d", axis->controller->servo[axis->channel].position);
+			}
+		}
+
+		printf("\n");
+	}
+}