The code because it wouldnt fit in one post.

`#include <SoftwareSerial.h>`

//Digital pin 13 is serial transmit pin to sabertooth

#define SABER_TX_PIN 13

#define SABER_RX_PIN 12

//set baudrate to match sabertooth dip settings

#define SABER_BAUDRATE 9600

#define SABER_MOTOR1_FULL_FORWARD 255

#define SABER_MOTOR1_FULL_REVERSE 0

#define SABER_ALL_STOP 127

SoftwareSerial SaberSerial (SABER_RX_PIN, SABER_TX_PIN );

void initSabertooth (void) {

pinMode ( SABER_TX_PIN, OUTPUT );

SaberSerial.begin( SABER_BAUDRATE );

SaberSerial.write(127); //kill motors when first switched on

}

float level=0;

float aa = 0.005;

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 ti = 2.2;

//float k1;

//int k2;

//int k3;

int k4;

//float k5;

float overallgain;

float gyroangledt;

float angle;

float anglerads;

float balance_torque;

float softstart;

float cur_speed;

float cycle_time = 0.008;

//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;

//ANALOG INPUTS. Wire up the IMU as in my Instructable and these inputs will be valid.

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 low-res input pin. Y-rate to analog pin 3

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 INPUT

int deadmanbuttonPin = 9; // deadman button is digital input pin 9

//DIGITAL OUTPUT

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

void setup() // run once, when the sketch starts

{

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.begin(9600); // HARD wired Serial feedback to PC for debugging in Wiring

}

void sample_inputs() {

gyrosum = 0;

// steersum = 0;

hiresgyrosum = 0;

accraw = analogRead(accelPin); //read the accelerometer pin (0-1023)

//Take a set of 7 readings very fast

for (j=0; j<7; j++) {

adc1 = analogRead(gyroPin);

adc5 = analogRead(hiresgyroPin);

gyrosum = (float) gyrosum + adc1; //sum of the 7 readings

hiresgyrosum = (float)hiresgyrosum +adc5; //sum of the 7 readings

}

k4 = digitalRead(deadmanbuttonPin); //this is needed - if you let go the motors both stop for your own safety

overallgain = 0.2;//now fixed in the software NOTE, if your board is too "jumpy" reduce this value a little or vice versa

//ACCELEROMETER READINGS

// 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

//NOTE: FINE-TUNE THE VALUE OF 338 BELOW USING METHOD DESCRIBED IN THE IMU TESTER PROCEDURE It sets the balance point

x_accdeg = (float)((accsum - (336 + balancetrim))*0.862); //approx 1.16 steps per degree so divide by 1.16 i.e. multiply by 0.862

// x_accdeg = (float)((accsum - 350) * 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;

//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;

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.

gyroangledt = (float) ti * 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);

anglerads = (float) angle * 0.017453; //converting to radians again a historic scaling issue from past software

//debugging

//Serial.print("Angle = ");

//Serial.println(angle);

balance_torque = (float) (4.5 * anglerads) + (0.5 * gangleraterads);

cur_speed = (float) (cur_speed + (anglerads * 10 * cycle_time)) * 0.999;

level = (float)balance_torque * overallgain;

}

void set_motor() {

unsigned char cSpeedVal_Motor1 = 0;

level = level * 200; //changes it to a scale of about -100 to +100

Steer = 0;

//All steering is currently removed from this program.

Motor1percent = (signed char) level + Steer;

if (Motor1percent > 100) Motor1percent = 100;

if (Motor1percent < -100) Motor1percent = -100;

//if not pressing deadman button on hand controller - cut everything

if (k4 < 1) {

level = 0;

Steer = 0;

Motor1percent = 0;

}

cSpeedVal_Motor1 = map (Motor1percent,

-100,

100,

SABER_MOTOR1_FULL_REVERSE,

SABER_MOTOR1_FULL_FORWARD);

SaberSerial.write (cSpeedVal_Motor1);

delay(4); //4ms delay

}

void loop () {

tipstart = 0;

overallgain = 0;

cur_speed = 0;

level = 0;

Steer = 0;

balancetrim = 0;

for (i=0; i<200; i++) {

sample_inputs();

}

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

//tiltstart routine now comes in.

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) {

tipstart = 0;

overallgain = 0;

cur_speed = 0;

level = 0;

Steer = 0;

balancetrim = 0;

}

else {

tipstart = 5;

}

}

overallgain = 0.3; //THIS JUST SETS IT THE FIRST TIME

angle = 0;

cur_speed = 0;

balancetrim = 0;

//end of tiltstart code.

sample_inputs();

set_motor();

}

}