Quadrotor

/* 

 * rosserial IR Ranger Example  

 * 

 * This example is calibrated for the Sharp GP2D120XJ00F.

 */

#include <ros.h>

#include <ros/time.h>

#include <sensor_msgs/Range.h>

ros::NodeHandle  nh;

sensor_msgs::Range range_msg;

ros::Publisher pub_range( "range_data", &range_msg);

const int analog_pin = 0;

unsigned long range_timer;

/*

 * getRange() - samples the analog input from the ranger

 * and converts it into meters.  

 */

float getRange(int pin_num){

    int sample;

    // Get data

    sample = analogRead(pin_num)/4;

    // if the ADC reading is too low, 

    //   then we are really far away from anything

    if(sample < 10)

        return 254;     // max range

    // Magic numbers to get cm

    sample= 1309/(sample-3);

    return (sample - 1)/100; //convert to meters

}

char frameid[] = "/ir_ranger";

void setup()

{

  nh.initNode();

  nh.advertise(pub_range);

  range_msg.radiation_type = sensor_msgs::Range::INFRARED;

  range_msg.header.frame_id =  frameid;

  range_msg.field_of_view = 0.01;

  range_msg.min_range = 0.03;

  range_msg.max_range = 0.4;

}

void loop()

{

  // publish the range value every 50 milliseconds

  //   since it takes that long for the sensor to stabilize

  if ( (millis()-range_timer) > 50){

    range_msg.range = getRange(analog_pin);

    range_msg.header.stamp = nh.now();

    pub_range.publish(&range_msg);

    range_timer =  millis() + 50;

  }

  nh.spinOnce();

}