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;
}
Ejemplo n.º 2
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) 
{
  Aria::init();
  
  ArArgumentParser argParser(&argc, argv);

  ArSimpleConnector con(&argParser);

  ArRobot robot;

  // the connection handler from above
  ConnHandler ch(&robot);

  if(!Aria::parseArgs())
  {
    Aria::logOptions();
    Aria::shutdown();
    return 1;
  }

  if(!con.connectRobot(&robot))
  {
    ArLog::log(ArLog::Normal, "directMotionExample: Could not connect to the robot. Exiting.");
    return 1;
  }

  ArLog::log(ArLog::Normal, "directMotionExample: Connected.");

  // Run the robot processing cycle in its own thread. Note that after starting this
  // thread, we must lock and unlock the ArRobot object before and after
  // accessing it.
  robot.runAsync(false);


  // Send the robot a series of motion commands directly, sleeping for a 
  // few seconds afterwards to give the robot time to execute them.
  printf("directMotionExample: Setting rot velocity to 100 deg/sec then sleeping 3 seconds\n");
  robot.lock();
  robot.setRotVel(100);
  robot.unlock();
  ArUtil::sleep(3*1000);
  printf("Stopping\n");
  robot.lock();
  robot.setRotVel(0);
  robot.unlock();
  ArUtil::sleep(200);

  printf("directMotionExample: Telling the robot to go 300 mm on left wheel and 100 mm on right wheel for 5 seconds\n");
  robot.lock();
  robot.setVel2(300, 100);
  robot.unlock();
  ArTime start;
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (start.mSecSince() > 5000)
    {
      robot.unlock();
      break;
    }   
    robot.unlock();
    ArUtil::sleep(50);
  }
  
  printf("directMotionExample: Telling the robot to move forwards one meter, then sleeping 5 seconds\n");
  robot.lock();
  robot.move(1000);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (robot.isMoveDone())
    {
      printf("directMotionExample: Finished distance\n");
      robot.unlock();
      break;
    }
    if (start.mSecSince() > 5000)
    {
      printf("directMotionExample: Distance timed out\n");
      robot.unlock();
      break;
    }   
    robot.unlock();
    ArUtil::sleep(50);
  }
  printf("directMotionExample: Telling the robot to move backwards one meter, then sleeping 5 seconds\n");
  robot.lock();
  robot.move(-1000);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (robot.isMoveDone())
    {
      printf("directMotionExample: Finished distance\n");
      robot.unlock();
      break;
    }
    if (start.mSecSince() > 10000)
    {
      printf("directMotionExample: Distance timed out\n");
      robot.unlock();
      break;
    }
    robot.unlock();
    ArUtil::sleep(50);
  }
  printf("directMotionExample: Telling the robot to turn to 180, then sleeping 4 seconds\n");
  robot.lock();
  robot.setHeading(180);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (robot.isHeadingDone(5))
    {
      printf("directMotionExample: Finished turn\n");
      robot.unlock();
      break;
    }
    if (start.mSecSince() > 5000)
    {
      printf("directMotionExample: Turn timed out\n");
      robot.unlock();
      break;
    }
    robot.unlock();
    ArUtil::sleep(100);
  }
  printf("directMotionExample: Telling the robot to turn to 90, then sleeping 2 seconds\n");
  robot.lock();
  robot.setHeading(90);
  robot.unlock();
  start.setToNow();
  while (1)
  {
    robot.lock();
    if (robot.isHeadingDone(5))
    {
      printf("directMotionExample: Finished turn\n");
      robot.unlock();
      break;
    }
    if (start.mSecSince() > 5000)
    {
      printf("directMotionExample: turn timed out\n");
      robot.unlock();
      break;
    }
    robot.unlock();
    ArUtil::sleep(100);
  }
  printf("directMotionExample: Setting vel2 to 200 mm/sec on both wheels, then sleeping 3 seconds\n");
  robot.lock();
  robot.setVel2(200, 200);
  robot.unlock();
  ArUtil::sleep(3000);
  printf("directMotionExample: Stopping the robot, then sleeping for 2 seconds\n");
  robot.lock();
  robot.stop();
  robot.unlock();
  ArUtil::sleep(2000);
  printf("directMotionExample: Setting velocity to 200 mm/sec then sleeping 3 seconds\n");
  robot.lock();
  robot.setVel(200);
  robot.unlock();
  ArUtil::sleep(3000);
  printf("directMotionExample: Stopping the robot, then sleeping for 2 seconds\n");
  robot.lock();
  robot.stop();
  robot.unlock();
  ArUtil::sleep(2000);
  printf("directMotionExample: Setting vel2 with 0 on left wheel, 200 mm/sec on right, then sleeping 5 seconds\n");
  robot.lock();
  robot.setVel2(0, 200);
  robot.unlock();
  ArUtil::sleep(5000);
  printf("directMotionExample: Telling the robot to rotate at 50 deg/sec then sleeping 5 seconds\n");
  robot.lock();
  robot.setRotVel(50);
  robot.unlock();
  ArUtil::sleep(5000);
  printf("directMotionExample: Telling the robot to rotate at -50 deg/sec then sleeping 5 seconds\n");
  robot.lock();
  robot.setRotVel(-50);
  robot.unlock();
  ArUtil::sleep(5000);
  printf("directMotionExample: Setting vel2 with 0 on both wheels, then sleeping 3 seconds\n");
  robot.lock();
  robot.setVel2(0, 0);
  robot.unlock();
  ArUtil::sleep(3000);
  printf("directMotionExample: Now having the robot change heading by -125 degrees, then sleeping for 6 seconds\n");
  robot.lock();
  robot.setDeltaHeading(-125);
  robot.unlock();
  ArUtil::sleep(6000);
  printf("directMotionExample: Now having the robot change heading by 45 degrees, then sleeping for 6 seconds\n");
  robot.lock();
  robot.setDeltaHeading(45);
  robot.unlock();
  ArUtil::sleep(6000);
  printf("directMotionExample: Setting vel2 with 200 on left wheel, 0 on right wheel, then sleeping 5 seconds\n");
  robot.lock();
  robot.setVel2(200, 0);
  robot.unlock();
  ArUtil::sleep(5000);

  printf("directMotionExample: Done, shutting down Aria and exiting.\n");
  Aria::shutdown();
  return 0;
}