albatross
/
ControlMain2017
2017年度の製作を開始します
Fork of Control_Main_Full_20160608 by
Diff: main.cpp
- Revision:
- 24:72f9cb6e9440
- Parent:
- 21:9b1f5123f4a8
- Child:
- 25:12408563540d
--- a/main.cpp Sun Mar 19 15:07:25 2017 +0000 +++ b/main.cpp Mon Mar 20 16:10:41 2017 +0000 @@ -10,16 +10,8 @@ #define SUM_UP_NUM 20 #define SAMPLE_INTERVAL 0.05 -#define PITCH_INPUT_NEUTRAL 0.468 //1って書いた方 -#define PITCH_INPUT_MAX 0.884 -#define PITCH_INPUT_MIN 0.110 - -#define OLD_ROLL_NEUTRAL 0.739 -#define ROLL_INPUT_NEUTRAL 0.468 //2て書いた方 -#define ROLL_INPUT_MAX 0.963 - (OLD_ROLL_NEUTRAL - PITCH_INPUT_NEUTRAL) -#define ROLL_INPUT_MIN 0.555 - (OLD_ROLL_NEUTRAL - PITCH_INPUT_NEUTRAL) - #define PHASE_NUM 6 //偶数にしてください +#define SUM_UP_NUM 10.0 /* roll入力とピッチ入力だとピッチの方が1.5倍効くように */ @@ -60,14 +52,58 @@ bool stateQ = true; bool stateP_old = true; bool stateQ_old = true; -float changedRollMax; -float changedRollMin; -float changedPitchMax; -float changedPitchMin; + +float changedRollneutral = 0.739; +float rollUpperDiff = 0.224; +float rollLowerDiff = -0.184; + +float changedPitchNeutral = 0.468 ;//1って書いた方 +float pitchUpperDiff = 0.416; +float pitchLowerDiff = -0.358; +float neutralDiff; CANMessage recmsg_R; CANMessage recmsg_L; +void setNeutral() +{ + float rollSum; + float pitchSum; + + for(int i = 0; i < SUM_UP_NUM; i++) { + rollSum += rollPin.read(); + pitchSum += pitchPin.read(); + } + changedRollneutral = rollSum / SUM_UP_NUM; + changedPitchNeutral = pitchSum / SUM_UP_NUM; +} + + +void setRollMaxAndMin(float value) +{ + if(value >changedRollneutral + rollUpperDiff) + rollUpperDiff = value - changedRollneutral; + else if(value < changedRollneutral - rollLowerDiff) + rollLowerDiff = changedRollneutral - value; +} + +void setPitchMaxAndMin(float value) +{ + if(value >changedPitchNeutral + pitchUpperDiff) + pitchUpperDiff = value - changedPitchNeutral; + else if(value < changedRollneutral - pitchLowerDiff) + pitchLowerDiff = changedPitchNeutral - value; +} + + +void calibrateNeutral() +{ + neutralDiff = changedPitchNeutral - changedRollneutral; //ピッチの初期値の方がい小さいと仮定 + changedRollneutral = changedRollneutral + neutralDiff; + changedRollneutral = rollUpperDiff + neutralDiff; + changedRollneutral = rollLowerDiff + neutralDiff; +} + //ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします float MatchRollUpperAndLower(float max,float min,float neutral,float value) { @@ -76,14 +112,12 @@ if(Upper > Lower) { if(value < neutral) { value = neutral - ((neutral - value) * (Upper / Lower)); - changedRollMin = ROLL_INPUT_NEUTRAL - (ROLL_INPUT_NEUTRAL - ROLL_INPUT_MIN)* (Upper / Lower); - changedRollMax = ROLL_INPUT_MAX; + rollLowerDiff = (rollLowerDiff)* (Upper / Lower); } } else { if(value > neutral) { value = neutral + ((value - neutral) * (Lower / Upper)); - changedRollMax = ROLL_INPUT_NEUTRAL + (ROLL_INPUT_MAX - ROLL_INPUT_NEUTRAL)* (Upper / Lower); - changedRollMin = ROLL_INPUT_MIN; + rollUpperDiff = rollUpperDiff* (Upper / Lower); } } return value; @@ -97,14 +131,12 @@ if(Upper > Lower) { if(value < neutral) { value = neutral - ((neutral - value) * (Upper / Lower)); - changedPitchMin = PITCH_INPUT_NEUTRAL - (PITCH_INPUT_NEUTRAL - PITCH_INPUT_MIN)* (Upper / Lower); - changedPitchMax = PITCH_INPUT_MAX; + pitchLowerDiff = (pitchLowerDiff)* (Upper / Lower); } } else { if(value > neutral) { value = neutral + ((value - neutral) * (Lower / Upper)); - changedPitchMax = PITCH_INPUT_NEUTRAL + (PITCH_INPUT_MAX - PITCH_INPUT_NEUTRAL)* (Upper / Lower); - changedPitchMin = PITCH_INPUT_MIN; + pitchUpperDiff= (pitchUpperDiff)* (Upper / Lower); } } return value; @@ -146,15 +178,17 @@ void InputControlValues() { - pc.printf("Roll:%f Pitch:%f ",rollPin.read() - (OLD_ROLL_NEUTRAL - PITCH_INPUT_NEUTRAL),pitchPin.read()); - float MatchedRoll = MatchRollUpperAndLower(ROLL_INPUT_MAX,ROLL_INPUT_MIN,ROLL_INPUT_NEUTRAL,rollPin.read() - (OLD_ROLL_NEUTRAL - PITCH_INPUT_NEUTRAL)); - float MatchedPitch = MatchPitchUpperAndLower(PITCH_INPUT_MAX,ROLL_INPUT_MIN,PITCH_INPUT_NEUTRAL,pitchPin.read()); - - float FormatedRollR = Format2Range((MatchedPitch +MatchedRoll ) / 2.0,(changedRollMax + changedPitchMax) / 2.0,(changedRollMin + changedPitchMin) / 2.0); - *(int *)inputDatas_R =PhaseFloat(FormatedRollR,(changedRollMax + changedPitchMax) / 2.0,(changedRollMin + changedPitchMin) / 2.0); - - float FormatedRollL = Format2Range((MatchedPitch - MatchedRoll) / 2.0,(changedPitchMax - changedRollMin) /2.0,(changedPitchMin - changedRollMax) / 2.0); - *(int *)inputDatas_L = PhaseFloat(FormatedRollR,changedPitchMax - changedRollMin,changedPitchMin - changedRollMax); + setRollMaxAndMin(rollPin.read()); + setPitchMaxAndMin(pitchPin.read()); + calibrateNeutral(); + float MatchedRoll = MatchRollUpperAndLower(changedRollneutral + rollUpperDiff,rollLowerDiff + rollLowerDiff,changedRollneutral,rollPin.read() - neutralDiff); + float MatchedPitch = MatchPitchUpperAndLower(changedPitchNeutral + pitchUpperDiff,changedPitchNeutral + pitchLowerDiff,changedPitchNeutral,pitchPin.read()); + + float FormatedRollR = Format2Range((MatchedPitch +MatchedRoll ) / 2.0,(changedRollneutral + rollUpperDiff+ changedPitchNeutral + pitchUpperDiff) / 2.0,(changedRollneutral + rollLowerDiff+ changedPitchNeutral + pitchLowerDiff) / 2.0); + *(int *)inputDatas_R =PhaseFloat(FormatedRollR,(changedRollneutral + rollUpperDiff+ changedPitchNeutral + pitchUpperDiff) / 2.0,(changedRollneutral + rollLowerDiff+ changedPitchNeutral + pitchLowerDiff) / 2.0); + + float FormatedRollL = Format2Range((MatchedPitch - MatchedRoll) / 2.0,(changedRollneutral + rollUpperDiff- changedPitchNeutral -pitchLowerDiff) / 2.0,(changedRollneutral + rollLowerDiff- changedPitchNeutral - pitchUpperDiff) / 2.0); + *(int *)inputDatas_L = PhaseFloat(FormatedRollR,(changedRollneutral + rollUpperDiff- changedPitchNeutral - pitchLowerDiff) / 2.0,(changedRollneutral + rollLowerDiff- changedPitchNeutral + pitchUpperDiff) / 2.0); if(*(int *)inputDatas_R < 0) *(int *)inputDatas_R = 0; @@ -243,6 +277,7 @@ int main() { init(); + setNeutral(); while(1) { InputControlValues(); wait_us(5);