WARNING: This alghorith is property of Vittorio Margherita. The author is not responsible for any damage caused by changes to the present software nor for the present software installed on any hardware not properly controlled. Be carefull before to connect the controller running this software to any mechnanical equipment. It could cause damage to persons or to things (this is not a game)

Here some initial definitions

    #define ACCEL_GAIN				18.0 
    #define GYRO_GAIN				5.0
    float ti_constant = 3;

    const float ANGLE_GAIN = 1.30; //30% increase in angle measurement.
                         

The following constant 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 to experiment.
too high though and the board may become too vibration sensitive.

    
    float aa_constant = 0.005; 
                         

Here some definition to work with Intel curie MPU

    #define MPU_INT 0 //is on pin 2


    //CurieIMUClass mpu;
    //MPU6050 mpu;   // AD0 low = 0x68

    // MPU control/status vars
    bool dmpReady = false;  // set true if DMP init was successful
    uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
    uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
    uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
    uint16_t fifoCount;     // count of all bytes currently in FIFO
    uint8_t fifoBuffer[64]; // FIFO storage buffer
                         

In 2009 Sebastian Madgwick developed an IMU and AHRS sensor fusion algorithm as part of his Ph.D research at the University of Bristol.This algorithm was initially used to balance images from a camera but once distributed on the internet the success was so big that it has been used for many pourpose. It is able to transformate data coming from an mpu in 3D position and will be used in our algorithm to find the position of our platform in the space.

So lets declare a Madgwick filter (and other useful variables)

    Madgwick filter;
    // orientation/motion vars
    //Quaternion q;           // [w, x, y, z]         quaternion container
    //VectorFloat gravity;    // [x, y, z]            gravity vector
    int16_t gyro[3];        // [x, y, z]            gyro vector
    float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
    
    float angle_Y, angular_rate_Y, angular_rate_X;
    float angle_X, angle_Z, angular_rate_Z;
    bool blinkState = false;
    int previousDirection = 99;
    
    
    volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
    float cycle_time = 0.01; //seconds per cycle - currently 10 milliseconds per loop of the program.  
    // Need to know it as gyro measures rate of turning. 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.

    int STD_LOOP_TIME = 9; //9= 10mS loop time // code that keeps loop time at 10ms per cycle of main program loop 
    int lastLoopTime = STD_LOOP_TIME;
    int lastLoopUsefulTime = STD_LOOP_TIME;

                         

Now we will declare a callbackfunction to use once the interrupt has been generated

    void dmpDataReady()
    {
        mpuInterrupt = true;
    }
                         

Now we will initialize the curie IMU

    CurieIMU.begin();
    CurieIMU.setGyroRate(25);
    CurieIMU.setAccelerometerRate(25);
    
    CurieIMU.setAccelerometerRange(2);
    CurieIMU.setGyroRange(250);
    
    CurieIMU.initialize();
    
    if (!CurieIMU.testConnection()) {
        SerialTransfer.SendDisplayString((char*)"Error connecting the Gyro\r", 0);
        while (1)		//STOP the Robot
            ;
    }
    
    
    if (CalibrateOffset == false)  {
        
        CurieIMU.setXGyroOffset(10);
        CurieIMU.setYGyroOffset(7);
        CurieIMU.setZGyroOffset(14);
        CurieIMU.setZAccelOffset(900); // 1688 factory default for  test chip
    }
    
    else
    {
    
            
        CurieIMU.autoCalibrateGyroOffset();
        CurieIMU.autoCalibrateXAccelOffset(0);
        CurieIMU.autoCalibrateYAccelOffset(0);
        CurieIMU.autoCalibrateZAccelOffset(1);
        
    
    }
    // make sure it worked (returns 0 if so)
    
    
    // enable Arduino interrupt detection
    CurieIMU.attachInterrupt(dmpDataReady);
    
    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    SerialTransfer.SendDisplayString((char*)"DMP: Interrupt attached\r",0);
    dmpReady = true;
        
    // get expected DMP packet size for later comparison
    packetSize = CurieIMU.getFIFOCount();
                             

