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 "AbstractInterface.h"
#include "Response.h"
#include "Types.h"
#include <string>
#include <stdio.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;
}