#include <ESP32Servo.h>

// SERVO
#define SPWM 2
float pos = 0.0; // Variable where the arm's position will be stored (in degrees)
float step = 0.5; // Variable used for the arm's position step
Servo servo;

//BUTTONS
#define Button1 13
#define Button2 4


void setup()
{

  Serial.begin(9600);
  pinMode(Button1, INPUT_PULLUP); // Set the A1 pin to a pushbutton in pullup mode
  pinMode(Button2, INPUT_PULLUP); // Set the A1 pin to a pushbutton in pullup mode

  servo.attach(SPWM);
  servo.write(pos); // Initialize the arm's position to 0 (leftmost)

}

void loop()
{

  if (!digitalRead(Button1)) // Check for the yellow button input
  {
    if (pos>0) // Check that the position won't go lower than 0°
    {
      servo.write(pos); // Set the arm's position to "pos" value
      pos-=step; // Decrement "pos" of "step" value
      delay(5); // Wait 5ms for the arm to reach the position
    }
  }

  if (!digitalRead(Button2)) // Check for the blue button input
  {
    if (pos<180) // Check that the position won't go higher than 180°
    {
      servo.write(pos); // Set the arm's position to "pos" value
      pos+=step; // Increment "pos" of "step" value
      delay(5); // Wait 5ms for the arm to reach the position
    }

		Serial.print("Position: ");
		Serial.println(pos);

  }

}