       delay(4000);
       ledmatrix.clearMatrix();
     } 
-  tone(PIN_Buzzer,frec,duration);
-  delay(duration);
-  noTone(PIN_Buzzer);
-}
+
+  ZowiTone(PIN_Buzzer, frec, duration, duration);   
+  // tone(PIN_Buzzer,frec,duration);
+  // delay(duration);
+  // noTone(PIN_Buzzer);
+}
+
+
+
+//Ultrasound Sensor
+long TP_init(int trigger_pin, int echo_pin)
+   {
+    digitalWrite(trigger_pin, LOW);
+    delayMicroseconds(2);
+    digitalWrite(trigger_pin, HIGH);
+    delayMicroseconds(10);
+    digitalWrite(trigger_pin, LOW);
+    long microseconds = pulseIn(echo_pin ,HIGH,100000);
+    return microseconds;
+}
+
+long Distance(int trigger_pin, int echo_pin)
+  {
+    long microseconds = TP_init(trigger_pin, echo_pin);
+    long distance;
+    distance = microseconds/29/2;
+    if (distance == 0){
+      distance = 999;
+    }
+    return distance;
+}
+
+
+void requestDistance()
+{
+    int distance=Distance(PIN_TriggerBat,PIN_EchoBat);
+    Serial.print("&&");
+    Serial.print("D ");
+    Serial.print(distance);
+    Serial.println("%%");
+    Serial.flush();
+}
+
+void requestNoise()
+{
+    int microphone=analogRead(PIN_NoiseSensor);
+    Serial.print("&&");
+    Serial.print("N ");
+    Serial.print(microphone);
+    Serial.println("%%");
+    Serial.flush();
+}
+
+void requestBattery()
+{
+       //The first read of the batery is often a wrong reading, so we read it two times. 
+    double batteryLevel= battery.readBatPercent();
+    batteryLevel= battery.readBatPercent();
+
+    Serial.print("&&");
+    Serial.print("B ");
+    Serial.print(batteryLevel);
+    Serial.println("%%");
+    Serial.flush();
+}
+
+void requestProgramId()
+{
+    Serial.print("&&");
+    Serial.print("I ");
+    Serial.print(programID);
+    Serial.println("%%");
+    Serial.flush();
+}
+
+void sendAck()
+{
+    Serial.print("&&");
+    Serial.print("A");
+    Serial.println("%%");
+    Serial.flush();
+}
+
+
+void ZowiTone(int speakerPin, float noteFrequency, long noteDuration, long silentDuration)
+{    
+  int x;
+  // Convert the frequency to microseconds
+  float microsecondsPerWave = 1000000/noteFrequency;
+  // Calculate how many HIGH/LOW cycles there are per millisecond
+  float millisecondsPerCycle = 1000/(microsecondsPerWave * 2);
+  // Multiply noteDuration * number or cycles per millisecond
+  float loopTime = noteDuration * millisecondsPerCycle;
+  // Play the note for the calculated loopTime.
+  for (x=0;x<loopTime;x++)   
+  {   
+              digitalWrite(speakerPin,HIGH); 
+              delayMicroseconds(microsecondsPerWave); 
+              digitalWrite(speakerPin,LOW); 
+              delayMicroseconds(microsecondsPerWave); 
+  }
+  delay(silentDuration); 
+}  
+
+
+
+
+//Pruebas sonidoo
+//---
+void ZowiMelody(float melodyNotes[], int melodySize){
+
+     for (int i=0;i<=melodySize;i=i+3) //por cada "fila"
+     {
+        ZowiTone(PIN_Buzzer, melodyNotes[i],melodyNotes[i+1],melodyNotes[i+2]);
+     }
+
+}
+
+void conexion (){
+          ZowiTone(PIN_Buzzer, note_E6,50,30);
+          ZowiTone(PIN_Buzzer, note_E7,50,30);
+          ZowiTone(PIN_Buzzer, note_A7,50,30);
+}
+
+void desconexion (){
+          ZowiTone(PIN_Buzzer, note_E6,50,30);
+          ZowiTone(PIN_Buzzer, note_A7,50,30);
+          ZowiTone(PIN_Buzzer, note_E7,50,30);
+}
+
+
+
+
+
+
+
 
 //Constant feedback of the sensors. F 1 = Feedback ON  F 0 = Feedback 0
 /*void sensorFeedback()




//Function to manage the Zowi states with the back buttons
void checkButtonsforState(){
  
  if (digitalRead(PIN_SecondButton)==1 && (state<3)){
    newstate++;
  }
  else if ((digitalRead(PIN_ThirdButton)==1) && (state>1)){
    newstate--;
  }
  if  (Serial.available()>0){
    newstate=4;
  }
}



//Function to receive LED commands
void LED(){  
  //Examples of LED Bluetooth commands
  //L 00000000100001010010001100000000
  //L 00111111111111111111111111111111 (todos los LED encendidos)
  unsigned long int matrix;

////////////////////////////

int programID=1; //Each program will have a ID in order to 
int T=1000; //Initial duration of movement
int moveId=0; //Number of movement
int endmove=1; // 1= stop 0= moving
int moveSize=15; //Many movements accept a moveSize parameter asociated with the height of that movement
int surprised=0; //For US Test
volatile int state=0; //State of zowi state machine
volatile int buttonPushed=0; //Variable to remember when a button has been pushed
unsigned long previousMillis=0;
int sensorFeedbackOn=0; //Variable to activate the sensor comunication from Zowi to the App
