Ultrasonic obstacle avoiding tank

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);
}

If I were you I would write a short test sketch with nothing in it than the code from moveForward() to make sure that it's capable of going straight with those pwm values. That way you'll take out any errors in your logic and just test that function.

Also, use serial prints to prove to yourself that the logic is taking it where you expect. Put messages like "I'm in movefForward now" or "I'm going backwards now" in.

  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);

How is this useful?
How is this useful?
How is this useful?
How is this useful?

    moveStop();        //and determine longest distance

That is NOT what that code does. Why does the function have move in the name when the purpose of the function is to stop the motors?

Why do you look ahead at the end of loop()? Seems to me that you should do that FIRST, and then decide what to do based on the latest distance in front of you, not some old data.

Plus what kenwood120s said.

PaulS:
Why does the function [moveStop] have move in the name when the purpose of the function is to stop the motors?

Haha I once actually had some similar code with functions like goForward() and goRight() and so on. To preserve my naming convention, I actually called the stop function goNot() :wink:

I don't have enough experience in coding to write something like this so most of the time i find codes similar to what i need and adapt it, so that's what i've tried to do with this. I've done what kenwood has suggested and tested all functions by themselves (moveForward etc) and they have all worked. Here's the code for that: (it just goes through all functions with a 1 second delay in between)

const int in3 = 5;
const int in4 = 4; 
const int in1 = 7; 
const int in2 = 6; 
const int lpwm = 8;
const int rpwm = 9;

