Small scale segway prototype running

Ok after getting all the parts, just assemble up and start writing the code. I don't start the code myself from scratch, I take the a reference of the code from here http://tlb.org/scooter.html, the original code was written in C, so i changed it to arduino language... it looks much simpler with arduino.

Because the prototype is very small and I have no background in the mechanical, so this design is not the perfect design, just to prove that the algorithm and code works correctly before starting to build the human scale segway. This is my most successful build after a lot of tuning on the PD value. Note I only used PD, haven't tried the I, which is the PID control loop.

and here is the code,

#define PwmPinMotorA 10 //pin connected to motor left
#define PwmPinMotorB 11 //pin connected to motor right
#define DirectionPinMotorA 12 //pin to control the left motor forward/reverse
#define DirectionPinMotorB 13 //pin to control the right motor forward/reverse 

const int gyro = 0;
const int xpin = 1;                   // x-axis of the accelerometer

float gyro_angle = 0;
float gyro_offset = 0;
float angle = 0;
float gyro_integration = 0;
float xZero = 0;
float angle_offset=0;
float distance = 0;
float p = 3; // This value is the constant for tuning P 
float i = 0; // The I was not used in this code yet
float d = 10; //This value is the constant for tuning D
float angle_old = 0;
float angle_real = 0;

float angle_integral = 0;
float balancetorque = 0;

int pwmA = 0;
int pwmB = 0;
void setup()
{
  pinMode(PwmPinMotorA, OUTPUT);
  pinMode(PwmPinMotorB, OUTPUT);
  pinMode(DirectionPinMotorA, OUTPUT);
  pinMode(DirectionPinMotorB, OUTPUT);
  Serial.begin(115200); //initialize the usb serial command
 
  delay(1000);
  xZero = analogRead(xpin);
  
  gyro_offset += analogRead(gyro);
  gyro_offset += analogRead(gyro);
  gyro_offset += analogRead(gyro);
  gyro_offset += analogRead(gyro);
  gyro_offset += analogRead(gyro);
  gyro_offset += analogRead(gyro);
  gyro_offset += analogRead(gyro);
  gyro_offset += analogRead(gyro);

  gyro_offset = gyro_offset/8;
  
  for(int z = 0;z<300;z++)
  {
  calculatei();
  angle_offset +=angle;
  }
  
  angle_offset=angle_offset/300;
}
void loop()
{
  
  calculate();
  motor_speed();
  motor_control();
 
  Serial.print(analogRead(xpin));
  Serial.print("\t");
  Serial.print(gyro_angle);
  Serial.print("\t");
  Serial.print(angle_real);
  Serial.print("\t");
  Serial.print(pwmA);
  Serial.println();
  angle_old = angle;
 
}
void init_pos()
{
  
  angle = xZero-350;
  distance = (sin(angle*3.141592654/180))*7; 
  if (distance<0)
  {
    distance=distance*(-1);
    pwmA = distance/0.1 + 60;
    pwmB = distance/0.1 + 60;
    analogWrite(PwmPinMotorA, pwmA); 
    digitalWrite(DirectionPinMotorA, LOW); 
    analogWrite(PwmPinMotorB, pwmB); 
    digitalWrite(DirectionPinMotorB, HIGH); 
  }
  else
  {
    pwmA = distance/0.1 + 60;
    pwmB = distance/0.1 + 60;
    analogWrite(PwmPinMotorB, pwmB); 
    digitalWrite(DirectionPinMotorB, LOW);
    analogWrite(PwmPinMotorA, pwmA); 
    digitalWrite(DirectionPinMotorA, HIGH);
  
  }
}

void calculatei()
{
  float acc_reading = 0;
  acc_reading = ((analogRead(xpin)-xZero));
  gyro_angle = ((analogRead(gyro)-gyro_offset))*1/80;
  gyro_integration = gyro_angle + gyro_integration;
  angle = (gyro_integration*0.99) + (acc_reading*0.01);
  //angle = angle - angle_offset;
  gyro_integration = angle;
  distance = (sin(angle*3.141592654/180))*11;
   
}

void calculate()
{
  float acc_reading = 0;
  acc_reading = ((analogRead(xpin)-xZero));
  gyro_angle = ((analogRead(gyro)-gyro_offset))*1/80;
  gyro_integration = gyro_angle + gyro_integration;
  angle = (gyro_integration*0.98) + (acc_reading*0.02);
  angle_real = angle - angle_offset;
  gyro_integration = angle;
  distance = (sin(angle*3.141592654/180))*11;
   
}
void motor_speed()
{
  if (distance<0)
  {
    distance=distance*(-1);
  }
 
 angle_integral +=angle;
 balancetorque = (angle_real*p) + (gyro_angle*d);
 
  //pwm = (((distance/(0.044444444))/0.5)+80);
  if(balancetorque>180)
  {
    balancetorque=180;
  }
   if(balancetorque<-180)
  {
    balancetorque= (-180);
  }
  
}
void motor_control()
{

if (balancetorque<0)
{
  pwmA = (((-1)*balancetorque));
  pwmB = (((-1)*balancetorque));
analogWrite(PwmPinMotorA, pwmA); 
digitalWrite(DirectionPinMotorA, LOW); 
analogWrite(PwmPinMotorB, pwmB); 
digitalWrite(DirectionPinMotorB, HIGH);
}
if (balancetorque>=0)
{
  pwmA = (balancetorque);
  pwmB = (balancetorque);
analogWrite(PwmPinMotorB, pwmB); 
digitalWrite(DirectionPinMotorB, LOW);
analogWrite(PwmPinMotorA, pwmA); 
digitalWrite(DirectionPinMotorA, HIGH); 
}
}

Some part in the code was redundant, as I had tried many different ways to fine tune it, however I didn't remove the line after that.... but basically you just need to know that the robot was controlled using the angle of the tilt, so the most important part is the calculate() function which uses complementary filter to get the tilted angle.