#include <Servo.h>

#define MaxServo 130
#define MinServo 50

const byte blink_left_pin  = 4;
const byte blink_right_pin = 5;
const byte servo_pin = 8;
const int servoMaxPulseWidth = 2000;
const int servoMinPulseWidth = 1000;

const byte servoMaxAngle = 180;
const byte servoMinAngle = 0;

unsigned long servoDelayTime = 100;

Servo myservo;  // create servo object to control a servo
// twelve servo objects can be created on most boards

int steering_angle = 0;
byte angle;

char input;

void setup() {
  Serial.begin(115200);
  Serial.println("Setup-Start");

  myservo.attach(servo_pin, servoMinPulseWidth, servoMaxPulseWidth);  // attaches the servo on pin 9 to the servo object

  angle = 0;
  Serial.print("Servotest drive to ");
  Serial.print(angle);
  Serial.println("°");
  myservo.write(angle);
  delay(4000);

  angle = 180;
  Serial.print("Servotest drive to ");
  Serial.print(angle);
  Serial.println("°");
  myservo.write(angle);
  delay(4000);

  angle = 90;
  Serial.print("Servotest drive to ");
  Serial.print(angle);
  Serial.println("°");
  myservo.write(angle);
  delay(4000);
  steering_angle = myservo.read(); 
  Serial.println("setup done");
  Serial.println("waiting for serial input a or d");
}


void loop() {
  
  if (Serial.available() ) {
    input = Serial.read();    
  }

  if (input == 'a') {
    steering_angle = steering_angle + 10;
    if (steering_angle > servoMaxAngle) { // limit variable to max-value
      steering_angle = servoMaxAngle;
    }
    Serial.print("new angle=");
    Serial.println(steering_angle);
  }
  else if (input == 'd') {
    steering_angle = steering_angle - 10;
    if (steering_angle < servoMinAngle) { // limit variable to min-value
      steering_angle = servoMinAngle;
    }
    Serial.println(steering_angle);
  }

  turnServoSlow(steering_angle);
  delay(100);
}

void turnServoSlow(int angle ) {
  int pos = 0;
  int current_angle = myservo.read();
  if (angle > current_angle) {
    for (pos = current_angle; pos <= angle ; pos += 1) {  // goes from 0 degrees to 180 degrees
      // in steps of 1 degree
      myservo.write(pos);  // tell servo to go to position in variable 'pos'
      delay(servoDelayTime);           // waits 15 ms for the servo to reach the position
    }
  }
  
  if (angle < current_angle) {
    for (pos = current_angle; pos >= angle ; pos -= 1) {  // goes from 180 degrees to 0 degrees
      myservo.write(pos);                                     // tell servo to go to position in variable 'pos'
      delay(servoDelayTime);                                              // waits 15 ms for the servo to reach the position
    }
  }
}