/* LICENSE: ========================================================================= CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7 Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso] School of Computer Science, Carnegie Mellon University All rights reserved. ========================================================================= */ #include "../headers/Geometry.h" #include "../headers/field.h" #include "state_machine.h" #include "BehaviorPacketEncoder.h" #include "BePrimaryAttacker.h" #include "BeLowLevel.h" /* ============================================================================ */ /* =========================================================> BeGotoPointGlobal */ /* ============================================================================ */ char const * const BeGotoPointGlobal::beh_name = "BeGotoPointGlobal"; char const * const BeGotoPointGlobal::state_names[BeGotoPointGlobal::NumStates] = { "CLEAR_PATH"}; BeGotoPointGlobal::BeGotoPointGlobal() { fsm.init(state_names,NumStates,CLEAR_PATH,16,16); resetVars(); } BeGotoPointGlobal::~BeGotoPointGlobal() { } void BeGotoPointGlobal::resetVars() { target_pt.set(0.0,0.0); target_angle = 0.0; heading_ego = 0.0; heading_global = 0.0; spin_dir = 1.0; obey_final_angle = false; obey_heading_ego = false; obey_heading_global = false; walk = MOTION_WALK_TROT; } void BeGotoPointGlobal::reset(ulong timestamp) { fsm.setState(CLEAR_PATH,0,"Reset",timestamp); resetVars(); } void BeGotoPointGlobal::sleep() { fsm.sleep(); } void BeGotoPointGlobal::setWalk(int walk) { this->walk = walk; } void BeGotoPointGlobal::setTarget(double X,double Y) { target_pt.set(X,Y); obey_final_angle = false; } void BeGotoPointGlobal::setTarget(vector2d target) { target_pt = target; obey_final_angle = false; } void BeGotoPointGlobal::setTarget(double X,double Y,double A) { target_pt.set(X,Y); target_angle = A; obey_final_angle = true; } vector2d BeGotoPointGlobal::getTarget(){ return target_pt; } void BeGotoPointGlobal::setHeadingEgo(double A) { heading_ego = A; obey_heading_ego = true; obey_heading_global = false; } void BeGotoPointGlobal::setHeadingGlobal(double A) { heading_global = A; obey_heading_global = true; obey_heading_ego = false; } double BeGotoPointGlobal::operator()(FeatureSet *features, MotionCommand *command) { double my_angle = features->my_angle; vector2d body_pos = features->my_position_body; vector2d delta = target_pt - body_pos; double da = 0; double distance_left = delta.length(); delta.normalize(); double final_angle_da = norm_angle(target_angle - my_angle); double heading_angle_da = 0.0; //calculate the angle while walking if(obey_heading_ego){ heading_angle_da = heading_ego; }else if(obey_heading_global){ heading_angle_da = norm_angle(heading_global - my_angle); }else{ double angle_mix; heading_angle_da = norm_angle(delta.angle() - my_angle); //Allow turning while we are far away. Close up angle_mix is bounded to 0 angle_mix = (distance_left - 150.0)/100.0; angle_mix = bound(angle_mix,0.0,1.0); heading_angle_da *= angle_mix; } //adjust if we need to take the final angle into account if(obey_final_angle){ double heading_mix; heading_mix = bound((distance_left-300.0)/200.0,0.0,1.0); da = (vector2d(cos(heading_angle_da),sin(heading_angle_da))* heading_mix + vector2d(cos(final_angle_da ),sin(final_angle_da ))*(1-heading_mix)).angle(); }else{ da = heading_angle_da; } /* We need to rotate our goal to account for the fact that movement vectors are specified relative to the robot's own coordinate frame rather than the global coordinate frame. */ delta = delta.rotate(-my_angle); da = norm_angle(da); double speed,speed_a; speed = MaxDX*bound(distance_left/300.0,.25,1.0); speed_a = MaxDA*bound(da/MaxDA,-1.0,1.0); //pprintf(TextOutputStream, "da = %f, speed_a = %f\n", da, speed_a); if(sign(speed_a) != spin_dir && M_PI-fabs(da) < RAD(15.0)){ speed_a *= -1.0; //pprintf(TextOutputStream, "flipping\n"); } spin_dir = sign(speed_a); //pprintf(TextOutputStream, "spin_dir = %f\n", spin_dir); bool done=false; fsm.startLoop(features->current_time); while(!done && fsm.error==0){ switch(fsm.getState()){ case CLEAR_PATH: command->motion_cmd = walk; command->vx = delta.x*speed; command->vy = delta.y*speed; command->va = speed_a; done = true; break; } } if(fsm.error!=0){ fsm.handleErr(command); } fsm.endLoop(); return 1.0; } uchar *BeGotoPointGlobal::encodeAllNames(ulong time,uchar *buf,int buf_size) { uchar *bufp; bufp = BehaviorEncodeNames::encodeAllNames(buf,beh_id,beh_name,NumStates,state_names,&fsm); // sub state machines SPOutEncoder::encodeAs(&bufp,0); return bufp; } uchar *BeGotoPointGlobal::encodeTrace(uchar *buf,int buf_size) { uchar *bufp; bufp = BehaviorEncodeTrace::encodeTrace(buf,beh_id,&fsm); // sub state machines SPOutEncoder::encodeAs(&bufp,0); return bufp; } /* ============================================================ */ /* ================================================> BeFindBall */ /* ============================================================ */ char const * const BeFindBall::beh_name = "BeFindBall"; char const * const BeFindBall::state_names[BeFindBall::NumStates] = { "SETTLE", "LOOK_AT_BALL", "SEARCH_WITH_HEAD", "SEARCH_SPIN", "WALK_AROUND"}; const int fb_min_look_x = 40; /* If the ball is this close to use (and in front of us) then we should be able to see it by looking at it. */ const double fb_should_see_thresh = 750.0; const double fb_should_see_angle_thresh = M_PI/2.0; /* These got changed at Robocup because we went into spins at inappropriate times. */ const ulong fb_settle_time = SecToTime(1.0); const ulong fb_fixate_time = SecToTime(1.0); const ulong fb_head_scan_time = SecToTime(2.0); const ulong fb_scan_state_time= SecToTime(5.0); const ulong fb_spin_time = SecToTime(3.2); const ulong fb_face_time = SecToTime(2.0); const ulong fb_walk_time = SecToTime(4.0); /* This is a hack to stabilize the ball distance we're getting from the world model. ball_dist = (1.0 - decay)*new + decay*old */ const double fb_decay_rate = .5; BeFindBall::BeFindBall() { fsm.init(state_names,NumStates,LOOK_AT_BALL,16,16); goto_point = new BeGotoPointGlobal(); track_objects = new BeTrackObjects(); resetVars(); goto_state = MIDDLE; // do this here, NOT in reset. } BeFindBall::~BeFindBall() { if(goto_point!=NULL){ delete goto_point; goto_point = NULL; } if(track_objects!=NULL){ delete track_objects; track_objects = NULL; } } void BeFindBall::resetVars() { spin_direction = MaxDA; ball_distance = 0; timer = 0; } void BeFindBall::reset(ulong timestamp) { fsm.setState(LOOK_AT_BALL,0,"Reset",timestamp); resetVars(); /* if(track_objects!=NULL){ track_objects->reset(timestamp); }*/ } void BeFindBall::sleep() { fsm.sleep(); goto_point->sleep(); track_objects->sleep(); } double BeFindBall::operator()(FeatureSet *features, MotionCommand *command) { static const bool debug_sound = false; vector3d model_ball_loc; vector2d global_ball; vector2d TargetPoint; if(ball_distance==0){ ball_distance = features->ball_distance; }else{ ball_distance = (1.0 - fb_decay_rate)*features->ball_distance + fb_decay_rate*ball_distance; } global_ball = features->world_ball_vector; /* Where do we think the ball is? */ model_ball_loc = features->ball_vector3d; if(model_ball_loc.x < fb_min_look_x){ model_ball_loc.x = fb_min_look_x; } bool done=false; fsm.startLoop(features->current_time); while(!done && fsm.error==0){ switch(fsm.getState()){ case SETTLE: command->head_cmd = HEAD_LOOKAT; command->head_lookat = model_ball_loc; //command->head_cmd = HEAD_SCAN_BALL; command->motion_cmd = MOTION_WALK_TROT; command->vx = 0; command->vy = 0; command->va = 0; if(fsm.timeInState() > fb_settle_time){ TRANS_CONT(fsm,LOOK_AT_BALL,5,"Timeout"); } if(debug_sound){ command->sound_cmd = Motion::SOUND_NOTE; command->sound_frequency = (int)(1000.0 + 4000.0*fsm.timeInState()/fb_settle_time); command->sound_duration = SecToTime(0.100); } done = true; break; case LOOK_AT_BALL: { double ball_dist = features->ball_distance; ulong dist_fixate_time = SecToTime(min(ball_dist,700.0)/MaxDX); if(fsm.timeInState() > max(fb_fixate_time,dist_fixate_time)){ TRANS_CONT(fsm,SEARCH_WITH_HEAD,5,"Timeout"); } /* Set our spin direction here so we can't flip flop between two different directions when we actually use it. */ if(ball_dist > 0){ spin_direction = features->ball_vector.angle(); // Make sure we're in bounds spin_direction = bound(spin_direction,-MaxDA,MaxDA); // Don't set spin speed to anything less than full // if we don't actually see the ball now. if(!features->see_ball_low) spin_direction = sign(spin_direction)*MaxDA; }else{ spin_direction = MaxDA; } /* Look at the ball. */ command->head_cmd = HEAD_SCAN_POINT; // HEAD_LOOKAT; command->head_lookat = model_ball_loc; command->head_tilt_offset = 0; if(ball_dist < 300){ command->motion_cmd = MOTION_STAND_NEUTRAL; if(debug_sound){ command->sound_cmd = Motion::SOUND_NOTE; command->sound_frequency = 1000; command->sound_duration = SecToTime(0.100); } }else{ vector2d near_ball = features->ball_vector; near_ball = near_ball.norm()*(ball_dist - 200); command->motion_cmd = MOTION_WALK_TROT; command->vx = near_ball.x; command->vy = near_ball.y; command->va = near_ball.angle(); if(debug_sound){ command->sound_cmd = Motion::SOUND_NOTE; command->sound_frequency = 5000; command->sound_duration = SecToTime(0.100); } } done = true; } break; case SEARCH_WITH_HEAD: if(fsm.isNewState()){ timer = 0; } /* Do we actually see the ball now? */ if(features->see_ball_med){ TRANS_CONT(fsm,LOOK_AT_BALL,5,"SeeBall"); } if(timer == 0 && features->motion_state == Motion::STATE_WALKING){ timer = fsm.timeInState() + fb_scan_state_time; } if(fsm.timeInState() > timer && timer != 0){ timer = 0; TRANS_CONT(fsm,SEARCH_SPIN,6,"Timeout"); } //(timer-fb_head_scan_time) = the timeInState value when the //kick ended //this says - during the first second of scanning after the kick //is done we want to stand still if( fsm.timeInState() - (timer-fb_scan_state_time) < fb_head_scan_time){ command->head_cmd = HEAD_SCAN_BALL; command->motion_cmd = MOTION_STAND_NEUTRAL; }else{ // Back up and scan for the ball. vector2d our_dir(1,0); our_dir = our_dir.rotate(features->my_angle); vector2d goal_dir = features->world_model->score_goal().norm(); // dot(our_dir,goal_dir) == 1.0 -> 0.0 // dot(our_dir,goal_dir) == -1.0 -> 1.0 double backup = 0.25 + 0.75*0.5*(1.0 - dot(our_dir,goal_dir)); if(features->ball_distance < 400){ command->head_cmd = HEAD_SCAN_BALL; command->motion_cmd = MOTION_WALK_TROT; command->vx = -MaxDX * backup; command->vy = 0.0; command->va = 0.0; }else{ command->head_cmd = HEAD_SCAN_BALL; command->motion_cmd = MOTION_WALK_TROT; command->vx = features->ball_vector.x; command->vy = 0.0; command->va = features->ball_vector.angle();; } } done = true; break; case SEARCH_SPIN: /* Do we actually see the ball now? */ if(features->see_ball_med){ TRANS_CONT(fsm,SETTLE,5,"SeeBall"); } if(fsm.timeInState() > fb_spin_time){ TRANS_CONT(fsm,WALK_AROUND,6,"Timeout"); } command->head_cmd = HEAD_SCAN_BALL_TURN; command->motion_cmd = MOTION_WALK_TROT; command->vx = 0; command->vy = 0; command->va = sign(spin_direction)*MaxDA; done = true; break; case WALK_AROUND: if(fsm.isNewState()){ if(goto_state == MIDDLE){ goto_state = BALL; }else{ goto_state = MIDDLE; } goto_point->reset(features->current_time); } /* Do we actually see the ball now? */ if(features->see_ball_med){ goto_point->sleep(); track_objects->sleep(); TRANS_CONT(fsm,LOOK_AT_BALL,5,"SeeBall"); } if(fsm.timeInState() > fb_walk_time){ TRANS_CONT(fsm,SEARCH_WITH_HEAD,6,"Timeout"); } if(goto_state == MIDDLE){ TargetPoint.set(0, 0); } else { TargetPoint.set(global_ball.x, global_ball.y); } goto_point->setTarget(TargetPoint.x,TargetPoint.y); (*goto_point)(features,command); if(fabs(TargetPoint.x - features->my_position_body.x) < 150 && fabs(TargetPoint.y - features->my_position_body.y) < 150){ resetVars(); goto_point->sleep(); track_objects->sleep(); TRANS_CONT(fsm,SEARCH_SPIN,7,"ReachedTarget"); } // Goto point setup our leg and angle commands. // Set the head to scan for the ball. // Look for the ball and a few markers while going to the point (*track_objects)(features, command); done = true; break; } } if(fsm.error!=0){ fsm.handleErr(command); } fsm.endLoop(); return 1.0; } bool BeFindBall::shouldSeeBall(FeatureSet *features) { /* Do we actually see it? Then the answer is a resounding yes. */ if(features->see_ball_high || features->see_ball_med) return true; /* Is the ball too far away from us? */ if(ball_distance > fb_should_see_thresh) return false; /* Is the ball behind us or too far off to the side? */ if(features->ball_distance > 1){ if(fabs(features->ball_vector.angle()) > fb_should_see_angle_thresh) return false; } /* We should check to see if something could be occluding the ball. */ return true; } uchar *BeFindBall::encodeAllNames(ulong time,uchar *buf,int buf_size) { uchar *bufp; bufp = BehaviorEncodeNames::encodeAllNames(buf,beh_id,beh_name,NumStates,state_names,&fsm); // sub state machines SPOutEncoder::encodeAs(&bufp,2); bufp = goto_point ->encodeAllNames(time,bufp,buf_size - (bufp - buf)); bufp = track_objects->encodeAllNames(time,bufp,buf_size - (bufp - buf)); return bufp; } uchar *BeFindBall::encodeTrace(uchar *buf,int buf_size) { uchar *bufp; bufp = BehaviorEncodeTrace::encodeTrace(buf,beh_id,&fsm); // sub state machines SPOutEncoder::encodeAs(&bufp,2); bufp = goto_point ->encodeTrace(bufp,buf_size - (bufp - buf)); bufp = track_objects->encodeTrace(bufp,buf_size - (bufp - buf)); return bufp; } /* =========================================================== */ /* ================================================> BeGetBall */ /* =========================================================== */ char const * const BeGetBall::beh_name = "BeGetBall"; char const * const BeGetBall::state_names[BeGetBall::NumStates] = { "APPROACH", "LOST_BALL", "LOCALIZE", "LOCALIZE_LOST", "LOOK_BALL"}; /* Take posession of the ball. Preferably with our body behind it so we can push/kick it upfield and score. */ // How long to we fixate on the ball when we're // checking to make sure it's there? const ulong gb_ball_fixate_time = 1000000L; // How long can we go without seeing the ball // before we transition to LOST_BALL? //was 4000000L, but localization took waay too //much time sometimes const ulong gb_lost_ball_timeout = 2000000L; // Don't do active localization if the ball // is too close - otherwise we risk losing // track of it. // Was 350 going into Robocup const double gb_active_loc_cutoff = 650; const double gb_lost_loc_cutoff = 700; //time we allow the robot not the see the ball //but to keep walking to it. Same as fb_fixate time //right now. const ulong gb_ball_seen_timeout = 500000L; // Active localize if the localization says we are lost // amount of deviation in x,y at which we localize const double gb_must_loc_pos_dev = 400.0; // amount of deviation in angle at which we localize const double gb_must_loc_ang_dev = .3; //time between attempting to localize const double gb_lost_loc_interval = 3000000; //time we take to localize/scan when lost const double gb_lost_localize_time = 1500000; // What IS this? It's not actually used anywhere. // Perhaps we should delete it. Blah. // RoboCup==sleepy team. // const double gb_active_loc_force = 250; // This is the slowest speed at which we'll // walk toward the ball. const double gb_min_forward_vel = MaxDX/2.0; /* If the angle from the dog->ball->goal is LESS than this value, we add a bias to our motion vector that's perpendicular to the vector from us to the ball so we end up moving behind it (so we can kick it into the opponent's goal) */ const double gb_perp_angular_cutoff = 5*M_PI/8; /* Linear scaling factor that modifies the contribution of the perpendicular (to the path from us to the ball) vector when we're approaching the ball. The actual calculation is perp.norm()*ball_distance*(this constant)*MaxDX. 0.002 means we add MaxDX to the side at .5 m from the ball. */ const double gb_perp_scale_factor = 0.002; /* Exponential decay of old information in our p2b2g angle tracking. */ const double gb_angle_decay = 0.5; /* Just go straight to the ball instead of doing curved approach that circles behind it. */ const bool gb_straight_to_ball = true; const double gb_final_approach_start_dist = 200.0; const double gb_final_approach_abort_dist = 300.0; const double gb_final_approach_max_da = MaxDA / 8.0; const double gb_lab_off_angle = 0.4; // lab == look at ball // start scaling down translational velocity const double gb_lab_off_dist = 300.0; // Don't translate at all - only spin to face ball const double gb_lab_stop_dist = 150.0; const double gb_final_approach_start_y = 80.0; const double gb_final_approach_abort_y = 130.0; BeGetBall::BeGetBall() { fsm.init(state_names,NumStates,LOST_BALL,16,16); find_ball = new BeFindBall(); track_objects = new BeTrackObjects(); localize = new BeActiveLocalize(); look_at = new BeLookAtPoint(); look_at_object = new BeLookAtObject(); look_at_object->setTargetNoReset(BALL); defensebox_timer = 0L; p2b2g_angle = 0.0; } BeGetBall::~BeGetBall() { if(find_ball!=NULL) { delete find_ball; find_ball = NULL; } if(track_objects!=NULL) { delete track_objects; track_objects = NULL; } if(localize!=NULL) { delete localize; localize = NULL; } if(look_at!=NULL) { delete look_at; look_at = NULL; } if(look_at_object!=NULL) { delete look_at_object; look_at_object = NULL; } } void BeGetBall::reset(ulong timestamp) { fsm.setState(LOST_BALL,0,"Reset",timestamp); p2b2g_angle = 0.0; defensebox_timer = 0L; last_lost_localize = 0; timer = 0L; if(find_ball!=NULL) find_ball->reset(timestamp); if(track_objects!=NULL) track_objects->reset(timestamp); if(localize!=NULL) localize->reset(timestamp); if(look_at!=NULL) look_at->reset(); if(look_at_object!=NULL){ look_at_object->reset(timestamp); look_at_object->setTarget(timestamp,BALL); } } void BeGetBall::sleep() { fsm.sleep(); find_ball->sleep(); track_objects->sleep(); localize->sleep(); look_at_object->sleep(); } double BeGetBall::operator()(FeatureSet *features, BeGetBallInfo *info, MotionCommand *command) { double x_vel = 0, y_vel = 0, a_vel = 0; bool use_verbatim; vector2d move_vel = getMove(features,command,use_verbatim); x_vel = move_vel.x; //we dont want a y velocity to go to the ball. Since this is used //now for just getting close to the ball, the angular velocity will //point us at the ball, we dont need to compensate with y_vel which //slows up down a lot. y_vel = 0.0; if(features->ball_distance>0) a_vel = features->ball_vector.angle(); command->motion_cmd = MOTION_WALK_TROT_FAST; command->vx = x_vel; command->vy = y_vel; command->va = a_vel; if(features->see_ball_med){ prev_ball_loc = features->vision_info->ball.loc; } info->have_ball = false; bool done=false; fsm.startLoop(features->current_time); while(!done && fsm.error==0){ switch(fsm.getState()){ /* We should be able to see the ball. So let's fixate on it and walk toward it. Keep in mind that we must be able to see the ball to enter this state. */ case APPROACH: { if(fsm.isNewState()){ timer = gb_ball_fixate_time; look_at->reset(); } vector2d ball; ball = features->ball_vector; if(features->see_ball_med){ ball.x = features->vision_info->ball.loc.x; ball.y = features->vision_info->ball.loc.y; } if(features->have_ball_center){ info->have_ball = true; } // If we don't see the ball, pick a different // behavior. if(!features->see_ball_low && features->current_time - features->ball_last_seen_med > gb_ball_seen_timeout && fsm.timeInState() > gb_ball_seen_timeout){ defensebox_timer = 0; TRANS_CONT(fsm,LOST_BALL,5,"LostBall"); } const ulong gb_role_switch_loc_delay = SecToTime(4.0); // Keep in mind that we know we see the ball here; // otherwise we'd be in LOST_BALL. Just try to make // sure that doesn't change if(fsm.timeInState() > timer && features->vision_info->ball.loc.length() > gb_active_loc_cutoff && fabs(a_vel) < 0.2 && features->team_msg_mgr->timeSinceGotToken() > gb_role_switch_loc_delay){ TRANS_CONT(fsm,LOCALIZE,6,"LocalizeOk"); } // if( features->vision_info->ball.loc.length() > gb_lost_loc_cutoff && // (features->my_position.position.sMaj > gb_must_loc_pos_dev || // features->my_position.heading.sMaj > gb_must_loc_ang_dev) && // features->current_time - last_lost_localize > gb_lost_loc_interval){ // TRANS_CONT(fsm,LOCALIZE_LOST,7,"MustLocalize"); // } if(features->see_ball_med){ (*look_at)(features, command, features->vision_info->ball.loc); }else{ (*look_at)(features, command, features->ball_vector3d); // prev_ball_loc); } done = true; } break; case LOOK_BALL: if(features->see_ball_med){ TRANS_CONT(fsm,APPROACH,5,"SeeBall"); } if(fsm.timeInState() > gb_ball_fixate_time){ TRANS_CONT(fsm,LOST_BALL,6,"Timeout"); } //(*look_at)(features, command, features->ball_vector3d); (*look_at)(features, command, prev_ball_loc); done = true; break; case LOST_BALL: if(features->see_ball_med){ find_ball->sleep(); TRANS_CONT(fsm,APPROACH,5,"SeeBall"); } if(fsm.isNewState()){ find_ball->reset(features->current_time); } (*find_ball)(features, command); done = true; break; case LOCALIZE: { if(fsm.isNewState()){ timer = gb_lost_ball_timeout; localize->reset(features->current_time); } BeActiveLocalizeInfo info; (*localize)(features, &info, command); if(info.fixated){ localize->sleep(); TRANS_CONT(fsm,LOOK_BALL,6,"UpdateDone"); } if(fsm.timeInState() > timer){ localize->sleep(); TRANS_CONT(fsm,LOOK_BALL,7,"TimeOut"); } done = true; } break; case LOCALIZE_LOST: { last_lost_localize = features->current_time; (*track_objects)(features, command, true, false); if(fsm.timeInState() > gb_lost_localize_time){ TRANS_CONT(fsm,LOOK_BALL,5,"DoneLocalizing"); } done = true; } break; } } if(fsm.error!=0){ fsm.handleErr(command); } fsm.endLoop(); return 1.0; } vector2d BeGetBall::getMove(FeatureSet *features, MotionCommand *command,bool &use_verbatim) { use_verbatim = false; // direction the robot should move [duh] vector2d retval = vector2d(0, 0); // relative vector pointing at ball (not normalized) vector2d ball_rel = features->ball_vector; // position of the ball in global coords vector2d ball_global = features->getGlobalPosn(ball_rel); // (normalized) vector pointing from ball to opponent goal vector2d ball_to_goal = (features->score_goal_const - ball_global).norm(); // normalized vector pointing from ball to the puppy vector2d ball_to_player = (features->my_position.position.mean - ball_global).norm(); // A vector perpendicular to the path from us to // the ball. It's biased so that it points away from // the goal we're trying to score on. // (It's normalized, don't forget) vector2d ball_rel_perp = ball_rel.perp().norm(); if(features->pointsToScoreGoal(ball_rel_perp)) ball_rel_perp = -ball_rel_perp; // The angle formed by the line segments from the // dog to the ball to the goal. Vectors are already // normalized. if(p2b2g_angle==0) p2b2g_angle = acos(ball_to_goal.dot(ball_to_player)); else p2b2g_angle = gb_angle_decay*p2b2g_angle + (1.0 - gb_angle_decay)*acos(ball_to_goal.dot(ball_to_player)); // Are we near a wall. // Are we near the ball. // If so, check if we're on the wrong side and rotate around so // we can move the ball toward their goal rather than our own. if( ((features->my_position.position.mean.y > (halfWidth - 100)) || (features->my_position.position.mean.y < (-halfWidth + 100)) || ((fabs(features->world_model->score_goal().x - features->my_position.position.mean.x) < 100) && (fabs(features->my_position.position.mean.y) > goalHalfWidth + 100))) && (features->ball_distance < 200)) { // Okay, now are we on the correct side of the ball? if(fabs(features->score_goal_const.x - features->my_position.position.mean.x) < fabs(features->score_goal_const.x - features->getGlobalPosn(features->ball_vector).x)) { vector2d global_perp; use_verbatim = true; global_perp = features->getGlobalPosn(ball_rel_perp) - features->my_position.position.mean; if(features->my_position.position.mean.y > 0) { // We're on the +Y side of the field. if(global_perp.y > 0) ball_rel_perp = -ball_rel_perp; } else { // We're on the -Y side of the field if(global_perp.y < 0) ball_rel_perp = -ball_rel_perp; } return ball_rel_perp.norm()*MaxDX; } } // Ininitally assume we're going to run straight // at the ball. retval = ball_rel; if(retval.length() < 200 && retval.y > retval.x/3.0){ command->motion_cmd = MOTION_WALK_TROT_FAST; command->vx = 0.0; command->vy = 0.0; command->va = retval.angle(); } // If we're close to the ball, retval.length() can // approach zero. Which means that the robot just // marches in place and looks silly. if(retval.length() < gb_min_forward_vel) retval = retval.norm()*gb_min_forward_vel; if(ball_rel.length() > 200.0){ double xdistToGoal = features->defend_goal_const.x - features->my_position.position.mean.x; if(fabs(xdistToGoal) < (halfLength - penaltyRegionLengthOffset) && fabs(features->my_position.position.mean.y) < penaltyRegionHalfWidth){ vector2d repel_vec = features->my_position.position.mean - vector2d(sign(features->defend_goal_const.x) * (halfLength+1500), features->my_position.position.mean.y); repel_vec = repel_vec.norm(); vector2d walk_dir = features->getGlobalPosn(retval); walk_dir += repel_vec * (600.0 - xdistToGoal) * 100.0; retval = features->getRelativePosn(walk_dir); } } if(gb_straight_to_ball) return retval; // If there is an opponent in front of us, don't // mess around - just go straight for the ball. if(features->opponent_in_front) return retval; // Are we next to a wall? Rubbing our legs against // the wall really slows us down, so don't add // in the sideways bias if that's the case. if(features->my_position.position.mean.y < -(halfWidth - 100) || features->my_position.position.mean.y > (halfWidth - 100)) { return retval; } // We should swing around to get around the ball // if we're not behind it if(p2b2g_angle < gb_perp_angular_cutoff) { // Add in a component perpendicular // to the ball so we circle behind it. /* BTW: The max(0, blah) bit below is because the ball distance approaches about 90 mm instead of 0. So we want to cut out perpendicular motion before ball_distance hits 0. */ ball_rel_perp = ball_rel_perp.norm() * max(0.0, features->ball_distance - 90) * gb_perp_scale_factor * MaxDX; retval = retval + ball_rel_perp; } return retval; } uchar *BeGetBall::encodeAllNames(ulong time,uchar *buf,int buf_size) { uchar *bufp; bufp = BehaviorEncodeNames::encodeAllNames(buf,beh_id,beh_name,NumStates,state_names,&fsm); // sub state machines SPOutEncoder::encodeAs(&bufp,2); bufp = find_ball->encodeAllNames(time,bufp,buf_size - (bufp - buf)); bufp = localize ->encodeAllNames(time,bufp,buf_size - (bufp - buf)); return bufp; } uchar *BeGetBall::encodeTrace(uchar *buf,int buf_size) { uchar *bufp; bufp = BehaviorEncodeTrace::encodeTrace(buf,beh_id,&fsm); // sub state machines SPOutEncoder::encodeAs(&bufp,2); bufp = find_ball->encodeTrace(bufp,buf_size - (bufp - buf)); bufp = localize ->encodeTrace(bufp,buf_size - (bufp - buf)); return bufp; } /* ============================================================== */ /* ======================================================> BeTurn */ /* ============================================================== */ char const * const BeTurn::state_names[BeTurn::NumStates] = { "START", "MAIN_TURN"}; //timeout so we dont get called for holding const ulong turn_timeout = SecToTime(2.5); //if we dont get an IR reading or chin button press in this time //then we assume we dont have the ball const ulong turn_sensor_reading_time = SecToTime(0.5); BeTurn::BeTurn() { //set state to overtime, otherwise the InState() timer //is not correct the first time fsm.init(state_names,NumStates,START,16,16); timer = 0; } BeTurn::~BeTurn() { } void BeTurn::reset(ulong timestamp) { timer = 0; fsm.setState(START,0,"Reset",timestamp); last_ball_on_chest_reading = 0UL; } double BeTurn::getTurnTime(double da, // double vel0,double vel1, // double max_vel,double max_accel, double &t_accel, double &t_cruise, double &t_decel) // Borrowed from the small size code. Thanks guys! // We do a LOT of simplifying because vel0 and vel1 = 0. // Calculates the timings for a trapezoidal velocity profile with // constant acceleration. 'dx' is the distance to be traveled, and // 'vel0' and 'vel1' are the desired initial and final velocities. // 'max_vel' == maximum veloicty, 'max_accel' == maximum acceleration // the individual timings are returned in the last three parameters: // 't_accel' is the time spent in the acceleration phase, 't_cruise' // is the time spent at maximum velocity, and 't_decel' is the time // spent in the deceleration phase. Any of these can be 0. The total // of the three times is returned by the function. { double tmp,vmax; //double vel0 = 0, vel1 = 0, double max_vel = MaxDA, max_accel = MaxDA/1.5; if(da < 0){ da = -da; // vel0 = -vel0; // We KNOW these are 0 // vel1 = -vel1; // just don't forget if we ever do other cases. } // calculate time spent at max velocity tmp = 2*max_accel*da; // + sq(vel0) + sq(vel1); t_cruise = (tmp - 2*sq(max_vel)) / (2*max_vel*max_accel); if(t_cruise > 0){ vmax = max_vel; }else{ vmax = sqrt((max_accel*da /* + sq(vel0) + sq(vel1) */)/2); t_cruise = 0; } t_accel = max(vmax /*- vel0*/,(double)0.0) / max_accel; t_decel = fabs(vmax/* - vel1*/) / max_accel; return(t_accel + t_cruise + t_decel); } double BeTurn::turnToAngle(FeatureSet *features, MotionCommand *command, double targetAngle, double thresh) { targetAngle = norm_angle(targetAngle); command->motion_cmd = MOTION_WALK_TROT; command->vx = 0.0; command->vy = 0.0; command->va = bound(norm_angle(targetAngle - features->my_angle),-MaxDA,MaxDA); if(fabs(targetAngle - features->my_angle) < thresh){ //done return 0.0; } return 1.0; } //returns 1.0 if keep turning, 0.0 if stop turning. //0.0 might mean that we finished turning or that //we are out of time double BeTurn::turnBallToAngle(FeatureSet *features, MotionCommand *command, double targetAngle, double thresh) { double t_accel, t_cruise, t_decel; double turn_angle; double a_vel; double retval = 1.0; turn_angle = targetAngle - features->my_angle; turn_angle = norm_angle(turn_angle); a_vel = fabs(turn_angle)*2.0; a_vel = bound(a_vel, 0.8, MaxDA); a_vel = a_vel*sign(turn_angle); if(strcmp(model,"ERS210")==0) { command->motion_cmd = MOTION_WALK_TROT; command->vx = MaxDX/3; command->vy = 0; command->va = a_vel; command->head_lookat.set(100,sign(a_vel)*35,0); }else{ command->motion_cmd = MOTION_WALK_DRIBBLE; command->vx = 0; command->vy = 0; command->va = a_vel; command->head_cmd = HEAD_ANGLES; command->head_tilt = -1.15; command->head_pan = 0.0; command->head_tilt2 = 0.5; } // We can't hold the ball for more than 3 seconds. // Lift the head for the rest of the turn // Would scanning be better? if(fsm.timeInState() > turn_timeout && fsm.getState() != START){ command->head_cmd = HEAD_LOOKAT; command->head_lookat.set(1500, 0, 0); command->vx = MaxDX; } if(features->ball_on_chest || features->sensors->button & HeadBackButton) last_ball_on_chest_reading = features->current_time; bool done=false; fsm.startLoop(features->current_time); while(!done && fsm.error==0){ switch(fsm.getState()){ case START: TRANS_CONT(fsm,MAIN_TURN,5,"BeginTurn"); done = true; break; case MAIN_TURN: if(fsm.isNewState()){ double turn_time_sec; turn_time_sec = getTurnTime(fabs(turn_angle), t_accel, t_cruise, t_decel); timer = (ulong)(1.0E6*turn_time_sec/* + 200000*/); } //are we done turning? if(fabs(turn_angle) < thresh){ //done retval = 0.0; } //approach failed if no readings if((fsm.timeInState() > turn_sensor_reading_time && last_ball_on_chest_reading == 0UL)){ //features->current_time - last_ball_on_chest_reading > turn_sensor_reading_time)){ command->setFaceLEDs(LED_RED_EYES_LOW, LED_BRIGHT, MODE_A); retval = -1; } done = true; break; } } if(fsm.error!=0){ fsm.handleErr(command); } fsm.endLoop(); return retval; } double BeTurn::circleBallToAngle(FeatureSet *features, MotionCommand *command, double targetAngle, double thresh) { double turn_angle; double a_vel; turn_angle = targetAngle - features->my_angle; turn_angle = norm_angle(turn_angle); a_vel = fabs(turn_angle); a_vel = bound(a_vel, MaxDA/4, .9); //command->head_cmd = HEAD_SCAN_MARKERS; command->head_cmd = HEAD_LOOKAT; command->head_lookat.set(115.0,0.0,0.0); command->motion_cmd = MOTION_WALK_TROT; command->vx = 0; command->vy = -MaxDY * sign(turn_angle); command->va = a_vel * sign(turn_angle); if(fabs(turn_angle) < thresh){ return 0.0; }else{ return 1.0; } } /* ============================================================ */ /* ================================================> FindBallNear */ /* ============================================================ */ char const * const FindBallNear::beh_name = "FindBallNear"; char const * const FindBallNear::state_names[FindBallNear::NumStates] = { "LOOK", "SCAN","BACKUP" }; const ulong fbn_scan_time = SecToTime(2); const ulong fbn_look_time = SecToTime(.5); FindBallNear::FindBallNear() { fsm.init(state_names,NumStates,LOOK,16,16); } FindBallNear::~FindBallNear() { } void FindBallNear::reset(ulong timestamp) { fsm.setState(LOOK,0,"Reset",timestamp); } void FindBallNear::sleep() { fsm.sleep(); } double FindBallNear::operator()(FeatureSet *features,MotionCommand *command) { bool done=false; fsm.startLoop(features->current_time); while(!done && fsm.error==0){ switch(fsm.getState()){ case LOOK: command->motion_cmd = MOTION_STAND_NEUTRAL; command->head_cmd = HEAD_LOOKAT; command->head_lookat.set(120,0,0); if(fsm.timeInState() > fbn_look_time){ TRANS_CONT(fsm,BACKUP,5,"Timeout"); } done = true; break; case SCAN: command->motion_cmd = MOTION_STAND_NEUTRAL; command->head_cmd = HEAD_SCAN_BALL_NEAR; if(fsm.timeInState() > fbn_scan_time){ TRANS_CONT(fsm,BACKUP,5,"Timeout"); } done = true; break; case BACKUP: command->motion_cmd = MOTION_WALK_TROT; command->vx = -MaxDX/2.0; command->vy = 0; command->va = 0; command->head_cmd = HEAD_SCAN_BALL_NEAR; done = true; break; }; } if(fsm.error!=0){ fsm.handleErr(command); } fsm.endLoop(); return 0.0; } /* ======================================================================= */ /* ================================================> BeApproachBall */ /* ======================================================================= */ char const * const BeApproachBall::beh_name = "BeApproachBall"; char const * const BeApproachBall::state_names[BeApproachBall::NumStates] = { "APPROACH_CENTER","APPROACH_SIDE","APPROACH_NEAR","GRAB_SLOW","GRAB_FAST","GOALIE"}; //if we dont get an IR sensor reading or chin button press in this time //we assume the grab has failed const ulong ab_sensor_reading_time = SecToTime(0.7); BeApproachBall::BeApproachBall() { fsm.init(state_names,NumStates,APPROACH_CENTER,16,16); track_objects = new BeTrackObjects(); gpoint.set(0.0,0.0); } BeApproachBall::~BeApproachBall() { if(track_objects!=NULL) { delete track_objects; track_objects = NULL; } } void BeApproachBall::reset(ulong timestamp) { fsm.setState(APPROACH_CENTER,0,"Reset",timestamp); track_objects->reset(timestamp); last_ball_on_chest_reading = 0UL; } void BeApproachBall::setApproachType(State s, int side, ulong timestamp){ if(side == RIGHT){ gpoint.set(185.0,-75.0); }else if(side == LEFT){ gpoint.set(185.0,75.0); }else{ gpoint.set(0,0); } fsm.setState(s,0,"SetApproachType",timestamp); } void BeApproachBall::sleep() { fsm.sleep(); } double BeApproachBall::operator()(FeatureSet *features, MotionCommand *command) { double retval = 0; double vel_thresh; ball_v = features->ball_vector; //if the world model doesnt have a ball hypothesis //we can't approach to the ball if(!features->valid_ball_hyp){ return -1.0; } (*track_objects)(features, command, false, true); //velocity prediction // vector2d look_target; // if(features->ball_trj.velocity.length() > 30 /* && features->ball_trj.chi2 < 1000*/){ // look_target = features->world_model->estimateBallPosn(0.15,5); // command->head_cmd = HEAD_LOOKAT; // command->head_lookat.set(look_target.x,look_target.y,0.0); // } bool done=false; fsm.startLoop(features->current_time); while(!done && fsm.error==0){ switch(fsm.getState()){ case APPROACH_CENTER: vel_thresh = 150.0; //the ball is close, grab it with the head if(features->ball_vector.x < 150 && fabs(features->ball_vector.y) < 20.0 && features->time_since_last_ball_obs < SecToTime(0.5) /*&& features->velocity.x > vel_thresh*/){ TRANS_CONT(fsm,GRAB_FAST,5,"ball close"); } // //the ball is close, grab it with the head // if(features->ball_vector.x < 140 && // fabs(features->ball_vector.y) < 20.0 && // features->time_since_last_ball_obs < SecToTime(0.5) && // features->velocity.x < vel_thresh){ // TRANS_CONT(fsm,GRAB_SLOW,5,"ball close"); // } if(features->current_time - features->ball_last_seen_low > 700000){ retval = -1.0; } walk_rel = features->ball_vector; walk_rel.x += 67.5; //body to neck command->motion_cmd = MOTION_WALK_TROT_FAST; command->vx = min(MAX_DX, walk_rel.x/1.5); command->vy = 0; command->va = walk_rel.angle()*2.0; if(features->ball_vector.x < 200 && fabs(features->ball_vector.y) < 60.0){ command->head_cmd = HEAD_LOOKAT; command->head_lookat.set(210, features->ball_vector.y, BallRadius); } done = true; break; case GOALIE: //the ball is close, grab it with the head if(features->ball_vector.x < 150 && fabs(features->ball_vector.y) < 20.0 && features->time_since_last_ball_obs < SecToTime(0.5)){ TRANS_CONT(fsm,GRAB_FAST,5,"ball close"); } if(features->current_time - features->ball_last_seen_low > 700000){ retval = -1.0; } walk_rel = features->ball_vector; walk_rel.x += 67.5; //body to neck walk_rel = walk_rel.norm() * min(MAX_DX,features->ball_distance*MAX_DX/400.0); command->motion_cmd = MOTION_WALK_TROT_FAST; command->vx = walk_rel.x; command->vy = walk_rel.y * 1.5; command->va = walk_rel.angle(); if(features->ball_vector.x < 200 && fabs(features->ball_vector.y) < 60.0){ command->head_cmd = HEAD_LOOKAT; command->head_lookat.set(210, features->ball_vector.y, BallRadius); } done = true; break; case APPROACH_NEAR: if(fabs(ball_v.angle()) > 0.2){ command->motion_cmd = MOTION_WALK_TROT_FAST; command->vx = -MaxDX/4.0; command->vy = 0; command->va = bound(ball_v.angle()*MaxDA, -.9, 0.9); } else{ walk_rel = ball_v.norm() * MAX_DX; command->motion_cmd = MOTION_WALK_TROT_FAST; command->vx = walk_rel.x * 0.7; command->vy = 0.0; command->va = walk_rel.angle(); if(ball_v.x < 190 && fabs(ball_v.y) < 30){ command->head_cmd = HEAD_ANGLES; command->head_tilt = -1.05; command->head_pan = 0.0; command->head_tilt2 = 0.78; } } done = true; break; case APPROACH_SIDE: command->motion_cmd = MOTION_WALK_TROT_FAST; walk_rel.set(features->ball_vector.x-gpoint.x,features->ball_vector.y-gpoint.y); if(walk_rel.length() < 280){ walk_rel = walk_rel.norm() * MAX_DX/2.0; } else{ walk_rel = walk_rel.norm() * MAX_DX; } //walk_rel.x = sign(walk_rel.x)*bound(walk_rel.x,MaxDX/4.0, MaxDX); command->vx = walk_rel.x; command->vy = 0; command->va = walk_rel.angle(); //if(fabs(command->va) > 0.3 && ball_v.length() < 350){ // command->vx = 0; // command->va = bound(command->va,-0.25,0.25); //} done = true; break; case GRAB_FAST: if(features->see_ball_med && features->ball_vector.x > 190.0){ pprintf(TextOutputStream,"Fsaw ball at %f\n",features->ball_vector.x); TRANS_CONT(fsm,APPROACH_CENTER,5,"saw ball"); } if(fsm.timeInState() > SecToTime(0.3)){ pprintf(TextOutputStream,"Fgrab success\n"); retval = 1.0; } if(features->ball_vector.x > 130){ command->head_cmd = HEAD_ANGLES; command->head_tilt = -1.0; command->head_pan = 0.0; command->head_tilt2 = 0.78; }else{ command->head_cmd = HEAD_ANGLES; command->head_tilt = -1.3; command->head_pan = 0.0; command->head_tilt2 = 0.78; } walk_rel = ball_v; walk_rel.x += 67.5; //body to neck command->motion_cmd = MOTION_WALK_TROT_FAST; command->vx = walk_rel.x / 1.5; command->vy = 0; command->va = walk_rel.angle(); if(features->ball_on_chest || features->sensors->button & HeadBackButton) last_ball_on_chest_reading = features->current_time; if(fsm.timeInState() > ab_sensor_reading_time && last_ball_on_chest_reading == 0UL && !features->team_msg_mgr->i_am_goalie){ pprintf(TextOutputStream,"Fgrab failed\n"); //features->current_time - last_ball_on_chest_reading > ab_sensor_reading_time){ command->setFaceLEDs(LED_RED_EYES_LOW, LED_BRIGHT, MODE_A); retval = -1; } done = true; break; case GRAB_SLOW: if(features->see_ball_med && features->ball_vector.x > 180.0){ pprintf(TextOutputStream,"saw ball at %f\n",features->ball_vector.x); TRANS_CONT(fsm,APPROACH_CENTER,5,"saw ball"); } if(fsm.timeInState() > SecToTime(0.2)){ pprintf(TextOutputStream,"grab success\n"); retval = 1.0; } if(features->ball_vector.x > 130){ command->head_cmd = HEAD_ANGLES; command->head_tilt = -0.9; command->head_pan = 0.0; command->head_tilt2 = 0.78; }else{ command->head_cmd = HEAD_ANGLES; command->head_tilt = -1.1; command->head_pan = 0.0; command->head_tilt2 = 0.78; } walk_rel = ball_v; walk_rel.x += 67.5; walk_rel = walk_rel.norm() * MAX_DX * 0.5; command->motion_cmd = MOTION_WALK_TROT_FAST; command->vx = MaxDX*0.75;//walk_rel.x; command->vy = 0; command->va = walk_rel.angle(); if(features->ball_on_chest || features->sensors->button & HeadBackButton) last_ball_on_chest_reading = features->current_time; if(fsm.timeInState() > ab_sensor_reading_time && last_ball_on_chest_reading == 0UL && !features->team_msg_mgr->i_am_goalie){ pprintf(TextOutputStream,"grab failed\n"); //features->current_time - last_ball_on_chest_reading > ab_sensor_reading_time){ command->setFaceLEDs(LED_RED_EYES_LOW, LED_BRIGHT, MODE_A); retval = -1; } done = true; break; } } if(fsm.error!=0){ fsm.handleErr(command); } fsm.endLoop(); return retval;//0 if keep going, -1 if fail } uchar *BeApproachBall::encodeAllNames(ulong time,uchar *buf,int buf_size) { uchar *bufp; bufp = BehaviorEncodeNames::encodeAllNames(buf,beh_id,beh_name,NumStates,state_names,&fsm); // sub state machines SPOutEncoder::encodeAs(&bufp,0); return bufp; } uchar *BeApproachBall::encodeTrace(uchar *buf,int buf_size) { uchar *bufp; bufp = BehaviorEncodeTrace::encodeTrace(buf,beh_id,&fsm); // sub state machines SPOutEncoder::encodeAs(&bufp,0); return bufp; } /* ============================================================ */ /* ================================================> BeDribble */ /* ============================================================ */ /** * This behavior is called with the robot holding the ball. * It will try to carry the ball to global point p. */ const ulong timeout = 2800000; // Timeout is 2.8 secs template inline num1 bound_abs(num1 x,num2 low, num2 high) { // Bounds the magnitude while keeping the original sign num1 x_abs = fabs(x); num2 low_abs = fabs(low); num2 high_abs = fabs(high); if (low > high) swap(low, high); if(x_abs < low_abs) return((x < 0) ? -low_abs : low_abs); if(x_abs > high_abs) return((x < 0) ? -high_abs: high_abs); return(x); } char const * const BeDribble::beh_name = "BeDribble"; char const * const BeDribble::state_names[BeDribble::NumStates] = { "TURN", "WALK"}; BeDribble::BeDribble() { fsm.init(state_names, NumStates, TURN , 16, 16); setDefaultThresholds(); start_time = 0; turn = new BeTurn(); } BeDribble::~BeDribble() { if(turn != NULL){ delete turn; turn = NULL; } } /* DRIBBLE Behavior: if see ball, return -2.0 if distance from point <= some_threshold, return 1.0 for done if time since start or reset > 2.8, return -1.0 if angle > some_threshold, just turn. if angle <= some_threshold, walk and turn return 0.0 for continuing */ double BeDribble::operator()(FeatureSet *FS ,MotionCommand *command, vector2d target_glb) { vector2d target_rel; double retval = 0.0; // if we see the ball, we lost it, we can't dribble anymore if (FS->see_ball_med) return -2.0; // Get the local displament from the desired global-coordinates target target_rel = FS->getRelativePosn(target_glb); // return 1.0 if we're at the point within the thresholds if ((fabs(target_rel.x) < thresh_x) && (fabs(target_rel.y) < thresh_y)){ command->motion_cmd = MOTION_STAND_NEUTRAL; return 1.0; } /* * State machine used to first turn towards the perceived direction * of the target, and then walk there */ bool done=false; fsm.startLoop(FS->current_time); while(!done && fsm.error==0){ switch(fsm.getState()){ case TURN: if (fsm.isNewState()) { if(FS->my_position.position.mean.y > 0) turn_angle = -M_PI/2.0; else turn_angle = M_PI/2.0; } if(turn->turnBallToAngle(FS, command, turn_angle, RAD(5.0))==0.0){ TRANS_CONT(fsm, WALK, 5, "ReachedAngle"); } done = true; break; case WALK: command->motion_cmd = MOTION_KICK_BUMP; if(!fsm.isNewState()){ retval = 1.0; }else{ retval = 0.0; } // // look up and return -1.0 if we ran out of time // if (FS->current_time > start_time + timeout) { // command->head_cmd = HEAD_LOOKAT; // command->head_lookat.set(1500, 0, 0); // command->vx = 0; // command->vy = 0; // command->va = 0; // retval = -1.0; // done = true; // break; // } // // This will keep it walking until reached position or timed out // command->vx = MaxDX; // command->vy = 0; // command->va = 0; // // The following parameters are those used by the BeTurn behavior // if(strcmp(model,"ERS210")==0) { // command->motion_cmd = MOTION_WALK_TROT; // command->head_lookat.set(100, sign(command->va)*35, 0); // }else{ // command->motion_cmd = MOTION_WALK_DRIBBLE; // command->head_cmd = HEAD_ANGLES; // command->head_tilt = -1.1; // command->head_pan = 0.0; // command->head_tilt2 = 1.1; // } done = true; break; } } if(fsm.error!=0){ fsm.handleErr(command); } fsm.endLoop(); return retval; } void BeDribble::reset(ulong timestamp) { start_time = 0; fsm.setState(TURN,0,"Reset",timestamp); turn->reset(timestamp); } void BeDribble::setThresholds(double x, double y) { thresh_x = fabs(x); thresh_y = fabs(y); } void BeDribble::setDefaultThresholds() { thresh_x = thresh_y = 150; } /* ============================================================================ */ /* ========================================================> BeNavToPointGlobal */ /* ============================================================================ */ char const * const BeNavToPointGlobal::beh_name = "BeNavToPointGlobal"; char const * const BeNavToPointGlobal::state_names[BeNavToPointGlobal::NumStates] = { "NAV_TO_POINT", "STUCK"}; //Minimum time allowed for robot to be walking //before we check to see if it thinks its stuck. //This avoids thinking we're stuck while standing. const ulong ntp_min_walk_time = 2000000L; //Minimum time allowed for robot to think it //is stuck before we actually react to it being //stuck. Meant to prevent reacting to spurious //stuck detector readings. const ulong ntp_min_stuck_time = 500000L; //Minimum time required for reacting to being //stuck, to prevent for prematurely aborting //and getting stuck again. const ulong ntp_min_react_time = 3000000L; //Maximum time required for reacting to being //stuck, to cause re-entry into stuck state //in order to back up again. const ulong ntp_max_react_time = 6000000L; //Threshold for stuck_info.fraction_stuck above //which the robot is believed to be stuck. const double ntp_stuck_thresh = 0.5; //Threshold for stuck_info.fraction_stuck below //which the robot is believed to be unstuck. const double ntp_unstuck_thresh = 0.3; BeNavToPointGlobal::BeNavToPointGlobal() { fsm.init(state_names,NumStates,NAV_TO_POINT,16,16); resetVars(0); } BeNavToPointGlobal::~BeNavToPointGlobal() { } void BeNavToPointGlobal::resetVars(ulong timestamp) { target_pt.set(0.0,0.0); global_coord = true; no_walk = false; free_obs_params.set(200.0,200.0,2); stuck_obs_params.set(200.0,350.0,3); rec_length = free_obs_params.x; //Length of one rectangle rec_width = free_obs_params.y; //Width of one rectangle num_recs = (int)free_obs_params.z; //Number of rectangles forward to check //Defaults for black obstacle detection, if //these values are changed, they must also be changed //in the .h file. (setCheckObstParams) free_black_obs = false; stuck_black_obs = true; black_obs = free_black_obs; use_stuck_detector = true; start_time_stuck = timestamp; start_time_walk = timestamp; } void BeNavToPointGlobal::reset(ulong timestamp) { fsm.setState(NAV_TO_POINT,0,"Reset",timestamp); resetVars(timestamp); } void BeNavToPointGlobal::sleep() { fsm.sleep(); } void BeNavToPointGlobal::setTargetGlobal(double X,double Y) { target_pt.set(X,Y); global_coord = true; } void BeNavToPointGlobal::setTargetGlobal(vector2d target) { target_pt = target; global_coord = true; } void BeNavToPointGlobal::setTargetEgo(double X,double Y) { target_pt.set(X,Y); global_coord = false; } void BeNavToPointGlobal::setTargetEgo(vector2d target) { target_pt = target; global_coord = false; } void BeNavToPointGlobal::setNavParams(){ if(fsm.getState() == NAV_TO_POINT){ rec_length = free_obs_params.x; rec_width = free_obs_params.y; num_recs = (int)free_obs_params.z; black_obs = free_black_obs; } else{ rec_length = stuck_obs_params.x; rec_width = stuck_obs_params.y; num_recs = (int)stuck_obs_params.z; black_obs = stuck_black_obs; } } void BeNavToPointGlobal::setCheckObstParams(double length, double width, int recs, bool blkobs){ free_obs_params.set(length,width,(double)recs); free_black_obs = blkobs; } void BeNavToPointGlobal::setCheckObstParamsStuck(double length, double width, int recs, bool blkobs){ stuck_obs_params.set(length,width,(double)recs); stuck_black_obs = blkobs; } void BeNavToPointGlobal::setBlackObs(bool blkobs){ free_black_obs = blkobs; } void BeNavToPointGlobal::setBlackObsStuck(bool blkobs){ stuck_black_obs = blkobs; } void BeNavToPointGlobal::setStuckDetector(bool stuck){ use_stuck_detector = stuck; } bool BeNavToPointGlobal::isStuck(){ return (fsm.getState() == STUCK); } double BeNavToPointGlobal::operator()(FeatureSet *features, MotionCommand *command, bool trot_fast) { double retval = 0; /* Record time we start to get stuck */ if(features->stuck_info.fraction_stuck <= ntp_stuck_thresh){ start_time_stuck = features->current_time; } /* Record time we start to walk */ if(features->motion_state != Motion::STATE_WALKING){ start_time_walk = features->current_time; } bool done=false; fsm.startLoop(features->current_time); while(!done && fsm.error==0){ switch(fsm.getState()){ case NAV_TO_POINT: retval = navToPoint(features,command,trot_fast); if(features->current_time - start_time_stuck > ntp_min_stuck_time && features->current_time - start_time_walk > ntp_min_walk_time && use_stuck_detector){ TRANS_CONT(fsm,STUCK,5,"Stuck"); } done = true; break; case STUCK: if(!use_stuck_detector){ TRANS_CONT(fsm,NAV_TO_POINT,5,"DetectionOff"); } // commented this out for robocup, we need to use MODE_A to //make sure we don't use illegal colors. //command->setFaceLEDs(LED_4PURPLE,LED_BRIGHT,MODE_B); if(fsm.timeInState() < 2000000){ if(false){ command->sound_cmd = SOUND_NOTE; command->sound_frequency = 440 + (int)(880*features->stuck_info.fraction_stuck); command->sound_duration = SecToTime(.25); } command->motion_cmd = MOTION_WALK_TROT; command->vx = (-MAX_DX/2.0)*(features->stuck_info.fraction_stuck+.1); command->vy = 0.0; command->va = 0.0; } else{ retval = navToPoint(features,command,trot_fast); if((fsm.timeInState() > ntp_min_react_time && features->stuck_info.fraction_stuck <= ntp_unstuck_thresh) || (fsm.timeInState() > ntp_max_react_time && features->stuck_info.fraction_stuck > ntp_stuck_thresh)){ TRANS_CONT(fsm,NAV_TO_POINT,6,"NotStuck"); } } done = true; break; } } if(fsm.error!=0){ fsm.handleErr(command); } fsm.endLoop(); return retval; } /** * Appropriately sets the robot motion commands so that * the robot navigates to the target point while avoiding * obstacles. */ double BeNavToPointGlobal::navToPoint(FeatureSet *features, MotionCommand *command, bool trot_fast) { vector2d target_rel = target_pt; vector2d walk_dir; /* Get appropriate target vector */ if(global_coord){ target_rel = features->getRelativePosn(target_pt); } /* Assign correct walk trot speed */ if(trot_fast){ command->motion_cmd = MOTION_WALK_TROT_FAST; }else{ command->motion_cmd = MOTION_WALK_TROT; } /* Set query parameters and find best angle to walk at */ setNavParams(); walk_dir = pickClosestAngleToTarget(features,target_rel); if(!no_walk){ /* The generally straight path is clear */ command->vx = walk_dir.x; command->vy = 0.0; command->va = walk_dir.angle(); } else{ /* Obstacle in front of robot */ command->vx = 0.0; command->vy = 0.0; command->va = -1.0; } if(target_rel.length() < 150.0){ return 1.0; }else{ return 0.0; } } /** * Determines whether or not an occupancy * cell is free of obstacles. * Returns true if cell is clear, false * otherwise. */ bool BeNavToPointGlobal::cellIsClear() { if(cell.obs > 0.2 && cell.evidence > 0.3){ return false;//(cell.stripe_conf > 0.1 && cell.wall_conf < 0.2); } return true; } /** * Checks for obstacles at the given angle. * Returns true if there are obstacles at the angle * false if it is clear to walk at that angle. */ bool BeNavToPointGlobal::checkObsAngle(FeatureSet *features, double a){ /* Initializations */ int i; vector2f dir; //Directional unit vector towards angle vector2f len; //Center of local model rectangles /* Rotate vector to angle */ dir.set(100.0, 0.0); //Initial state of vector dir = dir.rotate(a); //Rotate to point to angle dir = dir.norm(); //Make it a unit vector /* Query rectangles in that direction */ for(i=0; iqueryLocalModel(&cell, dir, len, vector2f(rec_length,rec_width), black_obs); /* Look at result of query */ if(!cellIsClear()){ return true; //Obstacle found } } return false; //No obstacles were found at angle } /** * Determines which of the angles specified * would be the best to choose in walking towards * the given goal. Basically, the one with the * smallest angle between itself and the goal vector * that is free of obstacles. */ vector2d BeNavToPointGlobal::pickClosestAngleToTarget(FeatureSet *features, vector2d rel_target){ /* Initializations */ static const int num=9; //array size int i, minv = 0; //Minimum vector index double val, min = 4.0; //Temp return value and minimum angle vector2d walk[num]; //Holds vectors of all angles no_walk = false; //Reset walking boolean /* Angle offsets to be added to given angle */ double angles[num] = {-0.65,-0.5, -0.3836,-0.2618,0.0,0.2618,0.3836, 0.5,0.65}; /* Get info from all angles */ for(i=0; i= 0){ if(!checkObsAngle(features,angles[minv-i])) return walk[minv-i]; } } } /* All angles are blocked */ no_walk = true; return walk[0]; } uchar *BeNavToPointGlobal::encodeAllNames(ulong time,uchar *buf,int buf_size) { uchar *bufp; bufp = BehaviorEncodeNames::encodeAllNames(buf,beh_id,beh_name,NumStates,state_names,&fsm); // sub state machines SPOutEncoder::encodeAs(&bufp,0); return bufp; } uchar *BeNavToPointGlobal::encodeTrace(uchar *buf,int buf_size) { uchar *bufp; bufp = BehaviorEncodeTrace::encodeTrace(buf,beh_id,&fsm); // sub state machines SPOutEncoder::encodeAs(&bufp,0); return bufp; } /* ============================================================ */ /* ============================================> BeGetNearBall */ /* ============================================================ */ char const * const BeGetNearBall::beh_name = "BeGetNearBall"; char const * const BeGetNearBall::state_names[BeGetNearBall::NumStates] = { "LOOK_FORWARD","LOOK_NEAR","SPIN","SETTLE","WANDER_BALL","WANDER_RANDOM","GET_BALL","LOST_SCAN"}; //time spent in look_wm_ball state const ulong gnb_look_time = SecToTime(0.5); //time spent in look_near state const ulong gnb_look_near_time = SecToTime(1.5); //time spent in spin state const ulong gnb_spin_time = SecToTime(3.2); //walk in place this long to force a parameter switch //for the spin const ulong gnb_spin_halt_time = SecToTime(0.2); //time spent in settle state const ulong gnb_settle_time = SecToTime(1.0); //time between reseting wander target const ulong gnb_wander_target_time = SecToTime(5.0); //time between spins const ulong gnb_time_between_spins = SecToTime(5.0); //do a point scan with the head around the WM location //of the ball if it was lsat seen in less than this //many seconds const ulong gnb_look_ball_time = SecToTime(1.0); //if we enter GetNearBall and we've seen the ball //within this period of time we will not stand but //will wanter to that point const ulong gnb_saw_ball_wander_time = SecToTime(1.0); //dont do the above if the ball you saw was very close const double gnb_saw_ball_wander_thresh = 250.0; //time for lost scan const ulong gnb_lost_scan_time = SecToTime(1.5); const double gnb_ball_too_close_thresh = 60.0; const double gnb_look_ball_thresh = 700.0; const int gnb_saw_ball_frames = 5; BeGetNearBall::BeGetNearBall() { fsm.init(state_names, NumStates, LOOK_FORWARD, 64, 64); navtopoint = new BeNavToPointGlobal(); gotopoint = new BeGotoPointGlobal(); track_objects = new BeTrackObjects(); resetVars(); } BeGetNearBall::~BeGetNearBall() { if(track_objects != NULL){ delete track_objects; track_objects = NULL; } if(navtopoint != NULL){ delete navtopoint; navtopoint= NULL; } if(gotopoint != NULL){ delete gotopoint; gotopoint= NULL; } } void BeGetNearBall::reset(ulong timestamp) { fsm.setState(LOOK_FORWARD,0,"Reset",timestamp); track_objects->reset(timestamp); navtopoint->reset(timestamp); gotopoint->reset(timestamp); gotopoint->setWalk(MOTION_WALK_TROT_FAST); resetVars(); } void BeGetNearBall::resetVars() { timer = 0; nav_retval = 0.0; first_spin = true; prev_state = 0; prev_ball_hyp = -1; look_target.set(0,0,0); spin_direction = 1; walk_target_bound1.set(-1000.0,-1000.0); walk_target_bound2.set( 1000.0, 1000.0); } //bound the random targets we set for NavToPoint to this rectangular region void BeGetNearBall::setTargetBounds(vector2d pt1, vector2d pt2){ walk_target_bound1 = pt1; walk_target_bound2 = pt2; } //bound the random targets we set for NavToPoint to this rectangular region void BeGetNearBall::setTargetBounds(double pt1_x, double pt1_y, double pt2_x, double pt2_y){ walk_target_bound1.set(pt1_x, pt1_y); walk_target_bound2.set(pt2_x, pt2_y); } //set a random target for NavToPoint to walk to void BeGetNearBall::setRandomTarget(FeatureSet *features){ double midx = (walk_target_bound1.x+walk_target_bound2.x)/2.0; double midy = (walk_target_bound1.y+walk_target_bound2.y)/2.0; double dx = fabs(walk_target_bound1.x-walk_target_bound2.x)/2.0; double dy = fabs(walk_target_bound1.y-walk_target_bound2.y)/2.0; vector2d point(midx + GlobalRandom.uniformNeg1to1()*dx, midy + GlobalRandom.uniformNeg1to1()*dy); //make sure its not in or through the defense box point = avoidDefenseBoxTarget(features, point); navtopoint->setTargetGlobal(point); } //given a target we are trying to walk to, make sure we're not going to walk through //the defense box. To avoid the box we set new targets which are approximately the //corners of the box, so we walk around the perimeter vector2d BeGetNearBall::avoidDefenseBoxTarget(FeatureSet *features, vector2d global_target){ vector2d new_target = global_target; //these are the corners of the defense box that we use to walk around its perimeter vector2d def_box_pt1(sign(features->defend_goal_const.x)*(penaltyRegionLengthOffset-50), penaltyRegionHalfWidth+50.0); vector2d def_box_pt2(sign(features->defend_goal_const.x)*(penaltyRegionLengthOffset-50), -penaltyRegionHalfWidth-50.0); if(fabs(features->defend_goal_const.x - global_target.x) < 700.0 && fabs(features->defend_goal_const.x - features->my_position.position.mean.x) < 700.0){ if( global_target.y > def_box_pt1.y && features->my_position.position.mean.y < def_box_pt2.y+50){ new_target=def_box_pt2; }else if( global_target.y < def_box_pt2.y && features->my_position.position.mean.y > def_box_pt1.y-50){ new_target=def_box_pt1; }else if( global_target.y > def_box_pt1.y && fabs(features->my_position.position.mean.y) < def_box_pt1.y+50){ new_target=def_box_pt1; }else if( global_target.y < def_box_pt2.y && fabs(features->my_position.position.mean.y) < def_box_pt1.y-50){ new_target=def_box_pt2; } } return new_target; } double BeGetNearBall::operator()(FeatureSet *features,MotionCommand *command) { vector2d walk_target; vector2d predict_ball; vector2d best_ball; int hyp_type = features->getBestBallPosnPrimary(best_ball); bool new_ball_hyp = false; if(hyp_type != prev_ball_hyp){ new_ball_hyp = true; prev_ball_hyp = hyp_type; } bool done=false; fsm.startLoop(features->current_time); while(!done && fsm.error==0){ switch(fsm.getState()){ case LOOK_FORWARD: if(timer == 0UL && features->motion_state != Motion::STATE_KICKING){ timer = fsm.timeInState() + gnb_look_time; } //sometimes when we kick we have one frame in which we think we //are not kicking. In that case the timer would start. We reset //the timer if we detect that we are kicking again. if(timer != 0UL && features->motion_state == Motion::STATE_KICKING){ timer = 0UL; } if(timer != 0UL){ if(features->see_ball_low){ TRANS_CONT(fsm,WANDER_BALL,5,"SawBall"); } if(features->time_in_role < SecToTime(1.0)){ spin_direction = sign(features->getRelativePosn(best_ball).angle()); TRANS_CONT(fsm,SPIN,8,"Just turned primary"); } if(fsm.timeInState() > timer){ timer = 0UL; TRANS_CONT(fsm,LOOK_NEAR,6,"Timeout"); } //if we lost the ball briefly but were chasing it down we should keep going if(features->current_time-features->ball_last_seen_med < gnb_saw_ball_wander_time && features->ball_distance > gnb_saw_ball_wander_thresh){ TRANS_CONT(fsm,WANDER_BALL,7,"SawBallRecently"); } } command->motion_cmd = MOTION_WALK_TROT; command->vx = 0; command->vy = 0; command->va = 0; if(features->current_time - features->ball_last_seen_med < SecToTime(0.5)){ command->head_cmd = HEAD_LOOKAT; command->head_lookat = features->ball_vector3d; }else{ command->head_cmd = HEAD_LOOKAT; command->head_lookat.set(700,0,5); } done = true; break; case LOOK_NEAR: /* Look around you for the ball by scanning */ if(fsm.isNewState()){ timer = gnb_look_near_time; } //reset the timer if we are not walking for some reason //(if we are kicking mainly) if(features->motion_state != Motion::STATE_WALKING){ timer = fsm.timeInState() + gnb_look_near_time; } if(fsm.timeInState() > timer){ if(features->high_loc_uncertainty && features->near_front_wall){ TRANS_CONT(fsm,LOST_SCAN,5,"Timeout in corner"); }else{ spin_direction = sign( features->ball_vector.angle()); first_spin = true; TRANS_CONT(fsm,SPIN,6,"Timeout"); } } if(features->see_ball_low){ TRANS_CONT(fsm,WANDER_BALL,7,"SawBall"); } if(features->current_time - features->ball_last_seen_med < gnb_saw_ball_wander_time && features->ball_distance > gnb_saw_ball_wander_thresh){ TRANS_CONT(fsm,WANDER_BALL,8,"SawBallRecently"); } command->motion_cmd = MOTION_WALK_TROT; command->vx = 0; command->vy = 0; command->va = 0; command->head_cmd = HEAD_SCAN_BALL; done = true; break; case LOST_SCAN: /* Look for markers */ if(fsm.isNewState()){ timer = gnb_lost_scan_time; } if(fsm.timeInState() > timer){ spin_direction = sign( features->ball_vector.angle()); first_spin = true; TRANS_CONT(fsm,SPIN,6,"Timeout"); } if(features->see_ball_low){ TRANS_CONT(fsm,WANDER_BALL,7,"SawBall"); } command->motion_cmd = MOTION_WALK_TROT; command->vx = 0; command->vy = 0; command->va = 0; command->head_cmd = HEAD_SCAN_MARKERS_HIGH; done = true; break; case SPIN: /* Spin to search for the ball. Since we often dont really have a good idea of the ball's position if we see it during a spin, we exit to the settle state if we see something. */ if(features->see_ball_low){ first_spin = false; TRANS_CONT(fsm,SETTLE,5,"SeeBall"); } if(!first_spin && new_ball_hyp && features->getRelativePosn(best_ball).length() > 300.0){ navtopoint->reset(features->current_time); navtopoint->setTargetGlobal(best_ball); timer = gnb_wander_target_time; TRANS_CONT(fsm,WANDER_BALL,6,"Other robot saw ball"); } if(fsm.timeInState() > gnb_spin_time){ navtopoint->reset(features->current_time); timer = 0UL; first_spin = false; if(prev_state == WANDER_RANDOM){ TRANS_CONT(fsm,WANDER_BALL,7,"Timeout"); }else{ TRANS_CONT(fsm,WANDER_RANDOM,8,"Timeout"); } } if(fsm.timeInState() < gnb_spin_halt_time){ command->motion_cmd = MOTION_WALK_TROT; command->vx = 0; command->vy = 0; command->va = 0; }else{ command->head_cmd = HEAD_SCAN_BALL_TURN; command->motion_cmd = MOTION_WALK_TROT; command->vx = 0; command->vy = 0; command->va = spin_direction*MaxDA; } done = true; break; case SETTLE: /* we have seen the ball during the spin, but we're not very good at telling where it is. Stop and look at that point. If we time out or see the ball, start going towards that location. */ command->head_cmd = HEAD_LOOKAT; command->head_lookat = features->ball_vector3d; command->motion_cmd = MOTION_WALK_TROT; command->vx = 0; command->vy = 0; command->va = 0; if(fsm.timeInState() > gnb_settle_time || features->see_ball_med){ timer = 0; TRANS_CONT(fsm,WANDER_BALL,6,"SawBall"); } done = true; break; case WANDER_BALL: /*Use NavToPointGlobal to walk to the last known ball posn*/ if(fsm.isNewState()){ nav_retval = 0.0; timer = gnb_wander_target_time ; prev_state = WANDER_BALL; gotopoint->reset(features->current_time); } if(features->world_model->validBallsInHist() >= gnb_saw_ball_frames){ timer = 0UL; TRANS_CONT(fsm,GET_BALL,5,"FoundBall"); } //reset timer if(features->see_ball_low || new_ball_hyp){ timer = fsm.timeInState() + gnb_wander_target_time; } //saw the ball - set that as target, forget about obstacles if(features->see_ball_low || features->saw_ball_recently){ look_target.set(features->world_ball_vector.x, features->world_ball_vector.y,0.0); gotopoint->setTarget(features->world_ball_vector); }else{ vector2d further_best_ball = best_ball * 1.1; look_target.set(further_best_ball.x, further_best_ball.y, 0.0); gotopoint->setTarget(best_ball); } //got to the ball location, should spin if(features->getRelativePosn(gotopoint->getTarget()).length() < 200 && !features->see_ball_low){ spin_direction = 1; TRANS_CONT(fsm,SPIN,7,"At ball loc need to spin"); } if(fsm.timeInState() > SecToTime(10.0)){ spin_direction = 1; TRANS_CONT(fsm,SPIN,8,"timeout"); } if(features->saw_ball_recently && fabs(features->getRelativePosn(vector2d(look_target.x,look_target.y)).angle()) < RAD(70.0)){ command->head_cmd = HEAD_SCAN_POINT; command->head_lookat = features->getRelativePosn(look_target); command->head_tilt_offset = 0; }else{ if(fabs(features->velocity.z) < 0.3) command->head_cmd = HEAD_SCAN_BALL; else command->head_cmd = HEAD_SCAN_OBSTACLES; } (*gotopoint)(features, command);//use fast trot done = true; break; case WANDER_RANDOM: /* Use NavToPointGlobal to walk around the field. Keep randomly switching the target of where we are trying to go. */ if(fsm.isNewState()){ nav_retval = 0.0; prev_state = WANDER_RANDOM; navtopoint->setStuckDetector(true);//avoid obstacles setRandomTarget(features); timer = fsm.timeInState() + gnb_wander_target_time; } if(features->see_ball_low || new_ball_hyp){ TRANS_CONT(fsm,WANDER_BALL,5,"FoundBall"); } if(fsm.timeInState() > gnb_time_between_spins || nav_retval == 1.0){ timer = 0UL; spin_direction = 1; TRANS_CONT(fsm,SPIN,6,"Spin"); } command->head_cmd = HEAD_SCAN_BALL; nav_retval = (*navtopoint)(features, command);//use smooth trot done = true; break; case GET_BALL: /* Actively walk to the ball, we're not avoiding obstacles here right now. */ //havent seen the ball in the last 25 frames if(features->world_model->validBallsInHist() < gnb_saw_ball_frames){ //if(features->current_time - features->ball_last_seen_med > SecToTime(1.0)){ TRANS_CONT(fsm,WANDER_BALL,5,"LostBall"); } walk_target = avoidDefenseBoxTarget(features, features->world_ball_vector); command->motion_cmd = MOTION_WALK_TROT_FAST; command->vx = MaxDX; command->vy = 0; command->va = features->getRelativePosn(walk_target).angle() * 2.0; //look only at the ball when we are turning or when it is very close if(fabs(command->va) < 0.3 && features->ball_distance > gnb_look_ball_thresh && features->world_model->validBallsInHist() > 15){ (*track_objects)(features, command, true, true); //velocity prediction // if(features->ball_trj.velocity.length() > 200 && features->velocity.z < 0.7){ // predict_ball = features->world_model->estimateBallPosn(0.15,5); // command->head_cmd = HEAD_LOOKAT; // command->head_lookat.set(predict_ball.x,predict_ball.y,BallRadius); // } }else{ (*track_objects)(features,command,false,true); // if(features->ball_trj.velocity.length() > 30 && features->velocity.z < 0.7){ // predict_ball = features->world_model->estimateBallPosn(0.15,5); // command->head_cmd = HEAD_LOOKAT; // command->head_lookat.set(predict_ball.x,predict_ball.y,BallRadius); // } } done = true; break; } } if(fsm.error!=0){ fsm.handleErr(command); } fsm.endLoop(); return 1.0; }