示例#1
0
void sort_balls(void) {
    start_spinner();
    
    while (!side_button()) {
        msleep(50);
        camera_update(); camera_update();
        if (get_object_count(kCameraChannelOrange) > 0 && get_object_confidence(kCameraChannelOrange, 0) >= 0.95 && get_object_area(kCameraChannelOrange, 0) > 500) {
            stop_spinner();
            set_servo_position(kServoPortSorter, kServoPositionSorterOrange);
            msleep(200);
            set_servo_position(kServoPortSorter, kServoPositionSorterCenter);
            msleep(200);
            start_spinner();
        } else if (get_object_count(kCameraChannelGreen) > 0  && get_object_confidence(kCameraChannelGreen, 0) >= 0.95 && get_object_area(kCameraChannelGreen, 0) > 500) {
            stop_spinner();
            set_servo_position(kServoPortSorter, kServoPositionSorterGreen);
            msleep(200);
            set_servo_position(kServoPortSorter, kServoPositionSorterCenter);
            msleep(200);
            start_spinner();
        }
    }
    
    alloff();
}
示例#2
0
/**
 * igt_get_stable_obj_count:
 * @driver: fd to drm/i915 GEM driver
 *
 * This puts the driver into a stable (quiescent) state and then returns the
 * current number of gem buffer objects as reported in the i915_gem_objects
 * debugFS interface.
 */