At the beginning of the Arduino loop (So at the end of the initialization phase, read the accel/gyro multiple times to get an average baseline value. This will be subtracted from the current value in the balance loop.

                    
    for (j=0; j<7; j++) {
                   read_accel_gyro();
                   initial_angular_rate_Y_sum=(float) initial_angular_rate_Y_sum + angular_rate_Y; //sum of the 7 readings of front/back tilt gyro
        initial_angular_rate_X_sum = (float) initial_angular_rate_X_sum  + angular_rate_X; //sum of the 7 readings left/right steer gyro
        //delay to do accel/gyro reads.
        delay (10); //10ms
    }

    initial_angular_rate_Y = (float) initial_angular_rate_Y_sum/7;  //initial front/back tilt gyro
    initial_angular_rate_X = (float) initial_angular_rate_X_sum/7;  //initial left/right steer gyro

    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
                            

Now it is the time of the loop function. The loop function is very easy. It simply contains 3 main functions. These function are executed in a cycle 100 times per second.
The execution time is really important because it differs based on the weight of the feature to balance. In our case it myst be very sensitive.
Don't forget that we are moving a platform and a person.

    read_accel_gyro(); // Read the gyro

    do_calculations(); //do math

    set_motor(); //set motors up

    //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();
    if (overallgain < 0.5) {
        overallgain = (float)overallgain + 0.005;
    }
    if (overallgain > 0.5) {
        overallgain = 0.5;
    }
                             

And now, lets go to the two most significant functions.
The first one read accell values and the second one performs calculation to move properly the two motors. You have also a section where we calculate the steering values

void read_accel_gyro()  {     
    
    int16_t ax, ay, az;
    int factor = 800;		//This factor is used to reduce Gyro results
    
    if (!dmpReady) return; // if programming failed, don't try to do anything
    while (!mpuInterrupt && fifoCount < packetSize) {
    }
    
    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    
    
    //To validate if this is the right function to call to validate the interrupt
    mpuIntStatus = CurieIMU.getIntStatus0();
    
    // get current FIFO count
    fifoCount = CurieIMU.getFIFOCount();
    
    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024)
        {
        // reset so we can continue cleanly
        CurieIMU.resetFIFO();
        SerialTransfer.SendDisplayString((char*)"FIFO overflow!\r",0);

        // otherwise, check for DMP data ready interrupt (this should happen frequently)
        }
    else if (mpuIntStatus & 0x02)
        {
        // wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) fifoCount = CurieIMU.getFIFOCount();

        // read a packet from FIFO
        CurieIMU.getFIFOBytes(fifoBuffer, packetSize);

        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;
    
        //Get sensor data
        
        CurieIMU.getMotion6(&ax, &ay, &az, &gyro[0], &gyro[1], &gyro[2]);
        filter.updateIMU(gyro[0] / factor, gyro[0] / factor, gyro[0] / factor, ax, ay, az);
        ypr[0] = filter.getYaw();
        ypr[1] = filter.getPitch();
        ypr[2] = filter.getRoll();
    
        // angle and angular rate
        angle_X = ypr[0]* RAD_TO_DEG;             // not used...0 is center of gravity offset
        angle_Y = ypr[1]* RAD_TO_DEG;             // Accel for Tilt, 0 is center of gravity offset
        angle_Z = ypr[2]* RAD_TO_DEG;             // not used...0 is center of gravity offset
        angular_rate_X = ((double)gyro[0]/131.0); // Gyro for steering, in degs/sec.
        angular_rate_Y = ((double)gyro[1]/131.0); // Gyro for tilt, in degs/sec.
        angular_rate_Z = ((double)gyro[2]/131.0); // Gyro for X, in degs/sec.
        
        angular_rate_X = angular_rate_X * RAD_TO_DEG; // Gyro for steering, in degs/sec.
        angular_rate_Y = angular_rate_Y * RAD_TO_DEG; // Gyro for tilt, 
        angular_rate_Z = angular_rate_Z * RAD_TO_DEG; // Gyro for X
        
        } //end else if (mpuIntStatus & 0x02)
    }//end of read_accel_gyro()  
    
    
    void do_calculations()  {     
                        
    SteerLeftPin = digitalRead(STEERINGLEFTPIN);
    SteerRightPin = digitalRead(STEERINGRIGHTPIN);
    
    DeadManPin = 0;
    
    balancelForward = digitalRead(BALANCEFORWARDPIN);
    balancelBackward = digitalRead(BALANCEBACKWARDPIN);
        
    if (balancelForward == 0) balancetrim = balancetrim - 0.04; //if pressing balance point adjust switch 
    //then slowly alter the balancetrim variable by 0.04 per loop of the program 
    //while you are pressing the switch
    if (balancelBackward == 0) balancetrim = balancetrim + 0.04; //same again in other direction
    if (balancetrim < -30) balancetrim = -30; //stops you going too far with this
    if (balancetrim > 30) balancetrim = 30; //stops you going too far the other way
        
    // 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.
    gv0 = gv1;
    gv1 = gv2;
    gv2 = gv3;
    gv3 = gv4;
    gv4 = gv5;
    gv5 = gv6;
    gv6 = (float) angle_Y; //from digital gyro accelerometer  IDH
    
    //SG_filter_result is the accelerometer value from the rolling SG filter on the 0-1023 scale
    SG_filter_result = (float) ((-2*gv0) + (3*gv1) + (6*gv2) + (7*gv3) + (6*gv4) + (3*gv5) + (-2*gv6))/21; 
    

    //*****START OF STEERING SECTION
    
    //Used to adjust steering from drift
    gangleratedeg2 = angular_rate_X - initial_angular_rate_X;  //IDH subtract curent value from inital 
    //value to get delta.
    
    if (SteerLeftPin == 1 && SteerRightPin == 1){ // NO steering wanted. Use second gyro to maintain a (roughly) 
        //straight line heading (it will drift a bit).
                            
        SteerCorrect = 0; //blocks the direction stabiliser unless rate of turn exceeds -10 or +10 degrees per sec
        if (gangleratedeg2 > 10 || gangleratedeg2 < -10) {   //resists turning if turn rate exceeds 10deg per sec
        SteerCorrect = (float) 0.4 * gangleratedeg2; //vary the 0.4 according to how much "resistance" to being 
        //nudged off course you want.
        //a value called SteerCorrect is added to the steering value proportional to the rate of unwanted turning. 
        //It keeps getting
        //larger if this condition is still being satisfied i.e. still turning >10deg per sec until the change 
        //has been resisted.
        //can experiment with the value of 10. Try 5 deg per sec if you want - play around - this can probably 
        //be improved
        //but if you try to turn it fast with your hand while balancing you will feel it resisting you so it does work
        //also, when coming to a stop, one motor has a bit more friction than the other so as this motor stops 
        //first then as board
        //comes to standstill it spins round and you can fall off. This is original reason I built in this feature.
        //if motors have same friction you will not notice it so much.
        }
        SteerValue = 512;            
    }
    else { //(SteerLeftPin == 0 || SteerRightPin == 0) We DO want to steer                        
            
        //note: SteerValue of 512 is straight ahead
    
        if (SteerLeftPin == 0) {
        SteerValue = 612; //add some some right turn power. Experimentally determined.
        }                                                                
                            
        //steer the other way             
        if (SteerRightPin == 0) {
        SteerValue = 412; //add some some left turn power. Experimentally determined.
        }                                                  
                    
        SteerCorrect = 0;
    }        
    //*****END OF STEERING SECTION
    
            
    //Angle Gain
    SG_filter_result = (float) SG_filter_result * ANGLE_GAIN;
    
    // Balancetrim is front/back balance tip adjustment from switch
    // Sensor tilt number below is Determined experimentally. Bigger is more tilted forward.  
    //It needs to change if you adjust ANGLE_GAIN.
    x_accdeg = (float)((SG_filter_result - (80 + balancetrim)) * (1.0));  
    
    if (x_accdeg < -72) x_accdeg = -72; //put in range.
    if (x_accdeg > 72) x_accdeg = 72;
    
    //For digital gyro here 
    gangleratedeg = (float)(angular_rate_Y - initial_angular_rate_Y); // IDH        
    if (gangleratedeg < -110) gangleratedeg = -110;
    if (gangleratedeg > 110) gangleratedeg = 110;
        
    //Key calculations. Gyro measures rate of tilt gangleratedeg in degrees. We know time since last measurement 
    //is cycle_time (10ms) so can work out much we have tipped over since last measurement
    //What is ti variable? Strictly it should be 1. However if you tilt board, then it moves along at an angle, 
    //then SLOWLY comes back to level point as it is moving along
    //this suggests the gyro is slightly underestimating the rate of tilt and the accelerometer is correcting 
    //it (slowly as it is meant to).
    //This is why, by trial and error, I have increased ti to 3 at start of program where I define my variables.
    //experiment with this variable and see how it behaves. Temporarily reconfigure the overallgain potentiometer 
    //as an input to change ti and experiment with this variable
    //potentiometer is useful for this sort of experiment. You can alter any variable on the fly by temporarily 
    //using the potentiometer to adjust it and see what effect it has
    
    gyroangle_dt = (float) ti_constant * cycle_time * gangleratedeg; //e.g  = 3*0.01*gyro_reading
    
    gangleraterads = (float) gangleratedeg * 0.017453; //convert to radians - just a scaling issue from history
    
    //Complementary Filter.
    angle = (float) ((1-aa_constant) * (angle + gyroangle_dt)) + (aa_constant * x_accdeg);//aa=(0.005) 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 time). 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
    
    //Complementary Filter. the approximate formula to combine the accelerometer and gyroscope data is:
    //Filtered Angle = α × (Gyroscope Angle) + (1 − α) × (Accelerometer Angle)     where
    //α = τ/(τ + Δt)   and   (Gyroscope Angle) = (Last Measured Filtered Angle) + ω×Δt
    //Δt = sampling rate, τ = time constant greater than timescale of typical accelerometer noise
    
    anglerads = (float) angle * 0.017453; //converting to radians again a historic scaling issue from past software
    
    balance_torque = (float) (ACCEL_GAIN * anglerads) +  //from accelerometer
        (GYRO_GAIN * gangleraterads); //from Gyro 
    
    //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 control etc on my website
    
    cur_speed = (float) (cur_speed + (anglerads * 6 * cycle_time)) * 0.999;
    //this is not current speed. We do not know actual speed as we have no wheel rotation encoders. 
    //This is a type of accelerator pedal effect:
    //this variable increases with each loop of the program IF board is deliberately held at an angle (by rider for example)
    //So it means "if we are STILL tilted, speed up a bit" and it keeps accelerating as long as you hold it tilted.
    //You do NOT need this to just balance, but to go up a slight incline for example you would need it: if board hits incline 
    //and then stops - if you hold it
    //tilted for long eneough, it will eventually go up the slope (so long as motors powerfull enough and motor controller 
    //powerful enough)
    //Why the 0.999 value? I got this from the SeWii project code - thanks!
    //If you have built up a large cur_speed value and you tilt it back to come to a standstill, you will have to keep it 
    //tilted back even when you have come to rest
    //i.e. board will stop moving OK but will now not be level as you are tiliting it back other way to counteract 
    //this large cur_speed value
    //The 0.999 means that if you bring board level after a long period tilted forwards, the cur_speed value magically decays away 
    //to nothing and your board
    //is now not only stationary but also level!
    
    level = (float)(balance_torque + cur_speed) * overallgain;  //final overall gain = 0.5
    
} //end do_calculations
    
                             

