Sonar auto/self balancing Robot

Trying to find something to do with the IMU and control board, I thought it'd be fun to balance a robot not two wheels. Since i have no clue how to use PIDs or calibrate the sensors, so I just used the AHRS sketch provided by Pololu for balancing the bot. It outputs pitch, roll, yaw directions to serial; I thought i could use the pitch output to balance the robot. I planned to calibrate the sensors when it is level and then tip the bot vertical. Then, the pitch output would determine the movement of the motors(see the italicized below). But strangely, every time the I would move the bot into the upright position it would freeze and reset. its caused by the servo code changing the servo direction; still havent figured out why. (any help here on this issue?)

So, i used a SR04 sonar module instead with a really simple approach. i held the robot in its upright posisition on a flat surfaced and found the distance to the table. The motor control loops throught the following for every sonar read(every 30-50ms):
If the sonar value is too low(close to the table) turn the servos to compensate
If too far from table, then turn servos the other way.
If sonar value balanced, lock motors.  

I used my old Boe-bot frame so it wasn't very balanced in the first place(made out of steel), then i added the battery pack and that totally messed it up in terms of weight distribution(messy as heck too :)). I will soon make a custom acrylic or light wood frame for it so that it will be more balanced. i think if i want to drive it around(maybe with that TV remote :)) im going to have to make this control setup a little more complex.

Pics:

Sonar Module

Code:

//-----Robot Stuff------------------------------------------------//
int LmtrSpd=0;
int RmtrSpd=0;
#define LServoMtrPin 2
#define RServoMtrPin 3
#include <Servo.h>
Servo LServoMtr;
Servo RServoMtr;
//----------------------------------------------------------------//


//Sonar------------------------------------------------------------
float dist=0;
int trig=6;
int echo=7;
float SonarVal=0;

#define BlncPnt 850 //find this !!!
//----------------------------------------------------------------//


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

  pinMode(echo, INPUT);
  pinMode(trig, OUTPUT);

  LServoMtr.attach(LServoMtrPin);
  LServoMtr.write(90);
  RServoMtr.attach(RServoMtrPin);
  RServoMtr.write(90);
}

void loop()
{
  //Sonar------------------------------------------------------------
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  SonarVal= pulseIn(echo, HIGH);
  Serial.println(SonarVal);

  //---Balance-Test---------------------------------------------
  if(SonarVal > BlncPnt+25)
  {
    LServoMtr.write(0);
    RServoMtr.write(180);
  }
  else if(SonarVal < BlncPnt-25)
  {
    LServoMtr.write(180);
    RServoMtr.write(0);
  }
  else
  {
    LServoMtr.write(90);
    RServoMtr.write(90);
  }
  delay(45);
}