Diff: main.cpp

Branch:

mpu????????

Revision:

59:f007e543f8c9

Parent:

58:f84bd22fd586

Child:

60:a45dc19a6001

--- a/main.cpp	Tue Jun 06 13:26:03 2017 +0000
+++ b/main.cpp	Tue Jun 06 15:46:30 2017 +0000
@@ -27,9 +27,6 @@
 #define ERURON_TRIM_INI_L  0.402  // 値をお大きいくすると頭上げ
 #define DRUG_TRIM_INI_L    0.73//値を小さくすると開く側
 
-#define PHASE_NUM 7 //奇数にしてください。そしてメインコードと必ず同じ値にしてください
-#define NEUTRAL_PHASE (PHASE_NUM + 1) / 2.0
-
 /*ドラッグラダー
 初期値 0.65
 最大角0.99
@@ -62,13 +59,13 @@
 I2C i2c_mpu(p9,p10);
 
 char toSendDatas[TO_SEND_DATAS_NUM];
-char controlValues[sizeof(int) +3];//0~3:eruruon,4( sizeof(float)で指定してください ):drug
-char intvalues[sizeof(int)];
+char controlValues[sizeof(float) +3];//0~3:eruruon,4( sizeof(float)で指定してください ):drug
+char intvalues[sizeof(float)];
 float eruronTrim;
 float drugTrim;
 float eruronMoveDeg;
 float drugMoveDeg;
-int eruronint = NEUTRAL_PHASE;
+float eruronfloat = 0.0;
 unsigned short ina_val;
 double V,C;
 bool SERVO_FLAG;
@@ -247,10 +244,10 @@
     if(can.read(recmsg)) {
         for(int i = 0; i < CONTROL_VALUES_NUM; i++) {
             controlValues[i] = recmsg.data[i];
-            if(i<sizeof(int)) intvalues[i] = controlValues[i];
+            if(i<sizeof(float)) intvalues[i] = controlValues[i];
         }
         //   intvalues[0] = controlValues[0];
-        eruronint = *(const int *)intvalues;
+        eruronfloat = *(const float *)intvalues;
         led1 = !led1;
     }
 }
@@ -277,11 +274,10 @@
 
 void WriteServo()
 {
-    drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *(float)controlValues[sizeof(int) + 2]));
-    eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * (eruronint - NEUTRAL_PHASE)/(PHASE_NUM-NEUTRAL_PHASE)));
-    pc.printf("WriteNum:%f    ef:%d  ",calcPulse( eruronTrim + eruronMoveDeg * (1.0/PHASE_NUM) * eruronint),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));
+    drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *(float)controlValues[sizeof(float) + 2]));
+    eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * eruronfloat));
+    pc.printf("WriteNum:%f    ef:%d  ",calcPulse( eruronTrim + eruronMoveDeg * eruronfloat),eruronfloat);
+    pc.printf("drValue::%f   ei::%f\n\r",drugTrim + drugMoveDeg *(float)controlValues[sizeof(float) + 2]);
 }
 
 void setTrim()
@@ -296,7 +292,7 @@
     }
     //pc.printf("eruronTrim:%f    drugTrim:%f\n\r",eruronTrim,drugTrim);
     pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
-    pc.printf("eMD:%f   dMD:%f    ei:%d\n\r",eruronMoveDeg,drugMoveDeg,eruronint);
+    pc.printf("eMD:%f   dMD:%f    ei:%d\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
 }
 
 void setMaxDeg()
@@ -312,7 +308,7 @@
         drugMoveDeg = drugTemp-drugTrim;
     }
     pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
-    pc.printf("eMD:%f   dMD:%f    ef:%d\n\r",eruronMoveDeg,drugMoveDeg,eruronint);
+    pc.printf("eMD:%f   dMD:%f    ef:%d\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
     wait_us(10);
 }