//I2C leitung 
#include <Wire.h>                         
//Wir für das PWM Board PCA9685 benötigt
#include <Adafruit_PWMServoDriver.h>

#define PIN_POTI_1 25 // ESP32 pin GPIO36 (ADC0) onnected to potentiometer
#define BUTTON_UP 26
#define BUTTON_DOWN 27


#define servoMIN 100
#define servoMAX 600


//Servo Frequenz
#define SERVO_FREQ 50   // Analog servos run at ~50 Hz updates  

//Objekt des Servotreibers
Adafruit_PWMServoDriver myServos = Adafruit_PWMServoDriver(); 

int servonum = 33;


////////////////Wokwi/////////////////////
#include <ESP32Servo.h>
const int servoPin1 = 33;
const int servoPin2 = 32;
Servo servo1;
Servo servo2;

int pos =0;
////////////////Wokwi/////////////////////


void setup() {
    Serial.begin(115200);                     //zum anzeigen von Werten
    Wire.begin();                             //Init von I2C schnittstelle

pinMode(BUTTON_UP,INPUT);
pinMode(BUTTON_DOWN, INPUT);
////////////////Wokwi/////////////////////
  servo1.attach(servoPin1, 500, 2400);
  servo2.attach(servoPin2, 500, 2400);
////////////////Wokwi/////////////////////

    Serial.println("Servomotorsteuerung gestartet");
}

void loop() 
{   

//////Stepper1//////////
  double Poti1 = analogRead(PIN_POTI_1);        //Liest den Poti Wert ein vom Schieber
  Stepper1(Poti1);

/////Stepper2//////////
 int ButtonU = digitalRead(BUTTON_UP);
 int ButtonD = digitalRead(BUTTON_DOWN);
  int step = 10;
  if(ButtonU == 1)
    {
      if(pos<4095)
      {
      pos = pos + step;
      }
    }
    if(ButtonD == 1)
    { 
      if(pos>0)
      {
      pos = pos - step;
      }
    }
  Stepper2(pos);

}

 ////////////////////Stepper 1/////////////////////
int Stepper1(double Poti)
  { 
  int angle = map(Poti, 0, 4095, 0, 180);         // Potentiometerwert auf Servowinkel abbilden
  int pulseWidth = map(angle, 0, 180, servoMIN, servoMAX);  //Winkel in pulsweite zurückrechnen mit den Grenzen des Servomotors (MAX/MIN)
  //myServos.setPWM(servonum, 0, pulseWidth);                 //PWM wird dem Servo geschickt
  servo1.write(angle);

  Serial.println("UnterArm: ");
  Serial.println(angle);
  Serial.println();
  return 0;
  }

////////////////////Stepper 2/////////////////////
int Stepper2(int pos)
  { 

  int angle = map(pos, 0, 4095, 0, 180);         // Potentiometerwert auf Servowinkel abbilden
  int pulseWidth = map(angle, 0, 180, servoMIN, servoMAX);  //Winkel in pulsweite zurückrechnen mit den Grenzen des Servomotors (MAX/MIN)
  //myServos.setPWM(servonum, 0, pulseWidth);                 //PWM wird dem Servo geschickt
  servo2.write(angle);

  Serial.println("Servo Greifer: ");
  Serial.println(angle);
  Serial.println();
  return 0;
  }