Diff: main.cpp

Branch:

mpu????????

Revision:

49:8522856fe0cd

Parent:

48:0bd406fa4a7f

Child:

50:b3a8f8e88c50

--- a/main.cpp	Thu Mar 23 15:58:55 2017 +0000
+++ b/main.cpp	Wed Mar 29 04:37:27 2017 +0000
@@ -16,17 +16,17 @@
 #define MPU_LOOP_TIME 0.01
 #define MPU_DELT_MIN 250
 
-#define ERURON_MOVE_DEG_INI_R 0.9
+#define ERURON_MOVE_DEG_INI_R 0.45
 #define DRUG_MOVE_DEG_INI_R -0.07
-#define ERURON_TRIM_INI_R 0.35
+#define ERURON_TRIM_INI_R 0.41
 #define DRUG_TRIM_INI_R 0.62
 
-#define ERURON_MOVE_DEG_INI_L 0.7
+#define ERURON_MOVE_DEG_INI_L 0.35
 #define DRUG_MOVE_DEG_INI_L -0.10
 #define ERURON_TRIM_INI_L 0.4
 #define DRUG_TRIM_INI_L 0.68//値を小さくすると開く側
 
-#define PHASE_NUM 13.0 //偶数にしてください。そしてメインコードと必ず同じ値にしてください
+#define PHASE_NUM 12.0 //奇数にしてください。そしてメインコードと必ず同じ値にしてください
 
 /*ドラッグラダー
 初期値 0.65
@@ -248,9 +248,9 @@
 void WriteServo()
 {
     drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *(float)controlValues[sizeof(int) + 2]));
-    eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * eruronint));
+    eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * (eruronint - 6.0)));
     pc.printf("WriteNum:%f      ",calcPulse( eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * eruronint));
-    pc.printf("drValue::%f   ef::%d\n\r",drugTrim + drugMoveDeg *(float)controlValues[sizeof(int) + 2],eruronint);
+    pc.printf("drValue::%f   ef::%f\n\r",drugTrim + drugMoveDeg *(float)controlValues[sizeof(int) + 2],eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * (eruronint - 6.0));
     //  pc.printf("raw:%f    sampled:%f\n\r",eruronfloat /3.0,SampleFloat(eruronfloat / 3.0));
 }
 
@@ -259,6 +259,7 @@
     led2 =  1;
     if(EDstatePin) {
         eruronTrim = eruronAna.read();
+    eruronServo.pulsewidth(calcPulse(eruronTrim));
     } else {
         drugTrim = drugAna.read();
         drugServo.pulsewidth(calcPulse(drugTrim));
@@ -275,6 +276,7 @@
     float drugTemp = drugAna.read();
     if(EDstatePin) {
         eruronMoveDeg = eruronTemp-eruronTrim;
+        eruronServo.pulsewidth(calcPulse(eruronTemp));
     } else {
         drugServo.pulsewidth(calcPulse(drugTemp));
         drugMoveDeg = drugTemp-drugTrim;