int main(void)
{
int ret;
std::string str;
CBTest cbTest;
ArFunctorC<CBTest> connectCB(&cbTest, &CBTest::connected);
ArFunctorC<CBTest> failedConnectCB(&cbTest, &CBTest::failedConnect);
ArFunctorC<CBTest> disconnectCB(&cbTest, &CBTest::disconnected);
ArFunctorC<CBTest> disconnectErrorCB(&cbTest, &CBTest::disconnectedError);
ArSerialConnection con;
ArRobot robot;
printf("If a robot is attached to your port you should see:\n");
printf("Failed connect, Connected, Disconnected Error, Connected, Disconnected\n");
printf("If no robot is attached you should see:\n");
printf("Failed connect, Failed connect, Failed connect\n");
printf("-------------------------------------------------------\n");
ArLog::init(ArLog::None, ArLog::Terse);
srand(time(NULL));
robot.setDeviceConnection(&con);
robot.addConnectCB(&connectCB, ArListPos::FIRST);
robot.addFailedConnectCB(&failedConnectCB, ArListPos::FIRST);
robot.addDisconnectNormallyCB(&disconnectCB, ArListPos::FIRST);
robot.addDisconnectOnErrorCB(&disconnectErrorCB, ArListPos::FIRST);
// this should fail since there isn't an open port yet
robot.blockingConnect();
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
exit(0);
}
robot.blockingConnect();
con.close();
robot.loopOnce();
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
exit(0);
}
robot.blockingConnect();
robot.disconnect();
exit(0);
}
int main(int argc, char **argv)
{
std::string str;
int ret;
ArTime start;
// connection to the robot
ArSerialConnection con;
// the robot
ArRobot robot;
// the connection handler from above
ConnHandler ch(&robot);
// init area with a dedicated signal handling thread
Aria::init(Aria::SIGHANDLE_THREAD);
// open the connection with the defaults, exit if failed
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
// set the robots connection
robot.setDeviceConnection(&con);
// try to connect, if we fail, the connection handler should bail
if (!robot.blockingConnect())
{
// this should have been taken care of by the connection handler
// but just in case
printf(
"asyncConnect failed because robot is not running in its own thread.\n");
Aria::shutdown();
return 1;
}
// run the robot in its own thread, so it gets and processes packets and such
robot.runAsync(false);
int i;
while (Aria::getRunning())
{
robot.lock();
robot.comStr(ArCommands::TTY3, "1234567890");
robot.unlock();
}
robot.disconnect();
// shutdown and ge tout
Aria::shutdown();
return 0;
}
int main()
{
/*
TODO
Check that the Starclass is updating the global pose right - not done
Check that the MapClass is giving the right pose - not done
check khenglee can use the behaviours - not done
check emergency control - not done
*/
Network yarp;
SamgarModule VR("Vrobot","Loco","wheel",SamgarModule::run); // Cant have spaces or underscores
VR.AddPortS("TransitIn");
SetupRobot();
ActionEmergencyControl EmergencyControl;
UpdateMap UpdMap (&VR);
UpOdo OdoUp (&VR);
Transit TransitIn (&VR);
// PlaySounder SoundPlayer (&VR);
// BehaveMove MoveBehave (&VR);
// MoveCAM CAMMove (&VR);
// lowest priority might actully be highest becouse coms direct to robot and not through desiredaction.
robot.addAction(&EmergencyControl,99); // need to check this works
robot.addAction(&UpdMap,99);
robot.addAction(&OdoUp,99);
robot.addAction(&TransitIn,70);
// robot.addAction(&SoundPlayer,100);
// robot.addAction(&MoveBehave,100);
// robot.addAction(&CAMMove,100);
robot.run(true);
robot.disconnect();
Aria::shutdown();
return 0;
}
int main(int argc, char **argv)
{
std::string str;
int ret;
int dist;
ArTime start;
ArPose startPose;
bool vel2 = false;
// connection to the robot
ArSerialConnection con;
// the robot
ArRobot robot;
// the connection handler from above
ConnHandler ch(&robot);
// init area with a dedicated signal handling thread
Aria::init(Aria::SIGHANDLE_THREAD);
if (argc != 2 || (dist = atoi(argv[1])) == 0)
{
printf("Usage: %s <distInMM>\n", argv[0]);
exit(0);
}
if (dist < 1000)
{
printf("You must go at least a meter\n");
exit(0);
}
// open the connection with the defaults, exit if failed
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
// set the robots connection
robot.setDeviceConnection(&con);
// try to connect, if we fail, the connection handler should bail
if (!robot.blockingConnect())
{
// this should have been taken care of by the connection handler
// but just in case
printf(
"asyncConnect failed because robot is not running in its own thread.\n");
Aria::shutdown();
return 1;
}
// run the robot in its own thread, so it gets and processes packets and such
robot.runAsync(false);
// just a big long set of printfs, direct motion commands and sleeps,
// it should be self-explanatory
robot.lock();
/*
robot.setAbsoluteMaxTransVel(2000);
robot.setTransVelMax(2000);
robot.setTransAccel(1000);
robot.setTransDecel(1000);
robot.comInt(82, 30); // rotkp
robot.comInt(83, 200); // rotkv
robot.comInt(84, 0); // rotki
robot.comInt(85, 30); // transkp
robot.comInt(86, 450); // transkv
robot.comInt(87, 4); // transki
*/
printf("Driving %d mm (going full speed for that far minus a meter then stopping)\n", dist);
if (vel2)
robot.setVel2(2200, 2200);
else
robot.setVel(2200);
robot.unlock();
start.setToNow();
startPose = robot.getPose();
while (1)
{
robot.lock();
printf("\r vel: %.0f x: %.0f y: %.0f: dist: %.0f heading: %.2f",
robot.getVel(), robot.getX(), robot.getY(),
startPose.findDistanceTo(robot.getPose()),
robot.getTh());
if (startPose.findDistanceTo(robot.getPose()) > abs(dist) - 1000)
{
printf("\nFinished distance\n");
robot.setVel(0);
robot.unlock();
break;
}
if (start.mSecSince() > 10000)
{
printf("\nDistance timed out\n");
robot.setVel(0);
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(50);
}
if (vel2)
robot.setVel2(0, 0);
else
robot.setVel(0);
start.setToNow();
while (1)
{
robot.lock();
if (vel2)
robot.setVel2(0, 0);
else
robot.setVel(0);
if (fabs(robot.getVel()) < 20)
{
printf("Stopped\n");
robot.unlock();
break;
}
if (start.mSecSince() > 2000)
{
printf("\nStop timed out\n");
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(50);
}
robot.lock();
robot.disconnect();
robot.unlock();
// shutdown and ge tout
Aria::shutdown();
return 0;
}
int main(int argc, char **argv)
{
std::string str;
int ret;
int successes = 0, failures = 0;
int action;
bool exitOnFailure = true;
ArSerialConnection con;
ArRobot robot;
//ArLog::init(ArLog::StdOut, ArLog::Verbose);
srand(time(NULL));
robot.runAsync(false);
// if (!exitOnFailure)
// ArLog::init(ArLog::None, ArLog::Terse);
//else
//ArLog::init(ArLog::None);
while (1)
{
if (con.getStatus() != ArDeviceConnection::STATUS_OPEN &&
(ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
++failures;
if (exitOnFailure)
{
printf("Failed\n");
exit(0);
}
else
{
ArUtil::sleep(200);
robot.unlock();
continue;
}
}
robot.lock();
robot.setDeviceConnection(&con);
robot.unlock();
ArUtil::sleep((rand() % 5) * 100);
if (robot.asyncConnect())
{
robot.waitForConnectOrConnFail();
robot.lock();
if (!robot.isConnected())
{
if (exitOnFailure)
{
printf("Failed after %d tries.\n", successes);
exit(0);
}
printf("Failed to connect successfully");
++failures;
}
robot.comInt(ArCommands::SONAR, 0);
robot.comInt(ArCommands::SOUNDTOG, 0);
//robot.comInt(ArCommands::PLAYLIST, 0);
robot.comInt(ArCommands::ENCODER, 1);
ArUtil::sleep(((rand() % 20) + 3) * 100);
++successes;
// okay, now try to leave it in a messed up state
action = rand() % 8;
robot.dropConnection();
switch (action) {
case 0:
printf("Discon 0 ");
robot.disconnect();
ArUtil::sleep(100);
robot.com(0);
break;
case 1:
printf("Discon 1 ");
robot.disconnect();
ArUtil::sleep(100);
robot.com(0);
ArUtil::sleep(100);
robot.com(1);
break;
case 2:
printf("Discon 2 ");
robot.disconnect();
ArUtil::sleep(100);
robot.com(0);
ArUtil::sleep(100);
robot.com(1);
ArUtil::sleep(100);
robot.com(2);
break;
case 3:
printf("Discon 10 ");
robot.disconnect();
ArUtil::sleep(100);
robot.com(10);
break;
case 4:
printf("Discon ");
robot.disconnect();
break;
default:
printf("Leave ");
break;
}
robot.unlock();
}
else
{
if (exitOnFailure)
{
printf("Failed after %d tries.\n", successes);
exit(0);
}
printf("Failed to start connect ");
++failures;
}
if ((rand() % 2) == 0)
{
printf(" ! RadioDisconnect ! ");
con.write("|||\15", strlen("!!!\15"));
ArUtil::sleep(100);
con.write("WMD\15", strlen("WMD\15"));
ArUtil::sleep(200);
}
if ((rand() % 2) == 0)
{
printf(" ! ClosePort !\n");
con.close();
}
else
printf("\n");
printf("#### %d successes %d failures, %% %.2f success\n", successes, failures,
(float)successes/(float)(successes+failures)*100);
ArUtil::sleep((rand() % 2)* 1000);
}
return 0;
}
void TakeBlockToWall::handler(void)
{
Color tempColor;
switch (myState)
{
case STATE_START:
setState(STATE_ACQUIRE_BLOCK);
myDropWall = COLOR_FIRST_WALL;
myLapWall = COLOR_SECOND_WALL;
printf("!! Started state handling!\n");
//handler();
return;
break;
case STATE_ACQUIRE_BLOCK:
if (myNewState)
{
printf("!! Acquire block\n");
myNewState = false;
myAMPTU->panTilt(0, -40);
myAcquire->activate();
myAcquire->setChannel(COLOR_BLOCK);
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myGripper->getGripState() == 2 &&
myGripper->getBreakBeamState() != 0)
{
printf("###### AcquireBlock: Successful (have cube?)\n");
setState(STATE_PICKUP_BACKUP);
//handler();
return;
}
else if (myGripper->getBreakBeamState() != 0)
{
printf("###### AcquireBlock: Successful (cube in gripper?)\n");
setState(STATE_PICKUP_BLOCK);
//handler();
return;
}
if (myAcquire->getState() == Acquire::STATE_FAILED ||
myStateStartTime.mSecSince() > 35000)
{
printf("###### AcqiureBlock: failed\n");
setState(STATE_BACKUP);
//handler();
return;
}
else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
{
printf("###### AcquireBlock: successful\n");
setState(STATE_PICKUP_BLOCK);
//handler();
return;
}
break;
case STATE_PICKUP_BLOCK:
if (myNewState)
{
printf("!! Pickup block\n");
myNewState = false;
myAMPTU->panTilt(0, -35);
myAcquire->deactivate();
myPickUp->activate();
myPickUp->setChannel(COLOR_BLOCK);
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myPickUp->getState() == PickUp::STATE_FAILED)
{
printf("###### PickUpBlock: failed\n");
setState(STATE_BACKUP);
//handler();
return;
}
else if (myPickUp->getState() == PickUp::STATE_SUCCEEDED)
{
printf("###### PickUpBlock: successful\n");
setState(STATE_PICKUP_BACKUP);
//handler();
return;
}
break;
case STATE_BACKUP:
if (myNewState)
{
myNewState = false;
myRobot->move(BACKUP_DIST * .75);
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myRobot->isLeftMotorStalled() || myRobot->isRightMotorStalled())
{
printf("###### Backup: Failed, going forwards\n");
myRobot->clearDirectMotion();
setState(STATE_FORWARD);
}
if (myStateStartTime.mSecSince() > BACKUP_TIME ||
myStateStartPos.findDistanceTo(myRobot->getPose()) > BACKUP_DIST * .95 * .75)
{
printf("###### Backup: Succeeded\n");
myRobot->clearDirectMotion();
setState(STATE_ACQUIRE_BLOCK2);
//handler();
return;
}
break;
case STATE_FORWARD:
if (myNewState)
{
myNewState = false;
myRobot->move(-BACKUP_DIST * .75);
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myRobot->isLeftMotorStalled() || myRobot->isRightMotorStalled())
{
printf("###### Forward: Failed\n");
myRobot->clearDirectMotion();
setState(STATE_FAILED);
}
if (myStateStartTime.mSecSince() > BACKUP_TIME ||
myStateStartPos.findDistanceTo(myRobot->getPose()) >
ArMath::fabs(BACKUP_DIST * .95 * .75))
{
printf("###### Forward: Succeeded\n");
myRobot->clearDirectMotion();
setState(STATE_ACQUIRE_BLOCK2);
//handler();
return;
}
break;
case STATE_ACQUIRE_BLOCK2:
if (myNewState)
{
printf("!! Acquire block 2\n");
myNewState = false;
myAMPTU->panTilt(0, -40);
myAcquire->activate();
myAcquire->setChannel(COLOR_BLOCK);
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myGripper->getGripState() == 2 &&
myGripper->getBreakBeamState() != 0)
{
printf("###### AcquireBlock2: Successful (have cube?)\n");
setState(STATE_PICKUP_BACKUP);
//handler();
return;
}
else if (myGripper->getBreakBeamState() != 0)
{
printf("###### AcquireBlock2: Successful (cube in gripper?)\n");
setState(STATE_PICKUP_BLOCK2);
//handler();
return;
}
if (myAcquire->getState() == Acquire::STATE_FAILED ||
myStateStartTime.mSecSince() > 35000)
{
printf("###### AcqiureBlock2: failed\n");
setState(STATE_FAILED);
//handler();
return;
}
else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
{
printf("###### AcquireBlock2: successful\n");
setState(STATE_PICKUP_BLOCK2);
//handler();
return;
}
break;
case STATE_PICKUP_BLOCK2:
if (myNewState)
{
printf("!! Pickup block 2\n");
myNewState = false;
myAcquire->deactivate();
myPickUp->activate();
myAMPTU->panTilt(0, -55);
myPickUp->setChannel(COLOR_BLOCK);
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myPickUp->getState() == PickUp::STATE_FAILED)
{
printf("###### PickUpBlock2: failed\n");
setState(STATE_FAILED);
//handler();
return;
}
else if (myPickUp->getState() == PickUp::STATE_SUCCEEDED)
{
printf("###### PickUpBlock2: successful\n");
setState(STATE_PICKUP_BACKUP);
//handler();
return;
}
break;
case STATE_PICKUP_BACKUP:
if (myNewState)
{
myNewState = false;
myRobot->move(BACKUP_DIST);
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myStateStartTime.mSecSince() > BACKUP_TIME ||
myStateStartPos.findDistanceTo(myRobot->getPose()) >
ArMath::fabs(BACKUP_DIST * .95))
{
printf("###### PickUp_BackUp: done\n");
myRobot->clearDirectMotion();
setState(STATE_ACQUIRE_DROP_WALL);
//handler();
return;
}
break;
case STATE_ACQUIRE_DROP_WALL:
if (myNewState)
{
printf("!! Acquire Drop wall, channel %d\n", myDropWall);
myNewState = false;
myAMPTU->panTilt(0, -30);
myAcquire->activate();
myAcquire->setChannel(myDropWall);
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myGripper->getGripState() != 2 ||
myGripper->getBreakBeamState() == 0)
{
printf("###### AcquireDropWall:: failed (lost cube %d %d)\n",
myGripper->getGripState(), myGripper->getBreakBeamState());
setState(STATE_BACKUP);
//handler();
return;
}
if (myAcquire->getState() == Acquire::STATE_FAILED ||
myStateStartTime.mSecSince() > 35000)
{
printf("###### AcquireDropWall:: failed\n");
setState(STATE_FAILED);
//handler();
return;
}
else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
{
printf("###### AcquireDropWall: successful\n");
setState(STATE_DRIVETO_DROP_WALL);
//handler();
return;
}
break;
case STATE_DRIVETO_DROP_WALL:
if (myNewState)
{
printf("!! DropOff Drop wall, channel %d\n", myDropWall);
myNewState = false;
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->activate();
myDropOff->setChannel(myDropWall);
myTableLimiter->deactivate();
}
if (myDropOff->getState() == DropOff::STATE_FAILED)
{
printf("###### DropOffDropWall: failed\n");
setState(STATE_FAILED);
//handler();
return;
}
else if (myDropOff->getState() == DropOff::STATE_SUCCEEDED)
{
printf("###### DropOffDropWall: succesful\n");
setState(STATE_DROP_BACKUP);
//handler();
return;
}
break;
case STATE_DROP_BACKUP:
if (myNewState)
{
myNewState = false;
myRobot->move(BACKUP_DIST);
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myStateStartTime.mSecSince() > BACKUP_TIME ||
myStateStartPos.findDistanceTo(myRobot->getPose()) >
ArMath::fabs(BACKUP_DIST * .95))
{
printf("###### Drop_Backup: done\n");
myRobot->clearDirectMotion();
setState(STATE_ACQUIRE_LAP_WALL);
//handler();
return;
}
break;
case STATE_ACQUIRE_LAP_WALL:
if (myNewState)
{
printf("!! Acquire Lap wall, channel %d\n", myLapWall);
myNewState = false;
myAMPTU->panTilt(0, -30);
myAcquire->activate();
myAcquire->setChannel(myLapWall);
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->activate();
}
if (myAcquire->getState() == Acquire::STATE_FAILED ||
myStateStartTime.mSecSince() > 35000)
{
printf("###### AcquireLapWall:: failed\n");
setState(STATE_SWITCH);
//handler();
return;
}
else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
{
printf("###### AcquireLapWall: successful\n");
setState(STATE_DRIVETO_LAP_WALL);
//handler();
return;
}
break;
case STATE_DRIVETO_LAP_WALL:
if (myNewState)
{
printf("!! Driveto Lap wall, channel %d\n", myLapWall);
myNewState = false;
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->activate();
myDriveTo->setChannel(myLapWall);
myDropOff->deactivate();
myTableLimiter->activate();
}
if (myDriveTo->getState() == DriveTo::STATE_FAILED)
{
printf("###### DriveToLapWall: failed\n");
setState(STATE_BACKUP_LAP_WALL);
//handler();
return;
}
else if (myDriveTo->getState() == DriveTo::STATE_SUCCEEDED)
{
printf("###### DriveToLapWall: succesful\n");
setState(STATE_BACKUP_LAP_WALL);
//handler();
return;
}
break;
case STATE_BACKUP_LAP_WALL:
if (myNewState)
{
myNewState = false;
myRobot->move(BACKUP_DIST * .75);
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myRobot->isLeftMotorStalled() || myRobot->isRightMotorStalled())
{
printf("###### BackupLapWall: Failed, going forwards\n");
myRobot->clearDirectMotion();
setState(STATE_FORWARD_LAP_WALL);
}
if (myStateStartTime.mSecSince() > BACKUP_TIME ||
myStateStartPos.findDistanceTo(myRobot->getPose()) >
ArMath::fabs(BACKUP_DIST * .95 * .75))
{
printf("###### BackupLapWall: Succeeded\n");
myRobot->clearDirectMotion();
setState(STATE_SWITCH);
//handler();
return;
}
break;
case STATE_FORWARD_LAP_WALL:
if (myNewState)
{
myNewState = false;
myRobot->move(-BACKUP_DIST * .75);
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myDropOff->deactivate();
myTableLimiter->deactivate();
}
if (myRobot->isLeftMotorStalled() || myRobot->isRightMotorStalled())
{
printf("###### ForwardLapWall: Failed\n");
myRobot->clearDirectMotion();
setState(STATE_FAILED);
}
if (myStateStartTime.mSecSince() > BACKUP_TIME ||
myStateStartPos.findDistanceTo(myRobot->getPose()) >
ArMath::fabs(BACKUP_DIST * .95 * .75))
{
printf("###### ForwardLapWall: Succeeded\n");
myRobot->clearDirectMotion();
setState(STATE_SWITCH);
//handler();
return;
}
break;
case STATE_SWITCH:
printf("!! Switching walls around.\n");
tempColor = myDropWall;
myDropWall = myLapWall;
myLapWall = tempColor;
setState(STATE_ACQUIRE_BLOCK);
//handler();
return;
case STATE_FAILED:
printf("@@@@@ Failed to complete the task!\n");
myRobot->comInt(ArCommands::SONAR, 0);
ArUtil::sleep(50);
myRobot->comStr(ArCommands::SAY, "\52\77\37\62\42\70");
ArUtil::sleep(500);
Aria::shutdown();
myRobot->disconnect();
myRobot->stopRunning();
return;
}
}
int main(int argc, char **argv)
{
bool done;
double distToTravel = 2300;
// whether to use the sim for the laser or not, if you use the sim
// for hte laser, you have to use the sim for the robot too
bool useSim = false;
// the laser
ArSick sick;
// connection
ArDeviceConnection *con;
// Laser connection
ArSerialConnection laserCon;
// robot
ArRobot robot;
// set a default filename
//std::string filename = "c:\\log\\1scans.2d";
std::string filename = "1scans.2d";
// see if we want to use a different filename
//if (argc > 1)
//Lfilename = argv[1];
printf("Logging to file %s\n", filename.c_str());
// start the logger with good values
sick.configureShort(useSim, ArSick::BAUD38400,
ArSick::DEGREES180, ArSick::INCREMENT_HALF);
ArSickLogger logger(&robot, &sick, 300, 25, filename.c_str());
// mandatory init
Aria::init();
// add it to the robot
robot.addRangeDevice(&sick);
//ArAnalogGyro gyro(&robot);
// if we're not using the sim, make a serial connection and set it up
if (!useSim)
{
ArSerialConnection *serCon;
serCon = new ArSerialConnection;
serCon->setPort();
//serCon->setBaud(38400);
con = serCon;
}
// if we are using the sim, set up a tcp connection
else
{
ArTcpConnection *tcpCon;
tcpCon = new ArTcpConnection;
tcpCon->setPort();
con = tcpCon;
}
// set the connection on the robot
robot.setDeviceConnection(con);
// try to connect, if we fail exit
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
// set up a key handler so escape exits and attach to the robot
ArKeyHandler keyHandler;
robot.attachKeyHandler(&keyHandler);
// run the robot, true here so that the run will exit if connection lost
robot.runAsync(true);
// if we're not using the sim, set up the port for the laser
if (!useSim)
{
laserCon.setPort(ArUtil::COM3);
sick.setDeviceConnection(&laserCon);
}
// now that we're connected to the robot, connect to the laser
sick.runAsync();
if (!sick.blockingConnect())
{
printf("Could not connect to SICK laser... exiting\n");
robot.disconnect();
Aria::shutdown();
return 1;
}
#ifdef WIN32
// wait until someone pushes the motor button to go
while (1)
{
robot.lock();
if (!robot.isRunning())
exit(0);
if (robot.areMotorsEnabled())
{
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(100);
}
#endif
// basically from here on down the robot just cruises around a bit
robot.lock();
// enable the motors, disable amigobot sounds
robot.comInt(ArCommands::ENABLE, 1);
ArTime startTime;
// move a couple meters
robot.move(distToTravel);
robot.unlock();
startTime.setToNow();
do {
ArUtil::sleep(100);
robot.lock();
robot.setHeading(0);
done = robot.isMoveDone(60);
robot.unlock();
} while (!done);
/*
// rotate a few times
robot.lock();
robot.setVel(0);
robot.setRotVel(60);
robot.unlock();
ArUtil::sleep(12000);
*/
robot.lock();
robot.setHeading(180);
robot.unlock();
do {
ArUtil::sleep(100);
robot.lock();
robot.setHeading(180);
done = robot.isHeadingDone();
robot.unlock();
} while (!done);
// move a couple meters
robot.lock();
robot.move(distToTravel);
robot.unlock();
startTime.setToNow();
do {
ArUtil::sleep(100);
robot.lock();
robot.setHeading(180);
done = robot.isMoveDone(60);
robot.unlock();
} while (!done);
robot.lock();
robot.setHeading(0);
robot.setVel(0);
robot.unlock();
startTime.setToNow();
do {
ArUtil::sleep(100);
robot.lock();
robot.setHeading(0);
done = robot.isHeadingDone();
robot.unlock();
} while (!done);
sick.lockDevice();
sick.disconnect();
sick.unlockDevice();
robot.lock();
robot.disconnect();
robot.unlock();
// now exit
Aria::shutdown();
return 0;
}
void TakeBlockToWall::handler(void)
{
Color tempColor;
switch (myState)
{
case STATE_START:
setState(STATE_ACQUIRE_BLOCK);
myDropWall = COLOR_FIRST_WALL;
myLapWall = COLOR_SECOND_WALL;
printf("!! Started state handling!\n");
//handler();
return;
break;
case STATE_ACQUIRE_BLOCK:
if (myNewState)
{
printf("!! Acquire block\n");
myNewState = false;
mySony->panTilt(0, -10);
myAcquire->activate();
myAcquire->setChannel(COLOR_BLOCK);
myPickUp->deactivate();
myDriveTo->deactivate();
myBackup->deactivate();
}
if (myGripper->getGripState() == 2 &&
myGripper->getBreakBeamState() != 0)
{
printf("###### AcquireBlock: Successful (have cube?)\n");
setState(STATE_ACQUIRE_DROP_WALL);
//handler();
return;
}
else if (myGripper->getBreakBeamState() != 0)
{
printf("###### AcquireBlock: Successful (cube in gripper?)\n");
setState(STATE_PICKUP_BLOCK);
//handler();
return;
}
if (myAcquire->getState() == Acquire::STATE_FAILED ||
myStateStart.mSecSince() > 35000)
{
printf("###### AcqiureBlock: failed\n");
setState(STATE_BACKUP);
//handler();
return;
}
else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
{
printf("###### AcquireBlock: successful\n");
setState(STATE_PICKUP_BLOCK);
//handler();
return;
}
break;
case STATE_PICKUP_BLOCK:
if (myNewState)
{
printf("!! Pickup block\n");
myNewState = false;
mySony->panTilt(0, -15);
myAcquire->deactivate();
myPickUp->activate();
myPickUp->setChannel(COLOR_BLOCK);
myDriveTo->deactivate();
myBackup->deactivate();
}
if (myPickUp->getState() == PickUp::STATE_FAILED)
{
printf("###### PickUpBlock: failed\n");
setState(STATE_BACKUP);
//handler();
return;
}
else if (myPickUp->getState() == PickUp::STATE_SUCCEEDED)
{
printf("###### PickUpBlock: successful\n");
setState(STATE_ACQUIRE_DROP_WALL);
//handler();
return;
}
break;
case STATE_BACKUP:
if (myNewState)
{
printf("!! Backup\n");
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myBackup->activate();
myNewState = false;
}
if (myStateStart.mSecSince() > 2000)
{
printf("###### Backup: done\n");
setState(STATE_ACQUIRE_BLOCK2);
//handler();
return;
}
break;
case STATE_ACQUIRE_BLOCK2:
if (myNewState)
{
printf("!! Acquire block 2\n");
myNewState = false;
mySony->panTilt(0, -25);
myAcquire->activate();
myAcquire->setChannel(COLOR_BLOCK);
myPickUp->deactivate();
myDriveTo->deactivate();
myBackup->deactivate();
}
if (myGripper->getGripState() == 2 &&
myGripper->getBreakBeamState() != 0)
{
printf("###### AcquireBlock2: Successful (have cube?)\n");
setState(STATE_ACQUIRE_DROP_WALL);
//handler();
return;
}
else if (myGripper->getBreakBeamState() != 0)
{
printf("###### AcquireBlock2: Successful (cube in gripper?)\n");
setState(STATE_PICKUP_BLOCK2);
//handler();
return;
}
if (myAcquire->getState() == Acquire::STATE_FAILED ||
myStateStart.mSecSince() > 35000)
{
printf("###### AcqiureBlock2: failed\n");
setState(STATE_FAILED);
//handler();
return;
}
else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
{
printf("###### AcquireBlock2: successful\n");
setState(STATE_PICKUP_BLOCK2);
//handler();
return;
}
break;
case STATE_PICKUP_BLOCK2:
if (myNewState)
{
printf("!! Pickup block 2\n");
myNewState = false;
myAcquire->deactivate();
myPickUp->activate();
mySony->panTilt(0, -25);
myPickUp->setChannel(COLOR_BLOCK);
myDriveTo->deactivate();
myBackup->deactivate();
}
if (myPickUp->getState() == PickUp::STATE_FAILED)
{
printf("###### PickUpBlock2: failed\n");
setState(STATE_FAILED);
//handler();
return;
}
else if (myPickUp->getState() == PickUp::STATE_SUCCEEDED)
{
printf("###### PickUpBlock2: successful\n");
setState(STATE_ACQUIRE_DROP_WALL);
//handler();
return;
}
break;
case STATE_ACQUIRE_DROP_WALL:
if (myNewState)
{
printf("!! Acquire Drop wall, channel %d\n", myDropWall);
myNewState = false;
mySony->panTilt(0, -5);
myAcquire->activate();
myAcquire->setChannel(myDropWall);
myPickUp->deactivate();
myDriveTo->deactivate();
myBackup->deactivate();
}
if (myGripper->getGripState() != 2 ||
myGripper->getBreakBeamState() == 0)
{
printf("###### AcquireDropWall:: failed (lost cube)\n");
setState(STATE_BACKUP);
//handler();
return;
}
if (myAcquire->getState() == Acquire::STATE_FAILED ||
myStateStart.mSecSince() > 35000)
{
printf("###### AcquireDropWall:: failed\n");
setState(STATE_FAILED);
//handler();
return;
}
else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
{
printf("###### AcquireDropWall: successful\n");
setState(STATE_DRIVETO_DROP_WALL);
//handler();
return;
}
break;
case STATE_DRIVETO_DROP_WALL:
if (myNewState)
{
printf("!! Driveto Drop wall, channel %d\n", myDropWall);
myNewState = false;
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->activate();
myDriveTo->setChannel(myDropWall);
myBackup->deactivate();
}
if (myGripper->getGripState() != 2 ||
myGripper->getBreakBeamState() == 0)
{
printf("###### DriveToDropWall:: failed (lost cube)\n");
setState(STATE_BACKUP);
//handler();
return;
}
if (myDriveTo->getState() == DriveTo::STATE_FAILED)
{
printf("###### DriveToDropWall: failed\n");
setState(STATE_FAILED);
//handler();
return;
}
else if (myDriveTo->getState() == DriveTo::STATE_SUCCEEDED)
{
printf("###### DriveToDropWall: succesful\n");
setState(STATE_DROP);
//handler();
return;
}
break;
case STATE_DROP:
if (myNewState)
{
printf("!! Drop\n");
printf("@@@@@ Drop lowering lift\n");
myGripper->liftDown();
myNewState = false;
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myBackup->deactivate();
myGripOpenSent = false;
}
myGripper->liftDown();
if ((myStateStart.mSecSince() > 500 &&
myGripper->isLiftMaxed() && myStateStart.mSecSince() < 5000) ||
myStateStart.mSecSince() > 5000)
{
myGripper->gripOpen();
/*if (!myGripOpenSent)
{
ArUtil::sleep(3);
myGripper->gripOpen();
}
myGripOpenSent = true;
*/
}
if (myGripper->getGripState() == 1 || myStateStart.mSecSince() > 6000)
{
printf("###### Drop: success\n");
setState(STATE_DROP_BACKUP);
//handler();
return;
}
break;
case STATE_DROP_BACKUP:
if (myNewState)
{
printf("!! Drop backup\n");
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myBackup->activate();
myNewState = false;
}
if (myStateStart.mSecSince() > 2000)
{
printf("###### Drop_Backup: done\n");
setState(STATE_ACQUIRE_LAP_WALL);
//handler();
return;
}
break;
case STATE_ACQUIRE_LAP_WALL:
if (myNewState)
{
printf("!! Acquire Lap wall, channel %d\n", myLapWall);
myNewState = false;
mySony->panTilt(0, -5);
myAcquire->activate();
myAcquire->setChannel(myLapWall);
myPickUp->deactivate();
myDriveTo->deactivate();
myBackup->deactivate();
}
if (myAcquire->getState() == Acquire::STATE_FAILED ||
myStateStart.mSecSince() > 35000)
{
printf("###### AcquireLapWall:: failed\n");
setState(STATE_SWITCH);
//handler();
return;
}
else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
{
printf("###### AcquireLapWall: successful\n");
setState(STATE_DRIVETO_LAP_WALL);
//handler();
return;
}
break;
case STATE_DRIVETO_LAP_WALL:
if (myNewState)
{
printf("!! Driveto Lap wall, channel %d\n", myLapWall);
myNewState = false;
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->activate();
myDriveTo->setChannel(myLapWall);
myBackup->deactivate();
}
if (myDriveTo->getState() == DriveTo::STATE_FAILED)
{
printf("###### DriveToLapWall: failed\n");
setState(STATE_SWITCH);
//handler();
return;
}
else if (myDriveTo->getState() == DriveTo::STATE_SUCCEEDED)
{
printf("###### DriveToLapWall: succesful\n");
setState(STATE_SWITCH);
//handler();
return;
}
break;
case STATE_SWITCH:
printf("!! Switching walls around.\n");
tempColor = myDropWall;
myDropWall = myLapWall;
myLapWall = tempColor;
setState(STATE_ACQUIRE_BLOCK);
//handler();
return;
case STATE_FAILED:
printf("@@@@@ Failed to complete the task!\n");
myRobot->comInt(ArCommands::SONAR, 0);
ArUtil::sleep(50);
myRobot->comStr(ArCommands::SAY, "\52\77\37\62\42\70");
ArUtil::sleep(500);
Aria::shutdown();
myRobot->disconnect();
myRobot->stopRunning();
return;
}
}
void TakeBlockToWall::handler(void)
{
switch (myState)
{
case STATE_START:
setState(STATE_ACQUIRE_BLOCK);
printf("Started state handling!\n");
handler();
return;
break;
case STATE_ACQUIRE_BLOCK:
if (myNewState)
{
myNewState = false;
mySony->panTilt(0, -10);
myAcquire->activate();
myAcquire->setChannel(COLOR_BLOCK);
myPickUp->deactivate();
myDriveTo->deactivate();
myBackup->deactivate();
}
if (myAcquire->getState() == Acquire::STATE_FAILED)
{
printf("## AcqiureBlock: failed\n");
setState(STATE_FAILED);
handler();
return;
}
else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
{
printf("## AcquireBlock: successful\n");
setState(STATE_PICKUP_BLOCK);
handler();
return;
}
break;
case STATE_PICKUP_BLOCK:
if (myNewState)
{
myNewState = false;
myAcquire->deactivate();
myPickUp->activate();
myPickUp->setChannel(COLOR_BLOCK);
myDriveTo->deactivate();
myBackup->deactivate();
}
if (myPickUp->getState() == PickUp::STATE_FAILED)
{
printf("## PickUpBlock: failed\n");
setState(STATE_BACKUP);
handler();
return;
}
else if (myPickUp->getState() == PickUp::STATE_SUCCEEDED)
{
printf("## PickUpBlock: successful\n");
setState(STATE_ACQUIRE_WALL);
myGripper->liftUp();
handler();
return;
}
break;
case STATE_BACKUP:
if (myNewState)
{
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myBackup->activate();
}
if (myStateStart.mSecSince() > 2000)
{
printf("## Backup: done\n");
setState(STATE_PICKUP_BLOCK2);
handler();
return;
}
break;
case STATE_PICKUP_BLOCK2:
if (myNewState)
{
myNewState = false;
myAcquire->deactivate();
myPickUp->activate();
myPickUp->setChannel(COLOR_BLOCK);
myDriveTo->deactivate();
myBackup->deactivate();
}
if (myPickUp->getState() == PickUp::STATE_FAILED)
{
printf("## PickUpBlock2: failed\n");
setState(STATE_FAILED);
handler();
return;
}
else if (myPickUp->getState() == PickUp::STATE_SUCCEEDED)
{
printf("## PickUpBlock2: successful\n");
setState(STATE_ACQUIRE_WALL);
myGripper->liftUp();
handler();
return;
}
break;
case STATE_ACQUIRE_WALL:
if (myNewState)
{
myNewState = false;
mySony->panTilt(0, -5);
myAcquire->activate();
myAcquire->setChannel(COLOR_FIRST_WALL);
myPickUp->deactivate();
myDriveTo->deactivate();
myBackup->deactivate();
}
if (myAcquire->getState() == Acquire::STATE_FAILED)
{
printf("## AcquireWall:: failed\n");
setState(STATE_FAILED);
handler();
return;
}
else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
{
printf("## AcquireWall: successful\n");
setState(STATE_DRIVETO_WALL);
handler();
return;
}
break;
case STATE_DRIVETO_WALL:
if (myNewState)
{
myNewState = false;
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->activate();
myDriveTo->setChannel(COLOR_FIRST_WALL);
myBackup->deactivate();
}
if (myDriveTo->getState() == DriveTo::STATE_FAILED)
{
printf("## DriveToWall: failed\n");
setState(STATE_FAILED);
handler();
return;
}
else if (myDriveTo->getState() == DriveTo::STATE_SUCCEEDED)
{
printf("## DriveToWall: succesful\n");
setState(STATE_DROP);
handler();
return;
}
break;
case STATE_DROP:
if (myNewState)
{
myGripper->liftDown();
myNewState = false;
}
if (myStateStart.mSecSince() > 3500)
{
myGripper->gripOpen();
}
if (myStateStart.mSecSince() > 5500)
{
printf("## Drop: success\n");
setState(STATE_SUCCEEDED);
handler();
return;
}
break;
case STATE_SUCCEEDED:
printf("Succeeded at the task!\n");
Aria::shutdown();
myRobot->disconnect();
myRobot->stopRunning();
return;
case STATE_FAILED:
printf("Failed to complete the task!\n");
Aria::shutdown();
myRobot->disconnect();
myRobot->stopRunning();
return;
default:
printf("TakeBlockToWall::handler: Unknown state!\n");
}
}
int main(int argc, char **argv)
{
std::string str;
int ret;
ArTime start;
// connection to the robot
ArSerialConnection con;
// the robot
ArRobot robot;
// the connection handler from above
ConnHandler ch(&robot);
// init area with a dedicated signal handling thread
Aria::init(Aria::SIGHANDLE_THREAD);
// open the connection with the defaults, exit if failed
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
// set the robots connection
robot.setDeviceConnection(&con);
// try to connect, if we fail, the connection handler should bail
if (!robot.blockingConnect())
{
// this should have been taken care of by the connection handler
// but just in case
printf(
"asyncConnect failed because robot is not running in its own thread.\n");
Aria::shutdown();
return 1;
}
// run the robot in its own thread, so it gets and processes packets and such
robot.runAsync(false);
// just a big long set of printfs, direct motion commands and sleeps,
// it should be self-explanatory
printf("Telling the robot to go 300 mm for 5 seconds\n");
robot.lock();
robot.setVel(500);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (start.mSecSince() > 5000)
{
robot.unlock();
break;
}
printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel());
robot.unlock();
ArUtil::sleep(100);
}
printf("Telling the robot to turn at 50 deg/sec for 10 seconds\n");
robot.lock();
robot.setVel(0);
robot.setRotVel(50);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (start.mSecSince() > 10000)
{
robot.unlock();
break;
}
printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel());
robot.unlock();
ArUtil::sleep(100);
}
printf("Telling the robot to turn at 100 deg/sec for 10 seconds\n");
robot.lock();
robot.setVel(0);
robot.setRotVel(100);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (start.mSecSince() > 10000)
{
robot.unlock();
break;
}
printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel());
robot.unlock();
ArUtil::sleep(100);
}
printf("Done with tests, exiting\n");
robot.disconnect();
// shutdown and ge tout
Aria::shutdown();
return 0;
}
void Joydrive::drive(void)
{
int trans, rot;
ArPose pose;
ArPose rpose;
ArTransform transform;
ArRangeDevice *dev;
ArSensorReading *son;
if (!myRobot->isConnected())
{
printf("Lost connection to the robot, exiting\n");
exit(0);
}
printf("\rx %6.1f y %6.1f th %6.1f",
myRobot->getX(), myRobot->getY(), myRobot->getTh());
fflush(stdout);
if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(1))
{
if (ArMath::fabs(myRobot->getVel()) < 10.0)
myRobot->comInt(ArCommands::ENABLE, 1);
myJoyHandler.getAdjusted(&rot, &trans);
myRobot->setVel(trans);
myRobot->setRotVel(-rot);
}
else
{
myRobot->setVel(0);
myRobot->setRotVel(0);
}
if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(2) &&
time(NULL) - myLastPress > 1)
{
myLastPress = time(NULL);
printf("\n");
switch (myTest)
{
case 1:
printf("Moving back to the origin.\n");
pose.setPose(0, 0, 0);
myRobot->moveTo(pose);
break;
case 2:
printf("Moving over a meter.\n");
pose.setPose(myRobot->getX() + 1000, myRobot->getY(), 0);
myRobot->moveTo(pose);
break;
case 3:
printf("Doing a transform test....\n");
printf("\nOrigin should be transformed to the robots coords.\n");
transform = myRobot->getToGlobalTransform();
pose.setPose(0, 0, 0);
pose = transform.doTransform(pose);
rpose = myRobot->getPose();
printf("Pos: ");
pose.log();
printf("Robot: ");
rpose.log();
if (pose.findDistanceTo(rpose) < .1)
printf("Success\n");
else
printf("#### FAILURE\n");
printf("\nRobot coords should be transformed to the origin.\n");
transform = myRobot->getToLocalTransform();
pose = myRobot->getPose();
pose = transform.doTransform(pose);
rpose.setPose(0, 0, 0);
printf("Pos: ");
pose.log();
printf("Robot: ");
rpose.log();
if (pose.findDistanceTo(rpose) < .1)
printf("Success\n");
else
printf("#### FAILURE\n");
break;
case 4:
printf("Doing a tranform test...\n");
printf("A point 1 meter to the -x from the robot (in local coords) should be transformed into global coordinates.\n");
transform = myRobot->getToGlobalTransform();
pose.setPose(-1000, 0, 0);
pose = transform.doTransform(pose);
rpose = myRobot->getPose();
printf("Pos: ");
pose.log();
printf("Robot: ");
rpose.log();
if (ArMath::fabs(pose.findDistanceTo(rpose) - 1000.0) < .1)
printf("Probable Success\n");
else
printf("#### FAILURE\n");
break;
case 5:
printf("Doing a transform test on range devices..\n");
printf("Moving the robot +4 meters x and +4 meters y and seeing if the moveTo will move the sonar readings along with it.\n");
dev = myRobot->findRangeDevice("sonar");
if (dev == NULL)
{
printf("No sonar on the robot, can't do the test.\n");
break;
}
printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0));
printf("Sonar 0 reading is at ");
son = myRobot->getSonarReading(0);
if (son != NULL)
{
pose = son->getPose();
pose.log();
}
pose = myRobot->getPose();
pose.setX(pose.getX() + 4000);
pose.setY(pose.getY() + 4000);
myRobot->moveTo(pose);
printf("Moved robot.\n");
printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0));
printf("Sonar 0 reading is at ");
son = myRobot->getSonarReading(0);
if (son != NULL)
{
pose = son->getPose();
pose.log();
}
break;
case 6:
printf("Robot position now is:\n");
pose = myRobot->getPose();
pose.log();
printf("Disconnecting from the robot, then reconnecting.\n");
myRobot->disconnect();
myRobot->blockingConnect();
printf("Robot position now is:\n");
pose = myRobot->getPose();
pose.log();
break;
default:
printf("No test for second button.\n");
break;
}
}
}
