Diff: main.cpp

Revision:

1:7c69af307591

Parent:

0:68f5325bc455

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