This program is porting rosserial_arduino for mbed http://www.ros.org/wiki/rosserial_arduino This program supported the revision of 169 of rosserial. This program contains an example.
Dependencies: rosserial_mbed_lib mbed Servo
examples/ADC.cpp@4:2cbca0ac2569, 2012-02-29 (annotated)
- Committer:
- nucho
- Date:
- Wed Feb 29 23:02:12 2012 +0000
- Revision:
- 4:2cbca0ac2569
- Parent:
- 0:06fc856e99ca
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nucho | 0:06fc856e99ca | 1 | //#define COMPILE_ADC_CODE_ROSSERIAL |
nucho | 0:06fc856e99ca | 2 | #ifdef COMPILE_ADC_CODE_ROSSERIAL |
nucho | 0:06fc856e99ca | 3 | |
nucho | 0:06fc856e99ca | 4 | /* |
nucho | 0:06fc856e99ca | 5 | * rosserial ADC Example |
nucho | 0:06fc856e99ca | 6 | * |
nucho | 0:06fc856e99ca | 7 | * This is a poor man's Oscilloscope. It does not have the sampling |
nucho | 0:06fc856e99ca | 8 | * rate or accuracy of a commerical scope, but it is great to get |
nucho | 0:06fc856e99ca | 9 | * an analog value into ROS in a pinch. |
nucho | 0:06fc856e99ca | 10 | */ |
nucho | 0:06fc856e99ca | 11 | |
nucho | 0:06fc856e99ca | 12 | #include "mbed.h" |
nucho | 0:06fc856e99ca | 13 | #include <ros.h> |
nucho | 0:06fc856e99ca | 14 | #include <rosserial_mbed/Adc.h> |
nucho | 0:06fc856e99ca | 15 | |
nucho | 0:06fc856e99ca | 16 | ros::NodeHandle nh; |
nucho | 0:06fc856e99ca | 17 | |
nucho | 0:06fc856e99ca | 18 | rosserial_mbed::Adc adc_msg; |
nucho | 0:06fc856e99ca | 19 | ros::Publisher p("adc", &adc_msg); |
nucho | 0:06fc856e99ca | 20 | |
nucho | 0:06fc856e99ca | 21 | |
nucho | 0:06fc856e99ca | 22 | |
nucho | 0:06fc856e99ca | 23 | //We average the analog reading to elminate some of the noise |
nucho | 0:06fc856e99ca | 24 | int averageAnalog(PinName pin) { |
nucho | 0:06fc856e99ca | 25 | int v=0; |
nucho | 0:06fc856e99ca | 26 | for (int i=0; i<4; i++) v+= AnalogIn(pin).read_u16(); |
nucho | 0:06fc856e99ca | 27 | return v/4; |
nucho | 0:06fc856e99ca | 28 | } |
nucho | 0:06fc856e99ca | 29 | |
nucho | 0:06fc856e99ca | 30 | long adc_timer; |
nucho | 0:06fc856e99ca | 31 | |
nucho | 0:06fc856e99ca | 32 | int main() { |
nucho | 0:06fc856e99ca | 33 | nh.initNode(); |
nucho | 0:06fc856e99ca | 34 | |
nucho | 0:06fc856e99ca | 35 | nh.advertise(p); |
nucho | 0:06fc856e99ca | 36 | |
nucho | 0:06fc856e99ca | 37 | while (1) { |
nucho | 0:06fc856e99ca | 38 | adc_msg.adc0 = averageAnalog(p15); |
nucho | 0:06fc856e99ca | 39 | adc_msg.adc1 = averageAnalog(p16); |
nucho | 0:06fc856e99ca | 40 | adc_msg.adc2 = averageAnalog(p17); |
nucho | 0:06fc856e99ca | 41 | adc_msg.adc3 = averageAnalog(p18); |
nucho | 0:06fc856e99ca | 42 | adc_msg.adc4 = averageAnalog(p19); |
nucho | 0:06fc856e99ca | 43 | adc_msg.adc5 = averageAnalog(p20); |
nucho | 0:06fc856e99ca | 44 | |
nucho | 0:06fc856e99ca | 45 | p.publish(&adc_msg); |
nucho | 0:06fc856e99ca | 46 | |
nucho | 0:06fc856e99ca | 47 | nh.spinOnce(); |
nucho | 0:06fc856e99ca | 48 | } |
nucho | 0:06fc856e99ca | 49 | } |
nucho | 0:06fc856e99ca | 50 | |
nucho | 0:06fc856e99ca | 51 | #endif |