pinxian wu / Mbed 2 deprecated ROS_HCSR04

HCSR04 with ROS

Dependencies:   Pulse mbed ros_lib_kinetic

main.cpp@0:e70ca69c6311, 2017-07-12 (annotated)

Committer:
wupinxian
Date:
Wed Jul 12 02:23:53 2017 +0000
Revision:
0:e70ca69c6311
ROS_HCSR04;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wupinxian 0:e70ca69c6311 1 /*
wupinxian 0:e70ca69c6311 2 * rosserial Publisher Example
wupinxian 0:e70ca69c6311 3 * Prints "hello world!"
wupinxian 0:e70ca69c6311 4 */
wupinxian 0:e70ca69c6311 5
wupinxian 0:e70ca69c6311 6 #include"mbed.h"
wupinxian 0:e70ca69c6311 7 #include"Pulse.h"
wupinxian 0:e70ca69c6311 8 #include"stdio.h"
wupinxian 0:e70ca69c6311 9 #include <ros.h>
wupinxian 0:e70ca69c6311 10 #include <std_msgs/String.h>
wupinxian 0:e70ca69c6311 11
wupinxian 0:e70ca69c6311 12
wupinxian 0:e70ca69c6311 13 ros::NodeHandle nh;
wupinxian 0:e70ca69c6311 14
wupinxian 0:e70ca69c6311 15 std_msgs::String str_msg;
wupinxian 0:e70ca69c6311 16 ros::Publisher chatter("chatter", &str_msg);
wupinxian 0:e70ca69c6311 17
wupinxian 0:e70ca69c6311 18 char hello[20] = "How a fucking day!";
wupinxian 0:e70ca69c6311 19 //HCSR04 sonar(PB_0,PB_1) ;
wupinxian 0:e70ca69c6311 20 //PA_6 ->trig PA_7->echo
wupinxian 0:e70ca69c6311 21 PulseInOut trig(PB_1);
wupinxian 0:e70ca69c6311 22 PulseInOut echo(PB_0);
wupinxian 0:e70ca69c6311 23 float hc_distance(void)
wupinxian 0:e70ca69c6311 24 {
wupinxian 0:e70ca69c6311 25 long count=0;
wupinxian 0:e70ca69c6311 26 float distance=0;
wupinxian 0:e70ca69c6311 27 trig.write_us(1,10);
wupinxian 0:e70ca69c6311 28 count=echo.read_high_us();
wupinxian 0:e70ca69c6311 29 distance= float(count*0.017);
wupinxian 0:e70ca69c6311 30 return distance;
wupinxian 0:e70ca69c6311 31 }
wupinxian 0:e70ca69c6311 32
wupinxian 0:e70ca69c6311 33
wupinxian 0:e70ca69c6311 34 float dist=0.0;
wupinxian 0:e70ca69c6311 35 int main() {
wupinxian 0:e70ca69c6311 36 nh.initNode();
wupinxian 0:e70ca69c6311 37 nh.advertise(chatter);
wupinxian 0:e70ca69c6311 38 while (1) {
wupinxian 0:e70ca69c6311 39
wupinxian 0:e70ca69c6311 40 dist = hc_distance();
wupinxian 0:e70ca69c6311 41 sprintf(hello,"%f \n",dist);
wupinxian 0:e70ca69c6311 42 str_msg.data = hello;
wupinxian 0:e70ca69c6311 43 chatter.publish( &str_msg );
wupinxian 0:e70ca69c6311 44 nh.spinOnce();
wupinxian 0:e70ca69c6311 45 wait_ms(1000);
wupinxian 0:e70ca69c6311 46
wupinxian 0:e70ca69c6311 47 }
wupinxian 0:e70ca69c6311 48 }