/* Control Servo motor with Rotary encoder and display angle on LCD display
// for Best performace connect this both pins to interupt capable pins of Arduino
 */
#include <Encoder.h>
#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

//define a pin for rotary encode switch
const int SW_PIN = 4;
const int PIN_A =2;
const int PIN_B =3;
// for Best performace connect this both pins to interupt capable pins of Arduino

Encoder myEnc(PIN_A, PIN_B);
//avoid using pins with LEDs attached

//Define Servo motors
const int homePosition = 90; //initial position
const int stepValue = 3;//how fast the servo should rotate when turning the knob
const int servoPin = 9;//~must be a pin that that is labeled with ~

Servo myservo;  // create servo object to control a servo
int servoAngle =homePosition;

// Set the LCD address to 0x27 or 0x3F for a 16 chars and 2 line display
LiquidCrystal_I2C lcd(0x27, 16, 2);

void setup() {
  Serial.begin(9600);
  pinMode(SW_PIN, INPUT);
  Serial.println("Control Servo with Rotary encoder:CircuitSchools");
   myservo.attach(servoPin);  // attaches the servo on pin
   myservo.write(servoAngle);//move servo to initial position

  lcd.init();
  // Turn on the blacklight and print a message.
  lcd.backlight();     
  lcd.print("Encoder Servo");
  lcd.setCursor (0,1); // go to start of 2nd line
  lcd.print("Servo Test");   
  delay(2000);   
   lcd.clear();// clear previous values from screen   
  lcd.print("Encoder Servo");
  lcd.setCursor (0,1); // go to start of 2nd line  
  lcd.print("Angle: ");   
}

long oldPosition  = -999;

void loop() {
  long newPosition = myEnc.read();
  if (newPosition != oldPosition) {

    if(newPosition >  oldPosition)
    {
    int newStep = abs(newPosition - oldPosition);
      Serial.print("Angle ");
      Serial.println(servoAngle);      
      servoAngle -= stepValue;
      if(servoAngle <0)
          servoAngle =0;      
      myservo.write(servoAngle); //move servo to new angel
      lcdAngle(servoAngle);//print on LCD
    }

    if(newPosition <  oldPosition )
    {
    int newStep = abs(newPosition - oldPosition);
      Serial.print("Angle ");
      Serial.println(servoAngle);        
      servoAngle += stepValue;
      if(servoAngle >180)
          servoAngle =180;
      myservo.write(servoAngle); //move servo to new angel
      lcdAngle(servoAngle);//print on LCD
    }
   oldPosition = newPosition;//remember the new position
  }

  if( digitalRead(SW_PIN) == LOW)
  {
    Serial.print("Home: ");
    Serial.println(homePosition);
    servoAngle =homePosition;
      myservo.write(servoAngle); //move servo to new angel
      lcdAngle(servoAngle);//print on LCD
  }

  //delay(200);
}

void lcdAngle(int angle)
{
  
  int startChar =7;
  for(int i=startChar; i<16; i++)
  {
   lcd.setCursor (i,1);    
    lcd.print(" ");   
  }
 
   lcd.setCursor (startChar,1); // line 1 
   lcd.print(angle); 
   lcd.print((char)223);
}