Go Down

Topic: Pole Assignment control algorithm with Full-Order Observer for N states model (Read 173 times) previous topic - next topic

Hi folks

I have written some code in order to control a pole-cart inverted pendulum with Pole Assignment method design. This code implements a Full-Order Observer and it should work for N states model but when I run the code into my arduino uno with N=4, the u variable goes to overflow. This problem not happen when I run it with N=2.

Here's my code:

Code: [Select]
#define N   4
#define pi  3.1416
unsigned long lastTime;
unsigned long now;
int timeChange;
int i,j;
float M=0.15;
float m=0.03;
float l=0.3;
float g=9.8;
float k[N]={4.821,0.788,1.492,0.827};
float ke[N]={7641.6,57869.44,-24.5,-420};
float A[N][N]={{0,1,0,0},{((M+m)*g)/(M*l),0,0,0},{0,0,0,1},{-(m*g)/M,0,0,0}};
float B[N]={0,-1/M*l,0,1/M};
float C[N]={0,0,1,0};
float xhat[N];
float xhatp[N];
float dt=0.01;
float y,yhat,e,u,pwm,yg,uc,yprom,ysum;

void setup() {

  Serial.begin(9600);
  pinMode(A0,INPUT);
  pinMode(A1,INPUT);
  pinMode(3,OUTPUT);
  pinMode(5,OUTPUT);
  //analogReference(INTERNAL);
  /*     for ( i = 0; i < N; i++ )
   {
      for ( j = 0; j < N; j++ )
      {
         Serial.println(A[i][j]);
      }
   }*/
}

void loop() {
   //y=map1(double(analogRead(A0)),100,195,0,pi/2);
   y=(6787/(analogRead(A1)-9))-4;
  /*for(i=0;i < 6;i++){
  y=map1(double(analogRead(A0)),100,195,0,pi/2);
  ysum+=y;
  }
  yprom=ysum/5;
  ysum=0;*/
  //y=double(analogRead(A0))*((pi/2)/(916-850))-pi/2;
//  yg=y*(360/(2*pi));
  //Serial.println(pwm);
  now=millis();
  timeChange = now - lastTime;
  /*if (uc>=1400)
  uc=12;
  if(uc<=-1400)
  uc=-12;*/
  if(uc<=34 && uc>=0){
  pwm=map(uc,0,34,0,255);
  analogWrite(3,pwm);
  digitalWrite(5,0);
  }
  if(uc<=0 && uc>=-34){
  pwm=map(uc,0,-34,0,255);
  analogWrite(5,pwm);
  digitalWrite(3,0);
  }
 
 
  if(timeChange>=dt*1000){
    e = y - yhat;
    yhat=0;
    for (i = 0; i < N; i++)
    {
       for (j = 0; j < N; j++)
       {
          xhatp[i]+=A[i][j]*xhat[j];
       }
    }
    for (i = 0; i < N; i++)
    {
        xhatp[i]+=(B[i]*uc)+(ke[i]*e);
    }
    for (i = 0; i < N; i++)
    {
        xhat[i]+=xhatp[i]*dt;
    }
    for (j = 0; j < N; j++)
    {
        xhatp[j]=0;
    }
    for (j = 0; j < N; j++)
    {
        yhat+=C[j]*xhat[j];
    }
    for (j = 0; j < N; j++)
    {
        u+=k[j]*xhat[j];
    }
    //Serial.println(u);
    Serial.println(y);
    uc=u;
    u=0;
    lastTime=now;
   
    //Serial.println(xhat[0]);
    //Serial.println(xhat[1]);
  }
}
double map1(double x, double in_min, double in_max, double out_min, double out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}


The system for this N=4 model with full-order observer should work by measuring the lineal distance of the car through a rail. Originally I measure the lineal displacement with a infrared sensor sharp gp2y0a21yk0f but the code can be debugged with a simple potentiometer.

robtillaart

the arduino UNO/MEGA  float is only 32bit and it has no double (in fact it defines double as float)

The due and yun has 64bit double IIRC

Rob Tillaart

Nederlandse sectie - http://arduino.cc/forum/index.php/board,77.0.html -
(Please do not PM for private consultancy)

Go Up
 


Please enter a valid email to subscribe

Confirm your email address

We need to confirm your email address.
To complete the subscription, please click the link in the email we just sent you.

Thank you for subscribing!

Arduino
via Egeo 16
Torino, 10131
Italy