Beispiel #1
0
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);
	}
}
Beispiel #2
0
int main()
{
	prepare();
	findBall(GREEN);
	stop();
	
	return 0;
}
Beispiel #3
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);
	}
Beispiel #4
0
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
}
Beispiel #5
0
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();
}
Beispiel #6
0
/* 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 &currentBall, 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);
}
Beispiel #7
0
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;
}
Beispiel #8
0
// getBalls:
void getBalls(){
	moveToBalls();
	findBall();
	collectBall();
}// get a group of balls