File content as of revision 1:7c69af307591:

#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,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()
{
    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;
        }
    }

    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);
}
*/