File content as of revision 0:200d9e41a3cf:

#include "mbed.h"
#include "math.h"
#include "USBSerial.h"
#include "QEI.h"
#include "Matrix.h"
#include "MatrixMath.h"


#define Ts 0.01
#define en2rad_roda 0.006698491798699
//#define en2rad_pend 0.001591485640116 
//#define en2rad_roda 0.014660765717029
#define en2rad_pend 0.001570796326795
#define rad2graus 57.295779513082320
#define pi 3.141592653589793
#define omega_nd 210

USBSerial  PC;
Ticker tick;

QEI enc_roda (PTB17, PTD0, NC, 469);
QEI enc_pend (PTA12, PTA13, NC, 1000);
//QEI enc_roda (PTC2, PTC1, NC, 341.2);
//QEI enc_pend (PTD6, PTD5, NC, 1974);

DigitalOut OnBoardLed (D13);
int aux = 0;
float pac = 0.0;

// Variaveis encoders
float theta = 0.0, theta_1 = 0.0, dtheta = 0.0;
float alpha = 0.0, alpha_1 = 0.0, dalpha = 0.0;
float angulo = 0.0;

float xc1 = 0.0, xc2 = 0.0, xc3 = 0.0, xc4 = 0.0;
float dxc1 = 0.0, dxc2 = 0.0, dxc3 = 0.0, dxc4 = 0.0;

// Motor
DigitalOut InA(PTD5);
DigitalOut InB(PTC1);
PwmOut V(PTD6);
float th5=0.0;
float th5_1=0.0;
float dth5=0.0;

// Alimentacao encoder motor
DigitalOut vcc_enc (PTB0);

// Variaveis de medida 
int N=0;
const int tempo = 15/Ts;
float th[tempo], dth[tempo], alp[tempo], dalp[tempo], u[tempo];


// variavel controle
Matrix Z1(1,3);
Matrix Z2(1,3);
Matrix Z3(1,3);
Matrix Y1(3,3);
Matrix Y2(3,3);
Matrix Y3(3,3);
Matrix Z(1,3);
Matrix Y(3,3);
Matrix K(1,3);


float lim_1 = -0.261799387799149;
float lim_2 = 0.261799387799149;   
float rho1 = 0.0, rho2 = 0.0;   
float u_pwm = 0.0;

// drive motor
void drive(float u)
{
    int direction=0;                                            
    if (u>0)direction=1;                                    
    if (u<0)direction=2;                                    
    if(u==0)direction=0;                                    
    switch(direction) {                                        
        case 0:                                                 
            V.write(u);                                   
            InA=0;
            InB=0;
            break;
        case 1:                                                 
            V.write(u);                                   
            InA=1;
            InB=0;
            break;
        case 2:                                                 
            V.write(-u);                                   
            InA=0;
            InB=1;
            break;
        default:                                                
            V.write(0.0);                                   
            InA=0;
            InB=0;
            break;
    }
}

// Interrupção
void inverte() {
    aux=1; 
    //N++; 
}
 
int main ()
{
Z1 << 0.0528635583471 << 55.1775667591881 << -334.6107239814101;
Z2 << 2.2178052729892 << 89.4963037908543 << -632.0606939025414;
Z3 << 0.0527550080233 << 55.1818544517557 << -334.6407739018230;
Y1 <<   52.229273 <<   -293.202697 <<  -1000.523522
   << -351.739401 <<   8417.246435 << -49726.191528
   << -487.165029 << -53707.951308 << 505666.562609;        
Y2 <<   79.768194  <<    -492.424621 << -1138.974682
   <<  -454.513497 <<   15400.080131 << -103976.618630
   << -1476.885923 << -101235.956011 << 992352.224275;
Y3 <<   52.229040 <<   -293.201469 <<  -1000.518230
   << -351.740390 <<   8417.269581 << -49726.325976
   << -487.140469 << -53708.234657 << 505667.351490;

// LQR contuinuo
float K1 = 6.307583719273756;
float K2 = 0.808578597390600;
float K3 = 0.091291588353526;
   
    vcc_enc = 1;
    OnBoardLed = 1;
    V.period(0.0001);
    wait(5);
    tick.attach(&inverte,Ts);
    
    while (1)
    { 
        if(aux == 1){
            aux = 0;
            
            theta=enc_roda.getPulses()*en2rad_roda;
            dtheta=(theta-theta_1)/Ts;
            theta_1=theta;  
            
            angulo=enc_pend.getPulses()*en2rad_pend;
            alpha = angulo - pi*angulo/abs(angulo);
            dalpha=(alpha-alpha_1)/Ts;
            alpha_1=alpha;    
            
            // Disturbio
            /*if (N>7/Ts && N<7.2/Ts){
                alpha = alpha-7*pi/180;   
            }*/
            
            // Variavel LPV
            rho1 = (alpha - lim_1)/(lim_2-lim_1);
            rho2 = 1-rho1;
            
            // Controle
            Y = rho1*rho1*Y1+rho1*rho2*Y2+rho2*rho2*Y3;
            Z = rho1*rho1*Z1+rho1*rho2*Z2+rho2*rho2*Z3;
            K = Z*MatrixMath::Inv(Y);
            u_pwm = K.getNumber(1,1)*alpha + K.getNumber(1,2)*dalpha + K.getNumber(1,3)*dtheta;
            //u_pwm = K1*alpha + K2*dalpha + K3*dtheta; 
                        
                                    
            // Saturação da roda
            if (u_pwm>1.0)
            u_pwm=1.0;
            if (u_pwm<-1.0)
            u_pwm=-1.0;  
                        
            drive(u_pwm);
            
            
            if (N<tempo) {
                th[N] = theta;
                dth[N] = dtheta;
                alp[N] = alpha;
                dalp[N] = dalpha;
                u[N] = u_pwm;
            }

            if (N == tempo) {
                tick.detach();
                drive(0.0);
                for (N=0; N<tempo; N++) 
                PC.printf("%f %f %f %f %f\n\r", alp[N], dalp[N], th[N], dth[N], u[N]);
            }

            //PC.printf( "%f \n\r",alpha);
            //PC.printf( "\n" );
           
        }
    }
}