void Robot::spin() {//S型过人 cv::Point2f robot_coord(x, y); std::vector<cv::Point2f> ans = findMulBall(); cv::Point2f ball1 = ans[0]; cv::Point2f ball2 = ans[1]; //printf("%f %f\n %f %f\n", ball1.x, ball1.y, ball2.x, ball2.y); float delta = 30; float r12 = cal_distance(ball1, ball2) / 2; float rspin = (delta * delta + r12 * r12) / (2 * delta); rspin += 10;//误差修正 float disr1 = cal_distance(ball1, robot_coord); int inrspin = 0; while(inrspin < rspin) inrspin++; //printf("rspin:%f, disr1:%f, inrspin:%d\n", rspin, disr1, inrspin); cv::Point2f rto1 = ball1 - robot_coord; cv::Point2f b1to2 = ball2 - ball1; float arc = asin(r12 / rspin); cv::Point2f pturn1(ball1.x - b1to2.x * 0.5, ball1.y - b1to2.y *0.5); moveTo(pturn1, 20); cv::Point2f robotPosition(x,y); cv::Point2f dir= ball1 - robotPosition; float dir_len = length(dir); if (dir_len <= 0) return; cv::Point2f new_dir = dir*(1/dir_len); rotateTo(new_dir); turnRight(arc * 180 / M_PI); moveRotate(true, rspin, arc * 2 + 0.2);//误差修正 moveRotate(false, rspin, arc * 2 + 0.2); }
void Robot::moveTo(const cv::Point2f& wCoord,float max_speed) { cv::Point2f robotPosition(x,y); cv::Point2f delta = wCoord - robotPosition; float delta_len = length(delta); if (delta_len <= 0) return; cv::Point2f new_dir = delta*(1/delta_len); rotateTo(new_dir); moveForward(delta_len,max_speed); }
void Robot::shoot() { findBall(); float radius = BALL_RADIUS + ROBOT_RADIUS + DELTA_RADIUS; cv::Point2f ball2goal = ownGoal_coord - ball_coord; float dist_ball2goal = sqrt(pow(ball2goal.x,2)+pow(ball2goal.y,2)); cv::Point2f prep2ball = ball2goal*(1/dist_ball2goal)*SHOOT_PREP_DIST; cv::Point2f shootPrepPosition = ball_coord - prep2ball; cv::Point2f targetPosition = ownGoal_coord - prep2ball*(TO_GOAL_DIST/float(SHOOT_PREP_DIST)); cv::Point2f robotPosition(x,y); cv::Point2f robot2prep = shootPrepPosition - robotPosition; float dist_robot2prep = sqrt(pow(robot2prep.x,2)+pow(robot2prep.y,2)); float dot = robot2prep.dot(prep2ball)/dist_robot2prep/SHOOT_PREP_DIST; dot = dot>1?1:(dot<-1?-1:dot); float dist = SHOOT_PREP_DIST*sin(acos(dot)); bool hidden = dot<0 && dist<radius && dist_robot2prep>SHOOT_PREP_DIST; cv::Point2f turningPoint; bool turn = false; if(hidden) { printf("rp = %f,%f\n",robotPosition.x,robotPosition.y); printf("bp = %f,%f\n",ball_coord.x,ball_coord.y); printf("tp = %f,%f\n",shootPrepPosition.x,shootPrepPosition.y); printf("r = %f\n",radius); turn = getTurningPoint(robotPosition,ball_coord,shootPrepPosition,turningPoint,radius); printf("tp = %f,%f\n",turningPoint.x,turningPoint.y); } shootRoute.push_back(robotPosition); if(turn) shootRoute.push_back(turningPoint); shootRoute.push_back(shootPrepPosition); shootRoute.push_back(targetPosition); updateRadar(); if(turn) { //getImage(); //adjustWorldCoordinate(image_r,1); moveTo(turningPoint,30); } //getImage(); //adjustWorldCoordinate(image_r,1); moveTo(shootPrepPosition,30); //getImage(); //adjustWorlfdCoordinate(image_r,1); moveTo(targetPosition,50); shootRoute.clear(); updateRadar(); }
void ObstacleCombinator::generateObstacleFromCurrentCluster(std::vector<ObstacleModel::Obstacle>& obstacles) { Vector2<int>& c = currentCluster.cells[0]; int xMin(c.x); int yMin(c.y); int xMax(c.x); int yMax(c.y); float rightAngle = 10.0; float leftAngle = -10.0; Vector2<int> rightCorner; Vector2<int> leftCorner; Vector2<> centerCells; Vector2<int> robotPosition(USObstacleGrid::GRID_LENGTH / 2, USObstacleGrid::GRID_LENGTH / 2); Vector2<int> closestPoint(USObstacleGrid::GRID_LENGTH, USObstacleGrid::GRID_LENGTH); int closestPointSqrDist(USObstacleGrid::GRID_SIZE * 2); for(unsigned int i = 0; i < currentCluster.cells.size(); ++i) { c = currentCluster.cells[i]; centerCells.x += c.x; centerCells.y += c.y; Vector2<int> point(c.x - robotPosition.x, c.y - robotPosition.y); int sqrDistToRobot = sqr(point.x) + sqr(point.y); float angleToRobot = atan2(static_cast<float>(point.y), static_cast<float>(point.x)); if(angleToRobot < rightAngle) { rightAngle = angleToRobot; rightCorner = Vector2<int>(c.x, c.y); } if(angleToRobot > leftAngle) { leftAngle = angleToRobot; leftCorner = Vector2<int>(c.x, c.y); } if(sqrDistToRobot < closestPointSqrDist) { closestPoint = c; closestPointSqrDist = sqrDistToRobot; } if(c.x < xMin) xMin = c.x; else if(c.x > xMax) xMax = c.x; if(c.y < yMin) yMin = c.y; else if(c.y > yMax) yMax = c.y; } centerCells /= static_cast<float>(currentCluster.cells.size()); const float angleToCenterPoint = Geometry::angleTo(Pose2D(USObstacleGrid::GRID_LENGTH / 2, USObstacleGrid::GRID_LENGTH / 2), centerCells); //calculates the angle to the center of the cluster in grid coordinate system (independent of robot rotation) const float cosinus = cos(-angleToCenterPoint); const float sinus = sin(-angleToCenterPoint); float newX; float newY; //initializing of the rectangle float xMinRotated(FLT_MAX); float yMinRotated(FLT_MAX); float xMaxRotated(-FLT_MAX); float yMaxRotated(-FLT_MAX); for(unsigned int i = 0; i < currentCluster.cells.size(); ++i) { newX = cosinus * currentCluster.cells[i].x - sinus * currentCluster.cells[i].y; // rotates each cell of the cluster newY = sinus * currentCluster.cells[i].x + cosinus * currentCluster.cells[i].y; //sets new values for rectangle if(newX < xMinRotated) xMinRotated = newX; else if(newX > xMaxRotated) xMaxRotated = newX; if(newY < yMinRotated) yMinRotated = newY; else if(newY > yMaxRotated) yMaxRotated = newY; } Vector2<> closestPointWorld = gridToWorld(Vector2<>(closestPoint.x + 0.5f, closestPoint.y + 0.5f)); Vector2<> centerWorld = gridToWorld(centerCells); //expansion (length of x- and y-axis through the center point) and orientation (dependent on robot rotation) of the cluster Vector3<> covarianceEllipse(((xMaxRotated - xMinRotated) * USObstacleGrid::CELL_SIZE) * parameters.covarianceFactor, ((yMaxRotated - yMinRotated) * USObstacleGrid::CELL_SIZE) * parameters.covarianceFactor, atan2(centerWorld.y, centerWorld.x)); Matrix2x2<> covariance(covarianceEllipse.x, 0, 0, covarianceEllipse.y); // covariance is initialised with uncorrelated values (only expansion of cluster as variances) rotateMatrix(covariance, covarianceEllipse.z); // rotates the covariance so that it fits to the orientation and expansion of the cluster obstacles.push_back(ObstacleModel::Obstacle(gridToWorld(Vector2<>((float)(leftCorner.x), (float)(leftCorner.y))), gridToWorld(Vector2<>((float)(rightCorner.x), (float)(rightCorner.y))), centerWorld, closestPointWorld, static_cast<int>(currentCluster.cells.size()), covariance)); }