tests/driver_feetech: initial support

tests/driver_feetech/Makefile

+APPLICATION = driver_feetech
+include ../Makefile.tests_common
+
+# chronos : USART_1 undeclared
+BOARD_BLACKLIST += chronos
+
+# mips-malta : undefined reference to uart_write
+BOARD_BLACKLIST += mips-malta
+
+USEMODULE += feetech
+USEMODULE += shell
+
+include $(RIOTBASE)/Makefile.include

tests/driver_feetech/README.md

+# About
+
+This application is intended for testing Feetech TTL bus.
+It is especially designed to test Feetech SCS15 servomotors.
+
+# Usage
+
+To have the list of available commands :
+```
+help
+```
+
+You will need to initialize one UART at 1000000 baudrate (if the servomotor is in factory configuration) :
+```
+init 1 1000000
+```
+
+To ping the servomotor :
+```
+ping 1
+```
+
+Be careful ! If 2 servomotors with the same ID are connected on the same bus, you will have no response.
+Factory configuration ID is 1, you need to change this to connect an other servo.
+
+To scan every connected servomotors (IDs from 0 to 253) :
+```
+scan
+```
+
+To read a servo register :
+```
+read 1 PRESENT_POSITION
+```
+
+To move a servo, you need to enable the torque and set the goal position [0-1024] :
+```
+write 1 ENABLE_TORQUE 1
+write 1 GOAL_POSITION 512
+```

tests/driver_feetech/main.c

