albatross / Mbed 2 deprecated ControlMain2017_2

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

Dependencies:   mbed-rtos mbed

Fork of ControlMain2017 by albatross

ValueProcessor.cpp@42:6cc53727acc2, 2017-04-26 (annotated)

Committer:
YusukeWakuta
Date:
Wed Apr 26 11:57:00 2017 +0000
Branch:
?????
Revision:
42:6cc53727acc2
Parent:
41:d2d123d7a6fa
Child:
43:2d8bffac06cd
?????????????????????

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