s_model_ethercat_client.cpp
Go to the documentation of this file.
00001 #include "robotiq_s_model_control/s_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_s_model_control
00009 {
00010 SModelEtherCatClient::SModelEtherCatClient(robotiq_ethercat::EtherCatManager& manager, 
00011                                                     int slave_no)
00012   : manager_(manager)
00013   , slave_no_(slave_no)
00014 {}
00015 
00016 /*
00017   See support.robotiq.com -> manual for the register output meanings
00018 */
00019 void SModelEtherCatClient::writeOutputs(const GripperOutput& output)
00020 {
00021   uint8_t map[15] = {0}; // array containing all 15 output registers
00022 
00023   // Pack the Action Request register byte
00024   map[0] = (output.rACT & 0x1) | (output.rMOD << 0x1) & 0x6 | ((output.rGTO << 0x3) & 0x8) | ((output.rATR << 0x4) & 0x10);
00025 
00026   // Pack the Gripper Options register byte
00027   map[1] =  ((output.rICF << 0x2) & 0x4) | ((output.rICS << 0x3) & 0x8);
00028 
00029   // map[2] is empty
00030 
00031   // Requested Position, Speed and Force (Finger A).
00032   map[3]  = output.rPRA;
00033   map[4]  = output.rSPA;
00034   map[5]  = output.rFRA;
00035 
00036   // Finger B
00037   map[6]  = output.rPRB;
00038   map[7]  = output.rSPB;
00039   map[8]  = output.rFRB;
00040 
00041   // Finger C
00042   map[9]  = output.rPRC;
00043   map[10] = output.rSPC;
00044   map[11] = output.rFRC;
00045 
00046   // Scissor Mode
00047   map[12] = output.rPRS;
00048   map[13] = output.rSPS;
00049   map[14] = output.rFRS;
00050 
00051   for (unsigned i = 0; i < 15; ++i)
00052   {
00053     manager_.write(slave_no_, i, map[i]);
00054   }
00055 }
00056 
00057 SModelEtherCatClient::GripperInput SModelEtherCatClient::readInputs() const
00058 {
00059   uint8_t map[15];
00060 
00061   for (unsigned i = 0; i < 15; ++i)
00062   {
00063     map[i] = manager_.readInput(slave_no_, i);
00064   }
00065 
00066   // Decode Input Registers
00067   GripperInput input;
00068 
00069   // Gripper Status
00070   input.gACT = map[0] & 0x1;
00071   input.gMOD = (map[0] >> 0x1) & 0x3;
00072   input.gGTO = (map[0] >> 0x3) & 0x1;
00073   input.gIMC = (map[0] >> 0x4) & 0x3;
00074   input.gSTA = (map[0] >> 0x6) & 0x3;
00075 
00076   // Object Status
00077   input.gDTA = map[1] & 0x3;
00078   input.gDTB = (map[1] >> 0x2) & 0x3;
00079   input.gDTC = (map[1] >> 0x4) & 0x3;
00080   input.gDTS = (map[1] >> 0x6) & 0x3;
00081 
00082   // Fault Status
00083   input.gFLT = map[2] & 0xF;
00084 
00085   // Requested Position, Speed and Force (Finger A).
00086   input.gPRA = map[3];
00087   input.gPOA = map[4];
00088   input.gCUA = map[5];
00089 
00090   // Finger B
00091   input.gPRB = map[6];
00092   input.gPOB = map[7];
00093   input.gCUB = map[8];
00094 
00095   // Finger C
00096   input.gPRC = map[9];
00097   input.gPOC = map[10];
00098   input.gCUC = map[11];
00099 
00100   // Scissor Mode
00101   input.gPRS = map[12];
00102   input.gPOS = map[13];
00103   input.gCUS = map[14];
00104 
00105   return input;
00106 }
00107 
00108 SModelEtherCatClient::GripperOutput SModelEtherCatClient::readOutputs() const
00109 {
00110   uint8_t map[15];
00111   for (unsigned i = 0; i < 15; ++i)
00112   {
00113     map[i] = manager_.readOutput(slave_no_, i);
00114   }
00115 
00116   GripperOutput output;
00117   output.rACT = map[0] & 1;
00118   output.rMOD = (map[0] >> 0x1) & 0x6;
00119   output.rGTO = (map[0] >> 0x3) & 0x1;
00120   output.rATR = (map[0] >> 0x4) & 0x1;
00121 
00122   output.rICF = (map[1] >> 0x2) & 0x4;
00123   output.rICS = (map[1] >> 0x3) & 0x8;
00124 
00125   // Finger A
00126   output.rPRA = map[3];
00127   output.rSPA = map[4];
00128   output.rFRA = map[5];
00129 
00130   // Finger B
00131   output.rPRB = map[6];
00132   output.rSPB = map[7];
00133   output.rFRB = map[8];
00134 
00135   // Finger C
00136   output.rPRC = map[9];
00137   output.rSPC = map[10];
00138   output.rFRC = map[11];
00139 
00140   // Scissor Mode
00141   output.rPRS = map[12];
00142   output.rSPS = map[13];
00143   output.rFRS = map[14];
00144 
00145   return output;
00146 }
00147 } // end of robotiq_s_model_control namespace


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