#include <Servo.h>

#define PIN_PushB1 PA7
#define PIN_PushB2 PA6
#define PIN_SERVO PA9

#define BTN_PRESS_THR 200

#define SERVO_INTERVAL 20
#define SERVO_STEP 2

#define PIN_LED1 PB5
#define PIN_LED2 PB6
#define PIN_LED3 PB7
#define PIN_LED4 PB8
#define PIN_LED5 PB9

Servo myServo;

int servoAngle = 90;
uint32_t lastServoMove = 0;

bool lastPB1 = false;
bool lastPB2 = false;


void setup(){
Serial.begin(115200);
myServo.attach(PIN_SERVO);
myServo.write(servoAngle);
Serial.println("=== READY ===");
}

void Prog_Servo(){
int rawPB1 = analogRead(PIN_PushB1);
int rawPB2 = analogRead(PIN_PushB2);

bool pb1 = (rawPB1 < BTN_PRESS_THR);
bool pb2 = (rawPB2 < BTN_PRESS_THR);

if (millis() - lastServoMove >= SERVO_INTERVAL){
  if(pb1 && !pb2){
    servoAngle -= SERVO_STEP;
  }
  else if (!pb1 && pb2){
    servoAngle += SERVO_STEP;
  }
  servoAngle = constrain(servoAngle, 0, 180);
  myServo.write(servoAngle);
  lastServoMove = millis();
}

}


void loop(){



}