main.cpp

00001 //
00002 // OV7670 + FIFO AL422B camera board test
00003 //
00004 #include "mbed.h"
00005 #include "ov7670.h"
00006 #include <ros.h>
00007 #include <ros/time.h>
00008 #include <std_msgs/UInt8MultiArray.h>
00009 #include <std_msgs/Byte.h>
00010 #include <std_msgs/Bool.h>
00011 
00012 std_msgs::UInt8MultiArray img;
00013 //std_msgs::Byte img;
00014 std_msgs::Bool sync;
00015 ros::NodeHandle nh;
00016 ros::Publisher img_pub("image_data", &img);
00017 ros::Publisher sync_pub("sync", &sync);
00018 
00019 
00020 
00021 OV7670 camera(
00022     D14,D15,       // SDA,SCL(I2C / SCCB)
00023     D9,D8,D11,   // VSYNC,HREF,WEN(FIFO)
00024     //p18,p17,p16,p15,p11,p12,p14,p13, // D7-D0
00025     PortB,0x00FF,
00026     D13,D12,D6,D2) ; // RRST,OE,RCLK,WRST
00027 
00028 //Serial pc(USBTX,USBRX,115200) ;
00029 
00030 #define NUMBYTE 480
00031 
00032 //#undef QQVGA
00033 #define QQVGA
00034 
00035 #ifdef QQVGA
00036 # define SIZEX (160)
00037 # define SIZEY (120)
00038 
00039 #else
00040 # define SIZEX (320)
00041 # define SIZEY (240)
00042 #endif
00043 
00044 
00045 uint8_t fdata[SIZEX*SIZEY];
00046 uint8_t fdata_ros[NUMBYTE];
00047 int loop_num;
00048 
00049 
00050 size_t size = SIZEX*SIZEY;
00051 
00052 int main()
00053 {   
00054     nh.initNode();
00055     loop_num = size/NUMBYTE;
00056     sync.data=true;
00057     wait_ms(1);
00058 //    pc.baud(115200) ;
00059     camera.Reset() ;
00060     nh.advertise(img_pub);
00061     nh.advertise(sync_pub);
00062 
00063 #ifdef QQVGA
00064 //    camera.InitQQVGA565(true,false) ;
00065     camera.InitQQVGAYUV(false,false);
00066 
00067 #else
00068     //camera.InitQVGA565(true,false) ;
00069 #endif
00070 
00071 
00072     // CAPTURE and SEND LOOP
00073 
00074     
00075     while(1) {
00076 
00077             
00078         camera.CaptureNext() ;
00079         while(camera.CaptureDone() == false) ;
00080 
00081         camera.ReadStart() ;
00082         
00083         sync.data=true;
00084         
00085         for (int y = 0; y < SIZEX*SIZEY ; y++) {
00086             camera.ReadOneByte();
00087             fdata[y]=camera.ReadOneByte();
00088                 
00089         }
00090         
00091 
00092         camera.ReadStop() ;
00093 
00094         for(int x=0; x< loop_num; x++) {
00095             for(int y=0; y < NUMBYTE; y++) {
00096                 fdata_ros[y]=fdata[(NUMBYTE*(x))+y];
00097             }
00098             
00099             sync_pub.publish(&sync);
00100             img.data_length = NUMBYTE;
00101             img.data = &fdata_ros[0];
00102             img_pub.publish(&img);
00103             nh.spinOnce();
00104             
00105             sync.data=false;
00106             wait_us(3125); // 25000us 12500
00107             
00108         }
00109 
00110        
00111     }
00112 }