My attempt to add your code

My Attempt

//TWIN WHEELER MODIFIED FOR ARDUINO SIMPLIFIED SERIAL PROTOCOL TO SABERTOOTH V2
//With nunchuck potentiometers controlling it
//or could use two regular 10K potentiometers, one for steering and one for fine tuning of balance point
//J. Dingley  For Arduino 328 Duemalinove or similar with a 3.3V accessory power output
//i.e. the current standard Arduino board.

//XXXXXXXXXXXXXXXXX UPDATES June 2012 XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

// If you open serial view window at 9600 baud, you can see values for angle etc (once tiltstart has engaged)
//REMEMBER: Put one end down on floor, turn on, count to 5 (while it calibrates itself) 
//THEN, press deadman switch, stand on it and bring it level to engage the "tiltstart"
//This version also has "Softstart" function

//MAKE SURE ARDUINO IS PROPERLY POWERED BY A BATTERY, NOT JUST THE USB CABLE.







#include <SoftwareSerial.h>

//Set dip switches on the Sabertooth for simplified serial and 9600 Buadrate. Diagram of this on my Instructables page//

//Digital pin 13 is serial transmit pin to sabertooth
#define SABER_TX_PIN  13
//Not used but still initialised, Digital pin 12 is serial receive from Sabertooth
#define SABER_RX_PIN  12

//set baudrate to match sabertooth dip settings
#define SABER_BAUDRATE  9600

//simplifierd serial limits for each motor
#define SABER_MOTOR1_FULL_FORWARD 127
#define SABER_MOTOR1_FULL_REVERSE 1

#define SABER_MOTOR2_FULL_FORWARD 255
#define SABER_MOTOR2_FULL_REVERSE 128

//motor level to send when issuing full stop command
#define SABER_ALL_STOP  0


SoftwareSerial SaberSerial = SoftwareSerial (SABER_RX_PIN, SABER_TX_PIN );
                                             
void initSabertooth (void)  {
  //initialise software to communicate with sabertooth
pinMode ( SABER_TX_PIN, OUTPUT );


SaberSerial.begin( SABER_BAUDRATE );
//2 sec delay to let it settle
delay (2000);
SaberSerial.print(byte(0));   //kill motors when first switched on
}


//setup all variables. Terms may have strange names but this software has evolved from bits and bobs done by other segway clone builders

//xxx code that keeps loop time at 10ms per cycle of main program loop xxxxxxxxxxxxxxx
int STD_LOOP_TIME = 9;
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;


//XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX SETUP VARIABLES XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
float overallgain = 0.2; //START value for overallgain
float overallgaintarget = 0.5; //Value you want overallgain to slowly increase to over first few seconds after tipstart has been engaged (i.e. is softer to begin with)
int motorpercentlimit = 100;  //max speed limiter when debuggiong
int balancetarget = 350;  //adjust this value, it sets the angle it balances at. if not quite level increase it a bit or make it a bit smaller.
//XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX


float level=0;
float aa = 0.005; //this means 0.5% of the accelerometer reading is fed into angle of tilt calculation with every loop of program (to correct the gyro).
//accel is sensitive to vibration which is why we effectively average it over time in this manner. You can increase aa if you want experiment. 
//too high though and the board may become too vibration sensitive.
float Steering;
float SteerValue;
float SteerCorrect;
float steersum;
int Steer = 0;

float accraw;
float x_acc;
float accsum;
float x_accdeg;

float gyrosum;
float hiresgyrosum;
float g;
float s;
float t;

float gangleratedeg;
float gangleratedeg2;

int adc1;
int adc4;
int adc5;


float gangleraterads;
float gyroscalingfactor = 2.2;



float k1;
//int k2;
//int k3;
int k4;
float k5;



float gyroangledt;
float angle;
float anglerads;
float balance_torque;
float softstart;

float cur_speed;
// Needs to know time between each measurement
//so it can then work out angle it has turned through since the last measurement - so it can know angle of tilt from vertical.
float cycle_time = 0.01;

