Diff: main.cpp

Branch:

mpu????????

Revision:

78:e272e65f5a0e

Parent:

77:ca4ab599ba2b

--- a/main.cpp	Wed Feb 28 03:36:22 2018 +0000
+++ b/main.cpp	Thu Mar 08 08:07:31 2018 +0000
@@ -23,7 +23,7 @@
 #include "XBusServo.h"
 #include "math.h"
 
-#define INIT_SERVO_PERIOD_MS 20
+#define INIT_SERVO_PERIOD_US 10000
 #define WAIT_LOOP_TIME 0.02
 #define YOKUTAN_DATAS_NUM 7
 #define INPUT_DATAS_NUM 4 //ここは8バイトまでしかCANでは一度に送れないため、8以下。そして、操舵コードと数字を合わせる必要あり。
@@ -35,15 +35,19 @@
 #define MPU_LOOP_TIME 0.01
 #define MPU_DELT_MIN 250
 
-#define ERURON_MOVE_DEG_INI_R 14.4*0.8//18.0       //degree
-#define DRUG_MOVE_DEG_INI_R 0.49
-#define ERURON_TRIM_INI_R 0.39633  //値lowerすると頭上げ 0.37で後縁一致
-#define DRUG_TRIM_INI_R  0.37
-
-#define ERURON_MOVE_DEG_INI_L -15.52//-19.4     //degree
-#define DRUG_MOVE_DEG_INI_L -0.52
-#define ERURON_TRIM_INI_L  0.41567  // 値を大きくすると頭上げ
-#define DRUG_TRIM_INI_L    0.73//値を小さくすると開く側
+//値を大きくするとサーボに向かって反時計回りになる
+//Rエレボンは頭上げ==値大きく
+#define ERURON_MOVE_DEG_INI_R 0.3
+#define ERURON_TRIM_INI_R 0.5
+//Rラダーは右回り(時計回り)が開く
+#define DRUG_MOVE_DEG_INI_R -1.41297
+#define DRUG_TRIM_INI_R  1.014408
+//Lエレボンは頭上げ==値小さく
+#define ERURON_MOVE_DEG_INI_L -0.32
+#define ERURON_TRIM_INI_L 0.51567
+//Lラダーは左回りが開く
+#define DRUG_MOVE_DEG_INI_L 0.983027
+#define DRUG_TRIM_INI_L -0.1
 
 /*ドラッグラダー
 初期値 0.65
@@ -55,6 +59,7 @@
 #define GETTING_DATA_DEBUG_FLAG debugflag[1].flag
 #define SERVO_CONFIG_DEBUG_FLAG debugflag[2].flag
 #define DEBUG_FLAG debugflag[3].flag
+#define DRUG_OPEN_FLAG debugflag[4].flag
 
 struct flaglist{
     char key;
@@ -65,6 +70,7 @@
     {'g',0},
     {'c',0},
     {'j',0},
+    {'d',0},
     {'0',0}
 };
 
@@ -123,6 +129,7 @@
 void receiveDatas();
 void WriteServo();
 void MpuInit();
+int calcPulse_us(float analog);
 void mpuProcessing(void const *arg);
 
 Ticker gTimer;
@@ -141,7 +148,7 @@
 
 bool servoInit()
 {
-    drugServo.period_ms(INIT_SERVO_PERIOD_MS);
+    drugServo.period_us(INIT_SERVO_PERIOD_US);
     return true;
 }
 
@@ -305,14 +312,17 @@
         if(IsRPin) {
             eruronTrim = ERURON_TRIM_INI_R;
             drugTrim = DRUG_TRIM_INI_R;
-            eruronMoveDeg =GetFloatByErebon(ERURON_MOVE_DEG_INI_R);
+//            eruronMoveDeg =GetFloatByErebon(ERURON_MOVE_DEG_INI_R);
+            eruronMoveDeg = ERURON_MOVE_DEG_INI_R;
             drugMoveDeg =DRUG_MOVE_DEG_INI_R;
         } else {
             eruronTrim = ERURON_TRIM_INI_L;
             drugTrim = DRUG_TRIM_INI_L;
-            eruronMoveDeg = GetFloatByErebon(ERURON_MOVE_DEG_INI_L);
+//            eruronMoveDeg = GetFloatByErebon(ERURON_MOVE_DEG_INI_L);
+            eruronMoveDeg = ERURON_MOVE_DEG_INI_L;
             drugMoveDeg = DRUG_MOVE_DEG_INI_L;
         }
+        writeConfig();
     }
     SERVO_FLAG = servoInit();
     INA_FLAG = inaInit();
@@ -360,9 +370,19 @@
     servoOff = servoOffVer;
 }
 
-double calcPulse(float analog)
+//double calcPulse(float analog)
+//{
+//    return (0.0006 + (analog)*(0.00240-0.00060) );
+//}
+int calcPulse_us(float analog)
 {
-    return (0.0006 + (analog)*(0.00240-0.00060) );
+    return (1000 + 1000*analog);
+}
+
+float getPinDeg()
+{
+    float setPin10 = -setDeg10.read() + 1.0;
+    return (int)(setPin10*9)/10.0 + setDeg1.read()/10;
 }
 
 void WriteServo()
@@ -372,46 +392,61 @@
 //    }
 //    pc.printf(" : %f",eruronfloat);
 //    pc.printf("\n\r");
-    drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *drugInput));
-    eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * eruronfloat));
+    eruronServo.pulsewidth_us(calcPulse_us( eruronTrim + eruronMoveDeg * eruronfloat));
+    if(DRUG_OPEN_FLAG)drugServo.pulsewidth_us(calcPulse_us(drugTrim + drugMoveDeg));
+    else drugServo.pulsewidth_us(calcPulse_us(drugTrim + drugMoveDeg * drugInput));
     print2pc(GETTING_DATA_DEBUG_FLAG,"ef:%5.2f di:%d so:%d\n\r",eruronfloat,drugInput,int(servoOff));
     print2pc(SERVO_CONFIG_DEBUG_FLAG,"eTr:%f   dTr:%f   eMD:%f   dMD:%f\n\r",eruronTrim,drugTrim,eruronMoveDeg,drugMoveDeg);
     print2pc(DEBUG_FLAG,"servoOffVer:%d\n\r",servoOffVer);
 }
 
-void setTrim(float initEruronState, float initDrugState, float initPinState)
+void setTrim(PwmOut servo, float* servoTr, float initServoTrim, float initPinState)
 {
     led2 = 1;
     led4 = 0;
-    if(eruronTrim < 0 || drugTrim < 0)led2 = 0;
-    if(EDstatePin) {
-        eruronTrim = -initPinState + (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10 + initEruronState;
-        eruronServo.pulsewidth(calcPulse(eruronTrim));
-    } else {
-        drugTrim = -initPinState + (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10 + initDrugState;
-        drugServo.pulsewidth(calcPulse(drugTrim));
-    }
-    //pc.printf("eruronTrim:%f    drugTrim:%f\n\r",eruronTrim,drugTrim);
+    if(*servoTr<0)led2 = 0;
+    if(*servoTr>1.1) led1 = 1;
+    if(int(*servoTr * 10) == 0) led4 = 1;
+//    if(EDstatePin) {
+//        eruronTrim = -initPinState + (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10 + initEruronState;
+//        eruronServo.pulsewidth_us(calcPusle_us(eruronTrim));
+//    } else {
+//        drugTrim = -initPinState + (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10 + initDrugState;
+//        drugServo.pulsewidth_us(calcPulse_us(drugTrim));
+//    }
+    *servoTr = getPinDeg() - initPinState + initServoTrim;
+    servo.pulsewidth_us(calcPulse_us(*servoTr));
+    
     pc.printf("eTr:%f   dTr:%f   ",eruronTrim,drugTrim);
     pc.printf("eMD:%f   dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
 }
 
-void setMaxDeg(float initEruronState, float initDrugState, float initPinState)
+void setMaxDeg(PwmOut servo, float* servoMD, float initServoTr, float initServoMD, float initPinState)
 {
     led4 = 1;
     led2 = 0;
-    float eruronTemp = -initPinState + (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10 + initEruronState;
-    float drugTemp = -initPinState + (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10 + initDrugState;
-    if(eruronTemp < 0 || drugTemp < 0)led4 = 0;
-    if(EDstatePin) {
-        eruronMoveDeg = eruronTemp-eruronTrim;
-        eruronServo.pulsewidth(calcPulse(eruronTemp));
-    } else {
-        drugMoveDeg = drugTemp-drugTrim;
-        drugServo.pulsewidth(calcPulse(drugTemp));
-    }
+//    float eruronTemp = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10 - initPinState + initEruronState;
+//    float drugTemp = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10 - initPinState + initDrugState;
+    float temp = getPinDeg() - initPinState + initServoTr + initServoMD;
+    
+//    if(EDstatePin) {
+//        eruronMoveDeg = eruronTemp-eruronTrim;
+//        if(eruronTemp < 0)led4 = 0;
+//        else eruronServo.pulsewidth_us(calcPusle_us(eruronTemp));
+//    } else {
+//        drugMoveDeg = drugTemp-drugTrim;
+//        if(drugTemp < 0)led4 = 0;
+//        else drugServo.pulsewidth_us(calcPusle_us(drugTemp));
+//    }
+    *servoMD = temp-initServoTr;
+    if(temp<0)led4 = 0;
+    if(temp>1.1) led1 = 1;
+    if(int(temp * 10) == 0) led2 = 1;
+    else servo.pulsewidth_us(calcPulse_us(temp));
+    
     pc.printf("eTr:%f   dTr:%f   ",eruronTrim,drugTrim);
     pc.printf("eMD:%f   dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
+    
     wait_us(10);
 }
 
@@ -425,14 +460,25 @@
 
     while(1) {
         if(InSetModePin){
-            float initEruronState = eruronTrim;
-            float initDrugState = drugTrim;
-            float initPinState = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10;
+            float initEruronTrim = eruronTrim;
+            float initDrugTrim = drugTrim;
+            float initEruronMD = eruronMoveDeg;
+            float initDrugMD = drugMoveDeg;
+            float initPinState = getPinDeg();
             do{
-                if(TrimMaxDegPin)
-                    setTrim(initEruronState,initDrugState,initPinState);
-                else
-                    setMaxDeg(initEruronState,initDrugState,initPinState);
+                if(TrimMaxDegPin){
+                    if(EDstatePin)
+                        setTrim(eruronServo, &eruronTrim, initEruronTrim, initPinState);
+                    else
+                        setTrim(drugServo, &drugTrim, initDrugTrim, initPinState);
+                }
+                else{
+                    if(EDstatePin)
+                        setMaxDeg(eruronServo, &eruronMoveDeg, initEruronTrim, initEruronMD, initPinState);
+                    else
+                        setMaxDeg(drugServo, &drugMoveDeg, initDrugTrim, initDrugMD, initPinState);
+                }
+                led1 = 0;
             }while(InSetModePin);
             writeConfig();
         }
@@ -440,9 +486,9 @@
         led2 = 0;
         receiveDatas();
         sendDatas();
+        receiveFromPc();
         WriteServo();
         updateDatas();
-        receiveFromPc();
         led3 = !led3;
         wait(WAIT_LOOP_TIME);
     }