#include <Servo.h>

const int trigPin = 9;
const int echoPin = 10;
const int startButtonPin = 2;

Servo servo;
int startButtonState = 0;
int lastStartButtonState = 0;
int angle = 0;
int duration, distance;

void setup() {
 Serial.begin(9600);
 pinMode(trigPin, OUTPUT);
 pinMode(echoPin, INPUT);
 pinMode(startButtonPin, INPUT);

 servo.attach(11); // assuming A4988 driver is connected to pin 11
 servo.write(0);

 // Ensure cleaning cycle is not triggered accidentally during setup
 digitalWrite(trigPin, LOW);
 delay(1000);
 digitalWrite(trigPin, HIGH);
 delay(10);
 digitalWrite(trigPin, LOW);
}

void loop() {
 startButtonState = digitalRead(startButtonPin);

 if (startButtonState != lastStartButtonState) {
    if (startButtonState == HIGH) {
      while (angle < 180) {
        servo.write(angle);
        delay(15);
        getDistance();
        Serial.print("Distance: ");
        Serial.print(distance);
        Serial.print(" Angle: ");
        Serial.println(angle);

        if (distance < 30) {
          delay(1000); // Add delay to ensure duster doesn't damage the lightboard
        }

        angle++;
      }

      angle = 0;
      servo.write(angle);
    }
 }

 lastStartButtonState = startButtonState;
}

void getDistance() {
 digitalWrite(trigPin, LOW);
 delayMicroseconds(2);
 digitalWrite(trigPin, HIGH);
 delayMicroseconds(10);
 digitalWrite(trigPin, LOW);

 duration = pulseIn(echoPin, HIGH);
 distance = duration * 0.0344 / 2;
}