Statistics
| Branch: | Revision:

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

History | View | Annotate | Download (1.39 KB)

1
/* 
2
 * rosserial IR Ranger Example  
3
 * 
4
 * This example is calibrated for the Sharp GP2D120XJ00F.
5
 */
6

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

    
11
ros::NodeHandle  nh;
12

    
13

    
14
sensor_msgs::Range range_msg;
15
ros::Publisher pub_range( "range_data", &range_msg);
16

    
17
const int analog_pin = 0;
18
unsigned long range_timer;
19

    
20
/*
21
 * getRange() - samples the analog input from the ranger
22
 * and converts it into meters.  
23
 */
24
float getRange(int pin_num){
25
    int sample;
26
    // Get data
27
    sample = analogRead(pin_num)/4;
28
    // if the ADC reading is too low, 
29
    //   then we are really far away from anything
30
    if(sample < 10)
31
        return 254;     // max range
32
    // Magic numbers to get cm
33
    sample= 1309/(sample-3);
34
    return (sample - 1)/100; //convert to meters
35
}
36

    
37
char frameid[] = "/ir_ranger";
38

    
39
void setup()
40
{
41
  nh.initNode();
42
  nh.advertise(pub_range);
43
  
44
  range_msg.radiation_type = sensor_msgs::Range::INFRARED;
45
  range_msg.header.frame_id =  frameid;
46
  range_msg.field_of_view = 0.01;
47
  range_msg.min_range = 0.03;
48
  range_msg.max_range = 0.4;
49
  
50
}
51

    
52
void loop()
53
{
54
  // publish the range value every 50 milliseconds
55
  //   since it takes that long for the sensor to stabilize
56
  if ( (millis()-range_timer) > 50){
57
    range_msg.range = getRange(analog_pin);
58
    range_msg.header.stamp = nh.now();
59
    pub_range.publish(&range_msg);
60
    range_timer =  millis() + 50;
61
  }
62
  nh.spinOnce();
63
}
64