ValueProcessor.cpp

00001 #include "ValueProcessor.h"
00002 #include "mbed.h"
00003 
00004 #define PITCHPERROLL 1.5
00005 
00006 ValueProcess::ValueProcess()
00007 {
00008     rollNeutral = 0.739;
00009     rollUpperDiff = 0;
00010     rollLowerDiff = 0;
00011 
00012     pitchNeutral = 0.468 ;//1って書いた方
00013     pitchUpperDiff = 0;
00014     pitchLowerDiff = 0;
00015     neutralDiff = 0;
00016 }
00017 
00018 //中央の値の幅を増やします
00019 int ValueProcess::WidenNeutral(int phased)
00020 {
00021     int AddNeutral = ADDITIONALPHASE / 2;
00022     int neutralPhase = (int)((PHASE_NUM + ADDITIONALPHASE+1) / 2.0);
00023     int Np = (int)((PHASE_NUM + ADDITIONALPHASE ) / 2.0);
00024     if (neutralPhase - AddNeutral <= phased && phased <= neutralPhase + AddNeutral)
00025         phased = neutralPhase - AddNeutral;
00026     else if (neutralPhase + AddNeutral < phased)
00027         phased -= ADDITIONALPHASE;
00028 
00029     return phased;
00030 }
00031 
00032 void ValueProcess::setMaxAndMin(InputType it,float value)
00033 {
00034     if(it == enumPitch) {
00035         if(value >pitchNeutral + pitchUpperDiff)
00036             pitchUpperDiff = value - pitchNeutral;
00037         else if(value < rollNeutral + pitchLowerDiff)
00038             pitchLowerDiff = value - pitchNeutral;
00039         return;
00040     } else if(it == enumRoll) {
00041         if(value >rollNeutral + rollUpperDiff)
00042             rollUpperDiff = value - rollNeutral;
00043         else if(value < rollNeutral + rollLowerDiff)
00044             rollLowerDiff = value  -rollNeutral ;
00045         return;
00046     }
00047 }
00048 
00049 void ValueProcess::setNeutral(float rollRead,float pitchRead)
00050 {
00051     float rollSum;
00052     float pitchSum;
00053 
00054     for(int i = 0; i < SUM_UP_NUM; i++) {
00055         rollSum += rollRead;
00056         pitchSum += pitchRead;
00057     }
00058     rollNeutral = rollSum / SUM_UP_NUM;
00059     pitchNeutral = pitchSum / SUM_UP_NUM;
00060 
00061     neutralDiff = pitchNeutral - rollNeutral; //ピッチの初期値の方がい小さいと仮定
00062     rollNeutral += neutralDiff;
00063 }
00064 
00065 //ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします
00066 float ValueProcess::MatchUpperAndLower(InputType it, float max,float min,float neutral,float value)
00067 {
00068     float Upper = max- neutral;
00069     float Lower  = neutral - min;
00070 
00071     if(it == enumRoll) {
00072         if(Upper > Lower) {
00073             if(value < neutral) {
00074                 value =  neutral + ((value - neutral) * (Upper / Lower));
00075                 rollLowerDiff  =  -rollUpperDiff;
00076             }
00077         } else  {
00078             if(value > neutral) {
00079                 value =  neutral + ((value - neutral) * (Lower / Upper));
00080                 rollUpperDiff = -rollLowerDiff;
00081             }
00082         }
00083     } else if(it == enumPitch) {
00084         if(Upper > Lower) {
00085             if(value < neutral) {
00086                 value =  neutral + ((value -neutral) * (Upper / Lower));
00087                 pitchLowerDiff =  -pitchUpperDiff;
00088             }
00089         } else  {
00090             if(value > neutral) {
00091                 value =  neutral + ((value - neutral) * (Lower / Upper));
00092                 pitchUpperDiff= -pitchLowerDiff;
00093             }
00094         }
00095     }
00096     return value;
00097 }
00098 
00099 //範囲外に値がない場合にエラーが発生するので範囲内に収める
00100 float ValueProcess::Format2Range(float value,float max,float min)
00101 {
00102     float result;
00103 
00104     if(value > max)
00105         result= max;
00106     else if(value < min)
00107         result = min;
00108     else
00109         result = value;
00110     return result;
00111 }
00112 
00113 //値をint型の段階に分ける
00114 int ValueProcess::PhaseFloat(float value,float max,float min)
00115 {
00116     int tempPhase = PHASE_NUM + ADDITIONALPHASE;          //ここで足す値は偶数
00117     float PhaseWidth = (max - min) / tempPhase;
00118     if(value< max&& value > min) {
00119         for(int i = 1; i <= tempPhase; i++) {
00120             if(value <= min + PhaseWidth * i&& value > min + PhaseWidth * (i - 1) )
00121                 return i;
00122         }
00123     } else  if(value <= min)
00124         return 1;
00125     else if(value>=max)
00126         return tempPhase;
00127 }
00128 
00129 float ValueProcess::SetRollPitchRacio(float pitch,float roll)
00130 {
00131     return (roll + pitch * PITCHPERROLL) / (1.0 + PITCHPERROLL);
00132 }
00133 
00134 float ValueProcess::MatchRange(float value)
00135 {
00136     float RangeRacio;
00137     if(pitchUpperDiff - pitchLowerDiff > rollUpperDiff - rollLowerDiff) {
00138         RangeRacio = pitchUpperDiff / rollUpperDiff;                //このメソッドを呼び出す前に、ピッチとロールの上側と下側の範囲は同じようにしてるので、上側だけから比を取ってくる
00139         if(value > rollNeutral)
00140             value =  rollNeutral + (value - rollNeutral) * RangeRacio;
00141         else
00142             value = rollNeutral + (value - rollNeutral) * RangeRacio;
00143     } else {
00144         RangeRacio = rollUpperDiff / pitchUpperDiff;
00145         if(value > pitchNeutral)
00146             value =  pitchNeutral + (value - pitchNeutral) * RangeRacio;
00147         else
00148             value = pitchNeutral + (value - pitchNeutral) * RangeRacio;
00149     }
00150     return value;
00151 }
00152 
00153 void ValueProcess::Processing(float rollRead,float pitchRead,int* input_R,int* input_L)
00154 {
00155     setMaxAndMin(enumRoll, rollRead);
00156     setMaxAndMin(enumPitch, pitchRead);
00157 
00158     float MatchedRoll = MatchUpperAndLower(enumRoll, rollNeutral + rollUpperDiff,rollNeutral + rollLowerDiff,rollNeutral,rollRead + neutralDiff);
00159     float MatchedPitch = MatchUpperAndLower(enumPitch,pitchNeutral + pitchUpperDiff,pitchNeutral + pitchLowerDiff,pitchNeutral,pitchRead);
00160 
00161     MatchedRoll = MatchRange(MatchedRoll);
00162     MatchedPitch = MatchRange(MatchedPitch);
00163 
00164     float Formated_R = Format2Range(SetRollPitchRacio(MatchedPitch,MatchedRoll),SetRollPitchRacio(pitchNeutral + pitchUpperDiff,rollNeutral + rollUpperDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff,rollNeutral + rollLowerDiff));
00165 
00166     int phased_R=PhaseFloat(Formated_R,SetRollPitchRacio(pitchNeutral + pitchUpperDiff,rollNeutral + rollUpperDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff,rollNeutral + rollLowerDiff));
00167     // *input_R = WidenNeutral(phased_R);
00168     *input_R = phased_R;//WidenNeutral( phased_R);
00169     float Formated_L = Format2Range(SetRollPitchRacio(MatchedPitch, - MatchedRoll),SetRollPitchRacio(pitchNeutral + pitchUpperDiff,-rollNeutral - rollLowerDiff),SetRollPitchRacio(pitchNeutral  +pitchLowerDiff, - rollNeutral - rollUpperDiff));
00170     int phased_L = PhaseFloat(Formated_L,SetRollPitchRacio(pitchNeutral + pitchUpperDiff ,- rollNeutral - rollLowerDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff ,- rollNeutral - rollUpperDiff));
00171     *input_L = phased_L;             // WidenNeutral(phased_L);
00172 
00173     if(*input_R < 1)
00174         *input_R = 1;
00175     else if(*input_R > PHASE_NUM)
00176         *input_R   = PHASE_NUM;
00177     if(*input_L < 1)
00178         *input_L = 1 ;
00179     else if(*input_L > PHASE_NUM)
00180         *input_L =PHASE_NUM;
00181 }