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;
}