albatross / Mbed 2 deprecated ControlMain2017

2017年度の製作を開始します

Dependencies:   mbed mbed-rtos

Fork of Control_Main_Full_20160608 by albatross

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?

UserRevisionLine numberNew 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 }