Hello, I am making a robot that goes through a maze for my science fair project. I am using an HC-SR04 to make it navigate and the microcontroller is an arduino mega 2560(clone). I had the code written and everything ready but when I uploaded the code two days ago, the servo went left quickly, right a little slower, and then stopped and it will not do anything else 
I checked the motors on a different code, and they work. So does the servo, there is plenty of power(I have 3-4 AA battery holders wired together(should be 12 volts...?)), and the only other thing I can think of is that the HC-SR04 is shorted out or something and that confuses(or messes up) the robot.
Here is the code:
//Arduno Maze-Solving OS Robot
#include <AFMotor.h>//Include Library for motor sheild
#include <NewPing.h>//include Library for 'HC-SR04'
#include <Servo.h>//include Library for the servo
#define TRIGGER_PIN 15 //trigger is on this pin
#define ECHO_PIN 16//echo on this one
#define MAX_DISTANCE 50//max distance to wait for is 50 CM
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); //Setup newPing
const int BLED = 50;//the pin for blue LED
const int RLED = 46;//the pin for red LED
const int GLED = 48;//the pin for green LED
const int ArmSensor = 35;//the pin the bump sensor is on
int val = 0;//create variable for bump sensor
int old_val = 0;//old value of bump semsor
Servo Head;//Create servo "Head"
int distance = 0;//variable to Sense distance
int LeftDistance = 0;//used to store distance to the left
int RightDistance = 0;//used to store distance to the right
float HeadPosition;//used to move the servo that turns the head
AF_DCMotor Left(2);//left motor is on motor 2
AF_DCMotor Right(1);//right motor is on motor 1
AF_DCMotor Fan(3);//fan is on motor three
//const int LightSensor = 12;//light sensor is on analog pin 12
//int LightLevel;//used to calculate light level
void setup() {//(Goes through this at the begining when robot is powered up. doesn't repeat)
pinMode(BLED, OUTPUT);//vvvvvvvvv
pinMode(GLED, OUTPUT);//LED's are outputs
pinMode(RLED, OUTPUT);//^^^^^^^^^
pinMode(ArmSensor, INPUT);//bump sensor is an input
Left.setSpeed(78);//set speed of left motor
Left.run(RELEASE);//stop the left motor
Right.setSpeed(75);//set speed of Right motor
Right.run(RELEASE);//stop right motor
// Fan.setSpeed(100);
// Fan.run(FORWARD);
Head.attach(10);//the head servo motor is on pin 10
// Head.write(56);//move the head a little to make sure it works
delay(300);//wait a little
}
void loop() {//(main code. repeats untill powered off)
val = digitalRead(ArmSensor);//see if the bump sensor has been pressed
if((val = HIGH) && (old_val = LOW)) {//if we have crashed
STOP();//stop moving(wil not start back up untill reset button is pressed);
}else{//otherwise
old_val = val;//store as old val
// delay(2000);
scan();//scan to see distance
// delay(2000);
if(distance > 16){//if we have room
Forward();//keep going forward
delay(400);//for .4 of a second
Brake();
}else if(distance <= 16) {//otherwise the distance is (Probably) going to be < or equal to 16 CM
for(HeadPosition = 56; HeadPosition = 130; HeadPosition +5) {
Head.write(HeadPosition);//look to the left
delay(15);
}
scan();//read distance
// delay(2000);
distance = LeftDistance;//store the distance as left
// delay(2000);
for(HeadPosition = 130; HeadPosition = 30; HeadPosition -5) {
Head.write(30);//look to the right
delay(15);//give time to get there
}
scan();//read distance
// delay(2000);
distance = RightDistance;//store the distance as right
if(LeftDistance > RightDistance) { //if there is more room to the left
TurnLeft(); //turn left
delay(200);//(aproxamately 90 degree's)
Brake();
}else if(RightDistance >= LeftDistance) { //if there is more room to the right
TurnRight();// turn right
delay(200);//(aproxamately 90 degree's)
Brake();
}
}
}
}
void Forward() {//defining function forward
Left.run(FORWARD); Right.run(FORWARD);//make both wheels go forward
digitalWrite(GLED, HIGH);
digitalWrite(BLED, LOW);//signal with LEDs
digitalWrite(RLED, LOW);
}
void TurnLeft() {//defining left
Left.run(BACKWARD); Right.run(FORWARD);//make wheels go left
digitalWrite(BLED, HIGH);
digitalWrite(GLED, LOW);//signal with LEDs
digitalWrite(RLED, LOW);
}
void TurnRight() {//defining right
Left.run(FORWARD); Right.run(BACKWARD);//make wheels go right
digitalWrite(RLED, HIGH);
digitalWrite(GLED, LOW);//signaal with LED's
digitalWrite(BLED, LOW);
}
void Backward() {//defining backward
Left.run(BACKWARD); Right.run(BACKWARD);//make wheels go backward
digitalWrite(GLED, HIGH);
digitalWrite(BLED, HIGH);//signal with LED's
digitalWrite(RLED, LOW);
}
void scan() {//defining scan
delay(50);//pause between pings
unsigned int uS = sonar.ping();///calculateing distance infront of us
distance = uS / US_ROUNDTRIP_CM;//^^^
digitalWrite(GLED, HIGH);
digitalWrite(RLED, HIGH);//signal that we are scanning
digitalWrite(BLED, LOW);
}
void Brake() {
Left.run(RELEASE);Right.run(RELEASE);
}
void STOP() {//define how to stop
Left.run(RELEASE);//stop left
Right.run(RELEASE);//stop right
Fan.run(RELEASE);//stop the fan
digitalWrite(GLED, LOW);
digitalWrite(BLED, HIGH);//signal that we are stopped
digitalWrite(RLED, HIGH);
Head.write(90.5);//turn head exactly to center
}
Oh and I haven't built the maze yet (if that tells you anything that you need to know).
Thank you for your time
!