This commit is contained in:
Kayman 2018-10-02 10:40:52 +09:00
commit a6b392e502
3 changed files with 52 additions and 3 deletions

View File

@ -61,6 +61,11 @@ class BallFollower
return approach_ball_position_;
}
bool isBallInRange()
{
return (approach_ball_position_ == OnRight || approach_ball_position_ == OnLeft);
}
protected:
const bool DEBUG_PRINT;
const double CAMERA_HEIGHT;

View File

@ -91,6 +91,7 @@ class SoccerDemo : public OPDemo
void process();
void handleKick(int ball_position);
void handleKick();
bool handleFallen(int fallen_status);
void playMotion(int motion_index);

View File

@ -144,10 +144,12 @@ void SoccerDemo::process()
// check states for kick
int ball_position = ball_follower_.getBallPosition();
if (ball_position != robotis_op::BallFollower::NotFound)
bool in_range = ball_follower_.isBallInRange();
if(in_range == true)
{
ball_follower_.stopFollowing();
handleKick(ball_position);
handleKick();
}
break;
}
@ -490,12 +492,53 @@ void SoccerDemo::handleKick(int ball_position)
// change to motion module
setModuleToDemo("action_module");
//usleep(1500 * 1000);
if (handleFallen(stand_state_) == true || enable_ == false)
return;
// kick motion
switch (ball_position)
{
case robotis_op::BallFollower::OnRight:
std::cout << "Kick Motion [R]: " << ball_position << std::endl;
playMotion(is_grass_ ? RightKick + ForGrass : RightKick);
break;
case robotis_op::BallFollower::OnLeft:
std::cout << "Kick Motion [L]: " << ball_position << std::endl;
playMotion(is_grass_ ? LeftKick + ForGrass : LeftKick);
break;
default:
break;
}
on_following_ball_ = false;
restart_soccer_ = true;
usleep(2000 * 1000);
if (handleFallen(stand_state_) == true)
return;
// ceremony
//playMotion(Ceremony);
}
void SoccerDemo::handleKick()
{
usleep(1500 * 1000);
// change to motion module
setModuleToDemo("action_module");
if (handleFallen(stand_state_) == true || enable_ == false)
return;
// kick motion
int ball_position = ball_follower_.getBallPosition();
if(ball_position == BallFollower::NotFound || ball_position == BallFollower::OutOfRange)
return;
switch (ball_position)
{
case robotis_op::BallFollower::OnRight: