fira机器人足球基础动作源码

fira机器人足球基础动作源码

#include 
#include “Strategy.h“
#include “Action.h“


// 设置小车轮数
void Velocity （ Robot *robot int vl int vr ）
{
	robot->velocityLeft  = vl;
	robot->velocityRight = vr;
}

// 小车转角控制
void Angle （ Robot *robot int desired_angle）
{
	int theta_e vl vr;
	theta_e = desired_angle - （int）robot->rotation;	
	
	while （theta_e > 180） theta_e -= 360;
	while （theta_e < -180） theta_e += 360;

	if （theta_e < -90） theta_e += 180;
	
	else if （theta_e > 90） theta_e -= 180;

	if （abs（theta_e） > 50） 
	{
		vl = （int）（-9./90.0 * （double）theta_e）;
		vr = （int）（9./90.0 * （double）theta_e）;
	}
	else if （abs（theta_e） > 20） 
	{
		vl = （int）（-11.0/90.0 * （double）theta_e）;
		vr = （int）（11.0/90.0 * （double）theta_e）;
	}
	Velocity （robot vl vr）;
}

//移动小车到制定位置
void Position（ Robot *robot double x double y ）
{
	int desired_angle = 0 theta_e = 0 d_angle = 0 vl vr vc = 70;

	double dx dy d_e Ka = 10.0/90.0;
	//计算当前位置与目标的相对位移
	dx = x - robot->pos.x;
	dy = y - robot->pos.y;

	//计算到目标位置的直线距离
	d_e = sqrt（dx * dx + dy * dy）;

	//计算当前位置到目标点的角度
	if （dx == 0 && dy == 0）
		desired_angle = 90;
	else
		desired_angle = （int）（180. / PI * atan2（（double）（dy） （double）（dx）））;

	//当前机器人与目标的夹角
	theta_e = desired_angle - （int）robot->rotation;
	
	while （theta_e > 180） theta_e -= 360;
	while （theta_e < -180） theta_e += 360;

	if （d_e > 100.） 
		Ka = 17. / 90.;
	else if （d_e > 50）
		Ka = 19. / 90.;
	else if （d_e > 30）
		Ka = 21. / 90.;
	else if （d_e > 20）
		Ka = 23. / 90.;
	else 
		Ka = 25. / 90.;
	
	if （theta_e > 95 || theta_e < -95）
	{
		theta_e += 180;
		
		if （theta_e > 180） 
			theta_e -= 360;
		if （theta_e > 80）
			theta_e = 80;
		if （theta_e < -80）
			theta_e = -80;
		if （d_e < 5.0 && abs（theta_e） < 40）
			Ka = 0.1;
		vr = （int）（-vc * （1.0 / （1.0 + exp（-3.0 * d_e）） - 0.3） + Ka * theta_e）;
		vl = （int）（-vc * （1.0 / （1.0 + exp（-3.0 * d_e）） - 0.3） - Ka * theta_e）;
	}
	
	else if （theta_e < 85 && theta_e > -85）
	{
		if （d_e < 5.0 && abs（theta_e） < 40）
			Ka = 0.1;
		vr = （int）（ vc * （1.0 / （1.0 + exp（-3.0 * d_e）） - 0.3） + Ka * theta_e）;
		vl = （int）（ vc * （1.0 / （1.0 + exp（-3.0 * d_e）） - 0.3） - Ka * theta_e）;
	}

	else
	{
		vr = （int）（+.17 * theta_e）;
		vl = （int）（-.17 * theta_e）;
	}

	Velocity（robot vl vr）;
}

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件    1055744  2006-04-08 08:57  基础动作源码\第一讲 课程及资料\1.ppt

     文件       1557  2006-03-22 17:21  基础动作源码\第一讲 课程及资料\第一讲.txt

     文件     733696  2006-04-01 17:04  基础动作源码\第三讲 课程及资料\3.ppt

     文件       2393  2006-03-15 17:28  基础动作源码\第三讲 课程及资料\soft1000 mls\Action.cpp

     文件        385  2006-03-07 11:17  基础动作源码\第三讲 课程及资料\soft1000 mls\Action.h

     文件       2367  2006-03-16 10:22  基础动作源码\第三讲 课程及资料\soft1000 mls\InterFace.cpp

     文件       2973  2006-03-15 17:28  基础动作源码\第三讲 课程及资料\soft1000 mls\InterFace.h

     文件        145  2006-03-07 11:38  基础动作源码\第三讲 课程及资料\soft1000 mls\Restore.1\Strategy.lst

     文件       5454  2006-03-07 11:19  基础动作源码\第三讲 课程及资料\soft1000 mls\Strategy.cpp

     文件       4392  2006-03-15 17:28  基础动作源码\第三讲 课程及资料\soft1000 mls\Strategy.dsp

     文件        541  2006-03-07 11:19  基础动作源码\第三讲 课程及资料\soft1000 mls\Strategy.dsw

     文件        748  2006-03-07 11:17  基础动作源码\第三讲 课程及资料\soft1000 mls\Strategy.h

     文件     140288  2007-12-05 23:43  基础动作源码\第三讲 课程及资料\soft1000 mls\Strategy.ncb

     文件      54784  2007-12-05 23:43  基础动作源码\第三讲 课程及资料\soft1000 mls\Strategy.opt

     文件       1665  2006-03-15 17:16  基础动作源码\第三讲 课程及资料\soft1000 mls\Strategy.plg

     文件       2393  2006-04-02 16:16  基础动作源码\第三讲 课程及资料\空案例softmls\Action.cpp

     文件        407  2006-03-23 17:21  基础动作源码\第三讲 课程及资料\空案例softmls\Action.h

     文件       8485  2007-12-05 23:37  基础动作源码\第三讲 课程及资料\空案例softmls\Debug\Action.obj

     文件          0  2007-12-05 23:37  基础动作源码\第三讲 课程及资料\空案例softmls\Debug\Action.sbr

     文件      12736  2007-12-05 23:37  基础动作源码\第三讲 课程及资料\空案例softmls\Debug\InterFace.obj

     文件          0  2007-12-05 23:37  基础动作源码\第三讲 课程及资料\空案例softmls\Debug\InterFace.sbr

     文件    1934336  2007-12-05 23:37  基础动作源码\第三讲 课程及资料\空案例softmls\Debug\Strategy.bsc

     文件     237661  2007-12-05 23:37  基础动作源码\第三讲 课程及资料\空案例softmls\Debug\Strategy.dll

     文件        736  2007-12-05 23:37  基础动作源码\第三讲 课程及资料\空案例softmls\Debug\Strategy.exp

     文件     297516  2007-12-05 23:37  基础动作源码\第三讲 课程及资料\空案例softmls\Debug\Strategy.ilk

     文件       2268  2007-12-05 23:37  基础动作源码\第三讲 课程及资料\空案例softmls\Debug\Strategy.lib

     文件       1574  2007-12-05 23:37  基础动作源码\第三讲 课程及资料\空案例softmls\Debug\Strategy.obj

     文件     187060  2007-12-05 23:37  基础动作源码\第三讲 课程及资料\空案例softmls\Debug\Strategy.pch

     文件     549888  2007-12-05 23:37  基础动作源码\第三讲 课程及资料\空案例softmls\Debug\Strategy.pdb

     文件          0  2007-12-05 23:37  基础动作源码\第三讲 课程及资料\空案例softmls\Debug\Strategy.sbr

............此处省略51个文件信息