Diff: main.cpp

Revision:

2:af928306ca36

Parent:

1:ea4150fafa3b

Child:

3:0de505949008

--- a/main.cpp	Mon Aug 14 09:12:34 2017 +0000
+++ b/main.cpp	Mon Aug 14 09:47:07 2017 +0000
@@ -103,6 +103,8 @@
     nh.advertise(pub_G2);
 }
 
+float pX1 = 0, pX2 = 0, pY1 = 0, pY2 = 0, pG1 = 0, pG2 = 0, pG3 = 0;
+
 int main() {
     nodeInit();
     
@@ -120,13 +122,35 @@
         G1.data = g[0];
         G2.data = g[1];
         G3.data = g[2];
-        pub_X1.publish( &X1 );
-        pub_Y1.publish( &Y1 );
-        pub_X2.publish( &X2 );
-        pub_Y2.publish( &Y2 );
-        pub_G1.publish( &G1 );
-        pub_G2.publish (&G2 );
-        pub_G3.publish( &G3 );
+        
+        if(pX1 != X1.data){
+            pX1 = X1.data;
+            pub_X1.publish( &X1 );
+        }
+        if(pX2 != X2.data){
+            pX2 = X2.data;
+            pub_X2.publish( &X2 );
+        }
+        if(pY1 != Y1.data){
+            pY1 = Y1.data;
+            pub_Y1.publish( &Y1 );
+        }
+        if(pY2 != Y2.data){
+            pY2 = Y2.data;
+            pub_Y2.publish( &Y2 );
+        }
+        if(pG1 != G1.data){
+            pG1 = G1.data;
+            pub_G1.publish( &G1 );
+        }
+        if(pG2 != G2.data){
+            pG2 = G2.data;
+            pub_G2.publish( &G2 );
+        }
+        if(pG3 != G3.data){
+            pG3 = G3.data;
+            pub_G3.publish( &G3 );
+        }
         nh.spinOnce();
         wait_ms(50); // 20hz
     }