Diff: main.cpp

Revision:

9:ccd662244071

Parent:

8:221b2fc093e4

Child:

10:ca4e4062701a

--- a/main.cpp	Fri Sep 06 02:09:38 2019 +0000
+++ b/main.cpp	Thu Nov 21 10:36:11 2019 +0000
@@ -6,9 +6,11 @@
 #include <ros.h>
 #include <ros/time.h>
 #include <std_msgs/UInt8MultiArray.h>
+#include <std_msgs/Byte.h>
 #include <std_msgs/Bool.h>
 
 std_msgs::UInt8MultiArray img;
+//std_msgs::Byte img;
 std_msgs::Bool sync;
 ros::NodeHandle nh;
 ros::Publisher img_pub("image_data", &img);
@@ -25,6 +27,7 @@
 
 //Serial pc(USBTX,USBRX,115200) ;
 
+#define NUMBYTE 480
 
 //#undef QQVGA
 #define QQVGA
@@ -32,7 +35,7 @@
 #ifdef QQVGA
 # define SIZEX (160)
 # define SIZEY (120)
-# define NUMBYTE (2)
+
 #else
 # define SIZEX (320)
 # define SIZEY (240)
@@ -40,14 +43,17 @@
 
 
 uint8_t fdata[SIZEX*SIZEY];
-uint8_t fdata_ros[480];
-
+uint8_t fdata_ros[NUMBYTE];
+int loop_num;
 
 
 size_t size = SIZEX*SIZEY;
 
 int main()
-{   sync.data=true;
+{   
+    nh.initNode();
+    loop_num = size/NUMBYTE;
+    sync.data=true;
     wait_ms(1);
 //    pc.baud(115200) ;
     camera.Reset() ;
@@ -74,30 +80,32 @@
 
         camera.ReadStart() ;
         
-      
+        sync.data=true;
+        
         for (int y = 0; y < SIZEX*SIZEY ; y++) {
             camera.ReadOneByte();
             fdata[y]=camera.ReadOneByte();
+                
         }
-       sync.data=true;
-       
+        
 
         camera.ReadStop() ;
-        
-       for(int x=0; x<40; x++) {
-            for(int y=0; y<480; y++) {
-                fdata_ros[y]=fdata[(480*(x))+y];
+
+        for(int x=0; x< loop_num; x++) {
+            for(int y=0; y < NUMBYTE; y++) {
+                fdata_ros[y]=fdata[(NUMBYTE*(x))+y];
             }
+            
             sync_pub.publish(&sync);
-            img.data_length = 480;
+            img.data_length = NUMBYTE;
             img.data = &fdata_ros[0];
             img_pub.publish(&img);
             nh.spinOnce();
-            sync.data=false;
-            wait_ms(600); //max...
             
-       }
-
+            sync.data=false;
+            wait_us(3125); // 25000us 12500
+            
+        }
 
        
     }