//-------------------------------------------------------------------
// Get the commanded pulse position in pulse
//-------------------------------------------------------------------
BOOL Ros_CtrlGroup_GetPulsePosCmd(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_AXES])
{
LONG status = 0;
MP_CTRL_GRP_SEND_DATA sData;
MP_PULSE_POS_RSP_DATA pulse_data;
int i;
memset(pulsePos, 0, MAX_PULSE_AXES*sizeof(long)); // clear result, in case of error
// Set the control group
switch(ctrlGroup->groupId)
{
case MP_R1_GID: sData.sCtrlGrp = 0; break;
case MP_R2_GID: sData.sCtrlGrp = 1; break;
case MP_B1_GID: sData.sCtrlGrp = 8; break;
case MP_B2_GID: sData.sCtrlGrp = 9; break;
case MP_S1_GID: sData.sCtrlGrp = 16; break;
case MP_S2_GID: sData.sCtrlGrp = 17; break;
case MP_S3_GID: sData.sCtrlGrp = 18; break;
default:
printf("Failed to get pulse feedback position\nInvalid groupId: %d", ctrlGroup->groupId);
return FALSE;
}
// get the command joint positions
status = mpGetPulsePos (&sData,&pulse_data);
if (0 != status)
{
printf("Failed to get pulse position (command): %u", status);
return FALSE;
}
// assign return value
for (i=0; i<ctrlGroup->numAxes; ++i)
pulsePos[i] = pulse_data.lPos[i];
return TRUE;
}
int handleSys1FunctionRequest(int acceptHandle, char *inBuffer, char *outBuffer, int type, int msgSize) {
int sendRet = 0;
int i = 0;
MP_VAR_INFO varInfo[25];
MP_VAR_DATA varData[25];
MP_IO_INFO ioInfo[25];
MP_IO_DATA ioData[25];
LONG rData[25];
USHORT iorData[25];
MP_MODE_RSP_DATA modeData;
MP_CYCLE_RSP_DATA cycleData;
MP_ALARM_STATUS_RSP_DATA alarmStatusData;
MP_ALARM_CODE_RSP_DATA alarmCodeData;
LONG num;
int ret;
MP_CTRL_GRP_SEND_DATA ctrlGrpSendData;
MP_CART_POS_RSP_DATA cartPosRspData;
MP_PULSE_POS_RSP_DATA pulsePosRspData;
MP_FB_PULSE_POS_RSP_DATA fbPulsePosRspData;
MP_DEG_POS_RSP_DATA_EX degPosRspDataEx;
MP_SERVO_POWER_RSP_DATA servoPowerRspData;
MP_SERVO_POWER_SEND_DATA servoPowerSendData;
MP_STD_RSP_DATA stdRspData;
int32_t controlGroup = 0;
int nowait = NO_WAIT;
int waitforever = WAIT_FOREVER;
switch (type) {
case SYS1_GET_VAR_DATA:
num = getInt32(inBuffer, 12);
if (num < 1 || num > 24) {
fprintf(stderr, "tcpSvr: invalid num for mpGetVarData num = %ld\n", num);
return -1;
}
if (msgSize != 12 + (4 * num)) {
fprintf(stderr, "tcpSvr: invalid msgSize for mpGetVarData = %d != %ld for num = %ld\n", msgSize, 12 + (4 * num), num);
return -1;
}
for (i = 0; i < num; i++) {
varInfo[i].usType = getInt16(inBuffer, 16 + (4 * i));
varInfo[i].usIndex = getInt16(inBuffer, 18 + (4 * i));
}
ret = mpGetVarData(varInfo, rData, num);
setInt32(outBuffer, 0, 4 + num * 4);
setInt32(outBuffer, 4, ret);
for (i = 0; i < num; i++) {
setInt32(outBuffer, 8 + i * 4, rData[i]);
}
sendRet = sendN(acceptHandle, outBuffer, 8 + num * 4, 0);
if (sendRet != 8 + num * 4) {
fprintf(stderr, "tcpSvr: sendRet = %d != 8 + num*4\n", sendRet);
return -1;
}
break;
case SYS1_PUT_VAR_DATA:
num = getInt32(inBuffer, 12);
if (num < 1 || num > 24) {
fprintf(stderr, "tcpSvr: invalid num for mpPutVarData num = %ld\n", num);
return -1;
}
if (msgSize != 12 + (num * 8)) {
fprintf(stderr, "tcpSvr: invalid msgSize for mpPutVarData = %d != %ld for num = %ld\n", msgSize, 12 + (num * 8), num);
return -1;
}
for (i = 0; i < num; i++) {
varData[i].usType = getInt16(inBuffer, 16 + (8 * i));
varData[i].usIndex = getInt16(inBuffer, 18 + (8 * i));
varData[i].ulValue = getInt32(inBuffer, 20 + (8 * i));
}
ret = mpPutVarData(varData, num);
setInt32(outBuffer, 0, 4);
setInt32(outBuffer, 4, ret);
sendRet = sendN(acceptHandle, outBuffer, 8, 0);
if (sendRet != 8) {
fprintf(stderr, "tcpSvr: sendRet = %d != 8\n", sendRet);
return -1;
}
break;
case SYS1_GET_CURRENT_CART_POS:
if (msgSize != 12) {
fprintf(stderr, "tcpSvr: invalid msgSize for mpMotTargetClear = %d != 12\n", msgSize);
return -1;
}
memset(&ctrlGrpSendData, 0, sizeof (ctrlGrpSendData));
memset(&cartPosRspData, 0, sizeof (cartPosRspData));
controlGroup = getInt32(inBuffer, 12);
ctrlGrpSendData.sCtrlGrp = controlGroup;
ret = mpGetCartPos(&ctrlGrpSendData, &cartPosRspData);
setInt32(outBuffer, 0, 54);
setInt32(outBuffer, 4, ret);
for (i = 0; i < 6; i++) {
setInt32(outBuffer, 8 + 4 * i, cartPosRspData.lPos[i]);
}
setInt16(outBuffer, 56, cartPosRspData.sConfig);
sendRet = sendN(acceptHandle, outBuffer, 58, 0);
if (sendRet != 58) {
fprintf(stderr, "tcpSvr: sendRet = %d != 58\n", sendRet);
return -1;
}
break;
case SYS1_GET_CURRENT_PULSE_POS:
if (msgSize != 12) {
fprintf(stderr, "tcpSvr: invalid msgSize for mpMotTargetClear = %d != 12\n", msgSize);
return -1;
}
memset(&ctrlGrpSendData, 0, sizeof (ctrlGrpSendData));
memset(&pulsePosRspData, 0, sizeof (pulsePosRspData));
controlGroup = getInt32(inBuffer, 12);
ctrlGrpSendData.sCtrlGrp = controlGroup;
ret = mpGetPulsePos(&ctrlGrpSendData, &pulsePosRspData);
setInt32(outBuffer, 0, 68);
setInt32(outBuffer, 4, ret);
for (i = 0; i < 8; i++) {
setInt32(outBuffer, 8 + 4 * i, pulsePosRspData.lPos[i]);
}
sendRet = sendN(acceptHandle, outBuffer, 72, 0);
if (sendRet != 72) {
fprintf(stderr, "tcpSvr: sendRet = %d != 72\n", sendRet);
return -1;
}
break;
case SYS1_GET_CURRENT_FEEDBACK_PULSE_POS:
if (msgSize != 12) {
fprintf(stderr, "tcpSvr: invalid msgSize for mpMotTargetClear = %d != 12\n", msgSize);
return -1;
}
memset(&ctrlGrpSendData, 0, sizeof (ctrlGrpSendData));
memset(&fbPulsePosRspData, 0, sizeof (fbPulsePosRspData));
controlGroup = getInt32(inBuffer, 12);
ctrlGrpSendData.sCtrlGrp = controlGroup;
ret = mpGetFBPulsePos(&ctrlGrpSendData, &fbPulsePosRspData);
setInt32(outBuffer, 0, 68);
setInt32(outBuffer, 4, ret);
for (i = 0; i < 8; i++) {
setInt32(outBuffer, 8 + 4 * i, fbPulsePosRspData.lPos[i]);
}
sendRet = sendN(acceptHandle, outBuffer, 72, 0);
if (sendRet != 72) {
fprintf(stderr, "tcpSvr: sendRet = %d != 72\n", sendRet);
return -1;
}
break;
case SYS1_GET_DEG_POS_EX:
if (msgSize != 12) {
fprintf(stderr, "tcpSvr: invalid msgSize for mpMotTargetClear = %d != 12\n", msgSize);
return -1;
}
memset(&ctrlGrpSendData, 0, sizeof (ctrlGrpSendData));
memset(°PosRspDataEx, 0, sizeof (degPosRspDataEx));
controlGroup = getInt32(inBuffer, 12);
ctrlGrpSendData.sCtrlGrp = controlGroup;
ret = mpGetDegPosEx(&ctrlGrpSendData, °PosRspDataEx);
setInt32(outBuffer, 0, 132);
setInt32(outBuffer, 4, ret);
for (i = 0; i < 8; i++) {
setInt32(outBuffer, 8 + 4 * i, degPosRspDataEx.lDegPos[i]);
}
for (i = 0; i < 8; i++) {
setInt32(outBuffer, 72 + 4 * i, degPosRspDataEx.lDegUnit[i]);
}
sendRet = sendN(acceptHandle, outBuffer, 136, 0);
if (sendRet != 136) {
fprintf(stderr, "tcpSvr: sendRet = %d != 136\n", sendRet);
return -1;
}
break;
case SYS1_GET_SERVO_POWER:
if (msgSize != 8) {
fprintf(stderr, "tcpSvr: invalid msgSize for mpGetServoPower = %d != 8\n", msgSize);
return -1;
}
memset(&servoPowerRspData, 0, sizeof (servoPowerRspData));
ret = mpGetServoPower(&servoPowerRspData);
setInt32(outBuffer, 0, 6);
setInt32(outBuffer, 4, ret);
setInt16(outBuffer, 8, servoPowerRspData.sServoPower);
sendRet = sendN(acceptHandle, outBuffer, 10, 0);
if (sendRet != 10) {
fprintf(stderr, "tcpSvr: sendRet = %d != 10\n", sendRet);
return -1;
}
break;
case SYS1_SET_SERVO_POWER:
if (msgSize != 10) {
fprintf(stderr, "tcpSvr: invalid msgSize for mpSetServoPower = %d != 12\n", msgSize);
return -1;
}
memset(&servoPowerSendData, 0, sizeof (servoPowerSendData));
memset(&stdRspData, 0, sizeof (stdRspData));
servoPowerSendData.sServoPower = getInt16(inBuffer, 12);
ret = mpSetServoPower(&servoPowerSendData, &stdRspData);
setInt32(outBuffer, 0, 6);
setInt32(outBuffer, 4, ret);
setInt16(outBuffer, 8, stdRspData.err_no);
sendRet = sendN(acceptHandle, outBuffer, 10, 0);
if (sendRet != 10) {
fprintf(stderr, "tcpSvr: sendRet = %d != 136\n", sendRet);
return -1;
}
break;
case SYS1_READIO:
num = getInt32(inBuffer, 12);
if (num < 1 || num > 24) {
fprintf(stderr, "tcpSvr: invalid num for mpReadIO num = %ld\n", num);
return -1;
}
if (msgSize != 12 + (4 * num)) {
fprintf(stderr, "tcpSvr: invalid msgSize for mpReadIO = %d != %ld for num = %ld\n", msgSize, 12 + (4 * num), num);
return -1;
}
for (i = 0; i < num; i++) {
ioInfo[i].ulAddr = getInt32(inBuffer, 16 + (4 * i));
}
memset(iorData, 0, sizeof (iorData));
ret = mpReadIO(ioInfo, iorData, num);
setInt32(outBuffer, 0, 4 + num * 2);
setInt32(outBuffer, 4, ret);
for (i = 0; i < num; i++) {
setInt16(outBuffer, 8 + i * 2, iorData[i]);
}
sendRet = sendN(acceptHandle, outBuffer, 8 + num * 2, 0);
if (sendRet != 8 + num * 2) {
fprintf(stderr, "tcpSvr: sendRet = %d != 8 + num*4\n", sendRet);
return -1;
}
break;
case SYS1_WRITEIO:
num = getInt32(inBuffer, 12);
if (num < 1 || num > 24) {
fprintf(stderr, "tcpSvr: invalid num for mpPutVarData num = %ld\n", num);
return -1;
}
if (msgSize != 12 + (num * 8)) {
fprintf(stderr, "tcpSvr: invalid msgSize for mpPutVarData = %d != %ld for num = %ld\n", msgSize, 12 + (num * 8), num);
return -1;
}
for (i = 0; i < num; i++) {
ioData[i].ulAddr = getInt32(inBuffer, 16 + (8 * i));
ioData[i].ulValue = getInt32(inBuffer, 20 + (8 * i));
}
ret = mpWriteIO(ioData, num);
setInt32(outBuffer, 0, 4);
setInt32(outBuffer, 4, ret);
sendRet = sendN(acceptHandle, outBuffer, 8, 0);
if (sendRet != 8) {
fprintf(stderr, "tcpSvr: sendRet = %d != 8\n", sendRet);
return -1;
}
break;
case SYS1_GET_MODE:
if (msgSize != 8) {
fprintf(stderr, "tcpSvr: invalid msgSize for mpGetMode = %d != 8\n", msgSize);
return -1;
}
memset(&modeData, 0, sizeof (modeData));
ret = mpGetMode(&modeData);
setInt32(outBuffer, 0, 8);
setInt32(outBuffer, 4, ret);
setInt16(outBuffer, 8, modeData.sMode);
setInt16(outBuffer, 10, modeData.sRemote);
sendRet = sendN(acceptHandle, outBuffer, 12, 0);
if (sendRet != 12) {
fprintf(stderr, "tcpSvr: sendRet = %d != 12\n", sendRet);
return -1;
}
break;
case SYS1_GET_CYCLE:
if (msgSize != 8) {
fprintf(stderr, "tcpSvr: invalid msgSize for mpGetCycle = %d != 8\n", msgSize);
return -1;
}
memset(&cycleData, 0, sizeof (cycleData));
ret = mpGetCycle(&cycleData);
setInt32(outBuffer, 0, 6);
setInt32(outBuffer, 4, ret);
setInt16(outBuffer, 8, cycleData.sCycle);
sendRet = sendN(acceptHandle, outBuffer, 10, 0);
if (sendRet != 10) {
fprintf(stderr, "tcpSvr: sendRet = %d != 10\n", sendRet);
return -1;
}
break;
case SYS1_GET_ALARM_STATUS:
if (msgSize != 8) {
fprintf(stderr, "tcpSvr: invalid msgSize for mpGetAlarmStatus = %d != 8\n", msgSize);
return -1;
}
memset(&alarmStatusData, 0, sizeof (alarmStatusData));
ret = mpGetAlarmStatus(&alarmStatusData);
setInt32(outBuffer, 0, 6);
setInt32(outBuffer, 4, ret);
setInt16(outBuffer, 8, alarmStatusData.sIsAlarm);
sendRet = sendN(acceptHandle, outBuffer, 10, 0);
if (sendRet != 10) {
fprintf(stderr, "tcpSvr: sendRet = %d != 10\n", sendRet);
return -1;
}
break;
case SYS1_GET_ALARM_CODE:
if (msgSize != 8) {
fprintf(stderr, "tcpSvr: invalid msgSize for mpGetAlarmCode = %d != 8\n", msgSize);
return -1;
}
memset(&alarmCodeData, 0, sizeof (alarmCodeData));
ret = mpGetAlarmCode(&alarmCodeData);
setInt32(outBuffer, 0, 10 + 4 * ((alarmCodeData.usAlarmNum > 4) ? 4 : alarmCodeData.usAlarmNum));
setInt32(outBuffer, 4, ret);
setInt16(outBuffer, 8, alarmCodeData.usErrorNo);
setInt16(outBuffer, 10, alarmCodeData.usErrorData);
setInt16(outBuffer, 12, alarmCodeData.usAlarmNum);
for (i = 0; i < alarmCodeData.usAlarmNum && i < 4; i++) {
setInt16(outBuffer, 14 + i * 4, alarmCodeData.AlarmData.usAlarmNo[i]);
setInt16(outBuffer, 16 + i * 4, alarmCodeData.AlarmData.usAlarmData[i]);
}
sendRet = sendN(acceptHandle, outBuffer, 14 + 4 * ((alarmCodeData.usAlarmNum > 4) ? 4 : alarmCodeData.usAlarmNum), 0);
if (sendRet != 14 + 4 * ((alarmCodeData.usAlarmNum > 4) ? 4 : alarmCodeData.usAlarmNum)) {
fprintf(stderr, "tcpSvr: sendRet = %d != 14 +4*((alarmCodeData.usAlarmNum>4)?4:alarmCodeData.usAlarmNum)\n", sendRet);
return -1;
}
break;
default:
fprintf(stderr, "tcpSvr: invalid sys1 function type = %d\n", type);
return -1;
}
return 0;
}