albatross / Mbed 2 deprecated ControlMain2017_2

今までのジョイスティックの入力を段階分けしていた方式から、アナログ値をできる限りそのまま使用する方式に変更しました。

Dependencies:   mbed-rtos mbed

Fork of ControlMain2017 by albatross

Diff: main.cpp

Revision:
29:8bff47ab602d
Parent:
28:056287cb001c
Child:
30:eb7b7af1070a
diff -r 056287cb001c -r 8bff47ab602d main.cpp
--- a/main.cpp	Tue Mar 21 17:53:31 2017 +0000
+++ b/main.cpp	Tue Mar 21 19:47:11 2017 +0000
@@ -9,8 +9,9 @@
 #define THRESHOLD_OH_VALUE 0.14
 #define SAMPLE_INTERVAL 0.05
 
-#define PHASE_NUM 12 //奇数にしてください
+#define PHASE_NUM 13 //奇数にしてください
 #define SUM_UP_NUM 10.0
+#define PITCHPERROLL 1.5
 /*
 roll入力とピッチ入力だとピッチの方が1.5倍効くように
 */
@@ -166,6 +167,10 @@
         return PHASE_NUM;
 }
 
+float SetRollPitchRacio(float pitch,float roll){
+return (roll + pitch * PITCHPERROLL) / (1.0 + PITCHPERROLL);
+}
+
 void InputControlValues()
 {
     setMaxAndMin(enumRoll, rollPin.read());
@@ -175,12 +180,12 @@
     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_R = Format2Range(SetRollPitchRacio(MatchedPitch,MatchedRoll ),SetRollPitchRacio(pitchNeutral + pitchUpperDiff,rollNeutral + rollUpperDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff,rollNeutral + rollLowerDiff));
+    *(int *)inputDatas_R =PhaseFloat(FormatedRoll_R,SetRollPitchRacio(pitchNeutral + pitchUpperDiff,rollNeutral + rollUpperDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff,rollNeutral + rollLowerDiff));
 
-    float FormatedRoll_L = Format2Range((MatchedPitch - MatchedRoll) / 2.0,(pitchNeutral + pitchUpperDiff-rollNeutral - rollLowerDiff) / 2.0,(pitchNeutral  +pitchLowerDiff - rollNeutral - rollUpperDiff) / 2.0);
+    float FormatedRoll_L = Format2Range(SetRollPitchRacio(MatchedPitch, - MatchedRoll),SetRollPitchRacio(pitchNeutral + pitchUpperDiff,-rollNeutral - rollLowerDiff),SetRollPitchRacio(pitchNeutral  +pitchLowerDiff, - rollNeutral - rollUpperDiff));
     //pc.printf("matched:%f    max:%f  min:%f\n\r",(MatchedPitch - MatchedRoll) / 2.0,(pitchNeutral  +pitchLowerDiff - rollNeutral - rollUpperDiff) / 2.0,(pitchNeutral + pitchUpperDiff-rollNeutral - rollLowerDiff) / 2.0);
-    *(int *)inputDatas_L = PhaseFloat(FormatedRoll_L,(pitchNeutral + pitchUpperDiff - rollNeutral - rollLowerDiff) / 2.0,(pitchNeutral + pitchLowerDiff - rollNeutral - rollUpperDiff) / 2.0);
+    *(int *)inputDatas_L = PhaseFloat(FormatedRoll_L,SetRollPitchRacio(pitchNeutral + pitchUpperDiff ,- rollNeutral - rollLowerDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff ,- rollNeutral - rollUpperDiff));
     //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 < 1)
         *(int *)inputDatas_R = 1;