int
main (int argc, char ** argv)
{
if (argc != 3) {
usage ();
}
#ifdef DEBUG_WIN_X
if (fork() != 0) {
printf ("Forked to background, exiting.\n");
exit (0);
}
#endif
log_open (LOGPATH);
int fill_color = gdTrueColorAlpha (100, 100, 100, gdAlphaTransparent / 2);
int line_color = gdTrueColorAlpha (175, 175, 175, gdAlphaOpaque);
int text_color = gdTrueColorAlpha (255, 255, 255, gdAlphaOpaque);
FILE * img_out;
int img_time;
char img_filename[64];
gdImagePtr camera_1;
gdImagePtr camera_2;
gdImagePtr output;
int numloops = 0;
int fpstimer;
double a1, a2;
tolerance_file_t * tol;
int alliance;
tol = get_tolerance (TOLPATH);
log_write ("Got tolerance values from %s.", TOLPATH);
if (argv[1][0] == 98) {
log_write ("Seeking blue targets.");
alliance = ALLIANCE_BLUE;
} else {
log_write ("Seeking red targets.");
alliance = ALLIANCE_RED;
}
char * jpeg_buff;
char * jpeg_buff_2;
int jpeg_buff_size;
int jpeg_buff_size_2;
int x, y;
blob * cam1_green;
blob * cam2_green;
blob * cam1_red;
blob * cam2_red;
blob * target_blob;
int loop_ctr;
loop_ctr = 0;
float lastfps;
log_write ("Beginning main loop.");
while (1) {
#ifndef DISABLE_IMG
output = gdImageCreateTrueColor (1280, 480);
#endif
#ifndef DEBUG_WIN
log_write ("Getting first jpeg.");
jpeg_buff = get_jpeg (CAM1, &jpeg_buff_size);
if (jpeg_buff == NULL) {
log_write ("First jpeg buffer is null!");
exit (1);
}
if (jpeg_buff_size <= 0) {
log_write ("First jpeg buffer size is invalid!");
exit (1);
}
log_write ("First jpeg received. Size: %d", jpeg_buff_size);
camera_1 = gdImageCreateFromJpegPtr (jpeg_buff_size, jpeg_buff);
if (camera_1 == NULL) {
log_write ("gdImageCreateFromJpegPtr () failed for jpeg 1!");
exit (1);
}
log_write ("Getting second jpeg.");
jpeg_buff_2 = get_jpeg (CAM2, &jpeg_buff_size_2);
if (jpeg_buff_2 == NULL) {
log_write ("Second jpeg buffer is null!");
exit (1);
}
if (jpeg_buff_size_2 <= 0) {
log_write ("Second jpeg buffer size is invalid!");
exit (1);
}
log_write ("Second jpeg received. Size: %d", jpeg_buff_size_2);
camera_2 = gdImageCreateFromJpegPtr (jpeg_buff_size_2, jpeg_buff_2);
if (camera_2 == NULL) {
log_write ("gdImageCreateFromJpegPtr () failed for jpeg 2!");
exit (1);
}
#endif // !DEBUG_WIN
#ifdef DEBUG_WIN
img_out = fopen ("cam1.jpg", "rb");
camera_1 = gdImageCreateFromJpeg (img_out);
fclose (img_out);
img_out = fopen ("cam2.jpg", "rb");
camera_2 = gdImageCreateFromJpeg (img_out);
fclose (img_out);
if (camera_1 == NULL) {
log_write ("Camera 1 image did not load properly.");
exit (1);
}
if (camera_2 == NULL) {
log_write ("Camera 2 image did not load properly.");
exit (1);
}
#endif // DEBUG_WIN
log_write ("Detecting blobs on camera 1.");
cam1_red = find (camera_1, &(tol->cam1_red));
cam1_green = find (camera_1, &(tol->cam1_green));
log_write ("Detecting blobs on camera 2.");
cam2_red = find (camera_2, &(tol->cam2_red));
cam2_green = find (camera_2, &(tol->cam2_green));
if ((target_blob = target (cam1_red, cam1_green, alliance)) != NULL) {
if ((target_blob->center_x + 50) > 400) {
log_write ("Sending left motor command.");
gpio (59, GPIO_ON);
gpio (58, GPIO_OFF);
}
else if ((target_blob->center_x + 50) < 240) {
log_write ("Sending right motor command.");
gpio (58, GPIO_ON);
gpio (59, GPIO_OFF);
}
else {
log_write ("Sending left & right motor commands.");
gpio (58, GPIO_ON);
gpio (59, GPIO_ON);
}
#ifndef DISABLE_IMG
//print_blobs (camera_1, target_blob, 100, 100, 100);
#endif
free_blobs (target_blob);
} else {
log_write ("EE: No targets found.");
}
if (cam1_red != NULL && cam1_green != NULL) {
log_write ("Blob detection completed. Building image.");
#ifndef DISABLE_IMG
print_blobs (camera_1, cam1_red, 200, 0, 0);
//print_blobs (camera_2, cam2_red, 200, 0, 0);
print_blobs (camera_1, cam1_green, 0, 180, 20);
//print_blobs (camera_2, cam2_green, 0, 180, 20);
#endif
free_blobs (cam1_red);
free_blobs (cam2_red);
free_blobs (cam1_green);
free_blobs (cam2_green);
#ifndef DISABLE_IMG
gdImageCopy (output, camera_1, 0, 0, 0, 0, 640, 480);
gdImageCopy (output, camera_2, 640, 0, 0, 0, 640, 480);
#endif
/*gdImageFilledRectangle (output, 540, 340, 760, 450, fill_color);
gdImageRectangle (output, 540, 340, 760, 450, line_color);
time_t rawtime;
time (&rawtime);
char * time_str = ctime (&rawtime);
time_str[strlen(time_str)-1] = '\0';
render_text (output, text_color, 550, 360, "%s", time_str);
if (cam1_red != NULL && cam2_red != NULL) {
render_text (output, text_color, 550, 375, "Left Centroid: 300, 424", cam1_red->center_x, cam1_red->center_y);
render_text (output, text_color, 550, 390, "Right Centroid: 422, 233", cam2_red->center_x, cam2_red->center_y);
a1 = image_angle (cam1_red->center_x);
a2 = image_angle (cam2_red->center_x);
render_text (output, text_color, 550, 405, "Left Angle: %f", a1 * (180/PI));
render_text (output, text_color, 550, 420, "Right Angle: %f", a2 * (180/PI));
render_text (output, text_color, 550, 435, "Depth: %f", find_depth (a1, a2));
}*/
#ifndef DISABLE_IMG
img_time = time (NULL);
snprintf (img_filename, sizeof (img_filename), "%s%s%d.jpg", OUTPATH, argv[2], img_time);
log_write ("Image built. Writing to file: %s", img_filename);
img_out = fopen (img_filename, "wb");
gdImageJpeg (output, img_out, 100);
fclose (img_out);
#endif
} else {
log_write ("EE: Some cams didn't detect red+green blobs.");
}
#ifndef DISABLE_IMG
gdImageDestroy (output);
#endif
gdImageDestroy (camera_1);
gdImageDestroy (camera_2);
log_write ("Loop %d finished.", loop_ctr);
loop_ctr++;
}
return 0;
}
int
main (int argc, char ** argv)
{
int numloops = 0;
int fpstimer;
double a1, a2;
tolerance_file_t * tol = get_tolerance ("colors.tol");
memset (tol, '\0', sizeof (tolerance_file_t));
int do_blob = -1;
int do_text = -1;
Uint8 mouse_button;
SDL_Surface * image;
SDL_Surface * image_2;
SDL_Surface * screen;
SDL_Surface * back = IMG_Load ("back.png");
SDL_Color white = {255, 255, 255};
SDL_Event event;
char * jpeg_buff;
char * jpeg_buff_2;
int jpeg_buff_size;
int jpeg_buff_size_2;
int x, y;
FILE * log_fp;
blob * cam1_green;
blob * cam1_red;
blob * cam2_green;
blob * cam2_red;
blob * vision_targets = NULL;
CvCapture * capture;
CvCapture * capture_2;
IplImage * cam_img;
IplImage * cam_img_2;
FILE * color_fp;
if (SDL_Init (SDL_INIT_VIDEO | SDL_INIT_TIMER) == -1) {
SDL_Quit ();
printf ("SDL Initialization failed\n");
exit (1);
}
if ((screen = SDL_SetVideoMode (800, 600, 32, SDL_HWSURFACE | SDL_DOUBLEBUF | SDL_FULLSCREEN )) == NULL) {
SDL_Quit ();
printf ("Could not create output surface\n");
exit (1);
}
if (TTF_Init () == -1) {
SDL_Quit ();
printf ("TTF_Init: %s\n", TTF_GetError());
exit (1);
}
if (!(capture = cvCaptureFromCAM (0))) {
SDL_Quit ();
printf ("Failed to start capture\n");
exit (1);
}
if (!(capture_2 = cvCaptureFromCAM (1))) {
SDL_Quit ();
printf ("Failed to start capture\n");
exit (1);
}
int start = SDL_GetTicks ();
int lastfps;
TTF_Font * arial = TTF_OpenFont ("arial.ttf", 12);
while (1) {
fpstimer = SDL_GetTicks ();
numloops++;
if (fpstimer - start > 1000) {
start = SDL_GetTicks ();
lastfps = numloops;
numloops = 0;
}
while (SDL_PollEvent (&event)) {
switch (event.type) {
case SDL_KEYDOWN:
switch (event.key.keysym.sym) {
case 'q':
TTF_Quit ();
SDL_Quit ();
exit (0);
break;
case 'b':
do_blob = -do_blob;
break;
case 'v':
do_text = -do_text;
break;
case 'i':
tol->cam1_green.blob_minlength++;
break;
case 'k':
tol->cam1_green.blob_minlength--;
break;
case 'o':
tol->cam1_green.blob_tolerance++;
break;
case 'l':
tol->cam1_green.blob_tolerance--;
break;
case 'y':
tol->cam1_red.blob_minlength++;
break;
case 'h':
tol->cam1_red.blob_minlength--;
break;
case 'u':
tol->cam1_red.blob_tolerance++;
break;
case 'j':
tol->cam1_red.blob_tolerance--;
break;
case 'w':
tol->cam2_green.blob_minlength++;
break;
case 's':
tol->cam2_green.blob_minlength--;
break;
case 'e':
tol->cam2_green.blob_tolerance++;
break;
case 'd':
tol->cam2_green.blob_tolerance--;
break;
case 'r':
tol->cam2_red.blob_minlength++;
break;
case 'f':
tol->cam2_red.blob_minlength--;
break;
case 't':
tol->cam2_red.blob_tolerance++;
break;
case 'g':
tol->cam2_red.blob_tolerance--;
break;
case 'z':
color_fp = fopen ("colors.tol", "wb");
fwrite (tol, sizeof (tolerance_file_t), 1, color_fp);
fclose (color_fp);
break;
}
break;
case SDL_MOUSEBUTTONDOWN:
mouse_button = SDL_GetMouseState (&x,&y);
if (mouse_button & SDL_BUTTON(1)) {
if (x > 352) {
set_tracking (x, y, screen, &(tol->cam2_green));
}
else {
set_tracking (x, y, screen, &(tol->cam1_green));
}
}
else {
if (x > 352) {
set_tracking (x, y, screen, &(tol->cam2_red));
}
else {
set_tracking (x, y, screen, &(tol->cam1_red));
}
}
break;
}
}
cam_img = cvQueryFrame (capture);
image = ipl_to_surface (cam_img);
cam_img_2 = cvQueryFrame (capture_2);
image_2 = ipl_to_surface (cam_img_2);
easy_blit (0, 0, back, screen);
if (do_blob == 1) {
cam1_green = find (image, &(tol->cam1_green));
cam1_red = find (image, &(tol->cam1_red));
cam2_green = find (image_2, &(tol->cam2_green));
cam2_red = find (image_2, &(tol->cam2_red));
vision_targets = target (cam1_red, cam1_green, ALLIANCE_BLUE);
print_blobs_lighter (image, cam1_green, 0, 150, 0);
print_blobs_lighter (image, cam1_red, 150, 0, 0);
print_blobs_lighter (image_2, cam2_green, 0, 150, 0);
print_blobs_lighter (image_2, cam2_red, 150, 0, 0);
if (vision_targets != NULL) {
render_text (screen, arial, 100, 490, "Found target!");
print_blobs (image, vision_targets, 0, 0, 0);
free_blobs (vision_targets);
}
}
if (do_text == 1) {
render_text (screen, arial, 600, 308, "FPS: %d", (lastfps));
//print_blobs (image_2, cam2_red, 0, 0, 0);
render_text (screen, arial, 10, 308, "Hotkey list:");
render_text (screen, arial, 20, 328, "i - increase left green blob minimum length: %d", tol->cam1_green.blob_minlength);
render_text (screen, arial, 20, 348, "k - decrease left green blob minimum length");
render_text (screen, arial, 20, 368, "o - increase left green check tolerance: %d", tol->cam1_green.blob_tolerance);
render_text (screen, arial, 20, 388, "l - decrease left green check tolerance");
render_text (screen, arial, 20, 408, "y - increase left red blob minimum length: %d", tol->cam1_red.blob_minlength);
render_text (screen, arial, 20, 428, "h - decrease left red blob minimum length");
render_text (screen, arial, 20, 448, "u - increase left red check tolerance: %d", tol->cam1_red.blob_tolerance);
render_text (screen, arial, 20, 468, "j - decrease left red check tolerance");
render_text (screen, arial, 50, 508, "Green check color: %d, %d, %d", tol->cam1_green.r, tol->cam1_green.g, tol->cam1_green.b);
SPG_RectFilled (screen, 20, 500, 40, 520, SDL_MapRGB (screen->format, tol->cam1_green.r, tol->cam1_green.g, tol->cam1_green.b));
render_text (screen, arial, 50, 548, "Red check color: %d, %d, %d", tol->cam1_red.r, tol->cam1_red.g, tol->cam1_red.b);
SPG_RectFilled (screen, 20, 540, 40, 560, SDL_MapRGB (screen->format, tol->cam1_red.r, tol->cam1_red.g, tol->cam1_red.b));
render_text (screen, arial, 320, 328, "w - increase right green blob minimum length: %d", tol->cam2_green.blob_minlength);
render_text (screen, arial, 320, 348, "s - decrease right green blob minimum length");
render_text (screen, arial, 320, 368, "e - increase right green check tolerance: %d", tol->cam2_green.blob_tolerance);
render_text (screen, arial, 320, 388, "d - decrease right green check tolerance");
render_text (screen, arial, 320, 408, "r - increase right red blob minimum length: %d", tol->cam2_red.blob_minlength);
render_text (screen, arial, 320, 428, "f - decrease right red blob minimum length");
render_text (screen, arial, 320, 448, "t - increase right red check tolerance: %d", tol->cam2_red.blob_tolerance);
render_text (screen, arial, 320, 468, "g - decrease right red check tolerance");
render_text (screen, arial, 350, 508, "Green check color: %d, %d, %d", tol->cam2_green.r, tol->cam2_green.g, tol->cam2_green.b);
SPG_RectFilled (screen, 320, 500, 340, 520, SDL_MapRGB (screen->format, tol->cam2_green.r, tol->cam2_green.g, tol->cam2_green.b));
render_text (screen, arial, 350, 548, "Red check color: %d, %d, %d", tol->cam2_red.r, tol->cam2_red.g, tol->cam2_red.b);
SPG_RectFilled (screen, 320, 540, 340, 560, SDL_MapRGB (screen->format, tol->cam2_red.r, tol->cam2_red.g, tol->cam2_red.b));
if ((cam1_green != NULL) && (cam2_green != NULL)) {
a1 = image_angle (cam1_green->center_x);
a2 = image_angle (cam2_green->center_x);
render_text (screen, arial, 580, 348, "Image 1 centroid: %d, %d", cam1_green->center_x, cam1_green->center_y);
render_text (screen, arial, 580, 368, "Image 2 centroid: %d, %d", cam2_green->center_x, cam2_green->center_y);
render_text (screen, arial, 580, 388, "Depth, Method 1: %f", find_depth (a1, a2, 0));
render_text (screen, arial, 580, 408, "Depth, Method 2: %f", find_depth (a1, a2, 1));
render_text (screen, arial, 580, 428, "Depth, Method 3: %f", find_depth (a1, a2, 2));
render_text (screen, arial, 580, 448, "Angle, Left: %f", a1 * (180/PI));
render_text (screen, arial, 580, 468, "Angle, Right: %f", a2 * (180/PI));
SPG_RectFilled (screen, 780, (int)(35 * find_depth (a1, a2, 2)) - 300, 800, 20 + (int)(35 * find_depth (a1, a2, 1)) - 300, SDL_MapRGB (screen->format, tol->cam2_green.r, tol->cam2_green.g, tol->cam2_green.b));
}
}
if (do_blob == 1) {
free_blobs (cam1_green);
free_blobs (cam1_red);
free_blobs (cam2_green);
free_blobs (cam2_red);
}
easy_blit (0, 0, image, screen);
easy_blit (352, 0, image_2, screen);
SDL_FreeSurface (image);
SDL_FreeSurface (image_2);
SDL_Flip (screen);
log_fp = fopen ("colors.tol", "wb");
fwrite (&tol, sizeof (tolerance_file_t), 1, log_fp);
fclose (log_fp);
}
return 0;
}
static void mainloop (void) {
int i,j;
unsigned char processedPixels[FRAME_HEIGHT][FRAME_WIDTH];
for (;;) {
fd_set fds;
struct timeval tv;
int r;
FD_ZERO (&fds);
FD_SET (fd, &fds);
/* Timeout. */
tv.tv_sec = 2;
tv.tv_usec = 0;
r = select (fd + 1, &fds, NULL, NULL, &tv);
if (-1 == r) {
if (EINTR == errno)
continue;
errno_exit ("select");
}
if (0 == r) {
fprintf (stderr, "select timeout\n");
exit (EXIT_FAILURE);
}
if (read_frame ()) {
// process the video
process_video(pixels, processedPixels);
// convert grayscale processed pixels to an RGB format
convert_grayscale_to_rgb(pixels, processedPixels,FRAME_WIDTH, FRAME_HEIGHT);
// process blobs
int labels[FRAME_HEIGHT][FRAME_WIDTH];
for(i = 0; i < FRAME_HEIGHT; i++) {
for(j = 0; j < FRAME_WIDTH; j++) {
labels[i][j] = -1;
}
}
int numBlobs = two_pass(processedPixels, labels, FRAME_WIDTH, FRAME_HEIGHT);
blob blobs[MAX_BLOBS];
extract_blobs(blobs, numBlobs, labels, FRAME_WIDTH, FRAME_HEIGHT);
// remove too small/big blobs
numBlobs = apply_blob_size_heuristic(blobs, numBlobs);
// get all center points for blobs
coord centerCoords[MAX_BLOBS];// = (coord*)malloc(sizeof(coord) * numBlobs);
get_blob_centers(blobs, numBlobs, centerCoords);
collinear points[MAX_BLOBS];
int shortSegments = get_more_straight_sides(centerCoords, numBlobs, points);
coord shapeCoords[5];
int shapeFound = 0;
for(i = 0; i < shortSegments; i++) {
coord p1 = points[i].point1;
coord p2 = points[i].point2;
coord p3 = points[i].point3;
double m1 = fabs(distance(p1, p2));
double m2 = fabs(distance(p2, p3));
double m3 = fabs(distance(p1, p3));
if(is_long_side(points[i])) {
collinear shortSide;
int shortSideFound = get_short_side(centerCoords, numBlobs, points[i], &shortSide);
if(shortSideFound) {
draw_collinear(pixels, shortSide);
draw_collinear(pixels, points[i]);
/*print_point(p1);
print_point(p2);
print_point(p3); */
//printf(" -------------------- SHORT SIDE \n");
/*
print_point(shortSide.point1);
print_point(shortSide.point2);
print_point(shortSide.point3);*/
printf("\n");
shapeCoords[0] = p1;
shapeCoords[1] = p2;
shapeCoords[2] = p3;
shapeCoords[3] = shortSide.point1;
shapeCoords[4] = shortSide.point3;
shapeFound = 1;
break;
}
} else if(is_short_side(points[i])) {
collinear longSide;
int longSideFound = get_long_side(centerCoords, numBlobs, points[i], &longSide);
if(longSideFound) {
draw_collinear(pixels, longSide);
draw_collinear(pixels, points[i]);
/*
print_point(p1);
print_point(p2);
print_point(p3); */
//printf(" ======================== LONG SIDE\n");
/*
print_point(longSide.point1);
print_point(longSide.point2);
print_point(longSide.point3); */
printf("\n");
shapeCoords[0] = p1;
shapeCoords[1] = p2;
shapeCoords[2] = p3;
shapeCoords[3] = longSide.point1;
shapeCoords[4] = longSide.point3;
shapeFound = 1;
break;
}
}
}
if(shapeFound) {
int imageCenterX = FRAME_WIDTH / 2;
int imageCenterY = FRAME_HEIGHT / 2;
coord center;
center.x = imageCenterX;
center.y = imageCenterY;
double distance = distance_from_center(shapeCoords[1]);
double angle = quadrant_angle(shapeCoords[1]);
if(distance > 100) {
printf("MOVE TOWARDS CENTER :: ANGLE : %f :: DISTANCE : %f(pixels)\n",
angle, distance);
//print_direction(angle);
} else {
printf("MOVE DOWN HEIGHT : ?\n");
}
print_image(centerCoords, numBlobs, shapeCoords, 5);
}
free_blobs(blobs, numBlobs);
frame = create_cam_img(pixels, FRAME_WIDTH, FRAME_HEIGHT);
apply_surface(0, 0, frame, screen);
SDL_Flip(screen);
}
}
}
