├── .gitignore
├── src
    ├── simplyfile
    │   ├── ThreadTime.h
    │   ├── FileDescriptor.cpp
    │   ├── README.md
    │   ├── .gitrepo
    │   ├── ThreadTime.cpp
    │   ├── SerialPort.h
    │   ├── Timer.h
    │   ├── socket
    │   │   ├── Host.h
    │   │   ├── Socket.h
    │   │   ├── Host.cpp
    │   │   └── Socket.cpp
    │   ├── INotify.h
    │   ├── Event.h
    │   ├── INotify.cpp
    │   ├── LICENSE
    │   ├── FileDescriptor.h
    │   ├── Timer.cpp
    │   ├── SerialPort.cpp
    │   ├── Epoll.h
    │   └── Epoll.cpp
    ├── usb2dynamixel
    │   ├── Layout.h
    │   ├── file_io.h
    │   ├── .gitrepo
    │   ├── LICENSE
    │   ├── ProtocolV1.h
    │   ├── ProtocolV2.h
    │   ├── MotorMetaInfo.h
    │   ├── dynamixel.h
    │   ├── MotorMetaInfo.cpp
    │   ├── file_io.cpp
    │   ├── ProtocolBase.h
    │   ├── USB2Dynamixel.h
    │   ├── USB2Dynamixel.cpp
    │   ├── LayoutXL320.h
    │   ├── LayoutAX.h
    │   ├── LayoutPart.h
    │   ├── LayoutMX_V1.h
    │   ├── LayoutXL320.cpp
    │   ├── ProtocolV1.cpp
    │   ├── LayoutPro.h
    │   ├── LayoutAX.cpp
    │   ├── LayoutMX_V2.h
    │   └── LayoutMX_V1.cpp
    ├── utils
    │   ├── ExceptionPrinter.h
    │   ├── Singleton.h
    │   ├── Finally.h
    │   ├── demangle.h
    │   ├── demangle.cpp
    │   ├── ExceptionPrinter.cpp
    │   ├── Factory.h
    │   ├── CanCallBack.h
    │   └── Registry.h
    ├── commonTasks.h
    ├── sargparse
    │   ├── .gitignore
    │   ├── .gitrepo
    │   ├── Parameter.cpp
    │   ├── ArgumentParsing.h
    │   ├── LICENSE
    │   ├── README.md
    │   └── ParameterParsing.h
    ├── simplyfuse
    │   ├── .gitrepo
    │   ├── LICENSE
    │   ├── FuseFile.cpp
    │   ├── README.md
    │   ├── FuseFS.h
    │   ├── FuseFile.h
    │   └── FuseFS.cpp
    ├── rebootCmd.cpp
    ├── globalOptions.h
    ├── commonTasks.cpp
    ├── main.cpp
    ├── globalOptions.cpp
    ├── metaCmd.cpp
    ├── setValues.cpp
    ├── interactCmd.cpp
    ├── fsCommand.cpp
    └── detect.cpp
├── scripts
    ├── bash_completion
    ├── zsh_completion
    └── generateMan.sh
├── LICENSE
├── Makefile
└── README.md


/.gitignore:
--------------------------------------------------------------------------------
1 | .cproject
2 | .project
3 | inspexel
4 | obj/
5 | 


--------------------------------------------------------------------------------
/src/simplyfile/ThreadTime.h:
--------------------------------------------------------------------------------
1 | #pragma once
2 | 
3 | #include <chrono>
4 | 
5 | namespace simplyfile
6 | {
7 | std::chrono::nanoseconds getThreadTime();
8 | }
9 | 


--------------------------------------------------------------------------------
/src/usb2dynamixel/Layout.h:
--------------------------------------------------------------------------------
1 | #pragma once
2 | 
3 | #include "LayoutMX_V1.h"
4 | #include "LayoutMX_V2.h"
5 | #include "LayoutPro.h"
6 | #include "LayoutXL320.h"
7 | #include "LayoutAX.h"
8 | 


--------------------------------------------------------------------------------
/src/utils/ExceptionPrinter.h:
--------------------------------------------------------------------------------
1 | #pragma once
2 | 
3 | #include <string>
4 | #include <stdexcept>
5 | 
6 | struct ExceptionPrinter {
7 | 	static std::string print(std::exception const& e);
8 | };
9 | 


--------------------------------------------------------------------------------
/src/simplyfile/FileDescriptor.cpp:
--------------------------------------------------------------------------------
 1 | #include "FileDescriptor.h"
 2 | 
 3 | #include <stdexcept>
 4 | #include <cstring>
 5 | #include <sys/ioctl.h>
 6 | #include <sys/un.h>
 7 | 
 8 | namespace simplyfile {
 9 | 
10 | }
11 | 


--------------------------------------------------------------------------------
/src/simplyfile/README.md:
--------------------------------------------------------------------------------
1 | # simplyfile
2 | 
3 | C++ wrappers around unix file descriptors
4 | 
5 | Most of this code is super easy to use and the epoll abstraction can be 
6 | used as super easy yet super scaling load balancer 
7 | 


--------------------------------------------------------------------------------
/src/utils/Singleton.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | 
 4 | template<typename T>
 5 | struct Singleton {
 6 | 	static T& getInstance() {
 7 | 		static T instance;
 8 | 		return instance;
 9 | 	}
10 | protected:
11 | 	Singleton() = default;
12 | 	~Singleton() = default;
13 | };
14 | 
15 | 


--------------------------------------------------------------------------------
/scripts/bash_completion:
--------------------------------------------------------------------------------
1 | _GetInspexel ()
2 | {  
3 |   HINTS=$(${COMP_LINE[0]} ${COMP_WORDS[@]:1} --bash_completion)
4 |   COMPREPLY=( $( compgen -W '${HINTS}' -- "${COMP_WORDS[COMP_CWORD]}"))
5 |   return 0
6 | }
7 | complete -F _GetInspexel ./inspexel
8 | complete -F _GetInspexel inspexel
9 | 


--------------------------------------------------------------------------------
/src/usb2dynamixel/file_io.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <vector>
 4 | #include <cstddef>
 5 | 
 6 | namespace dynamixel::file_io {
 7 | auto read(int _fd, size_t maxReadBytes) -> std::vector<std::byte>;
 8 | size_t flushRead(int _fd);
 9 | void write(int _fd, std::vector<std::byte> const& txBuf);
10 | }


--------------------------------------------------------------------------------
/src/utils/Finally.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | template<typename Func>
 4 | struct Finally final {
 5 | 	Finally(Func && f) : _f(f) {}
 6 | 	~Finally() {_f();}
 7 | private:
 8 | 	Func _f;
 9 | };
10 | 
11 | template<typename Func>
12 | Finally<Func> makeFinally(Func && f) { return Finally<Func>(std::move(f)); }
13 | 
14 | 
15 | 


--------------------------------------------------------------------------------
/src/commonTasks.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include "usb2dynamixel/USB2Dynamixel.h"
 4 | #include "usb2dynamixel/MotorMetaInfo.h"
 5 | 
 6 | #include <chrono>
 7 | 
 8 | auto detectMotor(dynamixel::MotorID motor, dynamixel::USB2Dynamixel& usb2dyn, std::chrono::microseconds timeout) -> std::tuple<dynamixel::LayoutType, uint16_t>;
 9 | 
10 | 


--------------------------------------------------------------------------------
/src/utils/demangle.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <cxxabi.h>
 4 | #include <exception>
 5 | #include <iostream>
 6 | 
 7 | std::string demangle(std::type_info const& ti);
 8 | 
 9 | template<typename T>
10 | std::string demangle(T const& t) {
11 | 	return demangle(typeid(t));
12 | }
13 | 
14 | template<typename T>
15 | std::string demangle() {
16 | 	return demangle(typeid(T));
17 | }
18 | 
19 | 


--------------------------------------------------------------------------------
/src/utils/demangle.cpp:
--------------------------------------------------------------------------------
 1 | 
 2 | #include "demangle.h"
 3 | 
 4 | 
 5 | std::string demangle(std::type_info const& ti) {
 6 | 	int status;
 7 | 	char* name_ = abi::__cxa_demangle(ti.name(), 0, 0, &status);
 8 | 	if (status) {
 9 | 		throw std::runtime_error("cannot demangle a type!");
10 | 	}
11 | 	std::string demangledName{ name_ };
12 | 	free(name_); // need to use free here :/
13 | 	return demangledName;
14 | }
15 | 


--------------------------------------------------------------------------------
/src/sargparse/.gitignore:
--------------------------------------------------------------------------------
 1 | # Prerequisites
 2 | *.d
 3 | 
 4 | # Compiled Object files
 5 | *.slo
 6 | *.lo
 7 | *.o
 8 | *.obj
 9 | 
10 | # Precompiled Headers
11 | *.gch
12 | *.pch
13 | 
14 | # Compiled Dynamic libraries
15 | *.so
16 | *.dylib
17 | *.dll
18 | 
19 | # Fortran module files
20 | *.mod
21 | *.smod
22 | 
23 | # Compiled Static libraries
24 | *.lai
25 | *.la
26 | *.a
27 | *.lib
28 | 
29 | # Executables
30 | *.exe
31 | *.out
32 | *.app
33 | 


--------------------------------------------------------------------------------
/src/sargparse/.gitrepo:
--------------------------------------------------------------------------------
 1 | ; DO NOT EDIT (unless you know what you are doing)
 2 | ;
 3 | ; This subdirectory is a git "subrepo", and this file is maintained by the
 4 | ; git-subrepo command. See https://github.com/git-commands/git-subrepo#readme
 5 | ;
 6 | [subrepo]
 7 | 	remote = git@github.com:gottliebtfreitag/sargparse
 8 | 	branch = master
 9 | 	commit = dc86929d7aa91ab07fc11a7178d4396cd73ff1d9
10 | 	parent = 3f2c37d211906ab4ba5feb695edba9d589a17f56
11 | 	cmdver = 0.3.1
12 | 


--------------------------------------------------------------------------------
/src/simplyfile/.gitrepo:
--------------------------------------------------------------------------------
 1 | ; DO NOT EDIT (unless you know what you are doing)
 2 | ;
 3 | ; This subdirectory is a git "subrepo", and this file is maintained by the
 4 | ; git-subrepo command. See https://github.com/git-commands/git-subrepo#readme
 5 | ;
 6 | [subrepo]
 7 | 	remote = git@github.com:gottliebtfreitag/simplyfile
 8 | 	branch = master
 9 | 	commit = 549e0aa4a7b29ac7afb84cdc7888c21355b00a1f
10 | 	parent = 1880e2728f7dddcefb535025f5858c83ad04be01
11 | 	cmdver = 0.3.1
12 | 


--------------------------------------------------------------------------------
/src/simplyfuse/.gitrepo:
--------------------------------------------------------------------------------
 1 | ; DO NOT EDIT (unless you know what you are doing)
 2 | ;
 3 | ; This subdirectory is a git "subrepo", and this file is maintained by the
 4 | ; git-subrepo command. See https://github.com/git-commands/git-subrepo#readme
 5 | ;
 6 | [subrepo]
 7 | 	remote = git@github.com:gottliebtfreitag/simplyfuse
 8 | 	branch = master
 9 | 	commit = bdd181e576eb2046bbf41f487add4cb2cc87b5b2
10 | 	parent = 82b4c98439351e081fe3523b460221221aa05242
11 | 	cmdver = 0.3.1
12 | 


--------------------------------------------------------------------------------
/src/usb2dynamixel/.gitrepo:
--------------------------------------------------------------------------------
 1 | ; DO NOT EDIT (unless you know what you are doing)
 2 | ;
 3 | ; This subdirectory is a git "subrepo", and this file is maintained by the
 4 | ; git-subrepo command. See https://github.com/git-commands/git-subrepo#readme
 5 | ;
 6 | [subrepo]
 7 | 	remote = git@github.com:gottliebtfreitag/usb2dynamixel
 8 | 	branch = master
 9 | 	commit = db348e3ea3c757c1d01e4449d32051fb8e3347c9
10 | 	parent = 780b57a7d674a14d0c15b0976744a5f1618707cf
11 | 	cmdver = 0.3.1
12 | 


--------------------------------------------------------------------------------
/src/rebootCmd.cpp:
--------------------------------------------------------------------------------
 1 | #include "usb2dynamixel/USB2Dynamixel.h"
 2 | 
 3 | #include "globalOptions.h"
 4 | 
 5 | namespace {
 6 | 
 7 | void runReboot();
 8 | auto rebootCmd = sargp::Command{"reboot", "reboot device with specified id", runReboot};
 9 | 
10 | void runReboot() {
11 | 	if (not g_id) {
12 | 		throw std::runtime_error("must specify a id");
13 | 	}
14 | 	auto usb2dyn = dynamixel::USB2Dynamixel(*g_baudrate, *g_device, *g_protocolVersion);
15 | 	usb2dyn.reboot(*g_id);
16 | }
17 | 
18 | }
19 | 


--------------------------------------------------------------------------------
/src/simplyfile/ThreadTime.cpp:
--------------------------------------------------------------------------------
 1 | #include "ThreadTime.h"
 2 | #include <time.h>
 3 | #include <stdexcept>
 4 | 
 5 | namespace simplyfile
 6 | {
 7 | 
 8 | std::chrono::nanoseconds getThreadTime()
 9 | {
10 | 	struct timespec ts{};
11 | 	if (0 != clock_gettime(CLOCK_THREAD_CPUTIME_ID, &ts)) {
12 | 		throw std::runtime_error("cannot call clock_gettime for CLOCK_THREAD_CPUTIME_ID");
13 | 	}
14 | 	return std::chrono::nanoseconds{ts.tv_sec*1000000000} + std::chrono::nanoseconds{ts.tv_nsec};
15 | }
16 | }
17 | 


--------------------------------------------------------------------------------
/src/simplyfile/SerialPort.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include "FileDescriptor.h"
 4 | 
 5 | #include <string>
 6 | 
 7 | namespace simplyfile
 8 | {
 9 | 
10 | struct SerialPort : FileDescriptor
11 | {
12 | 	SerialPort() = default;
13 | 	SerialPort(std::string const& name, int baudrate);
14 | 	SerialPort(SerialPort&&) noexcept = default;
15 | 	SerialPort& operator=(SerialPort&&) noexcept = default;
16 | 	virtual ~SerialPort() = default;
17 | 
18 | 	void setBaudrate(int baudrate);
19 | };
20 | 
21 | 
22 | }
23 | 


--------------------------------------------------------------------------------
/src/utils/ExceptionPrinter.cpp:
--------------------------------------------------------------------------------
 1 | #include "ExceptionPrinter.h"
 2 | 
 3 | namespace
 4 | {
 5 | 
 6 | 
 7 | std::string print_(std::exception const& e, int level) {
 8 | 	std::string ret = std::string(level, ' ') + e.what();
 9 | 	try {
10 | 		std::rethrow_if_nested(e);
11 | 	} catch(const std::exception& nested) {
12 | 		ret += "\n" + print_(nested, level+1);
13 | 	} catch(...) {}
14 | 	return ret;
15 | }
16 | 
17 | }
18 | 
19 | std::string ExceptionPrinter::print(std::exception const& e) {
20 | 	return print_(e, 0);
21 | }
22 | 


--------------------------------------------------------------------------------
/src/simplyfile/Timer.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include "FileDescriptor.h"
 4 | 
 5 | #include <chrono>
 6 | #include <sys/timerfd.h>
 7 | 
 8 | namespace simplyfile {
 9 | 
10 | struct Timer : FileDescriptor {
11 | 	using FileDescriptor::FileDescriptor;
12 | 	Timer(std::chrono::nanoseconds duration, bool oneShot=false, int flags=TFD_NONBLOCK);
13 | 
14 |     Timer(Timer&&) noexcept = default;
15 |     Timer& operator=(Timer&&) noexcept = default;
16 | 
17 | 	int getElapsed() const;
18 | 	void cancel();
19 | 	void reset(std::chrono::nanoseconds duration, bool oneShot=false);
20 | };
21 | 
22 | }
23 | 


--------------------------------------------------------------------------------
/src/simplyfile/socket/Host.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <sys/socket.h>
 4 | 
 5 | #include <string>
 6 | #include <vector>
 7 | 
 8 | namespace simplyfile
 9 | {
10 | 
11 | struct Host {
12 | 	int family {};
13 | 	int socktype {};
14 | 	int protocol {};
15 | 	socklen_t sockaddrLen {};
16 | 	struct sockaddr_storage sockaddr {};
17 | 
18 | 	std::string getName() const;
19 | };
20 | 
21 | std::vector<Host> getHosts(std::string const& node, std::string const& service, int socktype=SOCK_STREAM, int family = AF_UNSPEC);
22 | 
23 | Host makeUnixDomainHost(std::string const& path, int socktype=SOCK_STREAM);
24 | }
25 | 
26 | 
27 | 


--------------------------------------------------------------------------------
/src/sargparse/Parameter.cpp:
--------------------------------------------------------------------------------
 1 | #include "Parameter.h"
 2 | 
 3 | #include <algorithm>
 4 | 
 5 | namespace sargp
 6 | {
 7 | 
 8 | ParameterBase::ParameterBase(std::string const& argName, DescribeFunc const& describeFunc, Callback cb, ValueHintFunc const& hintFunc, Command& command)
 9 | 	: _argName(argName)
10 | 	, _describeFunc(describeFunc)
11 | 	, _cb(cb)
12 | 	, _hintFunc(hintFunc)
13 | 	, _command(command)
14 | {
15 | 	_command.registerParameter(_argName, *this);
16 | }
17 | 
18 | ParameterBase::~ParameterBase() {
19 | 	_command.deregisterParameter(_argName, *this);
20 | }
21 | 
22 | Command& getDefaultCommand() {
23 | 	return Command::getDefaultCommand();
24 | }
25 | 
26 | 
27 | }
28 | 
29 | 


--------------------------------------------------------------------------------
/src/simplyfile/INotify.h:
--------------------------------------------------------------------------------
 1 | #include "FileDescriptor.h"
 2 | 
 3 | #include <map>
 4 | #include <optional>
 5 | #include <string>
 6 | 
 7 | #include <sys/inotify.h>
 8 | 
 9 | namespace simplyfile {
10 | 
11 | struct INotify : FileDescriptor {
12 | 	using FileDescriptor::FileDescriptor;
13 | 	INotify(int flags=0);
14 | 
15 |     INotify(INotify&&) noexcept = default;
16 |     INotify& operator=(INotify&&) noexcept = default;
17 | 
18 | 	// check inotify for mask meaning
19 | 	void watch(std::string const& _path, uint32_t mask);
20 | 
21 | 	std::map<int, std::string> mIDs;
22 | 
23 | 	struct Result {
24 | 		std::string path;
25 | 		std::string file;
26 | 	};
27 | 
28 | 	[[nodiscard]] auto readEvent() -> std::optional<INotify::Result>;
29 | };
30 | 
31 | }
32 | 


--------------------------------------------------------------------------------
/src/simplyfile/Event.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include "FileDescriptor.h"
 4 | 
 5 | #include <sys/eventfd.h>
 6 | #include <stdexcept>
 7 | 
 8 | namespace simplyfile
 9 | {
10 | 
11 | struct Event : FileDescriptor {
12 | 	Event(int flags=0, int initval=0) noexcept
13 | 		: FileDescriptor(eventfd(initval, flags))
14 | 	{}
15 | 
16 |     Event(Event&&) noexcept = default;
17 |     Event& operator=(Event&&) noexcept = default;
18 | 
19 | 	uint64_t get() {
20 | 		uint64_t val {0};
21 | 		if (::eventfd_read(*this, &val) != 0) {
22 | 			throw std::runtime_error("cannot read from eventfd");
23 | 		}
24 | 		return val;
25 | 	}
26 | 
27 | 	void put(uint64_t val) {
28 | 		if (::eventfd_write(*this, val) != 0) {
29 | 			throw std::runtime_error("cannot write to eventfd");
30 | 		}
31 | 	}
32 | };
33 | 
34 | }
35 | 
36 | 


--------------------------------------------------------------------------------
/src/sargparse/ArgumentParsing.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <string>
 4 | #include <regex>
 5 | #include <set>
 6 | 
 7 | #include "Parameter.h"
 8 | 
 9 | namespace sargp
10 | {
11 | 
12 | /**
13 |  * parse the arguments and set all parameters with the parsed values
14 |  */
15 | void parseArguments(int argc, char const* const* argv);
16 | /**
17 |  * parse the arguments but only set the parameters specified in the set to the parsed values
18 |  */
19 | void parseArguments(int argc, char const* const* argv, std::set<ParameterBase*> const& targetParameters);
20 | 
21 | std::string generateHelpString(std::regex const& filter=std::regex{".*"});
22 | std::string generateGroffString();
23 | 
24 | std::set<std::string> getNextArgHint(int argc, char const* const* argv);
25 | 
26 | void callCommands();
27 | 
28 | }
29 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | Copyright (c) 2018, Gottlieb+Freitag <info@gottliebtfreitag.de>
 2 | 
 3 | Permission to use, copy, modify, and/or distribute this software for any
 4 | purpose with or without fee is hereby granted, provided that the above
 5 | copyright notice and this permission notice appear in all copies.
 6 | 
 7 | THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 8 | WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 9 | MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
10 | ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
11 | WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
12 | ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
13 | OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
14 | 


--------------------------------------------------------------------------------
/src/sargparse/LICENSE:
--------------------------------------------------------------------------------
 1 | Copyright (c) 2018, Gottlieb+Freitag <info@gottliebtfreitag.de>
 2 | 
 3 | Permission to use, copy, modify, and/or distribute this software for any
 4 | purpose with or without fee is hereby granted, provided that the above
 5 | copyright notice and this permission notice appear in all copies.
 6 | 
 7 | THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 8 | WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 9 | MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
10 | ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
11 | WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
12 | ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
13 | OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
14 | 


--------------------------------------------------------------------------------
/src/simplyfuse/LICENSE:
--------------------------------------------------------------------------------
 1 | Copyright (c) 2018, Gottlieb+Freitag <info@gottliebtfreitag.de>
 2 | 
 3 | Permission to use, copy, modify, and/or distribute this software for any
 4 | purpose with or without fee is hereby granted, provided that the above
 5 | copyright notice and this permission notice appear in all copies.
 6 | 
 7 | THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 8 | WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 9 | MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
10 | ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
11 | WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
12 | ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
13 | OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
14 | 


--------------------------------------------------------------------------------
/src/usb2dynamixel/LICENSE:
--------------------------------------------------------------------------------
 1 | Copyright (c) 2018, Gottlieb+Freitag <info@gottliebtfreitag.de>
 2 | 
 3 | Permission to use, copy, modify, and/or distribute this software for any
 4 | purpose with or without fee is hereby granted, provided that the above
 5 | copyright notice and this permission notice appear in all copies.
 6 | 
 7 | THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 8 | WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 9 | MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
10 | ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
11 | WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
12 | ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
13 | OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
14 | 


--------------------------------------------------------------------------------
/src/simplyfile/INotify.cpp:
--------------------------------------------------------------------------------
 1 | #include "INotify.h"
 2 | 
 3 | #include <linux/limits.h>
 4 | 
 5 | #include <iostream>
 6 | 
 7 | namespace simplyfile {
 8 | 
 9 | INotify::INotify(int flags)
10 | 	: FileDescriptor(::inotify_init1(flags))
11 | {}
12 | 
13 | void INotify::watch(std::string const& _path, uint32_t mask) {
14 | 	int id = inotify_add_watch(*this, _path.c_str(), mask);
15 | 	mIDs[id] = _path;
16 | }
17 | 
18 | auto INotify::readEvent() -> std::optional<INotify::Result> {
19 | 	std::array<std::byte, sizeof(inotify_event) + NAME_MAX + 1> buffer;
20 |     read(*this, buffer.data(), buffer.size());
21 | 	inotify_event const& event = *reinterpret_cast<inotify_event const*>(buffer.data());
22 | 
23 | 	if (0 == event.wd) {
24 | 		return std::nullopt;
25 | 	}
26 | 
27 | 	INotify::Result res;
28 | 	res.path = mIDs.at(event.wd);
29 | 	if (event.len > 0) {
30 | 		res.file = event.name;
31 | 	}
32 | 
33 | 	return res;
34 | }
35 | 
36 | }
37 | 
38 | 


--------------------------------------------------------------------------------
/scripts/zsh_completion:
--------------------------------------------------------------------------------
 1 | _inspexel ()
 2 | {
 3 | 	globalParams=$(inspexel --bash_completion | tr ' ' '\n')
 4 | 	globalCommands=$(echo ${globalParams} | grep "^[^-][^-]")
 5 | 	globalOptions=$(echo ${globalParams} | grep "^--")
 6 | 
 7 | 	localParams=$(inspexel ${words[@]:1} --bash_completion | tr ' ' '\n')
 8 | 	localCommands=$(echo ${localParams} | grep "^[^-][^-]")
 9 | 	localOptions=$(echo ${localParams} | grep "^--")
10 | 
11 | 	localCommands=$(comm -23  <(echo ${localCommands} |  sort ) <(echo ${globalCommands} | sort))
12 | 	localOptions=$(comm -23  <(echo ${localOptions} |  sort ) <(echo ${globalOptions} | sort))
13 | 
14 | 	compadd -J "G4" -X "%USuggestions%u" -- $(echo ${localCommands} | tr '\n' ' ')
15 | 	compadd -J "G3" -X "%ULocal Options%u" -- $(echo ${localOptions} | tr '\n' ' ')
16 | 	compadd -J "G1" -X "%UCommands%u" -- $(echo ${globalCommands} | tr '\n' ' ')
17 | 	compadd -J "G2" -X "%UGlobal Options%u" -- $(echo ${globalOptions} | tr '\n' ' ')
18 | }
19 | compdef _inspexel -P inspexel -N
20 | 


--------------------------------------------------------------------------------
/src/usb2dynamixel/ProtocolV1.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include "ProtocolBase.h"
 4 | 
 5 | namespace dynamixel {
 6 | 
 7 | struct ProtocolV1 : public ProtocolBase {
 8 | 	[[nodiscard]] auto createPacket(MotorID motorID, Instruction instr, Parameter data) const -> Parameter override;
 9 | 	[[nodiscard]] auto readPacket(Timeout timeout, MotorID expectedMotorID, std::size_t numParameters, simplyfile::SerialPort const& port) const -> std::tuple<bool, MotorID, ErrorCode, Parameter> override;
10 | 	[[nodiscard]] auto extractPayload(Parameter const& raw_packet) const -> std::tuple<MotorID, ErrorCode, Parameter>;
11 | 
12 | 	auto convertLength(size_t len) const -> Parameter override;
13 | 	auto convertAddress(int addr)  const -> Parameter override;
14 | 
15 | 	auto buildBulkReadPackage(std::vector<std::tuple<MotorID, int, size_t>> const& motors) const -> std::vector<std::byte> override;
16 | 
17 | private:
18 | 	Parameter synchronizeOnHeader(Timeout timeout, MotorID expectedMotorID, std::size_t numParameters, simplyfile::SerialPort const& port) const;
19 | };
20 | 
21 | }
22 | 


--------------------------------------------------------------------------------
/src/usb2dynamixel/ProtocolV2.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include "ProtocolBase.h"
 4 | 
 5 | namespace dynamixel {
 6 | 
 7 | struct ProtocolV2 : public ProtocolBase {
 8 | 	[[nodiscard]] auto createPacket(MotorID motorID, Instruction instr, Parameter data) const -> Parameter override;
 9 | 	[[nodiscard]] auto readPacket(Timeout timeout, MotorID expectedMotorID, std::size_t numParameters, simplyfile::SerialPort const& port) const -> std::tuple<bool, MotorID, ErrorCode, Parameter> override;
10 | 	[[nodiscard]] auto extractPayload(Parameter const& raw_packet) const -> std::tuple<MotorID, ErrorCode, Parameter>;
11 | 
12 | 	auto convertLength(size_t len) const -> Parameter override;
13 | 	auto convertAddress(int addr)  const -> Parameter override;
14 | 
15 | 	auto buildBulkReadPackage(std::vector<std::tuple<MotorID, int, size_t>> const& motors) const -> std::vector<std::byte> override;
16 | 
17 | private:
18 | 	Parameter synchronizeOnHeader(Timeout timeout, MotorID expectedMotorID, std::size_t numParameters, simplyfile::SerialPort const& port) const;
19 | };
20 | 
21 | }
22 | 


--------------------------------------------------------------------------------
/src/simplyfile/socket/Socket.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include "../FileDescriptor.h"
 4 | #include "Host.h"
 5 | 
 6 | namespace simplyfile
 7 | {
 8 | 
 9 | struct ClientSocket : FileDescriptor {
10 | 	using FileDescriptor::FileDescriptor;
11 | 
12 | 	ClientSocket(Host const& host);
13 | 
14 | 	// this constructor is used by ServerSocket::accept() to pass peer information to the socket
15 | 	ClientSocket(int fd, Host const& host);
16 | 
17 |     ClientSocket(ClientSocket&&) noexcept = default;
18 |     ClientSocket& operator=(ClientSocket&&) noexcept = default;
19 | 
20 | 	void connect();
21 | 	Host const& getHost() const {
22 | 		return host;
23 | 	}
24 | 	int getBytesAvailable() const;
25 | protected:
26 | 	Host host;
27 | };
28 | 
29 | struct ServerSocket : FileDescriptor {
30 | 	ServerSocket(Host const& host, bool reusePort=true);
31 | 	ServerSocket(ServerSocket&&)= default;
32 | 	ServerSocket& operator=(ServerSocket&&)= default;
33 | 
34 | 	virtual ~ServerSocket();
35 | 
36 | 	ClientSocket accept();
37 | 	void listen();
38 | protected:
39 | 	Host host;
40 | };
41 | 
42 | }
43 | 
44 | 
45 | 


--------------------------------------------------------------------------------
/src/usb2dynamixel/MotorMetaInfo.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <map>
 4 | #include <optional>
 5 | #include <stdint.h>
 6 | #include <string>
 7 | #include <string_view>
 8 | #include <variant>
 9 | #include <vector>
10 | #include <functional>
11 | #include "Layout.h"
12 | 
13 | namespace dynamixel::meta {
14 | 
15 | template <LayoutType Head, LayoutType... Types, typename CB>
16 | void forLayoutTypes(CB&& cb) {
17 | 	cb(MotorLayoutInfo<Head>{});
18 | 	if constexpr (sizeof...(Types) > 0) {
19 | 		forLayoutTypes<Types...>(std::forward<CB>(cb));
20 | 	}
21 | };
22 | 
23 | template <typename CB>
24 | void forAllLayoutTypes(CB&& cb) {
25 | 	forLayoutTypes<LayoutType::MX_V1, LayoutType::MX_V2, LayoutType::Pro, LayoutType::XL320, LayoutType::AX>(std::forward<CB>(cb));
26 | };
27 | 
28 | 
29 | struct MotorInfo {
30 | 	uint16_t                        modelNumber;
31 | 	LayoutType layout;
32 | 
33 | 	std::string                     shortName;
34 | 	std::vector<std::string>        motorNames;
35 | };
36 | 
37 | auto getMotorInfo(uint16_t _modelNumber) -> MotorInfo const*;
38 | auto getMotorInfo(std::string const& _name) -> MotorInfo const*;
39 | 
40 | }
41 | 


--------------------------------------------------------------------------------
/src/globalOptions.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <sargparse/Parameter.h>
 4 | #include "usb2dynamixel/USB2Dynamixel.h"
 5 | 
 6 | 
 7 | auto listDeviceFiles(std::vector<std::string> const& _str) -> std::pair<bool, std::set<std::string>>;
 8 | auto listTypicalBaudrates(std::vector<std::string> const& _str) -> std::pair<bool, std::set<std::string>>;
 9 | 
10 | auto getDefaultSerialPort() -> std::string;
11 | 
12 | inline auto g_device          = sargp::Parameter<std::string>(getDefaultSerialPort(), "device", "the usb2dynamixel device (e.g.: /dev/ttyUSB0)", {}, &listDeviceFiles);
13 | inline auto g_id              = sargp::Parameter<int>(0, "id", "the target Id (values: 0x00 - 0xfd)");
14 | inline auto g_baudrate        = sargp::Parameter<int>(1000000, "baudrate", "baudrate to use (e.g.: 1m)", {}, &listTypicalBaudrates);
15 | inline auto g_timeout         = sargp::Parameter<int>(10000, "timeout", "timeout in us");
16 | inline auto g_protocolVersion = sargp::Choice<dynamixel::Protocol>(dynamixel::Protocol::V1, "protocol_version", {
17 |     {"1", dynamixel::Protocol::V1},
18 |     {"2", dynamixel::Protocol::V2}
19 | }, "the dynamixel protocol version (values: 1, 2)");
20 | 


--------------------------------------------------------------------------------
/src/simplyfile/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2018 Lutz Freitag
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/src/commonTasks.cpp:
--------------------------------------------------------------------------------
 1 | #include "commonTasks.h"
 2 | #include "usb2dynamixel/MotorMetaInfo.h"
 3 | 
 4 | 
 5 | using namespace dynamixel;
 6 | auto detectMotor(MotorID motor, USB2Dynamixel& usb2dyn, std::chrono::microseconds timeout) -> std::tuple<dynamixel::LayoutType, uint16_t> {
 7 | 	// only read model information, when model is known read full motor
 8 | 	auto [timeoutFlag, motorID, errorCode, layout] = usb2dyn.read<mx_v1::Register::MODEL_NUMBER, 2>(motor, timeout);
 9 | 	if (timeoutFlag) {
10 | 		return std::make_tuple(LayoutType::None, 0);
11 | 	}
12 | 	if (motorID == MotorIDInvalid) {
13 | 		std::cout << "something answered when pinging " << int(motor) << " but answer was not valid\n";
14 | 		return std::make_tuple(LayoutType::None, 0);
15 | 	}
16 | 	auto modelPtr = meta::getMotorInfo(layout.model_number);
17 | 	if (modelPtr) {
18 | 		std::cout << int(motor) << " " <<  modelPtr->shortName << " (" << layout.model_number << ") Layout " << to_string(modelPtr->layout) << "\n";
19 | 		return std::make_tuple(modelPtr->layout, layout.model_number);
20 | 	}
21 | 
22 | 	std::cout << int(motor) << " unknown model (" << layout.model_number << ")\n";
23 | 	return std::make_tuple(LayoutType::None, layout.model_number);
24 | }
25 | 


--------------------------------------------------------------------------------
/scripts/generateMan.sh:
--------------------------------------------------------------------------------
 1 | #!/bin/bash
 2 | 
 3 | echo "
 4 | .\" Manpage for inspexel
 5 | .\" Contact info@gottliebtfreitag.de
 6 | "
 7 | 
 8 | ./inspexel --groff | head -n 1
 9 | 
10 | echo "
11 | .SH NAME
12 | inspexel - inspecting motors using the dynamixel protocol
13 | 
14 | .SH SYNOPSIS
15 | inspexel <COMMAND> [OPTION]... 
16 | 
17 | .SH DESCRIPTION
18 | inspexel is a program to inspect connected motors that uses the dynamixel protocol. It supports protocol v1 and v2.
19 | "
20 | ./inspexel --groff | tail -n +2
21 | 
22 | echo ".SH EXAMPLE
23 | .SH
24 | 	$ inspexel
25 | .TP
26 | 	detects all connected motors
27 | 
28 | .SH
29 | 	$ inspexel detect --read_all
30 | .TP
31 | 	detects all connected motors and gives verbose information about them
32 | 
33 | .SH
34 | 	$ inspexel meta
35 | .TP
36 | 	list all supported motor types
37 | 
38 | .SH
39 | 	$ inspexel reboot --id 3
40 | .TP
41 | 	reboots motor with id 3
42 | 
43 | .SH
44 | 	$ inspexel set_register --register 0x44 --values 1 --id 0x03
45 | .TP
46 | 	set register 0x44 of motor 3 to value 1
47 | 
48 | "
49 | 
50 | echo "
51 | .SH BUGS
52 | No known bugs.
53 | 
54 | .SH AUTHOR
55 | Developed by Lutz Freitag <lutz@gottliebtfreitag.de> and Simon Gene Gottlieb <simon@gottliebtfreitag.de>
56 | "
57 | 
58 | echo ".SH LICENSE"
59 | cat LICENSE
60 | 


