00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "MotoPlus.h"
00038 #include "ParameterExtraction.h"
00039 #include "CtrlGroup.h"
00040 #include "SimpleMessage.h"
00041 #include "Controller.h"
00042 #include "MotionServer.h"
00043
00044
00045
00046
00047
00048 void Ros_MotionServer_StartNewConnection(Controller* controller, int sd);
00049 void Ros_MotionServer_StopConnection(Controller* controller, int connectionIndex);
00050
00051 void Ros_MotionServer_WaitForSimpleMsg(Controller* controller, int connectionIndex);
00052 BOOL Ros_MotionServer_SimpleMsgProcess(Controller* controller, SimpleMsg* receiveMsg, int byteSize, SimpleMsg* replyMsg);
00053 int Ros_MotionServer_MotionCtrlProcess(Controller* controller, SimpleMsg* receiveMsg, SimpleMsg* replyMsg);
00054 BOOL Ros_MotionServer_StopMotion(Controller* controller);
00055 BOOL Ros_MotionServer_StartTrajMode(Controller* controller);
00056 BOOL Ros_MotionServer_StopTrajMode(Controller* controller);
00057 int Ros_MotionServer_JointTrajDataProcess(Controller* controller, SimpleMsg* receiveMsg, SimpleMsg* replyMsg);
00058 int Ros_MotionServer_InitTrajPointFull(CtrlGroup* ctrlGroup, SmBodyJointTrajPtFull* jointTrajData);
00059 int Ros_MotionServer_AddTrajPointFull(CtrlGroup* ctrlGroup, SmBodyJointTrajPtFull* jointTrajData);
00060
00061 void Ros_MotionServer_AddToIncQueueProcess(Controller* controller, int groupNo);
00062 void Ros_MotionServer_JointTrajDataToIncQueue(Controller* controller, int groupNo);
00063 BOOL Ros_MotionServer_AddPulseIncPointToQ(Controller* controller, int groupNo, Incremental_data* dataToEnQ);
00064 BOOL Ros_MotionServer_ClearQ_All(Controller* controller);
00065 BOOL Ros_MotionServer_HasDataInQueue(Controller* controller);
00066 int Ros_MotionServer_GetQueueCnt(Controller* controller, int groupNo);
00067 void Ros_MotionServer_IncMoveLoopStart(Controller* controller);
00068
00069 void Ros_MotionServer_ConvertToJointMotionData(SmBodyJointTrajPtFull* jointTrajData, JointMotionData* jointMotionData);
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081 void Ros_MotionServer_StartNewConnection(Controller* controller, int sd)
00082 {
00083 int groupNo;
00084 int connectionIndex;
00085
00086
00087 if(controller->tidIncMoveThread == INVALID_TASK)
00088 {
00089 controller->tidIncMoveThread = mpCreateTask(MP_PRI_IP_CLK_TAKE, MP_STACK_SIZE,
00090 (FUNCPTR)Ros_MotionServer_IncMoveLoopStart,
00091 (int)controller, 0, 0, 0, 0, 0, 0, 0, 0, 0);
00092 }
00093
00094
00095 for(groupNo = 0; groupNo < controller->numGroup; groupNo++)
00096 {
00097 controller->ctrlGroups[groupNo]->tidAddToIncQueue = mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE,
00098 (FUNCPTR)Ros_MotionServer_AddToIncQueueProcess,
00099 (int)controller, groupNo, 0, 0, 0, 0, 0, 0, 0, 0);
00100 }
00101
00102
00103 for (connectionIndex = 0; connectionIndex < MAX_MOTION_CONNECTIONS; connectionIndex++)
00104 {
00105 if (controller->sdMotionConnections[connectionIndex] == INVALID_SOCKET)
00106 {
00107
00108
00109
00110 controller->sdMotionConnections[connectionIndex] = sd;
00111
00112
00113 controller->tidMotionConnections[connectionIndex] = mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE,
00114 (FUNCPTR)Ros_MotionServer_WaitForSimpleMsg,
00115 (int)controller, connectionIndex, 0, 0, 0, 0, 0, 0, 0, 0);
00116
00117
00118 Ros_Controller_SetIOState(IO_FEEDBACK_MOTIONSERVERCONNECTED, TRUE);
00119
00120 break;
00121 }
00122 }
00123
00124 if (connectionIndex == MAX_MOTION_CONNECTIONS)
00125 {
00126 printf("Motion server already connected... not accepting last attempt.\n");
00127 mpClose(sd);
00128 }
00129 }
00130
00131
00132
00133
00134
00135 void Ros_MotionServer_StopConnection(Controller* controller, int connectionIndex)
00136 {
00137 int i;
00138 int tid;
00139 BOOL bDeleteIncMovTask;
00140
00141 printf("Closing Motion Server Connection\r\n");
00142
00143
00144 mpClose(controller->sdMotionConnections[connectionIndex]);
00145
00146 controller->sdMotionConnections[connectionIndex] = INVALID_SOCKET;
00147
00148
00149 bDeleteIncMovTask = TRUE;
00150 for(i=0; i<MAX_MOTION_CONNECTIONS; i++)
00151 {
00152 if(controller->sdMotionConnections[connectionIndex] != INVALID_SOCKET)
00153 {
00154 bDeleteIncMovTask = FALSE;
00155 break;
00156 }
00157 }
00158
00159
00160 if(bDeleteIncMovTask)
00161 {
00162
00163 Ros_Controller_SetIOState(IO_FEEDBACK_MOTIONSERVERCONNECTED, FALSE);
00164
00165
00166 for(i=0; i < controller->numGroup; i++)
00167 {
00168 controller->ctrlGroups[i]->hasDataToProcess = FALSE;
00169 tid = controller->ctrlGroups[i]->tidAddToIncQueue;
00170 controller->ctrlGroups[i]->tidAddToIncQueue = INVALID_TASK;
00171 mpDeleteTask(tid);
00172 }
00173
00174
00175 tid = controller->tidIncMoveThread;
00176 controller->tidIncMoveThread = INVALID_TASK;
00177 mpDeleteTask(tid);
00178 }
00179
00180
00181 tid = controller->tidMotionConnections[connectionIndex];
00182 controller->tidMotionConnections[connectionIndex] = INVALID_TASK;
00183 printf("Motion Server Connection Closed\r\n");
00184
00185 mpDeleteTask(tid);
00186 }
00187
00188
00189
00190
00191
00192
00193 void Ros_MotionServer_WaitForSimpleMsg(Controller* controller, int connectionIndex)
00194 {
00195 SimpleMsg receiveMsg;
00196 SimpleMsg replyMsg;
00197 int byteSize = 0;
00198 int minSize = sizeof(SmPrefix) + sizeof(SmHeader);
00199 int expectedSize;
00200 int ret = 0;
00201 BOOL bDisconnect = FALSE;
00202
00203 while(!bDisconnect)
00204 {
00205 mpTaskDelay(0);
00206
00207
00208 memset(&receiveMsg, 0x00, sizeof(receiveMsg));
00209 byteSize = mpRecv(controller->sdMotionConnections[connectionIndex], (char*)(&receiveMsg), sizeof(receiveMsg), 0);
00210 if (byteSize <= 0)
00211 break;
00212
00213
00214 expectedSize = -1;
00215 if(byteSize >= minSize)
00216 {
00217 switch(receiveMsg.header.msgType)
00218 {
00219 case ROS_MSG_ROBOT_STATUS:
00220 expectedSize = minSize + sizeof(SmBodyRobotStatus);
00221 break;
00222 case ROS_MSG_JOINT_TRAJ_PT_FULL:
00223 expectedSize = minSize + sizeof(SmBodyJointTrajPtFull);
00224 break;
00225 case ROS_MSG_JOINT_FEEDBACK:
00226 expectedSize = minSize + sizeof(SmBodyJointFeedback);
00227 break;
00228 case ROS_MSG_MOTO_MOTION_CTRL:
00229 expectedSize = minSize + sizeof(SmBodyMotoMotionCtrl);
00230 break;
00231 case ROS_MSG_MOTO_MOTION_REPLY:
00232 expectedSize = minSize + sizeof(SmBodyMotoMotionReply);
00233 break;
00234 }
00235 }
00236
00237
00238 if(byteSize == expectedSize)
00239 {
00240
00241 ret = Ros_MotionServer_SimpleMsgProcess(controller, &receiveMsg, byteSize, &replyMsg);
00242 if(ret == 1)
00243 bDisconnect = TRUE;
00244 }
00245 else
00246 {
00247
00248 Ros_SimpleMsg_MotionReply(&receiveMsg, ROS_RESULT_INVALID, 0, &replyMsg);
00249
00250
00251 }
00252
00253
00254 byteSize = mpSend(controller->sdMotionConnections[connectionIndex], (char*)(&replyMsg), replyMsg.prefix.length + sizeof(SmPrefix), 0);
00255 if (byteSize <= 0)
00256 break;
00257 }
00258
00259 mpTaskDelay(50);
00260
00261
00262 Ros_MotionServer_StopConnection(controller, connectionIndex);
00263 }
00264
00265
00266
00267
00268
00269
00270 int Ros_MotionServer_SimpleMsgProcess(Controller* controller, SimpleMsg* receiveMsg,
00271 int byteSize, SimpleMsg* replyMsg)
00272 {
00273 int ret = 0;
00274 int expectedBytes = sizeof(SmPrefix) + sizeof(SmHeader);
00275 int invalidSubcode = 0;
00276
00277
00278
00279 switch(receiveMsg->header.msgType)
00280 {
00281 case ROS_MSG_JOINT_TRAJ_PT_FULL:
00282
00283 expectedBytes += sizeof(SmBodyJointTrajPtFull);
00284 if(expectedBytes == byteSize)
00285 ret = Ros_MotionServer_JointTrajDataProcess(controller, receiveMsg, replyMsg);
00286 else
00287 invalidSubcode = ROS_RESULT_INVALID_MSGSIZE;
00288 break;
00289 case ROS_MSG_MOTO_MOTION_CTRL:
00290
00291 expectedBytes += sizeof(SmBodyMotoMotionCtrl);
00292 if(expectedBytes == byteSize)
00293 ret = Ros_MotionServer_MotionCtrlProcess(controller, receiveMsg, replyMsg);
00294 else
00295 invalidSubcode = ROS_RESULT_INVALID_MSGSIZE;
00296 break;
00297 default:
00298 printf("Invalid message type: %d\n", receiveMsg->header.msgType);
00299 invalidSubcode = ROS_RESULT_INVALID_MSGTYPE;
00300 break;
00301 }
00302
00303
00304 if(invalidSubcode != 0)
00305 {
00306 Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_INVALID, invalidSubcode, replyMsg);
00307 ret = -1;
00308 }
00309
00310 return ret;
00311 }
00312
00313
00314
00315
00316
00317
00318 int Ros_MotionServer_MotionCtrlProcess(Controller* controller, SimpleMsg* receiveMsg,
00319 SimpleMsg* replyMsg)
00320 {
00321 SmBodyMotoMotionCtrl* motionCtrl;
00322
00323
00324
00325
00326 motionCtrl = &receiveMsg->body.motionCtrl;
00327 switch(motionCtrl->command)
00328 {
00329 case ROS_CMD_CHECK_MOTION_READY:
00330 {
00331 if(Ros_Controller_IsMotionReady(controller))
00332 Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_TRUE, 0, replyMsg);
00333 else
00334 Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_FALSE, 0, replyMsg);
00335 break;
00336 }
00337 case ROS_CMD_CHECK_QUEUE_CNT:
00338 {
00339 int count = Ros_MotionServer_GetQueueCnt(controller, motionCtrl->groupNo);
00340 if(count >= 0)
00341 Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_TRUE, count, replyMsg);
00342 else
00343 Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_FAILURE, count, replyMsg);
00344 break;
00345 }
00346 case ROS_CMD_STOP_MOTION:
00347 {
00348
00349 BOOL bRet = Ros_MotionServer_StopMotion(controller);
00350
00351
00352 if(bRet)
00353 Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_SUCCESS, 0, replyMsg);
00354 else
00355 Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_FAILURE, 0, replyMsg);
00356 break;
00357 }
00358 case ROS_CMD_START_TRAJ_MODE:
00359 {
00360
00361 BOOL bRet = Ros_MotionServer_StartTrajMode(controller);
00362 if(bRet)
00363 Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_SUCCESS, 0, replyMsg);
00364 else
00365 Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_NOT_READY,
00366 Ros_Controller_GetNotReadySubcode(controller), replyMsg);
00367 break;
00368 }
00369 case ROS_CMD_STOP_TRAJ_MODE:
00370 case ROS_CMD_DISCONNECT:
00371 {
00372 BOOL bRet = Ros_MotionServer_StopTrajMode(controller);
00373 if(bRet)
00374 {
00375 Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_SUCCESS, 0, replyMsg);
00376 if(motionCtrl->command == ROS_CMD_DISCONNECT)
00377 return 1;
00378 }
00379 else
00380 Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_FAILURE, 0, replyMsg);
00381 break;
00382 }
00383 }
00384
00385 return 0;
00386 }
00387
00388
00389
00390
00391
00392 BOOL Ros_MotionServer_StopMotion(Controller* controller)
00393 {
00394
00395 BOOL bRet;
00396 BOOL bStopped;
00397 int checkCnt;
00398 int groupNo;
00399
00400
00401 controller->bStopMotion = TRUE;
00402
00403
00404 for(checkCnt=0; checkCnt<MOTION_STOP_TIMEOUT; checkCnt++)
00405 {
00406 bStopped = TRUE;
00407 for(groupNo=0; groupNo<controller->numGroup; groupNo++)
00408 bStopped &= !controller->ctrlGroups[groupNo]->hasDataToProcess;
00409 if(bStopped)
00410 break;
00411 else
00412 mpTaskDelay(1);
00413 }
00414
00415
00416 bRet = Ros_MotionServer_ClearQ_All(controller);
00417
00418
00419 controller->bStopMotion = FALSE;
00420
00421 return(bStopped && bRet);
00422 }
00423
00424
00425
00426
00427
00428
00429 BOOL Ros_MotionServer_StartTrajMode(Controller* controller)
00430 {
00431 int ret;
00432 MP_STD_RSP_DATA rData;
00433 MP_START_JOB_SEND_DATA sStartData;
00434 int checkCount;
00435
00436 printf("In StartTrajMode\r\n");
00437
00438
00439 Ros_Controller_StatusUpdate(controller);
00440
00441
00442 if(Ros_Controller_IsMotionReady(controller))
00443 return TRUE;
00444
00445
00446 if(Ros_Controller_IsOperating(controller))
00447 return FALSE;
00448
00449
00450 if(Ros_Controller_IsEStop(controller)
00451 || Ros_Controller_IsHold(controller)
00452 || !Ros_Controller_IsRemote(controller))
00453 return FALSE;
00454
00455
00456 if(Ros_Controller_IsError(controller))
00457 {
00458
00459 memset(&rData, 0x00, sizeof(rData));
00460 ret = mpCancelError(&rData);
00461 if(ret != 0)
00462 goto updateStatus;
00463 }
00464
00465
00466 if(Ros_Controller_IsAlarm(controller))
00467 {
00468
00469 memset(&rData, 0x00, sizeof(rData));
00470 ret = mpResetAlarm(&rData);
00471 if(ret == 0)
00472 {
00473
00474 int checkCount;
00475 for(checkCount=0; checkCount<MOTION_START_TIMEOUT; checkCount+=MOTION_START_CHECK_PERIOD)
00476 {
00477
00478 Ros_Controller_StatusUpdate(controller);
00479
00480 if(Ros_Controller_IsAlarm(controller) == FALSE)
00481 continue;
00482
00483 mpTaskDelay(MOTION_START_CHECK_PERIOD);
00484 }
00485 if(Ros_Controller_IsAlarm(controller))
00486 goto updateStatus;
00487 }
00488 else
00489 goto updateStatus;
00490 }
00491
00492
00493
00494 if(Ros_Controller_IsServoOn(controller) == FALSE)
00495 {
00496 MP_SERVO_POWER_SEND_DATA sServoData;
00497 memset(&rData, 0x00, sizeof(rData));
00498 memset(&sServoData, 0x00, sizeof(sServoData));
00499 sServoData.sServoPower = 1;
00500 ret = mpSetServoPower(&sServoData, &rData);
00501 if( (ret == 0) && (rData.err_no ==0) )
00502 {
00503
00504 int checkCount;
00505 for(checkCount=0; checkCount<MOTION_START_TIMEOUT; checkCount+=MOTION_START_CHECK_PERIOD)
00506 {
00507
00508 Ros_Controller_StatusUpdate(controller);
00509
00510 if(Ros_Controller_IsServoOn(controller) == TRUE)
00511 continue;
00512
00513 mpTaskDelay(MOTION_START_CHECK_PERIOD);
00514 }
00515 if(Ros_Controller_IsServoOn(controller) == FALSE)
00516 goto updateStatus;
00517 }
00518 else
00519 {
00520 char errMsg[ERROR_MSG_MAX_SIZE];
00521 memset(errMsg, 0x00, ERROR_MSG_MAX_SIZE);
00522 Ros_Controller_ErrNo_ToString(rData.err_no, errMsg, ERROR_MSG_MAX_SIZE);
00523 printf("Can't turn on servo because: %s\r\n", errMsg);
00524 goto updateStatus;
00525 }
00526 }
00527
00528
00529 memset(&rData, 0x00, sizeof(rData));
00530 memset(&sStartData, 0x00, sizeof(sStartData));
00531 sStartData.sTaskNo = 0;
00532 memcpy(sStartData.cJobName, MOTION_INIT_ROS_JOB, MAX_JOB_NAME_LEN);
00533 ret = mpStartJob(&sStartData, &rData);
00534 if( (ret != 0) || (rData.err_no !=0) )
00535 {
00536 char errMsg[ERROR_MSG_MAX_SIZE];
00537 memset(errMsg, 0x00, ERROR_MSG_MAX_SIZE);
00538 Ros_Controller_ErrNo_ToString(rData.err_no, errMsg, ERROR_MSG_MAX_SIZE);
00539 printf("Can't start job %s because: %s\r\n", MOTION_INIT_ROS_JOB, errMsg);
00540 goto updateStatus;
00541 }
00542
00543
00544 for(checkCount=0; checkCount<MOTION_START_TIMEOUT; checkCount+=MOTION_START_CHECK_PERIOD)
00545 {
00546
00547 Ros_Controller_StatusUpdate(controller);
00548
00549 if(Ros_Controller_IsMotionReady(controller))
00550 return(TRUE);
00551
00552 mpTaskDelay(MOTION_START_CHECK_PERIOD);
00553 }
00554
00555 updateStatus:
00556
00557 Ros_Controller_StatusUpdate(controller);
00558
00559 return (Ros_Controller_IsMotionReady(controller));
00560 }
00561
00562
00563
00564
00565
00566
00567
00568 BOOL Ros_MotionServer_StopTrajMode(Controller* controller)
00569 {
00570
00571 if(Ros_MotionServer_HasDataInQueue(controller))
00572 {
00573
00574 return FALSE;
00575 }
00576
00577
00578 if(!Ros_MotionServer_StopMotion(controller))
00579 {
00580
00581 return FALSE;
00582 }
00583
00584
00585 Ros_Controller_SetIOState(IO_FEEDBACK_MP_INCMOVE_DONE, TRUE);
00586
00587 return TRUE;
00588 }
00589
00590
00591
00592
00593
00594
00595 int Ros_MotionServer_JointTrajDataProcess(Controller* controller, SimpleMsg* receiveMsg,
00596 SimpleMsg* replyMsg)
00597 {
00598 SmBodyJointTrajPtFull* trajData;
00599 CtrlGroup* ctrlGroup;
00600 int ret;
00601
00602
00603 if(!Ros_Controller_IsMotionReady(controller))
00604 {
00605 int subcode = Ros_Controller_GetNotReadySubcode(controller);
00606 printf("ERROR: Controller is not ready (code: %d). Can't process ROS_MSG_JOINT_TRAJ_PT_FULL.\r\n", subcode);
00607 Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_NOT_READY, subcode, replyMsg);
00608 return 0;
00609 }
00610
00611
00612 trajData = &receiveMsg->body.jointTrajData;
00613
00614
00615 if(Ros_Controller_IsValidGroupNo(controller, trajData->groupNo))
00616 {
00617 ctrlGroup = controller->ctrlGroups[trajData->groupNo];
00618 }
00619 else
00620 {
00621 Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_INVALID, ROS_RESULT_INVALID_GROUPNO, replyMsg);
00622 return 0;
00623 }
00624
00625
00626 if( (trajData->validFields & 0x07) != 0x07 )
00627 {
00628 printf("ERROR: Validfields = %d\r\n", trajData->validFields);
00629 Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_INVALID, ROS_RESULT_INVALID_DATA_INSUFFICIENT, replyMsg);
00630 return 0;
00631 }
00632
00633
00634 if(trajData->sequence == 0)
00635 {
00636
00637 ret = Ros_MotionServer_InitTrajPointFull(ctrlGroup, trajData);
00638
00639
00640 if(ret == 0)
00641 Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_SUCCESS, 0, replyMsg);
00642 else
00643 Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_INVALID, ret, replyMsg);
00644 }
00645 else if(trajData->sequence > 0)
00646 {
00647
00648 ret = Ros_MotionServer_AddTrajPointFull(ctrlGroup, trajData);
00649
00650
00651 if(ret == 0)
00652 Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_SUCCESS, 0, replyMsg);
00653 else if(ret == 1)
00654 Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_BUSY, 0, replyMsg);
00655 else
00656 Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_INVALID, ret, replyMsg);
00657 }
00658 else
00659 {
00660 Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_INVALID, ROS_RESULT_INVALID_SEQUENCE, replyMsg);
00661 }
00662
00663 return 0;
00664 }
00665
00666
00667
00668
00669
00670 int Ros_MotionServer_InitTrajPointFull(CtrlGroup* ctrlGroup, SmBodyJointTrajPtFull* jointTrajData)
00671 {
00672 long pulsePos[MAX_PULSE_AXES];
00673 long curPos[MAX_PULSE_AXES];
00674 int i;
00675
00676 if(ctrlGroup->groupNo == jointTrajData->groupNo)
00677 {
00678
00679 Ros_MotionServer_ConvertToJointMotionData(jointTrajData, &ctrlGroup->jointMotionData);
00680 ctrlGroup->timeLeftover_ms = 0;
00681 ctrlGroup->q_time = ctrlGroup->jointMotionData.time;
00682
00683
00684 Ros_CtrlGroup_ConvertToMotoPos(ctrlGroup, ctrlGroup->jointMotionData.pos, pulsePos);
00685 Ros_CtrlGroup_GetPulsePosCmd(ctrlGroup, curPos);
00686
00687
00688 for(i=0; i<MAX_PULSE_AXES; i++)
00689 {
00690
00691 if(abs(pulsePos[i] - curPos[i]) > START_MAX_PULSE_DEVIATION)
00692 {
00693 printf("ERROR: Trajectory start position doesn't match current position.\r\n");
00694 printf(" %d, %d, %d, %d, %d, %d, %d, %d\r\n",
00695 pulsePos[0], pulsePos[1], pulsePos[2],
00696 pulsePos[3], pulsePos[4], pulsePos[5],
00697 pulsePos[6], pulsePos[7]);
00698 printf(" %d, %d, %d, %d, %d, %d, %d, %d\r\n",
00699 curPos[0], curPos[1], curPos[2],
00700 curPos[3], curPos[4], curPos[5],
00701 curPos[6], curPos[7]);
00702 return ROS_RESULT_INVALID_DATA_START_POS;
00703 }
00704
00705
00706 if(abs(ctrlGroup->jointMotionData.vel[i]) > ctrlGroup->maxSpeedRad[i])
00707 {
00708
00709 return ROS_RESULT_INVALID_DATA_SPEED;
00710 }
00711 }
00712
00713
00714
00715 return 0;
00716 }
00717
00718 return ROS_RESULT_INVALID_GROUPNO;
00719 }
00720
00721
00722
00723
00724
00725 int Ros_MotionServer_AddTrajPointFull(CtrlGroup* ctrlGroup, SmBodyJointTrajPtFull* jointTrajData)
00726 {
00727 int i;
00728 JointMotionData jointData;
00729
00730
00731 if(ctrlGroup->hasDataToProcess)
00732 {
00733
00734 return ROS_RESULT_BUSY;
00735 }
00736
00737
00738 Ros_MotionServer_ConvertToJointMotionData(jointTrajData, &jointData);
00739
00740
00741 for(i=0; i<ctrlGroup->numAxes; i++)
00742 {
00743
00744
00745
00746
00747 if(abs(jointData.vel[i]) > ctrlGroup->maxSpeedRad[i])
00748 {
00749
00750 printf("ERROR: Invalid speed in message TrajPointFull data: \r\n axis: %d, speed: %f, limit: %f\r\n",
00751 i, jointData.vel[i], ctrlGroup->maxSpeedRad[i]);
00752
00753 #ifdef DEBUG
00754 Ros_SimpleMsg_DumpTrajPtFull(jointTrajData);
00755 #endif
00756
00757 return ROS_RESULT_INVALID_DATA_SPEED;
00758 }
00759 }
00760
00761
00762 memcpy(&ctrlGroup->jointMotionDataToProcess, &jointData, sizeof(JointMotionData));
00763 ctrlGroup->hasDataToProcess = TRUE;
00764
00765 return 0;
00766 }
00767
00768
00769
00770
00771
00772
00773
00774 void Ros_MotionServer_AddToIncQueueProcess(Controller* controller, int groupNo)
00775 {
00776 int interpolPeriod;
00777 CtrlGroup* ctrlGroup = controller->ctrlGroups[groupNo];
00778
00779
00780 interpolPeriod = controller->interpolPeriod;
00781 ctrlGroup->hasDataToProcess = FALSE;
00782
00783 FOREVER
00784 {
00785
00786 if(ctrlGroup->hasDataToProcess)
00787 {
00788
00789 Ros_MotionServer_JointTrajDataToIncQueue(controller, groupNo);
00790
00791
00792 ctrlGroup->hasDataToProcess = FALSE;
00793 }
00794
00795 mpTaskDelay(interpolPeriod);
00796 }
00797 }
00798
00799
00800
00801
00802
00803
00804
00805
00806 void Ros_MotionServer_JointTrajDataToIncQueue(Controller* controller, int groupNo)
00807 {
00808 int interpolPeriod = controller->interpolPeriod;
00809 CtrlGroup* ctrlGroup = controller->ctrlGroups[groupNo];
00810 int i;
00811 JointMotionData _startTrajData;
00812 JointMotionData* startTrajData;
00813 JointMotionData* endTrajData;
00814 JointMotionData* curTrajData;
00815 float interval;
00816 float accCoef1[MP_GRP_AXES_NUM];
00817 float accCoef2[MP_GRP_AXES_NUM];
00818 int timeInc_ms;
00819 int calculationTime_ms;
00820 float interpolTime;
00821 long prevPulsePos[MP_GRP_AXES_NUM];
00822 long newPulsePos[MP_GRP_AXES_NUM];
00823 Incremental_data incData;
00824
00825
00826
00827
00828 curTrajData = &ctrlGroup->jointMotionData;
00829 endTrajData = &ctrlGroup->jointMotionDataToProcess;
00830 startTrajData = &_startTrajData;
00831
00832 memcpy(startTrajData, curTrajData, sizeof(JointMotionData));
00833
00834
00835 memset(prevPulsePos, 0x00, sizeof(prevPulsePos));
00836 Ros_CtrlGroup_ConvertToMotoPos(ctrlGroup, curTrajData->pos, prevPulsePos);
00837 memset(newPulsePos, 0x00, sizeof(newPulsePos));
00838 memset(&incData, 0x00, sizeof(incData));
00839 incData.frame = MP_INC_PULSE_DTYPE;
00840
00841
00842 memset(&accCoef1, 0x00, sizeof(accCoef1));
00843 memset(&accCoef2, 0x00, sizeof(accCoef2));
00844 interval = (endTrajData->time - startTrajData->time) / 1000.0f;
00845 if (interval > 0.0)
00846 {
00847 for (i = 0; i < ctrlGroup->numAxes; i++)
00848 {
00849
00850 accCoef1[i] = ( 6 * (endTrajData->pos[i] - startTrajData->pos[i]) / (interval * interval) )
00851 - ( 2 * (endTrajData->vel[i] + 2 * startTrajData->vel[i]) / interval);
00852 accCoef2[i] = ( -12 * (endTrajData->pos[i] - startTrajData->pos[i]) / (interval * interval * interval))
00853 + ( 6 * (endTrajData->vel[i] + startTrajData->vel[i]) / (interval * interval) );
00854 }
00855 }
00856 else
00857 {
00858 printf("ERROR: Time difference between prevTrajData and newTrajData is 0 or less.\r\n");
00859 }
00860
00861
00862 calculationTime_ms = startTrajData->time;
00863 if(ctrlGroup->timeLeftover_ms == 0)
00864 timeInc_ms = interpolPeriod;
00865 else
00866 timeInc_ms = ctrlGroup->timeLeftover_ms;
00867
00868
00869 while( (curTrajData->time < endTrajData->time) && Ros_Controller_IsMotionReady(controller) && !controller->bStopMotion)
00870 {
00871
00872 calculationTime_ms += timeInc_ms;
00873 interpolTime = (calculationTime_ms - startTrajData->time) / 1000.0f;
00874
00875 if( calculationTime_ms < endTrajData->time )
00876 {
00877
00878 curTrajData->time = calculationTime_ms;
00879
00880
00881 for (i = 0; i < ctrlGroup->numAxes; i++)
00882 {
00883
00884 curTrajData->pos[i] = startTrajData->pos[i]
00885 + startTrajData->vel[i] * interpolTime
00886 + accCoef1[i] * interpolTime * interpolTime / 2
00887 + accCoef2[i] * interpolTime * interpolTime * interpolTime / 6;
00888
00889
00890 curTrajData->vel[i] = startTrajData->vel[i]
00891 + accCoef1[i] * interpolTime
00892 + accCoef2[i] * interpolTime * interpolTime / 2;
00893 }
00894
00895
00896 if(timeInc_ms < interpolPeriod)
00897 {
00898 timeInc_ms = interpolPeriod;
00899 ctrlGroup->timeLeftover_ms = 0;
00900 }
00901 }
00902 else
00903 {
00904
00905 memcpy(curTrajData, endTrajData, sizeof(JointMotionData));
00906
00907
00908 if(calculationTime_ms > endTrajData->time)
00909 {
00910 ctrlGroup->timeLeftover_ms = calculationTime_ms - endTrajData->time;
00911 }
00912 }
00913
00914
00915
00916
00917
00918
00919
00920 Ros_CtrlGroup_ConvertToMotoPos(ctrlGroup, curTrajData->pos, newPulsePos);
00921
00922
00923 incData.time = curTrajData->time;
00924 for (i = 0; i < ctrlGroup->numAxes; i++)
00925 {
00926 incData.inc[i] = (newPulsePos[i]- prevPulsePos[i]);
00927 }
00928
00929
00930 if(!Ros_MotionServer_AddPulseIncPointToQ(controller, groupNo, &incData))
00931 break;
00932
00933
00934
00935
00936
00937
00938
00939 memcpy(prevPulsePos, newPulsePos, sizeof(prevPulsePos));
00940 }
00941 }
00942
00943
00944
00945
00946
00947 BOOL Ros_MotionServer_AddPulseIncPointToQ(Controller* controller, int groupNo, Incremental_data* dataToEnQ)
00948 {
00949 int index;
00950
00951
00952 Incremental_q* q = &controller->ctrlGroups[groupNo]->inc_q;
00953
00954 while( q->cnt >= Q_SIZE )
00955 {
00956
00957 mpTaskDelay(controller->interpolPeriod);
00958
00959
00960 if (!Ros_Controller_IsMotionReady(controller))
00961 {
00962 return FALSE;
00963 }
00964 }
00965
00966
00967 if(mpSemTake(q->q_lock, Q_LOCK_TIMEOUT) == OK)
00968 {
00969
00970 index = Q_OFFSET_IDX( q->idx, q->cnt , Q_SIZE );
00971
00972 q->data[index] = *dataToEnQ;
00973
00974 q->cnt++;
00975
00976
00977 mpSemGive(q->q_lock);
00978 }
00979 else
00980 {
00981 printf("ERROR: Unable to add point to queue. Queue is locked up!\r\n");
00982 return FALSE;
00983 }
00984
00985 return TRUE;
00986 }
00987
00988
00989
00990
00991
00992 BOOL Ros_MotionServer_ClearQ(Controller* controller, int groupNo)
00993 {
00994 Incremental_q* q;
00995
00996
00997 if(!Ros_Controller_IsValidGroupNo(controller, groupNo))
00998 return FALSE;
00999
01000
01001 q = &controller->ctrlGroups[groupNo]->inc_q;
01002
01003
01004 if(mpSemTake(q->q_lock, Q_LOCK_TIMEOUT) == OK)
01005 {
01006
01007 q->cnt = 0;
01008
01009
01010 mpSemGive(q->q_lock);
01011
01012 return TRUE;
01013 }
01014
01015 return FALSE;
01016 }
01017
01018
01019
01020
01021
01022 BOOL Ros_MotionServer_ClearQ_All(Controller* controller)
01023 {
01024 int groupNo;
01025 BOOL bRet = TRUE;
01026
01027 for(groupNo=0; groupNo<controller->numGroup; groupNo++)
01028 {
01029 bRet &= Ros_MotionServer_ClearQ(controller, groupNo);
01030 }
01031
01032 return bRet;
01033 }
01034
01035
01036
01037
01038
01039 int Ros_MotionServer_GetQueueCnt(Controller* controller, int groupNo)
01040 {
01041 Incremental_q* q;
01042 int count;
01043
01044
01045 if(!Ros_Controller_IsValidGroupNo(controller, groupNo))
01046 return -1;
01047
01048
01049 q = &controller->ctrlGroups[groupNo]->inc_q;
01050
01051
01052 if(mpSemTake(q->q_lock, Q_LOCK_TIMEOUT) == OK)
01053 {
01054 count = q->cnt;
01055
01056
01057 mpSemGive(q->q_lock);
01058
01059 return count;
01060 }
01061
01062 printf("ERROR: Unable to access queue count. Queue is locked up!\r\n");
01063 return -1;
01064 }
01065
01066
01067
01068
01069
01070
01071 BOOL Ros_MotionServer_HasDataInQueue(Controller* controller)
01072 {
01073 int groupNo;
01074
01075 for(groupNo=0; groupNo<controller->numGroup; groupNo++)
01076 {
01077 if(Ros_MotionServer_GetQueueCnt(controller, groupNo) > 0)
01078 return TRUE;
01079 }
01080
01081 return FALSE;
01082 }
01083
01084
01085
01086
01087
01088
01089 void Ros_MotionServer_IncMoveLoopStart(Controller* controller)
01090 {
01091 #ifdef DX100
01092 MP_POS_DATA moveData;
01093 #else
01094 MP_EXPOS_DATA moveData;
01095 #endif
01096
01097 Incremental_q* q;
01098 int i;
01099 int ret;
01100 LONG time;
01101 LONG q_time;
01102
01103
01104 printf("IncMoveTask Started\r\n");
01105
01106 memset(&moveData, 0x00, sizeof(moveData));
01107
01108 for(i=0; i<controller->numGroup; i++)
01109 {
01110 moveData.ctrl_grp |= (0x01 << i);
01111 moveData.grp_pos_info[i].pos_tag.data[0] = Ros_CtrlGroup_GetAxisConfig(controller->ctrlGroups[i]);
01112 }
01113
01114 FOREVER
01115 {
01116 mpClkAnnounce(MP_INTERPOLATION_CLK);
01117
01118 if (Ros_Controller_IsMotionReady(controller)
01119 && Ros_MotionServer_HasDataInQueue(controller)
01120 && !controller->bStopMotion )
01121 {
01122
01123
01124 for(i=0; i<controller->numGroup; i++)
01125 {
01126 q = &controller->ctrlGroups[i]->inc_q;
01127
01128
01129 if(mpSemTake(q->q_lock, Q_LOCK_TIMEOUT) == OK)
01130 {
01131 if(q->cnt > 0)
01132 {
01133 time = q->data[q->idx].time;
01134 q_time = controller->ctrlGroups[i]->q_time;
01135 moveData.grp_pos_info[i].pos_tag.data[2] = q->data[q->idx].tool;
01136 moveData.grp_pos_info[i].pos_tag.data[3] = q->data[q->idx].frame;
01137 moveData.grp_pos_info[i].pos_tag.data[4] = q->data[q->idx].user;
01138
01139 memcpy(&moveData.grp_pos_info[i].pos, &q->data[q->idx].inc, sizeof(LONG) * MP_GRP_AXES_NUM);
01140
01141
01142 q->idx = Q_OFFSET_IDX( q->idx, 1, Q_SIZE );
01143 q->cnt--;
01144
01145
01146 while(q->cnt > 0)
01147 {
01148 if( (q_time <= q->data[q->idx].time)
01149 && (q->data[q->idx].time - q_time <= controller->interpolPeriod) )
01150 {
01151
01152 int axis;
01153
01154
01155 if( (moveData.grp_pos_info[i].pos_tag.data[2] != q->data[q->idx].tool)
01156 || (moveData.grp_pos_info[i].pos_tag.data[3] != q->data[q->idx].frame)
01157 || (moveData.grp_pos_info[i].pos_tag.data[4] != q->data[q->idx].user) )
01158 {
01159
01160 break;
01161 }
01162
01163
01164 for(axis=0; axis<MP_GRP_AXES_NUM; axis++)
01165 moveData.grp_pos_info[i].pos[axis] += q->data[q->idx].inc[axis];
01166 time = q->data[q->idx].time;
01167
01168
01169 q->idx = Q_OFFSET_IDX( q->idx, 1, Q_SIZE );
01170 q->cnt--;
01171 }
01172 else
01173 {
01174
01175 break;
01176 }
01177 }
01178
01179 controller->ctrlGroups[i]->q_time = time;
01180 }
01181 else
01182 {
01183 moveData.grp_pos_info[i].pos_tag.data[2] = 0;
01184 moveData.grp_pos_info[i].pos_tag.data[3] = MP_INC_PULSE_DTYPE;
01185 moveData.grp_pos_info[i].pos_tag.data[4] = 0;
01186 memset(&moveData.grp_pos_info[i].pos, 0x00, sizeof(LONG) * MP_GRP_AXES_NUM);
01187 }
01188
01189
01190 mpSemGive(q->q_lock);
01191 }
01192 else
01193 {
01194 printf("ERROR: Can't get data from queue. Queue is locked up.\r\n");
01195 memset(&moveData.grp_pos_info[i].pos, 0x00, sizeof(LONG) * MP_GRP_AXES_NUM);
01196 continue;
01197 }
01198 }
01199
01200 #ifdef DX100
01201
01202 moveData.ctrl_grp = 1;
01203 ret = mpMeiIncrementMove(MP_SL_ID1, &moveData);
01204 if(ret != 0)
01205 {
01206 if(ret == -3)
01207 printf("mpMeiIncrementMove returned: %d (ctrl_grp = %d)\r\n", ret, moveData.ctrl_grp);
01208 else
01209 printf("mpMeiIncrementMove returned: %d\r\n", ret);
01210 }
01211
01212 moveData.ctrl_grp = 2;
01213 if(controller->numRobot > 1)
01214 {
01215 ret = mpMeiIncrementMove(MP_SL_ID2, &moveData);
01216 if(ret != 0)
01217 {
01218 if(ret == -3)
01219 printf("mpMeiIncrementMove returned: %d (ctrl_grp = %d)\r\n", ret, moveData.ctrl_grp);
01220 else
01221 printf("mpMeiIncrementMove returned: %d\r\n", ret);
01222 }
01223 }
01224 #else
01225 ret = mpExRcsIncrementMove(&moveData);
01226 if(ret != 0)
01227 {
01228 if(ret == -3)
01229 printf("mpExRcsIncrementMove returned: %d (ctrl_grp = %d)\r\n", ret, moveData.ctrl_grp);
01230 else
01231 printf("mpExRcsIncrementMove returned: %d\r\n", ret);
01232 }
01233 #endif
01234
01235 }
01236
01237
01238
01239
01240
01241
01242
01243
01244 }
01245 }
01246
01247
01248
01249
01250
01251
01252 void Ros_MotionServer_ConvertToJointMotionData(SmBodyJointTrajPtFull* jointTrajData, JointMotionData* jointMotionData)
01253 {
01254 int i, maxAxes;
01255
01256 memset(jointMotionData, 0x00, sizeof(JointMotionData));
01257
01258 maxAxes = min(ROS_MAX_JOINT, MP_GRP_AXES_NUM);
01259
01260 jointMotionData->flag = jointTrajData->validFields;
01261 jointMotionData->time = (int)(jointTrajData->time * 1000);
01262
01263 for(i=0; i<maxAxes; i++)
01264 {
01265 jointMotionData->pos[i] = jointTrajData->pos[i];
01266 jointMotionData->vel[i] = jointTrajData->vel[i];
01267 jointMotionData->acc[i] = jointTrajData->acc[i];
01268 }
01269 }
01270
01271