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; }
Aris::Core::MSG parseSwing(const std::string &cmd, const map<std::string, std::string> ¶ms) { SWING_PARAM param; for(auto &i:params) { if(i.first=="y") { param.centreP[1]=stod(i.second); } else if(i.first=="z") { param.centreP[2]=stod(i.second); } else if(i.first=="deg") { param.swingRad=stod(i.second)/180*PI;//计算身体摆动的弧度 } else { std::cout<<"parse failed"<<std::endl; return MSG{}; } } param.periodCount=3000; Aris::Core::MSG msg; msg.CopyStruct(param); std::cout<<"finished parse"<<std::endl; return msg; }
Aris::Core::MSG parsePre2Shv(const std::string &cmd, const map<std::string, std::string> ¶ms) { PRE2SHV_PARAM param; for(auto &i:params) { if(i.first=="distance") { param.distance=stod(i.second); } else if(i.first=="height") { param.height=stod(i.second); } else { std::cout<<"parse failed"<<std::endl; return MSG{}; } } param.periodCount[0]=3000; param.periodCount[1]=3000; Aris::Core::MSG msg; msg.CopyStruct(param); std::cout<<"finished parse"<<std::endl; return msg; }
int On_VS_Connected(Aris::Core::MSG &msg) { cout<<"Received Connection from Vision System:"<<endl; cout<<" Remote IP is: "<<msg.GetDataAddress()+sizeof(int)<<endl; cout<<" Port is : "<<*((int*)msg.GetDataAddress()) << endl << endl; return 0; }
int show(const Aris::Core::MSG &msg) { cout << "Msg Length:" << msg.GetLength()<<endl; cout << "Msg MsgID :" << msg.GetMsgID()<<endl; //cout << "Msg Type :" << msg.GetType()<<endl; cout << "Msg Data :" << msg.GetDataAddress()<<endl<<endl; return 0; }
int autoTurnRight(Aris::Core::MSG &msg) { EGAIT cmd=GAIT_TURN_RIGHT; Aris::Core::MSG data; data.SetLength(sizeof(cmd)); data.Copy(&cmd,sizeof(cmd)); cs.NRT_SendData(data); return 0; }
int autoEndDiscover(Aris::Core::MSG &msg) { EGAIT cmd=GAIT_END_DISCOVER; Aris::Core::MSG data; data.SetLength(sizeof(cmd)); data.Copy(&cmd,sizeof(cmd)); cs.NRT_SendData(data); return 0; }
int autoMoveBack(Aris::Core::MSG &msg) { EGAIT cmd=GAIT_MOVE_BACK; Aris::Core::MSG data; data.SetLength(sizeof(cmd)); data.Copy(&cmd,sizeof(cmd)); cs.NRT_SendData(data); 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; }
//MSG call back functions int On_CS_Connected(Aris::Core::MSG &msg) { cout<<"Received Connection from Control System:"<<endl; cout<<" Remote IP is: "<<msg.GetDataAddress()+sizeof(int)<<endl; cout<<" Port is : "<<*((int*)msg.GetDataAddress()) << endl << endl; Aris::Core::MSG data(0,0); ControlSystem.SendData(data); return 0; }
int On_CS_DataReceived(Aris::Core::CONN *pConn,Aris::Core::MSG &data) { int cmd=data.GetMsgID()+100; Aris::Core::MSG CMD=CS_CMD_Received; CMD.SetLength(sizeof(int)); bool IsCMDExecutable=true; CMD.Copy(&cmd,sizeof(int)); cout<<"received CMD is"<<cmd<<endl; PostMsg(CMD); LastCMD=data.GetMsgID(); return 0; }
int autoStart(Aris::Core::MSG &msg) { EGAIT cmd=GAIT_HOME2START; Aris::Core::MSG data; data.SetLength(sizeof(cmd)); data.Copy(&cmd,sizeof(cmd)); cs.NRT_SendData(data); return 0; }
int Aris::RT_CONTROL::ACTUATION::NRT_PostMsg(Aris::Core::MSG &p_data) { int ret; //printf("sizeof %d\n",sizeof(p_data)); //p_data.SetMsgID((int)Aris::RT_CONTROL::CM_CUS_MESSAGE); ret=sysBase->NRT_SendDataRaw(p_data._pData,p_data.GetLength()+MSG_HEADER_LENGTH); //printf("NRT_SendData is me :%d\n",ret); return 0; }//ok
int On_CS_DataReceived(Aris::Core::CONN *pConn, Aris::Core::MSG &data) { int cmd=data.GetMsgID(); MSG CMD=CS_CMD_Received; CMD.SetLength(sizeof(int)); CMD.Copy(&cmd,sizeof(int)); cout<<"received CMD is "<<cmd<<endl; PostMsg(CMD); return 0; }
int On_CS_CMD_Received(Aris::Core::MSG &msg) { int Command_Gait; msg.Paste(&Command_Gait,sizeof(int)); Aris::Core::MSG Gait=Command_Gait; cout<<"Gait Needed is: "<<Gait.GetMsgID()<<endl; PostMsg(Gait); Aris::Core::MSG data(0,0); ControlSystem.SendData(data); return 0; }
Aris::Core::MSG parseShovel(const std::string &cmd, const map<std::string, std::string> ¶ms) { SHOVEL_PARAM param; param.radius=2; param.periodNum = 7; for(int i = 0; i < param.periodNum; i++) { param.periodCount[i]=3000; } for(auto &i:params) { if(i.first=="height") { param.height=stod(i.second); } else if(i.first=="length") { param.length=stod(i.second); } else if(i.first=="degree") { param.rad=stod(i.second)/180*PI; } else { std::cout<<"parse failed"<<std::endl; return MSG{}; } } Aris::Core::MSG msg; msg.CopyStruct(param); std::cout<<"finished parse"<<std::endl; return msg; }
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 OnVisualSystemDataNeeded(Aris::Core::MSG &msg) { static int i = 0; cout<<"Capture "<<i<<" begin!!!"<<endl; visionsensor.capture(&i); while(1) { if(i>40) { Kinect::ControlCommand = NoValidCommand; Aris::Core::MSG data; data.SetLength(1 * sizeof(int)); memset(data.GetDataAddress(), 0, data.GetLength()); data.Copy(&Kinect::ControlCommand, sizeof(i)); pVisualSystem->SendData(data); break; } else { if(Kinect::IsCaptureEnd == true) { Kinect::IsCaptureEnd = false; Aris::Core::MSG data; data.SetLength(1 * sizeof(int)); memset(data.GetDataAddress(), 0, data.GetLength()); data.Copy(&Kinect::ControlCommand, sizeof(i)); pVisualSystem->SendData(data); break; } } } return 0; }
Aris::Core::MSG parseCWF(const std::string &cmd, const std::map<std::string, std::string> ¶ms) { Robots::WALK_PARAM param; for (auto &i : params) { if (i.first == "totalCount") { param.totalCount = std::stoi(i.second); } else if (i.first == "walkDirection") { param.walkDirection = std::stoi(i.second); } else if (i.first == "upDirection") { param.upDirection = std::stoi(i.second); } else if (i.first == "distance") { param.d = std::stod(i.second); } else if (i.first == "height") { param.h = std::stod(i.second); } } isStoppingCWF=false; Aris::Core::MSG msg; msg.CopyStruct(param); return msg; }
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; }
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: { 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 SendRequest(int argc, char *argv[], const char *xmlFileName) { /*需要去除命令名的路径和扩展名*/ std::string cmdName(argv[0]); #ifdef PLATFORM_IS_WINDOWS if (cmdName.rfind('\\')) { cmdName = cmdName.substr(cmdName.rfind('\\') + 1, cmdName.npos); } #endif #ifdef PLATFORM_IS_LINUX if (cmdName.rfind('/')) { cmdName = cmdName.substr(cmdName.rfind('/') + 1, cmdName.npos); } #endif if (cmdName.rfind('.')) { cmdName = cmdName.substr(0, cmdName.rfind('.')); } /*添加命令的所有参数*/ for (int i = 1; i < argc; ++i) { cmdName = cmdName + " " + argv[i]; } /*构造msg,这里需要先copy命令名称,然后依次copy各个参数*/ Aris::Core::MSG msg; msg.Copy(cmdName.c_str()); /*连接并发送msg*/ Aris::Core::DOCUMENT doc; if (doc.LoadFile(xmlFileName) != 0) throw std::logic_error("failed to read configuration xml file"); std::string ip = doc.RootElement()->FirstChildElement("Server")->FirstChildElement("Connection")->Attribute("IP"); std::string port = doc.RootElement()->FirstChildElement("Server")->FirstChildElement("Connection")->Attribute("Port"); Aris::Core::CONN conn; while (true) { try { conn.Connect(ip.c_str(), port.c_str()); break; } catch (std::exception &) { std::cout << "failed to connect server, will retry in 1 second" << std::endl; Aris::Core::Sleep(1000); } } Aris::Core::MSG ret = conn.SendRequest(msg); /*错误处理*/ if (ret.GetLength() > 0) { std::cout << "cmd has fault, please regard to following information:" << std::endl; std::cout << " " << ret.GetDataAddress() << std::endl; } else { std::cout << "send command successful" << std::endl; } 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; }
Aris::Core::MSG parseMove2(const std::string &cmd, const map<std::string, std::string> ¶ms) { MOVES_PARAM param; double targetPos[3]; //移动目标位置 for(auto &i:params) { if(i.first=="component") { if(i.second=="lf") { param.comID=0; } else if(i.second=="lm") { param.comID=1; } else if(i.second=="lr") { param.comID=2; } else if(i.second=="rf") { param.comID=3; } else if(i.second=="rm") { param.comID=4; } else if(i.second=="rr") { param.comID=5; } else if(i.second=="bd") { param.comID=6; } else { std::cout<<"parse failed"<<std::endl; return MSG{}; } } //绝对坐标移动 else if(i.first=="x") { targetPos[0]=stod(i.second); param.isAbsolute=true; } else if(i.first=="y") { targetPos[1]=stod(i.second); param.isAbsolute=true; } else if(i.first=="z") { targetPos[2]=stod(i.second); param.isAbsolute=true; } //相对坐标移动 else if(i.first=="u") { targetPos[0]=stod(i.second); } else if(i.first=="v") { targetPos[1]=stod(i.second); } else if(i.first=="w") { targetPos[2]=stod(i.second); } else { std::cout<<"parse failed"<<std::endl; return MSG{}; } } if(param.comID==6) { std::copy_n(targetPos, 3, param.targetBodyPE); } else { std::copy_n(targetPos, 3, ¶m.targetPee[3*param.comID]); param.legNum=1; param.motorNum=3; param.legID[0]=param.comID; int motors[3] = { 3*param.comID, 3*param.comID+1, 3*param.comID+2 }; std::copy_n(motors, 9, param.motorID); } param.periodCount=3000; Aris::Core::MSG msg; msg.CopyStruct(param); std::cout<<"finished parse"<<std::endl; return msg; }
int On_VS_DataReceived(Aris::Core::CONN *pConn, Aris::Core::MSG &data) { cout<<"Calibration Singnal Recieved"<<endl; int num; memcpy(&num,data.GetDataAddress(),data.GetLength()); //stop num >= 54 if (num >= 62 ) { cout<<"Calibration finished"<<endl; CGait::IsAutoCalibrateRegistered = false; } else { ifstream inf; stringstream filenum; string filename; filenum<<(num-1); filename = "./calibration_txt/"+filenum.str() + "F.txt"; inf.open(filename); int Line_Num; double temp; for(int i=0;i<GAIT_AUTOCALIB_LEN/2;i++) { inf>>Line_Num; for(int j=0;j<GAIT_WIDTH;j++) { inf>>temp; CGait::GaitAutoCalib[i][j]=-(int)temp; } } inf.close(); filenum.str(""); filename.clear(); filenum<<num; filename = "./calibration_txt/"+filenum.str()+".txt"; inf.open(filename); for(int i=GAIT_AUTOCALIB_LEN/2;i<GAIT_AUTOCALIB_LEN;i++) { inf>>Line_Num; for(int j=0;j<GAIT_WIDTH;j++) { inf>>temp; CGait::GaitAutoCalib[i][j]=-(int)temp; } } inf.close(); cout<<"Calibration "<<num<<" begin"<<endl; cout<<CGait::GaitAutoCalib[3999][0]<<endl; EGAIT cmd=GAIT_AUTO_CALIBRATION; Aris::Core::MSG data; data.SetLength(sizeof(cmd)); data.Copy(&cmd,sizeof(cmd)); cout<<"Send Message to CS"<<endl; cs.NRT_SendData(data); } return 0; }