--------------------------------------------------------------------------------
/src/simplyfile/FileDescriptor.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <fcntl.h>
 4 | #include <functional>
 5 | #include <map>
 6 | #include <sys/ioctl.h>
 7 | #include <sys/un.h>
 8 | #include <unistd.h>
 9 | #include <vector>
10 | 
11 | namespace simplyfile {
12 | 
13 | struct FileDescriptor {
14 | 
15 | 	FileDescriptor(int _fd=-1) : fd(_fd) {}
16 | 
17 | 	FileDescriptor(FileDescriptor&& other) noexcept {
18 | 		fd = other.fd;
19 | 		other.fd = -1;
20 | 	}
21 | 	FileDescriptor& operator=(FileDescriptor&& other) noexcept {
22 | 		close();
23 | 		std::swap(fd, other.fd);
24 | 		return *this;
25 | 	}
26 | 
27 | 	FileDescriptor& operator=(int _fd) noexcept {
28 | 		close();
29 | 		fd = _fd;
30 | 		return *this;
31 | 	}
32 | 
33 | 	virtual ~FileDescriptor() {
34 | 		close();
35 | 	}
36 | 
37 | 	bool valid() const noexcept {
38 | 		return fd >= 0;
39 | 	}
40 | 
41 | 	operator int() const noexcept {
42 | 		return fd;
43 | 	}
44 | 
45 | 	void close() noexcept {
46 | 		if (valid()) {
47 | 			::close(*this);
48 | 			fd = -1;
49 | 		}
50 | 	}
51 | 
52 | 	void setFlags(int flags) noexcept {
53 | 		fcntl(*this, F_SETFL, flags | getFlags());
54 | 	}
55 | 	void clearFlags(int flags) noexcept {
56 | 		fcntl(*this, F_SETFL, ~flags & getFlags());
57 | 	}
58 | 	int getFlags() noexcept {
59 | 		return ::fcntl(*this, F_GETFL, 0);
60 | 	}
61 | 
62 | private:
63 | 	int fd;
64 | };
65 | 
66 | }
67 | 


--------------------------------------------------------------------------------
/src/usb2dynamixel/dynamixel.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <cstdint>
 4 | #include <functional>
 5 | #include <stdexcept>
 6 | #include <vector>
 7 | 
 8 | namespace dynamixel {
 9 | 
10 | 	using MotorID = uint8_t;
11 | 
12 | 	constexpr MotorID MotorIDInvalid = 0xFF;
13 | 	constexpr MotorID BroadcastID    = 0xFE;
14 | 
15 | 	using Parameter = std::vector<std::byte>;
16 | 
17 | 	enum class Instruction : std::underlying_type_t<std::byte>
18 | 	{
19 | 		PING       = 0x01,
20 | 		READ       = 0x02,
21 | 		WRITE      = 0x03,
22 | 		REG_WRITE  = 0x04,
23 | 		ACTION     = 0x05,
24 | 		RESET      = 0x06,
25 | 		REBOOT     = 0x08,
26 | 		STATUS     = 0x55,
27 | 		SYNC_READ  = 0x82,
28 | 		SYNC_WRITE = 0x83,
29 | 		BULK_READ  = 0x92,
30 | 		BULK_WRITE = 0x93,
31 | 	};
32 | 
33 | 	inline uint32_t baudIndexToBaudrate(uint8_t baudIdx) {
34 | 		if (baudIdx < 250) {
35 | 			return (2000000 / (baudIdx + 1));
36 | 		} else {
37 | 			switch (baudIdx) {
38 | 				case 250:
39 | 					return 2250000;
40 | 					break;
41 | 				case 251:
42 | 					return 2500000;
43 | 					break;
44 | 				case 252:
45 | 					return 3000000;
46 | 					break;
47 | 				default:
48 | 					break;
49 | 			}
50 | 		}
51 | 		throw std::runtime_error("no valid baud index given");
52 | 	}
53 | 
54 | 	template<class... Ts> struct overloaded : Ts... { using Ts::operator()...; };
55 | 	template<class... Ts> overloaded(Ts...) -> overloaded<Ts...>;
56 | }
57 | 


--------------------------------------------------------------------------------
/src/usb2dynamixel/MotorMetaInfo.cpp:
--------------------------------------------------------------------------------
 1 | #include "MotorMetaInfo.h"
 2 | 
 3 | #include <cmath>
 4 | 
 5 | 
 6 | namespace dynamixel::meta {
 7 | 
 8 | 
 9 | auto getMotorInfos() -> std::vector<MotorInfo> const& {
10 | 	static auto data = std::vector<MotorInfo>{};
11 | 	static bool firstRun{true};
12 | 	if (firstRun) {
13 | 		firstRun = false;
14 | 
15 | 		meta::forAllLayoutTypes([&](auto const& _info) {
16 | 			using Info = std::decay_t<decltype(_info)>;
17 | 			auto& defaults = Info::getDefaults();
18 | 			for (auto [id, d] : defaults) {
19 | 				MotorInfo info;
20 | 				info.modelNumber = d.modelNumber;
21 | 				info.layout      = d.layout;
22 | 
23 | 				info.shortName   = d.shortName;
24 | 				info.motorNames  = d.motorNames;
25 | 
26 | 				data.push_back(info);
27 | 			}
28 | 		});
29 | 	}
30 | 	return data;
31 | }
32 | auto getMotorInfo(uint16_t _modelNumber) -> MotorInfo const* {
33 | 	for (auto const& motorInfo : getMotorInfos()) {
34 | 		if (motorInfo.modelNumber == _modelNumber) {
35 | 			return &motorInfo;
36 | 		}
37 | 	}
38 | 	return nullptr;
39 | 
40 | }
41 | auto getMotorInfo(std::string const& _name) -> MotorInfo const* {
42 | 	for (auto const& motorInfo : getMotorInfos()) {
43 | 		if (motorInfo.shortName == _name) {
44 | 			return &motorInfo;
45 | 		}
46 | 		for (auto const& _other : motorInfo.motorNames) {
47 | 			if (_other == _name) {
48 | 				return &motorInfo;
49 | 			}
50 | 		}
51 | 	}
52 | 	return nullptr;
53 | }
54 | }
55 | 


--------------------------------------------------------------------------------
/src/simplyfuse/FuseFile.cpp:
--------------------------------------------------------------------------------
 1 | #include "FuseFile.h"
 2 | #include "FuseFS.h"
 3 | 
 4 | #include <cstring>
 5 | 
 6 | namespace simplyfuse {
 7 | 
 8 | FuseFile::~FuseFile() {
 9 | 	if (fuseFS) {
10 | 		fuseFS->unregisterFile(*this);
11 | 	}
12 | }
13 | int FuseFile::onOpen()  { return 0; }
14 | int FuseFile::onClose() { return 0; }
15 | int FuseFile::onRead(char*, std::size_t, off_t)  { return -ENOENT; }
16 | int FuseFile::onWrite(const char*, std::size_t, off_t) { return -ENOENT; }
17 | std::size_t FuseFile::getSize() { return 4096; }
18 | int FuseFile::onTruncate(off_t) { return -ENOENT; }
19 | 
20 | int FuseFile::getFilePermissions() {
21 | 	return 0666;
22 | }
23 | 
24 | int SimpleROFile::onRead(char* buf, std::size_t size, off_t) {
25 | 	size = std::min(size, content.size()+1);
26 | 	std::memcpy(buf, content.data(), size);
27 | 	return size;
28 | }
29 | 
30 | int SimpleROFile::getFilePermissions() {
31 | 	return 0444;
32 | }
33 | 
34 | int SimpleWOFile::onTruncate(off_t) {
35 | 	return 0;
36 | }
37 | 
38 | int SimpleWOFile::getFilePermissions() {
39 | 	return 0222;
40 | }
41 | 
42 | 
43 | int SimpleRWFile::onWrite(const char* buf, std::size_t size, off_t offset) {
44 | 	content.resize(content.size() + offset + size);
45 | 	std::memcpy(content.data() + offset, buf, size);
46 | 	return size;
47 | }
48 | 
49 | int SimpleRWFile::onTruncate(off_t offset) {
50 | 	content.resize(offset);
51 | 	return 0;
52 | }
53 | 
54 | int SimpleRWFile::getFilePermissions() {
55 | 	return 0666;
56 | }
57 | 
58 | 
59 | }
60 | 


--------------------------------------------------------------------------------
/src/simplyfuse/README.md:
--------------------------------------------------------------------------------
 1 | # simplyfuse
 2 | 
 3 | simplyfuse is a very simple C++ wrapper around fuse (the filesystem).
 4 | 
 5 | here is a minimal example about how to use it:
 6 | 
 7 | ~~~
 8 | #include <simplyfuse/FuseFS.h>
 9 | 
10 | int main() {
11 | 	// create a fuseFS and mount it at "mountpoint" (this will create the mountpoint directory if necessary
12 | 	simplyfuse::FuseFS fs{"mountpoint"};
13 | 
14 | 	// a simple read writable file containing a std::string
15 | 	// files will automatically unregister themselves upon destruction
16 | 	simplyfuse::SimpleRWFile myRWFile{"Hello from a read writable file\n"};
17 | 	simplyfuse::SimpleROFile myROFile{"Hello from a read only file\n"};
18 | 
19 | 	// reflect the file "myFile" under /my_file
20 | 	fs.registerFile("/my_file", myRWFile);
21 | 	// files can be registered multiple times
22 | 	// reflect the file "myFile" also under /some_path/my_file (intermediate directories will be create automatically)
23 | 	fs.registerFile("/some_path/my_file", myRWFile);
24 | 
25 | 	fs.registerFile("/my_read_only_file", myROFile);
26 | 
27 | 	// create a directory in the filesystem (intermediate directories will be created automatically)
28 | 	fs.mkdir("/some/random/path");
29 | 	// remove "ramdom/path" from the above path (rmdir is always recursive)
30 | 	fs.rmdir("/some/random");
31 | 
32 | 	while (true) {
33 | 		// loop lets the underlying libfuse handle a single command and then returns
34 | 		fs.loop();
35 | 	}
36 | 	return 0;
37 | }
38 | ~~~
39 | 
40 | there is actually not much to say about simplyfuse...
41 | 


--------------------------------------------------------------------------------
/src/simplyfuse/FuseFS.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <string>
 4 | #include <memory>
 5 | #include <type_traits>
 6 | #include <stdexcept>
 7 | #include <filesystem>
 8 | 
 9 | #include "FuseFile.h"
10 | 
11 | namespace simplyfuse {
12 | 
13 | struct FileExistsError : std::logic_error {
14 | 	FileExistsError(std::string const& msg) : std::logic_error(msg) {}
15 | };
16 | struct InvalidPathError : std::logic_error {
17 | 	InvalidPathError(std::string const& msg) : std::logic_error(msg) {}
18 | };
19 | struct MountError : std::logic_error {
20 | 	MountError(std::string const& msg) : std::logic_error(msg) {}
21 | };
22 | 
23 | struct FuseFS {
24 | 	friend class FuseFile;
25 | 	FuseFS(std::filesystem::path const& mountPoint);
26 | 	virtual ~FuseFS();
27 | 
28 | 	void loop();
29 | 
30 | 	int getFD() const;
31 | 
32 | 	// register a file in the file system (a file can be registered multiple times)
33 | 	void registerFile(std::filesystem::path const& path, FuseFile& file);
34 | 
35 | 	// unregister a file from the file system
36 | 	void unregisterFile(std::filesystem::path const& path, FuseFile& file);
37 | 
38 | 	// unregister all instances of this file from the file system
39 | 	void unregisterFile(FuseFile& file);
40 | 
41 | 	// create a directory (will create intermediate directories, too)
42 | 	void mkdir(std::filesystem::path const& path);
43 | 
44 | 	// delete a directory and all its contents.
45 | 	// files registered within the deleted directory will simply be deregistered
46 | 	void rmdir(std::filesystem::path const& path);
47 | 
48 | 	struct Pimpl;
49 | 	std::unique_ptr<Pimpl> pimpl;
50 | 
51 | };
52 | 
53 | }
54 | 


--------------------------------------------------------------------------------
/src/utils/Factory.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include "Registry.h"
 4 | 
 5 | #include <functional>
 6 | #include <string>
 7 | #include <memory>
 8 | 
 9 | namespace factory {
10 | 
11 | template<typename Base, typename KeyType>
12 | struct Factory;
13 | 
14 | template<typename Base, typename KeyType=std::string>
15 | struct Creator {
16 | 	using RegisterableType = registry::UniqueKeyedRegisterable<KeyType, Creator<Base, KeyType>>;
17 | 	using FactoryType = Factory<Base, KeyType>;
18 | 	using CreatorFunc = std::function<std::unique_ptr<Base>(void)>;
19 | 
20 | 	Creator(KeyType const& _key, CreatorFunc && _creator)
21 | 	: registerable(_key, this)
22 | 	, creator(std::move(_creator))
23 | 	, key(_key) {}
24 | 	~Creator() = default;
25 | 
26 | 	typename CreatorFunc::result_type create() const {
27 | 		return creator();
28 | 	}
29 | 
30 | protected:
31 | 	RegisterableType registerable;
32 | 	CreatorFunc creator;
33 | 	KeyType key;
34 | };
35 | 
36 | 
37 | template<typename Base, typename KeyType=std::string>
38 | struct Factory {
39 | private:
40 | 	using RegistryType = typename Creator<Base, KeyType>::RegisterableType::RegistryType;
41 | public:
42 | 	static auto getCreatables() -> typename RegistryType::ContainerType const& {
43 | 		return RegistryType::getInstance().getInstances();
44 | 	}
45 | 
46 | 	static std::unique_ptr<Base> build(KeyType const& name) {
47 | 		auto const& items = RegistryType::getInstance().getInstances();
48 | 		auto it = items.find(name);
49 | 		if (it == items.end()) {
50 | 			throw std::runtime_error("cannot build something with name \""  + name + "\" as there is no creator for it");
51 | 		}
52 | 		return it->second->create();
53 | 	}
54 | };
55 | 
56 | }
57 | 


--------------------------------------------------------------------------------
/src/usb2dynamixel/file_io.cpp:
--------------------------------------------------------------------------------
 1 | #include "file_io.h"
 2 | 
 3 | #include <array>
 4 | #include <cerrno>
 5 | #include <cstdint>
 6 | #include <cstring>
 7 | #include <stdexcept>
 8 | #include <string>
 9 | 
10 | #include <sys/types.h>
11 | #include <unistd.h>
12 | 
13 | 
14 | namespace dynamixel::file_io {
15 | 
16 | auto read(int _fd, size_t maxReadBytes) -> std::vector<std::byte> {
17 | 	std::vector<std::byte> rxBuf(maxReadBytes);
18 | 
19 | 	size_t bytesRead = 0;
20 | 
21 | 	do {
22 | 		ssize_t r = ::read(_fd, rxBuf.data() + bytesRead, rxBuf.size() - bytesRead);
23 | 		if (r == -1) {
24 | 			if (errno == EAGAIN) {
25 | 				break;
26 | 			}
27 | 			throw std::runtime_error(std::string{"unexpected read error: "} + strerror(errno) + " (" + std::to_string(errno) + ")");
28 | 		} else if (r == 0) {
29 | 			break;
30 | 		}
31 | 		bytesRead += r;
32 | 	} while (bytesRead < maxReadBytes);
33 | 	rxBuf.resize(bytesRead);
34 | 	return rxBuf;
35 | }
36 | 
37 | size_t flushRead(int _fd) {
38 | 	size_t bytesRead {0};
39 | 	std::array<uint8_t, 4096> dummy;
40 | 	ssize_t r {1};
41 | 	while (r != -1 and r != 0) {
42 | 		bytesRead += r;
43 | 		r = ::read(_fd, dummy.data(), dummy.size()); // read flush
44 | 	}
45 | 	return bytesRead;
46 | }
47 | 
48 | void write(int _fd, std::vector<std::byte> const& txBuf) {
49 | 	uint32_t bytesWritten = 0;
50 | 	const size_t count = txBuf.size();
51 | 	do {
52 | 		ssize_t w = ::write(_fd, txBuf.data() + bytesWritten, count - bytesWritten);
53 | 
54 | 		if (w == -1) {
55 | 			throw std::runtime_error(std::string{"write to the dyanmixel bus failed: "} + strerror(errno) + " (" + std::to_string(errno) + ")");
56 | 		}
57 | 		bytesWritten += w;
58 | 	} while (bytesWritten < count);
59 | }
60 | }
61 | 


--------------------------------------------------------------------------------
/src/main.cpp:
--------------------------------------------------------------------------------
 1 | #include <sargparse/ArgumentParsing.h>
 2 | #include <sargparse/Parameter.h>
 3 | #include <iostream>
 4 | 
 5 | namespace {
 6 | 
 7 | #define VERSION "1.3.0"
 8 | #define DATE    "16 October 2018"
 9 | 
10 | #define TERM_RED                        "\033[31m"
11 | #define TERM_RESET                      "\033[0m"
12 | 
13 | auto printHelp  = sargp::Parameter<std::optional<std::string>>{{}, "help", "print this help add a string which will be used in a grep-like search through the parameters"};
14 | 
15 | auto cmdVersion = sargp::Command("version", "prints the version", []() {
16 | 	std::cout << "inspexel version " << VERSION << " - " << DATE << "\n";
17 | });
18 | 
19 | }
20 | 
21 | int main(int argc, char** argv)
22 | {
23 | 	if (std::string(argv[argc-1]) == "--bash_completion") {
24 | 		auto hints = sargp::getNextArgHint(argc-2, argv+1);
25 | 		for (auto const& hint : hints) {
26 | 			std::cout << hint << " ";
27 | 		}
28 | 		return 0;
29 | 	}
30 | 	if (argc == 2 and std::string(argv[1]) == "--groff") {
31 | 		std::cout << ".TH man 1 \"" << DATE << "\" \"" << VERSION << "\" inspexel man page\"\n";
32 | 		std::cout << sargp::generateGroffString();
33 | 		return 0;
34 | 	}
35 | 
36 | 	try {
37 | 		// pass everything except the name of the application
38 | 		sargp::parseArguments(argc-1, argv+1);
39 | 
40 | 		if (printHelp) {
41 | 			std::cout << "inspexel version " << VERSION << " - " << DATE << "\n";
42 | 			std::cout << sargp::generateHelpString(std::regex{".*" + printHelp->value_or("") + ".*"});
43 | 			return 0;
44 | 		}
45 | 		sargp::callCommands();
46 | 	} catch (std::exception const& e) {
47 | 		std::cerr << "exception: " << TERM_RED << e.what() << TERM_RESET "\n";
48 | 	}
49 | 	return 0;
50 | }
51 | 


--------------------------------------------------------------------------------
/src/globalOptions.cpp:
--------------------------------------------------------------------------------
 1 | #include "globalOptions.h"
 2 | 
 3 | #include <string>
 4 | #include <iostream>
 5 | #include <filesystem>
 6 | 
 7 | namespace fs = std::filesystem;
 8 | 
 9 | auto listDeviceFiles(std::vector<std::string> const& _str) -> std::pair<bool, std::set<std::string>> {
10 | 	std::set<std::string> res;
11 | 	std::vector<fs::path> passedPaths;
12 | 	for (auto const& s : _str) {
13 | 		try {
14 | 			passedPaths.emplace_back(fs::canonical(fs::path(s)));
15 | 		} catch (...) {} // ignore invalid paths
16 | 	}
17 | 	if (fs::is_directory("/dev/serial/by-id/")) {
18 | 		for (auto p : fs::directory_iterator("/dev/serial/by-id/")) {
19 | 			// if p resolves to anything specifies by _str remove it
20 | 			auto canonical = fs::canonical(p);
21 | 			if (std::find(passedPaths.begin(), passedPaths.end(), p) == passedPaths.end() and
22 | 				std::find(passedPaths.begin(), passedPaths.end(), canonical) == passedPaths.end()) {
23 | 				res.insert(p.path().string());
24 | 				res.insert(canonical.string());
25 | 			}
26 | 		}
27 | 	}
28 | 
29 | 	return {not _str.empty(), res};
30 | }
31 | auto listTypicalBaudrates(std::vector<std::string> const& _str) -> std::pair<bool, std::set<std::string>> {
32 | 	std::set<std::string> res;
33 | 	res.emplace("9600");
34 | 	res.emplace("11520");
35 | 	res.emplace("57142");
36 | 	res.emplace("1m");
37 | 	res.emplace("2m");
38 | 	res.emplace("3m");
39 | 	for (auto const& s : _str) {
40 | 		if (res.count(s) > 0) {
41 | 			res.erase(s);
42 | 		}
43 | 	}
44 | 
45 | 	return {true, res};
46 | }
47 | 
48 | 
49 | auto getDefaultSerialPort() -> std::string {
50 | 	auto devices = listDeviceFiles({}).second;
51 | 	if (devices.empty()) {
52 | 		return "";
53 | 	}
54 | 	return *devices.begin();
55 | }
56 | 
57 | 


--------------------------------------------------------------------------------
/src/simplyfuse/FuseFile.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <errno.h>
 4 | #include <iterator>
 5 | #include <unistd.h>
 6 | 
 7 | namespace simplyfuse {
 8 | 
 9 | struct FuseFS;
10 | 
11 | // subclass this struct to implement functionality represented by a file
12 | struct FuseFile {
13 | 	FuseFile() = default;
14 | 	FuseFile(FuseFile const&) = delete;
15 | 	FuseFile(FuseFile &&) = delete;
16 | 	FuseFile operator=(FuseFile const&) = delete;
17 | 	FuseFile operator=(FuseFile&&) = delete;
18 | 
19 | 	virtual ~FuseFile();
20 | 	virtual int onOpen();
21 | 	virtual int onClose();
22 | 	virtual int onRead(char* buf, std::size_t size, off_t offset);
23 | 	virtual int onWrite(const char* buf, std::size_t size, off_t offset);
24 | 	virtual std::size_t getSize();
25 | 	virtual int onTruncate(off_t offset);
26 | 
27 | 	virtual int getFilePermissions();
28 | 
29 | 	friend class FuseFS;
30 | protected:
31 | 	FuseFS* fuseFS {nullptr};
32 | };
33 | 
34 | struct SimpleROFile : FuseFile {
35 | 	std::string content;
36 | 	using FuseFile::FuseFile;
37 | 	SimpleROFile(std::string const& _content="") : content(_content) {};
38 | 	virtual ~SimpleROFile() = default;
39 | 	int onRead(char* buf, std::size_t size, off_t offset) override;
40 | 	int getFilePermissions() override;
41 | };
42 | 
43 | struct SimpleWOFile : FuseFile {
44 | 	using FuseFile::FuseFile;
45 | 	virtual ~SimpleWOFile() = default;
46 | 	int onTruncate(off_t offset);
47 | 	int getFilePermissions() override;
48 | };
49 | 
50 | struct SimpleRWFile : SimpleROFile {
51 | 	using SimpleROFile::SimpleROFile;
52 | 	virtual ~SimpleRWFile() = default;
53 | 	int onWrite(const char* buf, std::size_t size, off_t offset);
54 | 	int onTruncate(off_t offset);
55 | 	int getFilePermissions() override;
56 | };
57 | 
58 | } /* namespace simplyfile */
59 | 


--------------------------------------------------------------------------------
/src/utils/CanCallBack.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <set>
 4 | #include <functional>
 5 | #include <memory>
 6 | #include <mutex>
 7 | 
 8 | template<typename... Args>
 9 | struct CanCallBack;
10 | 
11 | namespace detail
12 | {
13 | 
14 | template<typename... Args>
15 | struct Callback {
16 | 	struct CallbackInfo {
17 | 		std::mutex mutex;
18 | 		CanCallBack<Args...>* owner{};
19 | 		std::function<void(Args...)> cb;
20 | 		~CallbackInfo();
21 | 	};
22 | 	std::unique_ptr<CallbackInfo> data{std::make_unique<CallbackInfo>()};
23 | };
24 | 
25 | }
26 | 
27 | template<typename... Args>
28 | struct CanCallBack {
29 | 	using Callback = detail::Callback<Args...>;
30 | private:
31 | 	using CallbackInfo = typename Callback::CallbackInfo;
32 | 	std::set<CallbackInfo*> infos;
33 | 	std::recursive_mutex mutex;
34 | 
35 | protected:
36 | 	void callCallbacks(Args... args) {
37 | 		std::lock_guard lock(mutex);
38 | 		for (auto info : infos) {
39 | 			info->cb(args...);
40 | 		}
41 | 	}
42 | 
43 | 	~CanCallBack() {
44 | 		std::lock_guard lock(mutex);
45 | 		for (auto info : infos) {
46 | 			std::lock_guard lock2(info->mutex);
47 | 			info->owner = nullptr;
48 | 		}
49 | 	}
50 | public:
51 | 	template<typename Func>
52 | 	[[nodiscard]] Callback addCallback(Func&& cb) {
53 | 		Callback ret;
54 | 		ret.data->cb    = std::forward<Func>(cb);
55 | 		ret.data->owner = this;
56 | 		std::lock_guard lock(mutex);
57 | 		infos.emplace(ret.data.get());
58 | 		return ret;
59 | 	}
60 | 
61 | 	void deregisterCB(CallbackInfo* info) {
62 | 		std::lock_guard lock(mutex);
63 | 		infos.erase(info);
64 | 	}
65 | };
66 | 
67 | namespace detail
68 | {
69 | 
70 | template<typename... Args>
71 | Callback<Args...>::CallbackInfo::~CallbackInfo() {
72 | 	std::lock_guard lock{mutex};
73 | 	if (owner) {
74 | 		owner->deregisterCB(this);
75 | 	}
76 | }
77 | 
78 | }
79 | 
80 | 
81 | 


--------------------------------------------------------------------------------
/src/usb2dynamixel/ProtocolBase.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include "dynamixel.h"
 4 | 
 5 | #include <simplyfile/SerialPort.h>
 6 | 
 7 | #include <chrono>
 8 | 
 9 | namespace dynamixel {
10 | 
11 | enum class ErrorCode : uint8_t {
12 | 	InpultVoltage = 0x01,
13 | 	AngleLimit    = 0x02,
14 | 	Overheating   = 0x04,
15 | 	Range         = 0x08,
16 | 	Checksum      = 0x10,
17 | 	Overload      = 0x20,
18 | 	Instruction   = 0x40
19 | };
20 | 
21 | 
22 | struct ProtocolBase {
23 | 	using Timeout = std::chrono::high_resolution_clock::duration;
24 | 	virtual ~ProtocolBase() {}
25 | 
26 | 	[[nodiscard]] virtual auto createPacket(MotorID motorID, Instruction instr, Parameter data) const -> Parameter = 0;
27 | 	/**
28 | 	 * receive a packet that contains numParameters bytes of payload
29 | 	 * return the whole raw packet or an empty vector if a timeout happened or and invalid packet was received
30 | 	 */
31 | 	[[nodiscard]] virtual auto readPacket(Timeout timeout, uint8_t expectedMotorID, std::size_t numParameters, simplyfile::SerialPort const& port) const -> std::tuple<bool, MotorID, ErrorCode, Parameter> = 0;
32 | 
33 | 
34 | 	/** process a received packet by validating it and stripping it to the payload
35 | 	 *
36 | 	 *  return value
37 | 	 *  [validFlag, motorID, errorCode, parameters] = readPacket(...);
38 | 	 *
39 | 	 *  validFlag   indicates if the packet was valid
40 | 	 *  motorID     inidcates if parameters form a valid packet, withc MotorIDInvalid
41 | 	 *  errorCode   errorCode flags from the return message
42 | 	 *  paremeters  is a vector with the actual payload
43 | 	 */
44 | //	[[nodiscard]] virtual auto validateRawPacket(Parameter const& raw_packet) const -> std::tuple<MotorID, ErrorCode, Parameter> = 0;
45 | 
46 | 	[[nodiscard]] virtual auto convertLength(size_t len) const -> Parameter = 0;
47 | 	[[nodiscard]] virtual auto convertAddress(int addr) const -> Parameter = 0;
48 | 
49 | 	[[nodiscard]] virtual auto buildBulkReadPackage(std::vector<std::tuple<MotorID, int, size_t>> const& motors) const -> std::vector<std::byte> = 0;
50 | };
51 | 
52 | }
53 | 


--------------------------------------------------------------------------------
/src/simplyfile/Timer.cpp:
--------------------------------------------------------------------------------
 1 | #include "Timer.h"
 2 | 
 3 | #include <cstring>
 4 | #include <ctime>
 5 | #include <stdexcept>
 6 | #include <string>
 7 | 
 8 | 
 9 | namespace simplyfile {
10 | 
11 | namespace {
12 | void normalize(struct timespec& time) {
13 | 	while (time.tv_nsec > 1000000000) {
14 | 		time.tv_nsec -= 1000000000;
15 | 		time.tv_sec += 1;
16 | 	}
17 | 	while (time.tv_nsec < -1000000000) {
18 | 		time.tv_nsec += 1000000000;
19 | 		time.tv_sec -= 1;
20 | 	}
21 | }
22 | }
23 | 
24 | 
25 | Timer::Timer(std::chrono::nanoseconds duration, bool oneShot, int flags)
26 | 	: FileDescriptor(::timerfd_create(CLOCK_MONOTONIC, flags))
27 | {
28 | 	reset(duration, oneShot);
29 | }
30 | int Timer::getElapsed() const {
31 | 	uint64_t elapsed {0};
32 | 	::read(*this, &elapsed, sizeof(elapsed));
33 | 	return elapsed;
34 | }
35 | 
36 | void Timer::cancel() {
37 | 	struct itimerspec new_value{};
38 | 	if (timerfd_settime(*this, TFD_TIMER_ABSTIME, &new_value, NULL) == -1) {
39 | 		throw std::runtime_error("cannot cancel timer " + std::string(strerror(errno)));
40 | 	}
41 | }
42 | 
43 | void Timer::reset(std::chrono::nanoseconds duration, bool oneShot) {
44 | 	cancel();
45 | 	struct itimerspec new_value {};
46 | 	struct timespec now;
47 | 	if (clock_gettime(CLOCK_MONOTONIC, &now) == -1) {
48 | 		throw std::runtime_error("cannot get the current time");
49 | 	}
50 | 
51 | 	int64_t seconds     = std::chrono::duration_cast<std::chrono::seconds>(duration).count();
52 | 	int64_t nanoSeconds = std::chrono::duration_cast<std::chrono::nanoseconds>(duration - std::chrono::seconds(seconds)).count();
53 | 
54 | 	new_value.it_value.tv_sec = now.tv_sec + seconds;
55 | 	new_value.it_value.tv_nsec = now.tv_nsec + nanoSeconds;
56 | 	normalize(new_value.it_value);
57 | 
58 | 	if (not oneShot) {
59 | 		new_value.it_interval.tv_sec = seconds;
60 | 		new_value.it_interval.tv_nsec = nanoSeconds;
61 | 		normalize(new_value.it_interval);
62 | 	}
63 | 
64 | 	if (timerfd_settime(*this, TFD_TIMER_ABSTIME, &new_value, NULL) == -1) {
65 | 		throw std::runtime_error("cannot set timeout for timerfd " + std::string(strerror(errno)));
66 | 	}
67 | }
68 | 
69 | 
70 | }
71 | 
72 | 


--------------------------------------------------------------------------------
/src/simplyfile/socket/Host.cpp:
--------------------------------------------------------------------------------
 1 | 
 2 | #include "Host.h"
 3 | #include <cstring>
 4 | #include <errno.h>
 5 | #include <stdexcept>
 6 | #include <netdb.h>
 7 | #include <sys/un.h>
 8 | 
 9 | namespace simplyfile
10 | {
11 | 
12 | 
13 | std::string Host::getName() const {
14 |     char hbuf[NI_MAXHOST], sbuf[NI_MAXSERV];
15 |     if (getnameinfo(reinterpret_cast<struct sockaddr const*>(&sockaddr), sockaddrLen, hbuf, sizeof(hbuf), sbuf,
16 |                 sizeof(sbuf), 0) != 0) {
17 |     	throw std::runtime_error("getnameinfo returned an error");
18 |     }
19 |     return std::string(hbuf) + ":" + std::string(sbuf);
20 | }
21 | 
22 | 
23 | std::vector<Host> getHosts(std::string const& node, std::string const& service, int socktype, int family) {
24 | 	std::vector<Host> hosts;
25 | 
26 | 	struct addrinfo *result;
27 | 	struct addrinfo hints;
28 | 	memset(&hints, 0, sizeof(struct addrinfo));
29 | 	hints.ai_family = family;
30 | 	hints.ai_socktype = socktype;
31 | 	hints.ai_flags = AI_PASSIVE;    /* For wildcard IP address */
32 | 	hints.ai_protocol = 0;          /* Any protocol */
33 | 	hints.ai_canonname = NULL;
34 | 	hints.ai_addr = NULL;
35 | 	hints.ai_next = NULL;
36 | 
37 | 	if (getaddrinfo(node.c_str(), service.c_str(), &hints, &result)) {
38 | 		throw std::runtime_error("cannot call getaddrinfo");
39 | 	}
40 | 	for (struct addrinfo *rp = result; rp != NULL; rp = rp->ai_next) {
41 | 		Host h;
42 | 		h.family      = rp->ai_family;
43 | 		h.socktype    = rp->ai_socktype;
44 | 		h.protocol    = rp->ai_protocol;
45 | 		h.sockaddrLen = rp->ai_addrlen;
46 | 		memcpy(&h.sockaddr, rp->ai_addr, h.sockaddrLen);
47 | 		hosts.emplace_back(h);
48 | 	}
49 | 	freeaddrinfo(result);           /* No longer needed */
50 | 
51 | 	return hosts;
52 | }
53 | 
54 | 
55 | Host makeUnixDomainHost(std::string const& path, int socktype) {
56 | 	Host host;
57 | 	host.family   = AF_LOCAL;
58 | 	host.protocol = 0;
59 | 	host.socktype = socktype;
60 | 	struct sockaddr_un& address = reinterpret_cast<struct sockaddr_un&>(host.sockaddr);
61 | 	address.sun_family = AF_LOCAL;
62 | 	strncpy(address.sun_path, path.c_str(), std::min(sizeof(address.sun_path), path.size()));
63 | 	host.sockaddrLen = sizeof(address);
64 | 	return host;
65 | }
66 | 
67 | 
68 | }
69 | 


--------------------------------------------------------------------------------
/src/simplyfile/socket/Socket.cpp:
--------------------------------------------------------------------------------
 1 | #include "Socket.h"
 2 | 
 3 | #include <cstring>
 4 | #include <cerrno>
 5 | #include <stdexcept>
 6 | #include <sys/ioctl.h>
 7 | #include <sys/un.h>
 8 | #include <unistd.h>
 9 | 
