Trouble with obstacle avoiding robot

Other Hardware Robotics
11

Okay, so far the best code I have is the following (cutting out the motor functions for length). There's still the occasion where I get a zero for both left and right so it will just turn left. Until I know what situations cause that, I can't modify or add an argument for it. Thanks for the input so far folks, sometimes I just need to work it out with someone to help me understand what I'm trying to do.

#include <NewPing.h>
#include <Servo.h>

#define TRIGGER_PIN 12
#define ECHO_PIN 13
#define MAX_DISTANCE 50
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);  // distance sensor

int enableA = 11; // H bridge pins
int pinIN1 = 6;
int pinIN2 = 5;
int enableB = 10;
int pinIN3 = 4;
int pinIN4 = 3;

Servo head;  // servo naming

const int distanceLimit = 20; // collision distance limit

unsigned int frontcm; // distance measurements
unsigned int leftcm;
unsigned int rightcm;

void setup() {
  Serial.begin(9600);
  head.attach(2);
  head.write(90);
  pinMode(enableA, OUTPUT);
  pinMode(pinIN1, OUTPUT);
  pinMode(pinIN2, OUTPUT);
  pinMode(enableB, OUTPUT);
  pinMode(pinIN3, OUTPUT);
  pinMode(pinIN4, OUTPUT);
  enableMotors();
}

void loop() {
	delay(50); // this delay is supposedly for pings?
	frontcm = sonar.ping_cm(); // take a front measurement
        Serial.print("front ");
        Serial.println(frontcm);
	if (frontcm > distanceLimit || frontcm == 0) {
		forward();
	}
	else {
                Serial.print("stopping distance");
                Serial.println(frontcm);
		stopMove();
		checkPath();
		turnDirection();
	}
}
	
void checkPath() {
	head.write(170);  // look left
	delay(300); // wait for servo to turn
	leftcm = sonar.ping_cm(); // take left measurement
        Serial.print("left ping ");
        Serial.println(leftcm);
	head.write(10); // now turn right
	delay(300);
	rightcm = sonar.ping_cm();
        Serial.print("right ping ");
        Serial.println(rightcm);
	head.write(90); //return to front
	delay(200);
}
	
void turnDirection() {
        if (leftcm < distanceLimit && leftcm > 0 && rightcm < distanceLimit && rightcm > 0) {
		reverse();
                Serial.println("going reverse");
		delay(400);
        }
	else if (leftcm > rightcm || leftcm == 0) {
                Serial.println("turning left");
		turnLeft();
		delay(300);
	}
	else if (rightcm > leftcm || rightcm == 0) {
                Serial.println("turning right");
		turnRight();
		delay(300);
	}
}