float forwardback;
float leftright;


float Balance_point;
float balancetrim;

int balleft;
int balright;

float a0, a1, a2, a3, a4, a5, a6;  //Sav Golay variables. The TOBB bot describes this neat filter for accelerometer called Savitsky Golay filter.
//More on this plus links on my website.

int i;
int j;
int tipstart;

signed char Motor1percent;
signed char Motor2percent;

//analog inputs. Wire up the IMU as in my Instructable and these inputs will be valid.
int leftrightPin = 5;//left right joystick voltage fron nunchuck joystick
int accelPin = 4; //pin 4 is analog input pin 4. z-acc to analog pin 4
int gyroPin = 3; //pin 3 is analog balance gyro input pin. Y-rate to analog pin 3
int steergyroPin = 2; //steergyro analog input pin 2. x-rate to analog pin 2. This gyro allows software to "resist" sudden turns (i.e. when one wheel hits small object)
//I have just used the low res output from this gyro.
int forwardbackPin = 1;  //analog voltage from nunchuck joystick
int hiresgyroPin = 0; //hi res balance gyro input is analog pin 0. Y-rate 4.5 to analog pin 0
//The IMU has a low res and a high res output for each gyro. Low res one used by software when tipping over fast and higher res one when tipping gently.
//digital inputs
int deadmanbuttonPin = 9;  // deadman button is digital input pin 9

//digital outputs
int oscilloscopePin = 8;  //oscilloscope pin is digital pin 8 so you can work out the program loop time (currently about 5.5millisec)

#include <Wire.h>
#include "Kalman.h"

Kalman kalmanX;
Kalman kalmanY;

uint8_t IMUAddress = 0x68;

/* IMU Data */
int16_t accX;
int16_t accY;
int16_t accZ;
int16_t gyroX;
int16_t gyroY;

double accXangle; // Angle calculate using the accelerometer
double accYangle;

double kalAngleX; // Calculate the angle using a Kalman filter
double kalAngleY;

uint32_t timer;



void setup()                    // run once, when the sketch starts
{
  
  Serial.begin(115200);
  Wire.begin();  
  i2cWrite(0x6B,0x00); // Disable sleep mode  
  if(i2cRead(0x75,1)[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("MPU-6050 with address 0x"));
    Serial.print(IMUAddress,HEX);
    Serial.println(F(" is not connected"));
    while(1);
  } 
  if(i2cRead(0x75,1)[0] = 0x68) { 
    Serial.print(F("MPU-6050 with address 0x"));
    Serial.print(IMUAddress,HEX);
    Serial.println(F(" is connected"));
  }
  kalmanX.setAngle(180); // Set starting angle
  kalmanY.setAngle(180);
  timer = micros();
  
  
  initSabertooth( );
    //analogINPUTS
  pinMode(accelPin, INPUT);
  pinMode(gyroPin, INPUT);
  pinMode(steergyroPin, INPUT);
  pinMode(leftrightPin, INPUT);
  pinMode(forwardbackPin, INPUT);
  
  pinMode(hiresgyroPin, INPUT);
  
  //digital inputs
  pinMode(deadmanbuttonPin, INPUT);
  
  
  //digital outputs
  pinMode(oscilloscopePin, OUTPUT);
  
      
    
   Serial.println("Leave tilted and STILL after turning on/resetting the arduino, count to 10, then slowly bring it level");

  
}








