Automatic Guitar Tuner using servo motor

Im currently creating a project where I can tune a guitar using servo motors and a frequency sensing circuit.

The code below is what im using for now:

#include <avr/io.h>
#include <avr/interrupt.h>


//analog guitar signal input
int guitarInput = A0; 

//LED inputs
int led_high = 13; 
int led_ok = 12; 
int led_low = 11;
int led_e4 = 10;
int led_b3 = 9;
int led_g3 = 8;
int led_d3 = 7;
int led_a2 = 6;
int led_e2 = 5;

//string selection
int button_pin = 3; //button pin 
int string_select;
int select_pin_val;

//Mic Variables
int inputPin = A0; 
byte inputValue; 
const unsigned int len =  500;
volatile unsigned int data[len];
volatile int count = 0;
const double sample_freq = 6400;

//Frequency Variables
float Frequency = 0;
float lastFrequency = 0;
float URange = 0;
float LRange = 0;
float targetFreq = 0;

//Servo
int servoPin = 2; // control pin for servo motor

//Timer
long timer;
int idle_timer;



void setup()
{
  timer = 0; // 1/6400 second
  pinMode(inputPin, INPUT);

  //setup timing and frequency measuring 
  cli();
  TCCR1A = 0;
  TCCR1B = 0; 

  //set compare match register to desired timer count
  //Value = (time between interrupts / 0.0000000625)s - 1
  OCR1A = 2499;
 
  TCCR1B |= (1 << WGM12);
  TCCR1B |= (1 << CS10);
  TIMSK1 |= (1 << OCIE1A);

//  //setup low pass filter
//  // filters out changes faster that 5 Hz.
//  float filterFrequency = 400.0;
//  
//  // create a one pole (RC) lowpass filter
//  FilterOnePole lowpassFilter(LOWPASS, filterFrequency);   

  
  //Set the input and output pins
  pinMode(button_pin, INPUT);
  pinMode(servoPin, OUTPUT);
  pinMode(led_high, OUTPUT);
  pinMode(led_ok, OUTPUT);
  pinMode(led_low, OUTPUT);
  pinMode(led_e4, OUTPUT);
  pinMode(led_b3, OUTPUT);
  pinMode(led_g3, OUTPUT);
  pinMode(led_d3, OUTPUT);
  pinMode(led_a2, OUTPUT);
  pinMode(led_e2, OUTPUT);
  
  sei();
  Serial.begin(9600); //reenable interrupts
}

// Nothing is done in the loop
void loop()
{ 
}

// interrupt service routine every microsecond
ISR(TIMER1_COMPA_vect)
{
  timer++;
  

// 
////  Read button press to determine which string is to be detected
//  if (timer % 1000 == 0)
//  {
//    select_pin_val = digitalRead(button_pin);
//    if (select_pin_val == HIGH)
//    {
//      string_select = ((string_select + 1) % 6);
//      Serial.print("string: ");
//      Serial.println(string_select);
//    }
//  }



  //get signal data and store to array, process every 600 samples
  data[count] = analogRead(inputPin);
  //  Serial.println(data[count]);
  count ++;
  if (count == len)
  {
    count = 0;
    freqFind();
  }

  //ALL LEDS OFF
  digitalWrite(led_e4, LOW); // sets the proper LED on, all else off
  digitalWrite(led_b3, LOW);
  digitalWrite(led_g3, LOW);
  digitalWrite(led_d3, LOW);
  digitalWrite(led_a2, LOW);
  digitalWrite(led_e2, LOW);

  
  // Depending on which string is selected, the proper variables are set
  switch (string_select)
  {
  case 0: //LOW E STRING
    digitalWrite(led_e2, HIGH);
    targetFreq = 105.41;
    URange = targetFreq + 30;
    LRange = targetFreq - 30;
    break;
  
  case 1: //A STRING
    digitalWrite(led_a2, HIGH);

    targetFreq = 141.00;
    URange = targetFreq + 30;
    LRange = targetFreq - 30;
    break;
  
  case 2: //D STRING
    digitalWrite(led_d3, HIGH);

    targetFreq = 184.83;
    URange = targetFreq + 30;
    LRange = targetFreq - 30;
    break;
  
  case 3: //G STRING
    digitalWrite(led_g3, HIGH);

    targetFreq = 245.00;
    URange = targetFreq + 30;
    LRange = targetFreq - 30;
    break;
  
  case 4: //B STIRNG
    digitalWrite(led_b3, HIGH);

    targetFreq = 315.94;
    URange = targetFreq + 30;
    LRange = targetFreq - 30;
    break; 
  
  case 5: //HIGH E STRING
    digitalWrite(led_e4, HIGH); 

    targetFreq = 414;
    URange = targetFreq + 30;
    LRange = targetFreq - 30;
    break;
  }
  
  
  // After the string input has been idle for a while, we take the average of a number of cross counts that were in bound.
  if (idle_timer >= 3000)
  {
    Serial.println("Chillin");
  }

}

void tune(float average)
{  
  //allowable +/- 
  float max_allowable = 4.0;
  float min_allowable = 4.0;
  digitalWrite(led_high, LOW);
  digitalWrite(led_ok, LOW);
  digitalWrite(led_low, LOW);
//  cli(); //disable interrupts
 
  if (average > targetFreq + 2)
  {
    Serial.println("     TOO HIGH");
    tooHigh(average - targetFreq);
  }
  else if (average < targetFreq -2)
  {
    Serial.println("     TOO LOW");
    tooLow(targetFreq - average);
  }
  else
  {
    Serial.println("     JUST RIGHT");
    OK();
  }

  
    

}

