CServoeNeck.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#pragma once

#include <mrpt/comms/CInterfaceFTDI.h>
#include <mrpt/core/exceptions.h>

namespace mrpt::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() override;

    /** 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{10000};
    /** The range of turn of the servo will be truncated to
     * "+-m_truncate_factor*(pi/2)". */
    double m_TruncateFactor{0.5};
    /** 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{5};
    /** 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 mrpt::hwdrivers