body.c

以AIROBOT为平台构建的一个智能机器人。主源码见main.c。其余为使用的函数接口定义

#include "Robot.h"

//用户设置的想要转动的转角
static double angle = 0;
//用户设置的想要到达的速度
static double preferVelocity = 0;

//当前速度
static double velocity = 0;
//当前方位
static double heading = 0;
//所在地的坐标
static double x = 0;
static double y = 0;;

//最大转动速度
static const double MAX_VELOCITY_TURN = (10 * 3.14 / 180);

//////////////////////////////////////////////////////////////////
// 设置所要转动的度数,用于控制车身的方向
void turnBody(double a)
{ 
	angle = a; 
}
	
/*
 * 设置想到达的速度，用于控制车身的移动
 */
void moveBody(double v)
{ 
	preferVelocity = v;	
}
	
/*
 * 得到用户想用转动的角度的剩余量
 * 比如：上个单位时间，用户用turn(Math.toRadians(15))让车身旋转15度
 * 但是车身的最大旋转速度为10度/单位时间，所以这个单位时间调用getTurnRemaining()
 * 会得到返回值0.08754(5/180*PI),说明还有5度需要转动，如果用户此时没有新的turn函数的调用
 * 那么下个单位时间车身将会只转动5度
 */
double getBodyTurnRemaining()
{ 
	return angle; 
}
	
/*
 * 得到用户想要到达的速度
 */
double getBodyPreferVelocity()
{ 
	return preferVelocity;
}
	
/*
 * 得到当前的x坐标
 */
double getBodyX()
{	
	 return x; 
}
	
/*
 * 得到当前的y坐标
 */
double getBodyY()
{ 
	return y; 
}

/*
 * 得到当前的车身方向
 */
double getBodyHeading()
{
	return heading;
}
	
/*
 * 得到当前的移动速度
 */
double getBodyVelocity()
{
	return velocity;
}
	
// 这些函数由系统调用，设置一些关于机器人当前状态的信息
/*
 * 设置当前的x坐标
 */
void setBodyPosition(double newX, double newY)
{ 
	x = newX; 
	y = newY;
}
	
/*
 * 设置当前的车身方向
 */
void setBodyHeading(double h)
{
	heading = h;

	/* 重新计算要转的角度 */
    if (angle >= 0)
	{
	    if( angle < MAX_VELOCITY_TURN )
			angle = 0;
		else angle -= MAX_VELOCITY_TURN;
	}else{
		if( angle > - MAX_VELOCITY_TURN )
		    angle = 0;
		else angle += MAX_VELOCITY_TURN;
	}
}
	
/*
 * 设置当前的移动速度
 */
void setBodyVelocity(double v)
{
	velocity = v;
}