X-Git-Url: http://git.droids-corp.org/?p=protos%2Fxbee-avr.git;a=blobdiff_plain;f=commands.c;h=ce85088fcc30b4f1b80cd73e04169f14beb5f181;hp=083f85fa4fe17bb68f640a616ead8893fb7673b5;hb=87d9d882059ebe9e34adc088e0d6f4bea984bc91;hpb=cc6ac756becd992b51ddc7127846ff0fc3d4c662

diff --git a/commands.c b/commands.c
index 083f85f..ce85088 100644
--- a/commands.c
+++ b/commands.c
@@ -1299,6 +1299,116 @@ const parse_inst_t PROGMEM cmd_baudrate = {
 	},
 };
 
+/**********************************************************/
+
+/* this structure is filled when cmd_servo is parsed successfully */
+struct cmd_servo_result {
+	fixed_string_t arg0;
+	fixed_string_t arg1;
+	uint16_t num;
+	uint16_t val;
+};
+
+/* function called when cmd_servo is parsed successfully */
+static void cmd_servo_parsed(void * parsed_result, void *data)
+{
+	struct cmd_servo_result *res = parsed_result;
+	uint8_t i;
+
+	(void)data;
+
+	if (!strcmp_P(res->arg1, PSTR("set"))) {
+		if (res->num >= N_SERVO) {
+			printf_P(PSTR("bad servo num\n"));
+			return;
+		}
+		if (res->val >= 1024) {
+			printf_P(PSTR("bad servo val\n"));
+			return;
+		}
+		spi_servo_set(res->num, res->val);
+	}
+	else if (!strcmp_P(res->arg1, PSTR("bypass"))) {
+		spi_servo_set_bypass(!!res->val);
+	}
+	else if (!strcmp_P(res->arg1, PSTR("ppm"))) {
+		spi_servo_set_ppm(!!res->val);
+	}
+	else if (!strcmp_P(res->arg1, PSTR("show"))) {
+		for (i = 0; i < 6; i++)
+			printf_P(PSTR("%d: %d\r\n"), i, spi_servo_get(i));
+		printf_P(PSTR("bypass=%d ppm=%d\n"),
+			 spi_servo_get_bypass(), spi_servo_get_ppm());
+		printf_P(PSTR("\r\n"));
+	}
+}
+
+const char PROGMEM str_servo_arg0[] = "servo";
+const parse_token_string_t PROGMEM cmd_servo_arg0 =
+	TOKEN_STRING_INITIALIZER(struct cmd_servo_result, arg0,
+				 str_servo_arg0);
+const char PROGMEM str_servo_arg1_set[] = "set";
+const parse_token_string_t PROGMEM cmd_servo_arg1_set =
+	TOKEN_STRING_INITIALIZER(struct cmd_servo_result, arg1,
+				 str_servo_arg1_set);
+const parse_token_num_t PROGMEM cmd_servo_num =
+	TOKEN_NUM_INITIALIZER(struct cmd_servo_result, num,
+			      UINT16);
+const parse_token_num_t PROGMEM cmd_servo_val =
+	TOKEN_NUM_INITIALIZER(struct cmd_servo_result, val,
+			      UINT16);
+
+const char PROGMEM help_servo_set[] = "set servo value";
+const parse_inst_t PROGMEM cmd_servo_set = {
+	.f = cmd_servo_parsed,  /* function to call */
+	.data = NULL,      /* 2nd arg of func */
+	.help_str = help_servo_set,
+	.tokens = {        /* token list, NULL terminated */
+		(PGM_P)&cmd_servo_arg0,
+		(PGM_P)&cmd_servo_arg1_set,
+		(PGM_P)&cmd_servo_num,
+		(PGM_P)&cmd_servo_val,
+		NULL,
+	},
+};
+
+const char PROGMEM str_servo_arg1_show[] = "show";
+const parse_token_string_t PROGMEM cmd_servo_arg1_show =
+	TOKEN_STRING_INITIALIZER(struct cmd_servo_result, arg1,
+				 str_servo_arg1_show);
+
+const char PROGMEM help_servo_show[] = "read servo and config";
+const parse_inst_t PROGMEM cmd_servo_show = {
+	.f = cmd_servo_parsed,  /* function to call */
+	.data = NULL,      /* 2nd arg of func */
+	.help_str = help_servo_show,
+	.tokens = {        /* token list, NULL terminated */
+		(PGM_P)&cmd_servo_arg0,
+		(PGM_P)&cmd_servo_arg1_show,
+		NULL,
+	},
+};
+
+const char PROGMEM str_servo_arg1_bypassppm[] = "bypass#ppm";
+const parse_token_string_t PROGMEM cmd_servo_arg1_bypassppm =
+	TOKEN_STRING_INITIALIZER(struct cmd_servo_result, arg1,
+				 str_servo_arg1_bypassppm);
+
+const char PROGMEM help_servo_bypassppm[] = "change bypass/ppm";
+const parse_inst_t PROGMEM cmd_servo_bypassppm = {
+	.f = cmd_servo_parsed,  /* function to call */
+	.data = NULL,      /* 2nd arg of func */
+	.help_str = help_servo_bypassppm,
+	.tokens = {        /* token list, NULL terminated */
+		(PGM_P)&cmd_servo_arg0,
+		(PGM_P)&cmd_servo_arg1_bypassppm,
+		(PGM_P)&cmd_servo_val,
+		NULL,
+	},
+};
+
+/**********************************************************/
+
 /* this structure is filled when cmd_test_spi is parsed successfully */
 struct cmd_test_spi_result {
 	fixed_string_t arg0;
@@ -1312,13 +1422,13 @@ static void cmd_test_spi_parsed(void * parsed_result, void *data)
 	(void)parsed_result;
 	(void)data;
 
-	spi_servo_bypass(0);
-	spi_servo_ppm(0);
+	spi_servo_set_bypass(0);
+	spi_servo_set_ppm(0);
 
 	/* stress test: send many commands, no wait between each servo
 	 * of a series, and a variable delay between series */
 	printf_P(PSTR("stress test\r\n"));
-	while (!cmdline_keypressed()) {
+ 	while (!cmdline_keypressed()) {
 
 		wait_time++;
 		if (wait_time > 20)
@@ -1341,7 +1451,7 @@ static void cmd_test_spi_parsed(void * parsed_result, void *data)
 	}
 
 	printf_P(PSTR("bypass mode, with spi commands in background\r\n"));
-	spi_servo_bypass(1);
+	spi_servo_set_bypass(1);
 
 	/* test bypass mode */
 	while (!cmdline_keypressed()) {
@@ -1367,8 +1477,8 @@ static void cmd_test_spi_parsed(void * parsed_result, void *data)
 	}
 
 	printf_P(PSTR("PPM to servo\r\n"));
-	spi_servo_bypass(0);
-	spi_servo_ppm(0);
+	spi_servo_set_bypass(0);
+	spi_servo_set_ppm(0);
 
 	/* test PPM to servo (bypass) mode */
 	while (!cmdline_keypressed()) {
@@ -1379,8 +1489,8 @@ static void cmd_test_spi_parsed(void * parsed_result, void *data)
 	}
 
 	printf_P(PSTR("PPM to (servo + PPM)\r\n"));
-	spi_servo_bypass(0);
-	spi_servo_ppm(1);
+	spi_servo_set_bypass(0);
+	spi_servo_set_ppm(1);
 
 	/* test PPM to servo (bypass) mode */
 	while (!cmdline_keypressed()) {
@@ -1443,6 +1553,9 @@ const parse_ctx_t PROGMEM main_ctx[] = {
 	&cmd_dump,
 	&cmd_debug,
 	&cmd_baudrate,
+	&cmd_servo_set,
+	&cmd_servo_bypassppm,
+	&cmd_servo_show,
 	&cmd_test_spi,
 	NULL,
 };