robocup(3D).rar_3d robocup_RoboCup c_Robocup3D_robcup_robocup

/** DEFINE THE BEHAVIOR OF AGNETS **/ #include<iostream> #include<sstream> #include<fstream> #include <math.h> #include "behavior.h" #include "worldmodel.h" #include "agent.h" using namespace std; const float Behavior::bLeg = 1.3 ; // const float Behavior::sLeg = 0.964 ; //С const float Behavior::foot = 0.8 ; const float Behavior::d2r = 3.1415926 / 180 ; // const float Behavior::r2d = 180 / 3.1415926 ; const float Behavior::semiTime = 0.2; //? const float Behavior::football = 0.35 ; const float Behavior::LegWidth = 0.56 ; const float Behavior::footHeight = 0.095 ; const float Behavior::fMass = 0.1 ; //fmass bmass smassС hmassܵ const float Behavior::bMass = 0.25 ; const float Behavior::sMass = 0.25 ; const float Behavior::hMass = (0.1 * 0.095 / 2 + 0.25 * (0.095 + 0.964 / 2) + 0.25 * (0.095 + 0.964 + 1.3 / 2)) / 0.6 ; const float Leglen = 0.57 / 2 + 1.3 + 0.025 + 1.059; //ȳ const float Behavior::ZERO1 = 3 ; const float Behavior::ZERO2 = 15 ; const float Behavior::ZERO3 = 2 ; TPace Behavior::pace = P_ST; int Behavior::Step = 0; int Behavior::UpType = 1; int Behavior::count2 = 0; int Behavior::status = 0; int Behavior::stateCount = -1; int Behavior::KickStep = 0; ///////////////////////////////////////////////////////////////////////////////// //////////initiate////////// bool Behavior::singleAdjustLeftRotateDone1 = true; //for ReadyToKick bool Behavior::singleAdjustRightRotateDone1 = true; bool Behavior::singleSmallWalkForwardDone = true; bool Behavior::singleSideWalkLeftDone = true; bool Behavior::singleSideWalkRightDone = true; bool Behavior::closeToMiddleLineDone = true; bool Behavior::singleAdjustDirectionDone = true; bool Behavior::DirectionNotComputed=true; float Behavior::FinalTargetDirection=0.0; bool Behavior::GotoOK = false; //for CloseToBall bool Behavior::TurnOK1 = false; //for goTo bool Behavior::TurnOK2 = false; bool Behavior::TurnOK3 = false; bool Behavior::TurnOK4 = false; bool Behavior::TurnOK5 = false; bool Behavior::TurnOK6 = false; bool Behavior::singleWalkForwardDone2 = true; bool Behavior::singleWalkForwardDone3 = true; bool Behavior::singleWalkForwardDone4 = true; bool Behavior::singleWalkSideLeftDone2 = true; bool Behavior::singleWalkSideRightDone2 = true; bool Behavior::singleBackwardDone2 = true; bool Behavior:: SingleStepDone = false; //for walkForwardTo bool Behavior:: SingleTinyStepDone = false; bool Behavior::SingleLeftRotateDone = true; //for TurnTo bool Behavior::SingleRightRotateDone = true; bool Behavior::SingleTinyLeftRotateDone = true; bool Behavior::SingleTinyRightRotateDone = true; bool Behavior::SingleSlightLeftRotateDone = true; bool Behavior::SingleSlightRightRotateDone = true; bool Behavior:: SingleAdjustLeftRotateDone = true; //for adjustDirection bool Behavior:: SingleAdjustRightRotateDone = true; bool Behavior:: SingleMinuteLeftRotateDone = true; bool Behavior:: SingleMinuteRightRotateDone = true; bool Behavior::singleFinalSideWalkLeftDone = true; //for Final Adjust bool Behavior::singleFinalSideWalkRightDone = true; bool Behavior::singleFinalWalkForwardDone = true; bool Behavior::singleFinalWalkBackwardDone = true; bool Behavior::leftRightDone = false; bool Behavior::finalBackwardDone = false; bool Behavior::finalForwardDone = false; static int walkstatus=0; bool Behavior::finalTurnDone = false; int Behavior::ActionKickStep=0; //动作序列计数器。 int Behavior::PreKickStep=0; //预动作序列计数器。 void Behavior::InitKickAction() { //将动作序列从文件中读进来。 /* float LeftSlight[MAXACTIONS]; //左脚最小踢。 float LeftSmall[MAXACTIONS]; //左脚中踢。 float LeftBig[MAXACTIONS]; //左脚最大踢。 float RightSlight[MAXACTIONS]; //右脚最小踢。 float RightSmall[MAXACTIONS]; //右脚中踢。 float RightBig[MAXACTIONS]; //右脚最大踢。 int Number[3]; //动作序列个数。 */ int ReadInCounter=0; //读取文件的计数器。 string FileNames[6]={"./ActionLeftSlight","./ActionLeftSmall","./ActionLeftBig","./ActionRightSlight","./ActionRightSmall","./ActionRightBig"}; ifstream ReadInFile; //输入文件。 ReadInFile.open ( FileNames[0].c_str() ); //打开文件。 while ( !ReadInFile.eof() ) //文件未读完。 { ReadInFile >> gl.LeftSlight[ReadInCounter]; ReadInCounter++; } ; //while gl.Number[SLIGHTKICK]=ReadInCounter/22; ReadInFile.close(); ReadInCounter=0; ReadInFile.open ( FileNames[1].c_str() ); //打开文件。 while ( !ReadInFile.eof() ) //文件未读完。 { ReadInFile >> gl.LeftSmall[ReadInCounter]; ReadInCounter++; } ; //while gl.Number[SMALLKICK]=ReadInCounter/22; ReadInFile.close(); ReadInCounter=0; ReadInFile.open ( FileNames[2].c_str() ); //打开文件。 while ( !ReadInFile.eof() ) //文件未读完。 { ReadInFile >> gl.LeftBig[ReadInCounter]; ReadInCounter++; }; //while gl.Number[BIGKICK]=ReadInCounter/22; ReadInFile.close(); ReadInCounter=0; ReadInFile.open ( FileNames[3].c_str() ); //打开文件。 while ( !ReadInFile.eof() ) //文件未读完。 { ReadInFile >> gl.RightSlight[ReadInCounter]; ReadInCounter++; } ; //while ReadInFile.close(); ReadInCounter=0; ReadInFile.open ( FileNames[4].c_str() ); //打开文件。 while ( !ReadInFile.eof() ) //文件未读完。 { ReadInFile >> gl.RightSmall[ReadInCounter]; ReadInCounter++; } ; //while ReadInFile.close(); ReadInCounter=0; ReadInFile.open ( FileNames[5].c_str() ); //打开文件。 while ( !ReadInFile.eof() ) //文件未读完。 { ReadInFile >> gl.RightBig[ReadInCounter]; ReadInCounter++; } ; //while ReadInFile.close(); ReadInCounter=0; return; } ; //void Behavior::InitKickAction() string Behavior::LeftKick ( int MyKickPower,bool &MyKickOK ) { stringstream Command2Send; //回发的命令。 int JointCounter=0; //遍历关节用的计数器。 float l1 , l2 , l3 , l4 , l5 , l6 , l7 , l8 , l9 , la; float r1 , r2 , r3 , r4 , r5 , r6 , r7 , r8 , r9 , ra; float head, neck; Behavior::GetJointInfo ( l1, l2, l3, l4, l5, l6, l7, l8, l9, la, r1, r2, r3, r4, r5, r6, r7, r8, r9, ra, head, neck ); if ( PreKickStep<10 ) //先变到自己要的角度。 { PreKickStep++; switch ( MyKickPower ) { case SLIGHTKICK: case SMALLKICK: return SetJointInfo ( 10,l1, l2, l3, l4, l5, l6, l7, l8, l9, la, r1, r2, r3, r4, r5, r6, r7, r8, r9, ra, head, neck,-63.0f,0.0f,0.0f,0.0f,0.0f,0.0f,24.0f,-40.0f,24.0f,0.0f,-63.0f,0.0f,0.0f,0.0f,0.0f,0.0f,24.0f,-40.0f,24.0f,0.0f,0.0f,0.0f ); break; case BIGKICK: return SetJointInfo ( 10,l1, l2, l3, l4, l5, l6, l7, l8, l9, la, r1, r2, r3, r4, r5, r6, r7, r8, r9, ra, head, neck,-63.0f,0.0f,0.0f,0.0f,0.0f,0.0f,21.0f,-35.0f,21.0f,0.0f,-63.0f,0.0f,0.0f,0.0f,0.0f,0.0f,21.0f,-35.0f,21.0f,0.0f,0.0f,0.0f ); break; default: return Standing(); } ; //switch } //if else if (PreKickStep<15) //先停一下，稳下来。 { PreKickStep++; return SetJointSpeed(0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0); } //if else //发送踢球序列。 { if ( ActionKickStep<gl.Number[MyKickPower] ) //动作序列未发送完成。 { switch ( MyKickPower ) //根据MyKickPower发送不同的动作序列。 { case SLIGHTKICK: //轻微的踢。 for ( JointCounter=0;JointCounter<22;JointCounter++ ) //遍历关节。 { Command2Send << "(" << gc.strEffName[JointCounter] << " " << gl.LeftSlight[22*ActionKickStep+JointCounter] << ")"; //构造命令。 } ; //for break; case SMALLKICK: //稍重的踢。 for ( JointCounter=0;JointCounter<22;JointCounter++ ) //遍历关节。 { Command2Send << "(" << gc.strEffName[JointCounter] << " " << gl.LeftSmall[22*Acti