Diff: main.cpp

Revision:

0:200d9e41a3cf

diff -r 000000000000 -r 200d9e41a3cf main.cpp
--- /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" );
+           
+        }
+    }
+}