Visual Servoing Platform  version 3.5.1 under development (2022-07-06)
test-serial-mbot.cpp
1 #include <visp3/core/vpColVector.h>
2 #include <visp3/core/vpSerial.h>
3 #include <visp3/core/vpTime.h>
4 
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"
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 0;
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.
Definition: vpColVector.h:131
static double rad(double deg)
Definition: vpMath.h:117
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeSecond()
VISP_EXPORT void sleepMs(double t)