//用指定的时间和参数复位滤波器
void RobotTracker::reset(double timestamp, float state[7])
{
    Matrix x(7,1,state), P(7);
    //XY值
    //ROBOT_POSITION_VARIANCE = 4.0 # Stddev = 2mm
    P.e(0,0) = DVAR(ROBOT_POSITION_VARIANCE);
    P.e(1,1) = DVAR(ROBOT_POSITION_VARIANCE);
    //ROBOT_THETA_VARIANCE = 0.00006 # Stddev = 0.5 degree
    P.e(2,2) = DVAR(ROBOT_THETA_VARIANCE);

    //速度矢量
    P.e(3,3) = 0.0; // 0m/s
    if (type == ROBOT_TYPE_DIFF)
    {
        P.e(4,4) = 0.0;
    }
    else
    {
        P.e(4,4) = 0.0; // 0m/s
    }
    //角速度
    P.e(5,5) = 0.0;

    //机器人被困住相关的参数
    P.e(6,6) = 0.0;

    initial(timestamp, x, P);

    reset_on_observation = false;
}
static
void print_discrim_bind_tree(FILE *fp, Discrim d, int n, int depth)
{
  int arity, i;

  for (i = 0; i < depth; i++)
    printf(" -");

  if (depth == 0)
    fprintf(fp, "\nroot");
  else if (DVAR(d))
    fprintf(fp, "v%d", d->symbol);
  else
    fprintf(fp, "%s", sn_to_str(d->symbol));

  fprintf(fp, "(%p)", d);
  if (n == 0) {
    Plist p;
    for (i = 0, p = d->u.data; p; i++, p = p->next);
    fprintf(fp, ": leaf has %d objects.\n", i);
  }
  else {
    Discrim d1;
    fprintf(fp, "\n");
    for (d1 = d->u.kids; d1 != NULL; d1 = d1->next) {
      if (DVAR(d1))
	arity = 0;
      else
	arity = sn_to_arity(d1->symbol);
      print_discrim_bind_tree(fp, d1, n+arity-1, depth+1);
    }
  }
}  /* print_discrim_bind_tree */
Matrix& RobotTracker::R(const Matrix &x)
{
    static Matrix R;

    if (!R.nrows())
    {
        R.identity(3);
        R.e(0,0) = (DVAR(ROBOT_POSITION_VARIANCE));
        R.e(1,1) = (DVAR(ROBOT_POSITION_VARIANCE));
        R.e(2,2) = (DVAR(ROBOT_THETA_VARIANCE));
    }

    return R;
}
示例#4
0
LPWSTR GetDeviceName(IMMDeviceCollection *DeviceCollection, UINT DeviceIndex)
{
    IMMDevice *device;
    LPWSTR deviceId;
    HRESULT hr;

    hr = DeviceCollection->Item(DeviceIndex, &device);
    if (FAILED(hr))
    {
        printf("Unable to get device %d: %x\n", DeviceIndex, hr);
        return nullptr;
    }
    hr = device->GetId(&deviceId);
    if (FAILED(hr))
    {
        printf("Unable to get device %d id: %x\n", DeviceIndex, hr);
        return nullptr;
    }

    IPropertyStore *propertyStore;
    hr = device->OpenPropertyStore(STGM_READ, &propertyStore);
    if (FAILED(hr))
    {
        printf("Unable to open device %d property store: %x\n", DeviceIndex, hr);
        return nullptr;
    }

    PROPVARIANT friendlyName;
    PropVariantInit(&friendlyName);
    hr = propertyStore->GetValue(PKEY_Device_FriendlyName, &friendlyName);

    if (FAILED(hr))
    {
        printf("Unable to retrieve friendly name for device %d : %x\n", DeviceIndex, hr);
        return nullptr;
    }

    DVAR(friendlyName.vt);
    if (friendlyName.vt != VT_LPWSTR)
    	DVAR("Unknown");
    else
    	DVAR((const wchar_t *)friendlyName.pwszVal);

    PropVariantClear(&friendlyName);
    CoTaskMemFree(deviceId);

    return nullptr;
}
示例#5
0
int main(int argc, char *argv[])
{
	CoInitializeEx(nullptr, COINIT_MULTITHREADED);

	char *buffer = new char[BUFFER_SIZE];

	if (buffer == nullptr)
		return -11;

	try
	{
		audio.open(XAUDIO2_DEBUG_ENGINE);
		DMSG("open");
		Wav wav("D:\\openAL\\11k16bitpcm.wav");
		uint32_t readSize = wav.read(buffer, BUFFER_SIZE);
		DVAR(readSize);
		wav.close();
		DMSG("play");
		audio.play(wav.getFormat(), buffer, wav.getSize());

		while (audio.isPlaying())
			Thread::sleep(10);
	}
	catch (...)
	{
		std::cout << "error" << std::endl;
	}

	DMSG("close");
	audio.close();

	CoUninitialize();

	return 0;
}
static
Discrim discrim_bind_end(Term t, Discrim d, Plist *path_p)
{
  Discrim d1;
  Plist dp;
  int symbol, sym;

  /* add current node to the front of the path list. */

  dp = get_plist();
  dp->v = d;
  dp->next = *path_p;
  *path_p = dp;

  if (VARIABLE(t)) {
    d1 = d->u.kids;
    symbol = VARNUM(t);
    while (d1 && DVAR(d1) && d1->symbol < symbol) 
      d1 = d1->next;

    if (d1 == NULL || !DVAR(d1) || d1->symbol != symbol)
      return NULL;
    else   /* found node */
      return d1;
  }

  else {  /* constant || complex */
    d1 = d->u.kids;
    sym = SYMNUM(t);  /* arities fixed: handle both NAME and COMPLEX */
    while (d1 && DVAR(d1))  /* skip variables */
      d1 = d1->next;
    while (d1 && d1->symbol < sym)
      d1 = d1->next;

    if (d1 == NULL || d1->symbol != sym)
      return NULL;
    else {
      int i;
      for (i = 0; d1 && i < ARITY(t); i++)
	d1 = discrim_bind_end(ARG(t,i), d1, path_p);
      return d1;
    }
  }
}  /* discrim_bind_end */
/* PUBLIC */
void check_discrim_bind_tree(Discrim d, int n)
{
  if (n > 0) {
    int arity;
    Discrim d1;
    for (d1 = d->u.kids; d1; d1 = d1->next) {
      if (DVAR(d1))
	arity = 0;
      else
	arity = sn_to_arity(d1->symbol);
      check_discrim_bind_tree(d1, n+arity-1);
    }
  }
}  /* check_discrim_bind_tree */
Matrix& RobotTracker::A(const Matrix &x)
{
    static Matrix A(7);

    double theta = x.e(2,0);

    double vpar = x.e(3,0), vperp = x.e(4,0), vtheta = x.e(5,0);
    double stuck = x.e(6,0);
    double cos_theta = cos(theta), sin_theta = sin(theta);

    A.e(0,2) = (1.0 - stuck) * stepsize_ *
            (vpar * -sin_theta + vperp * -cos_theta);

    A.e(0,3) = cos_theta * (1.0 - stuck) * stepsize_;
    A.e(0,4) = -sin_theta * (1.0 - stuck) * stepsize_;
    A.e(0,6) = -stepsize_ * (vpar * cos_theta + vperp * -sin_theta);

    A.e(1,2) = (1.0 - stuck) * stepsize_ *
            (vpar * cos_theta + vperp * -sin_theta);
    A.e(1,3) = sin_theta * (1.0 - stuck) * stepsize_;
    A.e(1,4) = cos_theta * (1.0 - stuck) * stepsize_;
    A.e(1,6) = -stepsize_ * (vpar * sin_theta + vperp * cos_theta);

    A.e(2,5) = (1.0 - stuck) * stepsize_;
    A.e(2,6) = -stepsize_ * vtheta;

    A.e(3,3) = DVAR(ROBOT_VELOCITY_NEXT_STEP_COVARIANCE);

    A.e(4,4) = DVAR(ROBOT_VELOCITY_NEXT_STEP_COVARIANCE);

    A.e(5,5) = DVAR(ROBOT_VELOCITY_NEXT_STEP_COVARIANCE);

    A.e(6,6) = DVAR(ROBOT_STUCK_DECAY);

    return A;
}
//返回预测的速度
//速度以场地坐标系为基准
MyVector2d RobotTracker::velocity(double time)
{
    Matrix x;

    if (IVAR(ROBOT_FAST_PREDICT))
    {
        if (time > latency)
        {
            x = predict(latency);
        }
        else
        {
            x = predict(time);
        }
    }
    else
    {
        x = predict(time);
    }

    double a = x.e(2,0);
    double c = cos(a);
    double s = sin(a);

    double vx = x.e(3,0);
    double vy = x.e(4,0);

    double stuck = x.e(6,0);

    // Threshold after which the robot is considered stuck and zero's velocity.
    //ROBOT_STUCK_THRESHOLD = 0.6 # Set to 1.1 to turn off.
    if (stuck > DVAR(ROBOT_STUCK_THRESHOLD))
    {
        return MyVector2d(0.0, 0.0);
    }
    //fprintf(stderr, "=============>\nx =\n");
    //x.print();
    //fprintf(stderr, "---->\nx =\n");

    MyVector2d v0;
    v0.set(c * vx - s * vy, s * vx + c * vy);
    //旋转角度为负
    return v0;//MyVector2d(c * vx - s * vy, s * vx + c * vy);

}
static
Discrim discrim_bind_insert_rec(Term t, Discrim d)
{
  Discrim d1, d2, prev;
  int symbol, i;

  if (VARIABLE(t)) {
    d1 = d->u.kids;
    prev = NULL;
    symbol = VARNUM(t);
    while (d1 && DVAR(d1) && d1->symbol < symbol) {
      prev = d1;
      d1 = d1->next;
    }
    if (d1 == NULL || !DVAR(d1) || d1->symbol != symbol) {
      d2 = get_discrim();
      d2->type = DVARIABLE;
      d2->symbol = VARNUM(t);
      d2->next = d1;
      if (prev == NULL)
	d->u.kids = d2;
      else
	prev->next = d2;
      return d2;
    }
    else  /* found node */
      return d1;
  }

  else {  /* constant || complex */
    d1 = d->u.kids;
    prev = NULL;
    /* arities fixed: handle both NAME and COMPLEX */
    symbol = SYMNUM(t);
    while (d1 && DVAR(d1)) {  /* skip variables */
      prev = d1;
      d1 = d1->next;
    }
    while (d1 && d1->symbol < symbol) {
      prev = d1;
      d1 = d1->next;
    }
    if (d1 == NULL || d1->symbol != symbol) {
      d2 = get_discrim();
      d2->type = DRIGID;
      d2->symbol = symbol;
      d2->next = d1;
      d1 = d2;
    }
    else
      d2 = NULL;  /* new node not required at this level */

    for (i = 0; i < ARITY(t); i++)
      d1 = discrim_bind_insert_rec(ARG(t,i), d1);

    if (d2 != NULL) {  /* link in new subtree (possibly a leaf) */
      if (prev == NULL)
	d->u.kids = d2;
      else
	prev->next = d2;
    }
	    
    return d1;  /* d1 is leaf corresp. to end of input term */
  }
}  /* discrim_bind_insert_rec */
示例#11
0
//检查球是否被机器人捕捉
//通过半径判断
bool Simulator::RobotBallCollisionRadius(SimRobot &r, SimBall &b, double dt)
{
    static int iDribbleBy=-1;
    //球相对机器人坐标
    MyVector2d ball_rel  = b.pos - r.pos.p;
    //球相对机器人矢量旋转到X轴方向
    MyVector2d robot_ball = ball_rel.rotate(-r.pos.dir);
    //球在机器人内部
    //bool ball_in_front = (robot_ball.x > -20) &&
    //                  (fabs(robot_ball.y) < 60);
    //球在机器人前方
    //bool ball_on_front = ball_in_front && (robot_ball.x < 120);

    double dist = ball_rel.length();
    //计算球相对机器人方向与机器人正方向的角度差
    double da=anglemod(ball_rel.angle()-r.pos.dir);
    if(da>M_PI)
    {
        da-=M_2PI;
    }
    if(da<-M_PI)
    {
        da+=M_2PI;
    }

    //检查球与机器人是否碰撞
    if (dist > robotHalfSize + BALL_RADIUS)
    {
        return (false);
    }
    bool bBounce=false;

    //if(ball_on_front)
    if(fabs(da)<M_PI/4)
    {
        if (r.vcmd.kick_power)
        {
            long kick_power=labs(r.vcmd.kick_power);
            if(kick_power>giShootPowerMax)
            {
                kick_power=giShootPowerMax;
            }
            b.vel.set(DVAR(OMNIBOT_KICKER_SPEED)*kick_power*1.0/giShootPowerMax,0);
            b.vel=b.vel.rotate(r.pos.dir);
            printf("Kicked=%d\r\n",r.vcmd.kick_power);

            b.pos.set(OMNIBOT_RADIUS+BALL_RADIUS+10,0);
            b.pos=b.pos.rotate(r.pos.dir);
            b.pos+= r.pos.p;
            return true;
        }
        else
        {
            //计算球相对机器人速度
            MyVector2d brel = b.vel - r.vel.v;
            //计算相对速度在机器人正方向上投影
            MyVector2d n(1,0);
            n = n.rotate(r.pos.dir);
            MyVector2d p = n.perp();
            MyVector2d bdrib(dot(brel, n), dot(brel, p));

            MyVector2d bpos=b.pos-r.pos.p;
            double bposx=dot(bpos, n);
            double bposy=dot(bpos, p);
            if(bposx<OMNIBOT_RADIUS-5)
            {
                b.vel.set(0,0);
                if(r.vcmd.dribble_power>0)
                {
                    b.pos.set(OMNIBOT_RADIUS-18,0);
                }
                else
                {
                    b.pos.set(OMNIBOT_RADIUS-18,bposy);

                }
                b.pos=b.pos.rotate(r.pos.dir);
                b.pos+= r.pos.p;
                //iDribbleBy=r.id;
            }
            //else if(bposx<OMNIBOT_RADIUS-5)
            //{
            //	b.pos.set(OMNIBOT_RADIUS-18,0);
            //	b.pos=b.pos.rotate(r.pos.dir);
            //	b.pos+= r.pos.p;
            //}
            //if(r.vcmd.dribble_power>0 &&


            //if (bposx<OMNIBOT_RADIUS-5 && (bdrib.x < 0.001 || (r.vcmd.dribble_power>0) || iDribbleBy>=0) )
            //{
            //	//printf("Dribble!!!!... %4.3f,%4.3f\n",bdrib.x,bdrib.y);
            //	double dlimit;
            //	dlimit = DVAR(OMNIBOT_DRIB_CATCH_SPEED)*3;

            //	if (fabs(bdrib.x) < dlimit)
            //	{
            //		b.vel.x = 0;
            //	}
            //	else
            //	{
            //	//	b.vel.x *= DVAR(OMNIBOT_DRIB_BOUNCE_DECAY);
            //	}

            //	//b.vel = r.vel.v.rotate(r.pos.dir);
            //	//b.vel += r.vel.v;
            //	b.vel.set(0,0);
            //	b.pos.set(OMNIBOT_RADIUS-18,0);
            //	b.pos=b.pos.rotate(r.pos.dir);
            //	b.pos+= r.pos.p;
            //	iDribbleBy=r.id;
            //}
            //else
            //{
            //	iDribbleBy=-1;
            //}
            return true;
        }
    }
    else
    {
        iDribbleBy=-1;
        bBounce=true;
    }
    if(!bBounce)
    {
        return true;
    }
    /* last final catch all check to prevent screw ups */
    //如果机器人与球相撞,球被弹开到机器人半径加球半径的位置
    b.pos = r.pos.p + ball_rel.norm() * (robotHalfSize + BALL_RADIUS);
    b.vel=r.vel.v.rotate(r.pos.dir)-b.vel;

    b.pos = b.pos + b.vel*dt;

    /* find the closest point on teh line segment to our robot
   * center and check if it is close enough. If so we hit the ball
   */
    //  MyVector2d closest;
    //	dist = ball_seg.Distance(r.pos, closest);
    //	if (dist > robotHalfSize + BALL_RADIUS)
    //		return (false);

    /* TO DO: need to work this part out still */


    return (true);
}
Matrix& RobotTracker::Q(const Matrix &x)
{
    static Matrix Q(4);

    switch (type) {
    case ROBOT_TYPE_DIFF:
        Q.e(0,0) = DVAR(ROBOT_DIFF_VELOCITY_VARIANCE);
        Q.e(1,1) = DVAR(ROBOT_DIFF_VELOCITY_VARIANCE_PERP);
        Q.e(2,2) = DVAR(ROBOT_DIFF_ANGVEL_VARIANCE);
        Q.e(3,3) = DVAR(ROBOT_STUCK_VARIANCE);
        break;

    case ROBOT_TYPE_OMNI:
        Q.e(0,0) = DVAR(ROBOT_OMNI_VELOCITY_VARIANCE);
        Q.e(1,1) = DVAR(ROBOT_OMNI_VELOCITY_VARIANCE);
        Q.e(2,2) = DVAR(ROBOT_OMNI_ANGVEL_VARIANCE);
        Q.e(3,3) = DVAR(ROBOT_STUCK_VARIANCE);
        break;

    case ROBOT_TYPE_NONE:
        Q.e(0,0) = DVAR(ROBOT_NONE_VELOCITY_VARIANCE);
        Q.e(1,1) = DVAR(ROBOT_NONE_VELOCITY_VARIANCE);
        Q.e(2,2) = DVAR(ROBOT_NONE_ANGVEL_VARIANCE);
        Q.e(3,3) = 0.0;
        break;
    }

    return Q;
}
//Matrix x: [0,0]=x [0,1]=y [0,2]=angle [0,3]=vx [0,4]=vy [0,5]=vangle [0,6]
//计算下一步的x,y,角度
Matrix& RobotTracker::f(const Matrix &x, Matrix &I)
{
    I = Matrix();

    static Matrix f;
    f = x;

    rcommand c = get_command(stepped_time);

    double
            &_x = f.e(0,0),
            &_y = f.e(1,0),
            &_theta = f.e(2,0),
            &_vpar = f.e(3,0),
            &_vperp = f.e(4,0),
            &_vtheta = f.e(5,0),
            &_stuck = f.e(6,0);

    _stuck = bound(_stuck, 0, 1) * DVAR(ROBOT_STUCK_DECAY);

    double avg_vpar = 0, avg_vperp = 0, avg_vtheta = 0, avg_theta = 0;
    double avg_weight = 0.5;

    //# Maybe an improved method for propagating.
    //ROBOT_USE_AVERAGES_IN_PROPAGATION = 0 # false
    if (IVAR(ROBOT_USE_AVERAGES_IN_PROPAGATION)) {
        avg_vpar = avg_weight * _vpar;
        avg_vperp = avg_weight * _vperp;
        avg_vtheta = avg_weight * _vtheta;
    }

    if (type != ROBOT_TYPE_NONE)
    {
        _vpar = c.vs.x;
        _vperp = c.vs.y;
        _vtheta = c.vs.z;
    }

    //# Maybe an improved method for propagating.
    //ROBOT_USE_AVERAGES_IN_PROPAGATION = 0 # false
    if (IVAR(ROBOT_USE_AVERAGES_IN_PROPAGATION))
    {
        avg_vpar += (1.0 - avg_weight) * _vpar;
        avg_vperp += (1.0 - avg_weight) * _vperp;
        avg_vtheta += (1.0 - avg_weight) * _vtheta;

        avg_theta = avg_weight * _theta;

        _theta += (1.0 - _stuck) * stepsize_ * avg_vtheta;
        avg_theta += (1.0 - avg_weight) * _theta;

        _x += (1.0 - _stuck) * stepsize_ *
                (avg_vpar * cos(avg_theta) + avg_vperp * -sin(avg_theta));
        _y += (1.0 - _stuck) * stepsize_ *
                (avg_vpar * sin(avg_theta) + avg_vperp * cos(avg_theta));
    }
    else
    {
        //根据速度更新位置
        _theta += (1.0 - _stuck) * stepsize_ * _vtheta;
        _x += (1.0 - _stuck) * stepsize_ *
                (_vpar * cos(_theta) + _vperp * -sin(_theta));
        _y += (1.0 - _stuck) * stepsize_ *
                (_vpar * sin(_theta) + _vperp * cos(_theta));
    }

    _theta = anglemod(_theta);

    return f;
}
// observe the raw infomation, but just like human ut will use some method to filter the noice
// this is just for one robot
void RobotTracker::observe(VisionRawInfo obs, double timestamp)
{
    //如果有1秒没有来更新信号,自动复位,防止繁殖的程序内存不够
    if(timestamp-time>0.5)
    {
        printf("Robot obvserve reset\r\n");
        reset_on_observation = true;
    }

    // if there is no renew signal
    // if the observation has been reset
    if (reset_on_observation)
    {
        if (obs.conf <= 0.0)
        {
            return;
        }

        static Matrix observe_matrix(7,1), P(7);

        observe_matrix.e(0,0) = obs.pos.x;
        observe_matrix.e(1,0) = obs.pos.y;
        observe_matrix.e(2,0) = obs.angle;
        observe_matrix.e(3,0) = 0.0;
        observe_matrix.e(4,0) = 0.0;
        observe_matrix.e(5,0) = 0.0;
        observe_matrix.e(6,0) = 0.0;

        P.e(0,0) = DVAR(ROBOT_POSITION_VARIANCE);
        P.e(1,1) = DVAR(ROBOT_POSITION_VARIANCE);
        P.e(2,2) = DVAR(ROBOT_THETA_VARIANCE);

        P.e(3,3) = 0.0; // 0m/s
        if (type == ROBOT_TYPE_DIFF)
        {
            P.e(4,4) = 0.0;
        }
        else
        {
            P.e(4,4) = 0.0; // 0m/s
        }
        P.e(5,5) = 0.0;

        P.e(6,6) = 0.0;

        initial(obs.timestamp, observe_matrix, P);

        reset_on_observation = false;

    }
    else
    {
        // If this is a new observation.
        if (timestamp > time)
        {
            // Tick to current time.
            tick(timestamp - time);

            // Make observation
            if (obs.timestamp == timestamp)
            {
                double xtheta = xs[0].e(2,0);

                Matrix o(3,1);
                o.e(0,0) = obs.pos.x;
                o.e(1,0) = obs.pos.y;
                o.e(2,0) = anglemod(obs.angle - xtheta) + xtheta;

                update(o);
            }

            if (error_time_elapsed() > 10.0)
            {
                fprintf(stderr, "Kalman Error (pos, theta, vpos, vtheta): ");
                fprintf(stderr, "%f ",
                        hypot(error_mean().e(0, 0), error_mean().e(1, 0)));
                fprintf(stderr, "%f ", error_mean().e(2, 0));
                fprintf(stderr, "%f ",
                        hypot(error_mean().e(3, 0), error_mean().e(4, 0)));
                fprintf(stderr, "%f\n", error_mean().e(5, 0));
                error_reset();
            }
        }
    }
}
static
Plist discrim_bind_retrieve_leaf(Term t_in, Discrim root,
			    Context subst, Flat2 *ppos)
{
  Flat2 f, f1, f2, f_save;
  Term t = NULL;
  Discrim d = NULL;
  int symbol = 0;
  int match = 0;
  int bound = 0;
  int status = 0;

  f = *ppos;  /* Don't forget to reset before return. */
  t = t_in;
  f_save = NULL;

  if (t != NULL) {  /* if first call */
    d = root->u.kids;
    if (d != NULL) {
      f = get_flat2();
      f->t = t;
      f->last = f;
      f->prev = NULL;
      f->place_holder = (COMPLEX(t));
      status = GO;
    }
    else
      status = FAILURE;
  }
  else
    status = BACKTRACK;

  while (status == GO || status == BACKTRACK) {
    if (status == BACKTRACK) {
      while (f && !f->alternatives) {  /* clean up HERE??? */
	if (f->bound) {
	  subst->terms[f->varnum] = NULL;
	  f->bound = 0;
	}
	f_save = f;
	f = f->prev;
      }
      if (f != NULL) {
	if (f->bound) {
	  subst->terms[f->varnum] = NULL;
	  f->bound = 0;
	}
	d = f->alternatives;
	f->alternatives = NULL;
	status = GO;
      }
      else
	status = FAILURE;
    }

    if (status == GO) {
      match = 0;
      while (!match && d && DVAR(d)) {
	symbol = d->symbol;
	if (subst->terms[symbol]) { /* if already bound */
	  match = term_ident(subst->terms[symbol], f->t);
	  bound = 0;
	}
	else { /* bind variable in discrimb tree */
	  match = 1;
	  subst->terms[symbol] = f->t;
	  bound = 1;
	}
	if (!match)
	  d = d->next;
      }
      if (match) {
	/* push alternatives */
	f->alternatives = d->next;
	f->bound = bound;
	f->varnum = symbol;
	f = f->last;
      }
      else if (VARIABLE(f->t))
	status = BACKTRACK;
      else {
	symbol = SYMNUM(f->t);
	while (d && d->symbol < symbol)
	  d = d->next;
	if (!d || d->symbol != symbol)
	  status = BACKTRACK;
	else if (f->place_holder) {
	  int i;
	  /* insert skeleton in place_holder */
	  f1 = get_flat2();
	  f1->t = f->t;
	  f1->prev = f->prev;
	  f1->last = f;
	  f_save = f1;
	  if (f1->prev)
	    f1->prev->next = f1;
		    
	  t = f->t;
	  for (i = 0; i < ARITY(t); i++) {
	    if (i < ARITY(t)-1)
	      f2 = get_flat2();
	    else
	      f2 = f;
	    f2->place_holder = COMPLEX(ARG(t,i));
	    f2->t = ARG(t,i);
	    f2->last = f2;
	    f2->prev = f1;
	    f1->next = f2;
	    f1 = f2;
	  }
	  f = f_save;
	}  /* if f->place_holder */
      }
      if (status == GO) {
	if (f->next) {
	  f = f->next;
	  d = d->u.kids;
	}
	else
	  status = SUCCESS;
      }
    }
  }  /* while */
  if (status == SUCCESS) {
    *ppos = f;
    return d->u.data;
  }
  else {
    /* Free flat2s. */
#ifdef SPEED
    Flat2count = 0;
#else
    while (f_save) {
      f1 = f_save;
      f_save = f_save->next;
      free_flat2(f1);
    }
#endif
    return NULL;
  }
	   
}  /* discrim_bind_retrieve_leaf */
示例#16
0
void TPositionForKick::command(World &world, int me,
                               Robot::RobotCommand &command,
                               bool debug)
{

    MyVector2d ball_position = world.ball_position();
    MyVector2d robot_position = world.GetRobotPositionByID(me);
    MyVector2d robot_velocity = world.GetRobotVelocityByID(me);

	MyVector2d target;
	double angle_tolerance;
	if (!prev_target_set)
	{
		prev_target = world.their_goal;
		prev_target_set = true;
	}
	//# The amount we'd prefer to shoot at our previous angle.  If an another
	//#  at least this much bigger appears we'll switch to that.
	//SHOOT_AIM_PREF_AMOUNT = 0.01745 # 1 degree
	// (1) Try shooting on goal.
    if (!evaluation.aim(world, world.now, ball_position,
	                    world.their_goal_r,
	                    world.their_goal_l,
	                    OBS_EVERYTHING_BUT_US,
	                    prev_target, DVAR(SHOOT_AIM_PREF_AMOUNT),
	                    target, angle_tolerance))
	{
        //back border of the goal lu_test
		MyVector2d downfield[2];
                downfield[0].set(ball_position.x + 180.0, -FIELD_WIDTH_H);
                downfield[1].set(ball_position.x + 180.0, FIELD_WIDTH_H);
		// (2) Try clearing the ball

        if (!evaluation.aim(world, world.now, ball_position,
		                    downfield[0], downfield[1],
		                    OBS_EVERYTHING_BUT_ME(me),
		                    prev_target, DVAR(SHOOT_AIM_PREF_AMOUNT),
		                    target, angle_tolerance))

		{
			// Guaranteed to return true and fill in the parameters when
			// obs_flags is empty.
			// (3) Just shoot downfield.
			//
            evaluation.aim(world, world.now, ball_position,
			               downfield[0], downfield[1],
			               0, target, angle_tolerance);
            qDebug()<<"shoot downfield";
		}
	}
	if (debug)
	{
                gui_debug_line(me, GDBG_TACTICS, ball_position, target);
                gui_debug_line(me, GDBG_TACTICS, ball_position,
                               (target - ball_position).rotate(angle_tolerance) + ball_position);
                gui_debug_line(me, GDBG_TACTICS, ball_position,
                               (target - ball_position).rotate(-angle_tolerance) + ball_position);
	}
	prev_target = target;
	//
        double ball_distance = (robot_position - ball_position).length();
    //question lu_test
    if (robot_velocity.length() < 20.0)
	{
                ball_distance -= 20.0;
	}
	// put this in config
	double closest = 85.0;
    //question lu_test constant setting
        ball_distance = bound(ball_distance, closest, 150.0);
	//target85150
        MyVector2d targetp = ball_position - (target - ball_position).norm(ball_distance);
        double angle = (ball_position - targetp).angle();
	int obs = OBS_EVERYTHING_BUT_ME(me);
    MyVector2d r2target = (targetp - robot_position);
	double d2target = r2target.sqlength();
	//20150
    //question lu_test ???
	if ((d2target < 150.0 * 150.0) && (d2target > 20.0 * 20.0) &&
	        (fabs(angle_mod(angle - r2target.angle())) < M_PI_4))
	{
		//    obs = OBS_WALLS;
		obs = 0;
		//    printf("turning off obstacle avoidance!!!\n");
		if (debug)
		{
			gui_debug_printf(me, GDBG_TACTICS, "turning off obstacle avoidance!!!\n");
		}
	}
	command.cmd = Robot::CmdPosition;
	command.target = targetp;
	command.velocity = MyVector2d(0, 0);
	command.angle = angle;
    //command.obs = OBS_EVERYTHING_BUT_ME(me);
	command.observation_type = obs;
    command.goto_point_type = Robot::GotoPointMoveForw;
}
示例#17
0
int main(int argc, char *argv[])
{
	CoInitialize(nullptr);

	listDevices();

	IAudioClient *pAudioClient;
	IMMDevice *device;

	getDefaultDevice(&device);

    HRESULT hr = device->Activate(__uuidof(IAudioClient),
        CLSCTX_ALL, nullptr, (void**)&pAudioClient);
    if (FAILED(hr)) {
        printf("IMMDevice::Activate(IAudioClient) failed: hr = 0x%08x", hr);
        return hr;
    }

	REFERENCE_TIME hnsDefaultDevicePeriod;
    hr = pAudioClient->GetDevicePeriod(&hnsDefaultDevicePeriod, nullptr);
    if (FAILED(hr)) {
        printf("IAudioClient::GetDevicePeriod failed: hr = 0x%08x\n", hr);
        pAudioClient->Release();
        return hr;
    }

	// get the default device format
    WAVEFORMATEX *pwfx;
    hr = pAudioClient->GetMixFormat(&pwfx);
    if (FAILED(hr)) {
        printf("IAudioClient::GetMixFormat failed: hr = 0x%08x\n", hr);
        CoTaskMemFree(pwfx);
        pAudioClient->Release();
        return hr;
    }

    DVAR(pwfx->wFormatTag);
    DVAR(pwfx->wBitsPerSample);
    DVAR(pwfx->nBlockAlign);
    DVAR(pwfx->nAvgBytesPerSec);

    switch (pwfx->wFormatTag) {
        case WAVE_FORMAT_IEEE_FLOAT:
            pwfx->wFormatTag = WAVE_FORMAT_PCM;
            pwfx->wBitsPerSample = 16;
            pwfx->nBlockAlign = pwfx->nChannels * pwfx->wBitsPerSample / 8;
            pwfx->nAvgBytesPerSec = pwfx->nBlockAlign * pwfx->nSamplesPerSec;
            break;

        case WAVE_FORMAT_EXTENSIBLE:
            {
                // naked scope for case-local variable
                PWAVEFORMATEXTENSIBLE pEx = reinterpret_cast<PWAVEFORMATEXTENSIBLE>(pwfx);
                if (IsEqualGUID(KSDATAFORMAT_SUBTYPE_IEEE_FLOAT, pEx->SubFormat)) {
                    pEx->SubFormat = KSDATAFORMAT_SUBTYPE_PCM;
                    pEx->Samples.wValidBitsPerSample = 16;
                    pwfx->wBitsPerSample = 16;
                    pwfx->nBlockAlign = pwfx->nChannels * pwfx->wBitsPerSample / 8;
                    pwfx->nAvgBytesPerSec = pwfx->nBlockAlign * pwfx->nSamplesPerSec;
                } else {
                    printf("Don't know how to coerce mix format to int-16\n");
                    CoTaskMemFree(pwfx);
                    pAudioClient->Release();
                    return E_UNEXPECTED;
                }
            }
            break;

        default:
            printf("Don't know how to coerce WAVEFORMATEX with wFormatTag = 0x%08x to int-16\n", pwfx->wFormatTag);
            CoTaskMemFree(pwfx);
            pAudioClient->Release();
            return E_UNEXPECTED;
    }

    DVAR(pwfx->wFormatTag);
    DVAR(pwfx->wBitsPerSample);
    DVAR(pwfx->nBlockAlign);
    DVAR(pwfx->nAvgBytesPerSec);

	hr = pAudioClient->Initialize(AUDCLNT_SHAREMODE_SHARED, AUDCLNT_STREAMFLAGS_LOOPBACK, 0, 0, pwfx, 0 );
    if (FAILED(hr)) {
        printf("IAudioClient::Initialize failed: hr = 0x%08x\n", hr);
        pAudioClient->Release();
        return hr;
    }

	IAudioCaptureClient *pAudioCaptureClient;
    hr = pAudioClient->GetService(__uuidof(IAudioCaptureClient), (void**)&pAudioCaptureClient);
    if (FAILED(hr)) {
        printf("IAudioClient::GetService(IAudioCaptureClient) failed: hr 0x%08x\n", hr);
        pAudioClient->Release();
        return hr;
    }


    hr = pAudioClient->Start();
    if (FAILED(hr)) {
        printf("IAudioClient::Start failed: hr = 0x%08x\n", hr);
        pAudioCaptureClient->Release();
        pAudioClient->Release();
        return hr;
    }


for (int i = 0; i < 10; ++i)
{

	UINT32 nNextPacketSize;
    hr = pAudioCaptureClient->GetNextPacketSize(&nNextPacketSize);
    if (FAILED(hr)) {
        printf("IAudioCaptureClient::GetNextPacketSize failed on pass %u after %u frames: hr = 0x%08x\n", 0, 0, hr);
        pAudioClient->Stop();
        pAudioCaptureClient->Release();
        pAudioClient->Release();
        return hr;
    }


    // get the captured data
    BYTE *pData;
    UINT32 nNumFramesToRead;
    DWORD dwFlags;

    hr = pAudioCaptureClient->GetBuffer(&pData, &nNumFramesToRead, &dwFlags, nullptr,
        nullptr);
    if (FAILED(hr)) {
        printf("IAudioCaptureClient::GetBuffer failed on pass %u after %u frames: hr = 0x%08x\n", 0, 0, hr);
        pAudioClient->Stop();
        pAudioCaptureClient->Release();
        pAudioClient->Release();
        return hr;
    }

    DVAR(nNumFramesToRead);


    // if (bFirstPacket && AUDCLNT_BUFFERFLAGS_DATA_DISCONTINUITY == dwFlags) {
        // printf("Probably spurious glitch reported on first packet\n");
    if (0 != dwFlags && AUDCLNT_BUFFERFLAGS_DATA_DISCONTINUITY != dwFlags) {
        printf("IAudioCaptureClient::GetBuffer set flags to 0x%08x on pass %u after %u frames\n", dwFlags, 0, 0);
        // pAudioClient->Stop();
        // pAudioCaptureClient->Release();
        // pAudioClient->Release();
        // return E_UNEXPECTED;
    }
    else
    	DVAR((int)*pData);

    if (0 == nNumFramesToRead) {
        printf("IAudioCaptureClient::GetBuffer said to read 0 frames on pass %u after %u frames\n", 0, 0);
        pAudioClient->Stop();
        pAudioCaptureClient->Release();
        pAudioClient->Release();
        return E_UNEXPECTED;
    }


    UINT32 nBlockAlign = pwfx->nBlockAlign;
    LONG lBytesToWrite = nNumFramesToRead * nBlockAlign;
    hr = pAudioCaptureClient->ReleaseBuffer(nNumFramesToRead);
}

	pAudioClient->Stop();
    pAudioCaptureClient->Release();
    pAudioClient->Release();


	CoUninitialize();

	return 0;
}