// Find the width of a clear lane between the two locations passed as pairs. // Returns 0.0 if the lane is blocked by any other robot. float laneHalfWidth ( Pair passerLoc, // Position of the passing robot. Pair passDestination, // location of the pass destination const VisionData &field, // where everyone else is now const SystemParameters &rp,// contains necessary game parameters bool checkOurRobots) //to see if we check for our robots or not... { float minPassBuffer = 10000.0f; //Distance from pass line to nearest enemy robot float tempPassBuffer; //Used when comparing buffer from one robot to min buffer Pair closestPoint; //Point on pass line closest to robot bool onSegment; //true if closestPoint is on pass line segment Pair originalDest(passDestination.getX(),passDestination.getY()); //extend the pass destination point so an enemy a little behind will be considered float lineLength = dist(passerLoc,passDestination); passDestination.setX(passDestination.getX() + 2*rp.general.PLAYER_RADIUS*( (passDestination.getX() - passerLoc.getX()) / lineLength)); passDestination.setY(passDestination.getY() + 2*rp.general.PLAYER_RADIUS*( (passDestination.getY() - passerLoc.getY()) / lineLength)); //Check to make sure no enemy robots are in the way for(RobotIndex i=ROBOT0; i<NUM_ROBOTS; i++) { //If enemy robot is found, check to see if it is in the way if(theirRobotFound(i, field, rp)) { tempPassBuffer = distFromPointToSegment(passerLoc, passDestination, getTheirRobotLocation(i, field, rp), &closestPoint, onSegment)-rp.general.OPPONENT_RADIUS; //If enemy is on segment, return 0. Pass is occluded if(onSegment && tempPassBuffer <0.0f ) return 0.0f; //Replace the min pass buffer if smaller buffer is found. if (tempPassBuffer < minPassBuffer && onSegment) minPassBuffer = tempPassBuffer; } } //CODE TO CHECK OUR ROBOTS ALSO. REMEMBER TO IGNORE PASSER & RECEIVER if(checkOurRobots) { //Check to make sure no enemy robots are in the way for(RobotIndex i=ROBOT0; i<NUM_ROBOTS; i++) { //If our robot is found, and he is not the passer or receiver => check to see if it is in the way if(robotFound(i, field, rp)&& (dist(i,passerLoc,field,rp)>rp.general.PLAYER_RADIUS)&& (dist(i,originalDest,field,rp)>rp.general.PLAYER_RADIUS)) { tempPassBuffer = distFromPointToSegment(passerLoc, passDestination, getLocation(i, field, rp), &closestPoint, onSegment)- rp.general.PLAYER_RADIUS; //If robot is on segment, return 0. Pass is occluded if(onSegment && tempPassBuffer <0.0f) return 0.0f; //Replace the min pass buffer if smaller buffer is found. if (tempPassBuffer < minPassBuffer && onSegment) minPassBuffer = tempPassBuffer; } } } //Buffer = distance from pass line to edge (not center) of nearest enemy. float buf = minPassBuffer; if(buf <= 0.0f) return 0.001f; //to avoid divide-by-zero errors, and so dist>0 else return buf; }
//=============================================================== //=============================================================== //=============================================================== //=============================================================== void intersectFastMovingBall(const VisionData& field, const SystemParameters& params, float xValueOfDefendingLine, Destination* command) { Line defenderLine, ballVelocityVector; //------------------------------- defenderLine.setAposX( xValueOfDefendingLine ); defenderLine.setBposX( xValueOfDefendingLine ); defenderLine.setAposY( -200.0f); defenderLine.setAposY( 200.0f); //------------------------------- ballVelocityVector.setA( getRobocupBall(field).getPos() ); ballVelocityVector.setBposX( getRobocupBall(field).getXPos() + (5.0f*getRobocupBall(field).getXVel()) ); ballVelocityVector.setBposY( getRobocupBall(field).getYPos() + (5.0f*getRobocupBall(field).getYVel()) ); Pair intersection; //find the intersection point for where we can best intercept the ball along our line findLineIntersection(defenderLine, ballVelocityVector, &intersection); //if we cannot find an intersection, just match the ball's position if( (intersection.getX() == NEVER_INTERSECT) || (intersection.getY() == NEVER_INTERSECT) ) intersection.setY( getRobocupBall(field).getYPos() ); //always set our x to be the x line we sent in. intersection.setX( xValueOfDefendingLine ); //--------------------------------------------- //If the interception point is off the field, or the ball is moving slowly, //just follow the Y position of the ball if( offField(params, intersection) ) { if(intersection.getY() > (params.field.LEFT_SIDE_LINE - params.general.PLAYER_RADIUS)) intersection.setY( params.field.LEFT_SIDE_LINE - params.general.PLAYER_RADIUS ); if(intersection.getY() < (params.field.RIGHT_SIDE_LINE + params.general.PLAYER_RADIUS)) intersection.setY( params.field.RIGHT_SIDE_LINE + params.general.PLAYER_RADIUS ); } //actually set the desination for good =-) command->setPos( intersection ); //finally, set the rotation to look straight at the ball command->setRotation( angleToBall( command->getPos(), field) ); }
//===================================================== ///Execute the skill. This is the main part of the skill, where you tell the ///robot how to perform the skill. void CutGoalSkill::execute() { ///If not active, dont do anything! if(!initialized) { return; } else { //grab the ball location if(!presetBall){ ball = strategy->getCurrentRoboCupFrame()->getDefensiveBallLocation(); } ///Calculate the angle bisector of the area we want to cover float theta; float theta1=angleBetween(sp->field.OUR_GOAL_LINE,point1,ball.getX(),ball.getY()); float theta2=angleBetween(sp->field.OUR_GOAL_LINE,point2,ball.getX(),ball.getY()); theta=(theta1+theta2)/2.0f; theta=normalizeAngle(theta+PI); float halfAngle=ABS(angleDifference(theta1,theta2)/2.0f); //calculate midpoint to extend from Pair midpoint; midpoint.setY((sp->field.OUR_GOAL_LINE-ball.getX()) * TAN(theta) + ball.getY()); midpoint.setX(sp->field.OUR_GOAL_LINE); /*debugging helpful stuff GUI_Record.debuggingInfo.setDebugPoint(robotID,6,midpoint); GUI_Record.debuggingInfo.setDebugPoint(robotID,7,sp->field.OUR_GOAL_LINE,point1); GUI_Record.debuggingInfo.setDebugPoint(robotID,8,sp->field.OUR_GOAL_LINE,point2); Pair ang1(ball.getX()+.2f,ball.getY()); Pair ang2(ball.getX()+.2f,ball.getY()); rotateAboutPoint(ang1,ball,theta1,ang1); rotateAboutPoint(ang2,ball,theta2,ang2); GUI_Record.debuggingInfo.setDebugPoint(robotID,3,ang1); GUI_Record.debuggingInfo.setDebugPoint(robotID,4,ang2); Pair t(ball.getX()+.2f,ball.getY()); rotateAboutPoint(t,ball,theta,t); GUI_Record.debuggingInfo.setDebugPoint(robotID,5,t); */ // The ideal point we want the robot to be in this circumstances Pair dest; float distance = sp->general.PLAYER_RADIUS / SIN(ABS(halfAngle)) ; //char msg[80]; sprintf(msg,"dist: %5.2f",distance);GUI_Record.debuggingInfo.addDebugMessage(msg); extendPoint(midpoint,ball,-distance,dest); // We have to check if the destination point is between the Upper and lower limit // float slope = (midpoint.getY() - ball.getY()) / (midpoint.getX() - ball.getY()) ; // If it is above the limit if(dest.getX() > UPPER_X){ extrapolateForY(midpoint,ball,UPPER_X,dest); } // If it is below the limit if(dest.getX() < LOWER_X){ extrapolateForY(midpoint,ball,LOWER_X,dest); } command->setControl(OMNI_NORMAL); command->setPos(dest); command->setRotation(angleBetween(getLocation(robotID, *currentVisionData, *sp), ball)); strategy->getCurrentFrame()->setMessage(robotID,"Covering Goal"); } }