34 #include <visp3/core/vpConfig.h>
37 #if defined(VISP_HAVE_POLOLU) && defined(VISP_HAVE_THREADS)
42 #include <RPMSerialInterface.h>
44 #include <visp3/robot/vpRobot.h>
45 #include <visp3/robot/vpRobotException.h>
46 #include <visp3/robot/vpPololu.h>
48 std::chrono::milliseconds millis(1);
50 RPMSerialInterface *vpPololu::m_interface =
nullptr;
51 int vpPololu::m_nb_servo = 0;
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)
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)
62 connect(device, baudrate, channel);
67 std::string error_msg;
73 std::cout <<
"Creating serial interface '" << device <<
"' at " << baudrate <<
" bauds" << std::endl;
75 m_interface = RPMSerialInterface::createSerialInterface(device, baudrate, &error_msg);
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()));
85 std::thread t(&vpPololu::VelocityCmdThread,
this);
92 m_stop_velocity_cmd_thread =
true;
93 if (m_nb_servo == 1) {
95 std::cout <<
"Deleting serial interface" << std::endl;
105 ret = m_interface->setTargetMSSCP(m_channel, 0);
109 std::this_thread::sleep_for(std::chrono::milliseconds(2000));
112 ret = m_interface->setTargetMSSCP(m_channel, 254);
116 std::this_thread::sleep_for(std::chrono::milliseconds(2000));
122 float a = m_range_pwm / m_range_angle;
123 float b = m_min_pwm - m_min_angle * a;
125 return static_cast<unsigned short>(a * angle + b);
130 if (!m_interface->isOpen()) {
132 std::cout <<
"Serial Communication Failed!\n";
141 return static_cast<short>((speed_rad_s / 100.f) * (m_range_pwm / m_range_angle));
146 unsigned short position_pwm;
150 for (
int i = 0; i < nb_attempt && ret ==
false; i++) {
151 ret = m_interface->getPositionCP(m_channel, position_pwm);
153 std::this_thread::sleep_for(std::chrono::milliseconds(wait_ms));
155 std::cout <<
"Failed to get position, new attempt: " << i << std::endl;
168 float position_angle =
pwmToRad(position_pwm);
170 return position_angle;
175 minAngle = m_min_angle;
176 maxAngle = m_max_angle;
187 float a = m_range_angle / m_range_pwm;
188 float b = m_min_angle - m_min_pwm * a;
190 return (a * pwm + b);
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)));
199 if (pos_speed == 0) {
206 "Given position: %d is outside of the servo range. You can check the range using the method getRangeAngles()", pos_rad));
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);
222 "Given speed (pwm): %d is outside of the servo speed range. range is from 0 to 1000", speed_pwm));
225 std::cout <<
"Channel " << m_channel <<
" set position (pwm): " << pos_pwm <<
" with speed: " << speed_pwm << std::endl;
227 ret = m_interface->setTargetCP(m_channel, pos_pwm);
234 "Given position (pwm): %d is outside servo range. You can check the range using the method getRangePwm()", pos_pwm));
247 else if (vel_rad_s < 0) {
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));
261 vel_target_position = m_max_pwm;
263 else if (pwm_vel < 0) {
264 vel_target_position = m_min_pwm;
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();
278 "Given pwm velocity %d is outside of the servo velocity range. Range is from 0 to 1000", pwm_vel));
284 return (speed_pwm * 100) * (m_range_angle / m_range_pwm);
290 std::cout <<
"Stoping vel cmd channel: " << m_channel << std::endl;
293 m_mutex_velocity_cmd.lock();
294 m_apply_velocity_cmd =
false;
295 m_mutex_velocity_cmd.unlock();
300 std::cout <<
"Stoping channel " << m_channel <<
" at position " << pos_pwm << std::endl;
305 void vpPololu::VelocityCmdThread()
308 std::cout <<
"Create Velocity command thread" << std::endl;
310 unsigned short vel_speed;
311 unsigned short vel_target_position;
312 bool apply_velocity_cmd;
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) {
323 if (m_vel_speed_prev != vel_speed || m_vel_target_position_prev != vel_target_position) {
327 m_vel_speed_prev = vel_speed;
328 m_vel_target_position_prev = vel_target_position;
331 std::this_thread::sleep_for(20 * millis);
335 #elif !defined(VISP_BUILD_SHARED_LIBS)
337 void dummy_vpPololu() { };
error that can be emitted by ViSP classes.
short radSToSpeed(float speed_rad_s) const
unsigned short radToPwm(float angle) const
void getRangePwm(unsigned short &min, unsigned short &max)
unsigned short getPwmPosition() const
void calibrate(unsigned short &pwm_min, unsigned short &pwm_max)
float speedToRadS(short speed) const
void setAngularVelocity(float vel_rad_s)
vpPololu(bool verbose=false)
float pwmToRad(unsigned short pwm) const
void connect(const std::string &device, int baudrate, int channel)
float getAngularPosition() const
void setPwmPosition(unsigned short pos_pwm, unsigned short speed_pwm=0)
void setAngularPosition(float pos_rad, float vel_rad_s=0.f)
void getRangeAngles(float &minAngle, float &maxAngle) const
void setPwmVelocity(short pwm_vel)
Error that can be emitted by the vpRobot class and its derivatives.
@ constructionError
Error from constructor.
@ positionOutOfRangeError
Position is out of range.