コード例 #1
0
void Motion_controller::move(const uint16_t &mm,const uint8_t &dir)
{
	uint16_t segment = 1500;
	Message msg;
	uint16_t dis = 0;
	uint8_t count = mm / segment;
	uint8_t count_msg = 0;
	if(mm < CAR_MOVEMENT_THRESHOLD)
	{
		if(glo_debug_msg)
		printf("Motion_controller::move() -> the distance is too small\n");
		return ;
	}
	switch(dir)
	{
		case CAR_FORWARD://forward
			if(mm > segment)
			{
				for(count_msg = 0;count_msg < count;count_msg++)
				{
					msg.CarMoveUpMM(segment);
					if(glo_motion_enable)
						msg.safe_sendMessage(this->Com->fd);
				}
				dis = mm - count * segment;
				msg.CarMoveUpMM(dis);
				if(glo_motion_enable)
					msg.safe_sendMessage(this->Com->fd);
			}
			else
			{
				msg.CarMoveUpMM(mm);
				if(glo_motion_enable)
					msg.safe_sendMessage(this->Com->fd);
			}
		break;
			
		case CAR_BACKWARD://backward
			if(mm > segment)
			{
				for(count_msg = 0;count_msg < count;count_msg++)
				{
					msg.CarMoveDownMM(segment);
					if(glo_motion_enable)
						msg.safe_sendMessage(this->Com->fd);
				}
				dis = mm - count * segment;
				msg.CarMoveDownMM(dis);
				if(glo_motion_enable)
					msg.safe_sendMessage(this->Com->fd);
			}
			else
			{
				msg.CarMoveDownMM(mm);
				if(glo_motion_enable)
					msg.safe_sendMessage(this->Com->fd);
			}
		break;

		case CAR_LEFT://left
			if(mm > segment)
			{
				for(count_msg = 0;count_msg < count;count_msg++)
				{
					msg.CarMoveLeftMM(segment);
					if(glo_motion_enable)
						msg.safe_sendMessage(this->Com->fd);
				}
				dis = mm - count * segment;
				msg.CarMoveLeftMM(dis);
				if(glo_motion_enable)
					msg.safe_sendMessage(this->Com->fd);
			}
			else
			{
				msg.CarMoveLeftMM(mm);
				if(glo_motion_enable)
					msg.safe_sendMessage(this->Com->fd);
			}
		break;

		case CAR_RIGHT://right
			if(mm > segment)
			{
				for(count_msg = 0;count_msg < count;count_msg++)
				{
					msg.CarMoveRightMM(segment);
					if(glo_motion_enable)
						msg.safe_sendMessage(this->Com->fd);
				}
				dis = mm - count * segment;
				msg.CarMoveRightMM(dis);
				if(glo_motion_enable)
					msg.safe_sendMessage(this->Com->fd);
			}
			else
			{
				msg.CarMoveRightMM(mm);
				if(glo_motion_enable)
					msg.safe_sendMessage(this->Com->fd);
			}
		break;

		default:
		break;
	}
	return ;
}