CServoeNeck.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
 
#ifndef CServoeNeck_H
#define CServoeNeck_H
 
#include <mrpt/comms/CInterfaceFTDI.h>
#include <mrpt/core/exceptions.h>
 
namespace mrpt
{
namespace hwdrivers
{
/** A USB-interface for a custom "robotic neck" designed at MAPIR lab.
 * \ingroup mrpt_hwdrivers_grp */
class CServoeNeck : public mrpt::comms::CInterfaceFTDI
{
   public:
        CServoeNeck();
        ~CServoeNeck();
 
        /** Gets the firmware version of the eNeck board.
         * \param out_firmwareVersion: [OUTPUT] A string containing the firmware
         * version.
         * \return Whether or not the procedure succeded.
         */
        bool queryFirmwareVersion(std::string& out_firmwareVersion);
 
        /** Gets the current angle of the servo (in radians within (-pi,pi))
         * \param Angle: [OUT] The current angle.
         * \param Servo: [IN] The id of the servo (in our ATMEGA16, from 0 to 2).
         * \return Whether or not the procedure succeded.
         */
        bool getCurrentAngle(double& angle, const uint8_t servo = 0);
 
        /** Turns the servo up to the specified angle (in radians in the range
         * -pi,pi, other values will be saturated to the maximum or the mininum)
         * \param Angle: the desired angle to turn.
         * \param Servo: the id of the servo to move (in our ATMEGA16, from 0 to
         * 2).
         * \param Fast: indicates if the servo must reach the angle at maximum
         * speed
         * \return Whether or not the procedure succeded.
         */
        bool setAngle(double angle, const uint8_t servo = 0, bool fast = false);
 
        /** Turns the servo up to the specified angle (in radians in the range
         * -pi,pi, other values will be saturated to the maximum or the mininum)
         * \param Angle: the desired angle to turn.
         * \param Servo: the id of the servo to move (in our ATMEGA16, from 0 to
         * 2).
         * \param Speed: indicates the speed of the servo
         * \return Whether or not the procedure succeded.
         */
        bool setAngleAndSpeed(
                double angle, const uint8_t servo, const uint8_t speed);
 
        /** Turns the servo up to the specified angle (in radians in the range
         * -pi,pi) filtered by average with the last N specified angles.
         * \param Angle: the new desired angle to turn.
         * \param Servo: the id of the servo to move (in our ATMEGA16, from 0 to
         * 2).
         * \param Fast: indicates if the servo must reach the angle at maximum
         * speed
         * \return Whether or not the procedure succeded.
         */
        bool setAngleWithFilter(
                double angle, const uint8_t servo = 0, bool fast = false);
 
        /** Disables the servo so the neck will be loose.
         * \param Servo: the id of the servo to move (in our ATMEGA16, from 0 to
         * 2).
         * \return Whether or not the procedure succeded.
         */
        bool disableServo(const uint8_t servo = 0);
 
        /** Enables the servo so the neck will be tight.
         * \param Servo: the id of the servo to move (in our ATMEGA16, from 0 to
         * 2).
         * \return Whether or not the procedure succeded.
         */
        bool enableServo(const uint8_t servo = 0);
 
        /** Centers the servo at zero position
         */
        bool center(const uint8_t servo = 0);
 
        /** Gets the truncate factor of the turn
         */
        double getTruncateFactor() { return m_TruncateFactor; }
        /** Gets the truncate factor of the turn
         */
        void setTruncateFactor(const double factor)
        {
                ASSERT_(factor > 0 && factor < 1);
                m_TruncateFactor = factor;
        }
 
        /** Gets the truncate factor of the turn
         */
        void setNumberOfPreviousAngles(const unsigned int number)
        {
                m_NumPrevAngles = number;
        }
 
        /** Gets the truncate factor of the turn
         */
        unsigned int getNumberOfPreviousAngles() { return m_NumPrevAngles; }
        /**Load the Offset values for each servo
         */
        void setOffsets(float offset0, float offset1, float offset2);
 
   protected:
        /** A copy of the device serial number (to open the USB FTDI chip). */
        std::string m_usbSerialNumber;
        /** The value set in the ICR register within the ATMEGA16 controller. */
        double m_MaxValue;
        /** The range of turn of the servo will be truncated to
         * "+-m_truncate_factor*(pi/2)". */
        double m_TruncateFactor;
        /** A vector containing the last N angles which where passed to the servo
         * (for averaging) */
        std::deque<double> m_PrevAngles;
        /** Number of previous angles to store for averaging */
        unsigned int m_NumPrevAngles;
        /** The offset used for each servo */
        std::vector<float> m_offsets;
 
        bool setRegisterValue(
                const uint16_t value, const uint8_t servo = 0, bool fast = false);
        bool setRegisterValueAndSpeed(
                const uint16_t value, const uint8_t servo, const uint16_t speed);
        bool getRegisterValue(uint16_t& value, const uint8_t servo = 0);
 
   private:
        /** Converts from a decimal angle (in radians) to the corresponding register
         * value for the ATMEGA16 controller (for inner use only).
         * \param The angle to convert.
         * \return The value of the register to send.
         */
        unsigned int angle2RegValue(const double angle);  // Angle in rad
 
        /** Converts from a certain value of the ATMEGA16 PWM register to the
         * corresponding decimal angle (for inner use only).
         * \param The value to convert.
         * \return The corresponding angle.
         */
        double regValue2angle(const uint16_t value);
 
        /** Tries to connect to the USB device (if disconnected).
         * \return True on connection OK, false on error.
         */
        bool checkConnectionAndConnect();
 
};  // End of class
 
}  // namespace hwdrivers
 
}  // namespace mrpt
 
#endif