albatross / Mbed 2 deprecated ControlMain2017_2

今までのジョイスティックの入力を段階分けしていた方式から、アナログ値をできる限りそのまま使用する方式に変更しました。

Dependencies:   mbed-rtos mbed

Fork of ControlMain2017 by albatross

ValueProcessor.cpp@43:2d8bffac06cd, 2017-04-28 (annotated)

Committer:
YusukeWakuta
Date:
Fri Apr 28 10:21:00 2017 +0000
Branch:
?????
Revision:
43:2d8bffac06cd
Parent:
42:6cc53727acc2
Child:
44:7b6f627a31da
pitch?roll?????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YusukeWakuta 41:d2d123d7a6fa 1 #include "ValueProcessor.h"
YusukeWakuta 42:6cc53727acc2 2 #include "mbed.h"
YusukeWakuta 41:d2d123d7a6fa 3
YusukeWakuta 43:2d8bffac06cd 4 #define PITCHPERROLL 1.0
YusukeWakuta 43:2d8bffac06cd 5
YusukeWakuta 42:6cc53727acc2 6 ValueProcess::ValueProcess()
YusukeWakuta 42:6cc53727acc2 7 {
YusukeWakuta 42:6cc53727acc2 8 rollNeutral = 0.739;
YusukeWakuta 42:6cc53727acc2 9 rollUpperDiff = 0;
YusukeWakuta 42:6cc53727acc2 10 rollLowerDiff = 0;
YusukeWakuta 42:6cc53727acc2 11
YusukeWakuta 42:6cc53727acc2 12 pitchNeutral = 0.468 ;//1って書いた方
YusukeWakuta 42:6cc53727acc2 13 pitchUpperDiff = 0;
YusukeWakuta 42:6cc53727acc2 14 pitchLowerDiff = 0;
YusukeWakuta 42:6cc53727acc2 15 neutralDiff = 0;
YusukeWakuta 42:6cc53727acc2 16 }
YusukeWakuta 41:d2d123d7a6fa 17
YusukeWakuta 41:d2d123d7a6fa 18 void ValueProcess::setMaxAndMin(InputType it,float value)
YusukeWakuta 41:d2d123d7a6fa 19 {
YusukeWakuta 41:d2d123d7a6fa 20 if(it == enumPitch) {
YusukeWakuta 41:d2d123d7a6fa 21 if(value >pitchNeutral + pitchUpperDiff)
YusukeWakuta 41:d2d123d7a6fa 22 pitchUpperDiff = value - pitchNeutral;
YusukeWakuta 41:d2d123d7a6fa 23 else if(value < rollNeutral + pitchLowerDiff)
YusukeWakuta 41:d2d123d7a6fa 24 pitchLowerDiff = value - pitchNeutral;
YusukeWakuta 41:d2d123d7a6fa 25 return;
YusukeWakuta 41:d2d123d7a6fa 26 } else if(it == enumRoll) {
YusukeWakuta 41:d2d123d7a6fa 27 if(value >rollNeutral + rollUpperDiff)
YusukeWakuta 41:d2d123d7a6fa 28 rollUpperDiff = value - rollNeutral;
YusukeWakuta 41:d2d123d7a6fa 29 else if(value < rollNeutral + rollLowerDiff)
YusukeWakuta 41:d2d123d7a6fa 30 rollLowerDiff = value -rollNeutral ;
YusukeWakuta 41:d2d123d7a6fa 31 return;
YusukeWakuta 41:d2d123d7a6fa 32 }
YusukeWakuta 41:d2d123d7a6fa 33 }
YusukeWakuta 41:d2d123d7a6fa 34
YusukeWakuta 42:6cc53727acc2 35 void ValueProcess::setNeutral(float rollRead,float pitchRead)
YusukeWakuta 41:d2d123d7a6fa 36 {
YusukeWakuta 41:d2d123d7a6fa 37 float rollSum;
YusukeWakuta 41:d2d123d7a6fa 38 float pitchSum;
YusukeWakuta 41:d2d123d7a6fa 39
YusukeWakuta 41:d2d123d7a6fa 40 for(int i = 0; i < SUM_UP_NUM; i++) {
YusukeWakuta 42:6cc53727acc2 41 rollSum += rollRead;
YusukeWakuta 42:6cc53727acc2 42 pitchSum += pitchRead;
YusukeWakuta 41:d2d123d7a6fa 43 }
YusukeWakuta 41:d2d123d7a6fa 44 rollNeutral = rollSum / SUM_UP_NUM;
YusukeWakuta 41:d2d123d7a6fa 45 pitchNeutral = pitchSum / SUM_UP_NUM;
YusukeWakuta 41:d2d123d7a6fa 46
YusukeWakuta 41:d2d123d7a6fa 47 neutralDiff = pitchNeutral - rollNeutral; //ピッチの初期値の方がい小さいと仮定
YusukeWakuta 41:d2d123d7a6fa 48 rollNeutral += neutralDiff;
YusukeWakuta 41:d2d123d7a6fa 49 rollUpperDiff = 0;
YusukeWakuta 41:d2d123d7a6fa 50 }
YusukeWakuta 41:d2d123d7a6fa 51
YusukeWakuta 41:d2d123d7a6fa 52 //ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします
YusukeWakuta 42:6cc53727acc2 53 float ValueProcess::MatchUpperAndLower(InputType it, float max,float min,float neutral,float value)
YusukeWakuta 41:d2d123d7a6fa 54 {
YusukeWakuta 41:d2d123d7a6fa 55 float Upper = max- neutral;
YusukeWakuta 41:d2d123d7a6fa 56 float Lower = neutral - min;
YusukeWakuta 41:d2d123d7a6fa 57
YusukeWakuta 41:d2d123d7a6fa 58 if(it == enumRoll) {
YusukeWakuta 41:d2d123d7a6fa 59 if(Upper > Lower) {
YusukeWakuta 41:d2d123d7a6fa 60 if(value < neutral) {
YusukeWakuta 41:d2d123d7a6fa 61 value = neutral + ((value - neutral) * (Upper / Lower));
YusukeWakuta 41:d2d123d7a6fa 62 rollLowerDiff = -rollUpperDiff;
YusukeWakuta 41:d2d123d7a6fa 63 }
YusukeWakuta 41:d2d123d7a6fa 64 } else {
YusukeWakuta 41:d2d123d7a6fa 65 if(value > neutral) {
YusukeWakuta 41:d2d123d7a6fa 66 value = neutral + ((value - neutral) * (Lower / Upper));
YusukeWakuta 41:d2d123d7a6fa 67 rollUpperDiff = -rollLowerDiff;
YusukeWakuta 41:d2d123d7a6fa 68 }
YusukeWakuta 41:d2d123d7a6fa 69 }
YusukeWakuta 41:d2d123d7a6fa 70 } else if(it == enumPitch) {
YusukeWakuta 41:d2d123d7a6fa 71 if(Upper > Lower) {
YusukeWakuta 41:d2d123d7a6fa 72 if(value < neutral) {
YusukeWakuta 41:d2d123d7a6fa 73 value = neutral + ((value -neutral) * (Upper / Lower));
YusukeWakuta 41:d2d123d7a6fa 74 pitchLowerDiff = -pitchUpperDiff;
YusukeWakuta 41:d2d123d7a6fa 75 }
YusukeWakuta 41:d2d123d7a6fa 76 } else {
YusukeWakuta 41:d2d123d7a6fa 77 if(value > neutral) {
YusukeWakuta 41:d2d123d7a6fa 78 value = neutral + ((value - neutral) * (Lower / Upper));
YusukeWakuta 41:d2d123d7a6fa 79 pitchUpperDiff= -pitchLowerDiff;
YusukeWakuta 41:d2d123d7a6fa 80 }
YusukeWakuta 41:d2d123d7a6fa 81 }
YusukeWakuta 43:2d8bffac06cd 82 }
YusukeWakuta 43:2d8bffac06cd 83 return value;
YusukeWakuta 41:d2d123d7a6fa 84 }
YusukeWakuta 41:d2d123d7a6fa 85
YusukeWakuta 41:d2d123d7a6fa 86 //範囲外に値がない場合にエラーが発生するので範囲内に収める
YusukeWakuta 42:6cc53727acc2 87 float ValueProcess::Format2Range(float value,float max,float min)
YusukeWakuta 41:d2d123d7a6fa 88 {
YusukeWakuta 41:d2d123d7a6fa 89 float result;
YusukeWakuta 41:d2d123d7a6fa 90
YusukeWakuta 41:d2d123d7a6fa 91 if(value > max)
YusukeWakuta 41:d2d123d7a6fa 92 result= max;
YusukeWakuta 41:d2d123d7a6fa 93 else if(value < min)
YusukeWakuta 41:d2d123d7a6fa 94 result = min;
YusukeWakuta 41:d2d123d7a6fa 95 else
YusukeWakuta 41:d2d123d7a6fa 96 result = value;
YusukeWakuta 41:d2d123d7a6fa 97 return result;
YusukeWakuta 41:d2d123d7a6fa 98 }
YusukeWakuta 41:d2d123d7a6fa 99
YusukeWakuta 41:d2d123d7a6fa 100 //値をint型の段階に分ける
YusukeWakuta 42:6cc53727acc2 101 int ValueProcess::PhaseFloat(float value,float max,float min)
YusukeWakuta 41:d2d123d7a6fa 102 {
YusukeWakuta 41:d2d123d7a6fa 103 float PhaseWidth = (max - min) / PHASE_NUM;
YusukeWakuta 41:d2d123d7a6fa 104 if(value< max&& value > min) {
YusukeWakuta 41:d2d123d7a6fa 105 for(int i = 1; i <= PHASE_NUM; i++) {
YusukeWakuta 41:d2d123d7a6fa 106 if(value < min + PhaseWidth * i&& value > min + PhaseWidth * (i - 1) )
YusukeWakuta 41:d2d123d7a6fa 107 return i;
YusukeWakuta 41:d2d123d7a6fa 108 }
YusukeWakuta 41:d2d123d7a6fa 109 } else if(value <= min)
YusukeWakuta 41:d2d123d7a6fa 110 return 1;
YusukeWakuta 41:d2d123d7a6fa 111 else if(value>=max)
YusukeWakuta 41:d2d123d7a6fa 112 return PHASE_NUM;
YusukeWakuta 41:d2d123d7a6fa 113 }
YusukeWakuta 41:d2d123d7a6fa 114
YusukeWakuta 42:6cc53727acc2 115 float ValueProcess::SetRollPitchRacio(float pitch,float roll)
YusukeWakuta 41:d2d123d7a6fa 116 {
YusukeWakuta 41:d2d123d7a6fa 117 return (roll + pitch * PITCHPERROLL) / (1.0 + PITCHPERROLL);
YusukeWakuta 41:d2d123d7a6fa 118 }
YusukeWakuta 42:6cc53727acc2 119
YusukeWakuta 43:2d8bffac06cd 120 float ValueProcess::MatchRange(float value)
YusukeWakuta 43:2d8bffac06cd 121 {
YusukeWakuta 43:2d8bffac06cd 122 float RangeRacio;
YusukeWakuta 43:2d8bffac06cd 123 if(pitchUpperDiff - pitchLowerDiff > rollUpperDiff - rollLowerDiff) {
YusukeWakuta 43:2d8bffac06cd 124 RangeRacio = pitchUpperDiff / rollUpperDiff; //このメソッドを呼び出す前に、ピッチとロールの上側と下側の範囲は同じようにしてるので、上側だけから比を取ってくる
YusukeWakuta 43:2d8bffac06cd 125 if(value > rollNeutral)
YusukeWakuta 43:2d8bffac06cd 126 value = rollNeutral + (value - rollNeutral) * RangeRacio;
YusukeWakuta 43:2d8bffac06cd 127 else
YusukeWakuta 43:2d8bffac06cd 128 value = rollNeutral + (value - rollNeutral) * RangeRacio;
YusukeWakuta 43:2d8bffac06cd 129 } else {
YusukeWakuta 43:2d8bffac06cd 130 RangeRacio = rollUpperDiff / pitchUpperDiff;
YusukeWakuta 43:2d8bffac06cd 131 if(value > pitchNeutral)
YusukeWakuta 43:2d8bffac06cd 132 value = pitchNeutral + (value - pitchNeutral) * RangeRacio;
YusukeWakuta 43:2d8bffac06cd 133 else
YusukeWakuta 43:2d8bffac06cd 134 value = pitchNeutral + (value - pitchNeutral) * RangeRacio;
YusukeWakuta 43:2d8bffac06cd 135 }
YusukeWakuta 43:2d8bffac06cd 136 return value;
YusukeWakuta 43:2d8bffac06cd 137 }
YusukeWakuta 43:2d8bffac06cd 138
YusukeWakuta 42:6cc53727acc2 139 void ValueProcess::Processing(float rollRead,float pitchRead,int* input_R,int* input_L)
YusukeWakuta 42:6cc53727acc2 140 {
YusukeWakuta 43:2d8bffac06cd 141
YusukeWakuta 42:6cc53727acc2 142 setMaxAndMin(enumRoll, rollRead);
YusukeWakuta 42:6cc53727acc2 143 setMaxAndMin(enumPitch, pitchRead);
YusukeWakuta 42:6cc53727acc2 144
YusukeWakuta 42:6cc53727acc2 145 float MatchedRoll = MatchUpperAndLower(enumRoll, rollNeutral + rollUpperDiff,rollNeutral + rollLowerDiff,rollNeutral,rollRead + neutralDiff);
YusukeWakuta 42:6cc53727acc2 146 float MatchedPitch = MatchUpperAndLower(enumPitch,pitchNeutral + pitchUpperDiff,pitchNeutral + pitchLowerDiff,pitchNeutral,pitchRead);
YusukeWakuta 43:2d8bffac06cd 147
YusukeWakuta 43:2d8bffac06cd 148 MatchedRoll = MatchRange(MatchedRoll);
YusukeWakuta 43:2d8bffac06cd 149 MatchedPitch = MatchRange(MatchedPitch);
YusukeWakuta 43:2d8bffac06cd 150
YusukeWakuta 42:6cc53727acc2 151 float FormatedRoll_R = Format2Range(SetRollPitchRacio(MatchedPitch,MatchedRoll),SetRollPitchRacio(pitchNeutral + pitchUpperDiff,rollNeutral + rollUpperDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff,rollNeutral + rollLowerDiff));
YusukeWakuta 43:2d8bffac06cd 152
YusukeWakuta 42:6cc53727acc2 153 *input_R =PhaseFloat(FormatedRoll_R,SetRollPitchRacio(pitchNeutral + pitchUpperDiff,rollNeutral + rollUpperDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff,rollNeutral + rollLowerDiff));
YusukeWakuta 42:6cc53727acc2 154
YusukeWakuta 42:6cc53727acc2 155 float FormatedRoll_L = Format2Range(SetRollPitchRacio(MatchedPitch, - MatchedRoll),SetRollPitchRacio(pitchNeutral + pitchUpperDiff,-rollNeutral - rollLowerDiff),SetRollPitchRacio(pitchNeutral +pitchLowerDiff, - rollNeutral - rollUpperDiff));
YusukeWakuta 42:6cc53727acc2 156 *input_L = PhaseFloat(FormatedRoll_L,SetRollPitchRacio(pitchNeutral + pitchUpperDiff ,- rollNeutral - rollLowerDiff),SetRollPitchRacio(pitchNeutral + pitchLowerDiff ,- rollNeutral - rollUpperDiff));
YusukeWakuta 42:6cc53727acc2 157 if(*input_R < 1)
YusukeWakuta 42:6cc53727acc2 158 *input_R = 1;
YusukeWakuta 42:6cc53727acc2 159 else if(*input_R > PHASE_NUM)
YusukeWakuta 42:6cc53727acc2 160 *input_R = PHASE_NUM;
YusukeWakuta 42:6cc53727acc2 161 if(*input_L < 1)
YusukeWakuta 42:6cc53727acc2 162 *input_L = 1 ;
YusukeWakuta 42:6cc53727acc2 163 else if(*input_L > PHASE_NUM)
YusukeWakuta 42:6cc53727acc2 164 *input_L =PHASE_NUM;
YusukeWakuta 42:6cc53727acc2 165 }