PROJ515_USS_GPS - Q

Users » Luka_Danilovic » Code » PROJ515_USS_GPS
Luka Danilovic / Mbed OS PROJ515_USS_GPS
Dependencies: SRF05 ros_lib_kinetic TinyGPSPlus
Diff: PROJ515.cpp

Revision:: 7:6c26fdb1d226
Parent:: 6:40fc84f50432
Child:: 8:49306a01c52a
diff -r 40fc84f50432 -r 6c26fdb1d226 PROJ515.cpp
--- a/PROJ515.cpp	Tue May 07 09:24:58 2019 +0000
+++ b/PROJ515.cpp	Tue May 07 11:49:14 2019 +0000
@@ -10,60 +10,60 @@
 float cbDist3 = 0;     // Distance returned by curb  u_sensor3 (m)
 float cbDist4 = 0;     // Distance returned by curb  u_sensor4 (m)
 
-float wPeriod = 18;   // Wait period(ms) for ROS publisher to finish publishing
+float wPeriod = 10;    // Wait period(ms) for ROS publisher to finish publishing
 
 
 void setupRosMsg()
 {
     // Frame ID
     ultrasound_msg_cf1.header.frame_id = "uss_cf1";
-    //ultrasound_msg_cf2.header.frame_id = "uss_cf2";
-//    ultrasound_msg_cf3.header.frame_id = "uss_cf3";
-//    ultrasound_msg_cf4.header.frame_id = "uss_cf4";
+    ultrasound_msg_cf2.header.frame_id = "uss_cf2";
+    ultrasound_msg_cf3.header.frame_id = "uss_cf3";
+    ultrasound_msg_cf4.header.frame_id = "uss_cf4";
     ultrasound_msg_cb1.header.frame_id = "uss_cb1";
-//    ultrasound_msg_cb2.header.frame_id = "uss_cb2";
-//    ultrasound_msg_cb3.header.frame_id = "uss_cb3";
-//    ultrasound_msg_cb4.header.frame_id = "uss_cb4";
+    ultrasound_msg_cb2.header.frame_id = "uss_cb2";
+    ultrasound_msg_cb3.header.frame_id = "uss_cb3";
+    ultrasound_msg_cb4.header.frame_id = "uss_cb4";
 
     // Radiation Type
     ultrasound_msg_cf1.radiation_type = ULTRASOUND;
-//    ultrasound_msg_cf2.radiation_type = ULTRASOUND;
-//    ultrasound_msg_cf3.radiation_type = ULTRASOUND;
-//    ultrasound_msg_cf4.radiation_type = ULTRASOUND;
+    ultrasound_msg_cf2.radiation_type = ULTRASOUND;
+    ultrasound_msg_cf3.radiation_type = ULTRASOUND;
+    ultrasound_msg_cf4.radiation_type = ULTRASOUND;
     ultrasound_msg_cb1.radiation_type = ULTRASOUND;
-//    ultrasound_msg_cb2.radiation_type = ULTRASOUND;
-//    ultrasound_msg_cb3.radiation_type = ULTRASOUND;
-//    ultrasound_msg_cb4.radiation_type = ULTRASOUND;
+    ultrasound_msg_cb2.radiation_type = ULTRASOUND;
+    ultrasound_msg_cb3.radiation_type = ULTRASOUND;
+    ultrasound_msg_cb4.radiation_type = ULTRASOUND;
 
     // Field of View
     ultrasound_msg_cf1.field_of_view = FOV;
-//    ultrasound_msg_cf2.field_of_view = FOV;
-//    ultrasound_msg_cf3.field_of_view = FOV;
-//    ultrasound_msg_cf4.field_of_view = FOV;
+    ultrasound_msg_cf2.field_of_view = FOV;
+    ultrasound_msg_cf3.field_of_view = FOV;
+    ultrasound_msg_cf4.field_of_view = FOV;
     ultrasound_msg_cb1.field_of_view = FOV;
-//    ultrasound_msg_cb2.field_of_view = FOV;
-//    ultrasound_msg_cb3.field_of_view = FOV;
-//    ultrasound_msg_cb4.field_of_view = FOV;
+    ultrasound_msg_cb2.field_of_view = FOV;
+    ultrasound_msg_cb3.field_of_view = FOV;
+    ultrasound_msg_cb4.field_of_view = FOV;
 
     // Minumum Range
     ultrasound_msg_cf1.min_range = MIN_RANGE;
-//    ultrasound_msg_cf2.min_range = MIN_RANGE;
-//    ultrasound_msg_cf3.min_range = MIN_RANGE;
-//    ultrasound_msg_cf4.min_range = MIN_RANGE;
+    ultrasound_msg_cf2.min_range = MIN_RANGE;
+    ultrasound_msg_cf3.min_range = MIN_RANGE;
+    ultrasound_msg_cf4.min_range = MIN_RANGE;
     ultrasound_msg_cb1.min_range = MIN_RANGE;
-//    ultrasound_msg_cb2.min_range = MIN_RANGE;
-//    ultrasound_msg_cb3.min_range = MIN_RANGE;
-//    ultrasound_msg_cb4.min_range = MIN_RANGE;
+    ultrasound_msg_cb2.min_range = MIN_RANGE;
+    ultrasound_msg_cb3.min_range = MIN_RANGE;
+    ultrasound_msg_cb4.min_range = MIN_RANGE;
 
     // Maximum Range
     ultrasound_msg_cf1.max_range = MAX_RANGE;
-//    ultrasound_msg_cf2.max_range = MAX_RANGE;
-//    ultrasound_msg_cf3.max_range = MAX_RANGE;
-//    ultrasound_msg_cf4.max_range = MAX_RANGE;
+    ultrasound_msg_cf2.max_range = MAX_RANGE;
+    ultrasound_msg_cf3.max_range = MAX_RANGE;
+    ultrasound_msg_cf4.max_range = MAX_RANGE;
     ultrasound_msg_cb1.max_range = MAX_RANGE;
-//    ultrasound_msg_cb2.max_range = MAX_RANGE;
-//    ultrasound_msg_cb3.max_range = MAX_RANGE;
-//    ultrasound_msg_cb4.max_range = MAX_RANGE;
+    ultrasound_msg_cb2.max_range = MAX_RANGE;
+    ultrasound_msg_cb3.max_range = MAX_RANGE;
+    ultrasound_msg_cb4.max_range = MAX_RANGE;
 }
 
 void checkCliff()
