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