albatross
/
ControlMain2017
2017年度の製作を開始します
Fork of Control_Main_Full_20160608 by
Diff: main.cpp
- Revision:
- 25:12408563540d
- Parent:
- 24:72f9cb6e9440
- Child:
- 26:745735b0479d
--- a/main.cpp Mon Mar 20 16:10:41 2017 +0000 +++ b/main.cpp Mon Mar 20 23:33:54 2017 +0000 @@ -38,8 +38,8 @@ AnalogIn pitchPin(p20); DigitalIn drug_R(p14); DigitalIn drug_L(p19); -DigitalOut myled1(LED1); -DigitalOut myled2(LED2); +DigitalOut led1(LED1); +DigitalOut led2(LED2); DigitalOut led4(LED4); DigitalOut led3(LED3); //Ticker sendDatasTicker; @@ -48,98 +48,94 @@ char yokutanDatas_L[YOKUTAN_DATAS_NUM]; char inputDatas_R[INPUT_DATAS_NUM]; char inputDatas_L[INPUT_DATAS_NUM]; -bool stateP = true; -bool stateQ = true; -bool stateP_old = true; -bool stateQ_old = true; -float changedRollneutral = 0.739; -float rollUpperDiff = 0.224; -float rollLowerDiff = -0.184; +float rollNeutral = 0.739; +float rollUpperDiff = 0; +float rollLowerDiff = 0; -float changedPitchNeutral = 0.468 ;//1って書いた方 -float pitchUpperDiff = 0.416; -float pitchLowerDiff = -0.358; +float pitchNeutral = 0.468 ;//1って書いた方 +float pitchUpperDiff = 0; +float pitchLowerDiff = 0; float neutralDiff; CANMessage recmsg_R; CANMessage recmsg_L; +enum InputType { + roll, + pitch +}; + 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; + rollNeutral = rollSum / SUM_UP_NUM; + pitchNeutral = pitchSum / SUM_UP_NUM; +} + +void setMaxAndMin(InputType it,float value) +{ + if(it = InputType.pitch) { + if(value >pitchNeutral + pitchUpperDiff) + pitchUpperDiff = value - pitchNeutral; + else if(value < rollNeutral - pitchLowerDiff) + pitchLowerDiff = pitchNeutral - value; + } else if(it = InputType.roll) { + if(value >rollNeutral + rollUpperDiff) + rollUpperDiff = value - rollNeutral; + else if(value < rollNeutral - rollLowerDiff) + rollLowerDiff = rollNeutral - value; + } } -void setRollMaxAndMin(float value) -{ - if(value >changedRollneutral + rollUpperDiff) - rollUpperDiff = value - changedRollneutral; - else if(value < changedRollneutral - rollLowerDiff) - rollLowerDiff = changedRollneutral - value; -} - -void setPitchMaxAndMin(float value) +void calibrateRoll2Pitch() { - 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; + neutralDiff = pitchNeutral - rollNeutral; //ピッチの初期値の方がい小さいと仮定 + rollNeutral += neutralDiff; + rollUpperDiff += neutralDiff; + rollLowerDiff += neutralDiff; } //ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします -float MatchRollUpperAndLower(float max,float min,float neutral,float value) +float MatchRollUpperAndLower(InputType it, float max,float min,float neutral,float value) { float Upper = max- neutral; float Lower = neutral - min; - if(Upper > Lower) { - if(value < neutral) { - value = neutral - ((neutral - value) * (Upper / Lower)); - rollLowerDiff = (rollLowerDiff)* (Upper / Lower); + + if(it = InputType.roll) { + if(Upper > Lower) { + if(value < neutral) { + value = neutral - ((neutral - value) * (Upper / Lower)); + rollLowerDiff = (rollLowerDiff)* (Upper / Lower); + } + } else { + if(value > neutral) { + value = neutral + ((value - neutral) * (Lower / Upper)); + rollUpperDiff = rollUpperDiff* (Upper / Lower); + } } - } else { - if(value > neutral) { - value = neutral + ((value - neutral) * (Lower / Upper)); - rollUpperDiff = rollUpperDiff* (Upper / Lower); + return value; + } else if(it = InputType.pitch) { + if(Upper > Lower) { + if(value < neutral) { + value = neutral - ((neutral - value) * (Upper / Lower)); + pitchLowerDiff = (pitchLowerDiff)* (Upper / Lower); + } + } else { + if(value > neutral) { + value = neutral + ((value - neutral) * (Lower / Upper)); + pitchUpperDiff= (pitchUpperDiff)* (Upper / Lower); + } } + return value; } - return value; -} - -//ジョイスティックの中間値から上と下の幅を合わせます。値を取得するたびに呼び出してください。範囲は広い方に合わせる物とします -float MatchPitchUpperAndLower(float max,float min,float neutral,float value) -{ - float Upper = max- neutral; - float Lower = neutral - min; - if(Upper > Lower) { - if(value < neutral) { - value = neutral - ((neutral - value) * (Upper / Lower)); - pitchLowerDiff = (pitchLowerDiff)* (Upper / Lower); - } - } else { - if(value > neutral) { - value = neutral + ((value - neutral) * (Lower / Upper)); - pitchUpperDiff= (pitchUpperDiff)* (Upper / Lower); - } - } - return value; } //範囲外に値がない場合にエラーが発生するので範囲内に収める @@ -173,31 +169,31 @@ return 0; else if(value>=max) return PHASE_NUM; - } void InputControlValues() { - 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()); + 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()); - 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 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 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); + 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); - 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; + + 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); inputDatas_R[4] = (char)drug_R; @@ -208,25 +204,6 @@ // pc.printf(""); } - - -void toString_R() -{ - pc.printf("R:"); - for(int i = 0; i < INPUT_DATAS_NUM; i++) { - pc.printf("%d:%i ",i,inputDatas_R[i]); - } -} - -void toString_L() -{ - pc.printf("L:"); - for(int i = 0; i < INPUT_DATAS_NUM; i++) { - pc.printf("%d:%i ",i,inputDatas_L[i]); - } - pc.printf("\n\r"); -} - void SendDatas() { can_R.write(CANMessage(SEND_DATAS_CAN_ID, inputDatas_R, INPUT_DATAS_NUM)); @@ -262,7 +239,7 @@ yokutanDatas_R[i] = recmsg_R.data[i]; // pc.printf("%c",yokutanDatas_R[i]); } - myled1 = !myled1; + led1 = !led1; } if(can_L.read(recmsg_L)) { for(int i = 0; i < recmsg_L.len; i++) { @@ -270,7 +247,7 @@ led3 = !led3; // pc.printf("%c",yokutanDatas_L[i]); } - myled2 = !myled2; + led2 = !led2; } }