fan yang / Mbed 2 deprecated rosserial_led

using ros serial to publish joint angles

Dependencies:   mbed ros_lib_indigo

main.cpp@0:6370e1a55ca5, 2017-09-26 (annotated)

Committer:
robot_fan4
Date:
Tue Sep 26 10:39:00 2017 +0000
Revision:
0:6370e1a55ca5
rosserial;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
robot_fan4 0:6370e1a55ca5 1 ///*
robot_fan4 0:6370e1a55ca5 2 // * rosserial ADC Example
robot_fan4 0:6370e1a55ca5 3 // *
robot_fan4 0:6370e1a55ca5 4 // * This is a poor man's Oscilloscope. It does not have the sampling
robot_fan4 0:6370e1a55ca5 5 // * rate or accuracy of a commerical scope, but it is great to get
robot_fan4 0:6370e1a55ca5 6 // * an analog value into ROS in a pinch.
robot_fan4 0:6370e1a55ca5 7 // */
robot_fan4 0:6370e1a55ca5 8 //
robot_fan4 0:6370e1a55ca5 9 //#include "mbed.h"
robot_fan4 0:6370e1a55ca5 10 //#include <ros.h>
robot_fan4 0:6370e1a55ca5 11 //#include <geometry_msgs/Vector3Stamped.h>
robot_fan4 0:6370e1a55ca5 12 //#include <ros/time.h>
robot_fan4 0:6370e1a55ca5 13 //
robot_fan4 0:6370e1a55ca5 14 ////AnalogIn analog_value1(A0);
robot_fan4 0:6370e1a55ca5 15 ////AnalogIn analog_value2(A1);
robot_fan4 0:6370e1a55ca5 16 ////AnalogIn analog_value3(A2);
robot_fan4 0:6370e1a55ca5 17 //DigitalOut led(LED1);
robot_fan4 0:6370e1a55ca5 18 //
robot_fan4 0:6370e1a55ca5 19 //ros::NodeHandle nh;
robot_fan4 0:6370e1a55ca5 20 //
robot_fan4 0:6370e1a55ca5 21 //geometry_msgs::Vector3Stamped joint_angle;
robot_fan4 0:6370e1a55ca5 22 //
robot_fan4 0:6370e1a55ca5 23 //ros::Publisher p("adc", &joint_angle);
robot_fan4 0:6370e1a55ca5 24 //
robot_fan4 0:6370e1a55ca5 25 //
robot_fan4 0:6370e1a55ca5 26 ////We average the analog reading to elminate some of the noise
robot_fan4 0:6370e1a55ca5 27 //int averageAnalog(PinName pin) {
robot_fan4 0:6370e1a55ca5 28 // int v=0;
robot_fan4 0:6370e1a55ca5 29 // for (int i=0; i<4; i++) v+= AnalogIn(pin).read_u16();
robot_fan4 0:6370e1a55ca5 30 // return v/4;
robot_fan4 0:6370e1a55ca5 31 //}
robot_fan4 0:6370e1a55ca5 32 //
robot_fan4 0:6370e1a55ca5 33 //int main() {
robot_fan4 0:6370e1a55ca5 34 // nh.initNode();
robot_fan4 0:6370e1a55ca5 35 // nh.advertise(p);
robot_fan4 0:6370e1a55ca5 36 //
robot_fan4 0:6370e1a55ca5 37 // while (1) {
robot_fan4 0:6370e1a55ca5 38 // joint_angle.vector.x= averageAnalog(A0);
robot_fan4 0:6370e1a55ca5 39 // joint_angle.vector.y= averageAnalog(A1);
robot_fan4 0:6370e1a55ca5 40 // joint_angle.vector.z= averageAnalog(A2);
robot_fan4 0:6370e1a55ca5 41 // joint_angle.header.stamp = nh.now();
robot_fan4 0:6370e1a55ca5 42 // p.publish(&joint_angle);
robot_fan4 0:6370e1a55ca5 43 // nh.spinOnce();
robot_fan4 0:6370e1a55ca5 44 // wait_ms(10);
robot_fan4 0:6370e1a55ca5 45 // }
robot_fan4 0:6370e1a55ca5 46 //}
robot_fan4 0:6370e1a55ca5 47
robot_fan4 0:6370e1a55ca5 48 /*
robot_fan4 0:6370e1a55ca5 49 * rosserial ADC Example
robot_fan4 0:6370e1a55ca5 50 *
robot_fan4 0:6370e1a55ca5 51 * This is a poor man's Oscilloscope. It does not have the sampling
robot_fan4 0:6370e1a55ca5 52 * rate or accuracy of a commerical scope, but it is great to get
robot_fan4 0:6370e1a55ca5 53 * an analog value into ROS in a pinch.
robot_fan4 0:6370e1a55ca5 54 */
robot_fan4 0:6370e1a55ca5 55
robot_fan4 0:6370e1a55ca5 56 #include "mbed.h"
robot_fan4 0:6370e1a55ca5 57 #include <ros.h>
robot_fan4 0:6370e1a55ca5 58 #include <std_msgs/Float64.h>
robot_fan4 0:6370e1a55ca5 59 #include <ros/time.h>
robot_fan4 0:6370e1a55ca5 60
robot_fan4 0:6370e1a55ca5 61 //AnalogIn analog_value1(A0);
robot_fan4 0:6370e1a55ca5 62 //AnalogIn analog_value2(A1);
robot_fan4 0:6370e1a55ca5 63 //AnalogIn analog_value3(A2);
robot_fan4 0:6370e1a55ca5 64 DigitalOut led(LED1);
robot_fan4 0:6370e1a55ca5 65
robot_fan4 0:6370e1a55ca5 66 ros::NodeHandle nh;
robot_fan4 0:6370e1a55ca5 67
robot_fan4 0:6370e1a55ca5 68 std_msgs::Float64 feedback;
robot_fan4 0:6370e1a55ca5 69
robot_fan4 0:6370e1a55ca5 70 ros::Publisher p("state", &feedback);
robot_fan4 0:6370e1a55ca5 71
robot_fan4 0:6370e1a55ca5 72
robot_fan4 0:6370e1a55ca5 73 //We average the analog reading to elminate some of the noise
robot_fan4 0:6370e1a55ca5 74 int averageAnalog(PinName pin) {
robot_fan4 0:6370e1a55ca5 75 int v=0;
robot_fan4 0:6370e1a55ca5 76 for (int i=0; i<4; i++) v+= AnalogIn(pin).read();
robot_fan4 0:6370e1a55ca5 77 return v*2000/4-1000;
robot_fan4 0:6370e1a55ca5 78 }
robot_fan4 0:6370e1a55ca5 79
robot_fan4 0:6370e1a55ca5 80 int main() {
robot_fan4 0:6370e1a55ca5 81 nh.initNode();
robot_fan4 0:6370e1a55ca5 82 nh.advertise(p);
robot_fan4 0:6370e1a55ca5 83
robot_fan4 0:6370e1a55ca5 84 while (1) {
robot_fan4 0:6370e1a55ca5 85 feedback.data = averageAnalog(A0);
robot_fan4 0:6370e1a55ca5 86 p.publish(&feedback);
robot_fan4 0:6370e1a55ca5 87 nh.spinOnce();
robot_fan4 0:6370e1a55ca5 88 wait_ms(10);
robot_fan4 0:6370e1a55ca5 89 }
robot_fan4 0:6370e1a55ca5 90 }
robot_fan4 0:6370e1a55ca5 91