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