1 #include <visp3/core/vpColVector.h>
2 #include <visp3/core/vpSerial.h>
3 #include <visp3/core/vpTime.h>
4
main(int argc,char * argv[])5 int 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" << argv[0] << " --vx <linear velocity in m/s> --wz <rotational velocity in deg/s> --rpm_l <motor left RPM> --rpm_r <motor right RPM> --t <duration of the command in second> --help" << std::endl;
30 std::cout << "\nExample:\n" << argv[0] << " --vx 0.05 --wz 0 --t 4\n" << std::endl;
31 return 0;
32 }
33 }
34
35 vpSerial serial("/dev/ttyAMA0", 115200);
36
37 {
38 std::stringstream ss;
39 if (rpm_command) {
40 std::cout << "Apply rpm_l=" << rpm_l << " rpm_r=" << rpm_r << " during " << time << " seconds" << std::endl;
41 ss << "MOTOR_RPM=" << rpm_l << "," << rpm_r << "\n";
42 }
43 else {
44 vpColVector v(2);
45 v[0] = v_x;
46 v[1] = vpMath::rad(w_z);
47 std::cout << "Apply v_x=" << v_x << " m/s "<< " w_z=" << w_z << " deg/s during " << time << " seconds" << std::endl;
48 double radius = 0.0325;
49 double L = 0.0725;
50 double motor_left = -(v[0] + L * v[1]) / radius;
51 double motor_right = (v[0] - L * v[1]) / radius;
52 std::cout << "Motor left vel: " << motor_left << " motor right vel: " << motor_right << " (rad/s)" << std::endl;
53 double rpm_left = motor_left * 30. / M_PI;
54 double rpm_right = motor_right * 30. / M_PI;
55
56 ss << "MOTOR_RPM=" << (int)rpm_left << "," << (int)rpm_right << "\n";
57 }
58 std::cout << "Send: " << ss.str() << std::endl;
59 double t0 =vpTime::measureTimeSecond();
60 while(vpTime::measureTimeSecond() - t0 < time) {
61 serial.write(ss.str());
62 vpTime::wait(100);
63 }
64 return EXIT_SUCCESS;
65 }
66 serial.write("MOTOR_RPM=-100,100\n");
67 vpTime::sleepMs(500);
68 serial.write("MOTOR_RPM=-50,100\n");
69 vpTime::sleepMs(500);
70 serial.write("MOTOR_RPM=50,-50\n");
71 vpTime::sleepMs(500);
72 serial.write("LED_RING=0,0,10,0\n");
73 vpTime::sleepMs(500);
74 serial.write("LED_RING=0,0,0,10\n");
75 vpTime::sleepMs(500);
76 serial.write("LED_RING=0,0,0,0\n");
77 vpTime::sleepMs(500);
78 serial.close();
79 #else
80 (void) argc;
81 (void) argv;
82 std::cout << "Serial test is only working on unix-like OS." << std::endl;
83 #endif
84 return EXIT_SUCCESS;
85 }
86