command.cpp

RoboCup仿真组世界冠军源代码

/*
    Copyright (C) 2001  Tsinghuaeolus

    Authors : ChenJiang, YaoJinyi, CaiYunpeng, Lishi

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2.1 of the License, or (at your option) any later version.

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Lesser General Public License for more details.

	If you make any changes or have any comments we would appreciate a 
	message to [e-mail].
*/

#include "stdafx.h"
#include <math.h>

#include "Command.h"
#include "utils.h"
#include "Global.h"


/*******************    Commmand   *******************************/
void Command::move(float X,float Y){//move command
	x = X;
	y = Y;
	type = CMD_move;
	time = situation.CurrentTime;
}
  
void Command::dash(float pow){//dash command
	power = pow;
	type = CMD_dash;
	time = situation.CurrentTime;
}

void Command::turn(float Moment){
	angle = Moment;
	type = CMD_turn;
	time = situation.CurrentTime;
}

void Command::kick(float pow,float ang){ //kick command
	power = pow;
	angle = ang;
	type = CMD_kick;
	time = situation.CurrentTime;
}

void Command::seize(float ang){ //seize command
	if ( !CP_goalie ) my_error("Only goalies can catch");
	if ( situation.playmode == PM_Before_Kick_Off ) return;

	type=CMD_catch;
	angle=ang;
	time = situation.CurrentTime;
}

void Command::say(char* msg){
	sprintf(cmdbuf,"(say %s)",msg);
	type = CMD_say;
	time = situation.CurrentTime;
}

void Command::turn_neck(float ang){
	angle = ang;
	pose_limitation(angle, SP_min_neck_moment, SP_max_neck_moment);
  
	type = CMD_turn_neck;
	time = situation.CurrentTime;
}

void Command::sense_body(){
	type = CMD_sense_body;
	time = situation.CurrentTime;
}

void Command::change_view(){
	type = CMD_change_view;
	time = situation.CurrentTime;
}

void Command::disconnect(){
	type = CMD_bye;
	time = situation.CurrentTime;
}

void Command::stay(){
	type = CMD_stay;
	time = situation.CurrentTime;
}

void Command::donothing(){
	type = CMD_donothing;
	time = situation.CurrentTime;
}

bool Command::valid(){
	return (bool) (type != CMD_none);
}

bool Command::valid(Time time){
	 return (bool) ((type != CMD_none && this->time == time)||
		 ((type == CMD_move) && (this->time == -1)));
}

/***********************    Actions  **********************************/
Actions::Actions(){
	latest = 0;
	kicks = 0;
	dashes = 0;
	turns = 0;
	turn_necks = 0;
	says = 0;
	lastcatchtime = -5;
}

/************ send initial message *******/
void Actions::send_init_message(){//send initializing message	
	if (IP_reconnect)
		sprintf(cmdbuf, "(reconnect %s %d)", CP_TeamName, IP_reconnect);
	else if ( CP_goalie && SP_version >= 4.00){
		sprintf(cmdbuf, "(init %s (version %.2f) (goalie))", CP_TeamName, SP_version);
	}
	else
		sprintf(cmdbuf, "(init %s (version %.2f))", CP_TeamName, SP_version);
	send_message(cmdbuf,sock);
}

/*******   coach commands  ********************/
void Actions::change_mode(PlayMode playmode){
	switch(playmode){
	case PM_No_Mode: return;
	case PM_Before_Kick_Off: 
		sprintf(cmdbuf,"(before_kick_off)");break;
	case PM_My_Kick_Off:
		sprintf(cmdbuf,"(kick_off_%c)",situation.MySide);break;
	case PM_Their_Kick_Off:
		sprintf(cmdbuf,"(kick_off_%c)",situation.TheirSide);break;
	case PM_My_Kick_In:
		sprintf(cmdbuf,"(kick_in_%c)",situation.MySide);break;
	case PM_Their_Kick_In:
		sprintf(cmdbuf,"(kick_in_%c)",situation.TheirSide);break;
	case PM_My_Corner_Kick:
		sprintf(cmdbuf,"(corner_kick_%c)",situation.MySide);break;
	case PM_Their_Corner_Kick:
		sprintf(cmdbuf,"(corner_kick_%c)",situation.TheirSide);break;
	case PM_My_Goal_Kick:
		sprintf(cmdbuf,"(goal_kick_%c)",situation.MySide);break;
	case PM_Their_Goal_Kick:
		sprintf(cmdbuf,"(goal_kick_%c)",situation.TheirSide);break;
	case PM_My_Free_Kick:
		sprintf(cmdbuf,"(free_kick_%c)",situation.MySide);break;
	case PM_Their_Free_Kick:
		sprintf(cmdbuf,"(kick_in_%c)",situation.TheirSide);break;
	default:
		break;
	}
	send_message(cmdbuf, sock);
}

void Actions::move_ball(float X,float Y){
	sprintf(cmdbuf,"(move (ball) %.2f %.2f)",X,Y);
	send_message(cmdbuf, sock);
}

void Actions::move_player(int side,UNum no,float X,float Y){
	if (side == 0)
		sprintf(cmdbuf,"(move (%s) %.2f %.2f)",MyTeamName,X,Y);
	else if(side == 1)
		sprintf(cmdbuf,"(move (%s) %.2f %.2f)",TheirTeamName,X,Y);
	send_message(cmdbuf, sock);
}

