44 MP_GRP_ID_TYPE grp_id;
46 for(grp_id = MP_R1_GID; grp_id < MP_S3_GID; grp_id++)
48 if(groupNo == mpCtrlGrpId2GrpNo(grp_id))
67 long maxSpeedPulse[MP_GRP_AXES_NUM];
75 printf(
"Group %d: Num Axes %d\n", groupNo, numAxes);
82 memset(ctrlGroup, 0x00,
sizeof(
CtrlGroup));
126 for (i = 0; i < MP_GRP_AXES_NUM; i += 1)
133 ctrlGroup->
inc_q.
q_lock = mpSemBCreate(SEM_Q_FIFO, SEM_FULL);
139 for(i=0; i<MP_GRP_AXES_NUM; i++)
147 memset(maxSpeedPulse, 0x00,
sizeof(maxSpeedPulse));
148 for(i=0; i<MP_GRP_AXES_NUM; i++)
152 printf(
"axisType[%d]: ", groupNo);
153 for (i = 0; i < MP_GRP_AXES_NUM; i++)
167 printf(
"pulse->unit[%d]: ", groupNo);
168 for (i = 0; i < MP_GRP_AXES_NUM; i++)
179 printf(
"maxInc[%d] (in motoman joint order): %d, %d, %d, %d, %d, %d, %d\r\n",
185 printf(
"maxSpeed[%d] (in ros joint order): %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f\r\n",
215 MP_CTRL_GRP_SEND_DATA sData;
216 MP_PULSE_POS_RSP_DATA pulse_data;
217 float rosAnglePos[MP_GRP_AXES_NUM];
220 memset(pulsePos, 0, MAX_PULSE_AXES*
sizeof(
long));
225 case MP_R1_GID: sData.sCtrlGrp = 0;
break;
226 case MP_R2_GID: sData.sCtrlGrp = 1;
break;
227 case MP_R3_GID: sData.sCtrlGrp = 2;
break;
228 case MP_R4_GID: sData.sCtrlGrp = 3;
break;
229 case MP_B1_GID: sData.sCtrlGrp = 8;
break;
230 case MP_B2_GID: sData.sCtrlGrp = 9;
break;
231 case MP_B3_GID: sData.sCtrlGrp = 10;
break;
232 case MP_B4_GID: sData.sCtrlGrp = 11;
break;
233 case MP_S1_GID: sData.sCtrlGrp = 16;
break;
234 case MP_S2_GID: sData.sCtrlGrp = 17;
break;
235 case MP_S3_GID: sData.sCtrlGrp = 18;
break;
237 printf(
"Failed to get pulse feedback position\nInvalid groupId: %d\n", ctrlGroup->
groupId);
242 status = mpGetPulsePos (&sData,&pulse_data);
245 printf(
"Failed to get pulse position (command): %u\n", status);
250 for (i=0; i<MAX_PULSE_AXES; ++i)
251 pulsePos[i] = pulse_data.lPos[i];
259 rosAnglePos[3] += -rosAnglePos[1] + rosAnglePos[2];
274 MP_CTRL_GRP_SEND_DATA sData;
275 #ifndef DUMMY_SERVO_MODE 276 MP_FB_PULSE_POS_RSP_DATA pulse_data;
278 MP_PULSE_POS_RSP_DATA pulse_data;
282 memset(pulsePos, 0, MAX_PULSE_AXES*
sizeof(
long));
287 case MP_R1_GID: sData.sCtrlGrp = 0;
break;
288 case MP_R2_GID: sData.sCtrlGrp = 1;
break;
289 case MP_R3_GID: sData.sCtrlGrp = 2;
break;
290 case MP_R4_GID: sData.sCtrlGrp = 3;
break;
291 case MP_B1_GID: sData.sCtrlGrp = 8;
break;
292 case MP_B2_GID: sData.sCtrlGrp = 9;
break;
293 case MP_B3_GID: sData.sCtrlGrp = 10;
break;
294 case MP_B4_GID: sData.sCtrlGrp = 11;
break;
295 case MP_S1_GID: sData.sCtrlGrp = 16;
break;
296 case MP_S2_GID: sData.sCtrlGrp = 17;
break;
297 case MP_S3_GID: sData.sCtrlGrp = 18;
break;
299 printf(
"Failed to get pulse feedback position\nInvalid groupId: %d\n", ctrlGroup->
groupId);
303 #ifndef DUMMY_SERVO_MODE 305 status = mpGetFBPulsePos (&sData,&pulse_data);
308 printf(
"Failed to get pulse feedback position: %u\n", status);
316 for (i=0; i<MAX_PULSE_AXES; ++i)
323 pulse_data.lPos[dest_axis] -= (int)(pulse_data.lPos[src_axis] * corr->
fCorrectionRatio);
327 mpGetPulsePos(&sData, &pulse_data);
331 for (i=0; i<MAX_PULSE_AXES; ++i)
332 pulsePos[i] = pulse_data.lPos[i];
349 #ifndef DUMMY_SERVO_MODE 350 #if (DX100 || FS100) //Use mpSvsGetVelTrqFb for older controller models 351 MP_GRP_AXES_T dst_vel;
357 memset(&dst_vel, 0x00,
sizeof(MP_GRP_AXES_T));
359 status = mpSvsGetVelTrqFb(dst_vel, NULL);
363 for (i = 0; i < MAX_PULSE_AXES; i += 1)
365 pulseSpeed[i] = dst_vel[ctrlGroup->
groupNo][i] * 0.1;
372 for (i = 0; i< MAX_PULSE_AXES; ++i)
379 pulseSpeed[dest_axis] -= (int)(pulseSpeed[src_axis] * corr->
fCorrectionRatio);
383 #else //DX200 and newer supports the M-register analog feedback (higher precision feedback) 385 MP_IO_INFO registerInfo[MAX_PULSE_AXES * 2];
386 USHORT registerValues[MAX_PULSE_AXES * 2];
387 UINT32 registerValuesLong[MAX_PULSE_AXES * 2];
393 for (i = 0; i < MAX_PULSE_AXES; i += 1)
400 status = mpReadIO(registerInfo, registerValues, MAX_PULSE_AXES * 2);
403 printf(
"Failed to get pulse feedback speed: %u\n", status);
407 for (i = 0; i < MAX_PULSE_AXES; i += 1)
410 registerValuesLong[i * 2] = registerValues[i * 2];
411 registerValuesLong[(i * 2) + 1] = registerValues[(i * 2) + 1];
414 dblRegister = (registerValuesLong[(i * 2) + 1] << 16) | registerValuesLong[i * 2];
419 dblRegister /= 1.0E4;
425 dblRegister /= 1.0E6;
429 pulseSpeed[i] = (long)dblRegister;
433 #else //dummy-servo mode for testing 434 MP_CTRL_GRP_SEND_DATA sData;
435 MP_SERVO_SPEED_RSP_DATA pulse_data;
437 mpGetServoSpeed(&sData, &pulse_data);
440 for (i = 0; i<MAX_PULSE_AXES; ++i)
441 pulseSpeed[i] = pulse_data.lSpeed[i];
452 MP_GRP_AXES_T dst_vel;
453 MP_TRQ_CTL_VAL dst_trq;
457 memset(torqueValues, 0,
sizeof(torqueValues));
458 memset(dst_trq.data, 0,
sizeof(MP_TRQCTL_DATA));
459 dst_trq.unit = TRQ_NEWTON_METER;
461 memset(&dst_vel, 0x00,
sizeof(MP_GRP_AXES_T));
463 status = mpSvsGetVelTrqFb(dst_vel, &dst_trq);
467 for (i = 0; i < MAX_PULSE_AXES; i += 1)
469 torqueValues[i] = (double)dst_trq.data[ctrlGroup->
groupId][i] * 0.000001;
480 float rosPos[MAX_PULSE_AXES])
483 float conversion = 1;
491 for (i = 0; i < ctrlGroup->
numAxes; i++)
498 rosPos[i] = motopulsePos[i - 1] / ctrlGroup->
pulseToRad.
PtoR[i - 1];
508 for (i = rpi = mpi = 0; i < ctrlGroup->
numAxes; i += 1, rpi += 1, mpi += 1)
513 if (mpi >= MAX_PULSE_AXES)
524 rosPos[rpi] = motopulsePos[mpi] / conversion;
529 for (i = 0; i < MAX_PULSE_AXES; i++)
538 rosPos[i] = motopulsePos[i] / conversion;
551 float conversion = 1;
556 memset(motopulsePos, 0x00,
sizeof(
long)*MAX_PULSE_AXES);
562 for (i = 0; i < ctrlGroup->
numAxes; i++)
565 motopulsePos[i] = (int)(radPos[i] * ctrlGroup->
pulseToRad.
PtoR[i]);
567 motopulsePos[6] = (int)(radPos[2] * ctrlGroup->
pulseToRad.
PtoR[6]);
569 motopulsePos[i - 1] = (int)(radPos[i] * ctrlGroup->
pulseToRad.
PtoR[i - 1]);
579 for (i = rpi = mpi = 0; i < ctrlGroup->
numAxes; i += 1, rpi += 1, mpi += 1)
584 if (mpi >= MAX_PULSE_AXES)
595 motopulsePos[mpi] = (int)(radPos[rpi] * conversion);
601 for (i = 0; i < MAX_PULSE_AXES; i++)
610 motopulsePos[i] = (int)(radPos[i] * conversion);
623 for (i = 0; i < MAX_PULSE_AXES; i++)
626 axisConfig |= (0x01 << i);
629 return (UCHAR)axisConfig;
637 return((ctrlGroup->
groupId >= MP_R1_GID) && (ctrlGroup->
groupId <= MP_R4_GID));
BOOL bFeedbackSpeedEnabled
AXIS_TYPE type[MAX_PULSE_AXES]
#define MAX_CONTROLLABLE_GROUPS
BOOL Ros_CtrlGroup_GetPulsePosCmd(CtrlGroup *ctrlGroup, long pulsePos[MAX_PULSE_AXES])
JOINT_FEEDBACK_SPEED_ADDRESSES speedFeedbackRegisterAddress
float PtoR[MAX_PULSE_AXES]
BOOL Ros_CtrlGroup_GetFBPulsePos(CtrlGroup *ctrlGroup, long pulsePos[MAX_PULSE_AXES])
float PtoM[MAX_PULSE_AXES]
float maxSpeed[MP_GRP_AXES_NUM]
void Ros_CtrlGroup_ConvertToMotoPos(CtrlGroup *ctrlGroup, float radPos[MAX_PULSE_AXES], long motopulsePos[MAX_PULSE_AXES])
ULONG cioAddressForAxis[MAX_PULSE_AXES][2]
BOOL Ros_CtrlGroup_GetTorque(CtrlGroup *ctrlGroup, double torqueValues[MAX_PULSE_AXES])
UCHAR Ros_CtrlGroup_GetAxisConfig(CtrlGroup *ctrlGroup)
MP_GRP_ID_TYPE Ros_CtrlGroup_FindGrpId(int groupNo)
BOOL Ros_CtrlGroup_GetFBServoSpeed(CtrlGroup *ctrlGroup, long pulseSpeed[MAX_PULSE_AXES])
BOOL Ros_CtrlGroup_IsRobot(CtrlGroup *ctrlGroup)
MAX_INCREMENT_INFO maxInc
UINT32 maxIncrement[MAX_PULSE_AXES]
FB_AXIS_CORRECTION correction[MAX_PULSE_AXES]
AXIS_MOTION_TYPE axisType
FB_PULSE_CORRECTION_DATA correctionData
PULSE_TO_METER pulseToMeter
CtrlGroup * Ros_CtrlGroup_Create(int groupNo, BOOL bIsLastGrpToInit, float interpolPeriod)
void Ros_CtrlGroup_ConvertToRosPos(CtrlGroup *ctrlGroup, long motopulsePos[MAX_PULSE_AXES], float rosPos[MAX_PULSE_AXES])