// Ultrasonic + Servo Obstacle Avoidance (STM32 Wokwi Arduino Mode)

#define TRIG_PIN PA9
#define ECHO_PIN PA10
#define SERVO_PIN PA8

long duration;
int distance;

// Simple software PWM for servo
void setServoAngle(int angle) {
  int pulseWidth = map(angle, 0, 180, 500, 2500); // microseconds

  digitalWrite(SERVO_PIN, HIGH);
  delayMicroseconds(pulseWidth);
  digitalWrite(SERVO_PIN, LOW);

  delay(20); // 20ms cycle
}

// Distance function
int getDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);

  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  duration = pulseIn(ECHO_PIN, HIGH);
  distance = duration * 0.034 / 2;

  return distance;
}

void setup() {
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(SERVO_PIN, OUTPUT);

  Serial.begin(9600);
}

void loop() {

  int front = getDistance();
  Serial.println(front);

  if (front > 20) {
    // Move straight
    setServoAngle(90);
  } 
  else {
    // Stop and scan

    // LEFT
    setServoAngle(150);
    delay(500);
    int left = getDistance();

    // RIGHT
    setServoAngle(30);
    delay(500);
    int right = getDistance();

    if (left > right) {
      setServoAngle(150);
    } else {
      setServoAngle(30);
    }

    delay(500);
  }
}