volatile unsigned int temp, counter = 0; //This variable will increase or decrease depending on the rotation of encoder
#include <Servo.h>
// Rotary Encoder Inputs
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 20, 4);
#define inputCLK 3
#define inputDT 2
 
 // LED Outputs
#define ledCW 12
#define ledCCW 13

Servo myservo1;
Servo myservo2;
Servo myservo3;
Servo myservo4;

 
 int Position = 0;
 int servo1Position;
 int servo2Position;
 int servo3Position;
 int servo4Position;
 int MAX = 600;
 float rpm = 0;
 float encoder_pulse_counter = 0;
 float time = 0;
 float currentTime = 0;
 float previousTime = 0;

 String encdir ="";   


void setup() {
  Serial.begin (115200);

  pinMode(2, INPUT_PULLUP); // internal pullup input pin 2 
  
  pinMode(3, INPUT_PULLUP); // internalเป็น pullup input pin 3

// Set LED pins as outputs
  pinMode (ledCW,OUTPUT); 
  pinMode (ledCCW,OUTPUT);

// Set Servo Outputs
  myservo1.attach(9);
  myservo2.attach(10);
  myservo3.attach(11);
  myservo4.attach(6);

//Setting up interrupt
  //A rising pulse from encodenren activated ai0(). AttachInterrupt 0 is DigitalPin nr 2 on moust Arduino.
  attachInterrupt(0, ai0, RISING);
   
  //B rising pulse from encodenren activated ai1(). AttachInterrupt 1 is DigitalPin nr 3 on moust Arduino.
  attachInterrupt(1, ai1, RISING);

  
 //------------------------LCD---------------------
  lcd.init();
  lcd.backlight();
  lcd.setCursor(1, 0);
  lcd.print("Hello,");
    digitalWrite(ledCW, HIGH);
    digitalWrite(ledCCW, HIGH);
  delay(1000);
  lcd.clear();
  digitalWrite(ledCW, LOW);
  digitalWrite(ledCCW, LOW);
  }
   //----------------------------VOID LOOP--------------------------------------
  void loop() {
  // Send the value of counter
  if( counter != temp ){
  time = millis();
  


  Serial.println (Position);
  

//------------------------LCD--------------------------------
    lcd.clear();
    lcd.setCursor(1, 0);

    if (rpm >= 60 ){
       lcd.print("Position: >>>>");
    
    } else {

    lcd.print("Position: ");
    lcd.setCursor(11, 0);
    lcd.print(Position);
    lcd.print("\337");}
    lcd.setCursor(6, 1);
    lcd.print("RPM: ");
    lcd.setCursor(11, 1);
    lcd.print(rpm);

     myservo1.write(servo1Position);
     myservo2.write(servo2Position);
     myservo3.write(servo3Position);
     myservo4.write(servo4Position);
  temp = counter;

  }


  }
 //----------------------------VOID AI0--------------------------------------    
  void ai0() {
  // ai0 is activated if DigitalPin nr 2 is going from LOW to HIGH
  // Check pin 3 to determine the direction
  if(digitalRead(3)==LOW) {
  counter++;

      digitalWrite(ledCW, LOW);
      digitalWrite(ledCCW, HIGH);
 Position = Position - 1;
            if (Position <= -1){
        Position = 359;

        }

  }
    
  }
 //----------------------------VOID AI1--------------------------------------  
  void ai1() {
  // ai0 is activated if DigitalPin nr 3 is going from LOW to HIGH
  // Check with pin 2 to determine the direction
  if(digitalRead(2)==LOW) {
  counter--;

     digitalWrite(ledCW, HIGH);
     digitalWrite(ledCCW, LOW);
Position = Position + 1;
if (Position >= 360){
        Position = 0;
       }
  
encoder_pulse_counter = encoder_pulse_counter + 1;
      if (encoder_pulse_counter >= 3600){
      currentTime = time;
      encoder_pulse_counter = 0;
      rpm = (10/((currentTime - previousTime)/60000));


     } else if (encoder_pulse_counter <= 1) {
        previousTime = time;

       } else  { 
        }
  }else{
  }


  
  //---------------------------RPM Less Than 120---------------------------
if (rpm <= 120){
//-------------------------Servo 1--------------------------
          if (Position >= 350 & Position < 360 ){
          servo1Position=175;
          } else if (Position >= 00 & Position <= 10 ){
          servo1Position=05;}
       else if (Position >= 170 & Position < 180 ){
          servo1Position=05;
          } else if (Position >= 180 & Position <= 190 ){
          servo1Position=175;
     } else {
       servo1Position=90;}

//-------------------------Servo 2--------------------------
          if (Position >= 80 & Position < 90 ){
          servo2Position=175;
          } else if (Position >= 90 & Position <= 100 ){
          servo2Position=05;}
       else if (Position >= 260 & Position < 270 ){
          servo2Position=05;
          } else if (Position >= 270 & Position <= 280 ){
          servo2Position=175;
     } else {
       servo2Position=90;}

//-------------------------Servo 3--------------------------
          if (Position >= 170 & Position < 180 ){
          servo3Position=175;
          } else if (Position >= 180 & Position <= 190 ){
          servo3Position=05;}
       else if (Position >= 350 & Position < 360 ){
          servo3Position=05;
          } else if (Position >= 00 & Position <= 10 ){
          servo3Position=175;
     } else {
       servo3Position=90;}

//-------------------------Servo 4--------------------------
          if (Position >= 260 & Position < 270 ){
          servo4Position=175;
          } else if (Position >= 270 & Position <= 280 ){
          servo4Position=05;}
       else if (Position >= 80 & Position < 90 ){
          servo4Position=05;
          } else if (Position >= 90 & Position <= 100 ){
          servo4Position=175;
     } else {
       servo4Position=90;}

}

//---------------------------RPM More Than 120 less than MAX ---------------------------
if (rpm > 120 & rpm < MAX){
//-------------------------Servo 1--------------------------
          if (Position >= 350 & Position < 360 ){
          servo1Position=175;
          } else if (Position >= 00 & Position <= 10 ){
          servo1Position=05;

     } else {
       servo1Position=90;}

//-------------------------Servo 2--------------------------
          if (Position >= 80 & Position < 90 ){
          servo2Position=175;
          } else if (Position >= 90 & Position <= 100 ){
          servo2Position=05;

     } else {
       servo2Position=90;}

//-------------------------Servo 3--------------------------
          if (Position >= 170 & Position < 180 ){
          servo3Position=175;
          } else if (Position >= 180 & Position <= 190 ){
          servo3Position=05;

     } else {
       servo3Position=90;}

//-------------------------Servo 4--------------------------
          if (Position >= 260 & Position < 270 ){
          servo4Position=175;
          } else if (Position >= 270 & Position <= 280 ){
          servo4Position=05;

     } else {
       servo4Position=90;}

}
 //---------------------------RPM MAX ---------------------------
if (rpm >= MAX){
//-------------------------Servo 1--------------------------
          
       servo1Position=90;

//-------------------------Servo 2--------------------------
         
       servo2Position=90;

//-------------------------Servo 3--------------------------
       
       servo3Position=90;

//-------------------------Servo 4--------------------------
         
       servo4Position=90;

} 



  }