From 4c2ff902af073b996eb2f9ae2cf05342a57f8446 Mon Sep 17 00:00:00 2001 From: shark Date: Sun, 11 Nov 2018 09:35:17 -0600 Subject: [PATCH] add Makerfaire 2017 robot code --- robot-mf2017.ino | 491 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 491 insertions(+) create mode 100644 robot-mf2017.ino diff --git a/robot-mf2017.ino b/robot-mf2017.ino new file mode 100644 index 0000000..89c28a6 --- /dev/null +++ b/robot-mf2017.ino @@ -0,0 +1,491 @@ +#include +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); + } +} +