📄 kickselecta.cc
字号:
#include "KickSelecta.h"
#include "../Common/Common.h"
#include "Trick.h"
#include "BallTricks.h"
#include "BasicTricks.h"
#include "ComplexTricks.h"
#include "../Globals.h"
// KickSelecta class name courtesy of Vaughan Judd (vjudd@railservices.com.au)
// FastSelecta isn't allowed to turn.
Trick* KickSelecta::FastSelecta(double currx, double curry, double currheading, int type) {
double oppGoalHeading = utils.GetHeadingToGoal();
double ownGoalHeading = utils.GetHeadingToOwnGoal();
double zeroHeading = 0.0-RAD_TO_DEG(currheading);
double robotHeading = utils.GetClosestRobotAngle(75.0);
// if in defensive half
if (curry < 0) {
if (ABS(currx) >= 0) { // always !!
if (ABS(oppGoalHeading) > ABS(zeroHeading)) {
oppGoalHeading = zeroHeading;
}
}
}
bool avoidRobot = false;
//if (ABS(robotHeading) < 50.0 && wo_self_->y_ < 100) avoidRobot = true;
if (avoidRobot) {
if (robotHeading < 0) oppGoalHeading = 40;
else oppGoalHeading = -40;
//cout << "Avoiding robot at " << robotHeading << " by kicking " << oppGoalHeading << flush << endl;
}
// make sure globals are clear
lastKickedLeft_ = -100;
lastKickedRight_ = -100;
Trick* kick = NULL;
bool grabbed = true;
if (type > 0) grabbed = false;
bool facingObject = false;
if (grabbed && sensorValues_[S_INFRARED_NEAR] < 90000) facingObject = true;
// goal is practically lined up. hammer it in!
if (ABS(oppGoalHeading) < 40.0) {
if (curry <= 180) {
//if (GetDistanceToGoal(currx,curry,currheading) > 40.0) {
if (type != 0 && !avoidRobot) {
// kick = new BasicTricks::MultiTrick(3,new BallTricks::GrabTrick(),new BasicTricks::Kick(LocomotionCommand::TP_HEAD_LUNGE),
// new BasicTricks::Kick(LocomotionCommand::TP_GETUP));
kick = new BasicTricks::MultiTrick(3 ,new BallTricks::GrabTrick(), new BasicTricks::Kick(LocomotionCommand::TP_SUPER_HEAD_LUNGE,true),
new BasicTricks::Kick(LocomotionCommand::TP_GETUP));
} else {
if (curry < 135 || oppGoalHeading < 3 || (curry < 135 && oppGoalHeading < 10)) {
if (facingObject || avoidRobot) {
//kick = new BasicTricks::MultiTrick(2,new BasicTricks::Kick(LocomotionCommand::TP_HEAD_KICK_FORWARDS,true,oppGoalHeading),
// new BasicTricks::Kick(LocomotionCommand::TP_GETUP));
double turnSize = CROP(ABS(oppGoalHeading),40,80);
if (oppGoalHeading < 0) turnSize*=-1;
kick = new BallTricks::TurnKick(turnSize,false,grabbed);
} else {
if (wo_self_->y_ < 50)
kick = new BasicTricks::MultiTrick(2,new BasicTricks::Kick(LocomotionCommand::TP_SUPER_HEAD_LUNGE,true),
new BasicTricks::Kick(LocomotionCommand::TP_GETUP));
else
kick = new BasicTricks::MultiTrick(2,new BasicTricks::Kick(LocomotionCommand::TP_SUPER_HEAD_LUNGE,true),
new BasicTricks::Kick(LocomotionCommand::TP_GETUP));
}
} else {
kick = new BasicTricks::MultiTrick(2,new BasicTricks::Kick(LocomotionCommand::TP_HEAD_KICK_FORWARDS,true,oppGoalHeading),
new BasicTricks::Kick(LocomotionCommand::TP_GETUP));
}
}
} else {
if (vo_oppositionGoal_ != NULL && ABS(oppGoalHeading) < 2)
kick = new BasicTricks::Kick(LocomotionCommand::TP_CHEST_PUSH);
else if (ABS(oppGoalHeading) < 25) {
kick = new BasicTricks::MultiTrick(2,new BasicTricks::Kick(LocomotionCommand::TP_HEAD_KICK_FORWARDS,true,oppGoalHeading),
new BasicTricks::Kick(LocomotionCommand::TP_GETUP));
} else {
kick = new BallTricks::TurnKick(oppGoalHeading,false,grabbed);
}
}
} else {
double backRight150Angle = Normalise_PI(DEG_TO_RAD(oppGoalHeading) + DEG_TO_RAD(150.0));
double backLeft150Angle = Normalise_PI(DEG_TO_RAD(oppGoalHeading) - DEG_TO_RAD(150.0));
double absBestBackKick150Angle = MIN(ABS(backRight150Angle),ABS(backLeft150Angle));
double backRight110Angle = Normalise_PI(DEG_TO_RAD(oppGoalHeading) + DEG_TO_RAD(110.0));
double backLeft110Angle = Normalise_PI(DEG_TO_RAD(oppGoalHeading) - DEG_TO_RAD(110.0));
double absBestBackKick110Angle = MIN(ABS(backRight110Angle),ABS(backLeft110Angle));
bool doSpin = false;
bool doLowSlap = false;
double turnSize = CROP(ABS(oppGoalHeading-10),20.0,80.0);
if (type == 0 && ABS(oppGoalHeading) < 120 || (ABS(wo_self_->x_) > 90 || ABS(wo_self_->y_) > 170)) doLowSlap = true;
else doSpin = utils.SpinNotSlap(type,oppGoalHeading);
double averageSlap = 70;
if (type == 1) averageSlap = 90;
if (doSpin) averageSlap = turnSize;
double slapRightAngle = Normalise_PI(DEG_TO_RAD(oppGoalHeading) - DEG_TO_RAD(averageSlap));
double slapLeftAngle = Normalise_PI(DEG_TO_RAD(oppGoalHeading) + DEG_TO_RAD(averageSlap));
double absBestSlapKickAngle = MIN(ABS(slapRightAngle),ABS(slapLeftAngle));
// in own half, facing wrong way or near a side wall! special kicks.
if (ABS(wo_self_->heading_) > DEG_TO_RAD(90.0) && (curry < 50 || ABS(currx) > 100)) {
bool doSpin = false;
bool doHead = false;
// only types of 0 can spin... 1 can NOT spin
if (type == 0 && ABS(oppGoalHeading) > 120) {
if (ownGoalHeading > 0) {
kick = new BasicTricks::Kick(LocomotionCommand::TP_BACK_RIGHT_150);
lastKickedRight_ = frame_;
} else {
kick = new BasicTricks::Kick(LocomotionCommand::TP_BACK_LEFT_150);
lastKickedLeft_ = frame_;
}
}
else {
if (ownGoalHeading > 0) {
/*if (doHead) kick = new BasicTricks::Kick(LocomotionCommand::TP_HEAD_KICK_RIGHT);
else*/ if (doSpin || facingObject) kick = new BallTricks::TurnKick(-turnSize,true,grabbed);
else if (doHead || ABS(wo_self_->x_) > 90 || ABS(wo_self_->y_) > 170) kick = new BallTricks::SlapTrick(LocomotionCommand::TP_SLAP_RIGHT_LOW,doHead);
else kick = new BallTricks::SlapTrick(LocomotionCommand::TP_SLAP_RIGHT,false);
lastKickedRight_ = frame_;
} else {
/*if (doHead) kick = new BasicTricks::Kick(LocomotionCommand::TP_HEAD_KICK_LEFT);
else*/ if (doSpin || facingObject) kick = new BallTricks::TurnKick(turnSize,true,grabbed);
else if (doHead || ABS(wo_self_->x_) > 90 || ABS(wo_self_->y_) > 170) kick = new BallTricks::SlapTrick(LocomotionCommand::TP_SLAP_LEFT_LOW,doHead);
else kick = new BallTricks::SlapTrick(LocomotionCommand::TP_SLAP_LEFT,false);
lastKickedLeft_ = frame_;
}
}
} else {
bool hard = true;
if (wo_self_->y_ > 160) hard = false;
// only types of 0 can spin... 1 can NOT spin
if (type == 0 && ((absBestBackKick150Angle < absBestSlapKickAngle) || (absBestBackKick110Angle < absBestSlapKickAngle))) {
if (absBestBackKick150Angle < absBestBackKick110Angle) {
if (ABS(backLeft150Angle) < ABS(backRight150Angle)) {
kick = new BasicTricks::Kick(LocomotionCommand::TP_BACK_LEFT_150);
lastKickedRight_ = frame_;
} else {
kick = new BasicTricks::Kick(LocomotionCommand::TP_BACK_RIGHT_150);
lastKickedLeft_ = frame_;
}
} else {
if (ABS(backLeft110Angle) < ABS(backRight110Angle)) {
kick = new BasicTricks::Kick(LocomotionCommand::TP_BACK_LEFT_110);
lastKickedRight_ = frame_;
} else {
kick = new BasicTricks::Kick(LocomotionCommand::TP_BACK_RIGHT_110);
lastKickedLeft_ = frame_;
}
}
} else {
if (oppGoalHeading > 0) {
if (doSpin || facingObject) kick = new BallTricks::TurnKick(turnSize,hard,grabbed);
else if (doLowSlap) kick = new BallTricks::SlapTrick(LocomotionCommand::TP_SLAP_LEFT_LOW,grabbed);
else kick = new BallTricks::SlapTrick(LocomotionCommand::TP_SLAP_LEFT);
lastKickedLeft_ = frame_;
} else {
if (doSpin || facingObject) kick = new BallTricks::TurnKick(-1.0*turnSize,hard,grabbed);
else if (doLowSlap) kick = new BallTricks::SlapTrick(LocomotionCommand::TP_SLAP_RIGHT_LOW,grabbed);
else kick = new BallTricks::SlapTrick(LocomotionCommand::TP_SLAP_RIGHT);
lastKickedRight_ = frame_;
}
}
}
}
if (kick==NULL) {
//cout << "FastSelecta: Logic flaw! Returning default kick." << endl << flush;
return new BasicTricks::MultiTrick(2, new BasicTricks::Kick(16), new BallTricks::WaitForKick());
}
// Seeing why we appear to slap near the wall ... is this just because we are lost or is it for another reason
// if (strcmp(kick->GetName(),"Kick")==0 && (((BasicTricks::Kick*)kick)->type==LocomotionCommand::TP_SLAP_RIGHT || ((BasicTricks::Kick*)kick)->type==LocomotionCommand::TP_SLAP_RIGHT)) {
// cout << "Slap Kick (" << ((BasicTricks::Kick*)kick)->type << ") : x = " << currx << " y = " << curry << " h = " << RAD_TO_DEG(currheading) << " Type = " << type << endl << flush;
// }
// This is for general kick debugging ... is it just WM or our logic that is wrong !
/*if (strcmp(kick->GetName(),"Kick")==0) {
cout << "Kick Type (" << ((BasicTricks::Kick*)kick)->type << ") : x = " << currx << " y = " << curry << " h = " << RAD_TO_DEG(currheading) << " OppGoal = " << oppGoalHeading << " OwnGoal = " << ownGoalHeading << " Type = " << type << endl << flush;
} else if (strcmp(kick->GetName(),"TurnKick")==0) {
cout << "Turn Kick (" << ((BallTricks::TurnKick*)kick)->turnAmount << ") : x = " << currx << " y = " << curry << " h = " << RAD_TO_DEG(currheading) << " OppGoal = " << oppGoalHeading << " OwnGoal = " << ownGoalHeading << " Type = " << type << endl << flush;
} else if (strcmp(kick->GetName(),"MultiTrick")==0) {
cout << "Multi Tick : GoalDistance = " << GetDistanceToGoal(currx,curry,currheading) << " x = " << currx << " y = " << curry << " h = " << RAD_TO_DEG(currheading) << " OppGoal = " << oppGoalHeading << " OwnGoal = " << ownGoalHeading << " Type = " << type << endl << flush;
}
*/
if (strcmp(kick->GetName(),"TurnKick")==0) return kick;
return new BasicTricks::MultiTrick(2, kick, new BallTricks::WaitForKick());
}
//NormalSelecta generally just turns and then allows ComplexKick to call FastSelecta
Trick* KickSelecta::NormalSelecta(double currx, double curry, double currheading) {
#ifdef ERS_210
return NormalSelecta210(currx, curry, currheading);
#endif
#ifdef ERS_7
double oppGoalHeading = utils.GetHeadingToGoal();
double ownGoalHeading = utils.GetHeadingToOwnGoal();
double zeroHeading = 0.0-RAD_TO_DEG(currheading);
// if in defensive half
if (curry < 0) {
if (ABS(currx) >= 0) { // always !!
if (ABS(oppGoalHeading) > ABS(zeroHeading)) {
oppGoalHeading = zeroHeading;
}
}
}
Trick* complexKick = NULL;
double minTurnAmount = 0.0;
double maxTurnAmount = 0.0;
// if heading backwards and well inside your own half or near a side wall
if ((ABS(wo_self_->heading_) > DEG_TO_RAD(90.0) && (curry < 50 || ABS(currx) > 100)) ||
(ABS(oppGoalHeading) > 140 && ABS(currx) < 30 && curry > 140)) {
minTurnAmount = MIN(30.0,ABS(oppGoalHeading));
if (minTurnAmount < 15) minTurnAmount = 15.0;
if (minTurnAmount < 30.0 && wo_self_->uncertHeading_ > 0.2) minTurnAmount = 30.0;
maxTurnAmount = 90.0;
if (ownGoalHeading > 155 && ABS(oppGoalHeading) < 36) {
minTurnAmount = 0.0;
maxTurnAmount = 0.0;
} else if (ownGoalHeading >= 40) {
minTurnAmount = -maxTurnAmount;
maxTurnAmount = -maxTurnAmount;
} else if (ownGoalHeading >=0 ) {
minTurnAmount = -maxTurnAmount;
maxTurnAmount = -maxTurnAmount;
} else if (ownGoalHeading < -155 && ABS(oppGoalHeading) < 36 ) {
minTurnAmount = 0.0;
maxTurnAmount = 0.0;
} else if (ownGoalHeading <= -40) {
minTurnAmount = maxTurnAmount;
maxTurnAmount = maxTurnAmount;
} else {
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -