CServoeNeck.cpp

Go to the documentation of this file.

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

#include "hwdrivers-precomp.h"  // Precompiled headers

#include <mrpt/comms/net_utils.h>
#include <mrpt/core/bits_math.h>
#include <mrpt/hwdrivers/CServoeNeck.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/serialization/CMessage.h>
#include <cmath>
#include <cstdint>
#include <thread>

using namespace mrpt;
using namespace mrpt::comms;
using namespace mrpt::hwdrivers;
using namespace mrpt::serialization;
using namespace std::literals;

/*-------------------------------------------------------------
                    default constructor
-------------------------------------------------------------*/
CServoeNeck::CServoeNeck()
    : m_usbSerialNumber("eNeck001"),

      m_PrevAngles(0)

{
    m_offsets.resize(3, 0);
}  // end-constructor
/*-------------------------------------------------------------
                    default destructor
-------------------------------------------------------------*/
CServoeNeck::~CServoeNeck() = default;
/*-------------------------------------------------------------
                    queryFirmwareVersion
-------------------------------------------------------------*/
bool CServoeNeck::queryFirmwareVersion(std::string& out_firmwareVersion)
{
    try
    {
        mrpt::serialization::CMessage msg, msgRx;

        // Try to connect to the device:
        if (!checkConnectionAndConnect()) return false;

        msg.type = 0x10;
        archiveFrom(*this).sendMessage(msg);

        if (archiveFrom(*this).receiveMessage(msgRx))
        {
            msgRx.getContentAsString(out_firmwareVersion);
            std::this_thread::sleep_for(200ms);
            return true;
        }
        else
            return false;
    }
    catch (...)
    {
        Close();
        return false;
    }
}  // end-queryFirmwareVersion

/*-------------------------------------------------------------
                    angle2RegValue
-------------------------------------------------------------*/
unsigned int CServoeNeck::angle2RegValue(const double angle /* rad */)
{
    const uint16_t reg =
        1250 + (1000 / M_PI) * (angle - M_PI * 0.5);  // equation: v = s*(a-a0)
    // + v0 where s = 450/pi;
    // a0 = 0 and v0 = 750
    // cout << "Reg: " << reg << endl;
    return (reg);

    // return ( (uint16_t)( (m_MaxValue/20)*(-2*angle/M_PI+1.5) ) ); //
    // equation: v = s*(a-a0) + v0 where s = 450/pi; a0 = 0 and v0 = 750
    // return ( (uint16_t)( (m_MaxValue/20)*(2*angle/M_PI+1.5) ) );         //
    // equation: v = s*(a-a0) + v0 where s = 450/pi; a0 = 0 and v0 = 750
    // return ( (uint16_t)( (900/M_PI)*nangle + 750 ) );            // equation:
    // v
    // =
    // s*(a-a0) + v0 where s = 450/pi; a0 = 0 and v0 = 750
}  // end-angle2RegValue

/*-------------------------------------------------------------
                    regValue2angle
-------------------------------------------------------------*/
double CServoeNeck::regValue2angle(const uint16_t value)
{
    double angle = M_PI * 0.5 + (M_PI / 1000) * (value - 1250);
    return angle;

    // return ( -M_PI*0.5*(20*value/m_MaxValue-1.5) );                  //
    // equation:
    // angle
    // =
    // (pi/2)*(20*OCR/ICR-1.5)
    // return ( -M_PI*0.25+(M_PI/1800)*(value-1050) );                  //
    // equation:
    // angle
    // =
    // (pi/2)*(20*OCR/ICR-1.5)
    // return ( M_PI*0.5*(20*value/m_MaxValue-1.5) );                   //
    // equation:
    // angle
    // =
    // (pi/2)*(20*OCR/ICR-1.5)
    // return ( (value-750)/(900/M_PI) );
}  // end-regValue2angle

/*-------------------------------------------------------------
                    setRegisterValue
-------------------------------------------------------------*/
bool CServoeNeck::setRegisterValue(
    const uint16_t value, const uint8_t servo, bool fast)
{
    try
    {
        if (!isOpen()) return false;

        mrpt::serialization::CMessage msg, msgRx;

        // Send cmd for setting the value of the register:
        // ------------------------------------------------
        if (fast)
            msg.type = 0x15;
        else
            msg.type = 0x11;
        msg.content.resize(3);
        msg.content[2] = (uint8_t)value;  // Low byte
        msg.content[1] = (uint8_t)(value >> 8);  // High byte
        msg.content[0] = servo;  // Servo number

        archiveFrom(*this).sendMessage(msg);
        if (!archiveFrom(*this).receiveMessage(msgRx)) return false;  // Error

        std::this_thread::sleep_for(200ms);
        return true;
    }
    catch (...)
    {
        // Error opening device:
        Close();
        return false;
    }

}  // end-setRegisterValue

