勇輝 関本 / Mbed 2 deprecated Estrela_v10

４項目一応成功

Dependencies:   mbed

Fork of Estrela_v01 by Bot Furukawa

Files at this revision

API Documentation at this revision

Revision 9:182021b1df79, committed 2016-08-22

Comitter:
motoseki
Date:
Mon Aug 22 09:03:15 2016 +0000
Parent:
8:887d448db359
Commit message:
??????

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file 
diff -r 887d448db359 -r 182021b1df79 main.cpp
--- a/main.cpp	Sun Aug 21 10:37:09 2016 +0000
+++ b/main.cpp	Mon Aug 22 09:03:15 2016 +0000
@@ -30,7 +30,7 @@
 
 DigitalOut led1(PA_0);  //緑
 DigitalOut led2(PA_1);  //赤
-DigitalOut led3(PB_4);
+DigitalOut led3(PB_4);  //赤外線
 DigitalOut led4(PB_5);
 
 volatile uint8_t buf_sbus[25]; 
@@ -462,8 +462,8 @@
         //pc.printf("first%d\r\n",jj);
         jj++;
     }else{          //旋回中
-        Auto_RUD = RUD_N + -80 + int((-23-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
-        Auto_ELE = ELE_N + -110 + int( (-28 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
+        Auto_RUD = RUD_N + -70 + int((-23-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
+        Auto_ELE = ELE_N + -100 + int((-25 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
         Auto_THR = oldTHL;
     }
     
@@ -488,30 +488,30 @@
 void StabiGyro_Mobius()
 {
     if(jj<=25){     //右旋回導入
-        Auto_RUD = RUD_N + int((-25-roll)*RUDDER_GN - gy*RUD_DGN);   
-        Auto_ELE = ELE_N + int( (LAND_ANGLE + 3 - pitch)*ELEVATOR_GN - gx*ELE_DGN);     
-        Auto_THR = THR_N + 50;      
+        Auto_RUD = RUD_N + -105 + int((-30-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75);   //roll -> gy        3.5
+        Auto_ELE = ELE_N + -75 + int((-15 - pitch)*ELEVATOR_GN - gx*ELE_DGN);     //pitch  -> -gx
+        Auto_THR = oldTHL + 50;       
         //pc.printf("first%d\r\n",jj);
         jj++;
     }else if((25<jj) && (jj<=112)){     //右旋回途中
-        Auto_RUD = RUD_N + int((-20-roll)*RUDDER_GN - gy*RUD_DGN);
-        Auto_ELE = ELE_N + int( (LAND_ANGLE - 1 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
-        Auto_THR = THR_N;
+        Auto_RUD = RUD_N + -80 + int((-23-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
+        Auto_ELE = ELE_N + -110 + int((-28 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
+        Auto_THR = oldTHL;
         jj++;
     }else if((112<jj) && (jj<=117)){    //旋回遷移1
-        Auto_RUD = RUD_N + int((-roll)*RUDDER_GN - gy*RUD_DGN);  
-        Auto_ELE = ELE_N + int( (LAND_ANGLE + 2 - pitch)*ELEVATOR_GN - gx*ELE_DGN);   
+        Auto_RUD = RUD_N + int((0 -roll)*RUDDER_GN - (gy-100)*RUD_DGN);  
+        Auto_ELE = ELE_N + -30 + int((-10 - pitch)*ELEVATOR_GN - gx*ELE_DGN);   
         Auto_THR = THR_N - 100;      
         //pc.printf("first%d\r\n",jj);
         jj++;
     }else if((117<jj) && (jj<=132)){    //旋回遷移2
-        Auto_RUD = RUD_N + int((25-roll)*RUDDER_GN - gy*RUD_DGN);  
-        Auto_ELE = ELE_N + int( (LAND_ANGLE + 6 - pitch)*ELEVATOR_GN - gx*ELE_DGN);   
-        Auto_THR = THR_N - 20;   
+        Auto_RUD = RUD_N + 105 + int((30-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
+        Auto_ELE = ELE_N + -75 + int((-15 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
+        Auto_THR = oldTHL - 20; 
         jj++;
     }else{                              //左旋回途中
-        Auto_RUD = RUD_N + int((20-roll)*RUDDER_GN - gy*RUD_DGN);  
-        Auto_ELE = ELE_N + int( (LAND_ANGLE - pitch)*ELEVATOR_GN - gx*ELE_DGN);   
+        Auto_RUD = RUD_N + 80 + int((23-roll)*RUDDER_GN - gy*RUD_DGN);  
+        Auto_ELE = ELE_N + -110 + int((-28 - pitch)*ELEVATOR_GN - gx*ELE_DGN);   
         Auto_THR = THR_N + 30;   
     }
         
@@ -535,15 +535,15 @@
 void StabiGyro_Climb()
 {
     if(jj<=25){     //右旋回導入
-        Auto_RUD = RUD_N + int((-25-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75);   //roll -> gy
-        Auto_ELE = ELE_N + int( (LAND_ANGLE + 3 - pitch)*ELEVATOR_GN - gx*ELE_DGN);     //pitch  -> -gx
-        Auto_THR = THR_N + 50;     
+        Auto_RUD = RUD_N + -105 + int((-30-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75);   //roll -> gy        3.5
+        Auto_ELE = ELE_N + -75 + int((-15 - pitch)*ELEVATOR_GN - gx*ELE_DGN);     //pitch  -> -gx
+        Auto_THR = oldTHL + 50;            
         //pc.printf("first%d\r\n",jj);
         jj++;
     }else if((25<jj) && (jj<=224)){     //右旋回途中
-        Auto_RUD = RUD_N + int((-20-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
-        Auto_ELE = ELE_N + int( (LAND_ANGLE - pitch)*ELEVATOR_GN - gx*ELE_DGN);
-        Auto_THR = THR_N;
+        Auto_RUD = RUD_N + -80 + int((-23-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
+        Auto_ELE = ELE_N + -110 + int((-28 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
+        Auto_THR = oldTHL;
         jj++;
     /*}else if((224<jj) && (jj<=234)){    //上昇遷移
         Auto_RUD = RUD_N + 160 - int((g_HpfGyro[0]+25)*0.75);
@@ -552,14 +552,14 @@
         //pc.printf("first%d\r\n",jj);
         jj++;*/
     }else if((224<jj) && (jj<=314)){    //上昇中
-        Auto_RUD = RUD_N + int((-10-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
-        Auto_ELE = ELE_N + int( (LAND_ANGLE + 8 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
-        Auto_THR = THR_N + 200;
+        Auto_RUD = RUD_N + -60 + int((-20-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
+        Auto_ELE = ELE_N + -140 + int((-30 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
+        Auto_THR = oldTHL + 220;
         jj++;
     }else{                              //水平旋回
-        Auto_RUD = RUD_N + int((-15-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75);   //roll -> gy
-        Auto_ELE = ELE_N + int( (LAND_ANGLE + 3 - pitch)*ELEVATOR_GN - gx*ELE_DGN);     //pitch  -> -gx
-        Auto_THR = THR_N; 
+        Auto_RUD = RUD_N + -80 + int((-23-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
+        Auto_ELE = ELE_N + -110 + int((-28 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
+        Auto_THR = oldTHL;
     }
         
     if(Auto_RUD > RUD_MAX){
@@ -588,8 +588,8 @@
         //pc.printf("first%d\r\n",jj);
     //    jj++;
     //}else{
-        Auto_RUD = RUD_N + int((-10-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
-        Auto_ELE = ELE_N + int( (LAND_ANGLE + 3 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
+        Auto_RUD = RUD_N + -60 + int((-18-roll)*RUDDER_GN - gy*RUD_DGN);    //*0.75); 
+        Auto_ELE = ELE_N + -110 + int((-24 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
         Auto_THR = 1000;    //スロットルoff
     
     if(Auto_RUD > RUD_MAX){
@@ -607,6 +607,12 @@
     //Thread::wait(G_MS);
 }
 
+void StabiGyro_Landing()
+{
+    Auto_ELE = ELE_N + -80 + int((-20 - pitch)*ELEVATOR_GN - gx*ELE_DGN);
+    //Auto_THR = 
+}
+
 void Auto_Loop()
 {
     //while(true){
@@ -655,8 +661,11 @@
 }
 
 void Auto_Landing()
-{
-    update_mode = UPD_MANUAL;
+{   
+    sensing();
+    StabiGyro_Landing();
+    update_mode = UPD_LANDING;
+    wait_ms(50);
 }
 
 int main()
@@ -752,9 +761,9 @@
                 if(!CheckSW_up(7)&&!CheckSW_up(8)){     //チャンネル7,8が両方offなら
                     OperationMode=AUTOLOOP;             //変数OperationModeにAUTOLOOP(定数なので2(SkipperS.hで定義))を代入
                     pc.printf("go to autoloop\r\n");
-                }//else if(CheckSW_up(7)&&!CheckSW_up(8)){ //チャンネル７だけonなら
-                    //CalibrateCompass();
-                //}
+                }/*else if(CheckSW_up(7)&&!CheckSW_up(8)){ //チャンネル７だけonなら
+                    CalibrateCompass();
+                }*/
                 //変数update_modeはSBus(=プロポ＝操縦者)からのpwmをそのまま垂れ流すか、自動制御で計算したpwmをサーボに流すかを切り替えるための変数
                 update_mode = UPD_MANUAL;   //マニュアルモード(そのまま垂れ流す)
                // pc.printf("groundcheck mode\r\n");