 ** MXXX
 **	XXX = 100; reset
 **	XXX = 101; print free memory
+**	XXX = 102; report current servo desination positions
 **	XXX = 112; emergency stop
 */
 
-bool parseCommandGServo(char **buf, boolean speedControl, boolean wait) {
-	char *endptr;
-	uint8_t pos, i, len;
+bool parseCommandGServo(char **buf, bool speedControl, bool wait) {
+	char *endptr, servoShortName;
+	uint8_t pos;
 	int servoSpeedLocal = servoSpeed;
+	void (*moveServoFuncPtr)(uint8_t, uint8_t, bool, bool) = NULL;
 
 	if (!speedControl)
 		servoSpeedLocal = 0;
 
-	wifi.Send(chlID,F("parseCommandGServo"));
-	wifi.Send(chlID,*buf);
-	wifi.Send(chlID,String(*buf[0]));
-
-	if (*buf[0] == '\0')
+	if (*buf[0] == '\0' || *buf[0] == '\n' || *buf[0] == '\r')
 		return false;
 
-	switch(*buf[0]) {
+	// should this display an error?
+	if (servoMoveCount == MAX_CMD_SERVO_MOVES) {
+		return false;
+	}
+
+	servoShortName = *buf[0];
+	switch(servoShortName) {
 		case 'L': case 'l':
-			pos = strtoul(buf[1], &endptr, 10);
-			if (buf[1] != endptr) {
-				len = strlen(*buf);
-				for (i = 0; i < len; ++i) {
-					if (*buf[i] != ' ' && *buf[i] != '\t' && *buf[i] != '\0')
-						break;
-				}
-				if (i == len) {
-					moveServoL(defaultServoPos, servoSpeedLocal, wait, true);
-				} else {
-					moveServoL(defaultServoPos, servoSpeedLocal, false, false);
-				}
-			} else {
-				wifi.Send(chlID,F("ERROR: invalid servoL position"));
-			}
-			return true;
+			moveServoFuncPtr = &moveServoL;
+			break;
 		case 'R': case 'r':
-			pos = strtoul(buf[1], &endptr, 10);
-			if (buf[1] != endptr) {
-				len = strlen(*buf);
-				for (i = 0; i < len; ++i) {
-					if (*buf[i] != ' ' && *buf[i] != '\t' && *buf[i] != '\0')
-						break;
-				}
-				if (i == len) {
-					moveServoR(defaultServoPos, servoSpeedLocal, wait, true);
-				} else {
-					moveServoR(defaultServoPos, servoSpeedLocal, false, false);
-				}
-			} else {
-				wifi.Send(chlID,F("ERROR: invalid servoR position"));
-			}
-			return true;
+			moveServoFuncPtr = &moveServoR;
+			break;
 		case 'O': case 'o':
-			pos = strtoul(buf[1], &endptr, 10);
-			if (buf[1] != endptr) {
-				len = strlen(*buf);
-				for (i = 0; i < len; ++i) {
-					if (*buf[i] != ' ' && *buf[i] != '\t' && *buf[i] != '\0')
-						break;
-				}
-				if (i == len) {
-					moveServoRot(defaultServoPos, servoSpeedLocal, wait, true);
-				} else {
-					moveServoRot(defaultServoPos, servoSpeedLocal, false, false);
-				}
-			} else {
-				wifi.Send(chlID,F("ERROR: invalid servoRot position"));
-			}
-			return true;
+			moveServoFuncPtr = &moveServoRot;
+			break;
 		case 'T': case 't':
-			pos = strtoul(buf[1], &endptr, 10);
-			if (buf[1] != endptr) {
-				len = strlen(*buf);
-				for (i = 0; i < len; ++i) {
-					if (*buf[i] != ' ' && *buf[i] != '\t' && *buf[i] != '\0')
-						break;
-				}
-				if (i == len) {
-					moveServoHRot(defaultServoPos, servoSpeedLocal, wait, true);
-				} else {
-					moveServoHRot(defaultServoPos, servoSpeedLocal, false, false);
-				}
-			} else {
-				wifi.Send(chlID,F("ERROR: invalid servoHRot position"));
-			}
-			return true;
+			moveServoFuncPtr = &moveServoHRot;
+			break;
 		case 'H': case 'h':
-			pos = strtoul(buf[1], &endptr, 10);
-			if (buf[1] != endptr) {
-				len = strlen(*buf);
-				for (i = 0; i < len; ++i) {
-					if (*buf[i] != ' ' && *buf[i] != '\t' && *buf[i] != '\0')
-						break;
-				}
-				if (i == len) {
-					moveServoH(defaultServoPos, servoSpeedLocal, wait, true);
-				} else {
-					moveServoH(defaultServoPos, servoSpeedLocal, false, false);
-				}
-			} else {
-				wifi.Send(chlID,F("ERROR: invalid servoH position"));
-			}
-			return true;
-		default:
-			wifi.Send(chlID,F("ERROR: invalid servo identifier"));
-			return false;
+			moveServoFuncPtr = &moveServoH;
+			break;
 	};
+
+	if (moveServoFuncPtr != NULL) {
+		(*buf)++;
+		pos = strtoul(*buf, &endptr, 10);
+		if (*buf != endptr && pos >= 0 && pos <= 180) {
+			for (; **buf == ' ' || **buf == '\t'; (*buf)++);
+			servoMoves[servoMoveCount].moveServoFuncPtr = moveServoFuncPtr;
+			servoMoves[servoMoveCount].pos = pos;
+			servoMoves[servoMoveCount].servoSpeed = servoSpeedLocal;
+			servoMoveCount++;
+		} else {
+			servoMoveError = true;
+			if (*buf == endptr) {
+				Serial.print(F("ERROR: missing servo position for servo "));
+				Serial.println(servoShortName);
+			} else {
+				Serial.print(F("ERROR: missing servo position for servo "));
+				Serial.print(servoShortName);
+				Serial.print(F(": "));
+				Serial.println(pos, DEC);
+			}
+		}
+		*buf = endptr;
+		return true;
+	} else {
+		servoMoveError = true;
+		Serial.print(F("ERROR: invalid servo identifier: "));
+		Serial.println(servoShortName);
+	}
 	return false;
 }
 
 bool processCommand(char *cmd) {
 	char *endptr, *buf = cmd;
-	uint8_t len = strlen(buf);
-	uint8_t code;
+	uint8_t i, len = strlen(buf);
+	int code;
 	bool speedControl = true;
 	bool wait;
 
 	if (len == 0) return false;
-
-	wifi.Send(chlID,F("processCommand"));
-	wifi.Send(chlID,cmd);
 
 	switch (buf[0]) {
 		case 'G': case 'g':
 			code = strtoul(&buf[1], &endptr, 10);
 			if (endptr == &buf[1]) {
-				wifi.Send(chlID,F("ERROR: missing G command code"));
+				Serial.println(F("ERROR: missing G command code"));
 				return false;
 			}
 			if (code == 0) {

	servoH.detach();
}

void setupESP8266(void) {
	while (!wifi.begin());
	wifi.Initialize(AP_STA, SSID, PASSWORD);
	wifi.confMux(1);
	wifi.confServer(1, SERVER_PORT);
}

void setup() {
	// setup MicroView
	setupMicroView();



#include <VarSpeedServo.h>

// MicroView
int SCREEN_WIDTH = uView.getLCDWidth();
int SCREEN_HEIGHT = uView.getLCDHeight();