+/*
+ * Copyright (C) 2017 Inria
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser General
+ * Public License v2.1. See the file LICENSE in the top level directory for more
+ * details.
+ */
+
+#include "feetech.h"
+#include "shell.h"
+#include "shell_commands.h"
+#include "uart_stdio.h"
+
+#include <stdio.h>
+#include <string.h>
+
+#define ARRAY_LEN(array) (sizeof(array)/sizeof(array[0]))
+
+typedef struct {
+    const char *name;
+    int addr;
+} reg_name_addr_t;
+
+static const reg_name_addr_t regs8[] = {
+    { "ID", SCS15_ID },
+    { "BAUD_RATE", SCS15_BAUD_RATE },
+    { "RETURN_DELAY_TIME", SCS15_RETURN_DELAY_TIME },
+    { "RETURN_LEVEL", SCS15_RETURN_LEVEL },
+    { "LIMIT_TEMPERATURE", SCS15_LIMIT_TEMPERATURE },
+    { "MAX_LIMIT_VOLTAGE", SCS15_MAX_LIMIT_VOLTAGE },
+    { "MIN_LIMIT_VOLTAGE", SCS15_MIN_LIMIT_VOLTAGE },
+    { "ALARM_LED", SCS15_ALARM_LED },
+    { "ALARM_SHUTDOWN", SCS15_ALARM_SHUTDOWN },
+    { "COMPLIANCE_P", SCS15_COMPLIANCE_P },
+    { "COMPLIANCE_D", SCS15_COMPLIANCE_D },
+    { "COMPLIANCE_I", SCS15_COMPLIANCE_I },
+    { "CW_DEAD", SCS15_CW_DEAD },
+    { "CCW_DEAD", SCS15_CCW_DEAD },
+    { "TORQUE_ENABLE", SCS15_TORQUE_ENABLE },
+    { "LED", SCS15_LED },
+    { "LOCK", SCS15_LOCK },
+    { "PRESENT_VOLTAGE", SCS15_PRESENT_VOLTAGE },
+    { "PRESENT_TEMPERATURE", SCS15_PRESENT_TEMPERATURE },
+    { "REGISTERED_INSTRUCTION", SCS15_REGISTERED_INSTRUCTION },
+    { "ERROR", SCS15_ERROR },
+    { "MOVING", SCS15_MOVING },
+};
+
+static const reg_name_addr_t regs16[] = {
+    { "MODEL_NUMBER", SCS15_MODEL_NUMBER },
+    { "VERSION", SCS15_VERSION },
+    { "MIN_ANGLE_LIMIT", SCS15_MIN_ANGLE_LIMIT },
+    { "MAX_ANGLE_LIMIT", SCS15_MAX_ANGLE_LIMIT },
+    { "MAX_TORQUE", SCS15_MAX_TORQUE },
+    { "PUNCH", SCS15_PUNCH },
+    { "IMAX", SCS15_IMAX },
+    { "OFFSET", SCS15_OFFSET },
+    { "GOAL_POSITION", SCS15_GOAL_POSITION },
+    { "GOAL_TIME", SCS15_GOAL_TIME },
+    { "GOAL_SPEED", SCS15_GOAL_SPEED },
+    { "PRESENT_POSITION", SCS15_PRESENT_POSITION },
+    { "PRESENT_SPEED", SCS15_PRESENT_SPEED },
+    { "PRESENT_LOAD", SCS15_PRESENT_LOAD },
+    { "VIR_POSITION", SCS15_VIR_POSITION },
+    { "CURRENT", SCS15_CURRENT },
+};
+
+static const int32_t baudrates[] = {
+    1000000L,
+    500000L,
+    250000L,
+    128000L,
+    115200L,
+    76800L,
+    57600L,
+    38400L,
+};
+
+static uint8_t feetech_buffer[128];
+static uart_half_duplex_t stream;
+
+static int parse_uart(char *arg)
+{
+    unsigned uart = (unsigned)atoi(arg);
+    if (uart >= UART_NUMOF) {
+        printf("Error: Invalid UART_DEV device specified (%u).\n", uart);
+        return -1;
+    }
+    else if (UART_DEV(uart) == UART_STDIO_DEV) {
+        printf("Error: The selected UART_DEV(%u) is used for the shell!\n", uart);
+        return -2;
+    }
+    return uart;
+}
+
+static int32_t parse_baud(char *arg)
+{
+    int32_t baud = (int32_t)atoi(arg);
+
+    for (size_t i = 0 ; i < ARRAY_LEN(baudrates) ; i++) {
+        if(baud == baudrates[i]) {
+            return baud;
+        }
+    }
+
+    printf("Error: Invalid baudrate (%s)\n", arg);
+    return -1;
+}
+
+static int parse_dev(char *arg)
+{
+    int dev = (int)atoi(arg);
+    if (dev < 0 || 254 < dev) {
+        printf("Error: Invalid device id (%s)\n", arg);
+        return -1;
+    }
+    return dev;
+}
+
+static void parse_reg(char *arg, int *reg8, int *reg16)
+{
+    *reg8 = -1;
+    *reg16 = -1;
+
+    for (size_t i = 0 ; i < ARRAY_LEN(regs8) ; i++) {
+        if(strcmp(arg, regs8[i].name) == 0) {
+            *reg8 = regs8[i].addr;
+            return;
+        }
+    }
+
+    for (size_t i = 0 ; i < ARRAY_LEN(regs16) ; i++) {
+        if(strcmp(arg, regs16[i].name) == 0) {
+            *reg16 = regs16[i].addr;
+            return;
+        }
+    }
+
+    printf("Error: Invalid register (%s)\n", arg);
+}
+
+void print_registers(void) {
+    puts("available 8bits registers :");
+    for (size_t i = 0 ; i < ARRAY_LEN(regs8) ; i++) {
+        printf("\t%s\n", regs8[i].name);
+    }
+
+    puts("available 16bits registers :");
+    for (size_t i = 0 ; i < ARRAY_LEN(regs16) ; i++) {
+        printf("\t%s\n", regs16[i].name);
+    }
+}
+
+static int cmd_init(int argc, char **argv) {
+    int uart = -1;
+    int baud = -1;
+    uint32_t timeout = -1;
+
+    if (argc != 3 && argc != 4) {
+        printf("usage; %s <uart> <baudrate> [<timeout_us>]\n", argv[0]);
+        puts("available baudrates :");
+        for (size_t i = 0 ; i < ARRAY_LEN(baudrates) ; i++) {
+            printf("\t%ld\n", (long int)baudrates[i]);
+        }
+        return 1;
+    }
+    /* parse parameters */
+    uart = parse_uart(argv[1]);
+    if (uart < 0) {
+        return -1;
+    }
+
+    baud = parse_baud(argv[2]);
+    if (baud < 0) {
+        return -1;
+    }
+
+    if (argc == 4) {
+        timeout = (uint32_t)atol(argv[3]);
+        if (timeout == 0) {
+            printf("Error : Invalid timeout (%s)", argv[3]);
+            return -1;
+        }
+    }
+
+    /* init */
+    uart_half_duplex_params_t params = {
+        .uart = uart,
+        .baudrate = baud,
+        .dir = UART_HALF_DUPLEX_DIR_NONE,
+    };
+
+    int ret = uart_half_duplex_init(&stream, feetech_buffer, ARRAY_LEN(feetech_buffer), &params);
+
+    if (argc == 4) {
+        stream.timeout_us = timeout;
+    }
+
+    if (ret == UART_HALF_DUPLEX_NODEV) {
+        puts("Error: invalid UART device given");
+        return -1;
+    }
+    if (ret == UART_HALF_DUPLEX_NOBAUD) {
+        puts("Error: given baudrate is not applicable");
+        return -1;
+    }
+    if (ret == UART_HALF_DUPLEX_INTERR) {
+        puts("Error: internal error");
+        return -1;
+    }
+    if (ret == UART_HALF_DUPLEX_NOMODE) {
+        puts("Error: given mode is not applicable");
+        return -1;
+    }
+    if (ret == UART_HALF_DUPLEX_NOBUFF) {
+        puts("Error: invalid buffer given");
+        return -1;
+    }
+
+    printf("Successfully initialized Feetech TTL bus UART_DEV(%i)\n", uart);
+    return 0;
+}
+
+static int cmd_ping(int argc, char **argv) {
+    int id = -1;
+
+    if (argc != 2) {
+        printf("usage; %s <dev_id>\n", argv[0]);
+        return 1;
+    }
+    /* parse parameters */
+    id = parse_dev(argv[1]);
+    if (id < 0) {
+        return -1;
+    }
+
+    /* ping */
+    if (feetech_ping(&stream, id) == FEETECH_OK) {
+        printf("Device %i responded\n", id);
+    }
+    else {
+        printf("No response from %i\n", id);
+    }
+    return 0;
+}
+
+static int cmd_scan(int argc, char **argv) {
+    int min = -1;
+    int max = -1;
+
+    if (argc == 3) {
+        min = atoi(argv[1]);
+        max = atoi(argv[2]);
+        if (min < 0) {
+            return -1;
+        }
+        if (max > 254) {
+            return -1;
+        }
+        if (max < min) {
+            return -1;
+        }
+    }
+    else if (argc == 1) {
+        min = 0;
+        max = 254;
+    }
+    else {
+        printf("usage; %s [<min_id> <max_id>]\n", argv[0]);
+        return 1;
+    }
+
+    /* ping */
+    puts("Scanning...");
+    for (int id = min ; id < max ; id++) {
+        if (feetech_ping(&stream, id) == FEETECH_OK) {
+            printf("Device %i available\n", id);
+        }
+    }
+    puts("End");
+    return 0;
+}
+
+static int cmd_read(int argc, char **argv) {
+    int id = -1;
+    int reg8 = -1;
+    int reg16 = -1;
+
+    if (argc != 3) {
+        printf("usage; %s <dev_id> <reg>\n", argv[0]);
+        print_registers();
+        return 1;
+    }
+    /* parse parameters */
+    id = parse_dev(argv[1]);
+    if (id < 0) {
+        return -1;
+    }
+
+    parse_reg(argv[2], &reg8, &reg16);
+    if (reg8 < 0 && reg16 < 0) {
+        return -1;
+    }
+
+    /* read */
+    feetech_t dev;
+    feetech_init(&dev, &stream, id);
+    if (reg8 >= 0) {
+        uint8_t val = 0;
+        int ret = feetech_read8(&dev, reg8, &val);
+        if (ret != FEETECH_OK) {
+            printf("Error[%i] : No response from %i\n", ret, id);
+            return -1;
+        }
+        printf("%i\n", (int)val);
+    }
+    else {
+        uint16_t val = 0;
+        int ret = feetech_read16(&dev, reg16, &val);
+        if (ret != FEETECH_OK) {
+            printf("Error[%i] : No response from %i\n", ret, id);
+            return -1;
+        }
+        printf("%i\n", (int)val);
+    }
+    return 0;
+}
+
+static int cmd_write(int argc, char **argv) {
+    int id = -1;
+    int reg8 = -1;
+    int reg16 = -1;
+
+    if (argc != 4) {
+        printf("usage; %s <dev_id> <reg> <value>\n", argv[0]);
+        print_registers();
+        return 1;
+    }
+    /* parse parameters */
+    id = parse_dev(argv[1]);
+    if (id < 0) {
+        return -1;
+    }
+
+    parse_reg(argv[2], &reg8, &reg16);
+    if (reg8 < 0 && reg16 < 0) {
+        return -1;
+    }
+
+    int val = atoi(argv[3]);
+    if (val < 0) {
+        return -1;
+    }
+
+    /* read */
+    feetech_t dev;
+    feetech_init(&dev, &stream, id);
+    if (reg8 >= 0) {
+        int ret = feetech_write8(&dev, reg8, val);
+        if (ret != FEETECH_OK) {
+            printf("Error[%i] : No response from %i\n", ret, id);
+            return -1;
+        }
+        printf("Written %i at address %i\n", (int)val, reg8);
+    }
+    else {
+        int ret = feetech_write16(&dev, reg16, val);
+        if (ret != FEETECH_OK) {
+            printf("Error[%i] : No response from %i\n", ret, id);
+            return -1;
+        }
+        printf("Written %i at address %i\n", (int)val, reg16);
+    }
+    return 0;
+}
+
+static const shell_command_t shell_commands[] = {
+    { "init", "Initialize a Feetech TTL bus with a given baudrate", cmd_init },
+    { "ping", "Ping a Feetech device", cmd_ping },
+    { "scan", "Find all Feetech devices between min_id and max_id", cmd_scan },
+    { "read", "Read a Feetech device register", cmd_read },
+    { "write", "Write in a Feetech device register", cmd_write },
+    { NULL, NULL, NULL }
+};
+
+int main(void)
+{
+    puts("\nManual Feetech device driver test");
+    puts("===================================");
+    puts("This application is intended for testing Feetech TTL bus\n");
+
+    char line_buf[SHELL_DEFAULT_BUFSIZE];
+    shell_run(shell_commands, line_buf, SHELL_DEFAULT_BUFSIZE);
+
+    return 0;
+}