bool hormodular::SerialModularRobotInterface::sendJointValuesSerial ( std::vector< float > joint_values ) [private]

Sends the commands required for setting the joint position values on the modular robots.

{
    if ( serialPort && serialPort->IsOpen() )
    {
        if ( joint_values.size() <= 8)
        {
            //-- Convert joint position to servo values [0-180]
            for (int i = 0; i < joint_values.size(); i++)
            {
                joint_values[i]+=90;
                if (joint_values[i] < 0) joint_values[i] = 0;
                if (joint_values[i] > 180) joint_values[i] = 180;
            }


            SerialPort::DataBuffer outputBuff;
            outputBuff.push_back(0x50); //-- 0x50 -> Set pos to all joints

            for (int i = 0; i < joint_values.size(); i++)
                outputBuff.push_back( (char)( (int) (joint_values[i])));

            serialPort->Write( outputBuff );

            return true;
        }
        else
        {
            //-- Temporal solution to more than 8 modules configurations
            std::cerr << "[SerialModRobInterface] Error: more than 8 joints are not supported by "
                      << "the current architecture. Sending just 8 values..." << std::endl;

            SerialPort::DataBuffer outputBuff;
            outputBuff.push_back(0x50); //-- 0x50 -> Set pos to all joints

            for (int i = 0; i < 8; i++)
                outputBuff.push_back( (char)( (int) (joint_values[i])));

            serialPort->Write( outputBuff );

            return false;
        }
    }
    else
    {
        std::cerr << "Robot could not send joints (because it is not connected)"
                     << std::endl;
        return false;
    }
}