#include <Servo.h>
#include "IR_remote.h"
#include "Keymap.h"
IRremote ir(3);
Servo water_servo;
int starting_Angle;
int Avoidance_distance;
int manual_Or_Automatic;
byte ir_Status;
unsigned long previousMillis = 0UL;
unsigned long interval = 1000UL;
int currentMillis = 0;
//Flame sensor pin is D7, Pump pin is D8, Rotating platform (Servo) is D12.
//NOTE: if manual_Or_Automatic value is 0, then it is on Automatic mode. Else its manual.
//Used with Ultrasonic Avoidance Car made by LAFVIN
// Fadel Taha {Copyright 2023}
float checkdistance() {
digitalWrite(12, LOW);
delayMicroseconds(2);
digitalWrite(12, HIGH);
delayMicroseconds(10);
digitalWrite(12, LOW);
float distance = pulseIn(13, HIGH) / 58.00;
delay(10);
return distance;
}
void Stop() {
digitalWrite(2,LOW);
analogWrite(5,0);
digitalWrite(4,HIGH);
analogWrite(6,0);
}
void Move_Backward(int speed) {
digitalWrite(2,LOW);
analogWrite(5,speed);
digitalWrite(4,HIGH);
analogWrite(6,speed);
}
void Rotate_Left(int speed) {
digitalWrite(2,LOW);
analogWrite(5,speed);
digitalWrite(4,LOW);
analogWrite(6,speed);
}
void Rotate_Right(int speed) {
digitalWrite(2,HIGH);
analogWrite(5,speed);
digitalWrite(4,HIGH);
analogWrite(6,speed);
}
void Move_Forward(int speed) {
digitalWrite(2,HIGH);
analogWrite(5,speed);
digitalWrite(4,LOW);
analogWrite(6,speed);
}
void setup() {
Serial.begin(9600);
IRremote ir(3);
pinMode(8, OUTPUT);
pinMode(7, INPUT);
pinMode(12, OUTPUT);
pinMode(13, INPUT);
pinMode(2, OUTPUT);
pinMode(5, OUTPUT);
pinMode(4, OUTPUT);
pinMode(6, OUTPUT);
//water_servo.attach(12);
water_servo.attach(11);
digitalWrite(8, LOW);
manual_Or_Automatic = 0;
ir_Status = "";
}
/*void IR_control_Function() {
ir_Status = ir.getIrKey(ir.getCode(),1);
if (ir_Status == IR_KEYCODE_UP && manual_Or_Automatic == 1){
Move_Forward(100);
Serial.println("Moving forward...");
delay(300);
Stop();
} else if (ir_Status == IR_KEYCODE_DOWN && manual_Or_Automatic == 1){
Move_Backward(100);
Serial.println("Moving backward...");
delay(300);
Stop();
} else if (ir_Status == IR_KEYCODE_LEFT && manual_Or_Automatic == 1){
Rotate_Left(70);
Serial.println("Moving right...");
delay(300);
Stop();
} else if (ir_Status == IR_KEYCODE_RIGHT && manual_Or_Automatic == 1){
Rotate_Right(70);
Serial.println("Moving left...");
delay(300);
Stop();
}
}*/
/*void IR_recieve()
{
if (ir_Status == IR_KEYCODE_1)
{
Serial.println("AUTOMATIC MODE ON");
manual_Or_Automatic = 1;
} else if (ir_Status == IR_KEYCODE_0)
{
Serial.println("MANUAL MODE ON");
manual_Or_Automatic = 0;
}
}*/
void is_Fire_Detected()
{
if (digitalRead(7) == 0)
{ //IF FIRE IS DETECTED
Stop();
Serial.println("|||FLAME DETECTED|| Car Stopped. Servo about to start.");
water_servo.write(starting_Angle);
digitalWrite(8, HIGH);
for (int i=0; i < 60; i++)
{
water_servo.write(i);
Serial.println("ServoLoop "+i);
delay(10);
}
digitalWrite(8, LOW);
}
}
void ultrasonic_Function()
{
int awareness_Distance = 25;
while (true)
{
Avoidance_distance = checkdistance();
if (Avoidance_distance <= awareness_Distance)
{
if (Avoidance_distance <= 15)
{
Stop();
delay(100);
Move_Backward(50);
delay(1000);
} else
{
Stop();
delay(100);
Rotate_Left(100);
delay(600);
}
}
else
{
Move_Forward(70);
delay(1000);
break;
}
}
}
void loop() {
digitalWrite(8, LOW);
int starting_Angle = 0;
int Avoidance_distance = 0;
Avoidance_distance = checkdistance();
water_servo.write(starting_Angle);
Serial.println("Moving forward.");
//Move_Forward(75);
//delay(1000);
ultrasonic_Function();
Stop();
delay(1000);
if (digitalRead(7) == 0)
{ //IF FIRE IS DETECTED
while (digitalRead(7) == 0)
{
water_servo.write(starting_Angle);
Serial.println("|||FLAME DETECTED|| Car Stopped. Servo about to start.");
digitalWrite(8, HIGH); //PUMP WILL START WORKING
for (int i=0; i < 60; i++)
{
water_servo.write(i);
Serial.println("ServoLoop "+i);
delay(10);
}
digitalWrite(8, LOW);
}
}
digitalWrite(8, LOW);
Rotate_Left(75);
delay(2100);
Stop();
/*else
{
digitalWrite(8, LOW);
Rotate_Left(50);
delay(1200);
Stop();
}*/
}
Comments
Please log in or sign up to comment.