I am making an ultrasonic obstacle avoiding vehicle using a small tank chassis. I am controlling the 2 dc motors with an L298N motor driver with pulse width modulation to control speed. For the function; when the vehicle comes within 20cm of an object, the sensor will rotate on a servo and check the distance left and right. It determines which side has the greatest distance to an obstacle, and turn that direction.
Input 1 and 2 of the motor driver control the right motor and track and input 3 and 4 control the left.
I have checked hardware and wires, but i'm having trouble with the code. When there is nothing within 20cm, the tank should drive forward, but for some reason it always turns to the right. (the left track goes forward, turning it to the right) :-\
I am also using an arduino nano.
Here's the code:
#include <Servo.h> //Servo motor library. This is standard library
#include <NewPing.h> //Ultrasonic sensor function library. You must install this library
//our L298N control pins
const int in3 = 5; //left motor
const int in4 = 4; //left motor
const int in1 = 7; //right motor
const int in2 = 6; //right motor
const int lpwm = 8; //left motor speed
const int rpwm = 9; //right motor speed
const int trig_pin = 11;
const int echo_pin = 12 ;
#define maximum_distance 200
boolean goesForward = false;
int distance = 100;
NewPing sonar(trig_pin, echo_pin, maximum_distance); //sensor function
Servo servo_motor; //our servo name
void setup(){
pinMode(trig_pin, OUTPUT);
pinMode(echo_pin, INPUT);
pinMode(in1, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(lpwm, OUTPUT);
pinMode(rpwm, OUTPUT);
servo_motor.attach(10); //our servo pin
servo_motor.write(115);
delay(2000);
distance = readPing();
delay(100);
distance = readPing();
delay(100);
distance = readPing();
delay(100);
distance = readPing();
delay(100);
}
void loop(){
int distanceRight = 0;
int distanceLeft = 0;
delay(50);
if (distance <= 20) //if distance ahead of sensor is less than 20cm
{ //stop, reverse, look left and right with sensor
moveStop(); //and determine longest distance
delay(300);
moveBackward();
delay(400);
moveStop();
delay(300);
distanceRight = lookRight(); //sensor looks left and right to determine distances either side
delay(300);
distanceLeft = lookLeft();
delay(300);
if (distance >= distanceLeft) //tests the distance to the left against 100 (distance)
{
turnRight();
moveStop();
}
else{
turnLeft();
moveStop();
}
}
else{
moveForward();
}
distance = readPing();
}
int lookRight(){
servo_motor.write(50);
delay(500);
int distance = readPing();
delay(100);
servo_motor.write(115);
return distance;
}
int lookLeft(){
servo_motor.write(170);
delay(500);
int distance = readPing();
delay(100);
servo_motor.write(115);
return distance;
delay(100);
}
int readPing(){
delay(70);
int cm = sonar.ping_cm();
if (cm==0){
cm=250;
}
return cm;
}
void moveStop(){
digitalWrite(in1, LOW);
digitalWrite(in3, LOW);
digitalWrite(in2, LOW);
digitalWrite(in4, LOW);
}
void moveForward(){
if(!goesForward){
goesForward=true;
analogWrite(lpwm, 130);
analogWrite(rpwm, 130);
digitalWrite(in3, HIGH);
digitalWrite(in1, HIGH);
digitalWrite(in4, LOW);
digitalWrite(in2, LOW);
}
}
void moveBackward(){
goesForward=false;
analogWrite(lpwm, 120);
analogWrite(rpwm, 120);
digitalWrite(in3, LOW);
digitalWrite(in1, LOW);
digitalWrite(in4, HIGH);
digitalWrite(in2, HIGH);
}
void turnRight(){
analogWrite(lpwm, 130);
analogWrite(rpwm, 90);
digitalWrite(in3, HIGH);
digitalWrite(in2, HIGH);
digitalWrite(in4, LOW);
digitalWrite(in1, LOW);
}
void turnLeft(){
analogWrite(lpwm, 130);
analogWrite(rpwm, 90);
digitalWrite(in3, LOW);
digitalWrite(in2, LOW);
digitalWrite(in4, HIGH);
digitalWrite(in1, HIGH);
}