albatross
/
ControlMain2017
2017年度の製作を開始します
Fork of Control_Main_Full_20160608 by
Diff: main.cpp
- Revision:
- 18:31722545ecf1
- Parent:
- 16:336e8b102555
- Child:
- 19:6387e3f02b37
--- a/main.cpp Sat Mar 11 21:32:50 2017 +0000 +++ b/main.cpp Sun Mar 19 10:44:14 2017 +0000 @@ -10,6 +10,23 @@ #define SUM_UP_NUM 20 #define SAMPLE_INTERVAL 0.05 +#define PITCH_INPUT_NEUTRAL 0.457 //1って書いた方 +#define PITCH_INPUT_MAX 0.884 +#define PITCH_INPUT_MIN 0.110 + +#define ROLL_INPUT_NEUTRAL 0.739 //2て書いた方 +#define ROLL_INPUT_MAX 0.963 +#define ROLL_INPUT_MIN 0.555 + +#define SUMED_INPUT_NEUTRAL 0.598 +#define SUMED_INPUT_MAX 0.9235 +#define SUMED_INPUT_MIN 0.3325 + +#define DEC_INPUT_NEUTRAL -0.141 +#define DEC_INPUT_MAX -0.0395 +#define DEC_INPUT_MIN -0.2225 +#define PHASE_NUM 7 //奇数にしてください + //-----------------------------------(resetInterrupt def) extern "C" void mbed_reset(); InterruptIn resetPin(p22); @@ -29,7 +46,7 @@ Serial toKeikiSerial(p28,p27); Serial pc(USBTX,USBRX); AnalogIn rollPin(p15); -AnalogIn pichPin(p20); +AnalogIn pitchPin(p20); DigitalIn drug_R(p14); DigitalIn drug_L(p19); DigitalOut myled1(LED1); @@ -59,19 +76,91 @@ return val/SUM_UP_NUM; } +//値を0から1の範囲に収めます +float FormatSumed2Range(float value) +{ + float result; + if(value > 1) + result = 1; + else if(value < 0) + result = 0; + else + result = value; + return result; +} + +float FormatEach2Range(float value,float max,float min) +{ + float result; + if(value > max) + result= max; + else if(value < min) + result = min; + else + result = value; + return result; +} + +//ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします +float MatchUpperAndLower(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)); + } 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; + return result; + +} + +int 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 0; + else if(value>=max) + return PHASE_NUM; + +} + + void InputControlValues() { - *(float *)inputDatas_R = get_analogin_ave(pichPin) * (2.0/3.0) + get_analogin_ave(rollPin) * (1.0 / 3.0); - *(float *)inputDatas_L = get_analogin_ave(pichPin) * (2.0/3.0) - get_analogin_ave(rollPin) * (1.0 / 3.0); + float MatchedRoll = MatchUpperAndLower(ROLL_INPUT_MAX,ROLL_INPUT_MIN,ROLL_INPUT_NEUTRAL,rollPin.read()); + float MatchedPitch = MatchUpperAndLower(PITCH_INPUT_MAX,ROLL_INPUT_MIN,PITCH_INPUT_NEUTRAL,pitchPin.read() - (ROLL_INPUT_NEUTRAL - PITCH_INPUT_NEUTRAL)); + float FormatedEachRollR = FormatEach2Range((MatchedRoll + MatchedPitch) / 2.0,SUMED_INPUT_MAX,SUMED_INPUT_MIN); + float FormatSumedR = FormatSumed2Range(FormatedEachRollR); + *(int *)inputDatas_R =PhaseFloat(FormatSumedR,ROLL_INPUT_MAX,ROLL_INPUT_MIN); + + float FormatedEachRollL = FormatEach2Range((MatchedRoll - MatchedPitch) / 2.0,ROLL_INPUT_MAX,ROLL_INPUT_MIN); + float FormatSumedL = FormatSumed2Range(FormatedEachRollL); + *(int *)inputDatas_L = PhaseFloat(FormatSumedR,ROLL_INPUT_MAX,ROLL_INPUT_MIN); if(*(float *)inputDatas_R < 0) *(float *)inputDatas_R = 0; - else if(*(float *)inputDatas_R > 1) - *(float *)inputDatas_R = 1; + else if(*(int *)inputDatas_R > PHASE_NUM) + *(int *)inputDatas_R = PHASE_NUM; if(*(float *)inputDatas_L < 0) *(float *)inputDatas_L = 0 ; - else if(*(float *)inputDatas_L > 1) - *(float *)inputDatas_L =1; + else if(*(int *)inputDatas_L > PHASE_NUM) + *(int *)inputDatas_L =PHASE_NUM; pc.printf("input_R:%f input_L:%f\n\r",*(float *)inputDatas_R,*(float *)inputDatas_L); inputDatas_R[4] = (char)drug_R; @@ -82,6 +171,8 @@ // pc.printf(""); } + + void toString_R() { pc.printf("R:");