1. Trammell Hudson
  2. puma

Commits

Trammell Hudson  committed e2de0d3

factored command loop into separate function

  • Participants
  • Parent commits 0644cea
  • Branches default
  • Tags before-usb-split

Comments (0)

Files changed (1)

File servo.c

View file
  • Ignore whitespace
 
 static joint_cfg_t joints[] = {
 	[1] = {
+		.alpha = 0,
+		.a = 0,
+		.d = 0,
 		.scale = (-11200 - +12000),
 		.min = -16000,
 		.max = +16000,
 		return 0;
 	}
 
-	if (verbose)
+	if (verbose > 2)
 		printf("%s: type='%s' x=%d y=%d\n", controller->device, type, x, y);
 
 	if (strcmp(type, "ACTR") == 0)
 				continue;
 
 			perror(controller->device);
+			if (errno == ENXIO)
+				continue;
+
 			close(controller->fd);
 			controller->fd = -1;
 			break;
 }
 
 
+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
 {
 	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");
+		robot_print_counts(robot, stdout);
 		return;
 	}
 
 
 	if (strcmp(cmd, "v") == 0)
 	{
-		verbose = !verbose;
+		verbose = x;
 		return;
 	}
 
 	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 };
+	struct timeval timeout = { 1, 0 };
 
 	for (int i = 0 ; i < robot->controller_count ; i++)
 	{
 	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];
 }
 
 
+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,
 		robot_add(robot, controller);
 	}
 
-#if 0
 	pthread_t poll_id;
 	if (pthread_create(&poll_id, NULL, robot_poll_thread, robot) < 0)
 	{
 		return -1;
 	}
 
+#if 0
 	handheld_t * handheld = handheld_create(argv[1], robot);
 	if (handheld)
 	{
 	}
 #endif
 
-
+#if 0
 	while (1)
 		robot_poll(robot);
+#endif
 
+	command_loop(robot, STDIN_FILENO);
+	return 0;
 }