From 47cfe0ac6657c1e839543beb3f39f3669b9e8b22 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Lo=C3=AFc=20Dauphin?= Date: Fri, 31 Mar 2017 11:35:27 +0200 Subject: [PATCH] drivers/dynamixel: initial support --- drivers/Makefile.dep | 4 + drivers/Makefile.include | 3 + drivers/dynamixel/Makefile | 1 + drivers/dynamixel/crc.c | 65 +++++++ drivers/dynamixel/dynamixel.c | 124 +++++++++++++ drivers/dynamixel/include/dynamixel_crc.h | 37 ++++ .../dynamixel/include/dynamixel_protocol.h | 95 ++++++++++ drivers/dynamixel/include/dynamixel_reader.h | 150 +++++++++++++++ drivers/dynamixel/include/dynamixel_writer.h | 102 +++++++++++ drivers/dynamixel/reader.c | 54 ++++++ drivers/dynamixel/writer.c | 129 +++++++++++++ drivers/include/dynamixel.h | 171 ++++++++++++++++++ 12 files changed, 935 insertions(+) create mode 100644 drivers/dynamixel/Makefile create mode 100644 drivers/dynamixel/crc.c create mode 100644 drivers/dynamixel/dynamixel.c create mode 100644 drivers/dynamixel/include/dynamixel_crc.h create mode 100644 drivers/dynamixel/include/dynamixel_protocol.h create mode 100644 drivers/dynamixel/include/dynamixel_reader.h create mode 100644 drivers/dynamixel/include/dynamixel_writer.h create mode 100644 drivers/dynamixel/reader.c create mode 100644 drivers/dynamixel/writer.c create mode 100644 drivers/include/dynamixel.h diff --git a/drivers/Makefile.dep b/drivers/Makefile.dep index 3e891a1e8a..5c083df737 100644 --- a/drivers/Makefile.dep +++ b/drivers/Makefile.dep @@ -219,6 +219,10 @@ ifneq (,$(filter feetech,$(USEMODULE))) USEMODULE += uart_half_duplex endif +ifneq (,$(filter dynamixel,$(USEMODULE))) + USEMODULE += uart_half_duplex +endif + ifneq (,$(filter mtd_spi_nor,$(USEMODULE))) USEMODULE += mtd FEATURES_REQUIRED += periph_spi diff --git a/drivers/Makefile.include b/drivers/Makefile.include index 096e88bad3..cc2c4d4a1e 100644 --- a/drivers/Makefile.include +++ b/drivers/Makefile.include @@ -115,3 +115,6 @@ endif ifneq (,$(filter feetech,$(USEMODULE))) USEMODULE_INCLUDES += $(RIOTBASE)/drivers/feetech/include endif +ifneq (,$(filter dynamixel,$(USEMODULE))) + USEMODULE_INCLUDES += $(RIOTBASE)/drivers/dynamixel/include +endif diff --git a/drivers/dynamixel/Makefile b/drivers/dynamixel/Makefile new file mode 100644 index 0000000000..48422e909a --- /dev/null +++ b/drivers/dynamixel/Makefile @@ -0,0 +1 @@ +include $(RIOTBASE)/Makefile.base diff --git a/drivers/dynamixel/crc.c b/drivers/dynamixel/crc.c new file mode 100644 index 0000000000..5c6a63cff6 --- /dev/null +++ b/drivers/dynamixel/crc.c @@ -0,0 +1,65 @@ +/* + * 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. + */ + +/** + * @ingroup drivers_dynamixel + * @{ + * + * @file + * @brief Dynamixel CRC computation + * + * @author Loïc Dauphin + * + * @} + */ + +#include "dynamixel_crc.h" + +static const uint16_t _crc_table[256] = { + 0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, + 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022, + 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072, + 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041, + 0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2, + 0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, + 0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, + 0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082, + 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192, + 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1, + 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1, + 0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2, + 0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, + 0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, + 0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132, + 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101, + 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312, + 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321, + 0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371, + 0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, + 0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, + 0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2, + 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2, + 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381, + 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291, + 0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2, + 0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, + 0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, + 0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252, + 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261, + 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231, + 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202 +}; + +uint16_t dynamixel_crc_update(uint16_t crc_accum, const uint8_t *buffer, size_t size) +{ + for (size_t j = 0; j < size; j++) { + const uint16_t i = ((uint16_t)(crc_accum >> 8) ^ buffer[j]) & 0xFF; + crc_accum = (crc_accum << 8) ^ _crc_table[i]; + } + return crc_accum; +} diff --git a/drivers/dynamixel/dynamixel.c b/drivers/dynamixel/dynamixel.c new file mode 100644 index 0000000000..86e35fa8e8 --- /dev/null +++ b/drivers/dynamixel/dynamixel.c @@ -0,0 +1,124 @@ +/* + * 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. + */ + +/** + * @ingroup drivers_dynamixel + * @{ + * + * @file + * @brief Driver implementation for Dynamixel devices + * + * @author Loïc Dauphin + * + * @} + */ + +#include "dynamixel.h" + +#include "dynamixel_protocol.h" +#include "dynamixel_reader.h" +#include "dynamixel_writer.h" + +#include + +void dynamixel_init(dynamixel_t *device, uart_half_duplex_t *stream, dynamixel_id_t id) +{ + device->stream = stream; + device->id = id; +} + +int dynamixel_ping(uart_half_duplex_t *stream, dynamixel_id_t id) +{ + dynamixel_writer_t pw; + + uart_half_duplex_set_tx(stream); + dynamixel_writer_init(&pw, stream->buffer, stream->size); + dynamixel_writer_ping_make(&pw, id); + uart_half_duplex_send(stream, pw.size); + + uart_half_duplex_set_rx(stream); + if (uart_half_duplex_recv(stream, DXL_STATUS_SIZE(3)) != DXL_STATUS_SIZE(3)) { + return DYNAMIXEL_TIMEOUT; + } + + return DYNAMIXEL_OK; +} + +int dynamixel_write(dynamixel_t *device, dynamixel_addr_t reg, const uint8_t *data, size_t length) +{ + uart_half_duplex_set_tx(device->stream); + if (device->stream->size < length) { + return DYNAMIXEL_BUFFER_TOO_SMALL; + } + + dynamixel_writer_t pw; + + dynamixel_writer_init(&pw, device->stream->buffer, device->stream->size); + dynamixel_writer_write_make(&pw, device->id, reg, data, length); + uart_half_duplex_send(device->stream, pw.size); + + uart_half_duplex_set_rx(device->stream); + if (uart_half_duplex_recv(device->stream, DXL_STATUS_SIZE(0)) != DXL_STATUS_SIZE(0)) { + return DYNAMIXEL_TIMEOUT; + } + + return DYNAMIXEL_OK; +} + +int dynamixel_write8(dynamixel_t *device, dynamixel_addr_t reg, uint8_t value) +{ + return dynamixel_write(device, reg, &value, 1); +} + +int dynamixel_write16(dynamixel_t *device, dynamixel_addr_t reg, uint16_t value) +{ + return dynamixel_write(device, reg, (uint8_t*)&value, 2); +} + +int dynamixel_read(dynamixel_t *device, dynamixel_addr_t reg, uint8_t *data, size_t length) +{ + uart_half_duplex_set_tx(device->stream); + if (device->stream->size < length) { + return DYNAMIXEL_BUFFER_TOO_SMALL; + } + + dynamixel_writer_t pw; + + dynamixel_writer_init(&pw, device->stream->buffer, device->stream->size); + dynamixel_writer_read_make(&pw, device->id, reg, length); + uart_half_duplex_send(device->stream, pw.size); + + uart_half_duplex_set_rx(device->stream); + const size_t esize = DXL_STATUS_SIZE(length); + if (uart_half_duplex_recv(device->stream, esize) != esize) { + return DYNAMIXEL_TIMEOUT; + } + + dynamixel_reader_t pr; + dynamixel_reader_init(&pr, device->stream->buffer, esize); + if (!dynamixel_reader_is_valid(&pr)) { + return DYNAMIXEL_INVALID_MESSAGE; + } + + if (dynamixel_reader_status_get_payload_size(&pr) != length) { + return DYNAMIXEL_INVALID_MESSAGE; + } + + memcpy(data, dynamixel_reader_status_get_payload(&pr), length); + return DYNAMIXEL_OK; +} + +int dynamixel_read8(dynamixel_t *device, dynamixel_addr_t reg, uint8_t *value) +{ + return dynamixel_read(device, reg, value, 1); +} + +int dynamixel_read16(dynamixel_t *device, dynamixel_addr_t reg, uint16_t *value) +{ + return dynamixel_read(device, reg, (uint8_t*)value, 2); +} diff --git a/drivers/dynamixel/include/dynamixel_crc.h b/drivers/dynamixel/include/dynamixel_crc.h new file mode 100644 index 0000000000..603d0e1654 --- /dev/null +++ b/drivers/dynamixel/include/dynamixel_crc.h @@ -0,0 +1,37 @@ +/* + * 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. + */ + +/** + * @ingroup drivers_dynamixel + * + * @{ + * + * @file + * @brief Interface definition for Dynamixel crc + * + * @author Loïc Dauphin + */ + +#ifndef DYNAMIXEL_CRC_H +#define DYNAMIXEL_CRC_H + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +uint16_t dynamixel_crc_update(uint16_t crc_accum, const uint8_t *buffer, size_t size); + +#ifdef __cplusplus +} +#endif + +#endif +/** @} */ diff --git a/drivers/dynamixel/include/dynamixel_protocol.h b/drivers/dynamixel/include/dynamixel_protocol.h new file mode 100644 index 0000000000..221032d3a0 --- /dev/null +++ b/drivers/dynamixel/include/dynamixel_protocol.h @@ -0,0 +1,95 @@ +/* + * 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. + */ + +/** + * @ingroup drivers_dynamixel + * + * @{ + * + * @file + * @brief Dynamixel protocol definitions + * + * @author Loïc Dauphin + */ + +#ifndef DYNAMIXEL_PROTOCOL_H +#define DYNAMIXEL_PROTOCOL_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#define DXL_HEADER ((uint8_t[]){0xFF,0xFF,0xFD}) + +typedef enum { + XL320_B_9600 = 0, /**< XL320 available baudrate : 9600 */ + XL320_B_57600 = 1, /**< XL320 available baudrate : 57600 */ + XL320_B_115200 = 2, /**< XL320 available baudrate : 115200 */ + XL320_B_1000000 = 3, /**< XL320 available baudrate : 1000000 */ +} xl320_baudrate_t; + +typedef enum { + XL320_VERSION = 2, /**< Information on the version of firmware [R] */ + XL320_ID = 3, /**< ID of Dynamixel [RW] (default=1 ; min=0 ; max=252) */ + XL320_BAUD_RATE = 4, /**< Baud Rate of Dynamixel [RW] (default=3 ; min=0 ; max=3) */ + XL320_RETURN_DELAY_TIME = 5, /**< Return Delay Time [RW] (default=250 ; min=0 ; max=254) */ + XL320_CONTROL_MODE = 11, /**< Control Mode [RW] (default=2 ; min=1 ; max=2) */ + XL320_LIMIT_TEMPERATURE = 12, /**< Internal Limit Temperature [RW] (default=65 ; min=0 ; max=150) */ + XL320_LOWER_LIMIT_VOLTAGE = 13, /**< Lowest Limit Voltage [RW] (default=60 ; min=50 ; max=250) */ + XL320_UPPER_LIMIT_VOLTAGE = 14, /**< Upper Limit Voltage [RW] (default=90 ; min=50 ; max=250) */ + XL320_RETURN_LEVEL = 17, /**< Return Level [RW] (default=2 ; min=0 ; max=2) */ + XL320_ALARM_SHUTDOWN = 18, /**< Shutdown for Alarm [RW] (default=3 ; min=0 ; max=7) */ + XL320_TORQUE_ENABLE = 24, /**< Torque On/Off [RW] (default=0 ; min=0 ; max=1) */ + XL320_LED = 25, /**< LED On/Off [RW] (default=0 ; min=0 ; max=7) */ + XL320_D_GAIN = 27, /**< D Gain [RW] (default=0 ; min=0 ; max=254) */ + XL320_I_GAIN = 28, /**< I Gain [RW] (default=0 ; min=0 ; max=254) */ + XL320_P_GAIN = 29, /**< P Gain [RW] (default=32 ; min=0 ; max=254) */ + XL320_PRESENT_VOLTAGE = 45, /**< Current Voltage [R] */ + XL320_PRESENT_TEMPERATURE = 46, /**< Present temperature [R] */ + XL320_REGISTERED_INST = 47, /**< Registered Instruction [R] (default=0) */ + XL320_MOVING = 49, /**< Moving [R] (default=0) */ + XL320_ERROR = 50, /**< Hardware error status [R] (default=0) */ +} xl320_register8_t; + +typedef enum { + XL320_MODEL_NUMBER = 0, /**< Model number [R] (default=350) */ + XL320_CW_ANGLE_LIMIT = 6, /**< clockwise Angle Limit [RW] (default=0 ; min=0 ; max=1023) */ + XL320_CCW_ANGLE_LIMIT = 8, /**< counterclockwise Angle Limit [RW] (default=1023 ; min=0 ; max=1023) */ + XL320_MAX_TORQUE = 15, /**< Lowest byte of Max. Torque [RW] (default=1023 ; min=0 ; max=1023) */ + XL320_GOAL_POSITION = 30, /**< Goal Position [RW] (min=0 ; max=1023) */ + XL320_GOAL_VELOCITY = 32, /**< Goal Speed [RW] (min=0 ; max=2047) */ + XL320_GOAL_TORQUE = 35, /**< Goal Torque [RW] (min=0 ; max=1023) */ + XL320_PRESENT_POSITION = 37, /**< Current Position [R] */ + XL320_PRESENT_SPEED = 39, /**< Current Speed [R] */ + XL320_PRESENT_LOAD = 41, /**< Current Load [R] */ + XL320_PUNCH = 51, /**< Punch [RW] (default=32 ; min=0 ; max=1023) */ +} xl320_register16_t; + +typedef enum { + DXL_INST_PING = 0x01, /**< checks if ID is associated to a Device */ + DXL_INST_READ = 0x02, /**< read data from the Device */ + DXL_INST_WRITE = 0x03, /**< write data on the Device */ + DXL_INST_REG_WRITE = 0x04, /**< registers the write instruction to a standby status */ + DXL_INST_ACTION = 0x05, /**< executes the write instruction previously registered */ + DXL_INST_FACTORY_RESET = 0x06, /**< resets the Control Table to its initial factory default settings */ + DXL_INST_REBOOT = 0x08, /**< reboot the Device */ + DXL_INST_STATUS = 0x55, /**< Return Instruction for the Instruction Packet */ + DXL_INST_SYNC_READ = 0x82, /**< (Multiple devices) read data with same Address and length at once */ + DXL_INST_SYNC_WRITE = 0x83, /**< (Multiple devices) write data on the same Address and length at once */ + DXL_INST_BULK_READ = 0x92, /**< (Multiple devices) read data from different Addresses and lengths at once */ + DXL_INST_BULK_WRITE = 0x93, /**< (Multiple devices) write data on different Addresses and lengths at once */ +} dynamixel_intruction_t; + +#ifdef __cplusplus +} +#endif + +#endif +/** @} */ diff --git a/drivers/dynamixel/include/dynamixel_reader.h b/drivers/dynamixel/include/dynamixel_reader.h new file mode 100644 index 0000000000..7569df7157 --- /dev/null +++ b/drivers/dynamixel/include/dynamixel_reader.h @@ -0,0 +1,150 @@ +/* + * 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. + */ + +/** + * @ingroup drivers_dynamixel + * + * @{ + * + * @file + * @brief Interface definition for Dynamixel packet reader + * + * @author Loïc Dauphin + */ + +#ifndef DYNAMIXEL_READER_H +#define DYNAMIXEL_READER_H + +#include +#include + +#include "dynamixel_protocol.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define DXL_PING_SIZE (10) +#define DXL_STATUS_SIZE(len) (11+len) +#define DXL_READ_SIZE (14) +#define DXL_WRITE_SIZE(len) (12+len) + +/** + * @brief Dynamixel packet reader struct + */ +typedef struct { + const uint8_t *buffer; /**< data buffer */ + size_t size; /**< data buffer's size */ +} dynamixel_reader_t; + +/** + * @brief Initialize the Dynamixel packet reader + * + * @param[out] reader the packet reader + * @param[in] buffer the buffer used to store data + * @param[in] size the size of the buffer + */ +static inline void dynamixel_reader_init(dynamixel_reader_t *reader, const uint8_t *buffer, size_t size) +{ + reader->buffer = buffer; + reader->size = size; +} + + +/** + * @brief Check if the packet is valid + * + * @param[in] reader the packet reader + * + * @return true if the packet is valid + * @return false otherwise + */ +bool dynamixel_reader_is_valid(const dynamixel_reader_t *reader); + +/** + * @brief Get the packet's device id + * + * @param[in] reader the packet reader + * + * @return the packet's device id + */ +static inline uint8_t dynamixel_reader_get_id(const dynamixel_reader_t *reader) +{ + return reader->buffer[4]; +} + +/** + * @brief Get the packet's instruction code + * + * @param[in] reader the packet reader + * + * @return the packet's instruction code + */ +static inline uint8_t dynamixel_reader_get_instr(const dynamixel_reader_t *reader) +{ + return reader->buffer[7]; +} + +/** + * @brief Get the packet's length field + * + * @param[in] reader the packet reader + * + * @return the packet's length field + */ +static inline uint16_t dynamixel_reader_get_length(const dynamixel_reader_t *reader) +{ + return + (((uint16_t)reader->buffer[5]) & 0xFF) | + ((((uint16_t)reader->buffer[6]) & 0xFF) << 8); +} + +/** + * @brief Get the packet's crc + * + * @param[in] reader the packet reader + * + * @return the packet's length field + */ +static inline uint16_t dynamixel_reader_get_crc(const dynamixel_reader_t *reader) +{ + return + (((uint16_t)reader->buffer[reader->size - 2]) & 0xFF) | + ((((uint16_t)reader->buffer[reader->size - 1]) & 0xFF) << 8); +} + +/** + * @brief Get the packet's payload (response) + * + * @param[in] reader the packet reader + * + * @return the addess of the begining of the payload + */ +static inline const uint8_t *dynamixel_reader_status_get_payload(const dynamixel_reader_t *reader) +{ + return &reader->buffer[9]; +} + +/** + * @brief Get the packet's payload size (response) + * + * @param[in] reader the packet reader + * + * @return the size of the payload + */ +static inline size_t dynamixel_reader_status_get_payload_size(const dynamixel_reader_t *reader) +{ + return dynamixel_reader_get_length(reader) - 4; +} + +#ifdef __cplusplus +} +#endif + +#endif +/** @} */ diff --git a/drivers/dynamixel/include/dynamixel_writer.h b/drivers/dynamixel/include/dynamixel_writer.h new file mode 100644 index 0000000000..7f0f339682 --- /dev/null +++ b/drivers/dynamixel/include/dynamixel_writer.h @@ -0,0 +1,102 @@ +/* + * 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. + */ + +/** + * @ingroup drivers_dynamixel + * + * @{ + * + * @file + * @brief Interface definition for Dynamixel packet writer + * + * @author Loïc Dauphin + */ + +#ifndef DYNAMIXEL_WRITER_H +#define DYNAMIXEL_WRITER_H + +#include "dynamixel_protocol.h" +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief Dynamixel packet writer struct + */ +typedef struct { + uint8_t *buffer; /**< data buffer */ + size_t size; /**< packet's size */ + size_t limit; /**< data buffer's size */ +} dynamixel_writer_t; + +/** + * @brief Initialize the Dynamixel packet writer + * + * @param[out] writer the packet writer + * @param[in] buffer the buffer used to store data + * @param[in] limit the size of the buffer (= maximum packet size) + */ +void dynamixel_writer_init(dynamixel_writer_t *writer, uint8_t *buffer, size_t limit); + +/** + * @brief Get the data buffer to send + * + * @param[out] writer the packet writer + * + * @return the begining address of the buffer + */ +const uint8_t *dynamixel_writer_get_data(const dynamixel_writer_t *writer); + +/** + * @brief Get the data buffer's size to send + * + * @param[out] writer the packet writer + * + * @return the buffer's size + */ +size_t dynamixel_writer_get_size(const dynamixel_writer_t *writer); + +/** + * @brief Build a PING packet + * + * @param[out] writer the packet writer + * @param[in] id the destination's id + */ +void dynamixel_writer_ping_make(dynamixel_writer_t *writer, uint8_t id); + +/** + * @brief Build a WRITE packet + * + * @param[out] writer the packet writer + * @param[in] id the destination's id + * @param[in] reg the register to write in + * @param[in] buffer the data buffer to write + * @param[in] size the data buffer's size + */ +void dynamixel_writer_write_make(dynamixel_writer_t *writer, uint8_t id, uint16_t reg, const uint8_t *buffer, size_t size); + +/** + * @brief Build a READ packet + * + * @param[out] writer the packet writer + * @param[in] id the destination's id + * @param[in] reg the register to read + * @param[in] size the size to read + */ +void dynamixel_writer_read_make(dynamixel_writer_t *writer, uint8_t id, uint16_t reg, size_t size); + +#ifdef __cplusplus +} +#endif + +#endif +/** @} */ diff --git a/drivers/dynamixel/reader.c b/drivers/dynamixel/reader.c new file mode 100644 index 0000000000..5c4ee8171c --- /dev/null +++ b/drivers/dynamixel/reader.c @@ -0,0 +1,54 @@ +/* + * 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. + */ + +/** + * @ingroup drivers_dynamixel + * @{ + * + * @file + * @brief Dynamixel messages reader + * + * @author Loïc Dauphin + * + * @} + */ + +#include "dynamixel_reader.h" +#include "dynamixel_crc.h" + +static inline bool dynamixel_reader_check_minsize(const dynamixel_reader_t *reader) +{ + return 10 <= reader->size; +} + +static inline bool dynamixel_reader_check_start(const dynamixel_reader_t *reader) +{ + return + reader->buffer[0] == DXL_HEADER[0] && + reader->buffer[1] == DXL_HEADER[1] && + reader->buffer[2] == DXL_HEADER[2]; +} + +static inline bool dynamixel_reader_check_size(const dynamixel_reader_t *reader) +{ + return reader->size == (size_t)(dynamixel_reader_get_length(reader) + 7); +} + +static inline bool dynamixel_reader_check_sum(const dynamixel_reader_t *reader) +{ + return dynamixel_crc_update(0, reader->buffer, reader->size - 2) == dynamixel_reader_get_crc(reader); +} + +bool dynamixel_reader_is_valid(const dynamixel_reader_t *reader) +{ + return + dynamixel_reader_check_minsize(reader) && + dynamixel_reader_check_start(reader) && + dynamixel_reader_check_size(reader) && + dynamixel_reader_check_sum(reader); +} diff --git a/drivers/dynamixel/writer.c b/drivers/dynamixel/writer.c new file mode 100644 index 0000000000..05f21752e5 --- /dev/null +++ b/drivers/dynamixel/writer.c @@ -0,0 +1,129 @@ +/* + * 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. + */ + +/** + * @ingroup drivers_dynamixel + * @{ + * + * @file + * @brief Dynamixel messages writer + * + * @author Loïc Dauphin + * + * @} + */ + +#include "dynamixel_writer.h" +#include "dynamixel_crc.h" + +#include + +#define LOW(v) (v & 0xFF) +#define HIGH(v) ((v >> 8) & 0xFF) + +void dynamixel_writer_init(dynamixel_writer_t *writer, uint8_t *buffer, size_t limit) +{ + writer->buffer = buffer; + writer->size = 0; + writer->limit = limit; +} + +const uint8_t *dynamixel_writer_get_data(const dynamixel_writer_t *writer) +{ + return (const uint8_t*)writer->buffer; +} + +size_t dynamixel_writer_get_size(const dynamixel_writer_t *writer) +{ + return writer->size; +} + +void dynamixel_writer_ping_make(dynamixel_writer_t *writer, uint8_t id) +{ + const size_t len = 3; + if (len + 7 <= writer->limit) { + writer->size = len + 7; + + writer->buffer[0] = DXL_HEADER[0]; + writer->buffer[1] = DXL_HEADER[1]; + writer->buffer[2] = DXL_HEADER[2]; + writer->buffer[3] = 0x00; /* reserved */ + writer->buffer[4] = id; + writer->buffer[5] = LOW(len); + writer->buffer[6] = HIGH(len); + writer->buffer[7] = DXL_INST_PING; + + uint16_t crc = dynamixel_crc_update(0, writer->buffer, 8); + + writer->buffer[8] = LOW(crc); + writer->buffer[9] = HIGH(crc); + } + else { + writer->size = 0; + } +} + +void dynamixel_writer_write_make(dynamixel_writer_t *writer, uint8_t id, uint16_t reg, const uint8_t *buffer, size_t size) +{ + const size_t len = 5 + size; + if (len + 7 <= writer->limit) { + writer->size = len + 7; + + writer->buffer[0] = DXL_HEADER[0]; + writer->buffer[1] = DXL_HEADER[1]; + writer->buffer[2] = DXL_HEADER[2]; + writer->buffer[3] = 0x00; /* reserved */ + writer->buffer[4] = id; + writer->buffer[5] = LOW(len); + writer->buffer[6] = HIGH(len); + writer->buffer[7] = DXL_INST_WRITE; + + writer->buffer[8] = LOW(reg); + writer->buffer[9] = HIGH(reg); + + memcpy(&writer->buffer[10], buffer, size); + + uint16_t crc = dynamixel_crc_update(0, writer->buffer, len + 5); + + writer->buffer[writer->size - 2] = LOW(crc); + writer->buffer[writer->size - 1] = HIGH(crc); + } + else { + writer->size = 0; + } +} + +void dynamixel_writer_read_make(dynamixel_writer_t *writer, uint8_t id, uint16_t reg, size_t size) +{ + const size_t len = 7; + if (len + 7 <= writer->limit) { + writer->size = len + 7; + + writer->buffer[0] = DXL_HEADER[0]; + writer->buffer[1] = DXL_HEADER[1]; + writer->buffer[2] = DXL_HEADER[2]; + writer->buffer[3] = 0x00; /* reserved */ + writer->buffer[4] = id; + writer->buffer[5] = LOW(len); + writer->buffer[6] = HIGH(len); + writer->buffer[7] = DXL_INST_READ; + + writer->buffer[8] = LOW(reg); + writer->buffer[9] = HIGH(reg); + writer->buffer[10] = LOW(size); + writer->buffer[11] = HIGH(size); + + uint16_t crc = dynamixel_crc_update(0, writer->buffer, 12); + + writer->buffer[12] = LOW(crc); + writer->buffer[13] = HIGH(crc); + } + else { + writer->size = 0; + } +} diff --git a/drivers/include/dynamixel.h b/drivers/include/dynamixel.h new file mode 100644 index 0000000000..fc8108e70b --- /dev/null +++ b/drivers/include/dynamixel.h @@ -0,0 +1,171 @@ +/* + * 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. + */ + +/** + * @defgroup drivers_dynamixel Dynamixel driver + * @ingroup drivers_actuators + * + * This module contains drivers for any device using dynamixel's servomotors communication bus. + * The bus is mainly used for servomotors, but a device can be anything : sensors, other actuators. + * + * @{ + * + * @file + * @brief Interface definition for Dynamixel devices driver + * + * @author Loïc Dauphin + */ + +#ifndef DYNAMIXEL_H +#define DYNAMIXEL_H + +#include +#include + +#include "dynamixel_protocol.h" +#include "uart_half_duplex.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef uint8_t dynamixel_id_t; /**< device id type */ +typedef uint16_t dynamixel_addr_t; /**< register address type */ + +/** + * @brief Descriptor struct for a dynamixel device + */ +typedef struct { + uart_half_duplex_t *stream; /**< the stream used */ + dynamixel_id_t id; /**< the device address */ +} dynamixel_t; + +/** + * @brief Possible dynamixel return values + */ +enum { + DYNAMIXEL_OK, /**< Success */ + DYNAMIXEL_TIMEOUT, /**< No response from the device */ + DYNAMIXEL_BUFFER_TOO_SMALL, /**< Buffer is too small for the message */ + DYNAMIXEL_INVALID_MESSAGE, /**< Invalid message received */ +}; + +/** + * @brief Send a PING message to a device + * + * @param[in] stream the stream + * @param[in] id the device address + * + * @return DYNAMIXEL_OK if a device answered + * @return DYNAMIXEL_TIMEOUT if the device did not answer + * @return DYNAMIXEL_BUFFER_TOO_SMALL if buffer is too small for the message + * @return DYNAMIXEL_INVALID_MESSAGE if an invalid message was received + */ +int dynamixel_ping(uart_half_duplex_t *stream, dynamixel_id_t id); + +/** + * @brief Initialize a Dynamixel device + * + * @param[out] device the Dynamixel device + * @param[in] stream the stream + * @param[in] id the device address + */ +void dynamixel_init(dynamixel_t *device, uart_half_duplex_t *stream, dynamixel_id_t id); + +/** + * @brief Write to a device 8bits register + * + * @param[in] device the Dynamixel device + * @param[in] reg the register to write + * @param[in] value the value to write + * + * @return DYNAMIXEL_OK on success + * @return DYNAMIXEL_TIMEOUT if the device did not answer + * @return DYNAMIXEL_BUFFER_TOO_SMALL if buffer is too small for the message + * @return DYNAMIXEL_INVALID_MESSAGE if an invalid message was received + */ +int dynamixel_write8(dynamixel_t *device, dynamixel_addr_t reg, uint8_t value); + +/** + * @brief Write to a device 16bits register + * + * @param[in] device the Dynamixel device + * @param[in] reg the register to write + * @param[in] value the value to write + * + * @return DYNAMIXEL_OK on success + * @return DYNAMIXEL_TIMEOUT if the device did not answer + * @return DYNAMIXEL_BUFFER_TOO_SMALL if buffer is too small for the message + * @return DYNAMIXEL_INVALID_MESSAGE if an invalid message was received + */ +int dynamixel_write16(dynamixel_t *device, dynamixel_addr_t reg, uint16_t value); + +/** + * @brief Write to a device address + * + * @param[in] device the Dynamixel device + * @param[in] reg the address to start write + * @param[in] data the data to write + * @param[in] length the data length + * + * @return DYNAMIXEL_OK on success + * @return DYNAMIXEL_TIMEOUT if the device did not answer + * @return DYNAMIXEL_BUFFER_TOO_SMALL if buffer is too small for the message + * @return DYNAMIXEL_INVALID_MESSAGE if an invalid message was received + */ +int dynamixel_write(dynamixel_t *device, dynamixel_addr_t reg, const uint8_t *data, size_t length); + +/** + * @brief Read from a device 8bits register + * + * @param[in] device the Dynamixel device + * @param[in] reg the register to read + * @param[out] value the value to read + * + * @return DYNAMIXEL_OK on success + * @return DYNAMIXEL_TIMEOUT if the device did not answer + * @return DYNAMIXEL_BUFFER_TOO_SMALL if buffer is too small for the message + * @return DYNAMIXEL_INVALID_MESSAGE if an invalid message was received + */ +int dynamixel_read8(dynamixel_t *device, dynamixel_addr_t reg, uint8_t *value); + +/** + * @brief Read from a device 16bits register + * + * @param[in] device the Dynamixel device + * @param[in] reg the register to read + * @param[out] value the value to read + * + * @return DYNAMIXEL_OK on success + * @return DYNAMIXEL_TIMEOUT if the device did not answer + * @return DYNAMIXEL_BUFFER_TOO_SMALL if buffer is too small for the message + * @return DYNAMIXEL_INVALID_MESSAGE if an invalid message was received + */ +int dynamixel_read16(dynamixel_t *device, dynamixel_addr_t reg, uint16_t *value); + +/** + * @brief Read from a device address + * + * @param[in] device the Dynamixel device + * @param[in] reg the address to start read + * @param[out] data the data buffer to fill + * @param[in] length the data length + * + * @return DYNAMIXEL_OK on success + * @return DYNAMIXEL_TIMEOUT if the device did not answer + * @return DYNAMIXEL_BUFFER_TOO_SMALL if buffer is too small for the message + * @return DYNAMIXEL_INVALID_MESSAGE if an invalid message was received + */ +int dynamixel_read(dynamixel_t *device, dynamixel_addr_t reg, uint8_t *data, size_t length); + +#ifdef __cplusplus +} +#endif + +#endif +/** @} */