Commits

Trammell Hudson committed d0400f7

monitoring works

Comments (0)

Files changed (2)

 	-W \
 	-Wall \
 
-all: monitor
+all: servo
 
 servo: servo.o
 	$(CC) -o $@ $^ $(LDFLAGS)
 
+servo.o: servo.c
+
 clean:
 	$(RM) *.o *.a core a.out
 
 #include <unistd.h>
 #include <termios.h>
 #include <errno.h>
+#include <ctype.h>
 
 typedef struct _servo_t servo_t;
 typedef struct _controller_t controller_t;
 }
 
 
+static void
+print_escape(
+	const void * const ptr,
+	const size_t len
+)
+{
+	const char * const buf = ptr;
+
+	for (size_t i = 0 ; i < len ; i++)
+	{
+		char c = buf[i];
+		if (isprint(c))
+			putchar(c);
+		else
+		if (c == '\n')
+			printf("\\n");
+		else
+		if (c == '\r')
+			printf("\\r");
+		else
+			printf("\\%03o", (int) c);
+	}
+}
+
+
 int
 controller_poll(
 	controller_t * const controller
 	controller->offset += rc;
 	controller->buf[controller->offset] = '\0';
 
+	if (0)
+	{
+		printf("%s: '", controller->device);
+		print_escape(controller->buf, controller->offset);
+		printf("'\n");
+	}
+
 	// Look for a newline in the portion that we've read
 	while (1)
 	{
 		*nl++ = '\0';
 		size_t nl_offset = nl - controller->buf;
 
-		printf("%s: '%s' %zu/%zu\n", controller->device, controller->buf, nl_offset, controller->offset);
+		//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);
+		// shuffle things downwards, including the nul at the end
+		memmove(controller->buf, nl, controller->offset - nl_offset + 1);
 		controller->offset -= nl_offset;
 	}
 
 {
 	controller_send(controller, "# C\r\n");
 	controller_send(controller, "~ACTR\r\n");
-	controller_send(controller, "?P\r\n");
+	controller_send(controller, "?C\r\n");
 	controller_send(controller, "?M\r\n");
 	controller_send(controller, "?A\r\n");
 	controller_send(controller, "?S\r\n");
 			robot_axis_t * const axis = &robot->servos[i+1];
 			if (!axis->controller)
 			{
-				printf(" ?????");
-			} else {
-				printf(" %+5d", axis->controller->servo[axis->channel].position);
+				printf(" ?????@????");
+				continue;
 			}
+
+			servo_t * const servo = &axis->controller->servo[axis->channel];
+			printf(" %+5d@%+4d",
+				servo->position,
+				servo->speed
+			);
 		}
 
 		printf("\n");