dsa.h

Go to the documentation of this file.

/*
 * Copyright (c) 2008 SCHUNK GmbH & Co. KG
 * Copyright (c) 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

//======================================================================
//======================================================================

#ifndef DSA_h_
#define DSA_h_

#include "sdhlibrary_settings.h"

//----------------------------------------------------------------------
// System Includes - include with <>
//----------------------------------------------------------------------

#include <assert.h>

//----------------------------------------------------------------------
// Project Includes - include with ""
//----------------------------------------------------------------------

#include "sdhexception.h"

#include "dbg.h"
#if SDH_USE_VCC
# include "rs232-vcc.h"
#else
# include "rs232-cygwin.h"
#endif
#include "simpletime.h"
#include "crc.h"

//----------------------------------------------------------------------
// Defines, enums, unions, structs
//----------------------------------------------------------------------

NAMESPACE_SDH_START

#define DSA_MAX_PREAMBLE_SEARCH (2*3*(6*(14+13)) + 16)

//----------------------------------------------------------------------
// Global variables (declarations)
//----------------------------------------------------------------------


//----------------------------------------------------------------------
// External functions (function declarations)
//----------------------------------------------------------------------


//----------------------------------------------------------------------
// Function prototypes (function declarations)
//----------------------------------------------------------------------


//----------------------------------------------------------------------
// Class declarations
//----------------------------------------------------------------------




class VCC_EXPORT cDSAException: public cSDHLibraryException
{
public:
  cDSAException(cMsg const & _msg)
    : cSDHLibraryException("cDSAException", _msg)
  {}
};
//======================================================================


class VCC_EXPORT cDSA
{
public:
  typedef UInt16 tTexel;

  enum eDSAErrorCode
  {
    E_SUCCESS,
    E_NOT_AVAILABLE,
    E_NO_SENSOR,
    E_NOT_INITIALIZED,
    E_ALREADY_RUNNING,
    E_FEATURE_NOT_SUPPORTED,
    E_INCONSISTENT_DATA,
    E_TIMEOUT,
    E_READ_ERROR,
    E_WRITE_ERROR,
    E_INSUFFICIENT_RESOURCES,
    E_CHECKSUM_ERROR,
    E_CMD_NOT_ENOUGH_PARAMS,
    E_CMD_UNKNOWN,
    E_CMD_FORMAT_ERROR,
    E_ACCESS_DENIED,
    E_ALREADY_OPEN,
    E_CMD_FAILED,
    E_CMD_ABORTED,
    E_INVALID_HANDLE,
    E_DEVICE_NOT_FOUND,
    E_DEVICE_NOT_OPENED,
    E_IO_ERROR,
    E_INVALID_PARAMETER,
    E_INDEX_OUT_OF_BOUNDS,
    E_CMD_PENDING,
    E_OVERRUN,
    E_RANGE_ERROR
  };


#if SDH_USE_VCC
#pragma pack(push,1)  // for VCC (MS Visual Studio) we have to set the necessary 1 byte packing with this pragma
#endif
  struct sControllerInfo
  {
    UInt16 error_code;
    UInt32 serial_no;
    UInt8  hw_version;
    UInt16 sw_version;
    UInt8  status_flags;
    UInt8  feature_flags;
    UInt8  senscon_type;
    UInt8  active_interface;
    UInt32 can_baudrate;
    UInt16 can_id;
  }   SDH__attribute__((__packed__)); // for gcc we have to set the necessary 1 byte packing with this attribute

  struct sSensorInfo
  {
    UInt16 error_code;
    UInt16 nb_matrices;
    UInt16 generated_by;
    UInt8  hw_revision;
    UInt32 serial_no;
    UInt8  feature_flags;
  } SDH__attribute__((__packed__)); // for gcc we have to set the necessary 1 byte packing with this attribute

  struct sMatrixInfo
  {
    UInt16 error_code;
    float  texel_width;
    float  texel_height;
    UInt16 cells_x;
    UInt16 cells_y;
    UInt8  uid[6];
    UInt8  reserved[2];
    UInt8  hw_revision;

    float  matrix_center_x;
    float  matrix_center_y;
    float  matrix_center_z;

    float  matrix_theta_x;
    float  matrix_theta_y;
    float  matrix_theta_z;
    float  fullscale;
    UInt8  feature_flags;
  } SDH__attribute__((__packed__)); // for gcc we have to set the necessary 1 byte packing with this attribute

  struct sSensitivityInfo
  {
    UInt16 error_code;  
    UInt8  adj_flags;   



    float  cur_sens;    

    float  fact_sens;   


  } SDH__attribute__((__packed__)); // for gcc we have to set the necessary 1 byte packing with this attribute

#if SDH_USE_VCC
#pragma pack(pop)   // for VCC (MS Visual Studio) restore normal packing
#endif


  struct VCC_EXPORT sTactileSensorFrame
  {
    UInt32  timestamp;  
    UInt8   flags;      
    tTexel* texel;      

    sTactileSensorFrame(void)
    {
      texel = NULL;
    }
  };

  struct sContactInfo
  {
    double force;  
    double area;   
    double cog_x;  
    double cog_y;  
  };


protected:

#if SDH_USE_VCC
#pragma pack(push,1)    // for VCC (MS Visual Studio) we have to set the necessary 1 byte packing with this pragma
#endif
  struct sResponse
  {
    UInt8   packet_id;
    UInt16  size;
    UInt8*  payload;
    Int32   max_payload_size;

    sResponse(UInt8* _payload, int _max_payload_size)
    {
      payload = _payload;
      max_payload_size = _max_payload_size;
    }
  } SDH__attribute__((__packed__));  // for gcc we have to set the necessary 1 byte packing with this attribute
#if SDH_USE_VCC
#pragma pack(pop)   // for VCC (MS Visual Studio) restore normal packing
#endif

  friend VCC_EXPORT std::ostream &operator<<(std::ostream &stream, cDSA::sResponse const &response);

  cDBG dbg;

  cRS232 comm_interface;

  bool do_RLE;

  sControllerInfo controller_info;

  sSensorInfo     sensor_info;

  sMatrixInfo*    matrix_info;

  sTactileSensorFrame frame;

  int nb_cells;

  int* texel_offset;

  long read_timeout_us;

  cSimpleTime start_pc;
  UInt32      start_dsa;

  tTexel contact_area_cell_threshold;

  tTexel contact_force_cell_threshold;

  double force_factor;


  double calib_pressure;  // N/(mm*mm)
  double calib_voltage;   // "what the DSA reports:" ~mV


  void WriteCommandWithPayload(UInt8 command, UInt8* payload, UInt16 payload_len)
  throw (cDSAException*, cSDHErrorCommunication*);

  inline void WriteCommand(UInt8 command)
  {
    WriteCommandWithPayload(command, NULL, 0);
  }
  //----------------------------------------------------------------------

  void ReadResponse(sResponse* response, UInt8 command_id)
  throw (cDSAException*, cSDHErrorCommunication*);

  void ReadControllerInfo(sControllerInfo* _controller_info)
  throw (cDSAException*, cSDHErrorCommunication*);


  void ReadSensorInfo(sSensorInfo* _sensor_info)
  throw (cDSAException*, cSDHErrorCommunication*);


  void ReadMatrixInfo(sMatrixInfo* _matrix_info)
  throw (cDSAException*, cSDHErrorCommunication*);


  void ReadFrame(sTactileSensorFrame* frame_p)
  throw (cDSAException*, cSDHErrorCommunication*);


  void QueryControllerInfo(sControllerInfo* _controller_info)
  throw (cDSAException*, cSDHErrorCommunication*);


  void QuerySensorInfo(sSensorInfo* _sensor_info)
  throw (cDSAException*, cSDHErrorCommunication*);


  void QueryMatrixInfo(sMatrixInfo* _matrix_info, int matrix_no)
  throw (cDSAException*, cSDHErrorCommunication*);


  void QueryMatrixInfos(void)
  throw (cDSAException*, cSDHErrorCommunication*);


  void ParseFrame(sResponse* response, sTactileSensorFrame* frame_p)
  throw (cDSAException*);


public:
  cDSA(int debug_level = 0, int port = 1, char const* device_format_string = "/dev/ttyS%d");


  ~cDSA();


  inline sControllerInfo const & GetControllerInfo(void) const
  {
    return controller_info;
  }
  //-----------------------------------------------------------------

  inline sSensorInfo const & GetSensorInfo(void) const
  {
    return sensor_info;
  }
  //-----------------------------------------------------------------

  inline sMatrixInfo const & GetMatrixInfo(int m) const
  {
    assert(0 <= m  &&  m <= (int) sensor_info.nb_matrices);
    return matrix_info[m];
  }
  //-----------------------------------------------------------------

  inline sTactileSensorFrame const & GetFrame() const
  {
    return frame;
  }
  //-----------------------------------------------------------------

  inline sTactileSensorFrame const & UpdateFrame()
  {
    ReadFrame(&frame);
    return frame;
  }
  //-----------------------------------------------------------------

  void Open(void)
  throw (cDSAException*, cSDHErrorCommunication*);

  void Close(void)
  throw (cDSAException*, cSDHErrorCommunication*);


  void SetFramerate(UInt16 framerate, bool do_RLE = true, bool do_data_acquisition = true)
  throw (cDSAException*, cSDHErrorCommunication*);

  void SetFramerateRetries(UInt16 framerate, bool do_RLE = true, bool do_data_acquisition = true, unsigned int retries = 0, bool ignore_exceptions = false)
  throw (cDSAException*, cSDHErrorCommunication*);


  sSensitivityInfo GetMatrixSensitivity(int matrix_no)
  throw (cDSAException*, cSDHErrorCommunication*);


  void SetMatrixSensitivity(int matrix_no,
                            double sensitivity,
                            bool do_all_matrices = false,
                            bool do_reset = false,
                            bool do_persistent = false)
  throw (cDSAException*, cSDHErrorCommunication*);


  void SetMatrixThreshold(int matrix_no,
                          UInt16 threshold,
                          bool do_all_matrices = false,
                          bool do_reset = false,
                          bool do_persistent = false)
  throw (cDSAException*, cSDHErrorCommunication*);

  UInt16 GetMatrixThreshold(int matrix_no)
  throw (cDSAException*, cSDHErrorCommunication*);

  tTexel GetTexel(int m, int x, int y) const;



  inline int GetMatrixIndex(int fi, int part)
  {
    return fi * 2 + part;
  }


  inline unsigned long GetAgeOfFrame(sTactileSensorFrame* frame_p)
  {
    return ((unsigned long)(start_pc.Elapsed() * 1000.0)) - (frame_p->timestamp - start_dsa);
  }

  double GetContactArea(int m);

private:
  double VoltageToPressure(double voltage);

  void ReadAndCheckErrorResponse(char const* msg, UInt8 command_id)
  throw (cDSAException*, cSDHErrorCommunication*);


public:
  sContactInfo GetContactInfo(int m);


  static char const* ErrorCodeToString(eDSAErrorCode error_code);
  static char const* ErrorCodeToString(UInt16 error_code)
  {
    return ErrorCodeToString((eDSAErrorCode) error_code);
  }

private:
  bool acquiring_single_frame;

  void FlushInput(long timeout_us_first, long timeout_us_subsequent);
}; // end of class cDSA
//----------------------------------------------------------------------
//----------------------------------------------------------------------


VCC_EXPORT std::ostream &operator<<(std::ostream &stream,  cDSA::sControllerInfo const &controller_info);


VCC_EXPORT std::ostream &operator<<(std::ostream &stream,  cDSA::sSensorInfo const &sensor_info);


VCC_EXPORT std::ostream &operator<<(std::ostream &stream,  cDSA::sMatrixInfo const &matrix_info);


VCC_EXPORT std::ostream &operator<<(std::ostream &stream,  cDSA::sResponse const &response);


VCC_EXPORT std::ostream &operator<<(std::ostream &stream,  cDSA const &dsa);


NAMESPACE_SDH_END

#endif


//======================================================================
/*
{
  Here are some settings for the emacs/xemacs editor (and can be safely ignored):
  (e.g. to explicitely set C++ mode for *.h header files)

  Local Variables:
  mode:C
  mode:ELSE
  End:
}
*/
//======================================================================}