Self Balancing Robot Issues Programming

Hi guys,

I am trying to make a self balancing robot using two DC motors w/ gear box and wheels, an Ardunio UNO, and an accelerometer. I am currently using the PID Library into order to control my robot, however to much attempt it isn’t working. The angle readouts from the program is not correct with what it should be and the robot doesn’t balance with the library. Another issue I’m having is with motor control it runs at 255 PWM, but the motors are slower than at other PWM like 68. I am confused, does anyone know why this is?

/********************************************************
 * PID Basic Example
 * Reading analog input 0 to control analog PWM output 3
 ********************************************************/

#include <PID_v1.h>
#include <Wire.h> //define wire library
#include <Adafruit_MotorShield.h> //define adafruit motorshield library

//Define Variables we'll be connecting to
double Setpoint, Input, Output;

//Specify the links and initial tuning parametersu
PID myPID(&Input, &Output, &Setpoint,200,50,0, DIRECT);

//Specifiy Motor Control Parameters
Adafruit_MotorShield AFMS = Adafruit_MotorShield(); //Define the motorshield as an object
Adafruit_DCMotor *myMotor = AFMS.getMotor (1); //Define motor 1
Adafruit_DCMotor *myMotor2 = AFMS.getMotor (2); //Define motor 2

///Bias Parameters
float Vb = 0;
int sampleNum = 500;

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

/////////////////////////  PID ///////////////////////

//initialize the variables we're linked to
  Input = analogRead(0);
  Setpoint = 0;


  //turn the PID on
  myPID.SetMode(AUTOMATIC);
  
 myPID. SetOutputLimits (-170,80);
 myPID. SetSampleTime (14);


/////////////////////// Vb (Counts) ///////////////////////////

//500 samples of readings Vb 
 for(int n=0;n<sampleNum;n++)
 { 
    float Accel_X = analogRead(A0); 
     Vb += Accel_X; 
  }
  
Vb = Vb/sampleNum;

//////////////////// Motor Controller ///////////////////

//turn motorcontroller on
AFMS.begin(); //Turn on the motorshield

delay(1000);

}

void loop()
{
  Input = analogRead(0);
  Input = Input - Vb;
  
  myPID.Compute();

Serial.println();
Serial.println("PWM:");
Serial.print(Output);
Serial.println();
Serial.println("Angle:");
Serial.print(Input);

    if ( Input > 0)
      {
        myMotor->setSpeed(Output);
        myMotor2->setSpeed(Output);
        myMotor->run(BACKWARD);
        myMotor2->run(BACKWARD);
      }
        else if (Input < 0)
          {
              myMotor->setSpeed(Output);
              myMotor2->setSpeed(Output);
              myMotor->run(FORWARD);
              myMotor2->run(FORWARD);
          }
 
}