10 | namespace simplyfile
11 | {
12 | ClientSocket::ClientSocket(int _fd, Host const& _host)
13 | 	: FileDescriptor(_fd)
14 | 	, host(_host)
15 | {}
16 | 
17 | ClientSocket::ClientSocket(Host const& _host)
18 | 	: FileDescriptor(::socket(_host.family, _host.socktype, _host.protocol))
19 | 	, host(_host)
20 | {}
21 | 
22 | void ClientSocket::connect() {
23 | 	if (::connect(*this, reinterpret_cast<struct sockaddr const*>(&host.sockaddr), host.sockaddrLen) != 0) {
24 | 		throw std::runtime_error("cannot connect to host: " + host.getName() + " errno: " + std::string(strerror(errno)));
25 | 	}
26 | }
27 | 
28 | int ClientSocket::getBytesAvailable() const {
29 | 	int bytesAvailable;
30 | 	if (ioctl(*this, FIONREAD, &bytesAvailable)) {
31 | 		throw std::runtime_error("cannot determine how many bytes are available in socket");
32 | 	}
33 | 	return bytesAvailable;
34 | }
35 | 
36 | ServerSocket::ServerSocket(Host const& _host, bool reusePort)
37 | 	: host(_host)
38 | {
39 | 	FileDescriptor sock = ::socket(host.family, host.socktype, host.protocol);
40 | 	int optVal = reusePort;
41 |     ::setsockopt(sock, SOL_SOCKET, SO_REUSEPORT, &optVal, sizeof(optVal));
42 | 	if (not sock.valid()) {
43 | 		throw std::runtime_error("cannot create socket " + std::string(strerror(errno)));
44 | 	}
45 | 	if (host.family == AF_LOCAL) {
46 | 		// for unix sockets we might need to unlink the socket-file that is being created
47 | 		struct sockaddr_un& address = reinterpret_cast<struct sockaddr_un&>(host.sockaddr);
48 | 		::unlink(address.sun_path);
49 | 	}
50 | 	if (::bind(sock, reinterpret_cast<struct sockaddr const*>(&host.sockaddr), host.sockaddrLen)) {
51 | 		throw std::runtime_error("cannot bind socket to: " + host.getName());
52 | 	}
53 | 	FileDescriptor::operator =(std::move(sock));
54 | }
55 | 
56 | ServerSocket::~ServerSocket() {
57 | 	if (valid()) {
58 | 		if (host.family == AF_LOCAL) {
59 | 			// for unix sockets we might need to unlink the socket-file that is being created
60 | 			struct sockaddr_un& address = reinterpret_cast<struct sockaddr_un&>(host.sockaddr);
61 | 			::unlink(address.sun_path);
62 | 		}
63 | 	}
64 | }
65 | 
66 | ClientSocket ServerSocket::accept() {
67 | 	Host h = host;
68 | 	int _fd = ::accept(*this, reinterpret_cast<struct sockaddr*>(&h.sockaddr), &h.sockaddrLen);
69 | 	ClientSocket socket{_fd, h};
70 | 	if (not socket.valid()) {
71 | 		throw std::runtime_error("cannot accept incomming socket");
72 | 	}
73 | 	return socket;
74 | }
75 | 
76 | void ServerSocket::listen() {
77 | 	if (::listen(*this, 0)) {
78 | 		throw std::runtime_error("cannot listen on socket " + std::string(strerror(errno)));
79 | 	}
80 | }
81 | 
82 | }
83 | 


--------------------------------------------------------------------------------
/src/sargparse/README.md:
--------------------------------------------------------------------------------
 1 | # sargparse
 2 | A very easy to use argument parser
 3 | 
 4 | sargparse is intended to be used in very modular software where parameters can reside all over the source files
 5 | 
 6 | Here is a small yet extensive example about its usage:
 7 | 
 8 | File foo.cpp:
 9 | ~~~
10 | #include <sargparse/Parameter.h>
11 | #include <iostream>
12 | 
13 | namespace {
14 | // all parameters comming from this section have "mySection." as their name prefix
15 | auto mySection = sargparse::Section{"mySection"};
16 | // here are some demonstrations of how parameters can be registered
17 | // the arguments passed to Parameters are pretty easy: default_value, argument_name, description_for_help_text
18 | auto myIntParam    = mySection.Parameter<int>(123, "integer", "an integer argument");
19 | auto myDoubleParam = mySection.Parameter<double>(M_PI, "double", "a double argument");
20 | auto myStringParam = mySection.Parameter<std::string>("some string value", "string", "a string argument");
21 | auto myFlag        = mySection.Flag("flag", "a simple flag");
22 | 
23 | void myCommandCallback();
24 | // if "my_command" is passed as first argument to the executable myCommandCallback will be called from sargparse::callCommands()
25 | auto myCommand = sargparse::Command{"my_command", "help text for that command", myCommandCallback};
26 | auto myCommandSpecificParameter = myCommand.Flag("print_hello", "print hello");
27 | auto myTextToPrint = myCommand.Parameter<std::vector<std::string>>({"some", "words"}, "words_to_print", "print some words");
28 | void myCommandCallback() {
29 | 	std::cout << "executing \"my_command\"" << std::endl;
30 | 	if (myCommandSpecificParameter) {
31 | 		std::cout << "hello" << std::endl;
32 | 	}
33 | 	// if the compiler can infer a cast to the underlying type of the parameter it will do so
34 | 	// also: explicit casts are possible to get the value of a parameter or simply use .get()
35 | 	for (auto const& word : myTextToPrint.get()) {
36 | 		std::cout << word << " ";
37 | 	}
38 | 	std::cout << std::endl;
39 | }
40 | 
41 | // choices (e.g., for enums) are also possible
42 | enum class MyEnumType {Foo, Bar};
43 | auto myChoice = sargparse::Choice<MyEnumType>{MyEnumType::Foo, "my_enum",
44 | 		{{"Foo", MyEnumType::Foo}, {"Bar", MyEnumType::Bar}}, "a choice demonstration"
45 | 	};
46 | }
47 | ~~~
48 | 
49 | file main.cpp:
50 | ~~~
51 | #include <sargparse/ArgumentParsing.h>
52 | #include <sargparse/Parameter.h>
53 | #include <iostream>
54 | 
55 | namespace {
56 | auto printHelp  = sargparse::Parameter<std::optional<std::string>>{{}, "help", "print this help add a string which will be used in a grep-like search through the parameters"};
57 | }
58 | 
59 | int main(int argc, char** argv)
60 | {
61 | 	// create you own bash completion with this helper
62 | 	if (std::string(argv[argc-1]) == "--bash_completion") {
63 | 		auto hints = sargparse::getNextArgHint(argc-2, argv+1);
64 | 		for (auto const& hint : hints) {
65 | 			std::cout << hint << " ";
66 | 		}
67 | 		return 0;
68 | 	}
69 | 
70 | 	// parse the arguments (excluding the application name) and fill all parameters/flags/choices with their respective values
71 | 	sargparse::parseArguments(argc-1, argv+1);
72 | 	if (printHelp.isSpecified()) { // print the help
73 | 		std::cout << sargparse::generateHelpString(std::regex{".*" + printHelp.get().value_or("") + ".*"});
74 | 		return 0;
75 | 	}
76 | 	sargparse::callCommands();
77 | 	return 0;
78 | }
79 | 


--------------------------------------------------------------------------------
/src/simplyfile/SerialPort.cpp:
--------------------------------------------------------------------------------
  1 | #include "SerialPort.h"
  2 | 
  3 | #include <fcntl.h>
  4 | #include <string.h>        // String function definitions
  5 | #include <unistd.h>        // UNIX standard function definitions
  6 | #include <errno.h>         // Error number definitions
  7 | #include <termios.h>
  8 | #include <sys/ioctl.h>
  9 | #include <inttypes.h>
 10 | #include <linux/types.h>
 11 | #include <linux/serial.h>
 12 | #include <iostream>
 13 | #include <stdexcept>
 14 | 
 15 | namespace simplyfile
 16 | {
 17 | 
 18 | namespace {
 19 | 
 20 | struct termios2 {
 21 | 	tcflag_t c_iflag;		/* input mode flags */
 22 | 	tcflag_t c_oflag;		/* output mode flags */
 23 | 	tcflag_t c_cflag;		/* control mode flags */
 24 | 	tcflag_t c_lflag;		/* local mode flags */
 25 | 	cc_t c_line;			/* line discipline */
 26 | 	cc_t c_cc[19];		/* control characters */
 27 | 	speed_t c_ispeed;		/* input speed */
 28 | 	speed_t c_ospeed;		/* output speed */
 29 | };
 30 | #ifndef BOTHER
 31 | #define BOTHER 0010000
 32 | #endif
 33 | 
 34 | }
 35 | 
 36 | SerialPort::SerialPort(std::string const& name, int baudrate)
 37 | {
 38 | 	FileDescriptor iFace{::open(name.c_str(), O_RDWR | O_NOCTTY | O_NDELAY)};
 39 | 
 40 | 	if (not iFace.valid()) {
 41 | 		throw std::runtime_error("cannot open serial port: " + name);
 42 | 	}
 43 | 
 44 | 	// when reading, return immediately
 45 | 	if (0 > fcntl(iFace, F_SETFL, FNDELAY)) {
 46 | 		throw std::runtime_error("F_SETFL " +std::string(strerror(errno)));
 47 | 	}
 48 | 	struct serial_struct serial;
 49 | 	bzero(&serial, sizeof(serial));
 50 | 	if (0 > ioctl(iFace, TIOCGSERIAL, &serial)) {
 51 | 		throw std::runtime_error("TIOCGSERIAL " +std::string(strerror(errno)));
 52 | 	}
 53 | 
 54 | 	serial.flags |= ASYNC_LOW_LATENCY;  /* enable low latency  */
 55 | 	if (0 > ioctl(iFace, TIOCSSERIAL, &serial)) {
 56 | 		std::cout << "cannot do TIOCSSERIAL on " << name << " "  << strerror(errno) << std::endl;
 57 | 	}
 58 | 
 59 | 	struct termios2 options;
 60 | 	bzero(&options, sizeof(options));
 61 | 	if (0 > ioctl(iFace, TCGETS2, &options)) {
 62 | 		throw std::runtime_error("TCGETS2 " +std::string(strerror(errno)));
 63 | 	}
 64 | 
 65 | 	// local line that supports reading
 66 | 	options.c_cflag |= (CLOCAL | CREAD);
 67 | 
 68 | 	// set 8N1
 69 | 	options.c_cflag &= ~PARENB;
 70 | 	options.c_cflag &= ~CSTOPB;
 71 | 	options.c_cflag &= ~CSIZE;
 72 | 	options.c_cflag |= CS8;
 73 | 
 74 | 	// set baudrate
 75 | 	options.c_ospeed = baudrate;
 76 | 	options.c_ispeed = baudrate;
 77 | 	options.c_cflag  &= ~CBAUD;
 78 | 	options.c_cflag  |= BOTHER;
 79 | 
 80 | 	// disable hardware flow control
 81 | 	options.c_cflag &= ~CRTSCTS;
 82 | 
 83 | 	// use raw mode (see "man cfmakeraw")
 84 | 	options.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
 85 | 	options.c_oflag &= ~OPOST;
 86 | 	options.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
 87 | 	options.c_cflag &= ~(CSIZE | PARENB);
 88 | 	options.c_cflag |= CS8;
 89 | 
 90 | 	if (0 > ioctl(iFace, TCSETS2, &options)) {
 91 | 		throw std::runtime_error("TCSETS2 " +std::string(strerror(errno)));
 92 | 	}
 93 | 
 94 | 	FileDescriptor::operator=(std::move(iFace));
 95 | }
 96 | 
 97 | void SerialPort::setBaudrate(int baudrate) {
 98 | 	struct termios2 options;
 99 | 	bzero(&options, sizeof(options));
100 | 	ioctl(*this, TCGETS2, &options);
101 | 
102 | 	options.c_cflag &= ~CBAUD;
103 | 	options.c_cflag |= BOTHER;
104 | 
105 | 	// set baudrate
106 | 	options.c_ospeed = baudrate;
107 | 	options.c_ispeed = baudrate;
108 | 	ioctl(*this, TCSETS2, &options);
109 | }
110 | 
111 | }
112 | 


--------------------------------------------------------------------------------
/src/metaCmd.cpp:
--------------------------------------------------------------------------------
  1 | #include <sargparse/Parameter.h>
  2 | #include "usb2dynamixel/USB2Dynamixel.h"
  3 | #include "usb2dynamixel/MotorMetaInfo.h"
  4 | 
  5 | namespace {
  6 | 
  7 | using namespace dynamixel;
  8 | 
  9 | void runMeta();
 10 | auto metaCmd   = sargp::Command{"meta", "list all motors known to this program", runMeta};
 11 | auto optMotorName = metaCmd.Parameter<std::string>("", "motor", "motor to give detail specs", {},
 12 | 	[](std::vector<std::string> const& _str) -> std::pair<bool, std::set<std::string>> {
 13 | 		std::set<std::string> resList;
 14 | 		meta::forAllLayoutTypes([&](auto const& info) {
 15 | 			using Info = std::decay_t<decltype(info)>;
 16 | 			auto const& defaults = Info::getDefaults();
 17 | 			if (_str.empty()) {
 18 | 				for (auto const& [id, data] : defaults) {
 19 | 					resList.insert(data.shortName);
 20 | 				}
 21 | 			} else {
 22 | 				for (auto const& [id, data] : defaults) {
 23 | 					if (data.shortName.size() < _str.size()) continue;
 24 | 					auto str = _str.back();
 25 | 					if (std::equal(begin(str), end(str), begin(data.shortName))) {
 26 | 						resList.insert(data.shortName);
 27 | 					}
 28 | 				}
 29 | 			}
 30 | 		});
 31 | 		return std::make_pair(true, std::move(resList));
 32 | 	}
 33 | );
 34 | 
 35 | using namespace dynamixel;
 36 | 
 37 | template <typename Iter, typename Delim>
 38 | auto join(Iter iter, Iter end, Delim const& delim) -> std::decay_t<decltype(*iter)> {
 39 | 	if (iter == end) {
 40 | 		return {};
 41 | 	}
 42 | 	auto res = *(iter++);
 43 | 	while (iter != end) {
 44 | 		res = res + delim + *(iter++);
 45 | 	}
 46 | 	return res;
 47 | }
 48 | 
 49 | void printDetailInfoTable(meta::MotorInfo const& data) {
 50 | 	std::cout << " addr | l | Ac | mem |   init |        unit |                          name | description \n";
 51 | 	std::cout << "------+---+----+-----+--------+-------------+-------------------------------+-------------------------------\n";
 52 | 
 53 | 	auto printEntries = [](auto const& layout, auto const& defaults) {
 54 | 		for (auto [id, info] : layout) {
 55 | 			auto iter = defaults.find(id);
 56 | 			if (iter == defaults.end()) continue;
 57 | 
 58 | 			std::cout.width(5);
 59 | 			std::cout << int(id) << " |";
 60 | 			std::cout.width(2);
 61 | 			std::cout << int(info.length) << " |";
 62 | 			std::cout.width(3);
 63 | 			std::cout << to_string(info.access) << " |";
 64 | 			std::cout << (info.romArea?" ROM":" RAM") << " |";
 65 | 
 66 | 			auto const& [optDefault, convert] = iter->second;
 67 | 			if (optDefault) {
 68 | 				std::cout.width(7);
 69 | 				std::cout << int(optDefault.value()) << " |";
 70 | 			} else {
 71 | 				std::cout << "      - |";
 72 | 			}
 73 | 			if (convert.toMotor and convert.fromMotor and optDefault) {
 74 | 				std::cout.width(9);
 75 | 				std::cout << convert.fromMotor(optDefault.value());
 76 | 				std::cout.width(3);
 77 | 				std::cout << convert.unit;
 78 | 				std::cout << " |";
 79 | 			} else {
 80 | 				std::cout << "           - |";
 81 | 			}
 82 | 			std::cout.width(30);
 83 | 			std::cout << info.name << " |";
 84 | 			std::cout.width(30);
 85 | 			std::cout << info.description;
 86 | 			std::cout << "\n";
 87 | 		}
 88 | 	};
 89 | 
 90 | 	meta::forAllLayoutTypes([&](auto const& info) {
 91 | 		using Info = std::decay_t<decltype(info)>;
 92 | 		if (data.layout == Info::Type) {
 93 | 			auto const& layout   = Info::getInfos();
 94 | 			auto const& defaults = Info::getDefaults().at(data.modelNumber).defaultLayout;
 95 | 			printEntries(layout, defaults);
 96 | 		}
 97 | 	});
 98 | 	if (data.layout == LayoutType::None) {
 99 | 		throw std::runtime_error("unknown layout type");
100 | 	}
101 | }
102 | 
103 | 
104 | void runMeta() {
105 | 	if (optMotorName) {
106 | 		auto name = *optMotorName;
107 | 		std::transform(begin(name), end(name), begin(name), ::toupper);
108 | 		auto info = meta::getMotorInfo(name);
109 | 		if (not info) {
110 | 			throw std::runtime_error("motor with name: " + name + " not found");
111 | 		}
112 | 		printDetailInfoTable(*info);
113 | 	} else {
114 | 		std::cout << "motors: \n";
115 | 		meta::forAllLayoutTypes([&](auto const& info) {
116 | 			using Info = std::decay_t<decltype(info)>;
117 | 			auto const& defaults = Info::getDefaults();
118 | 			for (auto const& [id, data] : defaults) {
119 | 				std::cout << data.shortName << ": [";
120 | 				std::cout << join(begin(data.motorNames), end(data.motorNames), ", ") << "]\n";
121 | 			}
122 | 		});
123 | 	}
124 | }
125 | 
126 | }
127 | 


--------------------------------------------------------------------------------
/src/usb2dynamixel/USB2Dynamixel.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | 
  3 | #include "dynamixel.h"
  4 | #include "ProtocolBase.h"
  5 | #include <simplyfile/SerialPort.h>
  6 | 
  7 | #include <cassert>
  8 | #include <chrono>
  9 | #include <cstring>
 10 | #include <map>
 11 | #include <memory>
 12 | #include <mutex>
 13 | #include <set>
 14 | #include <string>
 15 | 
 16 | #include "Layout.h"
 17 | 
 18 | #include <iostream>
 19 | 
 20 | namespace dynamixel {
 21 | 
 22 | enum class Protocol : int {
 23 | 	V1 = 1,
 24 | 	V2 = 2,
 25 | };
 26 | 
 27 | struct USB2Dynamixel {
 28 | 	using Timeout = std::chrono::microseconds;
 29 | 
 30 | 	USB2Dynamixel(int baudrate, std::string const& device, Protocol protocol = Protocol::V1);
 31 | 	~USB2Dynamixel();
 32 | 
 33 | 	[[nodiscard]] bool ping(MotorID motor, Timeout timeout) const;
 34 | 	[[nodiscard]] auto read(MotorID motor, int baseRegister, size_t length, Timeout timeout) const -> std::tuple<bool, MotorID, ErrorCode, Parameter>;
 35 | 	[[nodiscard]] auto bulk_read(std::vector<std::tuple<MotorID, int, size_t>> const& motors, Timeout timeout) const -> std::vector<std::tuple<MotorID, int, ErrorCode, Parameter>>;
 36 | 
 37 | 	void write(MotorID motor, int baseRegister, Parameter const& txBuf) const;
 38 | 	auto writeRead(MotorID motor, int baseRegister, Parameter const& txBuf, Timeout timeout) const -> std::tuple<bool, MotorID, ErrorCode, Parameter>;
 39 | 
 40 | 	void sync_write(std::map<MotorID, Parameter> const& motorParams, int baseRegister) const;
 41 | 
 42 | 	void reset(MotorID motor) const;
 43 | 	void reboot(MotorID motor)const;
 44 | 
 45 | 	template <auto baseRegister, size_t length>
 46 | 	[[nodiscard]] auto read(MotorID motor, Timeout timeout) -> std::tuple<bool, MotorID, ErrorCode, Layout<baseRegister, size_t(length)>> {
 47 | 		using RType = Layout<baseRegister, length>;
 48 | 		static_assert(length == sizeof(RType));
 49 | 
 50 | 		auto [timeoutFlag, motorID, errorCode, rxBuf] = read(motor, int(baseRegister), length, timeout);
 51 | 		if (timeoutFlag) {
 52 | 			return std::make_tuple(true, MotorIDInvalid, errorCode, RType{});
 53 | 		} else if (motorID == MotorIDInvalid) {
 54 | 			return std::make_tuple(false, MotorIDInvalid, errorCode, RType{});
 55 | 		}
 56 | 		return std::make_tuple(false, motorID, errorCode, RType(rxBuf));
 57 | 	}
 58 | 
 59 | 	template <auto baseRegister, size_t length, typename ...Extras>
 60 | 	[[nodiscard]] auto bulk_read(std::vector<std::tuple<MotorID, Extras...>> const& motors, USB2Dynamixel::Timeout timeout) -> std::vector<std::tuple<MotorID, Extras..., ErrorCode, Layout<baseRegister, length>>> {
 61 | 		if (motors.empty()) return {};
 62 | 
 63 | 		std::vector<std::tuple<MotorID, int, size_t>> request;
 64 | 		for (auto data : motors) {
 65 | 			auto id = std::get<0>(data);
 66 | 			request.push_back(std::make_tuple(id, int(baseRegister), size_t(length)));
 67 | 		}
 68 | 		auto list = bulk_read(request, timeout);
 69 | 
 70 | 		std::vector<std::tuple<MotorID, Extras..., ErrorCode, Layout<baseRegister, length>>> response;
 71 | 		auto iter = begin(motors);
 72 | 		for (auto const& [id, _reg, errorCode, params] : list) {
 73 | 			response.push_back(std::tuple_cat(*iter, std::make_tuple(errorCode, Layout<baseRegister, length>{params})));
 74 | 			++iter;
 75 | 		}
 76 | 		return response;
 77 | 	}
 78 | 
 79 | 	template <auto baseRegister, size_t length>
 80 | 	void write(MotorID motor, Layout<baseRegister, length> layout) const {
 81 | 		std::vector<std::byte> txBuf(sizeof(layout));
 82 | 		memcpy(txBuf.data(), &layout, sizeof(layout));
 83 | 		write(motor, int(baseRegister), txBuf);
 84 | 	}
 85 | 
 86 | 	template <auto baseRegister, size_t length>
 87 | 	[[nodiscard]] auto writeRead(MotorID motor, Layout<baseRegister, length> layout, Timeout timeout) const {
 88 | 		std::vector<std::byte> txBuf(sizeof(layout));
 89 | 		memcpy(txBuf.data(), &layout, sizeof(layout));
 90 | 		return writeRead(motor, int(baseRegister), txBuf, timeout);
 91 | 	}
 92 | 
 93 | 
 94 | 
 95 | 	template <template<auto, size_t> class Layout, auto baseRegister, size_t Length>
 96 | 	void sync_write(std::map<MotorID, Layout<baseRegister, Length>> const& params) {
 97 | 		if (params.empty()) return;
 98 | 
 99 | 		std::map<MotorID, Parameter> motorParams;
100 | 		for (auto const& [id, layout] : params) {
101 | 			auto& buffer = motorParams[id];
102 | 			buffer.resize(Length);
103 | 			memcpy(buffer.data(), &layout, Length);
104 | 		}
105 | 		sync_write(motorParams, int(baseRegister));
106 | 	}
107 | 
108 | private:
109 | 	std::unique_ptr<ProtocolBase> mProtocol;
110 | 	mutable std::mutex mMutex;
111 | 
112 | 	simplyfile::SerialPort mPort;
113 | };
114 | 
115 | 
116 | }
117 | 


--------------------------------------------------------------------------------
/Makefile:
--------------------------------------------------------------------------------
  1 | TARGET = inspexel
  2 | 
  3 | # compiler
  4 | CROSS_COMPILE_PREFIX = 
  5 | CC      ?= $(CROSS_COMPILE_PREFIX)gcc
  6 | CXX     ?= $(CROSS_COMPILE_PREFIX)g++
  7 | LD      ?= $(CROSS_COMPILE_PREFIX)g++
  8 | SIZE    ?= $(CROSS_COMPILE_PREFIX)size
  9 | GDB     ?= $(CROSS_COMPILE_PREFIX)gdb
 10 | OBJ_CPY ?= $(CROSS_COMPILE_PREFIX)objcopy
 11 | 
 12 | PREFIX              ?=
 13 | INSTALL_BIN_DIR     ?= $(PREFIX)/usr/bin
 14 | BASH_COMPLETION_DIR ?= $(PREFIX)/usr/share/bash-completion/completions/
 15 | ZSH_COMPLETION_DIR  ?= $(PREFIX)/usr/share/zsh-completion/completions/
 16 | MAN_DIR             ?= $(PREFIX)/usr/share/man/man1/
 17 | 
 18 | SRC_FOLDERS = src/
 19 | LIBS = c pthread stdc++fs fuse atomic
 20 | LIB_PATHS =
 21 | INCLUDES = src/ \
 22 | 
 23 | MAP_FILE = $(TARGET).map
 24 | 
 25 | 
 26 | INCLUDE_CMD = $(addprefix -I, $(INCLUDES))
 27 | LIB_CMD = $(addprefix -l, $(LIBS))
 28 | LIB_PATH_CMD = $(addprefix -L, $(LIB_PATHS))
 29 | 
 30 | # Flags
 31 | 
 32 | DEFINES		+= -D_FILE_OFFSET_BITS=64
 33 | 
 34 | FP_FLAGS       ?=
 35 | COMMON_FLAGS	+= $(DEFINES) $(FP_FLAGS)
 36 | COMMON_FLAGS	+= -O0 -g3
 37 | COMMON_FLAGS	+= $(INCLUDE_CMD)
 38 | 
 39 | # Warnings
 40 | W_FLAGS      += -Wextra -Wredundant-decls
 41 | W_FLAGS      += -Wall -Wundef
 42 | 
 43 | ###############################################################################
 44 | # C flags
 45 | 
 46 | CFLAGS		+= $(COMMON_FLAGS)
 47 | CFLAGS		+= $(W_FLAGS)
 48 | CFLAGS      += -Wimplicit-function-declaration -Wmissing-prototypes -Wstrict-prototypes
 49 | 
 50 | ###############################################################################
 51 | # C++ flags
 52 | 
 53 | CPPFLAGS	+= $(COMMON_FLAGS)
 54 | CPPFLAGS	+= $(W_FLAGS)
 55 | # add this for link-time template instanciation
 56 | CPPFLAGS	+= -std=c++17
 57 | CPPFLAGS	+= -I$(INCLUDE_DIR)
 58 | 
 59 | 
 60 | ###############################################################################
 61 | # Linker flags
 62 | 
 63 | LINKERFLAGS +=  $(COMMON_FLAGS)
 64 | #LINKERFLAGS += -s
 65 | 
 66 | CPP_SUFFIX = .cpp
 67 | C_SUFFIX   = .c
 68 | OBJ_SUFFIX = .o
 69 | DEP_SUFFIX = .d
 70 | OBJ_DIR    = obj/
 71 | 
 72 | IGNORE_STRINGS = /archive/
 73 | 
 74 | CPP_FILES            += $(sort $(filter-out $(IGNORE_STRINGS), $(foreach SRC_FOLDER, $(SRC_FOLDERS), $(shell find $(SRC_FOLDER) -name "*$(CPP_SUFFIX)" | grep -v $(addprefix -e, $(IGNORE_STRINGS))))))
 75 | C_FILES              += $(sort $(filter-out $(IGNORE_STRINGS), $(foreach SRC_FOLDER, $(SRC_FOLDERS), $(shell find $(SRC_FOLDER) -name "*$(C_SUFFIX)" | grep -v $(addprefix -e, $(IGNORE_STRINGS))))))
 76 | 
 77 | CPP_OBJ_FILES        += $(addsuffix $(OBJ_SUFFIX), $(addprefix $(OBJ_DIR), $(CPP_FILES)))
 78 | C_OBJ_FILES          += $(addsuffix $(OBJ_SUFFIX), $(addprefix $(OBJ_DIR), $(C_FILES)))
 79 | 
 80 | DEP_FILES            += $(addprefix $(OBJ_DIR), $(addsuffix $(DEP_SUFFIX), $(CPP_FILES) $(C_FILES)))
 81 | 
 82 | ifndef VERBOSE
 83 | SILENT = @
 84 | endif
 85 | 
 86 | 
 87 | .phony: all clean flash
 88 | 
 89 | all: $(TARGET)
 90 | 
 91 | dbg:
 92 | 	@ echo $(CPP_FILES)
 93 | 
 94 | clean:
 95 | 	$(SILENT) rm -rf $(OBJ_DIR) $(TARGET) $(TARGET).map $(TARGET).bin
 96 | 
 97 | install: $(TARGET)
 98 | 	$(SILENT) mkdir -p $(INSTALL_BIN_DIR)
 99 | 	$(SILENT) cp $< $(INSTALL_BIN_DIR)
100 | 	$(SILENT) mkdir -p $(BASH_COMPLETION_DIR)
101 | 	$(SILENT) cp scripts/bash_completion $(BASH_COMPLETION_DIR)/$<
102 | 	$(SILENT) mkdir -p $(ZSH_COMPLETION_DIR)
103 | 	$(SILENT) cp scripts/zsh_completion $(ZSH_COMPLETION_DIR)/$<
104 | 	$(SILENT) mkdir -p $(MAN_DIR)
105 | 	$(SILENT) scripts/generateMan.sh | gzip > $(MAN_DIR)/$<.1.gz
106 | 
107 | $(TARGET): $(CPP_OBJ_FILES) $(C_OBJ_FILES)
108 | 	@echo linking $(TARGET)
109 | 	$(SILENT) $(CXX) -o $@ $^ $(LINKERFLAGS) $(LIB_PATH_CMD) $(LIB_CMD)
110 | 	$(SILENT) $(SIZE) $@
111 | 	@ echo done
112 | 
113 | $(OBJ_DIR)%$(C_SUFFIX)$(OBJ_SUFFIX): %$(C_SUFFIX)
114 | 	@echo building $<
115 | 	@ mkdir -p $(dir $@)
116 | 	@ $(CC) $(CFLAGS) $(INCLUDE_CMD) -MM -MF $(OBJ_DIR)$<.d -c $<
117 | 	@ mv -f $(OBJ_DIR)$<.d $(OBJ_DIR)$<.d.tmp
118 | 	@ sed -e 's|.*:|$@:|' < $(OBJ_DIR)$<.d.tmp > $(OBJ_DIR)$<.d
119 | 	@ rm -f $(OBJ_DIR)$<.d.tmp
120 | 
121 | 	$(SILENT) $(CC) $(CFLAGS) $(INCLUDE_CMD) -o $@ -c $<
122 | 
123 | 
124 | $(OBJ_DIR)%$(CPP_SUFFIX)$(OBJ_SUFFIX): %$(CPP_SUFFIX)
125 | 	@echo building $<
126 | 	@ mkdir -p $(dir $@)
127 | 	@ $(CXX) $(CPPFLAGS) $(INCLUDE_CMD) -MM -MF $(OBJ_DIR)$<.d -c $<
128 | 	@ mv -f $(OBJ_DIR)$<.d $(OBJ_DIR)$<.d.tmp
129 | 	@ sed -e 's|.*:|$@:|' < $(OBJ_DIR)$<.d.tmp > $(OBJ_DIR)$<.d
130 | 	@ rm -f $(OBJ_DIR)$<.d.tmp
131 | 
132 | 	$(SILENT) $(CXX) $(CPPFLAGS) $(INCLUDE_CMD) -o $@ -c $<
133 | 
134 | -include $(DEP_FILES)
135 | 


--------------------------------------------------------------------------------
/src/setValues.cpp:
--------------------------------------------------------------------------------
  1 | #include "usb2dynamixel/USB2Dynamixel.h"
  2 | #include "usb2dynamixel/Layout.h"
  3 | #include "usb2dynamixel/MotorMetaInfo.h"
  4 | 
  5 | #include "globalOptions.h"
  6 | 
  7 | 
  8 | namespace {
  9 | 
 10 | void runSetAngle();
 11 | auto setAngleCmd = sargp::Command{"set_angle", "set the angle of a motor", runSetAngle};
 12 | auto angle       = setAngleCmd.Parameter<int>(0, "angle", "the goal angle (raw register value)");
 13 | 
 14 | void runSetAngle() {
 15 | 	bool error = false;
 16 | 	if (not g_id) {
 17 | 		std::cout << "need to specify the target id!" << std::endl;
 18 | 		error = true;
 19 | 	}
 20 | 	if (not angle) {
 21 | 		std::cout << "target angle has to be specified!" << std::endl;
 22 | 		error = true;
 23 | 	}
 24 | 	if (error) {
 25 | 		exit(-1);
 26 | 	}
 27 | 
 28 | 	auto usb2dyn = dynamixel::USB2Dynamixel(*g_baudrate, *g_device, *g_protocolVersion);
 29 | 	auto [timeoutFlag, motorID, errorCode, layout] = usb2dyn.read<dynamixel::mx_v1::Register::MODEL_NUMBER, 2>(dynamixel::MotorID(g_id), std::chrono::microseconds{g_timeout});
 30 | 	if (timeoutFlag) {
 31 | 		std::cout << "the specified motor is not present" << std::endl;
 32 | 		exit(-1);
 33 | 	}
 34 | 	auto modelPtr = dynamixel::meta::getMotorInfo(layout.model_number);
 35 | 	if (not modelPtr) {
 36 | 		std::cout << "the specified motor has an unknown register layout" << std::endl;
 37 | 		exit(-1);
 38 | 	}
 39 | 	if (modelPtr->layout == dynamixel::LayoutType::MX_V1) {
 40 | 		usb2dyn.write<dynamixel::mx_v1::Register::GOAL_POSITION, 2>(g_id, {int16_t(angle)});
 41 | 	} else if (modelPtr->layout == dynamixel::LayoutType::MX_V2) {
 42 | 		usb2dyn.write<dynamixel::mx_v2::Register::GOAL_POSITION, 4>(g_id, {int32_t(angle)});
 43 | 	} else if (modelPtr->layout == dynamixel::LayoutType::Pro) {
 44 | 		usb2dyn.write<dynamixel::pro::Register::GOAL_POSITION, 4>(g_id, {int32_t(angle)});
 45 | 	} else if (modelPtr->layout == dynamixel::LayoutType::XL320) {
 46 | 		usb2dyn.write<dynamixel::xl320::Register::GOAL_POSITION, 4>(g_id, {int16_t(angle)});
 47 | 	}
 48 | }
 49 | 
 50 | 
 51 | void runSetRegister();
 52 | auto setRegisterCmd = sargp::Command{"set_register", "set registers of a motor", runSetRegister};
 53 | auto reg            = setRegisterCmd.Parameter<int>(0, "register", "register to write to");
 54 | auto values         = setRegisterCmd.Parameter<std::vector<uint8_t>>({}, "values", "values to write to the register");
 55 | auto ids            = setRegisterCmd.Parameter<std::vector<int>>({}, "ids", "use this if you want to set multiple devices at once");
 56 | 
 57 | void runSetRegister() {
 58 | 	if (not g_id and not ids) throw std::runtime_error("need to specify the target g_id!");
 59 | 	if (not reg) throw std::runtime_error("target register has to be specified!");
 60 | 	if (not values) throw std::runtime_error("values to be written to the register have to be specified!");
 61 | 
 62 | 	auto f = [&](int id) {
 63 | 		std::cout << "set register " << reg << " of motor " << id << " to";
 64 | 		for (uint8_t v : std::vector<uint8_t>(values)) {
 65 | 			std::cout << " " << int(v);
 66 | 		}
 67 | 		std::cout << "\n";
 68 | 		dynamixel::Parameter txBuf;
 69 | 		for (auto x : values.get()) {
 70 | 			txBuf.push_back(std::byte{x});
 71 | 		}
 72 | 		auto usb2dyn = dynamixel::USB2Dynamixel(g_baudrate, g_device.get(), dynamixel::Protocol(g_protocolVersion.get()));
 73 | 		usb2dyn.write(id, int(reg), txBuf);
 74 | 	};
 75 | 	if (g_id) {
 76 | 		f(g_id);
 77 | 	}
 78 | 	if (ids) {
 79 | 		for (auto id : ids.get()) {
 80 | 			f(id);
 81 | 		}
 82 | 	}
 83 | }
 84 | 
 85 | void runGetValue();
 86 | auto getRegisterCmd = sargp::Command{"get_register", "get register(s) of a motor", runGetValue};
 87 | auto read_reg       = getRegisterCmd.Parameter<int>(0, "register", "register to read from");
 88 | auto count          = getRegisterCmd.Parameter<int>(1, "count", "amount of registers to read");
 89 | auto timeout        = getRegisterCmd.Parameter<int>(10000, "timeout", "timeout in us");
 90 | 
 91 | void runGetValue() {
 92 | 	if (not g_id) throw std::runtime_error("need to specify the target g_id!");
 93 | 	if (not read_reg) throw std::runtime_error("target angle has to be specified!");
 94 | 
 95 | 	auto usb2dyn = dynamixel::USB2Dynamixel(g_baudrate, g_device.get(), dynamixel::Protocol(g_protocolVersion.get()));
 96 | 	auto [timeoutFlag, valid, errorCode, rxBuf] = usb2dyn.read(g_id, read_reg, count, std::chrono::microseconds{timeout});
 97 | 	if (valid) {
 98 | 		std::cout << "motor " << static_cast<int>(g_id) << "\n";
 99 | 		std::cout << "registers:\n";
100 | 		for (size_t idx{0}; idx < rxBuf.size(); ++idx) {
101 | 			std::cout << "  " << std::setw(3) << std::setfill(' ') << std::dec << idx << ": " <<
102 | 					std::setw(2) << std::setfill('0') << std::hex << static_cast<int>(rxBuf[idx]) << "\n";
103 | 		}
104 | 		std::cout << "\n";
105 | 	}
106 | }
107 | 
108 | }
109 | 


