Commits

Trammell Hudson committed c117509

separate thread for commands

Comments (0)

Files changed (1)

 #include <termios.h>
 #include <errno.h>
 #include <ctype.h>
+#include <pthread.h>
 
 typedef struct _servo_t servo_t;
 typedef struct _controller_t controller_t;
 		}
 	}
 
+	printf("%s: axis %d channel %d\n", controller->device, axis_num, channel);
 	axis->controller = controller;
 	axis->channel = channel;
 }
 
 	if (strcmp(type, "ACTR") == 0)
 	{
-		robot_add_mapping(controller->robot, x, controller, 0);
-		robot_add_mapping(controller->robot, y, controller, 1);
+		robot_add_mapping(controller->robot, x, controller, 1);
+		robot_add_mapping(controller->robot, y, controller, 2);
 		return 0;
 	}
 
 	if (len > sizeof(buf))
 		len = sizeof(buf);
 
+	//fprintf(stdout, "sending: '%s'\n", buf);
+
 	size_t offset = 0;
 	while (offset < len)
 	{
 
 
 int
+controller_move(
+	controller_t * const controller,
+	const int channel,
+	const int pos
+)
+{
+	printf("%s: channel %d pos %d\n", controller->device, channel, pos);
+	controller_send(controller, "!p %d %d\r\n", channel, pos);
+	return 0;
+}
+
+
+int
 controller_init(
 	controller_t * const controller
 )
 }
 
 
+int
+robot_move(
+	robot_t * const robot,
+	const int axis_num,
+	const int pos
+)
+{
+	robot_axis_t * const axis = &robot->servos[axis_num];
+	if (!axis->controller)
+		return -1;
+
+	return controller_move(axis->controller, axis->channel, pos);
+}
+	
+
+
+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
 main(
 		robot_add(robot, controller);
 	}
 
+	pthread_t poll_id;
+	if (pthread_create(&poll_id, NULL, robot_poll_thread, robot) < 0)
+	{
+		perror("pthread");
+		return -1;
+	}
+
+
+	char line[128];
+
 	while (1)
 	{
-		int rc = robot_poll(robot);
-		if (rc < 0)
-			break;
-		if (rc == 0)
-			continue;
+		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;
+				}
 
-		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];
+				printf(" %+5d@%+4d",
+					servo->position,
+					servo->speed
+				);
 			}
 
-			servo_t * const servo = &axis->controller->servo[axis->channel];
-			printf(" %+5d@%+4d",
-				servo->position,
-				servo->speed
-			);
+			printf("\n");
+
+			continue;
 		}
 
-		printf("\n");
+		int axis = 0;
+		int pos = 0;
+		if (sscanf(line, "%d %d", &axis, &pos) == 2)
+		{
+			robot_move(robot, axis, pos);
+			continue;
+		}
+
+		printf("?\n");
 	}
 }