I have just started on this project. So I used an Arduino Uno board maybe R3 and it is attached to an L298P motor shield that drives two motors and I got an SG90 servo and Ultrasonic sensor (HC SR04).
My problem is when I run the code, only one motor turns on. The servo and the sensors are working as well as I expected. I've checked the power sources and connection and they all working well so far. And I suppose the problem is in the code.
Here's the code:
#include<Servo.h>
#include<NewPing.h>
Servo servo;
#define TRIG_PIN A0
#define ECHO_PIN A1
#define MAX_DISTANCE 400
const int sped = 80;
const int dela = 30;
const int dL = 200;
int E1 = 10;
int M1 = 12;
int E2 = 11;
int M2 = 13;
int value;
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);
int distance = 100;
void setup() {
pinMode(M1, OUTPUT);
pinMode(M2, OUTPUT);
servo.attach(3);
distance = readPing();
delay(100);
servo.write(90);
delay(2000);}
void loop() {
// put your main code here, to run repeatedly:
int distanceR = 0;
int distanceL = 0;
delay(40);
if(distance<=15)
{
slow();
distanceR = lookRight();
delay(200);
distanceL = lookLeft();
delay(200);
if(distanceR>=distanceL)
{
turnright();
slow();
}else
{
turnleft();
slow();
}
}else if(distance>15)
{
straight();
}
distance = readPing();
}
void straight(){
for (value = 0 ; value <= sped; value += 5)
{
digitalWrite(M1, HIGH);
digitalWrite(M2, HIGH);
analogWrite(E1, value);
analogWrite(E2, value);
delay(dela);
} delay(dL);
for (value = sped; value >= 0; value -= 5)
{
digitalWrite(M1, HIGH);
digitalWrite(M2, HIGH);
analogWrite(E1, value);
analogWrite(E2, value);
delay(dela);
} delay(dL);
}
void turnleft(){
for (value = 0 ; value <= sped; value += 5)
{
digitalWrite(M1, HIGH);
digitalWrite(M2, HIGH);
analogWrite(E1, value);
analogWrite(E2,0);
delay(dela);
} delay(dL);
for (value = sped; value >= 0; value -= 5)
{
digitalWrite(M1, HIGH);
digitalWrite(M2, HIGH);
analogWrite(E1, value);
analogWrite(E2, 0);
delay(dela);
}
delay(dL);
}
void slow(){
for (value = sped; value >= 0; value -= 5)
{
digitalWrite(M1, HIGH);
digitalWrite(M2, LOW);
analogWrite(E1, value);
analogWrite(E2, value);
delay(dela);
} delay(dL);
}
void turnright(){
for (value = 0 ; value <= sped; value += 5)
{
digitalWrite(M1, HIGH);
digitalWrite(M2, HIGH);
analogWrite(E2, value);
analogWrite(E1,0);
delay(dela);}
delay(dL);
for (value = sped; value >= 0; value -= 5)
{
digitalWrite(M1, HIGH);
digitalWrite(M2, HIGH);
analogWrite(E1, 0);
analogWrite(E2, value);
delay(dela);
}
delay(dL);
}
void returnn(){
for (value = 0 ; value <= sped; value += 5)
{
digitalWrite(M1, LOW);
digitalWrite(M2, LOW);
analogWrite(E1, value);
analogWrite(E2, value);
delay(dela);
} delay(dL);
for (value = sped; value >= 0; value -= 5)
{
digitalWrite(M1, LOW);
digitalWrite(M2, LOW);
analogWrite(E1, value);
analogWrite(E2, value);
delay(dela);
} delay(dL);}
int lookRight()
{
servo.write(50);
delay(500);
int distance = readPing();
delay(100);
servo.write(90);
return distance;
delay(100);
}
int lookLeft()
{
servo.write(170);
delay(500);
int distance = readPing();
delay(100);
servo.write(90);
return distance;
delay(100);
}
int readPing() {
delay(70);
int cm = sonar.ping_cm();
if(cm==0)
{
cm = 250;
}
return cm;
}