//===============================================================================
//Execute the skill. This is the main part of the skill, where you tell the
//robot how to perform the skill.
void JamAndShootSkill::execute()
{
///If not active, dont do anything!
if(!initialized)
{
return;
}
//aggresive mode
if(sp->strategy2002.JAM_AND_SHOOT_MODE == 1)
{
skillSet->getSkill(AggressiveJamAndShootSkill::skillNum)->run();
return;
}
//stupid mode
else if(sp->strategy2002.JAM_AND_SHOOT_MODE == 2)
{
skillSet->getSkill(StupidJamAndShootSkill::skillNum)->run();
return;
}
//else normal mode
//decrease the shoot threshold linearly as the skill runs.
shoot_threshold = sp->strategy2002.SHOOT_LANE_THRESH;// *
//(WAIT_TIME - (float)timer->currentTime()) / WAIT_TIME;
Pair currentPos(getLocation(robotID,*currentVisionData,*sp));
float currentRot = getRotation(robotID,*currentVisionData,*sp);
float good_shoot_thresh=shoot_threshold;
if(kicked) good_shoot_thresh*=1.5f;
else if(aimed) good_shoot_thresh=.05f;
//finished waiting. Shoot!
//note, we only shoot if we're facing in the direction of the goal.
kickFrames--;
if(
!gaveup &&
towardGoal(currentPos,
currentRot,sp->field.THEIR_GOAL_LINE,
sp->field.LEFT_GOAL_POST,
sp->field.RIGHT_GOAL_POST) &&
(timer->currentTime() > WAIT_TIME ||
goodImmediateShot(sp->general.TEAM,robotID,sp->strategy2002.KICK_VELOCITY,
*currentVisionData,*sp,good_shoot_thresh)
//goodCurrentShot(currentPos,currentRot,*currentVisionData,*sp,good_shoot_thresh)
)
)
{
gaveup=true;
if(driftdir != 0){
kickFrames = PAUSE_FRAMES + 2*rand()*PAUSE_RANDOM/RAND_MAX - PAUSE_RANDOM;
char msg[80];
sprintf(msg,"Jam and Shoot pausing for %d frames",kickFrames);
GUI_Record.debuggingInfo.addDebugMessage(msg);
}
kicked=false;
timer->resetTimer();
}
if (gaveup && kickFrames <= 0){
//we're out of time or have a good current shot, so just kick.
SimpleKickSkill * kickSkill=(SimpleKickSkill *) skillSet->getSkill(SimpleKickSkill::skillNum);
if(!kickSkill->isInitialized()){
kickSkill->initialize(KICK_SHOT);
}
kickSkill->run();
strategy->getCurrentFrame()->setMessage(robotID,"No Turn and Kick. Just Shooting.");
}else if(skillSet->getSkill(PullBallOffWallSkill::skillNum)->isValid()){
//deal with the ball in the corner or on the wall
if(!skillSet->getSkill(PullBallOffWallSkill::skillNum)->isInitialized()){
skillSet->getSkill(PullBallOffWallSkill::skillNum)->initialize();
}
skillSet->getSkill(PullBallOffWallSkill::skillNum)->run();
}else if(kicked) {
//We have a shot, let's aim and kick.
TurnAndKickSkill *kicker=(TurnAndKickSkill *)skillSet->getSkill(TurnAndKickSkill::skillNum);
kicker->run();
//strategy->getCurrentFrame()->setMessage(robotID,"Have Shot - taking it");
}else{
bool isShot;
//find best shot (if we're already drifting, use a larger threshold)
if(aimed)isShot=calcShot(robotID,sp->field.THEIR_GOAL_LINE,sp->strategy2002.SHOOT_LANE_THRESH+2*sp->general.PLAYER_RADIUS,
sp->field.RIGHT_GOAL_POST,sp->field.LEFT_GOAL_POST,NO_ROBOT,*currentVisionData,*sp,&target);
else isShot=calcShot(robotID,sp->field.THEIR_GOAL_LINE,sp->strategy2002.SHOOT_LANE_THRESH,
sp->field.RIGHT_GOAL_POST,sp->field.LEFT_GOAL_POST,NO_ROBOT,*currentVisionData,*sp,&target);
if(isShot && driftdir==0)
{
//take the shot
TurnAndKickSkill *kicker=(TurnAndKickSkill *)skillSet->getSkill(TurnAndKickSkill::skillNum);
kicker->initialize(target,KICK_SHOT,true);
kicker->run();
kicked=true;
strategy->getCurrentFrame()->setMessage(robotID,"Have a shot!");
kickFrames=0;
}else{
target.set(sp->field.THEIR_GOAL_LINE,sp->field.SPLIT_LINE);
//if no shot, start to wait, dodge if possible
GUI_Record.debuggingInfo.setDebugPoint(robotID,4,upperbound,testline);
GUI_Record.debuggingInfo.setDebugPoint(robotID,5,lowerbound,testline);
if (driftdir==0){
//Calculate which direction to drift
//first, see if we can shoot by shifting to the near post.
//if not, drift across the field.
//Default to drift across.
float goalPost;
if(currentPos.getY()>sp->field.SPLIT_LINE){
driftdir = 1;
goalPost=sp->field.LEFT_GOAL_POST;
}else{
driftdir = -1;
goalPost=sp->field.RIGHT_GOAL_POST;
}
upperbound=sp->field.THEIR_GOAL_LINE;
lowerbound=sp->field.KILL_ZONE_LINE;
testline=sp->field.SPLIT_LINE + driftdir*(SIDE_DIST);
Pair loc;
//check near-side shot.
if(currentPos.getY() * driftdir >= goalPost*driftdir &&
calcYShot(Pair(sp->field.THEIR_GOAL_LINE,sp->field.SPLIT_LINE),
testline,
sp->strategy2002.SHOOT_LANE_THRESH*SIDE_LANE_FACTOR,
upperbound,
lowerbound,NO_ROBOT,
*currentVisionData,
*sp,
&loc)){
driftdir=-driftdir;
}
}
// printf("%f\n",ABS(angleDifference(angleBetween(currentPos,target),currentRot)));
if(ABS(angleDifference(angleBetween(currentPos,target),currentRot)) < AIM_ANGLE || aimed){
aimed=true;
//if(target.getX() != sp->field.THEIR_GOAL_LINE) target.setX(sp->field.THEIR_GOAL_LINE);
//drift to the side to see if we have a shot.
//calculate the direction to drift
Pair dest;
float changeAngle;
//switch directions if we've gone too far.
if(angleBetween(currentPos,target)*driftdir > BOUNCE_ANGLE ){
driftdir=-driftdir;
}
target.setY(target.getY() - AIM_DISTANCE*driftdir);
changeAngle= DRIFT_ANGLE*driftdir;
char msg[80];
sprintf(msg, "No Shot - drifting %s",( driftdir > 0 ? "right" : "left"));
strategy->getCurrentFrame()->setMessage(robotID,msg);
//drift around goal
//See if we can push forward:
rotateAboutPoint(frontOfRobot(robotID,*currentVisionData,*sp), target,changeAngle, dest);
//DUE TO J&S BACKING UP TOO MUCH, I'M GETTING RID OF THIS NEXT PART
if(!isLane(currentPos,dest,sp->general.PLAYER_RADIUS,*currentVisionData,*sp,true)){
//we can't. :( drift sideways.
rotateAboutPoint(currentPos, target,changeAngle, dest);
}
//see if one of their robots is in the way, and back up if they are.
if(!isLane(currentPos,dest,sp->general.PLAYER_RADIUS,*currentVisionData,*sp,true)){
//move destination back
extendPoint(target,dest,sp->general.PLAYER_RADIUS,dest);
}
command->setPos(dest);
//Adjust angle to fast the direction we're drifting a bit.
command->setRotation(angleBetween(currentPos,target) - driftdir*DRIFT_ADJUST_ANGLE);
command->setDribble(DRIBBLE_DEFAULT);
command->setVerticalDribble(V_DRIBBLE_DEFAULT);
command->setSpeed(BALL_POSSESSION_SPEED);
}else{
strategy->getCurrentFrame()->setMessage(robotID,"No Shot - turning to goal");
//turn to face goal
SpinAroundBallSkill *spin=(SpinAroundBallSkill *)strategy->getSkillSet(robotID)->getSkill(SpinAroundBallSkill::skillNum);
spin->initialize(target);
spin->run();
}
}
}
RobotIndex goalie;
if(getLocation(robotID,*currentVisionData,*sp).getX() > sp->field.HALF_LINE){
if(getTheirGoalie(*currentVisionData,*sp,&goalie)){
if(getLocation(sp->general.OTHER_TEAM,goalie,*currentVisionData).distanceTo(getLocation(robotID,*currentVisionData,*sp)) >=
ENTERBOX_CAUTION_DIST+sp->general.PLAYER_RADIUS + sp->general.OPPONENT_RADIUS){
command->setControl(OMNI_FAST_ENTERBOX);
}
}else{
//if no goalie, enter the box
command->setControl(OMNI_FAST_ENTERBOX);
}
}
GUI_Record.debuggingInfo.setDebugPoint(robotID,0,target);
//command->setControl(OMNI_NORMAL_ENTERBOX);
command->setRotation(angleBetween(currentPos,target));
}
void RCViewableTransform::dolly(float x, float y)
{
rotateAboutPoint(focalPoint(), x, y);
}
void RCViewableTransform::pan(float x, float y)
{
rotateAboutPoint(transform().translation(), x, y);
}
//===============================================================================
//Execute the skill. This is the main part of the skill, where you tell the
//robot how to perform the skill.
void RotateAroundBallSkill::execute()
{
///If not active, dont do anything!
if(!initialized) return;
command->setDribble(SLOW_DRIBBLE);
command->setVerticalDribble(SLOW_V_DRIBBLE);
if(USE_BALL_FOR_PIVOT)
{
pivotPoint.set(strategy->getCurrentRoboCupFrame()->getOffensiveBallLocation());
}
else
{
pivotPoint.set(frontOfRobot(robotID, *currentVisionData, *sp));
}
ultimateAngle = angleBetween(pivotPoint,ultimateDestination);
//get robots current location and angle
robotLoc.set(getLocation(robotID,*currentVisionData,*sp));
robotAngle = angleBetween(robotLoc, pivotPoint);
//if finished, then keep position and rotation
if(isFinished())
{
command->setPos(robotLoc);
command->setRotation(angleBetween(robotLoc, ultimateDestination));
strategy->getCurrentFrame()->setMessage(robotID, "rotate done");
return;
}
strategy->getCurrentFrame()->setMessage(robotID, "rotate rotating");
//determine next angle robot should be at
nextAngle = ultimateAngle;
//determine angle diff
angleDiff = angleDifference(ultimateAngle, robotAngle);
if( angleDiff > ROTATION_STEP )
{
nextAngle = robotAngle + ROTATION_STEP;
angleDiff = ROTATION_STEP;
}
else if (angleDiff < -ROTATION_STEP)
{
nextAngle = robotAngle - ROTATION_STEP;
}
if(ABS(angleDiff) < FINISHED_ROTATION_THRESHOLD)
{
finished = true;
}
Pair currLoc;
extendPoint(robotLoc,
pivotPoint,
-(sp->general.PLAYER_RADIUS - PUSH_FACTOR),
currLoc);
rotateAboutPoint(currLoc,
pivotPoint,
nextAngle - robotAngle,
rotatedDestination);
destRot = angleBetween(rotatedDestination, pivotPoint);
command->setPos(rotatedDestination);
command->setRotation(destRot);
GUI_Record.debuggingInfo.setDebugPoint(robotID,1,ultimateDestination);
GUI_Record.debuggingInfo.setDebugPoint(robotID,2,robotLoc);
GUI_Record.debuggingInfo.setDebugPoint(robotID,3,rotatedDestination);
}