int igt_get_stable_obj_count(int driver)
{
	int obj_count;
	gem_quiescent_gpu(driver);
	obj_count = get_object_count();
	/* The test relies on the system being in the same state before and
	 * after the test so any difference in the object count is a result of
	 * leaks during the test. gem_quiescent_gpu() mostly achieves this but
	 * on android occasionally obj_count can still change briefly.
	 * The loop ensures obj_count has remained stable over several checks
	 */
#ifdef ANDROID
	{
		int loop_count = 0;
		int prev_obj_count = obj_count;
		while (loop_count < 4) {
			usleep(200000);
			gem_quiescent_gpu(driver);
			obj_count = get_object_count();
			if (obj_count == prev_obj_count) {
				loop_count++;
			} else {
				igt_debug("loop_count=%d, obj_count=%d, prev_obj_count=%d\n",
					loop_count, obj_count, prev_obj_count);
				loop_count = 0;
				prev_obj_count = obj_count;
			}

		}
	}
#endif
	return obj_count;
}
示例#3
0
//routine to use the camera and see blob position based on standard create start routine
void calibrateCamera() {
	int channel = 0;
	while (a_button_clicked() == 0) {
		msleep(2000);
		camera_update();
		printf ("=> object count %d \n", get_object_count(channel));
		int i = 0; 
		int shelfPlaces[8] = {0,0,0,0,0,0,0,0};
		while (i < get_object_count(channel)) {
			printf("obj %d, ctr loc, area, shelf plcmt: %d, %d, %d\n", i, get_object_center(channel, i).x, get_object_area(channel, i), 
			getShelfPlacement(get_object_center(channel, i).x));
			shelfPlaces[getShelfPlacement(get_object_center(channel, i).x)] += get_object_area(channel, i);
			i++;
		}
		printf("==========================\n");
		printf("=RESULTS                 =\n");
		printf("==========================\n");
		i = 1; 
		while (i < 8) {
			printf ("shelfPlace : %d, total cube area : %d\n", i, shelfPlaces[i]);
			i++;
		}
		printf("==========================\n");
		printf("click b button for new camera capture, a to exit.\n");
		while (b_button_clicked() == 0 && a_button_clicked() == 0 )  {
			msleep(25); 
		}
	}
	printf("done with calibration function.\n");
	
}
示例#4
0
static void test_reimport_close_race(void)
{
	pthread_t *threads;
	int r, i, num_threads;
	int fds[2];
	int obj_count;
	void *status;
	uint32_t handle;
	int fake;

	/* Allocate exit handler fds in here so that we dont screw
	 * up the counts */
	fake = drm_open_driver(DRIVER_INTEL);

	gem_quiescent_gpu(fake);
	obj_count = get_object_count();

	num_threads = sysconf(_SC_NPROCESSORS_ONLN);

	threads = calloc(num_threads, sizeof(pthread_t));

	fds[0] = drm_open_driver(DRIVER_INTEL);

	handle = gem_create(fds[0], BO_SIZE);

	fds[1] = prime_handle_to_fd(fds[0], handle);

	for (i = 0; i < num_threads; i++) {
		r = pthread_create(&threads[i], NULL,
				   thread_fn_reimport_vs_close,
				   (void *)(uintptr_t)fds);
		igt_assert_eq(r, 0);
	}

	sleep(5);

	pls_die = 1;

	for (i = 0;  i < num_threads; i++) {
		pthread_join(threads[i], &status);
		igt_assert(status == 0);
	}

	close(fds[0]);
	close(fds[1]);

	gem_quiescent_gpu(fake);
	obj_count = get_object_count() - obj_count;

	igt_info("leaked %i objects\n", obj_count);

	close(fake);

	igt_assert_eq(obj_count, 0);
}
示例#5
0
void findBall(int color)
{
	camera_update();
	display_clear();
	int count = 1;
	int objNum = get_object_count(color);
	while(objNum == 0)
	{
		camera_update();
		objNum = get_object_count(color);
	}
	printf("%d time finding a ball", count);
	point2 objCen = get_object_center(color, 0);
	int errorX = 0, errorY = 0;
	errorX = OFFSET_X - objCen.x;
	errorY = OFFSET_Y - objCen.y;
	while(!(a_button_clicked()))
	{
		if(BALL_NUM_BOX >= 2)
			break;
		if(errorX > -4 && errorX < 4 && errorY > -4 && errorY < 4)
			break;
		int turnLM = -1 * errorX * P_X + errorY * P_Y;
		int turnRM = errorX * P_X + errorY * P_Y;
		if(turnLM > -1 * MINS && turnLM < 0)
			turnLM = -1 * MINS;
		if(turnLM > 0 && turnLM < MINS)
			turnLM = MINS;
		if(turnRM > -1 * MINS && turnRM < 0)
			turnRM = -1 * MINS;
		if(turnRM > 0 && turnRM < MINS)
			turnRM = MINS;
		motor(LM, turnLM);
		motor(RM, turnRM);
		camera_update();
		int objArea = get_object_area(color, 0);
		while(objArea < 200)
		{
			ao();
			camera_update();
			objArea = get_object_area(color, 0);
		}
		count++;
		display_clear();
		printf("%d time finding a ball   ", count);
		printf("%d NUM", BALL_NUM_BOX);
		objCen = get_object_center(color, 0);
		errorX = OFFSET_X - objCen.x;
		errorY = OFFSET_Y - objCen.y;
	}
	ao();
	catchBalls(color);
	
}
示例#6
0
void moveToBalls(){
	camera_update();
	camera_update();
	camera_update();
	camera_update();
	int objNum = get_object_count(GREEN);
	
	while(objNum == 0){
		motor(LM, 83);
		motor(RM, 100);
		camera_update();
		objNum = get_object_count(GREEN);
	}
	
	msleep(250);
}// move forward; stop when sees a target ball
示例#7
0
int check_stopping_event(int stopping_event) {
	if (stopping_event == STOP_BY_ET) {
		if (analog_et(ET_SENSOR) < 300) {
			return 1;
		}
	} else if (stopping_event == STOP_BY_TOPHAT) {
		display_printf(0, 0, "%4i", analog(L_TOPHAT));
		if (analog(L_TOPHAT) > 750 || analog(R_TOPHAT) > 750) {
			return 1;
		}
	} else if (stopping_event == STOP_BY_CAMERA) {
		camera_update();
		if (get_object_count(0) < 1) {
			display_printf(0, 0, "No Object Found");
			return 0;
		}
		if (get_object_area(0, 0) > PINGPONG_THRESHOLD) {
			display_clear();
			display_printf(0, 0, "Object Seen!");
			if (get_object_center(0, 0).x > 90 && get_object_center(0, 0).x < 110) {
				display_printf(0, 1, "Object centered");
				return 1;
			}
		}
	} else {
		printf("Stopping type is not defined!\n");
		return -1;
	}
	return 0;
}
示例#8
0
文件: pink.c 项目: 314piman/botball
int find_item()
{
	camera_update();
	point2 mcenter = get_object_center(1,0);
	set_servo_position (0 ,400);
	while(get_object_count(1) == 0){
		camera_update();
		create_spin_CCW(25);
	}
	create_stop();
	sleep(1);
	create_spin_CW(15);
	msleep(100);
	while(mcenter.x < 70 || mcenter.x > 100){
		camera_update();
		mcenter = get_object_center(1,0);
		if(mcenter.x < 70){
			create_spin_CCW(15);
			camera_update();
			mcenter = get_object_center(1,0);
		}
		if(mcenter.x < 100){
			create_spin_CW(15);
			camera_update();
			mcenter = get_object_center(1,0);
		}
		if(mcenter.x > 70 && mcenter.x < 100){
			create_stop();
		}
	}
	beep();
	
}
void cube_is_near(){
	camera_update();
	msleep(500);
	camera_update();
	enable_servos();
	claw_open();
	printf("cube is near");
	freeze(Motor_Left);
	freeze(Motor_Right);

	if(get_object_count(0)!=0){
		printf("gleich gefunden\n");
		if(get_object_bbox(0,0).width > 20){
			printf("groß genug");
			funden=1;
			found_something();
		}
	}
	double s=seconds();
	motor(Motor_Right,30);
	while(funden==0){//nichts gefunden
		camera_update();
		printf("nach rechts\n");
			if(get_object_count!=0){
				printf("1 gefunden \n");
				if(get_object_bbox(0,0).width > 20){
					printf("groß genug");
					found_something();
					funden=1;
					break;
				}
			}
			if(seconds()>s+2){
				printf("zeit rechts\n");
				freeze(Motor_Right);
				break;
			}
		}
		s=seconds();
		motor(Motor_Left,30);
		while(funden==0){
			camera_update();
			printf("nach links drehen \n");
			if(get_object_count!=0){
				printf("1 gefunden \n");
				if(get_object_bbox(0,0).width > 20){
					printf("groß genug");
					found_something();
					funden=1;
					break;
				}
			}
			if(seconds()>s+4){
				printf("zeit links\n");
				freeze(Motor_Left);
				break;
			}
		}
	}