void sample_inputs()  {
  gyrosum = 0;
  steersum = 0;
  hiresgyrosum = 0;
  
  uint8_t* data = i2cRead(0x3B,14);  
  accX = ((data[0] << 8) | data[1]);
  accY = ((data[2] << 8) | data[3]);
  accZ = ((data[4] << 8) | data[5]);  
  gyroX = ((data[8] << 8) | data[9]);
  gyroY = ((data[10] << 8) | data[11]);

  
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
  accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;    
  
  double gyroXrate = (double)gyroX/131.0;
  double gyroYrate = -((double)gyroY/131.0);

  //kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter
  //kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
  timer = micros();

  gyroXrate = map(gyroXrate, -32768, 32768, 0, 1023);
  gyroYrate = map(gyroYrate, -32768, 32768, 0, 1023);
  
  accZ = map(accZ, -32768, 32768, 0,1023);//map the values because it was an analog IMU being used originally???

  accraw = accZ;//analogRead(accelPin); //accelPin is z-acc//read the accelerometer pin (0-1023)
  
  //Take a set of 7 readings very fast
  for (j=0; j<7; j++) {
       adc1 = gyroYrate;//analogRead(gyroPin);//gyroPin is Yrate
       adc4 = gyroXrate;//analogRead(steergyroPin);//steergyroPin is Xrate
       adc5 = gyroYrate;//analogRead(hiresgyroPin);//hiresgyroPin is Yrate 4.5
       gyrosum = (float) gyrosum + adc1; //sum of the 7 readings
       steersum = (float) steersum + adc4; //sum of the 7 readings
       hiresgyrosum = (float)hiresgyrosum +adc5; //sum of the 7 readings
                       }
   
   
  //Potentiometer sends a voltage of 2.5V to the forwardbackPin (analog input pin 1) at mid point, 0V to adjust balance point one way, 5V to adjust balance point the other way.
 // Another potentiometer set up to send voltage to leftrightPin (analog input 5) where 2.5V is dead ahead.
//I actually have hacked a Wii Nunchuck and ripped out all electronics apart from the two 10K potentiometers on the thunb joystick, onto which I have soldered 3 new wires each
// The good thing about these pots is that they self-centre at about 2.5V if wired up right.
//I can use the forward/back pot to fine tune balance position of board, and the left-right pot to steer by sending their output voltages back to 2 analog inputs
//on the arduino.
   
                       
  k1 = analogRead(forwardbackPin);
  k5 = analogRead(leftrightPin);
  

  k4 = digitalRead(deadmanbuttonPin);
  
   
  
  // Savitsky Golay filter for accelerometer readings. It is better than a simple rolling average which is always out of date.
  // SG filter looks at trend of last few readings, projects a curve into the future, then takes mean of whole lot, giving you a more "current" value - Neat!
  // Lots of theory on this on net.
  a0 = a1;
  a1 = a2;
  a2 = a3;
  a3 = a4;
  a4 = a5;
  a5 = a6;
  a6 = (float) accraw;
  
  accsum = (float) ((-2*a0) + (3*a1) + (6*a2) + (7*a3) + (6*a4) + (3*a5) + (-2*a6))/21; 
  //accsum isnt really a "sum" (it used to be once), 
  //now it is the accelerometer value from the rolling SG filter on the 0-1023 scale
  
  digitalWrite(oscilloscopePin, HIGH);  //puts out signal to oscilloscope
  
  
  //XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
  SteerCorrect = 0; //NO STEERING FOR NOW AS DEBUGGING
  SteerValue = 512;
  //XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
  
  
 
  //ACCELEROMETER notes for the 5 d of f Sparfun IMU I have used in my Instructable:
//300mV (0.3V) per G i.e. at 90 degree angle
//Supply 3.3V is OK from Arduino NOT the 5V supply. Modern Arduinos have a 3.3V power out for small peripherals like this.
//Midpoint is 1.58 Volts when supply to IMU is 3.3V i.e. 323 on the 0-1024 scale when read from a 0-5V Arduino analog input pin 
//not the 512 we are all perhaps more used to with 5V powered accel and gyro systems.
//testing with voltmeter over 0-30 degree tilt range shows about 5.666mV per degree. Note: Should use the Sin to get angle i.e. trigonometry, but over our small 
//tilt angles (0-30deg from the vertical) the raw value is very similar to the Sin so we dont bother calculating it.
// 1mv is 1024/5000 = 0.2048 steps on the 0-1023 scale so 5.666mV is 1.16 steps on the 0-1023 scale

//***********ADJUST THE VALUE OF 338 FOR YOUR BOARD UNTIL IT BALANCES EXACTLY LEVEL WHEN TIPSTART KICKS IN (get the correct value from the IMU tester routine) ***************
  x_accdeg = (float)((accsum - (balancetarget + balancetrim))* 0.862); //approx 1.16 steps per degree so divide by 1.16 i.e. multiply by 0.862. 
 
  if (x_accdeg < -72) x_accdeg = -72; //rejects silly values to stop it going berserk!
  if (x_accdeg > 72) x_accdeg = 72;
  
  
  
  
  
 //GYRO NOTES: 
 //Low resolution gyro output: 2mV per degree per sec up to 500deg per sec. 5V = 1024 units on 0-1023 scale, 1Volt = 204.8 units on this scale. 
 //2mV = 0.41 units = 1deg per sec
 // Hi res gyro output pin(from the same gyro): 9.1mV per degree per sec up to 110deg per sec on hires input. 5V = 1024 units on 0-1023 scale, 1Volt = 204.8 units on this scale. 
 //9.1mV = 1.86 units = 1 deg per sec
  
  //Low res gyro rate of tipping reading calculated first
  gangleratedeg = (float)(((gyrosum/7) - g)*2.44); //divide by 0.41 for low res balance gyro i.e. multiply by 2.44
  
  if (gangleratedeg < -450) gangleratedeg = -450; //stops crazy values entering rest of the program
  if (gangleratedeg > 450) gangleratedeg = 450;
  
  
  
  
  
  //..BUT...Hi res gyro ideally used to re-calculate the rate of tipping in degrees per sec, i.e. use to calculate gangleratedeg IF rate is less than 100 deg per sec
  if (gangleratedeg < 100 && gangleratedeg > -100) {
      gangleratedeg = (float)(((hiresgyrosum/7) - t)*0.538); //divide by 1.86 i.e. multiply by 0.538
        if (gangleratedeg < -110) gangleratedeg = -110;
        if (gangleratedeg > 110) gangleratedeg = 110;
                           }
                       
  
  digitalWrite(oscilloscopePin, LOW);  //cuts signal to oscilloscope pin so we have one pulse on scope per cycle of the program so we can work out cycle time.

  
  //Key calculations. Gyro measures rate of tilt gangleratedeg in degrees. We know time since last measurement is cycle_time (5.5ms) so can work out much we have tipped over since last measurement
  //What is gyroscalingfactor variable? Strictly it should be 1. However if you tilt board/IMU to a fixed angle of say 10 degrees, with computer attached, then "angle" displayed on screen 
  //should be 10 (mainly due to the gyro data), if you hold it at 10 degrees, then accel will slowly fine tune the value. 
  //However, if you tilt it, and it reads 15 then slowly decreases back to 10, this means gyro is overreading, i.e. the "gyroscalingfactor" needs reducing slightly.
  //If you tilted it to 10 degree angle and readout gace angle of 7 which then slowly correcte to 10 with time, the gyro would be underrreading and the "gyroscalingfactor" 
  //needs to be increased slightly.

  //By trial and error, I have a gyroscalingfactor value of 2.2 at present.
  //experiment with this variable and see how it behaves. 
  gyroangledt = (float) gyroscalingfactor * cycle_time * gangleratedeg;
  gangleraterads = (float) gangleratedeg * 0.017453; //convert to radians - just a scaling issue from history
  //angle = (float) ((1-aa) * (angle + gyroangledt)) + (aa * x_accdeg);
  angle = (float) ((1-aa) * (angle + gyroangledt)) - (aa * x_accdeg); //I use a minus here as eccl is wrong way around relative to the gyro
  //aa allows us to feed a bit (0.5%) of the accelerometer data into the angle calculation
  //so it slowly corrects the gyro (which drifts slowly with tinme remember). Accel sensitive to vibration though so aa does not want to be too large.
  //this is why these boards do not work if an accel only is used. We use gyro to do short term tilt measurements because it is insensitive to vibration
  //the video on my instructable shows the skateboard working fine over a brick cobbled surface - vibration +++ !
  anglerads = (float) angle * 0.017453; //converting to radians again a historic scaling issue from past software
  
  
  
  
  
  balance_torque = (float) (4.5 * anglerads) + (0.5 * gangleraterads); //power to motors (will be adjusted for each motor later to create any steering effects
  //balance torque is motor control variable we would use even if we just ahd one motor. It is what is required to make the thing balance only.
  //the values of 4.5 and 0.5 came from Trevor Blackwell's segway clone experiments and were derived by good old trial and error
  //I have also found them to be about right
  //We set the torque proportionally to the actual angle of tilt (anglerads), and also proportional to the RATE of tipping over (ganglerate rads)
  //the 4.5 and the 0.5 set the amount of each we use - play around with them if you want.
  //Much more on all this, PID controlo etc on my website
 
  
  //level = (float)(balance_torque + cur_speed) * overallgain;  
  level = (float)balance_torque * overallgain;  //TEMP You can omit cur speed term during testing while just getting it to initially balance if you want to
  //avoids confusion
  
  
}

