Diff: PROJ515.hpp

Revision:

3:cd1f2bde7ac2

Parent:

2:7288dd12186e

Child:

4:f74d88f5f629

--- a/PROJ515.hpp	Thu May 02 11:37:55 2019 +0000
+++ b/PROJ515.hpp	Mon May 06 11:51:18 2019 +0000
@@ -5,7 +5,7 @@
 /* Libraries */
 #include "mbed.h"                   // Mbed RTOS
 #include "hcsr04.h"                 // HCSR04 ultrasound sensor libary
-//#include "SRF05_sp.h"                 // SRF05 single pin ultrasound sensor libary
+//#include "SRF05.h"                  // SRF05 ultrasound sensor custom libary
 #include "ros.h"                    // ROS Library
 #include "sensor_msgs/Range.h"      // ROS Sensor Messages component
 
@@ -31,6 +31,13 @@
 #define USP_CB4T  PE_2              // Curb  ultrasound 4 Trigger
 #define USP_CB4E  PG_14             // Curb  ultrasound 4 Echo
 
+#define FOV 1.396                   // Ultrasound sensor field of view
+#define MIN_RANGE 0.2               // Ultrasound sensor min range
+#define MAX_RANGE 2.5               // Ultrasound sensor max range
+#define CLIFF_TRH 0.12              // Robot cliff treshold
+#define CLIFF_ZRO 0                 // Cliff not existent
+#define EXCL_ZONE 0.255             // Robot footprint exclusion zone
+
 #define ROS_Baud 921600             // ROS Baud Rate
 #define frameID "ultrasound_range"  // ROS frame ID
 
@@ -71,12 +78,27 @@
 HCSR04 USS_CB3(USP_CB3T, USP_CB3E);  // Ultrasound sensor cliff 3
 HCSR04 USS_CB4(USP_CB4T, USP_CB4E);  // Ultrasound sensor cliff 4
 
-Serial usbSer(USBTX, USBRX);        // Serial over USB OTG
+//Serial usbSer(USBTX, USBRX);        // Serial over USB OTG (DEBUG with no ROS)
 NodeHandle nh;                      // Instance for ROS Node Handler
-Range ultrasound_msg;               // Instance for ROS NavSatFix message
+
+Range ultrasound_msg_cf1;           // Instance for ROS NavSatFix message 1
+Range ultrasound_msg_cf2;           // Instance for ROS NavSatFix message 2
+Range ultrasound_msg_cf3;           // Instance for ROS NavSatFix message 3
+Range ultrasound_msg_cf4;           // Instance for ROS NavSatFix message 4
+Range ultrasound_msg_cb1;           // Instance for ROS NavSatFix message 5
+Range ultrasound_msg_cb2;           // Instance for ROS NavSatFix message 6
+Range ultrasound_msg_cb3;           // Instance for ROS NavSatFix message 7
+Range ultrasound_msg_cb4;           // Instance for ROS NavSatFix message 8
 
-// Instance for ROS publishers (Range message)
-Publisher ultrasound_pub("/ultrasound/cb1", &ultrasound_msg); 
+// Instance for ROS publishers (Range message for cliff and curb sensors)
+Publisher ultrasound_pub_cf1("/ultrasound/cf1", &ultrasound_msg_cf1); 
+Publisher ultrasound_pub_cf2("/ultrasound/cf2", &ultrasound_msg_cf2); 
+Publisher ultrasound_pub_cf3("/ultrasound/cf3", &ultrasound_msg_cf3); 
+Publisher ultrasound_pub_cf4("/ultrasound/cf4", &ultrasound_msg_cf4); 
+Publisher ultrasound_pub_cb1("/ultrasound/cb1", &ultrasound_msg_cb1); 
+Publisher ultrasound_pub_cb2("/ultrasound/cb2", &ultrasound_msg_cb2); 
+Publisher ultrasound_pub_cb3("/ultrasound/cb3", &ultrasound_msg_cb3); 
+Publisher ultrasound_pub_cb4("/ultrasound/cb4", &ultrasound_msg_cb4); 
 
 /* Threads and Signals */
 //Thread myThread (osPriorityNormal);