albatross / Mbed 2 deprecated ControlMain2017_2

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

Dependencies:   mbed-rtos mbed

Fork of ControlMain2017 by albatross

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?

UserRevisionLine numberNew 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 }