RIOT-OS/RIOT
RIOT-OS/RIOT
1
0
Fork 0
mirror of https://github.com/RIOT-OS/RIOT.git synced 2024-12-29 04:50:03 +01:00
Code Releases Activity

drivers/feetech: initial support

Browse Source
This commit is contained in:
Loïc Dauphin 2017-03-27 15:51:11 +02:00
parent ebe9b37444
commit 08a066a0db
10 changed files with 1250 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
drivers/Makefile.dep
View File

@ -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

3
drivers/Makefile.include
View File

@ -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

1
drivers/feetech/Makefile Normal file
View File

@ -0,0 +1 @@
include $(RIOTBASE)/Makefile.base

133
drivers/feetech/feetech.c Normal file
View File

@ -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 <loic.dauphin@inria.fr>
*
* @}
*/
#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 <string.h>
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;
}

99
drivers/feetech/include/feetech_protocol.h Normal file
View File

@ -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 <loic.dauphin@inria.fr>
*/
#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
/** @} */

297
drivers/feetech/include/feetech_reader.h Normal file
View File

@ -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 <loic.dauphin@inria.fr>
*/
#ifndef FEETECH_READER_H
#define FEETECH_READER_H
#include "feetech_protocol.h"
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#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
/** @} */

185
drivers/feetech/include/feetech_writer.h Normal file
View File

@ -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 <loic.dauphin@inria.fr>
*/
#ifndef FEETECH_WRITER_H
#define FEETECH_WRITER_H
#include "feetech_protocol.h"
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#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
/** @} */

59
drivers/feetech/reader.c Normal file
View File

@ -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 <loic.dauphin@inria.fr>
*
* @}
*/
#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];
}

299
drivers/feetech/writer.c Normal file
View File

@ -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 <loic.dauphin@inria.fr>
*
* @}
*/
#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;
}
}

170
drivers/include/feetech.h Normal file
View File

@ -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 <loic.dauphin@inria.fr>
*/
#ifndef FEETECH_H
#define FEETECH_H
#include <stdlib.h>
#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
/** @} */