From a91a1612b1b4b039308c141f05495205eeb5d04e Mon Sep 17 00:00:00 2001 From: Pierre Perraud Date: Tue, 26 Sep 2023 10:32:14 +0200 Subject: [PATCH] Adding wrapper vpServoPololuMaestro class over RappaPololuMaestro sdk Allow position and velocity control using pololu board Move documentation from .cpp to .h --- CMakeLists.txt | 3 + cmake/FindMyRapaPololuMaestro.cmake | 71 +++++ cmake/templates/vpConfig.h.in | 61 ++-- doc/config-doxygen.in | 1 + modules/robot/CMakeLists.txt | 5 + .../include/visp3/robot/vpPTUPololuMaestro.h | 170 +++++++++++ .../include/visp3/robot/vpRobotException.h | 2 +- .../visp3/robot/vpServoPololuMaestro.h | 269 ++++++++++++++++++ .../pololu-maestro/vpPTUPololuMaestro.cpp | 167 +++++++++++ .../pololu-maestro/vpServoPololuMaestro.cpp | 257 +++++++++++++++++ .../testPololuMaestroPosition.cpp | 97 +++++++ .../testPololuMaestroVelocity.cpp | 94 ++++++ 12 files changed, 1167 insertions(+), 30 deletions(-) create mode 100644 cmake/FindMyRapaPololuMaestro.cmake create mode 100644 modules/robot/include/visp3/robot/vpPTUPololuMaestro.h create mode 100644 modules/robot/include/visp3/robot/vpServoPololuMaestro.h create mode 100644 modules/robot/src/real-robot/pololu-maestro/vpPTUPololuMaestro.cpp create mode 100644 modules/robot/src/real-robot/pololu-maestro/vpServoPololuMaestro.cpp create mode 100644 modules/robot/test/pololu-maestro/testPololuMaestroPosition.cpp create mode 100644 modules/robot/test/pololu-maestro/testPololuMaestroVelocity.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 2d18f91bf4..cb0c70fc69 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -519,6 +519,7 @@ VP_OPTION(USE_UEYE Ueye "" "Include uEye SDK support for IDS ca VP_OPTION(USE_COMEDI Comedi "" "Include comedi (linux control and measurement device interface) support" "" ON IF NOT WINRT AND NOT IOS) VP_OPTION(USE_FTIITSDK FTIITSDK "" "Include IIT force-torque SDK support" "" ON IF NOT WINRT AND NOT IOS) VP_OPTION(USE_BICLOPS BICLOPS "" "Include biclops support" "" ON IF NOT WINRT AND NOT IOS) +VP_OPTION(USE_RAPA_POLOLU_MAESTRO MyRapaPololuMaestro "" "Include Rapa Pololu Maestro 3rd party" "" ON IF UNIX) VP_OPTION(USE_PTU46 PTU46 "" "Include ptu-46 support" "" ON IF UNIX AND NOT WINRT AND NOT IOS) VP_OPTION(USE_FLIRPTUSDK FlirPtuSDK "" "Include Flir PTU SDK support" "" ON IF NOT WINRT AND NOT IOS) VP_OPTION(USE_CMU1394 CMU1394 "" "Include cmu1494 support" "" ON IF WIN32 AND NOT WINRT AND NOT IOS) @@ -914,6 +915,7 @@ VP_SET(VISP_HAVE_FRANKA TRUE IF (BUILD_MODULE_visp_robot AND USE_FRANKA)) VP_SET(VISP_HAVE_JACOSDK TRUE IF (BUILD_MODULE_visp_robot AND USE_JACOSDK)) VP_SET(VISP_HAVE_MAVSDK TRUE IF (BUILD_MODULE_visp_robot AND USE_MAVSDK)) VP_SET(VISP_HAVE_BICLOPS TRUE IF (BUILD_MODULE_visp_robot AND USE_BICLOPS)) +VP_SET(VISP_HAVE_RAPA_POLOLU_MAESTRO TRUE IF (BUILD_MODULE_visp_robot AND USE_RAPA_POLOLU_MAESTRO)) VP_SET(VISP_HAVE_PTU46 TRUE IF (BUILD_MODULE_visp_robot AND USE_PTU46)) VP_SET(VISP_HAVE_FLIR_PTU_SDK TRUE IF (BUILD_MODULE_visp_robot AND USE_FLIRPTUSDK)) VP_SET(VISP_HAVE_ARSDK TRUE IF (BUILD_MODULE_visp_robot AND USE_ARSDK)) @@ -1539,6 +1541,7 @@ status(" Use Kinova Jaco:" USE_JACOSDK THEN "yes" ELSE "no") status(" Use aria (Pioneer):" USE_ARIA THEN "yes" ELSE "no") status(" Use PTU46:" USE_PTU46 THEN "yes" ELSE "no") status(" Use Biclops PTU:" USE_BICLOPS THEN "yes" ELSE "no") +status(" Use Rapa Pololu Maestro:" USE_RAPA_POLOLU_MAESTRO THEN "yes" ELSE "no") status(" Use Flir PTU SDK:" USE_FLIRPTUSDK THEN "yes (ver ${FLIRPTUSDK_VERSION})" ELSE "no") status(" Use MAVSDK:" USE_MAVSDK THEN "yes (ver ${MAVSDK_VERSION})" ELSE "no") status(" Use Parrot ARSDK:" USE_ARSDK THEN "yes" ELSE "no") diff --git a/cmake/FindMyRapaPololuMaestro.cmake b/cmake/FindMyRapaPololuMaestro.cmake new file mode 100644 index 0000000000..2784b5a539 --- /dev/null +++ b/cmake/FindMyRapaPololuMaestro.cmake @@ -0,0 +1,71 @@ +############################################################################# +# +# ViSP, open source Visual Servoing Platform software. +# Copyright (C) 2005 - 2023 by Inria. All rights reserved. +# +# This software is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. +# See the file LICENSE.txt at the root directory of this source +# distribution for additional information about the GNU GPL. +# +# For using ViSP with software that can not be combined with the GNU +# GPL, please contact Inria about acquiring a ViSP Professional +# Edition License. +# +# See https://visp.inria.fr for more information. +# +# This software was developed at: +# Inria Rennes - Bretagne Atlantique +# Campus Universitaire de Beaulieu +# 35042 Rennes Cedex +# France +# +# If you have questions regarding the use of this file, please contact +# Inria at visp@inria.fr +# +# This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE +# WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. +# +# Description: +# Try to find Rapa Pololu Maestro library material +# See https://github.com/jbitoniau/RapaPololuMaestro +# +# RapaPololuMaestro_FOUND +# RapaPololuMaestro_INCLUDE_DIRS +# RapaPololuMaestro_LIBRARIES +# +############################################################################# + +find_path(RapaPololuMaestro_INCLUDE_DIR RPMSerialInterface.h + PATHS + $ENV{RapaPololuMaestro_DIR}/include + ${RapaPololuMaestro_DIR}/include + /usr/include + /usr/local/include +) + +find_library(RapaPololuMaestro_LIBRARY + NAMES RapaPololuMaestro + PATHS + $ENV{RapaPololuMaestro_DIR}/lib + ${RapaPololuMaestro_DIR}/lib + /usr/lib + /usr/local/lib +) + +if(RapaPololuMaestro_INCLUDE_DIR AND RapaPololuMaestro_LIBRARY) + set(RapaPololuMaestro_FOUND TRUE) + set(RapaPololuMaestro_INCLUDE_DIRS ${RapaPololuMaestro_INCLUDE_DIR}) + set(RapaPololuMaestro_LIBRARIES ${RapaPololuMaestro_LIBRARY}) +else() + set(RapaPololuMaestro_FOUND FALSE) +endif() + +mark_as_advanced( + RapaPololuMaestro_INCLUDE_DIR + RapaPololuMaestro_LIBRARY + RapaPololuMaestro_INCLUDE_DIRS + RapaPololuMaestro_LIBRARIES + ) diff --git a/cmake/templates/vpConfig.h.in b/cmake/templates/vpConfig.h.in index 67f9f61b93..124626b1aa 100644 --- a/cmake/templates/vpConfig.h.in +++ b/cmake/templates/vpConfig.h.in @@ -40,33 +40,33 @@ #include #if defined _MSC_VER && _MSC_VER >= 1200 - #pragma warning( disable: 4100 4127 4251 4275 4351 4514 4668 4710 4820 ) - #if _MSC_VER >= 1400 // 1400 = MSVC 8 2005 - #pragma warning( disable: 4548 ) - #endif - #if _MSC_VER > 1500 // 1500 = MSVC 9 2008 - #pragma warning( disable: 4986 ) - #endif - #ifdef WINRT - #pragma warning(disable:4447) - #endif - - // 4100 : undocumented ("unreferenced formal parameter") - // 4127 : conditional expression is constant - // 4251 : 'identifier' : class 'type' needs to have dll-interface to be used by clients of class 'type2', ie. disable warnings related to inline functions - // 4275 : non – DLL-interface classkey 'identifier' used as base for DLL-interface classkey 'identifier' - // 4351 : new behavior: elements of array will be default initialized - // 4447 : Disable warning 'main' signature found without threading model - // 4514 : 'function' : unreferenced inline function has been removed - // 4548 : expression before comma has no effect - // 4668 : 'symbol' is not defined as a preprocessor macro, replacing with '0' for 'directives' - // 4710 : 'function' : function not inlined - // 4820 : 'bytes' bytes padding added after construct 'member_name' - // 4986 : undocumented - - #ifndef NOMINMAX - #define NOMINMAX - #endif +#pragma warning( disable: 4100 4127 4251 4275 4351 4514 4668 4710 4820 ) +#if _MSC_VER >= 1400 // 1400 = MSVC 8 2005 +#pragma warning( disable: 4548 ) +#endif +#if _MSC_VER > 1500 // 1500 = MSVC 9 2008 +#pragma warning( disable: 4986 ) +#endif +#ifdef WINRT +#pragma warning(disable:4447) +#endif + +// 4100 : undocumented ("unreferenced formal parameter") +// 4127 : conditional expression is constant +// 4251 : 'identifier' : class 'type' needs to have dll-interface to be used by clients of class 'type2', ie. disable warnings related to inline functions +// 4275 : non – DLL-interface classkey 'identifier' used as base for DLL-interface classkey 'identifier' +// 4351 : new behavior: elements of array will be default initialized +// 4447 : Disable warning 'main' signature found without threading model +// 4514 : 'function' : unreferenced inline function has been removed +// 4548 : expression before comma has no effect +// 4668 : 'symbol' is not defined as a preprocessor macro, replacing with '0' for 'directives' +// 4710 : 'function' : function not inlined +// 4820 : 'bytes' bytes padding added after construct 'member_name' +// 4986 : undocumented + +#ifndef NOMINMAX +#define NOMINMAX +#endif #endif #if defined _MSC_VER && (_MSC_VER == 1500) @@ -104,7 +104,7 @@ #define VISP_VERSION_PATCH ${VISP_VERSION_PATCH} // ViSP version with dots "${VISP_VERSION_MAJOR}.${VISP_VERSION_MINOR}.${VISP_VERSION_PATCH}". -#cmakedefine VISP_VERSION ${VISP_VERSION} +#cmakedefine VISP_VERSION $ { VISP_VERSION } // ViSP version as an integer #define VP_VERSION_INT(a, b, c) (a<<16 | b<<8 | c) @@ -115,7 +115,7 @@ // Enable debug and trace printings #cmakedefine VP_TRACE #cmakedefine VP_DEBUG -#cmakedefine VP_DEBUG_MODE ${VP_DEBUG_MODE} +#cmakedefine VP_DEBUG_MODE $ { VP_DEBUG_MODE } // ViSP library is either compiled static or shared // Used to set declspec(import, export) in headers if required under Windows @@ -395,6 +395,9 @@ #cmakedefine VISP_HAVE_BICLOPS #cmakedefine VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION +// Defined if Rapa Pololu Maestro 3rd party library available. +#cmakedefine VISP_HAVE_RAPA_POLOLU_MAESTRO + // Defined if Irisa's Ptu-46 pan-tilt head available. #cmakedefine VISP_HAVE_PTU46 diff --git a/doc/config-doxygen.in b/doc/config-doxygen.in index bb4dffd702..6bfdbfb262 100644 --- a/doc/config-doxygen.in +++ b/doc/config-doxygen.in @@ -2212,6 +2212,7 @@ PREDEFINED = @DOXYGEN_SHOULD_SKIP_THIS@ \ VISP_HAVE_QBDEVICE \ VISP_HAVE_QT \ VISP_HAVE_QUALISYS \ + VISP_HAVE_RAPA_POLOLU_MAESTRO \ VISP_HAVE_REALSENSE \ VISP_HAVE_REALSENSE_VERSION=0x020000 \ VISP_HAVE_REALSENSE2 \ diff --git a/modules/robot/CMakeLists.txt b/modules/robot/CMakeLists.txt index b20f96706e..1015feded7 100644 --- a/modules/robot/CMakeLists.txt +++ b/modules/robot/CMakeLists.txt @@ -105,6 +105,11 @@ if(USE_BICLOPS) list(APPEND opt_libs ${BICLOPS_LIBRARIES}) endif() +if(USE_RAPA_POLOLU_MAESTRO) + list(APPEND opt_incs ${RapaPololuMaestro_INCLUDE_DIRS}) + list(APPEND opt_libs ${RapaPololuMaestro_LIBRARIES}) +endif() + if(USE_PTU46) list(APPEND opt_incs ${PTU46_INCLUDE_DIRS}) list(APPEND opt_libs ${PTU46_LIBRARIES}) diff --git a/modules/robot/include/visp3/robot/vpPTUPololuMaestro.h b/modules/robot/include/visp3/robot/vpPTUPololuMaestro.h new file mode 100644 index 0000000000..fb4c7abcc0 --- /dev/null +++ b/modules/robot/include/visp3/robot/vpPTUPololuMaestro.h @@ -0,0 +1,170 @@ +/* + * ViSP, open source Visual Servoing Platform software. + * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * + * This software is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * See the file LICENSE.txt at the root directory of this source + * distribution for additional information about the GNU GPL. + * + * For using ViSP with software that can not be combined with the GNU + * GPL, please contact Inria about acquiring a ViSP Professional + * Edition License. + * + * See https://visp.inria.fr for more information. + * + * This software was developed at: + * Inria Rennes - Bretagne Atlantique + * Campus Universitaire de Beaulieu + * 35042 Rennes Cedex + * France + * + * If you have questions regarding the use of this file, please contact + * Inria at visp@inria.fr + * + * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE + * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. + * + * Description: + * Common features for Pololu Maestro PanTiltUnit. + */ + +#ifndef _vpPTUPololuMaestro_h_ +#define _vpPTUPololuMaestro_h_ + +#include + +#ifdef VISP_HAVE_RAPA_POLOLU_MAESTRO + +#include +#include +#include +#include + +#include + +#include + +using namespace std; + +/*! + * \class vpPTUPololuMaestro + * \ingroup group_robot_real_arm + * + * \brief Interface for the Pololu Maestro Pan Tilt Unit using servo motors. + * + * See https://www.pololu.com/category/102/maestro-usb-servo-controllers for more details. + * + * This class handle the vpServoPololuMaestro class in a higher level and allows to control + * the Pan Tilt Unit using position or velocity commands. + * + */ +class VISP_EXPORT vpPTUPololuMaestro +{ +public: + typedef enum { pan, tilt, yaw } Axe; + + /*! + * Default constructor. + */ + vpPTUPololuMaestro(); + + + /*! + * Value constructor. + * + * \param baudrate : Baudrate used for the serial communication. + * + * \param device : Name of the serial interface used for communication. + */ + vpPTUPololuMaestro(const std::string &device, int baudrate); + + /*! + * Destructor. + */ + virtual ~vpPTUPololuMaestro(); + + /*! + * Get ranges for a given axe. + * + * \param minAngle : Pointer to minimum range of the servo. + * + * \param maxAngle : Pointer to maximum range of the servo. + * + * \param rangeAngle : Pointer to the range of the servo. + * + * \param axe : One of the axe define in vpPTUPololuMaestro.h Axe enum. + * + */ + void getRange(float &minAngle, float &maxAngle, float &rangeAngle, Axe axe = pan); + + /*! + * Get the current position of a servo motor in degree. + * + * \param angle : Angle, in degree. + * + * \param axe : One of the axe define in vpPTUPololuMaestro.h Axe enum. + * + */ + void getPositionAngle(float &angle, Axe axe); + + + /*! + * Initiate the serial connection with the Pololu board. + * + * \param device : Name of the serial interface used for communication. + * + * \param baudrate : Baudrate used for the serial communication. + * + */ + void setConnection(std::string device, int baudrate); + + /*! + * Set position in degree and the maximum speed of displacement for a given axe. + * + * \param angle : Angle to reach, in degree. + * + * \param speed : Maximum speed for movement in units of (0.25 μs)/(10 ms). + * You can use the vpServoPololuMaestro::degSToSpeed method for conversion. + * + * \param axe : One of the axe define in vpPTUPololuMaestro.h Axe enum. + * + */ + void setPositionAngle(float angle, unsigned short speed = 0, Axe axe = pan); + + /*! + * Set and start the velocity command for a given axe. + * + * \param speed : Speed for movement in units of (0.25 μs)/(10 ms). + * You can use the vpServoPololuMaestro::degSToSpeed method for conversion. + * + * \param axe : One of the axe define in vpPTUPololuMaestro.h Axe enum. + * + */ + void setVelocityCmd(short speed, Axe axe = pan); + + + /*! + * Stop the velocity command for a given axe. + * + * \param axe : One of the axe define in vpPTUPololuMaestro.h Axe enum. + * + */ + void stopVelocityCmd(Axe axe); + +private: + // Serial connection parameters. + int m_baudrate; + std::string m_device; + RPM::SerialInterface *m_serialInterface; + + vpServoPololuMaestro m_pan; + vpServoPololuMaestro m_tilt; + + bool m_verbose; +}; + +#endif +#endif diff --git a/modules/robot/include/visp3/robot/vpRobotException.h b/modules/robot/include/visp3/robot/vpRobotException.h index 28f1d2bc86..acf7b01cee 100644 --- a/modules/robot/include/visp3/robot/vpRobotException.h +++ b/modules/robot/include/visp3/robot/vpRobotException.h @@ -58,7 +58,7 @@ class VISP_EXPORT vpRobotException : public vpException enum errorRobotCodeEnum { -//! Error from constructor + //! Error from constructor constructionError, //! Not unique robot object construction diff --git a/modules/robot/include/visp3/robot/vpServoPololuMaestro.h b/modules/robot/include/visp3/robot/vpServoPololuMaestro.h new file mode 100644 index 0000000000..8971c4ce95 --- /dev/null +++ b/modules/robot/include/visp3/robot/vpServoPololuMaestro.h @@ -0,0 +1,269 @@ +/* + * ViSP, open source Visual Servoing Platform software. + * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * + * This software is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * See the file LICENSE.txt at the root directory of this source + * distribution for additional information about the GNU GPL. + * + * For using ViSP with software that can not be combined with the GNU + * GPL, please contact Inria about acquiring a ViSP Professional + * Edition License. + * + * See https://visp.inria.fr for more information. + * + * This software was developed at: + * Inria Rennes - Bretagne Atlantique + * Campus Universitaire de Beaulieu + * 35042 Rennes Cedex + * France + * + * If you have questions regarding the use of this file, please contact + * Inria at visp@inria.fr + * + * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE + * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. + * + * Description: + * Common features for Pololu Maestro Servo Motor. + */ + +#ifndef _vpServoPololuMaestro_h_ +#define _vpServoPololuMaestro_h_ + +#include + + +#ifdef VISP_HAVE_RAPA_POLOLU_MAESTRO + +#include +#include + +#include + +/*! + * \class vpServoPololuMaestro + * \ingroup group_robot_real_arm + * + * \brief Interface for the Pololu Board controlled servo. + * + * See https://www.pololu.com/category/102/maestro-usb-servo-controllers for more details. + * + * This class give a position and velocity control for the servo motors plugged into the Board. + * + */ +class VISP_EXPORT vpServoPololuMaestro +{ +public: + /*! + * Default constructor. + * The VelocityCmdThread is created in the constructor and will run independently of the rest of the class. + * You can set velocity commands using setVelCmd() method. + * + * \param device : Serial device name to dial with pololu board. + * + * \param baudrate : Baudrate used to dial with pololu board. + * + */ + vpServoPololuMaestro(const std::string &device = "/dev/ttyACM0", int baudrate = 9600); + + + /*! + * Value constructor. + * + * \param interface : Serial interface for communication. + * + * \param channel : Channel number in which the servo is connected on the pololu board. + * + * \param verbose : Enabling verbose, default is false. + */ + vpServoPololuMaestro(RPM::SerialInterface *interface, int channel, bool verbose = false); + + /*! + * Destructor. + */ + virtual ~vpServoPololuMaestro(); + + /*! + * Convert angles to PWM for servo commands. + * + * \param angle : Angle, in degree to be converted. + * + * \return Corresponding PWM value for the angle. + */ + int angle2PWM(float angle); + + /*! + * Check if the serial connection is still up. + * + * \return TRUE is the connection has an error. + */ + bool checkConnection(); + + /*! + * Convert deg/s speed into Pololu's speed. + * + * \param speedDegS : Speed converted to deg/s. + * + * \return speed : Speed in units of (0.25 μs)/(10 ms). + * + */ + short degSToSpeed(float speedDegS); + + /*! + * Return position in PWM. + * + * \return position: Return current position in PWM. + */ + unsigned short getPosition(); + + /*! + * Return position in PWM. + * + * \return positionAngle : Return current position in deg; + */ + float getPositionAngle(); + + /*! + * Get min, max and range for angle cmd. + * + * \param minAngle : Min range value for angle control. + * + * \param maxAngle : Max range value for angle control. + * + * \param rangeAngle : Range value for angle control. + * + */ + void getRangeAngle(float &minAngle, float &maxAngle, float &rangeAngle); + + /*! + * Get min, max and range for PWM cmd. + * + * \param minPWM : Min range value for PWM control. + * + * \param maxPWM : Max range value for PWM control. + * + * \param rangePWM : Range value for PWM control. + * + */ + void getRangePWM(int &minPWM, int &maxPWM, int &rangePWM); + + /*! + * Return the current speed parameter. + * + * \return speed : Speed to use for movement in units of (0.25 μs)/(10 ms). No speed (0) will use maximum speed. + * + */ + unsigned short getSpeed(); + + /*! + * Convert PWM to angles for servo commands. + * + * \param PWM : PWM value. + * + * \return Corresponding angle value for the PWM. + */ + float PWM2Angle(int PWM); + + /*! + * Set the position to reach in angle. + * + * \param targetAngle : Position in angle to reach. + * + * \param speed : OPTIONAL : Speed to use for movement in units of (0.25 μs)/(10 ms). Default is '0' and will use + * maximum speed. + * + * \return error : Return 1 if an error is encountered. + * + */ + void setPositionAngle(float targetAngle, unsigned short speed = 0); + + /*! + * Set the position to reach in PWM. + * + * \param targetPWM : Position in PWM to reach. + * + * \param speed : OPTIONAL : Speed to use for movement in units of (0.25 μs)/(10 ms). Default is 0, maximum speed. + * + * \exception When PWM out of range. + */ + void setPositionPWM(int targetPWM, unsigned short speed = 0); + + /*! + * Set the speed of the motor movements. + * + * \param speed : Speed to use for movement in units of (0.25 μs)/(10 ms). No speed (0) will use maximum speed. + * + * \return error : Return 1 if an error is encountered. + */ + void setSpeed(unsigned short speed); + + /*! + * Set the speed of the motor movements in deg/s. + * + * \param speedRadS : Speed to use for movement in deg/s. No speed (0) will use maximum speed + * + * \return error : return 1 if an error is encountered + * + */ + void setSpeedDegS(float speedRadS); + + /*! + * Set the speed of the motor movements and start the velocity command thread. The motor will move to the edge of the + * range at the given speed. + * + * \param velocityCmdSpeed : Speed to use for movement in units of (0.25 μs)/(10 ms). No speed (0) will use maximum speed. + * + * \return error : Return 1 if an error is encountered. + */ + void setVelocityCmd(short velocityCmdSpeed); + + /*! + * Convert Pololu's speed to deg/s speed. + * + * \param speed : Speed in units of (0.25 μs)/(10 ms). + * + * \return speedDegS : Speed converted to deg/s + * + */ + float speedToDeGS(short speed); + + /*! + * Stop the velocity command thread. + * + */ + void stopVelCmd(); + +private: + RPM::SerialInterface *m_interface; + int m_channel; + bool m_FlagVelCmdRunning; + + unsigned short m_position; + unsigned short m_speed; + int m_velocityDirection; + + // ranges + int m_minPWM = 2800; + int m_maxPWM = 8800; + int m_rangePWM = m_maxPWM - m_minPWM; + float m_minAngle = -40; + float m_maxAngle = 40; + float m_rangeAngle = abs(m_minAngle) + abs(m_maxAngle); + + bool m_verbose; + + /*! + * Thread use for Velocity control. This thread is launch in the constructor of the object and, unless crashes, will + * run until the process is ended. If the m_FlagVelCmdRunning is set to TRUE, by invoking the setVelocityCmd method, the + * motor will go to the edge of the motor range using the speed set in setVelocityCmd. The velocity command can be + * stopped invoking the stopVelCmd() method. + */ + void VelocityCmdThread(); +}; + +#endif +#endif diff --git a/modules/robot/src/real-robot/pololu-maestro/vpPTUPololuMaestro.cpp b/modules/robot/src/real-robot/pololu-maestro/vpPTUPololuMaestro.cpp new file mode 100644 index 0000000000..588a4341a8 --- /dev/null +++ b/modules/robot/src/real-robot/pololu-maestro/vpPTUPololuMaestro.cpp @@ -0,0 +1,167 @@ +/* + * ViSP, open source Visual Servoing Platform software. + * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * + * This software is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * See the file LICENSE.txt at the root directory of this source + * distribution for additional information about the GNU GPL. + * + * For using ViSP with software that can not be combined with the GNU + * GPL, please contact Inria about acquiring a ViSP Professional + * Edition License. + * + * See https://visp.inria.fr for more information. + * + * This software was developed at: + * Inria Rennes - Bretagne Atlantique + * Campus Universitaire de Beaulieu + * 35042 Rennes Cedex + * France + * + * If you have questions regarding the use of this file, please contact + * Inria at visp@inria.fr + * + * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE + * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. + * + * Description: + * Common features for Pololu Maestro PanTiltUnit. + */ + +#include + + +#ifdef VISP_HAVE_RAPA_POLOLU_MAESTRO + + +#include +#include +#include + + +vpPTUPololuMaestro::vpPTUPololuMaestro() + : m_baudrate(9600), m_device("/dev/ttyACM0"), m_serialInterface(NULL) +{ + std::string error_msg; + setConnection(this->m_device, this->m_baudrate, error_msg); + + m_pan = vpServoPololuMaestro(this->m_serialInterface, 0); + m_tilt = vpServoPololuMaestro(this->m_serialInterface, 1); +} + + +vpPTUPololuMaestro::vpPTUPololuMaestro(const std::string &device, int baudrate) + : m_baudrate(baudrate), m_device(device), m_serialInterface(NULL) +{ + std::string error_msg; + setConnection(this->m_device, this->m_baudrate, error_msg); + + m_pan = vpServoPololuMaestro(this->m_serialInterface, 0); + m_tilt = vpServoPololuMaestro(this->m_serialInterface, 1); +} + +vpPTUPololuMaestro::~vpPTUPololuMaestro() { } + +void vpPTUPololuMaestro::setConnection(std::string device, int baudrate) +{ + if (m_serialInterface != NULL) { + throw(vpRobotException(vpRobotException::constructionError, "Pololu board already connected")); + } + std::string error_msg; + this->m_serialInterface = RPM::SerialInterface::createSerialInterface(device, baudrate, &error_msg); + std::cout << error_msg; + std::cout << "Serial ls /dev Started!\n"; + std::cout << m_serialInterface->isOpen() << "\n"; + if (!m_serialInterface->isOpen()) { + throw(vpRobotException(vpRobotException::constructionError, "Cannot connect to pololu board with device: %s and baudrate: %d", device.c_str(), baudrate)); + } +} + +void vpPTUPololuMaestro::setPositionAngle(float angle, unsigned short speed, Axe axe) +{ + switch (axe) { + case pan: + m_pan.setPositionAngle(angle, speed); + break; + case tilt: + m_tilt.setPositionAngle(angle, speed); + break; + default: + throw(vpRobotException(vpRobotException::notImplementedError, "No corresponding axe.")); + break; + } +} + +void vpPTUPololuMaestro::getRange(float &minAngle, float &maxAngle, float &rangeAngle, Axe axe) +{ + switch (axe) { + case pan: + m_pan.getRangeAngle(minAngle, maxAngle, rangeAngle); + break; + case tilt: + m_tilt.getRangeAngle(minAngle, maxAngle, rangeAngle); + break; + default: + throw(vpRobotException(vpRobotException::notImplementedError, "No corresponding axe.")); + minAngle = 0; + maxAngle = 0; + rangeAngle = 0; + break; + } +} + +void vpPTUPololuMaestro::setVelocityCmd(short speed, Axe axe) +{ + std::cout << "start vel cmd" << std::endl; + switch (axe) { + case pan: + m_pan.setVelocityCmd(speed); + break; + case tilt: + m_tilt.setVelocityCmd(speed); + break; + default: + throw(vpRobotException(vpRobotException::notImplementedError, "No corresponding axe.")); + break; + } +} + + +void vpPTUPololuMaestro::stopVelocityCmd(Axe axe) +{ + switch (axe) { + case pan: + m_pan.stopVelCmd(); + break; + case tilt: + m_tilt.stopVelCmd(); + break; + default: + throw(vpRobotException(vpRobotException::notImplementedError, "No corresponding axe.")); + break; + } +} + + +void vpPTUPololuMaestro::getPositionAngle(float &angle, Axe axe) +{ + switch (axe) { + case pan: + angle = m_pan.getPositionAngle(); + break; + case tilt: + angle = m_tilt.getPositionAngle(); + break; + default: + throw(vpRobotException(vpRobotException::notImplementedError, "No corresponding axe.")); + break; + } +} + +#elif !defined(VISP_BUILD_SHARED_LIBS) +// Work around to avoid warning: libvisp_robot.a(vpPTUPololuMaestro.cpp.o) has no symbols +void dummy_vpPTUPololuMaestro() { }; +#endif diff --git a/modules/robot/src/real-robot/pololu-maestro/vpServoPololuMaestro.cpp b/modules/robot/src/real-robot/pololu-maestro/vpServoPololuMaestro.cpp new file mode 100644 index 0000000000..73996efb8d --- /dev/null +++ b/modules/robot/src/real-robot/pololu-maestro/vpServoPololuMaestro.cpp @@ -0,0 +1,257 @@ +/* + * ViSP, open source Visual Servoing Platform software. + * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * + * This software is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * See the file LICENSE.txt at the root directory of this source + * distribution for additional information about the GNU GPL. + * + * For using ViSP with software that can not be combined with the GNU + * GPL, please contact Inria about acquiring a ViSP Professional + * Edition License. + * + * See https://visp.inria.fr for more information. + * + * This software was developed at: + * Inria Rennes - Bretagne Atlantique + * Campus Universitaire de Beaulieu + * 35042 Rennes Cedex + * France + * + * If you have questions regarding the use of this file, please contact + * Inria at visp@inria.fr + * + * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE + * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. + * + * Description: + * Common features for Pololu Maestro Servo Motor. + */ + +#include + + +#ifdef VISP_HAVE_RAPA_POLOLU_MAESTRO + +#include +#include + +#include + +#include +#include +#include + +std::chrono::milliseconds millis(1); + + +vpServoPololuMaestro::vpServoPololuMaestro(const std::string &device, int baudrate) + : m_interface(NULL), m_channel(0), m_FlagVelCmdRunning(false), m_position(0), m_speed(0), m_velocityDirection(0), m_verbose(false) +{ + std::string error_msg; + this->m_interface = RPM::SerialInterface::createSerialInterface(device, baudrate, &error_msg); + + if (!m_interface->isOpen()) { + throw(vpRobotException(vpRobotException::constructionError, + "Cannot connect to pololu board with device: %s and baudrate: %d", device.c_str(), baudrate)); + } + + std::thread t(&vpServoPololuMaestro::VelocityCmdThread, this); + t.detach(); + + if (m_verbose) { + std::cout << "Default servo created on channel: " << this->m_channel << std::endl; + } +} + + +vpServoPololuMaestro::vpServoPololuMaestro(RPM::SerialInterface *interface, int channel, bool verbose) + : m_interface(interface), m_channel(channel), m_FlagVelCmdRunning(false), m_position(0), m_speed(0), m_velocityDirection(0), m_verbose(verbose) +{ + std::thread t(&vpServoPololuMaestro::VelocityCmdThread, this); + t.detach(); + + if (m_verbose) { + std::cout << "Value servo created on channel: " << this->m_channel << std::endl; + } +} + + +vpServoPololuMaestro::~vpServoPololuMaestro() { } + + +int vpServoPololuMaestro::angle2PWM(float angle) +{ + return ((angle + abs(m_minAngle)) / m_rangeAngle) * m_rangePWM + m_minPWM; +} + +bool vpServoPololuMaestro::checkConnection() +{ + if (!this->m_interface->isOpen()) { + if (m_verbose) { + std::cout << "Serial Communication Failed!\n"; + } + return 1; + } + return 0; +} + +short vpServoPololuMaestro::degSToSpeed(float speedDegS) +{ + return (speedDegS / 100) * (this->m_rangePWM / this->m_rangeAngle); +} + +unsigned short vpServoPololuMaestro::getPosition() +{ + m_interface->getPositionCP(this->m_channel, this->m_position); + return this->m_position; +} + +float vpServoPololuMaestro::getPositionAngle() +{ + m_interface->getPositionCP(this->m_channel, this->m_position); + float positionAngle = this->PWM2Angle(this->m_position); + return positionAngle; +} + +void vpServoPololuMaestro::getRangeAngle(float &minAngle, float &maxAngle, float &rangeAngle) +{ + minAngle = this->m_minAngle; + maxAngle = this->m_maxAngle; + rangeAngle = this->m_rangeAngle; +} + +void vpServoPololuMaestro::getRangePWM(int &minPWM, int &maxPWM, int &rangePWM) +{ + minPWM = this->m_minPWM; + maxPWM = this->m_maxPWM; + rangePWM = this->m_rangePWM; +} + +unsigned short vpServoPololuMaestro::getSpeed() { return this->m_speed; } + + +float vpServoPololuMaestro::PWM2Angle(int PWM) { return (PWM * (m_rangeAngle / m_rangePWM) + m_minAngle); } + +void vpServoPololuMaestro::setPositionAngle(float targetAngle, unsigned short speed) +{ + if ((this->m_minAngle <= targetAngle) && (targetAngle <= this->m_maxAngle)) { + int targetPWM = angle2PWM(targetAngle); + setPositionPWM(targetPWM, speed); + } + else { + throw(vpRobotException(vpRobotException::positionOutOfRangeError, + "given position: %d is outside of the servo range. You can check the range using the method getRangeAngle()", targetAngle)); + + } +} + +void vpServoPololuMaestro::setPositionPWM(int targetPWM, unsigned short speed) +{ + if ((this->m_minPWM <= targetPWM) && (targetPWM <= this->m_maxPWM)) { + this->setSpeed(speed); + // std::cout << "position (PWM):"<setTargetCP(this->m_channel, targetPWM); + } + else { + throw(vpRobotException(vpRobotException::positionOutOfRangeError, + "given position: %d is outside of the servo range. You can check the range using the method getRangePWM()", targetPWM)); + } +} + +void vpServoPololuMaestro::setSpeed(unsigned short speed) +{ + if (speed <= 1000) { + m_interface->setSpeedCP(this->m_channel, speed); + this->m_speed = speed; + } + else { + throw(vpRobotException(vpRobotException::positionOutOfRangeError, + "given speed : %d is outside of the servo speed range. range is from 0 to 1000", speed)); + } +} + + +void vpServoPololuMaestro::setSpeedDegS(float speedDegS) +{ + unsigned short speed = + (unsigned short)abs(this->degSToSpeed(speedDegS)); // making sure the speed is positive and convert it tu ushort + + this->setSpeed(speed); +} + +void vpServoPololuMaestro::setVelocityCmd(short velocityCmdSpeed) +{ + if (velocityCmdSpeed <= 1000) { + this->m_speed = abs(velocityCmdSpeed); + if (velocityCmdSpeed >= 0) { + this->m_velocityDirection = m_maxPWM; + } + else { + this->m_velocityDirection = m_minPWM; + } + this->m_FlagVelCmdRunning = true; + } + else { + throw(vpRobotException(vpRobotException::positionOutOfRangeError, + "given velocityCmdSpeed : %d is outside of the servo speed range. range is from 0 to 1000", velocityCmdSpeed)); + } +} + +float vpServoPololuMaestro::speedToDeGS(short speed) { return (speed * 100) * (this->m_rangeAngle / this->m_rangePWM); } + + +void vpServoPololuMaestro::stopVelCmd() +{ + if (m_verbose) { + std::cout << "stoping vel cmd \n"; + } + this->m_FlagVelCmdRunning = false; + + std::this_thread::sleep_for(10 * millis); + this->setPositionPWM(this->getPosition()); +} + + +void vpServoPololuMaestro::VelocityCmdThread() +{ + int antispamCounter = 0; + while (true) { + if (this->m_FlagVelCmdRunning) { + this->getPosition(); + if (int(this->m_position) != (this->m_velocityDirection)) { + this->setPositionPWM(this->m_velocityDirection, this->m_speed); + } + else { + if (m_verbose) { + std::cout << "edge of range reach" << std::endl; + } + } + } + else { + if (antispamCounter == 100) { + std::cout << "waiting" + << "flag is : " << this->m_FlagVelCmdRunning << std::endl; + antispamCounter = 0; + } + else { + antispamCounter++; + } + } + std::this_thread::sleep_for(10 * millis); + } + + throw(vpRobotException(vpRobotException::lowLevelError, "Vel cmd stopped unexpectedly ")); + + +} + + +#elif !defined(VISP_BUILD_SHARED_LIBS) +// Work around to avoid warning: libvisp_robot.a(vpServoPololuMaestro.cpp.o) has no symbols +void dummy_vpServoPololuMaestro() { }; +#endif diff --git a/modules/robot/test/pololu-maestro/testPololuMaestroPosition.cpp b/modules/robot/test/pololu-maestro/testPololuMaestroPosition.cpp new file mode 100644 index 0000000000..4793819af5 --- /dev/null +++ b/modules/robot/test/pololu-maestro/testPololuMaestroPosition.cpp @@ -0,0 +1,97 @@ +/* + * ViSP, open source Visual Servoing Platform software. + * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * + * This software is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * See the file LICENSE.txt at the root directory of this source + * distribution for additional information about the GNU GPL. + * + * For using ViSP with software that can not be combined with the GNU + * GPL, please contact Inria about acquiring a ViSP Professional + * Edition License. + * + * See https://visp.inria.fr for more information. + * + * This software was developed at: + * Inria Rennes - Bretagne Atlantique + * Campus Universitaire de Beaulieu + * 35042 Rennes Cedex + * France + * + * If you have questions regarding the use of this file, please contact + * Inria at visp@inria.fr + * + * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE + * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. + * + * Description: + * Common test for Pololu Maestro PanTiltUnit position control. + */ + +#include + +#include + +#ifdef VISP_HAVE_RAPA_POLOLU_MAESTRO + +#include "RPMSerialInterface.h" +#include +#include +#include +#include +#include + +int main() +{ + std::chrono::seconds sec(1); + + // Create the interface. + std::string error_msg; + const std::string &dev = "/dev/ttyACM0"; + RPM::SerialInterface *serialInterface = RPM::SerialInterface::createSerialInterface(dev, 9600, &error_msg); + + std::cout << error_msg; + std::cout << "Serial is: " << dev << " Started!\n"; + std::cout << serialInterface->isOpen() << "\n"; + + // Checking that the serial connection is open. + if (!serialInterface->isOpen()) { + std::cout << "Serial Communication Failed!\n"; + return -1; + } + + // Creating the servo object. + vpServoPololuMaestro servo1(serialInterface, 0); + + // Getting the ranges of the servo. + int min; + int max; + int range; + + servo1.getRangePWM(min, max, range); + std::cout << "Ranges are, min: " << min << " max: " << max << " range: " << range < + +#include + +#ifdef VISP_HAVE_RAPA_POLOLU_MAESTRO + +#include "RPMSerialInterface.h" +#include +#include +#include +#include +#include + +int main() +{ + std::chrono::seconds sec(1); + + // Create the interface. + std::string error_msg; + const std::string &dev = "/dev/ttyACM0"; + RPM::SerialInterface *serialInterface = RPM::SerialInterface::createSerialInterface(dev, 9600, &error_msg); + + std::cout << error_msg; + std::cout << "Serial is: " << dev << " Started!\n"; + std::cout << serialInterface->isOpen() << "\n"; + + // Checking that the serial connection is open. + if (!serialInterface->isOpen()) { + std::cout << "Serial Communication Failed!\n"; + return -1; + } + + // Creating the servo object. + vpServoPololuMaestro servo1(serialInterface, 0); + + // Servo objet will first move in one direction at a velocity of 10 for 3 sec and move back in the other direction for 3sec. + servo1.setVelocityCmd(10); + std::this_thread::sleep_for(3 * sec); + servo1.setVelocityCmd(-10); + std::this_thread::sleep_for(3 * sec); + + + // Adding a second servo to the test. + vpServoPololuMaestro servo2(serialInterface, 1); + + // Both servo ervo objet will first move in one direction at a velocity of 10 for 3 sec and move back in the other direction for 3sec. + servo1.setVelocityCmd(10); + servo2.setVelocityCmd(10); + std::this_thread::sleep_for(3 * sec); + servo1.setVelocityCmd(-10); + servo2.setVelocityCmd(-10); + std::this_thread::sleep_for(3 * sec); + + // Stopping the velocity command. + servo1.stopVelCmd(); + servo2.stopVelCmd(); +} + +#else +int main() { std::cout << "ViSP doesn't support Rapa Pololu Servo 3rd party library" << std::endl; } +#endif