From 87d9d882059ebe9e34adc088e0d6f4bea984bc91 Mon Sep 17 00:00:00 2001 From: Olivier Matz Date: Tue, 15 Oct 2013 22:20:31 +0200 Subject: [PATCH] add a command to read/write servo --- commands.c | 129 ++++++++++++++++++++++++++++++++++++++++++++++++---- spi_servo.c | 18 ++++++-- spi_servo.h | 8 +++- 3 files changed, 140 insertions(+), 15 deletions(-) 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, }; diff --git a/spi_servo.c b/spi_servo.c index 6e1da05..c4fe318 100644 --- a/spi_servo.c +++ b/spi_servo.c @@ -27,8 +27,6 @@ /* 1ms */ #define SPI_EVT_PERIOD (10000UL/SCHEDULER_UNIT) -#define N_SERVO 6 - #define PPM_BIT 0x01 #define BYPASS_BIT 0x02 @@ -189,7 +187,7 @@ void spi_servo_init(void) scheduler_add_periodical_event_priority(&spi_servo_cb, NULL, SPI_EVT_PERIOD, SPI_PRIO); - spi_servo_bypass(1); + spi_servo_set_bypass(1); } void spi_servo_set(uint8_t num, uint16_t val) @@ -220,7 +218,17 @@ uint16_t spi_servo_get(uint8_t num) return val; } -void spi_servo_bypass(uint8_t enable) +uint8_t spi_servo_get_bypass(void) +{ + return !!(spi_servo_tx.servo[N_SERVO] & BYPASS_BIT); +} + +uint8_t spi_servo_get_ppm(void) +{ + return !!(spi_servo_tx.servo[N_SERVO] & PPM_BIT); +} + +void spi_servo_set_bypass(uint8_t enable) { uint8_t flags; @@ -234,7 +242,7 @@ void spi_servo_bypass(uint8_t enable) IRQ_UNLOCK(flags); } -void spi_servo_ppm(uint8_t enable) +void spi_servo_set_ppm(uint8_t enable) { uint8_t flags; diff --git a/spi_servo.h b/spi_servo.h index 7156166..895df3a 100644 --- a/spi_servo.h +++ b/spi_servo.h @@ -1,5 +1,9 @@ +#define N_SERVO 6 + void spi_servo_init(void); void spi_servo_set(uint8_t num, uint16_t val); uint16_t spi_servo_get(uint8_t num); -void spi_servo_bypass(uint8_t enable); -void spi_servo_ppm(uint8_t enable); +void spi_servo_set_bypass(uint8_t enable); +void spi_servo_set_ppm(uint8_t enable); +uint8_t spi_servo_get_bypass(void); +uint8_t spi_servo_get_ppm(void); -- 2.39.5