#include<AFMotor.h>
AF_DCMotor motor1(1);
AF_DCMotor motor2(2);
int IR1=A3;
int IR2=A2;
int IR3=A1;
int IR4=A0;
int count=0;
boolean state = true;
void setup() {
pinMode(IR1, INPUT);
pinMode(IR2, INPUT);
pinMode(IR3, INPUT);
pinMode(IR4, INPUT);
Serial.begin(9600);
}
void loop() {
int s1 = digitalRead(IR1);
int s2 = digitalRead(IR2);
int s3 = digitalRead(IR3);
int s4 = digitalRead(IR4);
//Serial.println(s1);
shugamDagah();
tooloh();
}
void shugamDagah()
{
if(digitalRead(IR1)==0 && digitalRead(IR2)==0 && digitalRead(IR3)==0 && digitalRead(IR4)==0){uragshaa();}
else if(digitalRead(IR1)==1 && digitalRead(IR2)==1 && digitalRead(IR3)==1 && digitalRead(IR4)==1){ motor1.setSpeed(100);motor1.run(FORWARD);motor2.setSpeed(100);motor2.run(FORWARD); }
else if(digitalRead(IR1)==0 && digitalRead(IR2)==1 && digitalRead(IR3)==1 && digitalRead(IR4)==0){uragshaa();}
else if(digitalRead(IR1)==1 && digitalRead(IR2)==0 && digitalRead(IR3)==0 && digitalRead(IR4)==0){zuun();}
else if(digitalRead(IR1)==1 && digitalRead(IR2)==1 && digitalRead(IR3)==0 && digitalRead(IR4)==0){zuun();}
else if(digitalRead(IR1)==0 && digitalRead(IR2)==0 && digitalRead(IR3)==0 && digitalRead(IR4)==1){baruun();}
else if(digitalRead(IR1)==0 && digitalRead(IR2)==0 && digitalRead(IR3)==1 && digitalRead(IR4)==1){baruun();}
else if(digitalRead(IR1)==0 && digitalRead(IR2)==1 && digitalRead(IR3)==0 && digitalRead(IR4)==0){zuun();}
else if(digitalRead(IR1)==0 && digitalRead(IR2)==0 && digitalRead(IR3)==1 && digitalRead(IR4)==0){baruun();}
else zogs();
}
void tooloh()
{
if (digitalRead(IR1) && digitalRead(IR2) && digitalRead(IR3) && digitalRead(IR4) && state){
count++;
state = false;
Serial.print("Count: ");
Serial.println(count);
if(count==2) {zogs();delay(3000);baruun1();delay(350);zogs();while(1);}
}
if (!digitalRead(IR1) && !digitalRead(IR2) && !digitalRead(IR3) && !digitalRead(IR4))
{
state = true;
delay(100);
}
if (!digitalRead(IR1) && digitalRead(IR2) && digitalRead(IR3)&& !digitalRead(IR4))
{
state = true;
delay(100);
}
}
void uragshaa()
{
motor1.setSpeed(255);
motor1.run(FORWARD);
motor2.setSpeed(255);
motor2.run(FORWARD);
}
void hoishoo()
{
motor1.setSpeed(150);
motor1.run(BACKWARD);
motor2.setSpeed(150);
motor2.run(BACKWARD);
}
void baruun()
{
motor1.setSpeed(255);
motor1.run(FORWARD);
motor2.setSpeed(0);
motor2.run(FORWARD);
}
void zuun()
{
motor1.setSpeed(0);
motor1.run(FORWARD);
motor2.setSpeed(255);
motor2.run(FORWARD);
}
void baruun1()
{
motor1.setSpeed(100);
motor1.run(FORWARD);
motor2.setSpeed(100);
motor2.run(BACKWARD);
}
void zuun1()
{
motor1.setSpeed(255);
motor1.run(BACKWARD);
motor2.setSpeed(255);
motor2.run(FORWARD);
}
void zogs()
{
motor1.setSpeed(0);
motor1.run(RELEASE);
motor2.setSpeed(0);
motor2.run(RELEASE);
}
No comments:
Post a Comment