c_model_ethercat_client.cpp
Go to the documentation of this file.
00001 #include "robotiq_c_model_control/c_model_ethercat_client.h"
00002 
00003 #include "robotiq_ethercat/ethercat_manager.h"
00004 
00005 // See Robotiq's documentation for the register mapping
00006 
00007 // An effort to keep the lines less than 100 char long
00008 namespace robotiq_c_model_control
00009 {
00010 
00011 CModelEtherCatClient::CModelEtherCatClient(robotiq_ethercat::EtherCatManager& manager, 
00012                                                     int slave_no)
00013   : manager_(manager)
00014   , slave_no_(slave_no)
00015 {}
00016 
00017 /*
00018   See support.robotiq.com -> manual for the register output meanings
00019 */
00020 void CModelEtherCatClient::writeOutputs(const GripperOutput& output)
00021 {
00022   uint8_t map[6] = {0}; // array containing all 6 output registers
00023 
00024   // Pack the Action Request register byte
00025   map[0] = (output.rACT & 0x1) | ((output.rGTO << 0x3) & 0x8) | ((output.rATR << 0x4) & 0x10);
00026   // registers 1 & 2 reserved by Robotiq
00027   map[3] = output.rPR;
00028   map[4] = output.rSP;
00029   map[5] = output.rFR;
00030 
00031   for (unsigned i = 0; i < 6; ++i)
00032   {
00033     manager_.write(slave_no_, i, map[i]);
00034   }
00035 }
00036 
00037 CModelEtherCatClient::GripperInput CModelEtherCatClient::readInputs() const
00038 {
00039   uint8_t map[6];
00040 
00041   for (unsigned i = 0; i < 6; ++i)
00042   {
00043     map[i] = manager_.readInput(slave_no_, i);
00044   }
00045 
00046   // Decode Input Registers
00047   GripperInput input;
00048   input.gACT = map[0] & 0x1;
00049   input.gGTO = (map[0] >> 0x3) & 0x1;
00050   input.gSTA = (map[0] >> 0x4) & 0x3;
00051   input.gOBJ = (map[0] >> 0x6) & 0x3;
00052   // map[1] is reserved by the protocol
00053   input.gFLT = map[2] & 0xF;
00054   input.gPR = map[3];
00055   input.gPO = map[4];
00056   input.gCU = map[5];
00057 
00058   return input;
00059 }
00060 
00061 CModelEtherCatClient::GripperOutput CModelEtherCatClient::readOutputs() const
00062 {
00063   uint8_t map[6];
00064   for (unsigned i = 0; i < 6; ++i)
00065   {
00066     map[i] = manager_.readOutput(slave_no_, i);
00067   }
00068 
00069   GripperOutput output;
00070   output.rACT = map[0] & 1;
00071   output.rGTO = (map[0] >> 0x3) & 0x1;
00072   output.rATR = (map[0] >> 0x4) & 0x1;
00073   output.rPR = map[3];
00074   output.rSP = map[4];
00075   output.rFR = map[5];
00076 
00077   return output;
00078 }
00079 } // end of robotiq_c_model_control namespace


robotiq_c_model_control
Author(s): Nicolas Lauzier (Robotiq inc.)
autogenerated on Thu Aug 27 2015 14:44:22