@@ -75,47 +75,47 @@
     } else {
         cfDist1 = EXCL_ZONE;
     }
-//    if (static_cast<double>(cfDist2) <= CLIFF_TRH) {
-//        cfDist2 = EXCL_ZONE;
-//    } else {
-//        cfDist2 = CLIFF_ZRO;
-//    }
-//    if (static_cast<double>(cfDist3) <= CLIFF_TRH) {
-//        cfDist3 = EXCL_ZONE;
-//    } else {
-//        cfDist3 = CLIFF_ZRO;
-//    }
-//    if (static_cast<double>(cfDist4) <= CLIFF_TRH) {
-//        cfDist4 = EXCL_ZONE;
-//    } else {
-//        cfDist4 = CLIFF_ZRO;
-//    }
+    if (static_cast<double>(cfDist2) <= CLIFF_TRH) {
+        cfDist2 = CLIFF_ZRO;
+    } else {
+        cfDist2 = EXCL_ZONE;
+    }
+    if (static_cast<double>(cfDist3) <= CLIFF_TRH) {
+        cfDist3 = CLIFF_ZRO;
+    } else {
+        cfDist3 = EXCL_ZONE;
+    }
+    if (static_cast<double>(cfDist4) <= CLIFF_TRH) {
+        cfDist4 = CLIFF_ZRO;
+    } else {
+        cfDist4 = EXCL_ZONE;
+    }
 }
 
 void populateRosDist()
 {
     // Put distance in ROS messages
     ultrasound_msg_cf1.range = cfDist1;
-   // ultrasound_msg_cf2.range = cfDist2;
-//    ultrasound_msg_cf3.range = cfDist3;
-//    ultrasound_msg_cf4.range = cfDist4;
+    ultrasound_msg_cf2.range = cfDist2;
+    ultrasound_msg_cf3.range = cfDist3;
+    ultrasound_msg_cf4.range = cfDist4;
     ultrasound_msg_cb1.range = cbDist1;
-//    ultrasound_msg_cb2.range = cbDist2;
-//    ultrasound_msg_cb3.range = cbDist3;
-//    ultrasound_msg_cb4.range = cbDist4;
+    ultrasound_msg_cb2.range = cbDist2;
+    ultrasound_msg_cb3.range = cbDist3;
+    ultrasound_msg_cb4.range = cbDist4;
 }
 
 void populateRosStamp()
 {
     // Get current time and put into ROS messages
     ultrasound_msg_cf1.header.stamp = nh.now();
-//    ultrasound_msg_cf2.header.stamp = nh.now();
-//    ultrasound_msg_cf3.header.stamp = nh.now();
-//    ultrasound_msg_cf4.header.stamp = nh.now();
+    ultrasound_msg_cf2.header.stamp = nh.now();
+    ultrasound_msg_cf3.header.stamp = nh.now();
+    ultrasound_msg_cf4.header.stamp = nh.now();
     ultrasound_msg_cb1.header.stamp = nh.now();
-//    ultrasound_msg_cb2.header.stamp = nh.now();
-//    ultrasound_msg_cb3.header.stamp = nh.now();
-//    ultrasound_msg_cb4.header.stamp = nh.now();
+    ultrasound_msg_cb2.header.stamp = nh.now();
+    ultrasound_msg_cb3.header.stamp = nh.now();
+    ultrasound_msg_cb4.header.stamp = nh.now();
 }
 
 void publishRosMsg()
