// https://forum.arduino.cc/t/a-firefighting-robot-with-obstacle-avoider/1289843/
// https://europe1.discourse-cdn.com/arduino/original/4X/6/8/7/687c7ec5920f3fc3e8d93b01a88d0f2fa57b69e0.webp

#include <Servo.h>  //include servo.h library
Servo myservo;

int angle = 0;
int motor_speed = 63; // slowest
boolean fire = false;

#define sensor_L 9   // left sensor
#define sensor_R 10  // right sensor
#define sensor_F 8   // front sensor
#define motor_fwd_L 2   // left motor forward
#define motor_ena_L 3   // left motor enable
#define motor_rev_L 7   // left motor reverse
#define motor_fwd_R 4   // right motor forward
#define motor_ena_R 5   // right motor enable
#define motor_rev_R 12  // right motor reverse
#define PUMP 6
#define SRVO 11  // servo pin
#define HOME 90

void setup() {
  pinMode(sensor_L, INPUT);
  pinMode(sensor_R, INPUT);
  pinMode(sensor_F, INPUT);
  pinMode(motor_fwd_L, OUTPUT);
  pinMode(motor_rev_L, OUTPUT);
  pinMode(motor_fwd_R, OUTPUT);
  pinMode(motor_rev_R, OUTPUT);
  pinMode(PUMP, OUTPUT);
  analogWrite(motor_ena_L, motor_speed);
  analogWrite(motor_ena_R, motor_speed);
  myservo.attach(SRVO);
  myservo.write(HOME);
}

void loop() {
  if (digitalRead(sensor_L) == 1 && digitalRead(sensor_R) == 1 && digitalRead(sensor_F) == 1) {
    digitalWrite(motor_fwd_L, HIGH);
    digitalWrite(motor_rev_L, LOW);
    digitalWrite(motor_fwd_R, HIGH);
    digitalWrite(motor_rev_R, LOW);
  }

  else if (digitalRead(sensor_F) == 0) {
    stop();
    fire = true;
  }

  else if (digitalRead(sensor_L) == 0) {
    digitalWrite(motor_fwd_L, HIGH);
    digitalWrite(motor_rev_L, LOW);
    digitalWrite(motor_fwd_R, LOW);
    digitalWrite(motor_rev_R, HIGH);
  }

  else if (digitalRead(sensor_R) == 0) {
    digitalWrite(motor_fwd_L, HIGH);
    digitalWrite(motor_rev_L, HIGH);
    digitalWrite(motor_fwd_R, HIGH);
    digitalWrite(motor_rev_R, LOW);
  }

  while (fire) {
    stop();
    put_out_fire();
  }
}

void stop() {
  digitalWrite(motor_fwd_L, LOW);
  digitalWrite(motor_rev_L, LOW);
  digitalWrite(motor_fwd_R, LOW);
  digitalWrite(motor_rev_R, LOW);
}

void put_out_fire() {
  digitalWrite(PUMP, HIGH);
  for (int i = 0; i < 2; i++) {
    for (angle = 50; angle <= 130; angle++) {
      myservo.write(abs((i * 180) - angle));
      delay(10);
    }
  }
  digitalWrite(PUMP, LOW);
  fire = false;
}