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


//?設(shè)置小車輪數(shù)
void?Velocity?（?Robot?*robot?int?vl?int?vr?）
{
	robot->velocityLeft??=?vl;
	robot->velocityRight?=?vr;
}

//?小車轉(zhuǎn)角控制
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?
	if?（theta_e?	
	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;
	//計算當(dāng)前位置與目標(biāo)的相對位移
	dx?=?x?-?robot->pos.x;
	dy?=?y?-?robot->pos.y;

	//計算到目標(biāo)位置的直線距離
	d_e?=?sqrt（dx?*?dx?+?dy?*?dy）;

	//計算當(dāng)前位置到目標(biāo)點的角度
	if?（dx?==?0?&&?dy?==?0）
		desired_angle?=?90;
	else
		desired_angle?=?（int）（180.?/?PI?*?atan2（（double）（dy）?（double）（dx）））;

	//當(dāng)前機(jī)器人與目標(biāo)的夾角
	theta_e?=?desired_angle?-?（int）robot->rotation;
	
	while?（theta_e?>?180）?theta_e?-=?360;
	while?（theta_e?
	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?	{
		theta_e?+=?180;
		
		if?（theta_e?>?180）?
			theta_e?-=?360;
		if?（theta_e?>?80）
			theta_e?=?80;
		if?（theta_e?			theta_e?=?-80;
		if?（d_e?			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）
	{
		if?（d_e?			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??基礎(chǔ)動作源碼\第一講?課程及資料\1.ppt

?????文件???????1557??2006-03-22?17:21??基礎(chǔ)動作源碼\第一講?課程及資料\第一講.txt

?????文件?????733696??2006-04-01?17:04??基礎(chǔ)動作源碼\第三講?課程及資料\3.ppt

?????文件???????2393??2006-03-15?17:28??基礎(chǔ)動作源碼\第三講?課程及資料\soft1000?mls\Action.cpp

?????文件????????385??2006-03-07?11:17??基礎(chǔ)動作源碼\第三講?課程及資料\soft1000?mls\Action.h

?????文件???????2367??2006-03-16?10:22??基礎(chǔ)動作源碼\第三講?課程及資料\soft1000?mls\InterFace.cpp

?????文件???????2973??2006-03-15?17:28??基礎(chǔ)動作源碼\第三講?課程及資料\soft1000?mls\InterFace.h

?????文件????????145??2006-03-07?11:38??基礎(chǔ)動作源碼\第三講?課程及資料\soft1000?mls\Restore.1\Strategy.lst

?????文件???????5454??2006-03-07?11:19??基礎(chǔ)動作源碼\第三講?課程及資料\soft1000?mls\Strategy.cpp

?????文件???????4392??2006-03-15?17:28??基礎(chǔ)動作源碼\第三講?課程及資料\soft1000?mls\Strategy.dsp

?????文件????????541??2006-03-07?11:19??基礎(chǔ)動作源碼\第三講?課程及資料\soft1000?mls\Strategy.dsw

?????文件????????748??2006-03-07?11:17??基礎(chǔ)動作源碼\第三講?課程及資料\soft1000?mls\Strategy.h

?????文件?????140288??2007-12-05?23:43??基礎(chǔ)動作源碼\第三講?課程及資料\soft1000?mls\Strategy.ncb

?????文件??????54784??2007-12-05?23:43??基礎(chǔ)動作源碼\第三講?課程及資料\soft1000?mls\Strategy.opt

?????文件???????1665??2006-03-15?17:16??基礎(chǔ)動作源碼\第三講?課程及資料\soft1000?mls\Strategy.plg

?????文件???????2393??2006-04-02?16:16??基礎(chǔ)動作源碼\第三講?課程及資料\空案例softmls\Action.cpp

?????文件????????407??2006-03-23?17:21??基礎(chǔ)動作源碼\第三講?課程及資料\空案例softmls\Action.h

?????文件???????8485??2007-12-05?23:37??基礎(chǔ)動作源碼\第三講?課程及資料\空案例softmls\Debug\Action.obj

?????文件??????????0??2007-12-05?23:37??基礎(chǔ)動作源碼\第三講?課程及資料\空案例softmls\Debug\Action.sbr

?????文件??????12736??2007-12-05?23:37??基礎(chǔ)動作源碼\第三講?課程及資料\空案例softmls\Debug\InterFace.obj

?????文件??????????0??2007-12-05?23:37??基礎(chǔ)動作源碼\第三講?課程及資料\空案例softmls\Debug\InterFace.sbr

?????文件????1934336??2007-12-05?23:37??基礎(chǔ)動作源碼\第三講?課程及資料\空案例softmls\Debug\Strategy.bsc

?????文件?????237661??2007-12-05?23:37??基礎(chǔ)動作源碼\第三講?課程及資料\空案例softmls\Debug\Strategy.dll

?????文件????????736??2007-12-05?23:37??基礎(chǔ)動作源碼\第三講?課程及資料\空案例softmls\Debug\Strategy.exp

?????文件?????297516??2007-12-05?23:37??基礎(chǔ)動作源碼\第三講?課程及資料\空案例softmls\Debug\Strategy.ilk

?????文件???????2268??2007-12-05?23:37??基礎(chǔ)動作源碼\第三講?課程及資料\空案例softmls\Debug\Strategy.lib

?????文件???????1574??2007-12-05?23:37??基礎(chǔ)動作源碼\第三講?課程及資料\空案例softmls\Debug\Strategy.obj

?????文件?????187060??2007-12-05?23:37??基礎(chǔ)動作源碼\第三講?課程及資料\空案例softmls\Debug\Strategy.pch

?????文件?????549888??2007-12-05?23:37??基礎(chǔ)動作源碼\第三講?課程及資料\空案例softmls\Debug\Strategy.pdb

?????文件??????????0??2007-12-05?23:37??基礎(chǔ)動作源碼\第三講?課程及資料\空案例softmls\Debug\Strategy.sbr

............此處省略51個文件信息