You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
396 lines
9.7 KiB
396 lines
9.7 KiB
/* |
|
* 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 = 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 = 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 = 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), ¶ms); |
|
|
|
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], ®8, ®16); |
|
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], ®8, ®16); |
|
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; |
|
}
|
|
|