void init( const std::string& hostname )
{
// Initialize the actors
if (JPG_IMAGE_OUTPUT == imageOutputFormat)
{
camera.setJPGDecodingEnabled(false);
}
// Connect
std::cout << "Connecting...";
com.setAddress( hostname.c_str() );
com.connectToServer( true );
if( false == com.isConnected() )
{
std::cout << std::endl << "Could not connect to " << com.address() << std::endl;
#ifdef WIN32
std::cout << "Press any key to exit..." << std::endl;
rec::robotino::api2::waitForKey();
#endif
rec::robotino::api2::shutdown();
exit( 1 );
}
else
{
std::cout << "success" << std::endl;
}
}
void drive()
{
Bumper bumper;
while( com.isConnected() && false == bumper.value() && _run )
{
com.processEvents();
rec::robotino::api2::msleep( 1000 );
}
}
void init( const std::string& hostname)
{
// Initialize the actors
motor1.setMotorNumber( 0 );
motor2.setMotorNumber( 1 );
motor3.setMotorNumber( 2 );
// Connect
std::cout << "Connecting to "<< hostname << " ... " << std::endl;
com.setAddress( hostname.c_str() );
com.connect();
odometry.set(0, 0, 0);
std::cout << std::endl << "Connected" << std::endl;
}
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();
}
}
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 destroy()
{
com.disconnect();
}
void destroy()
{
com.disconnectFromServer();
}
