Quadrotor

/* 

 * rosserial Clapper Example

 * 

 * This code is a very simple example of the kinds of 

 * custom sensors that you can easily set up with rosserial

 * and Arduino.  This code uses a microphone attached to 

 * analog pin 5 detect two claps (2 loud sounds).

 * You can use this clapper, for example, to command a robot 

 * in the area to come do your bidding.

 */

#include <WProgram.h>

#include <ros.h>

#include <std_msgs/Empty.h>

ros::NodeHandle  nh;

std_msgs::Empty clap_msg;

ros::Publisher p("clap", &clap_msg);

enum clapper_state { clap1, clap_one_waiting,  pause, clap2};

clapper_state clap;

int volume_thresh = 200;  //a clap sound needs to be: 

                          //abs(clap_volume) > average noise + volume_thresh

int mic_pin = 5;

int adc_ave=0;

void setup()

{ 

  pinMode(13, OUTPUT);

  nh.initNode();

  nh.advertise(p);

  //measure the average volume of the noise in the area

  for (int i =0; i<10;i++) adc_ave += analogRead(mic_pin);

  adc_ave /= 10;

}

long event_timer;

void loop()

{

  int mic_val = 0;

  for(int i=0; i<4; i++) mic_val += analogRead(mic_pin);

  mic_val = mic_val/4-adc_ave;

  switch(clap){

   case clap1:

    if (abs(mic_val) > volume_thresh){

       clap = clap_one_waiting; 

       event_timer = millis();

    }

    break;

    case clap_one_waiting:

      if ( (abs(mic_val) < 30) && ( (millis() - event_timer) > 20 ) )  

      {

        clap= pause;

        event_timer = millis();

      }

      break;

    case pause: // make sure there is a pause between 

                // the loud sounds

      if ( mic_val > volume_thresh) 

      {

        clap = clap1;

      }

      else if ( (millis()-event_timer)> 60)  {

        clap = clap2;

        event_timer = millis();

      }

      break;

     case clap2:

        if (abs(mic_val) > volume_thresh){ //we have got a double clap!

            clap = clap1;

            p.publish(&clap_msg);

        }

        else if ( (millis()-event_timer)> 200) {

          clap= clap1; // no clap detected, reset state machine

        }

        break;   

  }

  nh.spinOnce();

}