示例#10
0
int getNumVisiblePingPongBalls(void)
{
	//camera_open();
	camera_update();
	int count = get_object_count(PING_PONG_BALL_CHANNEL_RED);
	//camera_close();
	return count;
	
}
示例#11
0
int main()
{
	printf("Hello, World!\n");
	camera_open(LOW_RES);
	press_A_to_continue();
	
	printf("Number of objects on channel 1: %i\n", get_object_count(1));
	printf("Object data length on channel 1: %i\n", get_object_data_length(1, 0));
	return 0;
}
示例#12
0
void moveToBalls(){
	camera_update();
	camera_update();
	camera_update();
	camera_update();
	int objNum = get_object_count(GREEN);
	
	while(objNum == 0){
		motor(LM, LM_SP_STR1);
		motor(RM, RM_SP_STR1);
		camera_update();
		objNum = get_object_count(GREEN);
	}
	mrp(LM, -1 * LM_SP_STR2, 120);
	mrp(RM, -1 * RM_SP_STR2, 120);
	bmd(LM);
	bmd(RM);
	ao();
	//msleep(250);
}// move forward; stop when sees a target ball
int main()
{
	       																				  //while the a button isn't pressed
		int x, y, color=0;             																					    //sets all variables to 0
		
		camera_open(LOW_RES);
	while(side_button()==0)
	{
		
	        																				 //sets camera to lowest resolution for minimal lag
		
																												//camera takes a picture
		
		
		
		if(get_object_count(color) > 0)	    																		// If the right color is detected, run this code
		
		{
			x = get_object_center(color,0).x;
			y = get_object_center(color,0).y;    																		//x and y variables now represent x and y coordinates of the biggest color blob
			
			if(x>80)     																										//if the x coordinate is on the right
			
			{
				
				printf("The biggest blob is on the right and the coordinates are (%d,%d)", x ,y);		//prints the words
				
			}
			if(x<80)                                                                                                          	 //if x is on th left 
			
			{
				printf("The biggest blob is on the left and the coordinates are (%d,%d)" ,  x ,y);		//print the words
			
			}
			else
			{
				printf("no object in sight");
				
				
				
			}
			camera_update();
		}
		
	}
	
}
示例#14
0
int main(){
	camera_open(LOW_RES);
	camera_load_config("color.config");
	while(!(a_button_clicked())){
		display_clear();
		camera_update();
		int objNum = get_object_count(GREEN);
		if(objNum == 0)
			printf("NO SUCH OBJECT!!!!!!!!");
		else if(objNum > 0){
			point2 objCen = get_object_center(GREEN, 0);
			int objArea = get_object_area(GREEN, 0);
			printf("X:\t%d\nY:\t%d\n", objCen.x, objCen.y);
			printf("Area:\t%d\n", objArea);
		}
		msleep(100);
	}
	return 0;
}
示例#15
0
int main(){
   armUp();
	clawOpen();
   camera_open(LOW_RES);
   double start_wait=seconds();
   while((seconds()-start_wait)<=25){//wait for poms or for 25 seconds
       int i=0;
       while(i<10){//picks latest image from the buffer
	   camera_update();
	   i++;
       }
       if(get_object_count(chan)>0){
           break;
       }else{
           stop(0.1);
       }
   }
   while((seconds()-start_wait)<=60){
	   int area=get_object_area(chan, 0);
	   if(area>=600){
		   int i=0;
		   while(i<10){
			   camera_update();
			   i++;
		   }
		   int x=get_object_center(chan, 0).x;
		   if(x<65){
			   rightF(0.1, 100, 80);
		   }else{
			   leftF(0.1, 100, 80);
		   }
       }else{
		   break;
	   }
   }
   armDown();
   clawClose();
   armUp();
   
}
示例#16
0
void main()
{
	calibrate();
	set_servo_position(CAM_SERVO,CAM_SERVO_POSITION_RIGHT);
	camera_open(LOW_RES);
	enable_servos();
	set_analog_pullup(3,0);
	while(!b_button()){
		 claw_at_linefollowing();
		do {
			camera_update(); 
			linefolowing();
		} while(get_object_count(green) ==0); //linefolowing bis er was sieht
		
		if(get_object_bbox(0,0).width>BLOB_SIZE_TAKE&&get_object_bbox(0,0).height>BLOB_SIZE_TAKE){
				takepom();
		}
		printf("kompletdurch \n");
	}
	disable_servos();
	stop();
}
示例#17
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
}
int findValidObject(){ //find an object for the purpose of tracking. Should not move.
	int channel = 1 //insert channel here
	int waitTime = 10000 //time robot waits for object to move
	int validTargets = 0 //number of valid targets
	camera_open();
	camera_update();
	count = get_object_count(channel);
	/*point2 obj_pos_before[count];  //check point2
	point2 obj_pos_after[count];
	for (i=0; i<count; i++){
		obj_pos_before[i] = get_object_center(channel, i);
	}
	msleep(waitTime)
	for (i=0; i<count; i++){
		point2 obj_pos_after[i] = get_obj_center(channel, i);
		if(obj_pos_before[i].x = obj_pos_after.x && obj_pos_before[i].y = obj_pos_after[i].y){
			printf("Object %d is a valid target\n", i)
			validTargets = validTargets + 1
		}
		if(validTargets = 0){
			printf("No valid targets")
		}
	}*/
}
示例#19
0
int main()
{
	printf("version 1.9.5\n");
	shut_down_in(0.010*1000);
	int start=seconds();//starting time for two minutes
	enable_servo(0);
	set_servo_position(0,2047);
	straight(1.8,  170);//gets in front of transport
	rightC(0.28, 100);
	leftC(0.28, -100);
	straight(1.65, 170);
	leftC(0.44, 100);
	printf("completed first dead reckoning\n");
	set_analog_pullup(ETport,  0);
	
	//until distance is 5 centimeter
	/*
	int claw_threshold = 13;
	while(analog10(ETport)>claw_threshold){
		back(0.1, -10);
		msleep(1000);
		printf("move once\n");
		printf("%i\n", analog10(ETport));
	}*/
	back(0.25, -20);//pushes against transport
	msleep(2000);
	set_servo_position(0, 500);//puts claw on transport
	msleep(1000);
	printf("done with attaching to transport\n");
	while(digital(8)==0)//goes forward with transport until it hits the wall
	{
		straight(0.1, 100);
		msleep(10);
	}
	printf("at wall\n");
	back(0.25, -50);//backs  up from wall to deposit transport
	set_servo_position(0, 2047);//raises claw
	msleep(1500);
	while(digital(8)==0){//goes forward until it hits wall for second time
		straight(0.1, 100);
		msleep(10);
	}
	
	printf("at wall 2\n");
	
	back(0.35, -100);//goes back and turns left in order to be oriented with left inner wall
	leftC(0.6, 142);
	
	int bump_counter=0;	
	
	while(digital(8)==0){//hits side wall
		straight(0.1, 100);
		msleep(10);
	}
	
	bump_counter++;//has one bump


    int i=0;
    while(i<10){//if hits side wall, pause
		stop(0.2);
		i++;
	}
	
	back(0.25, -100);
	leftC(0.46, 100);
	
	camera_open(LOW_RES);
	
	armUp();
	clawOpen();
   double start_wait=seconds();
   while((seconds()-start_wait)<=25){//wait for poms or for 25 seconds
       int i=0;
       while(i<10){//picks latest image from the buffer
	   camera_update();
	   i++;
       }
       if(get_object_count(chan)>0){//if camera sees objects, skip the 25 seconds and go onto picking up stuff
           break;
       }else{//if camera doesn't see any objects, keep waiting until 25 seconds is up
           stop(0.1);
       }
   }
   int x=get_object_center(chan, 0).x;//declares global unchanging variable for the x location of the largest object
   while((seconds()-start)<=120){//while there is still time left
	   int area=get_object_area(chan, 0);//creates local changing variable. this is th area of the largest object camera sees
	   if(area<=600){//if the object is small enough(far enough), navigate towards object. 600 is threshold
		   int i=0;
		   while(i<10){//buffer updating
			   camera_update();
			   i++;
		   }
		  
		   navigate_poms(x);//similar to line followig function; gets to poms.
		 
      
	   }
	   
	   armDown();
	   clawClose();
	   armUp();
	   
	   /*
	   leftF(0.5, 100, 80);//turn and go forward until transport is contacted
	   while(digital(8)==0){
	   straight(0.1, 100);*/
   
   }

   

	/*
	back(1.5, 100);
	leftC(0.44, 100);
	
	while(digital(8)==0 && digital(9)==1){
		straight(0.1, 100);
		msleep(10);
			if(digital(9)==0){
				rightF(0.1, 100, 40);
				left(0.1, 100);
				msleep(10);
			}
	}
	straight(2, 200);
	rightC(0.44, 100);
	straight(1, 80);
	rightC(0.92, 100);
	while(analog10(ETport<=600)){
			msleep(10);
	}
	msleep(10000);
	straight(1, 80);
	leftC(0.44, 100);
	straight(2, 200);
	*/
	
	return 0;
}
示例#20
0
void findBall(){
	display_clear();
	int temptLMSpeed = LM_MINS;			// changeable minimum speed for left motor
	int temptRMSpeed = RM_MINS;			// changeable minimum speed for right morot
	int lastLMSpeed, lastRMSpeed;	// motor speed
	int lastLMPos, lastRMPos;		// motor positions
	int errorX = 0, errorY = 0;		// difference of current ball coordinate and target coordinate
	int objArea;					// area of the largest object in sight
	point2 objCen; 					// object center coordinate
	
	camera_update();
	int objNum = get_object_count(GREEN);
	while(objNum == 0){
		camera_update();
		objNum = get_object_count(GREEN);
	}
	// find a ball in sight
	
	objCen = get_object_center(GREEN, 0);
	errorX = OFFSET_X - objCen.x;
	errorY = OFFSET_Y - objCen.y;
	
	while(!(a_button_clicked())){
		camera_update();
		objArea = get_object_area(GREEN, 0);
		while(objArea < 200){
			ao();
			camera_update();
			objArea = get_object_area(GREEN, 0);
		}
		// make sure really a ball is in sight
		
		objCen = get_object_center(GREEN, 0);
		errorX = OFFSET_X - objCen.x;
		errorY = OFFSET_Y - objCen.y;
		// find differences between coordinates
		
		if(errorX > -3 && errorX < 2 && errorY > -3 && errorY < 2)
			break;
		int turnLM = -1 * errorX * P_X + errorY * P_Y;
		int turnRM = errorX * P_X + errorY * P_Y;
		// turn for LM, RM
		
		if(turnLM == lastLMSpeed && lastLMPos == get_motor_position_counter(LM))
			temptLMSpeed = temptLMSpeed + 1;
		if(turnLM == lastLMSpeed && lastLMPos != get_motor_position_counter(LM))
			temptLMSpeed = temptLMSpeed - 1;
		if(turnLM == -1 * lastLMSpeed && lastLMPos != get_motor_position_counter(LM))
			temptLMSpeed = temptLMSpeed - 2;
		if(turnRM == lastRMSpeed && lastRMPos == get_motor_position_counter(RM))
			temptRMSpeed = temptRMSpeed + 1;
		if(turnRM == lastRMSpeed && lastRMPos != get_motor_position_counter(RM))
			temptRMSpeed = temptRMSpeed - 1;
		if(turnRM == -1 * lastRMSpeed && lastRMPos != get_motor_position_counter(RM))
			temptRMSpeed = temptRMSpeed - 2;
		lastLMSpeed = turnLM;
		lastLMPos = get_motor_position_counter(LM);
		lastRMSpeed = turnRM;
		lastRMPos = get_motor_position_counter(RM);
		// check if speed is too high or low, thus make modification to minimum speed
		
		if(turnLM > -1 * temptLMSpeed && turnLM < 0)
			turnLM = -1 * temptLMSpeed;
		if(turnLM > 0 && turnLM < temptLMSpeed)
			turnLM = temptLMSpeed;
		if(turnRM > -1 * temptRMSpeed && turnRM < 0)
			turnRM = -1 * temptRMSpeed;
		if(turnRM > 0 && turnRM < temptRMSpeed)
			turnRM = temptRMSpeed;
		// set minimum speed: avoid unmoving
		
		motor(LM, turnLM);
		motor(RM, turnRM);
		// move
	}
	
	ao();
	catchBall(GREEN);
	// stop and catch
}
示例#21
0
int main()
{
	// INITIALIZE
	
	int x, y, color=0, pink=1;  // set up for color channel 0 (green)
	int xvalue, yvalue;
	int greencentered = 0;
	int loop = 0;
	int centerx=77; // calibrate these number and enter manually
	int centery=63;
	int margin=8;
	int deltax = 0;
	int deltay = 0;
	int stepsizex;
	int stepsizey;
	int currpos;
	int s_time;
	
	light_start(L_SENSOR);				// light start
	shut_down_in(119);
	camera_open(LOW_RES);
	enable_servos();						// enable servos
	set_servo_position(S_CATCHER,S_DOWN);
	msleep(500);
	set_servo_position(S_GATE,S_OPEN);
	msleep(500);
	
	// TRIBBLE PILE 1
	
	//set_servo_position(S_GATE,S_OPEN);		// open gate
	msleep(5000);
	set_servo_position(S_CATCHER,CATCHER_UP);
	right(28,ks/2);									// right
	f_until_black(TOPHAT_RIGHT/*,TOPHAT_LEFT*/);	// forward until right sensor sees black]
	forward(2);
	//printf("see black! time to close the gate\n");
	set_servo_position(S_GATE,S_GAP);				// close gate
	//f_until_white(TOPHAT_RIGHT);
	set_servo_position(CATCHERARM, CATCHER_UP);
	
	// camera sort

	s_time = curr_time();
	while((curr_time()-s_time) < 14)
	{
		while (greencentered == 0)
		{
			camera_update(); // process the most recent image
			camera_update(); // process the most recent image
			camera_update(); // process the most recent image
			camera_update(); // process the most recent image
			camera_update(); // process the most recent image
			loop = loop + 1;
			printf("Update camera, loop %d\n", loop);
			//msleep(10);
			if (get_object_count(color) > 0)
			{
				xvalue = get_object_center(color,0).x;
				yvalue = get_object_center(color,0).y;
				deltax = abs(xvalue-centerx);
				deltay = abs(yvalue-centery);
				stepsizex = (deltax>10) ? 50: 20;
				stepsizey = (deltay>10) ? 50: 20;
				printf("x is %d, y is %d\n", xvalue, yvalue);
				greencentered = ((xvalue >= centerx-margin) & (xvalue <= centerx+margin)) && ((yvalue >= centery-margin) & (yvalue <= centery+margin));
				if(greencentered == 1)//get x, y for the biggest blob the channel sees		
				{	
					printf("Biggest blob at (%d,%d)\n",xvalue,yvalue);
					msleep(300);
					set_servo_position(CATCHERARM, CATCHER_MIDWAY);
					msleep(300);
					set_servo_position(CATCHERARM, CATCHER_DOWN);
					//slow_servo(CATCHERARM,20,40,CATCHER_UP,CATCHER_DOWN);
					msleep(300);
					currpos=get_servo_position(CATCHERARM);
					printf("%d\n",currpos);
					/*if(CATCHER_MIDWAY>=currpos>CATCHER_DOWN)
					{	
					currpos=get_servo_position(CATCHERARM);
					set_servo_position(CATCHERARM, currpos-INCREMENT);
					msleep(SLEEP_INCREMENT);
					currpos=get_servo_position(CATCHERARM);
					}
					if(currpos<CATCHER_MIDWAY)
					{	
					currpos=get_servo_position(CATCHERARM);
					set_servo_position(CATCHERARM, currpos+INCREMENT);
					msleep(SLEEP_INCREMENT);
					currpos=get_servo_position(CATCHERARM);
					}*/
					set_servo_position(CATCHERARM,CATCHER_UP);
					msleep(500);
				}
				else
				{
					if(xvalue <= centerx+margin) //moves right if senses value greater than 80
					{
						motor(MOT_RIGHT,70);
						motor(MOT_LEFT,-70);
						msleep(stepsizex);
						motor(MOT_RIGHT,0);
						motor(MOT_LEFT,0);
						//msleep(10);
						printf("blob is too right\n");
					}	
					if(xvalue >= centerx-margin) // moves left if senses value less than 70
					{
						motor(MOT_LEFT,70);
						motor(MOT_RIGHT,-70);
						msleep(stepsizex);	
						motor(MOT_LEFT,0);
						motor(MOT_RIGHT,0);
						//msleep(10);
						printf("blob is too left\n");
					}
					
					if ((xvalue > 67 /*centerx-margin*/) && (xvalue < 87 /*centerx+margin*/))
					{
						if(yvalue > centery+margin) //moves forward if senses value greater than 43
						{
							motor(MOT_LEFT,-50);
							motor(MOT_RIGHT,-50);
							msleep(stepsizey);
							motor(MOT_LEFT,0);
							motor(MOT_RIGHT,0);
							//msleep(10);
							printf("blob is too close\n");
						}
						
						if(yvalue < centery-margin) // moves backwar if senses value less than 38
						{
							motor(MOT_LEFT,50);
							motor(MOT_RIGHT,50);
							msleep(stepsizey);
							motor(MOT_LEFT,0);
							motor(MOT_RIGHT,0);
							//msleep(10);
							printf("blob is too far\n");
						}
					}
				}
			}
		}
	}
	set_servo_position(S_GATE,S_CLOSE);	
	while (analog10(2)<BLACK_SEN_THRESH)
	{
		motor(MOT_LEFT,-100);
	}
	
	// SCORE PILE 1
	
	left(5,0);
	//backward(50);
	touch_back(TOUCH_SEN);
	forward(10);
	backward(10);
	forward(20);
	right(55,0);
	forward(35);
	backward(4);
	right(45,0);
	while (analog10(3)<BLACK_SEN_THRESH)
	{
		motor(MOT_LEFT, 100);
		motor(MOT_RIGHT, 100);
	}
	msleep(10);
	backward(24);
	left(42,0);
	backward(10); 
	left(50,0);
	//backward(35);
	touch_back(TOUCH_SEN);
	forward(120.00);
	
	// TRIBBLE PILE 2
	
	set_servo_position(S_GATE,S_OPEN);
	forward(40.00);
	set_servo_position(S_GATE,S_GAP);
	
	// camera sort
	
	s_time = curr_time();
	while((curr_time()-s_time) < 14) //timer
	{
		while (greencentered == 0)
		{
			camera_update(); // process the most recent image
			camera_update(); // process the most recent image
			camera_update(); // process the most recent image
			camera_update(); // process the most recent image
			camera_update(); // process the most recent image
			loop = loop + 1;
			printf("Update camera, loop %d\n", loop);
			//msleep(10);
			if (get_object_count(color) > 0)
			{
				xvalue = get_object_center(color,0).x;
				yvalue = get_object_center(color,0).y;
				deltax = abs(xvalue-centerx);
				deltay = abs(yvalue-centery);
				stepsizex = (deltax>10) ? 50: 20;
				stepsizey = (deltay>10) ? 50: 20;
				printf("x is %d, y is %d\n", xvalue, yvalue);
				greencentered = ((xvalue >= centerx-margin) & (xvalue <= centerx+margin)) && ((yvalue >= centery-margin) & (yvalue <= centery+margin));
				if(greencentered == 1)//get x, y for the biggest blob the channel sees		
				{	
					printf("Biggest blob at (%d,%d)\n",xvalue,yvalue);
					msleep(300);
					set_servo_position(CATCHERARM, CATCHER_MIDWAY);
					msleep(300);
					set_servo_position(CATCHERARM, CATCHER_DOWN);
					//slow_servo(CATCHERARM,20,40,CATCHER_UP,CATCHER_DOWN);
					msleep(300);
					currpos=get_servo_position(CATCHERARM);
					printf("%d\n",currpos);
					/*if(CATCHER_MIDWAY>=currpos>CATCHER_DOWN)
					{	
					currpos=get_servo_position(CATCHERARM);
					set_servo_position(CATCHERARM, currpos-INCREMENT);
					msleep(SLEEP_INCREMENT);
					currpos=get_servo_position(CATCHERARM);
					}
					if(currpos<CATCHER_MIDWAY)
					{	
					currpos=get_servo_position(CATCHERARM);
					set_servo_position(CATCHERARM, currpos+INCREMENT);
					msleep(SLEEP_INCREMENT);
					currpos=get_servo_position(CATCHERARM);
					}*/
					set_servo_position(CATCHERARM,CATCHER_UP);
					msleep(500);
				}
				else
				{
					if(xvalue <= centerx+margin) //moves right if senses value greater than 80
					{
						motor(MOT_RIGHT,70);
						motor(MOT_LEFT,-70);
						msleep(stepsizex);
						motor(MOT_RIGHT,0);
						motor(MOT_LEFT,0);
						//msleep(10);
						printf("blob is too right\n");
					}	
					if(xvalue >= centerx-margin) // moves left if senses value less than 70
					{
						motor(MOT_LEFT,70);
						motor(MOT_RIGHT,-70);
						msleep(stepsizex);	
						motor(MOT_LEFT,0);
						motor(MOT_RIGHT,0);
						//msleep(10);
						printf("blob is too left\n");
					}
					
					if ((xvalue > 67 /*centerx-margin*/) && (xvalue < 87 /*centerx+margin*/))
					{
						if(yvalue > centery+margin) //moves forward if senses value greater than 43
						{
							motor(MOT_LEFT,-50);
							motor(MOT_RIGHT,-50);
							msleep(stepsizey);
							motor(MOT_LEFT,0);
							motor(MOT_RIGHT,0);
							//msleep(10);
							printf("blob is too close\n");
						}
						
						if(yvalue < centery-margin) // moves backwar if senses value less than 38
						{
							motor(MOT_LEFT,50);
							motor(MOT_RIGHT,50);
							msleep(stepsizey);
							motor(MOT_LEFT,0);
							motor(MOT_RIGHT,0);
							//msleep(10);
							printf("blob is too far\n");
						}
					}
				}
			}
		}
	}
	set_servo_position(S_GATE,S_CLOSE);
	
	// SCORE PILE 2
	
	right(55,0);
	forward(28);
	backward(12);
	right(55,0);
	touch_back(TOUCH_SEN);
	forward(200);
	right(50,0);
	touch_back(TOUCH_SEN);
	forward(15);
	left(50,0);
	while (analog10(3)<BLACK_SEN_THRESH)
	{
		motor(MOT_LEFT, 100);
		motor(MOT_RIGHT, 100);
	}
	backward(4);
	right(50,0);
	msleep(10);
	forward(24);
	right(42,0);
	touch_back(TOUCH_SEN);
	backward(4);
	
	// END
	ao();
	disable_servos();
	printf("done!");
}
示例#22
0
void takepom(){
		printf("takepom \n");
		set_servo_position(CLAW1, CLAW1_FRONT);
		set_servo_position(CLAW2, CLAW2_FRONT);
		set_servo_position(CAM_SERVO,CAM_SERVO_POSITION_FRONT);
		stop();
		msleep(1000);
		turn_right();
	    msleep(500);
	
	int findit=1;
		int left_or_right=1; //ob nach linksoder rechts drehen l=0, r=1
		int getit=0;   //wenn er das pom hat
		int foundNothingToLong=0;
	double sec=seconds();
		while(!getit){
				if((sec+10)<seconds()){ //wenn er 10 sec nichts findet 
					if(foundNothingToLong=2){
						forward();
						while((analog10(ET)<310&&( analog10(LEFT_SENSOR)<left_blk-150||analog10(RIGHT_SENSOR)<right_blk-150))){
							msleep(5);
						}
						if(analog10(ET)<310){
							turn_left();
							msleep(600);
							forward();
							while( analog10(LEFT_SENSOR)<left_blk-150||analog10(RIGHT_SENSOR)<right_blk-150){}
						}
					}
					
					else{
							foundNothingToLong++;
							back();
							msleep(400);			//etwas rückwerts
							sec=seconds();
							if(!get_object_count(green)>0){  //wenn ers nicht hat 
								forward();
								msleep(400);
								findit=0;							//nicht gefunden
								turn_left();
								while(analog10(LEFT_SENSOR)<left_blk-150){}
								turn_left();
								while(analog10(MIDDLE_SENSOR)<mid_blk-150){}
								break;
							}
						}
					}	
			if(left_or_right){ 
				turn_right_speed(slow_speed);
				while(analog10(RIGHT_SENSOR)<right_blk-150&&!get_object_count(green)>0){ //solange Kammera und Sensor nichts sehen rechts drehen
						camera_update();
				}
				stop();  //wenn etwas gefunden stehen bleiben
				msleep(200);
				if(analog10(RIGHT_SENSOR)>right_blk-150){ //schauen ob Kamera oder Sensor 
						left_or_right=0; //bei sensor drehrichtung ändern
				}
				else{
					camera_update();	
					if(get_object_bbox(0,0).width>5&&get_object_bbox(0,0).height>5&&analog10(RIGHT_SENSOR)<right_blk-150){ //wenn Kammera : Wa blob ein zu kleiner Blob(was falsches gesehen), wenn blob groß stehen bleiben und feinjustireung(foundit) machen 
							getit=foundit();

					}
				}
			}
			else{
				turn_left_speed(slow_speed);
				while(analog10(LEFT_SENSOR)<left_blk-150&&!get_object_count(green) >0){
						camera_update();

				}
				stop();
				msleep(200);
				if(analog10(LEFT_SENSOR)>left_blk-150){
								left_or_right=1;
					}
				else{
					camera_update();	
						if(get_object_bbox(0,0).width>5&&get_object_bbox(0,0).height>5){
								getit=foundit();

						}
					}
			}
		}
		if(findit){
		put_poms_out();  // poms nach drausen schieben
		}
		
}
示例#23
0
int foundit(){
	printf("foundit \n");
	int x;
	camera_update(); 
	if(get_object_count(green)>0){
		while(get_object_center(green,0).y < 100){ //solange bis blob nahe genug 
				camera_update(); 
				if(get_object_count(green)>0){ //schaut ob Blob noch in der Kamera
					camera_update(); 
					x = get_object_center(green,0).x;	
					if (x > 120)  {							//wenn Blob rechts--> rechts drehen ....
						turn_right_speed(50);  //je näher er dem pom kommt umso langsamer dreht er sich 
					} else if (x < 40) {
						turn_left_speed(50);    //je näher er dem pom kommt umso langsamer dreht er sich
					} else if ((x > 40)||(x < 120)) {
						forward();
						if(analog10(LEFT_SENSOR)>left_blk-150||analog10(RIGHT_SENSOR)>right_blk-150){
							foundOutArea=1;
							printf("drausen\n");
						}
					}
			   }
			   else{									//wenn blob nicht mehr in Kamera wieder in grobjustierung(takepom) gehen 
				   return 0;
			   }
		}
	printf("target aquired! \n");
	set_servo_position(CLAW1, CLAW1_FRONT);
	set_servo_position(CLAW2, CLAW2_FRONT);
	forward();						//etwas vor fahren um Blob sicher zu erwischen
	int time=200;	
	while((analog10(ET)<310&&!digital(15)&&!digital(14))&&time>=0 ){
		msleep(5);
		time--;
	}
	printf("%i",digital(15));
	printf("\nLast time  ");
	printf("%i",time);
	stop();
	msleep(100);
	if(time>=0){ //fals er angefahren ist 

		back();
		msleep(150);
		motor(RIGHT_MOTOR,TURN_SPEED);
		motor(LEFT_MOTOR,0);
		msleep(1500);
		claw_close();
	}
	claw_close();
	if(foundOutArea){ //wenn ers auserhalb des Schwarzen bereichs gefunden hat
		printf("zurück fahren\n");
		back();
		while(analog10(LEFT_SENSOR)>left_blk-150||analog10(RIGHT_SENSOR)>right_blk-150){}
		msleep(700);
	}
	back();
	msleep(800);
	if(analog10(LEFT_SENSOR)>left_blk-150||analog10(RIGHT_SENSOR)>right_blk-150){  //wenn er so weit zurück sein sollte das er auf schwarz is 
		forward();
		while(analog10(LEFT_SENSOR)<left_blk-150&&analog10(RIGHT_SENSOR)<right_blk-150){} //wieder forwärts bis er weis is damits funktioniert
	}
	camera_update(); 
	return 1;
	}
	return 0;
}
示例#24
0
int main(int argc, char *argv[])
{
  int blob_count = 0, i = 0;
  int main_channel = 2; /* red - botguy */
  double time_of_snapshot = 0.0;
  double start = 0.0;

  printf("args: %d\n", argc);
  if (argc > 1)
  {
    if (argv[1][0] == '0') main_channel = 0;
    else if (argv[1][0] == '1') main_channel = 1;
    else if (argv[1][0] == '2') main_channel = 2;
    else if (argv[1][0] == '3') main_channel = 3;
  }
  for (i = 0; i < argc; i++) {
    printf("arg[%d] = %s\n", i, argv[i]);
  }
  printf("\n");
  
  int area[NUM_TRACKING];
  point2 center[NUM_TRACKING];
  rectangle blob[NUM_TRACKING];
  //int area1 = 0, areaX = 0;
  //point2 center;

  init_grid();

  //wait_for_light(0);

  camera_open();
  printf("Num channels: %d", get_channel_count());
  camera_update();
  time_of_snapshot = seconds();
  msleep(300);
  
  start = seconds();
  do {
    // Initialize
    memset(center, 0, sizeof(center));
    memset(area, 0, sizeof(area));
    clear_grid();

    // Begin

    // Do not do anything with the camera until enough time has passed
    if (seconds() - time_of_snapshot >= 0.1) {
      camera_update();
      time_of_snapshot = seconds();
      blob_count = get_object_count(main_channel);
      //sprintf(msg, "#:%2d", blob_count); diag();
      printf("#:%-2d ", blob_count);
      if (blob_count > count_of(area)) {
        blob_count = count_of(area);
      }

      if (blob_count > 0) {
        for (i = 0; i < blob_count; ++i) {
          area[i]   = get_object_area(main_channel, i);
          center[i] = get_object_center(main_channel, i);
          blob[i] = get_object_bbox(main_channel, i);
          plot(&blob[i], i + '0');
        }

        for (i = 0; i < blob_count; ++i) {
          printf(" %3d", blob[i].width * blob[i].height);
        }

        for (; i < count_of(blob); ++i) {
          printf("    ");
        }

        printf("        ");
        for (i = 0; i < blob_count; ++i) {
          printf(" %2dx%2d", blob[i].width, blob[i].height);
        }

        //show_xy(center[0].x, center[0].y);
      }

      //printf(" |%3d %4d <-> %4d %3d|  %s\n", delta_left, left, right, delta_right, message);
      printf("%s\n", message);
      message[0] = 0;
      diag();

      show_grid();
    }

    //printf("The time is: %f   %f\n", seconds(), seconds() - start);
  } while (seconds() - start <= 4.0);

  camera_close();

  return 0;
}