code:
int motor1pin1 = 2;
int motor1pin2 = 4;
int motor2pin1 = 7;
int motor2pin2 = 8;
int enA = 5;
int enB = 6;
int trigPin1 = 11;
int echoPin1 = 3;
int trigPin2 = 10;
int echoPin2 = 9;
int trigPin3 = 13;
int echoPin3 = 12;
int prawo = 0;
int przod = 0;
int lewo = 0;
int position = 0;
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
pinMode(enA, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(trigPin1, OUTPUT);
pinMode(echoPin1, INPUT);
pinMode(trigPin2, OUTPUT);
pinMode(echoPin2, INPUT);
pinMode(trigPin3, OUTPUT);
pinMode(motor1pin1, OUTPUT);
pinMode(motor1pin2, OUTPUT);
pinMode(motor2pin1, OUTPUT);
pinMode(motor2pin2, OUTPUT);
}
int zmierzOdleglosc(int trigPin, int echoPin) {
long czas, dystans;
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
czas = pulseIn(echoPin, HIGH);
dystans = czas / 58;
return dystans;
}
int positionCheck(int prawo,int lewo, int przod) {
if (przod <= 15 || przod >=200) {
if (prawo <= 15 || przod >=200) {
//case 3;
Serial.println("3");
return 3;
}
if (lewo <= 15 || lewo >=200) {
//case 2;
Serial.println("2");
return 2;
}
Serial.println("1");
//case 0
return 1;
}
Serial.println("0");
//case 0
return 0;
}
void loop() {
analogWrite(enA, 92);
analogWrite(enB, 93);
Serial.print("PRAWO 1: ");
prawo = zmierzOdleglosc(trigPin1, echoPin1);
Serial.print(prawo);
Serial.println(" cm");
Serial.print("PRZOD 2: ");
przod = zmierzOdleglosc(trigPin2, echoPin2);
Serial.print(przod);
Serial.println(" cm");
Serial.print("LEWO 3: ");
lewo = zmierzOdleglosc(trigPin3, echoPin3);
Serial.print(lewo);
Serial.println(" cm");
position = positionCheck(prawo,lewo,przod);
switch (position) {
case (0):
digitalWrite(motor1pin1, HIGH);
digitalWrite(motor1pin2, LOW);
digitalWrite(motor2pin1, HIGH);
digitalWrite(motor2pin2, LOW);
Serial.println("CASE 0");
break;
case (1):
digitalWrite(motor1pin1, LOW);
digitalWrite(motor1pin2, LOW);
digitalWrite(motor2pin1, LOW);
digitalWrite(motor2pin2, LOW);
delay(500);
digitalWrite(motor1pin1, LOW);
digitalWrite(motor1pin2, HIGH);
digitalWrite(motor2pin1, HIGH);
digitalWrite(motor2pin2, LOW);
Serial.println("CASE 1");
delay(500);
digitalWrite(motor1pin1, LOW);
digitalWrite(motor1pin2, LOW);
digitalWrite(motor2pin1, LOW);
digitalWrite(motor2pin2, LOW);
delay(2000);
break;
case (2):
digitalWrite(motor1pin1, LOW);
digitalWrite(motor1pin2, HIGH);
digitalWrite(motor2pin1, HIGH);
digitalWrite(motor2pin2, LOW);
Serial.println("CASE 2");
delay(500);
digitalWrite(motor1pin1, LOW);
digitalWrite(motor1pin2, LOW);
digitalWrite(motor2pin1, LOW);
digitalWrite(motor2pin2, LOW);
delay(2000);
break;
case (3):
digitalWrite(motor1pin1, HIGH);
digitalWrite(motor1pin2, LOW);
digitalWrite(motor2pin1, LOW);
digitalWrite(motor2pin2, HIGH);
Serial.println("CASE 3");
delay(500);
digitalWrite(motor1pin1, LOW);
digitalWrite(motor1pin2, LOW);
digitalWrite(motor2pin1, LOW);
digitalWrite(motor2pin2, LOW);
delay(2000);
break;
}
}
diagram without sensors connected because im testing the motor and motion of robot for this moment
pic of motors connected