void VectorLocalization2D::predictParticle(Particle2D& p, float dx, float dy, float dtheta, const MotionModelParams &motionParams)
{
static const bool debug = false;
if(debug) printf("predict before: %7.2f,%7.2f %6.2f\u00b0 ",p.loc.x,p.loc.y,DEG(p.angle));
//Motion model
vector2f delta(dx,dy);
float d_trans = delta.length();
//Predict rotation
dtheta += randn(motionParams.Alpha1*dtheta,0.0f)+randn(motionParams.Alpha2*d_trans,0.0f);
p.angle = angle_mod(p.angle+dtheta);
//Predict translation
if(d_trans>FLT_MIN){
delta = delta.rotate(p.angle);
delta = delta.norm(d_trans + randn(motionParams.Alpha3*d_trans,0.0f));
}
if(debug) printf("delta: %f,%f %f\u00b0 ",V2COMP(delta),DEG(dtheta));
p.loc += delta;
if(debug) printf("after: %7.2f,%7.2f %6.2f\u00b0\n",p.loc.x,p.loc.y,DEG(p.angle));
}
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;
}