Based on the values exctracted and calculated we can now set the value to send to the motors

    
void set_motor()   {
    
    int cSpeedVal_Motor1 = 0;
    int cSpeedVal_Motor2 = 0;
    
    level = level * 20; //changes it to a scale of about -100 to +100 works ..OK 
    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.09;   //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
    
    //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;
    
    //debug:
    if (DEBUG_FORCE_DEADMAN_SWITCH == 1) {
    DeadManPin = 0; }
    //debug:
    
    //if not pressing deadman button on hand controller - cut everything
    if (DeadManPin > 0){ 
    level = 0;
    Steer = 0;
    Motor1percent = 0;
    Motor2percent = 0;
    digitalWrite(REDLEDPIN, LOW);      //LED is red when stopped.
    digitalWrite(GREENLEDPIN, HIGH);   //LED is red when stopped.
    deadman_occured_flag = 1; //set flag to force jump to start when deadman is released.
    }//End of deadman switch release 
    else if (deadman_occured_flag == 1 ) { //deadman is pressed
    deadman_occured_flag = 0; 
    digitalWrite(REDLEDPIN, HIGH);   //LED is green when running.
    digitalWrite(GREENLEDPIN, LOW);   //LED is green when running.
    
    loop(); //start loop again to start from the beginning.
    }//End of deadman 


    if (DEBUG_DISABLE_MOTORS == 1) { //only udes for debug to keep motors off
    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);
                            
    ST.motor(1, cSpeedVal_Motor1);
    ST.motor(2, cSpeedVal_Motor2);
    
}
                             

The remaining function are based on your requirement and fantasy