Diff: PROJ515.cpp

Revision:

14:b972d0d0f63d

Parent:

12:1a3272d67500

--- a/PROJ515.cpp	Sat May 11 17:52:33 2019 +0000
+++ b/PROJ515.cpp	Tue May 14 15:34:59 2019 +0000
@@ -9,6 +9,8 @@
 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 cbDist5 = 0;     // Distance returned by curb  u_sensor5 (m)
+float cbDist6 = 0;     // Distance returned by curb  u_sensor6 (m)
 
 float wPeriod = 10;    // Wait period(ms) for ROS publisher to finish publishing
 
@@ -90,6 +92,8 @@
     nh.advertise(ultrasound_pub_cb2);
     nh.advertise(ultrasound_pub_cb3);
     nh.advertise(ultrasound_pub_cb4);
+    nh.advertise(ultrasound_pub_cb5);
+    nh.advertise(ultrasound_pub_cb6);
 }
 
 void setupRosMsg()
@@ -105,6 +109,8 @@
     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_cb5.header.frame_id = "uss_cb5";
+    ultrasound_msg_cb6.header.frame_id = "uss_cb6";
 
     // Radiation Type
     ultrasound_msg_cf1.radiation_type = ULTRASOUND;
@@ -115,6 +121,8 @@
     ultrasound_msg_cb2.radiation_type = ULTRASOUND;
     ultrasound_msg_cb3.radiation_type = ULTRASOUND;
     ultrasound_msg_cb4.radiation_type = ULTRASOUND;
+    ultrasound_msg_cb5.radiation_type = ULTRASOUND;
+    ultrasound_msg_cb6.radiation_type = ULTRASOUND;
 
     // Field of View
     ultrasound_msg_cf1.field_of_view = FOV;
@@ -125,7 +133,9 @@
     ultrasound_msg_cb2.field_of_view = FOV;
     ultrasound_msg_cb3.field_of_view = FOV;
     ultrasound_msg_cb4.field_of_view = FOV;
-
+    ultrasound_msg_cb5.field_of_view = FOV;
+    ultrasound_msg_cb6.field_of_view = FOV;
+    
     // Minumum Range
     ultrasound_msg_cf1.min_range = MIN_RANGE;
     ultrasound_msg_cf2.min_range = MIN_RANGE;
@@ -135,6 +145,8 @@
     ultrasound_msg_cb2.min_range = MIN_RANGE;
     ultrasound_msg_cb3.min_range = MIN_RANGE;
     ultrasound_msg_cb4.min_range = MIN_RANGE;
+    ultrasound_msg_cb5.min_range = MIN_RANGE;
+    ultrasound_msg_cb6.min_range = MIN_RANGE;
 
     // Maximum Range
     ultrasound_msg_cf1.max_range = MAX_RANGE;
@@ -145,6 +157,8 @@
     ultrasound_msg_cb2.max_range = MAX_RANGE;
     ultrasound_msg_cb3.max_range = MAX_RANGE;
     ultrasound_msg_cb4.max_range = MAX_RANGE;
+    ultrasound_msg_cb5.max_range = MAX_RANGE;
+    ultrasound_msg_cb6.max_range = MAX_RANGE;
 }
 
 void startSensors()
@@ -158,6 +172,8 @@
     USS_CB2.start();
     USS_CB3.start();
     USS_CB4.start();
+    USS_CB5.start();
+    USS_CB6.start();
 }
 
 void recordDist()
@@ -171,6 +187,8 @@
     cbDist2 = USS_CB2.get_dist();
     cbDist3 = USS_CB3.get_dist();
     cbDist4 = USS_CB4.get_dist();
+    cbDist5 = USS_CB5.get_dist();
+    cbDist6 = USS_CB6.get_dist();
 }
 
 void checkCliff()
@@ -210,6 +228,8 @@
     ultrasound_msg_cb2.range = cbDist2;
     ultrasound_msg_cb3.range = cbDist3;
     ultrasound_msg_cb4.range = cbDist4;
+    ultrasound_msg_cb5.range = cbDist5;
+    ultrasound_msg_cb6.range = cbDist6;
 }
 
 void setRosStamp()
@@ -224,6 +244,8 @@
     ultrasound_msg_cb2.header.stamp = nh.now();
     ultrasound_msg_cb3.header.stamp = nh.now();
     ultrasound_msg_cb4.header.stamp = nh.now();
+    ultrasound_msg_cb5.header.stamp = nh.now();
+    ultrasound_msg_cb6.header.stamp = nh.now();
 }
 
 void publishRosMsg()
@@ -245,4 +267,8 @@
     wait_ms(wPeriod);
     ultrasound_pub_cb4.publish(&ultrasound_msg_cb4);
     wait_ms(wPeriod);
+    ultrasound_pub_cb5.publish(&ultrasound_msg_cb5);
+    wait_ms(wPeriod);
+    ultrasound_pub_cb6.publish(&ultrasound_msg_cb6);
+    wait_ms(wPeriod);
 }