Diff: PROJ515.hpp

Revision:

1:ea2ee36038e7

Parent:

0:215bdd87b602

Child:

2:7288dd12186e

Child:

5:bf0d8b28771a

--- a/PROJ515.hpp	Mon Apr 22 18:28:24 2019 +0000
+++ b/PROJ515.hpp	Wed May 01 16:17:43 2019 +0000
@@ -5,44 +5,73 @@
 /* Libraries */
 #include "mbed.h"                   // Mbed RTOS
 #include "hcsr04.h"                 // HCSR04 ultrasound sensor libary
+#include <math.h>
+//#include "SRF05.h"                  // SRF05 ultrasound sensor libary
+//#include "SRF05_sp.h"                 // SRF05 single pin ultrasound sensor libary
 #include "ros.h"                    // ROS Library
 #include "sensor_msgs/Range.h"      // ROS Sensor Messages component
 //#include "std_msgs/Char.h"        // ROS Standard Messages component
 
-
 /* Definitions */
 #define LEDS_OFF 0x00               // LED bus all off
 #define LEDS_ON  0x07               // LED bus all on
-#define ULS_EC   PC_10              // Ultrasound Trigger Pin
-#define ULS_TG   PC_11              // Ultrasound Echo Pin
+
+#define USP_CF1  PE_2               // Cliff ultrasound 1 Trigger/Echo
+#define USP_CF2  PE_4               // Cliff ultrasound 2 Trigger/Echo
+#define USP_CF3  PE_5               // Cliff ultrasound 3 Trigger/Echo
+#define USP_CF4  PE_6               // Cliff ultrasound 4 Trigger/Echo
+
+#define USP_CB1  PF_8               // Curb  ultrasound 1 Trigger/Echo
+#define USP_CB2  PF_7               // Curb  ultrasound 2 Trigger/Echo
+#define USP_CB3  PF_9               // Curb  ultrasound 3 Trigger/Echo
+#define USP_CB4  PG_1               // Curb  ultrasound 4 Trigger/Echo
 
 #define ROS_Baud 921600             // ROS Baud Rate
 #define frameID "ultrasound_range"  // ROS frame ID
 
-#define ULTRASOUND  0x00            // Ultrasound
+#define ULTRASOUND 0x00             // Ultrasound
 #define PRINT_DATA 0x01             // Thread communication event to print data 
 
+/* Namespaces */
+using namespace std;                // Make all std symbols visible
+using namespace ros;                // Make all ros symbols visible
+using namespace sensor_msgs;        // Make all sensor_msgs symbols visible
+using namespace std_msgs;           // Make all sdt_msgs symbols visible
+
 /* Declarations */
-extern unsigned int cmDistance1;    // Distance measured by u_sensor_1 in cm
-extern float sPeriod;               // Sampling period of the sensors (sec)
+extern float cfDist1;        // Distance returned by cliff u_sensor1 (cm)
+extern float cfDist2;        // Distance returned by cliff u_sensor2 (cm)
+extern float cfDist3;        // Distance returned by cliff u_sensor3 (cm)
+extern float cfDist4;        // Distance returned by cliff u_sensor4 (cm)
+
+extern float cbDist1;        // Distance returned by curb  u_sensor1 (cm)
+extern float cbDist2;        // Distance returned by curb  u_sensor2 (cm)
+extern float cbDist3;        // Distance returned by curb  u_sensor3 (cm)
+extern float cbDist4;        // Distance returned by curb  u_sensor4 (cm)
+
+extern float wPeriod;               // Waiting period for the sampling (sec)
 
 /* GPIO */
 DigitalIn bttn(USER_BUTTON);        // Onboard blue user button
 BusOut leds(LED1, LED2, LED3);      // Onboard LEDs bus
 
 /* Instantiations */
-HCSR04 usensor1(ULS_TG,ULS_EC);     // Ultrasound sensor 1
-Serial usbSer(USBTX, USBRX);        // Serial over USB OTG
+//SRF05 USS_CF1(USP_CF1,USP_CF1);     // Ultrasound sensor cliff 1
+//SRF05 USS_CF2(USP_CF2,USP_CF2);     // Ultrasound sensor cliff 2
+//SRF05 USS_CF3(USP_CF3,USP_CF3);     // Ultrasound sensor cliff 3
+//SRF05 USS_CF4(USP_CF4,USP_CF4);     // Ultrasound sensor cliff 4
+//
+//SRF05 USS_CB1(USP_CB1,USP_CB1);     // Ultrasound sensor cliff 1
+//SRF05 USS_CB2(USP_CB2,USP_CB2);     // Ultrasound sensor cliff 2
+//SRF05 USS_CB3(USP_CB3,USP_CB3);     // Ultrasound sensor cliff 3
+HCSR04 USS_CB4(PG_1,PG_0);     // Ultrasound sensor cliff 4
+
+//Serial usbSer(USBTX, USBRX);        // Serial over USB OTG
 NodeHandle nh;                      // Instance for ROS Node Handler
 Range ultrasound_msg;               // Instance for ROS NavSatFix message
+
 // Instance for ROS publisher (Odometry Message)
-Publisher ultrasound_pub("ultrasound_range", &ultrasound_msg); 
-
-/* Namespaces */
-using namespace std;                // Make all std symbols visible
-using namespace ros;                // Make all ros symbols visible
-using namespace sensor_msgs;        // Make all sensor_msgs symbols visible
-using namespace std_msgs;           // Make all sdt_msgs symbols visible
+Publisher ultrasound_pub("/ultrasound/cb1", &ultrasound_msg); 
 
 /* Threads and Signals */
 //Thread myThread (osPriorityNormal);