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/dynamixel: initial support

Browse Source
This commit is contained in:
Loïc Dauphin 2017-03-31 11:35:27 +02:00
parent 172203dc9d
commit 47cfe0ac66
12 changed files with 935 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
drivers/Makefile.dep
View File

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

3
drivers/Makefile.include
View File

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

1
drivers/dynamixel/Makefile Normal file
View File

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

65
drivers/dynamixel/crc.c Normal file
View File

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

124
drivers/dynamixel/dynamixel.c Normal file
View File

@ -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 <loic.dauphin@inria.fr>
*
* @}
*/
#include "dynamixel.h"
#include "dynamixel_protocol.h"
#include "dynamixel_reader.h"
#include "dynamixel_writer.h"
#include <string.h>
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);
}

37
drivers/dynamixel/include/dynamixel_crc.h Normal file
View File

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

95
drivers/dynamixel/include/dynamixel_protocol.h Normal file
View File

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

150
drivers/dynamixel/include/dynamixel_reader.h Normal file
View File

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

102
drivers/dynamixel/include/dynamixel_writer.h Normal file
View File

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

54
drivers/dynamixel/reader.c Normal file
View File

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

129
drivers/dynamixel/writer.c Normal file
View File

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

171
drivers/include/dynamixel.h Normal file
View File

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