Diff: main.cpp

Revision:

2:23daa5fa28b4

Parent:

1:f383708a5a52

Child:

3:206b17251f5b

--- a/main.cpp	Fri Sep 07 09:47:37 2018 +0000
+++ b/main.cpp	Sat Sep 08 05:32:55 2018 +0000
@@ -26,12 +26,11 @@
 #define KP_RUD 3.0
 #define KI_RUD 0.0
 #define KD_RUD 0.0
-#define KP_AIL 3.0
-#define KI_AIL 0.0
-#define KD_AIL 0.0
+#define KP_AIL 0.1
+#define KI_AIL 0.2
+#define KD_AIL 0.2
 
-
-
+//#define g_AIL_L_Ratio_rightloop 0.5
 
 #define GAIN_CONTROLVALUE_TO_PWM 3.0
 
@@ -44,6 +43,8 @@
 #define TAKEOFF_THR 0.8
 #define LOOP_THR 0.6
 
+#define g_rightloopRUD 1500 
+
 #define RIGHT_ROLL_SHORT -12.0
 #define RIGHT_PITCH_SHORT -5.0
 #define LEFT_ROLL_SHORT 12.0
@@ -130,6 +131,9 @@
 int16_t minpwm[6] = {1180,1180,1180,1180,1180,1180};
 int16_t oldTHR = 1000;
 
+int16_t g_AIL_L_Ratio_rightloop = 0.5;
+
+
 static float nowAngle[3] = {0,0,0};
 const float trimAngle[3] = {0.0, 0.0, 0.0};
 const float maxAngle[2] = {90, 90};
@@ -316,7 +320,7 @@
     //Rotate(targetAngle, 30.0);
     CalculateControlValue(targetAngle, controlValue);
     UpdateAutoPWM(controlValue);
-    wait_ms(23);
+    wait_ms(30);
 #if DEBUG_PRINT_INLOOP
     DebugPrint();
 #endif
@@ -366,7 +370,7 @@
 }
 
 void Init_PWM(){
-    for (uint8_t i = 0; i < 4; ++i){
+    for (uint8_t i = 0; i < 6; ++i){
         trimpwm[i] = 1500 + (int16_t)(lengthdivpwm * (trim[i]/100));
         maxpwm[i] = 1500 + (int16_t)(lengthdivpwm * (expMax[i]/100));
         minpwm[i] = 1500 - (int16_t)(lengthdivpwm * (expMin[i]/100));
@@ -725,12 +729,13 @@
     addpwm[PITCH] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[PITCH];    //センサ：機首下げ正    レバー:機首上げ正
     addpwm[ROLL] = (float)GAIN_CONTROLVALUE_TO_PWM * controlValue[ROLL];           //センサ：右回転正（8月13日時点；左回転が正！）     レバー:右回転正
     
-    //autopwm[ELE] = trimpwm[ELE] + reverce[ELE] * addpwm[PITCH];
+    autopwm[ELE] = trimpwm[ELE] + reverce[ELE] * addpwm[PITCH];                 //rewrite
     autopwm[AIL_R] = trimpwm[AIL_R] + reverce[AIL_R] * addpwm[AIL_R];
     //autopwm[THR] = oldTHR;
 
     autopwm[ELE] = ThresholdMaxMin(autopwm[ELE], maxpwm[ELE], minpwm[ELE]);
     autopwm[AIL_R] = ThresholdMaxMin(autopwm[AIL_R], maxpwm[AIL_R], minpwm[AIL_R]);
+    
 }
 
 inline float CalcRatio(float value, float trim, float limit){
@@ -755,6 +760,8 @@
     return minpwm[THR] + (int16_t)(2 * lengthdivpwm * ratio);
 }
 
+
+
 /*---SBUS割り込み処理---*/
 
 //udate_Inputで抽出したpwmデータを整形して各変数に代入する。(マニュアルモード)
@@ -769,8 +776,8 @@
                 for(uint8_t i=0;i<6;i++){
                     pwm[i] = sbus.manualpwm[i];  
                 }
-                pc.printf("%d ,",pwm[0]);//R
-                pc.printf("%d ,\r\n",pwm[5]);//L
+                /*pc.printf("%d ,",pwm[0]);//R
+                pc.printf("%d ,\r\n",pwm[5]);//L*/
                 oldTHR = sbus.manualpwm[THR];
                 //pc.printf("update_manual\r\n");
                 break;
@@ -782,7 +789,10 @@
                 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("update_auto\r\n");
+                
                 break;
                 
             default:
@@ -1176,9 +1186,12 @@
 //右旋回
 void UpdateTargetAngle_Rightloop(float targetAngle[3]){ //右旋回
 
+    autopwm[RUD]=g_rightloopRUD;
     targetAngle[ROLL] = g_rightloopROLL;
     targetAngle[PITCH] = g_rightloopPITCH ;
-    autopwm[THR] = SetTHRinRatio(g_loopTHR);
+    autopwm[THR] = oldTHR;//SetTHRinRatio(g_loopTHR);
+    autopwm[AIL_L]=trimpwm[AIL_L]+(autopwm[AIL_R]-trimpwm[AIL_R])*0.5;//*inverse;//g_AIL_L_Ratio_rightloop;
+    pc.printf("",)
     //checkHeight(targetAngle);
     //pc.printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
 }
@@ -1201,7 +1214,7 @@
     autopwm[THR] = SetTHRinRatio(g_loopTHR);
     //checkHeight(targetAngle);
 
-    //printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[ROLL], targetAngle[PITCH], autopwm[THR]);
+    //printf("Roll = %f, PITCH = %f, THR = %d \r\n", targetAngle[], targetAngle[PITCH], autopwm[THR]);
 }
 
 void UpdateTargetAngle_Leftloop_short(float targetAngle[3]){
@@ -1236,7 +1249,7 @@
     //for(uint8_t i=0; i<8; i++) pc.printf("%d ",sbus.manualpwm[i]);
     //for(uint8_t i=1; i<4; i++) pc.printf("%d ",sbus.manualpwm[i]);
     //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<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[ELE]);  pc.printf("%d\t",autopwm[RUD]);
     //pc.printf("%d",g_distance);