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:
#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.