void CorbaMQueueIPCTest::testPortBufferConnection()
{
    // This test assumes that there is a buffer connection mw1 => server => mr1 of size 3
    // Check if connection succeeded both ways:
    BOOST_CHECK( mw1->connected() );
    BOOST_CHECK( mr1->connected() );

    double value = 0;

    // Check if no-data works
    BOOST_CHECK( !mr1->read(value) );

    // Check if writing works
    ASSERT_PORT_SIGNALLING(mw1->write(1.0), mr1);
    ASSERT_PORT_SIGNALLING(mw1->write(2.0), mr1);
    ASSERT_PORT_SIGNALLING(mw1->write(3.0), mr1);
    // it will be emptied too fast by mqueue.
    //ASSERT_PORT_SIGNALLING(mw1->write(4.0), 0);
    BOOST_CHECK( mr1->read(value) );
    BOOST_CHECK_EQUAL( 1.0, value );
    BOOST_CHECK( mr1->read(value) );
    BOOST_CHECK_EQUAL( 2.0, value );
    BOOST_CHECK( mr1->read(value) );
    BOOST_CHECK_EQUAL( 3.0, value );
    BOOST_CHECK_EQUAL( mr1->read(value), OldData );
}
 void updateHook() {
     dummyInPort.read(dataVar);
     dummyOutPort.write(dataVar);
     dataInPort.read(dataVar);
     dataOutPort.write(dataVar);
     //log(Info) << "Worker relaying data!" << endlog();
 }
示例#3
0
void CorbaTest::testPortBufferConnection()
{
    // This test assumes that there is a buffer connection mo => mi of size 3
    // Check if connection succeeded both ways:
    BOOST_CHECK( mo->connected() );
    BOOST_CHECK( mi->connected() );

    double value = 0;

    // Check if no-data works
    BOOST_CHECK_EQUAL( mi->read(value), NoData );

    // Check if writing works
    ASSERT_PORT_SIGNALLING(mo->write(1.0), mi);
    ASSERT_PORT_SIGNALLING(mo->write(2.0), mi);
    ASSERT_PORT_SIGNALLING(mo->write(3.0), mi);
    ASSERT_PORT_SIGNALLING(mo->write(4.0), 0);
    BOOST_CHECK( mi->read(value) );
    BOOST_CHECK_EQUAL( 1.0, value );
    BOOST_CHECK( mi->read(value) );
    BOOST_CHECK_EQUAL( 2.0, value );
    BOOST_CHECK( mi->read(value) );
    BOOST_CHECK_EQUAL( 3.0, value );
    BOOST_CHECK_EQUAL( mi->read(value), OldData );
}
void CorbaMQueueIPCTest::testPortBufferConnection()
{
    // This test assumes that there is a buffer connection mw1 => server => mr1 of size 3
    // Check if connection succeeded both ways:
    BOOST_CHECK( mw1->connected() );
    BOOST_CHECK( mr1->connected() );

    double value = 0;

    // Check if no-data works
    BOOST_CHECK( !mr1->read(value) );

    // Check if writing works
    ASSERT_PORT_SIGNALLING(mw1->write(1.0), mr1);
    ASSERT_PORT_SIGNALLING(mw1->write(2.0), mr1);
    ASSERT_PORT_SIGNALLING(mw1->write(3.0), mr1);
    // There are two connections between mw1 and mr1. We first flush one of the
    // two, and then the second one (normal multi-writer single-reader
    // behaviour)
    BOOST_CHECK( mr1->read(value) );
    BOOST_CHECK_EQUAL( 1.0, value );
    BOOST_CHECK( mr1->read(value) );
    BOOST_CHECK_EQUAL( 2.0, value );
    BOOST_CHECK( mr1->read(value) );
    BOOST_CHECK_EQUAL( 3.0, value );
    BOOST_CHECK( mr1->read(value) );
    BOOST_CHECK_EQUAL( 1.0, value );
    BOOST_CHECK( mr1->read(value) );
    BOOST_CHECK_EQUAL( 2.0, value );
    BOOST_CHECK( mr1->read(value) );
    BOOST_CHECK_EQUAL( 3.0, value );
    BOOST_CHECK_EQUAL( mr1->read(value), OldData );
}
示例#5
0
void updateHook()
{
    if(NewData==posInc_in.read(posIncData) && NewData==deltaInc_in.read(deltaIncData))
    {
        /*for(int i=0; i<number_of_drives; i++)
        {
            std::cout << "POS INC" << posIncData[i] << std::endl;
        }*/
        computedPwm_out.write(computePwmValue(posIncData,deltaIncData));
    }
    /*else
        std::cout << "NO DATA" << std::endl;
    */


}
示例#6
0
文件: Plant.hpp 项目: F34140r/youbot
		void updateHook() {
            double input_sample;
            if ( input.read(input_sample) ) {
                current += this->engine()->getActivity()->getPeriod()*input_sample /inertia.value();
            }
            output.write( current );
		}
