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