usb2dynamixel/USB2Dynamixel.cpp at master · gottliebtfreitag/usb2dynamixel · GitHub

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
125
126
127
128
129
130
#include "USB2Dynamixel.h"

#include <cstdio>
#include <cstring>

#include <condition_variable>
#include <sstream>
#include <thread>

#include <simplyfile/SerialPort.h>
#include "ProtocolV1.h"
#include "ProtocolV2.h"
#include "file_io.h"

namespace dynamixel {

USB2Dynamixel::USB2Dynamixel(int baudrate, std::string const& device, Protocol protocol)
	: mPort(device, baudrate)
{
	file_io::flushRead(mPort);
	if (protocol == Protocol::V1) {
		mProtocol = std::make_unique<ProtocolV1>();
	} else {
		mProtocol = std::make_unique<ProtocolV2>();
	}
}

USB2Dynamixel::~USB2Dynamixel() {
}

bool USB2Dynamixel::ping(MotorID motor, Timeout timeout) const {
	auto g = std::lock_guard(mMutex);
	file_io::write(mPort, mProtocol->createPacket(motor, Instruction::PING, {}));
	auto [timeoutFlag, motorID, errorCode, rxBuf] = mProtocol->readPacket(timeout, motor, 0, mPort);
	return motorID != MotorIDInvalid;
}

auto USB2Dynamixel::read(MotorID motor, int baseRegister, size_t length, Timeout timeout) const -> std::tuple<bool, MotorID, ErrorCode, Parameter> {
	std::vector<std::byte> txBuf;
	for (auto b : mProtocol->convertAddress(baseRegister)) {
		txBuf.push_back(b);
	}
	for (auto b : mProtocol->convertLength(length)) {
		txBuf.push_back(b);
	}

	auto g = std::lock_guard(mMutex);
	file_io::write(mPort, mProtocol->createPacket(motor, Instruction::READ, txBuf));
	return mProtocol->readPacket(timeout, motor, length, mPort);
}

auto USB2Dynamixel::bulk_read(std::vector<std::tuple<MotorID, int, size_t>> const& motors, Timeout timeout) const -> std::vector<std::tuple<MotorID, int, ErrorCode, Parameter>> {

	std::vector<std::tuple<MotorID, int, ErrorCode, Parameter>> resList;
	resList.reserve(motors.size());

	auto txBuf = mProtocol->buildBulkReadPackage(motors);

	auto g = std::lock_guard(mMutex);
	file_io::write(mPort, mProtocol->createPacket(BroadcastID, Instruction::BULK_READ, txBuf));

	for (auto const& [id, baseRegister, length] : motors) {
		auto [timeoutFlag, motorID, errorCode, rxBuf] = mProtocol->readPacket(timeout, id, length, mPort);
		if (motorID == MotorIDInvalid or motorID != id) {
			break;
		}
		resList.push_back(std::make_tuple(id, baseRegister, errorCode, rxBuf));
	}
	return resList;
}

void USB2Dynamixel::write(MotorID motor, int baseRegister, Parameter const& txBuf) const {
	std::vector<std::byte> parameters;
	for (auto b : mProtocol->convertAddress(baseRegister)) {
		parameters.push_back(b);
	}
	parameters.insert(parameters.end(), txBuf.begin(), txBuf.end());
	auto g = std::lock_guard(mMutex);
	file_io::write(mPort, mProtocol->createPacket(motor, Instruction::WRITE, parameters));
}
auto USB2Dynamixel::writeRead(MotorID motor, int baseRegister, Parameter const& txBuf, Timeout timeout) const -> std::tuple<bool, MotorID, ErrorCode, Parameter> {
	write(motor, baseRegister, txBuf);
	return mProtocol->readPacket(timeout, motor, 0, mPort);
}


void USB2Dynamixel::sync_write(std::map<MotorID, Parameter> const& motorParams, int baseRegister) const {
	auto g = std::lock_guard(mMutex);

	if (motorParams.empty()) {
		throw std::runtime_error("sync_write: motorParams can't be empty");
	}

	const size_t len = motorParams.begin()->second.size();
	bool const okay = std::all_of(begin(motorParams), end(motorParams), [&](auto param) {
		return param.second.size() == len;
	});

	if (len <= 0 or not okay) {
		throw std::runtime_error("sync_write: data is not consistent");
	}

	Parameter txBuf;
	for (auto b : mProtocol->convertAddress(baseRegister)) {
		txBuf.push_back(b);
	}
	for (auto b : mProtocol->convertLength(len)) {
		txBuf.push_back(b);
	}

	for (auto const& [id, params] : motorParams) {
		txBuf.push_back(std::byte{id});
		txBuf.insert(txBuf.end(), params.begin(), params.end());
	}

	file_io::write(mPort, mProtocol->createPacket(BroadcastID, Instruction::SYNC_WRITE, txBuf));
}

void USB2Dynamixel::reset(MotorID motor) const {
	auto g = std::lock_guard(mMutex);
	file_io::write(mPort, mProtocol->createPacket(motor, Instruction::RESET, {}));

}

void USB2Dynamixel::reboot(MotorID motor) const {
	auto g = std::lock_guard(mMutex);
	file_io::write(mPort, mProtocol->createPacket(motor, Instruction::REBOOT, {}));
}

}