--------------------------------------------------------------------------------
/src/simplyfile/Epoll.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | 
  3 | #include "FileDescriptor.h"
  4 | 
  5 | #include <chrono>
  6 | #include <memory>
  7 | #include <string>
  8 | #include <sys/epoll.h>
  9 | 
 10 | namespace simplyfile
 11 | {
 12 | 
 13 | namespace detail
 14 | {
 15 | template<typename T>
 16 | struct unique_func;
 17 | 
 18 | template<typename Res, typename... Args>
 19 | struct unique_func<Res(Args...)> {
 20 | private:
 21 |     struct PimplBase {
 22 |         PimplBase() = default;
 23 |         virtual ~PimplBase() = default;
 24 |         virtual Res invoke(Args &&... args) = 0;
 25 |         virtual std::type_info const& target_type() const = 0;
 26 |     };
 27 | 
 28 |     template<typename Functor>
 29 |     struct TypedPimpl : PimplBase {
 30 |         Functor ftor;
 31 |         TypedPimpl(Functor _ftor) : ftor{std::move(_ftor)} {}
 32 |         Res invoke(Args &&... args) override {
 33 |             return std::invoke(ftor, std::forward<Args>(args)...);
 34 |         }
 35 |         std::type_info const& target_type() const override {
 36 |             return typeid(Functor);
 37 |         }
 38 |     };
 39 |     std::unique_ptr<PimplBase> pimpl;
 40 | public:
 41 |     template<typename Functor>
 42 |     unique_func(Functor functor) : pimpl{std::make_unique<TypedPimpl<Functor>>(std::move(functor))} {}
 43 |     
 44 |     unique_func() = default;
 45 |     unique_func(unique_func&&) = default;
 46 |     unique_func& operator=(unique_func&&) = default;
 47 | 
 48 |     Res operator()(Args&& ... args) {
 49 |         return pimpl->invoke(std::forward<Args>(args)...);
 50 |     }
 51 | 
 52 |     std::type_info const& target_type() const {
 53 |         return pimpl->target_type();
 54 |     }
 55 | };
 56 | 
 57 | template<typename>
 58 | struct __function_guide_helper {};
 59 | 
 60 | template<typename _Res, typename _Tp, bool _Nx, typename... _Args>
 61 | struct __function_guide_helper<_Res (_Tp::*) (_Args...) noexcept(_Nx)>
 62 | { using type = _Res(_Args...); };
 63 | 
 64 | template<typename _Res, typename _Tp, bool _Nx, typename... _Args>
 65 | struct __function_guide_helper<_Res (_Tp::*) (_Args...) & noexcept(_Nx)>
 66 | { using type = _Res(_Args...); };
 67 | 
 68 | template<typename _Res, typename _Tp, bool _Nx, typename... _Args>
 69 | struct __function_guide_helper<_Res (_Tp::*) (_Args...) const noexcept(_Nx)>
 70 | { using type = _Res(_Args...); };
 71 | 
 72 | template<typename _Res, typename _Tp, bool _Nx, typename... _Args>
 73 | struct __function_guide_helper<_Res (_Tp::*) (_Args...) const & noexcept(_Nx)>
 74 | { using type = _Res(_Args...); };
 75 | 
 76 | template<typename _Functor, typename _Signature = typename detail::__function_guide_helper<decltype(&_Functor::operator())>::type>
 77 | unique_func(_Functor) -> unique_func<_Signature>;
 78 | 
 79 | }
 80 | 
 81 | struct Epoll : FileDescriptor {
 82 | 	using Callback = detail::unique_func<void(int)>;
 83 | 
 84 | 	Epoll();
 85 | 	Epoll(Epoll &&other) noexcept;
 86 | 	Epoll& operator=(Epoll &&rhs) noexcept;
 87 | 	~Epoll();
 88 | 
 89 | 	void addFD(int fd, Callback callback, int epollFlags = EPOLLIN|EPOLLET, std::string const& name="");
 90 | 	void modFD(int fd, int epollFlags = EPOLLIN|EPOLLET);
 91 | 	void rmFD(int fd, bool blocking);
 92 | 
 93 | 	void work(int maxEvents=1, int timeout_ms=-1) {
 94 | 		dispatch(wait(maxEvents, timeout_ms));
 95 | 	}
 96 | 
 97 | 	// call epoll_wait internally and return the list of events
 98 | 	std::vector<struct epoll_event> wait(int maxEvents=32, int timeout_ms=-1);
 99 | 
100 | 	// call all callbacks of the event list
101 | 	void dispatch(std::vector<struct epoll_event> const&);
102 | 
103 | 	// wakes up count thread that is calling wait
104 | 	void wakeup(uint64_t count=1) noexcept;
105 | 
106 | 	struct RuntimeInfo {
107 | 		std::chrono::nanoseconds accumulatedRuntime {0};
108 | 		int64_t numExecutions {0};
109 | 		RuntimeInfo& operator+=(RuntimeInfo const& rhs) {
110 | 			accumulatedRuntime += rhs.accumulatedRuntime;
111 | 			numExecutions += rhs.numExecutions;
112 | 			return *this;
113 | 		}
114 | 		RuntimeInfo& operator-=(RuntimeInfo const& rhs) {
115 | 			accumulatedRuntime -= rhs.accumulatedRuntime;
116 | 			numExecutions -= rhs.numExecutions;
117 | 			return *this;
118 | 		}
119 | 
120 | 		RuntimeInfo operator+(RuntimeInfo const& rhs) const {
121 | 			RuntimeInfo info{*this};
122 | 			info.accumulatedRuntime += rhs.accumulatedRuntime;
123 | 			info.numExecutions += rhs.numExecutions;
124 | 			return info;
125 | 		}
126 | 
127 | 		RuntimeInfo operator-(RuntimeInfo const& rhs) const {
128 | 			RuntimeInfo info{*this};
129 | 			info.accumulatedRuntime -= rhs.accumulatedRuntime;
130 | 			info.numExecutions -= rhs.numExecutions;
131 | 			return info;
132 | 		}
133 | 	};
134 | 	std::map<std::string, RuntimeInfo> getRuntimes() const;
135 | private:
136 | 	struct Pimpl;
137 | 	std::unique_ptr<Pimpl> pimpl;
138 | };
139 | 
140 | }
141 | 


--------------------------------------------------------------------------------
/src/usb2dynamixel/USB2Dynamixel.cpp:
--------------------------------------------------------------------------------
  1 | #include "USB2Dynamixel.h"
  2 | 
  3 | #include <cstdio>
  4 | #include <cstring>
  5 | 
  6 | #include <condition_variable>
  7 | #include <sstream>
  8 | #include <thread>
  9 | 
 10 | #include <simplyfile/SerialPort.h>
 11 | #include "ProtocolV1.h"
 12 | #include "ProtocolV2.h"
 13 | #include "file_io.h"
 14 | 
 15 | namespace dynamixel {
 16 | 
 17 | USB2Dynamixel::USB2Dynamixel(int baudrate, std::string const& device, Protocol protocol)
 18 | 	: mPort(device, baudrate)
 19 | {
 20 | 	file_io::flushRead(mPort);
 21 | 	if (protocol == Protocol::V1) {
 22 | 		mProtocol = std::make_unique<ProtocolV1>();
 23 | 	} else {
 24 | 		mProtocol = std::make_unique<ProtocolV2>();
 25 | 	}
 26 | }
 27 | 
 28 | USB2Dynamixel::~USB2Dynamixel() {
 29 | }
 30 | 
 31 | bool USB2Dynamixel::ping(MotorID motor, Timeout timeout) const {
 32 | 	auto g = std::lock_guard(mMutex);
 33 | 	file_io::write(mPort, mProtocol->createPacket(motor, Instruction::PING, {}));
 34 | 	auto [timeoutFlag, motorID, errorCode, rxBuf] = mProtocol->readPacket(timeout, motor, 0, mPort);
 35 | 	return motorID != MotorIDInvalid;
 36 | }
 37 | 
 38 | auto USB2Dynamixel::read(MotorID motor, int baseRegister, size_t length, Timeout timeout) const -> std::tuple<bool, MotorID, ErrorCode, Parameter> {
 39 | 	std::vector<std::byte> txBuf;
 40 | 	for (auto b : mProtocol->convertAddress(baseRegister)) {
 41 | 		txBuf.push_back(b);
 42 | 	}
 43 | 	for (auto b : mProtocol->convertLength(length)) {
 44 | 		txBuf.push_back(b);
 45 | 	}
 46 | 
 47 | 	auto g = std::lock_guard(mMutex);
 48 | 	file_io::write(mPort, mProtocol->createPacket(motor, Instruction::READ, txBuf));
 49 | 	return mProtocol->readPacket(timeout, motor, length, mPort);
 50 | }
 51 | 
 52 | 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>> {
 53 | 
 54 | 	std::vector<std::tuple<MotorID, int, ErrorCode, Parameter>> resList;
 55 | 	resList.reserve(motors.size());
 56 | 
 57 | 	auto txBuf = mProtocol->buildBulkReadPackage(motors);
 58 | 
 59 | 	auto g = std::lock_guard(mMutex);
 60 | 	file_io::write(mPort, mProtocol->createPacket(BroadcastID, Instruction::BULK_READ, txBuf));
 61 | 
 62 | 	for (auto const& [id, baseRegister, length] : motors) {
 63 | 		auto [timeoutFlag, motorID, errorCode, rxBuf] = mProtocol->readPacket(timeout, id, length, mPort);
 64 | 		if (motorID == MotorIDInvalid or motorID != id) {
 65 | 			break;
 66 | 		}
 67 | 		resList.push_back(std::make_tuple(id, baseRegister, errorCode, rxBuf));
 68 | 	}
 69 | 	return resList;
 70 | }
 71 | 
 72 | void USB2Dynamixel::write(MotorID motor, int baseRegister, Parameter const& txBuf) const {
 73 | 	std::vector<std::byte> parameters;
 74 | 	for (auto b : mProtocol->convertAddress(baseRegister)) {
 75 | 		parameters.push_back(b);
 76 | 	}
 77 | 	parameters.insert(parameters.end(), txBuf.begin(), txBuf.end());
 78 | 	auto g = std::lock_guard(mMutex);
 79 | 	file_io::write(mPort, mProtocol->createPacket(motor, Instruction::WRITE, parameters));
 80 | }
 81 | auto USB2Dynamixel::writeRead(MotorID motor, int baseRegister, Parameter const& txBuf, Timeout timeout) const -> std::tuple<bool, MotorID, ErrorCode, Parameter> {
 82 | 	write(motor, baseRegister, txBuf);
 83 | 	return mProtocol->readPacket(timeout, motor, 0, mPort);
 84 | }
 85 | 
 86 | 
 87 | void USB2Dynamixel::sync_write(std::map<MotorID, Parameter> const& motorParams, int baseRegister) const {
 88 | 	auto g = std::lock_guard(mMutex);
 89 | 
 90 | 	if (motorParams.empty()) {
 91 | 		throw std::runtime_error("sync_write: motorParams can't be empty");
 92 | 	}
 93 | 
 94 | 	const size_t len = motorParams.begin()->second.size();
 95 | 	bool const okay = std::all_of(begin(motorParams), end(motorParams), [&](auto param) {
 96 | 		return param.second.size() == len;
 97 | 	});
 98 | 
 99 | 	if (len <= 0 or not okay) {
100 | 		throw std::runtime_error("sync_write: data is not consistent");
101 | 	}
102 | 
103 | 	Parameter txBuf;
104 | 	for (auto b : mProtocol->convertAddress(baseRegister)) {
105 | 		txBuf.push_back(b);
106 | 	}
107 | 	for (auto b : mProtocol->convertLength(len)) {
108 | 		txBuf.push_back(b);
109 | 	}
110 | 
111 | 	for (auto const& [id, params] : motorParams) {
112 | 		txBuf.push_back(std::byte{id});
113 | 		txBuf.insert(txBuf.end(), params.begin(), params.end());
114 | 	}
115 | 
116 | 	file_io::write(mPort, mProtocol->createPacket(BroadcastID, Instruction::SYNC_WRITE, txBuf));
117 | }
118 | 
119 | void USB2Dynamixel::reset(MotorID motor) const {
120 | 	auto g = std::lock_guard(mMutex);
121 | 	file_io::write(mPort, mProtocol->createPacket(motor, Instruction::RESET, {}));
122 | 
123 | }
124 | 
125 | void USB2Dynamixel::reboot(MotorID motor) const {
126 | 	auto g = std::lock_guard(mMutex);
127 | 	file_io::write(mPort, mProtocol->createPacket(motor, Instruction::REBOOT, {}));
128 | }
129 | 
130 | }
131 | 


--------------------------------------------------------------------------------
/src/usb2dynamixel/LayoutXL320.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include "LayoutPart.h"
 4 | 
 5 | namespace dynamixel::xl320 {
 6 | 
 7 | enum class Register : int {
 8 | 	MODEL_NUMBER          = 0x00,
 9 | 	FIRMWARE_VERSION      = 0x02,
10 | 	ID                    = 0x03,
11 | 	BAUD_RATE             = 0x04,
12 | 	RETURN_DELAY_TIME     = 0x05,
13 | 	CW_ANGLE_LIMIT        = 0x06,
14 | 	CCW_ANGLE_LIMIT       = 0x08,
15 | 	CONTROL_MODE          = 0x0b,
16 | 	TEMPERATURE_LIMIT     = 0x0c,
17 | 	MIN_VOLTAGE_LIMIT     = 0x0d,
18 | 	MAX_VOLTAGE_LIMIT     = 0x0e,
19 | 	MAX_TORQUE            = 0x0f,
20 | 	STATUS_RETURN_LEVEL   = 0x11,
21 | 	SHUTDOWN              = 0x12,
22 | 	TORQUE_ENABLE         = 0x18,
23 | 	LED                   = 0x19,
24 | 	D_GAIN                = 0x1a,
25 | 	I_GAIN                = 0x1b,
26 | 	P_GAIN                = 0x1c,
27 | 	GOAL_POSITION         = 0x1e,
28 | 	MOVING_SPEED          = 0x20,
29 | 	TORQUE_LIMIT          = 0x23,
30 | 	PRESENT_POSITION      = 0x25,
31 | 	PRESENT_SPEED         = 0x27,
32 | 	PRESENT_LOAD          = 0x29,
33 | 	PRESENT_VOLTAGE       = 0x2d,
34 | 	PRESENT_TEMPERATURE   = 0x2e,
35 | 	REGISTERED            = 0x2f,
36 | 	MOVING                = 0x31,
37 | 	HARDWARE_ERROR_STATUS = 0x32,
38 | 	PUNCH                 = 0x33,
39 | };
40 | 
41 | constexpr Register operator+(Register t1, size_t t2) {
42 | 	return Register(size_t(t1) + t2);
43 | }
44 | 
45 | struct MotorLayoutInfo {
46 | 	static constexpr LayoutType Type{LayoutType::XL320};
47 | 	using FullLayout = Layout<Register::MODEL_NUMBER, 53>;
48 | 
49 | 	static auto getInfos()    -> meta::Layout<Register> const&;
50 | 	static auto getDefaults() -> std::map<uint32_t, meta::Info<Register>> const&;
51 | };
52 | 
53 | using FullLayout = MotorLayoutInfo::FullLayout;
54 | 
55 | }
56 | namespace dynamixel {
57 | 
58 | template <> struct meta::MotorLayoutInfo<LayoutType::XL320> : xl320::MotorLayoutInfo {};
59 | 
60 | #pragma pack(push, 1)
61 | DynamixelLayoutPart(xl320::Register::MODEL_NUMBER         , uint16_t, model_number         );
62 | DynamixelLayoutPart(xl320::Register::FIRMWARE_VERSION     ,  uint8_t, firmware_version     );
63 | DynamixelLayoutPart(xl320::Register::ID                   ,  uint8_t, id                   );
64 | DynamixelLayoutPart(xl320::Register::BAUD_RATE            ,  uint8_t, baud_rate            );
65 | DynamixelLayoutPart(xl320::Register::RETURN_DELAY_TIME    ,  uint8_t, return_delay_time    );
66 | DynamixelLayoutPart(xl320::Register::CW_ANGLE_LIMIT       ,  int16_t, cw_angle_limit       );
67 | DynamixelLayoutPart(xl320::Register::CCW_ANGLE_LIMIT      ,  int16_t, ccw_angle_limit      );
68 | DynamixelLayoutPart(xl320::Register::CONTROL_MODE         ,  uint8_t, control_mode         );
69 | DynamixelLayoutPart(xl320::Register::TEMPERATURE_LIMIT    ,  uint8_t, temperature_limit    );
70 | DynamixelLayoutPart(xl320::Register::MIN_VOLTAGE_LIMIT    ,  uint8_t, min_voltage_limit    );
71 | DynamixelLayoutPart(xl320::Register::MAX_VOLTAGE_LIMIT    ,  uint8_t, max_voltage_limit    );
72 | DynamixelLayoutPart(xl320::Register::MAX_TORQUE           , uint16_t, max_torque           );
73 | DynamixelLayoutPart(xl320::Register::STATUS_RETURN_LEVEL  ,  uint8_t, status_return_level  );
74 | DynamixelLayoutPart(xl320::Register::SHUTDOWN             ,  uint8_t, shutdown             );
75 | DynamixelLayoutPart(xl320::Register::TORQUE_ENABLE        ,     bool, torque_enable        );
76 | DynamixelLayoutPart(xl320::Register::LED                  ,     bool, led                  );
77 | DynamixelLayoutPart(xl320::Register::D_GAIN               ,  uint8_t, d_gain               );
78 | DynamixelLayoutPart(xl320::Register::I_GAIN               ,  uint8_t, i_gain               );
79 | DynamixelLayoutPart(xl320::Register::P_GAIN               ,  uint8_t, p_gain               );
80 | DynamixelLayoutPart(xl320::Register::GOAL_POSITION        ,  int16_t, goal_position        );
81 | DynamixelLayoutPart(xl320::Register::MOVING_SPEED         ,  int16_t, moving_speed         );
82 | DynamixelLayoutPart(xl320::Register::TORQUE_LIMIT         , uint16_t, torque_limit         );
83 | DynamixelLayoutPart(xl320::Register::PRESENT_POSITION     ,  int16_t, present_position     );
84 | DynamixelLayoutPart(xl320::Register::PRESENT_SPEED        ,  int16_t, present_speed        );
85 | DynamixelLayoutPart(xl320::Register::PRESENT_LOAD         ,  int16_t, present_load         );
86 | DynamixelLayoutPart(xl320::Register::PRESENT_VOLTAGE      ,  uint8_t, present_voltage      );
87 | DynamixelLayoutPart(xl320::Register::PRESENT_TEMPERATURE  ,  uint8_t, present_temperature  );
88 | DynamixelLayoutPart(xl320::Register::REGISTERED           ,  uint8_t, registered           );
89 | DynamixelLayoutPart(xl320::Register::MOVING               ,     bool, moving               );
90 | DynamixelLayoutPart(xl320::Register::HARDWARE_ERROR_STATUS,  uint8_t, hardware_error_status);
91 | DynamixelLayoutPart(xl320::Register::PUNCH                , uint16_t, punch                );
92 | 
93 | #pragma pack(pop)
94 | 
95 | }
96 | 


--------------------------------------------------------------------------------
/src/usb2dynamixel/LayoutAX.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include "LayoutPart.h"
 4 | 
 5 | namespace dynamixel::ax {
 6 | 
 7 | enum class Register : int {
 8 | 	MODEL_NUMBER          = 0x00,
 9 | 	FIRMWARE_VERSION      = 0x02,
10 | 	ID                    = 0x03,
11 | 	BAUD_RATE             = 0x04,
12 | 	RETURN_DELAY_TIME     = 0x05,
13 | 	CW_ANGLE_LIMIT        = 0x06,
14 | 	CCW_ANGLE_LIMIT       = 0x08,
15 | 	TEMPERATURE_LIMIT     = 0x0b,
16 | 	MIN_VOLTAGE_LIMIT     = 0x0c,
17 | 	MAX_VOLTAGE_LIMIT     = 0x0d,
18 | 	MAX_TORQUE            = 0x0e,
19 | 	STATUS_RETURN_LEVEL   = 0x10,
20 | 	ALARM_LED             = 0x11,
21 | 	SHUTDOWN              = 0x12,
22 | 	TORQUE_ENABLE         = 0x18,
23 | 	LED                   = 0x19,
24 | 	CW_COMPLIANCE_MARGIN  = 0x1a,
25 | 	CCW_COMPLIANCE_MARGIN = 0x1b,
26 | 	CW_COMPLIANCE_SLOPE   = 0x1c,
27 | 	CCW_COMPLIANCE_SLOPE  = 0x1d,
28 | 	GOAL_POSITION         = 0x1e,
29 | 	MOVING_SPEED          = 0x20,
30 | 	TORQUE_LIMIT          = 0x22,
31 | 	PRESENT_POSITION      = 0x24,
32 | 	PRESENT_SPEED         = 0x26,
33 | 	PRESENT_LOAD          = 0x28,
34 | 	PRESENT_VOLTAGE       = 0x2a,
35 | 	PRESENT_TEMPERATURE   = 0x2b,
36 | 	REGISTERED            = 0x2c,
37 | 	MOVING                = 0x2e,
38 | 	LOCK                  = 0x2f,
39 | 	PUNCH                 = 0x30,
40 | };
41 | 
42 | constexpr Register operator+(Register t1, size_t t2) {
43 | 	return Register(size_t(t1) + t2);
44 | }
45 | 
46 | struct MotorLayoutInfo {
47 | 	static constexpr LayoutType Type{LayoutType::AX};
48 | 	using FullLayout = Layout<Register::MODEL_NUMBER, 50>;
49 | 
50 | 	static auto getInfos()    -> meta::Layout<Register> const&;
51 | 	static auto getDefaults() -> std::map<uint32_t, meta::Info<Register>> const&;
52 | };
53 | 
54 | using FullLayout = MotorLayoutInfo::FullLayout;
55 | 
56 | }
57 | namespace dynamixel {
58 | 
59 | template <> struct meta::MotorLayoutInfo<LayoutType::AX> : ax::MotorLayoutInfo {};
60 | 
61 | #pragma pack(push, 1)
62 | DynamixelLayoutPart(ax::Register::MODEL_NUMBER         , uint16_t, model_number         );
63 | DynamixelLayoutPart(ax::Register::FIRMWARE_VERSION     ,  uint8_t, firmware_version     );
64 | DynamixelLayoutPart(ax::Register::ID                   ,  uint8_t, id                   );
65 | DynamixelLayoutPart(ax::Register::BAUD_RATE            ,  uint8_t, baud_rate            );
66 | DynamixelLayoutPart(ax::Register::RETURN_DELAY_TIME    ,  uint8_t, return_delay_time    );
67 | DynamixelLayoutPart(ax::Register::CW_ANGLE_LIMIT       ,  int16_t, cw_angle_limit       );
68 | DynamixelLayoutPart(ax::Register::CCW_ANGLE_LIMIT      ,  int16_t, ccw_angle_limit      );
69 | DynamixelLayoutPart(ax::Register::TEMPERATURE_LIMIT    ,  uint8_t, temperature_limit    );
70 | DynamixelLayoutPart(ax::Register::MIN_VOLTAGE_LIMIT    ,  uint8_t, min_voltage_limit    );
71 | DynamixelLayoutPart(ax::Register::MAX_VOLTAGE_LIMIT    ,  uint8_t, max_voltage_limit    );
72 | DynamixelLayoutPart(ax::Register::MAX_TORQUE           , uint16_t, max_torque           );
73 | DynamixelLayoutPart(ax::Register::STATUS_RETURN_LEVEL  ,  uint8_t, status_return_level  );
74 | DynamixelLayoutPart(ax::Register::ALARM_LED            ,  uint8_t, alarm_led            );
75 | DynamixelLayoutPart(ax::Register::SHUTDOWN             ,  uint8_t, shutdown             );
76 | DynamixelLayoutPart(ax::Register::TORQUE_ENABLE        ,     bool, torque_enable        );
77 | DynamixelLayoutPart(ax::Register::LED                  ,     bool, led                  );
78 | DynamixelLayoutPart(ax::Register::CW_COMPLIANCE_MARGIN ,  uint8_t, cw_compliance_margin );
79 | DynamixelLayoutPart(ax::Register::CCW_COMPLIANCE_MARGIN,  uint8_t, ccw_compliance_margin);
80 | DynamixelLayoutPart(ax::Register::CW_COMPLIANCE_SLOPE  ,  uint8_t, cw_compliance_slope  );
81 | DynamixelLayoutPart(ax::Register::CCW_COMPLIANCE_SLOPE ,  uint8_t, ccw_compliance_slope );
82 | DynamixelLayoutPart(ax::Register::GOAL_POSITION        ,  int16_t, goal_position        );
83 | DynamixelLayoutPart(ax::Register::MOVING_SPEED         ,  int16_t, moving_speed         );
84 | DynamixelLayoutPart(ax::Register::TORQUE_LIMIT         , uint16_t, torque_limit         );
85 | DynamixelLayoutPart(ax::Register::PRESENT_POSITION     ,  int16_t, present_position     );
86 | DynamixelLayoutPart(ax::Register::PRESENT_SPEED        ,  int16_t, present_speed        );
87 | DynamixelLayoutPart(ax::Register::PRESENT_LOAD         ,  int16_t, present_load         );
88 | DynamixelLayoutPart(ax::Register::PRESENT_VOLTAGE      ,  uint8_t, present_voltage      );
89 | DynamixelLayoutPart(ax::Register::PRESENT_TEMPERATURE  ,  uint8_t, present_temperature  );
90 | DynamixelLayoutPart(ax::Register::REGISTERED           ,  uint8_t, registered           );
91 | DynamixelLayoutPart(ax::Register::MOVING               ,     bool, moving               );
92 | DynamixelLayoutPart(ax::Register::LOCK                 ,     bool, lock);
93 | DynamixelLayoutPart(ax::Register::PUNCH                , uint16_t, punch                );
94 | 
95 | #pragma pack(pop)
96 | 
97 | }
98 | 


