int main(void)
{
ArSerialConnection con;
ArRobot robot;
int ret;
std::string str;
ArActionLimiterForwards limiter("speed limiter near", 300, 600, 250);
ArActionLimiterForwards limiterFar("speed limiter far", 300, 1100, 400);
ArActionLimiterBackwards backwardsLimiter;
ArActionConstantVelocity stop("stop", 0);
ArActionConstantVelocity backup("backup", -200);
ArSonarDevice sonar;
ArACTS_1_2 acts;
ArSonyPTZ sony(&robot);
ArGripper gripper(&robot, ArGripper::GENIO);
Acquire acq(&acts, &gripper);
DriveTo driveTo(&acts, &gripper, &sony);
PickUp pickUp(&acts, &gripper, &sony);
TakeBlockToWall takeBlock(&robot, &gripper, &sony, &acq, &driveTo, &pickUp,
&backup);
Aria::init();
if (!acts.openPort(&robot))
{
printf("Could not connect to acts\n");
exit(1);
}
robot.addRangeDevice(&sonar);
//con.setBaud(38400);
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
robot.setDeviceConnection(&con);
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
sony.init();
ArUtil::sleep(1000);
//robot.setAbsoluteMaxTransVel(400);
robot.setStateReflectionRefreshTime(250);
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
ArUtil::sleep(200);
robot.addAction(&limiter, 100);
robot.addAction(&limiterFar, 99);
robot.addAction(&backwardsLimiter, 98);
robot.addAction(&acq, 77);
robot.addAction(&driveTo, 76);
robot.addAction(&pickUp, 75);
robot.addAction(&backup, 50);
robot.addAction(&stop, 30);
robot.run(true);
Aria::shutdown();
return 0;
}
int main(void)
{
ArSerialConnection con;
ArRobot robot;
int ret;
std::string str;
ArActionLimiterForwards limiter("speed limiter near", 225, 600, 250);
ArActionLimiterForwards limiterFar("speed limiter far", 225, 1100, 400);
ArActionTableSensorLimiter tableLimiter;
ArActionLimiterBackwards backwardsLimiter;
ArActionConstantVelocity stop("stop", 0);
ArSonarDevice sonar;
ArACTS_1_2 acts;
ArPTZ *ptz;
ptz = new ArVCC4(&robot, true);
ArGripper gripper(&robot);
Acquire acq(&acts, &gripper);
DriveTo driveTo(&acts, &gripper, ptz);
DropOff dropOff(&acts, &gripper, ptz);
PickUp pickUp(&acts, &gripper, ptz);
TakeBlockToWall takeBlock(&robot, &gripper, ptz, &acq, &driveTo, &pickUp,
&dropOff, &tableLimiter);
if (!acts.openPort(&robot))
{
printf("Could not connect to acts, exiting\n");
exit(0);
}
Aria::init();
robot.addRangeDevice(&sonar);
//con.setBaud(38400);
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
robot.setDeviceConnection(&con);
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
ptz->init();
ArUtil::sleep(8000);
printf("### 2222\n");
ptz->panTilt(0, -40);
printf("### whee\n");
ArUtil::sleep(8000);
robot.setAbsoluteMaxTransVel(400);
robot.setStateReflectionRefreshTime(250);
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
ArUtil::sleep(200);
robot.addAction(&tableLimiter, 100);
robot.addAction(&limiter, 99);
robot.addAction(&limiterFar, 98);
robot.addAction(&backwardsLimiter, 97);
robot.addAction(&acq, 77);
robot.addAction(&driveTo, 76);
robot.addAction(&pickUp, 75);
robot.addAction(&dropOff, 74);
robot.addAction(&stop, 30);
robot.run(true);
Aria::shutdown();
return 0;
}
int main(int argc, char **argv)
{
Aria::init();
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArRobot robot;
ArRobotConnector robotConnector(&parser, &robot);
if(!robotConnector.connectRobot())
{
ArLog::log(ArLog::Terse, "teleopActionsExample: Could not connect to the robot.");
if(parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::exit(1);
}
}
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::exit(1);
}
ArLog::log(ArLog::Normal, "teleopActionsExample: Connected.");
// limiter for close obstacles
ArActionLimiterForwards limiter("speed limiter near", 300, 600, 250);
// limiter for far away obstacles
ArActionLimiterForwards limiterFar("speed limiter far", 300, 1100, 400);
// limiter that checks IR sensors (like Peoplebot has)
ArActionLimiterTableSensor tableLimiter;
// limiter so we don't bump things backwards
ArActionLimiterBackwards backwardsLimiter;
// the joydrive action
ArActionJoydrive joydriveAct;
// the keydrive action
ArActionKeydrive keydriveAct;
// sonar device, used by the limiter actions.
ArSonarDevice sonar;
printf("This program will allow you to use a joystick or keyboard to control the robot.\nYou can use the arrow keys to drive, and the spacebar to stop.\nFor joystick control press the trigger button and then drive.\nPress escape to exit.\n");
// if we don't have a joystick, let 'em know
if (!joydriveAct.joystickInited())
printf("Do not have a joystick, only the arrow keys on the keyboard will work.\n");
// add the sonar to the robot
robot.addRangeDevice(&sonar);
// set the robots maximum velocity (sonar don't work at all well if you're
// going faster)
robot.setAbsoluteMaxTransVel(400);
// enable the motor
robot.enableMotors();
// Add the actions, with the limiters as highest priority, then the teleop.
// actions. This will keep the teleop. actions from being able to drive too
// fast and hit something
robot.addAction(&tableLimiter, 100);
robot.addAction(&limiter, 95);
robot.addAction(&limiterFar, 90);
robot.addAction(&backwardsLimiter, 85);
robot.addAction(&joydriveAct, 50);
robot.addAction(&keydriveAct, 45);
// Configure the joydrive action so it will let the lower priority actions
// (i.e. keydriveAct) request motion if the joystick button is
// not pressed.
joydriveAct.setStopIfNoButtonPressed(false);
// run the robot, true means that the run will exit if connection lost
robot.run(true);
Aria::exit(0);
}
