TactileSensor.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "TactileSensor.h"
 
#include <stdio.h>
 
#include <string>
 
#include "AbstractInterface.h"
#include "Response.h"
#include "Types.h"
 
TactileSensor::TactileSensor(std::shared_ptr<AbstractInterface> interface)
{
    this->interface = interface;
}
 
TactileSensor::~TactileSensor()
{
    // TODO Auto-generated destructor stub
}
 
tac_matrix_info_t
TactileSensor::getMatrixInformation()
{
    Response response = interface->submitCmd(0x30, nullptr, 0, false);
    response.ensureMinLength(12);
    response.ensureSuccess();
 
    tac_matrix_info_t matrix_info;
 
    matrix_info.res_x = response.getShort(2);
    matrix_info.res_y = response.getShort(4);
    matrix_info.cell_width = response.getShort(6);
    matrix_info.cell_height = response.getShort(8);
    matrix_info.fullscale = response.getShort(10);
 
    //delete response;
    return matrix_info;
}
 
void
TactileSensor::printMatrixInfo(tac_matrix_info_t* mi)
{
    printf("res_x = %d, res_y = %d, cell_width = %d, cell_height = %d, fullscale = %X\n",
           mi->res_x,
           mi->res_y,
           mi->cell_width,
           mi->cell_height,
           mi->fullscale);
}
 
void
TactileSensor::printMatrix(short* matrix, int width, int height)
{
    int x, y;
 
    for (y = 0; y < height; y++)
    {
        printf("%03X", matrix[y * width]);
 
        for (x = 1; x < width; x++)
        {
            printf(", %03X", matrix[y * width + x]);
        }
 
        printf("\n");
    }
}
 
FrameData
TactileSensor::readSingleFrame()
{
    unsigned char payload[1];
    payload[0] = 0x00; // FLAGS = 0
    Response response = interface->submitCmd(0x20, payload, sizeof(payload), false);
    return getFrameData(&response);
}
 
PeriodicFrameData
TactileSensor::receicePeriodicFrame()
{
    Response response = interface->receiveWithoutChecks();
 
    if (response.cmdId == 0x21)
    {
        response = interface->receiveWithoutChecks();
    }
 
    if (response.cmdId == 0x00)
    {
        return getPeriodicFrameData(&response);
    }
    else
    {
        throw TransmissionException(
            str(boost::format("Response ID (%02X) does not match submitted command ID (%02X)") %
                (int)response.cmdId % (int)0x00));
    }
}
 
PeriodicFrameData
TactileSensor::getPeriodicFrameData(Response* response)
{
    response->ensureMinLength(7);
 
    unsigned int timestamp = response->getUInt(0);
    int offset = 5;
 
    int count = (response->len - offset) / 2;
    int i;
    std::shared_ptr<std::vector<short>> data;
    data.reset(new std::vector<short>(count, 0));
 
    //short* data = new short[ count ];
    for (i = 0; i < count; i++)
    {
        short value = response->getShort(i * 2 + offset);
        (*data)[i] = value;
    }
 
    return PeriodicFrameData(data, count, timestamp);
}
 
FrameData
TactileSensor::getFrameData(Response* response)
{
    response->ensureMinLength(7);
    response->ensureSuccess();
 
    int offset = 5 + 2;
 
    int count = (response->len - offset) / 2;
    int i;
    std::shared_ptr<std::vector<short>> data;
    data.reset(new std::vector<short>(count, 0));
 
    for (i = 0; i < count; i++)
    {
        short value = response->getShort(i * 2 + offset);
        (*data)[i] = value;
    }
 
    return FrameData(data, count);
}
 
void
TactileSensor::startPeriodicFrameAcquisition(unsigned short delay_ms)
{
    unsigned char payload[3];
    payload[0] = 0x00; // FLAGS = 0
    payload[1] = delay_ms & 0xFF;
    payload[2] = (delay_ms >> 8) & 0xFF;
    interface->fireAndForgetCmd(0x21, payload, sizeof(payload), false);
}
 
void
TactileSensor::stopPeriodicFrameAcquisition(void)
{
    while (1)
    {
        interface->fireAndForgetCmd(0x22, nullptr, 0, false);
        int waitCount = 10;
 
        while (waitCount > 0)
        {
            Response response = interface->receiveWithoutChecks();
 
            if (response.cmdId == 0x22)
            {
                return;
            }
            else
            {
                std::cout
                    << boost::format(
                           "stopPeriodicFrameAcquisition :: Discarding Response with ID 0x%02X") %
                           (int)response.cmdId
                    << std::endl;
            }
 
            waitCount--;
        }
    }
}
 
void
TactileSensor::tareSensorMatrix(unsigned char operation)
{
    unsigned char payload[1];
    payload[0] =
        operation; // OPERATION: 0 = un-tare the sensor matrix using the currently set threshold value, 1 = tare the sensor matrix
    Response response = interface->submitCmd(0x23, payload, sizeof(payload), false);
    response.ensureMinLength(2);
    response.ensureSuccess();
}
 
