Revision 1:7c69af307591, committed 2019-07-16

Comitter:

lcaepusp

Date:

Tue Jul 16 19:39:11 2019 +0000

Parent:

0:68f5325bc455

Commit message:

Final version mpc

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPCG.cpp	Tue Jul 16 19:39:11 2019 +0000
@@ -0,0 +1,1123 @@
+#include "MPCG.h"
+#define NumBitsPerChar                 8U
+#include "mbed.h"
+#include "model.h"
+
+// Model step function
+void MPC_Gen::step()
+{
+    /*
+    Serial pc (USBTX,USBRX);        // Inicia serial
+    wait(0.1);
+    pc.baud(115200);
+    wait(0.1);
+    */
+    /*
+    for (int i = 0; i < nu*m; i++)
+    {
+        for (int j = 0; j < nu*m; j++)
+        {
+           pc.printf("%f ",Hinv[i][j]);
+        }
+           pc.printf("\n\r");
+    }
+    */
+    // Calcula ymk e xmk
+    real_T xmk[nx];
+
+    for (int i = 0; i < nx; i++) {
+        xmk[i] = rtU.x[i];
+    }
+
+
+    /*
+    for (int i = 0; i < nx; i++) {
+        pc.printf("%f\n\r",xmk[i]);
+    }
+    pc.printf("\n\r");
+    */
+
+    real_T ysp[p*ny];
+    // calcula ysp
+    /* for (int i = 0; i < p; i++){
+    ysp[((1 + i) << 1) - 2] = rtU.sp[0];
+    ysp[((1 + i) << 1) - 1] = rtU.sp[1];
+    } */ // Apenas se tiver 2 set-points
+    for (int i = 0; i < p; i++) {
+        ysp[i] = rtU.sp[0];
+    }
+
+    /*
+    for (int i = 0; i < p*ny; i++) {
+        pc.printf("%f\n\r",ysp[i]);
+    }
+    */
+    // calcula ct
+    real_T* el = new real_T[p*ny];
+    mvec_mult(p*ny,nx,Psi,xmk,el);
+
+    for (int i = 0; i < ny*p; i++) {
+        el[i] = el[i]-ysp[i];//-el por causa de ct
+    }
+    /*
+    for (int i = 0; i < p*ny; i++) {
+        pc.printf("%f\n\r",el[i]);
+    }
+    */
+    real_T* ct = new real_T[m*nu];
+
+    /*
+    for (int i = 0; i < ny*p; i++)
+    {
+        for (int j = 0; j < nu*m; j++)
+        {
+           pc.printf("%f ",Qtheta[i][j]);
+        }
+           pc.printf("\n\r");
+    }
+    */
+
+
+    mvec_multl(p*ny,m*nu,el,Qtheta,ct);
+
+    /*
+    for (int i = 0; i < m*nu; i++) {
+        pc.printf("%f\n\r",ct[i]);
+    }
+    */
+
+    // calcula bc
+    real_T* uk_1 = new real_T[nu];
+    for (int i = 0; i < nu; i++) {
+        uk_1[i]=rtDW.uk_1[i];
+    }
+
+    real_T* bc_int = new real_T[m*nu];
+
+    mvec_mult(m*nu,nu,Itil,uk_1,bc_int);
+
+    //bc=[Umax-Itil*uk_1;-Umin+Itil*uk_1;Dumax;Dumax];
+    bc = new real_T[4*nu*m];
+
+    for ( int i = 0; i < 4*nu*m; i++) {
+        if(i < nu*m)
+            bc[i] = Umax[i]-bc_int[i];
+        else if (i < 2*nu*m)
+            bc[i] = -Umin[i-nu*m]+bc_int[i-nu*m];
+        else if (i < 3*nu*m)
+            bc[i] = Dumax[i-2*nu*m];
+        else if (i < 4*nu*m)
+            bc[i] = Dumax[i-3*nu*m];
+    }
+    /*
+    for (int i = 0; i < m*nu; i++) {
+        pc.printf("%f\n\r",bc_int[i]);
+    }*/
+    /*
+    for (int i = 0; i < 4*m*nu; i++) {
+        pc.printf("%f\n\r",bc[i]);
+    }
+    */
+    // Memory Deallocation
+    delete [] el;
+    delete [] uk_1;
+    delete [] bc_int;
+    el = 0;
+    uk_1 = 0;
+    bc_int = 0;
+
+    // solve QP
+    // Unconstrained solution
+    real_T* x_un = new real_T[m*nu];
+    real_T* x_c = new real_T[m*nu];
+    real_T* x = new real_T[m*nu];
+
+    mvec_mult(m*nu,m*nu,nHinv,ct,x_un);
+
+    for(int i = 0; i < m*nu; i++) {
+        x[i] = x_un[i];
+    }
+
+    /*
+    for (int i = 0; i < m*nu; i++) {
+        pc.printf("%f\n\r",x_un[i]);
+    }
+    */
+
+    int_T kk = 0;
+    real_T aci = 0;
+
+    for (int i = 0; i < 4*m*nu; i++) {
+        aci = 0;
+        for(int j = 0; j < m*nu; j++) {
+            //A_cons(i,:)*eta>b(i)
+            aci = aci+ Ac[i][j]*x_un[j];
+        }
+        if (aci > bc[i])
+            kk++;
+    }
+
+    //pc.printf("%d\n\r",kk);
+
+
+    real_T* d = new real_T[4*m*nu];
+    if (kk != 0) {
+
+        //d=(A_cons*(H\f)+b);
+        mvec_mult(4*m*nu,m*nu,Ac,x_un,d);
+        for (int i = 0; i < 4*m*nu; i++) {
+            d[i] = -d[i];
+        }
+        /*
+        for (int i = 0; i < 4*m*nu; i++) {
+        pc.printf("%f\n\r",d[i]);
+        }
+        pc.printf("\n\r");
+        */
+
+        for( int i = 0; i < 4*m*nu; i++)
+            d[i] = d[i] + bc[i];
+        /*
+        for (int i = 0; i < 4*m*nu; i++) {
+        pc.printf("%f\n\r",d[i]);
+        }
+        pc.printf("\n\r");
+        */
+        //[n,ms]=size(d);
+        //x_ini=zeros(n,ms);
+        //lambda=x_ini;
+        real_T lambda[4*m*nu];
+        real_T lambdap[4*m*nu];
+        for( int i = 0; i < 4*m*nu; i++) {
+            lambda[i]=0;
+        }
+
+        int_T max_it = 100; // maximum solver iterations******************era 100
+        int_T km = 0;
+        real_T w = 0;
+        real_T al = 0;
+        //real_T normdif = 0;
+        //real_T normbase = 0;
+
+        while (km < max_it) {
+
+            for(int i = 0; i < 4*m*nu; i++) {
+                lambdap[i]=lambda[i];
+            }
+            km = km + 1;
+
+            for(int i = 0; i < 4*m*nu; i++) {
+                w = 0;
+                //w= P(i,:)*lambda-P(i,i)*lambda(i,1);
+                for(int j = 0; j < 4*m*nu; j++) {
+                    w = w+ P[i][j]*lambda[j];
+                }
+                w = w - P[i][i]*lambda[i] + d[i];
+
+                w = -w/P[i][i];
+                if (w > 0)
+                    lambda[i] = w;
+                else
+                    lambda[i] = 0;
+            }
+
+            /*
+            normdif = 0;
+            normbase = 0;
+
+            for(int i = 0; i < 4*m*nu; i++){
+                normdif = normdif + (lambda[i]-lambdap[i])*(lambda[i]-lambdap[i]);
+                normbase = normbase + lambda[i]*lambda[i];
+            }
+
+            al = sqrt(normdif/normbase);//percentage error
+            */
+
+            al = 0;
+            for(int i = 0; i < 4*m*nu; i++) {
+                al = al + (lambda[i]-lambdap[i])*(lambda[i]-lambdap[i]);
+            }
+
+            /*
+            if(al < 1e-7)
+                km = max_it;
+            */
+            /*
+            if(al < 1e-2)
+                km = max_it;
+            */
+        }
+
+
+        mvec_mult(m*nu,4*m*nu,nHinvtAc,lambda,x_c);
+        for(int i = 0; i < m*nu; i++) {
+            x[i] = x_un[i] + x_c[i];
+        }
+    }
+
+    /*
+    for (int i = 0; i < m*nu; i++) {
+        pc.printf("%f\n\r",x[i]);
+    }
+    pc.printf("\n\r");
+    */
+
+    // Set outputs
+
+    for (int i = 0; i < nu; i++) {
+        rtDW.duk_1[i] = x[i];
+        rtDW.uk_1[i] = x[i] + rtDW.uk_1[i];
+        rtY.u[i] = rtDW.uk_1[i];
+        rtY.du[i]= x[i];
+    }
+
+
+    // Deallocate Memory
+    delete [] x;
+    delete [] x_c;
+    delete [] x_un;
+    delete [] bc;
+    delete [] ct;
+    delete [] d;
+    x = 0;
+    x_c = 0;
+    x_un = 0;
+    bc = 0;
+    ct = 0;
+    d = 0;
+
+    // Print control signal
+    /*
+    for (int i = 0; i < nu; i++) {
+        pc.printf("%f\n\r",rtY.u[i]);
+    }
+    */
+
+
+
+}
+
+// Model initialize function
+void MPC_Gen::initialize()
+{
+    /*
+    Serial pc (USBTX,USBRX);        // Inicia serial
+    wait(0.1);
+    pc.baud(115200);
+    wait(0.1);
+    */
+    for (int i = 0; i < nu; i++) {
+        rtDW.uk_1[i] = 0;
+        rtDW.duk_1[i]=0;
+    }
+
+    //pc.printf("%d %d %f %f %f %f %f %f %f %f %f \n\r",m,p,q[0],q[1],r[0],r[1],umax[0],umax[1],umin[0],umin[1],dumax);
+    //int nphi = p*ny;
+
+
+    // Identidade dimensão nx
+    real_T** Inx = new real_T*[nx];//rows
+    for (int i = 0; i < nx; i++) {
+        Inx[i] = new real_T[nx];
+    }
+
+    eye(nx,Inx);
+
+    real_T *A[nx];  // surrogate
+    for (int i = 0; i < nx; i++) {
+        A[i] = Ar[i];
+    }
+
+    int rows = nx, col=nu;
+    real_T *B[rows];  // surrogate
+    for (int i = 0; i < rows; i++) {
+        B[i] = Br[i];
+    }
+
+    rows = ny, col=nx;
+    real_T *C[rows];  // surrogate
+    for (int i = 0; i < rows; i++) {
+        C[i] = Cr[i];
+    }
+
+    // Initialize Aux1, Aux2, Aux1pow, Aux2pow
+    rows = ny, col=nx;
+    real_T** Aux1 = new real_T*[rows];//rows
+    for (int i = 0; i < rows; i++) {
+        Aux1[i] = new real_T[col];
+    }
+    rows = ny, col=nu;
+    real_T** Aux2 = new real_T*[rows];//rows
+    for (int i = 0; i < rows; i++) {
+        Aux2[i] = new real_T[col];
+    }
+    rows = nx, col=nx;
+    real_T** Aux1pow = new real_T*[rows];//rows
+    for (int i = 0; i < rows; i++) {
+        Aux1pow[i] = new real_T[col];
+    }
+    rows = nx, col=nx;
+    real_T** Aux2pow = new real_T*[rows];//rows
+    for (int i = 0; i < rows; i++) {
+        Aux2pow[i] = new real_T[col];
+    }
+
+    rows = ny, col=nx;
+    real_T** Aux3pow = new real_T*[rows];//rows
+    for (int i = 0; i < rows; i++) {
+        Aux3pow[i] = new real_T[col];
+    }
+
+    rows = p*ny, col=nu;
+    real_T** ThA = new real_T*[rows];//rows
+    for (int i = 0; i < rows; i++) {
+        ThA[i] = new real_T[col];
+    }
+
+    rows = p*ny, col=nx;
+    Psi = new real_T*[rows];//rows
+    for (int i = 0; i < rows; i++) {
+        Psi[i] = new real_T[col];
+    }
+
+
+    // Inicializa Aux1pow e Aux2pow como A e Inx
+
+    matr_mult(nx,nx,nx,Inx,A,Aux1pow);
+    matr_mult(nx,nx,nx,Inx,Inx,Aux2pow);
+
+
+    for (int i = 0; i < p; i++) {
+        if (i > 0) {
+            //Aux1pow = dot_prod(Aux1pow,A);
+            //Aux2pow = dot_prod(Aux2pow,A);
+
+            matr_mult(nx,nx,nx,Aux1pow,A,Aux1pow);
+            matr_mult(nx,nx,nx,Aux2pow,A,Aux2pow);
+        }
+
+        matr_mult(ny,nx,nx,C,Aux1pow,Aux1);
+
+        matr_mult(ny,nx,nx,C,Aux2pow,Aux3pow);
+        matr_mult(ny,nx,nu,Aux3pow,B,Aux2);
+
+        for (int j = 0; j < ny; j++) { // ThA
+            for (int k = 0; k < nu; k++)
+                ThA[nu*i+k][j] = Aux2[k][j]; // 1º linha geral é nu*i+0/1/2... nu-1 outro for para caso geral
+            //ThA[2*i+1][j] = Aux2[1][j]; // 2º linha
+        }
+
+        for (int j = 0; j < nx; j++) { // Phi
+            for (int k = 0; k < ny; k++) {
+                Psi[ny*i+k][j] = Aux1[k][j];// talvez zoe para outros casos de ny nu
+            }
+        }
+        //pc.printf("oi5\n\r");
+    }
+
+    //pc.printf("%x",*(*(&Psi+1)+0));
+
+    // Será que funciona?
+    /*
+    for (int i = 0; i < p*ny; i++)
+    {
+        for (int j = 0; j < nx; j++)
+        {
+           real_T & ref = Psi[i][j];//seria a Psi
+           ref = Psi_in[i][j];//Seria a Psi_in
+           delete &ref;
+        }
+    }*/
+
+
+    /*
+    for (int i = 0; i < p*ny; i++)
+    {
+        for (int j = 0; j < nx; j++)
+        {
+           pc.printf("%f ",Psi[i][j]);
+
+        }
+           pc.printf("\n\r");
+    }*/
+    //pc.printf("oi\n\r");
+
+    // declarando matriz Thetabar de tamanho variável [rows][cols]
+    rows = p*ny, col = m*nu;
+    Thetabar = new real_T*[rows];
+    for (int i = 0; i < rows; i++)
+        Thetabar[i] = new real_T[col];
+
+
+    // initialize thetabar with zeros
+    for (int i = 0; i < ny*p; i++) {
+        for (int j = 0; j < nu*m; j++) {
+            Thetabar[i][j] = 0;
+        }
+    }
+    // Montando a matriz dinâmica
+    for(int j = 0; j < m; j++) {
+        for(int k = 0; k < nu; k++) {
+            for (int i = 0; i < ny*p; i++) {
+                Thetabar[i][k+nu*j] = ThA[i][k];
+            }
+        }
+        // começa de baixo
+        for (int i = ny*p-1; i > ny-1; i--) { // caso particular para nu = ny = 2 //*********(int i = ny*p-ny+1; i > ny-1; i--)
+            for (int k = 0; k < nu; k++)
+                ThA[i][k] = ThA[i-ny][k];
+        }
+
+
+        for (int k = 0; k < nu; k++) {
+            for (int i = 0; i < ny; i++)
+                ThA[ny*j+i][k] = 0;
+            //ThA[ny*j+1][k] = 0;
+        }
+
+    }
+    /*
+    pc.printf("oi\n\r");
+    for (int i = 0; i < p*ny; i++)
+    {
+        for (int j = 0; j < nu*m; j++)
+        {
+           pc.printf("%f ",Thetabar[i][j]);
+
+        }
+           pc.printf("\n\r");
+    }*/
+
+    // Deallocation first part
+    for (int i = 0; i < ny; i++) {
+        delete [] Aux1[i];
+        delete [] Aux2[i];
+        delete [] Aux3pow[i];
+    }
+    delete [] Aux1;
+    delete [] Aux2;
+    delete [] Aux3pow;
+    Aux1=0;
+    Aux2=0;
+    Aux3pow=0;
+
+    for (int i = 0; i < nx; i++) {
+        delete [] Aux1pow[i];
+        delete [] Aux2pow[i];
+        delete [] Inx[i];
+    }
+    delete [] Aux1pow;
+    delete [] Aux2pow;
+    delete [] Inx;
+    Aux1pow=0;
+    Aux2pow=0;
+    Inx=0;
+
+    for (int i = 0; i < p*ny; i++) {
+        delete [] ThA[i];
+    }
+    delete [] ThA;
+    ThA=0;
+
+    // Montando Mtil
+    // declarando matriz Mtil de tamanho variável [rows][cols]
+
+    rows = nu*m, col=nu*m;
+    Mtil = new real_T*[rows];//rows
+    for (int i = 0; i < rows; i++) {
+        Mtil[i] = new real_T[col];
+    }
+    for (int i = 0; i < nu*m; i++) {
+        for (int j = 0; j < nu*m; j++) {
+            Mtil[i][j] = 0;
+        }
+    }
+
+    for (int i = 0; i < m; i++) {
+        for (int j = 0; j < m*nu; j++) {
+            if(nu*i+j < m*nu)
+                Mtil[nu*i+j][j] = 1;
+        }
+    }
+
+
+    // declarando matriz Ac de tamanho variável [rows][cols]
+    rows = 4*m*nu, col = m*nu;
+    Ac = new real_T*[rows];
+    for (int i = 0; i < rows; i++)
+        Ac[i] = new real_T[col];
+
+
+    // Montando I(mu)
+    // Identidade dimensão m*nu
+    real_T** Imu = new real_T*[m*nu];//rows
+    for (int i = 0; i < m*nu; i++) {
+        Imu[i] = new real_T[m*nu];
+    }
+    eye(m*nu,Imu);
+
+
+    // Montando Ac = [Mtil;-Mtil]
+    //Ac.resize(nu*m,2*nu*m);
+    for ( int i = 0; i < 4*nu*m; i++) {
+        for ( int j = 0; j < nu*m; j++) {
+            if(i < nu*m)
+                Ac[i][j] = Mtil[i][j];
+            else if (i < 2*nu*m)
+                Ac[i][j] = -Mtil[i-nu*m][j];
+            else if (i < 3*nu*m)
+                Ac[i][j] = Imu[i-2*nu*m][j];
+            else if (i < 4*nu*m)
+                Ac[i][j] = -Imu[i-3*nu*m][j];
+        }
+    }
+    /*
+    for (int i = 0; i < 4*m*nu; i++)
+    {
+        for (int j = 0; j < nu*m; j++)
+        {
+           pc.printf("%f ",Ac[i][j]);
+
+        }
+           pc.printf("\n\r");
+    }
+    */
+
+    rows = m*nu, col = nu;
+    Itil = new real_T*[rows];
+    for (int i = 0; i < rows; i++)
+        Itil[i] = new real_T[col];
+
+    for (int i = 0; i < rows; i++) {
+        for (int j = 0; j < col; j++) {
+            Itil[i][j]=0;   //Set all elements to zero
+        }
+    }
+    for (int j = 0; j < nu; j++) {
+        for (int i = 0; i < m; i++) {
+            if(nu*i+j < m*nu)
+                Itil[nu*i+j][j] = 1;
+        }
+    }
+    /*
+    rows = nu, col = m*nu;
+    tItil = new real_T*[rows];
+    for (int i = 0; i < rows; i++)
+        tItil[i] = new real_T[col];
+
+    matr_t(m*nu,nu,Itil,tItil);
+    */
+    /*
+    for (int i = 0; i < m*nu; i++)
+    {
+        for (int j = 0; j < nu; j++)
+        {
+           pc.printf("%f ",Itil[i][j]);
+
+        }
+           pc.printf("\n\r");
+    }
+    */
+    // declarando matriz Qbar de tamanho variável [rows][cols]
+    rows = p*ny, col = p*ny;
+    Qbar = new real_T*[rows];
+    for (int i = 0; i < rows; i++)
+        Qbar[i] = new real_T[col];
+
+    for (int i = 0; i < rows; i++) {
+        for (int j = 0; j < col; j++) {
+            Qbar[i][j]=0;   //Set all elements to zero
+        }
+    }
+
+    rows = m*nu, col = m*nu;
+    real_T** Rbar = new real_T*[rows];
+    for (int i = 0; i < rows; i++)
+        Rbar[i] = new real_T[col];
+
+    for (int i = 0; i < rows; i++) {
+        for (int j = 0; j < col; j++) {
+            Rbar[i][j]=0;   //Set all elements to zero
+        }
+    }
+
+
+    //Montando Qbar
+    for( int j = 0; j < p; j++) {
+        for (int i = 0; i < p; i++) {
+            if(i == j) {
+                for(int k = 0; k < ny; k++)
+                    Qbar[ny*i+k][ny*j+k] = q[k];
+            }
+        }
+    }
+    //montando Rbar
+    for (int i = 0; i < nu*m; i++)
+        for(int j = 0; j < nu*m; j++)
+            Rbar[i][j] = 0;
+
+
+    for( int j = 0; j < m; j++) {
+        for (int i = 0; i < m; i++) {
+            if(i == j) {
+                for(int k = 0; k < nu; k++)
+                    Rbar[nu*i+k][nu*j+k] = r[k];
+            }
+        }
+    }
+    /*
+    for (int i = 0; i < nu*m; i++)
+    {
+        for (int j = 0; j < nu*m; j++)
+        {
+           pc.printf("%f ",Rbar[i][j]);
+        }
+           pc.printf("\n\r");
+    }
+    */
+
+
+
+    Dumax = new real_T[nu*m];
+    Umin = new real_T[nu*m];
+    Umax = new real_T[nu*m];
+
+    for (int i = 0; i < nu*m; i++) {
+        Dumax[i] = 0;
+        Umax[i] = 0;
+        Umin[i] = 0;
+    }
+
+    for (int i = 0; i < m; i++) {
+        for (int k = 0; k < nu; k++) {
+            Dumax[nu*i+k]=dumax;
+            Umax[nu*i+k]=umax[k];
+            Umin[nu*i+k]=umin[k];
+        }
+    }
+    /*
+    for (int i = 0; i < nu*m; i++)
+    {
+           pc.printf("%f \n\r",Dumax[i]);
+    }
+    for (int i = 0; i < nu*m; i++)
+    {
+           pc.printf("%f \n\r",Umax[i]);
+    }
+    for (int i = 0; i < nu*m; i++)
+    {
+           pc.printf("%f \n\r",Umin[i]);
+    }
+    */
+
+    // declarando matriz H de tamanho variável [rows][cols]
+    rows = m*nu, col = m*nu;
+    H = new real_T*[rows];
+
+    for (int i = 0; i < rows; i++)
+        H[i] = new real_T[col];
+
+
+    //H=Thetabar'*Qbar*Thetabar+Rbar;
+
+    rows = m*nu, col = p*ny;
+    real_T** tThetabar = new real_T*[rows];
+    for (int i = 0; i < rows; i++)
+        tThetabar[i] = new real_T[col];
+
+
+    matr_t(p*ny,m*nu,Thetabar,tThetabar);
+    matr_mult(m*nu,p*ny,p*ny,tThetabar,Qbar,tThetabar);
+    matr_mult(m*nu,p*ny,m*nu,tThetabar,Thetabar,H);
+    matr_sum(m*nu,m*nu,H,Rbar,H);
+
+
+    rows = ny*p, col=m*nu;
+    Qtheta = new real_T*[rows];//rows
+    for (int i = 0; i < rows; i++) {
+        Qtheta[i] = new real_T[col];
+    }
+
+    rows = m*nu, col=p*ny;
+
+
+    matr_mult(ny*p,ny*p,m*nu,Qbar,Thetabar,Qtheta);
+
+    /*
+    for (int i = 0; i < nu*m; i++)
+    {
+        for (int j = 0; j < nu*m; j++)
+        {
+           pc.printf("%f ",H[i][j]);
+        }
+           pc.printf("\n\r");
+    }
+    */
+    // Até agora, H foi calculado corretamente, cross-check com o MATLAB
+    double tol = 0.00001;
+
+    rows = m*nu, col = m*nu;
+    double** Hlu = new double*[rows];
+
+    for (int i = 0; i < rows; i++)
+        Hlu[i] = new double[col];
+
+    for (int i = 0; i < rows; i++) {
+        for (int j = 0; j < col; j++) {
+            Hlu[i][j]=H[i][j];   // Hlu = H
+        }
+    }
+
+    int_T* Plu = new int_T[m*nu+1];
+    //P of size N+1
+
+
+    int flag = LUPDecompose(Hlu,m*nu,tol,Plu);
+
+    //pc.printf("%d",flag);
+
+    rows = m*nu, col = m*nu;
+    Hinv = new real_T*[rows];
+
+    for (int i = 0; i < rows; i++)
+        Hinv[i] = new real_T[col];
+
+    rows = m*nu, col = m*nu;
+    double** Hinvdp = new double*[rows];
+
+    for (int i = 0; i < rows; i++)
+        Hinvdp[i] = new double[col];
+
+
+    LUPInvert(Hlu,Plu,m*nu,Hinvdp);
+
+    for (int i = 0; i < rows; i++) {
+        for (int j = 0; j < col; j++) {
+            Hinv[i][j]=Hinvdp[i][j];   // Hlu = H
+        }
+    }
+
+
+    rows = m*nu, col = m*nu;
+    nHinv = new real_T*[rows];
+    for (int i = 0; i < rows; i++)
+        nHinv[i] = new real_T[col];
+
+    for (int i = 0; i < rows; i++) {
+        for (int j = 0; j < col; j++) {
+            nHinv[i][j] = -Hinv[i][j];
+        }
+    }
+
+
+    //P=A_cons*(H\A_cons'); A_cons*inv(H)*A_cons'
+
+    rows = 4*m*nu, col = 4*m*nu;
+    P = new real_T*[rows];
+    for (int i = 0; i < rows; i++)
+        P[i] = new real_T[col];
+
+    rows = 4*m*nu, col = m*nu;
+    real_T** Acint = new real_T*[rows];
+    for (int i = 0; i < rows; i++)
+        Acint[i] = new real_T[col];
+
+    rows = m*nu, col = 4*m*nu;
+    real_T** tAc = new real_T*[rows];
+    for (int i = 0; i < rows; i++)
+        tAc[i] = new real_T[col];
+
+    matr_mult(4*m*nu,m*nu,m*nu,Ac,Hinv,Acint);
+    matr_t(4*m*nu,m*nu,Ac,tAc);
+    matr_mult(4*m*nu,m*nu,4*m*nu,Acint,tAc,P);
+
+    rows = m*nu, col = 4*m*nu;
+    nHinvtAc = new real_T*[rows];
+    for (int i = 0; i < rows; i++)
+        nHinvtAc[i] = new real_T[col];
+
+    matr_mult(m*nu,m*nu,4*m*nu,nHinv,tAc,nHinvtAc);
+
+
+    // Memory deallocation 2, not strictly necessary but who knows compiler's crazy -> function
+    for (int i = 0; i < m*nu; i++) {
+        delete [] Imu[i];
+        delete [] tThetabar[i];
+        delete [] Hlu[i];
+        delete [] tAc[i];
+    }
+    delete [] Imu;
+    delete [] tThetabar;
+    delete [] Hlu;
+    delete [] tAc;
+    Imu=0;
+    tThetabar=0;
+    Hlu=0;
+    tAc=0;
+
+    for (int i = 0; i < 4*m*nu; i++) {
+        delete [] Acint[i];
+    }
+    delete [] Acint;
+    Acint=0;
+
+
+
+    delete [] Plu;
+    Plu = 0;
+
+    //pc.printf("%p\n\r",&**Hinv);
+    //pc.printf("Tudo ok");
+}
+
+// Constructor
+// Inputs: control horizon m, prediction horizon p, output weigth q, control signal weight r, slew rate dumax
+MPC_Gen::MPC_Gen(int_T mi, int_T pi, real_T (&qi)[ny], real_T (&ri)[nu], real_T (&umaxi)[nu], real_T (&umini)[nu], real_T dumaxi)//, const int p, const float qi, const float ri, const float dumaxi, const int nxi
+    : m(mi),p(pi),q(qi),r(ri),umax(umaxi),umin(umini),dumax(dumaxi)
+{
+
+    // Constructor, no routines
+
+}
+
+// Destructor
+MPC_Gen::~MPC_Gen()
+{
+    // Currently there is no destructor body generated.
+}
+// Object routines
+void MPC_Gen::matr_mult(int_T n1,int_T n2,int_T n3,real_T **A,real_T **B, real_T **R) // must guarantee correct size
+{
+
+    real_T resp[n1][n3];
+    for(int i=0; i<n1; i++) {
+        for(int j=0; j<n3; j++) {
+            resp[i][j]=0;
+        }
+    }
+
+    for (int i = 0; i < n1; i++) {
+        for (int j = 0; j < n3; j++) {
+            for (int k = 0; k < n2; k++)
+                resp[i][j] += A[i][k]*B[k][j];
+        }
+    }
+
+    for (int i = 0; i < n1; i++) {
+        for (int j = 0; j < n3; j++)
+            R[i][j] = resp[i][j];
+    }
+
+}
+
+void MPC_Gen::eye(int_T n,real_T **I)
+{
+
+
+    real_T resp[n][n];
+
+    for(int i=0; i<n; i++) {
+        for(int j=0; j<n; j++) {
+            resp[i][j]=0;
+        }
+    }
+
+    for (int i = 0; i < n; i++) {
+        resp[i][i] = 1;
+    }
+
+    for (int i = 0; i < n; i++) {
+        for (int j = 0; j < n; j++) {
+            I[i][j]=resp[i][j];
+        }
+    }
+
+}
+
+void MPC_Gen::matr_sum(int_T n1,int_T n2,real_T **A,real_T **B, real_T **R) // must guarantee correct size
+{
+
+    real_T resp[n1][n2];
+    for(int i=0; i<n1; i++) {
+        for(int j=0; j<n2; j++) {
+            resp[i][j]=0;
+        }
+    }
+
+    for (int i = 0; i < n1; i++) {
+        for (int j = 0; j < n2; j++) {
+            resp[i][j] = A[i][j]+B[i][j];
+        }
+    }
+
+    for (int i = 0; i < n1; i++) {
+        for (int j = 0; j < n2; j++)
+            R[i][j] = resp[i][j];
+    }
+
+}
+
+void MPC_Gen::matr_dif(int_T n1,int_T n2,real_T **A,real_T **B, real_T **R) // must guarantee correct size
+{
+
+    real_T resp[n1][n2];
+    for(int i=0; i<n1; i++) {
+        for(int j=0; j<n2; j++) {
+            resp[i][j]=0;
+        }
+    }
+
+    for (int i = 0; i < n1; i++) {
+        for (int j = 0; j < n2; j++) {
+            resp[i][j] = A[i][j]-B[i][j];
+        }
+    }
+
+    for (int i = 0; i < n1; i++) {
+        for (int j = 0; j < n2; j++)
+            R[i][j] = resp[i][j];
+    }
+
+}
+
+void MPC_Gen::matr_t(int_T n1,int_T n2,real_T **A, real_T **R) // must guarantee correct size
+{
+
+    real_T resp[n2][n1];
+    for(int i=0; i<n2; i++) {
+        for(int j=0; j<n1; j++) {
+            resp[i][j]=0;
+        }
+    }
+
+    for (int i = 0; i < n1; i++) {
+        for (int j = 0; j < n2; j++) {
+            resp[j][i] = A[i][j];
+        }
+    }
+
+    for (int i = 0; i < n2; i++) {
+        for (int j = 0; j < n1; j++)
+            R[i][j] = resp[i][j];
+    }
+
+}
+
+/* INPUT: A - array of pointers to rows of a square matrix having dimension N
+ *        Tol - small tolerance number to detect failure when the matrix is near degenerate
+ * OUTPUT: Matrix A is changed, it contains both matrices L-E and U as A=(L-E)+U such that P*A=L*U.
+ *        The permutation matrix is not stored as a matrix, but in an integer vector P of size N+1
+ *        containing column indexes where the permutation matrix has "1". The last element P[N]=S+N,
+ *        where S is the number of row exchanges needed for determinant computation, det(P)=(-1)^S
+ */
+int MPC_Gen::LUPDecompose(double **A, int N, double Tol, int *P)
+{
+
+    int i, j, k, imax;
+    double maxA, *ptr, absA;
+
+    for (i = 0; i <= N; i++)
+        P[i] = i; //Unit permutation matrix, P[N] initialized with N
+
+    for (i = 0; i < N; i++) {
+        maxA = 0.0;
+        imax = i;
+
+        for (k = i; k < N; k++)
+            if ((absA = fabs(A[k][i])) > maxA) {
+                maxA = absA;
+                imax = k;
+            }
+
+        if (maxA < Tol) return 0; //failure, matrix is degenerate
+
+        if (imax != i) {
+            //pivoting P
+            j = P[i];
+            P[i] = P[imax];
+            P[imax] = j;
+
+            //pivoting rows of A
+            ptr = A[i];
+            A[i] = A[imax];
+            A[imax] = ptr;
+
+            //counting pivots starting from N (for determinant)
+            P[N]++;
+        }
+
+        for (j = i + 1; j < N; j++) {
+            A[j][i] /= A[i][i];
+
+            for (k = i + 1; k < N; k++)
+                A[j][k] -= A[j][i] * A[i][k];
+        }
+    }
+
+    return 1;  //decomposition done
+}
+
+/* INPUT: A,P filled in LUPDecompose; N - dimension
+ * OUTPUT: IA is the inverse of the initial matrix
+ */
+void MPC_Gen::LUPInvert(double **A, int *P, int N, double **IA)
+{
+
+    for (int j = 0; j < N; j++) {
+        for (int i = 0; i < N; i++) {
+            if (P[i] == j)
+                IA[i][j] = 1.0;
+            else
+                IA[i][j] = 0.0;
+
+            for (int k = 0; k < i; k++)
+                IA[i][j] -= A[i][k] * IA[k][j];
+        }
+
+        for (int i = N - 1; i >= 0; i--) {
+            for (int k = i + 1; k < N; k++)
+                IA[i][j] -= A[i][k] * IA[k][j];
+
+            IA[i][j] = IA[i][j] / A[i][i];
+        }
+    }
+}//mvec_mult(p*ny,nx,Psi,xmk,el);
+void MPC_Gen::mvec_mult(int_T n1,int_T n2,real_T **A,real_T *B, real_T *R) // must guarantee correct size
+{
+
+    real_T resp[n1];
+    for(int i=0; i<n1; i++) {
+        resp[i]=0;
+    }
+
+    for (int i = 0; i < n1; i++) {
+        for (int k = 0; k < n2; k++)
+            resp[i] += A[i][k]*B[k];
+    }
+
+    for (int i = 0; i < n1; i++) {
+        R[i] = resp[i];
+    }
+
+}
+
+void MPC_Gen::mvec_multl(int_T n1,int_T n2,real_T *B,real_T **A, real_T *R) // rowvec(n1)*Matrix(n1,n2)
+{
+
+    real_T resp[n2];
+    for(int i=0; i<n2; i++) {
+        resp[i]=0;
+    }
+
+    for (int i = 0; i < n2; i++) {
+        for (int k = 0; k < n1; k++)
+            resp[i] += A[k][i]*B[k];
+    }
+
+    for (int i = 0; i < n2; i++) {
+        R[i] = resp[i];
+    }
+
+}
+
+;
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPCG.h	Tue Jul 16 19:39:11 2019 +0000
@@ -0,0 +1,105 @@
+#include <stddef.h>
+#include <cmath>
+#include <string.h>
+#include "rtwtypes.h"
+
+#define ny 1
+#define nu 1
+
+#define nx 3
+
+// Block signals and states (auto storage) for system '<Root>'
+typedef struct {
+  real_T uk_1[nu];                      // '<Root>/Data Store Memory2'
+  real_T duk_1[nu];                     // '<Root>/Data Store Memory4'
+} DWg;
+
+// External inputs (root inport signals with auto storage)
+typedef struct {
+  real_T sp[ny];                        // '<Root>/sp'
+  real_T x[nx];                         // '<Root>/y'
+} ExtUg;
+
+// External outputs (root outports fed by signals with auto storage)
+typedef struct {
+  real_T u[nu];                         // '<Root>/u'
+  real_T du[nu];
+} ExtYg;
+
+
+// Class declaration for model MPC_R
+class MPC_Gen {
+  // public data and function members
+ public:
+  // External inputs
+  ExtUg rtU;
+
+  // External outputs
+  ExtYg rtY;
+
+  // model initialize function
+  void initialize();
+
+  // model step function
+  void step();
+  
+  void matr_mult(int_T n1, int_T n2,int_T n3, real_T **A,real_T **B, real_T **R);
+    
+  void eye(int_T n, real_T **R);
+  
+  void matr_sum(int_T n1,int_T n2, real_T **A,real_T **B, real_T **R);
+  
+  void matr_dif(int_T n1,int_T n2, real_T **A,real_T **B, real_T **R);
+  
+  void matr_t(int_T n1,int_T n2,real_T **A, real_T **R);
+  
+  void mvec_mult(int_T n1,int_T n2,real_T **A,real_T *B, real_T *R);
+  
+  void mvec_multl(int_T n1,int_T n2,real_T *B,real_T **A, real_T *R);
+  
+  // Wikipedia LUP decomposition routines and inverse, perhaps use LUP solve? Gotta try
+  int LUPDecompose(double **A, int N, double Tol, int *P);
+  
+  void LUPInvert(double **A, int *P, int N, double **IA);
+  
+  // Constructor
+  MPC_Gen(int_T mi, int_T pi, real_T (&qi)[ny], real_T (&ri)[nu], real_T (&umaxi)[nu], real_T (&umini)[nu], real_T dumax);
+  
+  // Destructor
+  ~MPC_Gen();
+
+  // private data and function members
+ private:
+  // Block signals and states
+  DWg rtDW;
+  
+  const int_T m;
+  const int_T p;
+  const real_T (&q)[ny];
+  const real_T (&r)[nu];
+  const real_T (&umax)[nu];
+  const real_T (&umin)[nu];
+  const real_T dumax;
+  
+  //MPC_R(ys,y,Psi, Qbar, Thetabar, Itil, Dumax, Umin, Umax, H, nu, Mtil, m, p)
+  real_T** H;
+  real_T** Qbar;
+  real_T** Thetabar;
+  real_T** Psi;
+  real_T** Mtil;
+  real_T** Itil;
+  real_T** Ac;
+  real_T* Dumax;
+  real_T* Umin;
+  real_T* Umax;
+  real_T** Hinv;
+  real_T** nHinv;
+  real_T** nHinvtAc;
+  real_T** Qtheta;
+  
+  //real_T** tItil;
+  
+  real_T** P;
+  real_T* bc;
+  
+};

