![](/media/cache/profiles/94aef38441efa3380a3bed3faf1f9d5d.50x50_q85.jpg)
This program is porting rosserial_arduino for mbed http://www.ros.org/wiki/rosserial_arduino This program supported the revision of 169 of rosserial. This program contains an example.
Dependencies: rosserial_mbed_lib mbed Servo
examples/Ultrasound.cpp
- Committer:
- nucho
- Date:
- 2012-02-29
- Revision:
- 4:2cbca0ac2569
- Parent:
- 0:06fc856e99ca
File content as of revision 4:2cbca0ac2569:
//#define COMPILE_ULTRASOUND_CODE_ROSSERIAL #ifdef COMPILE_ULTRASOUND_CODE_ROSSERIAL /* * rosserial Ultrasound Example * * This example is for the Maxbotix Ultrasound rangers. */ #include "mbed.h" #include <ros.h> #include <ros/time.h> #include <sensor_msgs/Range.h> ros::NodeHandle nh; sensor_msgs::Range range_msg; ros::Publisher pub_range( "/ultrasound", &range_msg); const PinName adc_pin = p20; char frameid[] = "/ultrasound"; float getRange_Ultrasound(PinName pin_num) { int val = 0; for (int i=0; i<4; i++) val += AnalogIn(pin_num).read_u16(); float range = val; return range /322.519685; // (0.0124023437 /4) ; //cvt to meters } Timer t; int main() { t.start(); nh.initNode(); nh.advertise(pub_range); range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND; range_msg.header.frame_id = frameid; range_msg.field_of_view = 0.1; // fake range_msg.min_range = 0.0; range_msg.max_range = 6.47; //pinMode(8,OUTPUT); //digitalWrite(8, LOW); long range_time=0; while (1) { //publish the adc value every 50 milliseconds //since it takes that long for the sensor to stablize if ( t.read_ms() >= range_time ) { int r =0; range_msg.range = getRange_Ultrasound(adc_pin); range_msg.header.stamp = nh.now(); pub_range.publish(&range_msg); range_time = t.read_ms() + 50; } nh.spinOnce(); } } #endif