albatross
/
ControlMain2017
2017年度の製作を開始します
Fork of Control_Main_Full_20160608 by
Diff: main.cpp
- Revision:
- 26:745735b0479d
- Parent:
- 25:12408563540d
- Child:
- 27:1af92a603532
--- a/main.cpp Mon Mar 20 23:33:54 2017 +0000 +++ b/main.cpp Tue Mar 21 10:34:08 2017 +0000 @@ -62,8 +62,8 @@ CANMessage recmsg_L; enum InputType { - roll, - pitch + enumRoll, + enumPitch }; void setNeutral() @@ -77,61 +77,59 @@ } rollNeutral = rollSum / SUM_UP_NUM; pitchNeutral = pitchSum / SUM_UP_NUM; + + neutralDiff = pitchNeutral - rollNeutral; //ピッチの初期値の方がい小さいと仮定 + rollNeutral += neutralDiff; } void setMaxAndMin(InputType it,float value) { - if(it = InputType.pitch) { + if(it == enumPitch) { if(value >pitchNeutral + pitchUpperDiff) pitchUpperDiff = value - pitchNeutral; - else if(value < rollNeutral - pitchLowerDiff) - pitchLowerDiff = pitchNeutral - value; - } else if(it = InputType.roll) { + else if(value < rollNeutral + pitchLowerDiff) + pitchLowerDiff = value - pitchNeutral; + led1 =!led1; + return; + } else if(it == enumRoll) { if(value >rollNeutral + rollUpperDiff) rollUpperDiff = value - rollNeutral; - else if(value < rollNeutral - rollLowerDiff) - rollLowerDiff = rollNeutral - value; + else if(value < rollNeutral + rollLowerDiff) + rollLowerDiff = value -rollNeutral ; + led2 =! led2; + return; } } - -void calibrateRoll2Pitch() -{ - neutralDiff = pitchNeutral - rollNeutral; //ピッチの初期値の方がい小さいと仮定 - rollNeutral += neutralDiff; - rollUpperDiff += neutralDiff; - rollLowerDiff += neutralDiff; -} - //ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします -float MatchRollUpperAndLower(InputType it, float max,float min,float neutral,float value) +float MatchUpperAndLower(InputType it, float max,float min,float neutral,float value) { float Upper = max- neutral; float Lower = neutral - min; - if(it = InputType.roll) { + if(it == enumRoll) { if(Upper > Lower) { if(value < neutral) { value = neutral - ((neutral - value) * (Upper / Lower)); - rollLowerDiff = (rollLowerDiff)* (Upper / Lower); + // rollLowerDiff = (rollLowerDiff)* (Upper / Lower); //現状範囲値を変えれてないから、ここは要対応。0で割ってしまってnanが出るようになったからコメントアウトした。 } } else { if(value > neutral) { value = neutral + ((value - neutral) * (Lower / Upper)); - rollUpperDiff = rollUpperDiff* (Upper / Lower); + // rollUpperDiff = rollUpperDiff* (Upper / Lower); } } return value; - } else if(it = InputType.pitch) { + } else if(it == enumPitch) { if(Upper > Lower) { if(value < neutral) { value = neutral - ((neutral - value) * (Upper / Lower)); - pitchLowerDiff = (pitchLowerDiff)* (Upper / Lower); + // pitchLowerDiff = (pitchLowerDiff)* (Upper / Lower); } } else { if(value > neutral) { value = neutral + ((value - neutral) * (Lower / Upper)); - pitchUpperDiff= (pitchUpperDiff)* (Upper / Lower); + // pitchUpperDiff= (pitchUpperDiff)* (Upper / Lower); } } return value; @@ -173,32 +171,32 @@ void InputControlValues() { - setRollMaxAndMin(InputType.roll, rollPin.read()); - setPitchMaxAndMin(InputType.pitch, pitchPin.read()); - calibrateRoll2Pitch(); - float MatchedRoll = MatchUpperAndLower(InputType.roll, rollNeutral + rollUpperDiff,rollLowerDiff + rollLowerDiff,rollNeutral,rollPin.read() - neutralDiff); - float MatchedPitch = MatchUpperAndLower(InputType.pitch, pitchNeutral + pitchUpperDiff,pitchNeutral + pitchLowerDiff,pitchNeutral,pitchPin.read()); + setMaxAndMin(enumRoll, rollPin.read()); + setMaxAndMin(enumPitch, pitchPin.read()); + + // pc.printf("rollN:%f rollMax:%f rollMin:%f pitchN:%f pitchMax:%f pitchUpper:%f pitchMin:%f pitchLo:%f raw:%f\r\n",rollNeutral,rollNeutral+rollUpperDiff,rollNeutral+rollLowerDiff,pitchNeutral,pitchNeutral+pitchUpperDiff,pitchUpperDiff,pitchNeutral+pitchLowerDiff,pitchLowerDiff,pitchPin.read()); + float MatchedRoll = MatchUpperAndLower(enumRoll, rollNeutral + rollUpperDiff,rollLowerDiff + rollLowerDiff,rollNeutral,rollPin.read() + neutralDiff); + float MatchedPitch = MatchUpperAndLower(enumPitch, pitchNeutral + pitchUpperDiff,pitchNeutral + pitchLowerDiff,pitchNeutral,pitchPin.read()); float FormatedRoll_R = Format2Range((MatchedPitch +MatchedRoll ) / 2.0,(rollNeutral + rollUpperDiff+ pitchNeutral + pitchUpperDiff) / 2.0,(rollNeutral + rollLowerDiff+ pitchNeutral + pitchLowerDiff) / 2.0); *(int *)inputDatas_R =PhaseFloat(FormatedRoll_R,(rollNeutral + rollUpperDiff+ pitchNeutral + pitchUpperDiff) / 2.0,(rollNeutral + rollLowerDiff+ pitchNeutral + pitchLowerDiff) / 2.0); - float FormatedRoll_L = Format2Range((MatchedPitch - MatchedRoll) / 2.0,(rollNeutral + rollUpperDiff- pitchNeutral -pitchLowerDiff) / 2.0,(rollNeutral + rollLowerDiff- pitchNeutral - pitchUpperDiff) / 2.0); - *(int *)inputDatas_L = PhaseFloat(FormatedRoll_R,(rollNeutral + rollUpperDiff- pitchNeutral - pitchLowerDiff) / 2.0,(rollNeutral + rollLowerDiff- pitchNeutral + pitchUpperDiff) / 2.0); + float FormatedRoll_L = Format2Range((MatchedPitch - MatchedRoll) / 2.0,(pitchNeutral +pitchLowerDiff - rollNeutral - rollUpperDiff) / 2.0,(pitchNeutral + pitchUpperDiff-rollNeutral - rollLowerDiff) / 2.0); + *(int *)inputDatas_L = PhaseFloat(FormatedRoll_L,(pitchNeutral + pitchLowerDiff - rollNeutral - rollUpperDiff) / 2.0,(pitchNeutral + pitchUpperDiff - rollNeutral - rollLowerDiff) / 2.0); + pc.printf("Format:%f max:%f min:%f\n\r",FormatedRoll_L,(pitchNeutral + pitchUpperDiff - rollNeutral - rollLowerDiff) / 2.0,(pitchNeutral + pitchLowerDiff - rollNeutral - rollUpperDiff) / 2.0); + if(*(int *)inputDatas_R < 0) + *(int *)inputDatas_R = 0; + else if(*(int *)inputDatas_R > PHASE_NUM * 2) + *(int *)inputDatas_R = PHASE_NUM *2; + if(*(int *)inputDatas_L < 0) + *(int *)inputDatas_L = 0 ; + else if(*(int *)inputDatas_L > PHASE_NUM*2) + *(int *)inputDatas_L =PHASE_NUM * 2; - - if(*(int *)inputDatas_R < 0) - *(int *)inputDatas_R = 0; - else if(*(int *)inputDatas_R > PHASE_NUM) - *(int *)inputDatas_R = PHASE_NUM; - if(*(int *)inputDatas_L < 0) - *(int *)inputDatas_L = 0 ; - else if(*(int *)inputDatas_L > PHASE_NUM) - *(int *)inputDatas_L =PHASE_NUM; - - pc.printf("input_R:%d input_L:%d\n\r",*(int *)inputDatas_R,*(int *)inputDatas_L); + // pc.printf("input_R:%d input_L:%d\n\r",*(int *)inputDatas_R,*(int *)inputDatas_L); inputDatas_R[4] = (char)drug_R; led4 =! led4; - pc.printf("%c",*(char *)inputDatas_R[4]); + // pc.printf("%c",*(char *)inputDatas_R[4]); //pc.printf("%c",(char)drug_R); inputDatas_L[4] = (char)drug_L; // pc.printf("");