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