Revision 10:652071c20bf6, committed 2018-09-15

Comitter:

taknokolat

Date:

Sat Sep 15 15:34:20 2018 +0000

Parent:

9:f6367b7fd7be

Commit message:

??????????????

Changed in this revision

diff -r f6367b7fd7be -r 652071c20bf6 main.cpp
--- a/main.cpp	Sat Sep 15 06:14:03 2018 +0000
+++ b/main.cpp	Sat Sep 15 15:34:20 2018 +0000
@@ -135,7 +135,7 @@
 
 //InterruptIn switch2(PC_14);
 MPU6050DMP6 mpu6050(PC_0,&pc); //割り込みピン，シリアルポインタ i2cのピン指定は MPU6050>>I2Cdev.h 内のdefine
-HCSR04 usensor(PB_9,PB_8); //trig,echo  9,8 
+//HCSR04 usensor(PB_9,PB_8); //trig,echo  9,8 
 
 PID pid_AIL(g_kpAIL,g_kiAIL,g_kdAIL);
 PID pid_ELE(g_kpELE,g_kiELE,g_kdELE);
@@ -167,9 +167,9 @@
 float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0;
 
 unsigned int g_distance;
-Ticker USsensor;
+//Ticker USsensor;
 static char g_buf[16];
-char g_landingcommand;
+char g_landingcommand='Z';
 float g_SerialTargetYAW;
 
 Timer t;
@@ -265,6 +265,8 @@
         
         loop();
         
