C++ (Cpp) Odometry::y示例

void drive(const int x, const int y, const int phi )
{
	const int vel=90;
	rec::core_lt::Timer timer;
	timer.start();

	//while( com.isConnected()
	//	&& false == bumper.value()
	//	&& timer.msecsElapsed() < 10000 )
	double M=sqrt(mysqr(odometry.x()-x)+mysqr(odometry.y()-y));
	while( com.isConnected()
		&& false == bumper.value()
		&& M>30 )
	{
		M=sqrt(mysqr(odometry.x()-x)+mysqr(odometry.y()-y));
		//std::cout<<M<<std::endl;
		omniDrive.setVelocity( vel*((double)(x-odometry.x()))/M, vel*((double)(y-odometry.y()))/M, 0 );
		com.waitForUpdate();
	}
	/*
	while( com.isConnected()
		&& false == bumper.value()
		&& odometry.x() < x )
	{
		omniDrive.setVelocity( 70, 0, 0 );
		com.waitForUpdate();
	}
	while( com.isConnected()
		&& false == bumper.value()
		&& odometry.y() < y )
	{
		omniDrive.setVelocity( 0, 70, 0 );
		com.waitForUpdate();
	}*/
	
	while( com.isConnected()
		&& false == bumper.value()
		&& phi>0
		&& (abs(odometry.phi()-phi)>5) )
	{
		omniDrive.setVelocity( 0, 0, +30 );
		com.waitForUpdate();
	}
	while( com.isConnected()
		&& false == bumper.value()
		&& phi<0
		&& (abs(odometry.phi()-phi)>5) )
	{
		omniDrive.setVelocity( 0, 0, -30 );
		com.waitForUpdate();
	}

}

示例#2

显示文件

文件： main.cpp 项目： fmulp/Robot_Guide

void drive(const int x, const int y, const int phi )
{
    rec::core_lt::Timer timer;
    timer.start();

    //while( com.isConnected()
    //	&& false == bumper.value()
    //	&& timer.msecsElapsed() < 10000 )
    while( com.isConnected()
            && false == bumper.value()
            && odometry.x() < x )
    {
        omniDrive.setVelocity( 70, 0, 0 );
        com.waitForUpdate();
    }
    while( com.isConnected()
            && false == bumper.value()
            && odometry.y() < y )
    {
        omniDrive.setVelocity( 0, 70, 0 );
        com.waitForUpdate();
    }
    while( com.isConnected()
            && false == bumper.value()
            && odometry.phi() < phi )
    {
        omniDrive.setVelocity( 0, 0, 30 );
        com.waitForUpdate();
    }

}