void set_motor()   {
  unsigned char cSpeedVal_Motor1 = 0;
  unsigned char cSpeedVal_Motor2 = 0;
  
  level = level * 200; //changes it to a scale of about -100 to +100
  if (level < -100) {level = -100;}
  if (level > 100) {level = 100;}
  
  
  /*
  Steer = (float) SteerValue - SteerCorrect;  //at this point is on the 0-1023 scale 
  //SteerValue is either 512 for dead ahead or bigger/smaller if you are pressing steering switch left or right
  //SteerCorrect is the "adjustment" made by the second gyro that resists sudden turns if one wheel hits a small object for example.
  Steer = (Steer - 512) * 0.19;   //gets it down from 0-1023 (with 512 as the middle no-steer point) to -100 to +100 with 0 as the middle no-steer point on scale
  */
  
  //XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
  Steer = 0;  //STEERING DISABLED AS DEBUGGING FOR NOW  
  //XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
  
//set motors using the simplified serial Sabertooth protocol (same for smaller 2 x 5 Watt Sabertooth by the way) 
                
 Motor1percent = (signed char) level + Steer;
 Motor2percent = (signed char) level - Steer;
 
 /*
 if (Motor1percent > 100) Motor1percent = 100;
 if (Motor1percent < -100) Motor1percent = -100;
 if (Motor2percent > 100) Motor2percent = 100;
 if (Motor2percent < -100) Motor2percent = -100;
 */
 
 if (Motor1percent > motorpercentlimit) Motor1percent = motorpercentlimit;
 if (Motor1percent < -motorpercentlimit) Motor1percent = -motorpercentlimit;
 if (Motor2percent > motorpercentlimit) Motor2percent = motorpercentlimit;
 if (Motor2percent < -motorpercentlimit) Motor2percent = -motorpercentlimit;
 
 
 //if not pressing deadman button on hand controller - cut everything
   if (k4 < 1) { 
    level = 0;
    Steer = 0;
    Motor1percent = 0;
    Motor2percent = 0;
               }
 
 cSpeedVal_Motor1 = map (Motor1percent,
                         -100,
                         100,
                         SABER_MOTOR1_FULL_REVERSE,
                         SABER_MOTOR1_FULL_FORWARD);
                         
 cSpeedVal_Motor2 = map (Motor2percent,
                         -100,
                          100,
                         SABER_MOTOR2_FULL_REVERSE,
                         SABER_MOTOR2_FULL_FORWARD);
                         
  SaberSerial.write (cSpeedVal_Motor1);
  SaberSerial.write (cSpeedVal_Motor2);
 //Serial.print(cSpeedVal_Motor1);
 //Serial.print("  ");
 //Serial.println(cSpeedVal_Motor2);
 
  
}






