Diff: main.cpp

Branch:

mpu????????

Revision:

57:d7b709dd1c4f

Parent:

56:2b947d31d307

Child:

58:f84bd22fd586

--- a/main.cpp	Fri Apr 28 15:25:10 2017 +0000
+++ b/main.cpp	Sat May 13 15:20:02 2017 +0000
@@ -4,6 +4,7 @@
 #include "INA226.hpp"
 #include "rtos.h"
 #include "XBusServo.h"
+#include "math.h"
 
 #define TO_SEND_DATAS_NUM 7
 #define INIT_SERVO_PERIOD_MS 20
@@ -16,17 +17,17 @@
 #define MPU_LOOP_TIME 0.01
 #define MPU_DELT_MIN 250
 
-#define ERURON_MOVE_DEG_INI_R 0.45
+#define ERURON_MOVE_DEG_INI_R 18.0        //degree
 #define DRUG_MOVE_DEG_INI_R 0.49
-#define ERURON_TRIM_INI_R 0.41  //値36werすると頭上げ
+#define ERURON_TRIM_INI_R 0.38  //値lowerすると頭上げ
 #define DRUG_TRIM_INI_R 0.37
 
-#define ERURON_MOVE_DEG_INI_L -0.45
+#define ERURON_MOVE_DEG_INI_L -19.4     //degree
 #define DRUG_MOVE_DEG_INI_L -0.44
-#define ERURON_TRIM_INI_L 0.48  // 値をお大きいくすると頭上げ
+#define ERURON_TRIM_INI_L 0.402  // 値をお大きいくすると頭上げ
 #define DRUG_TRIM_INI_L 0.68//値を小さくすると開く側
 
-#define PHASE_NUM 23 //奇数にしてください。そしてメインコードと必ず同じ値にしてください
+#define PHASE_NUM 15 //奇数にしてください。そしてメインコードと必ず同じ値にしてください
 #define NEUTRAL_PHASE (PHASE_NUM + 1) / 2.0
 
 /*ドラッグラダー
@@ -185,17 +186,37 @@
     return true;
 }
 
+//動かしたいエレボンの角度から、動かしたいサーボホーンの角度を得る。
+double ConvertDeg(double servo)
+{
+    double result = 0.0003*pow(servo,3)+0.0039*pow(servo,2)+1.746*servo - 0.0105;
+    return result;
+}
+
+//ホーンを動かしたい角度から、変化させるアナログ値の幅を得る。3度変化
+double GetValueByHorn(double deg)
+{
+    return deg*(0.10/25.7);
+}
+
+double GetFloatByErebon(double erebonDeg)
+{
+    double servoDeg = ConvertDeg(erebonDeg);
+    double FirstMoveDeg = GetValueByHorn(servoDeg);
+    return FirstMoveDeg;
+}
+
 void init()
 {
     if(IsRPin) {
         eruronTrim = ERURON_TRIM_INI_R;
         drugTrim = DRUG_TRIM_INI_R;
-        eruronMoveDeg = ERURON_MOVE_DEG_INI_R;
+        eruronMoveDeg =GetFloatByErebon(ERURON_MOVE_DEG_INI_R);
         drugMoveDeg =DRUG_MOVE_DEG_INI_R;
     } else {
         eruronTrim = ERURON_TRIM_INI_L;
         drugTrim = DRUG_TRIM_INI_L;
-        eruronMoveDeg = ERURON_MOVE_DEG_INI_L;
+        eruronMoveDeg = GetFloatByErebon(ERURON_MOVE_DEG_INI_L);
         drugMoveDeg = DRUG_MOVE_DEG_INI_L;
     }
     SERVO_FLAG = servoInit();
@@ -257,7 +278,7 @@
 void WriteServo()
 {
     drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *(float)controlValues[sizeof(int) + 2]));
-    eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * (eruronint - NEUTRAL_PHASE)));
+    eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (eruronint - NEUTRAL_PHASE)/(PHASE_NUM-NEUTRAL_PHASE)));
     pc.printf("WriteNum:%f      ",calcPulse( eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * eruronint));
     pc.printf("drValue::%f   ei::%f\n\r",drugTrim + drugMoveDeg *(float)controlValues[sizeof(int) + 2],eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * (eruronint - NEUTRAL_PHASE));
     //  pc.printf("raw:%f    sampled:%f\n\r",eruronfloat /3.0,SampleFloat(eruronfloat / 3.0));
@@ -315,9 +336,6 @@
         while (setMaxDegPin) {
             setMaxDeg();
         }
-        //  pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
-        ////  pc.printf("eMD:%f   dMD:%f    ",eruronMoveDeg,drugMoveDeg);
-        //  pc.printf("pitch:%d      roll:%d     yaw:%d\n\r",pitch,roll,yaw);
         led4 = 0;
         led2 = 0;
         //receiveDatas();