void
TactileSensor::setAquisitionWindow(unsigned char x1,
                                   unsigned char y1,
                                   unsigned char x2,
                                   unsigned char y2)
{
    unsigned char payload[4];
    payload[0] = x1;
    payload[1] = y1;
    payload[2] = x2;
    payload[3] = y2;
    Response response = interface->submitCmd(0x31, payload, sizeof(payload), false);
    response.ensureMinLength(2);
    response.ensureSuccess();
}
 
int
TactileSensor::setAdvanvedAcquisitionMask(char* mask)
{
    return 0;
}
 
int
TactileSensor::getAcquisitionMask(char** mask, int* mask_len)
{
    return 0;
}
 
void
TactileSensor::setThreshold(short threshold)
{
    unsigned char payload[2];
    payload[0] = threshold & 0xFF;
    payload[1] = (threshold >> 8) & 0xFF;
    Response response = interface->submitCmd(0x34, payload, sizeof(payload), false);
    response.ensureMinLength(2);
    response.ensureSuccess();
}
 
unsigned short
TactileSensor::getThreshold()
{
    Response response = interface->submitCmd(0x35, nullptr, 0, false);
    response.ensureMinLength(2);
    response.ensureSuccess();
    return response.getShort(2);
}
 
void
TactileSensor::setFrontEndGain(unsigned char gain)
{
    /*
     * Adjust the pressure sensitivity of a matrix by setting the gain of the Analog Front-End. The gain can
     * be set using an integer value in the range of 0 to 255, where 0 is the most insensitive (lowest gain)
     * and 255 is the most sensitive (highest gain) setting.
     */
    unsigned char payload[1];
    payload[0] = gain;
    Response response = interface->submitCmd(0x36, payload, sizeof(payload), false);
    response.ensureMinLength(2);
    response.ensureSuccess();
}
 
unsigned char
TactileSensor::getFrontEndGain()
{
    /*
     * Get the currently set analog front-end gain value. The gain is as an integer value ranging from 0 to
     * 255, where 0 is the most insensitive (lowest gain) and 255 is the most sensitive (highest gain) setting.
     */
    Response response = interface->submitCmd(0x37, nullptr, 0, false);
    response.ensureMinLength(3);
    response.ensureSuccess();
    unsigned char gain = response.getByte(2);
    return gain;
}
 
std::string
TactileSensor::getSensorType()
{
    /*
     * Return a string containing the sensor type.
     */
    Response response = interface->submitCmd(0x38, nullptr, 0, false);
    response.ensureMinLength(2);
    response.ensureSuccess();
    std::string type = std::string((char*)response.data.data() + 2, response.len - 2);
    return type;
}
 
float
TactileSensor::readDeviceTemperature()
{
    Response response = interface->submitCmd(0x46, nullptr, 0, false);
    response.ensureMinLength(2);
    short value = (short)response.getShort(2);
    return value * 0.1f;
}
 
tac_system_information_t
TactileSensor::getSystemInformation()
{
    Response response = interface->submitCmd(0x50, nullptr, 0, false);
    response.ensureMinLength(9);
    response.ensureSuccess();
    tac_system_information_t si;
    si.type = response.getByte(2);
    si.hw_rev = response.getByte(3);
    si.fw_version = response.getShort(4);
    si.sn = response.getShort(6);
    return si;
}
 
void
TactileSensor::printSystemInformation(tac_system_information_t si)
{
    int v1 = (si.fw_version & 0xF000) >> 12;
    int v2 = (si.fw_version & 0x0F00) >> 8;
    int v3 = (si.fw_version & 0x00F0) >> 4;
    int v4 = (si.fw_version & 0x000F) >> 0;
    std::cout << boost::format("System Type=%1%, Hardware Revision=%2%, Firmware "
                               "Version=%3%.%4%.%5%.%6% (0x%7$04X), Serial Number=%8%") %
                     (int)si.type % (int)si.hw_rev % v1 % v2 % v3 % v4 % si.fw_version % si.sn
              << std::endl;
}
 
void
TactileSensor::setDeviceTag(std::string tag)
{
    unsigned char* payload = (unsigned char*)tag.c_str();
    Response response = interface->submitCmd(0x51, payload, tag.length(), false);
    response.ensureMinLength(2);
    response.ensureSuccess();
}
 
std::string
TactileSensor::getDeviceTag()
{
    Response response = interface->submitCmd(0x52, nullptr, 0, false);
    response.ensureMinLength(2);
    response.ensureSuccess();
    std::string tag = std::string((char*)response.data.data() + 2, response.len - 2);
    return tag;
}
 
bool
TactileSensor::tryGetDeviceTag(std::string& tag)
{
    Response response = interface->submitCmd(0x52, nullptr, 0, false);
    response.ensureMinLength(2);
 
    if (response.status == E_NOT_AVAILABLE)
    {
        return false;
    }
 
    response.ensureSuccess();
    tag = std::string((char*)response.data.data() + 2, response.len - 2);
    return true;
}
 
int
TactileSensor::loop(char* data, int data_len)
{
    return 0;
}
 
std::string
TactileSensor::getInterfaceInfo()
{
    return interface->toString();
}
 
std::ostream&
operator<<(std::ostream& strm, const TactileSensor& a)
{
    return strm << a.interface;
}