Action ComponentBrainKamikaze::getNextAction_chase(float milliseconds)
{
if(!targetPlayer ||
isDead(targetPlayer) ||
(!canDetectPlayer(*targetPlayer) &&
getTimeInState()>resumeWanderThreshold))
{
nextState = StateWander;
}
ComponentPhysics* component =
dynamic_cast<ComponentPhysics*>(targetPlayer->getComponent("Physics"));
if(!component)
return Stand;
vec3 playerPosition3D = component->getPosition();
vec3 ourPosition3D = getPosition3D();
vec2 playerPosition = playerPosition3D.xy();
vec2 ourPosition = ourPosition3D.xy();
float angle = getAngle(playerPosition, ourPosition);
Direction desiredDirection = getDirectionFromAngle(angle);
Direction currentFacing = getCurrentFacing();
return getActionFromAngle(angle);
}
void GetupModule::cross() {
if (getTimeInState() > 0.025)
return;
float angles[NUM_JOINTS];
for (int i = 0; i < NUM_JOINTS; i++) {
angles[i] = 0;
}
angles[HeadPitch] = DEG_T_RAD*30;
angles[LShoulderRoll] = 1.5708;
angles[RShoulderRoll] = 1.5708;
angles[LShoulderPitch] = -1.5708;
angles[RShoulderPitch] = -1.5708;
// walk arms
//angles[LShoulderPitch] = DEG_T_RAD * -96; // -116 //moving to make it farther outside the body
//angles[LShoulderRoll] = DEG_T_RAD * 8;
angles[LElbowYaw] = DEG_T_RAD * 0;//25;
angles[LElbowRoll] = DEG_T_RAD * 0;//-53;
//angles[RShoulderPitch] = DEG_T_RAD * -96; // -116 //moving to make it farther outside the body
//angles[RShoulderRoll] = DEG_T_RAD * 8;
angles[RElbowYaw] = DEG_T_RAD * 0;//25;
angles[RElbowRoll] = DEG_T_RAD * 0;//-53;
processJointCommands(100,angles);
commands_->setAllStiffness(1.0,10);
}
void GetupModule::selectGetup() {
int tiltSideThreshold = 5;
if (numCrosses > 0) // be more confident after a cross
tiltSideThreshold = 2;
// decide which getup to do
if (walk_request_->tilt_fallen_counter_ < -tiltSideThreshold) {
setBackGetup();
} else if (walk_request_->tilt_fallen_counter_ > tiltSideThreshold) {
setFrontGetup();
} else if ((numCrosses == 0) && (walk_request_->roll_fallen_counter_ >= 2)) {
transitionToState(CROSS);
} else if (getTimeInState() < 0.2) {
// do nothing, maybe we'll end up somewhere good
} else {
setBackGetup(); // default to back in the end
}
}
Action ComponentBrainShooter::getNextAction_chase(float milliseconds)
{
if(!targetPlayer ||
isDead(targetPlayer) ||
(!canDetectPlayer(*targetPlayer) &&
getTimeInState()>resumeWanderThreshold))
{
nextState = StateWander;
}
ComponentPhysics* component =
dynamic_cast<ComponentPhysics*>(targetPlayer->getComponent("Physics"));
if(!component)
return Stand;
vec3 playerPosition3D = component->getPosition();
vec3 ourPosition3D = getPosition3D();
vec2 playerPosition = playerPosition3D.xy();
vec2 ourPosition = ourPosition3D.xy();
float angle = getAngle(playerPosition, ourPosition);
Direction desiredDirection = getDirectionFromAngle(angle);
Direction currentFacing = getCurrentFacing();
if(currentFacing == desiredDirection &&
getDistance(playerPosition, ourPosition) < shootDistance &&
rayCast(*targetPlayer,
ourPosition3D,
playerPosition3D - ourPosition3D,
maxSightDistance))
{
getParentBlackBoard().relayMessage(MessagePerformAction(Stand));
return AttackOnce;
}
else
{
Action action = getActionFromAngle(angle);
return action;
}
}
bool GetupModule::processFrameChild() {
//std::cout << frame_info_->frame_id << " " << getName(state) << std::endl;
// set getting up odometry
if (isGettingUp()){
// which way are we getting up?
odometry_->getting_up_side_ = getUpSide;
} else {
// no getup
odometry_->getting_up_side_ = Getup::NONE;
// set getup side back to unknown for the next getup
getUpSide = Getup::UNKNOWN;
}
bool fallenCountHigh = (abs(walk_request_->roll_fallen_counter_) >= 50) || (abs(walk_request_->tilt_fallen_counter_) >= 50);
switch (state) {
case INITIAL:
if (walk_request_->getup_from_keeper_dive_) {
transitionToState(CROSS); // goalie needs cross
} else {
if (fallenCountHigh)
transitionToState(STIFFNESS_ON);
else
transitionToState(PREPARE_ARMS);
}
break;
case PREPARE_ARMS:
if ((getTimeInState() > PREPARE_ARMS_TIME) || fallenCountHigh) { // if fallen count is high, it's too late to prepare arms
transitionToState(STIFFNESS_OFF);
} else {
if (getTimeInState() < 0.015) {
prepareArms();
}
// otherwise do nothing
}
break;
case STIFFNESS_OFF:
if ((getTimeInState() > STIFFNESS_OFF_TIME) || fallenCountHigh) { // if fallen count is high, it's too late to worry about the stiffness being off
transitionToState(STIFFNESS_ON);
} else {
commands_->setAllStiffness(0.0,10);
}
break;
case STIFFNESS_ON:
if (getTimeInState() > 0.001) {
//if (armsStuckBehindBack()) {
//std::cout << "Trying to free arms" << std::endl;
//transitionToState(FREE_ARMS);
//} else {
transitionToState(EXECUTE);
//}
} else {
commands_->setAllStiffness(1.0,10);
}
break;
case CROSS:
if (getTimeInState() < CROSS_TIME)
cross();
else {
transitionToState(EXECUTE);
numCrosses++;
}
break;
case EXECUTE:
if (currMotion == Getup) {
selectGetup();
if (currMotion == Getup) // didn't choose
return true; // don't do anything else
// yay! we chose a getup
std::cout << "Starting getup: " << getName(currMotion) << std::endl;
stateStartTime = frame_info_->seconds_since_start; // reset the time
}
if (isMotionDoneExecuting()) {
if ((abs(walk_request_->tilt_fallen_counter_) > 2) || (abs(walk_request_->roll_fallen_counter_) > 2)) {
// still fallen
selectGetup();
transitionToState(STIFFNESS_ON);
} else {
transitionToState(STAND);
}
} else {
if (getTimeInState() < 0.015)
commands_->setAllStiffness(1.0,10);
//if (shouldAbortGetup()) { // KG: for now, never abort. Consider adding it back in if needed, but will need to also update method for new getup states
// std::cout << "FAILED GETUP" << std::endl;
// numRestarts++;
// transitionToState(PAUSE_BEFORE_RESTART);
//}
executeMotionSequence();
}
break;
case STAND:
// do nothing, handled in MotionCore
if (getTimeInState() > 0.5)
transitionToState(FINISHED);
break;
case FREE_ARMS:
currMotion = backFreeArms;
if (isMotionDoneExecuting()) {
currMotion = Getup;
transitionToState(EXECUTE);
} else {
if (getTimeInState() < 0.015)
commands_->setAllStiffness(1.0,10);
executeMotionSequence();
}
break;
case PAUSE_BEFORE_RESTART:
if (getTimeInState() > PAUSE_BEFORE_RESTART_TIME) {
transitionToState(STIFFNESS_ON);
} else {
commands_->setAllStiffness(0.0,10);
}
break;
default:
return false;
break;
}
return true;
}
