actionsystem.cpp

浙江大学 RoboCup3D 2006 源代码

			}else{
				list.push_back(cmd1);
				cmds.push_back(list);
			}
		}
	} else {
		list.push_back(cmd1);
		cmds.push_back(list);
	}

}

void ActionSystem::runTo(CommandQueue& cmds,Vector3 dest)
{
	Vector3 F;
	AgentStatus a;
	a.pos = global.wm.nextPos;
	a.vel = global.wm.nextVel;
	F = predictor.predictForceToReachPoint(dest,a);
	SoccerCommand cmd(CT_DRIVE,F);
	CommandList list;
	list.push_back(cmd);
	cmds.push_back(list);
}

void ActionSystem::runTo(CommandQueue& cmds,Vector3 dest,Angle ang_to_kick)
{
	Vector3 F;
	AgentStatus a;
	a.pos = global.wm.nextPos;
	a.vel = global.wm.nextVel;
	F = predictor.predictForceToReachPoint(dest,a,ang_to_kick);
	SoccerCommand cmd(CT_DRIVE,F);

	CommandList list;
	list.push_back(cmd);
	cmds.push_back(list);
}

void ActionSystem::Init(ServerSettings *ss,Situation *st)
{
	SS = ss;
	ST = st;
	intercept.Init(st,ss);
}

void ActionSystem::StopBall(CommandQueue& cmds)
{
}

void ActionSystem::dribbleToTarget(CommandQueue& cmds,Vector3 target)
{
}

void ActionSystem::dribbleForward(CommandQueue& cmds)
{
	CommandList list;
	SoccerCommand cmd1,cmd2;
	double dribKickTheta = 0;
	double dribKickPower = 5;
	double dribbleForce= 100;

	if(global.wm.ballVel.mod()<=0.02){
		dribKickPower=10;
		dribKickTheta=10;
		dribbleForce=60;
	}

	double dribbleAng = (global.wm.ballPos+global.wm.ballVel-global.wm.nextPos).ang();
	Vector3 dribbleVec = Vector3(dribbleForce,dribbleAng,0,POLAR);
	cmd1 = SoccerCommand(CT_DRIVE,dribbleVec);
	cmd2 = SoccerCommand(CT_KICK,dribKickTheta,dribKickPower);
	for(int i=0;i<20;i++){
		list.clear();
		list.push_back(cmd1);
		list.push_back(cmd2);
		cmds.push_back(list);
	}

}

void ActionSystem::doWhenBeforeKickoff(CommandQueue& cmds)
{
	Vector3 pos = strategy.getFormationPosition();
	if (pos.x > 0) pos.x = -1;
	if((pos - Vector3(0, 0, 0)).mod() < 10) {
		if (global.wm.myNumber == 9) {
			pos.x = -7; pos.y = -7;
		} else {
			pos.x = -10;
		}
	}

	SoccerCommand cmd(CT_BEAM,pos);
	CommandList list;
	list.push_back(cmd);
	cmds.push_back(list);
}

void ActionSystem::AddCommand(SoccerCommand cmd1,SoccerCommand cmd2)
{
	Angle panAngInc;
	Angle tiltAngInc;
	SoccerCommand cmd_panTilt;
	ST->getAngInc(panAngInc,tiltAngInc);
	cmd_panTilt=SoccerCommand(CT_PANTILT, panAngInc, tiltAngInc);////yzp0527
	global.commands[global.nCommand].push_back(cmd_panTilt);
   	global.commands[global.nCommand].push_back(cmd1);
   	global.commands[global.nCommand].push_back(cmd2);
	global.nCommand ++;
}

void ActionSystem::positioningKickoff(CommandQueue& cmds)
{
	Vector3 pos = strategy.getFormationPosition();
	if (pos.x > 0) pos.x = -1;
	if((pos - Vector3(0, 0, 0)).mod() < 10) {
		if (global.wm.myNumber == 9) {
			pos.x = -7; pos.y = -7;
		} else {
			pos.x = -10;
		}
	}

	CommandList list;
	as.runTo(cmds, pos);
}

void ActionSystem::AddCommandQueue(CommandQueue cmds)
{
	Angle panAngInc;
	Angle tiltAngInc;
	SoccerCommand cmd_panTilt;
	CommandList list;

	ST->getAngInc(panAngInc,tiltAngInc);
	cmd_panTilt=SoccerCommand(CT_PANTILT, panAngInc, tiltAngInc);/////yzp0527
	cmds[0].push_back(cmd_panTilt);

	for(int i=0;i<cmds.size()&&i<COMMAND_MAX;i++)
		global.commands[global.nCommand++]=cmds[i];
}

string ActionSystem::GenerateCommandString(int index)
{
	if(index<0||index>COMMAND_MAX-1)
		return "";

	string s;
	int i;
	for(i=0;i<global.commands[index].size();i++){
		s += global.commands[index][i].getCommandString();
	}

	return s;
}

void ActionSystem::say(double n)
{
	char MSG[800];
	parseSayMsg(n, MSG);
	char tempmsg[800];
	sprintf(tempmsg, "%d", global.wm.myNumber);
	strcat(tempmsg, sayMsgSign.c_str());
	strcat(tempmsg, "_");
	strcat(tempmsg, MSG);
	strcpy(MSG, tempmsg);
	
	SoccerCommand cmd(CT_SAY, MSG);
	global.commands[0].push_back(cmd);
}

void ActionSystem::say(double *n, int sizeofI)
{
	char MSG[800];
	parseSayMsg(n, sizeofI, MSG);
	char tempmsg[800];
	sprintf(tempmsg, "%d", global.wm.myNumber);
	strcat(tempmsg, sayMsgSign.c_str());
	strcat(tempmsg, "_");
	strcat(tempmsg, MSG);
	strcpy(MSG, tempmsg);
	
	SoccerCommand cmd(CT_SAY, MSG);
	global.commands[0].push_back(cmd);
}
	
void ActionSystem::say(double (*b)[3], double (*n)[3], int sizeofB, int sizeofI)
{
	char MSG[800]="";
	char tempmsg[800]="";
	sprintf(tempmsg, "%d", global.wm.myNumber);
	strcat(tempmsg, "_");
	strcat(tempmsg, sayMsgSign.c_str());
	strcat(tempmsg, "_");
	if (sizeofB == 2) {
		parseSayMsg(b, MSG);
		strcat(tempmsg, "zzb_");
		strcat(tempmsg, MSG);
	}
	parseSayMsg(n, sizeofI, MSG);
	strcat(tempmsg, "zzp_");
	strcat(tempmsg, MSG);
	strcpy(MSG, tempmsg);
	strcat(MSG, "zzz_");
	
	SoccerCommand cmd(CT_SAY, MSG);
	global.commands[0].push_back(cmd);
}

void ActionSystem::parseSayMsg(double n, char *mymsg)
{
	char msg[5] = "", tmp[5] = "";
	int N = (int)n;
	int s;

	if (N == 0) {
		sprintf(tmp, "%c", N+33);
		strcpy(mymsg, tmp);
	} else {
		while(N) {
			s = N % 88;
			
			if (s == 0)
				s = s+33;
			else if (s < 5)
				s = s+34;
			else if (s < 55)
				s += 37;
			else if(s < 57)
				s += 38;
			else
				s += 39;

			sprintf(tmp, "%c", s);
			strcat(msg, tmp);
			N = N / 88;
		}
		strcpy(mymsg, msg);
	}
}

void ActionSystem::parseSayMsg(double *n, int sizeofI, char *mymsg)
{
	char msg[512]="", msg2[5]="", msg3[512]="";

	for (int i=0; i<sizeofI; i++) {
		parseSayMsg(*(n+i), msg2);
		strcpy(msg, msg3);
		strcat(msg, msg2);

		if (i != (sizeofI - 1))
			strcat(msg, "_");
		strcpy(msg3, msg);
	}
	strcpy(mymsg, msg3);
}

void ActionSystem::parseSayMsg(double (*n)[3], int sizeofI, char *mymsg)
{
	char msg2[20]="", msg4[512]="", tmp[20] = ""; 
	double p[22][2];
	for (int i=0; i<22; i++) {
		p[i][0] = (n[i][1]+110.0)*1000.0;
		p[i][1] = (n[i][2]+68.0)*1000.0;
	}

	for (int i=0; i<sizeofI; i++) {
		sprintf(tmp, "%d", (int)n[i][0]);
		strcat(tmp, "_");

		parseSayMsg(*(p+i), 2, msg2);
		strcat(tmp, msg2);
		strcat(tmp, "_");

		strcat(msg4, tmp);
	}
	strcpy(mymsg,msg4);
}

void ActionSystem::parseSayMsg(double (*b)[3], char *mymsg)
{
	char msg2[20]="", msg4[512]="", tmp[20] = ""; 
	double p[2][3];
	for (int i=0; i<2; i++) {
		p[i][0] = (b[i][0]+110.0)*1000.0;
		p[i][1] = (b[i][1]+68.0)*1000.0;
		p[i][2] = (b[i][2]+10.0)*1000.0;
	}

	for (int i=0; i<2; i++) {
		strcpy(tmp, "");
		parseSayMsg(*(p+i), 3, msg2);
		strcat(tmp, msg2);
		strcat(tmp, "_");

		strcat(msg4, tmp);
	}
	strcpy(mymsg,msg4);
}

void ActionSystem::revolve(CommandQueue & cmds)
{
	CommandList list;
	SoccerCommand cmd1,cmd2;
	double myVel=global.wm.myVel.mod();
	Angle myangle=global.wm.nextVel.ang();
	double force= 100;
	double ang = Normalize(myangle +80);

	Vector3 vec = Vector3(force,ang,0,POLAR);
	for(int i=0;i<20;i++){
		if(i%10==15){
			ang=ang+20;
			vec=Vector3(force,ang,0,POLAR);
		}
		cmd1 = SoccerCommand(CT_DRIVE,vec);
		list.clear();
		list.push_back(cmd1);
		cmds.push_back(list);
	}

}