--- a/main.cpp	Thu Aug 17 20:59:23 2017 +0000
+++ b/main.cpp	Tue Jul 16 19:39:11 2019 +0000
@@ -1,32 +1,287 @@
 #include "mbed.h"
- 
+#include "math.h"
+#include <stddef.h>
+#include <stdio.h>
+#include "MPCG.h"
+#include "rtwtypes.h"
+//#include "model.h"
+#include <cmath>
+#include <string.h>
+
+#define Ts 0.5
+
+/*real_T Kf[3]= {0.0558186098945737,-0.0449160413973365,0.0718943654541056}; //G2a
+//real_T Kf[2]= {0.0643422853303416,0.0211965312138430}; //G2a V=10, w=0.05
+
+real_T Asys[3][3]={{1.42227428393352,-0.461045022885804,1.000000000000000},
+                   {1.000000000000000,0.000000000000000,0.000000000000000},
+                   {0.000000000000000,0.000000000000000,0.000000000000000}};
+real_T Bsys[3][1]={{0.000000000000000},
+                   {0.000000000000000},
+                   {1.000000000000000}};
+real_T Csys[1][3]={{8.43785634657016,-4.13612077882932,4.70222686018174}};*/ 
+
+real_T Kf[3]= {0.120920680963698,-0.0386450608225193,0.101875328885088}; //G1a
+
+real_T Asys[3][3]={{1.31015519093579,-0.403006959028916,1.000000000000000},
+                   {1.000000000000000,0.000000000000000,0.000000000000000},
+                   {0.000000000000000,0.000000000000000,1.000000000000000}};
+real_T Bsys[3][1]={{0.000000000000000},
+                   {0.000000000000000},
+                   {1.000000000000000}};
+real_T Csys[1][3]={{6.20493044813789,-1.49984604232648,1.78016957292750}}; 
+
 Ticker ticker;
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
-CAN can1(p9, p10);
-CAN can2(p30, p29);
-char counter = 0;
- 
-void send() {
-    printf("send()\n");
-    if(can1.write(CANMessage(1337, &counter, 1))) {
-        printf("wloop()\n");
-        counter++;
-        printf("Message sent: %d\n", counter);
-    } 
-    led1 = !led1;
+CAN can1(p9, p10,500000); 
+Serial pc(USBTX, USBRX);
+
+int n= 480; //Número de dados a serem enviados
+//char dados[1000]= {}; //Adicionar PRBS
+
+int aux;
+int handle;
+
+int estado1=0;
+int estado2=0;
+
+int amostra=0;
+
+char u=0;
+
+float vel=0;
+float ysp=15; //Qual a velocidade desejada (selecionada pelo usuario)
+float vlead=15; //Velocidade do carro da frente, usar dados do radar depois
+float vleadref=10;
+float tau_ref = 5;
+float vcruise=0;
+float dest=10; // distancia estimada (depois usar dado do radar)
+float uk=0;
+float duk=0;
+float xc[3]= {0,0,0};
+float yc= 0;
+
+// MPC Parameters
+int_T m = 5; //5
+int_T p = 10; //10
+real_T q[1] = {5}; //50
+real_T r[1] = {1000}; //1
+real_T umax[1] = {1.0};
+real_T umin[1] = {0.0};
+real_T dumax = 0.1; //0.1
+
+// Objeto MPC
+
+static MPC_Gen MPC(m,p,q,r,umax,umin,dumax);// Instance of Controller
+
+// Definições do controle ACC
+float Lcar= 4;
+float dsafety= 1;
+float timeheadway= 2; //de 0.8 a 2
+float Kp=0.1; //Precisa ajustar
+float dref=0;
+float derror=0;
+float vrel=0;
+float vref=0;
+int flag=0;
+int modo=0; //0 para CC, 1 para ACC
+int flagmem=0;
+
+CANMessage msgconfig;
+CANMessage input;
+CANMessage msg;
+
+float acc(float vdes, float vlead, float vel);
+
+void send()
+{
+    if(aux>60) { //Exp 1
+        //vlead=10;
+        vleadref=10;
+        tau_ref = 2.5;
+        vlead = (tau_ref/(Ts+tau_ref))*vlead + (Ts/(Ts+tau_ref))*vleadref;
+    }
+
+    /*if(aux>80 && aux <=160) { //Exp 2, trocar inicial vlead=10
+        //vlead=10;
+        vleadref=15;
+        tau_ref = 2.5;
+        vlead = (tau_ref/(Ts+tau_ref))*vlead + (Ts/(Ts+tau_ref))*vleadref;
+    }
+    if(aux>160) {
+        //vlead=10;
+        vleadref=20;
+        tau_ref = 2.5;
+        vlead = (tau_ref/(Ts+tau_ref))*vlead + (Ts/(Ts+tau_ref))*vleadref;
+    }*/
+
+    // Chamada da função ACC (loop externo)
+    vcruise=acc(ysp,vlead,vel);
+    if(vcruise>ysp){
+        vcruise=ysp;
+        }
+
+    // Lei de controle para cruzeiro
+    MPC.rtU.sp[0] = vcruise; //No modo acc será vcruise
+    
+    yc = Csys[0][0]*xc[0]+Csys[0][1]*xc[1]+Csys[0][2]*xc[2]; //Não é usado no MPC, apenas para ajustar o observador
+    xc[0] = xc[0]+Kf[0]*(vel-yc); 
+    xc[1] = xc[1]+Kf[1]*(vel-yc);
+    xc[2] = xc[2]+Kf[2]*(vel-yc);
+
+    MPC.rtU.x[0] = xc[0];
+    MPC.rtU.x[1] = xc[1];
+    MPC.rtU.x[2] = xc[2];
+
+    MPC.step();
+
+    uk=MPC.rtY.u[0];
+    duk=MPC.rtY.du[0];
+    if (uk>1)
+        uk=1;
+    if (uk<0)
+        uk=0;
+        
+    /*xc[0] = Asys[0][0]*xc[0]+Asys[0][1]*xc[1]+Asys[0][2]*xc[2];//+Bsys[0][0]*uk;
+    xc[1] = Asys[1][0]*xc[0]+Asys[1][1]*xc[1]+Asys[1][2]*xc[2];//+Bsys[1][0]*uk;
+    xc[2] = uk;*/
+    
+    xc[0] = Asys[0][0]*MPC.rtU.x[0]+Asys[0][1]*MPC.rtU.x[1]+Asys[0][2]*MPC.rtU.x[2]+Bsys[0][0]*duk;
+    xc[1] = Asys[1][0]*MPC.rtU.x[0]+Asys[1][1]*MPC.rtU.x[1]+Asys[1][2]*MPC.rtU.x[2]+Bsys[1][0]*duk;
+    xc[2] = Asys[2][0]*MPC.rtU.x[0]+Asys[2][1]*MPC.rtU.x[1]+Asys[2][2]*MPC.rtU.x[2]+Bsys[2][0]*duk;
+        
+    u=255*uk; // Converter para 0-255
+    input.data[3]=u;
+
+    // Garantir que a ECU está no modo de pedal simulado
+    can1.write(msgconfig);
+    
+    wait(0.001);
+
+    // Enviar mensagem de entrada
+    if(can1.write(input)) {
+        led1=!led1;
+    }
+
+    amostra=1;
+    wait(0.001);
 }
- 
-int main() {
-    printf("main()\n");
-    ticker.attach(&send, 1);
-    CANMessage msg;
-    while(1) {
-        printf("loop()\n");
-        if(can2.read(msg)) {
-            printf("Message received: %d\n", msg.data[0]);
-            led2 = !led2;
-        } 
-        wait(0.2);
+
+int main()
+{
+    pc.baud(9600);
+    wait(10);
+
+    pc.printf("---Configuration of CAN messages---\n\r");
+    led1=1;
+    led2=0;
+    estado1=1;
+    estado2=0;
+    msgconfig.format = CANStandard; //Ou CANExtended
+    msgconfig.id = 0x201;
+    msgconfig.len = 8;
+    msgconfig.data[0]='E'; //se nao der, tentar maiusculo
+    msgconfig.data[1]='C';
+    msgconfig.data[2]='U';
+    msgconfig.data[3]=1; //modo pedal sim, 1 liga, 0 desliga
+    msgconfig.data[4]=0; //modo operacao, 1 econ, 0 normal
+    msgconfig.data[5]=0; //modo marcha lenta, 0 default
+    msgconfig.data[6]=0;
+    msgconfig.data[7]=0;
+
+    pc.printf("---Initializing MPC---\n\r");
+    MPC.initialize();                                   // Inicializa o MPC
+    pc.printf("---MPC is ready---\n\r");  //////////////////////////////////////////// Imprimir todas sintonias
+    pc.printf("MPC tuned with: m=%d, p=%d, q=%f, r=%f, umax=%f, umin=%f, dumax=%f \n\r",m,p,q[0],r[0],umax[0],umin[0],dumax);
+    pc.printf("Outer loop controller tuned with: Lcar=%f, ds=%f, Th=%f, Kp=%f \n\r",Lcar,dsafety,timeheadway,Kp);
+    wait(2);
+    
+    while (estado1==1) {
+        if(can1.write(msgconfig)) {
+            led1=0;
+            estado1=0;
+        }
     }
-}
\ No newline at end of file
+
+    pc.printf("---CAN network is configured and MPC is ready, starting loop---\n\r");
+    led2=1;
+    estado1=0;
+    estado2=1;
+    handle = can1.filter(0x590, 0x7FF, CANStandard); //Filtro (não está sendo usado)
+    input.format = CANStandard;
+    input.id = 0x200;
+    input.len = 4;
+    input.data[0]='E';
+    input.data[1]='C';
+    input.data[2]='U';
+
+    msg.format = CANStandard;
+    msg.len = 8;
+
+    aux=0; //Indice auxiliar para selecionar dado a ser enviado
+    amostra=0;
+    ticker.attach(&send, Ts); // Selecionar periodo de amostragem
+
+    while (estado2==1) {
+
+        can1.read(msg);
+        if(msg.id ==0x590) {
+            vel=(msg.data[3]*255 +msg.data[2])*0.01/3.6; //msg.data[2] LSB e msg.data[3] MSB
+        }
+
+        /*if(msg.id ==0x590 && amostra==1) {
+            pc.printf("Message received number %d : ",aux);
+            for (int i = 0; i<msg.len; i++)
+                pc.printf("%02X ", msg.data[i]);
+            pc.printf("\n\r");
+            aux++;
+            amostra=0;
+        }*/
+
+        if(msg.id ==0x590 && amostra==1) {
+            pc.printf("Message received number %d : ",aux);
+            pc.printf("%f %f %f %f %f %f %x",ysp,vcruise,vlead,vel,dest,dref,u); //alterado
+            //pc.printf("\n\r");
+            pc.printf(" States: %f %f %f \n\r",xc[0],xc[1],xc[2]);
+            aux++;
+            amostra=0;
+        }
+
+        // Comandos para parar o loop após n amostras
+        /*if (aux>n) {
+            led2=0;
+            estado2=0;
+        }*/
+    }
+
+    // Ações para desligar o controle/identificação //
+    /*ticker.detach();
+    pc.printf("---All data have been sent---\n");
+    led1=0;
+    led2=0;
+    estado1=0;
+    estado2=0;*/
+}
+
+float acc(float vsp, float vlead, float vel)
+{
+    dref=Lcar+dsafety+vel*timeheadway; 
+    vrel=vlead-vel;
+    dest=dest+vrel*Ts; 
+    derror=dref-dest;
+    vref=vlead-Kp*derror;
+    if (vref>vsp) //Saturação
+        vref=vsp;
+        
+    if (vref>vsp)
+        return vsp; //CC Mode
+    else 
+        return vref;//ACC Mode
+}
+/*
+int round(float number)
+{
+    return (number >= 0) ? (int)(number + 0.5) : (int)(number - 0.5);
+}
+*/

--- a/mbed.bld	Thu Aug 17 20:59:23 2017 +0000
+++ b/mbed.bld	Tue Jul 16 19:39:11 2019 +0000
@@ -1,1 +1,1 @@
-https://mbed.org/users/mbed_official/code/mbed/builds/86740a56073b
\ No newline at end of file
+https://os.mbed.com/users/mbed_official/code/mbed/builds/994bdf8177cb
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/model.h	Tue Jul 16 19:39:11 2019 +0000
@@ -0,0 +1,15 @@
+real_T Ar[3][3]={{1.31015519093579,-0.403006959028916,1.000000000000000}, //G1a
+                   {1.000000000000000,0.000000000000000,0.000000000000000},
+                   {0.000000000000000,0.000000000000000,1.000000000000000}};
+real_T Br[3][1]={{0.000000000000000},
+                   {0.000000000000000},
+                   {1.000000000000000}};
+real_T Cr[1][3]={{6.20493044813789,-1.49984604232648,1.78016957292750}}; 
+
+/*real_T Ar[3][3]={{1.42227428393352,-0.461045022885804,1.000000000000000}, //G2a
+                   {1.000000000000000,0.000000000000000,0.000000000000000},
+                   {0.000000000000000,0.000000000000000,1.000000000000000}};
+real_T Br[3][1]={{0.000000000000000},
+                   {0.000000000000000},
+                   {1.000000000000000}};
+real_T Cr[1][3]={{8.43785634657016,-4.13612077882932,4.70222686018174}};*/
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rtwtypes.h	Tue Jul 16 19:39:11 2019 +0000
@@ -0,0 +1,101 @@
+//
+// Academic License - for use in teaching, academic research, and meeting
+// course requirements at degree granting institutions only.  Not for
+// government, commercial, or other organizational use.
+//
+// File: rtwtypes.h
+//
+// Code generated for Simulink model 'MPC_R'.
+//
+// Model version                  : 1.244
+// Simulink Coder version         : 8.12 (R2017a) 16-Feb-2017
+// C/C++ source code generated on : Tue May 30 12:27:13 2017
+//
+// Target selection: ert.tlc
+// Embedded hardware selection: ARM Compatible->ARM 7
+// Code generation objectives:
+//    1. Execution efficiency
+//    2. RAM efficiency
+// Validation result: Not run
+//
+
+#ifndef RTWTYPES_H
+#define RTWTYPES_H
+
+// Logical type definitions
+#if (!defined(__cplusplus))
+#  ifndef false
+#   define false                       (0U)
+#  endif
+
+#  ifndef true
+#   define true                        (1U)
+#  endif
+#endif
+
+//=======================================================================*
+//  Target hardware information
+//    Device type: ARM Compatible->ARM 7
+//    Number of bits:     char:   8    short:   16    int:  32
+//                        long:  32
+//                        native word size:  32
+//    Byte ordering: LittleEndian
+//    Signed integer division rounds to: Zero
+//    Shift right on a signed integer as arithmetic shift: on
+// =======================================================================
+
+//=======================================================================*
+//  Fixed width word size data types:                                     *
+//    int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     *
+//    uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   *
+//    real32_T, real64_T           - 32 and 64 bit floating point numbers *
+// =======================================================================
+typedef signed char int8_T;
+typedef unsigned char uint8_T;
+typedef short int16_T;
+typedef unsigned short uint16_T;
+typedef int int32_T;
+typedef unsigned int uint32_T;
+typedef float real32_T;
+typedef float real64_T;
+
+//===========================================================================*
+//  Generic type definitions: boolean_T, char_T, byte_T, int_T, uint_T,       *
+//                            real_T, time_T, ulong_T.                        *
+// ===========================================================================
+typedef float real_T;
+typedef float time_T;
+typedef unsigned char boolean_T;
+typedef int int_T;
+typedef unsigned int uint_T;
+typedef unsigned long ulong_T;
+typedef char char_T;
+typedef unsigned char uchar_T;
+typedef char_T byte_T;
+
+//=======================================================================*
+//  Min and Max:                                                          *
+//    int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     *
+//    uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   *
+// =======================================================================
+#define MAX_int8_T                     ((int8_T)(127))
+#define MIN_int8_T                     ((int8_T)(-128))
+#define MAX_uint8_T                    ((uint8_T)(255U))
+#define MAX_int16_T                    ((int16_T)(32767))
+#define MIN_int16_T                    ((int16_T)(-32768))
+#define MAX_uint16_T                   ((uint16_T)(65535U))
+#define MAX_int32_T                    ((int32_T)(2147483647))
+#define MIN_int32_T                    ((int32_T)(-2147483647-1))
+#define MAX_uint32_T                   ((uint32_T)(0xFFFFFFFFU))
+
+// Block D-Work pointer type
+typedef void * pointer_T;
+
+#endif                                 // RTWTYPES_H
+
+//
+// File trailer for generated code.
+//
+// [EOF]
+//
+