Gabriel das Neves / Mbed 2 deprecated Pendulo_roda_LPV

controle pendulo roda reacao

Dependencies:   mbed MatrixMath QEI Matrix USBDevice

main.cpp@0:200d9e41a3cf, 2020-03-11 (annotated)

Committer:
gabrielpdn
Date:
Wed Mar 11 16:50:23 2020 +0000
Revision:
0:200d9e41a3cf
versao 1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gabrielpdn 0:200d9e41a3cf 1 #include "mbed.h"
gabrielpdn 0:200d9e41a3cf 2 #include "math.h"
gabrielpdn 0:200d9e41a3cf 3 #include "USBSerial.h"
gabrielpdn 0:200d9e41a3cf 4 #include "QEI.h"
gabrielpdn 0:200d9e41a3cf 5 #include "Matrix.h"
gabrielpdn 0:200d9e41a3cf 6 #include "MatrixMath.h"
gabrielpdn 0:200d9e41a3cf 7
gabrielpdn 0:200d9e41a3cf 8
gabrielpdn 0:200d9e41a3cf 9 #define Ts 0.01
gabrielpdn 0:200d9e41a3cf 10 #define en2rad_roda 0.006698491798699
gabrielpdn 0:200d9e41a3cf 11 //#define en2rad_pend 0.001591485640116
gabrielpdn 0:200d9e41a3cf 12 //#define en2rad_roda 0.014660765717029
gabrielpdn 0:200d9e41a3cf 13 #define en2rad_pend 0.001570796326795
gabrielpdn 0:200d9e41a3cf 14 #define rad2graus 57.295779513082320
gabrielpdn 0:200d9e41a3cf 15 #define pi 3.141592653589793
gabrielpdn 0:200d9e41a3cf 16 #define omega_nd 210
gabrielpdn 0:200d9e41a3cf 17
gabrielpdn 0:200d9e41a3cf 18 USBSerial PC;
gabrielpdn 0:200d9e41a3cf 19 Ticker tick;
gabrielpdn 0:200d9e41a3cf 20
gabrielpdn 0:200d9e41a3cf 21 QEI enc_roda (PTB17, PTD0, NC, 469);
gabrielpdn 0:200d9e41a3cf 22 QEI enc_pend (PTA12, PTA13, NC, 1000);
gabrielpdn 0:200d9e41a3cf 23 //QEI enc_roda (PTC2, PTC1, NC, 341.2);
gabrielpdn 0:200d9e41a3cf 24 //QEI enc_pend (PTD6, PTD5, NC, 1974);
gabrielpdn 0:200d9e41a3cf 25
gabrielpdn 0:200d9e41a3cf 26 DigitalOut OnBoardLed (D13);
gabrielpdn 0:200d9e41a3cf 27 int aux = 0;
gabrielpdn 0:200d9e41a3cf 28 float pac = 0.0;
gabrielpdn 0:200d9e41a3cf 29
gabrielpdn 0:200d9e41a3cf 30 // Variaveis encoders
gabrielpdn 0:200d9e41a3cf 31 float theta = 0.0, theta_1 = 0.0, dtheta = 0.0;
gabrielpdn 0:200d9e41a3cf 32 float alpha = 0.0, alpha_1 = 0.0, dalpha = 0.0;
gabrielpdn 0:200d9e41a3cf 33 float angulo = 0.0;
gabrielpdn 0:200d9e41a3cf 34
gabrielpdn 0:200d9e41a3cf 35 float xc1 = 0.0, xc2 = 0.0, xc3 = 0.0, xc4 = 0.0;
gabrielpdn 0:200d9e41a3cf 36 float dxc1 = 0.0, dxc2 = 0.0, dxc3 = 0.0, dxc4 = 0.0;
gabrielpdn 0:200d9e41a3cf 37
gabrielpdn 0:200d9e41a3cf 38 // Motor
gabrielpdn 0:200d9e41a3cf 39 DigitalOut InA(PTD5);
gabrielpdn 0:200d9e41a3cf 40 DigitalOut InB(PTC1);
gabrielpdn 0:200d9e41a3cf 41 PwmOut V(PTD6);
gabrielpdn 0:200d9e41a3cf 42 float th5=0.0;
gabrielpdn 0:200d9e41a3cf 43 float th5_1=0.0;
gabrielpdn 0:200d9e41a3cf 44 float dth5=0.0;
gabrielpdn 0:200d9e41a3cf 45
gabrielpdn 0:200d9e41a3cf 46 // Alimentacao encoder motor
gabrielpdn 0:200d9e41a3cf 47 DigitalOut vcc_enc (PTB0);
gabrielpdn 0:200d9e41a3cf 48
gabrielpdn 0:200d9e41a3cf 49 // Variaveis de medida
gabrielpdn 0:200d9e41a3cf 50 int N=0;
gabrielpdn 0:200d9e41a3cf 51 const int tempo = 15/Ts;
gabrielpdn 0:200d9e41a3cf 52 float th[tempo], dth[tempo], alp[tempo], dalp[tempo], u[tempo];
gabrielpdn 0:200d9e41a3cf 53
gabrielpdn 0:200d9e41a3cf 54
gabrielpdn 0:200d9e41a3cf 55 // variavel controle
gabrielpdn 0:200d9e41a3cf 56 Matrix Z1(1,3);
gabrielpdn 0:200d9e41a3cf 57 Matrix Z2(1,3);
gabrielpdn 0:200d9e41a3cf 58 Matrix Z3(1,3);
gabrielpdn 0:200d9e41a3cf 59 Matrix Y1(3,3);
gabrielpdn 0:200d9e41a3cf 60 Matrix Y2(3,3);
gabrielpdn 0:200d9e41a3cf 61 Matrix Y3(3,3);
gabrielpdn 0:200d9e41a3cf 62 Matrix Z(1,3);
gabrielpdn 0:200d9e41a3cf 63 Matrix Y(3,3);
gabrielpdn 0:200d9e41a3cf 64 Matrix K(1,3);
gabrielpdn 0:200d9e41a3cf 65
gabrielpdn 0:200d9e41a3cf 66
gabrielpdn 0:200d9e41a3cf 67 float lim_1 = -0.261799387799149;
gabrielpdn 0:200d9e41a3cf 68 float lim_2 = 0.261799387799149;
gabrielpdn 0:200d9e41a3cf 69 float rho1 = 0.0, rho2 = 0.0;
gabrielpdn 0:200d9e41a3cf 70 float u_pwm = 0.0;
gabrielpdn 0:200d9e41a3cf 71
gabrielpdn 0:200d9e41a3cf 72 // drive motor
gabrielpdn 0:200d9e41a3cf 73 void drive(float u)
gabrielpdn 0:200d9e41a3cf 74 {
gabrielpdn 0:200d9e41a3cf 75 int direction=0;
gabrielpdn 0:200d9e41a3cf 76 if (u>0)direction=1;
gabrielpdn 0:200d9e41a3cf 77 if (u<0)direction=2;
gabrielpdn 0:200d9e41a3cf 78 if(u==0)direction=0;
gabrielpdn 0:200d9e41a3cf 79 switch(direction) {
gabrielpdn 0:200d9e41a3cf 80 case 0:
gabrielpdn 0:200d9e41a3cf 81 V.write(u);
gabrielpdn 0:200d9e41a3cf 82 InA=0;
gabrielpdn 0:200d9e41a3cf 83 InB=0;
gabrielpdn 0:200d9e41a3cf 84 break;
gabrielpdn 0:200d9e41a3cf 85 case 1:
gabrielpdn 0:200d9e41a3cf 86 V.write(u);
gabrielpdn 0:200d9e41a3cf 87 InA=1;
gabrielpdn 0:200d9e41a3cf 88 InB=0;
gabrielpdn 0:200d9e41a3cf 89 break;
gabrielpdn 0:200d9e41a3cf 90 case 2:
gabrielpdn 0:200d9e41a3cf 91 V.write(-u);
gabrielpdn 0:200d9e41a3cf 92 InA=0;
gabrielpdn 0:200d9e41a3cf 93 InB=1;
gabrielpdn 0:200d9e41a3cf 94 break;
gabrielpdn 0:200d9e41a3cf 95 default:
gabrielpdn 0:200d9e41a3cf 96 V.write(0.0);
gabrielpdn 0:200d9e41a3cf 97 InA=0;
gabrielpdn 0:200d9e41a3cf 98 InB=0;
gabrielpdn 0:200d9e41a3cf 99 break;
gabrielpdn 0:200d9e41a3cf 100 }
gabrielpdn 0:200d9e41a3cf 101 }
gabrielpdn 0:200d9e41a3cf 102
gabrielpdn 0:200d9e41a3cf 103 // Interrupção
gabrielpdn 0:200d9e41a3cf 104 void inverte() {
gabrielpdn 0:200d9e41a3cf 105 aux=1;
gabrielpdn 0:200d9e41a3cf 106 //N++;
gabrielpdn 0:200d9e41a3cf 107 }
gabrielpdn 0:200d9e41a3cf 108
gabrielpdn 0:200d9e41a3cf 109 int main ()
gabrielpdn 0:200d9e41a3cf 110 {
gabrielpdn 0:200d9e41a3cf 111 Z1 << 0.0528635583471 << 55.1775667591881 << -334.6107239814101;
gabrielpdn 0:200d9e41a3cf 112 Z2 << 2.2178052729892 << 89.4963037908543 << -632.0606939025414;
gabrielpdn 0:200d9e41a3cf 113 Z3 << 0.0527550080233 << 55.1818544517557 << -334.6407739018230;
gabrielpdn 0:200d9e41a3cf 114 Y1 << 52.229273 << -293.202697 << -1000.523522
gabrielpdn 0:200d9e41a3cf 115 << -351.739401 << 8417.246435 << -49726.191528
gabrielpdn 0:200d9e41a3cf 116 << -487.165029 << -53707.951308 << 505666.562609;
gabrielpdn 0:200d9e41a3cf 117 Y2 << 79.768194 << -492.424621 << -1138.974682
gabrielpdn 0:200d9e41a3cf 118 << -454.513497 << 15400.080131 << -103976.618630
gabrielpdn 0:200d9e41a3cf 119 << -1476.885923 << -101235.956011 << 992352.224275;
gabrielpdn 0:200d9e41a3cf 120 Y3 << 52.229040 << -293.201469 << -1000.518230
gabrielpdn 0:200d9e41a3cf 121 << -351.740390 << 8417.269581 << -49726.325976
gabrielpdn 0:200d9e41a3cf 122 << -487.140469 << -53708.234657 << 505667.351490;
gabrielpdn 0:200d9e41a3cf 123
gabrielpdn 0:200d9e41a3cf 124 // LQR contuinuo
gabrielpdn 0:200d9e41a3cf 125 float K1 = 6.307583719273756;
gabrielpdn 0:200d9e41a3cf 126 float K2 = 0.808578597390600;
gabrielpdn 0:200d9e41a3cf 127 float K3 = 0.091291588353526;
gabrielpdn 0:200d9e41a3cf 128
gabrielpdn 0:200d9e41a3cf 129 vcc_enc = 1;
gabrielpdn 0:200d9e41a3cf 130 OnBoardLed = 1;
gabrielpdn 0:200d9e41a3cf 131 V.period(0.0001);
gabrielpdn 0:200d9e41a3cf 132 wait(5);
gabrielpdn 0:200d9e41a3cf 133 tick.attach(&inverte,Ts);
gabrielpdn 0:200d9e41a3cf 134
gabrielpdn 0:200d9e41a3cf 135 while (1)
gabrielpdn 0:200d9e41a3cf 136 {
gabrielpdn 0:200d9e41a3cf 137 if(aux == 1){
gabrielpdn 0:200d9e41a3cf 138 aux = 0;
gabrielpdn 0:200d9e41a3cf 139
gabrielpdn 0:200d9e41a3cf 140 theta=enc_roda.getPulses()*en2rad_roda;
gabrielpdn 0:200d9e41a3cf 141 dtheta=(theta-theta_1)/Ts;
gabrielpdn 0:200d9e41a3cf 142 theta_1=theta;
gabrielpdn 0:200d9e41a3cf 143
gabrielpdn 0:200d9e41a3cf 144 angulo=enc_pend.getPulses()*en2rad_pend;
gabrielpdn 0:200d9e41a3cf 145 alpha = angulo - pi*angulo/abs(angulo);
gabrielpdn 0:200d9e41a3cf 146 dalpha=(alpha-alpha_1)/Ts;
gabrielpdn 0:200d9e41a3cf 147 alpha_1=alpha;
gabrielpdn 0:200d9e41a3cf 148
gabrielpdn 0:200d9e41a3cf 149 // Disturbio
gabrielpdn 0:200d9e41a3cf 150 /*if (N>7/Ts && N<7.2/Ts){
gabrielpdn 0:200d9e41a3cf 151 alpha = alpha-7*pi/180;
gabrielpdn 0:200d9e41a3cf 152 }*/
gabrielpdn 0:200d9e41a3cf 153
gabrielpdn 0:200d9e41a3cf 154 // Variavel LPV
gabrielpdn 0:200d9e41a3cf 155 rho1 = (alpha - lim_1)/(lim_2-lim_1);
gabrielpdn 0:200d9e41a3cf 156 rho2 = 1-rho1;
gabrielpdn 0:200d9e41a3cf 157
gabrielpdn 0:200d9e41a3cf 158 // Controle
gabrielpdn 0:200d9e41a3cf 159 Y = rho1*rho1*Y1+rho1*rho2*Y2+rho2*rho2*Y3;
gabrielpdn 0:200d9e41a3cf 160 Z = rho1*rho1*Z1+rho1*rho2*Z2+rho2*rho2*Z3;
gabrielpdn 0:200d9e41a3cf 161 K = Z*MatrixMath::Inv(Y);
gabrielpdn 0:200d9e41a3cf 162 u_pwm = K.getNumber(1,1)*alpha + K.getNumber(1,2)*dalpha + K.getNumber(1,3)*dtheta;
gabrielpdn 0:200d9e41a3cf 163 //u_pwm = K1*alpha + K2*dalpha + K3*dtheta;
gabrielpdn 0:200d9e41a3cf 164
gabrielpdn 0:200d9e41a3cf 165
gabrielpdn 0:200d9e41a3cf 166 // Saturação da roda
gabrielpdn 0:200d9e41a3cf 167 if (u_pwm>1.0)
gabrielpdn 0:200d9e41a3cf 168 u_pwm=1.0;
gabrielpdn 0:200d9e41a3cf 169 if (u_pwm<-1.0)
gabrielpdn 0:200d9e41a3cf 170 u_pwm=-1.0;
gabrielpdn 0:200d9e41a3cf 171
gabrielpdn 0:200d9e41a3cf 172 drive(u_pwm);
gabrielpdn 0:200d9e41a3cf 173
gabrielpdn 0:200d9e41a3cf 174
gabrielpdn 0:200d9e41a3cf 175 if (N<tempo) {
gabrielpdn 0:200d9e41a3cf 176 th[N] = theta;
gabrielpdn 0:200d9e41a3cf 177 dth[N] = dtheta;
gabrielpdn 0:200d9e41a3cf 178 alp[N] = alpha;
gabrielpdn 0:200d9e41a3cf 179 dalp[N] = dalpha;
gabrielpdn 0:200d9e41a3cf 180 u[N] = u_pwm;
gabrielpdn 0:200d9e41a3cf 181 }
gabrielpdn 0:200d9e41a3cf 182
gabrielpdn 0:200d9e41a3cf 183 if (N == tempo) {
gabrielpdn 0:200d9e41a3cf 184 tick.detach();
gabrielpdn 0:200d9e41a3cf 185 drive(0.0);
gabrielpdn 0:200d9e41a3cf 186 for (N=0; N<tempo; N++)
gabrielpdn 0:200d9e41a3cf 187 PC.printf("%f %f %f %f %f\n\r", alp[N], dalp[N], th[N], dth[N], u[N]);
gabrielpdn 0:200d9e41a3cf 188 }
gabrielpdn 0:200d9e41a3cf 189
gabrielpdn 0:200d9e41a3cf 190 //PC.printf( "%f \n\r",alpha);
gabrielpdn 0:200d9e41a3cf 191 //PC.printf( "\n" );
gabrielpdn 0:200d9e41a3cf 192
gabrielpdn 0:200d9e41a3cf 193 }
gabrielpdn 0:200d9e41a3cf 194 }
gabrielpdn 0:200d9e41a3cf 195 }