1st semi intelligent robot, suggestions please!

Hello all!

This is my first post as well as my first arduino project (other than (basic - blink), (servo -sweep) and (hcsr04Ultrasonic - UltrasonicDemo).

i was hoping you all could look at my code and make any suggestions or recommendations.
I was also hoping you all could look at my hardware and make any suggestions or recommendations. (it all works, but I want to make it better!)

1st my harware:

arduino uno r2.
tamiya track and wheel set (TAM-008-000)
little bit of lexan (bought from home depot USA)
2 - JR sport st47 servos
HC-SR04 ultrasonic sensor
T-Pro Mini Servo SG-90
Losi 7.2V 1100mAh NiMH Batt
project box
breadboard piece
misc stuff.

1st hardware question:
I have all 3 servos as well as the HC-SR04 taking power off of the arduino 5v pin. Even though i've read this is bad, it is working. I understand that the arduino uno has a voltage regulator of sorts and will do just fine, as long as the current to the servos and HC-SR04 are less than the breaking point. Am I going to break something?

2nd hardware question:
these tracks blow on carpet... any suggestions?

next few posts will be pics

pics 1

IMG_7095-1.JPG

IMG_7096-1.JPG

IMG_7097-1.JPG

IMG_7099-1.JPG

pics2

IMG_7102-1.JPG

IMG_7104-1.JPG

IMG_7107-1.JPG

IMG_7108-1.JPG

pics3

IMG_7109-1.JPG

IMG_7111-1.JPG

IMG_7112-1.JPG

IMG_7113-1.JPG

pics4

IMG_7114-1.JPG

IMG_7115-1.JPG

IMG_7116-1.JPG

IMG_7117-1.JPG

pics5

IMG_7118-1.JPG

Here is the code! please be gentle! hope this helps anyone else! good luck! and please respond if you have any questions or suggestions

#include <Servo.h> //thank you Michael Margolis et al.
#include <Ultrasonic.h>  // thank you ITead studio. Alex, cnobile and Rowan Simms et al.


/*
  robot_7 - for Arduino
  Copyright (c) 2012 Richard Pesce.  All right reserved.

  This is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

  Richard Pesce (please give me a little credit
  PesceTech.com
*/

// -------------------------------------------------------
// 90 = stop                                              |
// forward   = right >90 & left < 90                      |
// backward  = right <90 & left > 90                      |
// left      = right >90 & left >90                       |
// right     = right <90 & left <90                       |
// i belive +- 20 is the fastest my driving servos turn   |
// -------------------------------------------------------

//  #define TRIGGER_PIN  8         // no need for variable
//  #define ECHO_PIN     12        // no need for variable
Ultrasonic ultrasonic(8, 12);      // set pins for HC-SRO4 (sonar) *why cant this be in the "void setup"?

Servo rightservo;                  // create servo object to control a servo *why cant this be in the "void setup"?
Servo leftservo;                   // create servo object to control a servo *why cant this be in the "void setup"?
Servo sonarservo;                  // create servo object to control a servo *why cant this be in the "void setup"?

int stopped = 90;                        // these servos stop moving at 90 *why cant this be in the "void setup"?
int sonarcenter = 98;                    // the center point of my sonar servo *why cant this be in the "void setup"?
int sonarleftlimit = 137;                // how far for sonar to turn left *why cant this be in the "void setup"? 128
int sonarrightlimit = 53;                // how far for sonar to turn right *why cant this be in the "void setup"? 60
int sonardirectionfacing = 0;            // where is sonar facing? *why cant this be in the "void setup"?
int sonarcurrentposition = sonarcenter;  // variable to store the servo position *why cant this be in the "void setup"?
int sonorleftORright = 0;                // 0=left 1=right *why cant this be in the "void setup"?
int StuckInACorner = 0;

void setup() 
{ 
  Serial.begin(9600);                // to talk to serial
  rightservo.attach(10);             // attaches the servo on pin 8 to the servo object 
  leftservo.attach(9);               // attaches the servo on pin 9 to the servo object
  sonarservo.attach(11);             // attaches the servo on pin 9 to the servo object
  pinMode(13, OUTPUT);               // LED
  sonarservo.write(sonarcenter);     // center position of my servo          
  delay(500);  
}

void RotateSonar()
{
  if (sonorleftORright == 0) { 
    sonarcurrentposition = sonarcurrentposition +10;                      // if sonar is heading left rotate sonar left 10 degrees
  }                  
  if (sonorleftORright == 1) { 
    sonarcurrentposition = sonarcurrentposition -10;                      // if sonar is heading right rotate sonar right 10 degrees
  }                  

  if (sonorleftORright == 0 && sonarcurrentposition >= sonarleftlimit) {  // if sonar is heading left but hits the left limit, go right
    sonorleftORright = 1;
  }    
  if (sonorleftORright == 1 && sonarcurrentposition <= sonarrightlimit) { // if sonar is heading right but hits the right limit, go left
    sonorleftORright = 0;
  }   

  sonarservo.write(sonarcurrentposition);                                 // move the sonar servo
  delay(60);                                                              // so servo can catch up
}

int distance()
{
  float inMsec;                                                  // some math with sonar
  long microsec = ultrasonic.timing();                           // some math with sonar
  inMsec = ultrasonic.convert(microsec, Ultrasonic::IN);         // convert to american (aka imperial)
  Serial.print(inMsec);                                          // print distance to serial
  Serial.println(" inches away");                                // print distance to serial
  return inMsec;                                                 // return the distance to the object in inches
}

void MoveForward()
{
  StuckInACorner = 0;
  digitalWrite(13, LOW);                                          // turn off led
  rightservo.write(stopped +14);                                  // move forward offset is due to servo performance
  leftservo.write (stopped -20);                                  // move forward
}

void EvasiveAction()
{
  int objectlocation;                                            // var to store the location of the object
  digitalWrite(13, HIGH);                                        // turn on the led

  if ( sonarcurrentposition <= sonarcenter ) {                   // if looking right
    Serial.println("Object detected on the right!");               
    objectlocation = 1;                                          // then the object must be on the right (1 = right)
  } 
  else {
    Serial.println("Object detected on the left!");
    objectlocation = 0;                                          // otherwise the object must be on the left (0 = left)
  }

  if (objectlocation == 1) {                                       // if object is on right
    rightservo.write(stopped +15);                                 // turn left until the object is not there anymore
    leftservo.write (stopped +15);                                 // see above
    delay(500);
  }

  if (objectlocation == 0) {                                       // if object is on left
    rightservo.write(stopped -15);                                 // turn right until the object is not there anymore
    leftservo.write (stopped -15);                                 // see above
    delay(500);
  }
}

void PlanAhead()
{
  int objectlocation;                                            // var to store the location of the object
  digitalWrite(13, HIGH);                                        // turn on the led

  if ( sonarcurrentposition <= sonarcenter ) {                   // if looking right
    Serial.println("Object detected on the right!");               
    objectlocation = 1;                                          // then the object must be on the right (1 = right)
  } 
  else {
    Serial.println("Object detected on the left!");
    objectlocation = 0;                                          // otherwise the object must be on the left (0 = left)
  }

  if (objectlocation == 1){
    StuckInACorner = StuckInACorner +1;
  }
  else{
    StuckInACorner = StuckInACorner -1;                          // this is a buffer of sorts, kinda like counting cards, prevents sporadic movement
  } 

  if (StuckInACorner >= 3 ) {                                    // if object is on right
    rightservo.write(stopped +14);                               // turn left until the object is not there anymore
    leftservo.write (stopped);                                   // see above
  }

  if (StuckInACorner <= -3 ) {                                   // if object is on left
    rightservo.write(stopped);                                   // turn right until the object is not there anymore
    leftservo.write (stopped -20);                               // see above
  }
}


void loop() 
{
  if ( distance() < 5)                                            // if an oject is less than 5" away
  {
    EvasiveAction();                                              // DANGER take evasive action
  }
  else{                                                           // or
    if ( distance() >= 5 and distance() < 14 ) {                  // if greater than 5" but less than 14"
      PlanAhead();                                                // avoid the object
      RotateSonar();                                              // and rotate sonar
    }
    else{                                                         // otherwise
      MoveForward();                                              // move forward
      RotateSonar();                                              // and rotate sonar
    }
  }
}

Working code for 2 servo drive motors and one sonar servo motor

I'm drunk. So please forgive lame posts.

Schematic of sorts....

Well I've made the switch... I'm now powering everything directly from the battery. Even though I get more speed from my servos. I still feel like I may break something. 7.2V straight to the servos? And also 7.2V straight to the HC-SR04 ultrasonic sensor? I feel like something is wrong.

Please help!

Well, I haven't read everything, but I find 7,2V straight is really too much, recommanded voltage input is usually MAX 6V

Do you know how much amps the servos draw? You can use a voltage regulator to to get the voltage down to 5-6V for the servos. I wouldn't power the ultrasonic module straight from the battery, take 5V from your Uno.