void catchBalls(int color)
{
set_servo_position(ASV, ASV_CATCH);
set_servo_position(SSV, SSV_GUARD);
msleep(1000);
set_servo_position(ASV, ASV_CHECK);
msleep(1100);
set_servo_position(SSV, SSV_BACK);
msleep(100);
if(check(color) == 1)
{
BALL_NUM_BOX++;
set_servo_position(ASV, ASV_BACK);
msleep(500);
findBall(color);
}
else if (check(PINK) == 1)
{
set_servo_position(SSV, SSV_PUSH);
msleep(500);
set_servo_position(SSV, SSV_BACK);
msleep(500);
set_servo_position(ASV, ASV_BACK);
msleep(500);
findBall(color);
}
else
{
set_servo_position(ASV, ASV_BACK);
msleep(500);
findBall(color);
}
}
int main()
{
prepare();
findBall(GREEN);
stop();
return 0;
}
int Board::computeHueristic()
{
//code to find value of huerstic
int m = 0;
for(int i = 0; i < m_balls.size(); i++)
{
m += abs(i+1 - findBall(m_balls[i]));
}
return floor((m+19)/20);
}
void catchBall(int color){
set_servo_position(ASV, ASV_CATCH);
set_servo_position(SSV, SSV_GUARD);
msleep(800);
// catch ball on ground
int count = 0;
for(count = 0; count < 4; count++){
motor(LM, 100);
motor(RM, 100);
msleep(100);
motor(LM, -100);
motor(RM, -100);
msleep(100);
}
// tremble ball into the claw
ao();
set_servo_position(ASV, ASV_CHECK);
msleep(600);
set_servo_position(SSV, SSV_BACK);
msleep(400);
// set ball before camera for check
if(check(color)){
// the ball is right
camera_update();
camera_update();
camera_update();
if(get_object_count(GREEN) > 0)
findBall();
// check if other balls emerge
}else
findBall();
// ball is elsewhere
}
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();
}
/* Função principal de localização da bola.
Encontra a região circular mais parecida com um circulo em 'frame', mais especificamente na região
'ROIat'.
Retorna o novo ROI em 'newROI' e a nova "bola" em 'newBall'.
*/
void trackBall(Mat &frame,const Rect &ROIat,const acmPoint ¤tBall, acmPoint &newBall, Rect &newROI,
int minr, int maxr, bool firstTime, bool filtraHistograma){
int deltaR=1;
int limMinR = 2;
double roiScale=4;
Rect roiRect;
Mat frameGray;
//Passa a imagem para tons de cinza.
cvtColor(frame, frameGray, CV_BGR2GRAY);
//Gaussian blur para remover ruídos e suavizar a imagem.
GaussianBlur(frameGray, frameGray, Size(5,5), 1.5, 1.5);
int limMaxR = min(frameGray.rows/2,frameGray.cols/2);
if(firstTime){ //Se for a primeira vez, definimos o ROI como a imagem toda.
roiRect = Rect(0,0,frameGray.cols,frameGray.rows);
}else{
roiRect = ROIat; //Se não, podemos utilizar o ROI anterior.
if(currentBall.inic){ //Se temos uma bola atualmente identificado,
//então vamos procupar por algo com um raio próximo
minr = max(limMinR,currentBall.rad-deltaR);
maxr = min(limMaxR,currentBall.rad+deltaR);
}
}
Mat roiImgGray = frameGray(roiRect);
Mat roiImg = frame(roiRect);
//Encontramos a nova "bola":
newBall = findBall(roiImg,roiImgGray,minr,maxr,filtraHistograma);
//Ajustando coordenadas da bola em razão do ROI utilizado.
newBall.cx += roiRect.x;
newBall.cy += roiRect.y;
Point center(newBall.cx,newBall.cy);
int rad = newBall.rad;
//Define novo ROI.
newROI = Rect(center.x - roiScale*rad -1, center.y - roiScale*rad -1,2*roiScale*rad + 1,2*roiScale*rad + 1);
newROI = newROI & Rect(0,0,frameGray.cols,frameGray.rows);
printf("vnorm = %lf\nvhist = %lf\nscore_final=%lf\nrad = %d\n",newBall.scoreCircular,newBall.scoreHistogram,
newBall.scoreFinal, newBall.rad);
}
void ImageProcessor::detectBall() {
int imageX, imageY;
if(!findBall(imageX, imageY)) return;
// function defined elsewhere that fills in imageX, imageY by reference
WorldObject* ball = &vblocks_.world_object->objects_[WO_BALL];
ball->imageCenterX = imageX;
ball->imageCenterY = imageY;
Position p = cmatrix_.getWorldPosition(imageX, imageY);
ball->visionBearing = cmatrix_.bearing(p);
ball->visionElevation = cmatrix_.elevation(p);
ball->visionDistance = cmatrix_.groundDistance(p);
ball->seen = true;
}
// getBalls:
void getBalls(){
moveToBalls();
findBall();
collectBall();
}// get a group of balls