@@ -124,27 +124,27 @@
     ultrasound_msg_cf1.header.stamp = nh.now();
     ultrasound_pub_cf1.publish(&ultrasound_msg_cf1);
     wait_ms(wPeriod);
-//    ultrasound_msg_cf2.header.stamp = nh.now();
-//    ultrasound_pub_cf2.publish(&ultrasound_msg_cf2);
-//    wait_ms(wPeriod);
-//    ultrasound_msg_cf3.header.stamp = nh.now();
-//    ultrasound_pub_cf3.publish(&ultrasound_msg_cf3);
-//    wait_ms(wPeriod);
-//    ultrasound_msg_cf4.header.stamp = nh.now();
-//    ultrasound_pub_cf4.publish(&ultrasound_msg_cf4);
-//    wait_ms(wPeriod);
+    ultrasound_msg_cf2.header.stamp = nh.now();
+    ultrasound_pub_cf2.publish(&ultrasound_msg_cf2);
+    wait_ms(wPeriod);
+    ultrasound_msg_cf3.header.stamp = nh.now();
+    ultrasound_pub_cf3.publish(&ultrasound_msg_cf3);
+    wait_ms(wPeriod);
+    ultrasound_msg_cf4.header.stamp = nh.now();
+    ultrasound_pub_cf4.publish(&ultrasound_msg_cf4);
+    wait_ms(wPeriod);
     ultrasound_msg_cb1.header.stamp = nh.now();
     ultrasound_pub_cb1.publish(&ultrasound_msg_cb1);
     wait_ms(wPeriod);
-//    ultrasound_msg_cb2.header.stamp = nh.now();
-//    ultrasound_pub_cb2.publish(&ultrasound_msg_cb2);
-//    wait_ms(wPeriod);
-//    ultrasound_msg_cb3.header.stamp = nh.now();
-//    ultrasound_pub_cb3.publish(&ultrasound_msg_cb3);
-//    wait_ms(wPeriod);
-//    ultrasound_msg_cb4.header.stamp = nh.now();
-//    ultrasound_pub_cb4.publish(&ultrasound_msg_cb4);
-//    wait_ms(wPeriod);
+    ultrasound_msg_cb2.header.stamp = nh.now();
+    ultrasound_pub_cb2.publish(&ultrasound_msg_cb2);
+    wait_ms(wPeriod);
+    ultrasound_msg_cb3.header.stamp = nh.now();
+    ultrasound_pub_cb3.publish(&ultrasound_msg_cb3);
+    wait_ms(wPeriod);
+    ultrasound_msg_cb4.header.stamp = nh.now();
+    ultrasound_pub_cb4.publish(&ultrasound_msg_cb4);
+    wait_ms(wPeriod);
 }
 
 int main()
@@ -153,13 +153,13 @@
     nh.initNode();                               // Initialise ROS Node Handler
 
     nh.advertise(ultrasound_pub_cf1);            // Adverstise Ultrasound topic
-//    nh.advertise(ultrasound_pub_cf2);            // Adverstise Ultrasound topic
-//    nh.advertise(ultrasound_pub_cf3);            // Adverstise Ultrasound topic
-//    nh.advertise(ultrasound_pub_cf4);            // Adverstise Ultrasound topic
+    nh.advertise(ultrasound_pub_cf2);            // Adverstise Ultrasound topic
+    nh.advertise(ultrasound_pub_cf3);            // Adverstise Ultrasound topic
+    nh.advertise(ultrasound_pub_cf4);            // Adverstise Ultrasound topic
     nh.advertise(ultrasound_pub_cb1);            // Adverstise Ultrasound topic
-//    nh.advertise(ultrasound_pub_cb2);            // Adverstise Ultrasound topic
-//    nh.advertise(ultrasound_pub_cb3);            // Adverstise Ultrasound topic
-//    nh.advertise(ultrasound_pub_cb4);            // Adverstise Ultrasound topic
+    nh.advertise(ultrasound_pub_cb2);            // Adverstise Ultrasound topic
+    nh.advertise(ultrasound_pub_cb3);            // Adverstise Ultrasound topic
+    nh.advertise(ultrasound_pub_cb4);            // Adverstise Ultrasound topic
 
     setupRosMsg();
 
