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));
}
