soccer.cpp

robocup3d源化码 Nexus3D.tar.gz

/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

this file is part of rcssserver3D
Fri May 9 2003
Copyright (C) 2002,2003 Koblenz University
Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
$Id: soccer.cpp,v 1.2 2004/02/12 14:07:21 fruit Exp $

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; version 2 of the License.

This program 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 General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#include "soccer.h"
#include <sstream>
#include "global.h" 
using namespace zeitgeist;
using namespace std;
using namespace boost;
using namespace oxygen;
using namespace salt;

Soccer::Soccer() : Behavior()
{
    mTeamName = "unnamed";
    mSensationCount = 0;
    mDriveVec = Vector3f(0,0,0);
}

Soccer::~Soccer()
{
}

void Soccer::InitAgent()
{
    GetLog()->Debug() << "sending init command\n";

    stringstream ss;
    ss << "A(init (unum " << mTeamUnum << ") (teamname " << mTeamName << "))";

    mComm->PutOutput(ss.str().c_str());
}

void Soccer::CreateAgent()
{
    // use the create effector to setup the Sensors and Effectors
    GetLog()->Debug() << "creating agent\n";

    mComm->PutOutput("A(create)");
}

void Soccer::Beam(const salt::Vector3f& position)
{
    GetLog()->Debug()
        << "beaming ["
        << position[0]
        << " " << position[1]
        << " " << position[2]
        << "]\n";

    stringstream ss;
    ss << "A(beam "
       << " " << position[0]
       << " " << position[1]
       << " " << position[2]
       << ")\n";

    mComm->PutOutput(ss.str());
}

void Soccer::Kick(const float angle, const float power)
{
    GetLog()->Debug()
        << "kicking Ball"
        << " angle=" << angle
        << " power=" << power
        << "\n";

    stringstream ss;
    ss << "A(kick " << angle << " " << power << ")";

    mComm->PutOutput(ss.str());
}

void Soccer::Drive(const salt::Vector3f& driveVec)
{
    GetLog()->Debug()
        << "driving ["
        << driveVec[0] << " "
        << driveVec[1] << " "
        << driveVec[2]
        << "]\n";

	/*
    if (driveVec == mDriveVec)
        {
            // no explicit change needed
            GetLog()->Debug()
                << "not changing drive vector\n";
            return;
        }
	*/

    stringstream ss;
    ss << "A(drive"
       << " " << driveVec[0]
       << " " << driveVec[1]
       << "  " << driveVec[2]
       << ")";

    mComm->PutOutput(ss.str());
}

void Soccer::Think()
{
    GetLog()->Debug()
        << "received sensation " << mSensationCount << "\n";

    switch (mSensationCount)
        {
        case 0:
            CreateAgent();
            break;

        case 1:
            InitAgent();
            break;

        default:
            Behave();
        }

    ++mSensationCount;

    mComm->PutOutput(DoneThinkingMessage());
}

void Soccer::GoTo(float x,float y,float z){

	Vector3f vector,next_pos(x,y,z),self_pos(self.pos[0],self.pos[1],self.pos[2]);
	vector=next_pos-self_pos;
	//DoLog(LOG_POS,"self.pos.x=%f   ,self.pos.y=%f   ,self.pos.z=%f",self.pos.x,self.pos.y,self.pos.z);
	//DoLog(LOG_POS,"vector.x=%f   ,vector.y=%f   ,vector.z=%f",vector[0],vector[1],vector[2]);
	float kickPosDist = vector.Length();
    	//float minKickDist = mWM->GetMinimalKickDistance();
	vector.Normalize();
	if(kickPosDist> 2 || (self.vel.Length()<1&&kickPosDist> 1)) vector*=100;
	if(kickPosDist> 1  ) vector*=90;
	else if(kickPosDist>0.5) vector*=50;
	else if(kickPosDist>0.2) vector*=30;
	else vector*=20;
	//vector*=100;
	Drive(vector);
	/*if (kickPosDist > 10)
        {
            // we are far away, go full power
            vector *= 100;
            Drive(vector);
        } else if (kickPosDist > minKickDist)
        {
            // we're getting closer, go slower
            vector *= 50;
            Drive(vector);
        }*/
}