/****** see if commands take effects  ******************/
void Actions::ActionsInvalidation(){
	turn_valid = Commanded(CMD_turn, situation.LastCycle());
	dash_valid = Commanded(CMD_dash, situation.LastCycle());
	turn_neck_valid = Commanded(CMD_turn_neck, situation.LastCycle());
	kick_valid = Commanded(CMD_kick, situation.LastCycle());
	say_valid = Commanded(CMD_say, situation.LastCycle());
	if (sensory.SensedInfKnown(situation.CurrentTime)){
		SenseBodyInfo& sensebody = sensory.GetSenseBodyInfo(situation.CurrentTime);
		if (sensebody.turns < turns){
			my_error("Miss %d turns",turns - sensebody.turns);
			DoLog(LOG_MISS,"%d turns", turns - sensebody.turns);
			turneffects.Data(situation.LastCycle()) = 0;
			turn_valid = false;
		}
		if (sensebody.dashes < dashes){
			my_error("Miss %d dashes",dashes - sensebody.dashes);
			DoLog(LOG_MISS,"%d dashs", dashes - sensebody.dashes);
			dasheffects.Data(situation.LastCycle()) = 0;
			usedpower.Data(situation.LastCycle()) = 0;
			dash_valid = false;
		}
		if (sensebody.kicks < kicks){
			my_error("Miss %d kicks",kicks - sensebody.kicks);
			DoLog(LOG_MISS,"%d kicks",kicks - sensebody.kicks);
			kickeffects.Data(situation.LastCycle()) = 0;
			usedpower.Data(situation.LastCycle()) = 0;
			kick_valid = false;
		}
		if (sensebody.turn_necks < turn_necks){
			my_error("Miss %d turn_necks",turn_necks - sensebody.turn_necks);
			DoLog(LOG_MISS,"%d turn_necks",turn_necks - sensebody.turn_necks);
			turn_neckeffects.Data(situation.LastCycle()) = 0;
			turn_neck_valid = false;
		}
		if (sensebody.says < says){
			say_valid = false;
			my_error("Miss %d says",says - sensebody.says);
			DoLog(LOG_MISS,"%d says",says - sensebody.says);
		}
		turns = sensebody.turns;
		dashes = sensebody.dashes;
		kicks = sensebody.kicks;
		says = sensebody.says;
		turn_necks = sensebody.turn_necks;
	}
}

/***************   commands are sent by this function  **************/
void Actions::send_action(){
	if (!commands[latest].valid(situation.CurrentTime)){
		my_error("Is this command valid? CMD_type : %d",commands[latest].type);
		return;
	}

	switch (commands[latest].type){
	case CMD_kick:
		sprintf(cmdbuf,"(kick %.2f %.2f)", commands[latest].power, commands[latest].angle);
		kicks++;
		kickeffects.Setdata(Polar2Vector(commands[latest].power * Kickrate(), NormalizeAngle(commands[latest].angle + Self.bodyfacing)), situation.CurrentTime);
		usedpower.Setdata((float)fabs(commands[latest].power) * (commands[latest].power > 0 ? 1.0f : CP_negativepower_factor), situation.CurrentTime);
		DoLog(LOG_SENDACTION,"kickeffect (%.2f %.2f)", KickEffect(situation.CurrentTime).x, KickEffect(situation.CurrentTime).y);
		break;
	case CMD_dash: 
		sprintf(cmdbuf,"(dash %.2f)", commands[latest].power);
		dashes++;
		dasheffects.Setdata(Polar2Vector(commands[latest].power * Dashrate(), Self.bodyfacing), situation.CurrentTime);
		usedpower.Setdata((float)fabs(commands[latest].power) * (commands[latest].power > 0 ? 1.0f : CP_negativepower_factor), situation.CurrentTime);
		DoLog(LOG_SENDACTION, "dasheffect (%.2f %.2f)", DashEffect(situation.CurrentTime).x, DashEffect(situation.CurrentTime).y);
		break;
	case CMD_turn: 
		sprintf(cmdbuf,"(turn %.2f)", commands[latest].angle);
		turns++;
		turneffects.Setdata(commands[latest].angle * Turnrate(), situation.CurrentTime);
		DoLog(LOG_SENDACTION, "turneffect %.2f", TurnEffect(situation.CurrentTime));
		break;
	case CMD_turn_neck: 
		sprintf(cmdbuf,"(turn_neck %.2f)", commands[latest].angle);
		turn_necks++;
		turn_neckeffects.Setdata(commands[latest].angle, situation.CurrentTime);
		DoLog(LOG_SENDACTION, "turnneckeffect %.2f", TurnNeckEffect(situation.CurrentTime));
		break;
	case CMD_catch:
		sprintf(cmdbuf,"(catch %.2f)", commands[latest].angle);
		lastcatchtime = situation.CurrentTime;
		DoLog(LOG_SENDACTION, "catch %.2f", commands[latest].angle);
		break;
	case CMD_move:
		sprintf(cmdbuf,"(move %.2f %.2f)",commands[latest].x, commands[latest].y);
		DoLog(LOG_SENDACTION, "move");
		break;
	case CMD_sense_body:
		sprintf(cmdbuf,"(sense_body)");
		DoLog(LOG_SENDACTION, "sense_body");
		break;
	case CMD_change_view:
		sprintf(cmdbuf, "(change_view %s %s)", width_string, qual_string);
		DoLog(LOG_SENDACTION, "changeview");
		watch.Changeview();
		break;
	case CMD_bye:
		sprintf(cmdbuf, "(bye)");
		DoLog(LOG_SENDACTION, "bye");
		break;
	case CMD_say:
		DoLog(LOG_SENDACTION, "say");
		break;
	default: 
		return;
	}
	send_message(cmdbuf, sock);
	//DoLog(LOG_SENDACTION, cmdbuf);
	latest = (latest + 1)% CP_num_stored_CMDs;
}

void Actions::turn(float Moment){
	if (Moment > SP_max_moment){
		//my_error("Should not turn that positive much!");
		Moment = SP_max_moment;
	}
	if (Moment < SP_min_moment){
		//my_error("Should not turn that negative much!");