PID Control for Inverted Pendulum & Sensor Issue

Hi guys,

I recently just started working on an inverted pendulum project and I am currently confused on a couple of things. The first issue that I have is that my accelometer tends to oscillate between -0.26 and 0.15 when at level. Is there any where to fix this issue via programming or did I do something wrong in my programming?

Accelometer Code:

//LAST REVISED: 03/29/2015

//This Accelometer code runs on a 10-Bit system i.e. Ardiuno UNO R3
//Accelometer: ADXL203EB is a 5 V sensor | Manual Tested Avg. Sensitivity: 0.0159 (V/Degrees) 
//Conversion factor from bits to volage is (V sensor/system bits) | (5V/1024 bits)
//Degrees is obtained from the equation: [Degree = (Vout - Vbias)/Sensitivity]

#include <Wire.h> //define wire library

//Define Accelometer Variables
const int xpin = A5; //accelometer X-AXIS connected to A5
const int ypin = A1; //accelometer Y-AXIS connected to A1 
float Accel_X; //sets accelometer X-AXIS read variable @ A5 >> enables reading 
float Accel_Y; //sets accelometer Y-AXIS read variable @ A1 >> enables reading
float Angle; //sets Angle variable to 0 >> enables redefining later on
float sum_angle = 0; //set prev_angle variable to 0 >> enables redefining later on
float final_angle =0; //set final_angle variable to 0 >> enables redefining later on

//Define Sampling Variables for Voltage Bias @ Ground Level
int sampleNum = 500; //500 samples will be conducted to determine a value
int sampleNumb = 2; //2 samples will be conducted to determine a value
float Vb = 0; //Set Voltage Bias variable to start at 0 V
int sampleTime = 10; //Sets sampleTime to 10 (milli-seconds in this case)
unsigned long time; //Sets the variable time to be multiple decimal places, non-negative

//Setup the Serial & Begin one time setup processes
void setup() {
  Serial.begin(115200); //Set Baudrate to 115200, fast refresh
  Serial.println("*Setup Begin*"); //print to indicate start of program

  //Accelometer Setup
  Serial.println(); //Space between text
  Serial.println ("Accelometer Setup Start"); //Indicate the setup of the Accelometer

  //Determine the voltage bias @ current ground level  
  Serial.println();//Space between text
  Serial.println ("Sample Voltage Bias"); //Indicate the Sampling of Vb (Voltage Bias)

  for(int n=0;n<sampleNum;n++){ //Take a total of 500 samples increasing by 1 increment
    Accel_X = analogRead(xpin); //Read values from X-AXIS port i.e. A5 --> Redfine Accel_X to value obtained
    Vb += Accel_X; //Add the readings from variable Accel_X to Vb (Voltage Bias) for each sample conducted
  }
   Serial.println();//Space between text
  Serial.println ("Vb Sampling Completed"); //Indicate end of Vb Sampling

   Vb = Vb/sampleNum; //Redefine the Voltage Bias (bits) according to sampling. (Total Voltage Bias / Sample Number) = Redefined Voltage Bias
   Vb = Vb*0.0048828125; //Convert the Voltage Bias from BITS to VOLTAGE using the factor (5/1024)

  Serial.println();//Space between text
  Serial.print("[Voltage Bias]:"); //Print "Voltage Bias:" on Serial Montior 
  Serial.print(Vb); // Print Vb value next to "Vb:"
  Serial.println();//Space between text
  Serial.println();//Space between text
  Serial.println ("*End of Setup Process*"); //Print this after setup has completed
  Serial.println();//Space between text
  Serial.println("*Accelometer Readings*"); //Print out Accelometer Readings in text

  delay(500); //delay the system for 500 milli-seconds -> allows time for user to read Vb
  }

  //Begin Looping Process for Controller
  void loop() { //Start Loop Sequence
  
    //Accelometer Reading
  for(int n=0;n<sampleNumb;n++){ //Take a total of 500 samples increasing by 1 increment
    if (millis() - time > sampleTime) //Take a smaple every 10 ms --> Total Time - Elasped Time > 10 ms --> Take Sample
      { 
  time = millis(); //Time elasped since last sample 
      Accel_X = analogRead(xpin); //Read values from A0, convert from bits->volts using conversion factor (5/1024), define them to accel_x
      Accel_X = Accel_X*0.0048828125; //Convert the readings from accelometer from bits into voltage
      Accel_X = Accel_X - Vb; //Subtract the voltage bias to voltage reading of sensor
      Angle = (Accel_X/0.0159); //Use the degree equation to determine the degree given the corrected voltage output & sensitivity
      sum_angle += Angle; //Remember the current angle for the next looping
      }
  }
  
      final_angle = sum_angle / sampleNumb; //Average the angles from the 2 samples taken in the for-loop
      Serial.println(); //Space between text
      Serial.print("Angle:"); //Print "Angle:" in the Serial Monitor
      Serial.print(final_angle); //Print the Angle variable value next to "Angle:" in Serial Monitor
 
     sum_angle = 0; //Redefine sum_angle back to 0 for next loop iteration
 
    delay (100); //Delay the system 500 ms before the next sample reading

    }

The next problem I have is programming the PID. I have looked at Kas’ posts and several others online, but I am still confused on how the motors are regulated given that I input angle into a PID controller. Given the error in my angle, which is fed into the PID and summed up I should then use this value to control the DC motors independently to adjust the robot back to 0 degrees, however I am not quite sure how to implement this.

The current list of parts I am using:

Tamiya Double Gearbox (Includes 2 DC motors):

Tamiya Wheel Set:

Arduino Uno R3:
http://arduino.cc/en/main/arduinoBoardUno

Adafruit Motor Shield V2:
https://learn.adafruit.com/adafruit-mot … o/overview

ADXL203EB Dual Axis Accelometer (alternative to IMU):
http://www.analog.com/media/en/technica … 03_203.pdf