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

#define joystickYPin A1
#define joystickXPin A2
#define enterButtonPin 6
#define maxSpeed 30

Adafruit_SSD1306 displayX(128, 64, &Wire, -1);  // SCL=A5  SDA=A4
int joystickCurrentXPosition;
int joystickCurrentYPosition;

Servo myservo;
int servoPos;
int servoSpeed;


void setup() {
Serial.begin(9600);
  if(!displayX.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
    }
  Serial.println(F("SSD1306 allocation successfull"));

  pinMode(joystickXPin, INPUT);
  pinMode(joystickYPin, INPUT);
  pinMode(enterButtonPin, INPUT_PULLUP);
  servoPos = 90;
  servoSpeed = 5;
  joystickCurrentXPosition = joystickCurrentYPosition = 512;

  myservo.attach(9);  // attaches the servo on pin 9 to the servo object
  servoGo ();
  displayParams();
  servoSpeed = 0;

  displayX.setTextSize(2);
  displayX.setTextColor(SSD1306_WHITE);
  displayX.clearDisplay();
  }


void loop() {
  joystickCurrentXPosition = analogRead(joystickXPin);
  if ((joystickCurrentXPosition < 450) && (servoPos < 180)) {
    servoPos ++;
    servoGo ();
    displayParams();
    }
  if ((joystickCurrentXPosition >550) && (servoPos > 0)) {
    servoPos --;
    servoGo ();
    displayParams();
    }

  joystickCurrentYPosition = analogRead(joystickYPin);
  if ((joystickCurrentYPosition < 450) && (servoSpeed < maxSpeed)) {
    servoSpeed ++;
    displayParams();
    }
  if ((joystickCurrentYPosition > 550) && (servoSpeed > 0)) {
    servoSpeed --;
    displayParams();
    }

  if (digitalRead(enterButtonPin) == LOW) {
    servoPos = 90;
    servoGo ();
    displayParams();
    delay(100);
    }

  delay(10);
  }


void servoGo () {
  if (servoSpeed == 0)  return;
  myservo.write(servoPos);
  delay(maxSpeed - servoSpeed);         
  }


void displayParams() {
  displayX.clearDisplay();
  displayX.setCursor(5, 20); displayX.print(servoPos);
  displayX.setCursor(50, 20); displayX.print(servoSpeed);
  displayX.display();
  }