Visual Servoing Platform  version 3.6.1 under development (2024-04-15)
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 RPMSerialInterface *vpPololu::m_interface = nullptr;
51 int vpPololu::m_nb_servo = 0;
52 
53 vpPololu::vpPololu(bool verbose)
54  : m_channel(0), m_apply_velocity_cmd(false), m_stop_velocity_cmd_thread(false),
55  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)
56 { }
57 
58 vpPololu::vpPololu(const std::string &device, int baudrate, int channel, bool verbose)
59  : m_channel(channel), m_apply_velocity_cmd(false), m_stop_velocity_cmd_thread(false),
60  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)
61 {
62  connect(device, baudrate, channel);
63 }
64 
65 void vpPololu::connect(const std::string &device, int baudrate, int channel)
66 {
67  std::string error_msg;
68  m_channel = channel;
69  m_nb_servo++;
70 
71  if (!m_interface) {
72  if (m_verbose) {
73  std::cout << "Creating serial interface '" << device << "' at " << baudrate << " bauds" << std::endl;
74  }
75  m_interface = RPMSerialInterface::createSerialInterface(device, baudrate, &error_msg);
76 
77  if (m_interface == nullptr) {
79  "Cannot connect to pololu board with serial port %s at baudrate %d.\n%s",
80  device.c_str(), baudrate, error_msg.c_str()));
81 
82  }
83  }
84 
85  std::thread t(&vpPololu::VelocityCmdThread, this);
86  t.detach();
87 }
88 
89 
91 {
92  m_stop_velocity_cmd_thread = true;
93  if (m_nb_servo == 1) {
94  if (m_verbose) {
95  std::cout << "Deleting serial interface" << std::endl;
96  }
97  delete m_interface;
98  }
99  m_nb_servo--;
100 }
101 
102 void vpPololu::calibrate(unsigned short &pwm_min, unsigned short &pwm_max)
103 {
104  bool ret = false;
105  ret = m_interface->setTargetMSSCP(m_channel, 0);
106  if (ret == false) {
107  throw(vpException(vpException::fatalError, "Unable to set servo normalized target at min position"));
108  }
109  std::this_thread::sleep_for(std::chrono::milliseconds(2000));
110  pwm_min = getPwmPosition();
111 
112  ret = m_interface->setTargetMSSCP(m_channel, 254);
113  if (ret == false) {
114  throw(vpException(vpException::fatalError, "Unable to set servo normalized target at max position"));
115  }
116  std::this_thread::sleep_for(std::chrono::milliseconds(2000));
117  pwm_max = getPwmPosition();
118 }
119 
120 unsigned short vpPololu::radToPwm(float angle) const
121 {
122  float a = m_range_pwm / m_range_angle;
123  float b = m_min_pwm - m_min_angle * a;
124 
125  return static_cast<unsigned short>(a * angle + b);
126 }
127 
129 {
130  if (!m_interface->isOpen()) {
131  if (m_verbose) {
132  std::cout << "Serial Communication Failed!\n";
133  }
134  return false;
135  }
136  return true;
137 }
138 
139 short vpPololu::radSToSpeed(float speed_rad_s) const
140 {
141  return static_cast<short>((speed_rad_s / 100.f) * (m_range_pwm / m_range_angle));
142 }
143 
144 unsigned short vpPololu::getPwmPosition() const
145 {
146  unsigned short position_pwm;
147  bool ret = false;
148  int nb_attempt = 10;
149  int wait_ms = 2;
150  for (int i = 0; i < nb_attempt && ret == false; i++) {
151  ret = m_interface->getPositionCP(m_channel, position_pwm);
152  if (ret == false) {
153  std::this_thread::sleep_for(std::chrono::milliseconds(wait_ms));
154  if (m_verbose) {
155  std::cout << "Failed to get position, new attempt: " << i << std::endl;
156  }
157  }
158  }
159  if (ret == false) {
160  throw(vpException(vpException::fatalError, "Unable to get servo position"));
161  }
162  return position_pwm;
163 }
164 
166 {
167  unsigned short position_pwm = getPwmPosition();
168  float position_angle = pwmToRad(position_pwm);
169 
170  return position_angle;
171 }
172 
173 void vpPololu::getRangeAngles(float &minAngle, float &maxAngle) const
174 {
175  minAngle = m_min_angle;
176  maxAngle = m_max_angle;
177 }
178 
179 void vpPololu::getRangePwm(unsigned short &min, unsigned short &max)
180 {
181  min = m_min_pwm;
182  max = m_max_pwm;
183 }
184 
185 float vpPololu::pwmToRad(unsigned short pwm) const
186 {
187  float a = m_range_angle / m_range_pwm;
188  float b = m_min_angle - m_min_pwm * a;
189 
190  return (a * pwm + b);
191 }
192 
193 void vpPololu::setAngularPosition(float pos_rad, float vel_rad_s)
194 {
195  if ((m_min_angle <= pos_rad) && (pos_rad <= m_max_angle)) {
196  unsigned short pos_pwm = radToPwm(pos_rad);
197  unsigned short pos_speed = static_cast<unsigned short>(std::fabs(radSToSpeed(vel_rad_s)));
198  // Handle the case where pos_speed = 0 which corresponds to the pwm max speed
199  if (pos_speed == 0) {
200  pos_speed = 1;
201  }
202  setPwmPosition(pos_pwm, pos_speed);
203  }
204  else {
206  "Given position: %d is outside of the servo range. You can check the range using the method getRangeAngles()", pos_rad));
207  }
208 }
209 
210 void vpPololu::setPwmPosition(unsigned short pos_pwm, unsigned short speed_pwm)
211 {
212  bool ret = false;
213  if ((m_min_pwm <= pos_pwm) && (pos_pwm <= m_max_pwm)) {
214  if (speed_pwm <= 1000) {
215  ret = m_interface->setSpeedCP(m_channel, speed_pwm);
216  if (ret == false) {
217  throw(vpException(vpException::fatalError, "Unable to set servo speed"));
218  }
219  }
220  else {
222  "Given speed (pwm): %d is outside of the servo speed range. range is from 0 to 1000", speed_pwm));
223  }
224  if (m_verbose) {
225  std::cout << "Channel " << m_channel << " set position (pwm): " << pos_pwm << " with speed: " << speed_pwm << std::endl;
226  }
227  ret = m_interface->setTargetCP(m_channel, pos_pwm);
228  if (ret == false) {
229  throw(vpException(vpException::fatalError, "Unable to set servo target position"));
230  }
231  }
232  else {
234  "Given position (pwm): %d is outside servo range. You can check the range using the method getRangePwm()", pos_pwm));
235  }
236 }
237 
238 void vpPololu::setAngularVelocity(float vel_rad_s)
239 {
240  short pwm_vel = radSToSpeed(vel_rad_s);
241 
242  // Handle the case where vel_rad_s != 0 but pwm_vel == 0. In that case we set a minimal speed
243  if (pwm_vel == 0) {
244  if (vel_rad_s > 0) {
245  pwm_vel = 1;
246  }
247  else if (vel_rad_s < 0) {
248  pwm_vel = -1;
249  }
250  }
251  setPwmVelocity(pwm_vel);
252 }
253 
254 void vpPololu::setPwmVelocity(short pwm_vel)
255 {
256  unsigned short vel_speed;
257  unsigned short vel_target_position;
258  if (pwm_vel <= 1000) {
259  vel_speed = static_cast<unsigned short>(std::abs(pwm_vel));
260  if (pwm_vel > 0) {
261  vel_target_position = m_max_pwm;
262  }
263  else if (pwm_vel < 0) {
264  vel_target_position = m_min_pwm;
265  }
266  else { // pwm_vel = 0
267  vel_target_position = getPwmPosition(); // Stay at current position
268  vel_speed = 1; // Set to min speed to keep current position
269  }
270  m_mutex_velocity_cmd.lock();
271  m_vel_speed = vel_speed;
272  m_vel_target_position = vel_target_position;
273  m_apply_velocity_cmd = true;
274  m_mutex_velocity_cmd.unlock();
275  }
276  else {
278  "Given pwm velocity %d is outside of the servo velocity range. Range is from 0 to 1000", pwm_vel));
279  }
280 }
281 
282 float vpPololu::speedToRadS(short speed_pwm) const
283 {
284  return (speed_pwm * 100) * (m_range_angle / m_range_pwm);
285 }
286 
288 {
289  if (m_verbose) {
290  std::cout << "Stoping vel cmd channel: " << m_channel << std::endl;
291  }
292 
293  m_mutex_velocity_cmd.lock();
294  m_apply_velocity_cmd = false;
295  m_mutex_velocity_cmd.unlock();
296 
297  //std::this_thread::sleep_for(10 * millis);
298  unsigned short pos_pwm = getPwmPosition();
299  if (m_verbose) {
300  std::cout << "Stoping channel " << m_channel << " at position " << pos_pwm << std::endl;
301  }
302  setPwmPosition(pos_pwm, 0); // 0 to be as fast as possible to reach pos_pwm
303 }
304 
305 void vpPololu::VelocityCmdThread()
306 {
307  if (m_verbose) {
308  std::cout << "Create Velocity command thread" << std::endl;
309  }
310  unsigned short vel_speed;
311  unsigned short vel_target_position;
312  bool apply_velocity_cmd;
313 
314  while (!m_stop_velocity_cmd_thread) {
315  m_mutex_velocity_cmd.lock();
316  vel_speed = m_vel_speed;
317  vel_target_position = m_vel_target_position;
318  apply_velocity_cmd = m_apply_velocity_cmd;
319  m_mutex_velocity_cmd.unlock();
320  if (apply_velocity_cmd) {
321  //unsigned short position = getPwmPosition();
322  //if (position != m_vel_target_position) {
323  if (m_vel_speed_prev != vel_speed || m_vel_target_position_prev != vel_target_position) {
324  setPwmPosition(vel_target_position, vel_speed);
325  }
326 
327  m_vel_speed_prev = vel_speed;
328  m_vel_target_position_prev = vel_target_position;
329  }
330 
331  std::this_thread::sleep_for(20 * millis);
332  }
333 }
334 
335 #elif !defined(VISP_BUILD_SHARED_LIBS)
336 // Work around to avoid warning: libvisp_robot.a(vpPololu.cpp.o) has no symbols
337 void dummy_vpPololu() { };
338 #endif
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ fatalError
Fatal error.
Definition: vpException.h:84
short radSToSpeed(float speed_rad_s) const
Definition: vpPololu.cpp:139
unsigned short radToPwm(float angle) const
Definition: vpPololu.cpp:120
void getRangePwm(unsigned short &min, unsigned short &max)
Definition: vpPololu.cpp:179
unsigned short getPwmPosition() const
Definition: vpPololu.cpp:144
void calibrate(unsigned short &pwm_min, unsigned short &pwm_max)
Definition: vpPololu.cpp:102
float speedToRadS(short speed) const
Definition: vpPololu.cpp:282
void setAngularVelocity(float vel_rad_s)
Definition: vpPololu.cpp:238
vpPololu(bool verbose=false)
Definition: vpPololu.cpp:53
float pwmToRad(unsigned short pwm) const
Definition: vpPololu.cpp:185
virtual ~vpPololu()
Definition: vpPololu.cpp:90
void connect(const std::string &device, int baudrate, int channel)
Definition: vpPololu.cpp:65
float getAngularPosition() const
Definition: vpPololu.cpp:165
void setPwmPosition(unsigned short pos_pwm, unsigned short speed_pwm=0)
Definition: vpPololu.cpp:210
void setAngularPosition(float pos_rad, float vel_rad_s=0.f)
Definition: vpPololu.cpp:193
bool connected() const
Definition: vpPololu.cpp:128
void getRangeAngles(float &minAngle, float &maxAngle) const
Definition: vpPololu.cpp:173
void stopVelocityCmd()
Definition: vpPololu.cpp:287
void setPwmVelocity(short pwm_vel)
Definition: vpPololu.cpp:254
Error that can be emitted by the vpRobot class and its derivatives.
@ constructionError
Error from constructor.
@ positionOutOfRangeError
Position is out of range.