diff --git a/drivers/Makefile.dep b/drivers/Makefile.dep index c2a8dd7d67..9ac61dfb66 100644 --- a/drivers/Makefile.dep +++ b/drivers/Makefile.dep @@ -209,3 +209,7 @@ ifneq (,$(filter uart_half_duplex,$(USEMODULE))) FEATURES_REQUIRED += periph_uart USEMODULE += xtimer endif + +ifneq (,$(filter feetech,$(USEMODULE))) + USEMODULE += uart_half_duplex +endif diff --git a/drivers/Makefile.include b/drivers/Makefile.include index 849e6cbf3d..434371db6d 100644 --- a/drivers/Makefile.include +++ b/drivers/Makefile.include @@ -109,3 +109,6 @@ endif ifneq (,$(filter uart_half_duplex,$(USEMODULE))) USEMODULE_INCLUDES += $(RIOTBASE)/drivers/uart_half_duplex/include endif +ifneq (,$(filter feetech,$(USEMODULE))) + USEMODULE_INCLUDES += $(RIOTBASE)/drivers/feetech/include +endif diff --git a/drivers/feetech/Makefile b/drivers/feetech/Makefile new file mode 100644 index 0000000000..48422e909a --- /dev/null +++ b/drivers/feetech/Makefile @@ -0,0 +1 @@ +include $(RIOTBASE)/Makefile.base diff --git a/drivers/feetech/feetech.c b/drivers/feetech/feetech.c new file mode 100644 index 0000000000..ad3802bf69 --- /dev/null +++ b/drivers/feetech/feetech.c @@ -0,0 +1,133 @@ +/* + * 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_feetech + * @{ + * + * @file + * @brief Driver implementation for Feetech devices + * + * @author Loïc Dauphin + * + * @} + */ + +#include "feetech.h" + +#include "feetech_protocol.h" +#include "feetech_reader.h" +#include "feetech_writer.h" + +#include "periph/uart.h" +#include "xtimer.h" +#include "byteorder.h" + +#include + +void feetech_init(feetech_t *device, uart_half_duplex_t *stream, feetech_id_t id) +{ + device->stream = stream; + device->id = id; +} + +int feetech_ping(uart_half_duplex_t *stream, feetech_id_t id) +{ + feetech_writer_t pw; + + uart_half_duplex_set_tx(stream); + feetech_writer_init(&pw, stream->buffer, stream->size); + feetech_writer_ping_make(&pw, id); + uart_half_duplex_send(stream, pw.size); + + uart_half_duplex_set_rx(stream); + if (uart_half_duplex_recv(stream, FEETECH_ACK_SIZE) != FEETECH_ACK_SIZE) { + return FEETECH_TIMEOUT; + } + + return FEETECH_OK; +} + +int feetech_write(feetech_t *device, feetech_addr_t reg, const uint8_t *data, size_t length) +{ + uart_half_duplex_set_tx(device->stream); + if (device->stream->size < length) { + return FEETECH_BUFFER_TOO_SMALL; + } + + feetech_writer_t pw; + + feetech_writer_init(&pw, device->stream->buffer, device->stream->size); + feetech_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, FEETECH_ACK_SIZE) != FEETECH_ACK_SIZE) { + return FEETECH_TIMEOUT; + } + + return FEETECH_OK; +} + +int feetech_write8(feetech_t *device, feetech_addr_t reg, uint8_t value) +{ + return feetech_write(device, reg, &value, 1); +} + +int feetech_write16(feetech_t *device, feetech_addr_t reg, uint16_t value) +{ + value = HTONS(value); + return feetech_write(device, reg, (uint8_t*)&value, 2); +} + +int feetech_read(feetech_t *device, feetech_addr_t reg, uint8_t *data, size_t length) +{ + uart_half_duplex_set_tx(device->stream); + if (device->stream->size < length) { + return FEETECH_BUFFER_TOO_SMALL; + } + + feetech_writer_t pw; + + feetech_writer_init(&pw, device->stream->buffer, device->stream->size); + feetech_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 = FEETECH_RESPONSE_SIZE(length); + if (uart_half_duplex_recv(device->stream, esize) != esize) { + return FEETECH_TIMEOUT; + } + + feetech_reader_t pr; + feetech_reader_init(&pr, device->stream->buffer, esize); + if (!feetech_reader_is_valid(&pr)) { + return FEETECH_INVALID_MESSAGE; + } + + if (feetech_reader_response_get_payload_size(&pr) != length) { + return FEETECH_INVALID_MESSAGE; + } + + memcpy(data, feetech_reader_response_get_payload(&pr), length); + return FEETECH_OK; +} + +int feetech_read8(feetech_t *device, feetech_addr_t reg, uint8_t *value) +{ + return feetech_read(device, reg, value, 1); +} + +int feetech_read16(feetech_t *device, feetech_addr_t reg, uint16_t *value) +{ + const int ret = feetech_read(device, reg, (uint8_t*)value, 2); + if (ret == FEETECH_OK) { + *value = NTOHS(*value); + } + return ret; +} diff --git a/drivers/feetech/include/feetech_protocol.h b/drivers/feetech/include/feetech_protocol.h new file mode 100644 index 0000000000..98ddfd4c5d --- /dev/null +++ b/drivers/feetech/include/feetech_protocol.h @@ -0,0 +1,99 @@ +/* + * 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_feetech + * + * @{ + * + * @file + * @brief Feetech protocol definitions + * + * @author Loïc Dauphin + */ + +#ifndef FEETECH_PROTOCOL_H +#define FEETECH_PROTOCOL_H + +#ifdef __cplusplus +extern "C" { +#endif + +#define FEETECH_START (0xFF) + +typedef enum { + SCS15_B_1M = 0, + SCS15_B_0_5M = 1, + SCS15_B_250K = 2, + SCS15_B_128K = 3, + SCS15_B_115200 = 4, + SCS15_B_76800 = 5, + SCS15_B_57600 = 6, + SCS15_B_38400 = 7 +} scs15_baudrate_t; + +typedef enum { + SCS15_ID = 5, + SCS15_BAUD_RATE = 6, + SCS15_RETURN_DELAY_TIME = 7, + SCS15_RETURN_LEVEL = 8, + SCS15_LIMIT_TEMPERATURE = 13, + SCS15_MAX_LIMIT_VOLTAGE = 14, + SCS15_MIN_LIMIT_VOLTAGE = 15, + SCS15_ALARM_LED = 18, + SCS15_ALARM_SHUTDOWN = 19, + SCS15_COMPLIANCE_P = 21, + SCS15_COMPLIANCE_D = 22, + SCS15_COMPLIANCE_I = 23, + SCS15_CW_DEAD = 26, + SCS15_CCW_DEAD = 27, + SCS15_TORQUE_ENABLE = 40, + SCS15_LED = 41, + SCS15_LOCK = 48, + SCS15_PRESENT_VOLTAGE = 62, + SCS15_PRESENT_TEMPERATURE = 63, + SCS15_REGISTERED_INSTRUCTION = 64, + SCS15_ERROR = 65, + SCS15_MOVING = 66, +} scs15_register8_t; + +typedef enum { + SCS15_MODEL_NUMBER = 0, + SCS15_VERSION = 3, + SCS15_MIN_ANGLE_LIMIT = 9, + SCS15_MAX_ANGLE_LIMIT = 11, + SCS15_MAX_TORQUE = 16, + SCS15_PUNCH = 24, + SCS15_IMAX = 28, + SCS15_OFFSET = 30, + SCS15_GOAL_POSITION = 42, + SCS15_GOAL_TIME = 44, + SCS15_GOAL_SPEED = 46, + SCS15_PRESENT_POSITION = 56, + SCS15_PRESENT_SPEED = 58, + SCS15_PRESENT_LOAD = 60, + SCS15_VIR_POSITION = 67, + SCS15_CURRENT = 69, +} scs15_register16_t; + +typedef enum { + INST_PING = 0x01, + INST_READ = 0x02, + INST_WRITE = 0x03, + INST_REG_WRITE = 0x04, + INST_ACTION = 0x05, + INST_RESET = 0x06, + INST_SYNC_WRITE = 0x83, +} feetech_intruction_t; + +#ifdef __cplusplus +} +#endif + +#endif +/** @} */ diff --git a/drivers/feetech/include/feetech_reader.h b/drivers/feetech/include/feetech_reader.h new file mode 100644 index 0000000000..a119819454 --- /dev/null +++ b/drivers/feetech/include/feetech_reader.h @@ -0,0 +1,297 @@ +/* + * 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_feetech + * + * @{ + * + * @file + * @brief Interface definition for Feetech packet reader + * + * @author Loïc Dauphin + */ + +#ifndef FEETECH_READER_H +#define FEETECH_READER_H + +#include "feetech_protocol.h" + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#define FEETECH_ACK_SIZE (6) +#define FEETECH_RESPONSE_SIZE(len) (6 + len) + +/** + * @brief Feetech packet reader struct + */ +typedef struct { + const uint8_t *buffer; /**< data buffer */ + size_t size; /**< data buffer's size */ +} feetech_reader_t; + +/** + * @brief Initialize the Feetech 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 feetech_reader_init(feetech_reader_t *reader, const uint8_t *buffer, size_t size) +{ + reader->buffer = buffer; + reader->size = size; +} + +/** + * @brief Compute the packet's sum + * + * @param[in] reader the packet reader + * + * @return the sum of the packet + */ +uint8_t feetech_reader_compute_sum(const feetech_reader_t *reader); + +/** + * @brief Check if the packet has the minimum required size + * + * @param[in] reader the packet reader + * + * @return true if the packet has the minimum required size + * @return false otherwise + */ +static inline bool feetech_reader_check_minsize(const feetech_reader_t *reader) +{ + return 5 < reader->size; +} + + +/** + * @brief Check if the packet begins with 2 FEETECH_START bits + * + * @param[in] reader the packet reader + * + * @return true if the packet begins with 2 FEETECH_START bits + * @return false otherwise + */ +static inline bool feetech_reader_check_start(const feetech_reader_t *reader) +{ + return + reader->buffer[0] == FEETECH_START && + reader->buffer[1] == FEETECH_START; +} + +/** + * @brief Check if the packet's size is the same as the buffer's size + * + * @param[in] reader the packet reader + * + * @return true if the packet's size is the same as the buffer's size + * @return false otherwise + */ +static inline bool feetech_reader_check_size(const feetech_reader_t *reader) +{ + return reader->size == (size_t)(reader->buffer[3] + 4); +} + +/** + * @brief Check if the computed sum and the sum of the packet are equal + * + * @param[in] reader the packet reader + * + * @return true if the computed sum and the sum of the packet are equal + * @return false otherwise + */ +static inline bool feetech_reader_check_sum(const feetech_reader_t *reader) +{ + return feetech_reader_compute_sum(reader) == reader->buffer[reader->size - 1]; +} + +/** + * @brief Check if the packet is valid + * + * @param[in] reader the packet reader + * + * @return true if the packet is valid + * @return false otherwise + */ +bool feetech_reader_is_valid(const feetech_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 feetech_reader_get_id(const feetech_reader_t *reader) +{ + return reader->buffer[2]; +} + +/** + * @brief Get the packet's instruction code + * + * @param[in] reader the packet reader + * + * @return the packet's instruction code + */ +static inline uint8_t feetech_reader_get_instr(const feetech_reader_t *reader) +{ + return reader->buffer[4]; +} + +/** + * @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 *feetech_reader_response_get_payload(const feetech_reader_t *reader) +{ + return &reader->buffer[5]; +} + +/** + * @brief Get the packet's payload size (response) + * + * @param[in] reader the packet reader + * + * @return the size of the payload + */ +static inline size_t feetech_reader_response_get_payload_size(const feetech_reader_t *reader) +{ + return reader->buffer[3] - 2; +} + +/** + * @brief Get the packet's payload (WRITE) + * + * @param[in] reader the packet reader + * + * @return the begining addess of the payload + */ +static inline const uint8_t *feetech_reader_write_get_payload(const feetech_reader_t *reader) +{ + return &reader->buffer[6]; +} + +/** + * @brief Get the packet's payload size (WRITE) + * + * @param[in] reader the packet reader + * + * @return the size of the payload + */ +static inline size_t feetech_reader_write_get_payload_size(const feetech_reader_t *reader) +{ + return reader->buffer[3] - 3; +} + +/** + * @brief Get the packet's target register address (WRITE) + * + * @param[in] reader the packet reader + * + * @return the register address + */ +static inline uint8_t feetech_reader_write_get_reg(const feetech_reader_t *reader) +{ + return reader->buffer[5]; +} + +/** + * @brief Get the packet's READ size + * + * @param[in] reader the packet reader + * + * @return the READ size + */ +static inline size_t feetech_reader_read_get_size(const feetech_reader_t *reader) +{ + return reader->buffer[6]; +} + +/** + * @brief Get the packet's target register address (READ) + * + * @param[in] reader the packet reader + * + * @return the register address + */ +static inline uint8_t feetech_reader_read_get_reg(const feetech_reader_t *reader) +{ + return reader->buffer[5]; +} + +/** + * @brief Get the packet items' payload size (SYNC_WRITE) + * + * @param[in] reader the packet reader + * + * @return the size of the items' payload + */ +static inline size_t feetech_reader_sync_write_get_payload_size(const feetech_reader_t *reader) +{ + return reader->buffer[6]; +} + +/** + * @brief Get the packet's target register address (SYNC_WRITE) + * + * @param[in] reader the packet reader + * + * @return the register address + */ +static inline uint8_t feetech_reader_sync_write_get_reg(const feetech_reader_t *reader) +{ + return reader->buffer[5]; +} + +/** + * @brief Get the packet items' count (SYNC_WRITE) + * + * @param[in] reader the packet reader + * + * @return the number of items in the packet + */ +size_t feetech_reader_sync_write_get_items_count(const feetech_reader_t *reader); + +/** + * @brief Get the packet item's device id (SYNC_WRITE) + * + * @param[in] reader the packet reader + * @param[in] index the item index + * + * @return the item's device id + */ +uint8_t feetech_reader_sync_write_item_get_id(const feetech_reader_t *reader, uint8_t index); + +/** + * @brief Get the packet item's payload (SYNC_WRITE) + * + * @param[in] reader the packet reader + * @param[in] index the item index + * + * @return the begining addess of the payload + */ +const uint8_t *feetech_reader_sync_write_item_get_payload(const feetech_reader_t *reader, uint8_t index); + +#ifdef __cplusplus +} +#endif + +#endif +/** @} */ diff --git a/drivers/feetech/include/feetech_writer.h b/drivers/feetech/include/feetech_writer.h new file mode 100644 index 0000000000..a09210430d --- /dev/null +++ b/drivers/feetech/include/feetech_writer.h @@ -0,0 +1,185 @@ +/* + * 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_feetech + * + * @{ + * + * @file + * @brief Interface definition for Feetech packet writer + * + * @author Loïc Dauphin + */ + +#ifndef FEETECH_WRITER_H +#define FEETECH_WRITER_H + +#include "feetech_protocol.h" + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief Feetech packet writer struct + */ +typedef struct { + uint8_t *buffer; /**< data buffer */ + size_t size; /**< packet's size */ + size_t buffer_max_size; /**< data buffer's size */ +} feetech_writer_t; + +/** + * @brief Initialize the Feetech packet writer + * + * @param[out] writer the packet writer + * @param[in] buffer the buffer used to store data + * @param[in] buffer_max_size the size of the buffer (= maximum packet size) + */ +void feetech_writer_init(feetech_writer_t *writer, uint8_t *buffer, size_t buffer_max_size); + +/** + * @brief Get the data buffer to send + * + * @param[out] writer the packet writer + * + * @return the begining address of the buffer + */ +const uint8_t *feetech_writer_get_data(const feetech_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 feetech_writer_get_size(const feetech_writer_t *writer); + +/** + * @brief Build a response packet + * + * @param[out] writer the packet writer + * @param[in] id the responder's id + * @param[in] buffer the response data + * @param[in] size the response size + */ +void feetech_writer_response_make(feetech_writer_t *writer, uint8_t id, const uint8_t *buffer, size_t size); + +/** + * @brief Build an ack packet + * + * @param[out] writer the packet writer + * @param[in] id the responder's id + */ +void feetech_writer_ack_make(feetech_writer_t *writer, uint8_t id); + +/** + * @brief Build a PING packet + * + * @param[out] writer the packet writer + * @param[in] id the destination's id + */ +void feetech_writer_ping_make(feetech_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 feetech_writer_write_make(feetech_writer_t *writer, uint8_t id, uint8_t reg, const uint8_t *buffer, size_t size); + +/** + * @brief Build a WRITE packet (8 bits) + * + * @param[out] writer the packet writer + * @param[in] id the destination's id + * @param[in] reg the register to write in + * @param[in] value the value to write in the register + */ +void feetech_writer_write8_make(feetech_writer_t *writer, uint8_t id, uint8_t reg, uint8_t value); + +/** + * @brief Build a WRITE packet (16 bits) + * + * @param[out] writer the packet writer + * @param[in] id the destination's id + * @param[in] reg the register to write in + * @param[in] value the value to write in the register + */ +void feetech_writer_write16_make(feetech_writer_t *writer, uint8_t id, uint8_t reg, uint16_t value); + +/** + * @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 feetech_writer_read_make(feetech_writer_t *writer, uint8_t id, uint8_t reg, size_t size); + +/** + * @brief Begin to build a SYNC_WRITE packet + * + * @param[out] writer the packet writer + * @param[in] reg the register to write in + * @param[in] size the data buffer's size + */ +void feetech_writer_sync_write_begin(feetech_writer_t *writer, uint8_t reg, size_t size); + +/** + * @brief End the building of a SYNC_WRITE packet + * + * @param[out] writer the packet writer + */ +void feetech_writer_sync_write_end(feetech_writer_t *writer); + +/** + * @brief Add an item to a SYNC_WRITE packet + * + * @param[out] writer the packet writer + * @param[in] id the destination's id + * @param[in] buffer the data buffer to write + * @param[in] size the data buffer's size + */ +void feetech_writer_sync_write_add(feetech_writer_t *writer, uint8_t id, const uint8_t *buffer, size_t size); + +/** + * @brief Add an item to a SYNC_WRITE packet (8 bits) + * + * @param[out] writer the packet writer + * @param[in] id the destination's id + * @param[in] value the value to write + */ +void feetech_writer_sync_write_add_8bits(feetech_writer_t *writer, uint8_t id, uint8_t value); + +/** + * @brief Add an item to a SYNC_WRITE packet (16 bits) + * + * @param[out] writer the packet writer + * @param[in] id the destination's id + * @param[in] value the value to write + */ +void feetech_writer_sync_write_add_16bits(feetech_writer_t *writer, uint8_t id, uint16_t value); + +#ifdef __cplusplus +} +#endif + +#endif +/** @} */ diff --git a/drivers/feetech/reader.c b/drivers/feetech/reader.c new file mode 100644 index 0000000000..62950aa0c5 --- /dev/null +++ b/drivers/feetech/reader.c @@ -0,0 +1,59 @@ +/* + * 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_feetech + * @{ + * + * @file + * @brief Feetech messages reader + * + * @author Loïc Dauphin + * + * @} + */ + +#include "feetech_reader.h" + +static uint8_t _compute_sum(const feetech_reader_t *reader) +{ + uint8_t sum = 0; + for (size_t i = 2 ; i < reader->size-1 ; i++) { + sum += reader->buffer[i]; + } + return sum; +} + +uint8_t feetech_reader_compute_sum(const feetech_reader_t *reader) +{ + return ~_compute_sum(reader); +} + +bool feetech_reader_is_valid(const feetech_reader_t *reader) +{ + return + feetech_reader_check_minsize(reader) && + feetech_reader_check_start(reader) && + feetech_reader_check_size(reader) && + feetech_reader_check_sum(reader); +} + +size_t feetech_reader_sync_write_get_items_count(const feetech_reader_t *reader) +{ + return (reader->buffer[3] - 4) / (reader->buffer[6] + 1); +} + +uint8_t feetech_reader_sync_write_item_get_id(const feetech_reader_t *reader, uint8_t index) +{ + return reader->buffer[7 + index * (feetech_reader_sync_write_get_payload_size(reader) + 1)]; +} + +const uint8_t *feetech_reader_sync_write_item_get_payload(const feetech_reader_t *reader, uint8_t index) +{ + return &reader->buffer[7 + index * (feetech_reader_sync_write_get_payload_size(reader) + 1) + 1]; +} diff --git a/drivers/feetech/writer.c b/drivers/feetech/writer.c new file mode 100644 index 0000000000..c8b14db8ed --- /dev/null +++ b/drivers/feetech/writer.c @@ -0,0 +1,299 @@ +/* + * 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_feetech + * @{ + * + * @file + * @brief Feetech messages writer + * + * @author Loïc Dauphin + * + * @} + */ + +#include "feetech_writer.h" + +void feetech_writer_init(feetech_writer_t *writer, uint8_t *buffer, size_t buffer_max_size) +{ + writer->buffer = buffer; + writer->size = 0; + writer->buffer_max_size = buffer_max_size; +} + +const uint8_t *feetech_writer_get_data(const feetech_writer_t *writer) +{ + return (const uint8_t*)writer->buffer; +} + +size_t feetech_writer_get_size(const feetech_writer_t *writer) +{ + return writer->size; +} + +void feetech_writer_response_make(feetech_writer_t *writer, uint8_t id, const uint8_t *buffer, size_t size) +{ + const size_t len = 2 + size; + if (len + 4 <= writer->buffer_max_size) { + writer->size = len + 4; + + uint8_t sum = 0; + + writer->buffer[0] = FEETECH_START; + writer->buffer[1] = FEETECH_START; + sum += writer->buffer[2] = id; + sum += writer->buffer[3] = len; + sum += writer->buffer[4] = 0; + + for (size_t i = 0 ; i < size ; i++) { + sum += writer->buffer[5 + i] = buffer[i]; + } + + writer->buffer[size - 1] = ~sum; + } + else { + writer->size = 0; + } +} + + +void feetech_writer_ack_make(feetech_writer_t *writer, uint8_t id) +{ + const size_t len = 2; + if (len + 4 <= writer->buffer_max_size) { + writer->size = len + 4; + + uint8_t sum = 0; + + writer->buffer[0] = FEETECH_START; + writer->buffer[1] = FEETECH_START; + sum += writer->buffer[2] = id; + sum += writer->buffer[3] = len; + sum += writer->buffer[4] = 0; + writer->buffer[5] = ~sum; + } + else { + writer->size = 0; + } +} + +void feetech_writer_ping_make(feetech_writer_t *writer, uint8_t id) +{ + const size_t len = 2; + if (len + 4 <= writer->buffer_max_size) { + writer->size = len + 4; + + uint8_t sum = 0; + + writer->buffer[0] = FEETECH_START; + writer->buffer[1] = FEETECH_START; + sum += writer->buffer[2] = id; + sum += writer->buffer[3] = len; + sum += writer->buffer[4] = INST_PING; + writer->buffer[5] = ~sum; + } + else { + writer->size = 0; + } +} + +void feetech_writer_write8_make(feetech_writer_t *writer, uint8_t id, uint8_t reg, uint8_t value) +{ + const size_t len = 4; + if (len + 4 <= writer->buffer_max_size) { + writer->size = len + 4; + + uint8_t sum = 0; + + writer->buffer[0] = FEETECH_START; + writer->buffer[1] = FEETECH_START; + sum += writer->buffer[2] = id; + sum += writer->buffer[3] = len; + sum += writer->buffer[4] = INST_WRITE; + + sum += writer->buffer[5] = reg; + sum += writer->buffer[6] = value; + + writer->buffer[7] = ~sum; + } + else { + writer->size = 0; + } +} + +void feetech_writer_write16_make(feetech_writer_t *writer, uint8_t id, uint8_t reg, uint16_t value) +{ + const size_t len = 5; + if (len + 4 <= writer->buffer_max_size) { + writer->size = len + 4; + + uint8_t sum = 0; + + writer->buffer[0] = FEETECH_START; + writer->buffer[1] = FEETECH_START; + sum += writer->buffer[2] = id; + sum += writer->buffer[3] = len; + sum += writer->buffer[4] = INST_WRITE; + + sum += writer->buffer[5] = reg; + sum += writer->buffer[6] = (value >> 8) & 0xFF; + sum += writer->buffer[7] = value & 0xFF; + + writer->buffer[8] = ~sum; + } + else { + writer->size = 0; + } +} + +void feetech_writer_write_make(feetech_writer_t *writer, uint8_t id, uint8_t reg, const uint8_t *buffer, size_t size) +{ + const size_t len = 3 + size; + if (len + 4 <= writer->buffer_max_size) { + writer->size = len + 4; + + uint8_t sum = 0; + + writer->buffer[0] = FEETECH_START; + writer->buffer[1] = FEETECH_START; + sum += writer->buffer[2] = id; + sum += writer->buffer[3] = len; + sum += writer->buffer[4] = INST_WRITE; + + sum += writer->buffer[5] = reg; + for (size_t i = 0 ; i < size ; i++) { + sum += writer->buffer[6 + i] = buffer[i]; + } + + writer->buffer[writer->size - 1] = ~sum; + } + else { + writer->size = 0; + } +} + +void feetech_writer_read_make(feetech_writer_t *writer, uint8_t id, uint8_t reg, size_t size) +{ + const size_t len = 4; + if (len + 4 <= writer->buffer_max_size) { + writer->size = len + 4; + + uint8_t sum = 0; + + writer->buffer[0] = FEETECH_START; + writer->buffer[1] = FEETECH_START; + sum += writer->buffer[2] = id; + sum += writer->buffer[3] = len; + sum += writer->buffer[4] = INST_READ; + + sum += writer->buffer[5] = reg; + sum += writer->buffer[6] = (uint8_t)size; + + writer->buffer[7] = ~sum; + } + else { + writer->size = 0; + } +} + +size_t feetech_writer_sync_write_required(feetech_writer_t *writer) +{ + if (8 <= writer->size && writer->buffer[4] == INST_SYNC_WRITE) { + return writer->buffer[6]; + } + return 0; +} + +void feetech_writer_sync_write_end(feetech_writer_t *writer) +{ + if (writer->size <= 8) { + writer->size = 0; + } +} + +void feetech_writer_sync_write_add(feetech_writer_t *writer, uint8_t id, const uint8_t *buffer, size_t size) +{ + if (feetech_writer_sync_write_required(writer) == size && + size != 0 && writer->size + size + 1 <= writer->buffer_max_size) { + + uint8_t sum = ~writer->buffer[writer->size - 1]; + + sum += writer->buffer[writer->size - 1] = id; + for (size_t i = 0 ; i < size ; i++) { + sum += writer->buffer[writer->size + i] = buffer[i]; + } + + writer->buffer[3] += size + 1; + writer->buffer[writer->size + size] = ~(sum + size + 1); + writer->size += size + 1; + } + else { + writer->size = 0; + } +} + +void feetech_writer_sync_write_add_8bits(feetech_writer_t *writer, uint8_t id, uint8_t value) +{ + if (feetech_writer_sync_write_required(writer) == 1 && + writer->size + 2 <= writer->buffer_max_size) { + + uint8_t sum = ~writer->buffer[writer->size - 1]; + + sum += writer->buffer[writer->size - 1] = id; + sum += writer->buffer[writer->size] = value; + + writer->buffer[3] += 2; + writer->buffer[writer->size + 1] = ~(sum + 2); + writer->size += 2; + } + else { + writer->size = 0; + } +} + +void feetech_writer_sync_write_add_16bits(feetech_writer_t *writer, uint8_t id, uint16_t value) +{ + if (feetech_writer_sync_write_required(writer) == 2 && + writer->size + 3 <= writer->buffer_max_size) { + + uint8_t sum = ~writer->buffer[writer->size - 1]; + + sum += writer->buffer[writer->size - 1] = id; + sum += writer->buffer[writer->size + 0] = (value >> 8) & 0xFF; + sum += writer->buffer[writer->size + 1] = value & 0xFF; + + writer->buffer[3] += 3; + writer->buffer[writer->size + 2] = ~(sum + 3); + writer->size += 3; + } + else { + writer->size = 0; + } +} + +void feetech_writer_sync_write_begin(feetech_writer_t *writer, uint8_t reg, size_t size) +{ + const size_t len = 4; + if (len + 4 <= writer->buffer_max_size) { + writer->size = len + 4; + + uint8_t sum = 0; + + writer->buffer[0] = FEETECH_START; + writer->buffer[1] = FEETECH_START; + sum += writer->buffer[2] = 0xFF; + sum += writer->buffer[3] = len; + sum += writer->buffer[4] = INST_SYNC_WRITE; + + sum += writer->buffer[5] = reg; + sum += writer->buffer[6] = (uint8_t)size; + + writer->buffer[7] = ~sum; + } +} diff --git a/drivers/include/feetech.h b/drivers/include/feetech.h new file mode 100644 index 0000000000..1a155f5d91 --- /dev/null +++ b/drivers/include/feetech.h @@ -0,0 +1,170 @@ +/* + * 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_feetech Feetech driver + * @ingroup drivers_actuators + * + * This module contains drivers for any device using feetech'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 Feetech devices driver + * + * @author Loïc Dauphin + */ + +#ifndef FEETECH_H +#define FEETECH_H + +#include + +#include "feetech_protocol.h" +#include "uart_half_duplex.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef uint8_t feetech_id_t; /**< device id type */ +typedef uint8_t feetech_addr_t; /**< address type */ + +/** + * @brief Descriptor struct for a feetech device + */ +typedef struct { + uart_half_duplex_t *stream; /**< the stream used */ + feetech_id_t id; /**< the device address */ +} feetech_t; + +/** + * @brief Possible feetech return values + */ +enum { + FEETECH_OK, /**< Success */ + FEETECH_TIMEOUT, /**< No response from the device */ + FEETECH_BUFFER_TOO_SMALL, /**< Buffer is too small for the message */ + FEETECH_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 FEETECH_OK if a device answered + * @return FEETECH_TIMEOUT if the device did not answer + * @return FEETECH_BUFFER_TOO_SMALL if buffer is too small for the message + * @return FEETECH_INVALID_MESSAGE if an invalid message was received + */ +int feetech_ping(uart_half_duplex_t *stream, feetech_id_t id); + +/** + * @brief Initialize a Feetech device + * + * @param[out] device the Feetech device + * @param[in] stream the stream + * @param[in] id the device address + */ +void feetech_init(feetech_t *device, uart_half_duplex_t *stream, feetech_id_t id); + +/** + * @brief Write to a device 8bits address + * + * @param[in] device the Feetech device + * @param[in] addr the address to write + * @param[in] value the value to write + * + * @return FEETECH_OK on success + * @return FEETECH_TIMEOUT if the device did not answer + * @return FEETECH_BUFFER_TOO_SMALL if buffer is too small for the message + * @return FEETECH_INVALID_MESSAGE if an invalid message was received + */ +int feetech_write8(feetech_t *device, feetech_addr_t addr, uint8_t value); + +/** + * @brief Write to a device 16bits address + * + * @param[in] device the Feetech device + * @param[in] addr the address to write + * @param[in] value the value to write + * + * @return FEETECH_OK on success + * @return FEETECH_TIMEOUT if the device did not answer + * @return FEETECH_BUFFER_TOO_SMALL if buffer is too small for the message + * @return FEETECH_INVALID_MESSAGE if an invalid message was received + */ +int feetech_write16(feetech_t *device, feetech_addr_t addr, uint16_t value); + +/** + * @brief Write to a device address + * + * @param[in] device the Feetech device + * @param[in] addr the address to start write + * @param[in] data the data to write + * @param[in] length the data length + * + * @return FEETECH_OK on success + * @return FEETECH_TIMEOUT if the device did not answer + * @return FEETECH_BUFFER_TOO_SMALL if buffer is too small for the message + * @return FEETECH_INVALID_MESSAGE if an invalid message was received + */ +int feetech_write(feetech_t *device, feetech_addr_t addr, const uint8_t *data, size_t length); + +/** + * @brief Read from a device 8bits address + * + * @param[in] device the Feetech device + * @param[in] addr the address to read + * @param[out] value the value to read + * + * @return FEETECH_OK on success + * @return FEETECH_TIMEOUT if the device did not answer + * @return FEETECH_BUFFER_TOO_SMALL if buffer is too small for the message + * @return FEETECH_INVALID_MESSAGE if an invalid message was received + */ +int feetech_read8(feetech_t *device, feetech_addr_t addr, uint8_t *value); + +/** + * @brief Read from a device 16bits address + * + * @param[in] device the Feetech device + * @param[in] addr the address to read + * @param[out] value the value to read + * + * @return FEETECH_OK on success + * @return FEETECH_TIMEOUT if the device did not answer + * @return FEETECH_BUFFER_TOO_SMALL if buffer is too small for the message + * @return FEETECH_INVALID_MESSAGE if an invalid message was received + */ +int feetech_read16(feetech_t *device, feetech_addr_t addr, uint16_t *value); + +/** + * @brief Read from a device address + * + * @param[in] device the Feetech device + * @param[in] addr the address to start read + * @param[out] data the data buffer to fill + * @param[in] length the data length + * + * @return FEETECH_OK on success + * @return FEETECH_TIMEOUT if the device did not answer + * @return FEETECH_BUFFER_TOO_SMALL if buffer is too small for the message + * @return FEETECH_INVALID_MESSAGE if an invalid message was received + */ +int feetech_read(feetech_t *device, feetech_addr_t addr, uint8_t *data, size_t length); + +#ifdef __cplusplus +} +#endif + +#endif +/** @} */