Diff: main.cpp

Revision:

20:d4951f491642

Parent:

19:6387e3f02b37

Child:

21:9b1f5123f4a8

--- a/main.cpp	Sun Mar 19 12:18:41 2017 +0000
+++ b/main.cpp	Sun Mar 19 14:22:39 2017 +0000
@@ -26,7 +26,7 @@
 #define DEC_INPUT_NEUTRAL 0
 #define DEC_INPUT_MAX (PITCH_INPUT_MAX -ROLL_INPUT_MIN ) / 2.0
 #define DEC_INPUT_MIN (PITCH_INPUT_MIN -ROLL_INPUT_MAX ) / 2.0
-#define PHASE_NUM 7 //奇数にしてください
+#define PHASE_NUM 6 //偶数にしてください
 /*
 roll入力とピッチ入力だとピッチの方が1.5倍効くように
 */
@@ -67,10 +67,22 @@
 bool stateQ = true;
 bool stateP_old = true;
 bool stateQ_old = true;
+float changedRollMax;
+float changedRollMin;
+float changedPitchMax;
+float changedPitchMin;
 
 CANMessage recmsg_R;
 CANMessage recmsg_L;
 
+
+int SampleFloat(float f)
+{
+    int temp = ((f + 0.025) * 100.0) / 5;
+    float result = temp / 20.0;
+    return result;
+}
+
 float get_analogin_ave(AnalogIn pin)
 {
     float val = 0;
@@ -80,17 +92,46 @@
     return val/SUM_UP_NUM;
 }
 
-//値を0から1の範囲に収めます
-float FormatSumed2Range(float value)
+//ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします
+float MatchRollUpperAndLower(float max,float min,float neutral,float value)
 {
-    float result;
-    if(value > 1)
-        result = 1;
-    else if(value < 0)
-        result = 0;
-    else
-        result  = value;
-    return result;
+    float Upper = max- neutral;
+    float Lower  = neutral - min;
+    if(Upper > Lower) {
+        if(value < neutral) {
+            value =  neutral - ((neutral - value) * (Upper / Lower));
+            changedRollMin = ROLL_INPUT_NEUTRAL - (ROLL_INPUT_NEUTRAL - ROLL_INPUT_MIN)* (Upper / Lower);
+            changedRollMax = ROLL_INPUT_MAX;
+        }
+    } else  {
+        if(value > neutral) {
+            value =  neutral + ((value - neutral) * (Lower / Upper));
+            changedRollMax = ROLL_INPUT_NEUTRAL + (ROLL_INPUT_MAX - ROLL_INPUT_NEUTRAL)* (Upper / Lower);
+            changedRollMin = ROLL_INPUT_MIN;
+        }
+    }
+    return value;
+}
+
+//ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします
+float MatchPitchUpperAndLower(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));
+            changedPitchMin = PITCH_INPUT_NEUTRAL - (PITCH_INPUT_NEUTRAL - PITCH_INPUT_MIN)* (Upper / Lower);
+            changedPitchMax = PITCH_INPUT_MAX;
+        }
+    } else  {
+        if(value > neutral) {
+            value =  neutral + ((value - neutral) * (Lower / Upper));
+            changedPitchMax = PITCH_INPUT_NEUTRAL + (PITCH_INPUT_MAX - PITCH_INPUT_NEUTRAL)* (Upper / Lower);
+            changedPitchMin = PITCH_INPUT_MIN;
+        }
+    }
+    return value;
 }
 
 float FormatEach2Range(float value,float max,float min)
@@ -105,28 +146,17 @@
     return result;
 }
 
-//ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします
-float MatchUpperAndLower(float max,float min,float neutral,float value)
+//値を0から1の範囲に収めます
+float FormatSumed2Range(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;
+    float result;
+    if(value > 1)
+        result = 1;
+    else if(value < 0)
+        result = 0;
+    else
+        result  = value;
     return result;
-
 }
 
 int PhaseFloat(float value,float max,float min)
@@ -148,16 +178,17 @@
 void InputControlValues()
 {
     pc.printf("Roll:%f      Pitch:%f    ",rollPin.read() - (OLD_ROLL_NEUTRAL - PITCH_INPUT_NEUTRAL),pitchPin.read());
-    float MatchedRoll = MatchUpperAndLower(ROLL_INPUT_MAX,ROLL_INPUT_MIN,ROLL_INPUT_NEUTRAL,rollPin.read() - (OLD_ROLL_NEUTRAL - PITCH_INPUT_NEUTRAL));
-    float MatchedPitch = MatchUpperAndLower(PITCH_INPUT_MAX,ROLL_INPUT_MIN,PITCH_INPUT_NEUTRAL,pitchPin.read());
-    float FormatedEachRollR = FormatEach2Range((MatchedPitch +MatchedRoll ) / 2.0,SUMED_INPUT_MAX,SUMED_INPUT_MIN);
+    float MatchedRoll = MatchRollUpperAndLower(ROLL_INPUT_MAX,ROLL_INPUT_MIN,ROLL_INPUT_NEUTRAL,rollPin.read() - (OLD_ROLL_NEUTRAL - PITCH_INPUT_NEUTRAL));
+    float MatchedPitch = MatchPitchUpperAndLower(PITCH_INPUT_MAX,ROLL_INPUT_MIN,PITCH_INPUT_NEUTRAL,pitchPin.read());
+    float FormatedEachRollR = FormatEach2Range((MatchedPitch +MatchedRoll ) / 2.0,(changedRollMax + changedPitchMax) / 2.0,(changedRollMin + changedPitchMin) / 2.0);
     float FormatSumedR =  FormatSumed2Range(FormatedEachRollR);
-    *(int *)inputDatas_R =PhaseFloat(FormatSumedR,SUMED_INPUT_MAX,SUMED_INPUT_MIN);
+    *(int *)inputDatas_R =PhaseFloat(FormatSumedR,(changedRollMax + changedPitchMax)  / 2.0,(changedRollMin + changedPitchMin) / 2.0);
     pc.printf("FormatR:%f    ",FormatSumedR);
-    float FormatedEachRollL = FormatEach2Range((MatchedPitch - MatchedRoll) / 2.0,DEC_INPUT_MAX,DEC_INPUT_MIN);
+    float FormatedEachRollL = FormatEach2Range((MatchedPitch - MatchedRoll) / 2.0,(changedPitchMax - changedRollMin)  /2.0,(changedPitchMin - changedRollMax) / 2.0);
+    pc.printf("FormatL:%f    ",FormatedEachRollL);
     float FormatSumedL =  FormatSumed2Range(FormatedEachRollL);
     pc.printf("FormatL:%f    ",FormatSumedL);
-    *(int *)inputDatas_L = PhaseFloat(FormatSumedR,DEC_INPUT_MAX,DEC_INPUT_MIN);
+    *(int *)inputDatas_L = PhaseFloat(FormatSumedR,changedPitchMax - changedRollMin,changedPitchMin - changedRollMax);
 
     if(*(int *)inputDatas_R < 0)
         *(int *)inputDatas_R = 0;