bodydecision.cpp

mersad源码 03年robocup 季军 可以研究一下大家

/*
 *  Copyright 2002-2004, Mersad Team, Allame Helli High School (NODET).
 *
 *  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.
 *
 *  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 Library General Public License for more details.
 *
 *  Created by: Ahmad Boorghany
 *  Released on Friday 1 April 2005 by Mersad RoboCup Team.
 *  For more information please read README file.
*/

#include <math.h>

#include <Logger.h>
#include <Defines.h>
#include <BodyDecision.h>

using namespace std;


BodyDecision::BodyDecision(const WorldModel *worldModel,
		SayDecisionFlags &sayFlags, TurnNeckMode &turnNeckMode):
		worldModel(worldModel),	sayFlags(sayFlags),
		turnNeckMode(turnNeckMode)
{
	command = NULL;
}

BodyDecision::~BodyDecision()
{
}

void BodyDecision::addAdvancedAction(AdvancedAction *advancedAction,
		AdvancedActionWeights weight, int quickLevel)
{
	advancedActions.push_back(advancedAction);
	weights.push_back(weight);
	quickLevels.push_back(quickLevel);
	values.push_back(NOVALUE);
}

DecideType BodyDecision::decide(int quickLevel)
{
	float curWeight;
	float max = -0xFFFF;
	unsigned maxer = NOVALUE;

	sayFlags.decideReset();

	LOG << "Decide weighting:" << endl;

	for (unsigned i = 0; i < advancedActions.size(); i++)
	{
		if (quickLevels[i] == -1) // this advanced action is turned off.
			continue;

		if (quickLevels[i] >= quickLevel) // we have enough time.
			values[i] = advancedActions[i]->getValue();
		else if (values[i] == NOVALUE)
			continue;

		LOG << "\tAdvancedAction[" << i << "]:"
			<< " Value:" << values[i];

		curWeight = weights[i].rate * pow(values[i], weights[i].power) +
				weights[i].alpha;

		LOG << " Weight:" << curWeight << endl;

		if (quickLevels[i] >= 2 && values[i] >= AD_ALWAYS_RUN_VALUE) // quick levels more than 2 are quantomic.
		{
			maxer = i;
			break;
		}

		if (curWeight > max)
		{
			maxer = i;
			max = curWeight;
		}
	}

	if (maxer != NOVALUE)
	{
		advancedActions[maxer]->execute(sayFlags);
		command = advancedActions[maxer]->getCommand();
		return advancedActions[maxer]->getType();
	}

	command = NULL;
	return DT_NONE;
}

Command *BodyDecision::getCommand()
{
	return command;
}

void BodyDecision::resetWeights()
{
	for (unsigned i = 0; i < advancedActions.size(); i++)
	{
		weights[i].rate = 1;
		weights[i].power = 1;
		weights[i].alpha = 0;
	}
}

void BodyDecision::resetValues()
{
	for (unsigned i = 0; i < advancedActions.size(); i++)
		values[i] = NOVALUE;
}

void BodyDecision::resetQuickLevels()
{
	for (unsigned i = 0; i < advancedActions.size(); i++)
		quickLevels[i] = -1; // turning all advanced actions off.
}

void BodyDecision::updateAfterSenseBody()
{
	resetValues();
}