Visual Servoing Platform  version 3.6.1 under development (2024-12-12)
vpPololu.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Common features for Pololu Maestro Servo Motor.
32  */
33 
34 #include <visp3/core/vpConfig.h>
35 
36 
37 #if defined(VISP_HAVE_POLOLU) && defined(VISP_HAVE_THREADS)
38 
39 #include <chrono>
40 #include <thread>
41 
42 #include <RPMSerialInterface.h>
43 
44 #include <visp3/robot/vpRobot.h>
45 #include <visp3/robot/vpRobotException.h>
46 #include <visp3/robot/vpPololu.h>
47 
48 std::chrono::milliseconds millis(1);
49 
50 BEGIN_VISP_NAMESPACE
51 RPMSerialInterface *vpPololu::m_interface = nullptr;
52 int vpPololu::m_nb_servo = 0;
53 
54 vpPololu::vpPololu(bool verbose)
55  : m_channel(0), m_apply_velocity_cmd(false), m_stop_velocity_cmd_thread(false),
56  m_vel_speed(1), m_vel_target_position(0), m_vel_speed_prev(1), m_vel_target_position_prev(0), m_mutex_velocity_cmd(), m_verbose(verbose)
57 { }
58 
59 vpPololu::vpPololu(const std::string &device, int baudrate, int channel, bool verbose)
60  : m_channel(channel), m_apply_velocity_cmd(false), m_stop_velocity_cmd_thread(false),
61  m_vel_speed(1), m_vel_target_position(0), m_vel_speed_prev(1), m_vel_target_position_prev(0), m_mutex_velocity_cmd(), m_verbose(verbose)
62 {
63  connect(device, baudrate, channel);
64 }
65 
66 void vpPololu::connect(const std::string &device, int baudrate, int channel)
67 {
68  std::string error_msg;
69  m_channel = channel;
70  m_nb_servo++;
71 
72  if (!m_interface) {
73  if (m_verbose) {
74  std::cout << "Creating serial interface '" << device << "' at " << baudrate << " bauds" << std::endl;
75  }
76  m_interface = RPMSerialInterface::createSerialInterface(device, baudrate, &error_msg);
77 
78  if (m_interface == nullptr) {
80  "Cannot connect to pololu board with serial port %s at baudrate %d.\n%s",
81  device.c_str(), baudrate, error_msg.c_str()));
82 
83  }
84  }
85 
86  std::thread t(&vpPololu::VelocityCmdThread, this);
87  t.detach();
88 }
89 
90 
92 {
93  m_stop_velocity_cmd_thread = true;
94  if (m_nb_servo == 1) {
95  if (m_verbose) {
96  std::cout << "Deleting serial interface" << std::endl;
97  }
98  delete m_interface;
99  }
100  m_nb_servo--;
101 }
102 
103 void vpPololu::calibrate(unsigned short &pwm_min, unsigned short &pwm_max)
104 {
105  bool ret = false;
106  ret = m_interface->setTargetMSSCP(m_channel, 0);
107  if (ret == false) {
108  throw(vpException(vpException::fatalError, "Unable to set servo normalized target at min position"));
109  }
110  std::this_thread::sleep_for(std::chrono::milliseconds(2000));
111  pwm_min = getPwmPosition();
112 
113  ret = m_interface->setTargetMSSCP(m_channel, 254);
114  if (ret == false) {
115  throw(vpException(vpException::fatalError, "Unable to set servo normalized target at max position"));
116  }
117  std::this_thread::sleep_for(std::chrono::milliseconds(2000));
118  pwm_max = getPwmPosition();
119 }
120 
121 unsigned short vpPololu::radToPwm(float angle) const
122 {
123  float a = m_range_pwm / m_range_angle;
124  float b = m_min_pwm - m_min_angle * a;
125 
126  return static_cast<unsigned short>(a * angle + b);
127 }
128 
130 {
131  if (!m_interface->isOpen()) {
132  if (m_verbose) {
133  std::cout << "Serial Communication Failed!\n";
134  }
135  return false;
136  }
137  return true;
138 }
139 
140 short vpPololu::radSToSpeed(float speed_rad_s) const
141 {
142  return static_cast<short>((speed_rad_s / 100.f) * (m_range_pwm / m_range_angle));
143 }
144 
145 unsigned short vpPololu::getPwmPosition() const
146 {
147  unsigned short position_pwm;
148  bool ret = false;
149  int nb_attempt = 10;
150  int wait_ms = 2;
151  for (int i = 0; i < nb_attempt && ret == false; i++) {
152  ret = m_interface->getPositionCP(m_channel, position_pwm);
153  if (ret == false) {
154  std::this_thread::sleep_for(std::chrono::milliseconds(wait_ms));
155  if (m_verbose) {
156  std::cout << "Failed to get position, new attempt: " << i << std::endl;
157  }
158  }
159  }
160  if (ret == false) {
161  throw(vpException(vpException::fatalError, "Unable to get servo position"));
162  }
163  return position_pwm;
164 }
165 
167 {
168  unsigned short position_pwm = getPwmPosition();
169  float position_angle = pwmToRad(position_pwm);
170 
171  return position_angle;
172 }
173 
174 void vpPololu::getRangeAngles(float &minAngle, float &maxAngle) const
175 {
176  minAngle = m_min_angle;
177  maxAngle = m_max_angle;
178 }
179 
180 void vpPololu::getRangePwm(unsigned short &min, unsigned short &max)
181 {
182  min = m_min_pwm;
183  max = m_max_pwm;
184 }
185 
186 float vpPololu::pwmToRad(unsigned short pwm) const
187 {
188  float a = m_range_angle / m_range_pwm;
189  float b = m_min_angle - m_min_pwm * a;
190 
191  return (a * pwm + b);
192 }
193 
194 void vpPololu::setAngularPosition(float pos_rad, float vel_rad_s)
195 {
196  if ((m_min_angle <= pos_rad) && (pos_rad <= m_max_angle)) {
197  unsigned short pos_pwm = radToPwm(pos_rad);
198  unsigned short pos_speed = static_cast<unsigned short>(std::fabs(radSToSpeed(vel_rad_s)));
199  // Handle the case where pos_speed = 0 which corresponds to the pwm max speed
200  if (pos_speed == 0) {
201  pos_speed = 1;
202  }
203  setPwmPosition(pos_pwm, pos_speed);
204  }
205  else {
207  "Given position: %d is outside of the servo range. You can check the range using the method getRangeAngles()", pos_rad));
208  }
209 }
210 
211 void vpPololu::setPwmPosition(unsigned short pos_pwm, unsigned short speed_pwm)
212 {
213  bool ret = false;
214  if ((m_min_pwm <= pos_pwm) && (pos_pwm <= m_max_pwm)) {
215  if (speed_pwm <= 1000) {
216  ret = m_interface->setSpeedCP(m_channel, speed_pwm);
217  if (ret == false) {
218  throw(vpException(vpException::fatalError, "Unable to set servo speed"));
219  }
220  }
221  else {
223  "Given speed (pwm): %d is outside of the servo speed range. range is from 0 to 1000", speed_pwm));
224  }
225  if (m_verbose) {
226  std::cout << "Channel " << m_channel << " set position (pwm): " << pos_pwm << " with speed: " << speed_pwm << std::endl;
227  }
228  ret = m_interface->setTargetCP(m_channel, pos_pwm);
229  if (ret == false) {
230  throw(vpException(vpException::fatalError, "Unable to set servo target position"));
231  }
232  }
233  else {
235  "Given position (pwm): %d is outside servo range. You can check the range using the method getRangePwm()", pos_pwm));
236  }
237 }
238 
239 void vpPololu::setAngularVelocity(float vel_rad_s)
240 {
241  short pwm_vel = radSToSpeed(vel_rad_s);
242 
243  // Handle the case where vel_rad_s != 0 but pwm_vel == 0. In that case we set a minimal speed
244  if (pwm_vel == 0) {
245  if (vel_rad_s > 0) {
246  pwm_vel = 1;
247  }
248  else if (vel_rad_s < 0) {
249  pwm_vel = -1;
250  }
251  }
252  setPwmVelocity(pwm_vel);
253 }
254 
255 void vpPololu::setPwmVelocity(short pwm_vel)
256 {
257  unsigned short vel_speed;
258  unsigned short vel_target_position;
259  if (pwm_vel <= 1000) {
260  vel_speed = static_cast<unsigned short>(std::abs(pwm_vel));
261  if (pwm_vel > 0) {
262  vel_target_position = m_max_pwm;
263  }
264  else if (pwm_vel < 0) {
265  vel_target_position = m_min_pwm;
266  }
267  else { // pwm_vel = 0
268  vel_target_position = getPwmPosition(); // Stay at current position
269  vel_speed = 1; // Set to min speed to keep current position
270  }
271  m_mutex_velocity_cmd.lock();
272  m_vel_speed = vel_speed;
273  m_vel_target_position = vel_target_position;
274  m_apply_velocity_cmd = true;
275  m_mutex_velocity_cmd.unlock();
276  }
277  else {
279  "Given pwm velocity %d is outside of the servo velocity range. Range is from 0 to 1000", pwm_vel));
280  }
281 }
282 
283 float vpPololu::speedToRadS(short speed_pwm) const
284 {
285  return (speed_pwm * 100) * (m_range_angle / m_range_pwm);
286 }
287 
289 {
290  if (m_verbose) {
291  std::cout << "Stoping vel cmd channel: " << m_channel << std::endl;
292  }
293 
294  m_mutex_velocity_cmd.lock();
295  m_apply_velocity_cmd = false;
296  m_mutex_velocity_cmd.unlock();
297 
298  //std::this_thread::sleep_for(10 * millis);
299  unsigned short pos_pwm = getPwmPosition();
300  if (m_verbose) {
301  std::cout << "Stoping channel " << m_channel << " at position " << pos_pwm << std::endl;
302  }
303  setPwmPosition(pos_pwm, 0); // 0 to be as fast as possible to reach pos_pwm
304 }
305 
306 void vpPololu::VelocityCmdThread()
307 {
308  if (m_verbose) {
309  std::cout << "Create Velocity command thread" << std::endl;
310  }
311  unsigned short vel_speed;
312  unsigned short vel_target_position;
313  bool apply_velocity_cmd;
314 
315  while (!m_stop_velocity_cmd_thread) {
316  m_mutex_velocity_cmd.lock();
317  vel_speed = m_vel_speed;
318  vel_target_position = m_vel_target_position;
319  apply_velocity_cmd = m_apply_velocity_cmd;
320  m_mutex_velocity_cmd.unlock();
321  if (apply_velocity_cmd) {
322  //unsigned short position = getPwmPosition();
323  //if (position != m_vel_target_position) {
324  if (m_vel_speed_prev != vel_speed || m_vel_target_position_prev != vel_target_position) {
325  setPwmPosition(vel_target_position, vel_speed);
326  }
327 
328  m_vel_speed_prev = vel_speed;
329  m_vel_target_position_prev = vel_target_position;
330  }
331 
332  std::this_thread::sleep_for(20 * millis);
333  }
334 }
335 END_VISP_NAMESPACE
336 #elif !defined(VISP_BUILD_SHARED_LIBS)
337 // Work around to avoid warning: libvisp_robot.a(vpPololu.cpp.o) has no symbols
338 void dummy_vpPololu() { };
339 #endif
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ fatalError
Fatal error.
Definition: vpException.h:72
short radSToSpeed(float speed_rad_s) const
Definition: vpPololu.cpp:140
unsigned short radToPwm(float angle) const
Definition: vpPololu.cpp:121
void getRangePwm(unsigned short &min, unsigned short &max)
Definition: vpPololu.cpp:180
unsigned short getPwmPosition() const
Definition: vpPololu.cpp:145
void calibrate(unsigned short &pwm_min, unsigned short &pwm_max)
Definition: vpPololu.cpp:103
float speedToRadS(short speed) const
Definition: vpPololu.cpp:283
void setAngularVelocity(float vel_rad_s)
Definition: vpPololu.cpp:239
vpPololu(bool verbose=false)
Definition: vpPololu.cpp:54
float pwmToRad(unsigned short pwm) const
Definition: vpPololu.cpp:186
virtual ~vpPololu()
Definition: vpPololu.cpp:91
void connect(const std::string &device, int baudrate, int channel)
Definition: vpPololu.cpp:66
float getAngularPosition() const
Definition: vpPololu.cpp:166
void setPwmPosition(unsigned short pos_pwm, unsigned short speed_pwm=0)
Definition: vpPololu.cpp:211
void setAngularPosition(float pos_rad, float vel_rad_s=0.f)
Definition: vpPololu.cpp:194
bool connected() const
Definition: vpPololu.cpp:129
void getRangeAngles(float &minAngle, float &maxAngle) const
Definition: vpPololu.cpp:174
void stopVelocityCmd()
Definition: vpPololu.cpp:288
void setPwmVelocity(short pwm_vel)
Definition: vpPololu.cpp:255
Error that can be emitted by the vpRobot class and its derivatives.
@ constructionError
Error from constructor.
@ positionOutOfRangeError
Position is out of range.