//用指定的时间和参数复位滤波器
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;
}
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;
}
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 */
//检查球是否被机器人捕捉
//通过半径判断
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 */
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;
}
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;
}
