Commits

Trammell Hudson committed 9632afd

Add handheld controller thread

  • Participants
  • Parent commits 04ea431

Comments (0)

Files changed (1)

 
 	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(
 	if (controller->fd >= 0)
 		return 1;
 
-	int fd = open(controller->device, O_RDWR | O_NONBLOCK | O_NOCTTY, 0666);
+	const int fd = serial_open(controller->device);
 	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);
 }
 
 
+typedef struct
+{
+	robot_t * robot;
+	int fd;
+	unsigned axis;
+} 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 (new_axis)
+		handheld_axis(hh, new_axis);
+
+	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;
+}
+
+
 int
 main(
 	int argc,
 		return -1;
 	}
 
+	handheld_t * handheld = handheld_create("/dev/tty.usbmodem82", 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");
+	}
+
 
 	char line[128];