Servo Tester Arduino UNO

ServoTester.ino

/* 
Analog inputs 0 & 1 should be hooked up to a joystick style potentiometer to control servos 3 & 5. 
Analog inputs 2 & 3 should be hooked up to dial style  potentiometers to control servos 6 & 9. 
Servo 10 & 11 will always be centered.
Analog inputs 4 & 5 are used by the screen.
*/

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

#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);

Servo servo3;
Servo servo5;
Servo servo6;
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("Input Pins:\nA0,A1,A2,A3,A4,A5");
  display.display();
  delay(2000);

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

  //Attaches the servo to a pin.
  servo3.attach(3);  
  servo5.attach(5);  
  servo6.attach(6);  
  servo9.attach(9);  
  servo10.attach(10);
  servo11.attach(11); 

  display.setTextSize(1);
}

int setServo(int input, Servo *servo ) {
  int angle = map(input,2,1022,0,180);
  servo->write(angle);
  return angle;
}

void loop() {
  int input = 0;
  int angle = 0;
  display.clearDisplay();
  display.setCursor(0,0);
  display.print( "PWM-05:");
  display.print( setServo(analogRead(0), &servo3 ));

  display.setCursor(65,0);
  display.print( "PWM-05:");
  display.print( setServo(analogRead(1), &servo5 ));

  display.setCursor(0,10);
  display.print( "PWM-06:");
  display.print( setServo(analogRead(2), &servo6 ));

  display.setCursor(65,10);
  display.print( "PWM-09:");
  display.print( setServo(analogRead(3), &servo9 ));

  display.setCursor(0,20);
  display.print( "PWM-10:");
  display.print( setServo(512, &servo10 ));

  display.setCursor(65  ,20);
  display.print( "PWM-11:");
  display.print( setServo(512, &servo11 ));
  display.display();
}