arduino代码

2026-07-18 18:22:58 +08:00
parent 2b7a320983
commit a2825ed71a
438 changed files with 36538 additions and 0 deletions
@@ -0,0 +1,64 @@
+/* 
+ * 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();
+}
+