今までのジョイスティックの入力を段階分けしていた方式から、アナログ値をできる限りそのまま使用する方式に変更しました。
Fork of ControlMain2017 by
ValueProcessor.cpp@48:2965c51ef540, 2017-06-02 (annotated)
- Committer:
- YusukeWakuta
- Date:
- Fri Jun 02 05:21:47 2017 +0000
- Branch:
- ?????
- Revision:
- 48:2965c51ef540
- Parent:
- 47:6f84dcf7d1d8
valueprocessor??????push; ;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
YusukeWakuta | 41:d2d123d7a6fa | 1 | #include "ValueProcessor.h" |
YusukeWakuta | 42:6cc53727acc2 | 2 | #include "mbed.h" |
YusukeWakuta | 41:d2d123d7a6fa | 3 | |
YusukeWakuta | 47:6f84dcf7d1d8 | 4 | #define PITCHPERROLL 1.5 |
YusukeWakuta | 43:2d8bffac06cd | 5 | |
YusukeWakuta | 42:6cc53727acc2 | 6 | ValueProcess::ValueProcess() |
YusukeWakuta | 42:6cc53727acc2 | 7 | { |
YusukeWakuta | 42:6cc53727acc2 | 8 | rollNeutral = 0.739; |
YusukeWakuta | 42:6cc53727acc2 | 9 | rollUpperDiff = 0; |
YusukeWakuta | 42:6cc53727acc2 | 10 | rollLowerDiff = 0; |
YusukeWakuta | 42:6cc53727acc2 | 11 | |
YusukeWakuta | 42:6cc53727acc2 | 12 | pitchNeutral = 0.468 ;//1って書いた方 |
YusukeWakuta | 42:6cc53727acc2 | 13 | pitchUpperDiff = 0; |
YusukeWakuta | 42:6cc53727acc2 | 14 | pitchLowerDiff = 0; |
YusukeWakuta | 42:6cc53727acc2 | 15 | neutralDiff = 0; |
YusukeWakuta | 42:6cc53727acc2 | 16 | } |
YusukeWakuta | 41:d2d123d7a6fa | 17 | |
YusukeWakuta | 44:7b6f627a31da | 18 | //中央の値の幅を増やします |
YusukeWakuta | 44:7b6f627a31da | 19 | int ValueProcess::WidenNeutral(int phased) |
YusukeWakuta | 44:7b6f627a31da | 20 | { |
YusukeWakuta | 44:7b6f627a31da | 21 | int AddNeutral = ADDITIONALPHASE / 2; |
YusukeWakuta | 46:53eb2ba661c5 | 22 | int neutralPhase = (int)((PHASE_NUM + ADDITIONALPHASE+1) / 2.0); |
YusukeWakuta | 46:53eb2ba661c5 | 23 | int Np = (int)((PHASE_NUM + ADDITIONALPHASE ) / 2.0); |
YusukeWakuta | 46:53eb2ba661c5 | 24 | if (neutralPhase - AddNeutral <= phased && phased <= neutralPhase + AddNeutral) |
YusukeWakuta | 46:53eb2ba661c5 | 25 | phased = neutralPhase - AddNeutral; |
YusukeWakuta | 46:53eb2ba661c5 | 26 | else if (neutralPhase + AddNeutral < phased) |
YusukeWakuta | 46:53eb2ba661c5 | 27 | phased -= ADDITIONALPHASE; |
YusukeWakuta | 45:df3b45a8cecc | 28 | |
YusukeWakuta | 46:53eb2ba661c5 | 29 | return phased; |
YusukeWakuta | 44:7b6f627a31da | 30 | } |
YusukeWakuta | 44:7b6f627a31da | 31 | |
YusukeWakuta | 41:d2d123d7a6fa | 32 | void ValueProcess::setMaxAndMin(InputType it,float value) |
YusukeWakuta | 41:d2d123d7a6fa | 33 | { |
YusukeWakuta | 41:d2d123d7a6fa | 34 | if(it == enumPitch) { |
YusukeWakuta | 41:d2d123d7a6fa | 35 | if(value >pitchNeutral + pitchUpperDiff) |
YusukeWakuta | 41:d2d123d7a6fa | 36 | pitchUpperDiff = value - pitchNeutral; |
YusukeWakuta | 41:d2d123d7a6fa | 37 | else if(value < rollNeutral + pitchLowerDiff) |
YusukeWakuta | 41:d2d123d7a6fa | 38 | pitchLowerDiff = value - pitchNeutral; |
YusukeWakuta | 41:d2d123d7a6fa | 39 | return; |
YusukeWakuta | 41:d2d123d7a6fa | 40 | } else if(it == enumRoll) { |
YusukeWakuta | 41:d2d123d7a6fa | 41 | if(value >rollNeutral + rollUpperDiff) |
YusukeWakuta | 41:d2d123d7a6fa | 42 | rollUpperDiff = value - rollNeutral; |
YusukeWakuta | 41:d2d123d7a6fa | 43 | else if(value < rollNeutral + rollLowerDiff) |
YusukeWakuta | 41:d2d123d7a6fa | 44 | rollLowerDiff = value -rollNeutral ; |
YusukeWakuta | 41:d2d123d7a6fa | 45 | return; |
YusukeWakuta | 41:d2d123d7a6fa | 46 | } |
YusukeWakuta | 41:d2d123d7a6fa | 47 | } |
YusukeWakuta | 41:d2d123d7a6fa | 48 | |
YusukeWakuta | 42:6cc53727acc2 | 49 | void ValueProcess::setNeutral(float rollRead,float pitchRead) |
YusukeWakuta | 41:d2d123d7a6fa | 50 | { |
YusukeWakuta | 41:d2d123d7a6fa | 51 | float rollSum; |
YusukeWakuta | 41:d2d123d7a6fa | 52 | float pitchSum; |
YusukeWakuta | 41:d2d123d7a6fa | 53 | |
YusukeWakuta | 41:d2d123d7a6fa | 54 | for(int i = 0; i < SUM_UP_NUM; i++) { |
YusukeWakuta | 42:6cc53727acc2 | 55 | rollSum += rollRead; |
YusukeWakuta | 42:6cc53727acc2 | 56 | pitchSum += pitchRead; |
YusukeWakuta | 41:d2d123d7a6fa | 57 | } |
YusukeWakuta | 41:d2d123d7a6fa | 58 | rollNeutral = rollSum / SUM_UP_NUM; |
YusukeWakuta | 41:d2d123d7a6fa | 59 | pitchNeutral = pitchSum / SUM_UP_NUM; |
YusukeWakuta | 41:d2d123d7a6fa | 60 | |
YusukeWakuta | 41:d2d123d7a6fa | 61 | neutralDiff = pitchNeutral - rollNeutral; //ピッチの初期値の方がい小さいと仮定 |
YusukeWakuta | 41:d2d123d7a6fa | 62 | rollNeutral += neutralDiff; |
YusukeWakuta | 41:d2d123d7a6fa | 63 | } |
YusukeWakuta | 41:d2d123d7a6fa | 64 | |
YusukeWakuta | 41:d2d123d7a6fa | 65 | //ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします |
YusukeWakuta | 42:6cc53727acc2 | 66 | float ValueProcess::MatchUpperAndLower(InputType it, float max,float min,float neutral,float value) |
YusukeWakuta | 41:d2d123d7a6fa | 67 | { |
YusukeWakuta | 41:d2d123d7a6fa | 68 | float Upper = max- neutral; |
YusukeWakuta | 41:d2d123d7a6fa | 69 | float Lower = neutral - min; |
YusukeWakuta | 41:d2d123d7a6fa | 70 | |
YusukeWakuta | 41:d2d123d7a6fa | 71 | if(it == enumRoll) { |
YusukeWakuta | 41:d2d123d7a6fa | 72 | if(Upper > Lower) { |
YusukeWakuta | 41:d2d123d7a6fa | 73 | if(value < neutral) { |
YusukeWakuta | 41:d2d123d7a6fa | 74 | value = neutral + ((value - neutral) * (Upper / Lower)); |
YusukeWakuta | 41:d2d123d7a6fa | 75 | rollLowerDiff = -rollUpperDiff; |
YusukeWakuta | 41:d2d123d7a6fa | 76 | } |
YusukeWakuta | 41:d2d123d7a6fa | 77 | } else { |
YusukeWakuta | 41:d2d123d7a6fa | 78 | if(value > neutral) { |
YusukeWakuta | 41:d2d123d7a6fa | 79 | value = neutral + ((value - neutral) * (Lower / Upper)); |
YusukeWakuta | 41:d2d123d7a6fa | 80 | rollUpperDiff = -rollLowerDiff; |
YusukeWakuta | 41:d2d123d7a6fa | 81 | } |
YusukeWakuta | 41:d2d123d7a6fa | 82 | } |
YusukeWakuta | 41:d2d123d7a6fa | 83 | } else if(it == enumPitch) { |
YusukeWakuta | 41:d2d123d7a6fa | 84 | if(Upper > Lower) { |
YusukeWakuta | 41:d2d123d7a6fa | 85 | if(value < neutral) { |
YusukeWakuta | 41:d2d123d7a6fa | 86 | value = neutral + ((value -neutral) * (Upper / Lower)); |
YusukeWakuta | 41:d2d123d7a6fa | 87 | pitchLowerDiff = -pitchUpperDiff; |
YusukeWakuta | 41:d2d123d7a6fa | 88 | } |
YusukeWakuta | 41:d2d123d7a6fa | 89 | } else { |
YusukeWakuta | 41:d2d123d7a6fa | 90 | if(value > neutral) { |
YusukeWakuta | 41:d2d123d7a6fa | 91 | value = neutral + ((value - neutral) * (Lower / Upper)); |
YusukeWakuta | 41:d2d123d7a6fa | 92 | pitchUpperDiff= -pitchLowerDiff; |
YusukeWakuta | 41:d2d123d7a6fa | 93 | } |
YusukeWakuta | 41:d2d123d7a6fa | 94 | } |
YusukeWakuta | 43:2d8bffac06cd | 95 | } |
YusukeWakuta | 43:2d8bffac06cd | 96 | return value; |
YusukeWakuta | 41:d2d123d7a6fa | 97 | } |
YusukeWakuta | 41:d2d123d7a6fa | 98 | |
YusukeWakuta | 41:d2d123d7a6fa | 99 | //範囲外に値がない場合にエラーが発生するので範囲内に収める |
YusukeWakuta | 42:6cc53727acc2 | 100 | float ValueProcess::Format2Range(float value,float max,float min) |
YusukeWakuta | 41:d2d123d7a6fa | 101 | { |
YusukeWakuta | 41:d2d123d7a6fa | 102 | float result; |
YusukeWakuta | 41:d2d123d7a6fa | 103 | |
YusukeWakuta | 41:d2d123d7a6fa | 104 | if(value > max) |
YusukeWakuta | 41:d2d123d7a6fa | 105 | result= max; |
YusukeWakuta | 41:d2d123d7a6fa | 106 | else if(value < min) |
YusukeWakuta | 41:d2d123d7a6fa | 107 | result = min; |
YusukeWakuta | 41:d2d123d7a6fa | 108 | else |
YusukeWakuta | 41:d2d123d7a6fa | 109 | result = value; |
YusukeWakuta | 41:d2d123d7a6fa | 110 | return result; |
YusukeWakuta | 41:d2d123d7a6fa | 111 | } |
YusukeWakuta | 41:d2d123d7a6fa | 112 | |
YusukeWakuta | 41:d2d123d7a6fa | 113 | //値をint型の段階に分ける |
YusukeWakuta | 42:6cc53727acc2 | 114 | int ValueProcess::PhaseFloat(float value,float max,float min) |
YusukeWakuta | 41:d2d123d7a6fa | 115 | { |
YusukeWakuta | 44:7b6f627a31da | 116 | int tempPhase = PHASE_NUM + ADDITIONALPHASE; //ここで足す値は偶数 |
YusukeWakuta | 44:7b6f627a31da | 117 | float PhaseWidth = (max - min) / tempPhase; |
YusukeWakuta | 41:d2d123d7a6fa | 118 | if(value< max&& value > min) { |
YusukeWakuta | 44:7b6f627a31da | 119 | for(int i = 1; i <= tempPhase; i++) { |
YusukeWakuta | 46:53eb2ba661c5 | 120 | if(value <= min + PhaseWidth * i&& value > min + PhaseWidth * (i - 1) ) |
YusukeWakuta | 41:d2d123d7a6fa | 121 | return i; |
YusukeWakuta | 41:d2d123d7a6fa | 122 | } |
YusukeWakuta | 41:d2d123d7a6fa | 123 | } else if(value <= min) |
YusukeWakuta | 41:d2d123d7a6fa | 124 | return 1; |
YusukeWakuta | 41:d2d123d7a6fa | 125 | else if(value>=max) |
YusukeWakuta | 44:7b6f627a31da | 126 | return tempPhase; |
YusukeWakuta | 41:d2d123d7a6fa | 127 | } |
YusukeWakuta | 41:d2d123d7a6fa | 128 | |
YusukeWakuta | 42:6cc53727acc2 | 129 | float ValueProcess::SetRollPitchRacio(float pitch,float roll) |
YusukeWakuta | 41:d2d123d7a6fa | 130 | { |
YusukeWakuta | 41:d2d123d7a6fa | 131 | return (roll + pitch * PITCHPERROLL) / (1.0 + PITCHPERROLL); |
YusukeWakuta | 41:d2d123d7a6fa | 132 | } |
YusukeWakuta | 42:6cc53727acc2 | 133 | |
YusukeWakuta | 43:2d8bffac06cd | 134 | float ValueProcess::MatchRange(float value) |
YusukeWakuta | 43:2d8bffac06cd | 135 | { |
YusukeWakuta | 43:2d8bffac06cd | 136 | float RangeRacio; |
YusukeWakuta | 43:2d8bffac06cd | 137 | if(pitchUpperDiff - pitchLowerDiff > rollUpperDiff - rollLowerDiff) { |
YusukeWakuta | 43:2d8bffac06cd | 138 | RangeRacio = pitchUpperDiff / rollUpperDiff; //このメソッドを呼び出す前に、ピッチとロールの上側と下側の範囲は同じようにしてるので、上側だけから比を取ってくる |
YusukeWakuta | 43:2d8bffac06cd | 139 | if(value > rollNeutral) |
YusukeWakuta | 43:2d8bffac06cd | 140 | value = rollNeutral + (value - rollNeutral) * RangeRacio; |
YusukeWakuta | 43:2d8bffac06cd | 141 | else |
YusukeWakuta | 43:2d8bffac06cd | 142 | value = rollNeutral + (value - rollNeutral) * RangeRacio; |
YusukeWakuta | 43:2d8bffac06cd | 143 | } else { |
YusukeWakuta | 43:2d8bffac06cd | 144 | RangeRacio = rollUpperDiff / pitchUpperDiff; |
YusukeWakuta | 43:2d8bffac06cd | 145 | if(value > pitchNeutral) |
YusukeWakuta | 43:2d8bffac06cd | 146 | value = pitchNeutral + (value - pitchNeutral) * RangeRacio; |
YusukeWakuta | 43:2d8bffac06cd | 147 | else |
YusukeWakuta | 43:2d8bffac06cd | 148 | value = pitchNeutral + (value - pitchNeutral) * RangeRacio; |
YusukeWakuta | 43:2d8bffac06cd | 149 | } |
YusukeWakuta | 43:2d8bffac06cd | 150 | return value; |
YusukeWakuta | 43:2d8bffac06cd | 151 | } |
YusukeWakuta | 43:2d8bffac06cd | 152 | |
YusukeWakuta | 42:6cc53727acc2 | 153 | void ValueProcess::Processing(float rollRead,float pitchRead,int* input_R,int* input_L) |
YusukeWakuta | 42:6cc53727acc2 | 154 | { |
YusukeWakuta | 42:6cc53727acc2 | 155 | setMaxAndMin(enumRoll, rollRead); |
YusukeWakuta | 42:6cc53727acc2 | 156 | setMaxAndMin(enumPitch, pitchRead); |
YusukeWakuta | 42:6cc53727acc2 | 157 | |
YusukeWakuta | 42:6cc53727acc2 | 158 | float MatchedRoll = MatchUpperAndLower(enumRoll, rollNeutral + rollUpperDiff,rollNeutral + rollLowerDiff,rollNeutral,rollRead + neutralDiff); |
YusukeWakuta | 42:6cc53727acc2 | 159 | float MatchedPitch = MatchUpperAndLower(enumPitch,pitchNeutral + pitchUpperDiff,pitchNeutral + pitchLowerDiff,pitchNeutral,pitchRead); |
YusukeWakuta | 43:2d8bffac06cd | 160 | |
YusukeWakuta | 43:2d8bffac06cd | 161 | MatchedRoll = MatchRange(MatchedRoll); |
YusukeWakuta | 43:2d8bffac06cd | 162 | MatchedPitch = MatchRange(MatchedPitch); |
YusukeWakuta | 43:2d8bffac06cd | 163 | |
YusukeWakuta | 45:df3b45a8cecc | 164 | float Formated_R = Format2Range(SetRollPitchRacio(MatchedPitch,MatchedRoll),SetRollPitchRacio(pitchNeutral + pitchUpperDiff,rollNeutral + rollUpperDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff,rollNeutral + rollLowerDiff)); |
YusukeWakuta | 43:2d8bffac06cd | 165 | |
YusukeWakuta | 45:df3b45a8cecc | 166 | int phased_R=PhaseFloat(Formated_R,SetRollPitchRacio(pitchNeutral + pitchUpperDiff,rollNeutral + rollUpperDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff,rollNeutral + rollLowerDiff)); |
YusukeWakuta | 45:df3b45a8cecc | 167 | // *input_R = WidenNeutral(phased_R); |
YusukeWakuta | 48:2965c51ef540 | 168 | *input_R = phased_R;//WidenNeutral( phased_R); |
YusukeWakuta | 45:df3b45a8cecc | 169 | float Formated_L = Format2Range(SetRollPitchRacio(MatchedPitch, - MatchedRoll),SetRollPitchRacio(pitchNeutral + pitchUpperDiff,-rollNeutral - rollLowerDiff),SetRollPitchRacio(pitchNeutral +pitchLowerDiff, - rollNeutral - rollUpperDiff)); |
YusukeWakuta | 45:df3b45a8cecc | 170 | int phased_L = PhaseFloat(Formated_L,SetRollPitchRacio(pitchNeutral + pitchUpperDiff ,- rollNeutral - rollLowerDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff ,- rollNeutral - rollUpperDiff)); |
YusukeWakuta | 48:2965c51ef540 | 171 | *input_L = phased_L; // WidenNeutral(phased_L); |
YusukeWakuta | 42:6cc53727acc2 | 172 | |
YusukeWakuta | 42:6cc53727acc2 | 173 | if(*input_R < 1) |
YusukeWakuta | 42:6cc53727acc2 | 174 | *input_R = 1; |
YusukeWakuta | 42:6cc53727acc2 | 175 | else if(*input_R > PHASE_NUM) |
YusukeWakuta | 42:6cc53727acc2 | 176 | *input_R = PHASE_NUM; |
YusukeWakuta | 42:6cc53727acc2 | 177 | if(*input_L < 1) |
YusukeWakuta | 42:6cc53727acc2 | 178 | *input_L = 1 ; |
YusukeWakuta | 42:6cc53727acc2 | 179 | else if(*input_L > PHASE_NUM) |
YusukeWakuta | 42:6cc53727acc2 | 180 | *input_L =PHASE_NUM; |
YusukeWakuta | 48:2965c51ef540 | 181 | } |