You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
492 lines
11 KiB
Arduino
492 lines
11 KiB
Arduino
6 years ago
|
#include <IRremote.h>
|
||
|
|
int RECV_PIN1 = 13;
|
||
|
|
IRrecv irrecvmain(RECV_PIN1);
|
||
|
|
int RECV_PIN2 = 2;
|
||
|
|
int RECV_PIN3 = 26;
|
||
|
|
int RECV_PIN4 = 28;
|
||
|
|
int RECV_PIN5 = 30;
|
||
|
|
IRrecv irrecv2(RECV_PIN2);
|
||
|
|
IRrecv irrecv3(RECV_PIN3);
|
||
|
|
decode_results resultsmain;
|
||
|
|
decode_results results2;
|
||
|
|
decode_results results3;
|
||
|
|
IRrecv irrecv4(RECV_PIN4);
|
||
|
|
IRrecv irrecv5(RECV_PIN5);
|
||
|
|
decode_results results4;
|
||
|
|
decode_results results5;
|
||
|
|
int checkloop = 0;
|
||
|
|
int intright = 0;
|
||
|
|
int intleft = 0;
|
||
|
|
int intfor = 0;
|
||
|
|
int intback = 0;
|
||
|
|
int check1 = 0;
|
||
|
|
//driving
|
||
|
|
int slow = 0;
|
||
|
|
int LF = 6;
|
||
|
|
int LB = 9;
|
||
|
|
int RB = 11;
|
||
|
|
int RF = 10;
|
||
|
|
int echo1 = 5;
|
||
|
|
int trig1 = 12;
|
||
|
|
int echo2 = 3;
|
||
|
|
int trig2 = 7;
|
||
|
|
int echo3 = 22;
|
||
|
|
int trig3 = 23;
|
||
|
|
int echo4 = 24;
|
||
|
|
int trig4 = 25;
|
||
|
|
int echo5 = 41;
|
||
|
|
int trig5 = 40;
|
||
|
|
long duration1;
|
||
|
|
long duration2;
|
||
|
|
long duration3;
|
||
|
|
long duration4;
|
||
|
|
long duration5;
|
||
|
|
int distanceF1;
|
||
|
|
int distanceF2;
|
||
|
|
int distanceR1;
|
||
|
|
int distanceR2;
|
||
|
|
int distanceup;
|
||
|
|
int F1;
|
||
|
|
int F2;
|
||
|
|
int b1 = 8;
|
||
|
|
int b2 = 4;
|
||
|
|
bool forward = false;
|
||
|
|
bool backward = false;
|
||
|
|
bool left = false;
|
||
|
|
bool right = false;
|
||
|
|
bool stop = true;
|
||
|
|
bool sens2;
|
||
|
|
bool sens3;
|
||
|
|
bool sens4;
|
||
|
|
bool sens5;
|
||
|
|
bool stopping = false;
|
||
|
|
unsigned long s2millis;
|
||
|
|
unsigned long s3millis;
|
||
|
|
unsigned long s4millis;
|
||
|
|
unsigned long s5millis;
|
||
|
|
unsigned long current_time;
|
||
|
|
int s2l=36;
|
||
|
|
int s3l=37;
|
||
|
|
int s4l=38;
|
||
|
|
int s5l=39;
|
||
|
|
|
||
|
|
void setup()
|
||
|
|
{
|
||
|
|
Serial.begin(9600);
|
||
|
|
irrecvmain.enableIRIn(); // Start the reciever
|
||
|
|
irrecv2.enableIRIn();
|
||
|
|
irrecv3.enableIRIn();
|
||
|
|
irrecv4.enableIRIn();
|
||
|
|
irrecv5.enableIRIn();
|
||
|
|
pinMode(RB, OUTPUT);
|
||
|
|
pinMode(RF, OUTPUT);
|
||
|
|
pinMode(LB, OUTPUT);
|
||
|
|
pinMode(LF, OUTPUT);
|
||
|
|
pinMode(trig1, OUTPUT);
|
||
|
|
pinMode(echo1, INPUT);
|
||
|
|
pinMode(trig2, OUTPUT);
|
||
|
|
pinMode(echo2, INPUT);
|
||
|
|
pinMode(trig3, OUTPUT);
|
||
|
|
pinMode(echo3, INPUT);
|
||
|
|
pinMode(trig4, OUTPUT);
|
||
|
|
pinMode(echo4, INPUT);
|
||
|
|
pinMode(trig5, OUTPUT);
|
||
|
|
pinMode(echo5, INPUT);
|
||
|
|
pinMode(b1, INPUT);
|
||
|
|
pinMode(b2, INPUT);
|
||
|
|
pinMode(s2l, OUTPUT);
|
||
|
|
pinMode(s3l, OUTPUT);
|
||
|
|
pinMode(s4l, OUTPUT);
|
||
|
|
pinMode(s5l, OUTPUT);
|
||
|
|
}
|
||
|
|
void loop() {
|
||
|
|
//Serial.println(distanceup);
|
||
|
|
delay(5);
|
||
|
|
if (stop == true) {
|
||
|
|
left = false;
|
||
|
|
right = false;
|
||
|
|
forward = false;
|
||
|
|
backward = false;
|
||
|
|
}
|
||
|
|
mainloop();
|
||
|
|
check();
|
||
|
|
if (irrecvmain.decode(&resultsmain)) {
|
||
|
|
//Serial.println("Control Sensor ");
|
||
|
|
Serial.println(resultsmain.value);
|
||
|
|
irrecvmain.resume(); // Receive the next value
|
||
|
|
if (resultsmain.value == 4294967295) {
|
||
|
|
stop = true;
|
||
|
|
forward = false;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
if (resultsmain.value == 3937909453) {
|
||
|
|
stop = false;
|
||
|
|
forward = true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if (irrecv2.decode(&results2)) {
|
||
|
|
//Serial.println("Sensor 2 ");
|
||
|
|
//Serial.println(results2.value);
|
||
|
|
irrecv2.resume(); // Receive the next value
|
||
|
|
s2millis = millis();
|
||
|
|
sens2=true;
|
||
|
|
//Serial.println("2");
|
||
|
|
digitalWrite(s2l, HIGH);
|
||
|
|
stop = false;
|
||
|
|
stopping = false;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (irrecv3.decode(&results3)) {
|
||
|
|
//Serial.println("Sensor 3 ");
|
||
|
|
//Serial.println(results3.value);
|
||
|
|
irrecv3.resume(); // Receive the next value
|
||
|
|
s3millis = millis();
|
||
|
|
sens3=true;
|
||
|
|
//Serial.println("3");
|
||
|
|
digitalWrite(s3l, HIGH);
|
||
|
|
stop = false;
|
||
|
|
stopping = false;
|
||
|
|
}
|
||
|
|
if (irrecv4.decode(&results4)) {
|
||
|
|
//Serial.println("Sensor 4 ");
|
||
|
|
//Serial.println(results4.value);
|
||
|
|
irrecv4.resume(); // Receive the next value
|
||
|
|
s4millis = millis();
|
||
|
|
sens4=true;
|
||
|
|
//Serial.println("4");
|
||
|
|
digitalWrite(s4l, HIGH);
|
||
|
|
stop = false;
|
||
|
|
stopping = false;
|
||
|
|
}
|
||
|
|
if (irrecv5.decode(&results5)) {
|
||
|
|
//Serial.println("Sensor 5 ");
|
||
|
|
//Serial.println(results5.value);
|
||
|
|
irrecv5.resume(); // Receive the next value
|
||
|
|
s5millis = millis();
|
||
|
|
sens5=true;
|
||
|
|
//Serial.println("5");
|
||
|
|
digitalWrite(s5l, HIGH);
|
||
|
|
stop = false;
|
||
|
|
stopping = false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
void ir1() {
|
||
|
|
|
||
|
|
|
||
|
|
if (resultsmain.value == 3772778233) {
|
||
|
|
//forward = true;
|
||
|
|
left = false;
|
||
|
|
right = false;
|
||
|
|
backward = false;
|
||
|
|
intfor=intfor+1;
|
||
|
|
resultsmain.value = 0;
|
||
|
|
}
|
||
|
|
if (resultsmain.value == 3772810873) {
|
||
|
|
//backward = true;
|
||
|
|
left = false;
|
||
|
|
right = false;
|
||
|
|
forward = false;
|
||
|
|
intback=intback+1;
|
||
|
|
resultsmain.value = 0;
|
||
|
|
}
|
||
|
|
if (resultsmain.value == 3772819033) {
|
||
|
|
backward = false;
|
||
|
|
//left = true;
|
||
|
|
right = false;
|
||
|
|
forward = false;
|
||
|
|
intleft=intleft+1;
|
||
|
|
resultsmain.value = 0;
|
||
|
|
}
|
||
|
|
if (resultsmain.value == 3772794553) {
|
||
|
|
backward = false;
|
||
|
|
left = false;
|
||
|
|
//right = true;
|
||
|
|
forward = false;
|
||
|
|
intright=intright+1;
|
||
|
|
resultsmain.value = 0;
|
||
|
|
}
|
||
|
|
if (resultsmain.value == 3772793023) {
|
||
|
|
stop = true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
void mainloop()
|
||
|
|
{
|
||
|
|
//Serial.println(digitalRead(b1));
|
||
|
|
//Serial.println(digitalRead(b2));
|
||
|
|
if (digitalRead(b1) == 1) {
|
||
|
|
if (stop == true) {
|
||
|
|
forward = true;
|
||
|
|
stop=false;
|
||
|
|
stopping=false;
|
||
|
|
delay(250);
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
stop=true;
|
||
|
|
delay(250);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if (digitalRead(b2) == 1) {
|
||
|
|
if (backward == false) {
|
||
|
|
backward = true;
|
||
|
|
forward = false;
|
||
|
|
left = false;
|
||
|
|
right = false;
|
||
|
|
delay(250);
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
backward = false;
|
||
|
|
delay(250);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
if (stop==false) {
|
||
|
|
digitalWrite(trig1, LOW);
|
||
|
|
delayMicroseconds(2);
|
||
|
|
digitalWrite(trig1, HIGH);
|
||
|
|
delayMicroseconds(10);
|
||
|
|
digitalWrite(trig1, LOW);
|
||
|
|
duration1 = pulseIn(echo1, HIGH);
|
||
|
|
distanceF1 = duration1*0.034/2;
|
||
|
|
|
||
|
|
digitalWrite(trig2, LOW);
|
||
|
|
delayMicroseconds(2);
|
||
|
|
digitalWrite(trig2, HIGH);
|
||
|
|
delayMicroseconds(10);
|
||
|
|
digitalWrite(trig2, LOW);
|
||
|
|
duration2 = pulseIn(echo2, HIGH);
|
||
|
|
distanceF2 = duration2*0.034/2;
|
||
|
|
F1=distanceF1+2;
|
||
|
|
F2=distanceF2+2;
|
||
|
|
//Serial.print("Distance F1: ");
|
||
|
|
Serial.print(distanceF1);
|
||
|
|
Serial.print(" ");
|
||
|
|
Serial.println(distanceF2);
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
//else {
|
||
|
|
// distanceF1 = 100;
|
||
|
|
//distanceF2 = 100;
|
||
|
|
//}
|
||
|
|
if (backward == true) {
|
||
|
|
digitalWrite(trig3, LOW);
|
||
|
|
delayMicroseconds(2);
|
||
|
|
digitalWrite(trig3, HIGH);
|
||
|
|
delayMicroseconds(10);
|
||
|
|
digitalWrite(trig3, LOW);
|
||
|
|
duration3 = pulseIn(echo3, HIGH);
|
||
|
|
distanceR1 = duration3*0.034/2;
|
||
|
|
|
||
|
|
digitalWrite(trig4, LOW);
|
||
|
|
delayMicroseconds(2);
|
||
|
|
digitalWrite(trig4, HIGH);
|
||
|
|
delayMicroseconds(10);
|
||
|
|
digitalWrite(trig4, LOW);
|
||
|
|
duration4 = pulseIn(echo4, HIGH);
|
||
|
|
distanceR2 = duration4*0.034/2;
|
||
|
|
//Serial.print("Distance R1: ");
|
||
|
|
//Serial.println(distanceR1);
|
||
|
|
//Serial.print("Distance R2: ");
|
||
|
|
//Serial.println(distanceR2);
|
||
|
|
//Serial.println("");
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
distanceR1 = 100;
|
||
|
|
distanceR2 = 100;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
void check()
|
||
|
|
{
|
||
|
|
//GO ---------------------------------------------------------------------------
|
||
|
|
if (stop == false) {
|
||
|
|
forward=true;
|
||
|
|
if (distanceF1<20) {
|
||
|
|
stopping=true;
|
||
|
|
forward = false;
|
||
|
|
Serial.print("f1<40");
|
||
|
|
if (distanceF1>distanceF2) {
|
||
|
|
Serial.print("f1?");
|
||
|
|
if (distanceF1>F2) {
|
||
|
|
Serial.print("f1");
|
||
|
|
right=true;
|
||
|
|
left=false;
|
||
|
|
forward=false;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
stop=true;
|
||
|
|
Serial.print("stop1");
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
right=false;
|
||
|
|
Serial.print("no2");
|
||
|
|
if (distanceF2>distanceF1) {
|
||
|
|
Serial.print("f2?");
|
||
|
|
if (distanceF2>F1) {
|
||
|
|
left=true;
|
||
|
|
right=false;
|
||
|
|
Serial.print("f2");
|
||
|
|
forward=false;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
stop=true;
|
||
|
|
Serial.print("stop3");
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
left=false;
|
||
|
|
Serial.println("no4");
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
if (distanceF2<20) {
|
||
|
|
forward = false;
|
||
|
|
stopping=true;
|
||
|
|
Serial.print("f2<40");
|
||
|
|
if (distanceF1>distanceF2) {
|
||
|
|
Serial.print("f1?");
|
||
|
|
if (distanceF1>F2) {
|
||
|
|
right=true;
|
||
|
|
left=false;
|
||
|
|
Serial.print("f1");
|
||
|
|
forward=false;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
stop=true;
|
||
|
|
Serial.print("stop4");
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
|
||
|
|
right=false;
|
||
|
|
Serial.print("no5");
|
||
|
|
if (distanceF2>distanceF1) {
|
||
|
|
Serial.print("f2?");
|
||
|
|
if (distanceF2>F1) {
|
||
|
|
left=true;
|
||
|
|
right=false;
|
||
|
|
Serial.print("f2");
|
||
|
|
forward=false;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
stop=true;
|
||
|
|
Serial.print("stop6");
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
left=false;
|
||
|
|
Serial.print("no7");
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
if (stopping = false) {
|
||
|
|
Serial.print("no8");
|
||
|
|
forward = true;
|
||
|
|
left=false;
|
||
|
|
right=false;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
Serial.print("stopping");
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if (distanceF1<10) {
|
||
|
|
right=false;
|
||
|
|
left=false;
|
||
|
|
forward=false;
|
||
|
|
backward=true;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
if (distanceF2<10) {
|
||
|
|
right=false;
|
||
|
|
left=false;
|
||
|
|
forward=false;
|
||
|
|
backward=true;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
backward=false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if (distanceup<15) {
|
||
|
|
stop = true;
|
||
|
|
}
|
||
|
|
if (stop == true) {
|
||
|
|
left = false;
|
||
|
|
right = false;
|
||
|
|
forward = false;
|
||
|
|
backward = false;
|
||
|
|
Serial.print("stop");
|
||
|
|
stopping = false;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
Serial.print("go");
|
||
|
|
}
|
||
|
|
if (distanceF1 > 25 && distanceF2 > 25 && forward == true) {
|
||
|
|
analogWrite(RF, 255);
|
||
|
|
analogWrite(LF, 255);
|
||
|
|
analogWrite(RB, 0);
|
||
|
|
analogWrite(LB, 0);
|
||
|
|
Serial.print("forward");
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
if (distanceR1 > 35 && distanceR2 > 35 && backward == true) {
|
||
|
|
analogWrite(RF, 0);
|
||
|
|
analogWrite(LF, 0);
|
||
|
|
delay(200);
|
||
|
|
analogWrite(RB, 255);
|
||
|
|
analogWrite(LB, 255);
|
||
|
|
analogWrite(RF, 0);
|
||
|
|
analogWrite(LF, 0);
|
||
|
|
Serial.print("back");
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
if (left == true) {
|
||
|
|
analogWrite(RF, 255);
|
||
|
|
analogWrite(LB, 255);
|
||
|
|
analogWrite(RB, 0);
|
||
|
|
analogWrite(LF, 0);
|
||
|
|
Serial.print("left");
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
if (right == true) {
|
||
|
|
analogWrite(RB, 255);
|
||
|
|
analogWrite(LF, 255);
|
||
|
|
analogWrite(RF, 0);
|
||
|
|
analogWrite(LB, 0);
|
||
|
|
Serial.print("right");
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
analogWrite(RF, 0);
|
||
|
|
analogWrite(LF, 0);
|
||
|
|
analogWrite(RB, 0);
|
||
|
|
analogWrite(LB, 0);
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
delay(100);
|
||
|
|
if (millis()-s2millis>=750) {
|
||
|
|
sens2=false;
|
||
|
|
digitalWrite(s2l, LOW);
|
||
|
|
}
|
||
|
|
if (millis()-s3millis>=750) {
|
||
|
|
sens3=false;
|
||
|
|
digitalWrite(s3l, LOW);
|
||
|
|
}
|
||
|
|
if (millis()-s4millis>=750) {
|
||
|
|
sens4=false;
|
||
|
|
digitalWrite(s4l, LOW);
|
||
|
|
}
|
||
|
|
if (millis()-s5millis>=750) {
|
||
|
|
sens5=false;
|
||
|
|
digitalWrite(s5l, LOW);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|