#include <ESP32Servo.h>

const int servoPin = 18;
const int poti_read = 27;
const int speed = 15;
const int servodelay = 50;
const int pinup = 35;
const int pindown = 34;

Servo servo;


void slow_sweep (void);
void servo_potentiometer (void);
void sinus_sweep (void);
void button_steering (void);
void button_control (void);



void setup() {
  Serial.begin(115200);
  Serial.println("esp32 startup");

  // servo.attach(servoPin);  works also fine
  servo.attach(servoPin, 544, 2400);
  // pin: the number of the pin that the servo is attached to
  // min (optional): the pulse width, in microseconds, corresponding to the minimum (0 degree) angle on the servo (defaults to 544)
  // max (optional): the pulse width, in microseconds, corresponding to the maximum (180 degree) angle on the servo (defaults to 2400)
  
  pinMode (pindown, INPUT_PULLUP);
  pinMode (pinup, INPUT_PULLUP);
}


float r = 0.0;          // in sinus_sweep benötigt
int position = 0;
int step = 0;


// --------------   main  function  --------------------------
void loop() {
  servo.write(position);
  delay (servodelay);
  // slow_sweep ();
  // servo_potentiometer ();
  // sinus_sweep (30, 150);
  // button_steering ();   // up & down from 0 to 180, no steps in between
  button_control ();
}



// ------------ servo movement as long button is pressed ------------
void button_control (void) {
  int speedo = 2, position_old = position;
  if ((digitalRead(pinup) == LOW) || (digitalRead(pindown) == LOW)) {
    if (digitalRead(pinup) == LOW)
      position += speedo;
    if (digitalRead(pindown) == LOW)
      position -= speedo;
  }
  if (position >180)
    position = 180;
  if (position < 0)
    position = 0;
  if (position != position_old)
    servo.write(position);
  Serial.print("Position: ");
  Serial.println(position);
  delay (servodelay);
}



// --------------  push button steering 0 - 180° --------------
void button_steering (void) {
  bool up = false;
  if( digitalRead(pinup) == LOW)
    up = true;
    bool down = false;
  if( digitalRead(pindown) == LOW)
    down = true;

  if( step == 0 and up)
    step = 1;
  else if( step == 0 and down)
    step = -1;
  else if( step > 0 and down)
    step = -1;
  else if( step < 0 and up)
    step = +1;
    
  position += step;
  if( position <= 0) {
    position = 0;
    step = 0;
  }
  else if( position >= 180)
  {
    position = 180;
    step = 0;
  }

  delay (100);
  servo.write( position);
  delay (speed);
}




// -------------- adjustable sinus wave sweep   ----------
void sinus_sweep (int start, int stop) {
  float s = sin( r);               // The sine from the angle in radian
  float t = (90.0 * s) + 90.0;     // convert -1...1 to 0...180
  int angle = int( t);             // convert it to integer for the servo angle
  angle = map(angle, 0, 180, start, stop);
  servo.write( angle);
  Serial.print("Winkel: ");
  Serial.println (angle);
  delay (speed);

  int potentiometer = analogRead(poti_read);
  float r_increment = 0.001 + (0.00005 * float(potentiometer));

  r += r_increment;
  if( r >= 2.0 * M_PI)  {
      r -= 2.0 * M_PI;
    }
}



// -------------------- potentiometer with 180° servo ---------------------
void servo_potentiometer (void) {
  int min =0, max=180;
  int potentiometer = analogRead(poti_read);
  int angle = map( potentiometer, 0, 4095, 0, 180);
  // map(value, fromLow, fromHigh, toLow, toHigh); = rescale range from "from" to "to"
  // potentiometer range: 0 - 4095, range of servo = 0 - 180°
  Serial.print ("Potentiometer: ");
  Serial.println(potentiometer);
  Serial.print ("Angle: ");
  Serial.println (angle);
  servo.write( angle);
  delay (speed);
}





// ------- function to slowly sweep the arm of the servo motor  ----------
void slow_sweep (void) {
  int upper_range = 180, lower_range = 0;
  int pos;

  for (pos = lower_range; pos <= upper_range; pos += 1) {
    servo.write(pos);
    delay(speed);
  }
  for (pos = upper_range; pos >= lower_range; pos -= 1) {
    servo.write(pos);
    delay(speed);
  }
}