nucho
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/Clapper.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_CLAPPER_CODE_ROSSERIAL |
| nucho | 0:06fc856e99ca | 2 | #ifdef COMPILE_CLAPPER_CODE_ROSSERIAL |
| nucho | 0:06fc856e99ca | 3 | |
| nucho | 0:06fc856e99ca | 4 | /* |
| nucho | 0:06fc856e99ca | 5 | * rosserial Clapper Example |
| nucho | 0:06fc856e99ca | 6 | * |
| nucho | 0:06fc856e99ca | 7 | * This code is a very simple example of the kinds of |
| nucho | 0:06fc856e99ca | 8 | * custom sensors that you can easily set up with rosserial |
| nucho | 0:06fc856e99ca | 9 | * and Arduino. This code uses a microphone attached to |
| nucho | 0:06fc856e99ca | 10 | * analog pin 5 detect two claps (2 loud sounds). |
| nucho | 0:06fc856e99ca | 11 | * You can use this clapper, for example, to command a robot |
| nucho | 0:06fc856e99ca | 12 | * in the area to come do your bidding. |
| nucho | 0:06fc856e99ca | 13 | */ |
| nucho | 0:06fc856e99ca | 14 | |
| nucho | 0:06fc856e99ca | 15 | #include <ros.h> |
| nucho | 0:06fc856e99ca | 16 | #include <std_msgs/Empty.h> |
| nucho | 0:06fc856e99ca | 17 | |
| nucho | 0:06fc856e99ca | 18 | ros::NodeHandle nh; |
| nucho | 0:06fc856e99ca | 19 | |
| nucho | 0:06fc856e99ca | 20 | std_msgs::Empty clap_msg; |
| nucho | 0:06fc856e99ca | 21 | ros::Publisher p("clap", &clap_msg); |
| nucho | 0:06fc856e99ca | 22 | |
| nucho | 0:06fc856e99ca | 23 | enum clapper_state { clap1, clap_one_waiting, pause, clap2}; |
| nucho | 0:06fc856e99ca | 24 | clapper_state clap; |
| nucho | 0:06fc856e99ca | 25 | |
| nucho | 0:06fc856e99ca | 26 | int volume_thresh = 200; //a clap sound needs to be: |
| nucho | 0:06fc856e99ca | 27 | //abs(clap_volume) > average noise + volume_thresh |
| nucho | 0:06fc856e99ca | 28 | AnalogIn mic_pin(p20); |
| nucho | 0:06fc856e99ca | 29 | int adc_ave=0; |
| nucho | 0:06fc856e99ca | 30 | |
| nucho | 0:06fc856e99ca | 31 | Timer t; |
| nucho | 0:06fc856e99ca | 32 | int main() { |
| nucho | 0:06fc856e99ca | 33 | t.start(); |
| nucho | 0:06fc856e99ca | 34 | nh.initNode(); |
| nucho | 0:06fc856e99ca | 35 | |
| nucho | 0:06fc856e99ca | 36 | nh.advertise(p); |
| nucho | 0:06fc856e99ca | 37 | |
| nucho | 0:06fc856e99ca | 38 | //measure the average volume of the noise in the area |
| nucho | 0:06fc856e99ca | 39 | for (int i =0; i<10; i++) adc_ave += mic_pin.read_u16(); |
| nucho | 0:06fc856e99ca | 40 | adc_ave /= 10; |
| nucho | 0:06fc856e99ca | 41 | |
| nucho | 0:06fc856e99ca | 42 | long event_timer = 0; |
| nucho | 0:06fc856e99ca | 43 | |
| nucho | 0:06fc856e99ca | 44 | while (1) { |
| nucho | 0:06fc856e99ca | 45 | int mic_val = 0; |
| nucho | 0:06fc856e99ca | 46 | for (int i=0; i<4; i++) mic_val += mic_pin.read_u16(); |
| nucho | 0:06fc856e99ca | 47 | |
| nucho | 0:06fc856e99ca | 48 | mic_val = mic_val/4-adc_ave; |
| nucho | 0:06fc856e99ca | 49 | |
| nucho | 0:06fc856e99ca | 50 | switch (clap) { |
| nucho | 0:06fc856e99ca | 51 | case clap1: |
| nucho | 0:06fc856e99ca | 52 | if (abs(mic_val) > volume_thresh) { |
| nucho | 0:06fc856e99ca | 53 | clap = clap_one_waiting; |
| nucho | 0:06fc856e99ca | 54 | event_timer = t.read_ms(); |
| nucho | 0:06fc856e99ca | 55 | } |
| nucho | 0:06fc856e99ca | 56 | break; |
| nucho | 0:06fc856e99ca | 57 | case clap_one_waiting: |
| nucho | 0:06fc856e99ca | 58 | if ( (abs(mic_val) < 30) && ( (t.read_ms()- event_timer) > 20 ) ) { |
| nucho | 0:06fc856e99ca | 59 | clap= pause; |
| nucho | 0:06fc856e99ca | 60 | event_timer = t.read_ms(); |
| nucho | 0:06fc856e99ca | 61 | |
| nucho | 0:06fc856e99ca | 62 | } |
| nucho | 0:06fc856e99ca | 63 | break; |
| nucho | 0:06fc856e99ca | 64 | case pause: // make sure there is a pause between |
| nucho | 0:06fc856e99ca | 65 | // the loud sounds |
| nucho | 0:06fc856e99ca | 66 | if ( mic_val > volume_thresh) { |
| nucho | 0:06fc856e99ca | 67 | clap = clap1; |
| nucho | 0:06fc856e99ca | 68 | |
| nucho | 0:06fc856e99ca | 69 | } else if ( (t.read_ms()-event_timer)> 60) { |
| nucho | 0:06fc856e99ca | 70 | clap = clap2; |
| nucho | 0:06fc856e99ca | 71 | event_timer = t.read_ms(); |
| nucho | 0:06fc856e99ca | 72 | |
| nucho | 0:06fc856e99ca | 73 | } |
| nucho | 0:06fc856e99ca | 74 | break; |
| nucho | 0:06fc856e99ca | 75 | case clap2: |
| nucho | 0:06fc856e99ca | 76 | if (abs(mic_val) > volume_thresh) { //we have got a double clap! |
| nucho | 0:06fc856e99ca | 77 | clap = clap1; |
| nucho | 0:06fc856e99ca | 78 | p.publish(&clap_msg); |
| nucho | 0:06fc856e99ca | 79 | } else if ( (t.read_ms()-event_timer)> 200) { |
| nucho | 0:06fc856e99ca | 80 | clap= clap1; // no clap detected, reset state machine |
| nucho | 0:06fc856e99ca | 81 | } |
| nucho | 0:06fc856e99ca | 82 | |
| nucho | 0:06fc856e99ca | 83 | break; |
| nucho | 0:06fc856e99ca | 84 | } |
| nucho | 0:06fc856e99ca | 85 | nh.spinOnce(); |
| nucho | 0:06fc856e99ca | 86 | } |
| nucho | 0:06fc856e99ca | 87 | } |
| nucho | 0:06fc856e99ca | 88 | |
| nucho | 0:06fc856e99ca | 89 | #endif |