#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Servo.h>

#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);

Servo servo9;
Servo servo10;
Servo servo11;

void setup() {
  Serial.begin(9600);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);  // initialize with the I2C addr 0x3C (for the 128x32)
  display.setTextColor(WHITE);
  display.stopscroll();
  // init done

  //Set Splash screen
  display.clearDisplay();
  display.setTextSize(2);
  display.print("Servo\nTester");
  display.display();
  delay(2000);

  display.clearDisplay();
  display.setTextSize(1);
  display.setCursor(0,0);
  display.print("PWM Pins:\n9,10,11");
  display.display();
  delay(2000);

  //Attaches the servo to a pin.
  servo9.attach(9);
  servo10.attach(10);
  servo11.attach(11);

  display.setTextSize(1);
}

void loop() {
  int input = 0;
  int angle = 0;
  display.clearDisplay();
  //Read analog value form a0 and set servo values.
  input = analogRead(0);
  angle = map(input,0,1024,0,180);
  servo9.write(angle);

  display.setCursor(0,0);
  display.print("A0:");
  display.print( input );
  display.setCursor(52,0);
  display.print( "PWM-09:");
  display.print( angle );

  //Read analog value form a1 and set servo values.
  input = analogRead(1);
  angle = map(input,0,1024,0,180);
  servo10.write(angle);

  display.setCursor(0,10);
  display.print("A1:");
  display.print( input );
  display.setCursor(52,10);
  display.print( "PWM-10:");
  display.print( angle );

  //Read analog value form a2 and set servo value.
  input = analogRead(2);
  angle = map(input,0,1024,0,180);
  servo11.write(angle);

  //Display servo 2
  display.setCursor(0,20);
  display.print("A1:");
  display.print( input );
  display.setCursor(52,20);
  display.print( "PWM-11:");
  display.print( angle );
  display.display();
  delay(100); //Give the servos time to respond not make every.
}