Diff: main.cpp

Revision:

24:72f9cb6e9440

Parent:

21:9b1f5123f4a8

Child:

25:12408563540d

--- a/main.cpp	Sun Mar 19 15:07:25 2017 +0000
+++ b/main.cpp	Mon Mar 20 16:10:41 2017 +0000
@@ -10,16 +10,8 @@
 #define SUM_UP_NUM 20
 #define SAMPLE_INTERVAL 0.05
 
-#define PITCH_INPUT_NEUTRAL 0.468 //1って書いた方
-#define PITCH_INPUT_MAX 0.884
-#define PITCH_INPUT_MIN 0.110
-
-#define OLD_ROLL_NEUTRAL 0.739
-#define ROLL_INPUT_NEUTRAL 0.468 //２て書いた方
-#define ROLL_INPUT_MAX 0.963 - (OLD_ROLL_NEUTRAL - PITCH_INPUT_NEUTRAL)
-#define ROLL_INPUT_MIN 0.555 - (OLD_ROLL_NEUTRAL - PITCH_INPUT_NEUTRAL)
-
 #define PHASE_NUM 6 //偶数にしてください
+#define SUM_UP_NUM 10.0
 /*
 roll入力とピッチ入力だとピッチの方が1.5倍効くように
 */
@@ -60,14 +52,58 @@
 bool stateQ = true;
 bool stateP_old = true;
 bool stateQ_old = true;
-float changedRollMax;
-float changedRollMin;
-float changedPitchMax;
-float changedPitchMin;
+
+float changedRollneutral = 0.739;
+float rollUpperDiff = 0.224;
+float rollLowerDiff = -0.184;
+
+float changedPitchNeutral = 0.468 ;//1って書いた方
+float pitchUpperDiff = 0.416;
+float pitchLowerDiff = -0.358;
+float neutralDiff;
 
 CANMessage recmsg_R;
 CANMessage recmsg_L;
 
+void setNeutral()
+{
+    float rollSum;
+    float pitchSum;
+    
+    for(int i = 0; i < SUM_UP_NUM; i++) {
+        rollSum += rollPin.read();
+        pitchSum += pitchPin.read();
+    }
+    changedRollneutral = rollSum / SUM_UP_NUM;
+    changedPitchNeutral = pitchSum / SUM_UP_NUM;
+}
+
+
+void setRollMaxAndMin(float value)
+{
+    if(value >changedRollneutral + rollUpperDiff)
+        rollUpperDiff = value - changedRollneutral;
+    else if(value < changedRollneutral - rollLowerDiff)
+        rollLowerDiff = changedRollneutral - value;
+}
+
+void setPitchMaxAndMin(float value)
+{
+    if(value >changedPitchNeutral + pitchUpperDiff)
+        pitchUpperDiff = value - changedPitchNeutral;
+    else if(value < changedRollneutral - pitchLowerDiff)
+        pitchLowerDiff = changedPitchNeutral - value;
+}
+
+
+void calibrateNeutral()
+{
+    neutralDiff = changedPitchNeutral - changedRollneutral; //ピッチの初期値の方がい小さいと仮定
+    changedRollneutral = changedRollneutral + neutralDiff;
+    changedRollneutral  = rollUpperDiff + neutralDiff;
+    changedRollneutral  = rollLowerDiff + neutralDiff;
+}
+
 //ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします
 float MatchRollUpperAndLower(float max,float min,float neutral,float value)
 {
@@ -76,14 +112,12 @@
     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;
+            rollLowerDiff  =  (rollLowerDiff)* (Upper / Lower);
         }
     } 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;
+            rollUpperDiff = rollUpperDiff* (Upper / Lower);
         }
     }
     return value;
@@ -97,14 +131,12 @@
     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;
+            pitchLowerDiff =  (pitchLowerDiff)* (Upper / Lower);
         }
     } 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;
+            pitchUpperDiff= (pitchUpperDiff)* (Upper / Lower);
         }
     }
     return value;
@@ -146,15 +178,17 @@
 
 void InputControlValues()
 {
-    pc.printf("Roll:%f      Pitch:%f    ",rollPin.read() - (OLD_ROLL_NEUTRAL - PITCH_INPUT_NEUTRAL),pitchPin.read());
-    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 FormatedRollR = Format2Range((MatchedPitch +MatchedRoll ) / 2.0,(changedRollMax + changedPitchMax) / 2.0,(changedRollMin + changedPitchMin) / 2.0);
-    *(int *)inputDatas_R =PhaseFloat(FormatedRollR,(changedRollMax + changedPitchMax)  / 2.0,(changedRollMin + changedPitchMin) / 2.0);
-    
-    float FormatedRollL = Format2Range((MatchedPitch - MatchedRoll) / 2.0,(changedPitchMax - changedRollMin)  /2.0,(changedPitchMin - changedRollMax) / 2.0);
-    *(int *)inputDatas_L = PhaseFloat(FormatedRollR,changedPitchMax - changedRollMin,changedPitchMin - changedRollMax);
+    setRollMaxAndMin(rollPin.read());
+    setPitchMaxAndMin(pitchPin.read());
+    calibrateNeutral();
+    float MatchedRoll = MatchRollUpperAndLower(changedRollneutral + rollUpperDiff,rollLowerDiff + rollLowerDiff,changedRollneutral,rollPin.read() - neutralDiff);
+    float MatchedPitch = MatchPitchUpperAndLower(changedPitchNeutral + pitchUpperDiff,changedPitchNeutral + pitchLowerDiff,changedPitchNeutral,pitchPin.read());
+
+    float FormatedRollR = Format2Range((MatchedPitch +MatchedRoll ) / 2.0,(changedRollneutral + rollUpperDiff+ changedPitchNeutral + pitchUpperDiff) / 2.0,(changedRollneutral + rollLowerDiff+ changedPitchNeutral + pitchLowerDiff) / 2.0);
+    *(int *)inputDatas_R =PhaseFloat(FormatedRollR,(changedRollneutral + rollUpperDiff+ changedPitchNeutral + pitchUpperDiff) / 2.0,(changedRollneutral + rollLowerDiff+ changedPitchNeutral + pitchLowerDiff) / 2.0);
+
+    float FormatedRollL = Format2Range((MatchedPitch - MatchedRoll) / 2.0,(changedRollneutral + rollUpperDiff- changedPitchNeutral  -pitchLowerDiff) / 2.0,(changedRollneutral + rollLowerDiff- changedPitchNeutral - pitchUpperDiff) / 2.0);
+    *(int *)inputDatas_L = PhaseFloat(FormatedRollR,(changedRollneutral + rollUpperDiff- changedPitchNeutral - pitchLowerDiff) / 2.0,(changedRollneutral + rollLowerDiff- changedPitchNeutral + pitchUpperDiff) / 2.0);
 
     if(*(int *)inputDatas_R < 0)
         *(int *)inputDatas_R = 0;
@@ -243,6 +277,7 @@
 int main()
 {
     init();
+    setNeutral();
     while(1) {
         InputControlValues();
         wait_us(5);