action.cpp

武汉工程大学曾经参加过仿真机器人世界杯的源程序

/***************************************************************************
* 文件名称：Action.cpp
* 摘    要：该文件部分或全部实现了Action.h中所声明的函数。
* 
* 当前版本：2.0
* 作    者：Moon, Dr Jun Jo, 李迅
* 完成日期：2005年10月24日
***************************************************************************/

#include "action.h"

//	向机器人发轮速
void Velocity(HRobot &robot, double pwm1, double pwm2)
{
	if(pwm1 > 125)			pwm1 = 125;
	else if(pwm1 < -125)	pwm1 = -125;

	if(pwm2 > 125)			pwm2 = 125;
	else if(pwm2 < -125)	pwm2 = -125;

	robot.pwm1 = pwm1;
	robot.pwm2 = pwm2;
}

//	转到指定角
void Rotation1(HRobot &robot, double desired_angle)
{
	double theta_e = desired_angle - robot.angle;

	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;

	double ka = 0.6;
	double pwm1 = -ka * theta_e;
	double pwm2 = ka * theta_e;

	Velocity (robot, pwm1, pwm2);
}

void Rotation2(HRobot &robot, double desired_angle)
{
	double theta_e = desired_angle - robot.angle;

	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;

	double pwm1  = -0.1 * theta_e;
	double pwm2  = 0.1 * theta_e;
	if (fabs(theta_e) > 50) 
	{
		pwm1 = -9./90.0 * theta_e;
		pwm2 = 9./90.0 * theta_e;
	}
	else if (fabs(theta_e) > 20)
	{
		pwm1 = -11.0/90.0 * theta_e;
		pwm2 = 11.0/90.0 * theta_e;
	}
	Velocity (robot, pwm1, pwm2);
}
//	跑到定点
void Position1(HRobot &robot, double x, double y)
{
	double dx = x - robot.x;
	double dy = y - robot.y;

	double d_e = sqrt(dx * dx + dy * dy);

	double desired_angle;
	if (dx == 0 && dy == 0)
		desired_angle = 90;
	else
		desired_angle = (180. / PI * atan2(dy, dx));

	double theta_e = desired_angle - robot.angle;

	while (theta_e > 180)	theta_e -= 360;
	while (theta_e < -180)	theta_e += 360;

	double ka = 0.5;
	double kd  = 4;

	if(d_e > 30)	d_e = 30;
	if(d_e < 15)	d_e = 15;

	double pwm1 = kd * d_e - ka * theta_e;
	double pwm2 = kd * d_e + ka * theta_e;

	Velocity (robot, pwm1, pwm2);
}

void Position2(HRobot &robot, double x, double y)
{
	double dx = x - robot.x;
	double dy = y - robot.y;

	double d_e = sqrt(dx * dx + dy * dy);

	double desired_angle;
	if (dx == 0 && dy == 0)
		desired_angle = 90;
	else
		desired_angle = 180. / PI * atan2(dy, dx);

	double theta_e = desired_angle - (int)robot.angle;
	
	while (theta_e > 180)	theta_e -= 360;
	while (theta_e < -180)	theta_e += 360;

	double vc = 70;
	double Ka = 10.0/90.0;

	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.;

	double pwm1, pwm2;
	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 && fabs(theta_e) < 40)
			Ka = 0.1;
		pwm2 = (-vc * (1.0 / (1.0 + exp(-3.0 * d_e)) - 0.3) + Ka * theta_e);
		pwm1 = (-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 && fabs(theta_e) < 40)
			Ka = 0.1;
		pwm2 = ( vc * (1.0 / (1.0 + exp(-3.0 * d_e)) - 0.3) + Ka * theta_e);
		pwm1 = ( vc * (1.0 / (1.0 + exp(-3.0 * d_e)) - 0.3) - Ka * theta_e);
	}

	else
	{
		pwm2 = (int)(+.17 * theta_e);
		pwm1 = (int)(-.17 * theta_e);
	}

	Velocity (robot, pwm1, pwm2);
}

//	简单守门员
void Goalie(HRobot &robot, const Ball &ball, const Field &fd)
{
	double x = FLEFTX + 8;
	double y = ball.y;
	if(y > fd.gtop)			y = fd.gtop;
	else if(y < fd.gbot)	y = fd.gbot;

	Position2(robot, x, y);
}