avoidwall.c

偶然收集到的一个C语言源码

#include <airobot/c/SimpleRobot.h>

/**
 * 这是一个不会撞墙的机器人，演示了如何用迭代的方法控制机器人的移动
 * @author xiemin
 */
 
//计算目标点时采用的搜索半径
#define SEARCH_DIS 100
//每次迭代的递增量
#define GAP toRadians(5)
//离墙的距离
#define AWAY_FROM_WALL 50

int isValid(double x, double y);

void onTick(struct TickAction* action)
{
	//机器人的移动方向
	double moveHeading = getHeading();
	double b = 0;
	double nextX, nextY;
	char buffer[100];
	
	//如果机器人的速度小于0，说明机器人向后运动，移动的方向和车身方向相反
	if(getVelocity()<0) moveHeading += PI;
	
	//进入迭代
	while(1)
	{
		//计算目标点
	  	nextPoint(getX(), getY(), moveHeading+b, 
	  		SEARCH_DIS, &nextX, &nextY);
		if(isValid(nextX, nextY))
		{
			//目标点是有效的，迭代结束
			moveTo(nextX, nextY);
			return;
		}
		else b += GAP; //进行下一次迭代
	}
}

/*
 * 判断给定的点是否是一个有效的目标点
 */
int isValid(double x, double y)
{
	return inCourt(x, y, AWAY_FROM_WALL);
}

//启动机器人程序
int main(int argC, char* argV[])
{
	tickHook = onTick;
	return startup(argC, argV);
}