48 map[0] = (output.rACT & 0x1) | (output.rMOD << 0x1) & 0x6 | ((output.rGTO << 0x3) & 0x8) | ((output.rATR << 0x4) & 0x10);
51 map[1] = ((output.rICF << 0x2) & 0x4) | ((output.rICS << 0x3) & 0x8);
67 map[10] = output.rSPC;
68 map[11] = output.rFRC;
71 map[12] = output.rPRS;
72 map[13] = output.rSPS;
73 map[14] = output.rFRS;
75 for (
unsigned i = 0; i < 15; ++i)
85 for (
unsigned i = 0; i < 15; ++i)
94 input.gACT = map[0] & 0x1;
95 input.gMOD = (map[0] >> 0x1) & 0x3;
96 input.gGTO = (map[0] >> 0x3) & 0x1;
97 input.gIMC = (map[0] >> 0x4) & 0x3;
98 input.gSTA = (map[0] >> 0x6) & 0x3;
101 input.gDTA = map[1] & 0x3;
102 input.gDTB = (map[1] >> 0x2) & 0x3;
103 input.gDTC = (map[1] >> 0x4) & 0x3;
104 input.gDTS = (map[1] >> 0x6) & 0x3;
107 input.gFLT = map[2] & 0xF;
121 input.gPOC = map[10];
122 input.gCUC = map[11];
125 input.gPRS = map[12];
126 input.gPOS = map[13];
127 input.gCUS = map[14];
135 for (
unsigned i = 0; i < 15; ++i)
141 output.rACT = map[0] & 1;
142 output.rMOD = (map[0] >> 0x1) & 0x6;
143 output.rGTO = (map[0] >> 0x3) & 0x1;
144 output.rATR = (map[0] >> 0x4) & 0x1;
146 output.rICF = (map[1] >> 0x2) & 0x4;
147 output.rICS = (map[1] >> 0x3) & 0x8;
150 output.rPRA = map[3];
151 output.rSPA = map[4];
152 output.rFRA = map[5];
155 output.rPRB = map[6];
156 output.rSPB = map[7];
157 output.rFRB = map[8];
160 output.rPRC = map[9];
161 output.rSPC = map[10];
162 output.rFRC = map[11];
165 output.rPRS = map[12];
166 output.rSPS = map[13];
167 output.rFRS = map[14];
robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotOutput GripperOutput
Robotiq3FGripperEtherCatClient(robotiq_ethercat::EtherCatManager &manager, int slave_no)
Constructs a control interface to a 3F Robotiq gripper on the given ethercat network and the given sl...
uint8_t readOutput(int slave_no, uint8_t channel) const
GripperOutput readOutputs() const
Reads set of output-register values from the gripper.
GripperInput readInputs() const
Reads set of input-register values from the gripper.
void write(int slave_no, uint8_t channel, uint8_t value)
robotiq_ethercat::EtherCatManager & manager_
uint8_t readInput(int slave_no, uint8_t channel) const
void writeOutputs(const GripperOutput &output)
Write the given set of control flags to the memory of the gripper.
robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotInput GripperInput