albatross
/
ControlMain2017
2017年度の製作を開始します
Fork of Control_Main_Full_20160608 by
Diff: main.cpp
- Revision:
- 20:d4951f491642
- Parent:
- 19:6387e3f02b37
- Child:
- 21:9b1f5123f4a8
--- a/main.cpp Sun Mar 19 12:18:41 2017 +0000 +++ b/main.cpp Sun Mar 19 14:22:39 2017 +0000 @@ -26,7 +26,7 @@ #define DEC_INPUT_NEUTRAL 0 #define DEC_INPUT_MAX (PITCH_INPUT_MAX -ROLL_INPUT_MIN ) / 2.0 #define DEC_INPUT_MIN (PITCH_INPUT_MIN -ROLL_INPUT_MAX ) / 2.0 -#define PHASE_NUM 7 //奇数にしてください +#define PHASE_NUM 6 //偶数にしてください /* roll入力とピッチ入力だとピッチの方が1.5倍効くように */ @@ -67,10 +67,22 @@ bool stateQ = true; bool stateP_old = true; bool stateQ_old = true; +float changedRollMax; +float changedRollMin; +float changedPitchMax; +float changedPitchMin; CANMessage recmsg_R; CANMessage recmsg_L; + +int SampleFloat(float f) +{ + int temp = ((f + 0.025) * 100.0) / 5; + float result = temp / 20.0; + return result; +} + float get_analogin_ave(AnalogIn pin) { float val = 0; @@ -80,17 +92,46 @@ return val/SUM_UP_NUM; } -//値を0から1の範囲に収めます -float FormatSumed2Range(float value) +//ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします +float MatchRollUpperAndLower(float max,float min,float neutral,float value) { - float result; - if(value > 1) - result = 1; - else if(value < 0) - result = 0; - else - result = value; - return result; + float Upper = max- neutral; + float Lower = neutral - min; + if(Upper > Lower) { + if(value < neutral) { + value = neutral - ((neutral - value) * (Upper / Lower)); + changedRollMin = ROLL_INPUT_NEUTRAL - (ROLL_INPUT_NEUTRAL - ROLL_INPUT_MIN)* (Upper / Lower); + changedRollMax = ROLL_INPUT_MAX; + } + } else { + if(value > neutral) { + value = neutral + ((value - neutral) * (Lower / Upper)); + changedRollMax = ROLL_INPUT_NEUTRAL + (ROLL_INPUT_MAX - ROLL_INPUT_NEUTRAL)* (Upper / Lower); + changedRollMin = ROLL_INPUT_MIN; + } + } + return value; +} + +//ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします +float MatchPitchUpperAndLower(float max,float min,float neutral,float value) +{ + float Upper = max- neutral; + float Lower = neutral - min; + if(Upper > Lower) { + if(value < neutral) { + value = neutral - ((neutral - value) * (Upper / Lower)); + changedPitchMin = PITCH_INPUT_NEUTRAL - (PITCH_INPUT_NEUTRAL - PITCH_INPUT_MIN)* (Upper / Lower); + changedPitchMax = PITCH_INPUT_MAX; + } + } else { + if(value > neutral) { + value = neutral + ((value - neutral) * (Lower / Upper)); + changedPitchMax = PITCH_INPUT_NEUTRAL + (PITCH_INPUT_MAX - PITCH_INPUT_NEUTRAL)* (Upper / Lower); + changedPitchMin = PITCH_INPUT_MIN; + } + } + return value; } float FormatEach2Range(float value,float max,float min) @@ -105,28 +146,17 @@ return result; } -//ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします -float MatchUpperAndLower(float max,float min,float neutral,float value) +//値を0から1の範囲に収めます +float FormatSumed2Range(float value) { - float Upper = max- neutral; - float Lower = neutral - min; - if(Upper > Lower) { - if(value < neutral) - value = neutral - ((neutral - value) * (Upper / Lower)); - } else { - if(value > neutral) - value = neutral + ((value - neutral) * (Lower / Upper)); - } - return value; -} - -//Format2Rangeの後に呼び出してください -int SampleFloat(float f) -{ - int temp = ((f + 0.025) * 100.0) / 5; - float result = temp / 20.0; + float result; + if(value > 1) + result = 1; + else if(value < 0) + result = 0; + else + result = value; return result; - } int PhaseFloat(float value,float max,float min) @@ -148,16 +178,17 @@ void InputControlValues() { pc.printf("Roll:%f Pitch:%f ",rollPin.read() - (OLD_ROLL_NEUTRAL - PITCH_INPUT_NEUTRAL),pitchPin.read()); - float MatchedRoll = MatchUpperAndLower(ROLL_INPUT_MAX,ROLL_INPUT_MIN,ROLL_INPUT_NEUTRAL,rollPin.read() - (OLD_ROLL_NEUTRAL - PITCH_INPUT_NEUTRAL)); - float MatchedPitch = MatchUpperAndLower(PITCH_INPUT_MAX,ROLL_INPUT_MIN,PITCH_INPUT_NEUTRAL,pitchPin.read()); - float FormatedEachRollR = FormatEach2Range((MatchedPitch +MatchedRoll ) / 2.0,SUMED_INPUT_MAX,SUMED_INPUT_MIN); + float MatchedRoll = MatchRollUpperAndLower(ROLL_INPUT_MAX,ROLL_INPUT_MIN,ROLL_INPUT_NEUTRAL,rollPin.read() - (OLD_ROLL_NEUTRAL - PITCH_INPUT_NEUTRAL)); + float MatchedPitch = MatchPitchUpperAndLower(PITCH_INPUT_MAX,ROLL_INPUT_MIN,PITCH_INPUT_NEUTRAL,pitchPin.read()); + float FormatedEachRollR = FormatEach2Range((MatchedPitch +MatchedRoll ) / 2.0,(changedRollMax + changedPitchMax) / 2.0,(changedRollMin + changedPitchMin) / 2.0); float FormatSumedR = FormatSumed2Range(FormatedEachRollR); - *(int *)inputDatas_R =PhaseFloat(FormatSumedR,SUMED_INPUT_MAX,SUMED_INPUT_MIN); + *(int *)inputDatas_R =PhaseFloat(FormatSumedR,(changedRollMax + changedPitchMax) / 2.0,(changedRollMin + changedPitchMin) / 2.0); pc.printf("FormatR:%f ",FormatSumedR); - float FormatedEachRollL = FormatEach2Range((MatchedPitch - MatchedRoll) / 2.0,DEC_INPUT_MAX,DEC_INPUT_MIN); + float FormatedEachRollL = FormatEach2Range((MatchedPitch - MatchedRoll) / 2.0,(changedPitchMax - changedRollMin) /2.0,(changedPitchMin - changedRollMax) / 2.0); + pc.printf("FormatL:%f ",FormatedEachRollL); float FormatSumedL = FormatSumed2Range(FormatedEachRollL); pc.printf("FormatL:%f ",FormatSumedL); - *(int *)inputDatas_L = PhaseFloat(FormatSumedR,DEC_INPUT_MAX,DEC_INPUT_MIN); + *(int *)inputDatas_L = PhaseFloat(FormatSumedR,changedPitchMax - changedRollMin,changedPitchMin - changedRollMax); if(*(int *)inputDatas_R < 0) *(int *)inputDatas_R = 0;