yal kaiyo / Mbed 2 deprecated 130111_YNU_MPU_4

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.

Fork of 130111_YNU_MPU_3 by yal kaiyo

Files at this revision

API Documentation at this revision

Revision 15:571b34671309, committed 2013-01-14

Comitter:
yal_kaiyo
Date:
Mon Jan 14 12:02:15 2013 +0000
Parent:
14:feea7e2a51b8
Commit message:
add control law

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file 
diff -r feea7e2a51b8 -r 571b34671309 main.cpp
--- a/main.cpp	Mon Jan 14 05:48:31 2013 +0000
+++ b/main.cpp	Mon Jan 14 12:02:15 2013 +0000
@@ -114,6 +114,7 @@
     float   psc     =   0;
     float   etc     =   0;
     float   xic     =   0;
+    float   apc     =   0;
     
     //homotopy
     //initial condition
@@ -172,17 +173,29 @@
     float   gmr     =   20;
     float   vmr     =   0;
     float   pse     =   0;
+    float   phir    =   0;
     float   ete     =   0;   
     float   hte     =   0;
+    float   xie     =   0;
     
     float   hte00   =   0;
-    float   dhdt    =   0;
+    float   dhdt    =   0; 
+    float   phic00  =   0;
+    float   dclda   =   0.084;
 
     float   v2      =   0;
+    float   a22     =   0;
+    float   a23     =   0;
+    float   a32     =   0;
+    float   b21     =   0;
+    float   aaa     =   0;
     
     float   gkp     =   0;
     float   gkd     =   0;
-
+    float   ck1     =   0;
+    float   ck2     =   0;
+    float   phff    =   0;
+    float   phic    =   0;
     float   avpc    =   0;
     float   avqc    =   0;
     float   avrc    =   0;
@@ -1172,30 +1185,43 @@
             // ******************** guidance law WO homo *************
             
             // ******************** control law **********************
+            xie=xic-xir;
             pse=(psc-psr)*dtr;
             ete=etc-etr;
             hte=htc-htr;
             
             //horizontal
-            /*
+             v2=vmr*vmr;
+            
             gkp=omg1*omg1/grv;
             gkd=(2*zet1*omg1*cos(gmr*dtr)/grv)*vmr;
-            avpc=1/tt1*(gkd*pse+gkp*ete);//
-            */
+            phff=atan(apc*v2/grv);
+            phic=-gkp*ete-gkd*pse-phff;
+            avpc=(phic-phic00)/dt;
+            phic00=phic;
             
-            //avpc=kp*hte;
+            
             
             //vertical
-            /*
-            v2=vmr*vmr;
-            dhdt=(hte-hte00)/dt
-            */
+            
+            a22=grv*sin(gmr*dtr)/vmr;
+            a23=0;
+            a32=vmr*cos(gmr*dtr);
+            b21=swg*rho*vmr*dclda*cos(phir*dtr)/(2*wgt);
+            aaa=a32*b21;
+            ck1=(a22+2*zet2*omg2)/aaa;
+            ck2=(a23*a32+omg2*omg2)/aaa;
+            
+            
+            
+            
+            dhdt=(hte-hte00)/dt;
+            
             
             // to translate deg/s to rad/s, add *pi/180. TORATANI 
-            //avqc=(-1/tt2*(2/(0.084*rho*v2*swg))*(wgt*grv*cos(gmr*dtr)-wgt*(2*zet2*omg2*dhdt+omg2*omg2*hte)))*pi/180;
-            //hte00=hte;
+            avqc=(ck1*dhdt+ck2*hte)/tt2;
+            hte00=hte;
             
-            //avqc=kq*(pse+ete);
             
 
             
@@ -1203,7 +1229,7 @@
             com[1]  =   avqc;
             com[2]  =   avrc;
             
-            if(hte > 12 || xic > 12 || etc >12){
+            if(fabs(hte) > 12 || fabs(xie) > 12 || fabs(ete) >12){
             com[0]  =   0;
             com[1]  =   0;
             com[2]  =   0;