예제 #1
0
  /** The method sets all actuators. */
  void setActuators()
  {
    // set all actuator values according to the values in the shared memory block
    try
    {
      dcmTime = proxy->getTime(0);

      data->readingActuators = data->newestActuators;
      if(data->readingActuators == lastReadingActuators)
      {
        if(actuatorDrops == 0)
          fprintf(stderr, "libbhuman: missed actuator request.\n");
        ++actuatorDrops;
      }
      else
        actuatorDrops = 0;
      lastReadingActuators = data->readingActuators;
      float* readingActuators = data->actuators[data->readingActuators];
      float* actuators = handleState(readingActuators);

      if(state != standing)
      {
        if(frameDrops > 0 || state == shuttingDown)
          setEyeLeds(actuators);
        else
          copyNonServos(readingActuators, actuators);
      }
      setBatteryLeds(actuators);

      // set position actuators
      positionRequest[4][0] = dcmTime; // 0 delay!
      for(int i = 0; i < lbhNumOfPositionActuatorIds; ++i)
        positionRequest[5][i][0] = actuators[i];
      proxy->setAlias(positionRequest);

      // set hardness actuators
      bool requestedHardness = false;
      for(int i = headYawHardnessActuator; i < headYawHardnessActuator + lbhNumOfHardnessActuatorIds; ++i)
        if(actuators[i] != requestedActuators[i])
        {
          hardnessRequest[4][0] = dcmTime; // 0 delay!
          for(int j = 0; j < lbhNumOfHardnessActuatorIds; ++j)
            hardnessRequest[5][j][0] = requestedActuators[headYawHardnessActuator + j] = actuators[headYawHardnessActuator + j];
          proxy->setAlias(hardnessRequest);
          requestedHardness = true;
          break;
        }

      // set us actuator
      bool requestedUs = false;
      if(requestedActuators[usActuator] != actuators[usActuator])
      {
        requestedActuators[usActuator] = actuators[usActuator];
        if(actuators[usActuator] >= 0.f)
        {
          resetUsMeasurements();
          usRequest[4][0] = dcmTime;
          usRequest[5][0][0] = actuators[usActuator];
          proxy->setAlias(usRequest);
          requestedUs = true;
        }
      }

      // set led
      if(!requestedHardness && !requestedUs)
        for(int i = 0; i < lbhNumOfLedActuatorIds; ++i)
        {
          int index = faceLedRedLeft0DegActuator + ledIndex;
          if(++ledIndex == lbhNumOfLedActuatorIds)
            ledIndex = 0;
          if(actuators[index] != requestedActuators[index])
          {
            ledRequest[0] = std::string(actuatorNames[index]);
            ledRequest[2][0][0] = requestedActuators[index] = actuators[index];
            ledRequest[2][0][1] = dcmTime;
            proxy->set(ledRequest);
            break;
          }
        }

      // set team info
      // since this should very rarely, we don't use a proxy here
      if(data->bhumanStartTime != lastBHumanStartTime)
      {
        for(int i = 0; i < lbhNumOfTeamInfoIds; ++i)
          memory->insertData(teamInfoNames[i], data->teamInfo[i]);
        lastBHumanStartTime = data->bhumanStartTime;
      }
    }
    catch(AL::ALError& e)
    {
      fprintf(stderr, "libbhuman: %s\n", e.toString().c_str());
    }
  }
예제 #2
0
/*!

   Connect toRomeo robot, and apply some motion.
   By default, this example connect to a robot with ip address: 198.18.0.1.
   If you want to connect on an other robot, run:

   ./motion --ip <robot ip address>

   Example:

   ./motion --ip 169.254.168.230
 */
int main(int argc, const char* argv[])
{
  try
  {
    std::string opt_ip = "198.18.0.1";;

    if (argc == 3) {
      if (std::string(argv[1]) == "--ip")
        opt_ip = argv[2];
    }

    // Create a general proxy to motion to use new functions not implemented in the specialized proxy

    std::string 	myIP = "";		// IP du portable (voir /etc/hosts)
    int 	myPort = 0 ;			// Default broker port

    //    AL::ALProxy *m_proxy;
    boost::shared_ptr<AL::ALBroker> broker = AL::ALBroker::createBroker("Broker", myIP, myPort, opt_ip, 9559);
    //    m_proxy = new AL::ALProxy(broker, "ALVideoDevice");

    //    m_proxy->callVoid("setCameraGroup",1 ,true);

    //AL::ALProxy* dcm = new AL::ALProxy(broker, "DCM_video");



    boost::shared_ptr<AL::ALProxy> proxy = boost::shared_ptr<AL::ALProxy>(new AL::ALProxy(broker, "DCM_video"));
    AL::DCMProxy* dcm = new AL::DCMProxy(proxy);

    //boost::shared_ptr<AL::ALProxy> dcm = boost::shared_ptr<AL::ALProxy>(new AL::ALProxy(broker, "DCM_video"));



    AL::ALValue commands;
    commands.arraySetSize(3);
    commands[0] = std::string("FaceBoard/CameraSwitch/Value");
    commands[1] = std::string("Merge");
    commands[2].arraySetSize(1);
    commands[2][0].arraySetSize(2);
    commands[2][0][0] = 1;
    //commands[2][0][1] = dcm->call<int>("getTime",0);// dcm->getTime(0);
    commands[2][0][1] = dcm->getTime(0);
    //dcm->callVoid("set",commands);
    dcm->set(commands);


    //int time = dcm->call<int>("getTime",0);
    int time = dcm->getTime(10);
    std::cout <<"Time " << time << std::endl;

    //dcm = naoqitools.myGetProxy( "DCM_video" );^M
    //dcm.set( ["FaceBoard/CameraSwitch/Value", "Merge",  [[rGroupNumber, dcm.getTime( 0 ) ]] ] );^M
    //dcm.set(\["ChestBoard/Led/Red/Actuator/Value", "Merge", \[[1.0, dcm.getTime(10000)]] ])




  }
  catch (const vpException &e)
  {
    std::cerr << "Caught exception: " << e.what() << std::endl;
  }
  catch (const AL::ALError &e)
  {
    std::cerr << "Caught exception: " << e.what() << std::endl;
  }



  return 0;
}