control.cpp

robocuo相关资料robocuo相关资料

#include <math.h>
#include <iostream>
using namespace std;

#include "control.h"
#include "body.h"
#include "world.h"
#include "connection.h"
#include "common.h"

control::control(world*w, body *b, connection *c)
{
	bd=b;
	wm=w;
	cn=c;
	state_number = 0;
	previous_action_name = "";
	previous_action_size = 0;
	previous_repeat_number = 0;
	state_time = 0;
	trajectory_it = 0;
}

void control::create_robot()
{
	cn->send("(scene rsg/agent/nao/nao.rsg)");
}

void control::init_robot()
{
	char s[100];
	sprintf(s,"(init (unum %d)(teamname %s))", wm->uniform_number, wm->team_name.c_str());
	cn->send(s);
}

void control::beam(double x, double y, double angle)
{
	char s[100];
	//to do: dar soorati ke samte raast boodim baayad x o y o angle taghir kone
	sprintf(s,"(beam %.2lf %.2lf %.2lf )",x,y,angle);
	cn->send(s);
}

bool control::go_to_state(state *st, command *cm)//agar reside bashe true bar migardoone
{
	bool flag=true;
	for(int i=0;i<20;i++)
		if(	st->imp[i] && !get_rate(st->angle[i],bd->j[i].angle,
				st->gain[i],st->precision[i],cm->power[i]))
			flag=false;
	return st->max_time > 0 && wm->simulation_time - state_time > st->max_time
		|| flag && (st->min_time < 0
		|| wm->simulation_time - state_time > st->min_time);
}

void control::do_command(command *c)
{
	char s[100];
	for(int i=0;i<22;i++)
	{
		sprintf(s,"(%s %.2lf)",joint_number_to_name[i].c_str(),c->power[i]);
		//cout<<"SENDCOMMAND--> "<<s<<endl;
		cn->send(s);
	}
}
//residan be zavie morede nazar         final_angle current_angle gain  precision   rate
bool control::get_rate(double a,double b,double g,double p,double &r)
{
	double d = a-b;
	while(d>180)
		d-=360;
	while(d<-180)
		d+=360;
	r = d * g / 100;
	return fabs(d)<p;
}

void control::reset()
{
	state_number = 0;
	state_time = wm->simulation_time;
}

void control::do_action(action *act)
{
	if(act->name == "_"||act->name == "")
		;
		//cerr<< "Action with no name" << endl;
	previous_action_name = act->name;
	previous_action_size = act->arr.size();
	previous_repeat_number = act->repeat;
	int t = state_number;
//	act->save(cout);
	while(state_number < act->arr.size() 
			&& go_to_state(&act->arr[state_number],&cmd))
	{
		state_time = wm->simulation_time;
		state_number++;
		if(state_number >= act->arr.size())
			state_number = act->repeat;
		if(state_number == t)
			break;
	}
}

void control::follow_trajectory(trajectory *trj)
{
	if(trj->name != trajectory_name)
	{
		trajectory_name = trj->name;
		trajectory_it = 222;
	}
	if(trajectory_it == trj->arr.size())
		trajectory_it = trj->repeat;
	do_command(&(trj->arr[trajectory_it++]));
}