/* 
 *  Lesson 97-2: Controlling Servo Motor using Rotary Encoder 

 *  
 *  Watch full details video: https://youtu.be/6xVLbNlmK-g
  This video is part of the Arduino Step by Step Course, which starts here: https://youtu.be/-6qSrDUA5a8
 

If you found this tutorial helpful, please support me so I can continue creating 
content like this. Make a donation using PayPal or a credit card: https://bit.ly/donate-robojax
  
 * This code is "AS IS" without warranty or liability. Free to be used as long as you keep this note intact.* 
 * This code has been downloaded from Robojax.com
    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <https://www.gnu.org/licenses/>.
 *  Updated by Ahmad Shamshiri on Jan 23, 2022
 *  Encoder Library - Basic Example
 * http://www.pjrc.com/teensy/td_libs_Encoder.html
 *
 * This example code is in the public domain.
 */
const int SW_PIN = 4;
const int PIN_A =2;
const int PIN_B =3;

#include <Encoder.h>

// Change these two numbers to the pins connected to your encoder.
//   Best Performance: both pins have interrupt capability
//   Good Performance: only the first pin has interrupt capability
//   Low Performance:  neither pin has interrupt capability
Encoder myEnc(PIN_A, PIN_B);
//   avoid using pins with LEDs attached

const int homePosition = 90; //initial position
const int stepValue = 5;
const int servoPin = 9;//must be a pin that that is labeled with ~
#include <Servo.h>

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

void setup() {
  Serial.begin(9600);
  pinMode(SW_PIN, INPUT);
  Serial.println("Basic Encoder Test:");
   myservo.attach(servoPin);  // attaches the servo on pin
   myservo.write(servoAngle);//move servo to initial position
}

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);
    }

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

  if( digitalRead(SW_PIN) == LOW)
  {
    Serial.print("Home: ");
    Serial.println(homePosition);
    servoAngle =homePosition;
    myservo.write(servoAngle);
  
  }

//Watch full details video: https://youtu.be/6xVLbNlmK-g
  //delay(200);
}