Ejemplo n.º 1
0
inline void get_pom()
{
	  
	while (1) // position to get the pom
	{
		blob_update();
		printf("(%d , %d) , (%d , %d)\n" , target.green.x , target.green.y , current.green.x , current.green.y);
		if ((current.green.y >= (target.green.y - TOL) && (current.green.y <= target.green.y + TOL)) && (current.green.x >= (target.green.x - TOL) && (			current.green.x <= target.green.x + TOL)))
		{
			ao();
			printf("IN POS\n");
			set_servo_position(arm_servo , ARM_DOWN);
			msleep(500);
			set_servo_position(arm_servo , ARM_OUT);
			msleep(500);
			set_servo_position(arm_servo , ARM_DOWN);
			msleep(500);
			set_servo_position(arm_servo , ARM_UP);
			msleep(500);
			break;
		}
		camera_move_y();
		msleep(10);
		camera_move_x();	
		msleep(10);
	}
}
Ejemplo n.º 2
0
inline int camera_move_y()
{ 	
	  
	int speed = 200;
	int back = -200;
	blob_update();
	if (current.green.y > (target.green.y - TOL))
	{
		printf("TOO CLOSE\n");
		mav(lego.left.port , back);
		mav(lego.right.port , back);
		return 1;
	}
	if (current.green.y < (target.green.y + TOL))
	{
		printf("TOO FAR\n");
		mav(lego.left.port , speed);
		mav(lego.right.port , speed);
		return 1;
	}
	if (current.green.y >= (target.green.y - TOL) && current.green.y <= (target.green.y + TOL))
	{
		printf("GOLDILOCKS\n");
		ao();
		return 0;
	}
}
Ejemplo n.º 3
0
inline int camera_move_x()
{
	  
	int lspeed = -100;
	int hspeed = 100;
	blob_update();
	printf("MOVING X\n");
	if (current.green.x < (target.green.x + TOL))
	{
		printf("LEFT\n");
		mav(lego.left.port , lspeed);
		mav(lego.right.port , hspeed);
		return 1;
	}
	if (current.green.x > (target.green.x - TOL))
	{
		printf("RIGHT\n");
		mav(lego.left.port , hspeed);
		mav(lego.right.port , lspeed);
		return 1;
	}
	if (current.green.x >= (target.green.x - TOL) && current.green.x <= (target.green.x + TOL))
	{
		ao();
		return 0;
	}
}
Ejemplo n.º 4
0
int extract_image(void* user_struct)
{
    struct stream_state stream;
    struct blob_list blist;
    struct blob* blob_now = NULL;
    struct blob* blob_prev = NULL;

    if (init_pixel_stream(user_struct, &stream))
        {printf("init malloc error!\n"); return 1;}
    if (stream.row == NULL)
        {printf("row malloc error!\n"); return 1;}
    blist.length = stream.w + 5;
    if (malloc_blobs(&blist))
        {printf("blob malloc error!\n"); return 1;}

    while (!next_frame(user_struct, &stream))
    {
        init_blobs(&blist);
        while (!next_row(user_struct, &stream))
        {
            blob_prev = blist.head->next;
            while (!stream.wrap)
            {
                blob_now = empty_blob(&blist);
                if (scan_segment(&stream, blob_now))
                    {blob_reap(&blist, blob_now); continue;}
                blob_update(blob_now, blob_now->x1, blob_now->x2, stream.y);
                // update structure
                sib_find(blist.head->next, blob_now);
                blob_insert(blob_prev, blob_now);
                flush_incremental(user_struct, &blist, blob_now);
                blob_prev = blob_now;
            }
            flush_old_blobs(user_struct, &blist, stream.y);
            //show_status(blist.head, &stream);
            //show_dead_sibs(blist.head);
            //show_blobs(blist.head);
            //printf("----------\n");
        }
        flush_old_blobs(user_struct, &blist, stream.h - 1);
    }

    close_pixel_stream(user_struct, &stream);
    free(blist.head);
    free(blist.empties);
    blist.head = NULL;
    blist.empties = NULL;
    return 0; 
}
Ejemplo n.º 5
0
void pom_push()
{
	msleep(100);
	set_servo_position(arm_servo , ARM_DUMP);
	msleep(500);
	set_servo_position(push_servo , P_UP);
	msleep(500);
	set_servo_position(push_servo , P_DOWN);
	msleep(500);
	set_servo_position(push_servo , P_UP);
	msleep(500);
	set_servo_position(push_servo , P_DOWN);
	msleep(500);
	set_servo_position(push_servo , P_UP);
	msleep(500);
	set_servo_position(push_servo , P_DOWN);
	msleep(500);
	ao();
	blob_update();
}
Ejemplo n.º 6
0
int main()
{
	msleep(2500);
	set_analog_pullup(ET_s , 0); 
	extra_buttons_show(1); // show three extra buttons
	set_a_button_text("COORDS"); // set the text of various buttons
	set_b_button_text("POM SIZE");
	set_c_button_text("BOTGUY SIZE");
	set_x_button_text("CUBE SIZE"); 

	lego.left.port = 0; // set motor ports
	lego.right.port = 2;
	
	camera_open(LOW_RES);
	camera_update();
	while (a_button() == 0) // press the a button to set the coordinates
	{
		camera_update();
		target.green.x = get_object_center(0 , 0).x; // sets target coordinates (x)
		target.green.y = get_object_center(0 , 0).y; // sets target coordinates (y)
		printf("(%d , %d)\n" , target.green.x , target.green.y);
		msleep(10);
	}
	while (b_button() == 0)
	{
		camera_update();
		target.green.size = get_object_area(0 , 0);
		target.orange.size = get_object_area(1 , 0);
		printf("Orange Size = %d" , target.orange.size);
		printf(" Green Size = %d\n" , target.green.size);
	}
	enable_servo(arm_servo); 
	enable_servo(push_servo);
	enable_servo(basket_servo);
	set_servo_position(arm_servo , ARM_UP);
	set_servo_position(push_servo , P_DOWN);
	set_servo_position(basket_servo , B_UP);
	printf("(%d , %d)\n" , target.green.x , target.green.y);
	while(a_button() == 0)
	{
		printf("%d , %d , %d\n" , get_left() , get_middle() , get_right());
	}
	while (1) // line follow until poms are seen
	{
		blob_update();
		t_line_follow();
		if (current.orange.size > target.orange.size && current.green.size > target.green.size)
			break;
	}
	blob_update(); 
	get_pom(); // pick up a pom
	pom_push(); // push it into the basket
	while (1) // turn to next pom
	{
		blob_update();
		mav(lego.left.port , 300); 
		mav(lego.right.port , -300);
		msleep(10);
		if (current.green.size > target.green.size)
			break;
	}
	blob_update();
	get_pom(); // pick up pom
	pom_push(); // push it into the basket
	avoid_cubeguy(); // avoid the cube or botguy
	avoid_booster();
	while (1)
	{
		blob_update();
		t_line_follow();
		if (current.orange.size > target.orange.size && current.green.size > target.green.size)
			break;
	}
	blob_update();
	pom_push();
	while (1)
	{
		blob_update();
		mav(lego.left.port , 300);
		mav(lego.right.port , -300);
		msleep(10);
		if (current.green.size > target.green.size)
			break;
	}
	blob_update();
	get_pom();
	pom_push();
	avoid_cubeguy();
	int start_time = seconds();
	int t;
	while (1)
	{
		t_line_follow();
		if (seconds() < start_time + t)
			break;
	}
	while (1)
	{

	}
}