![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
2017年度の製作を開始します
Fork of Control_Main_Full_20160608 by
InputHandler/InputHandler.cpp@59:04c26e2c20bb, 2018-01-19 (annotated)
- Committer:
- tsumagari
- Date:
- Fri Jan 19 11:07:39 2018 +0000
- Branch:
- ?????
- Revision:
- 59:04c26e2c20bb
- Parent:
- 54:65e6baedec04
rsp???input?????normedRoll drR,normedPitch drL?????????????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
YusukeWakuta | 49:87af91607b46 | 1 | #include "InputHandler.h" |
YusukeWakuta | 49:87af91607b46 | 2 | #include "mbed.h" |
YusukeWakuta | 49:87af91607b46 | 3 | |
YusukeWakuta | 49:87af91607b46 | 4 | InputCalc::InputCalc() |
YusukeWakuta | 49:87af91607b46 | 5 | { |
YusukeWakuta | 49:87af91607b46 | 6 | rawNeu = 0.0; |
YusukeWakuta | 49:87af91607b46 | 7 | rawMax = NORMMIN; |
YusukeWakuta | 49:87af91607b46 | 8 | rawMin = NORMMAX; |
YusukeWakuta | 49:87af91607b46 | 9 | |
YusukeWakuta | 49:87af91607b46 | 10 | shiftedMax = 0.0; |
YusukeWakuta | 49:87af91607b46 | 11 | shiftedMin = 0.0; |
YusukeWakuta | 49:87af91607b46 | 12 | |
YusukeWakuta | 49:87af91607b46 | 13 | NeushiftWidth = 0.0; |
YusukeWakuta | 49:87af91607b46 | 14 | upperShiftRate = 0.0; |
YusukeWakuta | 49:87af91607b46 | 15 | lowerShiftRate = 0.0; |
YusukeWakuta | 49:87af91607b46 | 16 | } |
YusukeWakuta | 49:87af91607b46 | 17 | |
YusukeWakuta | 49:87af91607b46 | 18 | void InputCalc::setNeutral(float input) |
YusukeWakuta | 49:87af91607b46 | 19 | { |
YusukeWakuta | 53:947eff37d01d | 20 | // float sum = 0.0; |
YusukeWakuta | 53:947eff37d01d | 21 | // for (int i = 0; i < NEUTRAL_SUM_NUM ; i++) { |
YusukeWakuta | 53:947eff37d01d | 22 | // sum += input; |
YusukeWakuta | 53:947eff37d01d | 23 | // } |
YusukeWakuta | 53:947eff37d01d | 24 | // rawNeu = sum / NEUTRAL_SUM_NUM; |
YusukeWakuta | 53:947eff37d01d | 25 | rawNeu = input; |
YusukeWakuta | 49:87af91607b46 | 26 | } |
YusukeWakuta | 49:87af91607b46 | 27 | |
YusukeWakuta | 49:87af91607b46 | 28 | void InputCalc::setMaxAndMin(double input) |
YusukeWakuta | 49:87af91607b46 | 29 | { |
YusukeWakuta | 49:87af91607b46 | 30 | if (input > rawMax) |
YusukeWakuta | 49:87af91607b46 | 31 | rawMax = input; |
YusukeWakuta | 49:87af91607b46 | 32 | else if (input < rawMin) |
YusukeWakuta | 49:87af91607b46 | 33 | rawMin = input; |
YusukeWakuta | 49:87af91607b46 | 34 | } |
YusukeWakuta | 49:87af91607b46 | 35 | |
YusukeWakuta | 49:87af91607b46 | 36 | //入力値の中央値を0とし、その分最大値と最小値をシフトします |
YusukeWakuta | 49:87af91607b46 | 37 | void InputCalc::shift2ZeroNeu() |
YusukeWakuta | 49:87af91607b46 | 38 | { |
YusukeWakuta | 51:8a579a19a4ff | 39 | shiftedMax = rawMax - rawNeu; //初期値を0へとシフトするので、最大値と最小値から中央値を引けばシフト各値もシフトできる |
YusukeWakuta | 49:87af91607b46 | 40 | shiftedMin = rawMin - rawNeu; |
YusukeWakuta | 49:87af91607b46 | 41 | } |
YusukeWakuta | 49:87af91607b46 | 42 | |
YusukeWakuta | 49:87af91607b46 | 43 | //シフトした後の値を-1.0~1.0へと正規化するための比率を取得します |
YusukeWakuta | 49:87af91607b46 | 44 | void InputCalc::GetNormRate() |
YusukeWakuta | 49:87af91607b46 | 45 | { |
YusukeWakuta | 49:87af91607b46 | 46 | upperShiftRate = 1.0 / shiftedMax; |
YusukeWakuta | 49:87af91607b46 | 47 | lowerShiftRate = -1.0 / shiftedMin; |
YusukeWakuta | 49:87af91607b46 | 48 | } |
YusukeWakuta | 49:87af91607b46 | 49 | |
YusukeWakuta | 49:87af91607b46 | 50 | //値を-1.0~1.0へと正規化します |
YusukeWakuta | 52:a9d23be503c6 | 51 | void InputCalc::NormValue(float *input) |
YusukeWakuta | 49:87af91607b46 | 52 | { |
YusukeWakuta | 49:87af91607b46 | 53 | *input -= rawNeu; |
YusukeWakuta | 49:87af91607b46 | 54 | if (*input > 0.0) |
YusukeWakuta | 49:87af91607b46 | 55 | *input *= upperShiftRate; |
YusukeWakuta | 49:87af91607b46 | 56 | else if (*input < 0.0) |
YusukeWakuta | 49:87af91607b46 | 57 | *input *= lowerShiftRate; |
YusukeWakuta | 49:87af91607b46 | 58 | } |
YusukeWakuta | 49:87af91607b46 | 59 | |
YusukeWakuta | 49:87af91607b46 | 60 | //初期値の分、入力値を0に寄せて、拡大します。 |
YusukeWakuta | 52:a9d23be503c6 | 61 | void InputCalc::WidenNeutral(float *input) |
YusukeWakuta | 49:87af91607b46 | 62 | { |
YusukeWakuta | 49:87af91607b46 | 63 | if (-NEUWIDTH < *input&& *input < NEUWIDTH) |
YusukeWakuta | 49:87af91607b46 | 64 | *input = 0; |
YusukeWakuta | 49:87af91607b46 | 65 | else if (*input < -NEUWIDTH) { |
YusukeWakuta | 49:87af91607b46 | 66 | *input += NEUWIDTH; |
YusukeWakuta | 49:87af91607b46 | 67 | *input *= WIDENRATE; |
YusukeWakuta | 49:87af91607b46 | 68 | } else if (NEUWIDTH < *input) { |
YusukeWakuta | 49:87af91607b46 | 69 | *input -= NEUWIDTH; |
YusukeWakuta | 49:87af91607b46 | 70 | *input *= WIDENRATE; |
YusukeWakuta | 49:87af91607b46 | 71 | } |
YusukeWakuta | 49:87af91607b46 | 72 | } |
YusukeWakuta | 49:87af91607b46 | 73 | |
YusukeWakuta | 51:8a579a19a4ff | 74 | //範囲外の値を下限値と上限値に置き換える |
YusukeWakuta | 54:65e6baedec04 | 75 | void InputCalc::CutInvalidInput(float* input) |
YusukeWakuta | 49:87af91607b46 | 76 | { |
YusukeWakuta | 54:65e6baedec04 | 77 | if(*input > NORMMAX) |
YusukeWakuta | 54:65e6baedec04 | 78 | *input = NORMMAX; |
YusukeWakuta | 54:65e6baedec04 | 79 | else if(*input < NORMMIN) |
YusukeWakuta | 54:65e6baedec04 | 80 | *input = NORMMIN; |
YusukeWakuta | 49:87af91607b46 | 81 | } |
YusukeWakuta | 49:87af91607b46 | 82 | |
YusukeWakuta | 52:a9d23be503c6 | 83 | float InputCalc::Processing(float input) |
YusukeWakuta | 49:87af91607b46 | 84 | { |
YusukeWakuta | 49:87af91607b46 | 85 | setMaxAndMin(input); |
YusukeWakuta | 49:87af91607b46 | 86 | shift2ZeroNeu(); |
YusukeWakuta | 49:87af91607b46 | 87 | GetNormRate(); |
YusukeWakuta | 49:87af91607b46 | 88 | NormValue(&input); |
YusukeWakuta | 49:87af91607b46 | 89 | WidenNeutral(&input); |
YusukeWakuta | 54:65e6baedec04 | 90 | CutInvalidInput(&input); |
YusukeWakuta | 53:947eff37d01d | 91 | return input; |
YusukeWakuta | 49:87af91607b46 | 92 | } |