controle pendulo roda reacao
Dependencies: mbed MatrixMath QEI Matrix USBDevice
Diff: main.cpp
- Revision:
- 0:200d9e41a3cf
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Mar 11 16:50:23 2020 +0000 @@ -0,0 +1,195 @@ +#include "mbed.h" +#include "math.h" +#include "USBSerial.h" +#include "QEI.h" +#include "Matrix.h" +#include "MatrixMath.h" + + +#define Ts 0.01 +#define en2rad_roda 0.006698491798699 +//#define en2rad_pend 0.001591485640116 +//#define en2rad_roda 0.014660765717029 +#define en2rad_pend 0.001570796326795 +#define rad2graus 57.295779513082320 +#define pi 3.141592653589793 +#define omega_nd 210 + +USBSerial PC; +Ticker tick; + +QEI enc_roda (PTB17, PTD0, NC, 469); +QEI enc_pend (PTA12, PTA13, NC, 1000); +//QEI enc_roda (PTC2, PTC1, NC, 341.2); +//QEI enc_pend (PTD6, PTD5, NC, 1974); + +DigitalOut OnBoardLed (D13); +int aux = 0; +float pac = 0.0; + +// Variaveis encoders +float theta = 0.0, theta_1 = 0.0, dtheta = 0.0; +float alpha = 0.0, alpha_1 = 0.0, dalpha = 0.0; +float angulo = 0.0; + +float xc1 = 0.0, xc2 = 0.0, xc3 = 0.0, xc4 = 0.0; +float dxc1 = 0.0, dxc2 = 0.0, dxc3 = 0.0, dxc4 = 0.0; + +// Motor +DigitalOut InA(PTD5); +DigitalOut InB(PTC1); +PwmOut V(PTD6); +float th5=0.0; +float th5_1=0.0; +float dth5=0.0; + +// Alimentacao encoder motor +DigitalOut vcc_enc (PTB0); + +// Variaveis de medida +int N=0; +const int tempo = 15/Ts; +float th[tempo], dth[tempo], alp[tempo], dalp[tempo], u[tempo]; + + +// variavel controle +Matrix Z1(1,3); +Matrix Z2(1,3); +Matrix Z3(1,3); +Matrix Y1(3,3); +Matrix Y2(3,3); +Matrix Y3(3,3); +Matrix Z(1,3); +Matrix Y(3,3); +Matrix K(1,3); + + +float lim_1 = -0.261799387799149; +float lim_2 = 0.261799387799149; +float rho1 = 0.0, rho2 = 0.0; +float u_pwm = 0.0; + +// drive motor +void drive(float u) +{ + int direction=0; + if (u>0)direction=1; + if (u<0)direction=2; + if(u==0)direction=0; + switch(direction) { + case 0: + V.write(u); + InA=0; + InB=0; + break; + case 1: + V.write(u); + InA=1; + InB=0; + break; + case 2: + V.write(-u); + InA=0; + InB=1; + break; + default: + V.write(0.0); + InA=0; + InB=0; + break; + } +} + +// Interrupção +void inverte() { + aux=1; + //N++; +} + +int main () +{ +Z1 << 0.0528635583471 << 55.1775667591881 << -334.6107239814101; +Z2 << 2.2178052729892 << 89.4963037908543 << -632.0606939025414; +Z3 << 0.0527550080233 << 55.1818544517557 << -334.6407739018230; +Y1 << 52.229273 << -293.202697 << -1000.523522 + << -351.739401 << 8417.246435 << -49726.191528 + << -487.165029 << -53707.951308 << 505666.562609; +Y2 << 79.768194 << -492.424621 << -1138.974682 + << -454.513497 << 15400.080131 << -103976.618630 + << -1476.885923 << -101235.956011 << 992352.224275; +Y3 << 52.229040 << -293.201469 << -1000.518230 + << -351.740390 << 8417.269581 << -49726.325976 + << -487.140469 << -53708.234657 << 505667.351490; + +// LQR contuinuo +float K1 = 6.307583719273756; +float K2 = 0.808578597390600; +float K3 = 0.091291588353526; + + vcc_enc = 1; + OnBoardLed = 1; + V.period(0.0001); + wait(5); + tick.attach(&inverte,Ts); + + while (1) + { + if(aux == 1){ + aux = 0; + + theta=enc_roda.getPulses()*en2rad_roda; + dtheta=(theta-theta_1)/Ts; + theta_1=theta; + + angulo=enc_pend.getPulses()*en2rad_pend; + alpha = angulo - pi*angulo/abs(angulo); + dalpha=(alpha-alpha_1)/Ts; + alpha_1=alpha; + + // Disturbio + /*if (N>7/Ts && N<7.2/Ts){ + alpha = alpha-7*pi/180; + }*/ + + // Variavel LPV + rho1 = (alpha - lim_1)/(lim_2-lim_1); + rho2 = 1-rho1; + + // Controle + Y = rho1*rho1*Y1+rho1*rho2*Y2+rho2*rho2*Y3; + Z = rho1*rho1*Z1+rho1*rho2*Z2+rho2*rho2*Z3; + K = Z*MatrixMath::Inv(Y); + u_pwm = K.getNumber(1,1)*alpha + K.getNumber(1,2)*dalpha + K.getNumber(1,3)*dtheta; + //u_pwm = K1*alpha + K2*dalpha + K3*dtheta; + + + // Saturação da roda + if (u_pwm>1.0) + u_pwm=1.0; + if (u_pwm<-1.0) + u_pwm=-1.0; + + drive(u_pwm); + + + if (N<tempo) { + th[N] = theta; + dth[N] = dtheta; + alp[N] = alpha; + dalp[N] = dalpha; + u[N] = u_pwm; + } + + if (N == tempo) { + tick.detach(); + drive(0.0); + for (N=0; N<tempo; N++) + PC.printf("%f %f %f %f %f\n\r", alp[N], dalp[N], th[N], dth[N], u[N]); + } + + //PC.printf( "%f \n\r",alpha); + //PC.printf( "\n" ); + + } + } +}