void loop ()   {
  
  
  
  tipstart = 0;
  overallgain = 0;
  cur_speed = 0;
  level = 0;
  Steer = 0;
  balancetrim = 0;
  
  //Tilt board one end on floor. Turn it on. This 200 loop creates a delay while you finish turning it on and let go i.e. stop wobbling it about
  //as now the software will read the gyro values when there is no rotational movement to find the zero point for each gyro. I could have used a simple delay command.
  for (i=0; i<200; i++) {
    sample_inputs();
                         }
                         
    //now you have stepped away from baord having turned it on, we get 7 readings from each gyro (and a hires reading from the balance gyro)                     
    g = (float) gyrosum/7;  //gyro balance value when stationary i.e. 1.35V
    s = (float) steersum/7;  //steer gyro value when stationary i.e. about 1.32V
    t = (float) hiresgyrosum/7; //hiresgyro balance output when stationary  i.e. about 1.38V
    
    //we divide sum of the 7 readings by 7 to get mean for each
  
  
//tiltstart routine now comes in. It is reading the angle from accelerometer. When you first tilt the board past the level point
//the self balancing algorithm will go "live". If it did not have this, it would fly across the room as you turned it on (tilted)!
         Serial.println("Waiting for you to slowly bring it level so tipstart engages");
    while (tipstart < 5) {
        for (i=0; i<10; i++) {
                  sample_inputs();
                  
                             }
  //x_accdeg is tilt angle from accelerometer in degrees                      
              if (x_accdeg < -2 || x_accdeg > 2) { //*****ADJUST THESE LIMITS TO SUIT YOUR BOARD SO TILTSTART KICKS IN WHERE YOU WANT IT TO*******
              //MY IMU IS NOT QUITE VERTICALLY MOUNTED
                                  tipstart = 0;
                                  overallgain = 0;
                                  cur_speed = 0;
                                  level = 0;
                                  Steer = 0;
                                  balancetrim = 0; 
                                                   }
              else {
                     tipstart = 5;
                    }   
      

  
      
       
    }

Serial.println("Tipstart active, it should self balance now - keep deadman pressed of course");
Serial.println("Hold it level at some point - wheels off ground if necessary,");
Serial.println("and WRITE DOWN value of X_accdeg showing on serial view window");


overallgain = 0.3;  //softstart value. Gain will now rise to final of 0.5 at rate of 0.005 per program loop. 
//i.e. it will go from 0.3 to 0.5 over the first 4 seconds after tipstart has been activated


angle = 0;
cur_speed = 0;
Steering = 512;
SteerValue = 512;
balancetrim = 0;

//end of tiltstart code. If go beyond this point then machine is active
//main balance routine, just loops forever. Machine is just trying to stay level. You "trick" it into moving by tilting one end down
//works best if keep legs stiff so you are more rigid like a broom handle is if you are balancing it vertically on end of your finger
//if you are all wobbly, the board will go crazy trying to correct your own flexibility.
//NB: This is why a segway has to have vertical handlebar otherwise ankle joint flexibility in fore-aft direction would make it oscillate wildly.
//NB: This is why the handlebar-less version of Toyota Winglet still has a vertical section you jam between your knees.
while (1) {
  
  sample_inputs();
  
  set_motor();
  
  //XXXXXXXXXXXXXXXXXXXXX loop timing control keeps it at 100 cycles per second XXXXXXXXXXXXXXX
   lastLoopUsefulTime = millis()-loopStartTime;
   if (lastLoopUsefulTime < STD_LOOP_TIME) {
                                delay(STD_LOOP_TIME-lastLoopUsefulTime);
                                            }
   lastLoopTime = millis() - loopStartTime;
   loopStartTime = millis();   
   //XXXXXXXXXXXXXXXXXXXXXX end of loop timing control XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX   



  
   serialOut_timing();//optional displays loop time on screen (first digit is time loop takes to run in millisec, 
   
   //second digit is final time for loop including the variable added delay to keep it at 100Hz
 
 //XXXXXXXXXXXXXXXXXXXX softstart function: board a bit squishy when you first bring it to balanced point, 
 //then ride becomes firmer over next 4 seconds as value for overallgain increases from starting value of 0.3 to 0.5 we have here 
   if (overallgain < overallgaintarget) {
       overallgain = (float)overallgain + 0.005;
                          }
   if (overallgain > overallgaintarget) {overallgain = overallgaintarget;}
 //XXXXXXXXXXXXXXX end of softstart code XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
 
  
           }
           
           
  }
  
  
  
  
  
  
  void serialOut_timing(){
   static int skip=0;
   
   if (skip++==5) { //display every 500ms (at 100Hz)
     skip = 0;
     Serial.print(lastLoopUsefulTime); Serial.print(",");
     Serial.print(lastLoopTime); Serial.println("\\n");
     
     
      //XXXXXXXXXXXXXX put any variables you want printed to serial window in here XXXXXXXXXXXXXXXXXXXXX
    //Serial.print("Motor1percent:");
    //Serial.print(Motor1percent);
    //Serial.print("   Motor2percent:");
    //Serial.print(Motor2percent);
    //Serial.print("balancegyroDegrees:");
    //Serial.print(gangleratedeg);
    //Serial.print("  X-accdeg:");
    //Serial.print(x_accdeg);
    //Serial.print("  overallAngleofTilt:");
    //Serial.print(angle);
    //Serial.print(" accsum: ");
    //Serial.println(accsum);
    
    //Serial.print("   Overallgain:");
    //Serial.println(overallgain);
    //Serial.print("   level:");
    //Serial.println(level);   




     

      //XXXXXXXXXXXXXX end of watch values in serial window XXXXXXXXXXXXXXXXXXXXXXXXXXX
                    }
                            }
                            
                            
                            void i2cWrite(uint8_t registerAddress, uint8_t data){
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.write(data);
  Wire.endTransmission(); // Send stop
}
uint8_t* i2cRead(uint8_t registerAddress, uint8_t nbytes) {
  uint8_t data[nbytes];  
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.endTransmission(false); // Don't release the bus
  Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading
  for(uint8_t i = 0; i < nbytes; i++)
    data[i] = Wire.read();
  return data;
}

