/*
LAB10 PT3-4 - SERVO MOTOR CONTROL
TESTS OK WITH POT

3.  Demonstrate that you can control a servo motor.
Hookup the servo motor from the example in the lecture.
Start with the example code in the lecture and test it.

Add code to continually sweep the servo 
moothly from 0 to 180 degrees and then from 180 back to 0 degrees 
with a delay of 30 ms between each degree.

4.  Remove the sweep code from #3. Add a potentiometer. 
Read the value and map it to 0-180. 
Use the value map returns to change the servo position.

*/

#include <Servo.h>
Servo myservo;  // Initializes the servo library
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 16, 2);  // lcd checks ok

const byte SERVO = 3;  // Any digital output pin

void setup() {
   Serial.begin(9600);
   myservo.attach(SERVO);  // Connects “servoPin” to controller library
   //lcd.begin();
   lcd.init();
   lcd.backlight();
   lcd.setCursor(0,0);
   lcd.print("LAB10PT3 Servo");
   lcd.setCursor(0,1);
   lcd.print("position = ");

}

void loop() {
   // for (int i = 0; i <= 180; i++) {  // Step from 0 to 180 degrees
   //    myservo.write(i); // Sets the servo to “i” degrees
   //    delay(30); // Slow it down so the human can see it
   // }
   int position = analogRead(A0);  // from 0 to 1023?
   int servoPosition = map (position, 0, 1023, 0, 180);
   Serial.print(position);
   Serial.print(", ");
   Serial.println(servoPosition);
   myservo.write(servoPosition);
   lcd.setCursor(11,1);
   paddedNumber(servoPosition);
   delay(30);
}
int paddedNumber(int num) {
   if (num < 100) lcd.print(" ");
   if (num < 10) lcd.print(" ");
   lcd.print(num);
   lcd.print((char)223);  // prints the degree symbol
}