示例#7
0
void CorbaTest::testPortDataConnection()
{
    // This test assumes that there is a data connection mo1 => mi2
    // Check if connection succeeded both ways:
    BOOST_CHECK( mo1->connected() );
    BOOST_CHECK( mi2->connected() );

    double value = 0;

    // Check if no-data works
    BOOST_CHECK_EQUAL( mi2->read(value), NoData );

    // Check if writing works (including signalling)
    ASSERT_PORT_SIGNALLING(mo1->write(1.0), mi2)
    BOOST_CHECK( mi2->read(value) );
    BOOST_CHECK_EQUAL( 1.0, value );
    ASSERT_PORT_SIGNALLING(mo1->write(2.0), mi2);
    BOOST_CHECK( mi2->read(value) );
    BOOST_CHECK_EQUAL( 2.0, value );
}
void CorbaMQueueIPCTest::testPortDataConnection()
{
    // This test assumes that there is a data connection mw1 => server => mr1
    // Check if connection succeeded both ways:
    BOOST_CHECK( mw1->connected() );
    BOOST_CHECK( mr1->connected() );

    double value = 0;

    // Check if no-data works
    BOOST_CHECK( !mr1->read(value) );

    // Check if writing works (including signalling)
    ASSERT_PORT_SIGNALLING(mw1->write(1.0), mr1)
    BOOST_CHECK( mr1->read(value) );
    BOOST_CHECK_EQUAL( 1.0, value );
    ASSERT_PORT_SIGNALLING(mw1->write(2.0), mr1);
    BOOST_CHECK( mr1->read(value) );
    BOOST_CHECK_EQUAL( 2.0, value );
}
  void getNextPose(brics_actuator::CartesianPose* curr_next)
  {

    //    cout<<"curr: "<<curr_next->orientation.x<<","<<curr_next->orientation.y<<","<<curr_next->orientation.z<<","<<curr_next->orientation.w<<","<<endl;
    //    relative_position_port->read(relative_position);
    measured_position_port->read(measured_position);

    //    curr_next->position.x += 0.001;
    dummy_x=(measured_position.position.x  - goal[0])*Kp+Ki*int_x;
    dummy_y=(measured_position.position.y  - goal[1])*Kp+Ki*int_y;
    dummy_z=(measured_position.position.z  - goal[2])*Kp+Ki*int_z;
    dummy_ox = (-curr_next->orientation.x + goal[3])*Kp+Ki*int_ox;
    dummy_oy = (-curr_next->orientation.y + goal[4])*Kp+Ki*int_oy;
    dummy_oz = (-curr_next->orientation.z + goal[5])*Kp+Ki*int_oz;
    dummy_ow = (-curr_next->orientation.w + goal[6])*Kp+Ki*int_ow;

    if (abs(dummy_x) > STEP_POS) {
      dummy_x=SIGN(dummy_x)*STEP_POS;
    }
    if (abs(dummy_y )> STEP_POS) {
      dummy_y=SIGN(dummy_y)*STEP_POS;
    }
    if (abs(dummy_z )> STEP_POS) {
      dummy_z=SIGN(dummy_z)*STEP_POS;
    }
    if (abs(dummy_ox )> STEP_OR) {
      dummy_ox=SIGN(dummy_ox)*STEP_OR;
    }
    if (abs(dummy_oy )> STEP_OR) {
      dummy_oy=SIGN(dummy_oy)*STEP_OR;
    }
    if (abs(dummy_oz )> STEP_OR) {
      dummy_oz=SIGN(dummy_oz)*STEP_OR;
    }
    if (abs(dummy_ow )> STEP_OR) {
      dummy_ow=SIGN(dummy_ow)*STEP_OR;
    }

//    log(Error)<<sqrt(  )<<endlog();
//    cout<<sqrt( (measured_position.position.x-goal[0]) *(measured_position.position.x-goal[0])   + (measured_position.position.y-goal[1]) *(measured_position.position.y -goal[1]) + (measured_position.position.z-goal[2]) *(measured_position.position.z-goal[2]) )<<endl;

    if(sqrt((measured_position.position.x-goal[0]) *(measured_position.position.x-goal[0])   + (measured_position.position.y-goal[1]) *(measured_position.position.y -goal[1]) + (measured_position.position.z-goal[2]) *(measured_position.position.z-goal[2])) < 0.3)
    {

//      ((Property<tFriKrlData>)getOwner()->getPeer("KULRobot")->properties()->getProperty("toKRL")).value().intData[1]=3;

//      ((Property<tFriKrlData>)getPeer("KULRobot")->getProperty("toKRL")).value().intData[1]=3;
      if(this->getOwner()->hasPeer("KULRobot")){
            this->getOwner()->getPeer("KULRobot")->properties()->getPropertyType<tFriKrlData>("toKRL")->value().intData[1]=3;
      }
      log(Warning)<<"Grasp position reached."<<endlog();
    }

    if(sqrt(dummy_x*dummy_x + dummy_y*dummy_y + dummy_z*dummy_z) < 0.01)
    {

//      ((Property<tFriKrlData>)getOwner()->getPeer("KULRobot")->properties()->getProperty("toKRL")).value().intData[1]=3;

//      ((Property<tFriKrlData>)getPeer("KULRobot")->getProperty("toKRL")).value().intData[1]=3;
      if(this->getOwner()->hasPeer("KULRobot")){
            this->getOwner()->getPeer("KULRobot")->properties()->getPropertyType<tFriKrlData>("toKRL")->value().intData[1]=3;
      }
      log(Warning)<<"Grasp position reached."<<endlog();
    }




    //        cout<<dummy_x<<","<<dummy_y<<","<<dummy_z<<","<<dummy_ox<<","<<dummy_oy<<","<<dummy_oz<<endl;


    curr_next->position.x +=dummy_x; //+Kd*(measured_position.position.x - 0.02427-prev_error_x)/0.005;

    curr_next->position.y +=dummy_y; //+Kd*(measured_position.position.y - 0.02427-prev_error_y)/0.005;
    curr_next->position.z += dummy_z;//+Kd*(measured_position.position.z - 0.02427-prev_error_z)/0.005;

    curr_next->orientation.x += dummy_ox;
    curr_next->orientation.y += dummy_oy;
    curr_next->orientation.z += dummy_oz;
    curr_next->orientation.w += dummy_ow;





    int_x=int_x+dummy_x*0.005;
    int_y=int_y+dummy_y*0.005;
    int_z=int_z+dummy_z*0.005;

    int_ox=int_ox+dummy_ox*0.005;
    int_oy=int_oy+dummy_oy*0.005;
    int_oz=int_oz+dummy_oz*0.005;
    int_ow=int_ow+dummy_ow*0.005;
    prev_error_x=dummy_x;
    prev_error_y=dummy_y;
    prev_error_z=dummy_z;


    //    (*curr_next) = measured_position;

    //    PID_return=calcDesiredIncremenPID(relative_position.position.x,x,dt);
    //    next->position.x=measured_position.position.x+PID_return.desInc;
    //    x.integral=PID_return.integral;
    //    x.prev_error=relative_position.position.x;
    //
    //    PID_return=calcDesiredIncremenPID(relative_position.position.y,y,dt);
    //    next->position.y=measured_position.position.y+PID_return.desInc;
    //    y.integral=PID_return.integral;
    //    y.prev_error=relative_position.position.y;
    //
    //    PID_return=calcDesiredIncremenPID(relative_position.position.z,z,dt);
    //    next->position.z=measured_position.position.z+PID_return.desInc;
    //    z.integral=PID_return.integral;
    //    z.prev_error=relative_position.position.z;
    //
    //    PID_return=calcDesiredIncremenPID(relative_position.orientation.wx,wx,dt);
    //    next->orientation.wx=measured_position.orientation.wx+PID_return.desInc;
    //    wx.integral=PID_return.integral;
    //    wx.prev_error=relative_position.orientation.wx;
    //
    //    PID_return=calcDesiredIncremenPID(relative_position.orientation.wy,wy,dt);
    //    next->orientation.wy=measured_position.orientation.wy+PID_return.desInc;
    //    wy.integral=PID_return.integral;
    //    wy.prev_error=relative_position.orientation.wy;
    //
    //    PID_return=calcDesiredIncremenPID(relative_position.orientation.wz,wz,dt);
    //    next->orientation.wz=measured_position.orientation.wz+PID_return.desInc;
    //    wz.integral=PID_return.integral;
    //    wz.prev_error=relative_position.orientation.wz;
    //
    //    PID_return=calcDesiredIncremenPID(relative_position.orientation.ww,ww,dt);
    //    next->orientation.ww=measured_position.orientation.ww+PID_return.desInc;
    //    ww.integral=PID_return.integral;
    //    ww.prev_error=relative_position.orientation.ww;

  }
示例#10
0
 bool evaluate() const
 {
     return port->read(mvalue, false) == NewData;
 }