albatross / Mbed 2 deprecated ControlMain2017_2

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

Dependencies:   mbed-rtos mbed

Fork of ControlMain2017 by albatross

ValueProcessor.cpp@41:d2d123d7a6fa, 2017-04-23 (annotated)

Committer:
YusukeWakuta
Date:
Sun Apr 23 15:12:59 2017 +0000
Branch:
?????
Revision:
41:d2d123d7a6fa
Child:
42:6cc53727acc2
????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YusukeWakuta 41:d2d123d7a6fa 1 #include "ValueProcessor.h"
YusukeWakuta 41:d2d123d7a6fa 2
YusukeWakuta 41:d2d123d7a6fa 3 enum InputType {
YusukeWakuta 41:d2d123d7a6fa 4 enumRoll,
YusukeWakuta 41:d2d123d7a6fa 5 enumPitch
YusukeWakuta 41:d2d123d7a6fa 6 };
YusukeWakuta 41:d2d123d7a6fa 7
YusukeWakuta 41:d2d123d7a6fa 8 void ValueProcess::setMaxAndMin(InputType it,float value)
YusukeWakuta 41:d2d123d7a6fa 9 {
YusukeWakuta 41:d2d123d7a6fa 10 if(it == enumPitch) {
YusukeWakuta 41:d2d123d7a6fa 11 if(value >pitchNeutral + pitchUpperDiff)
YusukeWakuta 41:d2d123d7a6fa 12 pitchUpperDiff = value - pitchNeutral;
YusukeWakuta 41:d2d123d7a6fa 13 else if(value < rollNeutral + pitchLowerDiff)
YusukeWakuta 41:d2d123d7a6fa 14 pitchLowerDiff = value - pitchNeutral;
YusukeWakuta 41:d2d123d7a6fa 15 return;
YusukeWakuta 41:d2d123d7a6fa 16 } else if(it == enumRoll) {
YusukeWakuta 41:d2d123d7a6fa 17 if(value >rollNeutral + rollUpperDiff)
YusukeWakuta 41:d2d123d7a6fa 18 rollUpperDiff = value - rollNeutral;
YusukeWakuta 41:d2d123d7a6fa 19 else if(value < rollNeutral + rollLowerDiff)
YusukeWakuta 41:d2d123d7a6fa 20 rollLowerDiff = value -rollNeutral ;
YusukeWakuta 41:d2d123d7a6fa 21 return;
YusukeWakuta 41:d2d123d7a6fa 22 }
YusukeWakuta 41:d2d123d7a6fa 23 }
YusukeWakuta 41:d2d123d7a6fa 24
YusukeWakuta 41:d2d123d7a6fa 25 void ValueProcessor::setNeutral(float rollRead,float pitchRead)
YusukeWakuta 41:d2d123d7a6fa 26 {
YusukeWakuta 41:d2d123d7a6fa 27 float rollSum;
YusukeWakuta 41:d2d123d7a6fa 28 float pitchSum;
YusukeWakuta 41:d2d123d7a6fa 29
YusukeWakuta 41:d2d123d7a6fa 30 for(int i = 0; i < SUM_UP_NUM; i++) {
YusukeWakuta 41:d2d123d7a6fa 31 rollSum += rollPin.read();
YusukeWakuta 41:d2d123d7a6fa 32 pitchSum += pitchPin.read();
YusukeWakuta 41:d2d123d7a6fa 33 }
YusukeWakuta 41:d2d123d7a6fa 34 rollNeutral = rollSum / SUM_UP_NUM;
YusukeWakuta 41:d2d123d7a6fa 35 pitchNeutral = pitchSum / SUM_UP_NUM;
YusukeWakuta 41:d2d123d7a6fa 36
YusukeWakuta 41:d2d123d7a6fa 37 neutralDiff = pitchNeutral - rollNeutral; //ピッチの初期値の方がい小さいと仮定
YusukeWakuta 41:d2d123d7a6fa 38 rollNeutral += neutralDiff;
YusukeWakuta 41:d2d123d7a6fa 39 rollUpperDiff = 0;
YusukeWakuta 41:d2d123d7a6fa 40 }
YusukeWakuta 41:d2d123d7a6fa 41
YusukeWakuta 41:d2d123d7a6fa 42 //ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします
YusukeWakuta 41:d2d123d7a6fa 43 float ValueProcessor::MatchUpperAndLower(InputType it, float max,float min,float neutral,float value)
YusukeWakuta 41:d2d123d7a6fa 44 {
YusukeWakuta 41:d2d123d7a6fa 45 float Upper = max- neutral;
YusukeWakuta 41:d2d123d7a6fa 46 float Lower = neutral - min;
YusukeWakuta 41:d2d123d7a6fa 47
YusukeWakuta 41:d2d123d7a6fa 48 if(it == enumRoll) {
YusukeWakuta 41:d2d123d7a6fa 49 if(Upper > Lower) {
YusukeWakuta 41:d2d123d7a6fa 50 if(value < neutral) {
YusukeWakuta 41:d2d123d7a6fa 51 value = neutral + ((value - neutral) * (Upper / Lower));
YusukeWakuta 41:d2d123d7a6fa 52 rollLowerDiff = -rollUpperDiff;
YusukeWakuta 41:d2d123d7a6fa 53 }
YusukeWakuta 41:d2d123d7a6fa 54 } else {
YusukeWakuta 41:d2d123d7a6fa 55 if(value > neutral) {
YusukeWakuta 41:d2d123d7a6fa 56 value = neutral + ((value - neutral) * (Lower / Upper));
YusukeWakuta 41:d2d123d7a6fa 57 rollUpperDiff = -rollLowerDiff;
YusukeWakuta 41:d2d123d7a6fa 58 }
YusukeWakuta 41:d2d123d7a6fa 59 }
YusukeWakuta 41:d2d123d7a6fa 60 return value;
YusukeWakuta 41:d2d123d7a6fa 61 } else if(it == enumPitch) {
YusukeWakuta 41:d2d123d7a6fa 62 if(Upper > Lower) {
YusukeWakuta 41:d2d123d7a6fa 63 if(value < neutral) {
YusukeWakuta 41:d2d123d7a6fa 64 value = neutral + ((value -neutral) * (Upper / Lower));
YusukeWakuta 41:d2d123d7a6fa 65 pitchLowerDiff = -pitchUpperDiff;
YusukeWakuta 41:d2d123d7a6fa 66 }
YusukeWakuta 41:d2d123d7a6fa 67 } else {
YusukeWakuta 41:d2d123d7a6fa 68 if(value > neutral) {
YusukeWakuta 41:d2d123d7a6fa 69 value = neutral + ((value - neutral) * (Lower / Upper));
YusukeWakuta 41:d2d123d7a6fa 70 pitchUpperDiff= -pitchLowerDiff;
YusukeWakuta 41:d2d123d7a6fa 71 }
YusukeWakuta 41:d2d123d7a6fa 72 }
YusukeWakuta 41:d2d123d7a6fa 73 return value;
YusukeWakuta 41:d2d123d7a6fa 74 } else return value;
YusukeWakuta 41:d2d123d7a6fa 75 }
YusukeWakuta 41:d2d123d7a6fa 76
YusukeWakuta 41:d2d123d7a6fa 77 //範囲外に値がない場合にエラーが発生するので範囲内に収める
YusukeWakuta 41:d2d123d7a6fa 78 float ValueProcessor::Format2Range(float value,float max,float min)
YusukeWakuta 41:d2d123d7a6fa 79 {
YusukeWakuta 41:d2d123d7a6fa 80 float result;
YusukeWakuta 41:d2d123d7a6fa 81
YusukeWakuta 41:d2d123d7a6fa 82 if(value > max)
YusukeWakuta 41:d2d123d7a6fa 83 result= max;
YusukeWakuta 41:d2d123d7a6fa 84 else if(value < min)
YusukeWakuta 41:d2d123d7a6fa 85 result = min;
YusukeWakuta 41:d2d123d7a6fa 86 else
YusukeWakuta 41:d2d123d7a6fa 87 result = value;
YusukeWakuta 41:d2d123d7a6fa 88 return result;
YusukeWakuta 41:d2d123d7a6fa 89 }
YusukeWakuta 41:d2d123d7a6fa 90
YusukeWakuta 41:d2d123d7a6fa 91 //値をint型の段階に分ける
YusukeWakuta 41:d2d123d7a6fa 92 int ValueProcessor::PhaseFloat(float value,float max,float min)
YusukeWakuta 41:d2d123d7a6fa 93 {
YusukeWakuta 41:d2d123d7a6fa 94 float PhaseWidth = (max - min) / PHASE_NUM;
YusukeWakuta 41:d2d123d7a6fa 95 if(value< max&& value > min) {
YusukeWakuta 41:d2d123d7a6fa 96 for(int i = 1; i <= PHASE_NUM; i++) {
YusukeWakuta 41:d2d123d7a6fa 97 if(value < min + PhaseWidth * i&& value > min + PhaseWidth * (i - 1) )
YusukeWakuta 41:d2d123d7a6fa 98 return i;
YusukeWakuta 41:d2d123d7a6fa 99 }
YusukeWakuta 41:d2d123d7a6fa 100 } else if(value <= min)
YusukeWakuta 41:d2d123d7a6fa 101 return 1;
YusukeWakuta 41:d2d123d7a6fa 102 else if(value>=max)
YusukeWakuta 41:d2d123d7a6fa 103 return PHASE_NUM;
YusukeWakuta 41:d2d123d7a6fa 104 }
YusukeWakuta 41:d2d123d7a6fa 105
YusukeWakuta 41:d2d123d7a6fa 106 float ValueProcessor::SetRollPitchRacio(float pitch,float roll)
YusukeWakuta 41:d2d123d7a6fa 107 {
YusukeWakuta 41:d2d123d7a6fa 108 return (roll + pitch * PITCHPERROLL) / (1.0 + PITCHPERROLL);
YusukeWakuta 41:d2d123d7a6fa 109 }