Problems Coding Accelometer

hi guys,

i trying make pid control robot, accelometer angles reading @ fixed angle. accelometer readings spike -150 down -0.6, isnt if i'm trying read angle. when call readings sensor same angle on , on again, doesn't make sense...

code: [select]

#include <wire.h> //define wire library
#include <adafruit_motorshield.h> //define adafruit motorshield library

//////////define accelometer variables//////////////////
const int xpin = a0;
const int ypin = a1;
float accel_x; 
float accel_y;
float angle;

/////////define sampling variables voltage bias @ ground level/////////
int samplenum = 500;
float vb = 0;
int sampletime = 10;
unsigned long time;

//////pid contorl variables////////////////
int setpoint = 0; //set point / desired angle robot
double kp = 0.3; //set gain portional --> kickstart correction system
double ki = 2; //set gain integral --> add little more correction if set point isn't met
double kd = 5; //set gain derivative --> control correction rate
float guard_gain = 30; //sets limit integral --> limit error correction?...
int last_error = 0; //sets last error of loop 0 @ start
int integrated_error = 0; //sets total error of system during loop 0 @ start
float pidval = 0; //sets pid value 0 @ start | returns pwm value correction of angle

////////define variables dc motors////////////////////////////
adafruit_motorshield afms = adafruit_motorshield(); //define motorshield object
adafruit_dcmotor *mymotor = afms.getmotor (1); //define motor 1
adafruit_dcmotor *mymotor2 = afms.getmotor (2); //define motor 2

/////////setup serial & begin 1 time setup processes///////////////////////
void setup() {
  serial.begin(115200);
  serial.println("*setup begin*");

  //accelometer setup
  serial.println();
  serial.println ("accelometer setup start");

  //determine voltage bias @ current ground level  
  serial.println();//space between text
  serial.println ("sample voltage bias");

  for(int n=0;n<samplenum;n++){ //take total of 500 samples increasing 1 increment
    accel_x = analogread(xpin); //read values x-axis port i.e. a5 --> redfine accel_x value obtained
    vb += accel_x; //add readings variable accel_x vb (voltage bias) each sample conducted
  }
  serial.println();//space between text
  serial.println ("vb sampling completed"); //indicate end of vb sampling

  vb = vb/samplenum; //redefine voltage bias (bits) according sampling. (total voltage bias / sample number) = redefined voltage bias
  vb = vb*0.00488758553; //convert voltage bias bits voltage using factor (5/1023)

  serial.println();//space between text
  serial.print("[voltage bias]:"); //print "voltage bias:" on serial montior
  serial.print(vb); // print vb value next "vb:"
  serial.println();//space between text
  serial.println();//space between text
  serial.println ("*end of accelometer setup process*"); //print after setup has completed
  serial.println();//space between text

  //motor setup
  serial.println();//space between text
  serial.println ("*start of motor setup*");
  
  afms.begin(); //turn on motorshield
  
  serial.println();//space between text
  serial.println ("*end of motor setup*");

  serial.println();//space between text
  serial.println("*accelometer readings*"); //print out accelometer readings in text

  delay(500); //delay system 500 milli-seconds -> allows time user read vb
}

///////////////begin looping process controller///////////////////////////
void loop() { //start loop sequence

  ////////////read raw data accelomoter & convert angle////////////////////
  accel_x = xpin; //read values a0, convert bits->volts using conversion factor (5/1024), define them accel_x
  accel_x = accel_x*0.00488758553; //convert readings accelometer bits voltage
  accel_x = accel_x - vb; //subtract voltage bias voltage reading of sensor
  angle = (accel_x/0.0159); //use degree equation determine degree given corrected voltage output & sensitivity

  serial.println(); //space between text
  serial.print("initial angle:"); //print "old angle:" in serial monitor
  serial.print(angle); //print angle variable value next "angle:" in serial monitor

////////////status check of robot angle////////////////////

  while (angle != setpoint) //check if angle equal setpoint i.e. 0 degrees --> if angle doesn't equal setpoint this...
  {
    serial.println(); // space
    serial.print("robot state: unbalanced"); //print out
        
        /////////////////read sensor again incase while-statement not met////////////////////////
         accel_x = xpin; //read values a0, convert bits->volts using conversion factor (5/1024), define them accel_x
         accel_x = accel_x*0.00488758553; //convert readings accelometer bits voltage
         accel_x = accel_x - vb; //subtract voltage bias voltage reading of sensor
         angle = (accel_x/0.0159); //use degree equation determine degree given corrected voltage output & sensitivity

            //////////////use pid control attempt correct error & reach setpoint/////////////
            
            serial.println(); //space between text
            serial.println("pid controller: start"); //print text
            float error = setpoint - angle; //determine error set point
            float pterm = kp * error; //find p term of pid
            integrated_error += error; //add sum of errors in order integrate
            float iterm = ki * constrain(integrated_error, -guard_gain, guard_gain); //find term of pid | limit value -30 30 degrees?
            float dterm = kd * (error - last_error); //find d term of pid
            last_error = error; //redefine last_error value current error next loop iteration
            pidval = constrain ((pterm + iterm + dterm), -255, 255); //redefine pidval variable pid value of controller | limited -255 255 --> pwm output
            serial.println(); //space between text
            serial.println("pid controller: end"); //print text
            
              //////////////////////send pid controller output motors correct angle error////////////////
              serial.println(); //space between text
              serial.println("motors: correcting"); //print this
              if (pidval > 0) //check whether pid value correction positive if this
              {
              mymotor->setspeed(pidval); //set motor 1 speed pid value correction
              mymotor2->setspeed(pidval); //set motor 2 speed pid value correction
              mymotor->run(forward); //tell motor 1 go forward
              mymotor2->run(forward); //tell more 2 go forward
              }
                else //if pid value correction negative this
                {
                  mymotor->setspeed(abs(pidval)); //set motor 1 speed pid value correction | take absolute value of pid value
                  mymotor2->setspeed(abs(pidval)); //set motor 2 speed pid value correction | take absolute value of pid value
                  mymotor->run(backward); //tell motor 1 go backward
                  mymotor2->run(backward); //tell motor 2 go backward
                }
      
       ///////////////////////////read corrected angle value after pid-motor corrections//////////  
        accel_x = xpin;
        accel_x = accel_x*0.00488758553;
        accel_x = accel_x - vb;
        angle = (accel_x/0.0159);
      
        serial.println(); //space between text
        serial.print("corrected angle:"); //print "old angle:" in serial monitor
        serial.print(angle); //print angle variable value next "angle:" in serial monitor
 }
    
       serial.println(); //space between text
       serial.println("robot state: balanced"); //print this

delay (100); //delay system 100 ms before next sample reading

}

in book of experiences accelerometer returns accelerations, measure of change in movement speed vertical accelerometer plane.

if accelerometer has build in calculator, returns angle found biggest x,y accelerations, understand disappointed.

i bit lost here, may missing point.
what type of acc unit using ?

Arduino Forum > Using Arduino > Programming Questions > Problems Coding Accelometer

arduino