void setup() {
pinMode(in1, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(lpwm, OUTPUT);
  pinMode(rpwm, OUTPUT);
}

void loop() //fwd,back,right,left functions
{

    analogWrite(lpwm, 130);  //forward
    analogWrite(rpwm, 130);
   digitalWrite(in3, HIGH);
    digitalWrite(in1, HIGH);
    digitalWrite(in4, LOW);
    digitalWrite(in2, LOW); 
    
    delay(1000);
    digitalWrite(in1, LOW); 
  digitalWrite(in3, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in4, LOW);
    delay(1000);
    
  analogWrite(lpwm, 130); //backwards
  analogWrite(rpwm, 130);
  digitalWrite(in3, LOW);
  digitalWrite(in1, LOW);
  digitalWrite(in4, HIGH);
  digitalWrite(in2, HIGH); 
  
 
    delay(1000);
     digitalWrite(in1, LOW); 
  digitalWrite(in3, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in4, LOW);
    delay(1000);

analogWrite(lpwm, 130); //right
  analogWrite(rpwm, 90);
  digitalWrite(in3, HIGH);
  digitalWrite(in2, HIGH);
  digitalWrite(in4, LOW);
  digitalWrite(in1, LOW);


    delay(1000);
     digitalWrite(in1, LOW); 
  digitalWrite(in3, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in4, LOW);
    delay(1000);
 
 analogWrite(lpwm, 130); //left
  analogWrite(rpwm, 90);
  digitalWrite(in3, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in4, HIGH);
  digitalWrite(in1, HIGH);
  
   
    delay(1000);
     digitalWrite(in1, LOW); 
  digitalWrite(in3, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in4, LOW);
    delay(1000);
  
}

I was just following some code i found for a similar project: http://mertarduinotutorial.blogspot.com.au/2017/03/arduino-project-tutorial-17-obstacle.html

OK then to put some serial monitor prints in do this..

In setup, add this line:

void setup() {
Serial.begin(9600);
..

And then in your functions like moveForward etc, add lines like this:

void moveForward(){
Serial.println("In moveForward now");
..

You can stick lines like that anywhere you like, to test (eg) that your code is responding as expected in ifs.

When your program's running, open the serial monitor ctrl+shift+m in the ide and check the baud rate (box in lower right of monitor screen) matches the Serial.begin(9600) or whatever value you used.

Then you should see your messages appearing as the code moves around and you can see if the logic flow is correct.

Thanks, i'll try that :slight_smile:

So i have tested each function (forward, back, left, right) and they all work, however the pwm values have to be higher than 130 otherwise the tracks won't spin. I then put these function back into the main code and when the serial monitor says "in moveFoward now" only the left track (controlled by in3 &in4) spins forwards. The other functions in the code also only spin the left track so i'm not sure what's happening.

Test function code: It runs through all functions with a 2 second pause. (same as the last one but with serial)

const int in3 = 5;//left tracks
const int in4 = 4; //left track
const int in1 = 7; //right track 
const int in2 = 6; //right track
const int lpwm = 8; //left pwm
const int rpwm = 9; //right pwm

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
pinMode(in1, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(lpwm, OUTPUT);
  pinMode(rpwm, OUTPUT);
}

void loop() //fwd,back,right,left
{

Serial.println("In moveForward now"); 
analogWrite(lpwm, 130);
analogWrite(rpwm, 130);
   digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW); 
     delay(2000);
     
   Serial.println("In moveStop now");
  digitalWrite(in1, LOW); 
  digitalWrite(in3, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in4, LOW);
  delay(2000);
  
Serial.println("In moveBackwards now");
  analogWrite(lpwm, 130);
  analogWrite(rpwm, 130);
  digitalWrite(in3, LOW);
  digitalWrite(in1, LOW);
  digitalWrite(in4, HIGH);
  digitalWrite(in2, HIGH); 
  
  delay(2000);
 Serial.println("In moveStop now");
  digitalWrite(in1, LOW); 
  digitalWrite(in3, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in4, LOW);
delay(2000);

  Serial.println("In turnRight now");
analogWrite(lpwm, 130);
  analogWrite(rpwm, 130);
  digitalWrite(in3, HIGH);
  digitalWrite(in2, HIGH);
  digitalWrite(in4, LOW);
  digitalWrite(in1, LOW);
  
    delay(2000);
 Serial.println("In moveStop now");
  digitalWrite(in1, LOW); 
  digitalWrite(in3, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in4, LOW);
  delay(2000);
  
  Serial.println("In turnLeft now");
 analogWrite(lpwm, 130);
  analogWrite(rpwm, 130);
  digitalWrite(in3, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in4, HIGH);
  digitalWrite(in1, HIGH);
  
  delay(2500);

}

Test function code: It runs through all functions with a 2 second pause.

I know that this is going to sound pedantic, but it does not actually run through the functions as the code is all inline.

Put the code to call the functions in turn in setup() in your original program. That will test the actual functions in your program before running the main code.

How would I put them all in setup()?
"original code" - Do you mean the one I used to test the functions?

How would I put them all in setup()?

Insert code to call each of the functions in turn with a delay() between them so that you can determine whether they work.

"original code" - Do you mean the one I used to test the functions?

I meant the code from your original post.

The reason to test the functions in the actual program is to avoid copy/paste or typing errors when testing the code typed in line.

I put the functions in with a delay between but i get the error "'moveStop' was not declared in this scope".

#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 = 3; //left motor speed
const int rpwm = 11; //right motor speed
const int trig_pin = 13;
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(){
  Serial.begin(9600);
  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);
  
    Serial.println("In turnRight now");
analogWrite(lpwm, 130);
  analogWrite(rpwm, 130);
  digitalWrite(in3, HIGH);
  digitalWrite(in2, HIGH);
  digitalWrite(in4, LOW);
  digitalWrite(in1, LOW);
  
    delay(2000);
 Serial.println("In moveStop now");
  digitalWrite(in1, LOW); 
  digitalWrite(in3, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in4, LOW);
  delay(2000);
  
  Serial.println("In turnLeft now");
 analogWrite(lpwm, 130);
  analogWrite(rpwm, 130);
  digitalWrite(in3, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in4, HIGH);
  digitalWrite(in1, HIGH);

  delay(2000);
Serial.println("In moveStop now");
  digitalWrite(in1, LOW); 
  digitalWrite(in3, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in4, LOW);
 delay(2000);
 
Serial.println("In moveBackwards now");
  analogWrite(lpwm, 130);
  analogWrite(rpwm, 130);
  digitalWrite(in3, LOW);
  digitalWrite(in1, LOW);
  digitalWrite(in4, HIGH);
  digitalWrite(in2, HIGH); 
  
   delay(2000);
Serial.println("In moveStop now");
  digitalWrite(in1, LOW); 
  digitalWrite(in3, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in4, LOW);
  delay(2000);

  Serial.println("In moveForward now");
  analogWrite(lpwm, 130);
   digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW); 
    analogWrite(rpwm, 130);
    
      delay(2000);
Serial.println("In moveStop now");
  digitalWrite(in1, LOW); 
  digitalWrite(in3, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in4, LOW);
  delay(3000);
}


void loop(){

  int distanceRight = 0;
  int distanceLeft = 0;
  delay(50);

  if (distance <= 30)  
  {                    
    moveStop();        
    delay(300);
    moveBackward();
    delay(400);
    moveStop();
    delay(300);
    distanceLeft = lookLeft();
    delay(300);
    distanceRight = lookRight();  //sensor looks left and right to determine distances either side
    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;
}

I added them in setup without the rest and the functions work.

#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 = 3; //left motor speed
const int rpwm = 11; //right motor speed
const int trig_pin = 13;
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(){
  Serial.begin(9600);
  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);
  

  
    Serial.println("In turnRight now");
analogWrite(lpwm, 130);
  analogWrite(rpwm, 130);
  digitalWrite(in3, HIGH);
  digitalWrite(in2, HIGH);
  digitalWrite(in4, LOW);
  digitalWrite(in1, LOW);
  
    delay(2000);
 Serial.println("In moveStop now");
  digitalWrite(in1, LOW); 
  digitalWrite(in3, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in4, LOW);
  delay(2000);
  
  Serial.println("In turnLeft now");
 analogWrite(lpwm, 130);
  analogWrite(rpwm, 130);
  digitalWrite(in3, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in4, HIGH);
  digitalWrite(in1, HIGH);

  delay(2000);
Serial.println("In moveStop now");
  digitalWrite(in1, LOW); 
  digitalWrite(in3, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in4, LOW);
 delay(2000);
 
Serial.println("In moveBackwards now");
  analogWrite(lpwm, 130);
  analogWrite(rpwm, 130);
  digitalWrite(in3, LOW);
  digitalWrite(in1, LOW);
  digitalWrite(in4, HIGH);
  digitalWrite(in2, HIGH); 
  
   delay(2000);
Serial.println("In moveStop now");
  digitalWrite(in1, LOW); 
  digitalWrite(in3, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in4, LOW);
  delay(2000);

  Serial.println("In moveForward now");
  analogWrite(lpwm, 130);
   digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW); 
    analogWrite(rpwm, 130);
    
      delay(2000);
