kicknrun.cpp

浙江大学 RoboCup3D 2006 源代码

/***************************************************************************
 *   Copyright (C) 2004 - 2006 by ZJUBase                                  *
 *                                National Lab of Industrial Control Tech. *
 *                                Zhejiang University, China               *
 *                                                                         *
 *   Achievements of ZJUBase in RoboCup Soccer 3D Simulation League:       *
 *    In RoboCup championship,                                             *
 *      - June 2006 - 3rd place in RoboCup 2006, Bremen, Germany (lost     *
 *        the semi-final with a coin toss)                                 *
 *      - July 2005 - 3rd place in RoboCup 2005, Osaka, Japan (share the   *
 *        3rd place with team Caspian)                                     *
 *    In RoboCup local events,                                             *
 *      - April 2006 - 2nd place in RoboCup Iran Open 2006, Tehran, Iran   *
 *        (lost the final with a coin toss)                                *
 *      - December 2005 - 2nd place in AI Games 2005, Isfahan, Iran        *
 *      - July 2005 - champion in China Robot Competition 2005, Changzhou, *
 *        China                                                            *
 *      - October 2004 - champion in China Soccer Robot Competition 2004,  *
 *        Guangzhou, China                                                 *

*                                                                         *
 *   Team members:                                                         *
 *    Currently the team leader is,                                        *
 *           Hao JIANG ([e-mail]; [e-mail])       *
 *    In the next season, the leader will be                               *
 *           Yifeng ZHANG ([e-mail])                        *
 *    ZJUBase 3D agent is created by                                       *
 *           Dijun LUO ([e-mail])                             *
 *    All the members who has ever contributed:                            *
 *           Jun JIANG                                                     *
 *           Xinfeng DU ([e-mail])                             *
 *           Yang ZHOU ([e-mail])                             *
 *           Zhipeng YANG                                                  *
 *           Xiang FAN                                                     *
 *                                                                         *
 *   Team Manager:                                                          *
 *      Ms. Rong XIONG ([e-mail])                            *
 *                                                                         *
 *   If you met any problems or you have something to discuss about        *
 *   ZJUBase. Please feel free to contact us through EMails given below.   *
 *                                                                         *
 *   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; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   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.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

#ifndef WIN32

#include "kicknrun.h"
#include <strstream>
#include "soccertypes.h"
using namespace std;


KickNRun::KickNRun() : Soccer()
{
}

KickNRun::~KickNRun()
{
}

void KickNRun::BehaveBeforeKickOff()
{
    int num = mWM->GetTeamUnum();
    double f = mWM->GetAgentRadius() * 10;
    
	Vector3 pos;
    pos[0] = (-1 - (num % 4)) * f;
    pos[1] = ((num / 4) - 2) *f;
    pos[2] = 0;

    Beam(Vector3(pos));
    Drive(Vector3(0,0,0));
}

void KickNRun::BehaveKickOff()
{
    TTeamIndex ti = mWM->GetMyTeam();
    if (ti == TI_NONE) {
        return;
    }
    
	if (mWM->GetPlayMode() == PM_KickIn_Our) {
        BehaveMyKickOff();
    } else {
		BehaveTheirKickOff();
    }
}

void KickNRun::BehaveMyKickOff()
{
    // for now, just kick and run
    BehavePlayOn();
}

void KickNRun::BehaveTheirKickOff()
{
    // do nothing
    Drive(Vector3(0,0,0));
}

void KickNRun::BehaveKickIn()
{
    TTeamIndex ti = mWM->GetMyTeam();
    if (ti == TI_NONE) {
        return;
    }
    if (mWM->GetPlayMode() == PM_KickIn_Our) {
        BehaveMyKickIn();
    } else {
        BehaveTheirKickIn();
    }
}

void KickNRun::BehaveMyKickIn()
{
    // for now just kick and run
    BehavePlayOn();
}

void KickNRun::BehaveTheirKickIn()
{
    // do nothing
    Drive(Vector3(0,0,0));
}

void KickNRun::BehavePlayOn()
{
    WorldModel::VisionSense ballSense =
        mWM->GetVisionSense(WorldModel::VO_BALL);

    // calculate kick position behind the ball, looking towards the
    // opponent goal
    Vector3 goal(
                  mWM->GetFieldLength(),
                  mWM->GetFieldWidth()/2.0,
                  0
                  );
    cout << "GOAL :" << goal[0] << " " << goal[1] << " " << goal[2] << endl;

    Vector3 ballPos = mWM->GetPosition(ballSense);
    cout << "BALL :" << ballPos[0] << " " << ballPos[1] << " " << ballPos[2] << endl;
    
	Vector3 goalDriveLine(goal - ballPos);
    goalDriveLine.normalize();
    
	double minKickDist = mWM->GetMinimalKickDistance();
    goalDriveLine *= minKickDist;
    Vector3 kickPos = ballPos + goalDriveLine;
    cout << "KICK :" << kickPos[0] << " " << kickPos[1] << " " << kickPos[2] << endl;
    
	// if we are far away from the kickPos drive towards it
    Vector3 myPos = mWM->GetMyPosition();
    cout << "MYPOS :" << myPos[0] << " " << myPos[1] << " " << myPos[2] << endl;
    
	Vector3 kickDriveLine = kickPos - myPos;
    double kickPosDist = kickDriveLine.mod();
    kickDriveLine.normalize();
    
	if (kickPosDist > 10) {
        // we are far away, go full power
        kickDriveLine *= 100;
        Drive(kickDriveLine);
    } else if (kickPosDist > minKickDist) {
        // we're getting closer, go slower
        kickDriveLine *= 20;
        Drive(kickDriveLine);
    } else {
        // we are close enough to kick the ball
        Kick(20,100);
    }
}

void KickNRun::Behave()
{
    switch (mWM->GetPlayMode()) {
        case PM_BeforeKickOff :
            BehaveBeforeKickOff();
            break;

        case PM_KickOff_Our :
        case PM_KickOff_Opp :
            BehaveKickOff();
            break;

        case PM_PlayOn :
            BehavePlayOn();
            break;

        case PM_KickIn_Our :
        case PM_KickIn_Opp :
            BehaveKickIn();
            break;

        default:
            break;
    }
}

#endif