Diff: main.cpp

Revision:

26:745735b0479d

Parent:

25:12408563540d

Child:

27:1af92a603532

--- a/main.cpp	Mon Mar 20 23:33:54 2017 +0000
+++ b/main.cpp	Tue Mar 21 10:34:08 2017 +0000
@@ -62,8 +62,8 @@
 CANMessage recmsg_L;
 
 enum InputType {
-    roll,
-    pitch
+    enumRoll,
+    enumPitch
 };
 
 void setNeutral()
@@ -77,61 +77,59 @@
     }
     rollNeutral = rollSum / SUM_UP_NUM;
     pitchNeutral = pitchSum / SUM_UP_NUM;
+
+    neutralDiff = pitchNeutral - rollNeutral; //ピッチの初期値の方がい小さいと仮定
+    rollNeutral += neutralDiff;
 }
 
 void setMaxAndMin(InputType it,float value)
 {
-    if(it = InputType.pitch) {
+    if(it == enumPitch) {
         if(value >pitchNeutral + pitchUpperDiff)
             pitchUpperDiff = value - pitchNeutral;
-        else if(value < rollNeutral - pitchLowerDiff)
-            pitchLowerDiff = pitchNeutral - value;
-    } else if(it = InputType.roll) {
+        else if(value < rollNeutral + pitchLowerDiff)
+            pitchLowerDiff = value - pitchNeutral;
+        led1 =!led1;
+        return;
+    } else if(it == enumRoll) {
         if(value >rollNeutral + rollUpperDiff)
             rollUpperDiff = value - rollNeutral;
-        else if(value < rollNeutral - rollLowerDiff)
-            rollLowerDiff = rollNeutral - value;
+        else if(value < rollNeutral + rollLowerDiff)
+            rollLowerDiff = value  -rollNeutral ;
+        led2 =! led2;
+        return;
     }
 }
 
-
-void calibrateRoll2Pitch()
-{
-    neutralDiff = pitchNeutral - rollNeutral; //ピッチの初期値の方がい小さいと仮定
-    rollNeutral += neutralDiff;
-    rollUpperDiff += neutralDiff;
-    rollLowerDiff += neutralDiff;
-}
-
 //ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします
-float MatchRollUpperAndLower(InputType it, float max,float min,float neutral,float value)
+float MatchUpperAndLower(InputType it, float max,float min,float neutral,float value)
 {
     float Upper = max- neutral;
     float Lower  = neutral - min;
 
-    if(it = InputType.roll) {
+    if(it == enumRoll) {
         if(Upper > Lower) {
             if(value < neutral) {
                 value =  neutral - ((neutral - value) * (Upper / Lower));
-                rollLowerDiff  =  (rollLowerDiff)* (Upper / Lower);
+                //      rollLowerDiff  =  (rollLowerDiff)* (Upper / Lower);     //現状範囲値を変えれてないから、ここは要対応。0で割ってしまってnanが出るようになったからコメントアウトした。
             }
         } else  {
             if(value > neutral) {
                 value =  neutral + ((value - neutral) * (Lower / Upper));
-                rollUpperDiff = rollUpperDiff* (Upper / Lower);
+                //       rollUpperDiff = rollUpperDiff* (Upper / Lower);
             }
         }
         return value;
-    } else if(it = InputType.pitch) {
+    } else if(it == enumPitch) {
         if(Upper > Lower) {
             if(value < neutral) {
                 value =  neutral - ((neutral - value) * (Upper / Lower));
-                pitchLowerDiff =  (pitchLowerDiff)* (Upper / Lower);
+                //  pitchLowerDiff =  (pitchLowerDiff)* (Upper / Lower);
             }
         } else  {
             if(value > neutral) {
                 value =  neutral + ((value - neutral) * (Lower / Upper));
-                pitchUpperDiff= (pitchUpperDiff)* (Upper / Lower);
+                //    pitchUpperDiff= (pitchUpperDiff)* (Upper / Lower);
             }
         }
         return value;
@@ -173,32 +171,32 @@
 
 void InputControlValues()
 {
-    setRollMaxAndMin(InputType.roll, rollPin.read());
-    setPitchMaxAndMin(InputType.pitch, pitchPin.read());
-    calibrateRoll2Pitch();
-    float MatchedRoll = MatchUpperAndLower(InputType.roll, rollNeutral + rollUpperDiff,rollLowerDiff + rollLowerDiff,rollNeutral,rollPin.read() - neutralDiff);
-    float MatchedPitch = MatchUpperAndLower(InputType.pitch, pitchNeutral + pitchUpperDiff,pitchNeutral + pitchLowerDiff,pitchNeutral,pitchPin.read());
+    setMaxAndMin(enumRoll, rollPin.read());
+    setMaxAndMin(enumPitch, pitchPin.read());
+
+    // pc.printf("rollN:%f     rollMax:%f     rollMin:%f   pitchN:%f   pitchMax:%f   pitchUpper:%f  pitchMin:%f   pitchLo:%f  raw:%f\r\n",rollNeutral,rollNeutral+rollUpperDiff,rollNeutral+rollLowerDiff,pitchNeutral,pitchNeutral+pitchUpperDiff,pitchUpperDiff,pitchNeutral+pitchLowerDiff,pitchLowerDiff,pitchPin.read());
+    float MatchedRoll = MatchUpperAndLower(enumRoll, rollNeutral + rollUpperDiff,rollLowerDiff + rollLowerDiff,rollNeutral,rollPin.read() + neutralDiff);
+    float MatchedPitch = MatchUpperAndLower(enumPitch, pitchNeutral + pitchUpperDiff,pitchNeutral + pitchLowerDiff,pitchNeutral,pitchPin.read());
 
     float FormatedRoll_R = Format2Range((MatchedPitch +MatchedRoll ) / 2.0,(rollNeutral + rollUpperDiff+ pitchNeutral + pitchUpperDiff) / 2.0,(rollNeutral + rollLowerDiff+ pitchNeutral + pitchLowerDiff) / 2.0);
     *(int *)inputDatas_R =PhaseFloat(FormatedRoll_R,(rollNeutral + rollUpperDiff+ pitchNeutral + pitchUpperDiff) / 2.0,(rollNeutral + rollLowerDiff+ pitchNeutral + pitchLowerDiff) / 2.0);
 
-    float FormatedRoll_L = Format2Range((MatchedPitch - MatchedRoll) / 2.0,(rollNeutral + rollUpperDiff- pitchNeutral  -pitchLowerDiff) / 2.0,(rollNeutral + rollLowerDiff- pitchNeutral - pitchUpperDiff) / 2.0);
-    *(int *)inputDatas_L = PhaseFloat(FormatedRoll_R,(rollNeutral + rollUpperDiff- pitchNeutral - pitchLowerDiff) / 2.0,(rollNeutral + rollLowerDiff- pitchNeutral + pitchUpperDiff) / 2.0);
+    float FormatedRoll_L = Format2Range((MatchedPitch - MatchedRoll) / 2.0,(pitchNeutral  +pitchLowerDiff - rollNeutral - rollUpperDiff) / 2.0,(pitchNeutral + pitchUpperDiff-rollNeutral - rollLowerDiff) / 2.0);
+    *(int *)inputDatas_L = PhaseFloat(FormatedRoll_L,(pitchNeutral + pitchLowerDiff - rollNeutral - rollUpperDiff) / 2.0,(pitchNeutral + pitchUpperDiff - rollNeutral - rollLowerDiff) / 2.0);
+    pc.printf("Format:%f    max:%f  min:%f\n\r",FormatedRoll_L,(pitchNeutral + pitchUpperDiff - rollNeutral - rollLowerDiff) / 2.0,(pitchNeutral + pitchLowerDiff - rollNeutral - rollUpperDiff) / 2.0);
+    if(*(int *)inputDatas_R < 0)
+        *(int *)inputDatas_R = 0;
+    else if(*(int *)inputDatas_R > PHASE_NUM * 2)
+        *(int *)inputDatas_R   = PHASE_NUM *2;
+    if(*(int *)inputDatas_L < 0)
+        *(int *)inputDatas_L = 0 ;
+    else if(*(int *)inputDatas_L > PHASE_NUM*2)
+        *(int *)inputDatas_L =PHASE_NUM * 2;
 
-    
-        if(*(int *)inputDatas_R < 0)
-            *(int *)inputDatas_R = 0;
-        else if(*(int *)inputDatas_R > PHASE_NUM)
-            *(int *)inputDatas_R   = PHASE_NUM;
-        if(*(int *)inputDatas_L < 0)
-            *(int *)inputDatas_L = 0 ;
-        else if(*(int *)inputDatas_L > PHASE_NUM)
-            *(int *)inputDatas_L =PHASE_NUM;
-
-    pc.printf("input_R:%d   input_L:%d\n\r",*(int *)inputDatas_R,*(int *)inputDatas_L);
+    //  pc.printf("input_R:%d   input_L:%d\n\r",*(int *)inputDatas_R,*(int *)inputDatas_L);
     inputDatas_R[4] = (char)drug_R;
     led4 =! led4;
-    pc.printf("%c",*(char *)inputDatas_R[4]);
+    // pc.printf("%c",*(char *)inputDatas_R[4]);
     //pc.printf("%c",(char)drug_R);
     inputDatas_L[4] = (char)drug_L;
     // pc.printf("");