Diff: main.cpp

Revision:

3:2a3664dc6634

Parent:

2:e98408458d2b

Child:

4:7b63cf3d205f

--- a/main.cpp	Mon Aug 12 07:12:38 2019 +0000
+++ b/main.cpp	Tue Aug 13 05:53:22 2019 +0000
@@ -3,7 +3,13 @@
 //
 #include "mbed.h"
 #include "ov7670.h"
+#include <ros.h>
+#include <ros/time.h>
+#include <std_msgs/UInt8MultiArray.h>
 
+std_msgs::UInt8MultiArray img;
+ros::NodeHandle nh;
+ros::Publisher img_pub("image_data", &img);
 
 
 OV7670 camera(
@@ -13,65 +19,72 @@
     PortB,0xFF,
     D13,D12,D6) ; // RRST,OE,RCLK
 
-Serial pc(USBTX,USBRX,115200) ;
+//Serial pc(USBTX,USBRX,115200) ;
 Timer t;
 
 //#undef QQVGA
 #define QQVGA
 
 #ifdef QQVGA
-# define SIZEX (160)
-# define SIZEY (120)
+# define SIZEX (12)
+# define SIZEY (8)
 #else
 # define SIZEX (320)
 # define SIZEY (240)
 #endif
 
-uint16_t fdata[SIZEX*SIZEY];
+
+uint8_t fdata[SIZEX*SIZEY];
+
+size_t size = SIZEX*SIZEY;
 
 int main() {
     int last ;
-    pc.baud(115200) ;
+//    pc.baud(115200) ;
     camera.Reset() ;
+    nh.advertise(img_pub);
 
 #ifdef QQVGA
 //    camera.InitQQVGA565(true,false) ;
-    camera.InitQVGAYUV(true,false);
+    camera.InitQVGAYUV(false,false);
 
 #else 
     //camera.InitQVGA565(true,false) ;
 #endif
-
+    
+    
     // CAPTURE and SEND LOOP
     t.start();
     last = t.read_ms() ;
-//    while(1)
-//    {
+    
+    
+    while(1)
+    {
+        
+        img.data_length = size;
+        
         camera.CaptureNext() ;
         while(camera.CaptureDone() == false) ;
-        printf("Caputure %d(ms)\r\n", t.read_ms() - last) ;
+
         last = t.read_ms() ;
         camera.ReadStart() ;
-//        lcd.Lcd_SetCursor(0,0);
-//        lcd.Lcd_WR_Start();
-//        lcd.rsout(1) ;
-        for (int y = 0; y < SIZEY*SIZEX;y++) {
-//            pc.printf("\r\n");
- //           lcd.Lcd_SetCursor(y,0);
- 
-//            lcd.Lcd_WR_Start();
-            for (int x = 0;x < SIZEX;x++) {
-//             pc.printf("%x ",camera.ReadOneWord());
-              //  lcd.csout(0) ;
-//                lcd.DataToWrite(camera.ReadOneWord());
-//                lcd.csout(1) ;
-            fdata[SIZEX*SIZEY]=camera.ReadOneWord();
-            }
+        
+        
+        for (int y = 0; y < SIZEX*SIZEY ; y++) {
+                
+            camera.ReadOneByte();
+            fdata[y]=camera.ReadOneByte();
+
         }
-   
+        
         camera.ReadStop() ; 
-        printf("%d\n", sizeof(fdata));
-        printf("FIFO Read & Lcd Out %d(ms)\r\n", t.read_ms() - last) ;
         last = t.read_ms() ;
- //   }
+        
+        img.data = &fdata[0]; 
+        img_pub.publish(&img);
+        
+
+        nh.spinOnce();
+        wait_ms(1); //max...
+    }
 }