Diff: PROJ515.cpp

Revision:

2:7288dd12186e

Parent:

1:ea2ee36038e7

Child:

3:cd1f2bde7ac2

--- a/PROJ515.cpp	Wed May 01 16:17:43 2019 +0000
+++ b/PROJ515.cpp	Thu May 02 11:37:55 2019 +0000
@@ -1,7 +1,16 @@
 #include "PROJ515.hpp" // Contains all Libraries, Definitions & Function Prototypes
 
-float cmDistance1;
-float wPeriod = 50;
+float cfDist1 = 0;     // Distance returned by cliff u_sensor1 (m)
+float cfDist2 = 0;     // Distance returned by cliff u_sensor2 (m)
+float cfDist3 = 0;     // Distance returned by cliff u_sensor3 (m)
+float cfDist4 = 0;     // Distance returned by cliff u_sensor4 (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
 
 
 void setupRosMsg()
@@ -9,30 +18,78 @@
 
 int main()
 {
-    nh.getHardware()->setBaud(921600);           // Set Baud Rate for ROS Serial
-    nh.initNode();                                 // Initialise ROS Node Handler
-    nh.advertise(ultrasound_pub);                    // Adverstise Odometry topic
-       
-    ultrasound_msg.header.frame_id = "uss_cb1";
-    ultrasound_msg.radiation_type = 0x00;
-    ultrasound_msg.field_of_view = ((80*3.14159)/180); //3.14159265358979323846
-    ultrasound_msg.min_range = 0.02f;
-    ultrasound_msg.max_range = 2.5;
-    
-    while(!nh.connected()) {                       // While node handler is not connected
-        nh.spinOnce();                             // Attempt to connect and synchronise
-    }
+    //nh.getHardware()->setBaud(921600);           // Set Baud Rate for ROS Serial
+//    nh.initNode();                                 // Initialise ROS Node Handler
+//    nh.advertise(ultrasound_pub);                    // Adverstise Odometry topic
+//       
+//    ultrasound_msg.header.frame_id = "uss_cb1";
+//    ultrasound_msg.radiation_type = 0x00;
+//    ultrasound_msg.field_of_view = ((80*3.14159)/180); //3.14159265358979323846
+//    ultrasound_msg.min_range = 0.02f;
+//    ultrasound_6msg.max_range = 2.5;
+//    
+//    while(!nh.connected()) {                       // While node handler is not connected
+//        nh.spinOnce();                             // Attempt to connect and synchronise
+//    }
     
     while(true) {
         
-        USS_CB4.start();
-        cmDistance1 = USS_CB4.get_dist_cm();
-        cmDistance1 = cmDistance1/100; // in meters
-        ultrasound_msg.range = cmDistance1;// m
-        nh.spinOnce();                             // Reccuring connect and synchronise
-        ultrasound_msg.header.stamp = nh.now();      // Get current time
-        ultrasound_pub.publish(&ultrasound_msg);
-        //usbSer.printf("Distance (m) = %d\n\n", cmDistance);
-        wait_ms(5);
+        USS_CF1.start();
+        USS_CF2.start();
+        USS_CF3.start();
+        USS_CF4.start();   
+        
+        USS_CB1.start();
+        USS_CB2.start();
+        USS_CB3.start();
+        USS_CB4.start(); 
+            
+        cfDist1 = USS_CF1.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();
+        
+        cfDist1 = cfDist1/100;
+        cfDist2 = cfDist2/100;
+        cfDist3 = cfDist3/100;
+        cfDist4 = cfDist4/100; 
+        
+        cbDist1 = cbDist1/100;
+        cbDist2 = cbDist2/100;
+        cbDist3 = cbDist3/100;
+        cbDist4 = cbDist4/100; 
+        
+        usbSer.printf("Cliff 1 = %f\n", cfDist1);
+        usbSer.printf("Cliff 2 = %f\n", cfDist2);
+        usbSer.printf("Cliff 3 = %f\n", cfDist3);
+        usbSer.printf("Cliff 4 = %f\n", cfDist4);
+        
+        usbSer.printf("Curb  1 = %f\n", cbDist1);
+        usbSer.printf("Curb  2 = %f\n", cbDist2);
+        usbSer.printf("Curb  3 = %f\n", cbDist3);
+        usbSer.printf("Curb  4 = %f  ", cbDist4);
+        
+        wait(1);
+        
+        puts(""); 
+        puts(""); 
+        puts(""); 
+        puts(""); 
+        puts(""); 
+        puts(""); 
+        puts(""); 
+        puts("");
+        
+        //ultrasound_msg.range = cmDistance1;// m
+//        nh.spinOnce();                             // Reccuring connect and synchronise
+//        ultrasound_msg.header.stamp = nh.now();      // Get current time
+//        ultrasound_pub.publish(&ultrasound_msg);
+
+        wait_ms(wPeriod);
     }
 }
\ No newline at end of file