/*-------------------------------------------------------------
                    setRegisterValue
-------------------------------------------------------------*/
bool CServoeNeck::setRegisterValueAndSpeed(
    const uint16_t value, const uint8_t servo, const uint16_t speed)
{
    try
    {
        if (!isOpen()) return false;

        mrpt::serialization::CMessage msg, msgRx;

        // Send cmd for setting the value of the register:
        // ------------------------------------------------
        msg.type = 0x16;
        msg.content.resize(5);
        msg.content[4] = (uint8_t)speed;  // Low byte of the speed of the servo
        msg.content[3] =
            (uint8_t)(speed >> 8);  // High byte of the speed of the servo
        msg.content[2] = (uint8_t)value;  // Low byte
        msg.content[1] = (uint8_t)(value >> 8);  // High byte
        msg.content[0] = servo;  // Servo number

        archiveFrom(*this).sendMessage(msg);
        if (!archiveFrom(*this).receiveMessage(msgRx)) return false;  // Error

        std::this_thread::sleep_for(200ms);
        return true;
    }
    catch (...)
    {
        // Error opening device:
        Close();
        return false;
    }

}  // end-setRegisterValue

/*-------------------------------------------------------------
                    getRegisterValue
-------------------------------------------------------------*/
bool CServoeNeck::getRegisterValue(uint16_t& value, const uint8_t servo)
{
    try
    {
        if (!isOpen()) return false;

        mrpt::serialization::CMessage msg, msgRx;

        // Send cmd for obtaining the value of the OCR1A register:
        // --------------------------------------------------------
        msg.type = 0x12;
        msg.content.resize(1);
        msg.content[0] = servo;

        archiveFrom(*this).sendMessage(msg);
        if (archiveFrom(*this).receiveMessage(msgRx))
        {
            if (msgRx.content.size() != 2) return false;

            value = (msgRx.content[0] << 8) + msgRx.content[1];
            return true;
        }
        else
            return false;

        return true;
    }
    catch (...)
    {
        // Error opening device:
        Close();
        return false;
    }

}  // end-getRegisterValue

/*-------------------------------------------------------------
                    getCurrentAngle
-------------------------------------------------------------*/
bool CServoeNeck::getCurrentAngle(double& angle, const uint8_t servo)
{
    uint16_t value;
    if (getRegisterValue(value, servo))
    {
        angle = regValue2angle(value);
        return true;
    }
    else
        return false;
}  // end-getCurrentAngle

/*-------------------------------------------------------------
                    setAngle
-------------------------------------------------------------*/
bool CServoeNeck::setAngle(double angle, const uint8_t servo, bool fast)
{
    // double nangle = -angle;

    // if( nangle < -m_TruncateFactor*M_PI/2 )  nangle =
    // -m_TruncateFactor*M_PI/2;
    // if( nangle > m_TruncateFactor*M_PI/2 )       nangle =
    // m_TruncateFactor*M_PI/2;

    // unsigned int reg = angle2RegValue( nangle );

    // std::cout << "Angle: " << RAD2DEG( nangle ) << " - Reg: " << reg <<
    // std::endl;
    // return setRegisterValue( reg, servo );

    if (angle < -m_TruncateFactor * M_PI / 2)
        angle = -m_TruncateFactor * M_PI / 2;
    if (angle > m_TruncateFactor * M_PI / 2)
        angle = m_TruncateFactor * M_PI / 2;

    unsigned int reg = angle2RegValue(m_offsets[servo] + angle);

    std::cout << "Angle: " << RAD2DEG(angle) << " - Reg: " << reg << std::endl;
    return setRegisterValue(reg, servo, fast);

}  // end-getCurrentAngle

