Diff: main.cpp

Revision:

18:31722545ecf1

Parent:

16:336e8b102555

Child:

19:6387e3f02b37

--- a/main.cpp	Sat Mar 11 21:32:50 2017 +0000
+++ b/main.cpp	Sun Mar 19 10:44:14 2017 +0000
@@ -10,6 +10,23 @@
 #define SUM_UP_NUM 20
 #define SAMPLE_INTERVAL 0.05
 
+#define PITCH_INPUT_NEUTRAL 0.457 //1って書いた方
+#define PITCH_INPUT_MAX 0.884
+#define PITCH_INPUT_MIN 0.110
+
+#define ROLL_INPUT_NEUTRAL 0.739 //２て書いた方
+#define ROLL_INPUT_MAX 0.963
+#define ROLL_INPUT_MIN 0.555
+
+#define SUMED_INPUT_NEUTRAL 0.598
+#define SUMED_INPUT_MAX 0.9235
+#define SUMED_INPUT_MIN 0.3325
+
+#define DEC_INPUT_NEUTRAL -0.141
+#define DEC_INPUT_MAX -0.0395
+#define DEC_INPUT_MIN -0.2225
+#define PHASE_NUM 7 //奇数にしてください
+
 //-----------------------------------(resetInterrupt def)
 extern "C" void mbed_reset();
 InterruptIn resetPin(p22);
@@ -29,7 +46,7 @@
 Serial toKeikiSerial(p28,p27);
 Serial pc(USBTX,USBRX);
 AnalogIn rollPin(p15);
-AnalogIn pichPin(p20);
+AnalogIn pitchPin(p20);
 DigitalIn drug_R(p14);
 DigitalIn drug_L(p19);
 DigitalOut myled1(LED1);
@@ -59,19 +76,91 @@
     return val/SUM_UP_NUM;
 }
 
+//値を0から1の範囲に収めます
+float FormatSumed2Range(float value)
+{
+    float result;
+    if(value > 1)
+        result = 1;
+    else if(value < 0)
+        result = 0;
+    else
+        result  = value;
+    return result;
+}
+
+float FormatEach2Range(float value,float max,float min)
+{
+    float result;
+    if(value > max)
+        result= max;
+    else if(value < min)
+        result = min;
+    else
+        result = value;
+    return result;
+}
+
+//ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします
+float MatchUpperAndLower(float max,float min,float neutral,float value)
+{
+    float Upper = max- neutral;
+    float Lower  = neutral - min;
+    if(Upper > Lower) {
+        if(value < neutral)
+            value =  neutral - ((neutral - value) * (Upper / Lower));
+    } else  {
+        if(value > neutral)
+            value =  neutral + ((value - neutral) * (Lower / Upper));
+    }
+    return value;
+}
+
+//Format2Rangeの後に呼び出してください
+int SampleFloat(float f)
+{
+    int temp = ((f + 0.025) * 100.0) / 5;
+    float result = temp / 20.0;
+    return result;
+
+}
+
+int PhaseFloat(float value,float max,float min)
+{
+    float PhaseWidth = (max - min) / PHASE_NUM;
+    if(value< max&& value > min) {
+        for(int i = 1; i <= PHASE_NUM; i++) {
+            if(value < min + PhaseWidth * i&& value > min + PhaseWidth * (i - 1) )
+                return i;
+        }
+    } else  if(value <= min)
+        return 0;
+    else if(value>=max)
+        return PHASE_NUM;
+
+}
+
+
 void InputControlValues()
 {
-    *(float *)inputDatas_R = get_analogin_ave(pichPin) * (2.0/3.0) + get_analogin_ave(rollPin) * (1.0 / 3.0);
-    *(float *)inputDatas_L = get_analogin_ave(pichPin) * (2.0/3.0) - get_analogin_ave(rollPin) * (1.0 / 3.0);
+    float MatchedRoll = MatchUpperAndLower(ROLL_INPUT_MAX,ROLL_INPUT_MIN,ROLL_INPUT_NEUTRAL,rollPin.read());
+    float MatchedPitch = MatchUpperAndLower(PITCH_INPUT_MAX,ROLL_INPUT_MIN,PITCH_INPUT_NEUTRAL,pitchPin.read() - (ROLL_INPUT_NEUTRAL - PITCH_INPUT_NEUTRAL));
+    float FormatedEachRollR = FormatEach2Range((MatchedRoll + MatchedPitch) / 2.0,SUMED_INPUT_MAX,SUMED_INPUT_MIN);
+    float FormatSumedR =  FormatSumed2Range(FormatedEachRollR);
+    *(int *)inputDatas_R =PhaseFloat(FormatSumedR,ROLL_INPUT_MAX,ROLL_INPUT_MIN);
+    
+     float FormatedEachRollL = FormatEach2Range((MatchedRoll - MatchedPitch) / 2.0,ROLL_INPUT_MAX,ROLL_INPUT_MIN);
+    float FormatSumedL =  FormatSumed2Range(FormatedEachRollL);
+    *(int *)inputDatas_L = PhaseFloat(FormatSumedR,ROLL_INPUT_MAX,ROLL_INPUT_MIN);
 
     if(*(float *)inputDatas_R < 0)
         *(float *)inputDatas_R = 0;
-    else if(*(float *)inputDatas_R > 1)
-        *(float *)inputDatas_R = 1;
+    else if(*(int *)inputDatas_R > PHASE_NUM)
+        *(int *)inputDatas_R = PHASE_NUM;
     if(*(float *)inputDatas_L < 0)
         *(float *)inputDatas_L = 0 ;
-    else if(*(float *)inputDatas_L > 1)
-        *(float *)inputDatas_L =1;
+    else if(*(int *)inputDatas_L > PHASE_NUM)
+        *(int *)inputDatas_L =PHASE_NUM;
 
     pc.printf("input_R:%f   input_L:%f\n\r",*(float *)inputDatas_R,*(float *)inputDatas_L);
     inputDatas_R[4] = (char)drug_R;
@@ -82,6 +171,8 @@
     // pc.printf("");
 }
 
+
+
 void toString_R()
 {
     pc.printf("R:");