#include <Servo.h>

unsigned long dur_t, start_t;
Servo arm; // Create a "Servo" object called "arm"
float pos = 0.0; // Variable where the arm's position will be stored (in degrees)
float step = 30.0; // Variable used for the arm's position step

void setup()
{
  Serial.begin(9600);
  pinMode(5, INPUT_PULLUP); // Set the D5 pin to a pushbutton in pullup mode
  pinMode(4, INPUT_PULLUP); // Set the D4 pin to a pushbutton in pullup mode

  arm.attach(3); // Attache the arm to the pin 3
  arm.write(pos); // Initialize the arm's position to 0 (leftmost)
}

void loop()
{

  if (digitalRead(5) == LOW) // Check for the yellow button input
  {
    dur_t = counting(5);
    Serial.println("dur_t: ");
    Serial.println(dur_t);
    if (pos>=0 && dur_t > 500) // Check that the position won't go lower than 0°
    {
      Serial.println("pos");
      Serial.println(pos);
      pos = pos - step; // Decrement "pos" of "step" value
      arm.write(pos); // Set the arm's position to "pos" value
      delay(50); // Wait 50ms for the arm to reach the position
    }
  }

  if (digitalRead(4 == LOW)) // Check for the blue button input
  {
    dur_t = counting(4);
    Serial.println("dur_t");
    Serial.println(dur_t);
    if (pos<=180 && dur_t < 500) // Check that the position won't go higher than 180°
    {
      Serial.println("pos");
      Serial.println(pos);
      pos = pos + step; // Increment "pos" of "step" value
      arm.write(pos); // Set the arm's position to "pos" value
      delay(50); // Wait 50ms for the arm to reach the position
    }
  }

}

unsigned long counting(int pin){
  start_t = millis();
  while(digitalRead(pin) == LOW){
    //do nothing
  }
  return millis() - start_t;
}