void OK()
{
  int currentString = string_select;
  digitalWrite(led_high, LOW);
  digitalWrite(led_ok, HIGH);
  digitalWrite(led_low, LOW);
  Serial.println("ONTO THE NEXT STRING!!"); 
  //move onto next string
  string_select = ((string_select + 1) % 6);
}

void tooLow(float difference)
{
  Serial.print("     DIFFERENCE: ");
  Serial.println(difference);
  digitalWrite(led_high, LOW);
  digitalWrite(led_ok, LOW);
  digitalWrite(led_low, HIGH);
  turnCCW(difference);
}

void tooHigh(float difference)
{
  Serial.print("     DIFFERENCE: ");
  Serial.println(difference);
  digitalWrite(led_high, HIGH);
  digitalWrite(led_ok, LOW);
  digitalWrite(led_low, LOW);
  turnCW(difference);
}

//1 HZ = 7.5 degrees

void turnCW(float difference)
{
  double angle = difference*.1;
  double pulseWidth = (angle * 10 ) + 600;
  for(long i = 0; i < difference * 100; i++)
  {
    digitalWrite(servoPin, HIGH); // start the pulse
    delayMicroseconds(pulseWidth); // pulse width
    digitalWrite(servoPin, LOW); // stop the pulse
  }
}

void turnCCW(float difference)
{
  double angle = difference*.1;
  double pulseWidth = (angle * 10 ) + 600;
  for(long i = 0; i < difference * 100; i++)
  {
    digitalWrite(servoPin, HIGH); // start the pulse
    delayMicroseconds(pulseWidth); // pulse width
    digitalWrite(servoPin, LOW); // stop the pulse
  }
}



float freqFind()
{
  static float count = 0;
  static float avgsum  = 0;
  float average;
  
//  cli(); //disable interrupts
  double sum = 0;
  int thresh = 0;
  byte pd_state = 0;
  double period = 0;
  long sum_old;
  double freq_per = 0;
  int k, i;
  // Autocorrelation
  for (int i=0; i < len && (pd_state != 3); i++)
  {
    sum_old = sum;
    sum = 0;

   for (k=0; k <len-i; k++)
   {
     sum += (data[k]) * (data[k+i]);
   }
   sum /= k ;

    // Peak Detect State Machine
    // 0: initial, set threshold
    // 1: detect positive slope
    // 2: detect negative slope
    // 3: peak found
    if (pd_state == 0)
    {
      thresh = sum / 2;
      pd_state = 1;
    } 
    else if (pd_state == 1 && (sum > thresh) && (sum - sum_old) > 0) 
    {
      pd_state = 2;
    }
    else if (pd_state == 2 && (sum - sum_old) <= 0) 
    {
      period = i;
      pd_state = 3;
    } 
  }


  freq_per = sample_freq*1.036/period;

  //if idling
  if (freq_per < 10)
  {
    idle_timer++;
  }

  else if(freq_per > LRange && freq_per < URange)
  {
    // Frequency identified in Hz
    Serial.println("Value Caputured");
    avgsum += freq_per;
    count += 1;
    idle_timer = 0;
  }

  Serial.println(freq_per);
  
  if (count == 2)
  {
    
    average = avgsum/count;
    Serial.print("     Average Frequency: ");
    Serial.print("\t");
    Serial.println(average);
    Serial.print("     Target Frequency: ");
    Serial.print("\t");
    Serial.println(targetFreq);
    
    

    //LED indicate sample complete
    digitalWrite(led_high, LOW);
    digitalWrite(led_ok, LOW);
    digitalWrite(led_low, LOW);
    digitalWrite(led_ok, HIGH);
    delay(500);
    digitalWrite(led_ok, LOW);
    delay(100);
    digitalWrite(led_ok, HIGH);
    delay(500);
    digitalWrite(led_ok, LOW);

    cli();//disable interrupts
    
    //TUNE THE GUITAR!
    tune(average);
    
    sei(); //reenable interrupts
    
    
    //reset counter variables
    count = 0;
    avgsum = 0;

    
  }
  
  return(average);
}

My question is why are the libraries included in the code not working? They must be in orange color text right? what am I doing wrong? Plus I cant find any of the libraries used in the code that are downloadable online.

They must be in orange color text right?

Wrong. Colour coding in the Arduino IDE is very Mickey Mouse in nature. If the code is working then all is OK. If it is not working then what error messages do you get when compiling ?

with or without the library, compilation is always successful.
But my prototype isnt working properly.

You mean that the libraries used in the code are working?

bembe24:
with or without the library, compilation is always successful.
But my prototype isnt working properly.

You mean that the libraries used in the code are working?

I think that you will find that the libraries are included automatically anyway so the explicit #includes are not needed.

What is the problem with your prototype ?

servo wont turn and pushbutton does not work. maybe theres something wrong with my connections.

servo wont turn

Why are you not using the Servo library?

pushbutton does not work.

What do you expect it to do? Push itself? Push back when you push it? Squeal like a pig?

Where do you actually read the state of the pin?

int guitarInput = A0;
int inputPin = A0;

So, you are using analog pin 0 for two different purposes?

ooppss that I didnt notice. Rookie mistake. Im sorry for that.
Just to be clear, so there is no problem with the libraries used in the code?

bembe24:
ooppss that I didnt notice. Rookie mistake. Im sorry for that.
Just to be clear, so there is no problem with the libraries used in the code?

There are major problems with how you use interrupts, but they are not the library's fault.

As has been pointed out, you can't use delay() in an interrupt handler. Interrupts are already disabled, and you should not re-enable them unless you really know what you are doing (which, obviously, you don't).