Laboratório Controle Aplicado / Mbed 2 deprecated CAN_MPC

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@1:7c69af307591, 2019-07-16 (annotated)

Committer:
lcaepusp
Date:
Tue Jul 16 19:39:11 2019 +0000
Revision:
1:7c69af307591
Parent:
0:68f5325bc455 
Final version mpc

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lcaepusp 0:68f5325bc455 1 #include "mbed.h"
lcaepusp 1:7c69af307591 2 #include "math.h"
lcaepusp 1:7c69af307591 3 #include <stddef.h>
lcaepusp 1:7c69af307591 4 #include <stdio.h>
lcaepusp 1:7c69af307591 5 #include "MPCG.h"
lcaepusp 1:7c69af307591 6 #include "rtwtypes.h"
lcaepusp 1:7c69af307591 7 //#include "model.h"
lcaepusp 1:7c69af307591 8 #include <cmath>
lcaepusp 1:7c69af307591 9 #include <string.h>
lcaepusp 1:7c69af307591 10
lcaepusp 1:7c69af307591 11 #define Ts 0.5
lcaepusp 1:7c69af307591 12
lcaepusp 1:7c69af307591 13 /*real_T Kf[3]= {0.0558186098945737,-0.0449160413973365,0.0718943654541056}; //G2a
lcaepusp 1:7c69af307591 14 //real_T Kf[2]= {0.0643422853303416,0.0211965312138430}; //G2a V=10, w=0.05
lcaepusp 1:7c69af307591 15
lcaepusp 1:7c69af307591 16 real_T Asys[3][3]={{1.42227428393352,-0.461045022885804,1.000000000000000},
lcaepusp 1:7c69af307591 17 {1.000000000000000,0.000000000000000,0.000000000000000},
lcaepusp 1:7c69af307591 18 {0.000000000000000,0.000000000000000,0.000000000000000}};
lcaepusp 1:7c69af307591 19 real_T Bsys[3][1]={{0.000000000000000},
lcaepusp 1:7c69af307591 20 {0.000000000000000},
lcaepusp 1:7c69af307591 21 {1.000000000000000}};
lcaepusp 1:7c69af307591 22 real_T Csys[1][3]={{8.43785634657016,-4.13612077882932,4.70222686018174}};*/
lcaepusp 1:7c69af307591 23
lcaepusp 1:7c69af307591 24 real_T Kf[3]= {0.120920680963698,-0.0386450608225193,0.101875328885088}; //G1a
lcaepusp 1:7c69af307591 25
lcaepusp 1:7c69af307591 26 real_T Asys[3][3]={{1.31015519093579,-0.403006959028916,1.000000000000000},
lcaepusp 1:7c69af307591 27 {1.000000000000000,0.000000000000000,0.000000000000000},
lcaepusp 1:7c69af307591 28 {0.000000000000000,0.000000000000000,1.000000000000000}};
lcaepusp 1:7c69af307591 29 real_T Bsys[3][1]={{0.000000000000000},
lcaepusp 1:7c69af307591 30 {0.000000000000000},
lcaepusp 1:7c69af307591 31 {1.000000000000000}};
lcaepusp 1:7c69af307591 32 real_T Csys[1][3]={{6.20493044813789,-1.49984604232648,1.78016957292750}};
lcaepusp 1:7c69af307591 33
lcaepusp 0:68f5325bc455 34 Ticker ticker;
lcaepusp 0:68f5325bc455 35 DigitalOut led1(LED1);
lcaepusp 0:68f5325bc455 36 DigitalOut led2(LED2);
lcaepusp 1:7c69af307591 37 CAN can1(p9, p10,500000);
lcaepusp 1:7c69af307591 38 Serial pc(USBTX, USBRX);
lcaepusp 1:7c69af307591 39
lcaepusp 1:7c69af307591 40 int n= 480; //Número de dados a serem enviados
lcaepusp 1:7c69af307591 41 //char dados[1000]= {}; //Adicionar PRBS
lcaepusp 1:7c69af307591 42
lcaepusp 1:7c69af307591 43 int aux;
lcaepusp 1:7c69af307591 44 int handle;
lcaepusp 1:7c69af307591 45
lcaepusp 1:7c69af307591 46 int estado1=0;
lcaepusp 1:7c69af307591 47 int estado2=0;
lcaepusp 1:7c69af307591 48
lcaepusp 1:7c69af307591 49 int amostra=0;
lcaepusp 1:7c69af307591 50
lcaepusp 1:7c69af307591 51 char u=0;
lcaepusp 1:7c69af307591 52
lcaepusp 1:7c69af307591 53 float vel=0;
lcaepusp 1:7c69af307591 54 float ysp=15; //Qual a velocidade desejada (selecionada pelo usuario)
lcaepusp 1:7c69af307591 55 float vlead=15; //Velocidade do carro da frente, usar dados do radar depois
lcaepusp 1:7c69af307591 56 float vleadref=10;
lcaepusp 1:7c69af307591 57 float tau_ref = 5;
lcaepusp 1:7c69af307591 58 float vcruise=0;
lcaepusp 1:7c69af307591 59 float dest=10; // distancia estimada (depois usar dado do radar)
lcaepusp 1:7c69af307591 60 float uk=0;
lcaepusp 1:7c69af307591 61 float duk=0;
lcaepusp 1:7c69af307591 62 float xc[3]= {0,0,0};
lcaepusp 1:7c69af307591 63 float yc= 0;
lcaepusp 1:7c69af307591 64
lcaepusp 1:7c69af307591 65 // MPC Parameters
lcaepusp 1:7c69af307591 66 int_T m = 5; //5
lcaepusp 1:7c69af307591 67 int_T p = 10; //10
lcaepusp 1:7c69af307591 68 real_T q[1] = {5}; //50
lcaepusp 1:7c69af307591 69 real_T r[1] = {1000}; //1
lcaepusp 1:7c69af307591 70 real_T umax[1] = {1.0};
lcaepusp 1:7c69af307591 71 real_T umin[1] = {0.0};
lcaepusp 1:7c69af307591 72 real_T dumax = 0.1; //0.1
lcaepusp 1:7c69af307591 73
lcaepusp 1:7c69af307591 74 // Objeto MPC
lcaepusp 1:7c69af307591 75
lcaepusp 1:7c69af307591 76 static MPC_Gen MPC(m,p,q,r,umax,umin,dumax);// Instance of Controller
lcaepusp 1:7c69af307591 77
lcaepusp 1:7c69af307591 78 // Definições do controle ACC
lcaepusp 1:7c69af307591 79 float Lcar= 4;
lcaepusp 1:7c69af307591 80 float dsafety= 1;
lcaepusp 1:7c69af307591 81 float timeheadway= 2; //de 0.8 a 2
lcaepusp 1:7c69af307591 82 float Kp=0.1; //Precisa ajustar
lcaepusp 1:7c69af307591 83 float dref=0;
lcaepusp 1:7c69af307591 84 float derror=0;
lcaepusp 1:7c69af307591 85 float vrel=0;
lcaepusp 1:7c69af307591 86 float vref=0;
lcaepusp 1:7c69af307591 87 int flag=0;
lcaepusp 1:7c69af307591 88 int modo=0; //0 para CC, 1 para ACC
lcaepusp 1:7c69af307591 89 int flagmem=0;
lcaepusp 1:7c69af307591 90
lcaepusp 1:7c69af307591 91 CANMessage msgconfig;
lcaepusp 1:7c69af307591 92 CANMessage input;
lcaepusp 1:7c69af307591 93 CANMessage msg;
lcaepusp 1:7c69af307591 94
lcaepusp 1:7c69af307591 95 float acc(float vdes, float vlead, float vel);
lcaepusp 1:7c69af307591 96
lcaepusp 1:7c69af307591 97 void send()
lcaepusp 1:7c69af307591 98 {
lcaepusp 1:7c69af307591 99 if(aux>60) { //Exp 1
lcaepusp 1:7c69af307591 100 //vlead=10;
lcaepusp 1:7c69af307591 101 vleadref=10;
lcaepusp 1:7c69af307591 102 tau_ref = 2.5;
lcaepusp 1:7c69af307591 103 vlead = (tau_ref/(Ts+tau_ref))*vlead + (Ts/(Ts+tau_ref))*vleadref;
lcaepusp 1:7c69af307591 104 }
lcaepusp 1:7c69af307591 105
lcaepusp 1:7c69af307591 106 /*if(aux>80 && aux <=160) { //Exp 2, trocar inicial vlead=10
lcaepusp 1:7c69af307591 107 //vlead=10;
lcaepusp 1:7c69af307591 108 vleadref=15;
lcaepusp 1:7c69af307591 109 tau_ref = 2.5;
lcaepusp 1:7c69af307591 110 vlead = (tau_ref/(Ts+tau_ref))*vlead + (Ts/(Ts+tau_ref))*vleadref;
lcaepusp 1:7c69af307591 111 }
lcaepusp 1:7c69af307591 112 if(aux>160) {
lcaepusp 1:7c69af307591 113 //vlead=10;
lcaepusp 1:7c69af307591 114 vleadref=20;
lcaepusp 1:7c69af307591 115 tau_ref = 2.5;
lcaepusp 1:7c69af307591 116 vlead = (tau_ref/(Ts+tau_ref))*vlead + (Ts/(Ts+tau_ref))*vleadref;
lcaepusp 1:7c69af307591 117 }*/
lcaepusp 1:7c69af307591 118
lcaepusp 1:7c69af307591 119 // Chamada da função ACC (loop externo)
lcaepusp 1:7c69af307591 120 vcruise=acc(ysp,vlead,vel);
lcaepusp 1:7c69af307591 121 if(vcruise>ysp){
lcaepusp 1:7c69af307591 122 vcruise=ysp;
lcaepusp 1:7c69af307591 123 }
lcaepusp 1:7c69af307591 124
lcaepusp 1:7c69af307591 125 // Lei de controle para cruzeiro
lcaepusp 1:7c69af307591 126 MPC.rtU.sp[0] = vcruise; //No modo acc será vcruise
lcaepusp 1:7c69af307591 127
lcaepusp 1:7c69af307591 128 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
lcaepusp 1:7c69af307591 129 xc[0] = xc[0]+Kf[0]*(vel-yc);
lcaepusp 1:7c69af307591 130 xc[1] = xc[1]+Kf[1]*(vel-yc);
lcaepusp 1:7c69af307591 131 xc[2] = xc[2]+Kf[2]*(vel-yc);
lcaepusp 1:7c69af307591 132
lcaepusp 1:7c69af307591 133 MPC.rtU.x[0] = xc[0];
lcaepusp 1:7c69af307591 134 MPC.rtU.x[1] = xc[1];
lcaepusp 1:7c69af307591 135 MPC.rtU.x[2] = xc[2];
lcaepusp 1:7c69af307591 136
lcaepusp 1:7c69af307591 137 MPC.step();
lcaepusp 1:7c69af307591 138
lcaepusp 1:7c69af307591 139 uk=MPC.rtY.u[0];
lcaepusp 1:7c69af307591 140 duk=MPC.rtY.du[0];
lcaepusp 1:7c69af307591 141 if (uk>1)
lcaepusp 1:7c69af307591 142 uk=1;
lcaepusp 1:7c69af307591 143 if (uk<0)
lcaepusp 1:7c69af307591 144 uk=0;
lcaepusp 1:7c69af307591 145
lcaepusp 1:7c69af307591 146 /*xc[0] = Asys[0][0]*xc[0]+Asys[0][1]*xc[1]+Asys[0][2]*xc[2];//+Bsys[0][0]*uk;
lcaepusp 1:7c69af307591 147 xc[1] = Asys[1][0]*xc[0]+Asys[1][1]*xc[1]+Asys[1][2]*xc[2];//+Bsys[1][0]*uk;
lcaepusp 1:7c69af307591 148 xc[2] = uk;*/
lcaepusp 1:7c69af307591 149
lcaepusp 1:7c69af307591 150 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;
lcaepusp 1:7c69af307591 151 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;
lcaepusp 1:7c69af307591 152 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;
lcaepusp 1:7c69af307591 153
lcaepusp 1:7c69af307591 154 u=255*uk; // Converter para 0-255
lcaepusp 1:7c69af307591 155 input.data[3]=u;
lcaepusp 1:7c69af307591 156
lcaepusp 1:7c69af307591 157 // Garantir que a ECU está no modo de pedal simulado
lcaepusp 1:7c69af307591 158 can1.write(msgconfig);
lcaepusp 1:7c69af307591 159
lcaepusp 1:7c69af307591 160 wait(0.001);
lcaepusp 1:7c69af307591 161
lcaepusp 1:7c69af307591 162 // Enviar mensagem de entrada
lcaepusp 1:7c69af307591 163 if(can1.write(input)) {
lcaepusp 1:7c69af307591 164 led1=!led1;
lcaepusp 1:7c69af307591 165 }
lcaepusp 1:7c69af307591 166
lcaepusp 1:7c69af307591 167 amostra=1;
lcaepusp 1:7c69af307591 168 wait(0.001);
lcaepusp 0:68f5325bc455 169 }
lcaepusp 1:7c69af307591 170
lcaepusp 1:7c69af307591 171 int main()
lcaepusp 1:7c69af307591 172 {
lcaepusp 1:7c69af307591 173 pc.baud(9600);
lcaepusp 1:7c69af307591 174 wait(10);
lcaepusp 1:7c69af307591 175
lcaepusp 1:7c69af307591 176 pc.printf("---Configuration of CAN messages---\n\r");
lcaepusp 1:7c69af307591 177 led1=1;
lcaepusp 1:7c69af307591 178 led2=0;
lcaepusp 1:7c69af307591 179 estado1=1;
lcaepusp 1:7c69af307591 180 estado2=0;
lcaepusp 1:7c69af307591 181 msgconfig.format = CANStandard; //Ou CANExtended
lcaepusp 1:7c69af307591 182 msgconfig.id = 0x201;
lcaepusp 1:7c69af307591 183 msgconfig.len = 8;
lcaepusp 1:7c69af307591 184 msgconfig.data[0]='E'; //se nao der, tentar maiusculo
lcaepusp 1:7c69af307591 185 msgconfig.data[1]='C';
lcaepusp 1:7c69af307591 186 msgconfig.data[2]='U';
lcaepusp 1:7c69af307591 187 msgconfig.data[3]=1; //modo pedal sim, 1 liga, 0 desliga
lcaepusp 1:7c69af307591 188 msgconfig.data[4]=0; //modo operacao, 1 econ, 0 normal
lcaepusp 1:7c69af307591 189 msgconfig.data[5]=0; //modo marcha lenta, 0 default
lcaepusp 1:7c69af307591 190 msgconfig.data[6]=0;
lcaepusp 1:7c69af307591 191 msgconfig.data[7]=0;
lcaepusp 1:7c69af307591 192
lcaepusp 1:7c69af307591 193 pc.printf("---Initializing MPC---\n\r");
lcaepusp 1:7c69af307591 194 MPC.initialize(); // Inicializa o MPC
lcaepusp 1:7c69af307591 195 pc.printf("---MPC is ready---\n\r"); //////////////////////////////////////////// Imprimir todas sintonias
lcaepusp 1:7c69af307591 196 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);
lcaepusp 1:7c69af307591 197 pc.printf("Outer loop controller tuned with: Lcar=%f, ds=%f, Th=%f, Kp=%f \n\r",Lcar,dsafety,timeheadway,Kp);
lcaepusp 1:7c69af307591 198 wait(2);
lcaepusp 1:7c69af307591 199
lcaepusp 1:7c69af307591 200 while (estado1==1) {
lcaepusp 1:7c69af307591 201 if(can1.write(msgconfig)) {
lcaepusp 1:7c69af307591 202 led1=0;
lcaepusp 1:7c69af307591 203 estado1=0;
lcaepusp 1:7c69af307591 204 }
lcaepusp 0:68f5325bc455 205 }
lcaepusp 1:7c69af307591 206
lcaepusp 1:7c69af307591 207 pc.printf("---CAN network is configured and MPC is ready, starting loop---\n\r");
lcaepusp 1:7c69af307591 208 led2=1;
lcaepusp 1:7c69af307591 209 estado1=0;
lcaepusp 1:7c69af307591 210 estado2=1;
lcaepusp 1:7c69af307591 211 handle = can1.filter(0x590, 0x7FF, CANStandard); //Filtro (não está sendo usado)
lcaepusp 1:7c69af307591 212 input.format = CANStandard;
lcaepusp 1:7c69af307591 213 input.id = 0x200;
lcaepusp 1:7c69af307591 214 input.len = 4;
lcaepusp 1:7c69af307591 215 input.data[0]='E';
lcaepusp 1:7c69af307591 216 input.data[1]='C';
lcaepusp 1:7c69af307591 217 input.data[2]='U';
lcaepusp 1:7c69af307591 218
lcaepusp 1:7c69af307591 219 msg.format = CANStandard;
lcaepusp 1:7c69af307591 220 msg.len = 8;
lcaepusp 1:7c69af307591 221
lcaepusp 1:7c69af307591 222 aux=0; //Indice auxiliar para selecionar dado a ser enviado
lcaepusp 1:7c69af307591 223 amostra=0;
lcaepusp 1:7c69af307591 224 ticker.attach(&send, Ts); // Selecionar periodo de amostragem
lcaepusp 1:7c69af307591 225
lcaepusp 1:7c69af307591 226 while (estado2==1) {
lcaepusp 1:7c69af307591 227
lcaepusp 1:7c69af307591 228 can1.read(msg);
lcaepusp 1:7c69af307591 229 if(msg.id ==0x590) {
lcaepusp 1:7c69af307591 230 vel=(msg.data[3]*255 +msg.data[2])*0.01/3.6; //msg.data[2] LSB e msg.data[3] MSB
lcaepusp 1:7c69af307591 231 }
lcaepusp 1:7c69af307591 232
lcaepusp 1:7c69af307591 233 /*if(msg.id ==0x590 && amostra==1) {
lcaepusp 1:7c69af307591 234 pc.printf("Message received number %d : ",aux);
lcaepusp 1:7c69af307591 235 for (int i = 0; i<msg.len; i++)
lcaepusp 1:7c69af307591 236 pc.printf("%02X ", msg.data[i]);
lcaepusp 1:7c69af307591 237 pc.printf("\n\r");
lcaepusp 1:7c69af307591 238 aux++;
lcaepusp 1:7c69af307591 239 amostra=0;
lcaepusp 1:7c69af307591 240 }*/
lcaepusp 1:7c69af307591 241
lcaepusp 1:7c69af307591 242 if(msg.id ==0x590 && amostra==1) {
lcaepusp 1:7c69af307591 243 pc.printf("Message received number %d : ",aux);
lcaepusp 1:7c69af307591 244 pc.printf("%f %f %f %f %f %f %x",ysp,vcruise,vlead,vel,dest,dref,u); //alterado
lcaepusp 1:7c69af307591 245 //pc.printf("\n\r");
lcaepusp 1:7c69af307591 246 pc.printf(" States: %f %f %f \n\r",xc[0],xc[1],xc[2]);
lcaepusp 1:7c69af307591 247 aux++;
lcaepusp 1:7c69af307591 248 amostra=0;
lcaepusp 1:7c69af307591 249 }
lcaepusp 1:7c69af307591 250
lcaepusp 1:7c69af307591 251 // Comandos para parar o loop após n amostras
lcaepusp 1:7c69af307591 252 /*if (aux>n) {
lcaepusp 1:7c69af307591 253 led2=0;
lcaepusp 1:7c69af307591 254 estado2=0;
lcaepusp 1:7c69af307591 255 }*/
lcaepusp 1:7c69af307591 256 }
lcaepusp 1:7c69af307591 257
lcaepusp 1:7c69af307591 258 // Ações para desligar o controle/identificação //
lcaepusp 1:7c69af307591 259 /*ticker.detach();
lcaepusp 1:7c69af307591 260 pc.printf("---All data have been sent---\n");
lcaepusp 1:7c69af307591 261 led1=0;
lcaepusp 1:7c69af307591 262 led2=0;
lcaepusp 1:7c69af307591 263 estado1=0;
lcaepusp 1:7c69af307591 264 estado2=0;*/
lcaepusp 1:7c69af307591 265 }
lcaepusp 1:7c69af307591 266
lcaepusp 1:7c69af307591 267 float acc(float vsp, float vlead, float vel)
lcaepusp 1:7c69af307591 268 {
lcaepusp 1:7c69af307591 269 dref=Lcar+dsafety+vel*timeheadway;
lcaepusp 1:7c69af307591 270 vrel=vlead-vel;
lcaepusp 1:7c69af307591 271 dest=dest+vrel*Ts;
lcaepusp 1:7c69af307591 272 derror=dref-dest;
lcaepusp 1:7c69af307591 273 vref=vlead-Kp*derror;
lcaepusp 1:7c69af307591 274 if (vref>vsp) //Saturação
lcaepusp 1:7c69af307591 275 vref=vsp;
lcaepusp 1:7c69af307591 276
lcaepusp 1:7c69af307591 277 if (vref>vsp)
lcaepusp 1:7c69af307591 278 return vsp; //CC Mode
lcaepusp 1:7c69af307591 279 else
lcaepusp 1:7c69af307591 280 return vref;//ACC Mode
lcaepusp 1:7c69af307591 281 }
lcaepusp 1:7c69af307591 282 /*
lcaepusp 1:7c69af307591 283 int round(float number)
lcaepusp 1:7c69af307591 284 {
lcaepusp 1:7c69af307591 285 return (number >= 0) ? (int)(number + 0.5) : (int)(number - 0.5);
lcaepusp 1:7c69af307591 286 }
lcaepusp 1:7c69af307591 287 */