You code you sent me

// Define these variables
int16_t accY;
int16_t accZ;
int16_t gyroX;
unsigned long timer;

// Put this in your setup()
Wire.begin();  
i2cWrite(0x6B,0x00); // Disable sleep mode  
if(i2cRead(0x75,1)[0] != 0x68) { // Read "WHO_AM_I" register
  Serial.println(F("Error reading sensor"));
  while(1); 
}

// And this in your loop
uint8_t* data = i2cRead(0x3D,8);  
accY = ((data[0] << 8) | data[1]);
accZ = ((data[2] << 8) | data[3]);
gyroX = ((data[6] << 8) | data[7]);

double accAngle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
double gyroRate = (double)gyroX/131.0;

pitch = kalman.getAngle(accAngle, gyroRate, (double)(micros()-timer)/1000000);
timer = micros();


// You also need the following functions
void calibrate() {
  uint8_t* data = i2cRead(0x3D,4);  
  accY = ((data[0] << 8) | data[1]);
  accZ = ((data[2] << 8) | data[3]);
  double accAngle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;

  if(accAngle < 180) // Check which side is lying down
    kalman.setAngle(90); // It starts at 90 degress and 270 when facing the other way
  else
    kalman.setAngle(270);
}

void i2cWrite(uint8_t registerAddress, uint8_t data){
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.write(data);
  Wire.endTransmission(); // Send stop
}
uint8_t* i2cRead(uint8_t registerAddress, uint8_t nbytes) {
  uint8_t data[nbytes];  
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.endTransmission(false); // Don't release the bus
  Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading
  for(uint8_t i = 0; i < nbytes; i++)
    data[i] = Wire.read();
  return data;
}