@@ -171,23 +171,23 @@
 
         // Start the Sensors
         USS_CF1.start();
-//        USS_CF2.start();
-//        USS_CF3.start();
-//        USS_CF4.start();
+        USS_CF2.start();
+        USS_CF3.start();
+        USS_CF4.start();
         USS_CB1.start();
-//        USS_CB2.start();
-//        USS_CB3.start();
-//        USS_CB4.start();
+        USS_CB2.start();
+        USS_CB3.start();
+        USS_CB4.start();
         
         // Record the Distances (meters)
         cfDist1 = USS_CF1.get_dist_cm();
-//        cfDist2 = USS_CF2.get_dist_cm();
-//        cfDist3 = USS_CF3.get_dist_cm();
-//        cfDist4 = USS_CF4.get_dist_cm();
+        cfDist2 = USS_CF2.get_dist_cm();
+        cfDist3 = USS_CF3.get_dist_cm();
+        cfDist4 = USS_CF4.get_dist_cm();
         cbDist1 = USS_CB1.get_dist_cm();
-//        cbDist2 = USS_CB2.get_dist_cm();
-//        cbDist3 = USS_CB3.get_dist_cm();
-//        cbDist4 = USS_CB4.get_dist_cm();
+        cbDist2 = USS_CB2.get_dist_cm();
+        cbDist3 = USS_CB3.get_dist_cm();
+        cbDist4 = USS_CB4.get_dist_cm();
 
         /* Check the Cliff Sensors (If distance more than cliff treshold,
         hardcode the cliff value in, else no obstacle)*/
@@ -195,7 +195,14 @@
 
         //populateRosDist();                       // Put distance in ROS messages
         ultrasound_msg_cf1.range = cfDist1;
+        ultrasound_msg_cf2.range = cfDist2;
+        ultrasound_msg_cf3.range = cfDist3;
+        ultrasound_msg_cf4.range = cfDist4;
         ultrasound_msg_cb1.range = cbDist1;
+        ultrasound_msg_cb2.range = cbDist2;
+        ultrasound_msg_cb3.range = cbDist3;
+        ultrasound_msg_cb4.range = cbDist4;
+        
         nh.spinOnce();                           // Reccuring connect and synchronise
         //populateRosStamp();                      // Put time into ROS messages
         //publishRosMsg();                         // Publish ROS messages
@@ -203,12 +210,36 @@
         ultrasound_msg_cf1.header.stamp = nh.now();
     ultrasound_pub_cf1.publish(&ultrasound_msg_cf1);
     wait_ms(wPeriod);
-       // wait(1);
+       
+       ultrasound_msg_cf2.header.stamp = nh.now();
+    ultrasound_pub_cf2.publish(&ultrasound_msg_cf2);
+    wait_ms(wPeriod);
+    
+        ultrasound_msg_cf3.header.stamp = nh.now();
+    ultrasound_pub_cf3.publish(&ultrasound_msg_cf3);
+    wait_ms(wPeriod);
+    
+        ultrasound_msg_cf4.header.stamp = nh.now();
+    ultrasound_pub_cf4.publish(&ultrasound_msg_cf4);
+    wait_ms(wPeriod);
         
-            ultrasound_msg_cb1.header.stamp = nh.now();
+        ultrasound_msg_cb1.header.stamp = nh.now();
     ultrasound_pub_cb1.publish(&ultrasound_msg_cb1);
     wait_ms(wPeriod);
-      //  wait(1);
-        
+    
+        ultrasound_msg_cb2.header.stamp = nh.now();
+    ultrasound_pub_cb2.publish(&ultrasound_msg_cb2);
+    wait_ms(wPeriod);
+    
+        ultrasound_msg_cb3.header.stamp = nh.now();
+    ultrasound_pub_cb3.publish(&ultrasound_msg_cb3);
+    wait_ms(wPeriod);
+    
+        ultrasound_msg_cb4.header.stamp = nh.now();
+    ultrasound_pub_cb4.publish(&ultrasound_msg_cb4);
+    wait_ms(wPeriod);
+    
+    wait_ms(15);
+                
     }
 }
\ No newline at end of file
Repository toolbox

Export to desktop IDE
Build repository
Repository details

Type:	Program
Mbed OS support:	Mbed OS
Created:	11 May 2019
Imports:	1
Forks:	0
Commits:	16
Dependents:	0
Dependencies:	3
Followers:	1
The code in this repository is Apache licensed.
Diff: PROJ515.cpp

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning
