Statistics
| Branch: | Revision:

root / quad2 / arduino / src / ros_lib / examples / Clapper / Clapper.pde @ c1426757

History | View | Annotate | Download (2.03 KB)

1
/* 
2
 * rosserial Clapper Example
3
 * 
4
 * This code is a very simple example of the kinds of 
5
 * custom sensors that you can easily set up with rosserial
6
 * and Arduino.  This code uses a microphone attached to 
7
 * analog pin 5 detect two claps (2 loud sounds).
8
 * You can use this clapper, for example, to command a robot 
9
 * in the area to come do your bidding.
10
 */
11

    
12
#include <WProgram.h>
13

    
14
#include <ros.h>
15
#include <std_msgs/Empty.h>
16

    
17
ros::NodeHandle  nh;
18

    
19
std_msgs::Empty clap_msg;
20
ros::Publisher p("clap", &clap_msg);
21

    
22
enum clapper_state { clap1, clap_one_waiting,  pause, clap2};
23
clapper_state clap;
24

    
25
int volume_thresh = 200;  //a clap sound needs to be: 
26
                          //abs(clap_volume) > average noise + volume_thresh
27
int mic_pin = 5;
28
int adc_ave=0;
29

    
30
void setup()
31
{ 
32
  pinMode(13, OUTPUT);
33
  nh.initNode();
34

    
35
  nh.advertise(p);
36

    
37
  //measure the average volume of the noise in the area
38
  for (int i =0; i<10;i++) adc_ave += analogRead(mic_pin);
39
  adc_ave /= 10;
40
}
41

    
42
long event_timer;
43

    
44
void loop()
45
{
46
  int mic_val = 0;
47
  for(int i=0; i<4; i++) mic_val += analogRead(mic_pin);
48
  
49
  mic_val = mic_val/4-adc_ave;
50
  
51
  switch(clap){
52
   case clap1:
53
    if (abs(mic_val) > volume_thresh){
54
       clap = clap_one_waiting; 
55
       event_timer = millis();
56
    }
57
    break;
58
    case clap_one_waiting:
59
      if ( (abs(mic_val) < 30) && ( (millis() - event_timer) > 20 ) )  
60
      {
61
        clap= pause;
62
        event_timer = millis();
63

    
64
      }
65
      break;
66
    case pause: // make sure there is a pause between 
67
                // the loud sounds
68
      if ( mic_val > volume_thresh) 
69
      {
70
        clap = clap1;
71

    
72
      }
73
      else if ( (millis()-event_timer)> 60)  {
74
        clap = clap2;
75
        event_timer = millis();
76

    
77
      }
78
      break;
79
     case clap2:
80
        if (abs(mic_val) > volume_thresh){ //we have got a double clap!
81
            clap = clap1;
82
            p.publish(&clap_msg);
83
        }
84
        else if ( (millis()-event_timer)> 200) {
85
          clap= clap1; // no clap detected, reset state machine
86
        }
87
        
88
        break;   
89
  }
90
  nh.spinOnce();
91
}