+        
+        
         if(!CheckSW_Up(Ch7)){
             led3=0;
         }else{
@@ -298,7 +300,7 @@
     Init_sensors();
     //switch2.rise(ResetTrim);
     pc.attach(getSF_Serial, Serial::RxIrq);
-    USsensor.attach(&UpdateDist, 0.05);
+    //USsensor.attach(&UpdateDist, 0.05);
     
     NVIC_SetPriority(USART1_IRQn,0);
     NVIC_SetPriority(EXTI0_IRQn,1);
@@ -341,11 +343,16 @@
     static float targetAngle[3] = {0.0, 0.0, 0.0}, controlValue[2] = {0.0, 0.0};
 
     SensingMPU();
+    NVIC_DisableIRQ(USART2_IRQn);
     UpdateTargetAngle(targetAngle);
     //Rotate(targetAngle, 30.0);
     CalculateControlValue(targetAngle, controlValue);
+    NVIC_DisableIRQ(USART1_IRQn);
     UpdateAutoPWM(controlValue);
-    wait_ms(25);
+    NVIC_EnableIRQ(USART1_IRQn);
+    NVIC_EnableIRQ(USART2_IRQn);
+    wait_ms(20);
+    pc.printf("%c",g_landingcommand);
 #if DEBUG_PRINT_INLOOP
     DebugPrint();
 #endif
@@ -796,6 +803,7 @@
     static int16_t pwm[6];
     static int16_t temppwm[6]={trimpwm[0],trimpwm[1],trimpwm[2],trimpwm[3],trimpwm[4],trimpwm[5]};
     if(sbus.flg_ch_update == true){
+        NVIC_DisableIRQ(USART1_IRQn);
         switch(output_status){    //マニュアルモード,自動モード,自動着陸もモードを切替
             case Manual: 
                 for(uint8_t i=0;i<6;i++){
@@ -814,8 +822,8 @@
                 pwm[RUD] = autopwm[RUD];
                 pwm[DROP] = autopwm[DROP];
                 pwm[AIL_L] = autopwm[AIL_L];
-                pc.printf("%d ,",pwm[AIL_R]);//R
-                pc.printf("%d ,\r\n",pwm[AIL_L]);//L
+                //pc.printf("%d ,",pwm[AIL_R]);//R
+                //pc.printf("%d ,\r\n",pwm[AIL_L]);//L
                 //pc.printf("update_auto\r\n");
                 
                 break;
@@ -825,6 +833,7 @@
                     pwm[i] = sbus.manualpwm[i];
                 } //pc.printf("update_manual\r\n");
                 break;
+            NVIC_EnableIRQ(USART1_IRQn);
         }
         for(uint8_t i=0;i<6;i++){
         if(pwm[i]<1000 || pwm[i]>2000) pwm[i]=temppwm[i];
@@ -841,12 +850,14 @@
 //pwmをサーボに出力。
 void Output_PWM(int16_t pwm[5])
 {
+    NVIC_DisableIRQ(USART1_IRQn);
     servo1.pulsewidth_us(pwm[0]);
     servo2.pulsewidth_us(pwm[1]);
     servo3.pulsewidth_us(pwm[2]);
     servo4.pulsewidth_us(pwm[3]);
     servo5.pulsewidth_us(pwm[4]);
     servo6.pulsewidth_us(pwm[5]);
+    NVIC_EnableIRQ(USART1_IRQn);
 
 }
 
@@ -868,8 +879,8 @@
     NVIC_DisableIRQ(USART1_IRQn);
     NVIC_DisableIRQ(USART2_IRQn);
     NVIC_DisableIRQ(TIM5_IRQn);
-    NVIC_DisableIRQ(EXTI0_IRQn);
-    NVIC_DisableIRQ(EXTI9_5_IRQn);
+    NVIC_DisableIRQ(EXTI0_IRQn);//MPU割り込み禁止
+    NVIC_DisableIRQ(EXTI9_5_IRQn);//超音波割り込み禁止
 
     mpu6050.getRollPitchYaw_Skipper(rpy);
  
@@ -911,6 +922,8 @@
         static char SFbuf[16];
         static int bufcounter=0;
         
+        pc.printf("%d",bufcounter);
+        
         SFbuf[bufcounter]=pc.getc();
         if(SFbuf[0]=='S'&&bufcounter<5) bufcounter++;
             
@@ -919,9 +932,9 @@
             if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
             bufcounter = 0;
             memset(SFbuf, 0, strlen(SFbuf));
-            pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW); 
+            //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW); 
         }
-            else if(bufcounter>=5 && g_buf[4]!='F'){
+            else if(bufcounter>=5 ){
                 pc.printf("Communication Falsed.\r\n");
                 bufcounter = 0;
             }
@@ -937,10 +950,10 @@
 
 
 //超音波割り込み
-void UpdateDist(){
+/*void UpdateDist(){
     g_distance = usensor.get_dist_cm();
     usensor.start();
-}
+}*/
 
 //8の字旋回
 void UpdateTargetAngle_Moebius(float targetAngle[3]){
@@ -984,8 +997,7 @@
     targetAngle[PITCH] = g_glideloopPITCH;
     
    // autopwm[THR]=oldTHR;
-    //シリアル通信受信の割り込みイベント登録
-    pc.attach(ISR_Serial_Rx, Serial::RxIrq);
+
     
     
     //時間計測開始設定
@@ -1003,11 +1015,14 @@
     if(flg_tstart){
         t_diff = t.read() - t_start;
         //一定高度or15秒でled点灯
+        NVIC_DisableIRQ(EXTI9_5_IRQn);
         if((groundcount>5 && g_distance>0) || t_diff > 15){
             led2 = 1;
             //pc.printf("Call [Stop!] calling!\r\n");
         }
+        
         if(g_distance<180 && g_distance > 0) groundcount++;
+        NVIC_EnableIRQ(EXTI9_5_IRQn);
     }else{
         t_diff = 0;
         groundcount = 0;
@@ -1017,7 +1032,9 @@
     if(t_diff > 17){
         autopwm[THR] = SetTHRinRatio(0.5);
     }else{
+        NVIC_DisableIRQ(EXTI9_5_IRQn);
         if(g_distance<150 && g_distance>0 ){
+        NVIC_EnableIRQ(EXTI9_5_IRQn);    
             THRcount++;
             if(THRcount>5){
                 autopwm[THR] = SetTHRinRatio(0.6);
@@ -1031,6 +1048,7 @@
 }
 //離陸-投下-着陸一連
 void Take_off_and_landing(float targetAngle[3]){
+    /*
     if(!CheckSW_Up(Ch7)) bombing_mode = Takeoff;
     
     switch(bombing_mode){
@@ -1055,10 +1073,10 @@
                 bombing_mode = Approach;
             }
             break;
-        /*    
-        case Chicken:    
-            break;
-        */    
+           
+        //case Chicken:    
+            //break;
+            
         case Transition:
             static int ApproachCount = 0;
             targetAngle[YAW]=180.0;
@@ -1069,13 +1087,17 @@
             break;
             
         case Approach:
+        */
+            autopwm[THR] = SetTHRinRatio(g_loopTHR); //0.7;スロットルの割合
             UpdateTargetAngle_Approach(targetAngle);
+            /*
             break;
             
         default:
             bombing_mode = Takeoff;
             break;    
     }
+    */
 }
 
 //離陸モード
@@ -1131,60 +1153,83 @@
 void ReturnChickenServo2(){
     autopwm[DROP] = 1392;
     pc.printf("second reverse\r\n");
-}
-
-//着陸モード（PCからの指令に従う）
-void UpdateTargetAngle_Approach(float targetAngle[3]){
-    pc.putc(g_buf[0]);
-    switch(g_buf[0]){
-        case 'R':   //右旋回セヨ
-            UpdateTargetAngle_Rightloop(targetAngle);
-            break;
-            
-        case 'L':   //左旋回セヨ
-            UpdateTargetAngle_Leftloop(targetAngle);
-            break;
-            
-        case 'G':   //直進セヨ
-            UpdateTargetAngle_GoStraight(targetAngle);
-            break;
+    }
+    
+    //着陸モード（PCからの指令に従う）
+    void UpdateTargetAngle_Approach(float targetAngle[3]){
+       
+        NVIC_DisableIRQ(USART2_IRQn);
+       
+        switch(g_landingcommand){
+            case 'R':   //右旋回セヨ
+                targetAngle[ROLL] = g_rightloopROLL;
+        targetAngle[PITCH] = g_rightloopPITCH ;
+        autopwm[RUD]=rightloopRUD;              //RUD固定
+        if(autopwm[AIL_R]>trimpwm[AIL_R]){                                                                                  //エルロン上がりやすさ調節
+            autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
+            }
+        else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionrightloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
+        autopwm[AIL_R]=autopwm[AIL_R]+AIL_R_correctionrightloop;
         
-        case 'Y':   //指定ノヨー方向ニ移動セヨ
-            Rotate(targetAngle, g_SerialTargetYAW);
-            break;
+                break;
+                
+                case 'L':   //左旋回セヨ
+                    targetAngle[ROLL] = g_leftloopROLL;
+            targetAngle[PITCH] = g_leftloopPITCH;
+            autopwm[RUD]=leftloopRUD;
+            if(autopwm[AIL_R]>trimpwm[AIL_R]){
+                autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioRising;
+                }
+            else autopwm[AIL_L]=trimpwm[AIL_L]+AIL_L_correctionleftloop+(autopwm[AIL_R]-trimpwm[AIL_R])*AIL_L_RatioDescent;
         
-        case 'B':   //ブザーヲ鳴ラセ
-            //buzzer = 1;
-            break;
+                    break;
+                    
+                case 'G':   //直進セヨ
+                    targetAngle[ROLL] = g_gostraightROLL;
+                    targetAngle[PITCH] = g_gostraightPITCH;
+    
+    
+                    break;
+                
+                case 'Y':   //指定ノヨー方向ニ移動セヨ
+                    Rotate(targetAngle, g_SerialTargetYAW);
+                    break;
+                
+                case 'B':   //ブザーヲ鳴ラセ
+                    //buzzer = 1;
+                    break;
+                    
+                case 'D':   //物資ヲ落トセ
+                    Chicken_Drop();
+                    break;
+                            
+                case 'C':   //停止セヨ
+                    targetAngle[ROLL] = 0.0;
+                    targetAngle[PITCH] = -3.0;
+                    autopwm[THR] = minpwm[THR];
+                    break;
             
-        case 'D':   //物資ヲ落トセ
-            Chicken_Drop();
-            break;
-                    
-        case 'C':   //停止セヨ
-            targetAngle[ROLL] = 0.0;
-            targetAngle[PITCH] = -3.0;
-            autopwm[THR] = minpwm[THR];
-            break;
+            }
+            NVIC_EnableIRQ(USART2_IRQn);
+}
+        
+void checkHeight(float targetAngle[3]){
+        
+        static int targetHeight = 200; 
+        NVIC_DisableIRQ(EXTI9_5_IRQn);
+        if(g_distance < targetHeight + ALLOWHEIGHT){
+            UpdateTargetAngle_NoseUP(targetAngle);
+            if(CheckSW_Up(Ch7)) led2 = 1;
+        }
+        else if(g_distance > targetHeight - ALLOWHEIGHT){
+            UpdateTargetAngle_NoseDOWN(targetAngle);
+            if(CheckSW_Up(Ch7)) led2 = 1;
+        }
+        else led2=0;
+        NVIC_EnableIRQ(EXTI9_5_IRQn);
     }
-}
-
-void checkHeight(float targetAngle[3]){
     
-    static int targetHeight = 200; 
-    
-    if(g_distance < targetHeight + ALLOWHEIGHT){
-        UpdateTargetAngle_NoseUP(targetAngle);
-        if(CheckSW_Up(Ch7)) led2 = 1;
-    }
-    else if(g_distance > targetHeight - ALLOWHEIGHT){
-        UpdateTargetAngle_NoseDOWN(targetAngle);
-        if(CheckSW_Up(Ch7)) led2 = 1;
-    }
-    else led2=0;
-}
-
-void UpdateTargetAngle_NoseUP(float targetAngle[3]){
+    void UpdateTargetAngle_NoseUP(float targetAngle[3]){
     
     //targetAngle[PITCH] += 2.0f;
     //if(nowAngle[PITCH]<targetAngle[PITCH]) autopwm[THR] = SetTHRinRatio(0.6);
@@ -1303,17 +1348,7 @@
 
     //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
 }
-void ISR_Serial_Rx()
-{
-    // シリアルの受信処理
-    char data = pc.getc();
-    
-    if(data=='C'){
-    autopwm[THR]=minpwm[2];
-    wait(60.0);
-    }
-    
-}
+
 
 //デバッグ用
 void DebugPrint(){    
@@ -1329,10 +1364,10 @@
     //pc.printf("\r\n");
     //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
     //for(uint8_t i=0; i<2; i++) pc.printf("%3.2f\t",nowAngle[i]); //ロール、ピッチのみ　小数点以下2ケタ
-    pc.printf("%d\t",autopwm[AIL_L]); // pc.printf("%d\t",autopwm[RUD]);
+    //pc.printf("%d\t",autopwm[AIL_L]); // pc.printf("%d\t",autopwm[RUD]);
     //pc.printf("%d",g_distance);
 
     //pc.printf("Mode: %c: ",g_buf[0]);
     //if(g_buf[0] == 'Y') pc.printf(" %3.1f",g_SerialTargetYAW);
-    pc.printf("\r\n");
+    //pc.printf("\r\n");
 }
\ No newline at end of file