main.cpp

00001 ///*
00002 // * rosserial ADC Example
00003 // *
00004 // * This is a poor man's Oscilloscope.  It does not have the sampling
00005 // * rate or accuracy of a commerical scope, but it is great to get
00006 // * an analog value into ROS in a pinch.
00007 // */
00008 //
00009 //#include "mbed.h"
00010 //#include <ros.h>
00011 //#include <geometry_msgs/Vector3Stamped.h>
00012 //#include <ros/time.h>
00013 //
00014 ////AnalogIn analog_value1(A0);
00015 ////AnalogIn analog_value2(A1);
00016 ////AnalogIn analog_value3(A2);
00017 //DigitalOut led(LED1);
00018 //
00019 //ros::NodeHandle nh;
00020 //
00021 //geometry_msgs::Vector3Stamped joint_angle;
00022 //
00023 //ros::Publisher p("adc", &joint_angle);
00024 //
00025 //
00026 ////We average the analog reading to elminate some of the noise
00027 //int averageAnalog(PinName pin) {
00028 //    int v=0;
00029 //    for (int i=0; i<4; i++) v+= AnalogIn(pin).read_u16();
00030 //    return v/4;
00031 //}
00032 //
00033 //int main() {
00034 //    nh.initNode();
00035 //    nh.advertise(p);
00036 //
00037 //    while (1) {
00038 //        joint_angle.vector.x= averageAnalog(A0);
00039 //        joint_angle.vector.y= averageAnalog(A1);
00040 //        joint_angle.vector.z= averageAnalog(A2);
00041 //        joint_angle.header.stamp = nh.now();
00042 //        p.publish(&joint_angle);
00043 //        nh.spinOnce();
00044 //        wait_ms(10);
00045 //    }
00046 //}
00047 
00048 /*
00049  * rosserial ADC Example
00050  *
00051  * This is a poor man's Oscilloscope.  It does not have the sampling
00052  * rate or accuracy of a commerical scope, but it is great to get
00053  * an analog value into ROS in a pinch.
00054  */
00055 
00056 #include "mbed.h"
00057 #include <ros.h>
00058 #include <std_msgs/Float64.h>
00059 #include <ros/time.h>
00060 
00061 //AnalogIn analog_value1(A0);
00062 //AnalogIn analog_value2(A1);
00063 //AnalogIn analog_value3(A2);
00064 DigitalOut led(LED1);
00065 
00066 ros::NodeHandle nh;
00067 
00068 std_msgs::Float64 feedback;
00069 
00070 ros::Publisher p("state", &feedback);
00071 
00072 
00073 //We average the analog reading to elminate some of the noise
00074 int averageAnalog(PinName pin) {
00075     int v=0;
00076     for (int i=0; i<4; i++) v+= AnalogIn(pin).read();
00077     return v*2000/4-1000;
00078 }
00079 
00080 int main() {
00081     nh.initNode();
00082     nh.advertise(p);
00083 
00084     while (1) {
00085         feedback.data = averageAnalog(A0);
00086         p.publish(&feedback);
00087         nh.spinOnce();
00088         wait_ms(10);
00089     }
00090 }
00091