Initializing sonar and controlling movement of robot while avoiding hitting anything

Go down

Initializing sonar and controlling movement of robot while avoiding hitting anything

Post  bharris on Thu Mar 26, 2009 9:48 am

/*
Very simple robot movement:
move foward until you almost hit something
turn around and come back to origin
*/

#include "Aria.h"

int main(int argc, char **argv)
{
ArRobot robot; // robot
ArSonarDevice sonar; // sonar

/*** Initialize the System ***/
printf("initializing aria...\n");
Aria::init(); // mandatory init
printf("initializing connection...\n");
ArSimpleConnector connector(&argc, argv);
if (!connector.parseArgs() || argc > 1)
{
connector.logOptions();
exit(1);
}
printf("initializing sonar...\n");
robot.addRangeDevice(&sonar);
printf("connecting to robot...\n");
if (!connector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
printf("turning on motors...\n");
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
printf("running the robot...\n");
robot.runAsync(false);

/**************************************/
printf("original location = %f %f\n",robot.getX(),robot.getY());
printf("moving at speed 200\n");
robot.setVel(200);
while(1)
{
ArUtil::sleep(500);
printf("sonars 2,3 - %d %d\n",
robot.getSonarRange(2), robot.getSonarRange(3));
if ((robot.getSonarRange(2) < 300) ||
(robot.getSonarRange(3) < 300))
break;
}
robot.setVel(0);

printf("done moving to wall, now at %f %f\n",robot.getX(),robot.getY());
robot.setDeltaHeading(180);
ArUtil::sleep(4000);

printf("moving back toward home at 200\n");
robot.setVel(200);
while(1)
{
ArUtil::sleep(500);
if ((robot.getX() < 200.0) && (robot.getY() < 200.0))
break;
}
robot.setVel(0);
printf("done moving back, now at %f %f\n",robot.getX(),robot.getY());

/*** Shut down ***/
robot.disconnect();
robot.stopRunning();
Aria::shutdown();
return 0;
}

bharris

Posts : 9
Join date : 2009-01-12

View user profile

Back to top Go down

Back to top

- Similar topics

 
Permissions in this forum:
You cannot reply to topics in this forum