--------------------------------------------------------------------------------
/src/utils/Registry.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | 
  3 | #include "Singleton.h"
  4 | 
  5 | #include <map>
  6 | #include <set>
  7 | #include <utility>
  8 | #include <stdexcept>
  9 | 
 10 | namespace registry
 11 | {
 12 | template<typename ValueType>
 13 | struct Registry : Singleton<Registry<ValueType>>{
 14 | 	using ContainerType = std::set<ValueType*>;
 15 | 
 16 | 	void registerInstance(ValueType* inst) {
 17 | 		instances.emplace(inst);
 18 | 	}
 19 | 	void deregisterInstance(ValueType* inst) {
 20 | 		instances.erase(inst);
 21 | 	}
 22 | 	ContainerType const& getInstances() {
 23 | 		return instances;
 24 | 	}
 25 | protected:
 26 | 	ContainerType instances;
 27 | };
 28 | 
 29 | template<typename ValueType>
 30 | struct Registerable {
 31 | 	using RegistryType = Registry<ValueType>;
 32 | 
 33 | 	Registerable(ValueType* _value)
 34 | 		: value(_value)
 35 | 	{
 36 | 		RegistryType::getInstance().registerInstance(value);
 37 | 	}
 38 | 	~Registerable() {
 39 | 		RegistryType::getInstance().deregisterInstance(value);
 40 | 	}
 41 | private:
 42 | 	ValueType* value;
 43 | };
 44 | 
 45 | template<typename ValueType>
 46 | struct IndirectRegisterable : ValueType {
 47 | 	using RegistryType = Registry<ValueType>;
 48 | 	template<typename... Args>
 49 | 	IndirectRegisterable(Args &&... args)
 50 | 	: ValueType(std::forward<Args>(args)...) {
 51 | 		RegistryType::getInstance().registerInstance(this);
 52 | 	}
 53 | 	~IndirectRegisterable() {
 54 | 		RegistryType::getInstance().deregisterInstance(this);
 55 | 	}
 56 | };
 57 | 
 58 | 
 59 | template<typename KeyType, typename ValueType>
 60 | struct UniqueKeyedRegistry : Singleton<UniqueKeyedRegistry<KeyType, ValueType>>{
 61 | 	using ContainerType = std::map<KeyType, ValueType*>;
 62 | 
 63 | 	void registerInstance(KeyType const& key, ValueType* inst) {
 64 | 		if (instances.find(key) != instances.end()) {
 65 | 			throw std::runtime_error("cannot insert multiple items with the same key into a UniqueRegistry");
 66 | 		}
 67 | 		instances.emplace(key, inst);
 68 | 	}
 69 | 	void deregisterInstance(KeyType const& key) {
 70 | 		instances.erase(key);
 71 | 	}
 72 | 	ContainerType const& getInstances() {
 73 | 		return instances;
 74 | 	}
 75 | protected:
 76 | 	ContainerType instances;
 77 | };
 78 | 
 79 | template<typename KeyType, typename ValueType>
 80 | struct UniqueKeyedRegisterable {
 81 | 	using RegistryType = UniqueKeyedRegistry<KeyType, ValueType>;
 82 | 
 83 | 	UniqueKeyedRegisterable(KeyType const& _key, ValueType* value)
 84 | 	: key(_key) {
 85 | 		RegistryType::getInstance().registerInstance(_key, value);
 86 | 	}
 87 | 	~UniqueKeyedRegisterable() {
 88 | 		RegistryType::getInstance().deregisterInstance(key);
 89 | 	}
 90 | 	KeyType const& getKey() const {
 91 | 		return key;
 92 | 	}
 93 | private:
 94 | 	KeyType key;
 95 | };
 96 | 
 97 | template<typename KeyType, typename ValueType>
 98 | struct UniqueKeyedInheritedRegisterable : ValueType {
 99 | 	using RegistryType = UniqueKeyedRegistry<KeyType, ValueType>;
100 | 
101 | 	template<typename... Args>
102 | 	UniqueKeyedInheritedRegisterable(KeyType const& _key, Args &&... args)
103 | 	: ValueType(std::forward<Args>(args)...), key(_key) {
104 | 		RegistryType::getInstance().registerInstance(_key, this);
105 | 	}
106 | 	~UniqueKeyedInheritedRegisterable() {
107 | 		RegistryType::getInstance().deregisterInstance(key);
108 | 	}
109 | 	KeyType const& getKey() const {
110 | 		return key;
111 | 	}
112 | private:
113 | 	KeyType key;
114 | };
115 | 
116 | 
117 | template<typename KeyType, typename ValueType>
118 | struct KeyedRegistry : Singleton<KeyedRegistry<KeyType, ValueType>>{
119 | 	using ContainerType = std::multimap<KeyType, ValueType*>;
120 | 
121 | 	void registerInstance(KeyType const& key, ValueType* inst) {
122 | 		instances.emplace(key, inst);
123 | 	}
124 | 	void deregisterInstance(KeyType const& key, ValueType* inst) {
125 | 		auto range = instances.equal_range(key);
126 | 		for (auto it = range.first; it != range.second; ++it) {
127 | 			if (it->second == inst) {
128 | 				instances.erase(it);
129 | 				break;
130 | 			}
131 | 		}
132 | 		instances.erase(key);
133 | 	}
134 | 	ContainerType const& getInstances() {
135 | 		return instances;
136 | 	}
137 | protected:
138 | 	ContainerType instances;
139 | };
140 | 
141 | template<typename KeyType, typename ValueType>
142 | struct KeyedRegisterable {
143 | 	using RegistryType = KeyedRegistry<KeyType, ValueType>;
144 | 	KeyedRegisterable(KeyType const& _key, ValueType* _value)
145 | 	: key(_key)
146 | 	, value(_value) {
147 | 		RegistryType::getInstance().registerInstance(_key, _value);
148 | 	}
149 | 	~KeyedRegisterable() {
150 | 		RegistryType::getInstance().deregisterInstance(key, this);
151 | 	}
152 | 	KeyType const& getKey() const {
153 | 		return key;
154 | 	}
155 | private:
156 | 	KeyType key;
157 | 	ValueType* value;
158 | };
159 | 
160 | template<typename KeyType, typename ValueType>
161 | struct KeyedInheritedRegisterable : ValueType {
162 | 	using RegistryType = KeyedRegistry<KeyType, ValueType>;
163 | 	template<typename... Args>
164 | 	KeyedInheritedRegisterable(KeyType const& _key, Args &&... args)
165 | 	: ValueType(std::forward<Args>(args)...), key(_key) {
166 | 		RegistryType::getInstance().registerInstance(_key, this);
167 | 	}
168 | 	~KeyedInheritedRegisterable() {
169 | 		RegistryType::getInstance().deregisterInstance(key, this);
170 | 	}
171 | 	KeyType const& getKey() const {
172 | 		return key;
173 | 	}
174 | private:
175 | 	KeyType key;
176 | };
177 | 
178 | }
179 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
  1 | # [inspexel](https://gottliebtfreitag.de/code/inspexel.html)
  2 | 
  3 | The swiss army knife for dynamixel servo motors.
  4 | 
  5 | This software's purpose is to have a command line tool to configure dynamixel motors. 
  6 | Dynamixel motors are being used in many robotic projects but it seems like some tooling around the motors is missing.
  7 | There exists the robotis tool RoboManager which only runs under Windows.
  8 | There are also other projects like [Mixcell](https://github.com/clebercoutof/mixcell) which bring some of the functionality to Linux.
  9 | One big issue with these tools is that they have a GUI which makes it hard to use them over ssh on remote computers.
 10 | Also it is not possible to use them inside of scripts.
 11 | Inspexel is a command-line only utility to fill this gap.
 12 | It also enables the generation of scripted (via bash) motions.
 13 | 
 14 | # Features
 15 | 
 16 | - Automatic motor discovery
 17 | - All baudrates (even nonstandard baudrates)
 18 | - Dynamixel Protocol V1 and V2
 19 | - Support for all currently produced dynamixel motors
 20 | - Reading registers with additional information and pretty output
 21 | - Reading/Writing individual registers or register groups
 22 | - Represent motors and their registers as a fuse mounted filesystem for easy scriptability
 23 | - Reboot motors
 24 | 
 25 | ## supported motors
 26 | 
 27 | | Family | Subtypes |
 28 | |--------|----------|
 29 | | MX28 | MX-28T, MX-28R, MX-28AT, MX-28AR |
 30 | | MX64 | MX-64T, MX-64R, MX-64AT, MX-64AR |
 31 | | MX106 | MX-106T, MX-106R |
 32 | | MX12 | MX-12W |
 33 | | MX28-V2 | MX-28T-V2, MX-28R-V2, MX-28AT-V2, MX-28AR-V2 |
 34 | | MX64-V2 | MX-64T-V2, MX-64R-V2, MX-64AT-V2, MX-64AR-V2 |
 35 | | MX106-V2 | MX-106T-V2, MX-106R-V2 |
 36 | | XH430-W350 | XH430-W350-T, XH430-W350-R |
 37 | | XH430-W210 | XH430-W210-T, XH430-W210-R |
 38 | | XM430-W350 | XM430-W350-T, XM430-W350-R |
 39 | | XM430-W210 | XM430-W210-T, XM430-W210-R |
 40 | | XH430-V350 | XH430-V350 |
 41 | | XH430-V210 | XH430-V210 |
 42 | | XL430-W250 | XL430-W250 |
 43 | | XM540-W150 | XM540-W150-T, XM540-W150-R |
 44 | | XM540-W270 | XM540-W270-T, XM540-W270-R |
 45 | | M42-10-S260-R | M42-10-S260-R |
 46 | | M54-40-S250-R | M54-40-S250-R |
 47 | | M54-60-S250-R | M54-60-S250-R |
 48 | | H42-20-S300-R | H42-20-S300-R |
 49 | | H54-100-S500-R | H54-100-S500-R |
 50 | | H54-200-S500-R | H54-200-S500-R |
 51 | | XL-320 | XL-320 |
 52 | | AX-12A | AX-12A |
 53 | | AX-18A | AX-18A |
 54 | | AX-12W | AX-12W |
 55 | 
 56 | # Usage
 57 | 
 58 | Inspexel comes with several subcommands.
 59 | Each subommand represents a different aspect or way to configure a dynamixel motor or inquire its configuration.
 60 | All commands accept the arguments
 61 | - `--device [path-to-serial-device]` select the serial device
 62 | - `--baudrate [baudrate-in-baud]` select the baudrate to communicate to the motors (some commands also support multiple baudrates)
 63 | - `--protocol_verion [1/2]` use either protocol version 1 or 2
 64 | 
 65 | ## detect all connected motors
 66 | ```
 67 | $ inspexel
 68 | ```
 69 | or:
 70 | ```
 71 | $ inspexel detect
 72 | ```
 73 | 
 74 | Add the flag `--read_all` flag to get the content of all registers nicely printed.
 75 | 
 76 | ```
 77 | $ inspexel detect --read_all
 78 | ```
 79 | 
 80 | <figure>
 81 |     {% picture default assets/images/inspexel.png --alt console output of inspexel %}
 82 |     <figcaption>console output of inspexel</figcaption>
 83 | </figure>
 84 | 
 85 | ## Reboot a motor
 86 | Reboots motor with id 3
 87 | 
 88 | ```
 89 | $ inspexel reboot --id 3
 90 | ```
 91 | 
 92 | ## Manual setting of register(s)
 93 | Set register 0x44 (68) of motor 3 to value 1
 94 | 
 95 | ```
 96 | $ inspexel set_register --register 0x44 --values 1 --id 0x03
 97 | ```
 98 | 
 99 | ## Fuse integration
100 | Inspexel can expose all registers of all connected as a fuse filesystem.
101 | 
102 | ```
103 | $ inspexel fuse
104 | ```
105 | 
106 | Inspexel will create a directory `dynamixelFS`.
107 | Then a detect cycle is run automatically and every detected motor will be represented in a subdirectory of `dynamixelFS` (e.g., `dynamixelFS/11/` for motor with id 11).
108 | Within that directory are two subdirectories containing files representing the registers either by name `dynamixelFS/11/by-register-name` or by address `dynamixelFS/11/by-register-id`.
109 | A read on any of the containing files will make inspexel perform a read of the corresponding register and return the content as string.
110 | You can use that to live monitor the value of a register:
111 | 
112 | ```
113 | $ inspexel fuse & && watch cat "dynamixelFS/11/by-register-name/Present\ Position"
114 | ```
115 | 
116 | Likewise you can set register values as if they were files:
117 | 
118 | ```
119 | $ inspexel fuse & && echo 1 > dynamixelFS/11/by-register-name/LED
120 | ```
121 | 
122 | Further you can manually trigger detection of a motor by writing the motorID to look for to `dynamixelFS/detect_motor`:
123 | 
124 | ```
125 | $ echo 11 > dynamixelFS/detect_motor
126 | ```
127 | 
128 | 
129 | ## Miscellaneous
130 | 
131 | ### getting help:
132 | 
133 | ```
134 | $ inspexel --help
135 | ```
136 | 
137 | ### Manpage:
138 | 
139 | ```
140 | $ man inspexel
141 | ```
142 | 
143 | # How to install
144 | ## Ubuntu 20.04
145 | 
146 | ```
147 | # install Fuse deps
148 | $ sudo apt-get install libfuse-dev
149 | # build inspexel
150 | $ git clone https://github.com/gottliebtfreitag/inspexel.git
151 | $ cd inspexel
152 | $ make && sudo make install
153 | ```
154 | 
155 | ## Archlinux
156 | ```
157 | # build inspexel
158 | $ git clone https://github.com/gottliebtfreitag/inspexel.git
159 | $ cd inspexel
160 | $ make && sudo make install
161 | ```
162 | 


--------------------------------------------------------------------------------
/src/usb2dynamixel/LayoutPart.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | 
  3 | #include <algorithm>
  4 | #include <cmath>
  5 | #include <cstddef>
  6 | #include <cstdint>
  7 | #include <cstring>
  8 | #include <functional>
  9 | #include <map>
 10 | #include <map>
 11 | #include <optional>
 12 | #include <stdexcept>
 13 | #include <vector>
 14 | 
 15 | namespace dynamixel {
 16 | 
 17 | template <auto type>
 18 | struct LayoutPart {
 19 | 	using PartType = uint8_t;
 20 | 	LayoutPart() = default;
 21 | 	LayoutPart(LayoutPart const& _other) = default;
 22 | 	auto operator=(LayoutPart const& _other) -> LayoutPart& = default;
 23 | 
 24 | 	LayoutPart(uint8_t value)
 25 | 		: _oneByte {value}
 26 | 	{}
 27 | 
 28 | 	PartType _oneByte {};
 29 | 	template <typename L> void visit(L l) const { l(type, _oneByte); }
 30 | 	template <typename L> void visit(L l) { l(type, _oneByte); }
 31 | 	[[nodiscard]] bool reserved() const { return true; }
 32 | };
 33 | 
 34 | #define DynamixelLayoutPart(enum, type, name) \
 35 | template <> \
 36 | struct LayoutPart<enum> { \
 37 | 	using PartType = type; \
 38 | 	LayoutPart() = default; \
 39 | 	LayoutPart(LayoutPart const& _other) = default; \
 40 | 	auto operator=(LayoutPart const& _other) -> LayoutPart& = default; \
 41 | 	LayoutPart(type value) \
 42 | 		: name {value} \
 43 | 	{} \
 44 | 	\
 45 | 	type name {}; \
 46 | 	template <typename L> void visit(L l) const { l(enum, name); } \
 47 | 	template <typename L> void visit(L l) { l(enum, name); } \
 48 | 	bool reserved() const { return false; } \
 49 | };
 50 | 
 51 | template <auto type, size_t L>
 52 | struct Layout : LayoutPart<type> , Layout<type+sizeof(LayoutPart<type>), L-sizeof(LayoutPart<type>)> {
 53 | 	using SuperClass = Layout<type+sizeof(LayoutPart<type>), L-sizeof(LayoutPart<type>)>;
 54 | 	using Part = LayoutPart<type>;
 55 | 	using PartType = typename Part::PartType;
 56 | 	using Type = std::decay_t<decltype(type)>;
 57 | 
 58 | 	Layout() = default;
 59 | 	Layout(Layout const& _other) = default;
 60 | 	auto operator=(Layout const& _other) -> Layout& = default;
 61 | 	explicit Layout(std::vector<std::byte> const& buffer) {
 62 | 		if (buffer.size() != sizeof(Layout)) {
 63 | 			throw std::runtime_error("buffer " + std::to_string(buffer.size()) + " has not same size as layout " + std::to_string(sizeof(Layout)));
 64 | 		}
 65 | 		memcpy((void*)this, buffer.data(), sizeof(Layout));
 66 | 	}
 67 | 
 68 | 	template <typename ...Args>
 69 | 	Layout(PartType head, Args...next)
 70 | 		: LayoutPart<type>{head}
 71 | 		, SuperClass{next...}
 72 | 	{}
 73 | 
 74 | 	static_assert(L >= sizeof(LayoutPart<type>), "must fit layout size");
 75 | 	static constexpr Type BaseRegister {type};
 76 | 	static constexpr size_t Length {L};
 77 | 
 78 | 	template <Type type2>
 79 | 	struct Has {
 80 | 		static constexpr bool value = type <= type2 and int(type) + L > int(type2);
 81 | 	};
 82 | 	template <Type type2>
 83 | 	static constexpr bool has = Has<type2>::value;
 84 | 
 85 | 	template <typename CB>
 86 | 	void visit(CB cb) const {
 87 | 		LayoutPart<type>::visit(cb);
 88 | 		SuperClass::visit(cb);
 89 | 	}
 90 | 	template <typename CB>
 91 | 	void visit(CB cb) {
 92 | 		LayoutPart<type>::visit(cb);
 93 | 		SuperClass::visit(cb);
 94 | 	}
 95 | };
 96 | 
 97 | template <auto type>
 98 | struct Layout<type, 0> {
 99 | 	template <typename L> void visit(L) const {}
100 | 	template <typename L> void visit(L) {}
101 | };
102 | 
103 | template <typename CB, auto Register, size_t L>
104 | void visit(CB cb, Layout<Register, L> const& o) {
105 | 	o.visit(cb);
106 | }
107 | 
108 | template <typename CB, auto Register, size_t L>
109 | void visit(CB cb, Layout<Register, L>& o) {
110 | 	o.visit(cb);
111 | }
112 | 
113 | enum class LayoutType { None, MX_V1, MX_V2, Pro, XL320, AX };
114 | 
115 | inline auto to_string(LayoutType layout) -> std::string {
116 | 	switch(layout) {
117 | 	case LayoutType::MX_V1:    return "MX_V1";
118 | 	case LayoutType::MX_V2:    return "MX_V2";
119 | 	case LayoutType::Pro:   return "Pro";
120 | 	case LayoutType::XL320: return "XL320";
121 | 	case LayoutType::AX:    return "AX";
122 | 	default:
123 | 		throw std::runtime_error("unknown layout");
124 | 	}
125 | }
126 | 
127 | 
128 | namespace meta {
129 | struct LayoutField {
130 | 	uint16_t   length;
131 | 	bool       romArea;
132 | 	enum class Access { R = 0x01, W = 0x02, RW = 0x03 };
133 | 	Access      access;
134 | 	std::string name;
135 | 	std::string description;
136 | };
137 | 
138 | inline auto to_string(LayoutField::Access access) -> std::string {
139 | 	switch(access) {
140 | 	case LayoutField::Access::R: return "R";
141 | 	case LayoutField::Access::W: return "W";
142 | 	case LayoutField::Access::RW: return "RW";
143 | 	}
144 | 	throw std::runtime_error("unknown access value");
145 | }
146 | 
147 | struct Convert {
148 | 	std::string unit;
149 | 	std::function<int(double)> toMotor;
150 | 	std::function<double(int)> fromMotor;
151 | };
152 | 
153 | template <typename Reg>
154 | using Layout = std::map<Reg, LayoutField>;
155 | 
156 | 
157 | template <typename Reg>
158 | using DefaultLayout = std::map<Reg, std::tuple<std::optional<int32_t>, Convert>>;
159 | 
160 | template <typename Register>
161 | struct Info {
162 | 	uint16_t modelNumber;
163 | 	LayoutType layout;
164 | 
165 | 	std::string shortName;
166 | 	std::vector<std::string> motorNames;
167 | 
168 | 	DefaultLayout<Register> defaultLayout;
169 | };
170 | 
171 | inline auto buildConverter(std::string unit, double resolution, int centerVal=0, int minValue=std::numeric_limits<int>::min(), int maxValue = std::numeric_limits<int>::max()) -> Convert {
172 | 	return Convert{
173 | 		unit,
174 | 		[=](double val) { return std::clamp(int(std::round(val / resolution + centerVal)), minValue, maxValue); },
175 | 		[=](int val)    { return (val - centerVal) * resolution; },
176 | 	};
177 | }
178 | 
179 | 
180 | template <LayoutType type> struct MotorLayoutInfo;
181 | 
182 | }
183 | 
184 | }
185 | 


--------------------------------------------------------------------------------
/src/interactCmd.cpp:
--------------------------------------------------------------------------------
  1 | #include "usb2dynamixel/USB2Dynamixel.h"
  2 | #include "usb2dynamixel/MotorMetaInfo.h"
  3 | #include "globalOptions.h"
  4 | 
  5 | #include <algorithm>
  6 | #include <numeric>
  7 | 
  8 | #include <thread>
  9 | #include <atomic>
 10 | #include <unistd.h>
 11 | 
 12 | #define TERM_RED                        "\033[31m"
 13 | #define TERM_GREEN                      "\033[32m"
 14 | #define TERM_RESET                      "\033[0m"
 15 | 
 16 | namespace {
 17 | 
 18 | void runInteract();
 19 | auto interactCmd  = sargp::Command{"interact", "interact with dynamixel", runInteract};
 20 | auto optTimeout   = interactCmd.Parameter<int>(10000, "timeout", "timeout in us");
 21 | 
 22 | using namespace dynamixel;
 23 | 
 24 | auto checkMotorVersion(dynamixel::MotorID motor, dynamixel::USB2Dynamixel& usb2dyn, std::chrono::microseconds timeout) -> int {
 25 | 	// only read model information, when model is known read full motor
 26 | 	auto [timeoutFlag, motorID, errorCode, layout] = usb2dyn.read<mx_v1::Register::MODEL_NUMBER, 2>(motor, timeout);
 27 | 	if (timeoutFlag or motorID == MotorIDInvalid) {
 28 | 		throw std::runtime_error("failed checking model number");
 29 | 	}
 30 | 
 31 | 	auto modelPtr = meta::getMotorInfo(layout.model_number);
 32 | 	if (modelPtr) {
 33 | 		std::cout << int(motor) << " " <<  modelPtr->shortName << " (" << layout.model_number << ") Layout " << to_string(modelPtr->layout) << "\n";
 34 | 		if (modelPtr->layout == LayoutType::MX_V1) {
 35 | 			return 1;
 36 | 		} else if (modelPtr->layout == LayoutType::MX_V2) {
 37 | 			return 2;
 38 | 		} else if (modelPtr->layout == LayoutType::Pro) {
 39 | 			return 3;
 40 | 		}
 41 | 	}
 42 | 	throw std::runtime_error("failed, unknown model (" + std::to_string(layout.model_number) + ")\n");
 43 | }
 44 | 
 45 | template <LayoutType LT, typename Layout>
 46 | auto readMotorInfos(dynamixel::USB2Dynamixel& usb2dyn, MotorID motor, std::chrono::microseconds timeout) {
 47 | 	auto [timeoutFlag, motorID, errorCode, layout] = usb2dyn.read<Layout::BaseRegister, Layout::Length>(motor, timeout);
 48 | 
 49 | 	if (timeoutFlag or motorID == MotorIDInvalid) {
 50 | 		throw std::runtime_error("trouble reading the motor");
 51 | 	}
 52 | 
 53 | 	return layout;
 54 | }
 55 | 
 56 | void runInteract() {
 57 | 	std::thread waitForInputThread;
 58 | 	std::mutex mInputMutex;
 59 | 	std::atomic_bool detectInput{false};
 60 | 	std::atomic_bool minValue{false};
 61 | 	waitForInputThread = std::thread([&]() {
 62 | 		while(true) {
 63 | 			std::cin.ignore();
 64 | 			auto g = std::lock_guard(mInputMutex);
 65 | 			detectInput = true;
 66 | 			minValue = not minValue;
 67 | 		}
 68 | 	});
 69 | 	try {
 70 | 		auto timeout = std::chrono::microseconds{optTimeout};
 71 | 
 72 | 		auto usb2dyn = dynamixel::USB2Dynamixel(g_baudrate, g_device.get(), dynamixel::Protocol(g_protocolVersion.get()));
 73 | 
 74 | 		int layoutVersion = checkMotorVersion(g_id, usb2dyn, timeout);
 75 | 
 76 | 		while(true) {
 77 | 			auto printVals = [&](int minV, int maxV, int val) {
 78 | 				if (not minValue) std::cout << TERM_GREEN;
 79 | 				std::cout << "min: " << minV << "\n";
 80 | 				if (not minValue) std::cout << TERM_RESET;
 81 | 
 82 | 				if (minValue) std::cout << TERM_GREEN;
 83 | 				std::cout << "max: " << maxV << "\n";
 84 | 				if (minValue) std::cout << TERM_RESET;
 85 | 				std::cout << "cur: " << val << "\n";
 86 | 				std::cout << "-------\n";
 87 | 			};
 88 | 			try {
 89 | 				if (layoutVersion == 1) {
 90 | 					auto layout = readMotorInfos<LayoutType::MX_V1, mx_v1::FullLayout>(usb2dyn, g_id, timeout);
 91 | 					printVals(layout.cw_angle_limit, layout.ccw_angle_limit, layout.present_position);
 92 | 					if (detectInput) {
 93 | 						auto g = std::lock_guard(mInputMutex);
 94 | 						detectInput = false;
 95 | 						if (minValue) {
 96 | 							usb2dyn.write<mx_v1::Register::CW_ANGLE_LIMIT, 2>(g_id, {layout.present_position});
 97 | 						} else {
 98 | 							usb2dyn.write<mx_v1::Register::CCW_ANGLE_LIMIT, 2>(g_id, {layout.present_position});
 99 | 						}
100 | 						std::cout << TERM_GREEN " writing limits " TERM_RESET "\n";
101 | 						usleep(100000);
102 | 					}
103 | 				} else if (layoutVersion == 2) {
104 | 					auto layout = readMotorInfos<LayoutType::MX_V2, mx_v2::FullLayout>(usb2dyn, g_id, timeout);
105 | 					printVals(layout.min_position_limit, layout.max_position_limit, layout.present_position);
106 | 
107 | 					if (detectInput) {
108 | 						auto g = std::lock_guard(mInputMutex);
109 | 						detectInput = false;
110 | 						if (minValue) {
111 | 							usb2dyn.write<mx_v2::Register::MIN_POSITION_LIMIT, 4>(g_id, {layout.present_position});
112 | 						} else {
113 | 							usb2dyn.write<mx_v2::Register::MAX_POSITION_LIMIT, 4>(g_id, {layout.present_position});
114 | 						}
115 | 						std::cout << TERM_GREEN " writing limits " TERM_RESET "\n";
116 | 						usleep(100000);
117 | 					}
118 | 				} else if (layoutVersion == 3) { //layout pro
119 | 					auto layout = readMotorInfos<LayoutType::Pro, pro::FullLayout>(usb2dyn, g_id, timeout);
120 | 					printVals(layout.min_position_limit, layout.max_position_limit, layout.present_position);
121 | 
122 | 					if (detectInput) {
123 | 						auto g = std::lock_guard(mInputMutex);
124 | 						detectInput = false;
125 | 						if (minValue) {
126 | 							usb2dyn.write<pro::Register::MIN_POSITION_LIMIT, 4>(g_id, {layout.present_position});
127 | 						} else {
128 | 							usb2dyn.write<pro::Register::MAX_POSITION_LIMIT, 4>(g_id, {layout.present_position});
129 | 						}
130 | 						std::cout << TERM_GREEN " writing limits " TERM_RESET "\n";
131 | 						usleep(100000);
132 | 					}
133 | 				}
134 | 
135 | 			} catch (std::exception const& e) {
136 | 				std::cout << TERM_RED << "exception: " << e.what() << TERM_RESET << "\n";
137 | 				usleep(1000000);
138 | 			}
139 | 		}
140 | 	} catch (std::exception const& e) {
141 | 		std::cout << TERM_RED << "exception: " << e.what() << "\n";
142 | 	}
143 | }
144 | 
145 | }
146 | 


--------------------------------------------------------------------------------
/src/usb2dynamixel/LayoutMX_V1.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | 
  3 | #include "LayoutPart.h"
  4 | 
  5 | namespace dynamixel::mx_v1 {
  6 | 
  7 | enum class Register : int {
  8 | 	MODEL_NUMBER        = 0x00,
  9 | 	VERSION_FIRMWARE    = 0x02,
 10 | 	ID                  = 0x03,
 11 | 	BAUD_RATE           = 0x04,
 12 | 	RETURN_DELAY_TIME   = 0x05,
 13 | 	CW_ANGLE_LIMIT      = 0x06,
 14 | 	CCW_ANGLE_LIMIT     = 0x08,
 15 | 	DRIVE_MODE          = 0x0a,
 16 | 	TEMPERATURE_LIMIT   = 0x0b,
 17 | 	VOLTAGE_LIMIT_LOW   = 0x0c,
 18 | 	VOLTAGE_LIMIT_HIGH  = 0x0d,
 19 | 	MAX_TORQUE          = 0x0e,
 20 | 	STATUS_RETURN_LEVEL = 0x10,
 21 | 	ALARM_LED           = 0x11,
 22 | 	ALARM_SHUTDOWN      = 0x12,
 23 | 	MULTI_TURN_OFFSET   = 0x14,
 24 | 	RESOLUTION_DIVIDER  = 0x16,
 25 | 	TORQUE_ENABLE       = 0x18,
 26 | 	LED                 = 0x19,
 27 | 	D_GAIN              = 0x1a,
 28 | 	I_GAIN              = 0x1b,
 29 | 	P_GAIN              = 0x1c,
 30 | 	GOAL_POSITION       = 0x1e,
 31 | 	MOVING_SPEED        = 0x20,
 32 | 	TORQUE_LIMIT        = 0x22,
 33 | 	PRESENT_POSITION    = 0x24,
 34 | 	PRESENT_SPEED       = 0x26,
 35 | 	PRESENT_LOAD        = 0x28,
 36 | 	PRESENT_VOLTAGE     = 0x2a,
 37 | 	PRESENT_TEMPERATURE = 0x2b,
 38 | 	REGISTERED          = 0x2c,
 39 | 	MOVING              = 0x2e,
 40 | 	LOCK                = 0x2f,
 41 | 	PUNCH               = 0x30,
 42 | 	REALTIME_TICK       = 0x32,
 43 | 	CURRENT             = 0x44,
 44 | 	TORQUE_CONTROL_MODE = 0x46,
 45 | 	GOAL_TORQUE         = 0x47,
 46 | 	GOAL_ACCELERATION   = 0x49,
 47 | };
 48 | constexpr Register operator+(Register t1, size_t t2) {
 49 | 	return Register(size_t(t1) + t2);
 50 | }
 51 | 
 52 | struct MotorLayoutInfo {
 53 | 	static constexpr LayoutType Type{LayoutType::MX_V1};
 54 | 	using FullLayout = Layout<Register::MODEL_NUMBER, 74>;
 55 | 
 56 | 	static auto getInfos()    -> meta::Layout<Register> const&;
 57 | 	static auto getDefaults() -> std::map<uint32_t, meta::Info<Register>> const&;
 58 | };
 59 | 
 60 | using FullLayout = MotorLayoutInfo::FullLayout;
 61 | 
 62 | }
 63 | namespace dynamixel {
 64 | 
 65 | template <> struct meta::MotorLayoutInfo<LayoutType::MX_V1> : mx_v1::MotorLayoutInfo {};
 66 | 
 67 | 
 68 | #pragma pack(push, 1)
 69 | DynamixelLayoutPart(mx_v1::Register::MODEL_NUMBER        , uint16_t, model_number       );
 70 | DynamixelLayoutPart(mx_v1::Register::VERSION_FIRMWARE    ,  uint8_t, version_firmware   );
 71 | DynamixelLayoutPart(mx_v1::Register::ID                  ,  uint8_t, id                 );
 72 | DynamixelLayoutPart(mx_v1::Register::BAUD_RATE           ,  uint8_t, baud_rate          );
 73 | DynamixelLayoutPart(mx_v1::Register::RETURN_DELAY_TIME   ,  uint8_t, return_delay_time  );
 74 | DynamixelLayoutPart(mx_v1::Register::CW_ANGLE_LIMIT      ,  int16_t, cw_angle_limit     );
 75 | DynamixelLayoutPart(mx_v1::Register::CCW_ANGLE_LIMIT     ,  int16_t, ccw_angle_limit    );
 76 | DynamixelLayoutPart(mx_v1::Register::DRIVE_MODE          ,  uint8_t, drive_mode         );
 77 | DynamixelLayoutPart(mx_v1::Register::TEMPERATURE_LIMIT   ,  uint8_t, temperature_limit  );
 78 | DynamixelLayoutPart(mx_v1::Register::VOLTAGE_LIMIT_LOW   ,  uint8_t, voltage_limit_low  );
 79 | DynamixelLayoutPart(mx_v1::Register::VOLTAGE_LIMIT_HIGH  ,  uint8_t, voltage_limit_high );
 80 | DynamixelLayoutPart(mx_v1::Register::MAX_TORQUE          , uint16_t, max_torque         );
 81 | DynamixelLayoutPart(mx_v1::Register::STATUS_RETURN_LEVEL ,  uint8_t, status_return_level);
 82 | DynamixelLayoutPart(mx_v1::Register::ALARM_LED           ,  uint8_t, alarm_led          );
 83 | DynamixelLayoutPart(mx_v1::Register::ALARM_SHUTDOWN      , uint16_t, alarm_shutdown     );
 84 | DynamixelLayoutPart(mx_v1::Register::MULTI_TURN_OFFSET   ,  int16_t, multi_turn_offset  );
 85 | DynamixelLayoutPart(mx_v1::Register::RESOLUTION_DIVIDER  , uint16_t, resolution_divider );
 86 | DynamixelLayoutPart(mx_v1::Register::TORQUE_ENABLE       ,     bool, torque_enable      );
 87 | DynamixelLayoutPart(mx_v1::Register::LED                 ,     bool, led                );
 88 | DynamixelLayoutPart(mx_v1::Register::D_GAIN              ,  uint8_t, d_gain             );
 89 | DynamixelLayoutPart(mx_v1::Register::I_GAIN              ,  uint8_t, i_gain             );
 90 | DynamixelLayoutPart(mx_v1::Register::P_GAIN              ,  uint8_t, p_gain             );
 91 | DynamixelLayoutPart(mx_v1::Register::GOAL_POSITION       ,  int16_t, goal_position      );
 92 | DynamixelLayoutPart(mx_v1::Register::MOVING_SPEED        ,  int16_t, moving_speed       );
 93 | DynamixelLayoutPart(mx_v1::Register::TORQUE_LIMIT        , uint16_t, torque_limit       );
 94 | DynamixelLayoutPart(mx_v1::Register::PRESENT_POSITION    ,  int16_t, present_position   );
 95 | DynamixelLayoutPart(mx_v1::Register::PRESENT_SPEED       ,  int16_t, present_speed      );
 96 | DynamixelLayoutPart(mx_v1::Register::PRESENT_LOAD        ,  int16_t, present_load       );
 97 | DynamixelLayoutPart(mx_v1::Register::PRESENT_VOLTAGE     ,  uint8_t, present_voltage    );
 98 | DynamixelLayoutPart(mx_v1::Register::PRESENT_TEMPERATURE ,  uint8_t, present_temperature);
 99 | DynamixelLayoutPart(mx_v1::Register::REGISTERED          , uint16_t, registered         );
100 | DynamixelLayoutPart(mx_v1::Register::MOVING              ,     bool, moving             );
101 | DynamixelLayoutPart(mx_v1::Register::LOCK                ,     bool, lock               );
102 | DynamixelLayoutPart(mx_v1::Register::PUNCH               , uint16_t, punch              );
103 | DynamixelLayoutPart(mx_v1::Register::REALTIME_TICK       , uint16_t, realtime_tick      );
104 | DynamixelLayoutPart(mx_v1::Register::CURRENT             ,  int16_t, current            );
105 | DynamixelLayoutPart(mx_v1::Register::TORQUE_CONTROL_MODE ,  uint8_t, torque_control_mode);
106 | DynamixelLayoutPart(mx_v1::Register::GOAL_TORQUE         , uint16_t, goal_torque        );
107 | DynamixelLayoutPart(mx_v1::Register::GOAL_ACCELERATION   ,  uint8_t, goal_acceleration  );
108 | 
109 | #pragma pack(pop)
110 | }
111 | 


--------------------------------------------------------------------------------
/src/usb2dynamixel/LayoutXL320.cpp:
--------------------------------------------------------------------------------
  1 | #include "LayoutXL320.h"
  2 | 
  3 | namespace dynamixel::xl320 {
  4 | 
  5 | auto MotorLayoutInfo::getInfos() -> meta::Layout<Register> const& {
  6 | 	using A = meta::LayoutField::Access;
  7 | 	static auto data = meta::Layout<Register> {
  8 | 		{Register::MODEL_NUMBER         , {2,  true, A:: R, "Model Number", "model number"}},
  9 | 		{Register::FIRMWARE_VERSION     , {1,  true, A:: R, "Version of Firmware", "Information on the version of firmware"}},
 10 | 		{Register::ID                   , {1,  true, A::RW, "ID", "ID of Dynamixel"}},
 11 | 		{Register::BAUD_RATE            , {1,  true, A::RW, "Baud Rate", "Baud Rate of Dynamixel"}},
 12 | 		{Register::RETURN_DELAY_TIME    , {1,  true, A::RW, "Return Delay Time", "Return Delay Time"}},
 13 | 		{Register::CW_ANGLE_LIMIT       , {2,  true, A::RW, "CW Angle Limit", "clockwise Angle Limit"}},
 14 | 		{Register::CCW_ANGLE_LIMIT      , {2,  true, A::RW, "CCW Angle Limit", "counterclockwise Angle Limit"}},
 15 | 		{Register::CONTROL_MODE         , {1,  true, A::RW, "Control Mode", "Dual Mode Setting"}},
 16 | 		{Register::TEMPERATURE_LIMIT    , {1,  true, A::RW, "Highest Limit Temperature", "Internal Limit Temperature"}},
 17 | 		{Register::MIN_VOLTAGE_LIMIT    , {1,  true, A::RW, "Min Limit Voltage", "Min Limit Voltage"}},
 18 | 		{Register::MAX_VOLTAGE_LIMIT    , {1,  true, A::RW, "Max Limit Voltage", "Max Limit Voltage"}},
 19 | 		{Register::MAX_TORQUE           , {2,  true, A::RW, "Max Torque", "Max. Torque"}},
 20 | 		{Register::STATUS_RETURN_LEVEL  , {1,  true, A::RW, "Status Return Level", "Status Return Level"}},
 21 | 		{Register::SHUTDOWN             , {1,  true, A::RW, "Shutdown", "Shutdown for Alarm"}},
 22 | 		{Register::TORQUE_ENABLE        , {1, false, A::RW, "Torque Enable", "Torque On/Off"}},
 23 | 		{Register::LED                  , {1, false, A::RW, "LED", "LED On/Off"}},
 24 | 		{Register::D_GAIN               , {1, false, A::RW, "D Gain", "Derivative Gain"}},
 25 | 		{Register::I_GAIN               , {1, false, A::RW, "I Gain", "Integral Gain"}},
 26 | 		{Register::P_GAIN               , {1, false, A::RW, "P Gain", "Proportional Gain"}},
 27 | 		{Register::GOAL_POSITION        , {2, false, A::RW, "Goal Position", "Goal Position"}},
 28 | 		{Register::MOVING_SPEED         , {2, false, A::RW, "Moving Speed", "Moving Speed (Moving Velocity)"}},
 29 | 		{Register::TORQUE_LIMIT         , {2, false, A::RW, "Torque Limit", "Torque Limit (Goal Torque)"}},
 30 | 		{Register::PRESENT_POSITION     , {2, false, A:: R, "Present Position", "Current Position (Present Velocity)"}},
 31 | 		{Register::PRESENT_SPEED        , {2, false, A:: R, "Present Speed", "Current Speed"}},
 32 | 		{Register::PRESENT_LOAD         , {2, false, A:: R, "Present Load", "Current Load"}},
 33 | 		{Register::PRESENT_VOLTAGE      , {1, false, A:: R, "Present Voltage", "Current Voltage"}},
 34 | 		{Register::PRESENT_TEMPERATURE  , {1, false, A:: R, "Present Temperature", "Current Temperature"}},
 35 | 		{Register::REGISTERED           , {1, false, A:: R, "Registered", "Means if Instruction is registered"}},
 36 | 		{Register::MOVING               , {1, false, A:: R, "Moving", "Means if there is any movement"}},
 37 | 		{Register::HARDWARE_ERROR_STATUS, {1, false, A:: R, "Hardware Error Status", "Hardware Error Status"}},
 38 | 		{Register::PUNCH                , {2, false, A::RW, "Punch", "Punch"}},
 39 | 	};
 40 | 	return data;
 41 | }
 42 | 
 43 | auto MotorLayoutInfo::getDefaults() -> std::map<uint32_t, meta::Info<Register>> const& {
 44 | 	static auto data = []() {
 45 | 		auto convertPosition    = meta::buildConverter("r", (2.*M_PI)/1023.*300./360., 512);
 46 | 		auto convertSpeed       = meta::buildConverter("r/s", 0.111/60*2.*M_PI);
 47 | 		auto convertTemperature = meta::buildConverter("C", 1.);
 48 | 		auto convertVoltage     = meta::buildConverter("V", 16./160);
 49 | 		auto convertPID_P       = meta::buildConverter("", 1./8., 0, 0, 254);
 50 | 		auto convertPID_I       = meta::buildConverter("", 1000./2048., 0, 0, 254);
 51 | 		auto convertPID_D       = meta::buildConverter("", 4/1000., 0, 0, 254);
 52 | 		auto convertTorque      = meta::buildConverter("%", 100./1023., 0);
 53 | 
 54 | 		auto data = std::map<uint32_t, meta::Info<Register>> {
 55 | 			{350, {
 56 | 				350,
 57 | 				LayoutType::XL320,
 58 | 				"XL-320",
 59 | 				{"XL-320"}, {
 60 | 					{Register::MODEL_NUMBER         , {          350, {}}},
 61 | 					{Register::FIRMWARE_VERSION     , { std::nullopt, {}}},
 62 | 					{Register::ID                   , {            1, {}}},
 63 | 					{Register::BAUD_RATE            , {            3, {}}},
 64 | 					{Register::RETURN_DELAY_TIME    , {          250, {}}},
 65 | 					{Register::CW_ANGLE_LIMIT       , {            0, convertPosition}},
 66 | 					{Register::CCW_ANGLE_LIMIT      , {       0x03ff, convertPosition}},
 67 | 					{Register::CONTROL_MODE         , {            2, {}}},
 68 | 					{Register::TEMPERATURE_LIMIT    , {           65, convertTemperature}},
 69 | 					{Register::MIN_VOLTAGE_LIMIT    , {           60, convertVoltage}},
 70 | 					{Register::MAX_VOLTAGE_LIMIT    , {           90, convertVoltage}},
 71 | 					{Register::MAX_TORQUE           , {       0x03ff, convertTorque}},
 72 | 					{Register::STATUS_RETURN_LEVEL  , {            2, {}}},
 73 | 					{Register::SHUTDOWN             , {            3, {}}},
 74 | 					{Register::TORQUE_ENABLE        , {            0, {}}},
 75 | 					{Register::LED                  , {            0, {}}},
 76 | 					{Register::D_GAIN               , {            0, convertPID_D}},
 77 | 					{Register::I_GAIN               , {            0, convertPID_I}},
 78 | 					{Register::P_GAIN               , {           32, convertPID_P}},
 79 | 					{Register::GOAL_POSITION        , { std::nullopt, convertPosition}},
 80 | 					{Register::MOVING_SPEED         , { std::nullopt, convertSpeed}},
 81 | 					{Register::TORQUE_LIMIT         , { std::nullopt, convertTorque}},
 82 | 					{Register::PRESENT_POSITION     , { std::nullopt, convertPosition}},
 83 | 					{Register::PRESENT_SPEED        , { std::nullopt, convertSpeed}},
 84 | 					{Register::PRESENT_LOAD         , { std::nullopt, {}}},
 85 | 					{Register::PRESENT_VOLTAGE      , { std::nullopt, convertVoltage}},
 86 | 					{Register::PRESENT_TEMPERATURE  , { std::nullopt, convertTemperature}},
 87 | 					{Register::REGISTERED           , {            0, {}}},
 88 | 					{Register::MOVING               , {            0, {}}},
 89 | 					{Register::HARDWARE_ERROR_STATUS, {            0, {}}},
 90 | 					{Register::PUNCH                , {           32, {}}},
 91 | 				}
 92 | 			}}
 93 | 		};
 94 | 		return data;
 95 | 	}();
 96 | 	return data;
 97 | };
 98 | 
 99 | 
100 | 
101 | }
102 | 


--------------------------------------------------------------------------------
/src/usb2dynamixel/ProtocolV1.cpp:
--------------------------------------------------------------------------------
  1 | #include "ProtocolV1.h"
  2 | #include "file_io.h"
  3 | 
  4 | #include <cstring>
  5 | #include <stdexcept>
  6 | 
  7 | 
  8 | namespace dynamixel {
  9 | namespace {
 10 | 
 11 | [[nodiscard]]
 12 | std::byte calculateChecksum(Parameter const& packet) {
 13 | 	uint32_t checkSum = 0;
 14 | 	for (size_t i(2); i < packet.size(); ++i) {
 15 | 		checkSum += uint8_t(packet[i]);
 16 | 	}
 17 | 	return std::byte(~checkSum);
 18 | }
 19 | 
 20 | [[nodiscard]]
 21 | bool validatePacket(Parameter const& rxBuf) {
 22 | 	if (rxBuf.size() > 255) {
 23 | 		return false;
 24 | 	}
 25 | 	if (rxBuf.size() < 4) {
 26 | 		return false;
 27 | 	}
 28 | 
 29 | 	bool success = 0xff == uint8_t(rxBuf[0]);
 30 | 	success &= 0xff == uint8_t(rxBuf[1]);
 31 | 	success &= 0xff != uint8_t(rxBuf[2]);
 32 | 	success &= (rxBuf.size() - 4) == uint8_t(rxBuf[3]);
 33 | 	uint8_t checkSum = 0;
 34 | 	for (size_t i(2); i < rxBuf.size(); ++i) {
 35 | 		checkSum += uint8_t(rxBuf[i]);
 36 | 	}
 37 | 	success &= 0xff == uint8_t(checkSum);
 38 | 	return success;
 39 | }
 40 | 
 41 | }
 42 | 
 43 | auto ProtocolV1::createPacket(MotorID motorID, Instruction instr, Parameter data) const -> Parameter {
 44 | 	if (data.size() > 253) {
 45 | 		throw std::runtime_error("packet is longer than 255 bytes, not supported in protocol v1");
 46 | 	}
 47 | 	uint8_t length = 2 + data.size();
 48 | 
 49 | 	Parameter txBuf {
 50 | 		std::byte{0xff}, std::byte{0xff}, std::byte{motorID}, std::byte{length}, std::byte(instr)
 51 | 	};
 52 | 	txBuf.insert(txBuf.end(), data.begin(), data.end());
 53 | 
 54 | 	txBuf.push_back(calculateChecksum(txBuf));
 55 | 
 56 | 	return txBuf;
 57 | }
 58 | 
 59 | 
 60 | Parameter ProtocolV1::synchronizeOnHeader(Timeout timeout, MotorID expectedMotorID, std::size_t numParameters, simplyfile::SerialPort const& port) const {
 61 | 	Parameter preambleBuffer;
 62 | 	struct __attribute__((packed)) Header {
 63 | 		std::array<std::byte, 2> syncMarker;
 64 | 		uint8_t id;
 65 | 		uint8_t length;
 66 | 		uint8_t error;
 67 | 	};
 68 | 	std::array<std::byte, 2> syncMarker = {std::byte{0xff}, std::byte{0xff}};
 69 | 	auto startTime = std::chrono::high_resolution_clock::now();
 70 | 	while (not ((timeout.count() != 0) and (std::chrono::high_resolution_clock::now() - startTime >= timeout))) {
 71 | 		// figure out how many bytes have to be read
 72 | 		int indexOfSyncMarker = 0;
 73 | 		for (;indexOfSyncMarker <= static_cast<int>(preambleBuffer.size())-static_cast<int>(sizeof(syncMarker)); ++indexOfSyncMarker) {
 74 | 			if (0 == std::memcmp(&preambleBuffer[indexOfSyncMarker], syncMarker.data(), sizeof(syncMarker))) {
 75 | 				break;
 76 | 			}
 77 | 		}
 78 | 		preambleBuffer.erase(preambleBuffer.begin(), preambleBuffer.begin()+indexOfSyncMarker);
 79 | 		int bytesToRead = std::max(1, static_cast<int>(sizeof(Header)+1) - static_cast<int>(preambleBuffer.size()));
 80 | 		auto buffer = file_io::read(port, bytesToRead);
 81 | 		preambleBuffer.insert(preambleBuffer.end(), buffer.begin(), buffer.end());
 82 | 		if (preambleBuffer.size() >= sizeof(Header)) {
 83 | 			// test if this preamble contains the header of the packet we were looking for
 84 | 			Header header;
 85 | 			std::memcpy(&header, preambleBuffer.data(), sizeof(Header));
 86 | 			if (header.syncMarker == syncMarker and header.length+2 >= static_cast<int>(numParameters)) {
 87 | 				// found a synchronization token and a "matching" packet
 88 | 				if (expectedMotorID != 0xfe) {
 89 | 					if (expectedMotorID == header.id) {
 90 | 						return preambleBuffer;
 91 | 					}
 92 | 					// received an unexpected header -> flush this header and continue reading
 93 | 					preambleBuffer.clear();
 94 | 				} else {
 95 | 					return preambleBuffer;
 96 | 				}
 97 | 			}
 98 | 		}
 99 | 	}
100 | 	return {};
101 | }
102 | 
103 | auto ProtocolV1::readPacket(Timeout timeout, MotorID expectedMotorID, std::size_t numParameters, simplyfile::SerialPort const& port) const -> std::tuple<bool, MotorID, ErrorCode, Parameter> {
104 | 	bool timeoutFlag = false;
105 | 	auto startTime = std::chrono::high_resolution_clock::now();
106 | 
107 | 	while (not timeoutFlag) {
108 | 		// read a header (headers consist of 5 bytes)
109 | 		Parameter preambleBuffer;
110 | 		auto testTimeout = [&]{ return (timeout.count() != 0) and (std::chrono::high_resolution_clock::now() - startTime >= timeout);};
111 | 		Parameter rxBuf = synchronizeOnHeader(timeout, expectedMotorID, numParameters, port);
112 | 		if (rxBuf.size() < 5) { // if we could not synchronize on a header bail out
113 | 			break;
114 | 		}
115 | 		std::size_t incomingLength = numParameters + 6;
116 | 		while (rxBuf.size() < incomingLength and not timeoutFlag) {
117 | 			auto buffer = file_io::read(port, incomingLength - rxBuf.size());
118 | 			rxBuf.insert(rxBuf.end(), buffer.begin(), buffer.end());
119 | 			timeoutFlag = testTimeout();
120 | 		};
121 | 		if (timeoutFlag) {
122 | 			break;
123 | 		}
124 | 		auto  [motorID, errorCode, payload] = extractPayload(rxBuf);
125 | 		if (payload.size() != numParameters or motorID != expectedMotorID) {
126 | 			continue;
127 | 		}
128 | 		return std::make_tuple(false, motorID, errorCode, payload);
129 | 	}
130 | 	file_io::flushRead(port);
131 | 	return std::make_tuple(true, MotorIDInvalid, ErrorCode{}, Parameter{});
132 | }
133 | 
134 | auto ProtocolV1::extractPayload(Parameter const& raw_packet) const -> std::tuple<MotorID, ErrorCode, Parameter> {
135 | 	if (not validatePacket(raw_packet)) {
136 | 		return std::make_tuple(MotorIDInvalid, ErrorCode{}, Parameter{});
137 | 	}
138 | 
139 | 	auto motorID = MotorID(raw_packet[2]);
140 | 	auto errorCode = ErrorCode(raw_packet[4]);
141 | 	int len = static_cast<int>(raw_packet[3]);
142 | 	Parameter payload;
143 | 	payload.insert(payload.end(), std::next(raw_packet.begin(), 5), std::next(raw_packet.begin(), 5+len-2));
144 | 
145 | 	return std::make_tuple(motorID, errorCode, std::move(payload));
146 | }
147 | 
148 | auto ProtocolV1::convertLength(size_t len) const -> Parameter {
149 | 	if (len > 255) {
150 | 		throw std::runtime_error("packet is longer than 255 bytes, not supported in protocol v1");
151 | 	}
152 | 
153 | 	return {std::byte(len)};
154 | }
155 | 
156 | auto ProtocolV1::convertAddress(int addr) const -> Parameter {
157 | 	if (addr > 255) {
158 | 		throw std::runtime_error("baseRegister above 255 are not supported in protocol v1");
159 | 	}
160 | 
161 | 	return {std::byte(addr)};
162 | }
163 | 
164 | auto ProtocolV1::buildBulkReadPackage(std::vector<std::tuple<MotorID, int, size_t>> const& motors) const -> std::vector<std::byte> {
165 | 	std::vector<std::byte> txBuf;
166 | 
167 | 	txBuf.reserve(motors.size()*3+1);
168 | 	txBuf.push_back(std::byte{0x00});
169 | 	for (auto const& [id, baseRegister, length] : motors) {
170 | 		for (auto b : convertLength(length)) {
171 | 			txBuf.push_back(b);
172 | 		}
173 | 		txBuf.push_back(std::byte{id});
174 | 		for (auto b : convertAddress(baseRegister)) {
175 | 			txBuf.push_back(b);
176 | 		}
177 | 	}
178 | 
179 | 	return txBuf;
180 | }
181 | 
182 | 
183 | }
184 | 


--------------------------------------------------------------------------------
/src/sargparse/ParameterParsing.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | 
  3 | #include <cstdint>
  4 | #include <cmath>
  5 | #include <sstream>
  6 | #include <iomanip>
  7 | #include <type_traits>
  8 | #include <vector>
  9 | #include <set>
 10 | #include <algorithm>
 11 | #include <stdexcept>
 12 | #include <numeric>
 13 | #include <optional>
 14 | 
 15 | namespace sargp
 16 | {
 17 | namespace parsing
 18 | {
 19 | namespace detail
 20 | {
 21 | 
 22 | struct ParseError : std::invalid_argument {
 23 | 	ParseError(std::string const& msg="")
 24 | 	: std::invalid_argument(msg)
 25 | 	{}
 26 | };
 27 | 
 28 | template <uint64_t maxValue, typename T>
 29 | auto parseSuffixHelper(std::string str, std::string_view suffix) -> T {
 30 | 	if (str != suffix) {
 31 | 		return 1;
 32 | 	}
 33 | 	if constexpr (maxValue >= uint64_t(std::numeric_limits<T>::max())) {
 34 | 		throw ParseError{"out of range"};
 35 | 	} else {
 36 | 		return maxValue;
 37 | 	}
 38 | }
 39 | 
 40 | template<typename T>
 41 | auto parseSuffix(std::string_view suffix) -> std::optional<T> {
 42 | 	auto ret = 1
 43 | 		* parseSuffixHelper<1000, T>("k", suffix)
 44 | 		* parseSuffixHelper<1024, T>("ki", suffix)
 45 | 		* parseSuffixHelper<1000*1000, T>("m", suffix)
 46 | 		* parseSuffixHelper<1024*1024, T>("mi", suffix)
 47 | 		* parseSuffixHelper<1000*1000*1000, T>("g", suffix)
 48 | 		* parseSuffixHelper<1024*1024*1024, T>("gi", suffix)
 49 | 		* parseSuffixHelper<1000ull*1000*1000*1000, T>("t", suffix)
 50 | 		* parseSuffixHelper<1024ull*1024*1024*1024, T>("ti", suffix)
 51 | 		* parseSuffixHelper<1000ull*1000*1000*1000*1000, T>("p", suffix)
 52 | 		* parseSuffixHelper<1024ull*1024*1024*1024*1024, T>("pi", suffix)
 53 | 		* parseSuffixHelper<1000ull*1000*1000*1000*1000*1000, T>("e", suffix)
 54 | 		* parseSuffixHelper<1024ull*1024*1024*1024*1024*1024, T>("ei", suffix)
 55 | 	;
 56 | 	if (ret == 1) {
 57 | 		return std::nullopt;
 58 | 	}
 59 | 	return {ret};
 60 | }
 61 | 
 62 | template<typename T>
 63 | T parseFromString(std::string str) {
 64 | 	if constexpr (std::is_same_v<T, bool>) {
 65 | 		std::transform(str.begin(), str.end(), str.begin(), ::tolower);
 66 | 		if (str == "true" or str == "1") {
 67 | 			return true;
 68 | 		}
 69 | 		if (str == "false" or str == "0") {
 70 | 			return false;
 71 | 		}
 72 | 		throw ParseError{"invalid boolean specifier"};
 73 | 	}
 74 | 
 75 | 	if constexpr (std::is_same_v<T, std::string>) {
 76 | 		return str;
 77 | 	}
 78 | 
 79 | 	T ret;
 80 | 	// parse all integer-like types
 81 | 	if constexpr (std::numeric_limits<T>::is_exact) {
 82 | 		std::transform(str.begin(), str.end(), str.begin(), ::tolower);
 83 | 		int base=0;
 84 | 		char const* strBegin = str.data();
 85 | 		char const* strEnd   = str.data() + str.size();
 86 | 		std::size_t nextIdx=0;
 87 | 		if (str.find("0b") == 0) {
 88 | 			base=2;
 89 | 			strBegin += 2;
 90 | 		}
 91 | 		try {
 92 | 			if constexpr (std::is_unsigned_v<T>) {
 93 | 				ret = std::stoull(strBegin, &nextIdx, base);
 94 | 			} else {
 95 | 				ret = std::stoll(strBegin, &nextIdx, base);
 96 | 			}
 97 | 		} catch (std::invalid_argument const&) {
 98 | 			throw ParseError{"not an integer"};
 99 | 		}
100 | 		// if we didnt parse everything check if it has some known suffix
101 | 		if (static_cast<int>(nextIdx) != strEnd - strBegin) {
102 | 			if constexpr (not std::is_same_v<bool, T>) {
103 | 				auto suffix = std::string_view{strBegin + nextIdx};
104 | 				auto value = parseSuffix<T>(suffix);
105 | 				if (not value) {
106 | 					throw ParseError{"unknown suffix"};
107 | 				}
108 | 				if (__int128_t(ret) * value.value() > std::numeric_limits<T>::max()
109 | 					or __int128_t(ret) * value.value() < std::numeric_limits<T>::min()) {
110 | 						throw ParseError{"out of range"};
111 | 				}
112 | 				ret *= value.value();
113 | 			}
114 | 		}
115 | 		return ret;
116 | 	}
117 | 
118 | 	// parse everything else
119 | 	std::stringstream ss{str};
120 | 	if (not (ss >> ret)) {
121 | 		throw ParseError{};
122 | 	}
123 | 
124 | 	// parse floats/doubles and convert if they are angles
125 | 	if constexpr (std::is_floating_point_v<T>) {
126 | 		if (not ss.eof()) {
127 | 			std::string ending;
128 | 			if (not (ss >> ending)) {
129 | 				throw ParseError{};
130 | 			}
131 | 			if (ending == "rad") {
132 | 			} else if (ending == "deg") {
133 | 				ret = ret / 180. * M_PI;
134 | 			} else if (ending == "tau") {
135 | 				ret = ret * 2. * M_PI;
136 | 			} else {
137 | 				throw ParseError{"unknown suffix"};
138 | 			}
139 | 		}
140 | 	}
141 | 
142 | 	if (not ss.eof()) {
143 | 		throw ParseError{};
144 | 	}
145 | 
146 | 	return ret;
147 | }
148 | 
149 | template<typename T>
150 | struct is_collection : std::false_type {};
151 | template<typename T>
152 | struct is_collection<std::vector<T>> : std::true_type {};
153 | template<typename T>
154 | struct is_collection<std::set<T>> : std::true_type {};
155 | template<typename T>
156 | inline constexpr bool is_collection_v = is_collection<T>::value;
157 | 
158 | template<typename T>
159 | struct is_optional : std::false_type {};
160 | template<typename T>
161 | struct is_optional<std::optional<T>> : std::true_type {};
162 | template<typename T>
163 | inline constexpr bool is_optional_v = is_optional<T>::value;
164 | 
165 | }
166 | 
167 | 
168 | template<typename T>
169 | T parse(std::vector<std::string> const& args) {
170 | 	if constexpr (detail::is_collection_v<T>) {
171 | 			using value_type = typename T::value_type;
172 | 			T collection;
173 | 			for (auto const& arg : args) {
174 | 				if constexpr (std::is_same_v<T, std::vector<value_type>>) {
175 | 					collection.emplace_back(detail::parseFromString<value_type>(arg));
176 | 				} else {
177 | 					collection.emplace(detail::parseFromString<value_type>(arg));
178 | 				}
179 | 			}
180 | 			return collection;
181 | 	} else if constexpr (detail::is_optional_v<T>) {
182 | 		using value_type = typename T::value_type;
183 | 		if (args.empty()) {
184 | 			return T{};
185 | 		}
186 | 		return parse<value_type>(args);
187 | 	} else {
188 | 		if (args.empty()) {
189 | 			if constexpr (std::is_same_v<bool, T>) {
190 | 				return true;
191 | 			}
192 | 		}
193 | 		if (args.size() != 1) {
194 | 			throw std::invalid_argument("wrong number of arguments given (expect 1, got " + std::to_string(args.size()) + ")");
195 | 		}
196 | 		return detail::parseFromString<T>(args[0]);
197 | 	}
198 | }
199 | 
200 | template<typename T>
201 | std::string stringify(T const& t) {
202 | 	if constexpr (detail::is_collection_v<T>) {
203 | 		if (not t.empty()) {
204 | 			return "{" + std::accumulate(std::next(t.begin()), t.end(), stringify(*t.begin()), [](std::string const& left, auto const& p) {
205 | 				return left + ", " + stringify(p);
206 | 			}) + "}";
207 | 		}
208 | 		return "{}";
209 | 	} else if constexpr (detail::is_optional_v<T>) {
210 | 		if (t) {
211 | 			return stringify(*t);
212 | 		}
213 | 		return "[]";
214 | 	} else if constexpr (std::is_same_v<std::string, T>) {
215 | 		return t;
216 | 	} else if constexpr (std::is_same_v<bool, T>) {
217 | 		return t?"true":"false";
218 | 	} else {
219 | 		return std::to_string(t);
220 | 	}
221 | }
222 | 
223 | }
224 | }
225 | 
226 | 
227 | 


--------------------------------------------------------------------------------
/src/fsCommand.cpp:
--------------------------------------------------------------------------------
  1 | #include "usb2dynamixel/USB2Dynamixel.h"
  2 | #include "usb2dynamixel/MotorMetaInfo.h"
  3 | #include "globalOptions.h"
  4 | #include "commonTasks.h"
  5 | 
  6 | #include "simplyfuse/FuseFS.h"
  7 | #include "simplyfile/Epoll.h"
  8 | 
  9 | #include <algorithm>
 10 | #include <numeric>
 11 | #include <atomic>
 12 | #include <future>
 13 | 
 14 | #include <unistd.h>
 15 | #include <functional>
 16 | #include <csignal>
 17 | 
 18 | namespace {
 19 | 
 20 | void runFuse();
 21 | auto interactCmd  = sargp::Command{"fuse", "create a fuse fs representing the connected motors", runFuse};
 22 | auto optTimeout   = interactCmd.Parameter<int>(10000, "timeout", "timeout in us");
 23 | auto ids          = interactCmd.Parameter<std::set<int>>({}, "ids", "the target Id");
 24 | auto mountPoint   = interactCmd.Parameter<std::string>("dynamixelFS", "mountpoint", "where to mount the fuse filesystem representing the motors");
 25 | using namespace dynamixel;
 26 | 
 27 | struct RegisterFile : simplyfuse::FuseFile {
 28 | 	RegisterFile(MotorID _motorID, int _registerID, meta::LayoutField const& _layoutField, USB2Dynamixel &_usb2dyn)
 29 | 		: motorID(_motorID)
 30 | 		, registerID(_registerID)
 31 | 		, layoutField(_layoutField)
 32 | 		, usb2dyn(_usb2dyn)
 33 | 	{}
 34 | 
 35 | 	virtual ~RegisterFile() = default;
 36 | 
 37 | 	int onRead(char* buf, std::size_t size, off_t)  override {
 38 | 		if (not (int(layoutField.access) & int(meta::LayoutField::Access::R))) {
 39 | 			return -EINVAL;
 40 | 		}
 41 | 		auto [timeout, motor, error, parameters] = usb2dyn.read(motorID, registerID, layoutField.length, std::chrono::microseconds{g_timeout});
 42 | 		if (timeout or motor != motorID or parameters.size() != layoutField.length) {
 43 | 			return -EINVAL;
 44 | 		}
 45 | 		int content {0};
 46 | 		memcpy(&content, parameters.data(), std::min(sizeof(content), std::size_t(layoutField.length)));
 47 | 
 48 | 		std::string renderedContent = std::to_string(content) + "\n";
 49 | 		size = std::min(size, renderedContent.size()+1);
 50 | 		std::memcpy(buf, renderedContent.data(), size);
 51 | 		return size;
 52 | 	}
 53 | 
 54 | 	int onWrite(const char* buf, std::size_t size, off_t) override {
 55 | 		if (not (int(layoutField.access) & int(meta::LayoutField::Access::W))) {
 56 | 			return -ENOENT;
 57 | 		}
 58 | 		try {
 59 | 			std::string stripped;
 60 | 			std::stringstream{std::string{buf, size}} >> stripped;
 61 | 			int toSet = sargp::parsing::detail::parseFromString<int>(stripped);
 62 | 			Parameter param;
 63 | 			for (std::size_t i{0}; i < layoutField.length; ++i) {
 64 | 				param.emplace_back(std::byte{reinterpret_cast<uint8_t const*>(&toSet)[i]});
 65 | 			}
 66 | 			usb2dyn.write(motorID, registerID, param);
 67 | 			return size;
 68 | 		} catch (std::exception const&) {}
 69 | 		return -ENOENT;
 70 | 	}
 71 | 
 72 | 	int onTruncate(off_t) override {
 73 | 		return 0;
 74 | 	}
 75 | 
 76 | 	std::size_t getSize() override {
 77 | 		return 4096;
 78 | 	}
 79 | 
 80 | 	int getFilePermissions() override {
 81 | 		int permissions {0};
 82 | 		if (int(layoutField.access) & int(meta::LayoutField::Access::R)) {
 83 | 			permissions |= 0444;
 84 | 		}
 85 | 		if (int(layoutField.access) & int(meta::LayoutField::Access::W)) {
 86 | 			permissions |= 0222;
 87 | 		}
 88 | 		return permissions;
 89 | 	}
 90 | 
 91 | 	MotorID motorID;
 92 | 	int registerID;
 93 | 	meta::LayoutField layoutField;
 94 | 	USB2Dynamixel &usb2dyn;
 95 | };
 96 | 
 97 | struct PingFile : simplyfuse::SimpleWOFile {
 98 | 	std::function<bool(MotorID)> callback;
 99 | 	PingFile(std::function<bool(MotorID)> cb) : callback{cb} {}
100 | 
101 | 	int onWrite(const char* buf, std::size_t size, off_t) override {
102 | 		int motorID;
103 | 		if (std::stringstream{std::string{buf, size}} >> motorID) {
104 | 			if (motorID >= 0xff) {
105 | 				return -EINVAL;
106 | 			}
107 | 			if (callback(motorID)) {
108 | 				return size;
109 | 			}
110 | 		}
111 | 		return -EINVAL;
112 | 	}
113 | };
114 | 
115 | std::atomic<bool> terminateFlag {false};
116 | 
117 | template <LayoutType LT>
118 | std::vector<std::unique_ptr<simplyfuse::FuseFile>> registerMotor(MotorID motorID, int modelNumber, USB2Dynamixel& usb2dyn, simplyfuse::FuseFS& fuseFS) {
119 | 	std::vector<std::unique_ptr<simplyfuse::FuseFile>> files;
120 | 
121 | 	auto motorInfoPtr = meta::getMotorInfo(modelNumber);
122 | 	auto& motorModelFile = files.emplace_back(std::make_unique<simplyfuse::SimpleROFile>(motorInfoPtr->shortName + "\n"));
123 | 	fuseFS.rmdir("/" + std::to_string(motorID));
124 | 	fuseFS.registerFile("/" + std::to_string(motorID) + "/motor_model", *motorModelFile);
125 | 
126 | 	using Info = meta::MotorLayoutInfo<LT>;
127 | 	auto const& defaults = Info::getDefaults().at(modelNumber).defaultLayout;
128 | 	auto const& infos    = Info::getInfos();
129 | 	for (auto const& [reg, entry] : defaults) {
130 | 		//!TODO should register convert function here
131 | 		auto const& info = infos.at(reg);
132 | 		auto& newFile = files.emplace_back(std::make_unique<RegisterFile>(motorID, int(reg), info, usb2dyn));
133 | 		fuseFS.registerFile("/" + std::to_string(motorID) + "/by-register-name/" + info.name, *newFile);
134 | 		fuseFS.registerFile("/" + std::to_string(motorID) + "/by-register-id/" + std::to_string(int(reg)), *newFile);
135 | 	}
136 | 	return files;
137 | }
138 | 
139 | void runFuse() {
140 | 	auto timeout = std::chrono::microseconds{*g_timeout};
141 | 	auto usb2dyn = USB2Dynamixel(*g_baudrate, *g_device, *g_protocolVersion);
142 | 
143 | 	std::vector<int> range;
144 | 	if (g_id) {
145 | 		range = {MotorID(*g_id)};
146 | 	} else if (ids) {
147 |         std::copy(begin(*ids), end(*ids), std::back_inserter(range));
148 | 	} else {
149 | 		range.resize(0xfe);
150 | 		std::iota(begin(range), end(range), 0);
151 | 	}
152 | 
153 | 	simplyfuse::FuseFS fuseFS{*mountPoint};
154 | 	std::map<MotorID, std::vector<std::unique_ptr<simplyfuse::FuseFile>>> files;
155 | 
156 | 	auto detectAndHandleMotor = [&](MotorID motor) {
157 | 		auto [layout, modelNumber] = detectMotor(MotorID(motor), usb2dyn, timeout);
158 | 		if (modelNumber == 0) {
159 | 			return false;
160 | 		}
161 | 		std::vector<std::unique_ptr<simplyfuse::FuseFile>> newFiles;
162 | 		meta::forAllLayoutTypes([&](auto const& info) {
163 | 			using Info = std::decay_t<decltype(info)>;
164 | 			if (layout == Info::Type) {
165 | 				newFiles= registerMotor<Info::Type>(motor, modelNumber,usb2dyn, fuseFS);
166 | 			}
167 | 		});
168 | 		files[motor] = std::move(newFiles);
169 | 		return true;
170 | 	};
171 | 
172 | 	auto detectSingleMotor = PingFile(detectAndHandleMotor);
173 | 	auto detectAllMotors  = PingFile([=](int v) {
174 | 		if (v != 1) {
175 | 			return false;
176 | 		}
177 | 		for (int i{0}; i < 0xfe; ++i) {
178 | 			detectAndHandleMotor(i);
179 | 		}
180 | 		return true;
181 | 	});
182 | 
183 | 	fuseFS.registerFile("/detect_motor", detectSingleMotor);
184 | 	fuseFS.registerFile("/detect_all_motors", detectAllMotors);
185 | 	auto future = std::async(std::launch::async, [&]{
186 | 		// ping all motors
187 | 		for (auto motor : range) {
188 | 			if (terminateFlag) {
189 | 				break;
190 | 			}
191 | 			detectAndHandleMotor(motor);
192 | 		}
193 | 	});
194 | 
195 | 	auto sigHandler = [](int){ terminateFlag = true; };
196 | 	std::signal(SIGINT, sigHandler);
197 | 
198 | 	simplyfile::Epoll epoll;
199 | 	epoll.addFD(fuseFS.getFD(), [&](int){
200 | 		fuseFS.loop();
201 | 		epoll.modFD(fuseFS.getFD(), EPOLLIN|EPOLLONESHOT);
202 | 	}, EPOLLIN|EPOLLONESHOT);
203 | 
204 | 	while (not terminateFlag) {
205 | 		epoll.work(1);
206 | 	}
207 | }
208 | 
209 | }
210 | 


--------------------------------------------------------------------------------
/src/usb2dynamixel/LayoutPro.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | 
  3 | #include "LayoutPart.h"
  4 | 
  5 | namespace dynamixel::pro {
  6 | 
  7 | enum class Register : int {
  8 | 	MODEL_NUMBER           =   0,
  9 | 	MODEL_INFORMATION      =   2,
 10 | 	FIRMWARE_VERSION       =   6,
 11 | 	ID                     =   7,
 12 | 	BAUD_RATE              =   8,
 13 | 	RETURN_DELAY_TIME      =   9,
 14 | 	OPERATING_MODE         =  11,
 15 | 	HOMING_OFFSET          =  13,
 16 | 	MOVING_THRESHOLD       =  17,
 17 | 	TEMPERATURE_LIMIT      =  21,
 18 | 	MAX_VOLTAGE_LIMIT      =  22,
 19 | 	MIN_VOLTAGE_LIMIT      =  24,
 20 | 	ACCELERATION_LIMIT     =  26,
 21 | 	TORQUE_LIMIT           =  30,
 22 | 	VELOCITY_LIMIT         =  32,
 23 | 	MAX_POSITION_LIMIT     =  36,
 24 | 	MIN_POSITION_LIMIT     =  40,
 25 | 	EXTERNAL_PORT_MODE_1   =  44,
 26 | 	EXTERNAL_PORT_MODE_2   =  45,
 27 | 	EXTERNAL_PORT_MODE_3   =  46,
 28 | 	EXTERNAL_PORT_MODE_4   =  47,
 29 | 	SHUTDOWN               =  48,
 30 | 	INDIRECT_ADDRESS_BLOCK =  49,
 31 | 	TORQUE_ENABLE          = 562,
 32 | 	LED_RED                = 563,
 33 | 	LED_GREEN              = 564,
 34 | 	LED_BLUE               = 565,
 35 | 	VELOCITY_I_GAIN        = 586,
 36 | 	VELOCITY_P_GAIN        = 588,
 37 | 	POSITION_P_GAIN        = 594,
 38 | 	GOAL_POSITION          = 596,
 39 | 	GOAL_VELOCITY          = 600,
 40 | 	GOAL_TORQUE            = 604,
 41 | 	GOAL_ACCELERATION      = 606,
 42 | 	MOVING                 = 610,
 43 | 	PRESENT_POSITION       = 611,
 44 | 	PRESENT_VELOCITY       = 615,
 45 | 	PRESENT_CURRENT        = 621,
 46 | 	PRESENT_INPUT_VOLTAGE  = 623,
 47 | 	PRESENT_TEMPERATURE    = 625,
 48 | 	EXTERNAL_PORT_DATA_1   = 626,
 49 | 	EXTERNAL_PORT_DATA_2   = 628,
 50 | 	EXTERNAL_PORT_DATA_3   = 630,
 51 | 	EXTERNAL_PORT_DATA_4   = 632,
 52 | 	INDIRECT_DATA_BLOCK    = 534,
 53 | 	REGISTERED_INSTRUCTION = 890,
 54 | 	STATUS_RETURN_LEVEL    = 891,
 55 | 	HARDWARE_ERROR_STATUS  = 892,
 56 | };
 57 | 
 58 | constexpr Register operator+(Register t1, size_t t2) {
 59 | 	return Register(size_t(t1) + t2);
 60 | }
 61 | 
 62 | struct MotorLayoutInfo {
 63 | 	static constexpr LayoutType Type{LayoutType::Pro};
 64 | 	using FullLayout = Layout<Register::MODEL_NUMBER, 893>;
 65 | 
 66 | 	static auto getInfos()    -> meta::Layout<Register> const&;
 67 | 	static auto getDefaults() -> std::map<uint32_t, meta::Info<Register>> const&;
 68 | };
 69 | 
 70 | using FullLayout = MotorLayoutInfo::FullLayout;
 71 | 
 72 | using IndirectAddresses = std::array<uint16_t, 28>;
 73 | using IndirectData      = std::array<uint8_t, 28>;
 74 | 
 75 | }
 76 | 
 77 | namespace dynamixel {
 78 | 
 79 | template <> struct meta::MotorLayoutInfo<LayoutType::Pro> : pro::MotorLayoutInfo {};
 80 | 
 81 | #pragma pack(push, 1)
 82 | DynamixelLayoutPart(pro::Register::MODEL_NUMBER          ,  uint16_t, model_number          );
 83 | DynamixelLayoutPart(pro::Register::MODEL_INFORMATION     ,  uint32_t, model_information     );
 84 | DynamixelLayoutPart(pro::Register::FIRMWARE_VERSION      ,   uint8_t, firmware_version      );
 85 | DynamixelLayoutPart(pro::Register::ID                    ,   uint8_t, id                    );
 86 | DynamixelLayoutPart(pro::Register::BAUD_RATE             ,   uint8_t, baud_rate             );
 87 | DynamixelLayoutPart(pro::Register::RETURN_DELAY_TIME     ,   uint8_t, return_delay_time     );
 88 | DynamixelLayoutPart(pro::Register::OPERATING_MODE        ,   uint8_t, operating_mode        );
 89 | DynamixelLayoutPart(pro::Register::HOMING_OFFSET         ,   int32_t, homing_offset         );
 90 | DynamixelLayoutPart(pro::Register::MOVING_THRESHOLD      ,  uint32_t, moving_threshold      );
 91 | DynamixelLayoutPart(pro::Register::TEMPERATURE_LIMIT     ,   uint8_t, temperature_limit     );
 92 | DynamixelLayoutPart(pro::Register::MAX_VOLTAGE_LIMIT     ,  uint16_t, max_voltage_limit     );
 93 | DynamixelLayoutPart(pro::Register::MIN_VOLTAGE_LIMIT     ,  uint16_t, min_voltage_limit     );
 94 | DynamixelLayoutPart(pro::Register::ACCELERATION_LIMIT    ,  uint32_t, acceleration_limit    );
 95 | DynamixelLayoutPart(pro::Register::TORQUE_LIMIT          ,  uint16_t, torque_limit          );
 96 | DynamixelLayoutPart(pro::Register::VELOCITY_LIMIT        ,  uint32_t, velocity_limit        );
 97 | DynamixelLayoutPart(pro::Register::MAX_POSITION_LIMIT    ,   int32_t, max_position_limit    );
 98 | DynamixelLayoutPart(pro::Register::MIN_POSITION_LIMIT    ,   int32_t, min_position_limit    );
 99 | DynamixelLayoutPart(pro::Register::EXTERNAL_PORT_MODE_1  ,   uint8_t, external_port_mode_1  );
100 | DynamixelLayoutPart(pro::Register::EXTERNAL_PORT_MODE_2  ,   uint8_t, external_port_mode_2  );
101 | DynamixelLayoutPart(pro::Register::EXTERNAL_PORT_MODE_3  ,   uint8_t, external_port_mode_3  );
102 | DynamixelLayoutPart(pro::Register::EXTERNAL_PORT_MODE_4  ,   uint8_t, external_port_mode_4  );
103 | DynamixelLayoutPart(pro::Register::SHUTDOWN              ,   uint8_t, shutdown              );
104 | DynamixelLayoutPart(pro::Register::INDIRECT_ADDRESS_BLOCK , pro::IndirectAddresses, indirect_address_block);
105 | DynamixelLayoutPart(pro::Register::TORQUE_ENABLE         ,   uint8_t, torque_enable         );
106 | DynamixelLayoutPart(pro::Register::LED_RED               ,   uint8_t, led_red               );
107 | DynamixelLayoutPart(pro::Register::LED_GREEN             ,   uint8_t, led_green             );
108 | DynamixelLayoutPart(pro::Register::LED_BLUE              ,   uint8_t, led_blue              );
109 | DynamixelLayoutPart(pro::Register::VELOCITY_I_GAIN       ,  uint16_t, velocity_i_gain       );
110 | DynamixelLayoutPart(pro::Register::VELOCITY_P_GAIN       ,  uint16_t, velocity_p_gain       );
111 | DynamixelLayoutPart(pro::Register::POSITION_P_GAIN       ,  uint16_t, position_p_gain       );
112 | DynamixelLayoutPart(pro::Register::GOAL_POSITION         ,   int32_t, goal_position         );
113 | DynamixelLayoutPart(pro::Register::GOAL_VELOCITY         ,   int32_t, goal_velocity         );
114 | DynamixelLayoutPart(pro::Register::GOAL_TORQUE           ,   int16_t, goal_torque           );
115 | DynamixelLayoutPart(pro::Register::GOAL_ACCELERATION     ,  uint32_t, goal_acceleration     );
116 | DynamixelLayoutPart(pro::Register::MOVING                ,   uint8_t, moving                );
117 | DynamixelLayoutPart(pro::Register::PRESENT_POSITION      ,   int32_t, present_position      );
118 | DynamixelLayoutPart(pro::Register::PRESENT_VELOCITY      ,   int32_t, present_velocity      );
119 | DynamixelLayoutPart(pro::Register::PRESENT_CURRENT       ,   int16_t, present_current       );
120 | DynamixelLayoutPart(pro::Register::PRESENT_INPUT_VOLTAGE ,   int16_t, present_input_voltage );
121 | DynamixelLayoutPart(pro::Register::PRESENT_TEMPERATURE   ,   uint8_t, present_temperature   );
122 | DynamixelLayoutPart(pro::Register::EXTERNAL_PORT_DATA_1  ,  uint16_t, external_port_data_1  );
123 | DynamixelLayoutPart(pro::Register::EXTERNAL_PORT_DATA_2  ,  uint16_t, external_port_data_2  );
124 | DynamixelLayoutPart(pro::Register::EXTERNAL_PORT_DATA_3  ,  uint16_t, external_port_data_3  );
125 | DynamixelLayoutPart(pro::Register::EXTERNAL_PORT_DATA_4  ,  uint16_t, external_port_data_4  );
126 | DynamixelLayoutPart(pro::Register::INDIRECT_DATA_BLOCK   ,   pro::IndirectData,      indirect_data_block);
127 | DynamixelLayoutPart(pro::Register::REGISTERED_INSTRUCTION,   uint8_t, registered_instruction);
128 | DynamixelLayoutPart(pro::Register::STATUS_RETURN_LEVEL   ,   uint8_t, status_return_level   );
129 | DynamixelLayoutPart(pro::Register::HARDWARE_ERROR_STATUS ,   uint8_t, hardware_error_status );
130 | 
131 | #pragma pack(pop)
132 | 
133 | }
134 | 


--------------------------------------------------------------------------------
/src/simplyfile/Epoll.cpp:
--------------------------------------------------------------------------------
  1 | #include "Epoll.h"
  2 | #include "Event.h"
  3 | 
  4 | #include <map>
  5 | #include <vector>
  6 | #include <atomic>
  7 | 
  8 | #include <errno.h>
  9 | #include <cstring>
 10 | #include <iostream>
 11 | #include <stdexcept>
 12 | #include <mutex>
 13 | #include <shared_mutex>
 14 | #include <condition_variable>
 15 | 
 16 | 
 17 | #include <cxxabi.h>
 18 | #include <iostream>
 19 | 
 20 | 
 21 | namespace
 22 | {
 23 | 
 24 | std::string demangle(std::type_info const& ti) {
 25 | 	int status;
 26 | 	char* name_ = abi::__cxa_demangle(ti.name(), 0, 0, &status);
 27 | 	if (status) {
 28 | 		throw std::runtime_error("cannot demangle a type!");
 29 | 	}
 30 | 	std::string demangledName{ name_ };
 31 | 	free(name_); // need to use free here :/
 32 | 	return demangledName;
 33 | }
 34 | 
 35 | std::string removeAnonNamespace(std::string const& in_str) {
 36 | 	size_t index = 0;
 37 | 	std::string str = in_str;
 38 | 	std::string toReplace = "(anonymous namespace)::";
 39 | 	while (true) {
 40 | 	     index = str.find(toReplace, index);
 41 | 	     if (index == std::string::npos) {
 42 | 	    	 break;
 43 | 	     }
 44 | 	     str.replace(index, toReplace.size(), "");
 45 | 	     index += toReplace.size();
 46 | 	}
 47 | 	return str;
 48 | }
 49 | 
 50 | 
 51 | template<typename Func>
 52 | struct Finally final {
 53 | 	Finally(Func && f) : _f(f) {}
 54 | 	~Finally() {_f();}
 55 | private:
 56 | 	Func _f;
 57 | };
 58 | template<typename Func>
 59 | Finally(Func) -> Finally<Func>;
 60 | 
 61 | }
 62 | 
 63 | 
 64 | namespace simplyfile
 65 | {
 66 | 
 67 | struct CallbackInfo {
 68 | 	Epoll::Callback cb;
 69 | 
 70 | 	bool tearDownFlag {false};
 71 | 	int numExecutors {0};
 72 | 
 73 | 	std::mutex mutex;
 74 | 	std::condition_variable cv {};
 75 | 
 76 | 	std::string name;
 77 | 	std::mutex runtimeMutex;
 78 | 	Epoll::RuntimeInfo accumulatedRuntime;
 79 | 
 80 | 	CallbackInfo(std::string const& _name) : name{_name} {}
 81 | 
 82 | 	bool startExecution() {
 83 | 		std::lock_guard lock(mutex);
 84 | 		if (tearDownFlag) {
 85 | 			return false;
 86 | 		}
 87 | 		++numExecutors;
 88 | 		return true;
 89 | 	}
 90 | 
 91 | 	void stopExecution() {
 92 | 		std::lock_guard lock(mutex);
 93 | 		--numExecutors;
 94 | 		if (numExecutors == 0  and tearDownFlag) {
 95 | 			cv.notify_all();
 96 | 		}
 97 | 	}
 98 | 
 99 | 	void tearDown(bool blocking) {
100 | 		std::unique_lock lock(mutex);
101 | 		tearDownFlag = true;
102 | 		if (blocking) {
103 | 			while (numExecutors > 0) {
104 | 				cv.wait(lock);
105 | 			}
106 | 		}
107 | 	}
108 | };
109 | 
110 | struct Epoll::Pimpl {
111 | 	std::shared_mutex infosMutex;
112 | 	std::map<int, std::shared_ptr<CallbackInfo>> infos;
113 | 	std::map<std::string, std::atomic<std::chrono::nanoseconds>> runtimes;
114 | 	Event eventFD{EFD_SEMAPHORE};
115 | };
116 | 
117 | Epoll::Epoll()
118 | 	: FileDescriptor(epoll_create1(0))
119 | 	, pimpl(new Pimpl)
120 | {
121 | 	if (not valid()) {
122 | 		throw std::runtime_error("cannot create epollfd");
123 | 	}
124 | 	addFD(pimpl->eventFD, [=](int){
125 | 		pimpl->eventFD.get();
126 | 		modFD(pimpl->eventFD, EPOLLIN|EPOLLONESHOT);
127 | 	}, EPOLLIN|EPOLLONESHOT, "self_wakeup");
128 | }
129 | 
130 | Epoll::Epoll(Epoll &&other) noexcept
131 | 	: FileDescriptor(std::move(other)) {
132 | 	std::swap(pimpl, other.pimpl);
133 | }
134 | 
135 | Epoll& Epoll::operator=(Epoll &&rhs) noexcept {
136 | 	FileDescriptor::operator =(std::move(rhs));
137 | 	std::swap(pimpl, rhs.pimpl);
138 | 	return *this;
139 | }
140 | 
141 | Epoll::~Epoll() {}
142 | 
143 | void Epoll::addFD(int _fd, Callback callback, int epollFlags, std::string const& name) {
144 | 	std::shared_ptr<CallbackInfo> info;
145 | 	std::string _name = name;
146 | 	if (_name == "") {
147 | 		_name = removeAnonNamespace(demangle(callback.target_type()));
148 | 	}
149 | 	{
150 | 		std::lock_guard lock{pimpl->infosMutex};
151 | 		info = pimpl->infos[_fd] = std::make_shared<CallbackInfo>(_name); // this and the next line could be a beautiful one liner if...gcc5.4 would not be used
152 | 		info->cb = std::move(callback);
153 | 	}
154 | 	struct epoll_event event {};
155 | 	event.events = epollFlags;
156 | 	event.data.fd = _fd;
157 | 	if (epoll_ctl(*this, EPOLL_CTL_ADD, _fd, &event) == -1) {
158 | 		std::lock_guard lock{pimpl->infosMutex};
159 | 		pimpl->infos.erase(_fd); // revert the add
160 | 		throw std::runtime_error("cannot call epoll_ctl EPOLL_CTL_ADD " + std::string(strerror(errno)));
161 | 	}
162 | }
163 | 
164 | void Epoll::modFD(int _fd, int epollFlags) {
165 | 	struct epoll_event event {};
166 | 	event.events = epollFlags;
167 | 	event.data.fd = _fd;
168 | 	if (epoll_ctl(*this, EPOLL_CTL_MOD, _fd, &event) == -1) {
169 | 		throw std::runtime_error("cannot call epoll_ctl EPOLL_CTL_MOD " + std::string(strerror(errno)));
170 | 	}
171 | }
172 | 
173 | void Epoll::rmFD(int _fd, bool blocking) {
174 | 	std::shared_ptr<CallbackInfo> info;
175 | 	{
176 | 		std::lock_guard lock{pimpl->infosMutex};
177 | 		auto it = pimpl->infos.find(_fd);
178 | 		if (it != pimpl->infos.end()) {
179 | 			info = it->second;
180 | 		}
181 | 	}
182 | 
183 | 	if (info) {
184 | 		info->tearDown(blocking);
185 | 		{
186 | 			std::lock_guard lock{pimpl->infosMutex};
187 | 			pimpl->infos.erase(_fd);
188 | 			if (epoll_ctl(*this, EPOLL_CTL_DEL, _fd, nullptr) == -1) {
189 | 				throw std::runtime_error("cannot call epoll_ctl EPOLL_CTL_DEL " + std::string(strerror(errno)));
190 | 			}
191 | 		}
192 | 	}
193 | }
194 | 
195 | std::vector<struct epoll_event> Epoll::wait(int maxEvents, int timeout_ms) {
196 | 	std::vector<epoll_event> events(maxEvents, epoll_event{});
197 | 	int num = epoll_wait(*this, events.data(), events.size(), timeout_ms);
198 | 	if (num >= 0) {
199 | 		events.resize(num);
200 | 	} else {
201 | 		events.resize(0);
202 | 	}
203 | 	return events;
204 | }
205 | 
206 | void Epoll::dispatch(std::vector<struct epoll_event> const& events) {
207 | 	struct Wrapper {
208 | 		std::shared_ptr<CallbackInfo> info;
209 | 		struct epoll_event event;
210 | 	};
211 | 
212 | 	std::vector<Wrapper> wrappers;
213 | 	wrappers.reserve(events.size());
214 | 	{
215 | 		// fetch the callback handles as the container map might be modified while we access it
216 | 		std::shared_lock lock{pimpl->infosMutex};
217 | 		for (auto const& event : events) {
218 | 			auto cbIt = pimpl->infos.find(event.data.fd);
219 | 			if (cbIt != pimpl->infos.end()) {
220 | 				wrappers.emplace_back(Wrapper{cbIt->second, event});
221 | 			}
222 | 		}
223 | 	}
224 | 	for (auto const& wrapper : wrappers) {
225 | 		if (wrapper.info->startExecution()) {
226 | 			try {
227 | 				auto start = std::chrono::high_resolution_clock::now();
228 | 				Finally finally{[=] {
229 | 					auto end = std::chrono::high_resolution_clock::now();
230 | 					{
231 | 						std::lock_guard lock{wrapper.info->runtimeMutex};
232 | 						Epoll::RuntimeInfo delta{end - start, 1};
233 | 						wrapper.info->accumulatedRuntime += delta;
234 | 					}
235 | 					wrapper.info->stopExecution();
236 | 				}};
237 | 				wrapper.info->cb(wrapper.event.events);
238 | 			} catch (...) {
239 | 				std::throw_with_nested(std::runtime_error(removeAnonNamespace(demangle(wrapper.info->cb.target_type())) + " threw an exception:"));
240 | 			}
241 | 		}
242 | 	}
243 | }
244 | 
245 | void Epoll::wakeup(uint64_t count) noexcept {
246 | 	pimpl->eventFD.put(count);
247 | }
248 | 
249 | auto Epoll::getRuntimes() const -> std::map<std::string, RuntimeInfo> {
250 | 	std::map<std::string, RuntimeInfo> runtimes;
251 | 	std::shared_lock lock{pimpl->infosMutex};
252 | 	for (auto const& info : pimpl->infos) {
253 | 		std::lock_guard lock{info.second->runtimeMutex};
254 | 		runtimes[info.second->name] += info.second->accumulatedRuntime;
255 | 	}
256 | 	return runtimes;
257 | }
258 | 
259 | 
260 | }
261 | 


--------------------------------------------------------------------------------
/src/usb2dynamixel/LayoutAX.cpp:
--------------------------------------------------------------------------------
  1 | #include "LayoutAX.h"
  2 | 
  3 | namespace dynamixel::ax {
  4 | 
  5 | auto MotorLayoutInfo::getInfos() -> meta::Layout<Register> const& {
  6 | 	using A = meta::LayoutField::Access;
  7 | 	static auto data = meta::Layout<Register> {
  8 | 		{Register::MODEL_NUMBER         , {2,  true, A:: R, "Model Number", "model number"}},
  9 | 		{Register::FIRMWARE_VERSION     , {1,  true, A:: R, "Version of Firmware", "Information on the version of firmware"}},
 10 | 		{Register::ID                   , {1,  true, A::RW, "ID", "ID of Dynamixel"}},
 11 | 		{Register::BAUD_RATE            , {1,  true, A::RW, "Baud Rate", "Baud Rate of Dynamixel"}},
 12 | 		{Register::RETURN_DELAY_TIME    , {1,  true, A::RW, "Return Delay Time", "Return Delay Time"}},
 13 | 		{Register::CW_ANGLE_LIMIT       , {2,  true, A::RW, "CW Angle Limit", "clockwise Angle Limit"}},
 14 | 		{Register::CCW_ANGLE_LIMIT      , {2,  true, A::RW, "CCW Angle Limit", "counterclockwise Angle Limit"}},
 15 | 		{Register::TEMPERATURE_LIMIT    , {1,  true, A::RW, "Highest Limit Temperature", "Internal Limit Temperature"}},
 16 | 		{Register::MIN_VOLTAGE_LIMIT    , {1,  true, A::RW, "Min Limit Voltage", "Min Limit Voltage"}},
 17 | 		{Register::MAX_VOLTAGE_LIMIT    , {1,  true, A::RW, "Max Limit Voltage", "Max Limit Voltage"}},
 18 | 		{Register::MAX_TORQUE           , {2,  true, A::RW, "Max Torque", "Max. Torque"}},
 19 | 		{Register::STATUS_RETURN_LEVEL  , {1,  true, A::RW, "Status Return Level", "Status Return Level"}},
 20 | 		{Register::ALARM_LED            , {1,  true, A::RW, "Alarm LED", "LED for Alarm"}},
 21 | 		{Register::SHUTDOWN             , {1,  true, A::RW, "Shutdown", "Shutdown for Alarm"}},
 22 | 		{Register::TORQUE_ENABLE        , {1, false, A::RW, "Torque Enable", "Torque On/Off"}},
 23 | 		{Register::LED                  , {1, false, A::RW, "LED", "LED On/Off"}},
 24 | 		{Register::CW_COMPLIANCE_MARGIN , {1, false, A::RW, "CW Compliance Margin", "CW Compliance Margin"}},
 25 | 		{Register::CCW_COMPLIANCE_MARGIN, {1, false, A::RW, "CCW Compliance Margin", "CCW Compliance Margin"}},
 26 | 		{Register::CW_COMPLIANCE_SLOPE  , {1, false, A::RW, "CW Compliance Slope", "CW Compliance Slope"}},
 27 | 		{Register::CCW_COMPLIANCE_SLOPE , {1, false, A::RW, "CCW Compliance Slope", "CCW Compliance Slope"}},
 28 | 		{Register::GOAL_POSITION        , {2, false, A::RW, "Goal Position", "Goal Position"}},
 29 | 		{Register::MOVING_SPEED         , {2, false, A::RW, "Moving Speed", "Moving Speed (Moving Velocity)"}},
 30 | 		{Register::TORQUE_LIMIT         , {2, false, A::RW, "Torque Limit", "Torque Limit (Goal Torque)"}},
 31 | 		{Register::PRESENT_POSITION     , {2, false, A:: R, "Present Position", "Current Position (Present Velocity)"}},
 32 | 		{Register::PRESENT_SPEED        , {2, false, A:: R, "Present Speed", "Current Speed"}},
 33 | 		{Register::PRESENT_LOAD         , {2, false, A:: R, "Present Load", "Current Load"}},
 34 | 		{Register::PRESENT_VOLTAGE      , {1, false, A:: R, "Present Voltage", "Current Voltage"}},
 35 | 		{Register::PRESENT_TEMPERATURE  , {1, false, A:: R, "Present Temperature", "Current Temperature"}},
 36 | 		{Register::REGISTERED           , {1, false, A:: R, "Registered", "Means if Instruction is registered"}},
 37 | 		{Register::MOVING               , {1, false, A:: R, "Moving", "Means if there is any movement"}},
 38 | 		{Register::LOCK                 , {1, false, A::RW, "Lock", "Locking EEPROM"}},
 39 | 		{Register::PUNCH                , {2, false, A::RW, "Punch", "Punch"}},
 40 | 	};
 41 | 	return data;
 42 | }
 43 | 
 44 | 
 45 | auto MotorLayoutInfo::getDefaults() -> std::map<uint32_t, meta::Info<Register>> const& {
 46 | 	 static auto data = []() {
 47 | 		auto convertPosition    = meta::buildConverter("r", (2.*M_PI)/1023.*300./360., 512);
 48 | 		auto convertSpeed       = meta::buildConverter("r/s", 0.111/60*2.*M_PI);
 49 | 		auto convertTemperature = meta::buildConverter("C", 1.);
 50 | 		auto convertVoltage     = meta::buildConverter("V", 16./160);
 51 | 		auto convertTorque      = meta::buildConverter("%", 100./1023., 0);
 52 | 
 53 | 		auto data = std::map<uint32_t, meta::Info<Register>> {
 54 | 			{300, {
 55 | 				300,
 56 | 				LayoutType::AX,
 57 | 				"AX-12W",
 58 | 				{"AX-12W"}, {
 59 | 					{Register::MODEL_NUMBER         , {          300, {}}},
 60 | 					{Register::FIRMWARE_VERSION     , { std::nullopt, {}}},
 61 | 					{Register::ID                   , {            1, {}}},
 62 | 					{Register::BAUD_RATE            , {            1, {}}},
 63 | 					{Register::RETURN_DELAY_TIME    , {          250, {}}},
 64 | 					{Register::CW_ANGLE_LIMIT       , {            0, convertPosition}},
 65 | 					{Register::CCW_ANGLE_LIMIT      , {       0x03ff, convertPosition}},
 66 | 					{Register::TEMPERATURE_LIMIT    , {           70, convertTemperature}},
 67 | 					{Register::MIN_VOLTAGE_LIMIT    , {           60, convertVoltage}},
 68 | 					{Register::MAX_VOLTAGE_LIMIT    , {          140, convertVoltage}},
 69 | 					{Register::MAX_TORQUE           , {       0x03ff, convertTorque}},
 70 | 					{Register::STATUS_RETURN_LEVEL  , {            2, {}}},
 71 | 					{Register::ALARM_LED            , {           36, {}}},
 72 | 					{Register::SHUTDOWN             , {           36, {}}},
 73 | 					{Register::TORQUE_ENABLE        , {            0, {}}},
 74 | 					{Register::LED                  , {            0, {}}},
 75 | 					{Register::CW_COMPLIANCE_MARGIN , {            4, {}}},
 76 | 					{Register::CCW_COMPLIANCE_MARGIN, {            4, {}}},
 77 | 					{Register::CW_COMPLIANCE_SLOPE  , {           64, {}}},
 78 | 					{Register::CCW_COMPLIANCE_SLOPE , {           64, {}}},
 79 | 					{Register::GOAL_POSITION        , { std::nullopt, convertPosition}},
 80 | 					{Register::MOVING_SPEED         , { std::nullopt, convertSpeed}},
 81 | 					{Register::TORQUE_LIMIT         , { std::nullopt, convertTorque}},
 82 | 					{Register::PRESENT_POSITION     , { std::nullopt, convertPosition}},
 83 | 					{Register::PRESENT_SPEED        , { std::nullopt, convertSpeed}},
 84 | 					{Register::PRESENT_LOAD         , { std::nullopt, {}}},
 85 | 					{Register::PRESENT_VOLTAGE      , { std::nullopt, convertVoltage}},
 86 | 					{Register::PRESENT_TEMPERATURE  , { std::nullopt, convertTemperature}},
 87 | 					{Register::REGISTERED           , {            0, {}}},
 88 | 					{Register::MOVING               , {            0, {}}},
 89 | 					{Register::LOCK                 , {            0, {}}},
 90 | 					{Register::PUNCH                , {           32, {}}},
 91 | 				}
 92 | 			}}
 93 | 		};
 94 | 
 95 | 		auto newMotor = [&](int number, std::string shortName, std::vector<std::string> names) -> meta::Info<Register>& {
 96 | 			auto& m = data[number];
 97 | 			m = data.at(300);
 98 | 			m.modelNumber = number;
 99 | 			m.shortName = std::move(shortName);
100 | 			m.motorNames = std::move(names);
101 | 			std::get<0>(m.defaultLayout[Register::MODEL_NUMBER]) = number;
102 | 			return m;
103 | 		};
104 | 
105 | 		{
106 | 			auto& m = newMotor(12, "AX-12A", {"AX-12A"});
107 | 			std::get<0>(m.defaultLayout[Register::CW_COMPLIANCE_MARGIN])  = 1;
108 | 			std::get<0>(m.defaultLayout[Register::CCW_COMPLIANCE_MARGIN]) = 1;
109 | 			std::get<0>(m.defaultLayout[Register::CW_COMPLIANCE_SLOPE])   = 32;
110 | 			std::get<0>(m.defaultLayout[Register::CCW_COMPLIANCE_SLOPE])  = 32;
111 | 		}
112 | 
113 | 		{
114 | 			auto& m = newMotor(18, "AX-18A", {"AX-18A"});
115 | 			std::get<0>(m.defaultLayout[Register::TEMPERATURE_LIMIT])     = 75;
116 | 			std::get<0>(m.defaultLayout[Register::MAX_TORQUE])            = 983;
117 | 			std::get<0>(m.defaultLayout[Register::CW_COMPLIANCE_MARGIN])  = 1;
118 | 			std::get<0>(m.defaultLayout[Register::CCW_COMPLIANCE_MARGIN]) = 1;
119 | 			std::get<0>(m.defaultLayout[Register::CW_COMPLIANCE_SLOPE])   = 32;
120 | 			std::get<0>(m.defaultLayout[Register::CCW_COMPLIANCE_SLOPE])  = 32;
121 | 		}
122 | 		return data;
123 | 	}();
124 | 	return data;
125 | };
126 | 
127 | }
128 | 


--------------------------------------------------------------------------------
/src/detect.cpp:
--------------------------------------------------------------------------------
  1 | #include "usb2dynamixel/USB2Dynamixel.h"
  2 | #include "usb2dynamixel/MotorMetaInfo.h"
  3 | #include "globalOptions.h"
  4 | 
  5 | #include "commonTasks.h"
  6 | 
  7 | #include <algorithm>
  8 | #include <numeric>
  9 | 
 10 | #define TERM_RED                        "\033[31m"
 11 | #define TERM_GREEN                      "\033[32m"
 12 | #define TERM_RESET                      "\033[0m"
 13 | 
 14 | namespace {
 15 | 
 16 | void runDetect();
 17 | auto detectCmd  = sargp::Command{{"", "detect"}, "detect all dynamixel motors", runDetect};
 18 | auto baudrates  = detectCmd.Parameter<std::set<int>>({57142}, "other_baudrates", "more baudrates to test", {}, &listTypicalBaudrates);
 19 | auto readAll    = detectCmd.Flag("read_all", "read all registers from the detected motors (instead of just printing the found motors)");
 20 | auto ids        = detectCmd.Parameter<std::set<int>>({}, "ids", "the target Id");
 21 | auto optCont    = detectCmd.Flag("continues", "runs bulk read repeatably after detecting motors");
 22 | 
 23 | 
 24 | using namespace dynamixel;
 25 | 
 26 | template <typename T>
 27 | struct is_array : std::false_type {};
 28 | 
 29 | template <typename T, size_t N>
 30 | struct is_array<std::array<T, N>> : std::true_type {};
 31 | 
 32 | template<typename T>
 33 | inline constexpr bool is_array_v = is_array<T>::value;
 34 | 
 35 | 
 36 | auto readDetailedInfosFromUnknown(dynamixel::USB2Dynamixel& usb2dyn, std::vector<std::tuple<MotorID, uint16_t>> const& motors, std::chrono::microseconds timeout, bool _print) -> std::tuple<int, int> {
 37 | 	int expectedTransactions = 1 + motors.size();
 38 | 	int successfullTransactions = 0;
 39 | 
 40 | 	std::vector<std::tuple<MotorID, int, size_t>> request;
 41 | 	for (auto const& [g_id, modelNumber] : motors) {
 42 | 		request.push_back(std::make_tuple(g_id, int(mx_v1::Register::MODEL_NUMBER), 74));
 43 | 	}
 44 | 	auto response = usb2dyn.bulk_read(request, timeout);
 45 | 
 46 | 	if (not response.empty()) {
 47 | 		successfullTransactions = 1 + response.size();
 48 | 	}
 49 | 	if (not _print) {
 50 | 		return {successfullTransactions, expectedTransactions};
 51 | 	}
 52 | 
 53 | 	if (response.size() != motors.size()) {
 54 | 		std::cout << "couldn't retrieve detailed information from all motors\n";
 55 | 	}
 56 | 	for (auto const& [motorID, baseRegister, errorCode, rxBuf] : response) {
 57 | 		std::cout << "found motor " << static_cast<int>(motorID) << "\n";
 58 | 		std::cout << "registers:\n";
 59 | 		for (size_t idx{0}; idx < rxBuf.size(); ++idx) {
 60 | 			std::cout << "  " << std::setw(3) << std::setfill(' ') << std::dec << idx << ": " <<
 61 | 				std::setw(2) << std::setfill('0') << std::hex << static_cast<int>(rxBuf.at(idx)) << std::dec << "\n";
 62 | 		}
 63 | 		std::cout << "\n";
 64 | 	}
 65 | 	std::cout << "-----------\n";
 66 | 
 67 | 	return {successfullTransactions, expectedTransactions};
 68 | }
 69 | 
 70 | template <LayoutType LT, typename Layout>
 71 | auto readDetailedInfos(dynamixel::USB2Dynamixel& usb2dyn, std::vector<std::tuple<MotorID, uint16_t>> const& motors, std::chrono::microseconds timeout, bool _print) -> std::tuple<int, int> {
 72 | 	int expectedTransactions = 1 + motors.size();
 73 | 	int successfullTransactions = 0;
 74 | 
 75 | 	auto response = usb2dyn.bulk_read<Layout::BaseRegister, Layout::Length>(motors, timeout);
 76 | 	if (not response.empty()) {
 77 | 		successfullTransactions = 1 + response.size();
 78 | 	}
 79 | 	if (not _print) {
 80 | 		return {successfullTransactions, expectedTransactions};
 81 | 	}
 82 | 
 83 | 	if (response.size() != motors.size()) {
 84 | 		std::cout << "couldn't retrieve detailed information from all motors\n";
 85 | 	}
 86 | 
 87 | 	std::cout << "             ";
 88 | 	for (auto const& [g_id, modelNumber, errorCode, layout] : response) {
 89 | 		auto motorInfoPtr = meta::getMotorInfo(modelNumber);
 90 | 		std::cout << std::setw(14) << motorInfoPtr->shortName;
 91 | 	}
 92 | 	std::cout << "\n";
 93 | 
 94 | 	using Info = meta::MotorLayoutInfo<LT>;
 95 | 	auto const& infos = Info::getInfos();
 96 | 	for (auto const& [reg, info] : infos) {
 97 | 		std::cout << "0x" << std::hex << std::setfill('0') << std::setw(2) << int(reg) << std::setfill(' ');
 98 | 		std::cout << std::dec << " " << int(info.length);
 99 | 		std::cout << " " << std::setw(2) << to_string(info.access);
100 | 		std::cout << " " << (info.romArea?"ROM":"RAM");
101 | 
102 | 		for (auto const& [g_id, modelNumber, errorCode, layout] : response) {
103 | 			visit([reg=reg, modelNumber=modelNumber](auto _reg, auto& value) {
104 | 				if (_reg != reg) return;
105 | 				if constexpr (is_array_v<std::decay_t<decltype(value)>>) {
106 | 					return;
107 | 				} else {
108 | 					auto const& defaults = Info::getDefaults().at(modelNumber).defaultLayout;
109 | 					auto iter = defaults.find(reg);
110 | 
111 | 					std::stringstream ss;
112 | 					int extra = 0;
113 | 					if (iter == defaults.end()) {
114 | 						ss << std::boolalpha;
115 | 						ss << "-na-";
116 | 					} else {
117 | 						auto const& [optDefault, convert] = iter->second;
118 | 						if (optDefault) {
119 | 							extra = 9;
120 | 							if (int(value) != int(optDefault.value())) {
121 | 								ss << " " TERM_RED << int(value) << TERM_RESET "(";
122 | 							} else {
123 | 								ss << " " << TERM_GREEN << int(value) << TERM_RESET "(";
124 | 							}
125 | 							ss << int(optDefault.value());
126 | 						} else {
127 | 							ss << " " << int(value) << "(";
128 | 							ss << "-";
129 | 						}
130 | 						ss << ")";
131 | 					}
132 | 					std::cout << std::setw(14+extra) << ss.str();
133 | 				}
134 | 			}, layout);
135 | 		}
136 | 		std::cout << std::setw(30) << info.name << " - " << info.description << "\n";
137 | 	}
138 | 	std::cout << "-----------\n";
139 | 	return {successfullTransactions, expectedTransactions};
140 | }
141 | 
142 | void runDetect() {
143 | 	baudrates->emplace(*g_baudrate);
144 | 	auto timeout = std::chrono::microseconds{*g_timeout};
145 | 	auto protocols = std::vector<dynamixel::Protocol>{dynamixel::Protocol::V1, dynamixel::Protocol::V2};
146 | 	if (g_protocolVersion) {
147 | 		protocols = {dynamixel::Protocol{*g_protocolVersion}};
148 | 	}
149 | 	for (auto protocolVersion : protocols) {
150 | 		std::cout << "# trying protocol version " << int(protocolVersion) << "\n";
151 | 		for (auto baudrate : *baudrates) {
152 | 			std::cout << "## trying baudrate: " << baudrate << "\n";
153 | 			auto usb2dyn = dynamixel::USB2Dynamixel(baudrate, *g_device, protocolVersion);
154 | 
155 | 			// generate range to check
156 | 			std::vector<int> range(0xFD);
157 | 			std::iota(begin(range), end(range), 0);
158 | 			if (g_id) {
159 | 				range = {*g_id};
160 | 			} else  if (ids) {
161 | 				range.clear();
162 | 				for (auto x : *ids) {
163 | 					range.push_back(x);
164 | 				}
165 | 			}
166 | 
167 | 			// ping all motors
168 | 			std::map<LayoutType, std::vector<std::tuple<MotorID, uint16_t>>> motors;
169 | 			for (auto motor : range) {
170 | 				auto [layout, modelNumber] = detectMotor(MotorID(motor), usb2dyn, timeout);
171 | 				if (modelNumber != 0) {
172 | 					motors[layout].push_back(std::make_tuple(motor, modelNumber));
173 | 				}
174 | 			}
175 | 			// read detailed infos if requested
176 | 			if ((readAll or optCont) and not motors.empty()) {
177 | 				static int count = 0;
178 | 				static int successful = 0;
179 | 				static int total = 0;
180 | 				auto start = std::chrono::high_resolution_clock::now();
181 | 				auto lastPrint = start;
182 | 				do {
183 | 					count += 1;
184 | 					auto now = std::chrono::high_resolution_clock::now();
185 | 					auto diff = std::chrono::duration_cast<std::chrono::milliseconds>(now - lastPrint);
186 | 					bool print = (diff.count() > 100) or readAll;
187 | 
188 | 					meta::forAllLayoutTypes([&](auto const& info) {
189 | 						using Info = std::decay_t<decltype(info)>;
190 | 						if (not motors[Info::Type].empty()) {
191 | 							using FullLayout = typename Info::FullLayout;
192 | 							auto [suc, tot] = readDetailedInfos<Info::Type, FullLayout>(usb2dyn, motors[Info::Type], timeout, print);
193 | 							successful += suc;
194 | 							total += tot;
195 | 						}
196 | 					});
197 | 
198 | 					if (not motors[LayoutType::None].empty()) {
199 | 						auto [suc, tot] = readDetailedInfosFromUnknown(usb2dyn, motors[LayoutType::None], timeout, print);
200 | 						successful += suc;
201 | 						total += tot;
202 | 					}
203 | 					if (print and optCont) {
204 | 						lastPrint = now;
205 | 						std::cout << successful << "/" << total << " successful/total transactions - ";
206 | 						std::cout << count << " loops\n";
207 | 						auto now = std::chrono::high_resolution_clock::now();
208 | 						auto diff = std::chrono::duration_cast<std::chrono::milliseconds>(now - start);
209 | 						std::cout << double(successful) / diff.count() * 1000. << "/" << double(total) / diff.count() * 1000. << " trans per second -        ";
210 | 						std::cout << double(count) / diff.count() * 1000. << " loops per second" << "\n";
211 | 					}
212 | 				} while (optCont);
213 | 			}
214 | 		}
215 | 	}
216 | }
217 | 
218 | }
219 | 


--------------------------------------------------------------------------------
/src/usb2dynamixel/LayoutMX_V2.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | 
  3 | #include "LayoutPart.h"
  4 | 
  5 | namespace dynamixel::mx_v2 {
  6 | 
  7 | enum class Register : int {
  8 | 	MODEL_NUMBER           = 0x00,
  9 | 	MODEL_INFORMATION      = 0x02,
 10 | 	VERSION_FIRMWARE       = 0x06,
 11 | 	ID                     = 0x07,
 12 | 	BAUD_RATE              = 0x08,
 13 | 	RETURN_DELAY_TIME      = 0x09,
 14 | 	DRIVE_MODE             = 0x0a,
 15 | 	OPERATING_MODE         = 0x0b,
 16 | 	SECONDARY_ID           = 12,
 17 | 	PROTOCOL_VERSION       = 13,
 18 | 	HOMING_OFFSET          = 20,
 19 | 	MOVING_THRESHOLD       = 24,
 20 | 	TEMPERATURE_LIMIT      = 31,
 21 | 	MAX_VOLTAGE_LIMIT      = 32,
 22 | 	MIN_VOLTAGE_LIMIT      = 34,
 23 | 	PWM_LIMIT              = 36,
 24 | 	CURRENT_LIMIT          = 38,
 25 | 	ACCELERATION_LIMIT     = 40,
 26 | 	VELOCITY_LIMIT         = 44,
 27 | 	MAX_POSITION_LIMIT     = 48,
 28 | 	MIN_POSITION_LIMIT     = 52,
 29 | 	EXTERNAL_PORT_MODE_1   = 56,
 30 | 	EXTERNAL_PORT_MODE_2   = 57,
 31 | 	EXTERNAL_PORT_MODE_3   = 58,
 32 | 	SHUTDOWN               = 63,
 33 | 	TORQUE_ENABLE          = 64,
 34 | 	LED                    = 65,
 35 | 	STATUS_RETURN_LEVEL    = 68,
 36 | 	REGISTERED_INSTRUCTION = 69,
 37 | 	HARDWARE_ERROR_STATUS  = 70,
 38 | 	VELOCITY_I_GAIN        = 76,
 39 | 	VELOCITY_P_GAIN        = 78,
 40 | 	POSITION_D_GAIN        = 80,
 41 | 	POSITION_I_GAIN        = 82,
 42 | 	POSITION_P_GAIN        = 84,
 43 | 	FEEDFORWARD_2ND_GAIN   = 88,
 44 | 	FEEDFORWARD_1ST_GAIN   = 90,
 45 | 	BUS_WATCHDOG           = 98,
 46 | 	GOAL_PWM               = 100,
 47 | 	GOAL_CURRENT           = 102,
 48 | 	GOAL_VELOCITY          = 104,
 49 | 	PROFILE_ACCELERATION   = 108,
 50 | 	PROFILE_VELOCITY       = 112,
 51 | 	GOAL_POSITION          = 116,
 52 | 	REALTIME_TICK          = 120,
 53 | 	MOVING                 = 122,
 54 | 	MOVING_STATUS          = 123,
 55 | 	PRESENT_PWM            = 124,
 56 | 	PRESENT_CURRENT        = 126,
 57 | 	PRESENT_VELOCITY       = 128,
 58 | 	PRESENT_POSITION       = 132,
 59 | 	VELOCITY_TRAJECTORY    = 136,
 60 | 	POSITION_TRAJECTORY    = 140,
 61 | 	PRESENT_INPUT_VOLTAGE  = 144,
 62 | 	PRESENT_TEMPERATURE    = 146,
 63 | 	INDIRECT_ADDRESS_BLOCK1 = 168,
 64 | 	INDIRECT_DATA_BLOCK1    = 224,
 65 | 	INDIRECT_ADDRESS_BLOCK2 = 578,
 66 | 	INDIRECT_DATA_BLOCK2    = 634,
 67 | };
 68 | constexpr Register operator+(Register t1, size_t t2) {
 69 | 	return Register(size_t(t1) + t2);
 70 | }
 71 | 
 72 | struct MotorLayoutInfo {
 73 | 	static constexpr LayoutType Type{LayoutType::MX_V2};
 74 | 	using FullLayout = Layout<Register::MODEL_NUMBER, 147>;
 75 | 
 76 | 	static auto getInfos()    -> meta::Layout<Register> const&;
 77 | 	static auto getDefaults() -> std::map<uint32_t, meta::Info<Register>> const&;
 78 | };
 79 | 
 80 | using FullLayout = MotorLayoutInfo::FullLayout;
 81 | 
 82 | using IndirectAddresses = std::array<uint16_t, 28>;
 83 | using IndirectData      = std::array<uint8_t, 28>;
 84 | 
 85 | }
 86 | 
 87 | namespace dynamixel {
 88 | 
 89 | template <> struct meta::MotorLayoutInfo<LayoutType::MX_V2> : mx_v2::MotorLayoutInfo {};
 90 | 
 91 | #pragma pack(push, 1)
 92 | DynamixelLayoutPart(mx_v2::Register::MODEL_NUMBER           , uint16_t, model_number           );
 93 | DynamixelLayoutPart(mx_v2::Register::MODEL_INFORMATION      , uint32_t, model_information      );
 94 | DynamixelLayoutPart(mx_v2::Register::VERSION_FIRMWARE       ,  uint8_t, version_firmware       );
 95 | DynamixelLayoutPart(mx_v2::Register::ID                     ,  uint8_t, id                     );
 96 | DynamixelLayoutPart(mx_v2::Register::BAUD_RATE              ,  uint8_t, baud_rate              );
 97 | DynamixelLayoutPart(mx_v2::Register::RETURN_DELAY_TIME      ,  uint8_t, return_delay_time      );
 98 | DynamixelLayoutPart(mx_v2::Register::DRIVE_MODE             ,  uint8_t, drive_mode             );
 99 | DynamixelLayoutPart(mx_v2::Register::OPERATING_MODE         ,  uint8_t, operating_mode         );
100 | DynamixelLayoutPart(mx_v2::Register::SECONDARY_ID           ,  uint8_t, secondary_id           );
101 | DynamixelLayoutPart(mx_v2::Register::PROTOCOL_VERSION       ,  uint8_t, protocol_version       );
102 | DynamixelLayoutPart(mx_v2::Register::HOMING_OFFSET          ,  int32_t, homing_offset          );
103 | DynamixelLayoutPart(mx_v2::Register::MOVING_THRESHOLD       ,  int32_t, moving_threshold       );
104 | DynamixelLayoutPart(mx_v2::Register::TEMPERATURE_LIMIT      ,  uint8_t, temperature_limit      );
105 | DynamixelLayoutPart(mx_v2::Register::MAX_VOLTAGE_LIMIT      , uint16_t, max_voltage_limit      );
106 | DynamixelLayoutPart(mx_v2::Register::MIN_VOLTAGE_LIMIT      , uint16_t, min_voltage_limit      );
107 | DynamixelLayoutPart(mx_v2::Register::PWM_LIMIT              , uint16_t, pwm_limit              );
108 | DynamixelLayoutPart(mx_v2::Register::CURRENT_LIMIT          , uint16_t, current_limit          );
109 | DynamixelLayoutPart(mx_v2::Register::ACCELERATION_LIMIT     , uint32_t, acceleration_limit     );
110 | DynamixelLayoutPart(mx_v2::Register::VELOCITY_LIMIT         , uint32_t, velocity_limit         );
111 | DynamixelLayoutPart(mx_v2::Register::MAX_POSITION_LIMIT     ,  int32_t, max_position_limit     );
112 | DynamixelLayoutPart(mx_v2::Register::MIN_POSITION_LIMIT     ,  int32_t, min_position_limit     );
113 | DynamixelLayoutPart(mx_v2::Register::EXTERNAL_PORT_MODE_1   ,  uint8_t, external_port_mode_1   );
114 | DynamixelLayoutPart(mx_v2::Register::EXTERNAL_PORT_MODE_2   ,  uint8_t, external_port_mode_2   );
115 | DynamixelLayoutPart(mx_v2::Register::EXTERNAL_PORT_MODE_3   ,  uint8_t, external_port_mode_3   );
116 | DynamixelLayoutPart(mx_v2::Register::SHUTDOWN               ,  uint8_t, shutdown               );
117 | DynamixelLayoutPart(mx_v2::Register::TORQUE_ENABLE          ,     bool, torque_enable          );
118 | DynamixelLayoutPart(mx_v2::Register::LED                    ,     bool, led                    );
119 | DynamixelLayoutPart(mx_v2::Register::STATUS_RETURN_LEVEL    ,  uint8_t, status_return_level    );
120 | DynamixelLayoutPart(mx_v2::Register::REGISTERED_INSTRUCTION ,  uint8_t, registered_instruction );
121 | DynamixelLayoutPart(mx_v2::Register::HARDWARE_ERROR_STATUS  ,  uint8_t, hardware_error_status  );
122 | DynamixelLayoutPart(mx_v2::Register::VELOCITY_I_GAIN        , uint16_t, velocity_i_gain        );
123 | DynamixelLayoutPart(mx_v2::Register::VELOCITY_P_GAIN        , uint16_t, velocity_p_gain        );
124 | DynamixelLayoutPart(mx_v2::Register::POSITION_D_GAIN        , uint16_t, position_d_gain        );
125 | DynamixelLayoutPart(mx_v2::Register::POSITION_I_GAIN        , uint16_t, position_i_gain        );
126 | DynamixelLayoutPart(mx_v2::Register::POSITION_P_GAIN        , uint16_t, position_p_gain        );
127 | DynamixelLayoutPart(mx_v2::Register::FEEDFORWARD_2ND_GAIN   , uint16_t, feedforward_2nd_gain   );
128 | DynamixelLayoutPart(mx_v2::Register::FEEDFORWARD_1ST_GAIN   , uint16_t, feedforward_1st_gain   );
129 | DynamixelLayoutPart(mx_v2::Register::BUS_WATCHDOG           ,  uint8_t, bus_watchdog           );
130 | DynamixelLayoutPart(mx_v2::Register::GOAL_PWM               , uint16_t, goal_pwm               );
131 | DynamixelLayoutPart(mx_v2::Register::GOAL_CURRENT           ,  int16_t, goal_current           );
132 | DynamixelLayoutPart(mx_v2::Register::GOAL_VELOCITY          ,  int32_t, goal_velocity          )
133 | DynamixelLayoutPart(mx_v2::Register::PROFILE_ACCELERATION   ,  int32_t, profile_acceleration   )
134 | DynamixelLayoutPart(mx_v2::Register::PROFILE_VELOCITY       ,  int32_t, profile_velocity       )
135 | DynamixelLayoutPart(mx_v2::Register::GOAL_POSITION          ,  int32_t, goal_position          )
136 | DynamixelLayoutPart(mx_v2::Register::REALTIME_TICK          , uint16_t, realtime_tick          )
137 | DynamixelLayoutPart(mx_v2::Register::MOVING                 ,     bool, moving                 )
138 | DynamixelLayoutPart(mx_v2::Register::MOVING_STATUS          ,  uint8_t, moving_status          )
139 | DynamixelLayoutPart(mx_v2::Register::PRESENT_PWM            , uint16_t, present_pwm            )
140 | DynamixelLayoutPart(mx_v2::Register::PRESENT_CURRENT        ,  int16_t, present_current        )
141 | DynamixelLayoutPart(mx_v2::Register::PRESENT_VELOCITY       ,  int32_t, present_velocity       )
142 | DynamixelLayoutPart(mx_v2::Register::PRESENT_POSITION       ,  int32_t, present_position       )
143 | DynamixelLayoutPart(mx_v2::Register::VELOCITY_TRAJECTORY    ,  int32_t, velocity_trajectory    )
144 | DynamixelLayoutPart(mx_v2::Register::POSITION_TRAJECTORY    ,  int32_t, position_trajectory    )
145 | DynamixelLayoutPart(mx_v2::Register::PRESENT_INPUT_VOLTAGE  , uint16_t, present_input_voltage  );
146 | DynamixelLayoutPart(mx_v2::Register::PRESENT_TEMPERATURE    ,  uint8_t, present_temperature    );
147 | DynamixelLayoutPart(mx_v2::Register::INDIRECT_ADDRESS_BLOCK1 , mx_v2::IndirectAddresses, indirect_address_block1);
148 | DynamixelLayoutPart(mx_v2::Register::INDIRECT_DATA_BLOCK1  ,   mx_v2::IndirectData,      indirect_data_block1);
149 | DynamixelLayoutPart(mx_v2::Register::INDIRECT_ADDRESS_BLOCK2 , mx_v2::IndirectAddresses, indirect_address_block2);
150 | DynamixelLayoutPart(mx_v2::Register::INDIRECT_DATA_BLOCK2 ,    mx_v2::IndirectData,      indirect_data_block2);
151 | 
152 | #pragma pack(pop)
153 | }
154 | 


--------------------------------------------------------------------------------
/src/simplyfuse/FuseFS.cpp:
--------------------------------------------------------------------------------
  1 | #include "FuseFS.h"
  2 | 
  3 | #define FUSE_USE_VERSION 31
  4 | #include <fuse.h>
  5 | #include <fuse/fuse_lowlevel.h>
  6 | 
  7 | #include <stdexcept>
  8 | #include <array>
  9 | #include <map>
 10 | #include <mutex>
 11 | #include <cstring>
 12 | #include <iostream>
 13 | #include <vector>
 14 | #include <algorithm>
 15 | 
 16 | #include <filesystem>
 17 | 
 18 | namespace simplyfuse {
 19 | namespace {
 20 | 
 21 | struct Node {
 22 | 	Node(std::string const& _name) : name(_name) {}
 23 | 	Node* parent {nullptr};
 24 | 
 25 | 	std::map<std::string, std::unique_ptr<Node>> children;
 26 | 	FuseFile* file {nullptr};
 27 | 	std::string name;
 28 | 
 29 | 	~Node() {}
 30 | 	auto getOrCreateChild(std::string const& name) -> std::pair<Node*, bool> {
 31 | 		auto& ptr = children[name];
 32 | 		bool needCreation = not ptr;
 33 | 		if (needCreation) {
 34 | 			ptr = std::make_unique<Node>(name);
 35 | 			ptr->parent = this;
 36 | 		}
 37 | 		return std::make_pair(ptr.get(), needCreation);
 38 | 	}
 39 | };
 40 | 
 41 | void destroyNode(Node* node) {
 42 | 	Node* parent = node->parent;
 43 | 	parent->children.erase(node->name);
 44 | }
 45 | 
 46 | static int getattr_callback(const char *path, struct stat *stbuf);
 47 | static int readdir_callback(const char *path, void *buf, fuse_fill_dir_t filler, off_t offset, struct fuse_file_info *fi);
 48 | static int open_callback(const char *path, struct fuse_file_info *fi);
 49 | static int read_callback(const char *path, char *buf, size_t size, off_t offset, struct fuse_file_info *fi);
 50 | static int write_callback(const char *path, const char *buf, size_t size, off_t offset, struct fuse_file_info *fi);
 51 | static int truncate_callback(const char *path, off_t offset);
 52 | 
 53 | }
 54 | 
 55 | struct FuseFS::Pimpl {
 56 | 	struct fuse_chan* channel {nullptr};
 57 | 	struct fuse* fuse {nullptr};
 58 | 	std::filesystem::path mountPoint;
 59 | 	int fuseFD {0};
 60 | 
 61 | 
 62 | 	std::recursive_mutex mutex;
 63 | 	std::map<Node*, FuseFile*> files;
 64 | 	std::multimap<FuseFile*, Node*> filesInvMap;
 65 | 
 66 | 	Node root{"/"};
 67 | 
 68 | 	bool tearDownMountPoint {false};
 69 | 
 70 | 	Node* getNode(std::filesystem::path const& path) {
 71 | 		if (not path.is_absolute()) {
 72 | 			throw InvalidPathError("path must be absolute");
 73 | 		}
 74 | 		std::lock_guard lock{mutex};
 75 | 		Node* node = &root;
 76 | 		for (auto it = std::next(path.begin()); it != path.end(); ++it) {
 77 | 			auto childit = node->children.find(*it);
 78 | 			if (childit == node->children.end()) {
 79 | 				return nullptr;
 80 | 			}
 81 | 			node = childit->second.get();
 82 | 		}
 83 | 		return node;
 84 | 	}
 85 | 
 86 | 	Node* getOrCreateNode(std::filesystem::path const& path) {
 87 | 		if (not path.is_absolute()) {
 88 | 			throw InvalidPathError("path must be absolute");
 89 | 		}
 90 | 		std::lock_guard lock{mutex};
 91 | 		Node* node = &root;
 92 | 		std::vector<Node*> createdNodes;
 93 | 		for (auto it = std::next(path.begin()); it != path.end(); ++it) {
 94 | 			auto goc = node->getOrCreateChild(*it);
 95 | 			node = goc.first;
 96 | 			if (goc.second) {
 97 | 				createdNodes.emplace_back(node);
 98 | 			}
 99 | 			if (node->file) {
100 | 				// unwind the inserts
101 | 				for (auto cleanupIt = createdNodes.rbegin(); cleanupIt != createdNodes.rend(); ++cleanupIt) {
102 | 					destroyNode(*cleanupIt);
103 | 				}
104 | 				throw InvalidPathError("path is invalid");
105 | 			}
106 | 		}
107 | 		return node;
108 | 	}
109 | 
110 | 	void deleteNode(Node* node) {
111 | 		if (not node->children.empty()) {
112 | 			throw std::logic_error("cannot destroy a node which has children");
113 | 		}
114 | 		if (node->file) {
115 | 			auto range = filesInvMap.equal_range(node->file);
116 | 			if (std::distance(range.first, range.second) == 1) {
117 | 				node->file->fuseFS = nullptr;
118 | 			}
119 | 			filesInvMap.erase(std::find_if(range.first, range.second, [&](auto const& p) { return p.second == node; }));
120 | 			files.erase(node);
121 | 		}
122 | 		destroyNode(node);
123 | 	}
124 | };
125 | 
126 | FuseFS::FuseFS(std::filesystem::path const& mountPoint) :
127 | 		pimpl { std::make_unique<Pimpl>() } {
128 | 	pimpl->mountPoint = mountPoint;
129 | 
130 | 	if (not std::filesystem::is_directory(mountPoint)) {
131 | 		// try to create the mountpoint
132 | 		std::filesystem::create_directory(mountPoint);
133 | 		pimpl->tearDownMountPoint = true;
134 | 	}
135 | 
136 | 	std::vector<std::string> argStrings {"", "-oauto_unmount"};
137 | 	char* args[] = {
138 | 			argStrings[0].data(), argStrings[1].data()
139 | 	};
140 | 	struct fuse_args fuse_args = {2, args, false};
141 | 	pimpl->channel = fuse_mount(pimpl->mountPoint.c_str(), &fuse_args);
142 | 	if (not pimpl->channel) {
143 | 		throw MountError("cannot mount");
144 | 	}
145 | 	struct fuse_operations fuse_operations = { };
146 | 	fuse_operations.getattr = getattr_callback;
147 | 	fuse_operations.open = open_callback;
148 | 	fuse_operations.read = read_callback;
149 | 	fuse_operations.write = write_callback;
150 | 	fuse_operations.readdir = readdir_callback;
151 | 	fuse_operations.truncate = truncate_callback;
152 | 	pimpl->fuse = fuse_new(pimpl->channel, nullptr, &fuse_operations,
153 | 			sizeof(fuse_operations), this);
154 | 	pimpl->fuseFD = fuse_chan_fd(pimpl->channel);
155 | }
156 | 
157 | FuseFS::~FuseFS() {
158 | 	fuse_unmount(pimpl->mountPoint.c_str(), pimpl->channel);
159 | 	fuse_destroy(pimpl->fuse);
160 | 
161 | 	if (pimpl->tearDownMountPoint) {
162 | 		std::filesystem::remove(pimpl->mountPoint);
163 | 	}
164 | }
165 | int FuseFS::getFD() const {
166 | 	return pimpl->fuseFD;
167 | }
168 | 
169 | void FuseFS::loop() {
170 | 	struct fuse_cmd *cmd = fuse_read_cmd(pimpl->fuse);
171 | 	if (cmd) {
172 | 		fuse_process_cmd(pimpl->fuse, cmd);
173 | 	}
174 | }
175 | 
176 | void FuseFS::registerFile(std::filesystem::path const& _path, FuseFile& file) {
177 | 	std::filesystem::path path = _path.lexically_normal();
178 | 	if (file.fuseFS and file.fuseFS != this) {
179 | 		throw std::invalid_argument("the passed file is already registered at a different fuse");
180 | 	}
181 | 	std::lock_guard lock{pimpl->mutex};
182 | 	Node* node = pimpl->getOrCreateNode(path);
183 | 	node->file = &file;
184 | 	file.fuseFS = this;
185 | 	if (not node->children.empty()) {
186 | 		throw InvalidPathError("file already exists");
187 | 	}
188 | 	pimpl->files[node] = &file;
189 | 	pimpl->filesInvMap.emplace(&file, node);
190 | }
191 | 
192 | void FuseFS::unregisterFile(FuseFile& file) {
193 | 	if (file.fuseFS != this) {
194 | 		throw std::invalid_argument("the passed file is not registered with this fuse instance");
195 | 	}
196 | 	std::lock_guard lock{pimpl->mutex};
197 | 	file.fuseFS = nullptr;
198 | 	auto range = pimpl->filesInvMap.equal_range(&file);
199 | 	std::for_each(range.first, range.second, [=] (auto const& p) {
200 | 		pimpl->files.erase(p.second);
201 | 		destroyNode(p.second);
202 | 	});
203 | 	pimpl->filesInvMap.erase(range.first, range.second);
204 | }
205 | 
206 | void FuseFS::unregisterFile(std::filesystem::path const& path, FuseFile& file) {
207 | 	if (file.fuseFS != this) {
208 | 		throw std::invalid_argument("the passed file is not registered with this fuse instance");
209 | 	}
210 | 
211 | 	std::lock_guard lock{pimpl->mutex};
212 | 	Node* node = pimpl->getNode(path);
213 | 	pimpl->deleteNode(node);
214 | }
215 | 
216 | void FuseFS::mkdir(std::filesystem::path const& _path) {
217 | 	std::filesystem::path path = _path.lexically_normal();
218 | 	std::lock_guard lock{pimpl->mutex};
219 | 	pimpl->getOrCreateNode(path);
220 | }
221 | 
222 | namespace {
223 | 
224 | void rmDirHelper(Node* node, FuseFS::Pimpl* pimpl) {
225 | 	if (not node) {
226 | 		return;
227 | 	}
228 | 	std::vector<Node*> children;
229 | 	std::transform(node->children.begin(), node->children.end(), std::back_inserter(children), [](auto const& p) { return p.second.get(); });
230 | 	for (auto & child : children) {
231 | 		rmDirHelper(child, pimpl);
232 | 	}
233 | 	pimpl->deleteNode(node);
234 | }
235 | 
236 | }
237 | 
238 | void FuseFS::rmdir(std::filesystem::path const& path) {
239 | 	Node* node = pimpl->getNode(path);
240 | 	if (node) {
241 | 		rmDirHelper(node, pimpl.get());
242 | 	}
243 | }
244 | 
245 | namespace {
246 | 
247 | Node* getNode(std::string const& path) {
248 | 	struct fuse_context* context = fuse_get_context();
249 | 	FuseFS* fusefs = reinterpret_cast<FuseFS*>(context->private_data);
250 | 	return fusefs->pimpl->getNode(path);
251 | }
252 | 
253 | int getattr_callback(const char *path, struct stat *stbuf) {
254 | 	memset(stbuf, 0, sizeof(*stbuf));
255 | 
256 | 	Node* node = getNode(path);
257 | 	if (not node) {
258 | 		return -ENOENT;
259 | 	}
260 | 	clock_gettime(CLOCK_REALTIME, &stbuf->st_mtim);
261 | 	if (node->file) {
262 | 		stbuf->st_mode = S_IFREG | node->file->getFilePermissions();
263 | 		stbuf->st_nlink = 1;
264 | 		stbuf->st_size = node->file->getSize();
265 | 		return 0;
266 | 	} else {
267 | 		stbuf->st_mode = S_IFDIR | 0755;
268 | 		stbuf->st_nlink = 2;
269 | 		return 0;
270 | 	}
271 | 	return -ENOENT;
272 | }
273 | 
274 | int readdir_callback(const char *path, void *buf, fuse_fill_dir_t filler, off_t, struct fuse_file_info *) {
275 | 	Node* node = getNode(path);
276 | 	if (not node) {
277 | 		return -ENOENT;
278 | 	}
279 | 	for (auto& child : node->children) {
280 | 		filler(buf, child.first.c_str(), NULL, 0);
281 | 	}
282 | 
283 | 	filler(buf, ".", NULL, 0);
284 | 	filler(buf, "..", NULL, 0);
285 | 	return 0;
286 | }
287 | 
288 | int open_callback(const char *path, struct fuse_file_info *) {
289 | 	Node* node = getNode(path);
290 | 	if (not node or not node->file) {
291 | 		return -ENOENT;
292 | 	}
293 | 	return node->file->onOpen();
294 | }
295 | 
296 | int read_callback(const char *path, char *buf, size_t size, off_t offset, struct fuse_file_info *) {
297 | 	Node* node = getNode(path);
298 | 	if (not node or not node->file) {
299 | 		return -ENOENT;
300 | 	}
301 | 	return node->file->onRead(buf, size, offset);
302 | }
303 | 
304 | int write_callback(const char *path, const char *buf, size_t size, off_t offset, struct fuse_file_info *) {
305 | 	Node* node = getNode(path);
306 | 	if (not node or not node->file) {
307 | 		return -ENOENT;
308 | 	}
309 | 	return node->file->onWrite(buf, size, offset);
310 | }
311 | 
312 | int truncate_callback(const char *path, off_t offset) {
313 | 	Node* node = getNode(path);
314 | 	if (not node or not node->file) {
315 | 		return -ENOENT;
316 | 	}
317 | 	return node->file->onTruncate(offset);
318 | }
319 | 
320 | 
321 | }
322 | 
323 | }
324 | 


--------------------------------------------------------------------------------
/src/usb2dynamixel/LayoutMX_V1.cpp:
--------------------------------------------------------------------------------
  1 | #include "LayoutMX_V1.h"
  2 | 
  3 | namespace dynamixel::mx_v1 {
  4 | 
  5 | auto MotorLayoutInfo::getInfos() -> meta::Layout<Register> const& {
  6 | 	using A = meta::LayoutField::Access;
  7 | 	static auto data = meta::Layout<Register> {
  8 | 		{Register::MODEL_NUMBER        ,{2,  true, A:: R, "Model Number", "model number"}},
  9 | 		{Register::VERSION_FIRMWARE    ,{1,  true, A:: R, "Version of Firmware", "Information on the version of firmware"}},
 10 | 		{Register::ID                  ,{1,  true, A::RW, "ID", "ID of Dynamixel"}},
 11 | 		{Register::BAUD_RATE           ,{1,  true, A::RW, "Baud Rate", "Baud Rate of Dynamixel"}},
 12 | 		{Register::RETURN_DELAY_TIME   ,{1,  true, A::RW, "Return Delay Time", "Return Delay Time"}},
 13 | 		{Register::CW_ANGLE_LIMIT      ,{2,  true, A::RW, "CW Angle Limit", "clockwise Angle Limit"}},
 14 | 		{Register::CCW_ANGLE_LIMIT     ,{2,  true, A::RW, "CCW Angle Limit", "counterclockwise Angle Limit"}},
 15 | 		{Register::DRIVE_MODE          ,{1,  true, A::RW, "Drive Mode", "Dual Mode Setting"}},
 16 | 		{Register::TEMPERATURE_LIMIT   ,{1,  true, A::RW, "Highest Limit Temperature", "Internal Limit Temperature"}},
 17 | 		{Register::VOLTAGE_LIMIT_LOW   ,{1,  true, A::RW, "Lowest Limit Voltage", "Lowest Limit Voltage"}},
 18 | 		{Register::VOLTAGE_LIMIT_HIGH  ,{1,  true, A::RW, "Highest Limit Voltage", "Highest Limit Voltage"}},
 19 | 		{Register::MAX_TORQUE          ,{2,  true, A::RW, "Max Torque", "Max. Torque"}},
 20 | 		{Register::STATUS_RETURN_LEVEL ,{1,  true, A::RW, "Status Return Level", "Status Return Level"}},
 21 | 		{Register::ALARM_LED           ,{1,  true, A::RW, "Alarm LED", "LED for Alarm"}},
 22 | 		{Register::ALARM_SHUTDOWN      ,{1,  true, A::RW, "Alarm Shutdown", "Shutdown for Alarm"}},
 23 | 		{Register::MULTI_TURN_OFFSET   ,{2,  true, A::RW, "Multi Turn Offset", "multi{}turn offset"}},
 24 | 		{Register::RESOLUTION_DIVIDER  ,{1,  true, A::RW, "Resolution Divider", "Resolution divider"}},
 25 | 		{Register::TORQUE_ENABLE       ,{1, false, A::RW, "Torque Enable", "Torque On/Off"}},
 26 | 		{Register::LED                 ,{1, false, A::RW, "LED", "LED On/Off"}},
 27 | 		{Register::D_GAIN              ,{1, false, A::RW, "D Gain", "Derivative Gain"}},
 28 | 		{Register::I_GAIN              ,{1, false, A::RW, "I Gain", "Integral Gain"}},
 29 | 		{Register::P_GAIN              ,{1, false, A::RW, "P Gain", "Proportional Gain"}},
 30 | 		{Register::GOAL_POSITION       ,{2, false, A::RW, "Goal Position", "Goal Position"}},
 31 | 		{Register::MOVING_SPEED        ,{2, false, A::RW, "Moving Speed", "Moving Speed (Moving Velocity)"}},
 32 | 		{Register::TORQUE_LIMIT        ,{2, false, A::RW, "Torque Limit", "Torque Limit (Goal Torque)"}},
 33 | 		{Register::PRESENT_POSITION    ,{2, false, A:: R, "Present Position", "Current Position (Present Velocity)"}},
 34 | 		{Register::PRESENT_SPEED       ,{2, false, A:: R, "Present Speed", "Current Speed"}},
 35 | 		{Register::PRESENT_LOAD        ,{2, false, A:: R, "Present Load", "Current Load"}},
 36 | 		{Register::PRESENT_VOLTAGE     ,{1, false, A:: R, "Present Voltage", "Current Voltage"}},
 37 | 		{Register::PRESENT_TEMPERATURE ,{1, false, A:: R, "Present Temperature", "Current Temperature"}},
 38 | 		{Register::REGISTERED          ,{1, false, A:: R, "Registered", "Means if Instruction is registered"}},
 39 | 		{Register::MOVING              ,{1, false, A:: R, "Moving", "Means if there is any movement"}},
 40 | 		{Register::LOCK                ,{1, false, A::RW, "Lock", "Locking EEPROM"}},
 41 | 		{Register::PUNCH               ,{2, false, A::RW, "Punch", "Punch"}},
 42 | 		{Register::REALTIME_TICK       ,{2, false, A:: R, "Realtime Tick", "Realtime Tick"}},
 43 | 		{Register::CURRENT             ,{2, false, A:: R, "Current", "Consuming Current"}},
 44 | 		{Register::TORQUE_CONTROL_MODE ,{1, false, A::RW, "Torque Control Mode Enable", "Torque control mode on/off"}},
 45 | 		{Register::GOAL_TORQUE         ,{2, false, A::RW, "Goal Torque", "goal torque value"}},
 46 | 		{Register::GOAL_ACCELERATION   ,{1, false, A::RW, "Goal Acceleration", "Goal Acceleration"}},
 47 | 	};
 48 | 	return data;
 49 | }
 50 | 
 51 | auto MotorLayoutInfo::getDefaults() -> std::map<uint32_t, meta::Info<Register>> const& {
 52 | 	static auto data = []() {
 53 | 		auto convertPosition    = meta::buildConverter("r", (2.*M_PI)/4095, 2048);
 54 | 		auto convertSpeed       = [](std::string unit, double resolution) {
 55 | 									return meta::Convert {
 56 | 										unit,
 57 | 										[=](double val) { return std::max(1., std::round(val / resolution)); },
 58 | 										[=](int val)    {
 59 | 											if (val & (1<<10)) {
 60 | 												val = - (val & ((1<<10)-1));
 61 | 											}
 62 | 											return val * resolution;
 63 | 										},
 64 | 									};
 65 | 								}("r/s", (116.62/60*2.*M_PI)/1023.);
 66 | 		auto convertTemperature = meta::buildConverter("C", 1.);
 67 | 		auto convertVoltage     = meta::buildConverter("V", 16./160);
 68 | 		auto convertPID_P       = meta::buildConverter("", 1./8., 0, 0, 254);
 69 | 		auto convertPID_I       = meta::buildConverter("", 1000./2048., 0, 0, 254);
 70 | 		auto convertPID_D       = meta::buildConverter("", 4/1000., 0, 0, 254);
 71 | 		auto convertCurrent     = meta::buildConverter("A", 4.5/1000., 2048);
 72 | 		auto convertTorque      = meta::buildConverter("%", 100./1023., 0);
 73 | 
 74 | 		auto data = std::map<uint32_t, meta::Info<Register>> {
 75 | 			{320, {
 76 | 				320,
 77 | 				LayoutType::MX_V1,
 78 | 				"MX106",
 79 | 				{"MX-106T", "MX-106R"}, {
 80 | 					{Register::MODEL_NUMBER        , {          320, {}}},
 81 | 					{Register::VERSION_FIRMWARE    , { std::nullopt, {}}},
 82 | 					{Register::ID                  , {            1, {}}},
 83 | 					{Register::BAUD_RATE           , {           34, {}}},
 84 | 					{Register::RETURN_DELAY_TIME   , {          250, {}}},
 85 | 					{Register::CW_ANGLE_LIMIT      , {            0, convertPosition}},
 86 | 					{Register::CCW_ANGLE_LIMIT     , {       0x0fff, convertPosition}},
 87 | 					{Register::DRIVE_MODE          , {            0, {}}},
 88 | 					{Register::TEMPERATURE_LIMIT   , {           80, convertTemperature}},
 89 | 					{Register::VOLTAGE_LIMIT_LOW   , {           60, convertVoltage}},
 90 | 					{Register::VOLTAGE_LIMIT_HIGH  , {          160, convertVoltage}},
 91 | 					{Register::MAX_TORQUE          , {       0x03ff, convertTorque}},
 92 | 					{Register::STATUS_RETURN_LEVEL , {            2, {}}},
 93 | 					{Register::ALARM_LED           , {           36, {}}},
 94 | 					{Register::ALARM_SHUTDOWN      , {           36, {}}},
 95 | 					{Register::MULTI_TURN_OFFSET   , {            0, {}}},
 96 | 					{Register::RESOLUTION_DIVIDER  , {            1, {}}},
 97 | 					{Register::TORQUE_ENABLE       , {            0, {}}},
 98 | 					{Register::LED                 , {            0, {}}},
 99 | 					{Register::D_GAIN              , {            0, convertPID_D}},
100 | 					{Register::I_GAIN              , {            0, convertPID_I}},
101 | 					{Register::P_GAIN              , {           32, convertPID_P}},
102 | 					{Register::GOAL_POSITION       , { std::nullopt, convertPosition}},
103 | 					{Register::MOVING_SPEED        , { std::nullopt, convertSpeed}},
104 | 					{Register::TORQUE_LIMIT        , { std::nullopt, convertTorque}},
105 | 					{Register::PRESENT_POSITION    , { std::nullopt, convertPosition}},
106 | 					{Register::PRESENT_SPEED       , { std::nullopt, convertSpeed}},
107 | 					{Register::PRESENT_LOAD        , { std::nullopt, {}}},
108 | 					{Register::PRESENT_VOLTAGE     , { std::nullopt, convertVoltage}},
109 | 					{Register::PRESENT_TEMPERATURE , { std::nullopt, convertTemperature}},
110 | 					{Register::REGISTERED          , {            0, {}}},
111 | 					{Register::MOVING              , {            0, {}}},
112 | 					{Register::LOCK                , {            0, {}}},
113 | 					{Register::PUNCH               , {            0, {}}},
114 | 					{Register::REALTIME_TICK       , {            0, {}}},
115 | 					{Register::CURRENT             , {         2048, convertCurrent}},
116 | 					{Register::TORQUE_CONTROL_MODE , {            0, {}}},
117 | 					{Register::GOAL_TORQUE         , {            0, convertTorque}},
118 | 					{Register::GOAL_ACCELERATION   , {            0, {}}},
119 | 				}
120 | 			}}
121 | 		};
122 | 		auto newMotor = [&](int number, std::string shortName, std::vector<std::string> names) -> meta::Info<Register>& {
123 | 			auto& m = data[number];
124 | 			m = data.at(320);
125 | 			m.modelNumber = number;
126 | 			m.shortName = std::move(shortName);
127 | 			m.motorNames = std::move(names);
128 | 			std::get<0>(m.defaultLayout[Register::MODEL_NUMBER]) = number;
129 | 			return m;
130 | 		};
131 | 
132 | 		{
133 | 			auto& m = newMotor(310, "MX64", {"MX-64T", "MX-64R", "MX-64AT", "MX-64AR"});
134 | 			m.defaultLayout.erase(Register::DRIVE_MODE);
135 | 		}
136 | 		{
137 | 			auto& m = newMotor(29, "MX28", {"MX-28T", "MX-28R", "MX-28AT", "MX-28AR"});
138 | 			m.defaultLayout.erase(Register::DRIVE_MODE);
139 | 			m.defaultLayout.erase(Register::TORQUE_CONTROL_MODE);
140 | 			m.defaultLayout.erase(Register::CURRENT);
141 | 			m.defaultLayout.erase(Register::GOAL_TORQUE);
142 | 		}
143 | 		{
144 | 			auto& m = newMotor(360, "MX12", {"MX-12W"});
145 | 			auto convertSpeed       = [](std::string unit, double resolution) {
146 | 											return meta::Convert {
147 | 												unit,
148 | 												[=](double val) { return std::max(1., std::round(val / resolution)); },
149 | 												[=](int val)    {
150 | 													if (val & (1<<10)) {
151 | 														val = - (val & ((1<<10)-1));
152 | 													}
153 | 													return val * resolution;
154 | 												},
155 | 											};
156 | 										}("r/s", (937.1/60*2.*M_PI)/1023.);
157 | 
158 | 			std::get<1>(m.defaultLayout[Register::MOVING_SPEED])  = convertSpeed;
159 | 			std::get<1>(m.defaultLayout[Register::PRESENT_SPEED]) = convertSpeed;
160 | 
161 | 			std::get<0>(m.defaultLayout[Register::MODEL_NUMBER]) = 360;
162 | 			std::get<0>(m.defaultLayout[Register::BAUD_RATE])    = 1;
163 | 			std::get<0>(m.defaultLayout[Register::D_GAIN])       = 8;
164 | 			std::get<0>(m.defaultLayout[Register::P_GAIN])       = 8;
165 | 			std::get<0>(m.defaultLayout[Register::PUNCH])        = 32;
166 | 			m.defaultLayout.erase(Register::DRIVE_MODE);
167 | 			m.defaultLayout.erase(Register::CURRENT);
168 | 			m.defaultLayout.erase(Register::TORQUE_CONTROL_MODE);
169 | 			m.defaultLayout.erase(Register::GOAL_TORQUE);
170 | 		}
171 | 		return data;
172 | 	}();
173 | 	return data;
174 | };
175 | 
176 | 
177 | }
178 | 


--------------------------------------------------------------------------------