dynamixel.c
3.38 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
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(const 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(const dynamixel_t *device, dynamixel_addr_t reg, uint8_t value)
{
return dynamixel_write(device, reg, &value, 1);
}
int dynamixel_write16(const dynamixel_t *device, dynamixel_addr_t reg, uint16_t value)
{
return dynamixel_write(device, reg, (uint8_t*)&value, 2);
}
int dynamixel_read(const 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(const dynamixel_t *device, dynamixel_addr_t reg, uint8_t *value)
{
return dynamixel_read(device, reg, value, 1);
}
int dynamixel_read16(const dynamixel_t *device, dynamixel_addr_t reg, uint16_t *value)
{
return dynamixel_read(device, reg, (uint8_t*)value, 2);
}