今までのジョイスティックの入力を段階分けしていた方式から、アナログ値をできる限りそのまま使用する方式に変更しました。
Fork of ControlMain2017 by
ValueProcessor.cpp
- Committer:
- YusukeWakuta
- Date:
- 2017-06-02
- Branch:
- ?????
- Revision:
- 48:2965c51ef540
- Parent:
- 47:6f84dcf7d1d8
File content as of revision 48:2965c51ef540:
#include "ValueProcessor.h" #include "mbed.h" #define PITCHPERROLL 1.5 ValueProcess::ValueProcess() { rollNeutral = 0.739; rollUpperDiff = 0; rollLowerDiff = 0; pitchNeutral = 0.468 ;//1って書いた方 pitchUpperDiff = 0; pitchLowerDiff = 0; neutralDiff = 0; } //中央の値の幅を増やします int ValueProcess::WidenNeutral(int phased) { int AddNeutral = ADDITIONALPHASE / 2; int neutralPhase = (int)((PHASE_NUM + ADDITIONALPHASE+1) / 2.0); int Np = (int)((PHASE_NUM + ADDITIONALPHASE ) / 2.0); if (neutralPhase - AddNeutral <= phased && phased <= neutralPhase + AddNeutral) phased = neutralPhase - AddNeutral; else if (neutralPhase + AddNeutral < phased) phased -= ADDITIONALPHASE; return phased; } void ValueProcess::setMaxAndMin(InputType it,float value) { if(it == enumPitch) { if(value >pitchNeutral + pitchUpperDiff) pitchUpperDiff = value - pitchNeutral; else if(value < rollNeutral + pitchLowerDiff) pitchLowerDiff = value - pitchNeutral; return; } else if(it == enumRoll) { if(value >rollNeutral + rollUpperDiff) rollUpperDiff = value - rollNeutral; else if(value < rollNeutral + rollLowerDiff) rollLowerDiff = value -rollNeutral ; return; } } void ValueProcess::setNeutral(float rollRead,float pitchRead) { float rollSum; float pitchSum; for(int i = 0; i < SUM_UP_NUM; i++) { rollSum += rollRead; pitchSum += pitchRead; } rollNeutral = rollSum / SUM_UP_NUM; pitchNeutral = pitchSum / SUM_UP_NUM; neutralDiff = pitchNeutral - rollNeutral; //ピッチの初期値の方がい小さいと仮定 rollNeutral += neutralDiff; } //ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします float ValueProcess::MatchUpperAndLower(InputType it, float max,float min,float neutral,float value) { float Upper = max- neutral; float Lower = neutral - min; if(it == enumRoll) { if(Upper > Lower) { if(value < neutral) { value = neutral + ((value - neutral) * (Upper / Lower)); rollLowerDiff = -rollUpperDiff; } } else { if(value > neutral) { value = neutral + ((value - neutral) * (Lower / Upper)); rollUpperDiff = -rollLowerDiff; } } } else if(it == enumPitch) { if(Upper > Lower) { if(value < neutral) { value = neutral + ((value -neutral) * (Upper / Lower)); pitchLowerDiff = -pitchUpperDiff; } } else { if(value > neutral) { value = neutral + ((value - neutral) * (Lower / Upper)); pitchUpperDiff= -pitchLowerDiff; } } } return value; } //範囲外に値がない場合にエラーが発生するので範囲内に収める float ValueProcess::Format2Range(float value,float max,float min) { float result; if(value > max) result= max; else if(value < min) result = min; else result = value; return result; } //値をint型の段階に分ける int ValueProcess::PhaseFloat(float value,float max,float min) { int tempPhase = PHASE_NUM + ADDITIONALPHASE; //ここで足す値は偶数 float PhaseWidth = (max - min) / tempPhase; if(value< max&& value > min) { for(int i = 1; i <= tempPhase; i++) { if(value <= min + PhaseWidth * i&& value > min + PhaseWidth * (i - 1) ) return i; } } else if(value <= min) return 1; else if(value>=max) return tempPhase; } float ValueProcess::SetRollPitchRacio(float pitch,float roll) { return (roll + pitch * PITCHPERROLL) / (1.0 + PITCHPERROLL); } float ValueProcess::MatchRange(float value) { float RangeRacio; if(pitchUpperDiff - pitchLowerDiff > rollUpperDiff - rollLowerDiff) { RangeRacio = pitchUpperDiff / rollUpperDiff; //このメソッドを呼び出す前に、ピッチとロールの上側と下側の範囲は同じようにしてるので、上側だけから比を取ってくる if(value > rollNeutral) value = rollNeutral + (value - rollNeutral) * RangeRacio; else value = rollNeutral + (value - rollNeutral) * RangeRacio; } else { RangeRacio = rollUpperDiff / pitchUpperDiff; if(value > pitchNeutral) value = pitchNeutral + (value - pitchNeutral) * RangeRacio; else value = pitchNeutral + (value - pitchNeutral) * RangeRacio; } return value; } void ValueProcess::Processing(float rollRead,float pitchRead,int* input_R,int* input_L) { setMaxAndMin(enumRoll, rollRead); setMaxAndMin(enumPitch, pitchRead); float MatchedRoll = MatchUpperAndLower(enumRoll, rollNeutral + rollUpperDiff,rollNeutral + rollLowerDiff,rollNeutral,rollRead + neutralDiff); float MatchedPitch = MatchUpperAndLower(enumPitch,pitchNeutral + pitchUpperDiff,pitchNeutral + pitchLowerDiff,pitchNeutral,pitchRead); MatchedRoll = MatchRange(MatchedRoll); MatchedPitch = MatchRange(MatchedPitch); float Formated_R = Format2Range(SetRollPitchRacio(MatchedPitch,MatchedRoll),SetRollPitchRacio(pitchNeutral + pitchUpperDiff,rollNeutral + rollUpperDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff,rollNeutral + rollLowerDiff)); int phased_R=PhaseFloat(Formated_R,SetRollPitchRacio(pitchNeutral + pitchUpperDiff,rollNeutral + rollUpperDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff,rollNeutral + rollLowerDiff)); // *input_R = WidenNeutral(phased_R); *input_R = phased_R;//WidenNeutral( phased_R); float Formated_L = Format2Range(SetRollPitchRacio(MatchedPitch, - MatchedRoll),SetRollPitchRacio(pitchNeutral + pitchUpperDiff,-rollNeutral - rollLowerDiff),SetRollPitchRacio(pitchNeutral +pitchLowerDiff, - rollNeutral - rollUpperDiff)); int phased_L = PhaseFloat(Formated_L,SetRollPitchRacio(pitchNeutral + pitchUpperDiff ,- rollNeutral - rollLowerDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff ,- rollNeutral - rollUpperDiff)); *input_L = phased_L; // WidenNeutral(phased_L); if(*input_R < 1) *input_R = 1; else if(*input_R > PHASE_NUM) *input_R = PHASE_NUM; if(*input_L < 1) *input_L = 1 ; else if(*input_L > PHASE_NUM) *input_L =PHASE_NUM; }