Akash Vibhute / mbed_roshydro_test

ros_serial for mbed updated to work with ROS Hydro. Library still needs to be debugged

Dependencies:   rosserial_hydro

Library and program still under development!

examples/Clapper.cpp@1:225298843679, 2015-02-15 (annotated)

Committer:
akashvibhute
Date:
Sun Feb 15 10:56:37 2015 +0000
Revision:
1:225298843679
Parent:
0:cb1dffdc7d05 
rosserial_mbed for hydro testing

Who changed what in which revision?

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