Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
test-serial-mbot.cpp
1#include <visp3/core/vpColVector.h>
2#include <visp3/core/vpSerial.h>
3#include <visp3/core/vpTime.h>
4
5int main(int argc, char *argv[])
6{
7#if !defined(_WIN32)
8 double time = 4;
9 double v_x = 0;
10 double w_z = 0;
11 bool rpm_command = false;
12 int rpm_l = 0;
13 int rpm_r = 0;
14
15 for (int i = 1; i < argc; i++) {
16 if ((std::string(argv[i]) == "--t" || std::string(argv[i]) == "-t") && i + 1 < argc) {
17 time = (double)atof(argv[i + 1]);
18 } else if ((std::string(argv[i]) == "--vx" || std::string(argv[i]) == "-vx") && i + 1 < argc) {
19 v_x = (double)atof(argv[i + 1]);
20 } else if ((std::string(argv[i]) == "--wz" || std::string(argv[i]) == "-wz") && i + 1 < argc) {
21 w_z = (double)atof(argv[i + 1]);
22 } else if ((std::string(argv[i]) == "--rpm_l" || std::string(argv[i]) == "-rpm_l") && i + 1 < argc) {
23 rpm_command = true;
24 rpm_l = (double)atoi(argv[i + 1]);
25 } else if ((std::string(argv[i]) == "--rpm_r" || std::string(argv[i]) == "-rpm_r") && i + 1 < argc) {
26 rpm_command = true;
27 rpm_r = (double)atoi(argv[i + 1]);
28 } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
29 std::cout << "Usage: \n"
30 << argv[0]
31 << " --vx <linear velocity in m/s> --wz <rotational velocity in deg/s> --rpm_l <motor left RPM> "
32 "--rpm_r <motor right RPM> --t <duration of the command in second> --help"
33 << std::endl;
34 std::cout << "\nExample:\n" << argv[0] << " --vx 0.05 --wz 0 --t 4\n" << std::endl;
35 return EXIT_SUCCESS;
36 }
37 }
38
39 vpSerial serial("/dev/ttyAMA0", 115200);
40
41 {
42 std::stringstream ss;
43 if (rpm_command) {
44 std::cout << "Apply rpm_l=" << rpm_l << " rpm_r=" << rpm_r << " during " << time << " seconds" << std::endl;
45 ss << "MOTOR_RPM=" << rpm_l << "," << rpm_r << "\n";
46 } else {
47 vpColVector v(2);
48 v[0] = v_x;
49 v[1] = vpMath::rad(w_z);
50 std::cout << "Apply v_x=" << v_x << " m/s "
51 << " w_z=" << w_z << " deg/s during " << time << " seconds" << std::endl;
52 double radius = 0.0325;
53 double L = 0.0725;
54 double motor_left = -(v[0] + L * v[1]) / radius;
55 double motor_right = (v[0] - L * v[1]) / radius;
56 std::cout << "Motor left vel: " << motor_left << " motor right vel: " << motor_right << " (rad/s)" << std::endl;
57 double rpm_left = motor_left * 30. / M_PI;
58 double rpm_right = motor_right * 30. / M_PI;
59
60 ss << "MOTOR_RPM=" << (int)rpm_left << "," << (int)rpm_right << "\n";
61 }
62 std::cout << "Send: " << ss.str() << std::endl;
63 double t0 = vpTime::measureTimeSecond();
64 while (vpTime::measureTimeSecond() - t0 < time) {
65 serial.write(ss.str());
66 vpTime::wait(100);
67 }
68 return EXIT_SUCCESS;
69 }
70 serial.write("MOTOR_RPM=-100,100\n");
71 vpTime::sleepMs(500);
72 serial.write("MOTOR_RPM=-50,100\n");
73 vpTime::sleepMs(500);
74 serial.write("MOTOR_RPM=50,-50\n");
75 vpTime::sleepMs(500);
76 serial.write("LED_RING=0,0,10,0\n");
77 vpTime::sleepMs(500);
78 serial.write("LED_RING=0,0,0,10\n");
79 vpTime::sleepMs(500);
80 serial.write("LED_RING=0,0,0,0\n");
81 vpTime::sleepMs(500);
82 serial.close();
83#else
84 (void)argc;
85 (void)argv;
86 std::cout << "Serial test is only working on unix-like OS." << std::endl;
87#endif
88 return EXIT_SUCCESS;
89}
Implementation of column vector and the associated operations.
static double rad(double deg)
Definition vpMath.h:116
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeSecond()
VISP_EXPORT void sleepMs(double t)