Statistics
| Branch: | Revision:

root / quad2 / arduino / src / ros_lib / examples / Ultrasound / Ultrasound.pde @ c1426757

History | View | Annotate | Download (1.19 KB)

1
/* 
2
 * rosserial Ultrasound Example
3
 * 
4
 * This example is for the Maxbotix Ultrasound rangers.
5
 */
6

    
7
#include <ros.h>
8
#include <ros/time.h>
9
#include <sensor_msgs/Range.h>
10

    
11
ros::NodeHandle  nh;
12

    
13
sensor_msgs::Range range_msg;
14
ros::Publisher pub_range( "/ultrasound", &range_msg);
15

    
16
const int adc_pin = 0;
17

    
18
char frameid[] = "/ultrasound";
19

    
20
float getRange_Ultrasound(int pin_num){
21
  int val = 0;
22
  for(int i=0; i<4; i++) val += analogRead(pin_num);
23
  float range =  val;
24
  return range /322.519685;   // (0.0124023437 /4) ; //cvt to meters
25
}
26

    
27
void setup()
28
{
29
  nh.initNode();
30
  nh.advertise(pub_range);
31
  
32
  
33
  range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND;
34
  range_msg.header.frame_id =  frameid;
35
  range_msg.field_of_view = 0.1;  // fake
36
  range_msg.min_range = 0.0;
37
  range_msg.max_range = 6.47;
38
  
39
  pinMode(8,OUTPUT);
40
  digitalWrite(8, LOW);
41
}
42

    
43

    
44
long range_time;
45

    
46
void loop()
47
{
48
  
49
  //publish the adc value every 50 milliseconds
50
  //since it takes that long for the sensor to stablize
51
  if ( millis() >= range_time ){
52
    int r =0;
53

    
54
    range_msg.range = getRange_Ultrasound(5);
55
    range_msg.header.stamp = nh.now();
56
    pub_range.publish(&range_msg);
57
    range_time =  millis() + 50;
58
  }
59
  
60
  nh.spinOnce();
61
}