//********************
// Fire Fighting Robot With Navigation Wall Following System
// Yohana Kusuma Kristiani
// 2137200765
// Program Studi Sistem Komputer
// STIMK-AUB Surakarta
//********************
//ultrasonik kiri
int echo1 = 9;
int trig1 = 8;
int distance1 = 0;
//ultrasonik depan
int echo2 = 11;
int trig2 = 10;
int distance2 = 0;
//ultrasonik kanan
int echo3 = 13;
int trig3 = 12;
int distance3 = 0;
//api dan relay
int api = B1;
int kipas = A1;
int buzzer = A3;
int nilaisensor = 0;
//roda
int IN1 = 4;
int IN2 = 5;
int IN3 = 6;
int IN4 = 7;
int state;
int flag = 0;
int stateStop;
void setup() {
// put your setup code here, to run once:
//roda
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
//api dan relay
pinMode(api, INPUT);
pinMode(buzzer, OUTPUT);
//pinMode(buzzer, LOW);
pinMode(kipas, OUTPUT);
//pinMode(kipas, LOW);
//ultrasonik
pinMode(trig1, OUTPUT);
pinMode(echo1, INPUT);
pinMode(trig2, OUTPUT);
pinMode(echo2, INPUT);
pinMode(trig3, OUTPUT);
pinMode(echo3, INPUT);
}
void mundur()
{
//ROBOT MUNDUR
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH );
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
}
void maju()
{
//ROBOT MAJU
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
}
void kiri()
{
//ROBOT KIRI
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH );
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
}
void kanan()
{
//ROBOT KANAN
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW );
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
}
void mati()
{
//ROBOT MATI
digitalWrite(IN1, HIGH);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, HIGH);
}
int getDistance (int trig, int echo){
digitalWrite(trig, HIGH);
delayMicroseconds(10);
digitalWrite(trig, LOW);
unsigned long pulseTime = pulseIn(echo, HIGH);
int distance = pulseTime/58;
return distance;
}
void printDistance(int id, int dist) {
Serial.print(id);
Serial.print("------>");
Serial.print(dist, DEC);
Serial.println(" cm");
}
void loop() {
// put your main code here, to run repeatedly:
nilaisensor = digitalRead(api);
distance2 = getDistance(trig2, echo2);
printDistance(2, distance2);
//distance1 = getDistance(trig1, echo1);
//printDistance(1, distance1);
//distance3 = getDistance(trig3, echo3);
//printDistance(3, distance1);
if (distance2 > 10)
{maju();}
//jika menemukan api
else if (nilaisensor == HIGH){
//state = nilaisensor.read(distance1 < 10);
//flag = 0;
digitalWrite(kipas,LOW);
digitalWrite(buzzer, HIGH);}
else if (distance2 < 10 )
{ mati();}
else (nilaisensor == LOW);
digitalWrite(kipas,HIGH);
digitalWrite(buzzer, LOW);
mundur();
}
Comments