[Help Needed] Arduino Ping))) and Sharp IR Robot Navigation

My high-school is about to enter the First Robotics Competition, and I was brushing up on my programming skills.
I wrote some seemingly simple code, but it’s giving me all kinds of weird problems.

I’ve commented best I could

#include <Servo.h> //include servo library for Servo.write, Servo.attach, etc.

//These I left as #define statements to see if it affected code performance

  #define leftIR 1 //Left IR Sensor Pin (SHARP 2Y0A21  F 07)
  #define rightIR 0 //Right IR Sensor Pin (SHARP 2Y0A21  F 07)
  #define pingLimit 3 //how close (in inches) the robot can be from an object
  #define pingPin 6 //Pin attached to parallax Ping))) sensor
  #define headPin 7 //servo pin for pingScan
  #define leftPin 8 //left servo wheel (continuous)
  #define rightPin 9 //right servo wheel (continuous)
Servo rightServo, leftServo, headServo; //create servo objects

float pingVal; //floating point variable used to store current ping distance

void setup(){
  pinMode(leftIR, OUTPUT);
  pinMode(rightIR, OUTPUT);

void loop(){  
    pingVal = Ping(); 

    if(pingVal <= pingLimit){
      int order = pingScan();


 int IR(){
  int L = analogRead(leftIR);
  int R = analogRead(rightIR); 

   if(L < 200 && R < 200)return('FORWARD');
   if(L > 900 && R > 900)return('STOP');

   if(L > 1000 || R > 1000)return('STOP');
   if(L < R)return('ANGLE LEFT');
   if(L > R)return('ANGLE RIGHT'); 
void speedMod(){
  //On my wood chassis; rightServo forward = 0, leftServo forward = 180
  int action = IR();
     if(action == 'FORWARD'){
     if(action == 'STOP'){
       //order = pingScan();
     if(action == 'ANGLE LEFT'){  //This section angles the robot left; speed depends on it's distance to an object
       int irR = analogRead(rightIR);

      leftServo.write(180 - (pow(-0.0000227273 * irR,2) + 0.120385 * irR - 9.16783)); //Formula; takes crappy ir data and sets max val = 90 and min val = 0 for servo speed control  
    if(action == 'ANGLE RIGHT'){  //This section angles the robot right; speed depend's on its distance to an object
      int irL = analogRead(leftIR);
      rightServo.write(180 - (pow(0.0000227273 * irL, 2) - 0.120385 * irL + 189.168)); //Formula; takes crappy ir data and sets its max val = 90 and min val = 180 for servo speed control
long Ping(){ // standard ping command

    pinMode(pingPin, OUTPUT);  //set ping as output
    digitalWrite(pingPin, LOW); //make sure ping is off
    digitalWrite(pingPin, HIGH); //turn ping on for 5 microseconds
    digitalWrite(pingPin, LOW); //turn ping off
    pinMode(pingPin, INPUT);
      return(pulseIn(pingPin,HIGH) / 74 / 2); //73.746 microseconds per inch. This distance is to and then from the object, so we only need one length(/2).
      //returns value in inches

int pingScan(){
  int leftping; // variable for left distance
  int rightping; // variable for right distance
    leftServo.write(90); // stops robot
    rightServo.write(90); // stops robot
  headServo.write(0);    //                        Turn Left
  delay(500);            //NEED
  leftping = Ping();     //HELP
  delay(50);             //CLEANING
  headServo.write(180);  //THIS
  delay(1000);          //UP
  rightping = Ping();   //(MAKING IT MORE EFFICIENT) 
  delay(50);            //
  headServo.write(90);  //
  //returns 1 2 or 3 depending on values
  if(leftping < rightping)return(1); // left is more open
  if(leftping > rightping)return(2); //right is more open
  if(leftping == rightping)return(3); //they both suck

void execute(int type){ // executes an opertion based on outputs from pingScan
  if(type == 1){ //turn left
  if(type == 2){ //turn right
  if (type == 3){ //backup and turn around

Now the problems: The Ping sensor takes a ping randomly (anywhere from every 1 - 2 seconds) half of the values it returns are 0’s
The ping should be taking measurements extremely fast (every loop through)

There are 2 x SHARP 2Y0A21 IR sensors on either side of the robot angled at 45 deg.

The robot successfully navigates when either one of the sensing systems (ping or stereo IR) are disabled. But when I enable both the IR and ping, the ping keeps stopping the robot and taking a measurement constantly, and the robot doesn’t angle away from the walls via IR

Any help is greatly appreciated, and I will do my best to clear anything up

if(L < 200 && R < 200)return('FORWARD');

Read that line carefully in the context of a function returning “int”

I haven't looked at all of your code in detail but your IR() function and how you use it is wrong. The function is supposed to return an int but you are returning things like 'FORWARD' and 'STOP'.

I noticed this too    leftServo.write(180 - (pow(-0.0000227273 * irR,2) + 0.120385 * irR - 9.16783)); //Formula; takes crappy ir data and sets max val = 90 and min val = 0 for servo speed controlAt the very best it is going to take a while to execute !

Sharpe IR sensors should be on the analog pins so you should not have these

  pinMode(leftIR, OUTPUT);
  pinMode(rightIR, OUTPUT);


Thank you all for the responses, and I realize there are inconsistencies in my code.

Int: return 'Forward' worked correctly (albeit it is written wrong) it was just one of those "it worked don't touch it" things, but I'll change it to return actual intergers.

It's almost like these Sharp sensors have never been used with an arduino, I searched for hours and have yet been able to find a working example. Remove the pinmode(output) statement? Will do.

Even the calculations worked, they aren't what is causing the delay(I believe that to be in the ping code somewhere)

As all of your suggestions may help clear up some problems, none of them proved to be a root cause.

Is there a way to seperate out everything? Right now it's like it just tries to run all the functions at once and give interference and errors

What functions does it run all at once ? How about some Serial.prints in the code so that you know which functions are being executed and the value of the variables ?

I think I figured out the problem. Everything was working correctly, but only when any 2 of the 3 navigation systems were activated (eg Servos + Ultrasonic, or US + IR) Even though the board is getting 9v, the system seems to be drawing too much power.

How do I fix this, I don't just want to add another battery pack, how do I regulate 9v (which should be more than plenty) between all four systems (board, servos(2 x 5-6v), Ping (5v), and IR (2 x 5v)

Feed the 9V to the board and use the board 5V and GND pins to supply the IR and Ping sensors. Use a separate power supply, perhaps 4 AA cells, for the servos making sure that the GND from the servo supply is commoned with the Arduino GND.

You do not say what form the 9V supply takes but if it is the typical small battery with 2 connectors on the top (PP3) it will not even power the Arduino for long, let alone anything else, and certainly not the servos.