Diff: main.cpp

Branch:

mpu????????

Revision:

48:0bd406fa4a7f

Parent:

47:5e3a836deaba

Child:

49:8522856fe0cd

Child:

52:ad28166cf6d3

--- a/main.cpp	Thu Mar 23 08:50:40 2017 +0000
+++ b/main.cpp	Thu Mar 23 15:58:55 2017 +0000
@@ -16,18 +16,16 @@
 #define MPU_LOOP_TIME 0.01
 #define MPU_DELT_MIN 250
 
-#define ERURON_MOVE_DEG_INI_R 1.0
-#define DRUG_MOVE_DEG_INI_R -0.32
+#define ERURON_MOVE_DEG_INI_R 0.9
+#define DRUG_MOVE_DEG_INI_R -0.07
 #define ERURON_TRIM_INI_R 0.35
 #define DRUG_TRIM_INI_R 0.62
 
-#define ERURON_MOVE_DEG_INI_L 0.8
-#define DRUG_MOVE_DEG_INI_L -0.43
+#define ERURON_MOVE_DEG_INI_L 0.7
+#define DRUG_MOVE_DEG_INI_L -0.10
 #define ERURON_TRIM_INI_L 0.4
 #define DRUG_TRIM_INI_L 0.68//値を小さくすると開く側
 
-#define ERURON_MOVE_RANGE 0.24
-
 #define PHASE_NUM 13.0 //偶数にしてください。そしてメインコードと必ず同じ値にしてください
 
 /*ドラッグラダー
@@ -45,7 +43,7 @@
 DigitalOut led1(LED1);
 AnalogIn drugAna(p20);
 AnalogIn eruronAna(p19);
-DigitalIn LRstatePin(p11);
+DigitalIn IsRPin(p11);
 DigitalIn setTrimPin(p12);
 DigitalIn EDstatePin(p14);
 DigitalIn setMaxDegPin(p15);
@@ -161,7 +159,7 @@
             pitch *= 180.0f / PI;
             yaw   *= 180.0f / PI;
             roll  *= 180.0f / PI;
-         //   myled= !myled;
+            //   myled= !myled;
             count = t.read_ms();
             sum = 0;
             sumCount = 0;
@@ -188,16 +186,16 @@
 
 void init()
 {
-    if(!LRstatePin) {
+    if(IsRPin) {
+        eruronTrim = ERURON_TRIM_INI_R;
+        drugTrim = DRUG_TRIM_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 = ERURON_MOVE_DEG_INI_L;
         drugMoveDeg = DRUG_MOVE_DEG_INI_L;
-    } else {
-        eruronTrim = ERURON_TRIM_INI_R;
-        drugTrim = DRUG_TRIM_INI_R;
-        eruronMoveDeg = ERURON_MOVE_DEG_INI_R;
-        drugMoveDeg =DRUG_MOVE_DEG_INI_R;
     }
     SERVO_FLAG = servoInit();
     INA_FLAG = inaInit();
@@ -250,8 +248,8 @@
 void WriteServo()
 {
     drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *(float)controlValues[sizeof(int) + 2]));
-    eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * ((1.0/PHASE_NUM) * eruronint) * ERURON_MOVE_RANGE));
-    pc.printf("WriteNum:%f      ",calcPulse( eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * eruronint* ERURON_MOVE_RANGE));
+    eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * eruronint));
+    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("raw:%f    sampled:%f\n\r",eruronfloat /3.0,SampleFloat(eruronfloat / 3.0));
 }
@@ -293,7 +291,7 @@
     setTrimPin.mode(PullDown);
     setMaxDegPin.mode(PullDown);
     EDstatePin.mode(PullDown);
-    LRstatePin.mode(PullDown);
+    IsRPin.mode(PullDown);
     Thread mpu_thread(&mpuProcessing);
 
     // start motion