int On_VS_DataReceived(Aris::Core::CONN *pConn, Aris::Core::MSG &data) { cout<<"receive data from visual system"<<endl; double *map = new double [data.GetLength()/sizeof(double)]; memcpy(map, data.GetDataAddress(), data.GetLength() ); static double currentH[6],nextH[6]; memcpy(nextH,map,sizeof(nextH)); HexIII.MotionPlanWithKinect(currentH,nextH,*Gait_Calculated_From_Map); memcpy(currentH,nextH,sizeof(nextH)); delete[] map; for(int j=0;j<GAIT_ADAPTIVEWALK_LEN;j++) { for(int i=0;i<GAIT_WIDTH;i++) { CGait::GaitAdaptiveWalk[j][i]=-(int)(Gait_Calculated_From_Map[j][i]); } } Aris::Core::MSG controldata; controldata.SetMsgID(WALKADAPTIVE); cs.NRT_PostMsg(controldata); return 0; }
int On_VS_ConnectionReceived(Aris::Core::CONN *pConn, const char* addr,int port) { Aris::Core::MSG msg; msg.SetMsgID(VS_Connected); msg.SetLength(sizeof(port)); msg.Copy(&port,sizeof(port)); msg.CopyMore(addr,strlen(addr)); PostMsg(msg); return 0; }
int On_CS_DataReceived(Aris::Core::CONN *pConn, Aris::Core::MSG &data) { int cmd_id=data.GetMsgID(); Aris::Core::MSG CMD; CMD.SetMsgID(CS_CMD_Received); CMD.SetLength(sizeof(int)); CMD.Copy(&cmd_id,sizeof(int)); // if the cmd has data, it should be repost together CMD.CopyMore(data.GetDataAddress(), data.GetLength()); cout<<"received CMD is "<<cmd_id<<endl; PostMsg(CMD); return 0; }
int tg(CMachineData& machineData,RT_MSG& msg) { const int MapAbsToPhy[18]= { 10, 11, 9, 12, 14, 13, 17, 15, 16, 6, 8, 7, 3, 5, 4, 0, 2, 1 }; const int MapPhyToAbs[18]= { 15, 17, 16, 12, 14, 13, 9, 11, 10, 2, 0, 1, 3, 5, 4, 7, 8, 6 }; int CommandID; CommandID=msg.GetMsgID(); switch(CommandID) { case NOCMD: for(int i=0;i<18;i++) { machineData.motorsCommands[i]=EMCMD_NONE; } rt_printf("NONE Command Get in NRT\n" ); break; case ENABLE: for(int i=0;i<18;i++) { machineData.motorsCommands[i]=EMCMD_ENABLE; } rt_printf("ENABLE Command Get in NRT\n" ); break; case POWEROFF: for(int i=0;i<18;i++) { machineData.motorsCommands[i]=EMCMD_POWEROFF; gait.IfReadytoSetGait(false,i); } rt_printf("POWEROFF Command Get in NRT\n" ); break; case STOP: for(int i=0;i<18;i++) { machineData.motorsCommands[i]=EMCMD_STOP; } rt_printf("STOP Command Get in NRT\n" ); break; case RUNNING: for(int i=0;i<18;i++) { machineData.motorsCommands[i]=EMCMD_RUNNING; gait.IfReadytoSetGait(true,i); } rt_printf("RUNNING Command Get in NRT\n" ); break; case GOHOME_1: machineData.motorsCommands[MapAbsToPhy[0]]=EMCMD_GOHOME; machineData.motorsCommands[MapAbsToPhy[1]]=EMCMD_GOHOME; machineData.motorsCommands[MapAbsToPhy[2]]=EMCMD_GOHOME; machineData.motorsCommands[MapAbsToPhy[6]]=EMCMD_GOHOME; machineData.motorsCommands[MapAbsToPhy[7]]=EMCMD_GOHOME; machineData.motorsCommands[MapAbsToPhy[8]]=EMCMD_GOHOME; machineData.motorsCommands[MapAbsToPhy[12]]=EMCMD_GOHOME; machineData.motorsCommands[MapAbsToPhy[13]]=EMCMD_GOHOME; machineData.motorsCommands[MapAbsToPhy[14]]=EMCMD_GOHOME; gaitcmd[MapAbsToPhy[0]]=EGAIT::GAIT_HOME; gaitcmd[MapAbsToPhy[1]]=EGAIT::GAIT_HOME; gaitcmd[MapAbsToPhy[2]]=EGAIT::GAIT_HOME; gaitcmd[MapAbsToPhy[6]]=EGAIT::GAIT_HOME; gaitcmd[MapAbsToPhy[7]]=EGAIT::GAIT_HOME; gaitcmd[MapAbsToPhy[8]]=EGAIT::GAIT_HOME; gaitcmd[MapAbsToPhy[12]]=EGAIT::GAIT_HOME; gaitcmd[MapAbsToPhy[13]]=EGAIT::GAIT_HOME; gaitcmd[MapAbsToPhy[14]]=EGAIT::GAIT_HOME; rt_printf("GOHOME_1 Command Get in NRT\n" ); break; case GOHOME_2: machineData.motorsCommands[MapAbsToPhy[3]]=EMCMD_GOHOME; machineData.motorsCommands[MapAbsToPhy[4]]=EMCMD_GOHOME; machineData.motorsCommands[MapAbsToPhy[5]]=EMCMD_GOHOME; machineData.motorsCommands[MapAbsToPhy[9]]=EMCMD_GOHOME; machineData.motorsCommands[MapAbsToPhy[10]]=EMCMD_GOHOME; machineData.motorsCommands[MapAbsToPhy[11]]=EMCMD_GOHOME; machineData.motorsCommands[MapAbsToPhy[15]]=EMCMD_GOHOME; machineData.motorsCommands[MapAbsToPhy[16]]=EMCMD_GOHOME; machineData.motorsCommands[MapAbsToPhy[17]]=EMCMD_GOHOME; gaitcmd[MapAbsToPhy[3]]=EGAIT::GAIT_HOME; gaitcmd[MapAbsToPhy[4]]=EGAIT::GAIT_HOME; gaitcmd[MapAbsToPhy[5]]=EGAIT::GAIT_HOME; gaitcmd[MapAbsToPhy[9]]=EGAIT::GAIT_HOME; gaitcmd[MapAbsToPhy[10]]=EGAIT::GAIT_HOME; gaitcmd[MapAbsToPhy[11]]=EGAIT::GAIT_HOME; gaitcmd[MapAbsToPhy[15]]=EGAIT::GAIT_HOME; gaitcmd[MapAbsToPhy[16]]=EGAIT::GAIT_HOME; gaitcmd[MapAbsToPhy[17]]=EGAIT::GAIT_HOME; rt_printf("GOHOME_2 Command Get in NRT\n" ); break; case HOME2START_1: if(gait.m_gaitState[MapAbsToPhy[0]]==GAIT_STOP) { for(int i=0;i<18;i++) { machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL; } gaitcmd[MapAbsToPhy[0]]=EGAIT::GAIT_HOME2START; gaitcmd[MapAbsToPhy[1]]=EGAIT::GAIT_HOME2START; gaitcmd[MapAbsToPhy[2]]=EGAIT::GAIT_HOME2START; gaitcmd[MapAbsToPhy[6]]=EGAIT::GAIT_HOME2START; gaitcmd[MapAbsToPhy[7]]=EGAIT::GAIT_HOME2START; gaitcmd[MapAbsToPhy[8]]=EGAIT::GAIT_HOME2START; gaitcmd[MapAbsToPhy[12]]=EGAIT::GAIT_HOME2START; gaitcmd[MapAbsToPhy[13]]=EGAIT::GAIT_HOME2START; gaitcmd[MapAbsToPhy[14]]=EGAIT::GAIT_HOME2START; rt_printf("HOME2START_1 Command Get in NRT\n" ); } break; case HOME2START_2: if(gait.m_gaitState[MapAbsToPhy[3]]==GAIT_STOP) { for(int i=0;i<18;i++) { machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL; } gaitcmd[MapAbsToPhy[3]]=EGAIT::GAIT_HOME2START; gaitcmd[MapAbsToPhy[4]]=EGAIT::GAIT_HOME2START; gaitcmd[MapAbsToPhy[5]]=EGAIT::GAIT_HOME2START; gaitcmd[MapAbsToPhy[9]]=EGAIT::GAIT_HOME2START; gaitcmd[MapAbsToPhy[10]]=EGAIT::GAIT_HOME2START; gaitcmd[MapAbsToPhy[11]]=EGAIT::GAIT_HOME2START; gaitcmd[MapAbsToPhy[15]]=EGAIT::GAIT_HOME2START; gaitcmd[MapAbsToPhy[16]]=EGAIT::GAIT_HOME2START; gaitcmd[MapAbsToPhy[17]]=EGAIT::GAIT_HOME2START; rt_printf("HOME2START_2 Command Get in NRT\n" ); } break; case FORWARD: if(gait.m_gaitState[MapAbsToPhy[0]]==GAIT_STOP) { for(int i=0;i<18;i++) { machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL; gaitcmd[MapAbsToPhy[i]]=EGAIT::GAIT_MOVE; rt_printf("FORWARD Command Get in NRT\n" ); } } break; case BACKWARD: if(gait.m_gaitState[MapAbsToPhy[0]]==GAIT_STOP) { for(int i=0;i<18;i++) { machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL; gaitcmd[MapAbsToPhy[i]]=EGAIT::GAIT_MOVE_BACK; rt_printf("BACKWARD Command Get in NRT\n" ); } } break; case TURNLEFT: if(gait.m_gaitState[MapAbsToPhy[0]]==GAIT_STOP) { for(int i=0;i<18;i++) { machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL; gaitcmd[MapAbsToPhy[i]]=EGAIT::GAIT_TURN_LEFT; rt_printf("TURNLEFT Command Get in NRT\n" ); } } break; case TURNRIGHT: if(gait.m_gaitState[MapAbsToPhy[0]]==GAIT_STOP) { for(int i=0;i<18;i++) { machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL; gaitcmd[MapAbsToPhy[i]]=EGAIT::GAIT_TURN_RIGHT; rt_printf("TURNRIGHT Command Get in NRT\n" ); } } break; case LEGUP: if(gait.m_gaitState[MapAbsToPhy[0]]==GAIT_STOP) { for(int i=0;i<18;i++) { machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL; gaitcmd[MapAbsToPhy[i]]=EGAIT::GAIT_LEGUP; rt_printf("LEGUP Command Get in NRT\n" ); } } break; case MOVE: if(gait.m_gaitState[MapAbsToPhy[0]]==GAIT_STOP) { for(int i=0;i<18;i++) { machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL; gaitcmd[MapAbsToPhy[i]]=EGAIT::GAIT_MOVE_MAP; rt_printf("MOVE_MAP Command Get in NRT\n" ); } } break; case TURN: if(gait.m_gaitState[MapAbsToPhy[0]]==GAIT_STOP) { for(int i=0;i<18;i++) { machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL; gaitcmd[MapAbsToPhy[i]]=EGAIT::GAIT_TURN_MAP; rt_printf("TURN_MAP Command Get in NRT\n" ); } } break; case BEGINDISCOVER: if(gait.m_gaitState[MapAbsToPhy[0]]==GAIT_STOP) { for(int i=0;i<18;i++) { machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL; gaitcmd[MapAbsToPhy[i]]=EGAIT::GAIT_BEGIN_DISCOVER; rt_printf("BEGINDISCOVER Command Get in NRT\n" ); } } break; case ENDDISCOVER: if(gait.m_gaitState[MapAbsToPhy[0]]== GAIT_STOP) { for(int i=0;i<18;i++) { machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL; gaitcmd[MapAbsToPhy[i]]=EGAIT::GAIT_END_DISCOVER; rt_printf("ENDDISCOVER Command Get in NRT\n" ); } } break; case STEPUP: if(gait.m_gaitState[MapAbsToPhy[0]]== GAIT_STOP) { for(int i=0;i<18;i++) { machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL; gaitcmd[MapAbsToPhy[i]]=EGAIT::GAIT_STEPUP_MAP; rt_printf("STEPUP_MAP Command Get in NRT\n" ); } } break; case STEPDOWN: if(gait.m_gaitState[MapAbsToPhy[0]]== GAIT_STOP) { for(int i=0;i<18;i++) { machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL; gaitcmd[MapAbsToPhy[i]]=EGAIT::GAIT_STEPDOWN_MAP; rt_printf("STEPDOWN_MAP Command Get in NRT\n" ); } } break; default: break; } gait.RunGait(gaitcmd,machineData); if(gait.IsGaitFinished()) { if(CGait::IsMove == true&&CGait::IsMoveEnd == true) { CGait::IsMove = false; CGait::IsMoveEnd = false; cout<<"MOVE FINISHED!"<<endl; Vision_Msg visioncmd = Vision_UpperControl; Aris::Core::MSG visionmsg; visionmsg.SetMsgID(VS_Capture); visionmsg.SetLength(sizeof(visioncmd)); visionmsg.Copy(&visioncmd, sizeof(visioncmd)); PostMsg(visionmsg); } if(CGait::IsTurn == true&&CGait::IsTurnEnd == true) { CGait::IsTurn = false; CGait::IsTurnEnd = false; cout<<"TURN FINISHED!"<<endl; Vision_Msg visioncmd = Vision_UpperControl; Aris::Core::MSG visionmsg; visionmsg.SetMsgID(VS_Capture); visionmsg.SetLength(sizeof(visioncmd)); visionmsg.Copy(&visioncmd, sizeof(visioncmd)); PostMsg(visionmsg); } if(CGait::IsBeginDiscoverStart == true && CGait::IsBeginDiscoverEnd == true) { cout<<"BEGINDISCOVER FINISHED!"<<endl; Vision_Msg visioncmd = Vision_StepUp; Aris::Core::MSG visionmsg; visionmsg.SetMsgID(VS_Capture); visionmsg.SetLength(sizeof(visioncmd)); visionmsg.Copy(&visioncmd, sizeof(visioncmd)); PostMsg(visionmsg); CGait::IsBeginDiscoverStart = false; CGait::IsBeginDiscoverStart = false; } if(CGait::IsEndDiscoverStart == true && CGait::IsEndDiscoverEnd == true) { cout<<"ENDDISCOVER FINISHED!"<<endl; Vision_Msg visioncmd = Vision_UpperControl; Aris::Core::MSG visionmsg; visionmsg.SetMsgID(VS_Capture); visionmsg.SetLength(sizeof(visioncmd)); visionmsg.Copy(&visioncmd, sizeof(visioncmd)); PostMsg(visionmsg); CGait::IsEndDiscoverStart = false; CGait::IsEndDiscoverStart = false; } if(CGait::IsStepUp == true&&CGait::IsStepUpstep == true) { CGait::IsStepUpstep = false; Vision_Msg visioncmd = Vision_StepUp; Aris::Core::MSG visionmsg; visionmsg.SetMsgID(VS_Capture); visionmsg.SetLength(sizeof(visioncmd)); visionmsg.Copy(&visioncmd, sizeof(visioncmd)); PostMsg(visionmsg); } if(CGait::IsStepDown == true&&CGait::IsStepDownstep == true) { CGait::IsStepDownstep = false; Vision_Msg visioncmd = Vision_StepDown; Aris::Core::MSG visionmsg; visionmsg.SetMsgID(VS_Capture); visionmsg.SetLength(sizeof(visioncmd)); visionmsg.Copy(&visioncmd, sizeof(visioncmd)); PostMsg(visionmsg); } if(CGait::IsStepOver == true&&CGait::IsStepOverstep == true) { CGait::IsStepOverstep = false; Vision_Msg visioncmd = Vision_StepOver; Aris::Core::MSG visionmsg; visionmsg.SetMsgID(VS_Capture); visionmsg.SetLength(sizeof(visioncmd)); visionmsg.Copy(&visioncmd, sizeof(visioncmd)); PostMsg(visionmsg); } } return 0; }
int OnGetControlCommand(MSG &msg) { int CommandID; msg.Paste(&CommandID,sizeof(int)); Aris::Core::MSG data; switch(CommandID) { case 1: data.SetMsgID(POWEROFF); cs.NRT_PostMsg(data); break; case 2: data.SetMsgID(STOP); cs.NRT_PostMsg(data); break; case 3: data.SetMsgID(ENABLE); cs.NRT_PostMsg(data); break; case 4: data.SetMsgID(RUNNING); cs.NRT_PostMsg(data); break; case 5: data.SetMsgID(GOHOME_1); cs.NRT_PostMsg(data); break; case 6: data.SetMsgID(GOHOME_2); cs.NRT_PostMsg(data); break; case 7: data.SetMsgID(HOME2START_1); cs.NRT_PostMsg(data); break; case 8: data.SetMsgID(HOME2START_2); cs.NRT_PostMsg(data); break; case 9: data.SetMsgID(FORWARD); cs.NRT_PostMsg(data); break; case 10: data.SetMsgID(BACKWARD); cs.NRT_PostMsg(data); break; case 11: data.SetMsgID(TURNLEFT); cs.NRT_PostMsg(data); break; case 12: data.SetMsgID(TURNRIGHT); cs.NRT_PostMsg(data); break; case 13: data.SetMsgID(LEGUP); cs.NRT_PostMsg(data); break; case 14: { CGait::IsVisionWalk = true; Vision_Msg visioncmd = Vision_UpperControl; Aris::Core::MSG visionmsg; visionmsg.SetMsgID(VS_Capture); visionmsg.SetLength(sizeof(visioncmd)); visionmsg.Copy(&visioncmd, sizeof(visioncmd)); PostMsg(visionmsg); } break; case 15: { CGait::IsWalkAvoidRegistered = true; Vision_Msg visioncmd = Walk_Avoid; Aris::Core::MSG visionmsg; visionmsg.SetMsgID(VS_Capture); visionmsg.SetLength(sizeof(visioncmd)); visionmsg.Copy(&visioncmd, sizeof(visioncmd)); PostMsg(visionmsg); } break; default: cout<<"Do Not Get Validate CMD"<<endl; break; } return CommandID; }
int On_VS_DataReceived(Aris::Core::CONN *pConn, Aris::Core::MSG &data) { cout<<"receive data from visual system"<<endl; switch (data.GetMsgID()) { case 34: { cout<<"MOVE!"<<endl; CGait::IsMove = true; double *move_data = new double [data.GetLength()/sizeof(double)]; memcpy(move_data, data.GetDataAddress(), data.GetLength()); cout<<move_data[0]<<" "<<move_data[1]<<" "<<move_data[2]<<endl; HexIII.RobotMove(move_data, *Gait_Move_Map); for(int j=0;j<GAIT_MOVE_MAP_LEN;j++) { for(int i=0;i<GAIT_WIDTH;i++) { CGait::GaitMoveMap[j][i]=-(int)(Gait_Move_Map[j][i]); } } Aris::Core::MSG controlcmd; controlcmd.SetMsgID(MOVE); cout<<"Send MOVE Message to CS"<<endl; cs.NRT_PostMsg(controlcmd); } break; case 35: { cout<<"TURN!"<<endl; CGait::IsTurn = true; double *turn_data = new double [data.GetLength()/sizeof(double)]; memcpy(turn_data, data.GetDataAddress(), data.GetLength()); *turn_data = *turn_data*180/M_PI; cout<<*turn_data<<endl; HexIII.RobotTurn(turn_data, *Gait_Turn_Map); for(int j=0;j<GAIT_TURN_MAP_LEN;j++) { for(int i=0;i<GAIT_WIDTH;i++) { CGait::GaitTurnMap[j][i]=-(int)(Gait_Turn_Map[j][i]); } } Aris::Core::MSG controlcmd; controlcmd.SetMsgID(TURN); cout<<"Send TURN Message to CS"<<endl; cs.NRT_PostMsg(controlcmd); } break; case 36: { cout<<"STEP UP!"<<endl; CGait::IsStepUp = true; double *map = new double [data.GetLength()/sizeof(double)]; memcpy(map, data.GetDataAddress(), data.GetLength()); static double StepUP_currentH[6] = {-1.05, -1.05, -1.05, -1.05, -1.05, -1.05}; static double StepUP_nextH[6]; memcpy(StepUP_nextH,map,sizeof(StepUP_nextH)); if(StepUP_currentH[0] == -0.85&&StepUP_currentH[1] == -0.85&&StepUP_currentH[2] == -0.85 &&StepUP_currentH[3] == -0.85&&StepUP_currentH[4] == -0.85&&StepUP_currentH[5] == -0.85 &&StepUP_nextH[0] == -0.85&&StepUP_nextH[1] == -0.85&&StepUP_nextH[2] == -0.85 &&StepUP_nextH[3] == -0.85&&StepUP_nextH[4] == -0.85&&StepUP_nextH[5] == -0.85) { cout<<"STEPUP FINISHED"<<endl; CGait::IsStepUp = false; delete[] map; Vision_Msg visioncmd = Vision_UpperControl; Aris::Core::MSG visionmsg; visionmsg.SetMsgID(VS_Capture); visionmsg.SetLength(sizeof(visioncmd)); visionmsg.Copy(&visioncmd, sizeof(visioncmd)); PostMsg(visionmsg); } else { double StepUpLen = 0.325; HexIII.RobotStepUp(&StepUpLen,StepUP_currentH,StepUP_nextH,*Gait_StepUp__Map); memcpy(StepUP_currentH,StepUP_nextH,sizeof(StepUP_nextH)); delete[] map; for(int j=0;j<GAIT_STEPUPANDDOWN_LEN;j++) { for(int i=0;i<GAIT_WIDTH;i++) { CGait::GaitStepUpMap[j][i]=-(int)(Gait_StepUp__Map[j][i]); } } Aris::Core::MSG controldata; controldata.SetMsgID(STEPUP); cs.NRT_PostMsg(controldata); } } break; case 37: { if(data.GetLength() == sizeof(int)) { Vision_Msg visioncmd = Vision_StepDown; Aris::Core::MSG visionmsg; visionmsg.SetMsgID(VS_Capture); visionmsg.SetLength(sizeof(visioncmd)); visionmsg.Copy(&visioncmd, sizeof(visioncmd)); PostMsg(visionmsg); } else { CGait::IsStepDown = true; cout<<"STEP DOWN!"<<endl; double *map = new double [data.GetLength()/sizeof(double)]; memcpy(map, data.GetDataAddress(), data.GetLength()); static double StepDown_currentH[6] = {-0.85, -0.85, -0.85, -0.85, -0.85, -0.85}; static double StepDown_nextH[6]; memcpy(StepDown_nextH,map,sizeof(StepDown_nextH)); if(StepDown_currentH[0] == -1.05&&StepDown_currentH[1] == -1.05&&StepDown_currentH[2] == -1.05 &&StepDown_currentH[3] == -1.05&&StepDown_currentH[4] == -1.05&&StepDown_currentH[5] == -1.05 &&StepDown_nextH[0] == -1.05&&StepDown_nextH[1] == -1.05&&StepDown_nextH[2] == -1.05 &&StepDown_nextH[3] == -1.05&&StepDown_nextH[4] == -1.05&&StepDown_nextH[5] == -1.05) { delete[] map; CGait::IsStepDown = false; cout<<"STEPDOWN FINISHED!"<<endl; cout<<"ENDISCOVER!"<<endl; CGait::IsEndDiscoverStart = true; Aris::Core::MSG controlcmd; controlcmd.SetMsgID(ENDDISCOVER); cout<<"Send ENDDISCOVER Message to CS"<<endl; cs.NRT_PostMsg(controlcmd); } else { double StepDownLen = 0.325; HexIII.RobotStepDown(&StepDownLen,StepDown_currentH,StepDown_nextH,*Gait_StepDown__Map); memcpy(StepDown_currentH,StepDown_nextH,sizeof(StepDown_nextH)); delete[] map; for(int j=0;j<GAIT_STEPUPANDDOWN_LEN;j++) { for(int i=0;i<GAIT_WIDTH;i++) { CGait::GaitStepDownMap[j][i]=-(int)(Gait_StepDown__Map[j][i]); } } Aris::Core::MSG controldata; controldata.SetMsgID(STEPDOWN); cs.NRT_PostMsg(controldata); } } } break; case 38: { if(data.GetLength() == sizeof(int)) { Vision_Msg visioncmd = Vision_StepOver; Aris::Core::MSG visionmsg; visionmsg.SetMsgID(VS_Capture); visionmsg.SetLength(sizeof(visioncmd)); visionmsg.Copy(&visioncmd, sizeof(visioncmd)); PostMsg(visionmsg); } else { cout<<"STEPOVER!"<<endl; CGait::IsStepOver = true; double *stepover_data = new double [data.GetLength()/sizeof(double)]; memcpy(stepover_data, data.GetDataAddress(), data.GetLength()); int count = int(stepover_data[0]); if(count == 5) { CGait::IsStepOver = false; cout<<"STEPOVER FINISHED!"<<endl; Vision_Msg visioncmd = Vision_UpperControl; Aris::Core::MSG visionmsg; visionmsg.SetMsgID(VS_Capture); visionmsg.SetLength(sizeof(visioncmd)); visionmsg.Copy(&visioncmd, sizeof(visioncmd)); PostMsg(visionmsg); } else { double move_data[3]; move_data[0] = stepover_data[1]; move_data[1] = stepover_data[2]; move_data[2] = stepover_data[3]; cout<<"Move Data From Sensor"<<endl; cout<<"Move X: "<<move_data[0]<<endl; cout<<"Move Y: "<<move_data[1]<<endl; cout<<"Move Z: "<<move_data[2]<<endl; HexIII.RobotMove(move_data, *Gait_Move_Map); for(int j=0;j<GAIT_MOVE_MAP_LEN;j++) { for(int i=0;i<GAIT_WIDTH;i++) { CGait::GaitMoveMap[j][i]=-(int)(Gait_Move_Map[j][i]); } } Aris::Core::MSG controlcmd; controlcmd.SetMsgID(MOVE); cout<<"Send MOVE Message to CS"<<endl; cs.NRT_PostMsg(controlcmd); } } } break; case 39: { cout<<"BEGINDISCOVER!"<<endl; CGait::IsBeginDiscoverStart = true; Aris::Core::MSG controlcmd; controlcmd.SetMsgID(BEGINDISCOVER); cout<<"Send BEGINDISCOVER Message to CS"<<endl; cs.NRT_PostMsg(controlcmd); } break; case 40: { cout<<"Vision Control Singnal Recieved, Beging Walk Avoid!"<<endl; int command; memcpy(&command, data.GetDataAddress(), data.GetLength()); Aris::Core::MSG controlcmd; switch (command) { case 0: CGait::IsWalkAvoidRegistered = false; break; case 1: { controlcmd.SetMsgID(FORWARD); cout<<"Send GAIT_MOVE Message to CS"<<endl; cs.NRT_PostMsg(controlcmd); } break; case 2: { controlcmd.SetMsgID(BACKWARD); cout<<"Send GAIT_MOVE_BACK Message to CS"<<endl; cs.NRT_PostMsg(controlcmd); } break; case 3: { controlcmd.SetMsgID(TURNLEFT); cout<<"Send GAIT_TURN_LEFT Message to CS"<<endl; cs.NRT_PostMsg(controlcmd); } break; case 4: { controlcmd.SetMsgID(TURNRIGHT); cout<<"Send GAIT_TURN_RIGHT Message to CS"<<endl; cs.NRT_PostMsg(controlcmd); } break; default: break; } } break; default: break; } return 0; }
int OnGetControlCommand(MSG &msg) { int CommandID; msg.Paste(&CommandID,sizeof(int)); Aris::Core::MSG data; switch(CommandID) { case 1: data.SetMsgID(POWEROFF); cs.NRT_PostMsg(data); break; case 2: data.SetMsgID(STOP); cs.NRT_PostMsg(data); break; case 3: data.SetMsgID(ENABLE); cs.NRT_PostMsg(data); break; case 4: data.SetMsgID(RUNNING); cs.NRT_PostMsg(data); break; case 5: data.SetMsgID(GOHOME_1); cs.NRT_PostMsg(data); break; case 6: data.SetMsgID(GOHOME_2); cs.NRT_PostMsg(data); break; case 7: data.SetMsgID(HOME2START_1); cs.NRT_PostMsg(data); break; case 8: data.SetMsgID(HOME2START_2); cs.NRT_PostMsg(data); break; case 9: data.SetMsgID(FORWARD); cs.NRT_PostMsg(data); break; case 10: data.SetMsgID(BACKWARD); cs.NRT_PostMsg(data); break; case 11: data.SetMsgID(TURNLEFT); cs.NRT_PostMsg(data); break; case 12: data.SetMsgID(TURNRIGHT); cs.NRT_PostMsg(data); break; case 13: data.SetMsgID(LEGUP); cs.NRT_PostMsg(data); break; case 14: data.SetMsgID(BEGINDISCOVER); cs.NRT_PostMsg(data); break; case 15: data.SetMsgID(ENDDISCOVER); cs.NRT_PostMsg(data); break; case 16: Aris::Core::PostMsg(Aris::Core::MSG(VS_Capture)); CGait::IsWalkAdaptiveRegistered=true; break; default: cout<<"Do Not Get Validate CMD"<<endl; break; } return CommandID; }