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();
}
}
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();
}
}