/* --------------------------------------------------------------------- */ /* - NEW_SCAN.C - */ /* --------------------------------------------------------------------- */ /* Filename: NEW_SCAN.C - */ /* Description: Contains the scanning calls for robot golf - */ /* Written by: - */ /* Mark DeBeradino - */ /* Other authors of this code include: - */ /* Kuo Huang - */ /* Roger W. Webster, Ph.D. - */ /* Robot Vision and Artificial Intelligence Lab - */ /* Department of Computer Science - */ /* Millersville University - */ /* Millersville, PA 17551 - */ /* (717) 872-3539 or (717) 872-3860 - */ /* --------------------------------------------------------------------- */ /* --------------------------------------------------------------------- */ /* - necessary includes & defines for this file - */ /* --------------------------------------------------------------------- */ #include #include "mytest.h" int values[1024]; void feedback(angle,distance,goalx,goaly,xarr,yarr) /********************************************************************** * * function : feedback ( location ) * * description : from the path of the golf ball to make adjustment * **********************************************************************/ float *angle, *distance; int goalx, goaly; int xarr[THREE]; int yarr[THREE]; { } /********************************************************************** * * function : scan ( location ) * * description : * **********************************************************************/ int scan(location,yscan,threshold,Cup_Position_Y,distance,angle) int location[THREE]; /* where ball was found stored */ int yscan[THREE]; /* where in y to scan */ int threshold; int Cup_Position_Y; /* the cups position in Y */ float distance, angle; { int xscan[THREE]; /* where in x to scan */ int dx[THREE]; /* how far to scan */ int timeout[THREE]; /* how long to scan for */ int found = 320; /* where ball was found on scan */ int scan_range,scan_range_start; /* ******** values set by outsiders --> shawn <-- ******** */ int Ball_Width = 40; /* defined outside and set to pixel width of ball */ char c; setup_screen(); calculate_y_scans(Cup_Position_Y,yscan); /* printf(" yscans %d %d %d \n",yscan[0],yscan[1],yscan[2]); */ upper_draw_line (DEFAULT_XSCAN,yscan[ZERO] - ONE,DEFAULT_DX); upper_draw_line (DEFAULT_XSCAN,yscan[ONE] - ONE ,DEFAULT_DX); upper_draw_line (DEFAULT_XSCAN,yscan[TWO] - ONE ,DEFAULT_DX); /* <------------------------ HIT Ball -----------------------> */ ROBOT$HIT_BALL (distance, angle); xscan[ZERO] = DEFAULT_XSCAN; dx[ZERO] = DEFAULT_DX; timeout[ZERO] = DEFAULT_TIMEOUT_SCAN1; xscan[ONE] = DEFAULT_XSCAN; dx[ONE] = DEFAULT_DX; timeout[ONE] = DEFAULT_TIMEOUT_SCAN2; xscan[TWO] = DEFAULT_XSCAN; dx[TWO] = DEFAULT_DX; timeout[TWO] = DEFAULT_TIMEOUT_SCAN3; /*-------------------------- First Scan -------------------------------*/ VIS$SCAN(xscan[ZERO],yscan[ZERO],dx[ZERO],threshold,COPY_Y1, Ball_Width,&found,timeout[ZERO]); printf ("first foundx at: %d\n", found); location[ZERO] = found; /* ------------------------- Second Scan -------------------------------*/ VIS$SCAN(xscan[ONE],yscan[ONE],dx[ONE],threshold,COPY_Y2, Ball_Width,&found,timeout[ONE]); printf ("second foundx at: %d\n", found); location[ONE] = found; /* -------------------------- Third Scan -------------------------------*/ VIS$SCAN(xscan[TWO],yscan[TWO],dx[TWO],threshold,COPY_Y3, Ball_Width,&found,timeout[TWO]); #ifdef ROGER #endif printf ("third foundx at: %d\n", found); location[TWO] = found; sleep(1); VIS$FREEZE(); VIS$COPY_BLOCK(location[ZERO]-Ball_Width,COPY_Y1, location[ZERO] + Ball_Width, COPY_Y1 + 2 * Ball_Width, location[ZERO] - Ball_Width, yscan[ZERO] - Ball_Width); VIS$COPY_BLOCK(location[ONE]-Ball_Width,COPY_Y2, location[ONE] + Ball_Width, COPY_Y2 + 2 * Ball_Width, location[ONE] - Ball_Width, yscan[ONE] - Ball_Width); VIS$COPY_BLOCK(location[TWO]-Ball_Width,COPY_Y3, location[TWO] + Ball_Width, COPY_Y3 + 2 * Ball_Width, location[TWO] - Ball_Width, yscan[TWO] - Ball_Width); /* if the scan line doesn't scan anything , it will have negative value. we set all the negative value to zero */ if (location[ONE] < 0) location[ONE] = 0; if (location[TWO] < 0) location[TWO] = 0; if (location[ZERO] < 0) location[ZERO] = 0; if (yscan[ONE] < 0) yscan[ONE] = 0; if (yscan[TWO] < 0) yscan[TWO] = 0; if (yscan[ZERO] < 0) yscan[ZERO] = 0; /* VIS$DRAWLINE(xscan[ZERO],yscan[ZERO],xscan[ZERO] + dx[ZERO],yscan[ZERO],255); VIS$DRAWLINE(xscan[ONE],yscan[ONE],xscan[ONE] + dx[ONE],yscan[ONE],255); VIS$DRAWLINE(xscan[TWO],yscan[TWO],xscan[TWO] + dx[TWO],yscan[TWO],255); */ ROBOT$HOME(); printf ("hit c/r \n"); getchar(); } /* end function scan */ /*********************************************************************** * * function : calculate y scans ( position, scan ) * * description : * * this function takes the cup postion passed in the integer position, * and calculates the three scan lines based on where this cup lies. * if the cup is in the higher half, closer to the robot, one scan line * is placed between the robot and the cup and two after the cup. if the * cup is in the lower half of view, farther from the robot, two scans * are placed before the cup and one after * *************************************************************************/ void calculate_y_scans(position,scan) int position; int scan[THREE]; { int offset; if (position < HALF_SCREEN) { scan[ZERO] = (int)(position + (TWO * (SCREEN_MAXY - position) / THREE)); scan[ONE] = (int)(position + ((SCREEN_MAXY - position) / THREE)); scan[TWO] = (int)(position / TWO); } else { scan[ZERO] = (int)(position + ((SCREEN_MAXY - position) / TWO)); scan[ONE] = (int)(TWO * (position / THREE)); scan[TWO] = (int)(position / THREE); } scan[ZERO] += 20; scan[ONE] += 20; scan[TWO] += 20; offset = scan[TWO] - position; if (position < 30) scan[TWO] = position + 20; else if (offset < 20 && offset > 0) scan[TWO] += 20; else if (offset > -20 && offset <= 0) scan[TWO] -= 20; } /********************************************************************** * * function : setup_screen * * description : * this function handles the blanking out of the video memory not * normally used. This is needed so that when we copy the scan * areas to high memory...things is ok!!! * ************************************************************************/ setup_screen() { int values[256]; int loop; VIS$OPEN(); VIS$INIT_LUTS(); VIS$SET_VIDEO_CHANNEL(0); sleep(1); VIS$INIT_FB(); VIS$INIT_LUTS(); VIS$VIEW(); VIS$SET_VIDEO_CHANNEL(0); sleep(1); VIS$SCROLL(480); VIS$SET_SCREEN(0); VIS$SCROLL(0); VIS$SET_SCREEN(0); VIS$SELECT_LUT(0,0); VIS$DYNAMIC_OUTLUT(1); VIS$VMASK(); VIS$VIEW(); VIS$SET_VIDEO_CHANNEL(0); sleep(1); for (loop = ZERO; loop < 256; loop++) values[loop] = loop; values[0] = 255; VIS$LOAD_LUT(1,1,values); values[0] = 0; VIS$LOAD_LUT(2,1,values); values[0] = 0; VIS$LOAD_LUT(3,1,values); } upper_draw_line (x1, y1, dx) int x1, y1, dx; { int r; r = VIS$H_WRITE_LINE (x1, y1, dx, 512); } VIS$SCAN(xscan,yscan,dx,threshold, copy_y1, ball_width,found,timeout) int xscan, yscan, dx, threshold, copy_y1, ball_width, *found, timeout; { int keep_going = YES; int count = 0; int x, foundx = 0; do { count++; VIS$READ_HORIZONTAL_LINE(xscan,yscan,dx,values); for (x=0; x< dx; x++) { if (values[x] > threshold) { foundx = xscan + x; if (copy_y1 > 0) { VIS$FREEZE(); carea (foundx - ball_width,yscan - ball_width, 2 * ball_width, 2 * ball_width, foundx - ball_width, copy_y1, 2 * ball_width, 2 * ball_width); VIS$VIEW(); } keep_going = NO; break; } /* end if */ } /* end for loop */ } while (keep_going && count < timeout); *found = foundx; } /* end function */ #ifdef ROGER VIS$COPY_BLOCK (x1, y1, x2, y2, to_x, to_y) int x1, y1, x2, y2, to_x, to_y; { int dx; dx = (x2-x1) + 1; carea (x1, y1, dxs, dys, to_x, to_y, dxd, dyd); } /* end function */ #endif