mJoint.hpp

Go to the documentation of this file.

 
#include "bioscara_arm_hardware_driver/mJoint.h"
#include "bioscara_arm_hardware_driver/uI2C.h"
#include "bioscara_arm_hardware_driver/common.h"
 
namespace bioscara_hardware_drivers
{
    template <typename T>
    int Joint::read(const stp_reg_t reg, T &data, u_int8_t &flags)
    {
        size_t size = sizeof(T) + RFLAGS_SIZE;
        char buf[MAX_BUFFER + RFLAGS_SIZE];
        int n = readFromI2CDev(this->handle, reg, buf, size);
        if (n != static_cast<int>(size))
        {
            return -1;
        }
        memcpy(&data, buf, size - RFLAGS_SIZE);
        memcpy(&flags, buf + size - RFLAGS_SIZE, RFLAGS_SIZE);
        return 0;
    }
 
    template <typename T>
    int Joint::write(const stp_reg_t reg, T data, u_int8_t &flags)
    {
        size_t size = sizeof(T) + RFLAGS_SIZE;
        char buf[MAX_BUFFER + RFLAGS_SIZE];
        memcpy(buf, &data, size - RFLAGS_SIZE);
        int rc = writeToI2CDev(this->handle, reg, buf, size - RFLAGS_SIZE, buf + size - RFLAGS_SIZE);
        rc = rc > 0 ? 0 : rc;
        memcpy(&flags, buf + size - RFLAGS_SIZE, RFLAGS_SIZE);
        return rc;
    }
}