//robot.c // //input = two cameras, serial data //output = serial data // #include /* Standard input/output definitions */ #include #include /* String function definitions */ #include /* UNIX standard function definitions */ #include /* File control definitions */ #include /* Error number definitions */ #include #include //#include #include //X Windows #include #include #include #include #include //Camera #include //need for struct video_window #include #include #include #define FALSE 0 #define TRUE 1 //Xwindows Display *xdisplay; Window xwindow; int xscreen; unsigned long xforeground, xbackground; XEvent xevent; GC xgc; Colormap cmap; XColor color, colorrgb; int depth; XImage *fximage; XImage *TitleImage; Visual *visual; char title[]="Robot Adventure"; char icon_title[]="Robot"; XFontStruct *font_info; char *font_name; char **font_path; int font_path_num; Pixmap pmap[3]; XImage *Img[3]; //tty FILE *input,*output; //Camera static int device_fd[3]; static struct video_capability vidcap[3]; static struct video_window vidwin[3]; static struct video_picture vidpic[3]; static struct video_clip vidclips[3][32]; //32? BITMAP *ibmp[3]; //eyes cameras FILE *fptr; int camera[5]; //0=camera off 1=camera on int getimages; //=1 get images 0=do not get images int showimages; int saveimages; int autobright; int numbt; int jpg_quality; FILE *jpgfile; struct tm *currenttime=NULL; time_t currenttimep; //time_t should have a ms (milliseconds) variable struct timeval *mtime; //for milliseconds char datestamp[255],timestamp[255]; char fname[255]; //file name for jpg unsigned char *buffer; #define BUFSIZE 70 char fifobuf[BUFSIZE]; //******************END UNIVERSE VARIABLES************* static void v4l_autobright (int dev, unsigned char *image, int height, int width) { struct video_picture vid_pic; int i, j=0, avg=0, offset=0; for (i=0; i 190 || avg < 64) { if (ioctl(dev, VIDIOCGPICT, &vid_pic)==-1) { perror("ioctl VIDIOCGPICT"); } if (avg > 190) { offset=avg-190; if (vid_pic.brightness > 100*offset) vid_pic.brightness-=100*offset; } if (avg < 64) { offset=64-avg; if (vid_pic.brightness < 65535-100*offset) vid_pic.brightness+=100*offset; } printf("auto_brightness: %d\n", vid_pic.brightness); if (ioctl(dev, VIDIOCSPICT, &vid_pic)==-1) { perror("ioctl VIDIOCSPICT"); } } } void put_jpeg (FILE *picture, char *image, int width, int height, int quality) { int y, x, line_width; JSAMPROW row_ptr[1]; struct jpeg_compress_struct cjpeg; struct jpeg_error_mgr jerr; unsigned char *line; line = malloc (width * 3); if (!line) return; cjpeg.err = jpeg_std_error(&jerr); jpeg_create_compress (&cjpeg); cjpeg.image_width = width; cjpeg.image_height= height; cjpeg.input_components = 3; cjpeg.in_color_space = JCS_RGB; jpeg_set_defaults (&cjpeg); jpeg_set_quality (&cjpeg, quality, TRUE); cjpeg.dct_method = JDCT_FASTEST; jpeg_stdio_dest (&cjpeg, picture); jpeg_start_compress (&cjpeg, TRUE); row_ptr[0] = line; line_width = width * 3; for ( y = 0; y < height; y++) { for (x = 0; x < line_width; x+=3) { line[x] = image[x+2]; line[x+1] = image[x+1]; line[x+2] = image[x]; } //fprintf(stderr,"here a\n"); jpeg_write_scanlines (&cjpeg, row_ptr, 1); // fprintf(stderr,"here b\n"); image += line_width; } jpeg_finish_compress (&cjpeg); jpeg_destroy_compress (&cjpeg); free (line); } //end put_jpeg //CAMERA FUNCTIONS int open_camera(const char *devicename,int numcam) { device_fd[numcam] = open(devicename, O_RDWR); if(device_fd[numcam] <= 0) { fprintf(stderr,"Device %s couldn't be opened\n", devicename); return 0; } return 1; } void close_camera(int numcam) { close(device_fd[numcam]); } void get_camera_info(int numcam) { ioctl(device_fd[numcam], VIDIOCGCAP, &vidcap[numcam]); ioctl(device_fd[numcam], VIDIOCGWIN, &vidwin[numcam]); ioctl(device_fd[numcam], VIDIOCGPICT, &vidpic[numcam]); vidwin[numcam].clips = vidclips[numcam]; vidwin[numcam].clipcount = 0; } void print_camera_info(int numcam) { fprintf(stderr," *** Camera Info ***\n\n"); fprintf(stderr,"Name: %s\n", vidcap[numcam].name); fprintf(stderr,"Type: %d\n", vidcap[numcam].type); fprintf(stderr,"Minimum Width: %d\n", vidcap[numcam].minwidth); fprintf(stderr,"Maximum Width: %d\n", vidcap[numcam].maxwidth); fprintf(stderr,"Minimum Height: %d\n", vidcap[numcam].minheight); fprintf(stderr,"Maximum Height: %d\n", vidcap[numcam].maxheight); fprintf(stderr,"X: %d\n", vidwin[numcam].x); fprintf(stderr,"Y: %d\n", vidwin[numcam].y); fprintf(stderr,"Width: %d\n", vidwin[numcam].width); fprintf(stderr,"Height: %d\n", vidwin[numcam].height); fprintf(stderr,"Depth: %d\n", vidpic[numcam].depth); if(vidcap[numcam].type & VID_TYPE_MONOCHROME) fprintf(stderr,"Color false\n"); else fprintf(stderr,"Color true\n"); } static void hexdump_data(const unsigned char *data, int len) { const int bytes_per_line = 32; char tmp[128]; int i = 0, k = 0; for(i = 0; len > 0; i++, len--) { if(i > 0 && ((i % bytes_per_line) == 0)) { fprintf(stderr,"%s\n", tmp); k = 0; } if ((i % bytes_per_line) == 0) k += sprintf(&tmp[k], "[%04x]: ", i); k += sprintf(&tmp[k], "%02x ", data[i]); } if (k > 0) fprintf(stderr,"%s\n", tmp); } BITMAP * init_bitmap(int w,int h) { BITMAP *bmap; BITMAPFILEHEADER *bmfh; //BITMAPFILEHEADER bmfh;//*bmfh; BITMAPINFOHEADER *bmih; int totalsize; totalsize=sizeof(BITMAPFILEHEADER)+sizeof(BITMAPINFOHEADER)+w*h*3; bmap=(BITMAP *)malloc(totalsize); bmap->bmf=malloc(sizeof(BITMAPFILEHEADER)); bmap->bmi=malloc(sizeof(BITMAPINFOHEADER)); memset(bmap->bmf,0,sizeof(BITMAPFILEHEADER)); memset(bmap->bmi,0,sizeof(BITMAPINFOHEADER)); //memset(bmap,0,sizeof(totalsize)); bmap->bmf->bfType=0x4d42; bmap->bmf->bfSize=sizeof(BITMAPFILEHEADER)+sizeof(BITMAPINFOHEADER)+w*h*3; bmap->bmf->bfOffBits=sizeof(BITMAPFILEHEADER)+sizeof(BITMAPINFOHEADER); bmap->bmi->biSize=sizeof(BITMAPINFOHEADER); bmap->bmi->biWidth=w; bmap->bmi->biHeight=h; bmap->bmi->biPlanes=1; bmap->bmi->biBitCount=24; bmap->bmi->biSizeImage=0;//w*h*3; return(bmap); } get_image() { //fprintf(stderr,"get_image called"); //the camera read takes time, and other commands are thrown off if (camera[0]>0) { if (autobright>0) { v4l_autobright(device_fd[0], buffer, vidcap[0].maxwidth, vidcap[0].maxheight); } numbt = read(device_fd[0], buffer, vidcap[0].maxwidth * vidcap[0].maxheight * 3); if (showimages) { Img[0]->data=buffer; XPutImage(xdisplay,pmap[0],xgc,Img[0],0,0,0,0,351,287); XCopyArea(xdisplay,pmap[0],xwindow,xgc,0,0,352,288,0,0); XFlush (xdisplay); } if (saveimages) { //save image as jpg currenttimep=time(NULL); currenttime=localtime(¤ttimep); gettimeofday(mtime,0); sprintf(datestamp,"%04d-%02d-%02d",currenttime->tm_year+1900,currenttime->tm_mon+1,currenttime->tm_mday); sprintf(timestamp,"%02d:%02d:%02d.%06d",currenttime->tm_hour,currenttime->tm_min,currenttime->tm_sec,mtime->tv_usec); strcpy(fname,"/root/robot/images/c0"); strcat(fname,datestamp); strcat(fname,"_"); strcat(fname,timestamp); strcat(fname,".jpg"); jpgfile=fopen(fname,"w"); put_jpeg (jpgfile, buffer, vidcap[0].maxwidth, vidcap[0].maxheight, jpg_quality); fclose(jpgfile); } } //camera[0]>0 if (camera[1]>0) { if (autobright>0) { v4l_autobright(device_fd[1], buffer, vidcap[1].maxwidth, vidcap[1].maxheight); } numbt = read(device_fd[1], buffer, vidcap[1].maxwidth * vidcap[1].maxheight * 3); if (showimages) { Img[1]->data=buffer; //copy image to pixmap XPutImage(xdisplay,pmap[1],xgc,Img[1],0,0,0,0,351,287); //copy pixmap to screen/window XCopyArea(xdisplay,pmap[1],xwindow,xgc,0,0,352,288,352,0); //352 XFlush (xdisplay); } if (saveimages) { //save image as jpg currenttimep=time(NULL); currenttime=localtime(¤ttimep); sprintf(datestamp,"%04d-%02d-%02d.%06d",currenttime->tm_year+1900,currenttime->tm_mon+1,currenttime->tm_mday); sprintf(timestamp,"%02d:%02d:%02d",currenttime->tm_hour,currenttime->tm_min,mtime->tv_usec); strcpy(fname,"/root/robot/images/c1"); strcat(fname,datestamp); strcat(fname,"_"); strcat(fname,timestamp); strcat(fname,".jpg"); jpgfile=fopen(fname,"w"); put_jpeg (jpgfile, buffer, vidcap[0].maxwidth, vidcap[0].maxheight, jpg_quality); fclose(jpgfile); } } //camera[1]>0 } main(int argc, char *argv[]) //argument count and values { unsigned char sk; char message[90]; int c,i; char key; unsigned char buf[255]; //buffer for where data is put unsigned int keyi; int a,ex; int len = 0; cell *tptr; int shft,nport; unsigned int pmask,ncom; int camok; int ififo,nb; //************************* // MALLOC/INIT VARIABLES //************************* font_path=(char **)malloc(1000); font_name=malloc(255); currenttime=malloc(sizeof(struct tm)); currenttimep=malloc(sizeof(time_t)); mtime=malloc(sizeof(struct timeval)); buffer=malloc(255); camera[0]=0; //one/first camera will always be /dev/video0 camera[1]=0; jpg_quality=95; //was 50 will need to uncompress to analyze getimages=0; showimages=0; saveimages=0; #if 0 //keyboard is captured in kernel module //************************* // KEYBOARD //*********************** input = fopen("/dev/tty", "r"); //open the terminal keyboard output = fopen("/dev/tty", "w"); //open the terminal screen if (!input || !output) { fprintf(stderr, "Unable to open /dev/tty\n"); exit(1); } #endif //******************************* // X WINDOWS //******************************* /* connect to the X server */ xdisplay = XOpenDisplay (""); if (xdisplay == NULL) { fprintf (stderr, "cannot connect to server\n"); exit (EXIT_FAILURE); } /* get default screen */ xscreen = DefaultScreen (xdisplay); /* get black and white representation on current screen */ xbackground = BlackPixel (xdisplay, xscreen); xforeground = WhitePixel (xdisplay, xscreen); /* Create window at (0,0), width 650, height 350, border width 2, in default root */ //camera= 176x144 352x288 xwindow = XCreateSimpleWindow (xdisplay, DefaultRootWindow(xdisplay), 0, 0, 704, 288, 2, xforeground, xbackground); if (xwindow == 0) { fprintf (stderr, "cannot open window\n"); exit (EXIT_FAILURE); } //strcpy(argv[0],""); //XSetStandardProperties(xdisplay,xwindow,title,icon_title,None,argv,argc,NULL); XSetStandardProperties(xdisplay,xwindow,title,icon_title,None,0,0,NULL); /* ask for exposure event */ XSelectInput(xdisplay, xwindow, ExposureMask|KeyPressMask|ButtonPressMask); /* pop this window up on the screen */ XMapRaised (xdisplay, xwindow); /* wait for the window showing up before continuing */ XNextEvent (xdisplay, &xevent); /* set graphics context of rectangle to red */ xgc= XCreateGC (xdisplay, xwindow, 0, 0); if (DisplayPlanes (xdisplay, xscreen) != 1) { cmap = DefaultColormap (xdisplay, xscreen); if (XAllocNamedColor (xdisplay, cmap, "white", &color, &colorrgb)) XSetForeground (xdisplay, xgc, color.pixel); } //font_name="*-helvetica-*-12-*"; strcpy(font_name,"*charter*"); font_info=XLoadQueryFont(xdisplay,font_name); if (!font_info) { fprintf(stderr, "Could not load font\n"); exit(1); } XSetFont(xdisplay,xgc,font_info->fid); depth = DefaultDepth(xdisplay, DefaultScreen(xdisplay)); visual=DefaultVisual(xdisplay,DefaultScreen(xdisplay)); //fximage=XCreateImage(xdisplay,visual,depth,ZPixmap,0,0,640,300,8,0); //640x3 %8? //fximage->data=(char *)malloc(fximage->bytes_per_lie*300+16); //pixmap[0] = XCreatePixmap(xdisplay, xwindow, 352,288, depth); //pixmap[1] = XCreatePixmap(xdisplay, xwindow, 640,350, depth); XFlush (xdisplay); if (camera[0]>0) { //initialize camera if(open_camera("/dev/video0",0) == 1) //argv[1]) == 1) { get_camera_info(0); print_camera_info(0); // read_test(0); } //#if 0 //352x288 or 176x144 ibmp[0]=init_bitmap(352,288); buffer =(unsigned char *) malloc(vidcap[0].maxwidth * vidcap[0].maxheight * 3); //len = read(device_fd[0], buffer, vidcap[0].maxwidth * vidcap[0].maxheight * 3); ibmp[0]->data=buffer; #if 0 fptr=fopen("test1.bmp","wb"); fwrite(&ibmp[0]->bmf->bfType,1,2,fptr); fwrite(&ibmp[0]->bmf->bfSize,1,4,fptr); fwrite(&ibmp[0]->bmf->bfReserved1,1,2,fptr); fwrite(&ibmp[0]->bmf->bfReserved2,1,2,fptr); fwrite(&ibmp[0]->bmf->bfOffBits,1,4,fptr); fwrite(&ibmp[0]->bmi->biSize,1,4,fptr); fwrite(&ibmp[0]->bmi->biWidth,1,4,fptr); fwrite(&ibmp[0]->bmi->biHeight,1,4,fptr); fwrite(&ibmp[0]->bmi->biPlanes,1,2,fptr); fwrite(&ibmp[0]->bmi->biBitCount,1,2,fptr); fwrite(&ibmp[0]->bmi->biCompression,1,4,fptr); fwrite(&ibmp[0]->bmi->biSizeImage,1,4,fptr); fwrite(&ibmp[0]->bmi->biXPelsPerMeter,1,4,fptr); fwrite(&ibmp[0]->bmi->biYPelsPerMeter,1,4,fptr); fwrite(&ibmp[0]->bmi->biClrUsed,1,4,fptr); fwrite(&ibmp[0]->bmi->biClrImportant,1,4,fptr); // fwrite(ibmp[0]->data,352*288*3,1,fptr); //is upside down for (a=ibmp[0]->bmi->biHeight-1;a>-1;a--) { //352*288 must be %32 fwrite((ibmp[0]->data+a*352*3),352*3,1,fptr); } fclose(fptr); #endif //#endif Img[0]= XCreateImage(xdisplay,visual,depth,ZPixmap,0,0,352,288,8,0); Img[0]->bytes_per_line=352*3;//1056; //352*3 Img[0]->bits_per_pixel=24; pmap[0]=XCreatePixmap(xdisplay,xwindow,352,288,depth); // fprintf(stderr,"depth=%d",Img[0]->bits_per_pixel); XFlush (xdisplay); } //end if camera1 if (camera[1]>0) //camera 2 { if(open_camera("/dev/video1",1) == 1) { get_camera_info(1); print_camera_info(1); // read_test(0,1); } //352x288 or 176x144 Img[1]= XCreateImage(xdisplay,visual,depth,ZPixmap,0,0,352,288,8,0); Img[1]->bytes_per_line=352*3;//1056; //352*3 Img[1]->bits_per_pixel=24; pmap[1]=XCreatePixmap(xdisplay,xwindow,352,288,depth); XFlush (xdisplay); } //end if camera2 //*** //test computer speed or set flag=test, then flag=notest with same loop //*** #if 0 //open fifos if ((ofif = open("/dev/rtf0", O_WRONLY)) < 0) { fprintf(stderr, "Error opening /dev/rtf0\n"); //exit(1); } if ((ifif = open("/dev/rtf1", O_RDONLY)) < 0) { fprintf(stderr, "Error opening /dev/rtf1\n"); //exit(1); } #endif if ((ififo = open("/dev/rtf0", O_RDONLY)) < 0) { fprintf(stderr, "Error opening /dev/rtf0\n"); //exit(1); } //************************************ // MAIN LOOP //************************************ ex=0; while (ex!=1) { //have to keep checking the fifo for getimage commands #if 0 if (FD_ISSET(fd0, &rfds)) { n = read(fd0, buf,1); buf[n] = 0; printf("FIFO 0: %s\n", buf); } #endif if(getimages>0) { //fprintf(stderr,"call to get_image"); get_image(); } //read (ififo, &rtm, sizeof(RTIME)); nb=read (ififo, &key, 1); if (nb>0) { fprintf(stderr,"%d\n",key); //fprintf(stderr,"%d %d\n",key,(int)rtm); if (key==1 || key==129) { ex=1; } } //red something #if 0 //kb get in kernel module if (XPending(xdisplay)) { XNextEvent (xdisplay, &xevent); //fprintf(stderr,"%d ",xevent.type); switch (xevent.type) { /* ignore this event */ case Expose: break; case ButtonPress: fprintf(stdout,"A mouse button was pressed.\n"); break; case KeyPress: Key=XLookupKeysym(&xevent.xkey,0); //fprintf(stderr,"key=%x \n",Key); if (Key==-30 || Key==-31) ex=0; //ignore shift keys switch (Key) { /* branch to appropiate key handler */ case 0x30: //0 //stop all motors keyi=0x0000; // addpicinst(curtime,keyi); //move motor if (write(ofif, &Key, 1) < 0) { //send 0 to fifo fprintf(stderr, "Can no send a command to RT-task\n"); //exit(1); } break; case 0x31: //1 #if 0 camera[0]=!camera[0]; if (camera[0]) { fprintf(stderr,"\nCamera 0 on\n"); } else { fprintf(stderr,"\nCamera 0 off\n"); } #endif write(ofif, &Key, 1); break; case 0x32: //2 #if 0 camera[1]=!camera[1]; if (camera[0]) { fprintf(stderr,"\nCamera 1 on\n"); } else { fprintf(stderr,"\nCamera 1 off\n"); } #endif write(ofif, &Key, 1); break; case 0x33: //3 write(ofif, &Key, 1); break; case 0x34: //4 write(ofif, &Key, 1); break; case 0x35: //5 write(ofif, &Key, 1); break; case 0x36: //6 write(ofif, &Key, 1); break; case 0x37: //7 write(ofif, &Key, 1); break; case 0x38: //8 write(ofif, &Key, 1); break; case 0x39: //9 write(ofif, &Key, 1); break; case 0x51: //left arrow if (write(ofif, &Key, 1) < 0) { fprintf(stderr, "Can no send a command to RT-task\n"); //exit(1); } break; case 0x52: //up arrow break; case 0x53: //right arrow write(ofif, &Key, 1); break; case 0x54: //down arrow break; case 0x61: //a autobright if (autobright==0) { autobright=1; fprintf(stderr,"Start Autobright\n"); } else { autobright=0; fprintf(stderr,"Stop Autobright\n"); } // write(ofif, &Key, 1); break; case 0x62: //b balance #if 0 balance=!balance; if (balance==1) { fprintf(stderr,"Balance ON\n"); for (a=0;a<4;a++) {bal[0].a[a]=0;} bal[0].numc=0; ballt=curtime; //balance last time= current time bal[0].kal[0]=0; bal[0].kal[1]=0; } else { fprintf(stderr,"Balance OFF\n"); } #endif write(ofif, &Key, 1); break; case 0x63: //c break; case 0x64: //d write(ofif, &Key, 1); break; case 0x65: //e write(ofif, &Key, 1); break; case 0x66: //f - get inputs from feet write(ofif, &Key, 1); break; case 0x68: //h half speed write(ofif, &Key, 1); break; case 0x69: //i start getting images //get_input(0x8); //get input for port 8 //write(ofif, &Key, 1); //for some reason getimages=!getimages; not functioning if (getimages==0) { getimages=1; fprintf(stderr,"Get Images %d\n",getimages); } else { getimages=0; fprintf(stderr,"Stop Get Images %d\n",getimages); } break; case 0x6a: //j write(ofif, &Key, 1); break; case 0x6b: //k write(ofif, &Key, 1); break; case 0x73: //s if (showimages==0) { showimages=1; fprintf(stderr,"Start Show Images\n"); } else { showimages=0; fprintf(stderr,"Stop Show Images\n"); } break; case 0x74: //t write(ofif, &Key, 1); break; case 0x75: //u undo walk step write(ofif, &Key, 1); break; case 0x76: //v if (saveimages==0) { saveimages=1; fprintf(stderr,"Start Save Images\n"); } else { saveimages=0; fprintf(stderr,"Stop Save Images\n"); } break; case 0x77: //w walk write(ofif, &Key, 1); break; case 0x78: //x write(ofif, &Key, 1); break; case 0x7a: //z write(ofif, &Key, 1); break; case 0x1b: /* Esc */ ex=1; //exit break; default: write(ofif, &Key, 1); //keyi[0]=Key; //fputc((int) Key,output); // write(fserial,&Key,1); //write 1 byte to the port break; } //end switch Key break; } //end switch Xevent } //end if Xevent Pending #endif //now get kb in kernel module //***************************** // END OF LOOP //***************************** } //while ex=0 // close(ofif); //close fifo /dev/rtf0 // close(ifif); //close fifo /dev/rtf1 if (camera[0]) { close_camera(0); //XFreePixmap(xdisplay,pmap[0]); } if (camera[1]) { close_camera(1); } close(ififo); // fclose(input); // fclose(output); return(0); } //end of main