/*-------------------------------------------------------------
                    setAngleAndSpeed
-------------------------------------------------------------*/
bool CServoeNeck::setAngleAndSpeed(
    double angle, const uint8_t servo, const uint8_t speed)
{
    // speed in the range 15/s-250/s
    if (angle < -m_TruncateFactor * M_PI / 2)
        angle = -m_TruncateFactor * M_PI / 2;
    if (angle > m_TruncateFactor * M_PI / 2)
        angle = m_TruncateFactor * M_PI / 2;

    unsigned int reg = angle2RegValue(m_offsets[servo] + angle);
    uint8_t thisSpeed = speed < 15 ? 15 : speed > 250 ? 250 : speed;
    auto delSpeed =
        uint16_t(0.25 * 1000000 / (500 + 1000 * (thisSpeed / 180.0f - 0.5)));
    // cout << "Speed: " << int(speed) << " -> " << delSpeed << endl;
    // std::cout << "Angle: " << RAD2DEG( angle ) << " - Reg: " << reg <<
    // std::endl;
    return setRegisterValueAndSpeed(reg, servo, delSpeed);

}  // end-getCurrentAngle

/*-------------------------------------------------------------
                    setAngleWithFilter
-------------------------------------------------------------*/
bool CServoeNeck::setAngleWithFilter(
    double angle, const uint8_t servo, bool fast)
{
    double nangle = 0;
    if (m_PrevAngles.size() == m_NumPrevAngles &&
        m_NumPrevAngles != 0)  // If the deque is full populated
        m_PrevAngles.erase(
            m_PrevAngles.begin());  // Erase the first angle of the deque

    m_PrevAngles.push_back(angle);  // Push back the new angle

    std::deque<double>::iterator it;
    for (it = m_PrevAngles.begin(); it != m_PrevAngles.end();
         ++it)  // Sum up all the elements in the deque
        nangle += *it;
    nangle /= m_PrevAngles.size();  // Mean angle

    return (setAngle(nangle, servo, fast));
}

/*-------------------------------------------------------------
                    disableServo
-------------------------------------------------------------*/
bool CServoeNeck::disableServo(const uint8_t servo)
{
    try
    {
        if (!isOpen()) return false;

        mrpt::serialization::CMessage msg, msgRx;

        // Send cmd for disabling servo:
        // ----------------------------
        msg.type = 0x13;
        msg.content.resize(1);
        msg.content[0] = servo;  // Servo number

        archiveFrom(*this).sendMessage(msg);
        if (!archiveFrom(*this).receiveMessage(msgRx)) return false;  // Error

        return true;
    }
    catch (...)
    {
        // Error opening device:
        Close();
        return false;
    }

}  // end-getCurrentAngle

/*-------------------------------------------------------------
                    enableServo
-------------------------------------------------------------*/
bool CServoeNeck::enableServo(const uint8_t servo)
{
    try
    {
        if (!isOpen()) return false;

        mrpt::serialization::CMessage msg, msgRx;

        // Send cmd for enabling the servo:
        // --------------------------------
        msg.type = 0x14;
        msg.content.resize(1);
        msg.content[0] = servo;  // Servo number

        archiveFrom(*this).sendMessage(msg);
        if (!archiveFrom(*this).receiveMessage(msgRx)) return false;  // Error

        return true;
    }
    catch (...)
    {
        // Error opening device:
        Close();
        return false;
    }

}  // end-getCurrentAngle

/*-------------------------------------------------------------
                        Center
-------------------------------------------------------------*/
bool CServoeNeck::center(const uint8_t servo)
{
    unsigned int value = angle2RegValue(m_offsets[servo]);
    return setRegisterValue(value, servo);
}  // end-Center

/*-------------------------------------------------------------
                    checkConnectionAndConnect
-------------------------------------------------------------*/
bool CServoeNeck::checkConnectionAndConnect()
{
    if (isOpen()) return true;

    try
    {
        OpenBySerialNumber(m_usbSerialNumber);
        std::this_thread::sleep_for(10ms);
        Purge();
        std::this_thread::sleep_for(10ms);
        SetLatencyTimer(1);
        SetTimeouts(300, 100);
        return true;
    }
    catch (...)
    {
        // Error opening device:
        Close();
        return false;
    }
}  // end-checkConnectionAndConnect

/*-------------------------------------------------------------
                    setOffsets
-------------------------------------------------------------*/
void CServoeNeck::setOffsets(float offset0, float offset1, float offset2)
{
    m_offsets.resize(3);
    m_offsets[0] = offset0;
    m_offsets[1] = offset1;
    m_offsets[2] = offset2;
}