Zill Khan
/
ROS_HCSR04_test
Integration of HCSR04 with Nucleo board. ROS Enabled
Revision 5:1a7d0889cbcf, committed 2021-09-28
- Comitter:
- zillkhan
- Date:
- Tue Sep 28 12:46:44 2021 +0000
- Parent:
- 4:13cda079f08d
- Commit message:
- final changes
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HCSR04.lib Tue Sep 28 12:46:44 2021 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/antoniolinux/code/HCSR04/#86b2086be101
--- a/SHARPIR.lib Mon Sep 27 09:34:09 2021 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://os.mbed.com/users/zillkhan/code/SHARPIR_GP2Y0A51SK0F/#acdd61a2aac7
--- a/main.cpp Mon Sep 27 09:34:09 2021 +0000 +++ b/main.cpp Tue Sep 28 12:46:44 2021 +0000 @@ -1,32 +1,31 @@ +#include "mbed.h" #include <ros.h> -#include <std_msgs/Float32.h> #include <std_msgs/String.h> -#include "SHARPIR.h" - +#include "hcsr04.h" ros::NodeHandle nh; -std_msgs::Float32 data; -ros::Publisher sharpir("sharpir", &data); +std_msgs::String data; +ros::Publisher ultrasonic("ultrasonic", &data); -SHARPIR Sensor(A0); - +//D12 TRIGGER D11 ECHO +HCSR04 sensor(D8, D9); int main() { - float DistanceCM; - + char buffer[50]; + nh.initNode(); - nh.advertise(sharpir); - + nh.advertise(ultrasonic); + while (1) { //creates an eternal loop - - DistanceCM=Sensor.cm(); - //sprintf (buffer, "%f", DistanceCM); - - data.data = DistanceCM; - sharpir.publish( &data ); - + + long distance = sensor.distance(); + sprintf (buffer, "%d", distance); + + data.data = buffer; + ultrasonic.publish( &data ); + nh.spinOnce(); wait_ms(1000); } -} +} \ No newline at end of file