Commits

Trammell Hudson committed 0d7604c

move command processing into same thread as serial ports

  • Participants
  • Parent commits f948caf

Comments (0)

Files changed (1)

 	return &controller->servo[robot->servos[i].channel-1];
 }
 
-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;
-	}
-
-	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;
-}
-
 
 void
 robot_add(
 }
 
 
-static void *
-robot_poll_thread(
-	void * const robot_ptr
-)
-{
-	robot_t * const robot = robot_ptr;
-
-	while (1)
-	{
-		int rc = robot_poll(robot);
-		if (rc < 0)
-			break;
-		if (rc == 0)
-			continue;
-	}
-
-	fprintf(stderr, "robot poll failed.  exiting\n");
-	exit(EXIT_FAILURE);
-	return NULL;
-}
-
-
 int
 robot_ik_test(
 	robot_t * const robot,
 }
 
 
+
+
+static void
+command_parse(
+	robot_t * const robot,
+	const char * const line
+)
+{
+	if (line[0] == '?')
+	{
+		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
+			);
+		}
+
+		printf("\n");
+		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 = !verbose;
+		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
+)
+{
+	const int cmd_fd = STDIN_FILENO;
+	int max_fd = 0;
+	fd_set fds;
+	FD_ZERO(&fds);
+	FD_SET(cmd_fd, &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;
+
+	if (FD_ISSET(cmd_fd, &fds))
+		command_handler(robot, cmd_fd);
+
+	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;
 	logfile = fopen("/tmp/robot.log", "w");
 	fprintf(logfile, "p1,a1,s1,p2,a2,s2,p3,a3,s3\n");
 
-	for (int i = 2 ; i < argc ; i++)
+	for (int i = 1 ; i < argc ; i++)
 	{
 		const char * const device = argv[i];
 		controller_t * const controller = controller_create(device);
 		robot_add(robot, controller);
 	}
 
+#if 0
 	pthread_t poll_id;
 	if (pthread_create(&poll_id, NULL, robot_poll_thread, robot) < 0)
 	{
 	} else {
 		fprintf(stderr, "handheld not found!\n");
 	}
+#endif
 
 
-	char line[128];
-
 	while (1)
-	{
-		fgets(line, sizeof(line), stdin);
-		if (line[0] == '?')
-		{
-			for (int i = 0 ; i < 6; i++)
-			{
-				robot_axis_t * const axis = &robot->servos[i+1];
-				if (!axis->controller)
-				{
-					printf(" ?????");
-					continue;
-				}
+		robot_poll(robot);
 
-				servo_t * const servo = &axis->controller->servo[axis->channel - 1];
-				printf(" %+5d",
-					servo->position
-				);
-			}
-
-			printf("\n");
-
-			continue;
-		}
-
-		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");
-
-			continue;
-		}
-
-		if (line[0] == 'h')
-		{
-			// go home
-			printf("HOME\n");
-			for (int i = 0 ; i < 6; i++)
-				robot_move(robot, i+1, 0, 1);
-
-			continue;
-		}
-
-		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");
-			continue;
-		}
-
-		if (strcmp(cmd, "v") == 0)
-		{
-			verbose = !verbose;
-			continue;
-		}
-
-		if (strcmp(cmd, "p") == 0)
-		{
-			robot_move(robot, x, y, 1);
-			continue;
-		}
-
-		if (strcmp(cmd, "r") == 0)
-		{
-			robot_test(robot, x, y, 0);
-			continue;
-		}
-
-		if (strcmp(cmd, "x") == 0)
-		{
-			robot_test(robot, x, y, z);
-			continue;
-		}
-
-		if (strcmp(cmd, "l") == 0)
-		{
-			robot_linear(robot, x, y, z, 20, 50000);
-			continue;
-		}
-
-		if (strcmp(cmd, "box") == 0)
-		{
-			robot_test_box(robot);
-			continue;
-		}
-
-
-		printf("?\n");
-	}
 }