#include <Servo.h>
#include <LiquidCrystal.h>

int Encod1CLK = A0; 
int Encod2CLK  = A1;
int speed = 50;            // Gia tri toc do
float angle = 90;        // Gia tri goc lai

Servo s1;
Servo s2;
LiquidCrystal lcd(12, 11, 7, 6, 5, 4);  // Khoi dong LCD 16x2

void Encod1();
void Encod2();
void setup() 
{
  Serial.begin(9600);
  pinMode(Encod1CLK, INPUT_PULLUP);
  pinMode(Encod2CLK, INPUT_PULLUP);
  attachInterrupt(0, Encod1, RISING);
  attachInterrupt(1, Encod2, RISING);



  s1.attach(9);            // Chan PWM servo 1
  s2.attach(8);            // Chan PWM servo 2
  lcd.begin(16, 2);        // Khoi tao lcd 16x2
  lcd.print("Speed:");   
  lcd.setCursor(0, 1);
  lcd.print("Angle:");
  s1.write(angle);  
  s2.write(angle);      
}

void loop() 
{
  // Update LCD with speed and angle
  lcd.setCursor(7, 0);
  lcd.print(speed);
  lcd.print("Km/h   ");              
  lcd.setCursor(7, 1);
  lcd.print(angle, 1);            
  lcd.print(" deg  ");            
  s1.write(angle); 
  s2.write(angle); 
}
float offset = 0.03;
void Encod1() // Dieu chinh toc do
{
  if(digitalRead(Encod1CLK)==HIGH)
    speed++;
  else
    speed--;
  if(speed > 50)
    offset = 0.1;
  else
    offset = 0.03;
}
void Encod2() // Dieu chinh servo
{
    if(digitalRead(Encod2CLK)==HIGH)
    {
      angle+= offset;
      if(angle>105)
          angle=105;
    }
    else
    {
      angle -= offset;
       if(angle<75)
          angle=75;
    }

}