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