Serial.println("In moveStop now");
  digitalWrite(in1, LOW); 
  digitalWrite(in3, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in4, LOW);
  delay(3000);
}
 void loop()
 {
 }

Those aren't functions though, they're just the code from the functions all just acting one after the other.

I meant that in your original code, where your logic (all the if's) is calling the functions, put them in the actual functions so you can tell if the correct function is being called when you expect it to, based on the sensors.

I'm not 100% understanding but would it be similar to this:

if (distance >= distanceLeft)  //if distance to the left is less, turn right
    {
      turnRight();
      moveStop();

Instead of that I write this: ?

if (distance >= distanceLeft) //if distance to the left is less, turn right
    {
      Serial.println("In turnRight now");
analogWrite(lpwm, 130);
  analogWrite(rpwm, 1300);
  digitalWrite(in3, HIGH);
  digitalWrite(in2, HIGH);
  digitalWrite(in4, LOW);
  digitalWrite(in1, LOW);
  delay(2000);
  Serial.println("In moveStop now");
  digitalWrite(in1, LOW); 
  digitalWrite(in3, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in4, LOW);

I put the functions in with a delay between

No you didn't. You pasted in the code from the functions. Not the same thing at all. Put calls to the functions in setup() not the code from the functions.

i get the error "'moveStop' was not declared in this scope".

Is there a moveStop() function in your code ? Answer : no
Looks like you have a copy/paste error.

Remember what I said earlier ?

The reason to test the functions in the actual program is to avoid copy/paste or typing errors when testing the code typed in line.

Sorry i got confused with calls and functions.
So the calls to the functions are these?:
turnRight();
moveStop();
turnLeft();
moveForward();

So the calls to the functions are these?:
turnRight();
moveStop();
turnLeft();
moveForward();

Yes

Doing it that way ensures that the test is using the same code as the main program.

You could even smarten things up a bit more by putting the test code in a function of its own and calling that from setup(). That way you can turn the test on and off by just commenting out the call to the test function.

So i tested all the functions by putting the calls in the setup and they all work.

#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 = 3; //left motor speed
const int rpwm = 11; //right motor speed
const int trig_pin = 13;
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() {
  Serial.begin(9600);
  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);
  
 
moveForward(); 
delay(2000);
moveStop(); 
delay(2000);
moveBackward();
delay(2000);
moveStop(); 
delay(2000);
turnRight();
delay(2000);
moveStop();
delay(2000);
turnLeft();
delay(2000);
moveStop();
delay(2000);
}


 void moveStop(){
  Serial.println("In moveStop now");
  digitalWrite(in1, LOW); 
  digitalWrite(in3, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in4, LOW);
}

void moveForward(){
Serial.println("In moveForward now");
  
  if(!goesForward){
    goesForward=true;
analogWrite(lpwm, 140);
   digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW); 
    analogWrite(rpwm, 140);   
  }
}

void moveBackward(){
Serial.println("In moveBackwards now");
  goesForward=false;
  analogWrite(lpwm, 130);
  analogWrite(rpwm, 130);
  digitalWrite(in3, LOW);
  digitalWrite(in1, LOW);
  digitalWrite(in4, HIGH);
  digitalWrite(in2, HIGH); 
  
}

void turnRight(){
  Serial.println("In turnRight now");
analogWrite(lpwm, 240);
  analogWrite(rpwm, 240);
  digitalWrite(in3, HIGH);
  digitalWrite(in2, HIGH);
  digitalWrite(in4, LOW);
  digitalWrite(in1, LOW);
    
}

void turnLeft(){
  Serial.println("In turnLeft now");
 analogWrite(lpwm, 240);
  analogWrite(rpwm, 240);
  digitalWrite(in3, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in4, HIGH);
  digitalWrite(in1, HIGH);  
}

The problem with my main code is that the tank will go forward until something is within 30cm, it will then stop and reverse, look either side with the sensor, but then it just goes straight forward again. So i'm thinking there is a problem with the sensor coding. :-\

So, it appears that the functions work and that your circuit is correct.

The problem with my main code is that the tank will go forward until something is within 30cm, it will then stop and reverse, look either side with the sensor, but then it just goes straight forward again

That is a different problem than you started with

Please post your program with the problem but leave the test function calls in setup() for now. When you post your code please indicate whether the tests in setup() work.