File content as of revision 42:6cc53727acc2:

#include "ValueProcessor.h"
#include "mbed.h"

ValueProcess::ValueProcess()
{
    rollNeutral = 0.739;
    rollUpperDiff = 0;
    rollLowerDiff = 0;

    pitchNeutral = 0.468 ;//1って書いた方
    pitchUpperDiff = 0;
    pitchLowerDiff = 0;
    neutralDiff = 0;
}

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;
    rollUpperDiff = 0;
}

//ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします
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;
            }
        }
        return value;
    } 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;
    } else 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)
{
    float PhaseWidth = (max - min) / PHASE_NUM;
    if(value< max&& value > min) {
        for(int i = 1; i <= PHASE_NUM; i++) {
            if(value < min + PhaseWidth * i&& value > min + PhaseWidth * (i - 1) )
                return i;
        }
    } else  if(value <= min)
        return 1;
    else if(value>=max)
        return PHASE_NUM;
}

float ValueProcess::SetRollPitchRacio(float pitch,float roll)
{
    return (roll + pitch * PITCHPERROLL) / (1.0 + PITCHPERROLL);
}

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);
    float FormatedRoll_R = Format2Range(SetRollPitchRacio(MatchedPitch,MatchedRoll),SetRollPitchRacio(pitchNeutral + pitchUpperDiff,rollNeutral + rollUpperDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff,rollNeutral + rollLowerDiff));
    *input_R =PhaseFloat(FormatedRoll_R,SetRollPitchRacio(pitchNeutral + pitchUpperDiff,rollNeutral + rollUpperDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff,rollNeutral + rollLowerDiff));

    float FormatedRoll_L = Format2Range(SetRollPitchRacio(MatchedPitch, - MatchedRoll),SetRollPitchRacio(pitchNeutral + pitchUpperDiff,-rollNeutral - rollLowerDiff),SetRollPitchRacio(pitchNeutral  +pitchLowerDiff, - rollNeutral - rollUpperDiff));
    *input_L = PhaseFloat(FormatedRoll_L,SetRollPitchRacio(pitchNeutral + pitchUpperDiff ,- rollNeutral - rollLowerDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff ,- rollNeutral - rollUpperDiff));

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