int analogleft = A0; int analogright = A1; int mode_1 = 2; int mode_2 = 3; int mode_3 = 4; int mode_4 = 5; int digleft = 6; int digright = 7; int trig1 = 8; int trig2 = 9; int trig3 = 12; int mot1 = 10; int mot2 = 11; int error = 13; void setup() { // put your setup code here, to run once: pinMode (mode_1, INPUT); pinMode (mode_2, INPUT); pinMode (mode_3, INPUT); pinMode (mode_4, INPUT); pinMode (digleft, INPUT); pinMode (digright, INPUT); pinMode (trig1, OUTPUT); pinMode (trig2, OUTPUT); pinMode (trig3, OUTPUT); pinMode (mot1, OUTPUT); pinMode (mot2, OUTPUT); pinMode (error, OUTPUT); } void sensor_trig(char a) //chooses sensor in automatic mode { if (a == 1) { digitalWrite(trig1, 0); digitalWrite(trig2, 1); digitalWrite(trig3, 1); } if (a == 2) { digitalWrite(trig1, 1); digitalWrite(trig2, 0); digitalWrite(trig3, 1); } if (a == 3) { digitalWrite(trig1, 1); digitalWrite(trig2, 1); digitalWrite(trig3, 0); } } void shutdown() { digitalWrite(mot1, 0); digitalWrite(mot2, 0); digitalWrite(trig1, 0); digitalWrite(trig2, 0); digitalWrite(trig3, 0); rep: digitalWrite(error, 1); delay(250); digitalWrite(error, 0); delay(250); goto rep; } void manual_run() // executes program in manual mode { char i = digitalRead(digleft); char j = digitalRead(digright); int a = analogRead(A0); int b = analogRead(A1); int c = abs(a - b); if (i == 1) { if (j == 1) { shutdown(); } if (c <= 50) { digitalWrite(mot1, 1); digitalWrite(mot2, 1); delay (300); digitalWrite(mot1, 0); digitalWrite(mot2, 0); } else { digitalWrite(mot2, 1); delay (200); digitalWrite(mot1, 1); delay (175); digitalWrite(mot1, 0); digitalWrite(mot2, 0); } } if (j == 1) { if (i == 1) { shutdown(); } if (c <= 50) { digitalWrite(mot1, 1); digitalWrite(mot2, 1); delay (300); digitalWrite(mot1, 0); digitalWrite(mot2, 0); } else { digitalWrite(mot2, 1); delay (200); digitalWrite(mot1, 1); delay (175); digitalWrite(mot1, 0); digitalWrite(mot2, 0); } } } void mode_select() { repeat: char i = digitalRead(mode_1); char j = digitalRead(mode_2); if (i == 0) { if (j == 0) { manual_run(); } if (j == 1); { digitalWrite (error, HIGH); goto repeat; } } if (i == 1) { if (j == 1) { shutdown(); } if (j == 0) { auto_run(); } } } void auto_run() //executes program in automatic mode { byte i = digitalRead (digleft); byte j = digitalRead (digright); int a = analogRead (A0); int b = analogRead (A1); int c = abs (a - b); sensor_select(); if (i == 1) { if (c <= 50) { digitalWrite(mot1, 1); digitalWrite(mot2, 1); delay (300); digitalWrite(mot1, 0); digitalWrite(mot2, 0); } else { digitalWrite(mot2, 1); delay (200); digitalWrite(mot1, 1); delay (175); digitalWrite(mot1, 0); digitalWrite(mot2, 0); } } if (j == 1) { if (i == 1) { shutdown(); } if (c <= 50) { digitalWrite(mot1, 1); digitalWrite(mot2, 1); delay (300); digitalWrite(mot1, 0); digitalWrite(mot2, 0); } else { digitalWrite(mot2, 1); delay (200); digitalWrite(mot1, 1); delay (175); digitalWrite(mot1, 0); digitalWrite(mot2, 0); } } } void sensor_select () { byte a = digitalRead (mode_3); byte b = digitalRead (mode_4); char c; if (a == 1) { if (b == 1) { shutdown(); } if (b == 0) { c = 1; } } if (a == 0) { if (b == 1) { c = 3; } if (b == 0) { c = 2; } } sensor_trig (a); } void loop() { // put your main code here, to run repeatedly: mode_select(); }