albatross / Mbed 2 deprecated ControlMain2017_2

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

Dependencies:   mbed-rtos mbed

Fork of ControlMain2017 by albatross

ValueProcessor.cpp@45:df3b45a8cecc, 2017-04-29 (annotated)

Committer:
YusukeWakuta
Date:
Sat Apr 29 04:13:17 2017 +0000
Branch:
?????
Revision:
45:df3b45a8cecc
Parent:
44:7b6f627a31da
Child:
46:53eb2ba661c5
?????????

Who changed what in which revision?

UserRevisionLine numberNew 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 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 45:df3b45a8cecc 22 int neutralPhase = (PHASE_NUM + ADDITIONALPHASE +1)/2.0;
YusukeWakuta 44:7b6f627a31da 23 if(neutralPhase - AddNeutral<=phased && phased <= neutralPhase +AddNeutral)
YusukeWakuta 45:df3b45a8cecc 24 phased = neutralPhase;
YusukeWakuta 44:7b6f627a31da 25 else if(phased < neutralPhase - AddNeutral)
YusukeWakuta 44:7b6f627a31da 26 phased += AddNeutral;
YusukeWakuta 44:7b6f627a31da 27 else if(neutralPhase +AddNeutral<phased)
YusukeWakuta 44:7b6f627a31da 28 phased -= AddNeutral;
YusukeWakuta 45:df3b45a8cecc 29
YusukeWakuta 45:df3b45a8cecc 30 return phased - AddNeutral;
YusukeWakuta 44:7b6f627a31da 31 }
YusukeWakuta 44:7b6f627a31da 32
YusukeWakuta 41:d2d123d7a6fa 33 void ValueProcess::setMaxAndMin(InputType it,float value)
YusukeWakuta 41:d2d123d7a6fa 34 {
YusukeWakuta 41:d2d123d7a6fa 35 if(it == enumPitch) {
YusukeWakuta 41:d2d123d7a6fa 36 if(value >pitchNeutral + pitchUpperDiff)
YusukeWakuta 41:d2d123d7a6fa 37 pitchUpperDiff = value - pitchNeutral;
YusukeWakuta 41:d2d123d7a6fa 38 else if(value < rollNeutral + pitchLowerDiff)
YusukeWakuta 41:d2d123d7a6fa 39 pitchLowerDiff = value - pitchNeutral;
YusukeWakuta 41:d2d123d7a6fa 40 return;
YusukeWakuta 41:d2d123d7a6fa 41 } else if(it == enumRoll) {
YusukeWakuta 41:d2d123d7a6fa 42 if(value >rollNeutral + rollUpperDiff)
YusukeWakuta 41:d2d123d7a6fa 43 rollUpperDiff = value - rollNeutral;
YusukeWakuta 41:d2d123d7a6fa 44 else if(value < rollNeutral + rollLowerDiff)
YusukeWakuta 41:d2d123d7a6fa 45 rollLowerDiff = value -rollNeutral ;
YusukeWakuta 41:d2d123d7a6fa 46 return;
YusukeWakuta 41:d2d123d7a6fa 47 }
YusukeWakuta 41:d2d123d7a6fa 48 }
YusukeWakuta 41:d2d123d7a6fa 49
YusukeWakuta 42:6cc53727acc2 50 void ValueProcess::setNeutral(float rollRead,float pitchRead)
YusukeWakuta 41:d2d123d7a6fa 51 {
YusukeWakuta 41:d2d123d7a6fa 52 float rollSum;
YusukeWakuta 41:d2d123d7a6fa 53 float pitchSum;
YusukeWakuta 41:d2d123d7a6fa 54
YusukeWakuta 41:d2d123d7a6fa 55 for(int i = 0; i < SUM_UP_NUM; i++) {
YusukeWakuta 42:6cc53727acc2 56 rollSum += rollRead;
YusukeWakuta 42:6cc53727acc2 57 pitchSum += pitchRead;
YusukeWakuta 41:d2d123d7a6fa 58 }
YusukeWakuta 41:d2d123d7a6fa 59 rollNeutral = rollSum / SUM_UP_NUM;
YusukeWakuta 41:d2d123d7a6fa 60 pitchNeutral = pitchSum / SUM_UP_NUM;
YusukeWakuta 41:d2d123d7a6fa 61
YusukeWakuta 41:d2d123d7a6fa 62 neutralDiff = pitchNeutral - rollNeutral; //ピッチの初期値の方がい小さいと仮定
YusukeWakuta 41:d2d123d7a6fa 63 rollNeutral += neutralDiff;
YusukeWakuta 41:d2d123d7a6fa 64 }
YusukeWakuta 41:d2d123d7a6fa 65
YusukeWakuta 41:d2d123d7a6fa 66 //ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします
YusukeWakuta 42:6cc53727acc2 67 float ValueProcess::MatchUpperAndLower(InputType it, float max,float min,float neutral,float value)
YusukeWakuta 41:d2d123d7a6fa 68 {
YusukeWakuta 41:d2d123d7a6fa 69 float Upper = max- neutral;
YusukeWakuta 41:d2d123d7a6fa 70 float Lower = neutral - min;
YusukeWakuta 41:d2d123d7a6fa 71
YusukeWakuta 41:d2d123d7a6fa 72 if(it == enumRoll) {
YusukeWakuta 41:d2d123d7a6fa 73 if(Upper > Lower) {
YusukeWakuta 41:d2d123d7a6fa 74 if(value < neutral) {
YusukeWakuta 41:d2d123d7a6fa 75 value = neutral + ((value - neutral) * (Upper / Lower));
YusukeWakuta 41:d2d123d7a6fa 76 rollLowerDiff = -rollUpperDiff;
YusukeWakuta 41:d2d123d7a6fa 77 }
YusukeWakuta 41:d2d123d7a6fa 78 } else {
YusukeWakuta 41:d2d123d7a6fa 79 if(value > neutral) {
YusukeWakuta 41:d2d123d7a6fa 80 value = neutral + ((value - neutral) * (Lower / Upper));
YusukeWakuta 41:d2d123d7a6fa 81 rollUpperDiff = -rollLowerDiff;
YusukeWakuta 41:d2d123d7a6fa 82 }
YusukeWakuta 41:d2d123d7a6fa 83 }
YusukeWakuta 41:d2d123d7a6fa 84 } else if(it == enumPitch) {
YusukeWakuta 41:d2d123d7a6fa 85 if(Upper > Lower) {
YusukeWakuta 41:d2d123d7a6fa 86 if(value < neutral) {
YusukeWakuta 41:d2d123d7a6fa 87 value = neutral + ((value -neutral) * (Upper / Lower));
YusukeWakuta 41:d2d123d7a6fa 88 pitchLowerDiff = -pitchUpperDiff;
YusukeWakuta 41:d2d123d7a6fa 89 }
YusukeWakuta 41:d2d123d7a6fa 90 } else {
YusukeWakuta 41:d2d123d7a6fa 91 if(value > neutral) {
YusukeWakuta 41:d2d123d7a6fa 92 value = neutral + ((value - neutral) * (Lower / Upper));
YusukeWakuta 41:d2d123d7a6fa 93 pitchUpperDiff= -pitchLowerDiff;
YusukeWakuta 41:d2d123d7a6fa 94 }
YusukeWakuta 41:d2d123d7a6fa 95 }
YusukeWakuta 43:2d8bffac06cd 96 }
YusukeWakuta 43:2d8bffac06cd 97 return value;
YusukeWakuta 41:d2d123d7a6fa 98 }
YusukeWakuta 41:d2d123d7a6fa 99
YusukeWakuta 41:d2d123d7a6fa 100 //範囲外に値がない場合にエラーが発生するので範囲内に収める
YusukeWakuta 42:6cc53727acc2 101 float ValueProcess::Format2Range(float value,float max,float min)
YusukeWakuta 41:d2d123d7a6fa 102 {
YusukeWakuta 41:d2d123d7a6fa 103 float result;
YusukeWakuta 41:d2d123d7a6fa 104
YusukeWakuta 41:d2d123d7a6fa 105 if(value > max)
YusukeWakuta 41:d2d123d7a6fa 106 result= max;
YusukeWakuta 41:d2d123d7a6fa 107 else if(value < min)
YusukeWakuta 41:d2d123d7a6fa 108 result = min;
YusukeWakuta 41:d2d123d7a6fa 109 else
YusukeWakuta 41:d2d123d7a6fa 110 result = value;
YusukeWakuta 41:d2d123d7a6fa 111 return result;
YusukeWakuta 41:d2d123d7a6fa 112 }
YusukeWakuta 41:d2d123d7a6fa 113
YusukeWakuta 41:d2d123d7a6fa 114 //値をint型の段階に分ける
YusukeWakuta 42:6cc53727acc2 115 int ValueProcess::PhaseFloat(float value,float max,float min)
YusukeWakuta 41:d2d123d7a6fa 116 {
YusukeWakuta 44:7b6f627a31da 117 int tempPhase = PHASE_NUM + ADDITIONALPHASE; //ここで足す値は偶数
YusukeWakuta 44:7b6f627a31da 118 float PhaseWidth = (max - min) / tempPhase;
YusukeWakuta 41:d2d123d7a6fa 119 if(value< max&& value > min) {
YusukeWakuta 44:7b6f627a31da 120 for(int i = 1; i <= tempPhase; i++) {
YusukeWakuta 41:d2d123d7a6fa 121 if(value < min + PhaseWidth * i&& value > min + PhaseWidth * (i - 1) )
YusukeWakuta 41:d2d123d7a6fa 122 return i;
YusukeWakuta 41:d2d123d7a6fa 123 }
YusukeWakuta 41:d2d123d7a6fa 124 } else if(value <= min)
YusukeWakuta 41:d2d123d7a6fa 125 return 1;
YusukeWakuta 41:d2d123d7a6fa 126 else if(value>=max)
YusukeWakuta 44:7b6f627a31da 127 return tempPhase;
YusukeWakuta 41:d2d123d7a6fa 128 }
YusukeWakuta 41:d2d123d7a6fa 129
YusukeWakuta 42:6cc53727acc2 130 float ValueProcess::SetRollPitchRacio(float pitch,float roll)
YusukeWakuta 41:d2d123d7a6fa 131 {
YusukeWakuta 41:d2d123d7a6fa 132 return (roll + pitch * PITCHPERROLL) / (1.0 + PITCHPERROLL);
YusukeWakuta 41:d2d123d7a6fa 133 }
YusukeWakuta 42:6cc53727acc2 134
YusukeWakuta 43:2d8bffac06cd 135 float ValueProcess::MatchRange(float value)
YusukeWakuta 43:2d8bffac06cd 136 {
YusukeWakuta 43:2d8bffac06cd 137 float RangeRacio;
YusukeWakuta 43:2d8bffac06cd 138 if(pitchUpperDiff - pitchLowerDiff > rollUpperDiff - rollLowerDiff) {
YusukeWakuta 43:2d8bffac06cd 139 RangeRacio = pitchUpperDiff / rollUpperDiff; //このメソッドを呼び出す前に、ピッチとロールの上側と下側の範囲は同じようにしてるので、上側だけから比を取ってくる
YusukeWakuta 43:2d8bffac06cd 140 if(value > rollNeutral)
YusukeWakuta 43:2d8bffac06cd 141 value = rollNeutral + (value - rollNeutral) * RangeRacio;
YusukeWakuta 43:2d8bffac06cd 142 else
YusukeWakuta 43:2d8bffac06cd 143 value = rollNeutral + (value - rollNeutral) * RangeRacio;
YusukeWakuta 43:2d8bffac06cd 144 } else {
YusukeWakuta 43:2d8bffac06cd 145 RangeRacio = rollUpperDiff / pitchUpperDiff;
YusukeWakuta 43:2d8bffac06cd 146 if(value > pitchNeutral)
YusukeWakuta 43:2d8bffac06cd 147 value = pitchNeutral + (value - pitchNeutral) * RangeRacio;
YusukeWakuta 43:2d8bffac06cd 148 else
YusukeWakuta 43:2d8bffac06cd 149 value = pitchNeutral + (value - pitchNeutral) * RangeRacio;
YusukeWakuta 43:2d8bffac06cd 150 }
YusukeWakuta 43:2d8bffac06cd 151 return value;
YusukeWakuta 43:2d8bffac06cd 152 }
YusukeWakuta 43:2d8bffac06cd 153
YusukeWakuta 42:6cc53727acc2 154 void ValueProcess::Processing(float rollRead,float pitchRead,int* input_R,int* input_L)
YusukeWakuta 42:6cc53727acc2 155 {
YusukeWakuta 43:2d8bffac06cd 156
YusukeWakuta 42:6cc53727acc2 157 setMaxAndMin(enumRoll, rollRead);
YusukeWakuta 42:6cc53727acc2 158 setMaxAndMin(enumPitch, pitchRead);
YusukeWakuta 42:6cc53727acc2 159
YusukeWakuta 42:6cc53727acc2 160 float MatchedRoll = MatchUpperAndLower(enumRoll, rollNeutral + rollUpperDiff,rollNeutral + rollLowerDiff,rollNeutral,rollRead + neutralDiff);
YusukeWakuta 42:6cc53727acc2 161 float MatchedPitch = MatchUpperAndLower(enumPitch,pitchNeutral + pitchUpperDiff,pitchNeutral + pitchLowerDiff,pitchNeutral,pitchRead);
YusukeWakuta 43:2d8bffac06cd 162
YusukeWakuta 43:2d8bffac06cd 163 MatchedRoll = MatchRange(MatchedRoll);
YusukeWakuta 43:2d8bffac06cd 164 MatchedPitch = MatchRange(MatchedPitch);
YusukeWakuta 43:2d8bffac06cd 165
YusukeWakuta 45:df3b45a8cecc 166 float Formated_R = Format2Range(SetRollPitchRacio(MatchedPitch,MatchedRoll),SetRollPitchRacio(pitchNeutral + pitchUpperDiff,rollNeutral + rollUpperDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff,rollNeutral + rollLowerDiff));
YusukeWakuta 43:2d8bffac06cd 167
YusukeWakuta 45:df3b45a8cecc 168 int phased_R=PhaseFloat(Formated_R,SetRollPitchRacio(pitchNeutral + pitchUpperDiff,rollNeutral + rollUpperDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff,rollNeutral + rollLowerDiff));
YusukeWakuta 45:df3b45a8cecc 169 // *input_R = WidenNeutral(phased_R);
YusukeWakuta 45:df3b45a8cecc 170 *input_R = phased_R;
YusukeWakuta 45:df3b45a8cecc 171 float Formated_L = Format2Range(SetRollPitchRacio(MatchedPitch, - MatchedRoll),SetRollPitchRacio(pitchNeutral + pitchUpperDiff,-rollNeutral - rollLowerDiff),SetRollPitchRacio(pitchNeutral +pitchLowerDiff, - rollNeutral - rollUpperDiff));
YusukeWakuta 45:df3b45a8cecc 172 int phased_L = PhaseFloat(Formated_L,SetRollPitchRacio(pitchNeutral + pitchUpperDiff ,- rollNeutral - rollLowerDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff ,- rollNeutral - rollUpperDiff));
YusukeWakuta 44:7b6f627a31da 173 *input_L = WidenNeutral(phased_L);
YusukeWakuta 42:6cc53727acc2 174
YusukeWakuta 42:6cc53727acc2 175 if(*input_R < 1)
YusukeWakuta 42:6cc53727acc2 176 *input_R = 1;
YusukeWakuta 42:6cc53727acc2 177 else if(*input_R > PHASE_NUM)
YusukeWakuta 42:6cc53727acc2 178 *input_R = PHASE_NUM;
YusukeWakuta 42:6cc53727acc2 179 if(*input_L < 1)
YusukeWakuta 42:6cc53727acc2 180 *input_L = 1 ;
YusukeWakuta 42:6cc53727acc2 181 else if(*input_L > PHASE_NUM)
YusukeWakuta 42:6cc53727acc2 182 *input_L =PHASE_NUM;
YusukeWakuta 42:6cc53727acc2 183 }