libusb2ax/demo.cpp at master · civ0/libusb2ax · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
#include <chrono>
#include <cmath>
#include <cstdint>
#include <cstdlib>
#include <iomanip>
#include <iostream>
#include <map>
#include <string>
#include <vector>

#include "src/Controller/USB2AX.hpp"
#include "src/Exception/Exceptions.hpp"
#include "src/Protocol/Protocols.hpp"
#include "src/Servo/Servos.hpp"
#include "src/Servo/ServoManager.hpp"

namespace dy = Dynamixel;
namespace con = Dynamixel::Controller;
namespace sv = Dynamixel::Servo;
namespace ex = Dynamixel::Exception;

int main(int argc, char **argv)
{
	using p1 = Dynamixel::Protocol::Protocol1;

	try
	{
		std::vector<uint8_t> servoIDs{19, 20, 21};
		// std::vector<uint8_t> servoIDs;
		// for (uint8_t i = 1; i <= 18; ++i)
		// servoIDs.push_back(i);
		sv::ServoManager<sv::ManagedProtocol1Servo, p1> manager(argv[1]);
		for (auto id : servoIDs)
			manager.Servos.emplace(id, sv::ManagedProtocol1Servo(&manager, sv::Protocol1Model::Name::AX12, id));
		manager.StartUpdating();
		sleep(1);

		auto start = std::chrono::steady_clock::now();
		auto printServos = [&]() {
			auto now = std::chrono::steady_clock::now();
			std::cout << "t=" << std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count() << std::endl;
			for (const auto &id : servoIDs)
				std::cout << "\t" << static_cast<unsigned>(id)
						  << "; Pos: " << std::setprecision(5) << std::setw(7) << manager.Servos[id].PresentPosition.Get()
						  << "; Speed: " << std::setprecision(5) << std::setw(7) << manager.Servos[id].PresentSpeed.Get()
						  << "; Temp: " << static_cast<unsigned>(manager.Servos[id].PresentTemperature.Get())
						  << "; Load: " << std::setprecision(5) << std::setw(7) << manager.Servos[id].PresentLoad.Get()
						  << std::endl;
		};

		auto setPositions = [&](double degree) {
			for (const auto &id : servoIDs)
				manager.Servos[id].SetPosition(degree);
		};

		auto setSpeeds = [&](double speed) {
			for (const auto &id : servoIDs)
				manager.Servos[id].SetSpeed(speed);
		};

		for (int i = 0; i != 5; ++i)
		{
			setSpeeds(0.2);
			setPositions(-50.0);
			do
			{
				printServos();
				std::this_thread::sleep_for(std::chrono::milliseconds(100));
			} while (std::abs(manager.Servos[servoIDs[0]].PresentPosition.Get() + 50) > 1);

			setPositions(50.0);
			do
			{
				printServos();
				std::this_thread::sleep_for(std::chrono::milliseconds(100));
			} while (abs(manager.Servos[servoIDs[0]].PresentPosition.Get() - 50.0) > 1.0);
		}

		setPositions(0.0);
		do
		{
			printServos();
			std::this_thread::sleep_for(std::chrono::milliseconds(100));
		} while (abs(manager.Servos[servoIDs[0]].PresentPosition.Get() - 0.0) > 1.0);

		manager.StopUpdating();
	}
	catch (boost::exception &e)
	{
		if (std::string const *error = boost::get_error_info<ex::StringInfo>(e))
			std::cerr << "Error: " << *error << std::endl;

		return 1;
	}
}