Diff: PROJ515.cpp

Revision:

6:40fc84f50432

Parent:

4:f74d88f5f629

Child:

7:6c26fdb1d226

--- a/PROJ515.cpp	Mon May 06 16:36:51 2019 +0000
+++ b/PROJ515.cpp	Tue May 07 09:24:58 2019 +0000
@@ -5,12 +5,12 @@
 float cfDist3 = 0;     // Distance returned by cliff u_sensor3 (m)
 float cfDist4 = 0;     // Distance returned by cliff u_sensor4 (m)
 
-float cbDist1 = 123456;     // Distance returned by curb  u_sensor1 (m)
+float cbDist1 = 0;     // Distance returned by curb  u_sensor1 (m)
 float cbDist2 = 0;     // Distance returned by curb  u_sensor2 (m)
 float cbDist3 = 0;     // Distance returned by curb  u_sensor3 (m)
 float cbDist4 = 0;     // Distance returned by curb  u_sensor4 (m)
 
-float wPeriod = 5;   // Wait period(ms) for ROS publisher to finish publishing
+float wPeriod = 18;   // Wait period(ms) for ROS publisher to finish publishing
 
 
 void setupRosMsg()
@@ -71,9 +71,9 @@
     /* Check the Cliff Sensors (If distance more than clif treshold,
     hardcode the cliff value in, else no obstacle)*/
     if (static_cast<double>(cfDist1) <= CLIFF_TRH) {
-        cfDist1 = EXCL_ZONE;
+        cfDist1 = CLIFF_ZRO;
     } else {
-        cfDist1 = CLIFF_ZRO;
+        cfDist1 = EXCL_ZONE;
     }
 //    if (static_cast<double>(cfDist2) <= CLIFF_TRH) {
 //        cfDist2 = EXCL_ZONE;
@@ -197,7 +197,18 @@
         ultrasound_msg_cf1.range = cfDist1;
         ultrasound_msg_cb1.range = cbDist1;
         nh.spinOnce();                           // Reccuring connect and synchronise
-        populateRosStamp();                      // Put time into ROS messages
-        publishRosMsg();                         // Publish ROS messages
+        //populateRosStamp();                      // Put time into ROS messages
+        //publishRosMsg();                         // Publish ROS messages
+        
+        ultrasound_msg_cf1.header.stamp = nh.now();
+    ultrasound_pub_cf1.publish(&ultrasound_msg_cf1);
+    wait_ms(wPeriod);
+       // wait(1);
+        
+            ultrasound_msg_cb1.header.stamp = nh.now();
+    ultrasound_pub_cb1.publish(&ultrasound_msg_cb1);
+    wait_ms(wPeriod);
+      //  wait(1);
+        
     }
 }
\ No newline at end of file